LeapMath.h

Go to the documentation of this file.

/******************************************************************************\
* Copyright (C) 2012-2013 Leap Motion, Inc. All rights reserved.               *
* Leap Motion proprietary and confidential. Not for distribution.              *
* Use subject to the terms of the Leap Motion SDK Agreement available at       *
* https://developer.leapmotion.com/sdk_agreement, or another agreement         *
* between Leap Motion and you, your company or other organization.             *
\******************************************************************************/

#if !defined(__LeapMath_h__)
#define __LeapMath_h__

#include <cmath>
#include <iostream>
#include <sstream>
#include <float.h>
#include <algorithm>

namespace Leap {

static const float PI          = 3.1415926536f;
static const float DEG_TO_RAD  = 0.0174532925f;
static const float RAD_TO_DEG  = 57.295779513f;

static const float EPSILON = 1.192092896e-07f;

struct Vector {
  Vector() :
    x(0), y(0), z(0) {}

  Vector(float _x, float _y, float _z) :
    x(_x), y(_y), z(_z) {}

  Vector(const Vector& vector) :
    x(vector.x), y(vector.y), z(vector.z) {}

  static const Vector& zero() {
    static Vector s_zero(0, 0, 0);
    return s_zero;
  }

  static const Vector& xAxis() {
    static Vector s_xAxis(1, 0, 0);
    return s_xAxis;
  }
  static const Vector& yAxis() {
    static Vector s_yAxis(0, 1, 0);
    return s_yAxis;
  }
  static const Vector& zAxis() {
    static Vector s_zAxis(0, 0, 1);
    return s_zAxis;
  }

  static const Vector& left() {
    static Vector s_left(-1, 0, 0);
    return s_left;
  }
  static const Vector& right() {
    return xAxis();
  }
  static const Vector& down() {
    static Vector s_down(0, -1, 0);
    return s_down;
  }
  static const Vector& up() {
    return yAxis();
  }
  static const Vector& forward() {
    static Vector s_forward(0, 0, -1);
    return s_forward;
  }
  static const Vector& backward() {
    return zAxis();
  }

  float magnitude() const {
    return std::sqrt(x*x + y*y + z*z);
  }

  float magnitudeSquared() const {
    return x*x + y*y + z*z;
  }

  float distanceTo(const Vector& other) const {
    return std::sqrt((x - other.x)*(x - other.x) +
                     (y - other.y)*(y - other.y) +
                     (z - other.z)*(z - other.z));
  }

  float angleTo(const Vector& other) const {
    float denom = this->magnitudeSquared() * other.magnitudeSquared();
    if (denom <= EPSILON) {
      return 0.0f;
    }
    float val = this->dot(other) / std::sqrt(denom);
    if (val >= 1.0f) {
      return 0.0f;
    } else if (val <= -1.0f) {
      return PI;
    }
    return std::acos(val);
  }

  float pitch() const {
    return std::atan2(y, -z);
  }

  float yaw() const {
    return std::atan2(x, -z);
  }

  float roll() const {
    return std::atan2(x, -y);
  }

  float dot(const Vector& other) const {
    return (x * other.x) + (y * other.y) + (z * other.z);
  }

  Vector cross(const Vector& other) const {
    return Vector((y * other.z) - (z * other.y),
                  (z * other.x) - (x * other.z),
                  (x * other.y) - (y * other.x));
  }

  Vector normalized() const {
    float denom = this->magnitudeSquared();
    if (denom <= EPSILON) {
      return Vector::zero();
    }
    denom = 1.0f / std::sqrt(denom);
    return Vector(x * denom, y * denom, z * denom);
  }

  Vector operator-() const {
    return Vector(-x, -y, -z);
  }

  Vector operator+(const Vector& other) const {
    return Vector(x + other.x, y + other.y, z + other.z);
  }

  Vector operator-(const Vector& other) const {
    return Vector(x - other.x, y - other.y, z - other.z);
  }

  Vector operator*(float scalar) const {
    return Vector(x * scalar, y * scalar, z * scalar);
  }

