Revision: 7490
http://playerstage.svn.sourceforge.net/playerstage/?rev=7490&view=rev
Author: hsujohnhsu
Date: 2009-03-17 00:01:48 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
Added a function to rotate a vector by a quaternion. Used to computed body CG
location for visualization.
Modified Paths:
--------------
code/gazebo/branches/ogre-1.4.9/server/Pose3d.cc
code/gazebo/branches/ogre-1.4.9/server/Pose3d.hh
Modified: code/gazebo/branches/ogre-1.4.9/server/Pose3d.cc
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/Pose3d.cc 2009-03-16 23:57:48 UTC
(rev 7489)
+++ code/gazebo/branches/ogre-1.4.9/server/Pose3d.cc 2009-03-17 00:01:48 UTC
(rev 7490)
@@ -211,3 +211,20 @@
return a;
}
+
+//////////////////////////////////////////////////////////////////////////////
+// Rotate a vector by a quaternion, added for computing CG location for the
Body
+Pose3d Pose3d::RotatePositionAboutOrigin(const Quatern &rot) const
+{
+ Pose3d a = *this;
+ a.pos.x = (1.0 - 2.0*rot.y*rot.y - 2.0*rot.z*rot.z) * this->pos.x
+ +(2.0*(rot.x*rot.y+rot.u*rot.z) ) * this->pos.y
+ +(2.0*(rot.x*rot.z-rot.u*rot.y) ) * this->pos.z;
+ a.pos.y = (2.0*(rot.x*rot.y-rot.u*rot.z) ) * this->pos.x
+ +(1.0 - 2.0*rot.x*rot.x - 2.0*rot.z*rot.z) * this->pos.y
+ +(2.0*(rot.y*rot.z+rot.u*rot.x) ) * this->pos.z;
+ a.pos.z = (2.0*(rot.x*rot.z+rot.u*rot.y) ) * this->pos.x
+ +(2.0*(rot.y*rot.z-rot.u*rot.x) ) * this->pos.y
+ +(1.0 - 2.0*rot.x*rot.x - 2.0*rot.y*rot.y) * this->pos.z;
+ return a;
+}
Modified: code/gazebo/branches/ogre-1.4.9/server/Pose3d.hh
===================================================================
--- code/gazebo/branches/ogre-1.4.9/server/Pose3d.hh 2009-03-16 23:57:48 UTC
(rev 7489)
+++ code/gazebo/branches/ogre-1.4.9/server/Pose3d.hh 2009-03-17 00:01:48 UTC
(rev 7490)
@@ -114,6 +114,9 @@
/// \brief Reset the pose
public: void Reset();
+ /// \brief Rotate vector part of a pose about the origin
+ public: Pose3d RotatePositionAboutOrigin(const Quatern &rot) const;
+
/// \brief The position
public: Vector3 pos;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
