CUQuaternion.h

//
//  CUQuaternion.h
//  Cornell University Game Library (CUGL)
//
//  This module provides support for a quaternion, which is a way of representing
//  rotations in 3d sapce..  It has support for basic arithmetic, as well as
//  as the standard quaternion interpolations.
//
//  Because math objects are intended to be on the stack, we do not provide
//  any shared pointer support in this class.
//
//  This module is based on an original file from GamePlay3D: http://gameplay3d.org.
//  It has been modified to support the CUGL framework.
//
//  CUGL MIT License:
//      This software is provided 'as-is', without any express or implied
//      warranty.  In no event will the authors be held liable for any damages
//      arising from the use of this software.
//
//      Permission is granted to anyone to use this software for any purpose,
//      including commercial applications, and to alter it and redistribute it
//      freely, subject to the following restrictions:
//
//      1. The origin of this software must not be misrepresented; you must not
//      claim that you wrote the original software. If you use this software
//      in a product, an acknowledgment in the product documentation would be
//      appreciated but is not required.
//
//      2. Altered source versions must be plainly marked as such, and must not
//      be misrepresented as being the original software.
//
//      3. This notice may not be removed or altered from any source distribution.
//
//  Author: Walker White
//  Version: 6/5/16
#ifndef __CU_QUATERNION_H__
#define __CU_QUATERNION_H__

#include <math.h>
#include "CUVec3.h"
#include "CUMathBase.h"

namespace cugl {
    
// Forward declarations
class Mat4;

class Quaternion {

#pragma mark Values
public:
#if defined CU_MATH_VECTOR_SSE
    union {
        struct {
            float x;
            float y;
            float z;
            float w;
        };
        __m128 v;
    };
#elif defined CU_MATH_VECTOR_NEON64
    union {
        struct {
            float x;
            float y;
            float z;
            float w;
        };
        float32x4_t v;
    };
#else

    float x;
    float y;
    float z;
    float w;
#endif
    
    static const Quaternion ZERO;
    static const Quaternion IDENTITY;
    
    
#pragma mark -
#pragma mark Constructors
public:
    Quaternion() : x(0.0f), y(0.0f), z(0.0f), w(0.0f) {}
    
    Quaternion(float x, float y, float z, float w) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_set_ps(w, z, y, x);
    #elif defined CU_MATH_VECTOR_NEON64
        v = {x, y, z, w};
    #else
        this->x = x; this->y = y; this->z = z; this->w = w;
    #endif
    }
    
    Quaternion(float* array);
    
    Quaternion(const Vec3& axis, float angle);

    
#pragma mark -
#pragma mark Static Constructors

    static Quaternion* createFromRotationMatrix(const Mat4& m, Quaternion* dst);
    
    static Quaternion* createFromAxisAngle(const Vec3& axis, float angle, Quaternion* dst);
    

#pragma mark -
#pragma mark Setters

    Quaternion& operator=(const float* array) {
        return set(array);
    }
    
    Quaternion& set(float x, float y, float z, float w) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_set_ps(w, z, y, x);
    #elif defined CU_MATH_VECTOR_NEON64
        v = {x, y, z, w};
    #else
        this->x = x; this->y = y; this->z = z; this->w = w;
    #endif
        return *this;
    }
    
    Quaternion& set(const float* array);
    
    Quaternion& set(const Vec3& axis, float angle);
    
    Quaternion& set(const Quaternion& q) {
    #if defined CU_MATH_VECTOR_SSE || defined CU_MATH_VECTOR_NEON64
        v = q.v;
    #else
        x = q.x; y = q.y; z = q.z; w = q.w;
    #endif
        return *this;
    }
    
    Quaternion& setIdentity() {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_setzero_ps();
        v[3] = 1.0f;
    #elif defined CU_MATH_VECTOR_NEON64
        v = vmovq_n_f32(0.0f);
        v[3] = 1.0f;
    #else
        x = y = z = 0; w = 1;
    #endif
        return *this;
    }

    Quaternion& setZero() {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_setzero_ps();
    #elif defined CU_MATH_VECTOR_NEON64
        v = vmovq_n_f32(0.0f);
    #else
        x = y = z = w = 0;
    #endif
        return *this;
    }
    
    