  Vector operator/(float scalar) const {
    return Vector(x / scalar, y / scalar, z / scalar);
  }

#if !defined(SWIG)

  friend Vector operator*(float scalar, const Vector& vector) {
    return Vector(vector.x * scalar, vector.y * scalar, vector.z * scalar);
  }
#endif

  Vector& operator+=(const Vector& other) {
    x += other.x;
    y += other.y;
    z += other.z;
    return *this;
  }

  Vector& operator-=(const Vector& other) {
    x -= other.x;
    y -= other.y;
    z -= other.z;
    return *this;
  }

  Vector& operator*=(float scalar) {
    x *= scalar;
    y *= scalar;
    z *= scalar;
    return *this;
  }

  Vector& operator/=(float scalar) {
    x /= scalar;
    y /= scalar;
    z /= scalar;
    return *this;
  }

  std::string toString() const {
    std::stringstream result;
    result << "(" << x << ", " << y << ", " << z << ")";
    return result.str();
  }
  friend std::ostream& operator<<(std::ostream& out, const Vector& vector) {
    return out << vector.toString();
  }

  bool operator==(const Vector& other) const {
    return x == other.x && y == other.y && z == other.z;
  }
  bool operator!=(const Vector& other) const {
    return x != other.x || y != other.y || z != other.z;
  }

  bool isValid() const {
    return (x <= FLT_MAX && x >= -FLT_MAX) &&
           (y <= FLT_MAX && y >= -FLT_MAX) &&
           (z <= FLT_MAX && z >= -FLT_MAX);
  }

  float operator[](unsigned int index) const {
    return index < 3 ? (&x)[index] : 0.0f;
  }

  const float* toFloatPointer() const {
    return &x; /* Note: Assumes x, y, z are aligned in memory. */
  }

  template<typename Vector3Type>
  const Vector3Type toVector3() const {
    return Vector3Type(x, y, z);
  }

  template<typename Vector4Type>
  const Vector4Type toVector4(float w=0.0f) const {
    return Vector4Type(x, y, z, w);
  }

  float x;
  float y;
  float z;
};


struct FloatArray {
  float& operator[] (unsigned int index) {
    return m_array[index];
  }

  operator float* () {
    return m_array;
  }

  operator const float* () const {
    return m_array;
  }

  float m_array[16];
};

struct Matrix
{
  Matrix() :
    xBasis(1, 0, 0),
    yBasis(0, 1, 0),
    zBasis(0, 0, 1),
    origin(0, 0, 0) {
  }

  Matrix(const Matrix& other) :
    xBasis(other.xBasis),
    yBasis(other.yBasis),
    zBasis(other.zBasis),
    origin(other.origin) {
  }

  Matrix(const Vector& _xBasis, const Vector& _yBasis, const Vector& _zBasis) :
    xBasis(_xBasis),
    yBasis(_yBasis),
    zBasis(_zBasis),
    origin(0, 0, 0) {
  }

  Matrix(const Vector& _xBasis, const Vector& _yBasis, const Vector& _zBasis, const Vector& _origin) :
    xBasis(_xBasis),
    yBasis(_yBasis),
    zBasis(_zBasis),
    origin(_origin) {
  }

  Matrix(const Vector& axis, float angleRadians) :
    origin(0, 0, 0) {
    setRotation(axis, angleRadians);
  }

  Matrix(const Vector& axis, float angleRadians, const Vector& translation)
    : origin(translation) {
    setRotation(axis, angleRadians);
  }

  static const Matrix& identity() {
    static Matrix s_identity;
    return s_identity;
  }

  void setRotation(const Vector& axis, float angleRadians) {
    const Vector n = axis.normalized();
    const float s = std::sin(angleRadians);
    const float c = std::cos(angleRadians);
    const float C = (1-c);

    xBasis = Vector(n[0]*n[0]*C + c,      n[0]*n[1]*C - n[2]*s, n[0]*n[2]*C + n[1]*s);
    yBasis = Vector(n[1]*n[0]*C + n[2]*s, n[1]*n[1]*C + c,      n[1]*n[2]*C - n[0]*s);
    zBasis = Vector(n[2]*n[0]*C - n[1]*s, n[2]*n[1]*C + n[0]*s, n[2]*n[2]*C + c     );
  }

