Added Quaternion into utils.

Used equalEpsilon in Sound to silence warning

include/utils/Quaternion.h

@@ -0,0 +1,201 @@
+/*!
+ * \file include/utils/Quaternion.h
+ * \brief Quaternion
+ *
+ * \author Mongoose
+ */
+
+#ifndef _UTILS_QUATERNION_H_
+#define _UTILS_QUATERNION_H_
+
+#include "utils/math.h"
+
+/*!
+ * \brief Quaternion
+ */
+class Quaternion {
+public:
+
+    /*!
+     * \brief Constructs an object of Quaternion
+     */
+    Quaternion();
+
+    /*!
+     * \brief Constructs an object of Quaternion
+     * \param w W part of new Quaternion
+     * \param x X part of new Quaternion
+     * \param y Y part of new Quaternion
+     * \param z Z part of new Quaternion
+     */
+    Quaternion(vec_t w, vec_t x, vec_t y, vec_t z);
+
+    /*!
+     * \brief Constructs an object of Quaternion
+     * \param v contents of new Quaternion
+     */
+    Quaternion(vec4_t v);
+
+    /*!
+     * \brief Get column order matrix equivalent of this quaternion
+     * \param m where matrix will be stored
+     */
+    void getMatrix(matrix_t m);
+
+    /*!
+     * \brief Multiplies this quaternion.
+     *
+     * Use normalize() call for unit quaternion.
+     *
+     * \param q what to multiply this quaternion with
+     * \returns resultant quaternion
+     * \sa Quaternion::normalize()
+     */
+    Quaternion operator *(const Quaternion &q);
+
+    /*!
+     * \brief Divide from this quaternion
+     * \param q what to divide from this quaternion
+     * \returns resultant quaternion
+     */
+    Quaternion operator /(const Quaternion &q);
+
+    /*!
+     * \brief Add to this quaternion
+     * \param q what to add to this quaternion
+     * \returns resultant quaternion
+     */
+    Quaternion operator +(const Quaternion &q);
+
+    /*!
+     * \brief Subtract from this quaternion
+     * \param q what to subtract from this quaternion
+     * \returns resultant quaternion
+     */
+    Quaternion operator -(const Quaternion &q);
+
+    /*!
+     * \brief Compares q to this quaternion
+     * \param q what to compare this quaternion to
+     * \returns true if equal, false otherwise
+     */
+    bool operator ==(const Quaternion &q);
+
+    /*!
+     * \brief Conjugate this quaternion
+     * \returns Conjugate of this quaternion
+     */
+    Quaternion conjugate();
+
+    /*!
+     * \brief Scale this quaternion
+     * \param s scaling factor
+     * \returns Scaled result of this quaternion
+     */
+    Quaternion scale(vec_t s);
+
+    /*!
+     * \brief Inverse this quaternion
+     * \returns inverse of this quaternion
+     */
+    Quaternion inverse();
+
+    /*!
+     * \brief Dot Product of quaternions
+     * \param a first argument to dot product
+     * \param b second argument to dot product
+     * \returns dot product between a and b quaternions
+     */
+    static vec_t dot(Quaternion a, Quaternion b);
+
+    /*!
+     * \brief Magnitude of this quaternion
+     * \returns Magnitude of this quaternion
+     */
+    vec_t magnitude();
+
+    /*!
+     * \brief Interpolates between a and b rotations.
+     *
+     * Using spherical linear interpolation:
+     * `I = (((B . A)^-1)^Time)A`
+     *
+     * \param a first argument for slerp
+     * \param b second argument for slerp
+     * \param time time argument for slerp
+     * \returns resultant quaternion
+     */
+    static Quaternion slerp(Quaternion a, Quaternion b, vec_t time);
+
+    /*!
+     * \brief Sets this quaternion to identity
+     */
+    void setIdentity();
+
+    /*!
+     * \brief Sets this quaternion
+     * \param angle new angle
+     * \param x new X coordinate
+     * \param y new Y coordinate
+     * \param z new Z coordinate
+     */
+    void set(vec_t angle, vec_t x, vec_t y, vec_t z);
+
+    /*!
+     * \brief Normalize this quaternion
+     */
+    void normalize();
+
+    /*!
+     * \brief Set this quaternion
+     * \param q will be copied into this quaternion
+     */
+    void copy(Quaternion q);
+
+    /*!
+     * \brief Sets matrix equivalent of this quaternion
+     * \param m matrix in valid column order
+     */
+    void setByMatrix(matrix_t m);
+
+private:
+
+    /*!
+     * \brief Multiplies two quaternions
+     * \param a first argument to multiplication
+     * \param b second argument to multiplication
+     * \returns resultant quaternion
+     */
+    static Quaternion multiply(Quaternion a, Quaternion b);
+
+    /*!
+     * \brief Divides B from A quaternion
+     * \param a first argument to division
+     * \param b second argument to division
+     * \returns quotient quaternion
+     */
+    static Quaternion divide(Quaternion a, Quaternion b);
+
+    /*!
+     * \brief Adds A and B quaternions
+     * \param a first argument to addition
+     * \param b second argument to addition
+     * \returns resultant quaternion
+     */
+    static Quaternion add(Quaternion a, Quaternion b);
+
+    /*!
+     * \brief Subtracts B from A quaternion
+     * \param a first argument to subtraction
+     * \param b second argument to subtraction
+     * \returns resultant quaternion
+     */
+    static Quaternion subtract(Quaternion a, Quaternion b);
+
+    vec_t mW; //!< Quaternion, W part
+    vec_t mX; //!< Quaternion, X part
+    vec_t mY; //!< Quaternion, Y part
+    vec_t mZ; //!< Quaternion, Z part
+};
+
+#endif

