Leap Motion C++ API Reference  0.8.0
LeapMath.h
1 /******************************************************************************\
2 * Copyright (C) 2012-2013 Leap Motion, Inc. All rights reserved. *
3 * Leap Motion proprietary and confidential. Not for distribution. *
4 * Use subject to the terms of the Leap Motion SDK Agreement available at *
5 * https://developer.leapmotion.com/sdk_agreement, or another agreement *
6 * between Leap Motion and you, your company or other organization. *
7 \******************************************************************************/
8 
9 #if !defined(__LeapMath_h__)
10 #define __LeapMath_h__
11 
12 #include <cmath>
13 #include <iostream>
14 #include <sstream>
15 #include <float.h>
16 
17 namespace Leap {
18 
20 static const float PI = 3.1415926536f;
25 static const float DEG_TO_RAD = 0.0174532925f;
30 static const float RAD_TO_DEG = 57.295779513f;
31 
46 struct Vector {
50  Vector() :
51  x(0), y(0), z(0) {}
52 
58  Vector(float _x, float _y, float _z) :
59  x(_x), y(_y), z(_z) {}
60 
66  Vector(const Vector& vector) :
67  x(vector.x), y(vector.y), z(vector.z) {}
68 
74  static const Vector& zero() {
75  static Vector s_zero(0, 0, 0);
76  return s_zero;
77  }
78 
84  static const Vector& xAxis() {
85  static Vector s_xAxis(1, 0, 0);
86  return s_xAxis;
87  }
93  static const Vector& yAxis() {
94  static Vector s_yAxis(0, 1, 0);
95  return s_yAxis;
96  }
102  static const Vector& zAxis() {
103  static Vector s_zAxis(0, 0, 1);
104  return s_zAxis;
105  }
106 
112  static const Vector& left() {
113  static Vector s_left(-1, 0, 0);
114  return s_left;
115  }
121  static const Vector& right() {
122  return xAxis();
123  }
129  static const Vector& down() {
130  static Vector s_down(0, -1, 0);
131  return s_down;
132  }
138  static const Vector& up() {
139  return yAxis();
140  }
146  static const Vector& forward() {
147  static Vector s_forward(0, 0, -1);
148  return s_forward;
149  }
155  static const Vector& backward() {
156  return zAxis();
157  }
158 
169  float magnitude() const {
170  return std::sqrt(x*x + y*y + z*z);
171  }
172 
180  float magnitudeSquared() const {
181  return x*x + y*y + z*z;
182  }
183 
193  float distanceTo(const Vector& other) const {
194  return std::sqrt((x - other.x)*(x - other.x) +
195  (y - other.y)*(y - other.y) +
196  (z - other.z)*(z - other.z));
197  }
198 
216  float angleTo(const Vector& other) const {
217  float denom = this->magnitudeSquared() * other.magnitudeSquared();
218  if (denom <= 0.0f) {
219  return 0.0f;
220  }
221  return std::acos(this->dot(other) / std::sqrt(denom));
222  }
223 
239  float pitch() const {
240  return std::atan2(y, -z);
241  }
242 
258  float yaw() const {
259  return std::atan2(x, -z);
260  }
261 
282  float roll() const {
283  return std::atan2(x, -y);
284  }
285 
299  float dot(const Vector& other) const {
300  return (x * other.x) + (y * other.y) + (z * other.z);
301  }
302 
318  Vector cross(const Vector& other) const {
319  return Vector((y * other.z) - (z * other.y),
320  (z * other.x) - (x * other.z),
321  (x * other.y) - (y * other.x));
322  }
323 
335  Vector normalized() const {
336  float denom = this->magnitudeSquared();
337  if (denom <= 0.0f) {
338  return Vector::zero();
339  }
340  denom = 1.0f / std::sqrt(denom);
341  return Vector(x * denom, y * denom, z * denom);
342  }
343 
351  Vector operator-() const {
352  return Vector(-x, -y, -z);
353  }
354 
360  Vector operator+(const Vector& other) const {
361  return Vector(x + other.x, y + other.y, z + other.z);
362  }
363 
369  Vector operator-(const Vector& other) const {
370  return Vector(x - other.x, y - other.y, z - other.z);
371  }
372 
378  Vector operator*(float scalar) const {
379  return Vector(x * scalar, y * scalar, z * scalar);
380  }
381 
387  Vector operator/(float scalar) const {
388  return Vector(x / scalar, y / scalar, z / scalar);
389  }
390 
391 #if !defined(SWIG)
392 
397  friend Vector operator*(float scalar, const Vector& vector) {
398  return Vector(vector.x * scalar, vector.y * scalar, vector.z * scalar);
399  }
400 #endif
401 
403  Vector& operator+=(const Vector& other) {
404  x += other.x;
405  y += other.y;
406  z += other.z;
407  return *this;
408  }
409 
411  Vector& operator-=(const Vector& other) {
412  x -= other.x;
413  y -= other.y;
414  z -= other.z;
415  return *this;
416  }
417 
419  Vector& operator*=(float scalar) {
420  x *= scalar;
421  y *= scalar;
422  z *= scalar;
423  return *this;
424  }
425 
427  Vector& operator/=(float scalar) {
428  x /= scalar;
429  y /= scalar;
430  z /= scalar;
431  return *this;
432  }
433 
435  std::string toString() const {
436  std::stringstream result;
437  result << "(" << x << ", " << y << ", " << z << ")";
438  return result.str();
439  }
441  friend std::ostream& operator<<(std::ostream& out, const Vector& vector) {
442  return out << vector.toString();
443  }
444 
450  bool operator==(const Vector& other) const {
451  return x == other.x && y == other.y && z == other.z;
452  }
458  bool operator!=(const Vector& other) const {
459  return x != other.x || y != other.y || z != other.z;
460  }
461 
468  bool isValid() const {
469  return (x <= FLT_MAX && x >= -FLT_MAX) &&
470  (y <= FLT_MAX && y >= -FLT_MAX) &&
471  (z <= FLT_MAX && z >= -FLT_MAX);
472  }
473 
482  float operator[](unsigned int index) const {
483  return index < 3 ? (&x)[index] : 0.0f;
484  }
485 
491  const float* toFloatPointer() const {
492  return &x; /* Note: Assumes x, y, z are aligned in memory. */
493  }
494 
501  template<typename Vector3Type>
502  const Vector3Type toVector3() const {
503  return Vector3Type(x, y, z);
504  }
505 
514  template<typename Vector4Type>
515  const Vector4Type toVector4(float w=0.0f) const {
516  return Vector4Type(x, y, z, w);
517  }
518 
520  float x;
522  float y;
524  float z;
525 };
526 
527 
533 struct FloatArray {
535  float& operator[] (unsigned int index) {
536  return m_array[index];
537  }
538 
540  operator float* () {
541  return m_array;
542  }
543 
545  operator const float* () const {
546  return m_array;
547  }
548 
550  float m_array[16];
551 };
552 
563 struct Matrix
564 {
566  Matrix() :
567  xBasis(1, 0, 0),
568  yBasis(0, 1, 0),
569  zBasis(0, 0, 1),
570  origin(0, 0, 0) {
571  }
572 
574  Matrix(const Matrix& other) :
575  xBasis(other.xBasis),
576  yBasis(other.yBasis),
577  zBasis(other.zBasis),
578  origin(other.origin) {
579  }
580 
588  Matrix(const Vector& _xBasis, const Vector& _yBasis, const Vector& _zBasis) :
589  xBasis(_xBasis),
590  yBasis(_yBasis),
591  zBasis(_zBasis),
592  origin(0, 0, 0) {
593  }
594 
603  Matrix(const Vector& _xBasis, const Vector& _yBasis, const Vector& _zBasis, const Vector& _origin) :
604  xBasis(_xBasis),
605  yBasis(_yBasis),
606  zBasis(_zBasis),
607  origin(_origin) {
608  }
609 
616  Matrix(const Vector& axis, float angleRadians) :
617  origin(0, 0, 0) {
618  setRotation(axis, angleRadians);
619  }
620 
629  Matrix(const Vector& axis, float angleRadians, const Vector& translation)
630  : origin(translation) {
631  setRotation(axis, angleRadians);
632  }
633 
639  static const Matrix& identity() {
640  static Matrix s_identity;
641  return s_identity;
642  }
643 
653  void setRotation(const Vector& axis, float angleRadians) {
654  const Vector n = axis.normalized();
655  const float s = std::sin(angleRadians);
656  const float c = std::cos(angleRadians);
657  const float C = (1-c);
658 
659  xBasis = Vector(n[0]*n[0]*C + c, n[0]*n[1]*C - n[2]*s, n[0]*n[2]*C + n[1]*s);
660  yBasis = Vector(n[1]*n[0]*C + n[2]*s, n[1]*n[1]*C + c, n[1]*n[2]*C - n[0]*s);
661  zBasis = Vector(n[2]*n[0]*C - n[1]*s, n[2]*n[1]*C + n[0]*s, n[2]*n[2]*C + c );
662  }
663 
673  Vector transformPoint(const Vector& in) const {
674  return xBasis*in.x + yBasis*in.y + zBasis*in.z + origin;
675  }
676 
684  Vector transformDirection(const Vector& in) const {
685  return xBasis*in.x + yBasis*in.y + zBasis*in.z;
686  }
687 
698  Matrix rotInverse = Matrix(Vector(xBasis[0], yBasis[0], zBasis[0]),
699  Vector(xBasis[1], yBasis[1], zBasis[1]),
700  Vector(xBasis[2], yBasis[2], zBasis[2]));
701  rotInverse.origin = rotInverse.transformDirection( -origin );
702  return rotInverse;
703  }
704 
714  Matrix operator*(const Matrix& other) const {
715  return Matrix(transformDirection(other.xBasis),
716  transformDirection(other.yBasis),
717  transformDirection(other.zBasis),
718  transformPoint(other.origin));
719  }
720 
722  Matrix& operator*=(const Matrix& other) {
723  return (*this) = (*this) * other;
724  }
725 
727  bool operator==(const Matrix& other) const {
728  return xBasis == other.xBasis &&
729  yBasis == other.yBasis &&
730  zBasis == other.zBasis &&
731  origin == other.origin;
732  }
734  bool operator!=(const Matrix& other) const {
735  return xBasis != other.xBasis ||
736  yBasis != other.yBasis ||
737  zBasis != other.zBasis ||
738  origin != other.origin;
739  }
740 
749  template<typename Matrix3x3Type>
750  const Matrix3x3Type toMatrix3x3() const {
751  return Matrix3x3Type(xBasis.x, xBasis.y, xBasis.z,
752  yBasis.x, yBasis.y, yBasis.z,
753  zBasis.x, zBasis.y, zBasis.z);
754  }
755 
762  template<typename Matrix4x4Type>
763  const Matrix4x4Type toMatrix4x4() const {
764  return Matrix4x4Type(xBasis.x, xBasis.y, xBasis.z, 0.0f,
765  yBasis.x, yBasis.y, yBasis.z, 0.0f,
766  zBasis.x, zBasis.y, zBasis.z, 0.0f,
767  origin.x, origin.y, origin.z, 1.0f);
768  }
769 
778  template<typename T>
779  T* toArray3x3(T* output) const {
780  output[0] = xBasis.x; output[1] = xBasis.y; output[2] = xBasis.z;
781  output[3] = yBasis.x; output[4] = yBasis.y; output[5] = yBasis.z;
782  output[6] = zBasis.x; output[7] = zBasis.y; output[8] = zBasis.z;
783  return output;
784  }
785 
794  FloatArray output;
795  toArray3x3((float*)output);
796  return output;
797  }
798 
805  template<typename T>
806  T* toArray4x4(T* output) const {
807  output[0] = xBasis.x; output[1] = xBasis.y; output[2] = xBasis.z; output[3] = 0.0f;
808  output[4] = yBasis.x; output[5] = yBasis.y; output[6] = yBasis.z; output[7] = 0.0f;
809  output[8] = zBasis.x; output[9] = zBasis.y; output[10] = zBasis.z; output[11] = 0.0f;
810  output[12] = origin.x; output[13] = origin.y; output[14] = origin.z; output[15] = 1.0f;
811  return output;
812  }
813 
820  FloatArray output;
821  toArray4x4((float*)output);
822  return output;
823  }
824 
826  std::string toString() const {
827  std::stringstream result;
828  result << "xBasis:" << xBasis.toString() << " yBasis:" << yBasis.toString()
829  << " zBasis:" << zBasis.toString() << " origin:" << origin.toString();
830  return result.str();
831  }
832 
834  friend std::ostream& operator<<(std::ostream& out, const Matrix& matrix) {
835  return out << matrix.toString();
836  }
837 
846 };
847 
848 }; // namespace Leap
849 
850 #endif // __LeapMath_h__