  Vector transformPoint(const Vector& in) const {
    return xBasis*in.x + yBasis*in.y + zBasis*in.z + origin;
  }

  Vector transformDirection(const Vector& in) const {
    return xBasis*in.x + yBasis*in.y + zBasis*in.z;
  }

  Matrix rigidInverse() const {
    Matrix rotInverse = Matrix(Vector(xBasis[0], yBasis[0], zBasis[0]),
                               Vector(xBasis[1], yBasis[1], zBasis[1]),
                               Vector(xBasis[2], yBasis[2], zBasis[2]));
    rotInverse.origin = rotInverse.transformDirection( -origin );
    return rotInverse;
  }

  Matrix operator*(const Matrix& other) const {
    return Matrix(transformDirection(other.xBasis),
                  transformDirection(other.yBasis),
                  transformDirection(other.zBasis),
                  transformPoint(other.origin));
  }

  Matrix& operator*=(const Matrix& other) {
    return (*this) = (*this) * other;
  }

  bool operator==(const Matrix& other) const {
    return xBasis == other.xBasis &&
           yBasis == other.yBasis &&
           zBasis == other.zBasis &&
           origin == other.origin;
  }
  bool operator!=(const Matrix& other) const {
    return xBasis != other.xBasis ||
           yBasis != other.yBasis ||
           zBasis != other.zBasis ||
           origin != other.origin;
  }

  template<typename Matrix3x3Type>
  const Matrix3x3Type toMatrix3x3() const {
    return Matrix3x3Type(xBasis.x, xBasis.y, xBasis.z,
                         yBasis.x, yBasis.y, yBasis.z,
                         zBasis.x, zBasis.y, zBasis.z);
  }

  template<typename Matrix4x4Type>
  const Matrix4x4Type toMatrix4x4() const {
    return Matrix4x4Type(xBasis.x, xBasis.y, xBasis.z, 0.0f,
                         yBasis.x, yBasis.y, yBasis.z, 0.0f,
                         zBasis.x, zBasis.y, zBasis.z, 0.0f,
                         origin.x, origin.y, origin.z, 1.0f);
  }

  template<typename T>
  T* toArray3x3(T* output) const {
    output[0] = xBasis.x; output[1] = xBasis.y; output[2] = xBasis.z;
    output[3] = yBasis.x; output[4] = yBasis.y; output[5] = yBasis.z;
    output[6] = zBasis.x; output[7] = zBasis.y; output[8] = zBasis.z;
    return output;
  }

  FloatArray toArray3x3() const {
    FloatArray output;
    toArray3x3((float*)output);
    return output;
  }

  template<typename T>
  T* toArray4x4(T* output) const {
    output[0]  = xBasis.x; output[1]  = xBasis.y; output[2]  = xBasis.z; output[3]  = 0.0f;
    output[4]  = yBasis.x; output[5]  = yBasis.y; output[6]  = yBasis.z; output[7]  = 0.0f;
    output[8]  = zBasis.x; output[9]  = zBasis.y; output[10] = zBasis.z; output[11] = 0.0f;
    output[12] = origin.x; output[13] = origin.y; output[14] = origin.z; output[15] = 1.0f;
    return output;
  }

  FloatArray toArray4x4() const {
    FloatArray output;
    toArray4x4((float*)output);
    return output;
  }

  std::string toString() const {
    std::stringstream result;
    result << "xBasis:" << xBasis.toString() << " yBasis:" << yBasis.toString()
           << " zBasis:" << zBasis.toString() << " origin:" << origin.toString();
    return result.str();
  }

  friend std::ostream& operator<<(std::ostream& out, const Matrix& matrix) {
    return out << matrix.toString();
  }

  Vector xBasis;
  Vector yBasis;
  Vector zBasis;
  Vector origin;
};

}; // namespace Leap

#endif // __LeapMath_h__