src/Sound.cpp

@@ -24,6 +24,7 @@
 #include <unistd.h>
 #include <assert.h>
 
+#include "utils/math.h"
 #include "Sound.h"
 
 Sound::Sound() {
@@ -91,7 +92,7 @@ void Sound::setVolume(float vol) {
     assert(mInit == true);
     assert(mSource.size() == mBuffer.size());
 
-    if ((mSource.size() > 0) && (mVolume != vol)) {
+    if ((mSource.size() > 0) && (!equalEpsilon(mVolume, vol))) {
         // Apply new volume to old sources if needed
         for (size_t i = 0; i < mSource.size(); i++)
             alSourcef(mSource[i], AL_GAIN, vol);

src/utils/CMakeLists.txt

@@ -1,8 +1,11 @@
 # Source files
 set (UTIL_SRCS ${UTIL_SRCS} "math.cpp")
+set (UTIL_SRCS ${UTIL_SRCS} "Matrix.cpp")
+set (UTIL_SRCS ${UTIL_SRCS} "Quaternion.cpp")
 set (UTIL_SRCS ${UTIL_SRCS} "strings.cpp")
 set (UTIL_SRCS ${UTIL_SRCS} "tga.cpp")
 set (UTIL_SRCS ${UTIL_SRCS} "time.cpp")
+set (UTIL_SRCS ${UTIL_SRCS} "Vector3d.cpp")
 
 # Include directory
 include_directories ("${PROJECT_SOURCE_DIR}/include")

src/utils/Quaternion.cpp

@@ -0,0 +1,289 @@
+/*!
+ * \file src/utils/Quaternion.cpp
+ * \brief Quaternion
+ *
+ * \author Mongoose
+ */
+
+#include <math.h>
+
+#include "utils/Quaternion.h"
+
+Quaternion::Quaternion() {
+    mW = 0;
+    mX = 0;
+    mY = 0;
+    mZ = 0;
+}
+
+Quaternion::Quaternion(vec_t w, vec_t x, vec_t y, vec_t z) {
+    mW = w;
+    mX = x;
+    mY = y;
+    mZ = z;
+}
+
+Quaternion::Quaternion(vec4_t v) {
+    mW = v[0];
+    mX = v[1];
+    mY = v[2];
+    mZ = v[3];
+}
+
+void Quaternion::getMatrix(matrix_t m) {
+    m[ 0] = 1.0f - 2.0f * (mY*mY + mZ*mZ);
+    m[ 1] = 2.0f * (mX*mY - mW*mZ);
+    m[ 2] = 2.0f * (mX*mZ + mW*mY);
+    m[ 3] = 0.0f;
+
+    m[ 4] = 2.0f * (mX*mY + mW*mZ);
+    m[ 5] = 1.0f - 2.0f * (mX*mX + mZ*mZ);
+    m[ 6] = 2.0f * (mY*mZ - mW*mX);
+    m[ 7] = 0.0f;
+
+    m[ 8] = 2.0f * (mX*mZ - mW*mY);
+    m[ 9] = 2.0f * (mY*mZ + mW*mX);
+    m[10] = 1.0f - 2.0f * (mX*mX + mY*mY);
+    m[11] = 0.0f;
+
+    m[12] = 0.0f;
+    m[13] = 0.0f;
+    m[14] = 0.0f;
+    m[15] = 1.0f;
+}
+
+Quaternion Quaternion::operator *(const Quaternion &q) {
+    return multiply(*this, q);
+}
+
+Quaternion Quaternion::operator /(const Quaternion &q) {
+    return divide(*this, q);
+}
+
+Quaternion Quaternion::operator +(const Quaternion &q) {
+    return add(*this, q);
+}
+
+Quaternion Quaternion::operator -(const Quaternion &q) {
+    return subtract(*this, q);
+}
+
+bool Quaternion::operator ==(const Quaternion &q) {
+    //return (mX == q.mX && mY == q.mY && mZ == q.mZ && mW == q.mW);
+    return (equalEpsilon(mX, q.mX) && equalEpsilon(mY, q.mY) &&
+            equalEpsilon(mZ, q.mZ) && equalEpsilon(mW, q.mW));
+}
+
+Quaternion Quaternion::conjugate() {
+    return Quaternion(mW, -mX, -mY, -mZ);
+}
+
+Quaternion Quaternion::scale(vec_t s) {
+    return Quaternion(mW * s, mX * s, mY * s, mZ * s);
+}
+
+Quaternion Quaternion::inverse() {
+    return conjugate().scale(1/magnitude());
+}
+
+vec_t Quaternion::dot(Quaternion a, Quaternion b) {
+    return ((a.mW * b.mW) + (a.mX * b.mX) + (a.mY * b.mY) + (a.mZ * b.mZ));
+}
+
+vec_t Quaternion::magnitude() {
+    return sqrtf(dot(*this, *this));
+}
+
+void Quaternion::setIdentity() {
+    mW = 1.0;
+    mX = 0.0;
+    mY = 0.0;
+    mZ = 0.0;
+}
+
+void Quaternion::set(vec_t angle, vec_t x, vec_t y, vec_t z) {
+    vec_t temp, dist;
+
+    // Normalize
+    temp = x*x + y*y + z*z;
+
+    dist = 1.0f / sqrtf(temp);
+
+    x *= dist;
+    y *= dist;
+    z *= dist;
+
+    mX = x;
+    mY = y;
+    mZ = z;
+
+    mW = cosf(angle / 2.0f);
+}
+
+void Quaternion::normalize() {
+    vec_t dist, square;
+
+    square = mX * mX + mY * mY + mZ * mZ + mW * mW;
+
+    if (square > 0.0) {
+        dist = 1.0f / sqrtf(square);
+    } else {
+        dist = 1;
+    }
+
+    mX *= dist;
+    mY *= dist;
+    mZ *= dist;
+    mW *= dist;
+}
+
+void Quaternion::copy(Quaternion q) {
+    mW = q.mW;
+    mX = q.mX;
+    mY = q.mY;
+    mZ = q.mZ;
+}
+
+Quaternion Quaternion::slerp(Quaternion a, Quaternion b, vec_t time) {
+    /*******************************************************************
+     * Spherical Linear Interpolation algorthim
+     *-----------------------------------------------------------------
+     *
+     * Interpolate between A and B rotations ( Find qI )
+     *
+     * qI = (((qB . qA)^ -1)^ Time) qA
+     *
+     * http://www.magic-software.com/Documentation/quat.pdf
+     *
+     * Thanks to digiben for algorithms and basis of the notes in
+     * this func
+     *
+     *******************************************************************/
+
+    vec_t result, scaleA, scaleB;
+    Quaternion i;
+
+
+    // Don't bother if it's the same rotation, it's the same as the result
+    if (a == b)
+        return a;
+
+    // A . B
+    result = dot(a, b);
+
+    // If the dot product is less than 0, the angle is greater than 90 degrees
+    if (result < 0.0f) {
+        // Negate quaternion B and the result of the dot product
+        b = Quaternion(-b.mW, -b.mX, -b.mY, -b.mZ);
+        result = -result;
+    }
+
+    // Set the first and second scale for the interpolation
+    scaleA = 1 - time;
+    scaleB = time;
+
+    // Next, we want to actually calculate the spherical interpolation.  Since this
+    // calculation is quite computationally expensive, we want to only perform it
+    // if the angle between the 2 quaternions is large enough to warrant it.  If the
+    // angle is fairly small, we can actually just do a simpler linear interpolation
+    // of the 2 quaternions, and skip all the complex math.  We create a "delta" value
+    // of 0.1 to say that if the cosine of the angle (result of the dot product) between
+    // the 2 quaternions is smaller than 0.1, then we do NOT want to perform the full on
+    // interpolation using.  This is because you won't really notice the difference.
+
+    // Check if the angle between the 2 quaternions was big enough
+    // to warrant such calculations
+    if (1 - result > 0.1f) {
+        // Get the angle between the 2 quaternions, and then
+        // store the sin() of that angle
+        vec_t theta = (float)acos(result);
+        vec_t sinTheta = (float)sin(theta);
+
+        // Calculate the scale for qA and qB, according to
+        // the angle and it's sine value
+        scaleA = (float)sin((1 - time) * theta) / sinTheta;
+        scaleB = (float)sin((time * theta)) / sinTheta;
+    }
+
+    // Calculate the x, y, z and w values for the quaternion by using a special
+    // form of linear interpolation for quaternions.
+    return (a.scale(scaleA) + b.scale(scaleB));
+}
+
+void Quaternion::setByMatrix(matrix_t matrix) {
+    float diagonal = matrix[0] + matrix[5] + matrix[10] + 1.0f;
+    float scale = 0.0f;
+    float w = 0.0f, x = 0.0f, y = 0.0f, z = 0.0f;
+
+    if (diagonal > 0.00000001) {
+        // Calculate the scale of the diagonal
+        scale = (float)(sqrt(diagonal) * 2);
+
+        w = 0.25f * scale;
+        x = (matrix[9] - matrix[6]) / scale;
+        y = (matrix[2] - matrix[8]) / scale;
+        z = (matrix[4] - matrix[1]) / scale;
+    } else {
+        // If the first element of the diagonal is the greatest value
+        if (matrix[0] > matrix[5] && matrix[0] > matrix[10]) {
+            // Find the scale according to the first element, and double it
+            scale = (float)sqrt(1.0f + matrix[0] - matrix[5] - matrix[10])*2.0f;
+
+            // Calculate the quaternion
+            w = (matrix[9] - matrix[6]) / scale;
+            x = 0.25f * scale;
+            y = (matrix[4] + matrix[1]) / scale;
+            z = (matrix[2] + matrix[8]) / scale;
+        } else if (matrix[5] > matrix[10]) {
+            // The second element of the diagonal is the greatest value
+            // Find the scale according to the second element, and double it
+            scale = (float)sqrt(1.0f + matrix[5] - matrix[0] - matrix[10])*2.0f;
+
+            // Calculate the quaternion
+            w = (matrix[2] - matrix[8]) / scale;
+            x = (matrix[4] + matrix[1]) / scale;
+            y = 0.25f * scale;
+            z = (matrix[9] + matrix[6]) / scale;
+        } else { // The third element of the diagonal is the greatest value
+            // Find the scale according to the third element, and double it
+            scale = (float)sqrt(1.0f + matrix[10] - matrix[0] - matrix[5])*2.0f;
+
+            // Calculate the quaternion
+            w = (matrix[4] - matrix[1]) / scale;
+            x = (matrix[2] + matrix[8]) / scale;
+            y = (matrix[9] + matrix[6]) / scale;
+            z = 0.25f * scale;
+        }
+    }
+
+    mW = w;
+    mX = x;
+    mY = y;
+    mZ = z;
+}
+
+Quaternion Quaternion::multiply(Quaternion a, Quaternion b) {
+    return Quaternion(a.mW * b.mW - a.mX * b.mX - a.mY * b.mY - a.mZ * b.mZ,
+            a.mW * b.mX + a.mX * b.mW + a.mY * b.mZ - a.mZ * b.mY,
+            a.mW * b.mY + a.mY * b.mW + a.mZ * b.mX - a.mX * b.mZ,
+            a.mW * b.mZ + a.mZ * b.mW + a.mX * b.mY - a.mY * b.mX);
+}
+
+Quaternion Quaternion::divide(Quaternion a, Quaternion b) {
+    return (a * (b.inverse()));
+}
+
+Quaternion Quaternion::add(Quaternion a, Quaternion b) {
+    return Quaternion(a.mW + b.mW,
+            a.mX + b.mX,
+            a.mY + b.mY,
+            a.mZ + b.mZ);
+}
+
+Quaternion Quaternion::subtract(Quaternion a, Quaternion b) {
+    return Quaternion(a.mW - b.mW,
+            a.mX - b.mX,
+            a.mY - b.mY,
+            a.mZ - b.mZ);
+}
+