CUVec4.h

//
//  CUVec4.h
//  Cornell University Game Library (CUGL)
//
//  This module provides support for a 4d vector.  It has support for basic
//  arithmetic, as well as conversions to color formats.  It also has
//  homogenous vector support for Vec3.
//
//  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: 5/30/16
 
#ifndef __CU_VEC4_H__
#define __CU_VEC4_H__
 
#include <math.h>
#include <functional>
#include <cugl/util/CUDebug.h>
#include "CUMathBase.h"
#include "CURect.h"
 
namespace cugl {
 
// Forward reference for conversions
class Color4;
class Color4f;
class Vec2;
class Vec3;
    
class Vec4 {
 
#pragma mark Values
public:
#if defined CU_MATH_VECTOR_SSE
    union {
        __attribute__((__aligned__(16))) struct {
            float x;
            float y;
            float z;
            float w;
        };
        __m128 v;
    };
#elif defined CU_MATH_VECTOR_NEON64
    union {
        __attribute__((__aligned__(16))) struct {
            float x;
            float y;
            float z;
            float w;
        };
        float32x4_t v;
    };
#else
 
    float x;
    float y;
    float z;
    float w;
#endif
    
    static const Vec4 ZERO;
    static const Vec4 ONE;
    static const Vec4 UNIT_X;
    static const Vec4 UNIT_Y;
    static const Vec4 UNIT_Z;
    static const Vec4 UNIT_W;
 
    // Homogenous constants
    static const Vec4 HOMOG_ORIGIN;
    static const Vec4 HOMOG_X;
    static const Vec4 HOMOG_Y;
    static const Vec4 HOMOG_Z;
    
#pragma mark -
#pragma mark Constructors
public:
    Vec4() : x(0), y(0), z(0), w(0) {}
    
    Vec4(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
    }
    
    Vec4(const float* array) {
        std::memcpy(this, array, 4*sizeof(float));
    }
    
    Vec4(const Vec4& p1, const Vec4& p2) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_sub_ps(p2.v,p1.v);
    #elif defined CU_MATH_VECTOR_NEON64
        v = vsubq_f32(p2.v,p1.v);
    #else
        x = p2.x-p1.x; y = p2.y-p1.y; z = p2.z-p1.z; w = p2.w-p1.w;
    #endif
    }
    
    
#pragma mark -
#pragma mark Setters
 
    Vec4& operator=(const float* array) {
        return set(array);
    }
    
    Vec4& 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;
    }
    
    Vec4& set(const float* array) {
        std::memcpy(this, array, 4*sizeof(float));
        return *this;
    }
    
    Vec4& set(const Vec4& v) {
    #if defined CU_MATH_VECTOR_SSE || defined CU_MATH_VECTOR_NEON64
        this->v = v.v;
    #else
        x = v.x; y = v.y; z = v.z; w = v.w;
    #endif
        return *this;
    }
    
    Vec4& set(const Vec4& p1, const Vec4& p2) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_sub_ps(p2.v,p1.v);
    #elif defined CU_MATH_VECTOR_NEON64
        v = vsubq_f32(p2.v,p1.v);
    #else
        x = p2.x-p1.x; y = p2.y-p1.y; z = p2.z-p1.z; w = p2.w-p1.w;
    #endif
        return *this;
    }
    
    Vec4& 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 Vec4* clamp(const Vec4& v, const Vec4& min, const Vec4& max, Vec4* dst);
    
    static float angle(const Vec4& v1, const Vec4& v2);
    
    static Vec4* add(const Vec4& v1, const Vec4& v2, Vec4* dst);
    
    static Vec4* subtract(const Vec4& v1, const Vec4& v2, Vec4* dst);
 
    static Vec4* scale(const Vec4& v, float s, Vec4* dst);
    
    static Vec4* scale(const Vec4& v1, const Vec4& v2, Vec4* dst);
 
    static Vec4* divide(const Vec4& v, float s, Vec4* dst);
    
    static Vec4* divide(const Vec4& v1, const Vec4& v2, Vec4* dst);
 
    static Vec4* reciprocate(const Vec4& v, Vec4* dst);
    
    static Vec4* negate(const Vec4& v, Vec4* dst);
    
    
