Leap C++ API Reference  0.7.9
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 {
48  Vector() :
49  x(0), y(0), z(0) {}
50 
52  Vector(float _x, float _y, float _z) :
53  x(_x), y(_y), z(_z) {}
54 
56  Vector(const Vector& vector) :
57  x(vector.x), y(vector.y), z(vector.z) {}
58 
60  static const Vector& zero() {
61  static Vector s_zero(0, 0, 0);
62  return s_zero;
63  }
64 
66  static const Vector& xAxis() {
67  static Vector s_xAxis(1, 0, 0);
68  return s_xAxis;
69  }
71  static const Vector& yAxis() {
72  static Vector s_yAxis(0, 1, 0);
73  return s_yAxis;
74  }
76  static const Vector& zAxis() {
77  static Vector s_zAxis(0, 0, 1);
78  return s_zAxis;
79  }
80 
82  static const Vector& left() {
83  static Vector s_left(-1, 0, 0);
84  return s_left;
85  }
87  static const Vector& right() {
88  return xAxis();
89  }
91  static const Vector& down() {
92  static Vector s_down(0, -1, 0);
93  return s_down;
94  }
96  static const Vector& up() {
97  return yAxis();
98  }
100  static const Vector& forward() {
101  static Vector s_forward(0, 0, -1);
102  return s_forward;
103  }
105  static const Vector& backward() {
106  return zAxis();
107  }
108 
117  float magnitude() const {
118  return std::sqrt(x*x + y*y + z*z);
119  }
120 
126  float magnitudeSquared() const {
127  return x*x + y*y + z*z;
128  }
129 
137  float distanceTo(const Vector& other) const {
138  return std::sqrt((x - other.x)*(x - other.x) +
139  (y - other.y)*(y - other.y) +
140  (z - other.z)*(z - other.z));
141  }
142 
158  float angleTo(const Vector& other) const {
159  float denom = this->magnitudeSquared() * other.magnitudeSquared();
160  if (denom <= 0.0f) {
161  return 0.0f;
162  }
163  return std::acos(this->dot(other) / std::sqrt(denom));
164  }
165 
179  float pitch() const {
180  return std::atan2(y, -z);
181  }
182 
196  float yaw() const {
197  return std::atan2(x, -z);
198  }
199 
218  float roll() const {
219  return std::atan2(x, -y);
220  }
221 
233  float dot(const Vector& other) const {
234  return (x * other.x) + (y * other.y) + (z * other.z);
235  }
236 
250  Vector cross(const Vector& other) const {
251  return Vector((y * other.z) - (z * other.y),
252  (z * other.x) - (x * other.z),
253  (x * other.y) - (y * other.x));
254  }
255 
265  Vector normalized() const {
266  float denom = this->magnitudeSquared();
267  if (denom <= 0.0f) {
268  return Vector::zero();
269  }
270  denom = 1.0f / std::sqrt(denom);
271  return Vector(x * denom, y * denom, z * denom);
272  }
273 
279  Vector operator-() const {
280  return Vector(-x, -y, -z);
281  }
282 
284  Vector operator+(const Vector& other) const {
285  return Vector(x + other.x, y + other.y, z + other.z);
286  }
287 
289  Vector operator-(const Vector& other) const {
290  return Vector(x - other.x, y - other.y, z - other.z);
291  }
292 
294  Vector operator*(float scalar) const {
295  return Vector(x * scalar, y * scalar, z * scalar);
296  }
297 
299  Vector operator/(float scalar) const {
300  return Vector(x / scalar, y / scalar, z / scalar);
301  }
302 
304 #if !defined(SWIG)
305  friend Vector operator*(float scalar, const Vector& vector) {
306  return Vector(vector.x * scalar, vector.y * scalar, vector.z * scalar);
307  }
308 #endif
309 
311  Vector& operator+=(const Vector& other) {
312  x += other.x;
313  y += other.y;
314  z += other.z;
315  return *this;
316  }
317 
319  Vector& operator-=(const Vector& other) {
320  x -= other.x;
321  y -= other.y;
322  z -= other.z;
323  return *this;
324  }
325 
327  Vector& operator*=(float scalar) {
328  x *= scalar;
329  y *= scalar;
330  z *= scalar;
331  return *this;
332  }
333 
335  Vector& operator/=(float scalar) {
336  x /= scalar;
337  y /= scalar;
338  z /= scalar;
339  return *this;
340  }
341 
343  std::string toString() const {
344  std::stringstream result;
345  result << "(" << x << ", " << y << ", " << z << ")";
346  return result.str();
347  }
349  friend std::ostream& operator<<(std::ostream& out, const Vector& vector) {
350  return out << vector.toString();
351  }
352 
354  bool operator==(const Vector& other) const {
355  return x == other.x && y == other.y && z == other.z;
356  }
358  bool operator!=(const Vector& other) const {
359  return x != other.x || y != other.y || z != other.z;
360  }
361 
366  bool isValid() const {
367  return (x <= FLT_MAX && x >= -FLT_MAX) &&
368  (y <= FLT_MAX && y >= -FLT_MAX) &&
369  (z <= FLT_MAX && z >= -FLT_MAX);
370  }
371 
378  float operator[](unsigned int index) const {
379  return index < 3 ? (&x)[index] : 0.0f;
380  }
381 
383  const float* toFloatPointer() const {
384  return &x; /* Note: Assumes x, y, z are aligned in memory. */
385  }
386 
393  template<typename Vector3Type>
394  const Vector3Type toVector3() const {
395  return Vector3Type(x, y, z);
396  }
397 
406  template<typename Vector4Type>
407  const Vector4Type toVector4(float w=0.0f) const {
408  return Vector4Type(x, y, z, w);
409  }
410 
412  float x;
414  float y;
416  float z;
417 };
418 
419 
425 struct FloatArray {
427  float& operator[] (unsigned int index) {
428  return m_array[index];
429  }
430 
432  operator float* () {
433  return m_array;
434  }
435 
437  operator const float* () const {
438  return m_array;
439  }
440 
442  float m_array[16];
443 };
444 
455 struct Matrix
456 {
458  Matrix() :
459  xBasis(1, 0, 0),
460  yBasis(0, 1, 0),
461  zBasis(0, 0, 1),
462  origin(0, 0, 0) {
463  }
464 
466  Matrix(const Matrix& other) :
467  xBasis(other.xBasis),
468  yBasis(other.yBasis),
469  zBasis(other.zBasis),
470  origin(other.origin) {
471  }
472 
480  Matrix(const Vector& _xBasis, const Vector& _yBasis, const Vector& _zBasis) :
481  xBasis(_xBasis),
482  yBasis(_yBasis),
483  zBasis(_zBasis),
484  origin(0, 0, 0) {
485  }
486 
495  Matrix(const Vector& _xBasis, const Vector& _yBasis, const Vector& _zBasis, const Vector& _origin) :
496  xBasis(_xBasis),
497  yBasis(_yBasis),
498  zBasis(_zBasis),
499  origin(_origin) {
500  }
501 
508  Matrix(const Vector& axis, float angleRadians) :
509  origin(0, 0, 0) {
510  setRotation(axis, angleRadians);
511  }
512 
521  Matrix(const Vector& axis, float angleRadians, const Vector& translation)
522  : origin(translation) {
523  setRotation(axis, angleRadians);
524  }
525 
531  static const Matrix& identity() {
532  static Matrix s_identity;
533  return s_identity;
534  }
535 
545  void setRotation(const Vector& _axis, float angleRadians) {
546  const Vector axis = _axis.normalized();
547  const float s = std::sin(angleRadians);
548  const float c = std::cos(angleRadians);
549  const float C = (1-c);
550 
551  xBasis = Vector(axis[0]*axis[0]*C + c, axis[0]*axis[1]*C - axis[2]*s, axis[0]*axis[2]*C + axis[1]*s);
552  yBasis = Vector(axis[1]*axis[0]*C + axis[2]*s, axis[1]*axis[1]*C + c, axis[1]*axis[2]*C - axis[0]*s);
553  zBasis = Vector(axis[2]*axis[0]*C - axis[1]*s, axis[2]*axis[1]*C + axis[0]*s, axis[2]*axis[2]*C + c);
554  }
555 
565  Vector transformPoint(const Vector& in) const {
566  return xBasis*in.x + yBasis*in.y + zBasis*in.z + origin;
567  }
568 
576  Vector transformDirection(const Vector& in) const {
577  return xBasis*in.x + yBasis*in.y + zBasis*in.z;
578  }
579 
590  Matrix rotInverse = Matrix(Vector(xBasis[0], yBasis[0], zBasis[0]),
591  Vector(xBasis[1], yBasis[1], zBasis[1]),
592  Vector(xBasis[2], yBasis[2], zBasis[2]));
593  rotInverse.origin = rotInverse.transformDirection( -origin );
594  return rotInverse;
595  }
596 
606  Matrix operator*(const Matrix& other) const {
607  return Matrix(transformDirection(other.xBasis),
608  transformDirection(other.yBasis),
609  transformDirection(other.zBasis),
610  transformPoint(other.origin));
611  }
612 
614  Matrix& operator*=(const Matrix& other) {
615  return (*this) = (*this) * other;
616  }
617 
619  bool operator==(const Matrix& other) const {
620  return xBasis == other.xBasis &&
621  yBasis == other.yBasis &&
622  zBasis == other.zBasis &&
623  origin == other.origin;
624  }
626  bool operator!=(const Matrix& other) const {
627  return xBasis != other.xBasis ||
628  yBasis != other.yBasis ||
629  zBasis != other.zBasis ||
630  origin != other.origin;
631  }
632 
641  template<typename Matrix3x3Type>
642  const Matrix3x3Type toMatrix3x3() const {
643  return Matrix3x3Type(xBasis.x, xBasis.y, xBasis.z,
644  yBasis.x, yBasis.y, yBasis.z,
645  zBasis.x, zBasis.y, zBasis.z);
646  }
647 
654  template<typename Matrix4x4Type>
655  const Matrix4x4Type toMatrix4x4() const {
656  return Matrix4x4Type(xBasis.x, xBasis.y, xBasis.z, 0.0f,
657  yBasis.x, yBasis.y, yBasis.z, 0.0f,
658  zBasis.x, zBasis.y, zBasis.z, 0.0f,
659  origin.x, origin.y, origin.z, 1.0f);
660  }
661 
670  template<typename T>
671  T* toArray3x3(T* output) const {
672  output[0] = xBasis.x; output[1] = xBasis.y; output[2] = xBasis.z;
673  output[3] = yBasis.x; output[4] = yBasis.y; output[5] = yBasis.z;
674  output[6] = zBasis.x; output[7] = zBasis.y; output[8] = zBasis.z;
675  return output;
676  }
677 
686  FloatArray output;
687  toArray3x3((float*)output);
688  return output;
689  }
690 
697  template<typename T>
698  T* toArray4x4(T* output) const {
699  output[0] = xBasis.x; output[1] = xBasis.y; output[2] = xBasis.z; output[3] = 0.0f;
700  output[4] = yBasis.x; output[5] = yBasis.y; output[6] = yBasis.z; output[7] = 0.0f;
701  output[8] = zBasis.x; output[9] = zBasis.y; output[10] = zBasis.z; output[11] = 0.0f;
702  output[12] = origin.x; output[13] = origin.y; output[14] = origin.z; output[15] = 1.0f;
703  return output;
704  }
705 
712  FloatArray output;
713  toArray4x4((float*)output);
714  return output;
715  }
716 
718  std::string toString() const {
719  std::stringstream result;
720  result << "xBasis:" << xBasis.toString() << " yBasis:" << yBasis.toString()
721  << " zBasis:" << zBasis.toString() << " origin:" << origin.toString();
722  return result.str();
723  }
724 
726  friend std::ostream& operator<<(std::ostream& out, const Matrix& matrix) {
727  return out << matrix.toString();
728  }
729 
738 };
739 
740 }; // namespace Leap
741 
742 #endif // __LeapMath_h__