Index: code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h
===================================================================
--- code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h	(revision 2192)
+++ code/branches/physics/src/bullet/LinearMath/btMatrix3x3.h	(revision 2430)
@@ -24,12 +24,14 @@
 
 
-///The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
-///Make sure to only include a pure orthogonal matrix without scaling.
+/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
+ * Make sure to only include a pure orthogonal matrix without scaling. */
 class btMatrix3x3 {
 	public:
+  /** @brief No initializaion constructor */
 		btMatrix3x3 () {}
 		
 //		explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
 		
+  /**@brief Constructor from Quaternion */
 		explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
 		/*
@@ -40,4 +42,5 @@
 		}
 		*/
+  /** @brief Constructor with row major formatting */
 		btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
 				  const btScalar& yx, const btScalar& yy, const btScalar& yz,
@@ -48,5 +51,5 @@
 					 zx, zy, zz);
 		}
-		
+  /** @brief Copy constructor */
 		SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
 		{
@@ -55,5 +58,5 @@
 			m_el[2] = other.m_el[2];
 		}
-
+  /** @brief Assignment Operator */
 		SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
 		{
@@ -64,4 +67,6 @@
 		}
 
+  /** @brief Get a column of the matrix as a vector 
+   *  @param i Column number 0 indexed */
 		SIMD_FORCE_INLINE btVector3 getColumn(int i) const
 		{
@@ -70,11 +75,14 @@
 		
 
-
+  /** @brief Get a row of the matrix as a vector 
+   *  @param i Row number 0 indexed */
 		SIMD_FORCE_INLINE const btVector3& getRow(int i) const
 		{
+			btFullAssert(0 <= i && i < 3);
 			return m_el[i];
 		}
 
-
+  /** @brief Get a mutable reference to a row of the matrix as a vector 
+   *  @param i Row number 0 indexed */
 		SIMD_FORCE_INLINE btVector3&  operator[](int i)
 		{ 
@@ -83,4 +91,6 @@
 		}
 		
+  /** @brief Get a const reference to a row of the matrix as a vector 
+   *  @param i Row number 0 indexed */
 		SIMD_FORCE_INLINE const btVector3& operator[](int i) const
 		{
@@ -89,7 +99,11 @@
 		}
 		
+  /** @brief Multiply by the target matrix on the right
+   *  @param m Rotation matrix to be applied 
+   * Equivilant to this = this * m */
 		btMatrix3x3& operator*=(const btMatrix3x3& m); 
 		
-	
+  /** @brief Set from a carray of btScalars 
+   *  @param m A pointer to the beginning of an array of 9 btScalars */
 	void setFromOpenGLSubMatrix(const btScalar *m)
 		{
@@ -99,5 +113,14 @@
 
 		}
-
+  /** @brief Set the values of the matrix explicitly (row major)
+   *  @param xx Top left
+   *  @param xy Top Middle
+   *  @param xz Top Right
+   *  @param yx Middle Left
+   *  @param yy Middle Middle
+   *  @param yz Middle Right
+   *  @param zx Bottom Left
+   *  @param zy Bottom Middle
+   *  @param zz Bottom Right*/
 		void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
 					  const btScalar& yx, const btScalar& yy, const btScalar& yz, 
@@ -108,5 +131,7 @@
 			m_el[2].setValue(zx,zy,zz);
 		}
-  
+
+  /** @brief Set the matrix from a quaternion
+   *  @param q The Quaternion to match */  
 		void setRotation(const btQuaternion& q) 
 		{
@@ -124,33 +149,25 @@
 		
 
-
+  /** @brief Set the matrix from euler angles using YPR around YXZ respectively
+   *  @param yaw Yaw about Y axis
+   *  @param pitch Pitch about X axis
+   *  @param roll Roll about Z axis 
+   */
 		void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
 		{
-
-			btScalar cy(btCos(yaw)); 
-			btScalar  sy(btSin(yaw)); 
-			btScalar  cp(btCos(pitch)); 
-			btScalar  sp(btSin(pitch)); 
-			btScalar  cr(btCos(roll));
-			btScalar  sr(btSin(roll));
-			btScalar  cc = cy * cr; 
-			btScalar  cs = cy * sr; 
-			btScalar  sc = sy * cr; 
-			btScalar  ss = sy * sr;
-			setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
-                     cp * sr,       cp * cr,      -sp,
-					 sc + sp * cs, -ss + sp * cc,  cy * cp);
-		
-		}
-
-	/**
-	 * setEulerZYX
-	 * @param euler a const reference to a btVector3 of euler angles
+			setEulerZYX(roll, pitch, yaw);
+		}
+
+	/** @brief Set the matrix from euler angles YPR around ZYX axes
+	 * @param eulerX Roll about X axis
+         * @param eulerY Pitch around Y axis
+         * @param eulerZ Yaw aboud Z axis
+         * 
 	 * These angles are used to produce a rotation matrix. The euler
 	 * angles are applied in ZYX order. I.e a vector is first rotated 
 	 * about X then Y and then Z
 	 **/
-	
-	void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
+	void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 
+  ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
 		btScalar ci ( btCos(eulerX)); 
 		btScalar cj ( btCos(eulerY)); 
@@ -169,4 +186,5 @@
 	}
 
+  /**@brief Set the matrix to the identity */
 		void setIdentity()
 		{ 
@@ -175,5 +193,6 @@
 					 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
 		}
-    
+  /**@brief Fill the values of the matrix into a 9 element array 
+   * @param m The array to be filled */
 		void getOpenGLSubMatrix(btScalar *m) const 
 		{
@@ -192,4 +211,6 @@
 		}
 
+  /**@brief Get the matrix represented as a quaternion 
+   * @param q The quaternion which will be set */
 		void getRotation(btQuaternion& q) const
 		{
@@ -225,33 +246,100 @@
 			q.setValue(temp[0],temp[1],temp[2],temp[3]);
 		}
