Math work

gameplay/src/Matrix.cpp

@@ -393,6 +393,17 @@ void Matrix::createRotationZ(float angle, Matrix* dst)
     dst->m[5] = c;
 }
 
+void Matrix::createFromEuler(float yaw, float pitch, float roll, Matrix* dst)
+{
+	GP_ASSERT(dst);
+
+	memcpy(dst, MATRIX_IDENTITY, MATRIX_SIZE);
+	
+	dst->rotateY(yaw);
+	dst->rotateX(pitch);
+	dst->rotateZ(roll);
+}
+
 void Matrix::createTranslation(const Vector3& translation, Matrix* dst)
 {
     GP_ASSERT(dst);

gameplay/src/Matrix.h

@@ -8,6 +8,7 @@ namespace gameplay
 {
 
 class Plane;
+class Quaternion;
 
 /**
  * Defines a 4 x 4 floating point matrix representing a 3D transformation.
@@ -317,6 +318,17 @@ public:
      */
     static void createRotationZ(float angle, Matrix* dst);
 
+	/**
+	* Creates a matrix describing the yaw, pitch and roll rotations
+	*
+	* @param yaw The yaw angle (in radians)
+	* @param pitch The pitch angle (in radians)
+	* @param roll The roll angle (in radians)
+	* @param dst A matrix to store the result in.
+	*/
+	static void createFromEuler(float yaw, float pitch, float roll, Matrix* dst);
+
+
     /**
      * Creates a translation matrix.
      *

gameplay/src/Quaternion.cpp

@@ -60,21 +60,25 @@ bool Quaternion::isZero() const
     return x == 0.0f && y == 0.0f && z == 0.0f && w == 0.0f;
 }
 
-void Quaternion::createFromEuler(const Vector3& euler, Quaternion* dst)
+void Quaternion::createFromEuler(float yaw, float pitch, float roll, Quaternion* dst)
 {
 	GP_ASSERT(dst);
 
-	float cx = cos(euler.x * 0.5f);
-	float cy = cos(euler.y * 0.5f);
-	float cz = cos(euler.z * 0.5f);
-	float sx = sin(euler.x * 0.5f);
-	float sy = sin(euler.y * 0.5f);
-	float sz = sin(euler.z * 0.5f);
+	pitch *= 0.5f;
+	yaw *= 0.5f;
+	roll *= 0.5f;
 
-	dst->x = sx * cy * cz - cx * sy * sz;
-	dst->y = cx * sy * cz + sx * cy * sz;
-	dst->z = cx * cy * sz - sx * sy * cz;
-	dst->w = cx * cy * cz + sx * sy * sz;
+	float sinp = sin(pitch);
+	float siny = sin(yaw);
+	float sinr = sin(roll);
+	float cosp = cos(pitch);
+	float cosy = cos(yaw);
+	float cosr = cos(roll);
+
+	dst->w = cosp * cosy * cosr + sinp * siny * sinr;
+	dst->x = sinp * cosy * cosr - cosp * siny * sinr;
+	dst->y = cosp * siny * cosr + sinp * cosy * sinr;
+	dst->z = cosp * cosy * sinr - sinp * siny * cosr;
 }
 
 void Quaternion::createFromRotationMatrix(const Matrix& m, Quaternion* dst)
@@ -97,6 +101,17 @@ void Quaternion::createFromAxisAngle(const Vector3& axis, float angle, Quaternio
     dst->w = cosf(halfAngle);
 }
 
+void Quaternion::computeEuler(float* yaw, float* pitch, float* roll)
+{
+	GP_ASSERT(yaw);
+	GP_ASSERT(pitch);
+	GP_ASSERT(roll);
+
+	*pitch = std::atan2(2 * (w*x + y*z), 1 - 2 * (x*x + y*y));
+	*yaw = std::asin(2 * (w*y - z*x));
+	*roll = atan2(2 * (w*z + x*y), 1 - 2 * (y*y + z*z));
+}
+
 void Quaternion::conjugate()
 {
     conjugate(this);
@@ -200,6 +215,24 @@ void Quaternion::normalize(Quaternion* dst) const
     dst->w *= n;
 }
 
+void Quaternion::rotatePoint(Vector3& point, Vector3* dst) const
+{
+	Quaternion vecQuat;
+	Quaternion resQuat;
+	vecQuat.x = point.x;
+	vecQuat.y = point.y;
+	vecQuat.z = point.z;
+	vecQuat.w = 0.0f;
+
+	Quaternion conQuat;
+	this->conjugate(&conQuat);
+
+	resQuat = vecQuat * conQuat;
+	resQuat = (*this) * resQuat;
+
+	dst->set(resQuat.x, resQuat.y, resQuat.z);
+}
+
 void Quaternion::set(float x, float y, float z, float w)
 {
     this->x = x;
@@ -236,11 +269,6 @@ void Quaternion::set(const Quaternion& q)
     this->w = q.w;
 }
 
-void Quaternion::set(const Vector3& euler)
-{
-	Quaternion::createFromEuler(euler, this);
-}
-
 void Quaternion::setIdentity()
 {
     x = 0.0f;

gameplay/src/Quaternion.h

@@ -143,10 +143,12 @@ public:
 	* Creates this quaternion equal to the rotation from the specified euler angles
 	* and stores the result in dst.
 	*
-	* @param euler A vector describing the euler angles (in radians).
-	* @param dst A quaternion to store the conjugate in.
+	* @param yaw The yaw angle (in radians)
+	* @param pitch The pitch angle (in radians)
+	* @param roll The roll angle (in radians)
+	* @param dst A quaternion to store the result in.
 	*/
-	static void createFromEuler(const Vector3& euler, Quaternion* dst);
+	static void createFromEuler(float yaw, float pitch, float roll, Quaternion* dst);
 
     /**
      * Creates a quaternion equal to the rotational part of the specified matrix
@@ -167,6 +169,16 @@ public:
      */
     static void createFromAxisAngle(const Vector3& axis, float angle, Quaternion* dst);
 
+	/**
+	* Calculates (in radians) the yaw, pitch and roll angles of this quaternion
+	* and stores the results in the specified pointers.
+	*
+	* @param yaw The returned yaw angle
+	* @param pitch The returned pitch angle
+	* @param roll The returned roll angle
+	*/
+	void computeEuler(float* yaw, float* pitch, float* roll);
+	
     /**
      * Sets this quaternion to the conjugate of itself.
      */
@@ -240,6 +252,17 @@ public:
      */
     void normalize(Quaternion* dst) const;
 
+	/**
+	* Rotate the specified point by this quaternion
+	* and stores the result in dst
+	*
+	* Note: The point must normalized.
+	*
+	* @param vec The vector to multiply.
+	* @return The multiplied vector.
+	*/
+	void rotatePoint(Vector3& point, Vector3* dst) const;
+
     /**
      * Sets the elements of the quaternion to the specified values.
      *
@@ -272,13 +295,6 @@ public:
      */
     void set(const Vector3& axis, float angle);
 
-	/**
-	* Sets the elements of this quaternion to a copy of the specified euler angle.
-	*
-	* @param euler The euler angle (in radians).
-	*/
-	void set(const Vector3& euler);
-
     /**
      * Sets the elements of this quaternion to a copy of the specified quaternion.
      *

gameplay/src/Vector3.cpp

@@ -1,6 +1,5 @@
 #include "Base.h"
 #include "Vector3.h"
-#include "Quaternion.h"
 #include "MathUtil.h"
 
 namespace gameplay
@@ -265,25 +264,6 @@ void Vector3::scale(float scalar)
     z *= scalar;
 }
 
-void Vector3::applyQuaternion(Quaternion& q)
-{
-
-	Quaternion vecQuat;
-	Quaternion resQuat;
-	vecQuat.x = this->x;
-	vecQuat.y = this->y;
-	vecQuat.z = this->z;
-	vecQuat.w = 0.0f;
-
-	Quaternion conQuat = q;
-	conQuat.conjugate();
-
-	resQuat = vecQuat * conQuat;
-	resQuat = q * resQuat;
-
-	this->set(resQuat.x, resQuat.y, resQuat.z);
-}
-
 void Vector3::set(float x, float y, float z)
 {
     this->x = x;

gameplay/src/Vector3.h

@@ -5,7 +5,6 @@ namespace gameplay
 {
 
 class Matrix;
-class Quaternion;
 
 /**
  * Defines a 3-element floating point vector.
@@ -301,16 +300,6 @@ public:
      */
     void scale(float scalar);
 
-	/**
-	* Apply a quaternion rotation to this vector
-	*
-	* Note: This vector must first be normalized.
-	*
-	* @param vec The vector to multiply.
-	* @return The multiplied vector.
-	*/
-	void applyQuaternion(Quaternion& q);
-
     /**
      * Sets the elements of this vector to the specified values.
      *