[Csp-commits] APPLICATIONS/SimData/Source Quaternion.cpp,1.3,1.4

[<mk...@us...>]

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);
  }