#pragma mark -
#pragma mark Arithmetic
 
    Vec4& clamp(const Vec4& min, const Vec4& max);
    
    Vec4 getClamp(const Vec4& min, const Vec4& max) const {
        return Vec4(clampf(x,min.x,max.x), clampf(y,min.y,max.y),
                    clampf(z,min.z,max.z), clampf(w,min.w,max.w));
    }
    
    Vec4& add(const Vec4& v) {
    #if defined CU_MATH_VECTOR_SSE
        this->v = _mm_add_ps(this->v,v.v);
    #elif defined CU_MATH_VECTOR_NEON64
        this->v = vaddq_f32(this->v,v.v);
    #else
        x += v.x; y += v.y; z += v.z;  w += v.w;
    #endif
        return *this;
    }
    
    Vec4& add(float x, float y, float z, float w) {
    #if defined CU_MATH_VECTOR_SSE
        this->v = _mm_add_ps(this->v,_mm_set_ps(w, z, y, x));
    #elif defined CU_MATH_VECTOR_NEON64
        float32x4_t tmp = {x, y, z, w};
        this->v = vaddq_f32(this->v,tmp);
    #else
        this->x += x; this->y += y; this->z += z; this->w += w;
    #endif
        return *this;
    }
    
    Vec4& subtract(const Vec4& v) {
    #if defined CU_MATH_VECTOR_SSE
        this->v = _mm_sub_ps(this->v,v.v);
    #elif defined CU_MATH_VECTOR_NEON64
        this->v = vsubq_f32(this->v,v.v);
    #else
        x -= v.x; y -= v.y; z -= v.z;  w -= v.w;
    #endif
        return *this;
    }
    
    Vec4& subtract(float x, float y, float z, float w) {
    #if defined CU_MATH_VECTOR_SSE
        this->v = _mm_sub_ps(this->v,_mm_set_ps(w, z, y, x));
    #elif defined CU_MATH_VECTOR_NEON64
        float32x4_t tmp = {x, y, z, w};
        this->v = vsubq_f32(this->v,tmp);
    #else
        this->x -= x; this->y -= y; this->z -= z; this->w -= w;
    #endif
        return *this;
    }
    
    Vec4& 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;
    }
    
    Vec4& scale(float sx, float sy, float sz, float sw) {
    #if defined CU_MATH_VECTOR_SSE
        v = _mm_mul_ps(v,_mm_set_ps(sw, sz, sy, sx));
    #elif defined CU_MATH_VECTOR_NEON64
        float32x4_t tmp = {sx, sy, sz, sw};
        v = vmulq_f32(v,tmp);
    #else
        x *= sx; y *= sy; z *= sz; w *= sw;
    #endif
        return *this;
    }
    
    Vec4& scale(const Vec4& v) {
    #if defined CU_MATH_VECTOR_SSE
        this->v = _mm_mul_ps(this->v,v.v);
    #elif defined CU_MATH_VECTOR_NEON64
        this->v = vmulq_f32(this->v,v.v);
    #else
        x *= v.x; y *= v.y; z *= v.z; w *= v.w;
    #endif
        return *this;
    }
    
    Vec4& divide(float s);
    
    Vec4& divide(float sx, float sy, float sz, float sw);
    
    Vec4& divide(const Vec4& v);
    
    Vec4& 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;
    }
 
    Vec4& reciprocate();
    
    Vec4 getNegation() const {
        Vec4 result(*this);
        return result.negate();
    }
 
    Vec4 getReciprocal() const {
        Vec4 result(*this);
        return result.reciprocate();
    }
    
    Vec4& map(std::function<float(float)> func) {
        x = func(x); y = func(y); z = func(z); w = func(w);
        return *this;
    }
    
    Vec4 getMap(std::function<float(float)> func) const {
        return Vec4(func(x), func(y), func(z), func(w));
    }
    
    
#pragma mark -
#pragma mark Operators
 
    Vec4& operator+=(const Vec4& v) {
        return add(v);
    }
    
    Vec4& operator-=(const Vec4& v) {
        return subtract(v);
    }
    
    Vec4& operator*=(float s) {
        return scale(s);
    }
    
    Vec4& operator*=(const Vec4& v) {
        return scale(v);
    }
    
    Vec4& operator/=(float s) {
        return divide(s);
    }
    
    Vec4& operator/=(const Vec4& v) {
        return divide(v);
    }
    
    const Vec4 operator+(const Vec4& v) const {
        Vec4 result(*this);
        return result.add(v);
    }
    
    const Vec4 operator-(const Vec4& v) const  {
        Vec4 result(*this);
        return result.subtract(v);
    }
    
    const Vec4 operator-() const {
        Vec4 result(*this);
        return result.negate();
    }
    
    const Vec4 operator*(float s) const {
        Vec4 result(*this);
        return result.scale(s);
    }
    
    const Vec4 operator*(const Vec4& v) const {
        Vec4 result(*this);
        return result.scale(v);
    }
    
    const Vec4 operator/(float s) const {
        Vec4 result(*this);
        return result.divide(s);
    }
    
    const Vec4 operator/(const Vec4& v) const {
        Vec4 result(*this);
        return result.divide(v);
    }
    
    