#pragma mark -
#pragma mark Static Arithmetic

    static Quaternion* add(const Quaternion& q1, const Quaternion& q2, Quaternion* dst);

    static Quaternion* subtract(const Quaternion& q1, const Quaternion& q2, Quaternion* dst);

    static Quaternion* multiply(const Quaternion& q1, const Quaternion& q2, Quaternion* dst);

    static Quaternion* divide(const Quaternion& q1, const Quaternion& q2, Quaternion* dst);

    static Quaternion* scale(const Quaternion& q1, float s, Quaternion* dst);

    static Quaternion* conjugate(const Quaternion& quat, Quaternion* dst);

    static Quaternion* invert(const Quaternion& quat, Quaternion* dst);

    static Quaternion* normalize(const Quaternion& quat, Quaternion* dst);

    static Quaternion* negate(const Quaternion& quat, Quaternion* dst);

    static float dot(const Quaternion& q1, const Quaternion& q2);
    
    
#pragma mark -
#pragma mark Arithmetic

    Quaternion& add(const Quaternion& q) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_add_ps(v,q.v);
    #elif defined CU_MATH_VECTOR_NEON64
        v = vaddq_f32(v,q.v);
    #else
        x += q.x; y += q.y; z += q.z;  w += q.w;
    #endif
        return *this;
    }

    Quaternion& subtract(const Quaternion& q) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_sub_ps(v,q.v);
    #elif defined CU_MATH_VECTOR_NEON64
        v = vsubq_f32(v,q.v);
    #else
        x -= q.x; y -= q.y; z -= q.z; w -= q.w;
    #endif
        return *this;
    }
    
    Quaternion& multiply(const Quaternion& q) {
        return *(multiply(*this,q,this));
    }

    
    Quaternion& divide(const Quaternion& q) {
        return *(divide(*this,q,this));
    }

    
    Quaternion& scale(float s) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_mul_ps(v,_mm_set1_ps(s));
    #elif defined CU_MATH_VECTOR_NEON64
        v = vmulq_n_f32(v,(float32_t)s);
    #else
        x *= s; y *= s; z *= s; w *= s;
    #endif
        return *this;
    }

    Quaternion& conjugate() {
    #if defined CU_MATH_VECTOR_SSE
        float tmp = w;
        v = _mm_sub_ps(_mm_setzero_ps(), v);
        w = tmp;
    #elif defined CU_MATH_VECTOR_NEON64
        float tmp = w;
        v = vnegq_f32(v);
        w = tmp;
    #else
        x = -x; y = -y; z = -z;
    #endif
        return *this;
  }
    
    Quaternion getConjugate() const {
        Quaternion result;
        return *(conjugate(*this,&result));
    }
    
    Quaternion& invert() {
        return *(invert(*this,this));
    }
    
    Quaternion getInverse() const {
        Quaternion result;
        return *(invert(*this,&result));
    }

    Quaternion& normalize() {
        return *(normalize(*this,this));
    }
    
    Quaternion getNormalization() const {
        Quaternion result;
        return *(normalize(*this,&result));
    }
    
    Quaternion& negate() {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_sub_ps(_mm_setzero_ps(), v);
    #elif defined CU_MATH_VECTOR_NEON64
        v = vsubq_f32(vmovq_n_f32(0.0f), v);
    #else
        x = -x; y = -y; z = -z; w = -w;
    #endif
        return *this;
    }
    
    Quaternion getNegation() const {
        Quaternion result;
        return *(negate(*this,&result));
    }
    
    float dot(const Quaternion& q) const {
        return dot(*this,q);
    }

    
    
