[Csp-commits] APPLICATIONS/SimData/Source Quaternion.cpp,1.6,1.7

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

Update of /cvsroot/csp/APPLICATIONS/SimData/Source
In directory sc8-pr-cvs1:/tmp/cvs-serv1956/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.6
retrieving revision 1.7
diff -C2 -d -r1.6 -r1.7
*** Quaternion.cpp	23 Apr 2003 09:14:24 -0000	1.6
--- Quaternion.cpp	25 Apr 2003 20:36:07 -0000	1.7
***************
*** 459,462 ****
--- 459,496 ----
  }
  
+ Vector3	Quaternion::MakeModifiedEulerAnglesFromQ(Quaternion const& q)
+ {
+ 	Vector3	u;
+ 
+ 	double q00 = q.w * q.w;
+ 	double q11 = q.x * q.x;
+ 	double q22 = q.y * q.y;
+ 	double q33 = q.z * q.z;
+ 
+ 	double r11 = q00 + q22 - q11 - q33;
+ 	double r21 = 2 * (  q.x*q.y  - q.w*q.z);
+ 	double r31 = 2 * (- q.y*q.z  - q.w*q.x);
+ 	double r32 = 2 * (- q.x*q.z  + q.w*q.y);
+ 	double r33 = q00 - q22 - q11 + q33;
+ 
+ 	double tmp = fabs(r31);
+ 	if(tmp > 0.999999)
+ 	{
+ 		double r12 = 2 * (  q.x*q.y + q.w*q.z);
+ 		double r13 = 2 * (- q.y*q.z + q.w*q.x);
+ 
+ 		u.x = -(G_PI/2) * r31/tmp; // pitch
+ 		u.y = 0.0f; //roll
+ 		u.z = atan2(-r12, -r31*r13); // yaw
+ 	}
+ 	else {
+ 		u.x = asin(-r31); // pitch
+ 		u.y = atan2(r32, r33); // roll
+ 		u.z = atan2(r21, r11); // yaw
+ 	}
+ 
+ 	return u;
+ }
+ 
  Vector3	Quaternion::MakeEulerAnglesFromQ(Quaternion const& q)
  {
***************
*** 485,496 ****
  	}
  	else {
! 	u.x = atan2(r32, r33); // roll
! 	u.y = asin(-r31); // pitch
! 	u.z = atan2(r21, r11); // yaw
  	}
  
  	return u;
- 	
- 
  }
  
--- 519,528 ----
  	}
  	else {
! 		u.x = atan2(r32, r33); // roll
! 		u.y = asin(-r31); // pitch
! 		u.z = atan2(r21, r11); // yaw
  	}
  
  	return u;
  }