1 /* Copyright 2011 Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file float3x3.cpp
16         @author Jukka Jyl�nki
17         @brief */
18 #include <string.h>
19 #include "assume.h"
20 #include "Math/MathFunc.h"
21 #include "Math/float3.h"
22 #include "Math/float4.h"
23 #include "Math/float3x3.h"
24 #include "Math/float3x4.h"
25 #include "Math/float4x4.h"
26 #include "Matrix.inl"
27 #include "Math/Quat.h"
28 #include "Algorithm/Random/LCG.h"
29 #include "Geometry/Plane.h"
30 #include "TransformOps.h"
31
32 MATH_BEGIN_NAMESPACE
33
34 float3x3::float3x3(float _00, float _01, float _02,
35                    float _10, float _11, float _12,
36                    float _20, float _21, float _22)
37 {
38         Set(_00, _01, _02,
39             _10, _11, _12,
40             _20, _21, _22);
41 }
42
43 float3x3::float3x3(const float3 &col0, const float3 &col1, const float3 &col2)
44 {
45         SetCol(0, col0);
46         SetCol(1, col1);
47         SetCol(2, col2);
48 }
49
50 float3x3::float3x3(const Quat &orientation)
51 {
52         SetRotatePart(orientation);
53 }
54
55 float3x3 float3x3::RotateX(float angle)
56 {
57         float3x3 r;
58         r.SetRotatePartX(angle);
59         return r;
60 }
61
62 float3x3 float3x3::RotateY(float angle)
63 {
64         float3x3 r;
65         r.SetRotatePartY(angle);
66         return r;
67 }
68
69 float3x3 float3x3::RotateZ(float angle)
70 {
71         float3x3 r;
72         r.SetRotatePartZ(angle);
73         return r;
74 }
75
76 float3x3 float3x3::RotateAxisAngle(const float3 &axisDirection, float angleRadians)
77 {
78         float3x3 r;
79         r.SetRotatePart(axisDirection, angleRadians);
80         return r;
81 }
82
83 float3x3 float3x3::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
84 {
85         float3x3 r;
86         r.SetRotatePart(Quat::RotateFromTo(sourceDirection, targetDirection));
87         return r;
88 }
89
90 float3x3 float3x3::RandomRotation(LCG &lcg)
91 {
92         // The easiest way to generate a random orientation is through quaternions, so convert a 
93         // random quaternion to a rotation matrix.
94         return FromQuat(Quat::RandomRotation(lcg));
95 }
96
97 float3x3 float3x3::RandomGeneral(LCG &lcg, float minElem, float maxElem)
98 {
99         float3x3 m;
100         for(int y = 0; y < 3; ++y)
101                 for(int x = 0; x < 3; ++x)
102                         m[y][x] = lcg.Float(minElem, maxElem);
103         return m;
104 }
105
106 float3x3 float3x3::FromQuat(const Quat &orientation)
107 {
108         float3x3 r;
109         r.SetRotatePart(orientation);
110         return r;
111 }
112
113 Quat float3x3::ToQuat() const
114 {
115         return Quat(*this);
116 }
117
118 float3x3 float3x3::FromRS(const Quat &rotate, const float3 &scale)
119 {
120         return float3x3(rotate) * float3x3::Scale(scale);
121 }
122
123 float3x3 float3x3::FromRS(const float3x3 &rotate, const float3 &scale)
124 {
125         return rotate * float3x3::Scale(scale);
126 }
127
128 float3x3 float3x3::FromEulerXYX(float x2, float y, float x)
129 {
130         float3x3 r;
131         Set3x3PartRotateEulerXYX(r, x2, y, x);
132         assume(r.Equals(float3x3::RotateX(x2) * float3x3::RotateY(y) * float3x3::RotateX(x)));
133         return r;
134 }
135
136 float3x3 float3x3::FromEulerXZX(float x2, float z, float x)
137 {
138         float3x3 r;
139         Set3x3PartRotateEulerXZX(r, x2, z, x);
140         assume(r.Equals(float3x3::RotateX(x2) * float3x3::RotateZ(z) * float3x3::RotateX(x)));
141         return r;
142 }
143
144 float3x3 float3x3::FromEulerYXY(float y2, float x, float y)
145 {
146         float3x3 r;
147         Set3x3PartRotateEulerYXY(r, y2, x, y);
148         assume(r.Equals(float3x3::RotateY(y2) * float3x3::RotateX(x) * float3x3::RotateY(y)));
149         return r;
150 }
151
152 float3x3 float3x3::FromEulerYZY(float y2, float z, float y)
153 {
154         float3x3 r;
155         Set3x3PartRotateEulerYZY(r, y2, z, y);
156         assume(r.Equals(float3x3::RotateY(y2) * float3x3::RotateZ(z) * float3x3::RotateY(y)));
157         return r;
158 }
159
160 float3x3 float3x3::FromEulerZXZ(float z2, float x, float z)
161 {
162         float3x3 r;
163         Set3x3PartRotateEulerZXZ(r, z2, x, z);
164         assume(r.Equals(float3x3::RotateZ(z2) * float3x3::RotateX(x) * float3x3::RotateZ(z)));
165         return r;
166 }
167
168 float3x3 float3x3::FromEulerZYZ(float z2, float y, float z)
169 {
170         float3x3 r;
171         Set3x3PartRotateEulerZYZ(r, z2, y, z);
172         assume(r.Equals(float3x3::RotateZ(z2) * float3x3::RotateY(y) * float3x3::RotateZ(z)));
173         return r;
174 }
175
176 float3x3 float3x3::FromEulerXYZ(float x, float y, float z)
177 {
178         float3x3 r;
179         Set3x3PartRotateEulerXYZ(r, x, y, z);
180         assume(r.Equals(float3x3::RotateX(x) * float3x3::RotateY(y) * float3x3::RotateX(z)));
181         return r;
182 }
183
184 float3x3 float3x3::FromEulerXZY(float x, float z, float y)
185 {
186         float3x3 r;
187         Set3x3PartRotateEulerXZY(r, x, z, y);
188         assume(r.Equals(float3x3::RotateX(x) * float3x3::RotateZ(z) * float3x3::RotateY(y)));
189         return r;
190 }
191
192 float3x3 float3x3::FromEulerYXZ(float y, float x, float z)
193 {
194         float3x3 r;
195         Set3x3PartRotateEulerYXZ(r, y, x, z);
196         assume(r.Equals(float3x3::RotateY(y) * float3x3::RotateX(x) * float3x3::RotateZ(z)));
197         return r;
198 }
199
200 float3x3 float3x3::FromEulerYZX(float y, float z, float x)
201 {
202         float3x3 r;
203         Set3x3PartRotateEulerYZX(r, y, z, x);
204         assume(r.Equals(float3x3::RotateY(y) * float3x3::RotateZ(z) * float3x3::RotateX(x)));
205         return r;
206 }
207
208 float3x3 float3x3::FromEulerZXY(float z, float x, float y)
209 {
210         float3x3 r;
211         Set3x3PartRotateEulerZXY(r, z, x, y);
212         assume(r.Equals(float3x3::RotateZ(z) * float3x3::RotateX(x) * float3x3::RotateY(y)));
213         return r;
214 }
215
216 float3x3 float3x3::FromEulerZYX(float z, float y, float x)
217 {
218         float3x3 r;
219         Set3x3PartRotateEulerZYX(r, z, y, x);
220         assume(r.Equals(float3x3::RotateZ(z) * float3x3::RotateY(y) * float3x3::RotateX(x)));
221         return r;
222 }
223
224 ScaleOp float3x3::Scale(float sx, float sy, float sz)
225 {
226         return ScaleOp(sx, sy, sz);
227 }
228
229 ScaleOp float3x3::Scale(const float3 &scale)
230 {
231         return ScaleOp(scale);
232 }
233
234 float3x3 float3x3::ScaleAlongAxis(const float3 &axis, float scalingFactor)
235 {
236         return Scale(axis * scalingFactor);
237 }
238
239 ScaleOp float3x3::UniformScale(float uniformScale)
240 {
241         return ScaleOp(uniformScale, uniformScale, uniformScale);
242 }
243
244 float3 float3x3::GetScale() const
245 {
246         return float3(Col(0).Length(), Col(1).Length(), Col(2).Length());
247 }
248
249 float3x3 float3x3::ShearX(float yFactor, float zFactor)
250 {
251         assume(isfinite(yFactor));
252         assume(isfinite(zFactor));
253         return float3x3(1.f, yFactor, zFactor,
254                         0.f,     1.f,     0.f,
255                         0.f,     0.f,     1.f);
256 }
257
258 float3x3 float3x3::ShearY(float xFactor, float zFactor)
259 {
260         assume(isfinite(xFactor));
261         assume(isfinite(zFactor));
262         return float3x3(1.f,     0.f,     0.f,
263                         xFactor, 1.f, zFactor,
264                         0.f,     0.f,     1.f);
265 }
266
267 float3x3 float3x3::ShearZ(float xFactor, float yFactor)
268 {
269         assume(isfinite(xFactor));
270         assume(isfinite(yFactor));
271         return float3x3(1.f,         0.f, 0.f,
272                         0.f,         1.f, 0.f,
273                         xFactor, yFactor, 1.f);
274 }
275
276 float3x3 float3x3::Mirror(const Plane &p)
277 {
278         assume(p.PassesThroughOrigin() && "A 3x3 matrix cannot represent mirroring about planes which do not pass through the origin! Use float3x4::Mirror instead!");
279         float3x3 v;
280         SetMatrix3x3LinearPlaneMirror(v, p.normal.x, p.normal.y, p.normal.z);
281         return v;
282 }
283
284 float3x3 float3x3::OrthographicProjection(const Plane &p)
285 {
286         assume(p.normal.IsNormalized());
287         assume(p.PassesThroughOrigin() && "A 3x3 matrix cannot represent projection onto planes which do not pass through the origin! Use float3x4::OrthographicProjection instead!");
288         float3x3 v;
289         SetMatrix3x3LinearPlaneProject(v, p.normal.x, p.normal.y, p.normal.z);
290         return v;
291 }
292
293 float3x3 float3x3::OrthographicProjectionYZ()
294 {
295         float3x3 v = identity;
296         v[0][0] = 0.f;
297         return v;
298 }
299
300 float3x3 float3x3::OrthographicProjectionXZ()
301 {
302         float3x3 v = identity;
303         v[1][1] = 0.f;
304         return v;
305 }
306
307 float3x3 float3x3::OrthographicProjectionXY()
308 {
309         float3x3 v = identity;
310         v[2][2] = 0.f;
311         return v;
312 }
313
314 MatrixProxy<float3x3::Cols> &float3x3::operator[](int row)
315 {
316         assume(row >= 0);
317         assume(row < Rows);
318
319 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
320         if (row < 0 || row >= Rows)
321                 row = 0; // Benign failure, just give the first row.
322 #endif
323         return *(reinterpret_cast<MatrixProxy<Cols>*>(v[row]));
324 }
325
326 const MatrixProxy<float3x3::Cols> &float3x3::operator[](int row) const
327 {
328         assume(row >= 0);
329         assume(row < Rows);
330
331 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
332         if (row < 0 || row >= Rows)
333                 row = 0; // Benign failure, just give the first row.
334 #endif
335         return *(reinterpret_cast<const MatrixProxy<Cols>*>(v[row]));
336 }
337
338 float &float3x3::At(int row, int col)
339 {
340         assume(row >= 0);
341         assume(row < Rows);
342         assume(col >= 0);
343         assume(col < Cols);
344 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
345         if (row < 0 || row >= Rows || col < 0 || col >= Cols)
346                 return v[0][0]; // Benign failure, return the first element.
347 #endif
348         return v[row][col];
349 }
350
351 CONST_WIN32 float float3x3::At(int row, int col) const
352 {
353         assume(row >= 0);
354         assume(row < Rows);
355         assume(col >= 0);
356         assume(col < Cols);
357 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
358         if (row < 0 || row >= Rows || col < 0 || col >= Cols)
359                 return FLOAT_NAN;
360 #endif
361         return v[row][col];
362 }
363
364 float3 &float3x3::Row(int row)
365 {
366         assume(row >= 0);
367         assume(row < Rows);
368 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
369         if (row < 0 || row >= Rows)
370                 row = 0; // Benign failure, just give the first row.
371 #endif
372         return reinterpret_cast<float3 &>(v[row]);
373 }
374
375 const float3 &float3x3::Row(int row) const
376 {
377         assume(row >= 0);
378         assume(row < Rows);
379 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
380         if (row < 0 || row >= Rows)
381                 row = 0; // Benign failure, just give the first row.
382 #endif
383         return reinterpret_cast<const float3 &>(v[row]);
384 }
385
386 CONST_WIN32 float3 float3x3::Col(int col) const
387 {
388         assume(col >= 0);
389         assume(col < Cols);
390 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
391         if (col < 0 || col >= Cols)
392                 return float3::nan;
393 #endif
394
395         return float3(v[0][col], v[1][col], v[2][col]);
396 }
397
398 CONST_WIN32 float3 float3x3::Diagonal() const
399 {
400         return float3(v[0][0], v[1][1], v[2][2]);
401 }
402
403 void float3x3::ScaleRow(int row, float scalar)
404 {
405         assume(row >= 0);
406         assume(row < Rows);
407 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
408         if (row < 0 || row >= Rows)
409                 return;
410 #endif
411         assume(isfinite(scalar));
412         Row(row) *= scalar;
413 }
414
415 void float3x3::ScaleCol(int col, float scalar)
416 {
417         assume(col >= 0);
418         assume(col < Cols);
419 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
420         if (col < 0 || col >= Cols)
421                 return;
422 #endif
423         v[0][col] *= scalar;
424         v[1][col] *= scalar;
425         v[2][col] *= scalar;
426 }
427
428 float3 float3x3::WorldX() const
429 {
430         return Col(0);
431 }
432
433 float3 float3x3::WorldY() const
434 {
435         return Col(1);
436 }
437
438 float3 float3x3::WorldZ() const
439 {
440         return Col(2);
441 }
442
443 float *float3x3::ptr()
444 {
445         return &v[0][0];
446 }
447
448 const float *float3x3::ptr() const
449 {
450         return &v[0][0];
451 }
452
453 void float3x3::SetRow(int row, float x, float y, float z)
454 {
455         assume(row >= 0);
456         assume(row < Rows);
457 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
458         if (row < 0 || row >= Rows)
459                 return;
460 #endif
461         assume(isfinite(x));
462         assume(isfinite(y));
463         assume(isfinite(z));
464         v[row][0] = x;
465         v[row][1] = y;
466         v[row][2] = z;
467 }
468
469 void float3x3::SetRow(int row, const float3 &rowVector)
470 {
471         SetRow(row, rowVector.x, rowVector.y, rowVector.z);
472 }
473
474 void float3x3::SetRow(int row, const float *data)
475 {
476         assume(data);
477 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
478         if (!data)
479                 return;
480 #endif
481         SetRow(row, data[0], data[1], data[2]);
482 }
483
484 void float3x3::SetCol(int column, float x, float y, float z)
485 {
486         assume(column >= 0);
487         assume(column < Cols);
488 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
489         if (column < 0 || column >= Cols)
490                 return;
491 #endif
492         assume(isfinite(x));
493         assume(isfinite(y));
494         assume(isfinite(z));
495         v[0][column] = x;
496         v[1][column] = y;
497         v[2][column] = z;
498 }
499
500 void float3x3::SetCol(int column, const float3 &columnVector)
501 {
502         SetCol(column, columnVector.x, columnVector.y, columnVector.z);
503 }
504
505 void float3x3::SetCol(int column, const float *data)
506 {
507         assume(data);
508 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
509         if (!data)
510                 return;
511 #endif
512         SetCol(column, data[0], data[1], data[2]);
513 }
514
515 void float3x3::Set(float _00, float _01, float _02,
516                    float _10, float _11, float _12,
517                    float _20, float _21, float _22)
518 {
519         v[0][0] = _00; v[0][1] = _01; v[0][2] = _02;
520         v[1][0] = _10; v[1][1] = _11; v[1][2] = _12;
521         v[2][0] = _20; v[2][1] = _21; v[2][2] = _22;
522 }
523
524 void float3x3::Set(const float3x3 &rhs)
525 {
526         Set(rhs.ptr());
527 }
528
529 void float3x3::Set(const float *values)
530 {
531         assume(values);
532 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
533         if (!values)
534                 return;
535 #endif
536         memcpy(ptr(), values, sizeof(float) * Rows * Cols);
537 }
538
539 void float3x3::Set(int row, int col, float value)
540 {
541         assume(0 <= row && row <= 2);
542         assume(0 <= col && col <= 2);
543 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
544         if (row < 0 || row >= Rows || col < 0 || col >= Cols)
545                 return;
546 #endif
547         v[row][col] = value;
548 }
549
550 void float3x3::SetIdentity()
551 {
552         Set(1,0,0,
553             0,1,0,
554             0,0,1);
555 }
556
557 void float3x3::SwapColumns(int col1, int col2)
558 {
559         assume(col1 >= 0);
560         assume(col1 < Cols);
561         assume(col2 >= 0);
562         assume(col2 < Cols);
563 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
564         if (col1 < 0 || col1 >= Cols || col2 < 0 || col2 >= Cols)
565                 return;
566 #endif
567         Swap(v[0][col1], v[0][col2]);
568         Swap(v[1][col1], v[1][col2]);
569         Swap(v[2][col1], v[2][col2]);
570 }
571
572 void float3x3::SwapRows(int row1, int row2)
573 {
574         assume(row1 >= 0);
575         assume(row1 < Rows);
576         assume(row2 >= 0);
577         assume(row2 < Rows);
578 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
579         if (row1 < 0 || row1 >= Rows || row2 < 0 || row2 >= Rows)
580                 return;
581 #endif
582         Swap(v[row1][0], v[row2][0]);
583         Swap(v[row1][1], v[row2][1]);
584         Swap(v[row1][2], v[row2][2]);
585 }
586
587 void float3x3::SetRotatePartX(float angle)
588 {
589         Set3x3PartRotateX(*this, angle);
590 }
591
592 void float3x3::SetRotatePartY(float angle)
593 {
594         Set3x3PartRotateY(*this, angle);
595 }
596
597 void float3x3::SetRotatePartZ(float angle)
598 {
599         Set3x3PartRotateZ(*this, angle);
600 }
601
602 void float3x3::SetRotatePart(const float3 &axisDirection, float angle)
603 {
604         SetRotatePart(Quat(axisDirection, angle));
605 }
606
607 void float3x3::SetRotatePart(const Quat &q)
608 {
609         SetMatrixRotatePart(*this, q);
610 }
611
612 float3x3 float3x3::LookAt(const float3 &localForward, const float3 &targetDirection, const float3 &localUp, const float3 &worldUp)
613 {
614         // The user must have inputted proper normalized input direction vectors.
615         assume(localForward.IsNormalized());
616         assume(targetDirection.IsNormalized());
617         assume(localUp.IsNormalized());
618         assume(worldUp.IsNormalized());
619
620         // In the local space, the forward and up directions must be perpendicular to be well-formed.
621         assume(localForward.IsPerpendicular(localUp));
622
623         // Generate the third basis vector in the local space.
624         float3 localRight = localUp.Cross(localForward).Normalized();
625
626         // A. Now we have an orthonormal linear basis { localRight, localUp, localForward } for the object local space.
627
628         // Generate the third basis vector for the world space.
629         float3 worldRight = worldUp.Cross(targetDirection).Normalized();
630         // Since the input worldUp vector is not necessarily perpendicular to the targetDirection vector, 
631         // we need to compute the real world space up vector that the "head" of the object will point 
632         // towards when the model is looking towards the desired target direction.
633         float3 perpWorldUp = targetDirection.Cross(worldRight).Normalized();
634         
635         // B. Now we have an orthonormal linear basis { worldRight, perpWorldUp, targetDirection } for the desired target orientation.
636
637         // We want to build a matrix M that performs the following mapping:
638         // 1. localRight must be mapped to worldRight.        (M * localRight = worldRight)
639         // 2. localUp must be mapped to perpWorldUp.          (M * localUp = perpWorldUp)
640         // 3. localForward must be mapped to targetDirection. (M * localForward = targetDirection)
641         // i.e. we want to map the basis A to basis B.
642
643         // This matrix M exists, and it is an orthonormal rotation matrix with a determinant of +1, because 
644         // the bases A and B are orthonormal with the same handedness.
645
646         // Below, use the notation that (a,b,c) is a 3x3 matrix with a as its first column, b second, and c third.
647         
648         // By algebraic manipulation, we can rewrite conditions 1, 2 and 3 in a matrix form:
649         //        M * (localRight, localUp, localForward) = (worldRight, perpWorldUp, targetDirection)
650         // or     M = (worldRight, perpWorldUp, targetDirection) * (localRight, localUp, localForward)^{-1}.
651         // or     M = m1 * m2, where
652
653         // m1 equals (worldRight, perpWorldUp, targetDirection):
654         float3x3 m1(worldRight, perpWorldUp, targetDirection);
655
656         // and m2 equals (localRight, localUp, localForward)^{-1}:
657         float3x3 m2;
658         m2.SetRow(0, localRight);
659         m2.SetRow(1, localUp);
660         m2.SetRow(2, localForward);
661         // Above we used the shortcut that for an orthonormal matrix M, M^{-1} = M^T. So set the rows
662         // and not the columns to directly produce the transpose, i.e. the inverse of (localRight, localUp, localForward).
663
664         // Compute final M.
665         m2 = m1 * m2;
666
667         // And fix any numeric stability issues by re-orthonormalizing the result.
668         m2.Orthonormalize(0, 1, 2);
669         return m2;
670 }
671
672 float3x3 &float3x3::operator =(const Quat &rhs)
673 {
674         SetRotatePart(rhs);
675         return *this;
676 }
677
678 float3x3 &float3x3::operator =(const float3x3 &rhs)
679 {
680         memcpy(this, &rhs, sizeof(rhs));
681         return *this;
682 }
683
684 float float3x3::Determinant() const
685 {
686         assume(IsFinite());
687         const float a = v[0][0];
688         const float b = v[0][1];
689         const float c = v[0][2];
690         const float d = v[1][0];
691         const float e = v[1][1];
692         const float f = v[1][2];
693         const float g = v[2][0];
694         const float h = v[2][1];
695         const float i = v[2][2];
696
697         return a*(e*i - f*h) + b*(f*g - d*i) + c*(d*h - e*g);
698 }
699
700 #define SKIPNUM(val, skip) (val < skip ? val : skip + 1)
701 /* ///@todo Enable when float2x2 is implemented.
702 float2x2 float3x3::SubMatrix(int i, int j) const
703 {
704         int r0 = SKIPNUM(0, i);
705         int r1 = SKIPNUM(1, i);
706         int c0 = SKIPNUM(0, j);
707         int c1 = SKIPNUM(1, j);
708
709         return float2x2(v[r0][c0], v[r0][c1],
710                         v[r1][c0], v[r1][c1]);
711 }
712
713 float float3x3::Minor(int i, int j) const
714 {
715         int r0 = SKIPNUM(0, i);
716         int r1 = SKIPNUM(1, i);
717         int c0 = SKIPNUM(0, j);
718         int c1 = SKIPNUM(1, j);
719
720         return v[r0][c0] * v[r1][c1] - v[r0][c1] * v[r1][c0];
721 }
722
723 float3x3 float3x3::Adjugate() const
724 {
725         float3x3 a;
726         for(int y = 0; y < Rows; ++y)
727                 for(int x = 0; x < Cols; ++x)
728                         a[y][x] = (((x+y) & 1) != 0) ? -Minor(y, x) : Minor(y, x);
729
730         return a;
731 }
732 */
733 bool float3x3::Inverse()
734 {
735 #ifdef MATH_ASSERT_CORRECTNESS
736         float3x3 orig = *this;
737 #endif
738
739         // There exists a generic matrix inverse calculator that uses Gaussian elimination.
740         // It would be invoked by calling
741         // return InverseMatrix(*this);
742         // Instead, compute the inverse directly using Cramer's rule.
743         float d = Determinant();
744         if (EqualAbs(d, 0.f))
745                 return false;
746
747         d = 1.f / d;
748         float3x3 i;
749         i[0][0] = d * (v[1][1] * v[2][2] - v[1][2] * v[2][1]);
750         i[0][1] = d * (v[0][2] * v[2][1] - v[0][1] * v[2][2]);
751         i[0][2] = d * (v[0][1] * v[1][2] - v[0][2] * v[1][1]);
752
753         i[1][0] = d * (v[1][2] * v[2][0] - v[1][0] * v[2][2]);
754         i[1][1] = d * (v[0][0] * v[2][2] - v[0][2] * v[2][0]);
755         i[1][2] = d * (v[0][2] * v[1][0] - v[0][0] * v[1][2]);
756
757         i[2][0] = d * (v[1][0] * v[2][1] - v[1][1] * v[2][0]);
758         i[2][1] = d * (v[2][0] * v[0][1] - v[0][0] * v[2][1]);
759         i[2][2] = d * (v[0][0] * v[1][1] - v[0][1] * v[1][0]);
760         *this = i;
761
762         mathassert((orig * *this).IsIdentity());
763         return true;
764 }
765
766 float3x3 float3x3::Inverted() const
767 {
768         float3x3 copy = *this;
769         copy.Inverse();
770         return copy;
771 }
772
773 bool float3x3::InverseColOrthogonal()
774 {
775 #ifdef MATH_ASSERT_CORRECTNESS
776         float3x3 orig = *this;
777 #endif
778         assume(IsColOrthogonal());
779         float s1 = float3(v[0][0], v[1][0], v[2][0]).LengthSq();
780         float s2 = float3(v[0][1], v[1][1], v[2][1]).LengthSq();
781         float s3 = float3(v[0][2], v[1][2], v[2][2]).LengthSq();
782         if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
783                 return false;
784         s1 = 1.f / s1;
785         s2 = 1.f / s2;
786         s3 = 1.f / s3;
787         Swap(v[0][1], v[1][0]);
788         Swap(v[0][2], v[2][0]);
789         Swap(v[1][2], v[2][1]);
790
791         v[0][0] *= s1; v[0][1] *= s1; v[0][2] *= s1;
792         v[1][0] *= s2; v[1][1] *= s2; v[1][2] *= s2;
793         v[2][0] *= s3; v[2][1] *= s3; v[2][2] *= s3;
794
795         mathassert(!orig.IsInvertible()|| (orig * *this).IsIdentity());
796         mathassert(IsRowOrthogonal());
797
798         return true;
799 }
800
801 bool float3x3::InverseOrthogonalUniformScale()
802 {
803 #ifdef MATH_ASSERT_CORRECTNESS
804         float3x3 orig = *this;
805 #endif
806         assume(IsColOrthogonal());
807         assume(HasUniformScale());
808         float scale = float3(v[0][0], v[1][0], v[2][0]).LengthSq();
809         if (scale < 1e-8f)
810                 return false;
811         scale = 1.f / scale;
812         Swap(v[0][1], v[1][0]);
813         Swap(v[0][2], v[2][0]);
814         Swap(v[1][2], v[2][1]);
815
816         v[0][0] *= scale; v[0][1] *= scale; v[0][2] *= scale;
817         v[1][0] *= scale; v[1][1] *= scale; v[1][2] *= scale;
818         v[2][0] *= scale; v[2][1] *= scale; v[2][2] *= scale;
819
820         assume(IsFinite());
821         assume(IsColOrthogonal());
822         assume(HasUniformScale());
823
824         mathassert(!orig.IsInvertible()|| (orig * *this).IsIdentity());
825
826         return true;
827 }
828
829 void float3x3::InverseOrthonormal()
830 {
831 #ifdef MATH_ASSERT_CORRECTNESS
832         float3x3 orig = *this;
833 #endif
834         assume(IsOrthonormal());
835         Transpose();
836         mathassert(!orig.IsInvertible()|| (orig * *this).IsIdentity());
837 }
838
839 void float3x3::Transpose()
840 {
841         Swap(v[0][1], v[1][0]);
842         Swap(v[0][2], v[2][0]);
843         Swap(v[1][2], v[2][1]);
844 }
845
846 float3x3 float3x3::Transposed() const
847 {
848         float3x3 copy = *this;
849         copy.Transpose();
850         return copy;
851 }
852
853 bool float3x3::InverseTranspose()
854 {
855         bool success = Inverse();
856         Transpose();
857         return success;
858 }
859
860 float3x3 float3x3::InverseTransposed() const
861 {
862         float3x3 copy = *this;
863         copy.Transpose();
864         copy.Inverse();
865         return copy;
866 }
867
868 float float3x3::Trace() const
869 {
870         return v[0][0] + v[1][1] + v[2][2];
871 }
872
873 void float3x3::Orthonormalize(int c0, int c1, int c2)
874 {
875         assume(c0 != c1 && c0 != c2 && c1 != c2);
876         assume(c0 >= 0 && c1 >= 0 && c2 >= 0 && c0 < Cols && c1 < Cols && c2 < Cols);
877 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
878         if (c0 == c1 || c0 == c2 || c1 == c2)
879                 return;
880 #endif
881         ///@todo Optimize away copies.
882         float3 v0 = Col(c0);
883         float3 v1 = Col(c1);
884         float3 v2 = Col(c2);
885         float3::Orthonormalize(v0, v1, v2);
886         SetCol(c0, v0);
887         SetCol(c1, v1);
888         SetCol(c2, v2);
889 }
890
891 void float3x3::RemoveScale()
892 {
893         assume(IsFinite());
894         float x = Row(0).Normalize();
895         float y = Row(1).Normalize();
896         float z = Row(2).Normalize();
897         assume(x != 0 && y != 0 && z != 0 && "float3x3::RemoveScale failed!");
898 }
899
900 float3 float3x3::Transform(const float3 &vector) const
901 {
902         return Transform(vector.x, vector.y, vector.z);
903 }
904
905 float3 float3x3::TransformLeft(const float3 &vector) const
906 {
907         return float3(DOT3STRIDED(vector, ptr(), 3),
908                       DOT3STRIDED(vector, ptr()+1, 3),
909                       DOT3STRIDED(vector, ptr()+2, 3));
910 }
911
912 float3 float3x3::Transform(float x, float y, float z) const
913 {
914         return float3(DOT3_xyz(Row(0), x,y,z),
915                      DOT3_xyz(Row(1), x,y,z),
916                      DOT3_xyz(Row(2), x,y,z));
917 }
918
919 float4 float3x3::Transform(const float4 &vector) const
920 {
921         return float4(DOT3(Row(0), vector),
922                       DOT3(Row(1), vector),
923                       DOT3(Row(2), vector),
924                       vector.w);
925 }
926
927 void float3x3::BatchTransform(float3 *pointArray, int numPoints) const
928 {
929         assume(pointArray || numPoints == 0);
930 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
931         if (!pointArray)
932                 return;
933 #endif
934         for(int i = 0; i < numPoints; ++i)
935                 pointArray[i] = *this * pointArray[i];
936 }
937
938 void float3x3::BatchTransform(float3 *pointArray, int numPoints, int stride) const
939 {
940         assume(pointArray || numPoints == 0);
941 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
942         if (!pointArray)
943                 return;
944 #endif
945         assume(stride >= sizeof(float3));
946         u8 *data = reinterpret_cast<u8*>(pointArray);
947         for(int i = 0; i < numPoints; ++i)
948         {
949                 float3 *v = reinterpret_cast<float3*>(data + stride*i);
950                 *v = *this * *v;
951         }
952 }
953
954 void float3x3::BatchTransform(float4 *vectorArray, int numVectors) const
955 {
956         assume(vectorArray || numVectors == 0);
957 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
958         if (!vectorArray)
959                 return;
960 #endif
961         for(int i = 0; i < numVectors; ++i)
962                 vectorArray[i] = *this * vectorArray[i];
963 }
964
965 void float3x3::BatchTransform(float4 *vectorArray, int numVectors, int stride) const
966 {
967         assume(vectorArray || numVectors == 0);
968 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
969         if (!vectorArray)
970                 return;
971 #endif
972         assume(stride >= sizeof(float4));
973         u8 *data = reinterpret_cast<u8*>(vectorArray);
974         for(int i = 0; i < numVectors; ++i)
975         {
976                 float4 *v = reinterpret_cast<float4*>(data + stride*i);
977                 *v = *this * *v;
978         }
979 }
980
981 float3x3 float3x3::operator *(const float3x3 &rhs) const
982 {
983         assume(IsFinite());
984         assume(rhs.IsFinite());
985         float3x3 r;
986         const float *c0 = rhs.ptr();
987         const float *c1 = rhs.ptr() + 1;
988         const float *c2 = rhs.ptr() + 2;
989         r[0][0] = DOT3STRIDED(v[0], c0, 3);
990         r[0][1] = DOT3STRIDED(v[0], c1, 3);
991         r[0][2] = DOT3STRIDED(v[0], c2, 3);
992
993         r[1][0] = DOT3STRIDED(v[1], c0, 3);
994         r[1][1] = DOT3STRIDED(v[1], c1, 3);
995         r[1][2] = DOT3STRIDED(v[1], c2, 3);
996
997         r[2][0] = DOT3STRIDED(v[2], c0, 3);
998         r[2][1] = DOT3STRIDED(v[2], c1, 3);
999         r[2][2] = DOT3STRIDED(v[2], c2, 3);
1000
1001         return r;
1002 }
1003
1004 float3x3 float3x3::operator *(const Quat &rhs) const
1005 {
1006         return *this * float3x3(rhs);
1007 }
1008
1009 float3 float3x3::operator *(const float3 &rhs) const
1010 {
1011         return float3(DOT3(v[0], rhs),
1012                       DOT3(v[1], rhs),
1013                       DOT3(v[2], rhs));
1014 }
1015
1016 float4 float3x3::operator *(const float4 &rhs) const
1017 {
1018         return float4(DOT3(v[0], rhs),
1019                       DOT3(v[1], rhs),
1020                       DOT3(v[2], rhs),
1021                       rhs.w);
1022 }
1023
1024 float3x3 float3x3::operator *(float scalar) const
1025 {
1026         float3x3 r = *this;
1027         r *= scalar;
1028         return r;
1029 }
1030
1031 float3x3 float3x3::operator /(float scalar) const
1032 {
1033         float3x3 r = *this;
1034         r /= scalar;
1035         return r;
1036 }
1037
1038 float3x3 float3x3::operator +(const float3x3 &rhs) const
1039 {
1040         float3x3 r = *this;
1041         r += rhs;
1042         return r;
1043 }
1044
1045 float3x3 float3x3::operator -(const float3x3 &rhs) const
1046 {
1047         float3x3 r = *this;
1048         r -= rhs;
1049         return r;
1050 }
1051
1052 float3x3 float3x3::operator -() const
1053 {
1054         float3x3 r;
1055         for(int y = 0; y < Rows; ++y)
1056                 for(int x = 0; x < Cols; ++x)
1057                         r[y][x] = -v[y][x];
1058         return r;
1059 }
1060
1061 float3x3 &float3x3::operator *=(float scalar)
1062 {
1063         assume(isfinite(scalar));
1064         for(int y = 0; y < Rows; ++y)
1065                 for(int x = 0; x < Cols; ++x)
1066                         v[y][x] *= scalar;
1067
1068         return *this;
1069 }
1070
1071 float3x3 &float3x3::operator /=(float scalar)
1072 {
1073         assume(isfinite(scalar));
1074         assume(!EqualAbs(scalar, 0));
1075         float invScalar = 1.f / scalar;
1076         for(int y = 0; y < Rows; ++y)
1077                 for(int x = 0; x < Cols; ++x)
1078                         v[y][x] *= invScalar;
1079
1080         return *this;
1081 }
1082
1083 float3x3 &float3x3::operator +=(const float3x3 &rhs)
1084 {
1085         assume(rhs.IsFinite());
1086         for(int y = 0; y < Rows; ++y)
1087                 for(int x = 0; x < Cols; ++x)
1088                         v[y][x] += rhs[y][x];
1089
1090         return *this;
1091 }
1092
1093 float3x3 &float3x3::operator -=(const float3x3 &rhs)
1094 {
1095         assume(rhs.IsFinite());
1096         for(int y = 0; y < Rows; ++y)
1097                 for(int x = 0; x < Cols; ++x)
1098                         v[y][x] -= rhs[y][x];
1099
1100         return *this;
1101 }
1102
1103 bool float3x3::IsFinite() const
1104 {
1105         for(int y = 0; y < Rows; ++y)
1106                 for(int x = 0; x < Cols; ++x)
1107                         if (!isfinite(v[y][x]))
1108                                 return false;
1109         return true;
1110 }
1111
1112 bool float3x3::IsIdentity(float epsilon) const
1113 {
1114         for(int y = 0; y < Rows; ++y)
1115                 for(int x = 0; x < Cols; ++x)
1116                         if (!EqualAbs(v[y][x], (x == y) ? 1.f : 0.f, epsilon))
1117                                 return false;
1118
1119         return true;
1120 }
1121
1122 bool float3x3::IsLowerTriangular(float epsilon) const
1123 {
1124         return EqualAbs(v[0][1], 0.f, epsilon)
1125             && EqualAbs(v[0][2], 0.f, epsilon)
1126             && EqualAbs(v[0][3], 0.f, epsilon)
1127             && EqualAbs(v[1][2], 0.f, epsilon)
1128             && EqualAbs(v[1][3], 0.f, epsilon)
1129             && EqualAbs(v[2][3], 0.f, epsilon);
1130 }
1131
1132 bool float3x3::IsUpperTriangular(float epsilon) const
1133 {
1134         return EqualAbs(v[1][0], 0.f, epsilon)
1135             && EqualAbs(v[2][0], 0.f, epsilon)
1136             && EqualAbs(v[3][0], 0.f, epsilon)
1137             && EqualAbs(v[2][1], 0.f, epsilon)
1138             && EqualAbs(v[3][1], 0.f, epsilon)
1139             && EqualAbs(v[3][2], 0.f, epsilon);
1140 }
1141
1142 bool float3x3::IsInvertible(float epsilon) const
1143 {
1144         ///@todo Optimize.
1145         float3x3 copy = *this;
1146         return copy.Inverse();
1147 }
1148
1149 bool float3x3::IsSymmetric(float epsilon) const
1150 {
1151         for(int y = 0; y < Rows; ++y)
1152                 for(int x = y+1; x < Cols; ++x)
1153                         if (!EqualAbs(v[y][x], v[x][y], epsilon))
1154                                 return false;
1155         return true;
1156 }
1157
1158 bool float3x3::IsSkewSymmetric(float epsilon) const
1159 {
1160         for(int y = 0; y < Rows; ++y)
1161                 for(int x = y; x < Cols; ++x)
1162                         if (!EqualAbs(v[y][x], -v[x][y], epsilon))
1163                                 return false;
1164         return true;
1165 }
1166
1167 bool float3x3::HasUnitaryScale(float epsilon) const
1168 {
1169         float3 scale = ExtractScale();
1170         return scale.Equals(1.f, 1.f, 1.f, epsilon);
1171 }
1172
1173 bool float3x3::HasNegativeScale() const
1174 {
1175         return Determinant() < 0.f;
1176 }
1177
1178 bool float3x3::HasUniformScale(float epsilon) const
1179 {
1180         float3 scale = ExtractScale();
1181         return EqualAbs(scale.x, scale.y, epsilon) && EqualAbs(scale.x, scale.z, epsilon);
1182 }
1183
1184 bool float3x3::IsRowOrthogonal(float epsilon) const
1185 {
1186         return Row(0).IsPerpendicular(Row(1), epsilon)
1187             && Row(0).IsPerpendicular(Row(2), epsilon)
1188             && Row(1).IsPerpendicular(Row(2), epsilon);
1189 }
1190
1191 bool float3x3::IsColOrthogonal(float epsilon) const
1192 {
1193         return Col(0).IsPerpendicular(Col(1), epsilon)
1194             && Col(0).IsPerpendicular(Col(2), epsilon)
1195             && Col(1).IsPerpendicular(Col(2), epsilon);
1196 }
1197
1198 bool float3x3::IsOrthonormal(float epsilon) const
1199 {
1200         ///@todo Epsilon magnitudes don't match.
1201         return IsColOrthogonal(epsilon) && Row(0).IsNormalized(epsilon) && Row(1).IsNormalized(epsilon) && Row(2).IsNormalized(epsilon);
1202 }
1203
1204 bool float3x3::Equals(const float3x3 &other, float epsilon) const
1205 {
1206         for(int y = 0; y < Rows; ++y)
1207                 for(int x = 0; x < Cols; ++x)
1208                         if (!EqualAbs(v[y][x], other[y][x], epsilon))
1209                                 return false;
1210         return true;
1211 }
1212
1213 #ifdef MATH_ENABLE_STL_SUPPORT
1214 std::string float3x3::ToString() const
1215 {
1216         char str[256];
1217         sprintf(str, "(%.2f, %.2f, %.2f) (%.2f, %.2f, %.2f) (%.2f, %.2f, %.2f)"
1218                 v[0][0], v[0][1], v[0][2],
1219                 v[1][0], v[1][1], v[1][2],
1220                 v[2][0], v[2][1], v[2][2]);
1221
1222         return std::string(str);
1223 }
1224
1225 std::string float3x3::ToString2() const
1226 {
1227         char str[256];
1228         sprintf(str, "float3x3(X:(%.2f,%.2f,%.2f) Y:(%.2f,%.2f,%.2f) Z:(%.2f,%.2f,%.2f)"
1229                 v[0][0], v[1][0], v[2][0],
1230                 v[0][1], v[1][1], v[2][1],
1231                 v[0][2], v[1][2], v[2][2]);
1232
1233         return std::string(str);
1234 }
1235 #endif
1236
1237 float3 float3x3::ToEulerXYX() const float3 f; ExtractEulerXYX(*this, f[0], f[1], f[2]); return f; }
1238 float3 float3x3::ToEulerXZX() const float3 f; ExtractEulerXZX(*this, f[0], f[1], f[2]); return f; }
1239 float3 float3x3::ToEulerYXY() const float3 f; ExtractEulerYXY(*this, f[0], f[1], f[2]); return f; }
1240 float3 float3x3::ToEulerYZY() const float3 f; ExtractEulerYZY(*this, f[0], f[1], f[2]); return f; }
1241 float3 float3x3::ToEulerZXZ() const float3 f; ExtractEulerZXZ(*this, f[0], f[1], f[2]); return f; }
1242 float3 float3x3::ToEulerZYZ() const float3 f; ExtractEulerZYZ(*this, f[0], f[1], f[2]); return f; }
1243 float3 float3x3::ToEulerXYZ() const float3 f; ExtractEulerXYZ(*this, f[0], f[1], f[2]); return f; }
1244 float3 float3x3::ToEulerXZY() const float3 f; ExtractEulerXZY(*this, f[0], f[1], f[2]); return f; }
1245 float3 float3x3::ToEulerYXZ() const float3 f; ExtractEulerYXZ(*this, f[0], f[1], f[2]); return f; }
1246 float3 float3x3::ToEulerYZX() const float3 f; ExtractEulerYZX(*this, f[0], f[1], f[2]); return f; }
1247 float3 float3x3::ToEulerZXY() const float3 f; ExtractEulerZXY(*this, f[0], f[1], f[2]); return f; }
1248 float3 float3x3::ToEulerZYX() const float3 f; ExtractEulerZYX(*this, f[0], f[1], f[2]); return f; }
1249
1250 float3 float3x3::ExtractScale() const
1251 {
1252         return float3(Col(0).Length(), Col(1).Length(), Col(2).Length());
1253 }
1254
1255 void float3x3::Decompose(Quat &rotate, float3 &scale) const
1256 {
1257         assume(this->IsColOrthogonal());
1258
1259         float3x3 r;
1260         Decompose(r, scale);
1261         rotate = Quat(r);
1262
1263         // Test that composing back yields the original float3x3.
1264         assume(float3x3::FromRS(rotate, scale).Equals(*this, 0.1f));
1265 }
1266
1267 void float3x3::Decompose(float3x3 &rotate, float3 &scale) const
1268 {
1269         assume(this->IsColOrthogonal());
1270
1271         rotate = *this;
1272         scale.x = rotate.Col(0).Length();
1273         scale.y = rotate.Col(1).Length();
1274         scale.z = rotate.Col(2).Length();
1275         assume(!EqualAbs(scale.x, 0));
1276         assume(!EqualAbs(scale.y, 0));
1277         assume(!EqualAbs(scale.z, 0));
1278         rotate.ScaleCol(0, 1.f / scale.x);
1279         rotate.ScaleCol(1, 1.f / scale.y);
1280         rotate.ScaleCol(2, 1.f / scale.z);
1281
1282         // Test that composing back yields the original float3x3.
1283         assume(float3x3::FromRS(rotate, scale).Equals(*this, 0.1f));
1284 }
1285
1286 #ifdef MATH_ENABLE_STL_SUPPORT
1287 std::ostream &operator <<(std::ostream &out, const float3x3 &rhs)
1288 {
1289         out << rhs.ToString();
1290         return out;
1291 }
1292 #endif
1293
1294 float3x3 float3x3::Mul(const float3x3 &rhs) const return *this * rhs; }
1295 float3x4 float3x3::Mul(const float3x4 &rhs) const return *this * rhs; }
1296 float4x4 float3x3::Mul(const float4x4 &rhs) const return *this * rhs; }
1297 float3x3 float3x3::Mul(const Quat &rhs) const return *this * rhs; }
1298 float3 float3x3::Mul(const float3 &rhs) const return *this * rhs; }
1299
1300 float3x3 operator *(const Quat &lhs, const float3x3 &rhs)
1301 {
1302         float3x3 lhsRot(lhs);
1303         return lhsRot * rhs;
1304 }
1305
1306 float3 operator *(const float3 &lhs, const float3x3 &rhs)
1307 {
1308         return rhs.TransformLeft(lhs);
1309 }
1310
1311 float4 operator *(const float4 &lhs, const float3x3 &rhs)
1312 {
1313         assume(lhs.IsWZeroOrOne());
1314         return float4(rhs.TransformLeft(lhs.xyz()), lhs.w);
1315 }
1316
1317 const float3x3 float3x3::zero    = float3x3(0,0,0, 0,0,0, 0,0,0);
1318 const float3x3 float3x3::identity = float3x3(1,0,0, 0,1,0, 0,0,1);
1319 const float3x3 float3x3::nan = float3x3(FLOAT_NANFLOAT_NANFLOAT_NANFLOAT_NANFLOAT_NANFLOAT_NANFLOAT_NANFLOAT_NANFLOAT_NAN);
1320
1321 MATH_END_NAMESPACE

Go back to previous page