|
From: <mk...@us...> - 2003-04-18 11:51:33
|
Update of /cvsroot/csp/APPLICATIONS/SimData/Source
In directory sc8-pr-cvs1:/tmp/cvs-serv10707/Source
Modified Files:
Quaternion.cpp
Log Message:
see CHANGES.current
Index: Quaternion.cpp
===================================================================
RCS file: /cvsroot/csp/APPLICATIONS/SimData/Source/Quaternion.cpp,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** Quaternion.cpp 11 Apr 2003 18:06:39 -0000 1.3
--- Quaternion.cpp 18 Apr 2003 11:51:28 -0000 1.4
***************
*** 62,84 ****
//----------------------------------------------------------------------------
- Quaternion::Quaternion (double fW, double fX, double fY, double fZ)
- {
- w = fW;
- x = fX;
- y = fY;
- z = fZ;
- }
-
- //----------------------------------------------------------------------------
-
- Quaternion::Quaternion (const Quaternion& rkQ)
- {
- w = rkQ.w;
- x = rkQ.x;
- y = rkQ.y;
- z = rkQ.z;
- }
-
- //----------------------------------------------------------------------------
void Quaternion::FromRotationMatrix (const Matrix3& kRot)
--- 62,65 ----
***************
*** 227,240 ****
//----------------------------------------------------------------------------
- Quaternion& Quaternion::operator= (const Quaternion& rkQ)
- {
- w = rkQ.w;
- x = rkQ.x;
- y = rkQ.y;
- z = rkQ.z;
- return *this;
- }
-
- //----------------------------------------------------------------------------
//Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
--- 208,211 ----
***************
*** 283,306 ****
//----------------------------------------------------------------------------
- Quaternion Quaternion::operator- () const
- {
- return Quaternion(-w,-x,-y,-z);
- }
-
- //----------------------------------------------------------------------------
-
- double Quaternion::Dot (const Quaternion& rkQ) const
- {
- return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
- }
-
- //----------------------------------------------------------------------------
-
- double Quaternion::Norm () const
- {
- return w*w+x*x+y*y+z*z;
- }
-
- //----------------------------------------------------------------------------
Quaternion Quaternion::Inverse () const //checked (delta)
--- 254,257 ----
***************
*** 321,332 ****
//----------------------------------------------------------------------------
- Quaternion Quaternion::UnitInverse () const //checked (delta)
- {
- // assert: 'this' is unit length
- return Quaternion(w,-x,-y,-z);
- }
-
- //----------------------------------------------------------------------------
-
Quaternion Quaternion::Exp () const
{
--- 272,275 ----
***************
*** 583,618 ****
}
- double Quaternion::Magnitude(void) const
- {
- return sqrt(w*w + x*x + y*y + z*z);
- }
-
- Vector3 QVRotate(Quaternion const& q, Vector3 const& v)
- {
- Quaternion t;
- t = (q*v)*q.Bar();
- return t.GetVector();
- }
-
- Quaternion Quaternion::Bar() const
- {
- return Quaternion(w,-x,-y,-z);
- }
-
- Quaternion QRotate(Quaternion const& q1, Quaternion const& q2)
- {
- return q1*q2*(~q1);
- }
-
- Vector3 Quaternion::GetVector(void)
- {
- return Vector3(x, y, z);
- }
-
- double Quaternion::GetScalar(void)
- {
- return w;
- }
-
Quaternion& Quaternion::operator/=(double s) // Checked (delta)
--- 526,529 ----
***************
*** 690,693 ****
--- 601,628 ----
bool operator!=(const Quaternion &a, const Quaternion &b) {
return a.w != b.w || a.x != b.x || a.y != b.y || a.z != b.z;
+ }
+
+ Vector3 Quaternion::GetRotated(Vector3 const &v) const
+ {
+ //Quaternion t = (*this*v)*Bar();
+ //return t.GetVector();
+ double qw = w;
+ double qx = x;
+ double qy = y;
+ double qz = z;
+ double vx = v.x;
+ double vy = v.y;
+ double vz = v.z;
+
+ double uw =-(qx*vx + qy*vy + qz*vz);
+ double ux = qw*vx + qy*vz - qz*vy;
+ double uy = qw*vy + qz*vx - qx*vz;
+ double uz = qw*vz + qx*vy - qy*vx;
+
+ vx = -uw*qx+ux*qw-uy*qz+uz*qy;
+ vy = -uw*qy+uy*qw-uz*qx+ux*qz;
+ vz = -uw*qz+uz*qw-ux*qy+uy*qx;
+
+ return Vector3(vx, vy, vz);
}
|