#pragma mark -
#pragma mark Comparisons
 
    bool operator<(const Vec4& v) const;
    
    bool operator<=(const Vec4& v) const;
    
    bool operator>(const Vec4& v) const;
    
    bool operator>=(const Vec4& v) const;
    
    bool operator==(const Vec4& v) const;
    
    bool operator!=(const Vec4& v) const;
    
    bool under(const Vec4& v) const;
    
    bool over(const Vec4& v) const;
    
    bool equals(const Vec4& v, float variance=CU_MATH_EPSILON) const;
    
#pragma mark -
#pragma mark Linear Attributes
 
    bool isZero() const;
    
    bool isNearZero(float variance=CU_MATH_EPSILON) const {
        return equals(ZERO);
    }
    
    bool isOne() const;
 
    bool isInvertible() const;
    
    bool isUnit(float variance=CU_MATH_EPSILON) const {
        float dot = length()-1.0f;
        return fabsf(dot) <= variance;
    }
    
    bool isHomogenous() const {
        return w == 1.0f;
    }
    
    float getAngle(const Vec4& other) const;
    
    float distance(const Vec4& v) const {
        return sqrt(distanceSquared(v));
    }
    
    float distanceSquared(const Vec4& v) const;
    
    float length() const {
        return sqrt(lengthSquared());
    }
    
    float lengthSquared() const {
    #if defined CU_MATH_VECTOR_SSE
        return _mm_dp_ps(v,v,0xF1)[0];
    #else
        return x*x+y*y+z*z+w*w;
    #endif
    }
    
    
#pragma mark -
#pragma mark Linear Algebra
 
    float dot(const Vec4& v) const {
        return Vec4::dot(*this,v);
    }
    
    Vec4 cross(const Vec4& v) {
        Vec4 result;
        Vec4::cross(*this,v,&result);
        return result;
    }
 
    Vec4& normalize();
    
    Vec4 getNormalization() const {
        Vec4 result(*this);
        return result.normalize();
    }
    
    Vec4 getMidpoint(const Vec4& other) const {
        Vec4 result;
        midpoint(*this,other, &result);
        return result;
    }
    
    Vec4& project(const Vec4& other) {
        *this = getProjection(other);
        return *this;
    }
    
    Vec4 getProjection(const Vec4& other) const {
        return other * (dot(other)/other.dot(other));
    }
    
    Vec4& homogenize() {
        float dw = (w == 0 ? 1 : 1/w);
        *this *= dw; w = 1;
        return *this;
    }
    
    Vec4 getHomogenized() {
        Vec4 copy(*this);
        copy.homogenize();
        return copy;
    }
    
    Vec4& lerp(const Vec4& other, float alpha) {
        *this *= (1.f - alpha);
        return *this += other * alpha;
    }
    
    Vec4 getLerp(const Vec4& other, float alpha) {
        return *this * (1.f - alpha) + other * alpha;
    }
    
    void smooth(const Vec4& target, float elapsed, float response);
    
    
#pragma mark -
#pragma mark Static Linear Algebra
 
    static float dot(const Vec4& v1, const Vec4& v2);
 
    static Vec4* cross(const Vec4& v1, const Vec4& v2, Vec4* dst);
 
    static Vec4* normalize(const Vec4& v, Vec4* dst);
    
    static Vec4* homogenize(const Vec4& v, Vec4* dst);
    
    static Vec4* midpoint(const Vec4& v1, const Vec4& v2, Vec4* dst);
    
    static Vec4* project(const Vec4& v1, const Vec4& v2, Vec4* dst);
    
    static Vec4* lerp(const Vec4& v1, const Vec4& v2, float alpha, Vec4* dst);
 
    
#pragma mark -
#pragma mark Conversion Methods
public:
    std::string toString(bool verbose = false) const;
    
    operator std::string() const { return toString(); }
    
    operator Color4() const;
    
    explicit Vec4(Color4 color);
    
    Vec4& operator= (Color4 color);
    
    operator Color4f() const;
    
    explicit Vec4(const Color4f& color);
    
    Vec4& operator= (const Color4f& color);
    
    operator Vec2() const;
    
    explicit Vec4(const Vec2& v);
    
    Vec4& operator= (const Vec2& size);
    
    operator Vec3() const;
    
    explicit Vec4(const Vec3& v);
 
    Vec4(const Vec3& v, float w);
 
    Vec4& operator= (const Vec3& v);
    
    Vec4& set(const Vec3& v, float w);
};
    
    
#pragma mark -
#pragma mark Friend Operations
 
    inline const Vec4 operator*(float x, const Vec4& v) {
        Vec4 result(v);
        return result.scale(x);
    }
 
}
 
#endif /* __CU_VEC4_H__ */