-	
-		void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+
+  /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
+   * @param yaw Yaw around Y axis
+   * @param pitch Pitch around X axis
+   * @param roll around Z axis */	
+		void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
 		{
 			
-			if (btScalar(m_el[1].z()) < btScalar(1))
-			{
-				if (btScalar(m_el[1].z()) > -btScalar(1))
-				{
-					yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
-					pitch = btScalar(btAsin(-m_el[1].y()));
-					roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
-				}
-				else 
-				{
-					yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
-					pitch = SIMD_HALF_PI;
-					roll = btScalar(0.0);
-				}
-			}
-			else
-			{
-				yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
-				pitch = -SIMD_HALF_PI;
-				roll = btScalar(0.0);
-			}
-		}
-		
-
-	
+			// first use the normal calculus
+			yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
+			pitch = btScalar(btAsin(-m_el[2].x()));
+			roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
+
+			// on pitch = +/-HalfPI
+			if (btFabs(pitch)==SIMD_HALF_PI)
+			{
+				if (yaw>0)
+					yaw-=SIMD_PI;
+				else
+					yaw+=SIMD_PI;
+
+				if (roll>0)
+					roll-=SIMD_PI;
+				else
+					roll+=SIMD_PI;
+			}
+		};
+
+
+  /**@brief Get the matrix represented as euler angles around ZYX
+   * @param yaw Yaw around X axis
+   * @param pitch Pitch around Y axis
+   * @param roll around X axis 
+   * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
+  void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
+  {
+    struct Euler{btScalar yaw, pitch, roll;};
+    Euler euler_out;
+    Euler euler_out2; //second solution
+    //get the pointer to the raw data
+    
+    // Check that pitch is not at a singularity
+    if (btFabs(m_el[2].x()) >= 1)
+    {
+      euler_out.yaw = 0;
+      euler_out2.yaw = 0;
+	
+      // From difference of angles formula
+      btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
+      if (m_el[2].x() > 0)  //gimbal locked up
+      {
+        euler_out.pitch = SIMD_PI / btScalar(2.0);
+        euler_out2.pitch = SIMD_PI / btScalar(2.0);
+        euler_out.roll = euler_out.pitch + delta;
+        euler_out2.roll = euler_out.pitch + delta;
+      }
+      else // gimbal locked down
+      {
+        euler_out.pitch = -SIMD_PI / btScalar(2.0);
+        euler_out2.pitch = -SIMD_PI / btScalar(2.0);
+        euler_out.roll = -euler_out.pitch + delta;
+        euler_out2.roll = -euler_out.pitch + delta;
+      }
+    }
+    else
+    {
+      euler_out.pitch = - btAsin(m_el[2].x());
+      euler_out2.pitch = SIMD_PI - euler_out.pitch;
+	
+      euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
+			       m_el[2].z()/btCos(euler_out.pitch));
+      euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
+				m_el[2].z()/btCos(euler_out2.pitch));
+	
+      euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
+			      m_el[0].x()/btCos(euler_out.pitch));
+      euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
+                               m_el[0].x()/btCos(euler_out2.pitch));
+    }
+    
+    if (solution_number == 1)
+    { 
+		yaw = euler_out.yaw; 
+		pitch = euler_out.pitch;
+		roll = euler_out.roll;
+    }
+    else
+    { 
+		yaw = euler_out2.yaw; 
+		pitch = euler_out2.pitch;
+		roll = euler_out2.roll;
+    }
+  }
+
+  /**@brief Create a scaled copy of the matrix 
+   * @param s Scaling vector The elements of the vector will scale each column */
 		
 		btMatrix3x3 scaled(const btVector3& s) const
@@ -262,8 +350,13 @@
 		}
 
+  /**@brief Return the determinant of the matrix */
 		btScalar            determinant() const;
+  /**@brief Return the adjoint of the matrix */
 		btMatrix3x3 adjoint() const;
+  /**@brief Return the matrix with all values non negative */
 		btMatrix3x3 absolute() const;
+  /**@brief Return the transpose of the matrix */
 		btMatrix3x3 transpose() const;
+  /**@brief Return the inverse of the matrix */
 		btMatrix3x3 inverse() const; 
 		
@@ -285,10 +378,13 @@
 		
 
-		///diagonalizes this matrix by the Jacobi method. rot stores the rotation
-		///from the coordinate system in which the matrix is diagonal to the original
-		///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
-		///stops when all off-diagonal elements are less than the threshold multiplied
-		///by the sum of the absolute values of the diagonal, or when maxSteps have
-		///been executed. Note that this matrix is assumed to be symmetric.
+  /**@brief diagonalizes this matrix by the Jacobi method.
+   * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
+   * coordinate system, i.e., old_this = rot * new_this * rot^T. 
+   * @param threshold See iteration
+   * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
+   * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
+   * 
+   * Note that this matrix is assumed to be symmetric. 
+   */
 		void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
 		{
@@ -372,9 +468,16 @@
 		
 	protected:
+  /**@brief Calculate the matrix cofactor 
+   * @param r1 The first row to use for calculating the cofactor
+   * @param c1 The first column to use for calculating the cofactor
+   * @param r1 The second row to use for calculating the cofactor
+   * @param c1 The second column to use for calculating the cofactor
+   * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+   */
 		btScalar cofac(int r1, int c1, int r2, int c2) const 
 		{
 			return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
 		}
-
+  ///Data storage for the matrix, each vector is a row of the matrix
 		btVector3 m_el[3];
 	};
@@ -495,4 +598,6 @@
 */
 
+/**@brief Equality operator between two matrices
+ * It will test all elements are equal.  */
 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
