float3x3.cpp

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