VecMath.h

#pragma once

#include <algorithm>
#include <iosfwd>
#include <cstdint>
#include <cmath>

namespace HandTrackingClient
{

template <typename T>
class Vector3
{
public:
    T x;

    T y;

    T z;

    Vector3<T>() : x(0), y(0), z(0) {}

    Vector3<T>(T x_in, T y_in, T z_in) : x(x_in), y(y_in), z(z_in) {}

    template <typename T2>
    Vector3<T2> cast() const { return Vector3<T2> ((T2) x, (T2) y, (T2) z); }

    Vector3<T>& operator+= (const Vector3<T>& rhs)     { x += rhs.x; y += rhs.y; z += rhs.z; return *this; }

    Vector3<T>& operator-= (const Vector3<T>& rhs)     { x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this; }

    Vector3<T> operator-() const                       { return Vector3<T>(-x, -y, -z); }

    Vector3<T>& operator*= (const T rhs)               { x *= rhs; y *= rhs; z *= rhs; return *this; }

    Vector3<T>& operator/= (const T rhs)               { x /= rhs; y /= rhs; z /= rhs; return *this; }

    T dot (const Vector3<T>& rhs) const                { return x*rhs.x + y*rhs.y + z*rhs.z; }

    Vector3<T> cross (const Vector3<T>& rhs) const;

    bool operator== (const Vector3<T>& rhs) const      { return (x == rhs.x) && (y == rhs.y) && (z == rhs.z); }

    bool operator!= (const Vector3<T>& rhs) const      { return !(*this == rhs); }

    T squaredNorm() const                              { return x*x + y*y + z*z; }

    T lInfNorm() const                                 { return std::max (std::max (std::abs(x), std::abs(y)), std::abs(z)); }

    T norm() const                                     { return std::sqrt(squaredNorm()); }

    void normalize();

    Vector3<T> normalized() const                      { Vector3<T> result(*this); result.normalize(); return result; }
};

template <typename T>
Vector3<T> operator* (const T lhs, const Vector3<T>& rhs)                  { Vector3<T> result(rhs); result *= lhs; return result; }

template <typename T>
inline Vector3<T> operator* (const Vector3<T>& lhs, const T rhs)           { Vector3<T> result(lhs); result *= rhs; return result; }

template <typename T>
inline Vector3<T> operator/ (const Vector3<T>& lhs, const T rhs)           { Vector3<T> result(lhs); result /= rhs; return result; }

template <typename T>
inline Vector3<T> operator+ (const Vector3<T>& lhs, const Vector3<T>& rhs) { Vector3<T> result(lhs); result += rhs; return result; }

template <typename T>
inline Vector3<T> operator- (const Vector3<T>& lhs, const Vector3<T>& rhs) { Vector3<T> result(lhs); result -= rhs; return result; }

template <typename T>
std::ostream& operator<< (std::ostream& os, const Vector3<T>& rhs);

typedef Vector3<float> Vector3f;
typedef Vector3<double> Vector3d;

template <typename T>
class Quaternion
{
public:
    Vector3<T> v;

    T w;

    Quaternion<T>() : v(0, 0, 0), w(1) {}

    Quaternion<T>(T x_in, T y_in, T z_in, T w_in) 
        : v(x_in, y_in, z_in), w(w_in) {}

    Quaternion<T>(const Vector3<T>& v_in, T w_in)
        : v(v_in), w(w_in) {}

    template <typename T2>
    Quaternion<T2> cast() { return Quaternion<T2> (v.template cast<T2>, (T2) w); }

    static Quaternion<T> fromAxisAngle (const Vector3<T> axis, const T angle);

    Quaternion<T> conjugate() const;

    Vector3<T> rotate (const Vector3<T>& v) const;

    void normalize();

    Quaternion<T> normalized() const { Quaternion<T> result (*this); result.normalize(); return result; }

    bool operator== (const Quaternion<T>& rhs) const    { return (v == rhs.v) && (w == rhs.w); }

    bool operator!= (const Quaternion<T>& rhs) const    { return !(*this == rhs); }
};

template <typename T>
Quaternion<T> operator* (const Quaternion<T>& a, const Quaternion<T>& b)
{
    // From Wikipedia:
    // http://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
    return Quaternion<T> (a.w*b.v + b.w*a.v + a.v.cross(b.v),
                          a.w*b.w - a.v.dot(b.v));
}


template <typename T>
std::ostream& operator<< (std::ostream& os, const Quaternion<T>& rhs);

typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;

template <typename T>
class Transform
{
public:
    Quaternion<T> rotation;

    Vector3<T> translation;

    Transform (const Quaternion<T>& rotation_in, const Vector3<T>& translation_in)
        : rotation (rotation_in), translation (translation_in) {}

    Transform() {}

    void invert();

    Transform<T> inverse() const { Transform<T> result(*this); result.invert(); return result; }
};

template <typename T>
Transform<T> operator* (const Transform<T>& lhs, const Transform<T>& rhs)
{
    // [ R_1 t_1 ] * [ R_2 t_2 ] = [ R_1*R_2  R_1*t_2 + t_1 ]
    // [  0   1  ]   [  0   1  ]   [    0           1       ]
    return Transform<T> (lhs.rotation * rhs.rotation, 
                         lhs.translation + lhs.rotation.rotate(rhs.translation));
}

template <typename T>
Vector3<T> operator* (const Transform<T>& lhs, const Vector3<T>& rhs)
{
    return lhs.translation + lhs.rotation.rotate (rhs);
}

typedef Transform<float> Transformf;
typedef Transform<double> Transformd;

template <typename T>
class Matrix4
{
public:
    T data[16];

    Matrix4() { std::fill(data, data+16, T(0)); }

    static Matrix4 identity();

    Matrix4(Quaternion<T> q, Vector3<T> t);

    const T& operator() (unsigned iRow, unsigned jCol) const { return data[jCol * 4 + iRow]; }

    T& operator() (unsigned iRow, unsigned jCol)             { return data[jCol * 4 + iRow]; }
};

typedef Matrix4<float> Matrix4f;
typedef Matrix4<double> Matrix4d;

} // namespace HandTrackingClient