#pragma mark -
#pragma mark Operators

    Quaternion& operator+=(const Quaternion& q) {
        return add(q);
    }
    
    Quaternion& operator-=(const Quaternion& q) {
        return subtract(q);
    }
    
    Quaternion& operator*=(float s) {
        return scale(s);
    }
    
    Quaternion& operator*=(const Quaternion& q) {
        return *(multiply(*this,q,this));
    }
    
    Quaternion& operator/=(float s) {
        return *(scale(*this,1.0f/s,this));
    }
    
    Quaternion& operator/=(const Quaternion& q) {
        return *(divide(*this,q,this));
    }
    
    const Quaternion operator+(const Quaternion& q) const {
        Quaternion result;
        return *(add(*this,q,&result));
    }
    
    const Quaternion operator-(const Quaternion& q) const  {
        Quaternion result;
        return *(subtract(*this,q,&result));
    }
    
    const Quaternion operator-() const {
        Quaternion result;
        return *(negate(*this,&result));
    }
    
    const Quaternion operator*(float s) const {
        Quaternion result;
        return *(scale(*this,s,&result));
    }
    
    const Quaternion operator*(const Quaternion& q) const {
        Quaternion result;
        return *(multiply(*this,q,&result));
    }
    
    const Quaternion operator/(float s) const {
        Quaternion result;
        return *(scale(*this,1.0f/s,&result));
    }
    
    const Quaternion operator/(const Quaternion& q) const {
        Quaternion result;
        return *(divide(*this,q,&result));
    }

    
#pragma mark -
#pragma mark Comparisons

    bool operator==(const Quaternion& q) const;
    
    bool operator!=(const Quaternion& q) const;
    
    bool equals(const Quaternion& q, float variance=CU_MATH_EPSILON) const;

#pragma mark -
#pragma mark Linear Attributes

    float norm() const {
        return sqrt(normSquared());
    }
    
    float normSquared() const {
        return dot(*this);
    }
    
    bool isZero() const;
    
    bool isNearZero(float variance=CU_MATH_EPSILON) const {
        return equals(ZERO,variance);
    }
    
    bool isIdentity() const;

    bool isNearIdentity(float variance=CU_MATH_EPSILON) const {
        return equals(IDENTITY,variance);
    }

    bool isUnit(float variance=CU_MATH_EPSILON) const {
        float dot = norm()-1.0f;
        return fabsf(dot) <= variance;
    }
    
    float toAxisAngle(Vec3* e) const;
    
    
#pragma mark -
#pragma mark Static Interpolation

    static Quaternion* lerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst);
    
    static Quaternion* slerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst);

    static Quaternion* nlerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst);
    
    static Vec3* rotate(const Vec3& v, const Quaternion& quat, Vec3* dst);
    
    
#pragma mark -
#pragma mark Interpolation

    Quaternion& lerp(const Quaternion& q, float t) {
        return *(lerp(*this,q,t,this));
    }
    
    Quaternion& slerp(const Quaternion& q, float t) {
        return *(slerp(*this,q,t,this));
    }
    
    Quaternion& nlerp(const Quaternion& q, float t) {
        return *(slerp(*this,q,t,this));
    }
    
    Quaternion getLerp(const Quaternion& q, float t) {
        Quaternion result;
        return *(lerp(*this,q,t,&result));
    }
    
    Quaternion getSlerp(const Quaternion& q, float t) {
        Quaternion result;
        return *(slerp(*this,q,t,&result));
    }
    
    Quaternion getNlerp(const Quaternion& q, float t) {
        Quaternion result;
        return *(nlerp(*this,q,t,&result));
    }
    
    Vec3 getRotation(const Vec3& v) {
        Vec3 result;
        return *(rotate(v,*this,&result));
    }
    
#pragma mark -
#pragma mark Conversion Methods
public:
    std::string toString(bool verbose = false) const;
    
    operator std::string() const { return toString(); }
    
    operator Vec4() const;
    
    explicit Quaternion(const Vec4& vector);
    
    Quaternion& operator= (const Vec4& vector) {
        return set(vector);
    }

    Quaternion& set(const Vec4& vector);
    
    operator Mat4() const;

    explicit Quaternion(const Mat4& m);

    Quaternion& operator= (const Mat4& m) {
        return set(m);
    }

    Quaternion& set(const Mat4& m);
    
};

inline Vec3& operator*=(Vec3& v, const Quaternion& quat) {
    return *(Quaternion::rotate(v,quat,&v));
}
    
inline const Vec3 operator*(const Vec3& v, const Quaternion& quat) {
    Vec3 result;
    return *(Quaternion::rotate(v,quat,&result));
}
    
inline const Quaternion operator*(float s, const Quaternion& quat) {
    Quaternion result;
    return *(Quaternion::scale(quat,s,&result));
}


}

#endif /* __CU_QUATERNION_H__ */