All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
VectorMath.h
1 // Copyright eeGeo Ltd (2012-2014), All Rights Reserved
2 
3 #pragma once
4 
5 #include "MathFunc.h"
6 #include "Types.h"
7 #include "VectorMathDecl.h"
8 #include <cmath>
9 
10 #include <cstring>
11 
12 namespace Eegeo
13 {
14  class v2
15  {
16  public:
17  float x;
18  float y;
19 
20  v2 ( ) {};
21  v2 ( const v2& src );
22  v2 ( float x, float y );
23  v2 ( float* pSrc );
24 
25  void Set ( const v2& src );
26  void Set ( float x, float y );
27  void SetZero ();
28 
29  float GetX () const;
30  float GetY () const;
31 
32  void SetX ( float data );
33  void SetY ( float data );
34 
35  v2 SplatX () const;
36  v2 SplatY () const;
37 
38  static v2 Add ( const v2& src1, const v2& src2 );
39  static v2 Sub ( const v2& src1, const v2& src2 );
40  static v2 Mul ( const v2& src1, const v2& src2 );
41  static v2 Div ( const v2& src1, const v2& src2 );
42 
43  static v2 Scale ( const v2& src1, float scale );
44  static v2 InvScale ( const v2& src1, float scale );
45 
46  static v2 Neg ( const v2& src );
47  static v2 Abs(const v2& src);
48 
49  static float Dot ( const v2& src1, const v2& src2 );
50 
51  static v2 Lerp ( const v2& src1, const v2& src2, float ratio );
52 
53  float Length () const;
54  float LengthSq () const;
55  v2 Norm () const;
56 
57  bool operator== (const v2 &rhs) const;
58  bool operator!= (const v2 &rhs) const;
59 
60  v2 operator - ( ) const;
61 
62  v2 operator + ( const v2& rhs ) const;
63  v2 operator - ( const v2& rhs ) const;
64  v2 operator * ( const v2& rhs ) const;
65  v2 operator / ( const v2& rhs ) const;
66  v2 operator * ( const float rhs ) const;
67  v2 operator / ( const float rhs ) const;
68 
69  v2& operator += ( const v2& rhs );
70  v2& operator -= ( const v2& rhs );
71  v2& operator *= ( const v2& rhs );
72  v2& operator /= ( const v2& rhs );
73 
74  v2& operator *= ( const float rhs );
75  v2& operator /= ( const float rhs );
76 
77  static v2 Min( const v2& src1, const v2& src2 );
78  static v2 Max( const v2& src1, const v2& src2 );
79  static bool IsFinite(const v2& v);
80 
81 
82  static const v2 Zero();
83  static const v2 One();
84  };
85 
86  v2 operator * ( const float lhs, const v2& rhs);
87 
88  class dv2
89  {
90  public:
91  double x;
92  double y;
93 
94  dv2 ( ) {};
95  dv2 ( const dv2& src );
96  dv2 ( double x, double y );
97  dv2 ( double* pSrc );
98 
99  void Set ( const dv2& src );
100  void Set ( double x, double y );
101  void SetZero ();
102 
103  double GetX () const;
104  double GetY () const;
105 
106  void SetX ( double data );
107  void SetY ( double data );
108 
109  dv2 SplatX () const;
110  dv2 SplatY () const;
111 
112  static dv2 Add ( const dv2& src1, const dv2& src2 );
113  static dv2 Sub ( const dv2& src1, const dv2& src2 );
114  static dv2 Mul ( const dv2& src1, const dv2& src2 );
115  static dv2 Div ( const dv2& src1, const dv2& src2 );
116 
117  static dv2 Scale ( const dv2& src1, double scale );
118  static dv2 InvScale ( const dv2& src1, double scale );
119 
120  static dv2 Neg ( const dv2& src );
121 
122  static double Dot ( const dv2& src1, const dv2& src2 );
123 
124  static dv2 Lerp ( const dv2& src1, const dv2& src2, double ratio );
125 
126  double Length () const;
127  double LengthSq () const;
128  dv2 Norm () const;
129 
130  static v2 ToSingle( const dv2& src);
131  v2 ToSingle() const;
132 
133  dv2 operator - ( ) const;
134 
135  dv2 operator + ( const dv2& rhs ) const;
136  dv2 operator - ( const dv2& rhs ) const;
137  dv2 operator * ( const dv2& rhs ) const;
138  dv2 operator / ( const dv2& rhs ) const;
139  dv2 operator * ( const double rhs ) const;
140  dv2 operator / ( const double rhs ) const;
141 
142  dv2& operator += ( const dv2& rhs );
143  dv2& operator -= ( const dv2& rhs );
144  dv2& operator *= ( const dv2& rhs );
145  dv2& operator /= ( const dv2& rhs );
146 
147  dv2& operator *= ( const double rhs );
148  dv2& operator /= ( const double rhs );
149 
150  static dv2 Min( const dv2& src1, const dv2& src2 );
151  static dv2 Max( const dv2& src1, const dv2& src2 );
152  static bool IsFinite(const dv2& v);
153 
154  static const dv2 Zero();
155  static const dv2 One();
156  };
157 
158  dv2 operator * ( const double lhs, const dv2& );
159 
160  class v3
161  {
162  public:
163  float x;
164  float y;
165  float z;
166 
167  friend class v4;
168  friend class dv4;
169  friend class dv3;
170  friend class m44;
171  friend class m33;
172  friend class dm44;
173  friend class dm33;
174 
175  v3 ( ) {};
176 
177  v3 ( const v3& src );
178  v3 ( float x, float y, float z );
179  v3 ( const float* pSrc );
180  v3 ( const v4& src );
181  v3& operator= (const v3& rhs);
182 
183  void Set ( float x, float y, float z );
184  void SetZero ();
185 
186  float GetX () const;
187  float GetY () const;
188  float GetZ () const;
189 
190  void SetX ( float data );
191  void SetY ( float data );
192  void SetZ ( float data );
193 
194  v3 SplatX () const;
195  v3 SplatY () const;
196  v3 SplatZ () const;
197 
198  static v3 Add ( const v3& src1, const v3& src2 );
199  static v3 Sub ( const v3& src1, const v3& src2 );
200  static v3 Mul ( const v3& src1, const v3& src2 );
201  static v3 Div ( const v3& src1, const v3& src2 );
202 
203  static v3 Scale ( const v3& src1, float scale );
204  static v3 InvScale ( const v3& src1, float scale );
205 
206  static v3 Neg ( const v3& src );
207 
208  static float Dot ( const v3& src1, const v3& src2 );
209  static v3 Cross ( const v3& src1, const v3& src2 );
210 
211  static v3 Lerp ( const v3& src1, const v3& src2, float ratio );
212 
213  static v3 Mul ( const v3& src, const m33& matrix );
214  static v3 Mul ( const v3& src, const m44& matrix );
215  static v3 MulRotate ( const v3& src, const m44& matrix );
216  static v3 Clamped ( const v3& src, float maxLength);
217  static v3 Abs(const v3& src);
218  inline float SquareDistanceTo( const v3& a ) const;
219 
220  float Length () const;
221  float LengthSq () const;
222  v3 Norm () const;
223 
224  bool operator== (const v3 &rhs) const;
225  bool operator!= (const v3 &rhs) const;
226 
227  v3 operator - ( ) const;
228 
229  v3 operator + ( const v3& rhs ) const;
230  v3 operator - ( const v3& rhs ) const;
231  v3 operator * ( const v3& rhs ) const;
232  v3 operator / ( const v3& rhs ) const;
233  v3 operator * ( const float rhs ) const;
234  v3 operator / ( const float rhs ) const;
235 
236  v3& operator += ( const v3& rhs );
237  v3& operator -= ( const v3& rhs );
238  v3& operator *= ( const v3& rhs );
239  v3& operator /= ( const v3& rhs );
240 
241  v3& operator *= ( const float rhs );
242  v3& operator /= ( const float rhs );
243 
244  static v3 Min( const v3& src1, const v3& src2 );
245  static v3 Max( const v3& src1, const v3& src2 );
246  static bool IsFinite(const v3& v);
247 
248  static const v3 Zero();
249  static const v3 One();
250  };
251 
252  v3 operator * ( const float lhs, const v3& rhs);
253 
254  class dv3
255  {
256  public:
257  double x;
258  double y;
259  double z;
260 
261  friend class v4;
262  friend class dv4;
263  friend class m44;
264  friend class dm44;
265  friend class dm33;
266 
267  dv3 ( ) : x(0.0), y(0.0), z(0.0) {};
268 
269  dv3 ( const dv3& src );
270  dv3 ( double x, double y, double z );
271  dv3 ( const double* pSrc );
272  dv3 ( const v4& src );
273  dv3 ( const dv4& src );
274  dv3 ( const v3& src );
275 
276  void Set ( double x, double y, double z );
277  void SetZero ();
278 
279  double GetX () const;
280  double GetY () const;
281  double GetZ () const;
282 
283  void SetX ( double data );
284  void SetY ( double data );
285  void SetZ ( double data );
286 
287  dv3 SplatX () const;
288  dv3 SplatY () const;
289  dv3 SplatZ () const;
290 
291  static dv3 Add ( const dv3& src1, const dv3& src2 );
292  static dv3 Sub ( const dv3& src1, const dv3& src2 );
293  static dv3 Mul ( const dv3& src1, const dv3& src2 );
294  static dv3 Div ( const dv3& src1, const dv3& src2 );
295 
296  static dv3 Add ( const dv3& src1, const v3& src2 );
297  static dv3 Sub ( const dv3& src1, const v3& src2 );
298  static dv3 Mul ( const dv3& src1, const v3& src2 );
299  static dv3 Div ( const dv3& src1, const v3& src2 );
300 
301  static dv3 Scale ( const dv3& src1, double scale );
302  static dv3 InvScale ( const dv3& src1, double scale );
303 
304  static dv3 Neg ( const dv3& src );
305 
306  static double Dot ( const dv3& src1, const dv3& src2 );
307  static dv3 Cross ( const dv3& src1, const dv3& src2 );
308 
309  static dv3 Lerp ( const dv3& src1, const dv3& src2, double ratio );
310 
311  static dv3 Mul ( const dv3& src, const m33& matrix );
312  static dv3 Mul ( const dv3& src, const m44& matrix );
313  static dv3 MulRotate ( const dv3& src, const m44& matrix );
314  static dv3 Mul ( const dv3& src, const dm33& matrix);
315  static dv3 Mul ( const dv3& src, const dm44& matrix );
316  static dv3 MulRotate ( const dv3& src, const dm44& matrix );
317 
318  static v3 ToSingle( const dv3& src);
319  static dv3 FromSingle( const v3& src);
320 
321  inline double SquareDistanceTo(const dv3& a) const;
322 
323  double Length () const;
324  double LengthSq () const;
325  dv3 Norm () const;
326  dv3& Normalise();
327  v3 ToSingle() const;
328 
329  dv3 operator - ( ) const;
330 
331  dv3 operator + ( const dv3& rhs ) const;
332  dv3 operator - ( const dv3& rhs ) const;
333  dv3 operator * ( const dv3& rhs ) const;
334  dv3 operator / ( const dv3& rhs ) const;
335  dv3 operator * ( const double rhs ) const;
336  dv3 operator / ( const double rhs ) const;
337 
338  dv3& operator += ( const dv3& rhs );
339  dv3& operator -= ( const dv3& rhs );
340  dv3& operator *= ( const dv3& rhs );
341  dv3& operator /= ( const dv3& rhs );
342 
343  dv3& operator *= ( const double rhs );
344  dv3& operator /= ( const double rhs );
345 
346  dv3 operator + ( const v3& rhs ) const;
347  dv3 operator - ( const v3& rhs ) const;
348  dv3 operator * ( const v3& rhs ) const;
349  dv3 operator / ( const v3& rhs ) const;
350  dv3 operator * ( const float rhs ) const;
351  dv3 operator / ( const float rhs ) const;
352 
353  dv3& operator += ( const v3& rhs );
354  dv3& operator -= ( const v3& rhs );
355  dv3& operator *= ( const v3& rhs );
356  dv3& operator /= ( const v3& rhs );
357 
358  static dv3 Min( const dv3& src1, const dv3& src2 );
359  static dv3 Max( const dv3& src1, const dv3& src2 );
360  static bool IsFinite(const dv3& v);
361 
362 
363  static const dv3 Zero();
364  static const dv3 One();
365  };
366 
367  dv3 operator * ( const double lhs, const dv3& rhs);
368 
369  class v4
370  {
371  public:
372  float x;
373  float y;
374  float z;
375  float w;
376 
377  friend class dv3;
378  friend class dv4;
379  friend class v3;
380  friend class Quaternion;
381  friend class m44;
382  friend class dm33;
383 
384  v4 () {}
385 
386  v4 ( const v4& src );
387  v4 ( float x, float y, float z, float w );
388  v4 ( const v3& src, float w );
389 
390  void Set ( float x, float y, float z, float w );
391  void SetZero ();
392 
393  float GetX () const;
394  float GetY () const;
395  float GetZ () const;
396  float GetW () const;
397 
398  void SetX ( float data );
399  void SetY ( float data );
400  void SetZ ( float data );
401  void SetW ( float data );
402 
403  v4 SplatX () const;
404  v4 SplatY () const;
405  v4 SplatZ () const;
406  v4 SplatW () const;
407 
408  void Load ( const float* pSrc );
409  void Store ( float* pDest ) const;
410 
411 
412  static v4 Add ( const v4& src1, const v4& src2 );
413  static v4 Sub ( const v4& src1, const v4& src2 );
414  static v4 Mul ( const v4& src1, const v4& src2 );
415  static v4 Div ( const v4& src1, const v4& src2 );
416  static v4 Dot ( const v4& src1, const v4& src2 );
417 
418  static v4 Lerp ( const v4& src1, const v4& src2, float ratio );
419 
420  static v4 Mul ( const v4& src, const m44& matrix );
421  static v4 MulTransposed ( const v4& src, const m44& matrix );
422  static v4 Neg(const v4& src);
423 
424  v4 Length () const;
425  v4 LengthSq () const;
426  v4 Norm () const;
427 
428  bool operator== (const v4 &rhs) const;
429  bool operator!= (const v4 &rhs) const;
430 
431  v4 operator - () const;
432 
433  v4 operator + ( const v4& rhs ) const;
434  v4 operator - ( const v4& rhs ) const;
435  v4 operator * ( const v4& rhs ) const;
436  v4 operator / ( const v4& rhs ) const;
437  v4 operator * ( const float rhs ) const;
438  v4 operator / ( const float rhs ) const;
439 
440  v4& operator += ( const v4& rhs );
441  v4& operator -= ( const v4& rhs );
442  v4& operator *= ( const v4& rhs );
443  v4& operator /= ( const v4& rhs );
444 
445  v4& operator *= ( const float rhs );
446  v4& operator /= ( const float rhs );
447 
448  static v4 Min( const v4& src1, const v4& src2 );
449  static v4 Max( const v4& src1, const v4& src2 );
450  static bool IsFinite(const v4& v);
451 
452  const static v4 Zero();
453  const static v4 One();
454  };
455 
456 
457 
458  class dv4
459  {
460  public:
461  double x;
462  double y;
463  double z;
464  double w;
465 
466  friend class v4;
467  friend class dv3;
468 
469  friend class m44;
470  friend class dm44;
471  friend class dm33;
472 
473  dv4 ( ) {};
474 
475  dv4 ( const dv4& src );
476  dv4 ( double x, double y, double z, double w );
477  dv4 ( const double* pSrc );
478  dv4 ( const v3& src );
479  //dv4 ( const dv3& src );
480  dv4 ( const dv3& src, double w );
481  dv4 ( const v4& src );
482 
483  void Set ( double x, double y, double z, double w );
484  void SetZero ();
485 
486 
487  double GetX () const;
488  double GetY () const;
489  double GetZ () const;
490  double GetW () const;
491 
492  void SetX ( double data );
493  void SetY ( double data );
494  void SetZ ( double data );
495  void SetW ( double data );
496 
497  dv4 SplatX () const;
498  dv4 SplatY () const;
499  dv4 SplatZ () const;
500  dv4 SplatW () const;
501 
502  static dv4 Add ( const dv4& src1, const dv4& src2 );
503  static dv4 Sub ( const dv4& src1, const dv4& src2 );
504  static dv4 Mul ( const dv4& src1, const dv4& src2 );
505  static dv4 Div ( const dv4& src1, const dv4& src2 );
506 
507  static dv4 Add ( const dv4& src1, const v4& src2 );
508  static dv4 Sub ( const dv4& src1, const v4& src2 );
509  static dv4 Mul ( const dv4& src1, const v4& src2 );
510  static dv4 Div ( const dv4& src1, const v4& src2 );
511 
512  static dv4 Scale ( const dv4& src1, double scale );
513  static dv4 InvScale ( const dv4& src1, double scale );
514 
515  static dv4 Neg ( const dv4& src );
516 
517  static double Dot ( const dv4& src1, const dv4& src2 );
518 
519  static dv4 Lerp ( const dv4& src1, const dv4& src2, double ratio );
520 
521  static dv4 Mul ( const dv4& src, const m44& matrix );
522  static dv4 MulTransposed ( const dv4& src, const m44& matrix );
523 
524  static v4 ToSingle( const dv4& src);
525  static dv4 FromSingle( const v4& src);
526 
527  double Length () const;
528  double LengthSq () const;
529  dv4 Norm () const;
530  dv4& Normalise();
531  v4 ToSingle() const;
532 
533  dv4 operator - ( ) const;
534 
535  dv4 operator + ( const dv4& rhs ) const;
536  dv4 operator - ( const dv4& rhs ) const;
537  dv4 operator * ( const dv4& rhs ) const;
538  dv4 operator / ( const dv4& rhs ) const;
539  dv4 operator * ( const double rhs ) const;
540  dv4 operator / ( const double rhs ) const;
541 
542  dv4& operator += ( const dv4& rhs );
543  dv4& operator -= ( const dv4& rhs );
544  dv4& operator *= ( const dv4& rhs );
545  dv4& operator /= ( const dv4& rhs );
546 
547  dv4& operator *= ( const double rhs );
548  dv4& operator /= ( const double rhs );
549 
550  dv4 operator + ( const v4& rhs ) const;
551  dv4 operator - ( const v4& rhs ) const;
552  dv4 operator * ( const v4& rhs ) const;
553  dv4 operator / ( const v4& rhs ) const;
554  dv4 operator * ( const float rhs ) const;
555  dv4 operator / ( const float rhs ) const;
556 
557  dv4& operator += ( const v4& rhs );
558  dv4& operator -= ( const v4& rhs );
559  dv4& operator *= ( const v4& rhs );
560  dv4& operator /= ( const v4& rhs );
561 
562  static dv4 Min( const dv4& src1, const dv4& src2 );
563  static dv4 Max( const dv4& src1, const dv4& src2 );
564  static bool IsFinite(const dv4& v);
565 
566  static const dv4 Zero();
567  static const dv4 One();
568  };
569 
570  class m33
571  {
572  public:
573  float _11, _12, _13;
574  float _21, _22, _23;
575  float _31, _32, _33;
576 
577  public:
578  m33 ();
579  m33 ( const m44& src );
580  void Identity ();
581  static void Transpose ( m33& dst, const m33& src );
582 
583  bool Decompose (v3& outScale, Quaternion& outRotation);
584  static bool Decompose (const m33& src, v3& outScale, Quaternion& outRotation);
585 
586  void SetRow ( int row, const v3& data );
587  void SetFromBasis(const v3& right, const v3& up, const v3& forward);
588  v3 GetRow ( int row ) const;
589 
590  void Scale ( float scale );
591  void Scale ( const v3& scale );
592 
593  void RotateX ( float theta );
594  void RotateY ( float theta );
595  void RotateZ ( float theta );
596  void Rotate ( float x, float y, float z );
597  void Rotate ( const v3& axis, float angle );
598 
599  static void Inverse ( m33& dst, const m33& src );
600  static void Mul ( m33& dst, const m33& src1, const m33& src2 );
601  void Mul ( const float scale );
602 
603  m44 ToM44() const;
604 
605  static m33 CreateIdentity();
606  };
607 
608  class dm33
609  {
610  public:
611  double _11, _12, _13;
612  double _21, _22, _23;
613  double _31, _32, _33;
614 
615  friend class dv3;
616  friend class dv4;
617  friend class v3;
618  friend class v4;
619  friend class m33;
620  friend class Quaternion;
621 
622  public:
623  dm33 ();
624  dm33 ( const dm44& src );
625  void Identity ();
626  static void Transpose ( dm33& dst, const dm33& src );
627 
628  bool Decompose (dv3& outScale, Quaternion& outRotation);
629  static bool Decompose (const dm33& src, dv3& outScale, Quaternion& outRotation);
630 
631  void SetRow ( int row, const dv3& data );
632  void SetFromBasis(const dv3& right, const dv3& up, const dv3& forward);
633  dv3 GetRow ( int row ) const;
634 
635  void Scale ( double scale );
636  void Scale ( const dv3& scale );
637 
638  void RotateX ( double theta );
639  void RotateY ( double theta );
640  void RotateZ ( double theta );
641  void Rotate ( double x, double y, double z );
642  void Rotate ( const dv3& axis, double angle );
643 
644  static void Inverse ( dm33& dst, const dm33& src );
645  static void Mul ( dm33& dst, const dm33& src1, const dm33& src2 );
646  void Mul ( const double scale );
647  };
648 
649  class m44
650  {
651  v4 m_rows[4];
652 
653  public:
654  friend class dv3;
655  friend class dv4;
656  friend class v3;
657  friend class v4;
658  friend class m33;
659  friend class Quaternion;
660 
661  m44 () {}
662  m44 ( const v4& row0, const v4& row1, const v4& row2, const v4& row3 );
663  m44 (float values[16]);
664  void SetRow ( int row, const v4& data ) { m_rows[row] = data; }
665  void SetFromBasis(const v3& right, const v3& up, const v3& forward, const v3& pos);
666  const v4& GetRow ( int row ) const { return m_rows[row]; }
667 
668  void Load ( const float* pSrc );
669  void Store ( float* pDest ) const;
670 
671  bool operator== (const m44 &rhs) const;
672  bool operator!= (const m44 &rhs) const;
673 
674  void Identity ();
675  bool IsIdentity () const;
676  void Scale ( float scale );
677  void Scale ( const v3& scale );
678 
679  void OrthProjection (float left, float right, float top, float bottom, float near, float far, float scale);
680  void Projection (float fovyRadians, float aspect, float nearZ, float farZ);
681 
682  void RotateX ( float theta );
683  void RotateY ( float theta );
684  void RotateZ ( float theta );
685  void Rotate ( float x, float y, float z );
686  void Rotate ( const v3& axis, float angle );
687 
688  bool Decompose (v3& outScale, Quaternion& outRotation, v3& outTranslation);
689  static bool Decompose (const m44& src, v3& outScale, Quaternion& outRotation, v3& outTranslation);
690 
691  static void Transpose ( m44& dst, const m44& src );
692  static void Inverse ( m44& dst, const m44& src );
693  static void OrthoInverse( m44& dst, const m44& src );
694  static void Mul ( m44& dst, const m44& src1, const m44& src2 );
695  void Mul ( const float scale );
696  static void Lerp ( m44& dst, const m44& from, const m44& to, float t );
697 
698  static float Determinant33(const m44& src);
699 
700  static const m44 Zero();
701 
702  static m44 CreateIdentity();
703  static m44 CreateTranslate(const v3& translate);
704  static m44 CreateScale(const v3& scale);
705  static m44 CreateFromRowMajorElements( const float* pRowMajorElements );
706  };
707 
708  class dm44
709  {
710  dv4 m_rows[4];
711 
712  public:
713  friend class dv3;
714  friend class dv4;
715  friend class v3;
716  friend class v4;
717  friend class m33;
718  friend class dm33;
719  friend class Quaternion;
720 
721  dm44 () {}
722  dm44 ( const dv4& row0, const dv4& row1, const dv4& row2, const dv4& row3 );
723  void SetRow ( int row, const dv4& data ) { m_rows[row] = data; }
724  void SetFromBasis(const dv3& right, const dv3& up, const dv3& forward, const dv3& pos);
725  const dv4& GetRow ( int row ) const { return m_rows[row]; }
726 
727  void Identity ();
728  void Scale ( double scale );
729  void Scale ( const dv3& scale );
730 
731  void OrthProjection (double left, double right, double top, double bottom, double near, double far, double scale);
732  void Projection (double fovyRadians, double aspect, double nearZ, double farZ);
733 
734  void RotateX ( double theta );
735  void RotateY ( double theta );
736  void RotateZ ( double theta );
737  void Rotate ( double x, double y, double z );
738  void Rotate ( const dv3& axis, double angle );
739 
740  bool Decompose (dv3& outScale, Quaternion& outRotation, dv3& outTranslation);
741  static bool Decompose (const dm44& src, dv3& outScale, Quaternion& outRotation, dv3& outTranslation);
742 
743  static void Transpose ( dm44& dst, const dm44& src );
744  static void Inverse ( dm44& dst, const dm44& src );
745  static void OrthoInverse( dm44& dst, const dm44& src );
746  static void Mul ( dm44& dst, const dm44& src1, const dm44& src2 );
747  void Mul ( const double scale );
748  };
749 
750 
751 
752 
753  inline v2::v2( const v2& src )
754  {
755  x = src.x;
756  y = src.y;
757  }
758 
759  inline v2::v2( float _x, float _y )
760  {
761  x = _x;
762  y = _y;
763  }
764 
765  inline v2::v2( float* pSrc )
766  {
767  x = pSrc[0];
768  y = pSrc[1];
769  }
770 
771  inline void v2::Set( const v2& src )
772  {
773  x = src.x;
774  y = src.y;
775  }
776 
777  inline void v2::Set( float _x, float _y )
778  {
779  x = _x;
780  y = _y;
781  }
782 
783  inline void v2::SetZero()
784  {
785  x = 0.0f;
786  y = 0.0f;
787  }
788 
789  inline v2 v2::SplatX() const
790  {
791  return v2(x, x);
792  }
793 
794  inline v2 v2::SplatY() const
795  {
796  return v2(y, y);
797  }
798 
799  inline float v2::GetX() const
800  {
801  return x;
802  }
803 
804  inline float v2::GetY() const
805  {
806  return y;
807  }
808 
809  inline void v2::SetX( float data )
810  {
811  x = data;
812  }
813 
814  inline void v2::SetY( float data )
815  {
816  y = data;
817  }
818 
819 
820  inline v2 v2::Add( const v2& src1, const v2& src2 )
821  {
822  return v2(src1.x + src2.x, src1.y + src2.y);
823  }
824 
825  inline v2 v2::Sub( const v2& src1, const v2& src2 )
826  {
827  return v2(src1.x - src2.x, src1.y - src2.y);
828  }
829 
830  inline v2 v2::Mul( const v2& src1, const v2& src2 )
831  {
832  return v2(src1.x * src2.x, src1.y * src2.y);
833  }
834 
835  inline v2 v2::Div( const v2& src1, const v2& src2 )
836  {
837  return v2(src1.x / src2.x, src1.y / src2.y);
838  }
839 
840  inline float v2::Dot( const v2& src1, const v2& src2 )
841  {
842  float dot = src1.x * src2.x + src1.y * src2.y;
843  return dot;
844  }
845 
846  inline v2 v2::Lerp( const v2& src1, const v2& src2, float ratio )
847  {
848  v2 temp = src2 * ratio;
849  temp += src1 * (1.0f - ratio);
850 
851  return temp;
852  }
853 
854  inline v2 v2::Scale( const v2& src, float scale )
855  {
856  return v2(src.x * scale, src.y * scale);
857  }
858 
859  inline v2 v2::InvScale( const v2& src, float scale )
860  {
861  return v2(src.x / scale, src.y / scale);
862  }
863 
864  inline float v2::Length() const
865  {
866  float dot = x * x + y * y;
867  float length = Math::Sqrtf(dot);
868  return length;
869  }
870 
871  inline float v2::LengthSq() const
872  {
873  return Dot(*this, *this);
874  }
875 
876  inline v2 v2::Norm() const
877  {
878  float dot = x * x + y * y;
879  float length = Math::Sqrtf(dot);
880  return v2(x/length, y/length);
881  }
882 
883  inline v2 v2::Neg(const v2& src)
884  {
885  return v2(-src.x, -src.y);
886  }
887 
888  inline v2 v2::Abs(const v2& src)
889  {
890  return v2(Math::Abs(src.x), Math::Abs(src.y));
891  }
892 
893  inline bool v2::operator== (const v2 &rhs) const
894  {
895  return (x == rhs.x) && (y == rhs.y);
896  }
897 
898  inline bool v2::operator!= (const v2 &rhs) const
899  {
900  return (x != rhs.x) || (y != rhs.y);
901  }
902 
903  inline v2 v2::operator - () const
904  {
905  return Neg(*this);
906  }
907 
908  inline v2 v2::operator + ( const v2& rhs ) const
909  {
910  return Add(*this, rhs);
911  }
912 
913  inline v2 v2::operator - ( const v2& rhs ) const
914  {
915  return Sub(*this, rhs);
916  }
917 
918  inline v2 v2::operator * ( const v2& rhs ) const
919  {
920  return Mul(*this, rhs);
921  }
922 
923  inline v2 v2::operator * ( const float rhs ) const
924  {
925  return Scale(*this, rhs);
926  }
927 
928  inline v2 operator * ( const float lhs, const v2& rhs)
929  {
930  return v2::Scale(rhs, lhs);
931  }
932 
933  inline v2 v2::operator / ( const v2& rhs ) const
934  {
935  return Div(*this, rhs);
936  }
937 
938  inline v2 v2::operator / ( const float rhs ) const
939  {
940  return InvScale(*this, rhs);
941  }
942 
943  inline v2& v2::operator += ( const v2& rhs )
944  {
945  *this = Add(*this, rhs);
946  return *this;
947  }
948 
949  inline v2& v2::operator -= ( const v2& rhs )
950  {
951  *this = Sub(*this, rhs);
952  return *this;
953  }
954 
955  inline v2& v2::operator *= ( const v2& rhs )
956  {
957  *this = Mul(*this, rhs);
958  return *this;
959  }
960 
961  inline v2& v2::operator /= ( const v2& rhs )
962  {
963  *this = Div(*this, rhs);
964  return *this;
965  }
966 
967  inline v2& v2::operator *= ( const float rhs )
968  {
969  x *= rhs;
970  y *= rhs;
971  return *this;
972  }
973 
974  inline v2& v2::operator /= ( const float rhs )
975  {
976  x /= rhs;
977  y /= rhs;
978 
979  return *this;
980  }
981 
982  inline v2 v2::Min( const v2& src1, const v2& src2 )
983  {
984  return v2( src1.x < src2.x ? src1.x : src2.x,
985  src1.y < src2.y ? src1.y : src2.y );
986  }
987 
988  inline v2 v2::Max( const v2& src1, const v2& src2 )
989  {
990  return v2( src1.x > src2.x ? src1.x : src2.x,
991  src1.y > src2.y ? src1.y : src2.y );
992  }
993 
994  inline dv2::dv2( const dv2& src )
995  {
996  x = src.x;
997  y = src.y;
998  }
999 
1000  inline dv2::dv2( double _x, double _y )
1001  {
1002  x = _x;
1003  y = _y;
1004  }
1005 
1006  inline dv2::dv2( double* pSrc )
1007  {
1008  x = pSrc[0];
1009  y = pSrc[1];
1010  }
1011 
1012  inline void dv2::Set( const dv2& src )
1013  {
1014  x = src.x;
1015  y = src.y;
1016  }
1017 
1018  inline void dv2::Set( double _x, double _y )
1019  {
1020  x = _x;
1021  y = _y;
1022  }
1023 
1024  inline void dv2::SetZero()
1025  {
1026  x = 0.0f;
1027  y = 0.0f;
1028  }
1029 
1030  inline dv2 dv2::SplatX() const
1031  {
1032  return dv2(x, x);
1033  }
1034 
1035  inline dv2 dv2::SplatY() const
1036  {
1037  return dv2(y, y);
1038  }
1039 
1040  inline double dv2::GetX() const
1041  {
1042  return x;
1043  }
1044 
1045  inline double dv2::GetY() const
1046  {
1047  return y;
1048  }
1049 
1050  inline void dv2::SetX( double data )
1051  {
1052  x = data;
1053  }
1054 
1055  inline void dv2::SetY( double data )
1056  {
1057  y = data;
1058  }
1059 
1060 
1061  inline dv2 dv2::Add( const dv2& src1, const dv2& src2 )
1062  {
1063  return dv2(src1.x + src2.x, src1.y + src2.y);
1064  }
1065 
1066  inline dv2 dv2::Sub( const dv2& src1, const dv2& src2 )
1067  {
1068  return dv2(src1.x - src2.x, src1.y - src2.y);
1069  }
1070 
1071  inline dv2 dv2::Mul( const dv2& src1, const dv2& src2 )
1072  {
1073  return dv2(src1.x * src2.x, src1.y * src2.y);
1074  }
1075 
1076  inline dv2 dv2::Div( const dv2& src1, const dv2& src2 )
1077  {
1078  return dv2(src1.x / src2.x, src1.y / src2.y);
1079  }
1080 
1081  inline double dv2::Dot( const dv2& src1, const dv2& src2 )
1082  {
1083  double dot = src1.x * src2.x + src1.y * src2.y;
1084  return dot;
1085  }
1086 
1087  inline dv2 dv2::Lerp( const dv2& src1, const dv2& src2, double ratio )
1088  {
1089  dv2 temp = src2 * ratio;
1090  temp += src1 * (1.0f - ratio);
1091 
1092  return temp;
1093  }
1094 
1095  inline dv2 dv2::Scale( const dv2& src, double scale )
1096  {
1097  return dv2(src.x * scale, src.y * scale);
1098  }
1099 
1100  inline dv2 dv2::InvScale( const dv2& src, double scale )
1101  {
1102  return dv2(src.x / scale, src.y / scale);
1103  }
1104 
1105  inline double dv2::Length() const
1106  {
1107  double dot = x * x + y * y;
1108  double length = Math::Sqrtd(dot);
1109  return length;
1110  }
1111 
1112  inline double dv2::LengthSq() const
1113  {
1114  return Dot(*this, *this);
1115  }
1116 
1117  inline dv2 dv2::Norm() const
1118  {
1119  double invMagnitude = 1.0 / Length();
1120  return dv2(x * invMagnitude, y * invMagnitude);
1121  }
1122 
1123  inline v2 dv2::ToSingle( const dv2& src)
1124  {
1125  return v2(static_cast<float>(src.x), static_cast<float>(src.y));
1126  }
1127 
1128  inline v2 dv2::ToSingle() const
1129  {
1130  return ToSingle(*this);
1131  }
1132 
1133  inline dv2 dv2::Neg(const dv2& src)
1134  {
1135  return dv2(-src.x, -src.y);
1136  }
1137 
1138  inline dv2 dv2::operator - () const
1139  {
1140  return Neg(*this);
1141  }
1142 
1143  inline dv2 dv2::operator + ( const dv2& rhs ) const
1144  {
1145  return Add(*this, rhs);
1146  }
1147 
1148  inline dv2 dv2::operator - ( const dv2& rhs ) const
1149  {
1150  return Sub(*this, rhs);
1151  }
1152 
1153  inline dv2 dv2::operator * ( const dv2& rhs ) const
1154  {
1155  return Mul(*this, rhs);
1156  }
1157 
1158  inline dv2 dv2::operator * ( const double rhs ) const
1159  {
1160  return Scale(*this, rhs);
1161  }
1162 
1163  inline dv2 operator * ( const double lhs, const dv2& rhs )
1164  {
1165  return dv2::Scale(rhs, lhs);
1166  }
1167 
1168  inline dv2 dv2::operator / ( const dv2& rhs ) const
1169  {
1170  return Div(*this, rhs);
1171  }
1172 
1173  inline dv2 dv2::operator / ( const double rhs ) const
1174  {
1175  return InvScale(*this, rhs);
1176  }
1177 
1178  inline dv2& dv2::operator += ( const dv2& rhs )
1179  {
1180  *this = Add(*this, rhs);
1181  return *this;
1182  }
1183 
1184  inline dv2& dv2::operator -= ( const dv2& rhs )
1185  {
1186  *this = Sub(*this, rhs);
1187  return *this;
1188  }
1189 
1190  inline dv2& dv2::operator *= ( const dv2& rhs )
1191  {
1192  *this = Mul(*this, rhs);
1193  return *this;
1194  }
1195 
1196  inline dv2& dv2::operator /= ( const dv2& rhs )
1197  {
1198  *this = Div(*this, rhs);
1199  return *this;
1200  }
1201 
1202  inline dv2& dv2::operator *= ( const double rhs )
1203  {
1204  x *= rhs;
1205  y *= rhs;
1206  return *this;
1207  }
1208 
1209  inline dv2& dv2::operator /= ( const double rhs )
1210  {
1211  x /= rhs;
1212  y /= rhs;
1213 
1214  return *this;
1215  }
1216 
1217  inline dv2 dv2::Min( const dv2& src1, const dv2& src2 )
1218  {
1219  return dv2( src1.x < src2.x ? src1.x : src2.x,
1220  src1.y < src2.y ? src1.y : src2.y );
1221  }
1222 
1223  inline dv2 dv2::Max( const dv2& src1, const dv2& src2 )
1224  {
1225  return dv2( src1.x > src2.x ? src1.x : src2.x,
1226  src1.y > src2.y ? src1.y : src2.y );
1227  }
1228 
1229  inline v3::v3( const v3& src )
1230  {
1231  x = src.x;
1232  y = src.y;
1233  z = src.z;
1234  }
1235 
1236  inline v3::v3( float _x, float _y, float _z )
1237  {
1238  x = _x;
1239  y = _y;
1240  z = _z;
1241  }
1242 
1243  inline v3::v3( const float* pSrc )
1244  {
1245  x = pSrc[0];
1246  y = pSrc[1];
1247  z = pSrc[2];
1248  }
1249 
1250  inline v3::v3(const v4& src)
1251  {
1252  x = src.x;
1253  y = src.y;
1254  z = src.z;
1255  }
1256 
1257  inline v3& v3::operator= (const v3& rhs)
1258  {
1259  x = rhs.x;
1260  y = rhs.y;
1261  z = rhs.z;
1262  return *this;
1263  }
1264 
1265  inline bool v3::operator== (const v3 &rhs) const
1266  {
1267  return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
1268  }
1269 
1270  inline bool v3::operator!= (const v3 &rhs) const
1271  {
1272  return (x != rhs.x) || (y != rhs.y) || (z != rhs.z);
1273  }
1274 
1275  inline void v3::Set( float _x, float _y, float _z )
1276  {
1277  x = _x;
1278  y = _y;
1279  z = _z;
1280  }
1281 
1282  inline void v3::SetZero()
1283  {
1284  x = 0.0f;
1285  y = 0.0f;
1286  z = 0.0f;
1287  }
1288 
1289  inline v3 v3::SplatX() const
1290  {
1291  return v3(x, x, x);
1292  }
1293 
1294  inline v3 v3::SplatY() const
1295  {
1296  return v3(y, y, y);
1297  }
1298 
1299  inline v3 v3::SplatZ() const
1300  {
1301  return v3(z, z, z);
1302  }
1303 
1304  inline float v3::GetX() const
1305  {
1306  return x;
1307  }
1308 
1309  inline float v3::GetY() const
1310  {
1311  return y;
1312  }
1313 
1314  inline float v3::GetZ() const
1315  {
1316  return z;
1317  }
1318 
1319  inline void v3::SetX( float data )
1320  {
1321  x = data;
1322  }
1323 
1324  inline void v3::SetY( float data )
1325  {
1326  y = data;
1327  }
1328 
1329  inline void v3::SetZ( float data )
1330  {
1331  z = data;
1332  }
1333 
1334  inline v3 v3::Add( const v3& src1, const v3& src2 )
1335  {
1336  return v3(src1.x + src2.x, src1.y + src2.y, src1.z + src2.z);
1337  }
1338 
1339  inline v3 v3::Sub( const v3& src1, const v3& src2 )
1340  {
1341  return v3(src1.x - src2.x, src1.y - src2.y, src1.z - src2.z);
1342  }
1343 
1344  inline v3 v3::Mul( const v3& src1, const v3& src2 )
1345  {
1346  return v3(src1.x * src2.x, src1.y * src2.y, src1.z * src2.z);
1347  }
1348 
1349  inline v3 v3::Div( const v3& src1, const v3& src2 )
1350  {
1351  return v3(src1.x / src2.x, src1.y / src2.y, src1.z / src2.z);
1352  }
1353 
1354  inline float v3::Dot( const v3& src1, const v3& src2 )
1355  {
1356 
1357  float dot = src1.x * src2.x + src1.y * src2.y + src1.z * src2.z;
1358  return dot;
1359 
1360  }
1361 
1362  inline v3 v3::Lerp( const v3& src1, const v3& src2, float ratio )
1363  {
1364  v3 temp = src2 * ratio;
1365  temp += src1 * (1.0f - ratio);
1366 
1367  return temp;
1368  }
1369 
1370  inline v3 v3::Cross( const v3& src1, const v3& src2 )
1371  {
1372  return v3( src1.y * src2.z - src1.z * src2.y,
1373  src1.z * src2.x - src1.x * src2.z,
1374  src1.x * src2.y - src1.y * src2.x );
1375  }
1376 
1377  inline v3 v3::Mul (const v3& src, const m33& matrix)
1378  {
1379 
1380 #if 0
1381  v3 destVector;
1382  int tmp;
1383  asm volatile (
1384  "mov %3, #12 \n\t"
1385  "vld1.32 {d0, d1}, [%1] \n\t"
1386  "vld1.32 {d2, d3}, [%0], %3 \n\t"
1387  "vld1.32 {d4, d5}, [%0], %3 \n\t"
1388  "vld1.32 {d6, d7}, [%0], %3 \n\t"
1389 
1390  "vmul.f32 q9, q1, d0[0] \n\t"
1391  "vmla.f32 q9, q2, d0[1] \n\t"
1392  "vmla.f32 q9, q3, d1[0] \n\t"
1393  "vmov.f32 q0, q9 \n\t"
1394 
1395  "vst1.32 d0, [%2]! \n\t"
1396  "fsts s2, [%2] \n\t"
1397 
1398  : "+r"(&matrix), "+r"(&src), "+r"(destVector), "+r"(tmp):
1399  : "q0", "q9", "q10","q11", "q12", "q13", "memory"
1400  );
1401 
1402  return destVector;
1403 #else
1404 
1405  v3 tempMatrix;
1406 
1407  tempMatrix.x = matrix._11*src.x + matrix._21*src.y + matrix._31*src.z;
1408  tempMatrix.y = matrix._12*src.x + matrix._22*src.y + matrix._32*src.z;
1409  tempMatrix.z = matrix._13*src.x + matrix._23*src.y + matrix._33*src.z;
1410 
1411  return tempMatrix;
1412 
1413 #endif
1414 
1415  }
1416 
1417  inline v3 v3::Mul( const v3& src, const m44& matrix )
1418  {
1419  float x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z + matrix.m_rows[3].x;
1420  float y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z + matrix.m_rows[3].y;
1421  float z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[3].z;
1422 
1423  return v3(x, y, z);
1424  }
1425 
1426  inline v3 v3::MulRotate( const v3& src, const m44& matrix )
1427  {
1428  float x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z;
1429  float y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z;
1430  float z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z;
1431 
1432  return v3(x, y, z);
1433  }
1434 
1435  inline v3 v3::Clamped( const v3& src, float maxLength)
1436  {
1437  float length = src.Length();
1438  v3 result = (length > maxLength) ? v3::Scale(src, maxLength/length) : src;
1439  return result;
1440  }
1441 
1442  inline v3 v3::Abs(const v3& src)
1443  {
1444  return v3(Math::Abs(src.x), Math::Abs(src.y), Math::Abs(src.z));
1445  }
1446 
1447  inline v3 v3::Scale( const v3& src, float scale )
1448  {
1449  return v3(src.x * scale, src.y * scale, src.z * scale);
1450  }
1451 
1452  inline v3 v3::InvScale( const v3& src, float scale )
1453  {
1454  return v3(src.x / scale, src.y / scale, src.z / scale);
1455  }
1456 
1457  inline float v3::Length() const
1458  {
1459  float dot = x * x + y * y + z * z;
1460  float length = Math::Sqrtf(dot);
1461  return length;
1462  }
1463 
1464  inline float v3::LengthSq() const
1465  {
1466  return Dot(*this, *this);
1467  }
1468 
1469  inline float v3::SquareDistanceTo( const v3& a ) const
1470  {
1471  float dx = a.x - x;
1472  float dy = a.y - y;
1473  float dz = a.z - z;
1474  return dx*dx + dy*dy + dz*dz;
1475  }
1476 
1477  inline v3 v3::Norm() const
1478  {
1479  float dot = x * x + y * y + z * z;
1480  float length = Math::Sqrtf(dot);
1481  return v3(x/length, y/length, z/length);
1482  }
1483 
1484  inline v3 v3::Neg(const v3& src)
1485  {
1486  return v3(-src.x, -src.y, -src.z);
1487  }
1488 
1489  inline v3 v3::operator - () const
1490  {
1491  return Neg(*this);
1492  }
1493 
1494  inline v3 v3::operator + ( const v3& rhs ) const
1495  {
1496  return Add(*this, rhs);
1497  }
1498 
1499  inline v3 v3::operator - ( const v3& rhs ) const
1500  {
1501  return Sub(*this, rhs);
1502  }
1503 
1504  inline v3 v3::operator * ( const v3& rhs ) const
1505  {
1506  return Mul(*this, rhs);
1507  }
1508 
1509  inline v3 v3::operator * ( const float rhs ) const
1510  {
1511  return Scale(*this, rhs);
1512  }
1513 
1514  inline v3 operator * ( const float lhs, const v3& rhs )
1515  {
1516  return v3::Scale(rhs, lhs);
1517  }
1518 
1519  inline v3 v3::operator / ( const v3& rhs ) const
1520  {
1521  return Div(*this, rhs);
1522  }
1523 
1524  inline v3 v3::operator / ( const float rhs ) const
1525  {
1526  return InvScale(*this, rhs);
1527  }
1528 
1529  inline v3& v3::operator += ( const v3& rhs )
1530  {
1531  *this = Add(*this, rhs);
1532  return *this;
1533  }
1534 
1535  inline v3& v3::operator -= ( const v3& rhs )
1536  {
1537  *this = Sub(*this, rhs);
1538  return *this;
1539  }
1540 
1541  inline v3& v3::operator *= ( const v3& rhs )
1542  {
1543  *this = Mul(*this, rhs);
1544  return *this;
1545  }
1546 
1547  inline v3& v3::operator /= ( const v3& rhs )
1548  {
1549  *this = Div(*this, rhs);
1550  return *this;
1551  }
1552 
1553  inline v3& v3::operator *= ( const float rhs )
1554  {
1555  x *= rhs;
1556  y *= rhs;
1557  z *= rhs;
1558  return *this;
1559  }
1560 
1561  inline v3& v3::operator /= ( const float rhs )
1562  {
1563  x /= rhs;
1564  y /= rhs;
1565  z /= rhs;
1566 
1567  return *this;
1568  }
1569 
1570  inline v3 v3::Min( const v3& src1, const v3& src2 )
1571  {
1572  return v3( src1.x < src2.x ? src1.x : src2.x,
1573  src1.y < src2.y ? src1.y : src2.y,
1574  src1.z < src2.z ? src1.z : src2.z );
1575  }
1576 
1577  inline v3 v3::Max( const v3& src1, const v3& src2 )
1578  {
1579  return v3( src1.x > src2.x ? src1.x : src2.x,
1580  src1.y > src2.y ? src1.y : src2.y,
1581  src1.z > src2.z ? src1.z : src2.z );
1582  }
1583 
1584  inline dv3::dv3( const dv3& src )
1585  {
1586  x = src.x;
1587  y = src.y;
1588  z = src.z;
1589  }
1590 
1591  inline dv3::dv3( double _x, double _y, double _z )
1592  {
1593  x = _x;
1594  y = _y;
1595  z = _z;
1596  }
1597 
1598  inline dv3::dv3( const double* pSrc )
1599  {
1600  x = pSrc[0];
1601  y = pSrc[1];
1602  z = pSrc[2];
1603  }
1604 
1605  inline dv3::dv3(const v4& src)
1606  {
1607  x = src.x;
1608  y = src.y;
1609  z = src.z;
1610  }
1611 
1612  inline dv3::dv3(const dv4& src)
1613  {
1614  x = src.x;
1615  y = src.y;
1616  z = src.z;
1617  }
1618 
1619  inline dv3::dv3(const v3& src)
1620  {
1621  x = src.x;
1622  y = src.y;
1623  z = src.z;
1624  }
1625 
1626  inline void dv3::Set( double _x, double _y, double _z )
1627  {
1628  x = _x;
1629  y = _y;
1630  z = _z;
1631  }
1632 
1633  inline void dv3::SetZero()
1634  {
1635  x = 0.0f;
1636  y = 0.0f;
1637  z = 0.0f;
1638  }
1639 
1640  inline dv3 dv3::SplatX() const
1641  {
1642  return dv3(x, x, x);
1643  }
1644 
1645  inline dv3 dv3::SplatY() const
1646  {
1647  return dv3(y, y, y);
1648  }
1649 
1650  inline dv3 dv3::SplatZ() const
1651  {
1652  return dv3(z, z, z);
1653  }
1654 
1655  inline double dv3::GetX() const
1656  {
1657  return x;
1658  }
1659 
1660  inline double dv3::GetY() const
1661  {
1662  return y;
1663  }
1664 
1665  inline double dv3::GetZ() const
1666  {
1667  return z;
1668  }
1669 
1670  inline void dv3::SetX( double data )
1671  {
1672  x = data;
1673  }
1674 
1675  inline void dv3::SetY( double data )
1676  {
1677  y = data;
1678  }
1679 
1680  inline void dv3::SetZ( double data )
1681  {
1682  z = data;
1683  }
1684 
1685  inline dv3 dv3::Add( const dv3& src1, const dv3& src2 )
1686  {
1687  return dv3(src1.x + src2.x, src1.y + src2.y, src1.z + src2.z);
1688  }
1689 
1690  inline dv3 dv3::Sub( const dv3& src1, const dv3& src2 )
1691  {
1692  return dv3(src1.x - src2.x, src1.y - src2.y, src1.z - src2.z);
1693  }
1694 
1695  inline dv3 dv3::Mul( const dv3& src1, const dv3& src2 )
1696  {
1697  return dv3(src1.x * src2.x, src1.y * src2.y, src1.z * src2.z);
1698  }
1699 
1700  inline dv3 dv3::Div( const dv3& src1, const dv3& src2 )
1701  {
1702  return dv3(src1.x / src2.x, src1.y / src2.y, src1.z / src2.z);
1703  }
1704 
1705  inline dv3 dv3::Add( const dv3& src1, const v3& src2 )
1706  {
1707  return dv3(src1.x + src2.x, src1.y + src2.y, src1.z + src2.z);
1708  }
1709 
1710  inline dv3 dv3::Sub( const dv3& src1, const v3& src2 )
1711  {
1712  return dv3(src1.x - src2.x, src1.y - src2.y, src1.z - src2.z);
1713  }
1714 
1715  inline dv3 dv3::Mul( const dv3& src1, const v3& src2 )
1716  {
1717  return dv3(src1.x * src2.x, src1.y * src2.y, src1.z * src2.z);
1718  }
1719 
1720  inline dv3 dv3::Div( const dv3& src1, const v3& src2 )
1721  {
1722  return dv3(src1.x / src2.x, src1.y / src2.y, src1.z / src2.z);
1723  }
1724 
1725  inline double dv3::Dot( const dv3& src1, const dv3& src2 )
1726  {
1727 #if MATH_NEON && !TARGET_IPHONE_SIMULATOR
1728 
1729  double ret[3];
1730  asm volatile (
1731  "vld1.32 {d2}, [%0] \n\t"
1732  "flds s6, [%0, #8] \n\t"
1733  "vld1.32 {d4}, [%1] \n\t"
1734  "flds s10, [%1, #8] \n\t"
1735 
1736  "vmul.f32 d0, d2, d4 \n\t"
1737  "vpadd.f32 d0, d0, d0 \n\t"
1738  "vmla.f32 d0, d3, d5 \n\t"
1739 
1740  "vstr.32 d0, [%2] \n\t"
1741 
1742  :: "r"(&src1), "r"(&src2), "=r"(ret)
1743  : "d0","d1","d2","d3","d4","d5"
1744  );
1745 
1746  return ret[0];
1747 
1748 #else
1749  double dot = src1.x * src2.x + src1.y * src2.y + src1.z * src2.z;
1750  return dot;
1751 #endif
1752  }
1753 
1754  inline dv3 dv3::Lerp( const dv3& src1, const dv3& src2, double ratio )
1755  {
1756  dv3 temp = src2 * ratio;
1757  temp += src1 * (1.0f - ratio);
1758 
1759  return temp;
1760  }
1761 
1762  inline dv3 dv3::Cross( const dv3& src1, const dv3& src2 )
1763  {
1764  return dv3( src1.y * src2.z - src1.z * src2.y,
1765  src1.z * src2.x - src1.x * src2.z,
1766  src1.x * src2.y - src1.y * src2.x );
1767  }
1768 
1769  inline dv3 dv3::Mul (const dv3& src, const m33& matrix)
1770  {
1771 
1772 #if 0
1773  dv3 destVector;
1774  int tmp;
1775  asm volatile (
1776  "mov %3, #12 \n\t"
1777  "vld1.32 {d0, d1}, [%1] \n\t"
1778  "vld1.32 {d2, d3}, [%0], %3 \n\t"
1779  "vld1.32 {d4, d5}, [%0], %3 \n\t"
1780  "vld1.32 {d6, d7}, [%0], %3 \n\t"
1781 
1782  "vmul.f32 q9, q1, d0[0] \n\t"
1783  "vmla.f32 q9, q2, d0[1] \n\t"
1784  "vmla.f32 q9, q3, d1[0] \n\t"
1785  "vmov.f32 q0, q9 \n\t"
1786 
1787  "vst1.32 d0, [%2]! \n\t"
1788  "fsts s2, [%2] \n\t"
1789 
1790  : "+r"(&matrix), "+r"(&src), "+r"(destVector), "+r"(tmp):
1791  : "q0", "q9", "q10","q11", "q12", "q13", "memory"
1792  );
1793 
1794  return destVector;
1795 #else
1796 
1797  dv3 tempMatrix;
1798 
1799  tempMatrix.x = matrix._11*src.x + matrix._21*src.y + matrix._31*src.z;
1800  tempMatrix.y = matrix._12*src.x + matrix._22*src.y + matrix._32*src.z;
1801  tempMatrix.z = matrix._13*src.x + matrix._23*src.y + matrix._33*src.z;
1802 
1803  return tempMatrix;
1804 
1805 #endif
1806 
1807  }
1808 
1809  inline dv3 dv3::Mul( const dv3& src, const m44& matrix )
1810  {
1811  double x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z + matrix.m_rows[3].x;
1812  double y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z + matrix.m_rows[3].y;
1813  double z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[3].z;
1814 
1815  return dv3(x, y, z);
1816  }
1817 
1818  inline dv3 dv3::MulRotate( const dv3& src, const m44& matrix )
1819  {
1820  double x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z;
1821  double y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z;
1822  double z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z;
1823 
1824  return dv3(x, y, z);
1825  }
1826 
1827  inline dv3 dv3::Mul(const dv3& src, const dm33& matrix)
1828  {
1829  double x = matrix._11 * src.x + matrix._21 * src.y + matrix._31 * src.z;
1830  double y = matrix._12 * src.x + matrix._22 * src.y + matrix._32 * src.z;
1831  double z = matrix._13 * src.x + matrix._23 * src.y + matrix._33 * src.z;
1832 
1833  return dv3(x, y, z);
1834  }
1835 
1836 
1837  inline dv3 dv3::Mul( const dv3& src, const dm44& matrix )
1838  {
1839  double x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z + matrix.m_rows[3].x;
1840  double y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z + matrix.m_rows[3].y;
1841  double z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[3].z;
1842 
1843  return dv3(x, y, z);
1844  }
1845 
1846  inline dv3 dv3::MulRotate( const dv3& src, const dm44& matrix )
1847  {
1848  double x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z;
1849  double y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z;
1850  double z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z;
1851 
1852  return dv3(x, y, z);
1853  }
1854 
1855  inline v3 dv3::ToSingle( const dv3& src)
1856  {
1857  return v3(static_cast<float>(src.x), static_cast<float>(src.y), static_cast<float>(src.z));
1858  }
1859 
1860  inline dv3 dv3::FromSingle( const v3& src)
1861  {
1862  return dv3(src.GetX(), src.GetY(), src.GetZ());
1863  }
1864 
1865  inline dv3 dv3::Scale( const dv3& src, double scale )
1866  {
1867  return dv3(src.x * scale, src.y * scale, src.z * scale);
1868  }
1869 
1870  inline dv3 dv3::InvScale( const dv3& src, double scale )
1871  {
1872  return dv3(src.x / scale, src.y / scale, src.z / scale);
1873  }
1874 
1875  inline double dv3::SquareDistanceTo(const dv3& a) const
1876  {
1877  double dx = a.x - x;
1878  double dy = a.y - y;
1879  double dz = a.z - z;
1880  return dx*dx + dy*dy + dz*dz;
1881  }
1882 
1883  inline double dv3::Length() const
1884  {
1885  double dot = x * x + y * y + z * z;
1886  double length = Math::Sqrtd(dot);
1887  return length;
1888  }
1889 
1890  inline double dv3::LengthSq() const
1891  {
1892  return Dot(*this, *this);
1893  }
1894 
1895  inline dv3 dv3::Norm() const
1896  {
1897  double invMagnitude = 1.0 / Length();
1898  return dv3(x * invMagnitude, y * invMagnitude, z * invMagnitude);
1899  }
1900 
1901  inline dv3& dv3::Normalise()
1902  {
1903  *this = Norm();
1904  return *this;
1905  }
1906 
1907  inline v3 dv3::ToSingle() const
1908  {
1909  return ToSingle(*this);
1910  }
1911 
1912  inline dv3 dv3::Neg(const dv3& src)
1913  {
1914  return dv3(-src.x, -src.y, -src.z);
1915  }
1916 
1917  inline dv3 dv3::operator - () const
1918  {
1919  return Neg(*this);
1920  }
1921 
1922  inline dv3 dv3::operator + ( const v3& rhs ) const
1923  {
1924  return Add(*this, rhs);
1925  }
1926 
1927  inline dv3 dv3::operator - ( const v3& rhs ) const
1928  {
1929  return Sub(*this, rhs);
1930  }
1931 
1932  inline dv3 dv3::operator * ( const v3& rhs ) const
1933  {
1934  return Mul(*this, rhs);
1935  }
1936 
1937  inline dv3 dv3::operator * ( const float rhs ) const
1938  {
1939  return Scale(*this, rhs);
1940  }
1941 
1942  inline dv3 operator * ( const double lhs, const dv3& rhs)
1943  {
1944  return dv3::Scale(rhs, lhs);
1945  }
1946 
1947  inline dv3 dv3::operator / ( const v3& rhs ) const
1948  {
1949  return Div(*this, rhs);
1950  }
1951 
1952  inline dv3 dv3::operator / ( const float rhs ) const
1953  {
1954  return InvScale(*this, rhs);
1955  }
1956 
1957  inline dv3& dv3::operator += ( const v3& rhs )
1958  {
1959  *this = Add(*this, rhs);
1960  return *this;
1961  }
1962 
1963  inline dv3& dv3::operator -= ( const v3& rhs )
1964  {
1965  *this = Sub(*this, rhs);
1966  return *this;
1967  }
1968 
1969  inline dv3& dv3::operator *= ( const v3& rhs )
1970  {
1971  *this = Mul(*this, rhs);
1972  return *this;
1973  }
1974 
1975  inline dv3& dv3::operator /= ( const v3& rhs )
1976  {
1977  *this = Div(*this, rhs);
1978  return *this;
1979  }
1980 
1981 
1982  inline dv3 dv3::operator + ( const dv3& rhs ) const
1983  {
1984  return Add(*this, rhs);
1985  }
1986 
1987  inline dv3 dv3::operator - ( const dv3& rhs ) const
1988  {
1989  return Sub(*this, rhs);
1990  }
1991 
1992  inline dv3 dv3::operator * ( const dv3& rhs ) const
1993  {
1994  return Mul(*this, rhs);
1995  }
1996 
1997  inline dv3 dv3::operator * ( const double rhs ) const
1998  {
1999  return Scale(*this, rhs);
2000  }
2001 
2002  inline dv3 dv3::operator / ( const dv3& rhs ) const
2003  {
2004  return Div(*this, rhs);
2005  }
2006 
2007  inline dv3 dv3::operator / ( const double rhs ) const
2008  {
2009  return InvScale(*this, rhs);
2010  }
2011 
2012  inline dv3& dv3::operator += ( const dv3& rhs )
2013  {
2014  *this = Add(*this, rhs);
2015  return *this;
2016  }
2017 
2018  inline dv3& dv3::operator -= ( const dv3& rhs )
2019  {
2020  *this = Sub(*this, rhs);
2021  return *this;
2022  }
2023 
2024  inline dv3& dv3::operator *= ( const dv3& rhs )
2025  {
2026  *this = Mul(*this, rhs);
2027  return *this;
2028  }
2029 
2030  inline dv3& dv3::operator /= ( const dv3& rhs )
2031  {
2032  *this = Div(*this, rhs);
2033  return *this;
2034  }
2035 
2036  inline dv3& dv3::operator *= ( const double rhs )
2037  {
2038  x *= rhs;
2039  y *= rhs;
2040  z *= rhs;
2041  return *this;
2042  }
2043 
2044  inline dv3& dv3::operator /= ( const double rhs )
2045  {
2046  x /= rhs;
2047  y /= rhs;
2048  z /= rhs;
2049 
2050  return *this;
2051  }
2052 
2053  inline dv3 dv3::Min( const dv3& src1, const dv3& src2 )
2054  {
2055  return dv3( src1.x < src2.x ? src1.x : src2.x,
2056  src1.y < src2.y ? src1.y : src2.y,
2057  src1.z < src2.z ? src1.z : src2.z );
2058  }
2059 
2060  inline dv3 dv3::Max( const dv3& src1, const dv3& src2 )
2061  {
2062  return dv3( src1.x > src2.x ? src1.x : src2.x,
2063  src1.y > src2.y ? src1.y : src2.y,
2064  src1.z > src2.z ? src1.z : src2.z );
2065  }
2066 
2067 
2068 
2069 
2070 
2071 
2072  inline v4::v4( const v4& src )
2073  {
2074  x = src.x;
2075  y = src.y;
2076  z = src.z;
2077  w = src.w;
2078  }
2079 
2080  inline v4::v4( float _x, float _y, float _z, float _w )
2081  {
2082  x = _x;
2083  y = _y;
2084  z = _z;
2085  w = _w;
2086  }
2087 
2088  inline v4::v4( const v3& src, float _w )
2089  {
2090  x = src.x;
2091  y = src.y;
2092  z = src.z;
2093  w = _w;
2094  }
2095 
2096  inline void v4::Set( float _x, float _y, float _z, float _w )
2097  {
2098  x = _x;
2099  y = _y;
2100  z = _z;
2101  w = _w;
2102  }
2103 
2104  inline void v4::SetZero()
2105  {
2106  x = 0.0f;
2107  y = 0.0f;
2108  z = 0.0f;
2109  w = 0.0f;
2110  }
2111 
2112  inline float v4::GetX() const
2113  {
2114  return x;
2115  }
2116 
2117  inline float v4::GetY() const
2118  {
2119  return y;
2120  }
2121 
2122  inline float v4::GetZ() const
2123  {
2124  return z;
2125  }
2126 
2127  inline float v4::GetW() const
2128  {
2129  return w;
2130  }
2131 
2132  inline void v4::SetX( float data )
2133  {
2134  x = data;
2135  }
2136 
2137  inline void v4::SetY( float data )
2138  {
2139  y = data;
2140  }
2141 
2142  inline void v4::SetZ( float data )
2143  {
2144  z = data;
2145  }
2146 
2147  inline void v4::SetW( float data )
2148  {
2149  w = data;
2150  }
2151 
2152  inline v4 v4::SplatX() const
2153  {
2154  return v4(x, x, x, x);
2155  }
2156 
2157  inline v4 v4::SplatY() const
2158  {
2159  return v4(y, y, y, y);
2160  }
2161 
2162  inline v4 v4::SplatZ() const
2163  {
2164  return v4(z, z, z, z);
2165  }
2166 
2167  inline v4 v4::SplatW() const
2168  {
2169  return v4(w, w, w, w);
2170  }
2171 
2172  inline void v4::Load( const float* pSrc )
2173  {
2174  x = pSrc[0];
2175  y = pSrc[1];
2176  z = pSrc[2];
2177  w = pSrc[3];
2178  }
2179 
2180  inline void v4::Store( float* pDest ) const
2181  {
2182  pDest[0] = x;
2183  pDest[1] = y;
2184  pDest[2] = z;
2185  pDest[3] = w;
2186  }
2187 
2188 
2189 
2190 
2191  inline v4 v4::Add( const v4& src1, const v4& src2 )
2192  {
2193  return v4(src1.x + src2.x, src1.y + src2.y, src1.z + src2.z, src1.w + src2.w);
2194  }
2195 
2196  inline v4 v4::Sub( const v4& src1, const v4& src2 )
2197  {
2198  return v4(src1.x - src2.x, src1.y - src2.y, src1.z - src2.z, src1.w - src2.w);
2199  }
2200  inline v4 v4::Mul( const v4& src1, const v4& src2 )
2201  {
2202  return v4(src1.x * src2.x, src1.y * src2.y, src1.z * src2.z, src1.w * src2.w);
2203  }
2204 
2205  inline v4 v4::Div( const v4& src1, const v4& src2 )
2206  {
2207  return v4(src1.x / src2.x, src1.y / src2.y, src1.z / src2.z, src1.w / src2.w);
2208  }
2209 
2210  inline v4 v4::Dot( const v4& src1, const v4& src2 )
2211  {
2212  float dot = src1.x * src2.x + src1.y * src2.y + src1.z * src2.z + src1.w * src2.w;
2213  return v4(dot, dot, dot, dot);
2214  }
2215 
2216  inline v4 v4::Lerp( const v4& src1, const v4& src2, float ratio )
2217  {
2218  v4 temp = src2 * ratio;
2219  temp += src1 * (1.0f - ratio);
2220 
2221  return temp;
2222  }
2223 
2224  inline v4 v4::Neg(const v4& src)
2225  {
2226  return v4(-src.x, -src.y, -src.z, -src.w);
2227  }
2228 
2229  inline bool v4::operator== (const v4 &rhs) const
2230  {
2231  return (x == rhs.x) && (y == rhs.y) && (z == rhs.z) && (w == rhs.w);
2232  }
2233 
2234  inline bool v4::operator!= (const v4 &rhs) const
2235  {
2236  return (x != rhs.x) || (y != rhs.y) || (z != rhs.z) || (w != rhs.w);
2237  }
2238 
2239  inline v4 v4::operator - () const
2240  {
2241  return Neg(*this);
2242  }
2243 
2244  inline v4 v4::operator + ( const v4& rhs ) const
2245  {
2246  return Add(*this, rhs);
2247  }
2248 
2249  inline v4 v4::operator - ( const v4& rhs ) const
2250  {
2251  return Sub(*this, rhs);
2252  }
2253 
2254  inline v4 v4::operator * ( const v4& rhs ) const
2255  {
2256  return Mul(*this, rhs);
2257  }
2258 
2259  inline v4 v4::operator / ( const v4& rhs ) const
2260  {
2261  return Div(*this, rhs);
2262  }
2263 
2264  inline v4 v4::operator * ( const float rhs ) const
2265  {
2266  return v4(x * rhs, y * rhs, z * rhs, w * rhs);
2267  }
2268 
2269  inline v4 v4::operator / ( const float rhs ) const
2270  {
2271  return v4(x / rhs, y / rhs, z / rhs, w / rhs);
2272  }
2273 
2274  inline v4& v4::operator += ( const v4& rhs )
2275  {
2276  *this = Add(*this, rhs);
2277  return *this;
2278  }
2279 
2280  inline v4& v4::operator -= ( const v4& rhs )
2281  {
2282  *this = Sub(*this, rhs);
2283  return *this;
2284  }
2285 
2286  inline v4& v4::operator *= ( const v4& rhs )
2287  {
2288  *this = Mul(*this, rhs);
2289  return *this;
2290  }
2291 
2292  inline v4& v4::operator /= ( const v4& rhs )
2293  {
2294  *this = Div(*this, rhs);
2295  return *this;
2296  }
2297 
2298  inline v4& v4::operator *= ( const float rhs )
2299  {
2300  x *= rhs;
2301  y *= rhs;
2302  z *= rhs;
2303  w *= rhs;
2304 
2305  return *this;
2306  }
2307 
2308  inline v4& v4::operator /= ( const float rhs )
2309  {
2310  x /= rhs;
2311  y /= rhs;
2312  z /= rhs;
2313  w /= rhs;
2314 
2315  return *this;
2316  }
2317 
2318  inline v4 v4::Min( const v4& src1, const v4& src2 )
2319  {
2320  return v4( src1.x < src2.x ? src1.x : src2.x,
2321  src1.y < src2.y ? src1.y : src2.y,
2322  src1.z < src2.z ? src1.z : src2.z,
2323  src1.w < src2.w ? src1.w : src2.w);
2324  }
2325 
2326  inline v4 v4::Max( const v4& src1, const v4& src2 )
2327  {
2328  return v4( src1.x > src2.x ? src1.x : src2.x,
2329  src1.y > src2.y ? src1.y : src2.y,
2330  src1.z > src2.z ? src1.z : src2.z,
2331  src1.w > src2.w ? src1.w : src2.w);
2332  }
2333 
2334  inline v4 v4::Mul( const v4& src, const m44& matrix )
2335  {
2336  float x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z + matrix.m_rows[3].x * src.w;
2337  float y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z + matrix.m_rows[3].y * src.w;
2338  float z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[3].z * src.w;
2339  float w = matrix.m_rows[0].w * src.x + matrix.m_rows[1].w * src.y + matrix.m_rows[2].w * src.z + matrix.m_rows[3].w * src.w;
2340 
2341  return v4(x, y, z, w);
2342  }
2343 
2344  inline v4 v4::MulTransposed( const v4& src, const m44& matrix )
2345  {
2346  float x = matrix.m_rows[0].x * src.x + matrix.m_rows[0].y * src.y + matrix.m_rows[0].z * src.z + matrix.m_rows[0].w * src.w;
2347  float y = matrix.m_rows[1].x * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[1].z * src.z + matrix.m_rows[1].w * src.w;
2348  float z = matrix.m_rows[2].x * src.x + matrix.m_rows[2].y * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[2].w * src.w;
2349  float w = matrix.m_rows[3].x * src.x + matrix.m_rows[3].y * src.y + matrix.m_rows[3].z * src.z + matrix.m_rows[3].w * src.w;
2350 
2351  return v4(x, y, z, w);
2352  }
2353 
2354  inline v4 v4::Length() const
2355  {
2356  float dot = x * x + y * y + z * z + w * w;
2357  float length = Math::Sqrtf(dot);
2358  return v4(length, length, length, length);
2359  }
2360 
2361  inline v4 v4::LengthSq() const
2362  {
2363  return Dot(*this, *this);
2364  }
2365 
2366 
2367  inline dv4::dv4( const dv4& src )
2368  {
2369  x = src.x;
2370  y = src.y;
2371  z = src.z;
2372  w = src.w;
2373  }
2374 
2375  inline dv4::dv4( const dv3& src, double _w )
2376  {
2377  x = src.x;
2378  y = src.y;
2379  z = src.z;
2380  w = _w;
2381  }
2382 
2383  inline dv4::dv4( double _x, double _y, double _z, double _w )
2384  {
2385  x = _x;
2386  y = _y;
2387  z = _z;
2388  w = _w;
2389  }
2390 
2391  inline dv4::dv4( const double* pSrc )
2392  {
2393  x = pSrc[0];
2394  y = pSrc[1];
2395  z = pSrc[2];
2396  w = pSrc[3];
2397  }
2398 
2399  inline dv4::dv4(const v4& src)
2400  {
2401  x = src.x;
2402  y = src.y;
2403  z = src.z;
2404  w = src.w;
2405  }
2406 
2407  inline dv4::dv4(const v3& src)
2408  {
2409  x = src.x;
2410  y = src.y;
2411  z = src.z;
2412  w = 0.0;
2413  }
2414 
2415  inline void dv4::Set( double _x, double _y, double _z , double _w )
2416  {
2417  x = _x;
2418  y = _y;
2419  z = _z;
2420  w = _w;
2421  }
2422 
2423  inline void dv4::SetZero()
2424  {
2425  x = 0.0f;
2426  y = 0.0f;
2427  z = 0.0f;
2428  w = 0.0f;
2429  }
2430 
2431  inline dv4 dv4::SplatX() const
2432  {
2433  return dv4(x, x, x, x);
2434  }
2435 
2436  inline dv4 dv4::SplatY() const
2437  {
2438  return dv4(y, y, y, y);
2439  }
2440 
2441  inline dv4 dv4::SplatZ() const
2442  {
2443  return dv4(z, z, z, z);
2444  }
2445 
2446  inline dv4 dv4::SplatW() const
2447  {
2448  return dv4(w, w, w, w);
2449  }
2450 
2451  inline double dv4::GetX() const
2452  {
2453  return x;
2454  }
2455 
2456  inline double dv4::GetY() const
2457  {
2458  return y;
2459  }
2460 
2461  inline double dv4::GetZ() const
2462  {
2463  return z;
2464  }
2465 
2466  inline double dv4::GetW() const
2467  {
2468  return w;
2469  }
2470 
2471  inline void dv4::SetX( double data )
2472  {
2473  x = data;
2474  }
2475 
2476  inline void dv4::SetY( double data )
2477  {
2478  y = data;
2479  }
2480 
2481  inline void dv4::SetZ( double data )
2482  {
2483  z = data;
2484  }
2485 
2486  inline void dv4::SetW( double data )
2487  {
2488  w = data;
2489  }
2490 
2491  inline dv4 dv4::Add( const dv4& src1, const dv4& src2 )
2492  {
2493  return dv4(src1.x + src2.x, src1.y + src2.y, src1.z + src2.z, src1.w + src2.w);
2494  }
2495 
2496  inline dv4 dv4::Sub( const dv4& src1, const dv4& src2 )
2497  {
2498  return dv4(src1.x - src2.x, src1.y - src2.y, src1.z - src2.z, src1.w - src2.w);
2499  }
2500 
2501  inline dv4 dv4::Mul( const dv4& src1, const dv4& src2 )
2502  {
2503  return dv4(src1.x * src2.x, src1.y * src2.y, src1.z * src2.z, src1.w * src2.w);
2504  }
2505 
2506  inline dv4 dv4::Div( const dv4& src1, const dv4& src2 )
2507  {
2508  return dv4(src1.x / src2.x, src1.y / src2.y, src1.z / src2.z, src1.w / src2.w);
2509  }
2510 
2511  inline dv4 dv4::Add( const dv4& src1, const v4& src2 )
2512  {
2513  return dv4(src1.x + src2.x, src1.y + src2.y, src1.z + src2.z, src1.w + src2.w);
2514  }
2515 
2516  inline dv4 dv4::Sub( const dv4& src1, const v4& src2 )
2517  {
2518  return dv4(src1.x - src2.x, src1.y - src2.y, src1.z - src2.z, src1.w - src2.w);
2519  }
2520 
2521  inline dv4 dv4::Mul( const dv4& src1, const v4& src2 )
2522  {
2523  return dv4(src1.x * src2.x, src1.y * src2.y, src1.z * src2.z, src1.w * src2.w);
2524  }
2525 
2526  inline dv4 dv4::Div( const dv4& src1, const v4& src2 )
2527  {
2528  return dv4(src1.x / src2.x, src1.y / src2.y, src1.z / src2.z, src1.w / src2.w);
2529  }
2530 
2531  inline double dv4::Dot( const dv4& src1, const dv4& src2 )
2532  {
2533  double dot = src1.x * src2.x + src1.y * src2.y + src1.z * src2.z + src1.w * src2.w;
2534  return dot;
2535  }
2536 
2537  inline dv4 dv4::Lerp( const dv4& src1, const dv4& src2, double ratio )
2538  {
2539  dv4 temp = src2 * ratio;
2540  temp += src1 * (1.0 - ratio);
2541  return temp;
2542  }
2543 
2544  inline dv4 dv4::Mul( const dv4& src, const m44& matrix )
2545  {
2546  double x = matrix.m_rows[0].x * src.x + matrix.m_rows[1].x * src.y + matrix.m_rows[2].x * src.z + matrix.m_rows[3].x * src.w;
2547  double y = matrix.m_rows[0].y * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[2].y * src.z + matrix.m_rows[3].y * src.w;
2548  double z = matrix.m_rows[0].z * src.x + matrix.m_rows[1].z * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[3].z * src.w;
2549  double w = matrix.m_rows[0].w * src.x + matrix.m_rows[1].w * src.y + matrix.m_rows[2].w * src.z + matrix.m_rows[3].w * src.w;
2550 
2551  return dv4(x, y, z, w);
2552  }
2553 
2554  inline dv4 dv4::MulTransposed( const dv4& src, const m44& matrix )
2555  {
2556  double x = matrix.m_rows[0].x * src.x + matrix.m_rows[0].y * src.y + matrix.m_rows[0].z * src.z + matrix.m_rows[0].w * src.w;
2557  double y = matrix.m_rows[1].x * src.x + matrix.m_rows[1].y * src.y + matrix.m_rows[1].z * src.z + matrix.m_rows[1].w * src.w;
2558  double z = matrix.m_rows[2].x * src.x + matrix.m_rows[2].y * src.y + matrix.m_rows[2].z * src.z + matrix.m_rows[2].w * src.w;
2559  double w = matrix.m_rows[3].x * src.x + matrix.m_rows[3].y * src.y + matrix.m_rows[3].z * src.z + matrix.m_rows[3].w * src.w;
2560 
2561  return dv4(x, y, z, w);
2562  }
2563 
2564  inline v4 dv4::ToSingle( const dv4& src)
2565  {
2566  return v4(static_cast<float>(src.x), static_cast<float>(src.y), static_cast<float>(src.z), static_cast<float>(src.w));
2567  }
2568 
2569  inline dv4 dv4::FromSingle( const v4& src)
2570  {
2571  return dv4(src.GetX(), src.GetY(), src.GetZ(), src.GetW());
2572  }
2573 
2574  inline dv4 dv4::Scale( const dv4& src, double scale )
2575  {
2576  return dv4(src.x * scale, src.y * scale, src.z * scale, src.w * scale);
2577  }
2578 
2579  inline dv4 dv4::InvScale( const dv4& src, double scale )
2580  {
2581  return dv4(src.x / scale, src.y / scale, src.z / scale, src.w / scale);
2582  }
2583 
2584  inline double dv4::Length() const
2585  {
2586  double dot = x * x + y * y + z * z + w * w;
2587  double length = Math::Sqrtd(dot);
2588  return length;
2589  }
2590 
2591  inline double dv4::LengthSq() const
2592  {
2593  return Dot(*this, *this);
2594  }
2595 
2596  inline dv4 dv4::Norm() const
2597  {
2598  double invMagnitude = 1.0 / Length();
2599  return dv4(x * invMagnitude, y * invMagnitude, z * invMagnitude, w * invMagnitude);
2600  }
2601 
2602  inline dv4& dv4::Normalise()
2603  {
2604  *this = Norm();
2605  return *this;
2606  }
2607 
2608  inline v4 dv4::ToSingle() const
2609  {
2610  return ToSingle(*this);
2611  }
2612 
2613  inline dv4 dv4::Neg(const dv4& src)
2614  {
2615  return dv4(-src.x, -src.y, -src.z, -src.w);
2616  }
2617 
2618  inline dv4 dv4::operator - () const
2619  {
2620  return Neg(*this);
2621  }
2622 
2623  inline dv4 dv4::operator + ( const v4& rhs ) const
2624  {
2625  return Add(*this, rhs);
2626  }
2627 
2628  inline dv4 dv4::operator - ( const v4& rhs ) const
2629  {
2630  return Sub(*this, rhs);
2631  }
2632 
2633  inline dv4 dv4::operator * ( const v4& rhs ) const
2634  {
2635  return Mul(*this, rhs);
2636  }
2637 
2638  inline dv4 dv4::operator * ( const float rhs ) const
2639  {
2640  return Scale(*this, rhs);
2641  }
2642 
2643  inline dv4 dv4::operator / ( const v4& rhs ) const
2644  {
2645  return Div(*this, rhs);
2646  }
2647 
2648  inline dv4 dv4::operator / ( const float rhs ) const
2649  {
2650  return InvScale(*this, rhs);
2651  }
2652 
2653  inline dv4& dv4::operator += ( const v4& rhs )
2654  {
2655  *this = Add(*this, rhs);
2656  return *this;
2657  }
2658 
2659  inline dv4& dv4::operator -= ( const v4& rhs )
2660  {
2661  *this = Sub(*this, rhs);
2662  return *this;
2663  }
2664 
2665  inline dv4& dv4::operator *= ( const v4& rhs )
2666  {
2667  *this = Mul(*this, rhs);
2668  return *this;
2669  }
2670 
2671  inline dv4& dv4::operator /= ( const v4& rhs )
2672  {
2673  *this = Div(*this, rhs);
2674  return *this;
2675  }
2676 
2677 
2678  inline dv4 dv4::operator + ( const dv4& rhs ) const
2679  {
2680  return Add(*this, rhs);
2681  }
2682 
2683  inline dv4 dv4::operator - ( const dv4& rhs ) const
2684  {
2685  return Sub(*this, rhs);
2686  }
2687 
2688  inline dv4 dv4::operator * ( const dv4& rhs ) const
2689  {
2690  return Mul(*this, rhs);
2691  }
2692 
2693  inline dv4 dv4::operator * ( const double rhs ) const
2694  {
2695  return Scale(*this, rhs);
2696  }
2697 
2698  inline dv4 dv4::operator / ( const dv4& rhs ) const
2699  {
2700  return Div(*this, rhs);
2701  }
2702 
2703  inline dv4 dv4::operator / ( const double rhs ) const
2704  {
2705  return InvScale(*this, rhs);
2706  }
2707 
2708  inline dv4& dv4::operator += ( const dv4& rhs )
2709  {
2710  *this = Add(*this, rhs);
2711  return *this;
2712  }
2713 
2714  inline dv4& dv4::operator -= ( const dv4& rhs )
2715  {
2716  *this = Sub(*this, rhs);
2717  return *this;
2718  }
2719 
2720  inline dv4& dv4::operator *= ( const dv4& rhs )
2721  {
2722  *this = Mul(*this, rhs);
2723  return *this;
2724  }
2725 
2726  inline dv4& dv4::operator /= ( const dv4& rhs )
2727  {
2728  *this = Div(*this, rhs);
2729  return *this;
2730  }
2731 
2732  inline dv4& dv4::operator *= ( const double rhs )
2733  {
2734  x *= rhs;
2735  y *= rhs;
2736  z *= rhs;
2737  w *= rhs;
2738  return *this;
2739  }
2740 
2741  inline dv4& dv4::operator /= ( const double rhs )
2742  {
2743  x /= rhs;
2744  y /= rhs;
2745  z /= rhs;
2746  w /= rhs;
2747  return *this;
2748  }
2749 
2750  inline dv4 dv4::Min( const dv4& src1, const dv4& src2 )
2751  {
2752  return dv4( src1.x < src2.x ? src1.x : src2.x,
2753  src1.y < src2.y ? src1.y : src2.y,
2754  src1.z < src2.z ? src1.z : src2.z,
2755  src1.w < src2.w ? src1.w : src2.w );
2756  }
2757 
2758  inline dv4 dv4::Max( const dv4& src1, const dv4& src2 )
2759  {
2760  return dv4( src1.x > src2.x ? src1.x : src2.x,
2761  src1.y > src2.y ? src1.y : src2.y,
2762  src1.z > src2.z ? src1.z : src2.z,
2763  src1.w > src2.w ? src1.w : src2.w );
2764  }
2765 
2766 
2767  inline void m33::Identity()
2768  {
2769  _11 = 1.0f; _12 = 0.0f; _13 = 0.0f;
2770  _21 = 0.0f; _22 = 1.0f; _23 = 0.0f;
2771  _31 = 0.0f; _32 = 0.0f; _33 = 1.0f;
2772  }
2773 
2774  inline m33 m33::CreateIdentity()
2775  {
2776  m33 newMatrix;
2777  newMatrix.Identity();
2778  return newMatrix;
2779  }
2780 
2781  inline void m33::Transpose( m33& dst, const m33& src )
2782  {
2783  m33 temp = src;
2784 
2785  dst._11 = temp._11;
2786  dst._12 = temp._21;
2787  dst._13 = temp._31;
2788 
2789  dst._21 = temp._12;
2790  dst._22 = temp._22;
2791  dst._23 = temp._32;
2792 
2793  dst._31 = temp._13;
2794  dst._32 = temp._23;
2795  dst._33 = temp._33;
2796  }
2797 
2798  inline void m33::SetRow ( int row, const v3& data )
2799  {
2800  switch (row)
2801  {
2802  case 0:
2803  _11 = data.x;
2804  _12 = data.y;
2805  _13 = data.z;
2806  break;
2807  case 1:
2808  _21 = data.x;
2809  _22 = data.y;
2810  _23 = data.z;
2811  break;
2812  case 2:
2813  _31 = data.x;
2814  _32 = data.y;
2815  _33 = data.z;
2816  break;
2817  default:
2818  Eegeo_ASSERT(false, "row out of range");
2819  };
2820  }
2821 
2822  inline void m33::SetFromBasis(const v3& right, const v3& up, const v3& forward)
2823  {
2824  _11 = right.x;
2825  _12 = right.y;
2826  _13 = right.z;
2827  _21 = up.x;
2828  _22 = up.y;
2829  _23 = up.z;
2830  _31 = forward.x;
2831  _32 = forward.y;
2832  _33 = forward.z;
2833  }
2834 
2835  inline v3 m33::GetRow ( int row ) const
2836  {
2837  switch (row)
2838  {
2839  case 0:
2840  return v3(_11, _12, _13);
2841  case 1:
2842  return v3(_21, _22, _23);
2843  case 2:
2844  return v3(_31, _32, _33);
2845  default:
2846  Eegeo_ASSERT(false, "row out of range");
2847  return v3::Zero();
2848  };
2849  }
2850 
2851  inline void m33::Scale(float scale)
2852  {
2853  _11 = scale; _12 = 0.0f; _13 = 0.0f;
2854  _21 = 0.0f; _22 = scale; _23 = 0.0f;
2855  _31 = 0.0f; _32 = 0.0f; _33 = scale;
2856  }
2857 
2858  inline void m33::Scale(const v3& s)
2859  {
2860  _11 = s.GetX(); _12 = 0.0f; _13 = 0.0f;
2861  _21 = 0.0f; _22 = s.GetY(); _23 = 0.0f;
2862  _31 = 0.0f; _32 = 0.0f; _33 = s.GetZ();
2863  }
2864 
2865  inline void m33::RotateX(float theta)
2866  {
2867  Identity();
2868 
2869  float sin = Math::Sin(theta);
2870  float cos = Math::Cos(theta);
2871 
2872  SetRow(1, v3(0.0f, cos, -sin));
2873  SetRow(2, v3(0.0f, sin, cos));
2874  }
2875 
2876  inline void m33::RotateY(float theta)
2877  {
2878  Identity();
2879 
2880  float sin = Math::Sin(theta);
2881  float cos = Math::Cos(theta);
2882 
2883  SetRow(0, v3(cos, 0.0f, -sin));
2884  SetRow(2, v3(sin, 0.0f , cos));
2885  }
2886 
2887  inline void m33::RotateZ(float theta)
2888  {
2889  Identity();
2890 
2891  float sin = Math::Sin(theta);
2892  float cos = Math::Cos(theta);
2893 
2894  SetRow(0, v3(cos, -sin, 0.0f));
2895  SetRow(1, v3(sin, cos, 0.0f));
2896  }
2897 
2898  inline void m33::Rotate( float x, float y, float z )
2899  {
2900  m33 tempX;
2901  tempX.RotateX(x);
2902  m33 tempY;
2903  tempY.RotateY(-y);
2904  m33 tempZ;
2905  tempZ.RotateZ(z);
2906  m33 temp;
2907 
2908  Mul(temp, tempZ, tempY);
2909  Mul(*this, temp, tempX);
2910  }
2911 
2912  inline void m33::Rotate( const v3& axis, float angle )
2913  {
2914  float c = Math::Cos(angle);
2915  float s = Math::Sin(angle);
2916  float t = 1.0f - c;
2917 
2918  float x = axis.GetX();
2919  float y = axis.GetY();
2920  float z = axis.GetZ();
2921 
2922  SetRow(0, v3( (t*x*x) + c, (t*y*x) - (s*z), (t*z*x) + (s*y)));
2923  SetRow(1, v3((t*x*y) + (s*z), (t*y*y) + c, (t*z*y) - (s*x)));
2924  SetRow(2, v3((t*x*z) - (s*y), (t*y*z) + (s*x), (t*z*z) + c));
2925  }
2926 
2927  inline void m33::Mul( m33& dest, const m33& src1, const m33& src2 )
2928  {
2929  m33 temp;
2930 
2931  const v3& src1row0 = src1.GetRow(0);
2932  const v3& src1row1 = src1.GetRow(1);
2933  const v3& src1row2 = src1.GetRow(2);
2934 
2935  for(int i = 0; i < 3; i++)
2936  {
2937  const v3& src2rowI = src2.GetRow(i);
2938 
2939  float x = src2rowI.x * src1row0.x +
2940  src2rowI.y * src1row1.x +
2941  src2rowI.z * src1row2.x;
2942 
2943  float y = src2rowI.x * src1row0.y +
2944  src2rowI.y * src1row1.y +
2945  src2rowI.z * src1row2.y;
2946 
2947  float z = src2rowI.x * src1row0.z +
2948  src2rowI.y * src1row1.z +
2949  src2rowI.z * src1row2.z;
2950 
2951  temp.SetRow(i, v3(x, y, z));
2952  }
2953 
2954  memcpy(&dest, &temp, sizeof(dest));
2955  }
2956 
2957  inline void m33::Mul( const float scale )
2958  {
2959  _11 *= scale;
2960  _12 *= scale;
2961  _13 *= scale;
2962  _21 *= scale;
2963  _22 *= scale;
2964  _23 *= scale;
2965  _31 *= scale;
2966  _32 *= scale;
2967  _33 *= scale;
2968  }
2969 
2970  inline m44 m33::ToM44() const
2971  {
2972  m44 m;
2973  m.m_rows[0].Set(_11, _12, _13, 0.f);
2974  m.m_rows[1].Set(_21, _22, _23, 0.f);
2975  m.m_rows[2].Set(_31, _32, _33, 0.f);
2976  m.m_rows[3].Set(0.f, 0.f, 0.f, 1.f);
2977  return m;
2978  }
2979 
2980  inline m44 m44::CreateIdentity()
2981  {
2982  m44 newMatrix;
2983  newMatrix.Identity();
2984  return newMatrix;
2985  }
2986 
2987  inline m44 m44::CreateTranslate(const v3& translate)
2988  {
2989  m44 m(CreateIdentity());
2990  m.m_rows[3] = v4(translate, 1.f);
2991  return m;
2992  }
2993 
2994  inline m44 m44::CreateScale(const v3& scale)
2995  {
2996  m44 m;
2997  m.Scale(scale);
2998  return m;
2999  }
3000 
3001  inline m44 m44::CreateFromRowMajorElements( const float* pRowMajorElements )
3002  {
3003  m44 matrix;
3004 
3005  matrix.SetRow(0, Eegeo::v4(pRowMajorElements[0], pRowMajorElements[1], pRowMajorElements[2], pRowMajorElements[3]));
3006  matrix.SetRow(1, Eegeo::v4(pRowMajorElements[4], pRowMajorElements[5], pRowMajorElements[6], pRowMajorElements[7]));
3007  matrix.SetRow(2, Eegeo::v4(pRowMajorElements[8], pRowMajorElements[9], pRowMajorElements[10], pRowMajorElements[11]));
3008  matrix.SetRow(3, Eegeo::v4(pRowMajorElements[12], pRowMajorElements[13], pRowMajorElements[14], pRowMajorElements[15]));
3009 
3010  return matrix;
3011  }
3012 
3013  inline m44::m44( const v4& row0, const v4& row1, const v4& row2, const v4& row3 )
3014  {
3015  m_rows[0] = row0;
3016  m_rows[1] = row1;
3017  m_rows[2] = row2;
3018  m_rows[3] = row3;
3019  }
3020 
3021  inline m44::m44(float values[16])
3022  {
3023  m_rows[0] = v4(values[0], values[1], values[2], values[3]);
3024  m_rows[1] = v4(values[4], values[5], values[6], values[7]);
3025  m_rows[2] = v4(values[8], values[9], values[10], values[11]);
3026  m_rows[3] = v4(values[12], values[13], values[14], values[15]);
3027  }
3028 
3029  inline void m44::SetFromBasis(const v3& right, const v3& up, const v3& forward, const v3& pos)
3030  {
3031  m_rows[0].Set(right.x, right.y, right.z, 0.f);
3032  m_rows[1].Set(up.x, up.y, up.z, 0.f);
3033  m_rows[2].Set(forward.x, forward.y, forward.z, 0.f);
3034  m_rows[3].Set(pos.x, pos.y, pos.z, 1.f);
3035  }
3036 
3037  inline void m44::Load( const float* pSrc )
3038  {
3039  m_rows[0].Load(&pSrc[0]);
3040  m_rows[1].Load(&pSrc[4]);
3041  m_rows[2].Load(&pSrc[8]);
3042  m_rows[3].Load(&pSrc[12]);
3043  }
3044 
3045  inline void m44::Store( float* pDest ) const
3046  {
3047  GetRow(0).Store(&pDest[0]);
3048  GetRow(1).Store(&pDest[4]);
3049  GetRow(2).Store(&pDest[8]);
3050  GetRow(3).Store(&pDest[12]);
3051  }
3052 
3053  inline bool m44::operator== (const m44 &rhs) const
3054  {
3055  return (m_rows[0] == rhs.m_rows[0]) &&
3056  (m_rows[1] == rhs.m_rows[1]) &&
3057  (m_rows[2] == rhs.m_rows[2]) &&
3058  (m_rows[3] == rhs.m_rows[3]);
3059  }
3060 
3061  inline bool m44::operator!= (const m44 &rhs) const
3062  {
3063  return (m_rows[0] != rhs.m_rows[0]) ||
3064  (m_rows[1] != rhs.m_rows[1]) ||
3065  (m_rows[2] != rhs.m_rows[2]) ||
3066  (m_rows[3] != rhs.m_rows[3]);
3067  }
3068 
3069  inline void m44::Identity()
3070  {
3071  m_rows[0].x = 1.0f; m_rows[0].y = 0.0f; m_rows[0].z = 0.0f; m_rows[0].w = 0.0f;
3072  m_rows[1].x = 0.0f; m_rows[1].y = 1.0f; m_rows[1].z = 0.0f; m_rows[1].w = 0.0f;
3073  m_rows[2].x = 0.0f; m_rows[2].y = 0.0f; m_rows[2].z = 1.0f; m_rows[2].w = 0.0f;
3074  m_rows[3].x = 0.0f; m_rows[3].y = 0.0f; m_rows[3].z = 0.0f; m_rows[3].w = 1.0f;
3075  }
3076 
3077  inline bool m44::IsIdentity() const
3078  {
3079  return
3080  (
3081  (m_rows[0].x == 1.0f) && (m_rows[0].y == 0.0f) && (m_rows[0].z == 0.0f) && (m_rows[0].w == 0.0f) &&
3082  (m_rows[1].x == 0.0f) && (m_rows[1].y == 1.0f) && (m_rows[1].z == 0.0f) && (m_rows[1].w == 0.0f) &&
3083  (m_rows[2].x == 0.0f) && (m_rows[2].y == 0.0f) && (m_rows[2].z == 1.0f) && (m_rows[2].w == 0.0f) &&
3084  (m_rows[3].x == 0.0f) && (m_rows[3].y == 0.0f) && (m_rows[3].z == 0.0f) && (m_rows[3].w == 1.0f)
3085  );
3086  }
3087 
3088  inline void m44::Scale(float scale)
3089  {
3090  m_rows[0].x = scale; m_rows[0].y = 0.0f; m_rows[0].z = 0.0f; m_rows[0].w = 0.0f;
3091  m_rows[1].x = 0.0f; m_rows[1].y = scale; m_rows[1].z = 0.0f; m_rows[1].w = 0.0f;
3092  m_rows[2].x = 0.0f; m_rows[2].y = 0.0f; m_rows[2].z = scale; m_rows[2].w = 0.0f;
3093  m_rows[3].x = 0.0f; m_rows[3].y = 0.0f; m_rows[3].z = 0.0f; m_rows[3].w = 1.0f;
3094  }
3095 
3096  inline void m44::Scale(const v3& scale)
3097  {
3098  m_rows[0] = v4(scale.GetX(), 0.0f, 0.0f, 0.0f);
3099  m_rows[1] = v4( 0.0f, scale.GetY(), 0.0f, 0.0f);
3100  m_rows[2] = v4( 0.0f, 0.0f, scale.GetZ(), 0.0f);
3101  m_rows[3] = v4( 0.0f, 0.0f, 0.0f, 1.0f);
3102  }
3103 
3104  inline void m44::RotateX(float theta)
3105  {
3106  Identity();
3107 
3108  float sin = Math::Sin(theta);
3109  float cos = Math::Cos(theta);
3110 
3111  m_rows[1] = v4(0.0f, cos, -sin, 0.0f);
3112  m_rows[2] = v4(0.0f, sin, cos, 0.0f);
3113  }
3114 
3115  inline void m44::RotateY(float theta)
3116  {
3117  Identity();
3118 
3119  float sin = Math::Sin(theta);
3120  float cos = Math::Cos(theta);
3121 
3122  m_rows[0] = v4( cos, 0.0f,-sin, 0.0f);
3123  m_rows[2] = v4( sin, 0.0f, cos, 0.0f);
3124  }
3125 
3126  inline void m44::RotateZ(float theta)
3127  {
3128  Identity();
3129 
3130  float sin = Math::Sin(theta);
3131  float cos = Math::Cos(theta);
3132 
3133  m_rows[0] = v4(cos, -sin, 0.0f, 0.0f);
3134  m_rows[1] = v4(sin, cos, 0.0f, 0.0f);
3135  }
3136 
3137  inline void m44::Rotate( float x, float y, float z )
3138  {
3139  m44 tempX;
3140  tempX.RotateX(x);
3141  m44 tempY;
3142  tempY.RotateY(-y);
3143  m44 tempZ;
3144  tempZ.RotateZ(z);
3145 
3146  m44 temp;
3147 
3148  Mul(temp, tempZ, tempY);
3149  Mul(*this, temp, tempX);
3150  }
3151 
3152  inline void m44::Rotate( const v3& axis, float angle )
3153  {
3154  float c = Math::Cos(angle);
3155  float s = Math::Sin(angle);
3156  float t = 1.0f - c;
3157 
3158  float x = axis.GetX();
3159  float y = axis.GetY();
3160  float z = axis.GetZ();
3161 
3162  SetRow(0, v4( (t*x*x) + c, (t*y*x) - (s*z), (t*z*x) + (s*y), 0.0f));
3163  SetRow(1, v4((t*x*y) + (s*z), (t*y*y) + c, (t*z*y) - (s*x), 0.0f));
3164  SetRow(2, v4((t*x*z) - (s*y), (t*y*z) + (s*x), (t*z*z) + c, 0.0f));
3165  SetRow(3, v4(0.0f, 0.0f, 0.0f, 1.0f));
3166  }
3167 
3168  inline void m44::Transpose(m44& dst, const m44& src)
3169  {
3170  m44 temp = src;
3171 
3172  v4 r0 = temp.GetRow(0);
3173  v4 r1 = temp.GetRow(1);
3174  v4 r2 = temp.GetRow(2);
3175  v4 r3 = temp.GetRow(3);
3176 
3177  dst.SetRow(0, v4(r0.x, r1.x, r2.x, r3.x));
3178  dst.SetRow(1, v4(r0.y, r1.y, r2.y, r3.y));
3179  dst.SetRow(2, v4(r0.z, r1.z, r2.z, r3.z));
3180  dst.SetRow(3, v4(r0.w, r1.w, r2.w, r3.w));
3181  }
3182 
3183  inline void m44::OrthoInverse(m44& dst, const m44& src)
3184  {
3185  v3 trans = v3(src.GetRow(3));
3186 
3187  v4 zero;
3188  zero.SetZero();
3189 
3190  dst = src;
3191  dst.SetRow(3, zero);
3192 
3193  Transpose(dst, dst);
3194 
3195  trans = -v3::MulRotate(trans, dst);
3196 
3197  dst.SetRow(3, v4(trans, 1.0f));
3198  }
3199 
3200  inline void m44::Mul( m44& dest, const m44& src1, const m44& src2 )
3201  {
3202  m44 temp;
3203  for(int i = 0; i < 4; i++)
3204  {
3205  temp.m_rows[i].x = src2.m_rows[i].x * src1.m_rows[0].x + src2.m_rows[i].y * src1.m_rows[1].x + src2.m_rows[i].z * src1.m_rows[2].x + src2.m_rows[i].w * src1.m_rows[3].x;
3206  temp.m_rows[i].y = src2.m_rows[i].x * src1.m_rows[0].y + src2.m_rows[i].y * src1.m_rows[1].y + src2.m_rows[i].z * src1.m_rows[2].y + src2.m_rows[i].w * src1.m_rows[3].y;
3207  temp.m_rows[i].z = src2.m_rows[i].x * src1.m_rows[0].z + src2.m_rows[i].y * src1.m_rows[1].z + src2.m_rows[i].z * src1.m_rows[2].z + src2.m_rows[i].w * src1.m_rows[3].z;
3208  temp.m_rows[i].w = src2.m_rows[i].x * src1.m_rows[0].w + src2.m_rows[i].y * src1.m_rows[1].w + src2.m_rows[i].z * src1.m_rows[2].w + src2.m_rows[i].w * src1.m_rows[3].w;
3209 
3210  }
3211 
3212  memcpy(&dest, &temp, sizeof(dest));
3213  }
3214 
3215  inline void m44::Lerp( m44& dst, const m44& from, const m44& to, float t )
3216  {
3217  for (int i = 0; i < 4; ++i)
3218  {
3219  const Eegeo::v4& va = from.GetRow(i);
3220  const Eegeo::v4& vb = to.GetRow(i);
3221  Eegeo::v4 dest = Eegeo::v4::Lerp(va, vb, t);
3222  dst.SetRow(i, dest);
3223  }
3224  }
3225 
3226  inline void m44::Mul( const float scale )
3227  {
3228  for(int i = 0; i < 4; i++)
3229  {
3230  m_rows[i].x *= scale;
3231  m_rows[i].y *= scale;
3232  m_rows[i].z *= scale;
3233  m_rows[i].w *= scale;
3234  }
3235  }
3236 
3237 
3238  inline dm44::dm44( const dv4& row0, const dv4& row1, const dv4& row2, const dv4& row3 )
3239  {
3240  m_rows[0] = row0;
3241  m_rows[1] = row1;
3242  m_rows[2] = row2;
3243  m_rows[3] = row3;
3244  }
3245 
3246  inline void dm44::SetFromBasis(const dv3& right, const dv3& up, const dv3& forward, const dv3& pos)
3247  {
3248  m_rows[0].Set(right.GetX(), right.GetY(), right.GetZ(), 0.0);
3249  m_rows[1].Set(up.GetX(), up.GetY(), up.GetZ(), 0.0);
3250  m_rows[2].Set(forward.GetX(), forward.GetY(), forward.GetZ(), 0.0);
3251  m_rows[3].Set(pos.GetX(), pos.GetY(), pos.GetZ(), 1.0);
3252  }
3253 
3254  inline void dm44::Identity()
3255  {
3256  m_rows[0].x = 1.0f; m_rows[0].y = 0.0f; m_rows[0].z = 0.0f; m_rows[0].w = 0.0f;
3257  m_rows[1].x = 0.0f; m_rows[1].y = 1.0f; m_rows[1].z = 0.0f; m_rows[1].w = 0.0f;
3258  m_rows[2].x = 0.0f; m_rows[2].y = 0.0f; m_rows[2].z = 1.0f; m_rows[2].w = 0.0f;
3259  m_rows[3].x = 0.0f; m_rows[3].y = 0.0f; m_rows[3].z = 0.0f; m_rows[3].w = 1.0f;
3260  }
3261 
3262  inline void dm44::Scale(double scale)
3263  {
3264  m_rows[0].x = scale; m_rows[0].y = 0.0f; m_rows[0].z = 0.0f; m_rows[0].w = 0.0f;
3265  m_rows[1].x = 0.0f; m_rows[1].y = scale; m_rows[1].z = 0.0f; m_rows[1].w = 0.0f;
3266  m_rows[2].x = 0.0f; m_rows[2].y = 0.0f; m_rows[2].z = scale; m_rows[2].w = 0.0f;
3267  m_rows[3].x = 0.0f; m_rows[3].y = 0.0f; m_rows[3].z = 0.0f; m_rows[3].w = 1.0f;
3268  }
3269 
3270  inline void dm44::Scale(const dv3& scale)
3271  {
3272  m_rows[0] = dv4(scale.GetX(), 0.0f, 0.0f, 0.0f);
3273  m_rows[1] = dv4( 0.0f, scale.GetY(), 0.0f, 0.0f);
3274  m_rows[2] = dv4( 0.0f, 0.0f, scale.GetZ(), 0.0f);
3275  m_rows[3] = dv4( 0.0f, 0.0f, 0.0f, 1.0f);
3276  }
3277 
3278  inline void dm44::RotateX(double theta)
3279  {
3280  Identity();
3281 
3282  double sin = std::sin(theta);
3283  double cos = std::cos(theta);
3284 
3285  m_rows[1] = dv4(0.0f, cos, -sin, 0.0f);
3286  m_rows[2] = dv4(0.0f, sin, cos, 0.0f);
3287  }
3288 
3289  inline void dm44::RotateY(double theta)
3290  {
3291  Identity();
3292 
3293  double sin = std::sin(theta);
3294  double cos = std::cos(theta);
3295 
3296  m_rows[0] = dv4( cos, 0.0f,-sin, 0.0f);
3297  m_rows[2] = dv4( sin, 0.0f, cos, 0.0f);
3298  }
3299 
3300  inline void dm44::RotateZ(double theta)
3301  {
3302  Identity();
3303 
3304  double sin = std::sin(theta);
3305  double cos = std::cos(theta);
3306 
3307  m_rows[0] = dv4(cos, -sin, 0.0f, 0.0f);
3308  m_rows[1] = dv4(sin, cos, 0.0f, 0.0f);
3309  }
3310 
3311  inline void dm44::Rotate( double x, double y, double z )
3312  {
3313  dm44 tempX;
3314  tempX.RotateX(x);
3315  dm44 tempY;
3316  tempY.RotateY(-y);
3317  dm44 tempZ;
3318  tempZ.RotateZ(z);
3319 
3320  dm44 temp;
3321 
3322  Mul(temp, tempZ, tempY);
3323  Mul(*this, temp, tempX);
3324  }
3325 
3326  inline void dm44::Rotate( const dv3& axis, double angle )
3327  {
3328  double c = std::cos(angle);
3329  double s = std::sin(angle);
3330  double t = 1.0f - c;
3331 
3332  double x = axis.GetX();
3333  double y = axis.GetY();
3334  double z = axis.GetZ();
3335 
3336  SetRow(0, dv4( (t*x*x) + c, (t*y*x) - (s*z), (t*z*x) + (s*y), 0.0f));
3337  SetRow(1, dv4((t*x*y) + (s*z), (t*y*y) + c, (t*z*y) - (s*x), 0.0f));
3338  SetRow(2, dv4((t*x*z) - (s*y), (t*y*z) + (s*x), (t*z*z) + c, 0.0f));
3339  SetRow(3, dv4(0.0f, 0.0f, 0.0f, 1.0f));
3340  }
3341 
3342  inline void dm44::Transpose(dm44& dst, const dm44& src)
3343  {
3344  dm44 temp = src;
3345 
3346  dv4 r0 = temp.GetRow(0);
3347  dv4 r1 = temp.GetRow(1);
3348  dv4 r2 = temp.GetRow(2);
3349  dv4 r3 = temp.GetRow(3);
3350 
3351  dst.SetRow(0, dv4(r0.x, r1.x, r2.x, r3.x));
3352  dst.SetRow(1, dv4(r0.y, r1.y, r2.y, r3.y));
3353  dst.SetRow(2, dv4(r0.z, r1.z, r2.z, r3.z));
3354  dst.SetRow(3, dv4(r0.w, r1.w, r2.w, r3.w));
3355  }
3356 
3357  inline void dm44::OrthoInverse(dm44& dst, const dm44& src)
3358  {
3359  dv3 trans = dv3(src.GetRow(3));
3360 
3361  dv4 zero;
3362  zero.SetZero();
3363 
3364  dst = src;
3365  dst.SetRow(3, zero);
3366 
3367  Transpose(dst, dst);
3368 
3369  trans = -dv3::MulRotate(trans, dst);
3370 
3371  dst.SetRow(3, dv4(trans, 1.0f));
3372  }
3373 
3374  inline void dm44::Mul( dm44& dest, const dm44& src1, const dm44& src2 )
3375  {
3376  dm44 temp;
3377  for(int i = 0; i < 4; i++)
3378  {
3379  temp.m_rows[i].x = src2.m_rows[i].x * src1.m_rows[0].x + src2.m_rows[i].y * src1.m_rows[1].x + src2.m_rows[i].z * src1.m_rows[2].x + src2.m_rows[i].w * src1.m_rows[3].x;
3380  temp.m_rows[i].y = src2.m_rows[i].x * src1.m_rows[0].y + src2.m_rows[i].y * src1.m_rows[1].y + src2.m_rows[i].z * src1.m_rows[2].y + src2.m_rows[i].w * src1.m_rows[3].y;
3381  temp.m_rows[i].z = src2.m_rows[i].x * src1.m_rows[0].z + src2.m_rows[i].y * src1.m_rows[1].z + src2.m_rows[i].z * src1.m_rows[2].z + src2.m_rows[i].w * src1.m_rows[3].z;
3382  temp.m_rows[i].w = src2.m_rows[i].x * src1.m_rows[0].w + src2.m_rows[i].y * src1.m_rows[1].w + src2.m_rows[i].z * src1.m_rows[2].w + src2.m_rows[i].w * src1.m_rows[3].w;
3383 
3384  }
3385 
3386  memcpy(&dest, &temp, sizeof(dest));
3387  }
3388 
3389  inline void dm44::Mul( const double scale )
3390  {
3391  for(int i = 0; i < 4; i++)
3392  {
3393  m_rows[i].x *= scale;
3394  m_rows[i].y *= scale;
3395  m_rows[i].z *= scale;
3396  m_rows[i].w *= scale;
3397  }
3398  }
3399 
3400  inline void dm33::Identity()
3401  {
3402  _11 = 1.0; _12 = 0.0; _13 = 0.0;
3403  _21 = 0.0; _22 = 1.0; _23 = 0.0;
3404  _31 = 0.0; _32 = 0.0; _33 = 1.0;
3405  }
3406 
3407  inline void dm33::Transpose( dm33& dst, const dm33& src )
3408  {
3409  dm33 temp = src;
3410 
3411  dst._11 = temp._11;
3412  dst._12 = temp._21;
3413  dst._13 = temp._31;
3414 
3415  dst._21 = temp._12;
3416  dst._22 = temp._22;
3417  dst._23 = temp._32;
3418 
3419  dst._31 = temp._13;
3420  dst._32 = temp._23;
3421  dst._33 = temp._33;
3422  }
3423 
3424  inline void dm33::SetRow ( int row, const dv3& data )
3425  {
3426  switch (row)
3427  {
3428  case 0:
3429  _11 = data.x;
3430  _12 = data.y;
3431  _13 = data.z;
3432  break;
3433  case 1:
3434  _21 = data.x;
3435  _22 = data.y;
3436  _23 = data.z;
3437  break;
3438  case 2:
3439  _31 = data.x;
3440  _32 = data.y;
3441  _33 = data.z;
3442  break;
3443  default:
3444  Eegeo_ASSERT(false, "row out of range");
3445  };
3446  }
3447 
3448  inline void dm33::SetFromBasis(const dv3& right, const dv3& up, const dv3& forward)
3449  {
3450  _11 = right.x;
3451  _12 = right.y;
3452  _13 = right.z;
3453  _21 = up.x;
3454  _22 = up.y;
3455  _23 = up.z;
3456  _31 = forward.x;
3457  _32 = forward.y;
3458  _33 = forward.z;
3459  }
3460 
3461  inline dv3 dm33::GetRow ( int row ) const
3462  {
3463  switch (row)
3464  {
3465  case 0:
3466  return dv3(_11, _12, _13);
3467  case 1:
3468  return dv3(_21, _22, _23);
3469  case 2:
3470  return dv3(_31, _32, _33);
3471  default:
3472  Eegeo_ASSERT(false, "row out of range");
3473  return dv3::Zero();
3474  };
3475  }
3476 
3477  inline void dm33::Scale(double scale)
3478  {
3479  _11 = scale; _12 = 0.0; _13 = 0.0;
3480  _21 = 0.0; _22 = scale; _23 = 0.0;
3481  _31 = 0.0; _32 = 0.0; _33 = scale;
3482  }
3483 
3484  inline void dm33::Scale(const dv3& s)
3485  {
3486  _11 = s.GetX(); _12 = 0.0; _13 = 0.0;
3487  _21 = 0.0; _22 = s.GetY(); _23 = 0.0;
3488  _31 = 0.0; _32 = 0.0; _33 = s.GetZ();
3489  }
3490 
3491  inline void dm33::RotateX(double theta)
3492  {
3493  Identity();
3494 
3495  double sin = std::sin(theta);
3496  double cos = std::cos(theta);
3497 
3498  SetRow(1, dv3(0.0, cos, -sin));
3499  SetRow(2, dv3(0.0, sin, cos));
3500  }
3501 
3502  inline void dm33::RotateY(double theta)
3503  {
3504  Identity();
3505 
3506  double sin = std::sin(theta);
3507  double cos = std::cos(theta);
3508 
3509  SetRow(0, dv3(cos, 0.0, -sin));
3510  SetRow(2, dv3(sin, 0.0 , cos));
3511  }
3512 
3513  inline void dm33::RotateZ(double theta)
3514  {
3515  Identity();
3516 
3517  double sin = std::sin(theta);
3518  double cos = std::cos(theta);
3519 
3520  SetRow(0, dv3(cos, -sin, 0.0));
3521  SetRow(1, dv3(sin, cos, 0.0));
3522  }
3523 
3524  inline void dm33::Rotate( double x, double y, double z )
3525  {
3526  dm33 tempX;
3527  tempX.RotateX(x);
3528  dm33 tempY;
3529  tempY.RotateY(-y);
3530  dm33 tempZ;
3531  tempZ.RotateZ(z);
3532  dm33 temp;
3533 
3534  Mul(temp, tempZ, tempY);
3535  Mul(*this, temp, tempX);
3536  }
3537 
3538  inline void dm33::Rotate( const dv3& axis, double angle )
3539  {
3540  double c = std::cos(angle);
3541  double s = std::sin(angle);
3542  double t = 1.0 - c;
3543 
3544  double x = axis.GetX();
3545  double y = axis.GetY();
3546  double z = axis.GetZ();
3547 
3548  SetRow(0, dv3( (t*x*x) + c, (t*y*x) - (s*z), (t*z*x) + (s*y)));
3549  SetRow(1, dv3((t*x*y) + (s*z), (t*y*y) + c, (t*z*y) - (s*x)));
3550  SetRow(2, dv3((t*x*z) - (s*y), (t*y*z) + (s*x), (t*z*z) + c));
3551  }
3552 
3553  inline void dm33::Mul( dm33& dest, const dm33& src1, const dm33& src2 )
3554  {
3555  dm33 temp;
3556 
3557  const dv3& src1row0 = src1.GetRow(0);
3558  const dv3& src1row1 = src1.GetRow(1);
3559  const dv3& src1row2 = src1.GetRow(2);
3560 
3561  for(int i = 0; i < 3; i++)
3562  {
3563  const dv3& src2rowI = src2.GetRow(i);
3564 
3565  double x = src2rowI.x * src1row0.x +
3566  src2rowI.y * src1row1.x +
3567  src2rowI.z * src1row2.x;
3568 
3569  double y = src2rowI.x * src1row0.y +
3570  src2rowI.y * src1row1.y +
3571  src2rowI.z * src1row2.y;
3572 
3573  double z = src2rowI.x * src1row0.z +
3574  src2rowI.y * src1row1.z +
3575  src2rowI.z * src1row2.z;
3576 
3577  temp.SetRow(i, dv3(x, y, z));
3578  }
3579 
3580  memcpy(&dest, &temp, sizeof(dest));
3581  }
3582 
3583  inline void dm33::Mul( const double scale )
3584  {
3585  _11 *= scale;
3586  _12 *= scale;
3587  _13 *= scale;
3588  _21 *= scale;
3589  _22 *= scale;
3590  _23 *= scale;
3591  _31 *= scale;
3592  _32 *= scale;
3593  _33 *= scale;
3594  }
3595 }
Generated on Sat Jun 10 2023 02:00:29 for eeGeo Platform SDK by   doxygen 1.8.3.1