VMTK
vmath.h
1 /* -*- C++ -*- */
2 /*
3 * vmath, set of classes for computer graphics mathematics.
4 * Copyright (c) 2005-2014, Jan Bartipan < barzto at gmail dot com >
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * - Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * - Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in
15 * the documentation and/or other materials provided with the
16 * distribution.
17 * - Neither the names of its contributors may be used to endorse or
18 * promote products derived from this software without specific
19 * prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
31 * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 */
34 
118 // Modified 2011-06-12, Davide Bacchet (davide.bacchet at gmail dot com)
119 // added () operators with standard matrix notation (opposite wrt at() ).
120 #ifndef __vmath_Header_File__
121 #define __vmath_Header_File__
122 
123 #include <cmath>
124 #include <cstring>
125 #include <iostream>
126 #include <sstream>
127 #include <string>
128 #include <cassert>
129 #define VMATH_NAMESPACE vmath
130 
131 #ifdef VMATH_NAMESPACE
132 namespace VMATH_NAMESPACE
133 {
134 #endif
135 
136 #ifndef M_PI
137 #define M_PI 3.14159265358979323846 /* pi */
138 #endif
139 
140 #define DEG2RAD(x) ((x * M_PI) / 180.0)
141 #define RAD2DEG(x) ((x *180) / M_PI)
142 //#define EPSILON (4.37114e-07)
143 
144 const double epsilon = 4.37114e-05;
145 #define EPSILON epsilon
146 #define sinf(x) (float)sin((double)(x))
147 #define cosf(x) (float)cos((double)(x))
148 template <typename T>
149 inline T radians(T angleInRadians)
150 {
151  return angleInRadians * static_cast<T>(180.0/M_PI);
152 }
153 
154 inline bool closeEnough( float a, float b, float eps = epsilon) {
155  return (eps > abs(a - b));
156 }
157 
158 
169 template<class T>
170 class Vector2
171 {
172 public:
173  union
174  {
178  T x;
179 
184  T s;
185  };
186 
187  union
188  {
192  T y;
193 
198  T t;
199  };
200 
201  //----------------[ constructors ]--------------------------
206  : x(0), y(0)
207  {
208  }
209 
215  Vector2(T nx, T ny)
216  : x(nx), y(ny)
217  {
218  }
219 
224  Vector2(const Vector2<T>& src)
225  : x(src.x), y(src.y)
226  {
227  }
228 
229 
234  template<class FromT>
236  : x(static_cast<T>(src.x)), y(static_cast<T>(src.y))
237  {
238  }
239 
240 
241  //----------------[ access operators ]-------------------
246  template<class FromT>
248  {
249  x = static_cast<T>(rhs.x);
250  y = static_cast<T>(rhs.y);
251  return *this;
252  }
253 
259  {
260  x = rhs.x;
261  y = rhs.y;
262  return *this;
263  }
264 
271  T& operator[](int n)
272  {
273  assert(n >= 0 && n <= 1);
274  if (0 == n)
275  return x;
276  else
277  return y;
278  }
279 
286  const T& operator[](int n) const
287  {
288  assert(n >= 0 && n <= 1);
289  if (0 == n)
290  return x;
291  else
292  return y;
293  }
294 
295  //---------------[ vector aritmetic operator ]--------------
300  Vector2<T> operator+(const Vector2<T>& rhs) const
301  {
302  return Vector2<T>(x + rhs.x, y + rhs.y);
303  }
304 
309  Vector2<T> operator-(const Vector2<T>& rhs) const
310  {
311  return Vector2<T>(x - rhs.x, y - rhs.y);
312  }
313 
318  Vector2<T> operator*(const Vector2<T>& rhs) const
319  {
320  return Vector2<T>(x * rhs.x, y * rhs.y);
321  }
322 
327  Vector2<T> operator/(const Vector2<T>& rhs) const
328  {
329  return Vector2<T>(x / rhs.x, y / rhs.y);
330  }
331 
337  {
338  x += rhs.x;
339  y += rhs.y;
340  return *this;
341  }
342 
348  {
349  x -= rhs.x;
350  y -= rhs.y;
351  return *this;
352  }
353 
359  {
360  x *= rhs.x;
361  y *= rhs.y;
362  return *this;
363  }
364 
370  {
371  x /= rhs.x;
372  y /= rhs.y;
373  return *this;
374  }
375 
376  //--------------[ scalar vector operator ]--------------------
381  Vector2<T> operator+(T rhs) const
382  {
383  return Vector2<T>(x + rhs, y + rhs);
384  }
385 
390  Vector2<T> operator-(T rhs) const
391  {
392  return Vector2<T>(x - rhs, y - rhs);
393  }
394 
399  Vector2<T> operator*(T rhs) const
400  {
401  return Vector2<T>(x * rhs, y * rhs);
402  }
403 
408  Vector2<T> operator/(T rhs) const
409  {
410  return Vector2<T>(x / rhs, y / rhs);
411  }
412 
418  {
419  x += rhs;
420  y += rhs;
421  return *this;
422  }
423 
429  {
430  x -= rhs;
431  y -= rhs;
432  return *this;
433  }
434 
440  {
441  x *= rhs;
442  y *= rhs;
443  return *this;
444  }
445 
451  {
452  x /= rhs;
453  y /= rhs;
454  return *this;
455  }
456 
457 
458 
459  //--------------[ equality operator ]------------------------
467  bool operator==(const Vector2<T>& rhs) const
468  {
469  return (std::abs(x - rhs.x) < EPSILON) && (std::abs(y - rhs.y) < EPSILON);
470  }
471 
477  bool operator!=(const Vector2<T>& rhs) const
478  {
479  return !(*this == rhs);
480  }
481 
482  //-------------[ unary operations ]--------------------------
488  {
489  return Vector2<T>(-x, -y);
490  }
491 
492  //-------------[ size operations ]---------------------------
497  T length() const
498  {
499  return (T) std::sqrt(x * x + y * y);
500  }
501 
505  void normalize()
506  {
507  T s = length();
508  x /= s;
509  y /= s;
510  }
511 
512 
513  Vector2<T> vectorDirection(const Vector2<T> v1, const Vector2<T> v2) const//add
514  {
515  Vector2<T> temp=v2-v1;
516  temp.normalize();
517  //std::cout<<"temp: "<<temp.toString()<<std::endl;
518  return temp;
519  }
520 
521  Vector2<T> vectorNormal(const Vector2<T> v1, const Vector2<T> v2) const//add
522  {
523  Vector2<T> temp=vectorDirection(v1,v2);
524  return Vector2<T>(-temp.y,temp.x);
525  }
526 
527 
528 
536  T lengthSq() const
537  {
538  return x * x + y * y;
539  }
540 
541  //--------------[ misc. operations ]-----------------------
551  Vector2<T> lerp(T fact, const Vector2<T>& r) const
552  {
553  return (*this) + (r - (*this)) * fact;
554  }
555 
556  //-------------[ conversion ]-----------------------------
562  operator T*()
563  {
564  return (T*) this;
565  }
571  operator const T*() const
572  {
573  return (const T*) this;
574  }
575 
576  //-------------[ output operator ]------------------------
583  friend std::ostream& operator<<(std::ostream& lhs, const Vector2<T>& rhs)
584  {
585  lhs << "[" << rhs.x << "," << rhs.y << "]";
586  return lhs;
587  }
588 
592  std::string toString() const
593  {
594  std::ostringstream oss;
595  oss << *this;
596  return oss.str();
597  }
598 
602  void print(){ std::cout<<toString()<<std::endl; }
603 };
604 
605 //--------------------------------------
606 // Typedef shortcuts for 2D vector
607 //-------------------------------------
609 typedef class Vector2<float> Vector2f;
611 typedef class Vector2<double> Vector2d;
613 typedef class Vector2<int> Vector2i;
614 
626 template<class T>
627 class Vector3
628 {
629 public:
630  //T x, y, z;
631  union
632  {
636  T x;
637 
642  T s;
643 
648  T r;
649  };
650 
651  union
652  {
656  T y;
661  T t;
666  T g;
667  };
668 
669  union
670  {
674  T z;
675 
680  T u;
685  T b;
686  };
687 
688  //----------------[ constructors ]--------------------------
693  : x(0), y(0), z(0)
694  {
695  }
696 
703  Vector3(T nx, T ny, T nz)
704  : x(nx), y(ny), z(nz)
705  {
706  }
707 
708 
709  Vector3(const Vector2<T>& v, T nz)
710  : x(v.x), y(v.y), z(nz)
711  {
712  }
713 
718  Vector3(const Vector3<T>& src)
719  : x(src.x), y(src.y), z(src.z)
720  {
721  }
722 
727  Vector3(const T v[3]) //add
728  : x(v[0]), y(v[1]), z(v[2])
729  {
730  }
731 
736  template<class FromT>
738  : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z))
739  {
740  }
741 
742  //----------------[ access operators ]-------------------
748  {
749  x = rhs.x;
750  y = rhs.y;
751  z = rhs.z;
752  return *this;
753  }
754 
759  template<class FromT>
761  {
762  x = static_cast<T>(rhs.x);
763  y = static_cast<T>(rhs.y);
764  z = static_cast<T>(rhs.z);
765  return *this;
766  }
767 
775  T & operator[](int n)
776  {
777  assert(n >= 0 && n <= 2);
778  if (0 == n)
779  return x;
780  else if (1 == n)
781  return y;
782  else
783  return z;
784  }
785 
793  const T & operator[](int n) const
794  {
795  assert(n >= 0 && n <= 2);
796  if (0 == n)
797  return x;
798  else if (1 == n)
799  return y;
800  else
801  return z;
802  }
803 
804  //---------------[ vector arithmetic operator ]--------------
809  Vector3<T> operator+(const Vector3<T>& rhs) const
810  {
811  return Vector3<T>(x + rhs.x, y + rhs.y, z + rhs.z);
812  }
813 
818  Vector3<T> operator-(const Vector3<T>& rhs) const
819  {
820  return Vector3<T>(x - rhs.x, y - rhs.y, z - rhs.z);
821  }
822 
827  Vector3<T> operator*(const Vector3<T>& rhs) const
828  {
829  return Vector3<T>(x * rhs.x, y * rhs.y, z * rhs.z);
830  }
831 
836  Vector3<T> operator/(const Vector3<T>& rhs) const
837  {
838  return Vector3<T>(x / rhs.x, y / rhs.y, z / rhs.z);
839  }
840 
846  {
847  x += rhs.x;
848  y += rhs.y;
849  z += rhs.z;
850  return *this;
851  }
852 
858  {
859  x -= rhs.x;
860  y -= rhs.y;
861  z -= rhs.z;
862  return *this;
863  }
864 
870  {
871  x *= rhs.x;
872  y *= rhs.y;
873  z *= rhs.z;
874  return *this;
875  }
876 
882  {
883  x /= rhs.x;
884  y /= rhs.y;
885  z /= rhs.z;
886  return *this;
887  }
888 
893  T dotProduct(const Vector3<T>& rhs) const
894  {
895  return x * rhs.x + y * rhs.y + z * rhs.z;
896  }
897 
903  {
904  return Vector3<T>(y * rhs.z - rhs.y * z, z * rhs.x - rhs.z * x, x * rhs.y - rhs.x * y);
905  }
906 
907  static Vector3<T> crossProduct(Vector3<T> v1, Vector3<T> v2)// const//add
908  {
909  return Vector3<T>(v1.y * v2.z - v1.z * v2.y,
910  v1.z * v2.x - v1.x * v2.z,
911  v1.x * v2.y - v1.y * v2.x);
912  }
913 
914  Vector3<T> vectorNormal(const Vector3<T> v) const//add
915  {
916  Vector3<T> n(crossProduct(v));
917  n.normalize();
918  return n;
919  }
920 
921  static Vector3<T> vectorNormal(Vector3<T> v1, Vector3<T> v2) //add
922  {
923  Vector3<T> temp(Vector3<T>::crossProduct(v1, v2));
924  temp.normalize();
925  return temp;
926  }
927 
928  Vector3<T> vectorNormalFromThreePoints(const Vector3<T> v1, const Vector3<T> v2, const Vector3<T> v3) const//add
929  {
930  Vector3<T> temp( Vector3<T>::crossProduct((v2 - v1), (v3 - v1)) );
931  temp.normalize();
932  return temp;
933  }
934 
935  Vector3<T> mean(const Vector3<T> v1, const Vector3<T> v2) const//add
936  {
937  return (v1 + v2)/2;
938  }
939 
940  Vector3<T> mean(const Vector3<T> v1, const Vector3<T> v2, const Vector3<T> v3) const//add
941  {
942  return (v1 + v2 + v3)/3;
943  }
944 
945  T distance(const Vector3<T> v) const//add
946  {
947  return (v - *this).length();
948  }
949 
950  static T distance(Vector3<T> v1, Vector3<T> v2)// const//add
951  {
952  return (v1 - v2).length();
953  }
954 
955  //--------------[ scalar vector operator ]--------------------
960  Vector3<T> operator+(T rhs) const
961  {
962  return Vector3<T>(x + rhs, y + rhs, z + rhs);
963  }
964 
969  Vector3<T> operator-(T rhs) const
970  {
971  return Vector3<T>(x - rhs, y - rhs, z - rhs);
972  }
973 
978  Vector3<T> operator*(T rhs) const
979  {
980  return Vector3<T>(x * rhs, y * rhs, z * rhs);
981  }
982 
987  Vector3<T> operator/(T rhs) const
988  {
989  return Vector3<T>(x / rhs, y / rhs, z / rhs);
990  }
991 
997  {
998  x += rhs;
999  y += rhs;
1000  z += rhs;
1001  return *this;
1002  }
1003 
1009  {
1010  x -= rhs;
1011  y -= rhs;
1012  z -= rhs;
1013  return *this;
1014  }
1015 
1021  {
1022  x *= rhs;
1023  y *= rhs;
1024  z *= rhs;
1025  return *this;
1026  }
1027 
1033  {
1034  x /= rhs;
1035  y /= rhs;
1036  z /= rhs;
1037  return *this;
1038  }
1039 
1040  //--------------[ Equality operator ]------------------------
1048  bool operator==(const Vector3<T>& rhs) const
1049  {
1050  return std::fabs(x - rhs.x) < EPSILON && std::fabs(y - rhs.y) < EPSILON && std::fabs(z - rhs.z) < EPSILON;
1051  }
1052 
1058  bool operator!=(const Vector3<T>& rhs) const
1059  {
1060  return !(*this == rhs);
1061  }
1062 
1063  //-------------[ unary operations ]--------------------------
1069  {
1070  return Vector3<T>(-x, -y, -z);
1071  }
1072 
1073  //-------------[ size operations ]---------------------------
1078  T length() const
1079  {
1080  return (T) std::sqrt(x * x + y * y + z * z);
1081  }
1082 
1090  T lengthSq() const
1091  {
1092  return x * x + y * y + z * z;
1093  }
1094 
1098  void normalize()
1099  {
1100  T s = length();
1101  x /= s;
1102  y /= s;
1103  z /= s;
1104  }
1105 
1106  //------------[ other operations ]---------------------------
1113  void rotate(T ax, T ay, T az)
1114  {
1115  T a = cos(DEG2RAD(ax));
1116  T b = sin(DEG2RAD(ax));
1117  T c = cos(DEG2RAD(ay));
1118  T d = sin(DEG2RAD(ay));
1119  T e = cos(DEG2RAD(az));
1120  T f = sin(DEG2RAD(az));
1121  T nx = c * e * x - c * f * y + d * z;
1122  T ny = (a * f + b * d * e) * x + (a * e - b * d * f) * y - b * c * z;
1123  T nz = (b * f - a * d * e) * x + (a * d * f + b * e) * y + a * c * z;
1124  x = nx;
1125  y = ny;
1126  z = nz;
1127 
1128  }
1129 
1139  Vector3<T> lerp(T fact, const Vector3<T>& r) const
1140  {
1141  return (*this) + (r - (*this)) * fact;
1142  }
1143 
1144  //-------------[ conversion ]-----------------------------
1145 
1151  operator T*()
1152  {
1153  return (T*) this;
1154  }
1155 
1161  operator const T*() const
1162  {
1163  return (const T*) this;
1164  }
1165 
1166  //-------------[ output operator ]------------------------
1173  friend std::ostream& operator<<(std::ostream& lhs, const Vector3<T> rhs)
1174  {
1175  lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "]";
1176  return lhs;
1177  }
1178 
1182  std::string toString() const
1183  {
1184  std::ostringstream oss;
1185  oss << *this;
1186  return oss.str();
1187  }
1188 
1192  void print(){ std::cout<<toString()<<std::endl; }
1193 };
1194 
1196 typedef Vector3<float> Vector3f;
1198 typedef Vector3<double> Vector3d;
1200 typedef Vector3<int> Vector3i;
1201 
1213 template<class T>
1214 class Vector4
1215 {
1216 public:
1217 
1218  union
1219  {
1224  T r;
1228  T x;
1229  };
1230 
1231  union
1232  {
1237  T g;
1241  T y;
1242  };
1243 
1244  union
1245  {
1250  T b;
1254  T z;
1255  };
1256 
1257  union
1258  {
1263  T a;
1269  T w;
1270  };
1271 
1272  //----------------[ constructors ]--------------------------
1277  : x(0), y(0), z(0), w(0)
1278  {
1279  }
1280 
1288  Vector4(T nx, T ny, T nz, T nw)
1289  : x(nx), y(ny), z(nz), w(nw)
1290  {
1291  }
1292 
1300  Vector4(T nx, T ny, T nz) //add
1301  : x(nx), y(ny), z(nz), w(1)
1302  {
1303  }
1304 
1305  Vector4(const Vector3<T>& v, T nw) //add
1306  : x(v.x), y(v.y), z(v.z), w(nw)
1307  {
1308  }
1309 
1310  Vector4(const Vector3<T>& v) //add
1311  : x(v.x), y(v.y), z(v.z), w(1)
1312  {
1313  }
1314 
1319  Vector4(const Vector4<T>& src)
1320  : x(src.x), y(src.y), z(src.z), w(src.w)
1321  {
1322  }
1323 
1328  template<class FromT>
1330  : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z)), w(static_cast<T>(src.w))
1331  {
1332  }
1333 
1334  template <typename FromT>
1335  Vector4(const Vector3<FromT>& src, FromT w)
1336  : x(static_cast<T>(src.x)), y(static_cast<T>(src.y)), z(static_cast<T>(src.z)), w(static_cast<T>(w))
1337  {}
1338 
1339  //----------------[ access operators ]-------------------
1345  {
1346  x = rhs.x;
1347  y = rhs.y;
1348  z = rhs.z;
1349  w = rhs.w;
1350  return *this;
1351  }
1352 
1357  template<class FromT>
1359  {
1360  x = static_cast<T>(rhs.x);
1361  y = static_cast<T>(rhs.y);
1362  z = static_cast<T>(rhs.z);
1363  w = static_cast<T>(rhs.w);
1364  return *this;
1365  }
1366 
1374  T & operator[](int n)
1375  {
1376  assert(n >= 0 && n <= 3);
1377  if (0 == n)
1378  return x;
1379  else if (1 == n)
1380  return y;
1381  else if (2 == n)
1382  return z;
1383  else
1384  return w;
1385  }
1386 
1394  const T & operator[](int n) const
1395  {
1396  assert(n >= 0 && n <= 3);
1397  if (0 == n)
1398  return x;
1399  else if (1 == n)
1400  return y;
1401  else if (2 == n)
1402  return z;
1403  else
1404  return w;
1405  }
1406 
1407  //---------------[ vector aritmetic operator ]--------------
1412  Vector4<T> operator+(const Vector4<T>& rhs) const
1413  {
1414  return Vector4<T>(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
1415  }
1416 
1421  Vector4<T> operator-(const Vector4<T>& rhs) const
1422  {
1423  return Vector4<T>(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
1424  }
1425 
1431  {
1432  return Vector4<T>(x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
1433  }
1434 
1439  Vector4<T> operator/(const Vector4<T>& rhs) const
1440  {
1441  return Vector4<T>(x / rhs.x, y / rhs.y, z / rhs.z, w / rhs.w);
1442  }
1443 
1449  {
1450  x += rhs.x;
1451  y += rhs.y;
1452  z += rhs.z;
1453  w += rhs.w;
1454  return *this;
1455  }
1456 
1462  {
1463  x -= rhs.x;
1464  y -= rhs.y;
1465  z -= rhs.z;
1466  w -= rhs.w;
1467  return *this;
1468  }
1469 
1475  {
1476  x *= rhs.x;
1477  y *= rhs.y;
1478  z *= rhs.z;
1479  w *= rhs.w;
1480  return *this;
1481  }
1482 
1488  {
1489  x /= rhs.x;
1490  y /= rhs.y;
1491  z /= rhs.z;
1492  w /= rhs.w;
1493  return *this;
1494  }
1495 
1496  //--------------[ equiality operator ]------------------------
1504  bool operator==(const Vector4<T>& rhs) const
1505  {
1506  return std::fabs(x - rhs.x) < EPSILON && std::fabs(y - rhs.y) < EPSILON && std::fabs(z - rhs.z) < EPSILON
1507  && std::fabs(w - rhs.w) < EPSILON;
1508  }
1509 
1515  bool operator!=(const Vector4<T>& rhs) const
1516  {
1517  return !(*this == rhs);
1518  }
1519 
1520  //-------------[ unary operations ]--------------------------
1526  {
1527  return Vector4<T>(-x, -y, -z, -w);
1528  }
1529 
1530  //--------------[ scalar vector operator ]--------------------
1531 
1536  Vector4<T> operator+(T rhs) const
1537  {
1538  return Vector4<T>(x + rhs, y + rhs, z + rhs, w + rhs);
1539  }
1540 
1545  Vector4<T> operator-(T rhs) const
1546  {
1547  return Vector4<T>(x - rhs, y - rhs, z - rhs, w - rhs);
1548  }
1549 
1554  Vector4<T> operator*(T rhs) const
1555  {
1556  return Vector4<T>(x * rhs, y * rhs, z * rhs, w * rhs);
1557  }
1558 
1563  Vector4<T> operator/(T rhs) const
1564  {
1565  return Vector4<T>(x / rhs, y / rhs, z / rhs, w / rhs);
1566  }
1567 
1573  {
1574  x += rhs;
1575  y += rhs;
1576  z += rhs;
1577  w += rhs;
1578  return *this;
1579  }
1580 
1586  {
1587  x -= rhs;
1588  y -= rhs;
1589  z -= rhs;
1590  w -= rhs;
1591  return *this;
1592  }
1593 
1599  {
1600  x *= rhs;
1601  y *= rhs;
1602  z *= rhs;
1603  w *= rhs;
1604  return *this;
1605  }
1606 
1612  {
1613  x /= rhs;
1614  y /= rhs;
1615  z /= rhs;
1616  w /= rhs;
1617  return *this;
1618  }
1619 
1620  //-------------[ size operations ]---------------------------
1625  T length() const
1626  {
1627  return (T) std::sqrt(x * x + y * y + z * z + w * w);
1628  }
1629 
1633  void normalize()
1634  {
1635  T s = length();
1636  x /= s;
1637  y /= s;
1638  z /= s;
1639  w /= s;
1640  }
1641 
1649  T lengthSq() const
1650  {
1651  return x * x + y * y + z * z + w * w;
1652  }
1653 
1654  //--------------[ misc. operations ]-----------------------
1664  Vector4<T> lerp(T fact, const Vector4<T>& r) const
1665  {
1666  return (*this) + (r - (*this)) * fact;
1667  }
1668 
1669  //-------------[ conversion ]-----------------------------
1670 
1676  operator T*()
1677  {
1678  return (T*) this;
1679  }
1680 
1686  operator const T*() const
1687  {
1688  return (const T*) this;
1689  }
1690 
1696  Vector3<T> xyz() const
1697  {
1698  if (w == 0 || w == 1)
1699  return Vector3<T>(x,y,z);
1700 
1701  const T invW = 1.0 / w;
1702  return Vector3<T>(x * invW, y * invW, z * invW);
1703  }
1704 
1705  //-------------[ output operator ]------------------------
1712  friend std::ostream& operator<<(std::ostream& lhs, const Vector4<T>& rhs)
1713  {
1714  lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "," << rhs.w << "]";
1715  return lhs;
1716  }
1717 
1721  std::string toString() const
1722  {
1723  std::ostringstream oss;
1724  oss << *this;
1725  return oss.str();
1726  }
1727 
1731  void print(){ std::cout<<toString()<<std::endl; }
1732 
1733 };
1734 
1736 typedef Vector4<float> Vector4f;
1738 typedef Vector4<double> Vector4d;
1740 typedef Vector4<int> Vector4i;
1741 
1748 template<class T>
1749 class Matrix3
1750 {
1751 public:
1753  T data[9];
1754 
1755  //--------------------------[ constructors ]-------------------------------
1760  {
1761  for (int i = 0; i < 9; i++)
1762  data[i] = (i % 4) ? 0 : 1;
1763  }
1764 
1769  Matrix3(const T * dt)
1770  {
1771  std::memcpy(data, dt, sizeof(T) * 9);
1772  }
1773 
1778  Matrix3(const Matrix3<T>& src)
1779  {
1780  std::memcpy(data, src.data, sizeof(T) * 9);
1781  }
1782 
1787  template<class FromT>
1789  {
1790  for (int i = 0; i < 9; i++)
1791  {
1792  data[i] = static_cast<T>(src.data[i]);
1793  }
1794  }
1795 
1799  void identity()
1800  {
1801  for (int i = 0; i < 9; i++)
1802  data[i] = (i % 4) ? 0 : 1;
1803  }
1804 
1811  {
1812  Vector3<T> vxm;
1813  for(int i = 0; i < 3; i++){
1814  for(int j = 0; j < 3; j++){
1815  vxm[i] += at(i,j) * v[j];
1816  }
1817  }
1818  return vxm;
1819  }
1820 
1828  {
1829  Vector3<T> vxm;
1830  for(int i = 0; i < 3; i++){
1831  for(int j = 0; j < 3; j++){
1832  vxm[i] += m.at(i,j) * v[j];
1833  }
1834  }
1835  return vxm;
1836  }
1837 
1838 
1845  static Matrix3<T> createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
1846  {
1847  T xRads(DEG2RAD(xDeg));
1848  T yRads(DEG2RAD(yDeg));
1849  T zRads(DEG2RAD(zDeg));
1850 
1851  Matrix3<T> ma, mb, mc;
1852  float ac = cos(xRads);
1853  float as = sin(xRads);
1854  float bc = cos(yRads);
1855  float bs = sin(yRads);
1856  float cc = cos(zRads);
1857  float cs = sin(zRads);
1858 
1859  ma.at(1, 1) = ac;
1860  ma.at(2, 1) = as;
1861  ma.at(1, 2) = -as;
1862  ma.at(2, 2) = ac;
1863 
1864  mb.at(0, 0) = bc;
1865  mb.at(2, 0) = -bs;
1866  mb.at(0, 2) = bs;
1867  mb.at(2, 2) = bc;
1868 
1869  mc.at(0, 0) = cc;
1870  mc.at(1, 0) = cs;
1871  mc.at(0, 1) = -cs;
1872  mc.at(1, 1) = cc;
1873 
1874  Matrix3<T> ret = ma * mb * mc;
1875  return ret;
1876  }
1877 
1878  static Vector3<T> getEulerAngles(Matrix3<T> R) {
1879 
1880  //check for gimbal lock
1881  if (closeEnough(R.at(2,0), -1.0f)) {
1882  float x = 0; //gimbal lock, value of x doesn't matter
1883  float y = M_PI / 2;
1884  float z = x + atan2(R.at(0,1), R.at(0,2));
1885  return Vector3<T>(x, y, z);
1886  } else if (closeEnough(R.at(2,0), 1.0f)) {
1887  float x = 0;
1888  float y = -M_PI / 2;
1889  float z = -x + atan2(-R.at(0,1), -R.at(0,2));
1890  return Vector3<T>(x, y, z);
1891  } else { //two solutions exist
1892  float x1 = -asin(R.at(2,0));
1893  float x2 = M_PI - x1;
1894 
1895  float y1 = atan2(R.at(2,1) / cos(x1), R.at(2,2) / cos(x1));
1896  float y2 = atan2(R.at(2,1) / cos(x2), R.at(2,2) / cos(x2));
1897 
1898  float z1 = atan2(R.at(1,0) / cos(x1), R.at(0,0) / cos(x1));
1899  float z2 = atan2(R.at(1,0) / cos(x2), R.at(0,0) / cos(x2));
1900 
1901  //choose one solution to return
1902  //for example the "shortest" rotation
1903  if ((abs(x1) + abs(y1) + abs(z1)) <= (abs(x2) + abs(y2) + abs(z2))) {
1904  return Vector3<T>(x1, y1, z1);
1905  } else {
1906  return Vector3<T>(x2, y2, z2);
1907  }
1908  }
1909  }
1910 
1914  template<class It>
1915  static Matrix3<T> fromOde(const It* mat)
1916  {
1917  Matrix3<T> ret;
1918  for (int i = 0; i < 3; i++)
1919  {
1920  for (int j = 0; j < 3; j++)
1921  {
1922  ret.at(i, j) = static_cast<T>(mat[j * 4 + i]);
1923  }
1924  }
1925  return ret;
1926  }
1927 
1934  template<class FromT>
1935  static Matrix3<T> fromRowMajorArray(const FromT* arr)
1936  {
1937  const T retData[] =
1938  { static_cast<T>(arr[0]), static_cast<T>(arr[3]), static_cast<T>(arr[6]), static_cast<T>(arr[1]),
1939  static_cast<T>(arr[4]), static_cast<T>(arr[7]), static_cast<T>(arr[2]), static_cast<T>(arr[5]),
1940  static_cast<T>(arr[8]) };
1941 
1942  return retData;
1943  }
1944 
1951  template<class FromT>
1952  static Matrix3<T> fromColumnMajorArray(const FromT* arr)
1953  {
1954  const T retData[] =
1955  { static_cast<T>(arr[0]), static_cast<T>(arr[1]), static_cast<T>(arr[2]), static_cast<T>(arr[3]),
1956  static_cast<T>(arr[4]), static_cast<T>(arr[5]), static_cast<T>(arr[6]), static_cast<T>(arr[7]),
1957  static_cast<T>(arr[8]) };
1958 
1959  return retData;
1960  }
1961 
1962  //---------------------[ equiality operators ]------------------------------
1971  bool operator==(const Matrix3<T>& rhs) const
1972  {
1973  for (int i = 0; i < 9; i++)
1974  {
1975  if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
1976  return false;
1977  }
1978  return true;
1979  }
1980 
1986  bool operator!=(const Matrix3<T>& rhs) const
1987  {
1988  return !(*this == rhs);
1989  }
1990 
1991  //---------------------[ access operators ]---------------------------------
1997  T& at(int x, int y)
1998  {
1999  assert(x >= 0 && x < 3);
2000  assert(y >= 0 && y < 3);
2001  return data[x * 3 + y];
2002  }
2003 
2009  const T& at(int x, int y) const
2010  {
2011  assert(x >= 0 && x < 3);
2012  assert(y >= 0 && y < 3);
2013  return data[x * 3 + y];
2014  }
2015 
2021  T& operator()(int i, int j)
2022  {
2023  assert(i >= 1 && i <= 3);
2024  assert(j >= 1 && j <= 3);
2025  return data[(j - 1) * 3 + i - 1];
2026  }
2027 
2033  const T& operator()(int i, int j) const
2034  {
2035  assert(i >= 1 && i <= 3);
2036  assert(j >= 1 && j <= 3);
2037  return data[(j - 1) * 3 + i - 1];
2038  }
2039 
2045  {
2046  std::memcpy(data, rhs.data, sizeof(T) * 9);
2047  return *this;
2048  }
2049 
2054  template<class FromT>
2056  {
2057  for (int i = 0; i < 9; i++)
2058  {
2059  data[i] = static_cast<T>(rhs.data[i]);
2060  }
2061  return *this;
2062  }
2063 
2068  Matrix3<T>& operator=(const T* rhs)
2069  {
2070  std::memcpy(data, rhs, sizeof(T) * 9);
2071  return *this;
2072  }
2073 
2074  /*Matrix3<T> & operator=(const double* m)
2075  {
2076  for (int i = 0; i < 9; i++) data[i] = (T)m[i];
2077  return * this;
2078  }*/
2079 
2080  //--------------------[ matrix with matrix operations ]---------------------
2085  Matrix3<T> operator+(const Matrix3<T>& rhs) const
2086  {
2087  Matrix3<T> ret;
2088  for (int i = 0; i < 9; i++)
2089  ret.data[i] = data[i] + rhs.data[i];
2090  return ret;
2091  }
2092 
2097  Matrix3<T> operator-(const Matrix3<T>& rhs) const
2098  {
2099  Matrix3<T> ret;
2100  for (int i = 0; i < 9; i++)
2101  ret.data[i] = data[i] - rhs.data[i];
2102  return ret;
2103  }
2104 
2105  //--------------------[ matrix with scalar operations ]---------------------
2110  Matrix3<T> operator+(T rhs) const
2111  {
2112  Matrix3<T> ret;
2113  for (int i = 0; i < 9; i++)
2114  ret.data[i] = data[i] + rhs;
2115  return ret;
2116  }
2117 
2122  Matrix3<T> operator-(T rhs) const
2123  {
2124  Matrix3<T> ret;
2125  for (int i = 0; i < 9; i++)
2126  ret.data[i] = data[i] - rhs;
2127  return ret;
2128  }
2129 
2134  Matrix3<T> operator*(T rhs) const
2135  {
2136  Matrix3<T> ret;
2137  for (int i = 0; i < 9; i++)
2138  ret.data[i] = data[i] * rhs;
2139  return ret;
2140  }
2141 
2146  Matrix3<T> operator/(T rhs) const
2147  {
2148  Matrix3<T> ret;
2149  for (int i = 0; i < 9; i++)
2150  ret.data[i] = data[i] / rhs;
2151  return ret;
2152  }
2153 
2154  //--------------------[ multiply operators ]--------------------------------
2159  Vector3<T> operator*(const Vector3<T>& rhs) const
2160  {
2161  return Vector3<T>(data[0] * rhs.x + data[3] * rhs.y + data[6] * rhs.z,
2162  data[1] * rhs.x + data[4] * rhs.y + data[7] * rhs.z,
2163  data[2] * rhs.x + data[5] * rhs.y + data[8] * rhs.z);
2164  }
2165 
2171  {
2172  static Matrix3<T> w;
2173  for (int i = 0; i < 3; i++)
2174  {
2175  for (int j = 0; j < 3; j++)
2176  {
2177  T n = 0;
2178  for (int k = 0; k < 3; k++)
2179  n += rhs.at(i, k) * at(k, j);
2180  w.at(i, j) = n;
2181  }
2182  }
2183  return w;
2184 
2185  }
2186 
2187  //---------------------------[ misc operations ]----------------------------
2192  {
2193  Matrix3<T> ret;
2194  for (int i = 0; i < 3; i++)
2195  {
2196  for (int j = 0; j < 3; j++)
2197  {
2198  ret.at(i, j) = at(j, i);
2199  }
2200  }
2201  return ret;
2202  }
2203 
2213  Matrix3<T> lerp(T fact, const Matrix3<T>& rhs) const
2214  {
2215  Matrix3<T> ret = (*this) + (rhs - (*this)) * fact;
2216  return ret;
2217  }
2218 
2219  T det()
2220  {
2221  return +at(0, 0) * at(1, 1) * at(2, 2) + at(0, 1) * at(1, 2) * at(2, 0) + at(0, 2) * at(1, 0) * at(2, 1)
2222  - at(0, 0) * at(1, 2) * at(2, 1) - at(0, 1) * at(1, 0) * at(2, 2) - at(0, 2) * at(1, 1) * at(2, 0);
2223  }
2224 
2230  {
2231  Matrix3<T> ret;
2232  ret.at(0, 0) = at(1, 1) * at(2, 2) - at(2, 1) * at(1, 2);
2233  ret.at(0, 1) = at(2, 1) * at(0, 2) - at(0, 1) * at(2, 2);
2234  ret.at(0, 2) = at(0, 1) * at(1, 2) - at(1, 1) * at(0, 2);
2235  ret.at(1, 0) = at(2, 0) * at(1, 2) - at(1, 0) * at(2, 2);
2236  ret.at(1, 1) = at(0, 0) * at(2, 2) - at(2, 0) * at(0, 2);
2237  ret.at(1, 2) = at(1, 0) * at(0, 2) - at(0, 0) * at(1, 2);
2238  ret.at(2, 0) = at(1, 0) * at(2, 1) - at(2, 0) * at(1, 1);
2239  ret.at(2, 1) = at(2, 0) * at(0, 1) - at(0, 0) * at(2, 1);
2240  ret.at(2, 2) = at(0, 0) * at(1, 1) - at(1, 0) * at(0, 1);
2241  return ret * (1.0f / det());
2242  }
2243 
2244  //-------------[ conversion ]-----------------------------
2245 
2251  operator T*()
2252  {
2253  return (T*) data;
2254  }
2255 
2261  operator const T*() const
2262  {
2263  return (const T*) data;
2264  }
2265 
2266  //----------[ output operator ]----------------------------
2273  friend std::ostream& operator <<(std::ostream& lhs, const Matrix3<T>& rhs)
2274  {
2275  for (int i = 0; i < 3; i++)
2276  {
2277  lhs << "|\t";
2278  for (int j = 0; j < 3; j++)
2279  {
2280  lhs << rhs.at(j, i) << "\t";
2281  }
2282  lhs << "|" << std::endl;
2283  }
2284  return lhs;
2285  }
2286 
2290  std::string toString() const
2291  {
2292  std::ostringstream oss;
2293  oss << *this;
2294  return oss.str();
2295  }
2296 };
2297 
2299 typedef Matrix3<float> Matrix3f;
2301 typedef Matrix3<double> Matrix3d;
2303 typedef Matrix3<int> Matrix3i;
2304 
2311 template<class T>
2312 class Matrix4
2313 {
2314 public:
2316  T data[16];
2317 
2318  //--------------------------[ constructors ]-------------------------------
2323  {
2324  for (int i = 0; i < 16; i++)
2325  data[i] = (i % 5) ? 0 : 1;
2326  }
2327 
2332  Matrix4(const T * dt)
2333  {
2334  std::memcpy(data, dt, sizeof(T) * 16);
2335  }
2336 
2340  Matrix4(const T dt[4][4])
2341  {
2342  Matrix4 m;
2343  for(int i=0;i<4;i++){
2344  for(int j=0;j<4;j++){
2345  m.at(j,i)=dt[i][j];
2346  }
2347  }
2348  std::memcpy(data, dt, sizeof(T) * 16);
2349  }
2350 
2351 // /**
2352 // * Copy matrix values from array.
2353 // */
2354 // Matrix4(const T dt[])
2355 // {
2356 // for (int i = 0; i < 16; i++)
2357 // {
2358 // data[i] = dt[i];
2359 // }
2360 // }
2361 
2366  Matrix4(const Matrix4<T>& src)
2367  {
2368  std::memcpy(data, src.data, sizeof(T) * 16);
2369  }
2370 
2371  Matrix4<T>(Vector4<T> v[]){ //add
2372  Matrix4 m;
2373  for(int i=0;i<4;i++){
2374  m.at(i,0)=v[i].x;
2375  m.at(i,1)=v[i].y;
2376  m.at(i,2)=v[i].z;
2377  m.at(i,3)=v[i].w;
2378  }
2379  std::memcpy(data,m.data,sizeof(T)*16);
2380  }
2381 
2386  template<class FromT>
2388  {
2389  for (int i = 0; i < 16; i++)
2390  {
2391  data[i] = static_cast<T>(src.data[i]);
2392  }
2393  }
2394 
2395 
2396 
2400  void identity()
2401  {
2402  for (int i = 0; i < 16; i++)
2403  data[i] = (i % 5) ? 0 : 1;
2404  }
2405 
2413  {
2414  Matrix4<T> m;
2415  m.at(0,0)= n.at(0,0); m.at(1,0)= n.at(1,0); m.at(2,0)= n.at(2,0); m.at(3,0)= 0.0f;
2416  m.at(0,1)= n.at(0,1); m.at(1,1)= n.at(1,1); m.at(2,1)= n.at(2,1); m.at(3,1)= 0.0f;
2417  m.at(0,2)= n.at(0,2); m.at(1,2)= n.at(1,2); m.at(2,2)= n.at(2,2); m.at(3,2)= 0.0f;
2418  m.at(0,3)= 0.0f; m.at(1,3)= 0.0f; m.at(2,3)= 0.0f; m.at(3,3)= 1.0f;
2419  return m;
2420  }
2421 
2428  {
2429  Vector4<T> vxm;
2430  for(int i = 0; i < 4; i++){
2431  for(int j = 0; j < 4; j++){
2432  vxm[i] += at(i,j) * v[j];
2433  }
2434  }
2435  return vxm;
2436  }
2437 
2445  {
2446  Vector4<T> vxm;
2447  for(int i = 0; i < 4; i++){
2448  for(int j = 0; j < 4; j++){
2449  vxm[i] += m.at(i,j) * v[j];
2450  }
2451  }
2452  return vxm;
2453  }
2454 
2461  static Matrix4<T> createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
2462  {
2463  T xRads(DEG2RAD(xDeg));
2464  T yRads(DEG2RAD(yDeg));
2465  T zRads(DEG2RAD(zDeg));
2466 
2467  Matrix4<T> ma, mb, mc;
2468  float ac = cos(xRads);
2469  float as = sin(xRads);
2470  float bc = cos(yRads);
2471  float bs = sin(yRads);
2472  float cc = cos(zRads);
2473  float cs = sin(zRads);
2474 
2475  ma.at(1, 1) = ac;
2476  ma.at(2, 1) = as;
2477  ma.at(1, 2) = -as;
2478  ma.at(2, 2) = ac;
2479 
2480  mb.at(0, 0) = bc;
2481  mb.at(2, 0) = -bs;
2482  mb.at(0, 2) = bs;
2483  mb.at(2, 2) = bc;
2484 
2485  mc.at(0, 0) = cc;
2486  mc.at(1, 0) = cs;
2487  mc.at(0, 1) = -cs;
2488  mc.at(1, 1) = cc;
2489 
2490  /*std::cout << "RotVec = " << a << "," << b << "," << c << std::endl;
2491  std::cout << "Rx = " << std::endl << ma;
2492  std::cout << "Ry = " << std::endl << mb;
2493  std::cout << "Rz = " << std::endl << mc;*/
2494 
2495  Matrix4<T> ret = ma * mb * mc;
2496  //std::cout << "Result = " << std::endl << ma * (mb * mc);
2497 
2498  return ret;
2499  }
2500 
2501  static Vector3<T> getEulerAngles(Matrix4<T> R) {
2502 
2503  //check for gimbal lock
2504  if (closeEnough(R.at(2,0), -1.0f)) {
2505  float x = 0; //gimbal lock, value of x doesn't matter
2506  float y = M_PI / 2;
2507  float z = x + atan2(R.at(0,1), R.at(0,2));
2508  return Vector3<T>(x, y, z);
2509  } else if (closeEnough(R.at(2,0), 1.0f)) {
2510  float x = 0;
2511  float y = -M_PI / 2;
2512  float z = -x + atan2(-R.at(0,1), -R.at(0,2));
2513  return Vector3<T>(x, y, z);
2514  } else { //two solutions exist
2515  float x1 = -asin(R.at(2,0));
2516  float x2 = M_PI - x1;
2517 
2518  float y1 = atan2(R.at(2,1) / cos(x1), R.at(2,2) / cos(x1));
2519  float y2 = atan2(R.at(2,1) / cos(x2), R.at(2,2) / cos(x2));
2520 
2521  float z1 = atan2(R.at(1,0) / cos(x1), R.at(0,0) / cos(x1));
2522  float z2 = atan2(R.at(1,0) / cos(x2), R.at(0,0) / cos(x2));
2523 
2524  //choose one solution to return
2525  //for example the "shortest" rotation
2526  if ((abs(x1) + abs(y1) + abs(z1)) <= (abs(x2) + abs(y2) + abs(z2))) {
2527  return Vector3<T>(x1, y1, z1);
2528  } else {
2529  return Vector3<T>(x2, y2, z2);
2530  }
2531  }
2532  }
2533 
2535 
2542  static Matrix4<T> createTranslation(T x, T y, T z, T w = 1)
2543  {
2544  Matrix4 ret;
2545  ret.at(3, 0) = x;
2546  ret.at(3, 1) = y;
2547  ret.at(3, 2) = z;
2548  ret.at(3, 3) = w;
2549 
2550  return ret;
2551  }
2552 
2561  static Matrix4<T> createScale(T sx, T sy, T sz)
2562  {
2563  Matrix4<T> ret;
2564  ret.at(0,0) = sx;
2565  ret.at(1,1) = sy;
2566  ret.at(2,2) = sz;
2567 
2568  return ret;
2569  }
2570 
2578  static Matrix4<T> createLookAt(const Vector3<T>& eyePos, const Vector3<T>& centerPos, const Vector3<T>& upDir)
2579  {
2580  Vector3<T> forward, side, up;
2581  Matrix4<T> m;
2582 
2583  forward = centerPos - eyePos;
2584  up = upDir;
2585 
2586  forward.normalize();
2587 
2588  // Side = forward x up
2589  side = forward.crossProduct(up);
2590  side.normalize();
2591 
2592  // Recompute up as: up = side x forward
2593  up = side.crossProduct(forward);
2594 
2595  m.at(0, 0) = side.x;
2596  m.at(1, 0) = side.y;
2597  m.at(2, 0) = side.z;
2598 
2599  m.at(0, 1) = up.x;
2600  m.at(1, 1) = up.y;
2601  m.at(2, 1) = up.z;
2602 
2603  m.at(0, 2) = -forward.x;
2604  m.at(1, 2) = -forward.y;
2605  m.at(2, 2) = -forward.z;
2606 
2607  m = m * Matrix4<T>::createTranslation(-eyePos.x, -eyePos.y, -eyePos.z);
2608  return m;
2609  }
2610 
2611 
2624  static Matrix4<T> createFrustum(T left, T right, T bottom, T top, T zNear, T zFar)
2625  {
2626  /*
2627  *
2628  2 zNear
2629  ------------ 0 A 0
2630  right - left
2631 
2632  2 zNear
2633  0 ------------ B 0
2634  top - bottom
2635 
2636  0 0 C D
2637 
2638  0 0 -1 0
2639 
2640  A = (right + left) / (right - left)
2641 
2642  B = (top + bottom) / (top - bottom)
2643 
2644  C = - (zFar + zNear) / (zFar - zNear)
2645 
2646  D = - (2 zFar zNear) / (zFar - zNear)
2647  *
2648  */
2649  Matrix4<T> ret;
2650 
2651  const T invWidth = 1.0 / (right - left);
2652  const T invHeight = 1.0 / (top - bottom);
2653  const T invDepth = 1.0 / (zFar - zNear);
2654 
2655  const T twoZNear = 2 * zNear;
2656 
2657  ret.at(0,0) = twoZNear * invWidth;
2658  ret.at(1,1) = twoZNear * invHeight;
2659 
2660  ret.at(2,0) = (right + left) * invWidth;
2661  ret.at(2,1) = (top + bottom) * invHeight;
2662  ret.at(2,2) = - (zFar + zNear) * invDepth;
2663  ret.at(2,3) = -1;
2664 
2665  ret.at(3,2) = - twoZNear * zFar * invDepth;
2666 
2667  return ret;
2668  }
2669 
2682  static Matrix4<T> createOrtho(T left, T right, T bottom, T top, T zNear, T zFar)
2683  {
2684  /*
2685  2
2686  ------------ 0 0 tx
2687  right - left
2688 
2689  2
2690  0 ------------ 0 ty
2691  top - bottom
2692 
2693  -2
2694  0 0 ------------ tz
2695  zFar-zNear
2696 
2697  0 0 0 1
2698 
2699  where
2700 
2701  tx = - (right + left) / (right - left)
2702 
2703  ty = - (top + bottom) / (top - bottom)
2704 
2705  tz = - (zFar + zNear) / (zFar - zNear)
2706 
2707  */
2708 
2709  const T invWidth = 1.0 / (right - left);
2710  const T invHeight = 1.0 / (top - bottom);
2711  const T invDepth = 1.0 / (zFar - zNear);
2712 
2713  Matrix4<T> ret;
2714 
2715  ret.at(0,0) = 2 * invWidth;
2716  ret.at(1,1) = 2 * invHeight;
2717  ret.at(2,2) = -2 * invDepth;
2718 
2719  ret.at(3,0) = -(right + left) * invWidth;
2720  ret.at(3,1) = -(top + bottom) * invHeight;
2721  ret.at(3,2) = -(zFar + zNear) * invDepth;
2722 
2723  return ret;
2724  }
2725 
2726 
2736  static Matrix4<T> Perspective_vmath(T fovy, T aspect, T n,T f) //add
2737  {
2738  T q = 1.0f / tan(radians(0.5f * fovy));
2739  T A = q / aspect;
2740  T B = (n + f) / (n - f);
2741  T C = (2.0f * n * f) / (n - f);
2742 
2743  Vector4<T> result[4];
2744  result[0] = Vector4<T>(A, 0.0f, 0.0f, 0.0f);
2745  result[1] = Vector4<T>(0.0f, q, 0.0f, 0.0f);
2746  result[2] = Vector4<T>(0.0f, 0.0f, B, -1.0f);
2747  result[3] = Vector4<T>(0.0f, 0.0f, C, 0.0f);
2748 
2749  return Matrix4<T>(result);
2750  }
2751 
2752 
2762  static Matrix4<T> Perspective(T verticalAngle, T aspectRatio, T nearPlane, T farPlane)
2763  {
2764  Matrix4<T> ret;
2765  // Bail out if the projection volume is zero-sized.
2766  if (nearPlane == farPlane || aspectRatio == 0.0f)
2767  return ret;
2768 
2769  // Construct the projection.
2770 
2771  T radians = (verticalAngle / 2.0f) * M_PI / 180.0f;
2772  T sine = sin(radians);
2773  if (sine == 0.0f)
2774  return ret;
2775  T cotan = cos(radians) / sine;
2776  T clip = farPlane - nearPlane;
2777  ret.at(0,0) = cotan / aspectRatio;
2778  ret.at(1,1) = cotan;
2779  ret.at(2,2) = -(nearPlane + farPlane) / clip;
2780  ret.at(3,2) = -(2.0f * nearPlane * farPlane) / clip;
2781  ret.at(2,3) = -1.0f;
2782  ret.at(3,3) = 0.0f;
2783  return ret;
2784  }
2785 
2786 
2787 
2794  template<class FromT>
2795  static Matrix4<T> fromRowMajorArray(const FromT* arr)
2796  {
2797  const T retData[] =
2798  { static_cast<T>(arr[0]), static_cast<T>(arr[4]), static_cast<T>(arr[8]), static_cast<T>(arr[12]),
2799  static_cast<T>(arr[1]), static_cast<T>(arr[5]), static_cast<T>(arr[9]), static_cast<T>(arr[13]),
2800  static_cast<T>(arr[2]), static_cast<T>(arr[6]), static_cast<T>(arr[10]), static_cast<T>(arr[14]),
2801  static_cast<T>(arr[3]), static_cast<T>(arr[7]), static_cast<T>(arr[11]), static_cast<T>(arr[15]) };
2802 
2803  return retData;
2804  }
2805 
2812  template<class FromT>
2813  static Matrix4<T> fromColumnMajorArray(const FromT* arr)
2814  {
2815  const T retData[] =
2816  { static_cast<T>(arr[0]), static_cast<T>(arr[1]), static_cast<T>(arr[2]), static_cast<T>(arr[3]),
2817  static_cast<T>(arr[4]), static_cast<T>(arr[5]), static_cast<T>(arr[6]), static_cast<T>(arr[7]),
2818  static_cast<T>(arr[8]), static_cast<T>(arr[9]), static_cast<T>(arr[10]), static_cast<T>(arr[11]),
2819  static_cast<T>(arr[12]), static_cast<T>(arr[13]), static_cast<T>(arr[14]), static_cast<T>(arr[15]) };
2820 
2821  return retData;
2822  }
2823 
2824  //---------------------[ Equality operators ]------------------------------
2833  bool operator==(const Matrix4<T>& rhs) const
2834  {
2835  for (int i = 0; i < 16; i++)
2836  {
2837  if (std::fabs(data[i] - rhs.data[i]) >= EPSILON
2838  )
2839  return false;
2840  }
2841  return true;
2842  }
2843 
2849  bool operator!=(const Matrix4<T>& rhs) const
2850  {
2851  return !(*this == rhs);
2852  }
2853 
2854  //---------------------[ access operators ]---------------------------------
2860  T& at(int x, int y)
2861  {
2862  assert(x >= 0 && x < 4);
2863  assert(y >= 0 && y < 4);
2864  return data[x * 4 + y];
2865  }
2866 
2872  const T& at(int x, int y) const
2873  {
2874  assert(x >= 0 && x < 4);
2875  assert(y >= 0 && y < 4);
2876  return data[x * 4 + y];
2877  }
2878 
2884  T& operator()(int i, int j)
2885  {
2886  assert(i >= 1 && i <= 4);
2887  assert(j >= 1 && j <= 4);
2888  return data[(j - 1) * 4 + i - 1];
2889  }
2890 
2896  const T& operator()(int i, int j) const
2897  {
2898  assert(i >= 1 && i <= 4);
2899  assert(j >= 1 && j <= 4);
2900  return data[(j - 1) * 4 + i - 1];
2901  }
2902 
2909  {
2910  at(3, 0) = v.x;
2911  at(3, 1) = v.y;
2912  at(3, 2) = v.z;
2913  at(3, 3) = 1;
2914  }
2915 
2922  void setTranslation(T nx,T ny,T nz) //add
2923  {
2924  at(3, 0) = nx;
2925  at(3, 1) = ny;
2926  at(3, 2) = nz;
2927  at(3, 3) = 1;
2928  }
2929 
2930  Vector3<T> getTranslation() const
2931  {
2932  return Vector3<T>(at(3, 0), at(3, 1), at(3, 2));
2933  }
2934 
2940  void setRotation(const Matrix3<T>& m)
2941  {
2942  for (int i = 0; i < 3; i++)
2943  {
2944  for (int j = 0; j < 3; j++)
2945  {
2946  at(i, j) = m.at(i, j);
2947  }
2948  }
2949  }
2950 
2956  { return Vector3<T>(at(0,0), at(1,1), at(2,2)); }
2957 
2962  void setScale(T s)
2963  {
2964  at(0,0) = at(1,1) = at(2,2) = s;
2965  }
2966 
2973  void setScale(T sx, T sy, T sz)
2974  {
2975  at(0,0) = sx;
2976  at(1,1) = sy;
2977  at(2,2) = sz;
2978  }
2979 
2984  void setScale(const Vector3<T>& s)
2985  {
2986  at(0,0) = s.x;
2987  at(1,1) = s.y;
2988  at(2,2) = s.z;
2989  }
2990 
2996  {
2997  std::memcpy(data, rhs.data, sizeof(T) * 16);
2998  return *this;
2999  }
3000 
3005  template<class FromT>
3007  {
3008  for (int i = 0; i < 16; i++)
3009  {
3010  data[i] = static_cast<T>(rhs.data[i]);
3011  }
3012  return *this;
3013  }
3014 
3019  Matrix4<T>& operator=(const T* rhs)
3020  {
3021  std::memcpy(data, rhs, sizeof(T) * 16);
3022  return *this;
3023  }
3024 
3025  /*Matrix4<T> & operator=(const double* m)
3026  {
3027  for (int i = 0; i < 16; i++) data[i] = (T)m[i];
3028  return * this;
3029  }*/
3030 
3031  //--------------------[ matrix with matrix operations ]---------------------
3036  Matrix4<T> operator+(const Matrix4<T>& rhs) const
3037  {
3038  Matrix4<T> ret;
3039  for (int i = 0; i < 16; i++)
3040  ret.data[i] = data[i] + rhs.data[i];
3041  return ret;
3042  }
3043 
3048  Matrix4<T> operator-(const Matrix4<T>& rhs) const
3049  {
3050  Matrix4<T> ret;
3051  for (int i = 0; i < 16; i++)
3052  ret.data[i] = data[i] - rhs.data[i];
3053  return ret;
3054  }
3055 
3056  //--------------------[ matrix with scalar operations ]---------------------
3061  Matrix4<T> operator+(T rhs) const
3062  {
3063  Matrix4<T> ret;
3064  for (int i = 0; i < 16; i++)
3065  ret.data[i] = data[i] + rhs;
3066  return ret;
3067  }
3068 
3073  Matrix4<T> operator-(T rhs) const
3074  {
3075  Matrix4<T> ret;
3076  for (int i = 0; i < 16; i++)
3077  ret.data[i] = data[i] - rhs;
3078  return ret;
3079  }
3080 
3085  Matrix4<T> operator*(T rhs) const
3086  {
3087  Matrix4<T> ret;
3088  for (int i = 0; i < 16; i++)
3089  ret.data[i] = data[i] * rhs;
3090  return ret;
3091  }
3092 
3097  Matrix4<T> operator/(T rhs) const
3098  {
3099  Matrix4<T> ret;
3100  for (int i = 0; i < 16; i++)
3101  ret.data[i] = data[i] / rhs;
3102  return ret;
3103  }
3104 
3105  //--------------------[ multiply operators ]--------------------------------
3110  Vector4<T> operator*(const Vector4<T>& rhs) const
3111  {
3112  return Vector4<T>(data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z + data[12] * rhs.w,
3113  data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z + data[13] * rhs.w,
3114  data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z + data[14] * rhs.w,
3115  data[3] * rhs.x + data[7] * rhs.y + data[11] * rhs.z + data[15] * rhs.w);
3116 
3117  }
3118 
3123  Vector3<T> operator*(const Vector3<T>& rhs) const
3124  {
3125  return Vector3<T>(data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z,
3126  data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z,
3127  data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z);
3128  }
3129 
3135  {
3136  static Matrix4<T> w;
3137  for (int i = 0; i < 4; i++)
3138  {
3139  for (int j = 0; j < 4; j++)
3140  {
3141  T n = 0;
3142  for (int k = 0; k < 4; k++)
3143  n += rhs.at(i, k) * at(k, j);
3144  w.at(i, j) = n;
3145  }
3146  }
3147  return w;
3148 
3149  }
3150 
3151  //---------------------------[ misc operations ]----------------------------
3152 
3158  T det()
3159  {
3160 
3161  return +at(3, 0) * at(2, 1) * at(1, 2) * at(0, 3) - at(2, 0) * at(3, 1) * at(1, 2) * at(0, 3)
3162  - at(3, 0) * at(1, 1) * at(2, 2) * at(0, 3) + at(1, 0) * at(3, 1) * at(2, 2) * at(0, 3)
3163 
3164  + at(2, 0) * at(1, 1) * at(3, 2) * at(0, 3) - at(1, 0) * at(2, 1) * at(3, 2) * at(0, 3)
3165  - at(3, 0) * at(2, 1) * at(0, 2) * at(1, 3) + at(2, 0) * at(3, 1) * at(0, 2) * at(1, 3)
3166 
3167  + at(3, 0) * at(0, 1) * at(2, 2) * at(1, 3) - at(0, 0) * at(3, 1) * at(2, 2) * at(1, 3)
3168  - at(2, 0) * at(0, 1) * at(3, 2) * at(1, 3) + at(0, 0) * at(2, 1) * at(3, 2) * at(1, 3)
3169 
3170  + at(3, 0) * at(1, 1) * at(0, 2) * at(2, 3) - at(1, 0) * at(3, 1) * at(0, 2) * at(2, 3)
3171  - at(3, 0) * at(0, 1) * at(1, 2) * at(2, 3) + at(0, 0) * at(3, 1) * at(1, 2) * at(2, 3)
3172 
3173  + at(1, 0) * at(0, 1) * at(3, 2) * at(2, 3) - at(0, 0) * at(1, 1) * at(3, 2) * at(2, 3)
3174  - at(2, 0) * at(1, 1) * at(0, 2) * at(3, 3) + at(1, 0) * at(2, 1) * at(0, 2) * at(3, 3)
3175 
3176  + at(2, 0) * at(0, 1) * at(1, 2) * at(3, 3) - at(0, 0) * at(2, 1) * at(1, 2) * at(3, 3)
3177  - at(1, 0) * at(0, 1) * at(2, 2) * at(3, 3) + at(0, 0) * at(1, 1) * at(2, 2) * at(3, 3);
3178 
3179  }
3180 
3188  {
3189  Matrix4<T> ret;
3190 
3191  ret.at(0, 0) = +at(2, 1) * at(3, 2) * at(1, 3) - at(3, 1) * at(2, 2) * at(1, 3) + at(3, 1) * at(1, 2) * at(2, 3)
3192  - at(1, 1) * at(3, 2) * at(2, 3) - at(2, 1) * at(1, 2) * at(3, 3) + at(1, 1) * at(2, 2) * at(3, 3);
3193 
3194  ret.at(1, 0) = +at(3, 0) * at(2, 2) * at(1, 3) - at(2, 0) * at(3, 2) * at(1, 3) - at(3, 0) * at(1, 2) * at(2, 3)
3195  + at(1, 0) * at(3, 2) * at(2, 3) + at(2, 0) * at(1, 2) * at(3, 3) - at(1, 0) * at(2, 2) * at(3, 3);
3196 
3197  ret.at(2, 0) = +at(2, 0) * at(3, 1) * at(1, 3) - at(3, 0) * at(2, 1) * at(1, 3) + at(3, 0) * at(1, 1) * at(2, 3)
3198  - at(1, 0) * at(3, 1) * at(2, 3) - at(2, 0) * at(1, 1) * at(3, 3) + at(1, 0) * at(2, 1) * at(3, 3);
3199 
3200  ret.at(3, 0) = +at(3, 0) * at(2, 1) * at(1, 2) - at(2, 0) * at(3, 1) * at(1, 2) - at(3, 0) * at(1, 1) * at(2, 2)
3201  + at(1, 0) * at(3, 1) * at(2, 2) + at(2, 0) * at(1, 1) * at(3, 2) - at(1, 0) * at(2, 1) * at(3, 2);
3202 
3203  ret.at(0, 1) = +at(3, 1) * at(2, 2) * at(0, 3) - at(2, 1) * at(3, 2) * at(0, 3) - at(3, 1) * at(0, 2) * at(2, 3)
3204  + at(0, 1) * at(3, 2) * at(2, 3) + at(2, 1) * at(0, 2) * at(3, 3) - at(0, 1) * at(2, 2) * at(3, 3);
3205 
3206  ret.at(1, 1) = +at(2, 0) * at(3, 2) * at(0, 3) - at(3, 0) * at(2, 2) * at(0, 3) + at(3, 0) * at(0, 2) * at(2, 3)
3207  - at(0, 0) * at(3, 2) * at(2, 3) - at(2, 0) * at(0, 2) * at(3, 3) + at(0, 0) * at(2, 2) * at(3, 3);
3208 
3209  ret.at(2, 1) = +at(3, 0) * at(2, 1) * at(0, 3) - at(2, 0) * at(3, 1) * at(0, 3) - at(3, 0) * at(0, 1) * at(2, 3)
3210  + at(0, 0) * at(3, 1) * at(2, 3) + at(2, 0) * at(0, 1) * at(3, 3) - at(0, 0) * at(2, 1) * at(3, 3);
3211 
3212  ret.at(3, 1) = +at(2, 0) * at(3, 1) * at(0, 2) - at(3, 0) * at(2, 1) * at(0, 2) + at(3, 0) * at(0, 1) * at(2, 2)
3213  - at(0, 0) * at(3, 1) * at(2, 2) - at(2, 0) * at(0, 1) * at(3, 2) + at(0, 0) * at(2, 1) * at(3, 2);
3214 
3215  ret.at(0, 2) = +at(1, 1) * at(3, 2) * at(0, 3) - at(3, 1) * at(1, 2) * at(0, 3) + at(3, 1) * at(0, 2) * at(1, 3)
3216  - at(0, 1) * at(3, 2) * at(1, 3) - at(1, 1) * at(0, 2) * at(3, 3) + at(0, 1) * at(1, 2) * at(3, 3);
3217 
3218  ret.at(1, 2) = +at(3, 0) * at(1, 2) * at(0, 3) - at(1, 0) * at(3, 2) * at(0, 3) - at(3, 0) * at(0, 2) * at(1, 3)
3219  + at(0, 0) * at(3, 2) * at(1, 3) + at(1, 0) * at(0, 2) * at(3, 3) - at(0, 0) * at(1, 2) * at(3, 3);
3220 
3221  ret.at(2, 2) = +at(1, 0) * at(3, 1) * at(0, 3) - at(3, 0) * at(1, 1) * at(0, 3) + at(3, 0) * at(0, 1) * at(1, 3)
3222  - at(0, 0) * at(3, 1) * at(1, 3) - at(1, 0) * at(0, 1) * at(3, 3) + at(0, 0) * at(1, 1) * at(3, 3);
3223 
3224  ret.at(3, 2) = +at(3, 0) * at(1, 1) * at(0, 2) - at(1, 0) * at(3, 1) * at(0, 2) - at(3, 0) * at(0, 1) * at(1, 2)
3225  + at(0, 0) * at(3, 1) * at(1, 2) + at(1, 0) * at(0, 1) * at(3, 2) - at(0, 0) * at(1, 1) * at(3, 2);
3226 
3227  ret.at(0, 3) = +at(2, 1) * at(1, 2) * at(0, 3) - at(1, 1) * at(2, 2) * at(0, 3) - at(2, 1) * at(0, 2) * at(1, 3)
3228  + at(0, 1) * at(2, 2) * at(1, 3) + at(1, 1) * at(0, 2) * at(2, 3) - at(0, 1) * at(1, 2) * at(2, 3);
3229 
3230  ret.at(1, 3) = +at(1, 0) * at(2, 2) * at(0, 3) - at(2, 0) * at(1, 2) * at(0, 3) + at(2, 0) * at(0, 2) * at(1, 3)
3231  - at(0, 0) * at(2, 2) * at(1, 3) - at(1, 0) * at(0, 2) * at(2, 3) + at(0, 0) * at(1, 2) * at(2, 3);
3232 
3233  ret.at(2, 3) = +at(2, 0) * at(1, 1) * at(0, 3) - at(1, 0) * at(2, 1) * at(0, 3) - at(2, 0) * at(0, 1) * at(1, 3)
3234  + at(0, 0) * at(2, 1) * at(1, 3) + at(1, 0) * at(0, 1) * at(2, 3) - at(0, 0) * at(1, 1) * at(2, 3);
3235 
3236  ret.at(3, 3) = +at(1, 0) * at(2, 1) * at(0, 2) - at(2, 0) * at(1, 1) * at(0, 2) + at(2, 0) * at(0, 1) * at(1, 2)
3237  - at(0, 0) * at(2, 1) * at(1, 2) - at(1, 0) * at(0, 1) * at(2, 2) + at(0, 0) * at(1, 1) * at(2, 2);
3238 
3239  return ret / det();
3240  }
3241 
3246  {
3247  Matrix4<T> ret;
3248  for (int i = 0; i < 4; i++)
3249  {
3250  for (int j = 0; j < 4; j++)
3251  {
3252  ret.at(i, j) = at(j, i);
3253  }
3254  }
3255  return ret;
3256  }
3257 
3267  Matrix4<T> lerp(T fact, const Matrix4<T>& rhs) const
3268  {
3269  Matrix4<T> ret = (*this) + (rhs - (*this)) * fact;
3270  return ret;
3271  }
3272 
3273  //-------------[ conversion ]-----------------------------
3279  operator T*()
3280  {
3281  return (T*) data;
3282  }
3283 
3289  operator const T*() const
3290  {
3291  return (const T*) data;
3292  }
3293 
3294  //----------[ output operator ]----------------------------
3301  friend std::ostream& operator <<(std::ostream& lhs, const Matrix4<T>& rhs)
3302  {
3303  for (int i = 0; i < 4; i++)
3304  {
3305  lhs << "|\t";
3306  for (int j = 0; j < 4; j++)
3307  {
3308  lhs << rhs.at(j, i) << "\t";
3309  }
3310  lhs << "|" << std::endl;
3311  }
3312  return lhs;
3313  }
3314 
3318  std::string toString() const
3319  {
3320  std::ostringstream oss;
3321  oss << *this;
3322  return oss.str();
3323  }
3324 
3325 };
3326 
3328 typedef Matrix4<float> Matrix4f;
3330 typedef Matrix4<double> Matrix4d;
3332 typedef Matrix4<int> Matrix4i;
3333 
3341 template<class T>
3343 {
3344 public:
3348  T w;
3353 
3358  : w(0), v(0, 0, 0)
3359  {
3360  }
3361 
3366  : w(q.w), v(q.v)
3367  {
3368  }
3369 
3373  template<class FromT>
3375  : w(static_cast<T>(q.w)), v(q.v)
3376  {
3377  }
3378 
3384  Quaternion(T w_, const Vector3<T>& v_)
3385  : w(w_), v(v_)
3386  {
3387  }
3388 
3396  Quaternion(T w_, T x, T y, T z)
3397  : w(w_), v(x, y, z)
3398  {
3399  }
3400 
3406  {
3407  v = rhs.v;
3408  w = rhs.w;
3409  return *this;
3410  }
3411 
3416  template<class FromT>
3418  {
3419  v = rhs.v;
3420  w = static_cast<T>(rhs.w);
3421  return *this;
3422  }
3423 
3429  {
3430  const Quaternion<T>& lhs = *this;
3431  return Quaternion<T>(lhs.w + rhs.w, lhs.v + rhs.v);
3432  }
3433 
3439  {
3440  const Quaternion<T>& lhs = *this;
3441  return Quaternion<T>(lhs.w * rhs.w - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
3442  lhs.w * rhs.v.x + lhs.v.x * rhs.w + lhs.v.y * rhs.v.z - lhs.v.z * rhs.v.y,
3443  lhs.w * rhs.v.y - lhs.v.x * rhs.v.z + lhs.v.y * rhs.w + lhs.v.z * rhs.v.x,
3444  lhs.w * rhs.v.z + lhs.v.x * rhs.v.y - lhs.v.y * rhs.v.x + lhs.v.z * rhs.w);
3445  }
3446 
3452  {
3453  return Quaternion<T>(w * rhs, v * rhs);
3454  }
3455 
3461  {
3462  const Quaternion<T>& lhs = *this;
3463  return Quaternion<T>(lhs.w - rhs.w, lhs.v - rhs.v);
3464  }
3465 
3471  {
3472  w += rhs.w;
3473  v += rhs.v;
3474  return *this;
3475  }
3476 
3482  {
3483  w -= rhs.w;
3484  v -= rhs.v;
3485  return *this;
3486  }
3487 
3493  {
3494  Quaternion q = (*this) * rhs;
3495  v = q.v;
3496  w = q.w;
3497  return *this;
3498  }
3499 
3505  {
3506  w *= rhs;
3507  v *= rhs;
3508  return *this;
3509  }
3510 
3518  bool operator==(const Quaternion<T>& rhs) const
3519  {
3520  const Quaternion<T>& lhs = *this;
3521  return (std::fabs(lhs.w - rhs.w) < EPSILON) && lhs.v == rhs.v;
3522  }
3523 
3529  bool operator!=(const Quaternion<T>& rhs) const
3530  {
3531  return !(*this == rhs);
3532  }
3533 
3534  //-------------[ unary operations ]--------------------------
3540  {
3541  return Quaternion<T>(-w, -v);
3542  }
3543 
3549  {
3550  return Quaternion<T>(w, -v);
3551  }
3552 
3557  T length() const
3558  {
3559  return (T) std::sqrt(w * w + v.lengthSq());
3560  }
3561 
3569  T lengthSq() const
3570  {
3571  return w * w + v.lengthSq();
3572  }
3573 
3577  void normalize()
3578  {
3579  T len = length();
3580  w /= len;
3581  v /= len;
3582  }
3583 
3591  static Quaternion<T> fromEulerAngles(T x, T y, T z)
3592  {
3593  Quaternion<T> ret = fromAxisRot(Vector3<T>(1, 0, 0), x) * fromAxisRot(Vector3<T>(0, 1, 0), y)
3594  * fromAxisRot(Vector3<T>(0, 0, 1), z);
3595  return ret;
3596  }
3597 
3603  static Quaternion<T> fromAxisRot(Vector3<T> axis, float angleDeg)
3604  {
3605  double angleRad = DEG2RAD(angleDeg);
3606  double sa2 = std::sin(angleRad / 2);
3607  double ca2 = std::cos(angleRad / 2);
3608  return Quaternion<T>(ca2, axis * sa2);
3609  }
3610 
3616  {
3617  Matrix3<T> ret;
3618 
3619  /*ret.at(0,0) = 1 - 2*v.y*v.y - 2*v.z*v.z;
3620  ret.at(1,0) = 2*v.x*v.y - 2*w*v.z;
3621  ret.at(2,0) = 2*v.x*v.z - 2*w*v.y;
3622 
3623  ret.at(0,1) = 2*v.x*v.y + 2*w*v.z;
3624  ret.at(1,1) = 1 - 2*v.x*v.x - 2*v.z*v.z;
3625  ret.at(2,1) = 2*v.y*v.z - 2*w*v.x;
3626 
3627  ret.at(0,2) = 2*v.x*v.z - 2*w*v.y;
3628  ret.at(1,2) = 2*v.y*v.z + 2*w*v.x;
3629  ret.at(2,2) = 1 - 2*v.x*v.x - 2*v.y*v.y;*/
3630 
3631  T xx = v.x * v.x;
3632  T xy = v.x * v.y;
3633  T xz = v.x * v.z;
3634  T xw = v.x * w;
3635 
3636  T yy = v.y * v.y;
3637  T yz = v.y * v.z;
3638  T yw = v.y * w;
3639 
3640  T zz = v.z * v.z;
3641  T zw = v.z * w;
3642 
3643  ret.at(0, 0) = 1 - 2 * (yy + zz);
3644  ret.at(1, 0) = 2 * (xy - zw);
3645  ret.at(2, 0) = 2 * (xz + yw);
3646 
3647  ret.at(0, 1) = 2 * (xy + zw);
3648  ret.at(1, 1) = 1 - 2 * (xx + zz);
3649  ret.at(2, 1) = 2 * (yz - xw);
3650 
3651  ret.at(0, 2) = 2 * (xz - yw);
3652  ret.at(1, 2) = 2 * (yz + xw);
3653  ret.at(2, 2) = 1 - 2 * (xx + yy);
3654 
3655  return ret;
3656  }
3657 
3665  {
3666  Matrix4<T> ret;
3667 
3668  T xx = v.x * v.x;
3669  T xy = v.x * v.y;
3670  T xz = v.x * v.z;
3671  T xw = v.x * w;
3672 
3673  T yy = v.y * v.y;
3674  T yz = v.y * v.z;
3675  T yw = v.y * w;
3676 
3677  T zz = v.z * v.z;
3678  T zw = v.z * w;
3679 
3680  ret.at(0, 0) = 1 - 2 * (yy + zz);
3681  ret.at(1, 0) = 2 * (xy - zw);
3682  ret.at(2, 0) = 2 * (xz + yw);
3683  ret.at(3, 0) = 0;
3684 
3685  ret.at(0, 1) = 2 * (xy + zw);
3686  ret.at(1, 1) = 1 - 2 * (xx + zz);
3687  ret.at(2, 1) = 2 * (yz - xw);
3688  ret.at(3, 1) = 0;
3689 
3690  ret.at(0, 2) = 2 * (xz - yw);
3691  ret.at(1, 2) = 2 * (yz + xw);
3692  ret.at(2, 2) = 1 - 2 * (xx + yy);
3693  ret.at(3, 2) = 0;
3694 
3695  ret.at(0, 3) = 0;
3696  ret.at(1, 3) = 0;
3697  ret.at(2, 3) = 0;
3698  ret.at(3, 3) = 1;
3699 
3700  return ret;
3701 
3702  }
3703 
3713  Quaternion<T> lerp(T fact, const Quaternion<T>& rhs) const
3714  {
3715  return Quaternion<T>((1 - fact) * w + fact * rhs.w, v.lerp(fact, rhs.v));
3716  }
3717 
3721  friend std::ostream& operator <<(std::ostream& oss, const Quaternion<T>& q)
3722  {
3723  oss << "Re: " << q.w << " Im: " << q.v;
3724  return oss;
3725  }
3726 
3730  std::string toString() const
3731  {
3732  std::ostringstream oss;
3733  oss << *this;
3734  return oss.str();
3735  }
3736 
3743  // 2011-07-02: Davide Bacchet: changed formula to fix degenerate cases
3745  {
3746  Quaternion<T> q;
3747 
3748  T tr, s;
3749  tr = m(1, 1) + m(2, 2) + m(3, 3);
3750  if (tr >= epsilon)
3751  {
3752  s = 0.5 / (T) sqrt(tr + 1.0);
3753  q.w = 0.25 / s;
3754  q.v.x = (m(3, 2) - m(2, 3)) * s;
3755  q.v.y = (m(1, 3) - m(3, 1)) * s;
3756  q.v.z = (m(2, 1) - m(1, 2)) * s;
3757  }
3758  else
3759  {
3760  T d0 = m(1, 1);
3761  T d1 = m(2, 2);
3762  T d2 = m(3, 3);
3763 
3764  char bigIdx = (d0 > d1) ? ((d0 > d2) ? 0 : 2):((d1 > d2) ? 1 : 2);
3765 
3766  if (bigIdx == 0)
3767  {
3768  s = 2.0 * (T) sqrt(1.0 + m(1, 1) - m(2, 2) - m(3, 3));
3769  q.w = (m(3, 2) - m(2, 3)) / s;
3770  q.v.x = 0.25 * s;
3771  q.v.y = (m(1, 2) + m(2, 1)) / s;
3772  q.v.z = (m(1, 3) + m(3, 1)) / s;
3773  }
3774  else if (bigIdx == 1)
3775  {
3776  s = 2.0 * (T) sqrt(1.0 + m(2, 2) - m(1, 1) - m(3, 3));
3777  q.w = (m(1, 3) - m(3, 1)) / s;
3778  q.v.x = (m(1, 2) + m(2, 1)) / s;
3779  q.v.y = 0.25 * s;
3780  q.v.z = (m(2, 3) + m(3, 2)) / s;
3781  }
3782  else
3783  {
3784  s = 2.0 * (T) sqrt(1.0 + m(3, 3) - m(1, 1) - m(2, 2));
3785  q.w = (m(2, 1) - m(1, 2)) / s;
3786  q.v.x = (m(1, 3) + m(3, 1)) / s;
3787  q.v.y = (m(2, 3) + m(3, 2)) / s;
3788  q.v.z = 0.25 * s;
3789  }
3790  }
3791 
3792  return q;
3793  }
3794 
3802  // 2011-07-02: Davide Bacchet: changed formula to fix degenerate cases
3804  {
3805  Quaternion<T> q;
3806 
3807  T tr, s;
3808  tr = m(1, 1) + m(2, 2) + m(3, 3);
3809  if (tr >= epsilon)
3810  {
3811  s = 0.5 / (T) sqrt(tr + 1.0);
3812  q.w = 0.25 / s;
3813  q.v.x = (m(3, 2) - m(2, 3)) * s;
3814  q.v.y = (m(1, 3) - m(3, 1)) * s;
3815  q.v.z = (m(2, 1) - m(1, 2)) * s;
3816  }
3817  else
3818  {
3819  T d0 = m(1, 1);
3820  T d1 = m(2, 2);
3821  T d2 = m(3, 3);
3822 
3823  char bigIdx = (d0 > d1) ? ((d0 > d2) ? 0 : 2):((d1 > d2) ? 1 : 2);
3824 
3825  if (bigIdx == 0)
3826  {
3827  s = 2.0 * (T) sqrt(1.0 + m(1, 1) - m(2, 2) - m(3, 3));
3828  q.w = (m(3, 2) - m(2, 3)) / s;
3829  q.v.x = 0.25 * s;
3830  q.v.y = (m(1, 2) + m(2, 1)) / s;
3831  q.v.z = (m(1, 3) + m(3, 1)) / s;
3832  }
3833  else if (bigIdx == 1)
3834  {
3835  s = 2.0 * (T) sqrt(1.0 + m(2, 2) - m(1, 1) - m(3, 3));
3836  q.w = (m(1, 3) - m(3, 1)) / s;
3837  q.v.x = (m(1, 2) + m(2, 1)) / s;
3838  q.v.y = 0.25 * s;
3839  q.v.z = (m(2, 3) + m(3, 2)) / s;
3840  }
3841  else
3842  {
3843  s = 2.0 * (T) sqrt(1.0 + m(3, 3) - m(1, 1) - m(2, 2));
3844  q.w = (m(2, 1) - m(1, 2)) / s;
3845  q.v.x = (m(1, 3) + m(3, 1)) / s;
3846  q.v.y = (m(2, 3) + m(3, 2)) / s;
3847  q.v.z = 0.25 * s;
3848  }
3849  }
3850 
3851  return q;
3852  }
3853 
3862  Quaternion<T> slerp(T r, const Quaternion<T>& q2) const
3863  {
3864  Quaternion<T> ret;
3865  T cosTheta = w * q2.w + v.x * q2.v.x + v.y * q2.v.y + v.z * q2.v.z;
3866  T theta = (T) acos(cosTheta);
3867  if (fabs(theta) < epsilon)
3868  {
3869  ret = *this;
3870  }
3871  else
3872  {
3873  T sinTheta = (T) sqrt(1.0 - cosTheta * cosTheta);
3874  if (fabs(sinTheta) < epsilon)
3875  {
3876  ret.w = 0.5 * w + 0.5 * q2.w;
3877  ret.v = v.lerp(0.5, q2.v);
3878  }
3879  else
3880  {
3881  T rA = (T) sin((1.0 - r) * theta) / sinTheta;
3882  T rB = (T) sin(r * theta) / sinTheta;
3883 
3884  ret.w = w * rA + q2.w * rB;
3885  ret.v.x = v.x * rA + q2.v.x * rB;
3886  ret.v.y = v.y * rA + q2.v.y * rB;
3887  ret.v.z = v.z * rA + q2.v.z * rB;
3888  }
3889  }
3890  return ret;
3891  }
3892 
3893 };
3894 
3895 typedef Quaternion<float> Quatf;
3896 typedef Quaternion<double> Quatd;
3897 
3898 #ifdef VMATH_NAMESPACE
3899 }
3900 #endif
3901 
3902 
3903 
3905 //
3906 // Standard C++ library extensions
3907 //
3909 
3910 
3911 // Shortcut defines
3912 #ifdef VMATH_NAMESPACE
3913 #define VEC2 VMATH_NAMESPACE::Vector2
3914 #define VEC3 VMATH_NAMESPACE::Vector3
3915 #define VEC4 VMATH_NAMESPACE::Vector4
3916 #else
3917 #define VEC2 Vector2
3918 #define VEC3 Vector3
3919 #define VEC4 Vector4
3920 #endif
3921 
3922 namespace std
3923 {
3924 
3929  template <typename T>
3930  VEC2<T> min(const VEC2<T>& a, const VEC2<T>& b)
3931  {
3932  return VEC2<T>(::std::min(a.x, b.x), ::std::min(a.y, b.y));
3933  }
3934 
3939  template <typename T>
3940  VEC3<T> min(const VEC3<T>& a, const VEC3<T>& b)
3941  {
3942  return VEC3<T>(::std::min(a.x, b.x), ::std::min(a.y, b.y), ::std::min(a.z, b.z));
3943  }
3944 
3949  template <typename T>
3950  VEC4<T> min(const VEC4<T>& a, const VEC4<T>& b)
3951  {
3952  return VEC4<T>(::std::min(a.x, b.x), ::std::min(a.y, b.y), ::std::min(a.z, b.z), ::std::min(a.w, b.w));
3953  }
3954 
3959  template <typename T>
3960  VEC2<T> max(const VEC2<T>& a, const VEC2<T>& b)
3961  {
3962  return VEC2<T>(::std::max(a.x, b.x), ::std::max(a.y, b.y));
3963  }
3964 
3969  template <typename T>
3970  VEC3<T> max(const VEC3<T>& a, const VEC3<T>& b)
3971  {
3972  return VEC3<T>(::std::max(a.x, b.x), ::std::max(a.y, b.y), ::std::max(a.z, b.z));
3973  }
3974 
3979  template <typename T>
3980  VEC4<T> max(const VEC4<T>& a, const VEC4<T>& b)
3981  {
3982  return VEC4<T>(::std::max(a.x, b.x), ::std::max(a.y, b.y), ::std::max(a.z, b.z), ::std::max(a.w, b.w));
3983  }
3984 }
3985 
3986 // cleanup shortcut defines
3987 #undef VEC2
3988 #undef VEC3
3989 #undef VEC4
3990 
3991 #ifdef VMATH_NAMESPACE
3992 namespace VMATH_NAMESPACE
3993 {
3994 #endif //VMATH_NAMESPACE
3995 
4012  template <typename T>
4013  class Aabb3
4014  {
4015  public:
4020 
4025 
4031  : min(1,1,1), max(-1,-1,-1)
4032  {}
4033 
4038  template <typename SrcT>
4039  Aabb3(const Vector3<SrcT>& point)
4040  : min(point), max(point)
4041  {}
4042 
4053  template <typename SrcT>
4054  Aabb3(SrcT x0, SrcT y0, SrcT z0, SrcT x1, SrcT y1, SrcT z1)
4055  : min(std::min(x0,x1), std::min(y0,y1), std::min(z0,z1)),
4056  max(std::max(x0,x1), std::max(y0,y1), std::max(z0,z1))
4057  {}
4058 
4065  template <typename SrcT>
4066  Aabb3(SrcT x, SrcT y, SrcT z)
4067  : min(x,y,z), max(x,y,z)
4068  {}
4069 
4074  template <typename SrcT>
4075  Aabb3(const Aabb3<SrcT>& src)
4076  : min(src.min), max(src.max)
4077  {}
4078 
4084  template <typename SrcT>
4086  {
4087  min = rhs.min;
4088  max = rhs.max;
4089  return *this;
4090  }
4091 
4092 
4099  bool valid() const
4100  { return min.x <= max.x && min.y <= max.y && min.z <= max.z; }
4101 
4106  void invalidate()
4107  { min = Vector3<T>(1,1,1); max = Vector3<T>(-1,-1,-1); }
4108 
4113  template <typename SrcT>
4114  void extend(const Vector3<SrcT>& point)
4115  {
4116  if (!valid())
4117  {
4118  min = max = point;
4119  }
4120  else
4121  {
4122  min = std::min(min, point);
4123  max = std::max(max, point);
4124  }
4125  }
4126 
4131  template <typename SrcT>
4132  void extend(const Aabb3<SrcT>& box)
4133  {
4134  if (!valid())
4135  {
4136  min = box.min;
4137  max = box.max;
4138  }
4139  else
4140  {
4141  min = std::min(min, box.min);
4142  max = std::max(max, box.max);
4143  }
4144  }
4145 
4151  template <typename SrcT>
4152  Aabb3<T> extended(const Vector3<SrcT>& point) const
4153  {
4154  Aabb3<T> ret(*this);
4155  ret.extend(point);
4156  return ret;
4157  }
4158 
4164  template <typename SrcT>
4165  Aabb3<T> extended(const Aabb3<SrcT>& box) const
4166  {
4167  Aabb3<T> ret(*this);
4168  ret.extend(box);
4169  return ret;
4170  }
4171 
4177  template <typename SrcT>
4178  bool intersects(const Vector3<SrcT>& point) const
4179  {
4180  /*
4181  for (size_t i = 0; i < 3; i++)
4182  {
4183  if (min[i] > point[i] || point[i] > max[i])
4184  return false;
4185  }
4186  */
4187  // unrolled loop above
4188  if (min.x > point.x || point.x > max.x)
4189  return false;
4190  if (min.y > point.y || point.y > max.y)
4191  return false;
4192  if (min.z > point.z || point.z > max.z)
4193  return false;
4194 
4195  return true;
4196  }
4197 
4203  template <typename SrcT>
4204  bool intersects(const Aabb3<SrcT>& box) const
4205  {
4206  /*
4207  for (size_t i = 0; i < 3; i++)
4208  {
4209  if (max[i] < box.min[i] || min[i] > box.max[i])
4210  return false;
4211  }
4212  */
4213  // unrolled loop above
4214  if (max.x < box.min.x || min.x > box.max.x)
4215  return false;
4216  if (max.y < box.min.y || min.y > box.max.y)
4217  return false;
4218  if (max.z < box.min.z || min.z > box.max.z)
4219  return false;
4220 
4221  return true;
4222  }
4223 
4231  template <typename SrcT>
4232  Aabb3<T> intersection(const Aabb3<SrcT>& other) const
4233  {
4234  Aabb3<T> ret;
4235  if (max.x < other.min.x || min.x > other.max.x)
4236  return ret;
4237  if (max.y < other.min.y || min.y > other.max.y)
4238  return ret;
4239  if (max.z < other.min.z || min.z > other.max.z)
4240  return ret;
4241 
4242  ret.min = std::max(min, other.min);
4243  ret.max = std::min(max, other.max);
4244 
4245  return ret;
4246  }
4247 
4253  { return (min + max) * 0.5f; }
4254 
4260  { return (max - min) * 0.5f; }
4261 
4267  { return max - min; }
4268 
4284  Vector3<T> point(size_t i) const
4285  {
4286  assert(i < 8);
4287  return Vector3<T>(i & 1 ? min.x : max.x, i & 2 ? min.y : max.y, i & 4 ? min.z : max.z);
4288  }
4289 
4296  {
4297  Aabb3<T> ret;
4298  for (size_t i = 0; i < 8; i++)
4299  {
4300  const Vector4<T> p(point(i), 1);
4301  ret.extend((t * p).xyz());
4302  }
4303 
4304  return ret;
4305  }
4306 
4307  //-------------------------------------------------------------------------------------------------------------
4308  // operators
4309  //-------------------------------------------------------------------------------------------------------------
4315  template <typename RhsT>
4316  bool operator==(const Aabb3<RhsT>& rhs) const
4317  {
4318  return min == rhs.min && max == rhs.max;
4319  }
4320 
4326  template <typename RhsT>
4327  bool operator!=(const Aabb3<RhsT>& rhs) const
4328  {
4329  return min != rhs.min || max != rhs.max;
4330  }
4331 
4337  Aabb3<T> operator*(const Matrix4<T>& rhs) const
4338  {
4339  return transformed(rhs);
4340  }
4341 
4348  {
4349  *this = transformed(rhs);
4350  return *this;
4351  }
4352 
4353 
4359  template <typename SrcT>
4360  Aabb3<T>& operator<<(const Vector3<SrcT>& rhs)
4361  {
4362  extend(rhs);
4363  return *this;
4364  }
4365 
4371  template <typename SrcT>
4372  Aabb3<T>& operator<<(const Aabb3<SrcT>& rhs)
4373  {
4374  extend(rhs);
4375  return *this;
4376  }
4377 
4383  template <typename RhsT>
4384  Aabb3<T> operator|(const Aabb3<RhsT>& rhs) const
4385  {
4386  return extended(rhs);
4387  }
4388 
4394  template <typename RhsT>
4395  Aabb3<T> operator&(const Aabb3<RhsT>& rhs) const
4396  {
4397  return intersection(rhs);
4398  }
4399 
4406  friend std::ostream& operator<<(std::ostream& lhs, const Aabb3<T>& rhs)
4407  {
4408  lhs << rhs.min << " x " << rhs.max;
4409  return lhs;
4410  }
4411 
4412  };
4413 
4414  typedef Aabb3<float> Aabb3f;
4415  typedef Aabb3<double> Aabb3d;
4416 
4417 #ifdef VMATH_NAMESPACE
4418 }
4419 #endif //VMATH_NAMESPACE
4420 
4421 
4422 #endif // __vmath_Header_File__
4423 
static Quaternion< T > fromAxisRot(Vector3< T > axis, float angleDeg)
Definition: vmath.h:3603
Vector4(T nx, T ny, T nz, T nw)
Definition: vmath.h:1288
Vector3< T > operator*(const Vector3< T > &rhs) const
Definition: vmath.h:2159
Vector4< T > & operator*=(T rhs)
Definition: vmath.h:1598
Vector2< T > operator*(T rhs) const
Definition: vmath.h:399
static Matrix3< T > fromOde(const It *mat)
Definition: vmath.h:1915
Aabb3< T > intersection(const Aabb3< SrcT > &other) const
Definition: vmath.h:4232
Vector3< T > & operator*=(T rhs)
Definition: vmath.h:1020
bool operator!=(const Vector3< T > &rhs) const
Definition: vmath.h:1058
Matrix3< T > inverse()
Definition: vmath.h:2229
Aabb3< T > extended(const Vector3< SrcT > &point) const
Definition: vmath.h:4152
bool operator==(const Aabb3< RhsT > &rhs) const
Definition: vmath.h:4316
Vector3(T nx, T ny, T nz)
Definition: vmath.h:703
Vector2< T > operator+(const Vector2< T > &rhs) const
Definition: vmath.h:300
Matrix3< T > operator/(T rhs) const
Definition: vmath.h:2146
std::string toString() const
Definition: vmath.h:1721
T dotProduct(const Vector3< T > &rhs) const
Definition: vmath.h:893
T & operator[](int n)
Definition: vmath.h:271
Vector3(const Vector3< T > &src)
Definition: vmath.h:718
static Matrix4< T > createLookAt(const Vector3< T > &eyePos, const Vector3< T > &centerPos, const Vector3< T > &upDir)
Definition: vmath.h:2578
Matrix4< T > & operator=(const Matrix4< T > &rhs)
Definition: vmath.h:2995
const T & operator[](int n) const
Definition: vmath.h:286
Matrix4< T > transform() const
Definition: vmath.h:3664
void setTranslation(T nx, T ny, T nz)
Sets translation part of matrix.
Definition: vmath.h:2922
Vector3< T > operator+(T rhs) const
Definition: vmath.h:960
Matrix3< T > operator*(Matrix3< T > rhs) const
Definition: vmath.h:2170
Vector3< T > point(size_t i) const
Definition: vmath.h:4284
Vector2< T > operator/(T rhs) const
Definition: vmath.h:408
Vector4< T > & operator*=(const Vector4< T > &rhs)
Definition: vmath.h:1474
Quaternion(const Quaternion< T > &q)
Definition: vmath.h:3365
Vector3< T > & operator*=(const Vector3< T > &rhs)
Definition: vmath.h:869
Matrix3(const Matrix3< FromT > &src)
Definition: vmath.h:1788
static Quaternion< T > fromMatrix(const Matrix4< T > &m)
Definition: vmath.h:3744
Vector4< T > operator-(const Vector4< T > &rhs) const
Definition: vmath.h:1421
T & operator[](int n)
Definition: vmath.h:775
Matrix3(const Matrix3< T > &src)
Definition: vmath.h:1778
Vector3< T > operator/(T rhs) const
Definition: vmath.h:987
Quaternion(const Quaternion< FromT > &q)
Definition: vmath.h:3374
bool operator==(const Quaternion< T > &rhs) const
Definition: vmath.h:3518
Vector2< T > operator/(const Vector2< T > &rhs) const
Definition: vmath.h:327
bool operator==(const Vector4< T > &rhs) const
Definition: vmath.h:1504
static Matrix4< T > createTranslation(T x, T y, T z, T w=1)
Creates translation matrix.
Definition: vmath.h:2542
T lengthSq() const
Definition: vmath.h:536
Quaternion< T > operator+(const Quaternion< T > &rhs) const
Definition: vmath.h:3428
void setTranslation(const Vector3< T > &v)
Definition: vmath.h:2908
Quaternion< T > & operator=(const Quaternion< T > &rhs)
Definition: vmath.h:3405
Vector3< T > operator*(T rhs) const
Definition: vmath.h:978
Matrix3< T > operator+(T rhs) const
Definition: vmath.h:2110
static Quaternion< T > fromEulerAngles(T x, T y, T z)
Definition: vmath.h:3591
Vector2< T > & operator-=(T rhs)
Definition: vmath.h:428
Vector3< T > & operator/=(const Vector3< T > &rhs)
Definition: vmath.h:881
void rotate(T ax, T ay, T az)
Definition: vmath.h:1113
Quaternion< T > slerp(T r, const Quaternion< T > &q2) const
Definition: vmath.h:3862
Vector3< T > lerp(T fact, const Vector3< T > &r) const
Definition: vmath.h:1139
static Matrix4< T > Perspective(T verticalAngle, T aspectRatio, T nearPlane, T farPlane)
set up a perspective projection matrix
Definition: vmath.h:2762
Vector3< T > xyz() const
Definition: vmath.h:1696
Vector2< T > & operator=(const Vector2< T > &rhs)
Definition: vmath.h:258
Vector2< T > & operator-=(const Vector2< T > &rhs)
Definition: vmath.h:347
std::string toString() const
Definition: vmath.h:3730
const T & operator[](int n) const
Definition: vmath.h:793
void extend(const Vector3< SrcT > &point)
Definition: vmath.h:4114
Definition: vmath.h:3922
Matrix3< T > operator-(T rhs) const
Definition: vmath.h:2122
Quaternion< T > & operator+=(const Quaternion< T > &rhs)
Definition: vmath.h:3470
Vector2< T > & operator/=(const Vector2< T > &rhs)
Definition: vmath.h:369
Vector3< T > crossProduct(const Vector3< T > &rhs) const
Definition: vmath.h:902
Vector4< T > lerp(T fact, const Vector4< T > &r) const
Definition: vmath.h:1664
Vector3< T > & operator-=(T rhs)
Definition: vmath.h:1008
const T & at(int x, int y) const
Definition: vmath.h:2872
const T & operator()(int i, int j) const
Definition: vmath.h:2033
Matrix4< T > & operator=(const T *rhs)
Definition: vmath.h:3019
Vector2< T > & operator*=(T rhs)
Definition: vmath.h:439
std::string toString() const
Definition: vmath.h:2290
Aabb3(const Vector3< SrcT > &point)
Definition: vmath.h:4039
Quaternion< T > operator*(T rhs) const
Definition: vmath.h:3451
Aabb3(SrcT x, SrcT y, SrcT z)
Definition: vmath.h:4066
Matrix4(const T *dt)
Definition: vmath.h:2332
T data[16]
Data stored in column major order.
Definition: vmath.h:2316
static Matrix3< T > fromColumnMajorArray(const FromT *arr)
Definition: vmath.h:1952
T & operator[](int n)
Definition: vmath.h:1374
Vector4(const Vector4< FromT > &src)
Definition: vmath.h:1329
Aabb3< T > extended(const Aabb3< SrcT > &box) const
Definition: vmath.h:4165
static Vector4< T > vectorProductMatrix(const Vector4< T > v, Matrix4< T > m)
vectorProductMatrix: Multiplication function
Definition: vmath.h:2444
Vector4< T > operator/(const Vector4< T > &rhs) const
Definition: vmath.h:1439
Aabb3< T > transformed(const Matrix4< T > &t) const
Definition: vmath.h:4295
bool intersects(const Aabb3< SrcT > &box) const
Definition: vmath.h:4204
static Matrix3< T > createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
Definition: vmath.h:1845
Vector2(const Vector2< T > &src)
Definition: vmath.h:224
Vector4< T > operator+(const Vector4< T > &rhs) const
Definition: vmath.h:1412
static Matrix4< T > createRotationAroundAxis(T xDeg, T yDeg, T zDeg)
Definition: vmath.h:2461
Vector3< T > operator-(const Vector3< T > &rhs) const
Definition: vmath.h:818
static Matrix3< T > fromRowMajorArray(const FromT *arr)
Definition: vmath.h:1935
Matrix3< T > operator-(const Matrix3< T > &rhs) const
Definition: vmath.h:2097
void setRotation(const Matrix3< T > &m)
Definition: vmath.h:2940
Vector4< T > operator-() const
Definition: vmath.h:1525
Vector3< T > operator*(const Vector3< T > &rhs) const
Definition: vmath.h:3123
Matrix3< T > transpose()
Definition: vmath.h:2191
Quaternion(T w_, T x, T y, T z)
Definition: vmath.h:3396
Vector2< T > operator+(T rhs) const
Definition: vmath.h:381
Matrix4< T > inverse()
Definition: vmath.h:3187
Vector3< T > operator-() const
Definition: vmath.h:1068
Vector4< T > operator-(T rhs) const
Definition: vmath.h:1545
bool operator==(const Vector3< T > &rhs) const
Definition: vmath.h:1048
Vector2< T > & operator+=(const Vector2< T > &rhs)
Definition: vmath.h:336
Aabb3< T > & operator=(const Aabb3< SrcT > &rhs)
Definition: vmath.h:4085
Vector3< T > operator+(const Vector3< T > &rhs) const
Definition: vmath.h:809
Vector2< T > & operator+=(T rhs)
Definition: vmath.h:417
Vector4< T > & operator+=(const Vector4< T > &rhs)
Definition: vmath.h:1448
T & operator()(int i, int j)
Definition: vmath.h:2884
Quaternion< T > & operator*=(T rhs)
Definition: vmath.h:3504
Quaternion(T w_, const Vector3< T > &v_)
Definition: vmath.h:3384
bool operator==(const Matrix3< T > &rhs) const
Definition: vmath.h:1971
Matrix4< T > lerp(T fact, const Matrix4< T > &rhs) const
Definition: vmath.h:3267
Quaternion< T > lerp(T fact, const Quaternion< T > &rhs) const
Definition: vmath.h:3713
bool operator==(const Matrix4< T > &rhs) const
Definition: vmath.h:2833
Vector4< T > operator*(const Vector4< T > rhs) const
Definition: vmath.h:1430
Matrix3(const T *dt)
Definition: vmath.h:1769
void extend(const Aabb3< SrcT > &box)
Definition: vmath.h:4132
Vector4< T > operator=(const Vector4< FromT > &rhs)
Definition: vmath.h:1358
Vector3< T > operator=(const Vector3< T > &rhs)
Definition: vmath.h:747
std::string toString() const
Definition: vmath.h:3318
T data[9]
Data stored in column major order.
Definition: vmath.h:1753
Vector4< T > & operator-=(T rhs)
Definition: vmath.h:1585
Quaternion< T > & operator*=(const Quaternion< T > &rhs)
Definition: vmath.h:3492
bool operator!=(const Vector4< T > &rhs) const
Definition: vmath.h:1515
Matrix3< T > & operator=(const T *rhs)
Definition: vmath.h:2068
Vector4< T > operator*(T rhs) const
Definition: vmath.h:1554
Vector2(T nx, T ny)
Definition: vmath.h:215
Vector2< T > operator-(T rhs) const
Definition: vmath.h:390
const T & at(int x, int y) const
Definition: vmath.h:2009
Aabb3(const Aabb3< SrcT > &src)
Definition: vmath.h:4075
Vector3(const Vector3< FromT > &src)
Definition: vmath.h:737
Quaternion< T > & operator-=(const Quaternion< T > &rhs)
Definition: vmath.h:3481
Vector4< T > & operator-=(const Vector4< T > &rhs)
Definition: vmath.h:1461
Vector4< T > & operator/=(T rhs)
Definition: vmath.h:1611
Vector3< T > max
Definition: vmath.h:4024
Vector4< T > & operator/=(const Vector4< T > &rhs)
Definition: vmath.h:1487
Quaternion< T > operator~() const
Definition: vmath.h:3548
Matrix4< T > operator+(const Matrix4< T > &rhs) const
Definition: vmath.h:3036
void setScale(const Vector3< T > &s)
Definition: vmath.h:2984
static Matrix4< T > Perspective_vmath(T fovy, T aspect, T n, T f)
set up a perspective projection matrix
Definition: vmath.h:2736
static Matrix4< T > createScale(T sx, T sy, T sz)
Definition: vmath.h:2561
Matrix4(const Matrix4< FromT > &src)
Definition: vmath.h:2387
bool operator!=(const Matrix4< T > &rhs) const
Definition: vmath.h:2849
Matrix4< T > operator*(Matrix4< T > rhs) const
Definition: vmath.h:3134
bool intersects(const Vector3< SrcT > &point) const
Definition: vmath.h:4178
std::string toString() const
Definition: vmath.h:1182
Matrix4< T > operator-(const Matrix4< T > &rhs) const
Definition: vmath.h:3048
Vector3< T > extent() const
Definition: vmath.h:4259
T & at(int x, int y)
Definition: vmath.h:2860
static Quaternion< T > fromMatrix(const Matrix3< T > &m)
Definition: vmath.h:3803
Vector2< T > operator-(const Vector2< T > &rhs) const
Definition: vmath.h:309
bool valid() const
Definition: vmath.h:4099
Vector2(const Vector2< FromT > &src)
Definition: vmath.h:235
Vector4< T > operator+(T rhs) const
Definition: vmath.h:1536
Quaternion< T > operator-(const Quaternion< T > &rhs) const
Definition: vmath.h:3460
Aabb3< T > operator|(const Aabb3< RhsT > &rhs) const
Definition: vmath.h:4384
Vector4(T nx, T ny, T nz)
Definition: vmath.h:1300
Vector3< T > operator/(const Vector3< T > &rhs) const
Definition: vmath.h:836
Matrix4< T > & operator=(const Matrix4< FromT > &rhs)
Definition: vmath.h:3006
Matrix4< T > operator/(T rhs) const
Definition: vmath.h:3097
bool operator!=(const Quaternion< T > &rhs) const
Definition: vmath.h:3529
Vector3< T > operator=(const Vector3< FromT > &rhs)
Definition: vmath.h:760
Aabb3< T > & operator*=(const Matrix4< T > &rhs)
Definition: vmath.h:4347
Vector4< T > operator=(const Vector4< T > &rhs)
Definition: vmath.h:1344
static Matrix4< T > fromRowMajorArray(const FromT *arr)
Definition: vmath.h:2795
Vector4< T > operator*(const Vector4< T > &rhs) const
Definition: vmath.h:3110
Matrix4< T > operator*(T rhs) const
Definition: vmath.h:3085
Vector2< T > lerp(T fact, const Vector2< T > &r) const
Definition: vmath.h:551
Matrix3< T > operator*(T rhs) const
Definition: vmath.h:2134
Matrix3< T > lerp(T fact, const Matrix3< T > &rhs) const
Definition: vmath.h:2213
Matrix4(const T dt[4][4])
Definition: vmath.h:2340
Vector4< T > & operator+=(T rhs)
Definition: vmath.h:1572
Matrix4< T > operator+(T rhs) const
Definition: vmath.h:3061
static Vector3< T > vectorProductMatrix(const Vector3< T > v, Matrix3< T > m)
vectorProductMatrix: Multiplication function
Definition: vmath.h:1827
void setScale(T sx, T sy, T sz)
Definition: vmath.h:2973
bool operator!=(const Matrix3< T > &rhs) const
Definition: vmath.h:1986
Vector2< T > operator-() const
Definition: vmath.h:487
Vector4< T > operator/(T rhs) const
Definition: vmath.h:1563
Vector4(const Vector4< T > &src)
Definition: vmath.h:1319
Vector3< T > getScale() const
Definition: vmath.h:2955
std::string toString() const
Definition: vmath.h:592
Aabb3< T > operator*(const Matrix4< T > &rhs) const
Definition: vmath.h:4337
Matrix3< T > rotMatrix()
Definition: vmath.h:3615
bool operator==(const Vector2< T > &rhs) const
Definition: vmath.h:467
static Matrix4< T > fromColumnMajorArray(const FromT *arr)
Definition: vmath.h:2813
bool operator!=(const Vector2< T > &rhs) const
Definition: vmath.h:477
Vector3< T > & operator+=(const Vector3< T > &rhs)
Definition: vmath.h:845
Vector3< T > operator*(const Vector3< T > &rhs) const
Definition: vmath.h:827
const T & operator()(int i, int j) const
Definition: vmath.h:2896
Matrix3< T > & operator=(const Matrix3< FromT > &rhs)
Definition: vmath.h:2055
Vector3< T > operator-(T rhs) const
Definition: vmath.h:969
Matrix4< T > operator-(T rhs) const
Definition: vmath.h:3073
const T & operator[](int n) const
Definition: vmath.h:1394
Vector3< T > vectorProductMatrix(const Vector3< T > &v) const
vectorProductMatrix: Multiplication function
Definition: vmath.h:1810
static Matrix4< T > createFrustum(T left, T right, T bottom, T top, T zNear, T zFar)
Definition: vmath.h:2624
Vector3< T > size() const
Definition: vmath.h:4266
Quaternion< T > operator-() const
Definition: vmath.h:3539
Vector3< T > & operator/=(T rhs)
Definition: vmath.h:1032
Vector3< T > min
Definition: vmath.h:4019
T & operator()(int i, int j)
Definition: vmath.h:2021
Matrix4(const Matrix4< T > &src)
Definition: vmath.h:2366
T & at(int x, int y)
Definition: vmath.h:1997
Vector3< T > & operator+=(T rhs)
Definition: vmath.h:996
Vector2< T > & operator=(const Vector2< FromT > &rhs)
Definition: vmath.h:247
bool operator!=(const Aabb3< RhsT > &rhs) const
Definition: vmath.h:4327
Vector2< T > operator*(const Vector2< T > &rhs) const
Definition: vmath.h:318
Quaternion< T > & operator=(const Quaternion< FromT > &rhs)
Definition: vmath.h:3417
Vector4< T > vectorProductMatrix(const Vector4< T > &v) const
vectorProductMatrix: Multiplication function
Definition: vmath.h:2427
Matrix3< T > operator+(const Matrix3< T > &rhs) const
Definition: vmath.h:2085
Aabb3(SrcT x0, SrcT y0, SrcT z0, SrcT x1, SrcT y1, SrcT z1)
Definition: vmath.h:4054
Vector2< T > & operator*=(const Vector2< T > &rhs)
Definition: vmath.h:358
Vector3(const T v[3])
Definition: vmath.h:727
Vector3< T > center() const
Definition: vmath.h:4252
Matrix4< T > transpose()
Definition: vmath.h:3245
static Matrix4< T > createOrtho(T left, T right, T bottom, T top, T zNear, T zFar)
Definition: vmath.h:2682
static Matrix4< T > createMat4FromMat3(Matrix3< T > n)
createMat4FromMat3 Creates 4x4 Matrix from 3x3 Matrix (Homogeneous Matrix).
Definition: vmath.h:2412
Matrix3< T > & operator=(const Matrix3< T > &rhs)
Definition: vmath.h:2044
Vector2< T > & operator/=(T rhs)
Definition: vmath.h:450
Vector3< T > & operator-=(const Vector3< T > &rhs)
Definition: vmath.h:857
Quaternion< T > operator*(const Quaternion< T > &rhs) const
Definition: vmath.h:3438