float3x4.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 float3x4.cpp
16         @author Jukka Jyl�nki
17         @brief */
18 #include "float3x4.h"
19 #include <string.h>
20
21 #include "MathFunc.h"
22 #include "float3.h"
23 #include "float4.h"
24 #include "float3x3.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 #include "SSEMath.h"
32 #include "float4x4_sse.h"
33 #include "simd.h"
34
35 #ifdef MATH_ENABLE_STL_SUPPORT
36 #include <iostream>
37 #endif
38
39 MATH_BEGIN_NAMESPACE
40
41 float3x4::float3x4(float _00, float _01, float _02, float _03,
42                  float _10, float _11, float _12, float _13,
43                  float _20, float _21, float _22, float _23)
44 {
45         Set(_00, _01, _02, _03,
46                 _10, _11, _12, _13,
47                 _20, _21, _22, _23);
48 }
49
50 float3x4::float3x4(const float3x3 &other)
51 {
52         SetRotatePart(other);
53         SetTranslatePart(0, 0, 0);
54 }
55
56 float3x4::float3x4(const float3x3 &other, const float3 &translate)
57 {
58         SetRotatePart(other);
59         SetTranslatePart(translate);
60 }
61
62 float3x4::float3x4(const float3 &col0, const float3 &col1, const float3 &col2, const float3 &col3)
63 {
64         SetCol(0, col0);
65         SetCol(1, col1);
66         SetCol(2, col2);
67         SetCol(3, col3);
68 }
69
70 float3x4::float3x4(const Quat &orientation)
71 {
72         SetRotatePart(orientation);
73         SetTranslatePart(0, 0, 0);
74 }
75
76 float3x4::float3x4(const Quat &orientation, const float3 &translation)
77 {
78         SetRotatePart(orientation);
79         SetTranslatePart(translation);
80 }
81
82 TranslateOp float3x4::Translate(float tx, float ty, float tz)
83 {
84         return TranslateOp(tx, ty, tz);
85 }
86
87 TranslateOp float3x4::Translate(const float3 &offset)
88 {
89         return TranslateOp(offset);
90 }
91
92 TranslateOp float3x4::Translate(const float4 &offset)
93 {
94         return TranslateOp(offset.xyz());
95 }
96
97 float3x4 float3x4::RotateX(float angle)
98 {
99         float3x4 r;
100         r.SetRotatePartX(angle);
101         r.SetTranslatePart(0, 0, 0);
102         return r;
103 }
104
105 float3x4 float3x4::RotateX(float angleRadians, const float3 &pointOnAxis)
106 {
107         return float3x4::Translate(pointOnAxis) * RotateX(angleRadians) * float3x4::Translate(-pointOnAxis);
108 }
109
110 float3x4 float3x4::RotateY(float angle)
111 {
112         float3x4 r;
113         r.SetRotatePartY(angle);
114         r.SetTranslatePart(0, 0, 0);
115         return r;
116 }
117
118 float3x4 float3x4::RotateY(float angleRadians, const float3 &pointOnAxis)
119 {
120         return float3x4::Translate(pointOnAxis) * RotateY(angleRadians) * float3x4::Translate(-pointOnAxis);
121 }
122
123 float3x4 float3x4::RotateZ(float angle)
124 {
125         float3x4 r;
126         r.SetRotatePartZ(angle);
127         r.SetTranslatePart(0, 0, 0);
128         return r;
129 }
130
131 float3x4 float3x4::RotateZ(float angleRadians, const float3 &pointOnAxis)
132 {
133         return float3x4::Translate(pointOnAxis) * RotateZ(angleRadians) * float3x4::Translate(-pointOnAxis);
134 }
135
136 float3x4 float3x4::RotateAxisAngle(const float3 &axisDirection, float angleRadians)
137 {
138         float3x4 r;
139         r.SetRotatePart(Quat::RotateAxisAngle(axisDirection, angleRadians));
140         r.SetTranslatePart(0, 0, 0);
141         return r;
142 }
143
144 float3x4 float3x4::RotateAxisAngle(const float3 &axisDirection, float angleRadians, const float3 &pointOnAxis)
145 {
146         return float3x4::Translate(pointOnAxis) * RotateAxisAngle(axisDirection, angleRadians) * float3x4::Translate(-pointOnAxis);
147 }
148
149 float3x4 float3x4::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
150 {
151         assume(sourceDirection.IsNormalized());
152         assume(targetDirection.IsNormalized());
153         float3x4 r;
154         r.SetRotatePart(Quat::RotateFromTo(sourceDirection, targetDirection));
155         r.SetTranslatePart(0, 0, 0);
156         return r;
157 }
158
159 float3x4 float3x4::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection, const float3 &centerPoint)
160 {
161         return float3x4::Translate(centerPoint) * RotateFromTo(sourceDirection, targetDirection) * float3x4::Translate(-centerPoint);
162 }
163
164 float3x4 float3x4::RandomGeneral(LCG &lcg, float minElem, float maxElem)
165 {
166         float3x4 m;
167         for(int y = 0; y < 3; ++y)
168                 for(int x = 0; x < 4; ++x)
169                         m[y][x] = lcg.Float(minElem, maxElem);
170         return m;
171 }
172
173 float3x4 float3x4::RandomRotation(LCG &lcg)
174 {
175         return float3x4(float3x3::RandomRotation(lcg));
176 }
177
178 float3x4 float3x4::FromQuat(const Quat &orientation)
179 {
180         float3x4 r;
181         r.SetRotatePart(orientation);
182         r.SetTranslatePart(0, 0, 0);
183         return r;
184 }
185
186 float3x4 float3x4::FromQuat(const Quat &orientation, const float3 &pointOnAxis)
187 {
188         return float3x4::Translate(pointOnAxis) * float3x4::FromQuat(orientation) * float3x4::Translate(-pointOnAxis);
189 }
190
191 float3x4 float3x4::FromTRS(const float3 &translate, const Quat &rotate, const float3 &scale)
192 {
193         return float3x4::Translate(translate) * float3x4(rotate) * float3x4::Scale(scale);
194 }
195
196 float3x4 float3x4::FromTRS(const float3 &translate, const float3x3 &rotate, const float3 &scale)
197 {
198         return float3x4::Translate(translate) * float3x4(rotate) * float3x4::Scale(scale);
199 }
200
201 float3x4 float3x4::FromTRS(const float3 &translate, const float3x4 &rotate, const float3 &scale)
202 {
203         return float3x4::Translate(translate) * float3x4(rotate) * float3x4::Scale(scale);
204 }
205
206 float3x4 float3x4::FromEulerXYX(float x2, float y, float x)
207 {
208         float3x4 r;
209         r.SetTranslatePart(0,0,0);
210         Set3x3PartRotateEulerXYX(r, x2, y, x);
211         assume(r.Equals(float3x4::RotateX(x2) * float3x4::RotateY(y) * float3x4::RotateX(x)));
212         return r;
213 }
214
215 float3x4 float3x4::FromEulerXZX(float x2, float z, float x)
216 {
217         float3x4 r;
218         r.SetTranslatePart(0,0,0);
219         Set3x3PartRotateEulerXZX(r, x2, z, x);
220         assume(r.Equals(float3x4::RotateX(x2) * float3x4::RotateZ(z) * float3x4::RotateX(x)));
221         return r;
222 }
223
224 float3x4 float3x4::FromEulerYXY(float y2, float x, float y)
225 {
226         float3x4 r;
227         r.SetTranslatePart(0,0,0);
228         Set3x3PartRotateEulerYXY(r, y2, x, y);
229         assume(r.Equals(float3x4::RotateY(y2) * float3x4::RotateX(x) * float3x4::RotateY(y)));
230         return r;
231 }
232
233 float3x4 float3x4::FromEulerYZY(float y2, float z, float y)
234 {
235         float3x4 r;
236         r.SetTranslatePart(0,0,0);
237         Set3x3PartRotateEulerYZY(r, y2, z, y);
238         assume(r.Equals(float3x4::RotateY(y2) * float3x4::RotateZ(z) * float3x4::RotateY(y)));
239         return r;
240 }
241
242 float3x4 float3x4::FromEulerZXZ(float z2, float x, float z)
243 {
244         float3x4 r;
245         r.SetTranslatePart(0,0,0);
246         Set3x3PartRotateEulerZXZ(r, z2, x, z);
247         assume(r.Equals(float3x4::RotateZ(z2) * float3x4::RotateX(x) * float3x4::RotateZ(z)));
248         return r;
249 }
250
251 float3x4 float3x4::FromEulerZYZ(float z2, float y, float z)
252 {
253         float3x4 r;
254         r.SetTranslatePart(0,0,0);
255         Set3x3PartRotateEulerZYZ(r, z2, y, z);
256         assume(r.Equals(float3x4::RotateZ(z2) * float3x4::RotateY(y) * float3x4::RotateZ(z)));
257         return r;
258 }
259
260 float3x4 float3x4::FromEulerXYZ(float x, float y, float z)
261 {
262         float3x4 r;
263         r.SetTranslatePart(0,0,0);
264         Set3x3PartRotateEulerXYZ(r, x, y, z);
265         assume(r.Equals(float3x4::RotateX(x) * float3x4::RotateY(y) * float3x4::RotateZ(z)));
266         return r;
267 }
268
269 float3x4 float3x4::FromEulerXZY(float x, float z, float y)
270 {
271         float3x4 r;
272         r.SetTranslatePart(0,0,0);
273         Set3x3PartRotateEulerXZY(r, x, z, y);
274         assume(r.Equals(float3x4::RotateX(x) * float3x4::RotateZ(z) * float3x4::RotateY(y)));
275         return r;
276 }
277
278 float3x4 float3x4::FromEulerYXZ(float y, float x, float z)
279 {
280         float3x4 r;
281         r.SetTranslatePart(0,0,0);
282         Set3x3PartRotateEulerYXZ(r, y, x, z);
283         assume(r.Equals(float3x4::RotateY(y) * float3x4::RotateX(x) * float3x4::RotateZ(z)));
284         return r;
285 }
286
287 float3x4 float3x4::FromEulerYZX(float y, float z, float x)
288 {
289         float3x4 r;
290         r.SetTranslatePart(0,0,0);
291         Set3x3PartRotateEulerYZX(r, y, z, x);
292         assume(r.Equals(float3x4::RotateY(y) * float3x4::RotateZ(z) * float3x4::RotateX(x)));
293         return r;
294 }
295
296 float3x4 float3x4::FromEulerZXY(float z, float x, float y)
297 {
298         float3x4 r;
299         r.SetTranslatePart(0,0,0);
300         Set3x3PartRotateEulerZXY(r, z, x, y);
301         assume(r.Equals(float3x4::RotateZ(z) * float3x4::RotateX(x) * float3x4::RotateY(y)));
302         return r;
303 }
304
305 float3x4 float3x4::FromEulerZYX(float z, float y, float x)
306 {
307         float3x4 r;
308         r.SetTranslatePart(0,0,0);
309         Set3x3PartRotateEulerZYX(r, z, y, x);
310         assume(r.Equals(float3x4::RotateZ(z) * float3x4::RotateY(y) * float3x4::RotateX(x)));
311         return r;
312 }
313
314 ScaleOp float3x4::Scale(float sx, float sy, float sz)
315 {
316         return ScaleOp(sx, sy, sz);
317 }
318
319 ScaleOp float3x4::Scale(const float3 &scale)
320 {
321         return ScaleOp(scale);
322 }
323
324 ScaleOp float3x4::Scale(const float4 &scale)
325 {
326         return ScaleOp(scale);
327 }
328
329 float3x4 float3x4::Scale(const float3 &scale, const float3 &scaleCenter)
330 {
331         return float3x4::Translate(scaleCenter) * float3x4::Scale(scale) * float3x4::Translate(-scaleCenter);
332 }
333
334 float3x4 float3x4::Scale(const float4 &scale, const float4 &scaleCenter)
335 {
336         return float3x4::Translate(scaleCenter) * float3x4::Scale(scale) * float3x4::Translate(-scaleCenter);
337 }
338
339 float3x4 float3x4::ScaleAlongAxis(const float3 &axis, float scalingFactor)
340 {
341         return Scale(axis * scalingFactor);
342 }
343
344 float3x4 float3x4::ScaleAlongAxis(const float3 &axis, float scalingFactor, const float3 &scaleCenter)
345 {
346         return float3x4::Translate(scaleCenter) * float3x4::Scale(axis * scalingFactor) * float3x4::Translate(-scaleCenter);
347 }
348
349 ScaleOp float3x4::UniformScale(float uniformScale)
350 {
351         return ScaleOp(uniformScale, uniformScale, uniformScale);
352 }
353
354 float3x4 float3x4::UniformScale(float uniformScale, const float3 &scaleCenter)
355 {
356         return float3x4::Translate(scaleCenter) * float3x4::UniformScale(uniformScale) * float3x4::Translate(-scaleCenter);
357 }
358
359 float3 float3x4::GetScale() const
360 {
361         return float3(Col(0).Length(), Col(1).Length(), Col(2).Length());
362 }
363
364 float3x4 float3x4::ShearX(float yFactor, float zFactor)
365 {
366         return float3x4(1.f, yFactor, zFactor, 0.f,
367                                         0.f, 1.f, 0.f, 0.f,
368                                         0.f, 0.f, 1.f, 0.f);
369 }
370
371 float3x4 float3x4::ShearY(float xFactor, float zFactor)
372 {
373         return float3x4(1.f, 0.f, 0.f,  0.f,
374                                         xFactor, 1.f, zFactor, 0.f,
375                                         0.f, 0.f, 1.f, 0.f);
376 }
377
378 float3x4 float3x4::ShearZ(float xFactor, float yFactor)
379 {
380         return float3x4(1.f, 0.f, 0.f, 0.f,
381                                         0.f, 1.f, 0.f, 0.f,
382                                         xFactor, yFactor, 1.f, 0.f);
383 }
384
385 float3x4 float3x4::Mirror(const Plane &p)
386 {
387         float3x4 v;
388         SetMatrix3x4AffinePlaneMirror(v, p.normal.x, p.normal.y, p.normal.z, p.d);
389         return v;
390 }
391
392 float3x4 float3x4::OrthographicProjection(const Plane &p)
393 {
394         float3x4 v;
395         SetMatrix3x4AffinePlaneProject(v, p.normal.x, p.normal.y, p.normal.z, p.d);
396         return v;
397 }
398
399 float3x4 float3x4::OrthographicProjectionYZ()
400 {
401         float3x4 v = identity;
402         v[0][0] = 0.f;
403         return v;
404 }
405
406 float3x4 float3x4::OrthographicProjectionXZ()
407 {
408         float3x4 v = identity;
409         v[1][1] = 0.f;
410         return v;
411 }
412
413 float3x4 float3x4::OrthographicProjectionXY()
414 {
415         float3x4 v = identity;
416         v[2][2] = 0.f;
417         return v;
418 }
419
420 MatrixProxy<float3x4::Cols> &float3x4::operator[](int row)
421 {
422         assume(row >= 0);
423         assume(row < Rows);
424 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
425         if (row < 0 || row >= Rows)
426                 row = 0; // Benign failure, just give the first row.
427 #endif
428
429         return *(reinterpret_cast<MatrixProxy<Cols>*>(v[row]));
430 }
431
432 const MatrixProxy<float3x4::Cols> &float3x4::operator[](int row) const
433 {
434         assume(row >= 0);
435         assume(row < Rows);
436 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
437         if (row < 0 || row >= Rows)
438                 row = 0; // Benign failure, just give the first row.
439 #endif
440
441         return *(reinterpret_cast<const MatrixProxy<Cols>*>(v[row]));
442 }
443
444 float &float3x4::At(int row, int col)
445 {
446         assume(row >= 0);
447         assume(row < Rows);
448         assume(col >= 0);
449         assume(col < Cols);
450 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
451         if (row < 0 || row >= Rows || col < 0 || col >= Cols)
452                 return v[0][0]; // Benign failure, return the first element.
453 #endif
454         return v[row][col];
455 }
456
457 CONST_WIN32 float float3x4::At(int row, int col) const
458 {
459         assume(row >= 0);
460         assume(row < Rows);
461         assume(col >= 0);
462         assume(col < Cols);
463 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
464         if (row < 0 || row >= Rows || col < 0 || col >= Cols)
465                 return FLOAT_NAN;
466 #endif
467         return v[row][col];
468 }
469
470 float4 &float3x4::Row(int row)
471 {
472         assume(row >= 0);
473         assume(row < Rows);
474
475 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
476         if (row < 0 || row >= Rows)
477                 row = 0; // Benign failure, just give the first row.
478 #endif
479         return reinterpret_cast<float4 &>(v[row]);
480 }
481
482 const float4 &float3x4::Row(int row) const
483 {
484         assume(row >= 0);
485         assume(row < Rows);
486
487 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
488         if (row < 0 || row >= Rows)
489                 row = 0; // Benign failure, just give the first row.
490 #endif
491         return reinterpret_cast<const float4 &>(v[row]);
492 }
493
494 float3 &float3x4::Row3(int row)
495 {
496         assume(row >= 0);
497         assume(row < Rows);
498
499 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
500         if (row < 0 || row >= Rows)
501                 row = 0; // Benign failure, just give the first row.
502 #endif
503         return reinterpret_cast<float3 &>(v[row]);
504 }
505
506 const float3 &float3x4::Row3(int row) const
507 {
508         assume(row >= 0);
509         assume(row < Rows);
510
511 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
512         if (row < 0 || row >= Rows)
513                 row = 0; // Benign failure, just give the first row.
514 #endif
515         return reinterpret_cast<const float3 &>(v[row]);
516 }
517
518 CONST_WIN32 float3 float3x4::Col(int col) const
519 {
520         assume(col >= 0);
521         assume(col < Cols);
522 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
523         if (col < 0 || col >= Cols)
524                 return float3::nan;
525 #endif
526         return float3(v[0][col], v[1][col], v[2][col]);
527 }
528
529 CONST_WIN32 float3 float3x4::Diagonal() const
530 {
531         return float3(v[0][0], v[1][1], v[2][2]);
532 }
533
534 void float3x4::ScaleRow3(int row, float scalar)
535 {
536         Row3(row) *= scalar;
537 }
538
539 void float3x4::ScaleRow(int r, float scalar)
540 {
541 #ifdef MATH_SIMD
542         row[r] = mul_ps(row[r], set1_ps(scalar));
543 #else
544         Row(r) *= scalar;
545 #endif
546 }
547
548 void float3x4::ScaleCol(int col, float scalar)
549 {
550         assume(col >= 0);
551         assume(col < Cols);
552         assume(MATH_NS::IsFinite(scalar));
553 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
554         if (col < 0 || col >= Cols)
555                 return; // Benign failure
556 #endif
557
558         v[0][col] *= scalar;
559         v[1][col] *= scalar;
560         v[2][col] *= scalar;
561 }
562
563 CONST_WIN32 float3x3 float3x4::Float3x3Part() const
564 {
565         return float3x3(v[0][0], v[0][1], v[0][2],
566                                         v[1][0], v[1][1], v[1][2],
567                                         v[2][0], v[2][1], v[2][2]);
568 }
569
570 CONST_WIN32 float3 float3x4::TranslatePart() const
571 {
572         return Col(3);
573 }
574
575 CONST_WIN32 float3x3 float3x4::RotatePart() const
576 {
577         return Float3x3Part();
578 }
579
580 float3 float3x4::WorldX() const
581 {
582         return Col(0);
583 }
584
585 float3 float3x4::WorldY() const
586 {
587         return Col(1);
588 }
589
590 float3 float3x4::WorldZ() const
591 {
592         return Col(2);
593 }
594
595 void float3x4::SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3)
596 {
597         assume(row >= 0);
598         assume(row < Rows);
599         assume(MATH_NS::IsFinite(m_r0));
600         assume(MATH_NS::IsFinite(m_r1));
601         assume(MATH_NS::IsFinite(m_r2));
602         assume(MATH_NS::IsFinite(m_r3));
603 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
604         if (row < 0 || row >= Rows)
605                 return; // Benign failure
606 #endif
607
608 #ifdef MATH_SIMD
609         this->row[row] = set_ps(m_r3, m_r2, m_r1, m_r0);
610 #else
611         v[row][0] = m_r0;
612         v[row][1] = m_r1;
613         v[row][2] = m_r2;
614         v[row][3] = m_r3;
615 #endif
616 }
617
618 void float3x4::SetRow(int row, const float3 &rowVector, float w)
619 {
620         SetRow(row, rowVector.x, rowVector.y, rowVector.z, w);
621 }
622
623 void float3x4::SetRow(int row, const float4 &rowVector)
624 {
625 #ifdef MATH_SIMD
626
627 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
628         if (row < 0 || row >= Rows)
629                 return; // Benign failure
630 #endif
631         this->row[row] = rowVector.v;
632 #else
633         SetRow(row, rowVector.x, rowVector.y, rowVector.z, rowVector.w);
634 #endif
635 }
636
637 void float3x4::SetRow(int row, const float *data)
638 {
639         assume(data);
640 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
641         if (!data)
642                 return;
643 #endif
644
645 #ifdef MATH_SIMD
646
647 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
648         if (row < 0 || row >= Rows)
649                 return; // Benign failure
650 #endif
651         this->row[row] = loadu_ps(data); // Assume unaligned load, since we don't know if data is 16-byte-aligned.
652 #else
653         SetRow(row, data[0], data[1], data[2], data[3]);
654 #endif
655 }
656
657 void float3x4::SetCol(int column, float m_0c, float m_1c, float m_2c)
658 {
659         assume(column >= 0);
660         assume(column < Cols);
661         assume(MATH_NS::IsFinite(m_0c));
662         assume(MATH_NS::IsFinite(m_1c));
663         assume(MATH_NS::IsFinite(m_2c));
664 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
665         if (column < 0 || column >= Cols)
666                 return; // Benign failure
667 #endif
668         v[0][column] = m_0c;
669         v[1][column] = m_1c;
670         v[2][column] = m_2c;
671 }
672
673 void float3x4::SetCol(int column, const float3 &columnVector)
674 {
675         SetCol(column, columnVector.x, columnVector.y, columnVector.z);
676 }
677
678 void float3x4::SetCol(int column, const float *data)
679 {
680         assume(data);
681 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
682         if (!data)
683                 return;
684 #endif
685         SetCol(column, data[0], data[1], data[2]);
686 }
687
688 void float3x4::Set(float _00, float _01, float _02, float _03,
689                                    float _10, float _11, float _12, float _13,
690                                    float _20, float _21, float _22, float _23)
691 {
692 #ifdef MATH_SIMD
693         row[0] = set_ps(_03, _02, _01, _00);
694         row[1] = set_ps(_13, _12, _11, _10);
695         row[2] = set_ps(_23, _22, _21, _20);
696 #else
697         v[0][0] = _00; v[0][1] = _01; v[0][2] = _02; v[0][3] = _03;
698         v[1][0] = _10; v[1][1] = _11; v[1][2] = _12; v[1][3] = _13;
699         v[2][0] = _20; v[2][1] = _21; v[2][2] = _22; v[2][3] = _23;
700 #endif
701 }
702
703 void float3x4::Set(const float3x4 &rhs)
704 {
705 #ifdef MATH_SIMD
706         row[0] = rhs.row[0];
707         row[1] = rhs.row[1];
708         row[2] = rhs.row[2];
709 #else
710         Set(rhs.ptr());
711 #endif
712 }
713
714 void float3x4::Set(const float *values)
715 {
716         assume(values);
717 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
718         if (!values)
719                 return;
720 #endif
721         v[0][0] = values[0];
722         v[0][1] = values[1];
723         v[0][2] = values[2];
724         v[0][3] = values[3];
725
726         v[1][0] = values[4];
727         v[1][1] = values[5];
728         v[1][2] = values[6];
729         v[1][3] = values[7];
730
731         v[2][0] = values[8];
732         v[2][1] = values[9];
733         v[2][2] = values[10];
734         v[2][3] = values[11];
735 }
736
737 void float3x4::Set(int row, int col, float value)
738 {
739         assume(row >= 0);
740         assume(row < Rows);
741         assume(col >= 0);
742         assume(col < Cols);
743 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
744         if (row < 0 || row >= Rows || col < 0 || col >= Cols)
745                 return; // Benign failure
746 #endif
747         v[row][col] = value;
748 }
749
750 void float3x4::SetIdentity()
751 {
752         Set(1,0,0,0,
753                 0,1,0,0,
754                 0,0,1,0);
755 }
756
757 void float3x4::Set3x3Part(const float3x3 &r)
758 {
759         assume(r.IsFinite());
760         v[0][0] = r[0][0]; v[0][1] = r[0][1]; v[0][2] = r[0][2];
761         v[1][0] = r[1][0]; v[1][1] = r[1][1]; v[1][2] = r[1][2];
762         v[2][0] = r[2][0]; v[2][1] = r[2][1]; v[2][2] = r[2][2];
763 }
764
765 void float3x4::SwapColumns(int col1, int col2)
766 {
767         assume(col1 >= 0);
768         assume(col1 < Cols);
769         assume(col2 >= 0);
770         assume(col2 < Cols);
771 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
772         if (col1 < 0 || col1 >= Cols || col2 < 0 || col2 >= Cols)
773                 return; // Benign failure
774 #endif
775         Swap(v[0][col1], v[0][col2]);
776         Swap(v[1][col1], v[1][col2]);
777         Swap(v[2][col1], v[2][col2]);
778 }
779
780 void float3x4::SwapRows(int row1, int row2)
781 {
782         assume(row1 >= 0);
783         assume(row1 < Rows);
784         assume(row2 >= 0);
785         assume(row2 < Rows);
786 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
787         if (row1 < 0 || row1 >= Rows || row2 < 0 || row2 >= Rows)
788                 return; // Benign failure
789 #endif
790
791 #ifdef MATH_SIMD
792         Swap(row[row1], row[row2]);
793 #else
794         Swap(v[row1][0], v[row2][0]);
795         Swap(v[row1][1], v[row2][1]);
796         Swap(v[row1][2], v[row2][2]);
797         Swap(v[row1][3], v[row2][3]);
798 #endif
799 }
800
801 void float3x4::SetRotatePartX(float angle)
802 {
803         Set3x3PartRotateX(*this, angle);
804 }
805
806 void float3x4::SetRotatePartY(float angle)
807 {
808         Set3x3PartRotateY(*this, angle);
809 }
810
811 void float3x4::SetRotatePartZ(float angle)
812 {
813         Set3x3PartRotateZ(*this, angle);
814 }
815
816 void float3x4::SetRotatePart(const float3 &axisDirection, float angle)
817 {
818         SetRotationAxis3x3(*this, axisDirection, angle);
819 }
820
821 void float3x4::SetRotatePart(const Quat &q)
822 {
823         SetMatrixRotatePart(*this, q);
824 }
825
826 float3x4 float3x4::LookAt(const float3 &localForwardDir, const float3 &targetForwardDir, const float3 &localUp, const float3 &worldUp)
827 {
828         float3x4 m;
829         m.SetRotatePart(float3x3::LookAt(localForwardDir, targetForwardDir, localUp, worldUp));
830         m.SetTranslatePart(0,0,0);
831         return m;
832 }
833
834 float3x4 float3x4::LookAt(const float3 &eyePos, const float3 &targetPos, const float3 &localForward,
835                           const float3 &localUp, const float3 &worldUp)
836 {
837         float3x4 m;
838         m.SetRotatePart(float3x3::LookAt(localForward, (targetPos-eyePos).Normalized(), localUp, worldUp));
839         m.SetTranslatePart(eyePos);
840         return m;
841 }
842
843 float3x4 &float3x4::operator =(const float3x3 &rhs)
844 {
845         SetRotatePart(rhs);
846         SetTranslatePart(0,0,0);
847         return *this;
848 }
849
850 float3x4 &float3x4::operator =(const float3x4 &rhs)
851 {
852         // We deliberately don't want to assume rhs is finite, it is ok
853         // to copy around uninitialized matrices.
854         // But note that when assigning through a conversion above (float3x3 -> float3x4),
855         // we do assume the input matrix is finite.
856 //      assume(rhs.IsFinite());
857 #ifdef MATH_SIMD
858         row[0] = rhs.row[0];
859         row[1] = rhs.row[1];
860         row[2] = rhs.row[2];
861 #else
862         v[0][0] = rhs.v[0][0];
863         v[0][1] = rhs.v[0][1];
864         v[0][2] = rhs.v[0][2];
865         v[0][3] = rhs.v[0][3];
866
867         v[1][0] = rhs.v[1][0];
868         v[1][1] = rhs.v[1][1];
869         v[1][2] = rhs.v[1][2];
870         v[1][3] = rhs.v[1][3];
871
872         v[2][0] = rhs.v[2][0];
873         v[2][1] = rhs.v[2][1];
874         v[2][2] = rhs.v[2][2];
875         v[2][3] = rhs.v[2][3];
876 #endif
877
878         return *this;
879 }
880
881 float3x4 &float3x4::operator =(const Quat &rhs)
882 {
883         *this = rhs.ToFloat3x4();
884         return *this;
885 }
886
887 float float3x4::Determinant() const
888 {
889         assume(Float3x3Part().IsFinite());
890 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
891         return mat3x4_determinant(row);
892 #else
893         const float a = v[0][0];
894         const float b = v[0][1];
895         const float c = v[0][2];
896         const float d = v[1][0];
897         const float e = v[1][1];
898         const float f = v[1][2];
899         const float g = v[2][0];
900         const float h = v[2][1];
901         const float i = v[2][2];
902
903         return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
904 #endif
905 }
906
907 bool float3x4::Inverse(float epsilon)
908 {
909 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
910         MARK_UNUSED(epsilon);
911         float det = mat3x4_inverse(row, row);
912         return MATH_NS::Abs(det) > 1e-5f;
913 #else
914         float4x4 temp(*this); ///@todo It is possible optimize to avoid copying here by writing the inverse function specifically for float3x4.
915         bool success = temp.Inverse(epsilon);
916         *this = temp.Float3x4Part();
917         return success;
918 #endif
919 }
920
921 float3x4 float3x4::Inverted() const
922 {
923 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
924         float3x4 copy;
925         mat4x4_inverse(row, copy.row);
926 #else
927         float3x4 copy = *this;
928         copy.Inverse();
929 #endif
930         return copy;
931 }
932
933 bool float3x4::InverseColOrthogonal()
934 {
935 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
936         mat3x4_inverse_colorthogonal(row, row);
937 #else
938         assume(IsColOrthogonal());
939         float s1 = float3(v[0][0], v[1][0], v[2][0]).LengthSq();
940         float s2 = float3(v[0][1], v[1][1], v[2][1]).LengthSq();
941         float s3 = float3(v[0][2], v[1][2], v[2][2]).LengthSq();
942         if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f)
943                 return false;
944         s1 = 1.f / s1;
945         s2 = 1.f / s2;
946         s3 = 1.f / s3;
947         Swap(v[0][1], v[1][0]);
948         Swap(v[0][2], v[2][0]);
949         Swap(v[1][2], v[2][1]);
950
951         v[0][0] *= s1; v[0][1] *= s1; v[0][2] *= s1;
952         v[1][0] *= s2; v[1][1] *= s2; v[1][2] *= s2;
953         v[2][0] *= s3; v[2][1] *= s3; v[2][2] *= s3;
954
955         SetTranslatePart(TransformDir(-v[0][3], -v[1][3], -v[2][3]));
956         mathassert(IsRowOrthogonal());
957 #endif
958         return true;
959 }
960
961 bool float3x4::InverseOrthogonalUniformScale()
962 {
963 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
964         mat3x4_inverse_orthogonal_uniformscale(row, row);
965         return true; ///\todo The return value is difficult here with SSE, figure out how to treat that.
966 #else
967         assume(IsColOrthogonal(1e-3f));
968         assume(HasUniformScale());
969         Swap(v[0][1], v[1][0]);
970         Swap(v[0][2], v[2][0]);
971         Swap(v[1][2], v[2][1]);
972         float scale = float3(v[0][0], v[1][0], v[2][0]).LengthSq();
973         if (scale == 0.f)
974                 return false;
975         scale = 1.f / scale;
976
977         v[0][0] *= scale; v[0][1] *= scale; v[0][2] *= scale;
978         v[1][0] *= scale; v[1][1] *= scale; v[1][2] *= scale;
979         v[2][0] *= scale; v[2][1] *= scale; v[2][2] *= scale;
980
981         SetTranslatePart(TransformDir(-v[0][3], -v[1][3], -v[2][3]));
982
983         return true;
984 #endif
985 }
986
987 void float3x4::InverseOrthonormal()
988 {
989         assume(IsOrthonormal());
990
991 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
992         mat3x4_inverse_orthonormal(row, row);
993 #else
994         /* In this function, we seek to optimize the matrix inverse in the case this
995            matrix is orthonormal, i.e. it can be written in the following form:
996
997                       [ R | T ]
998                   M = [---+---]
999                       [ 0 | 1 ]
1000
1001            where R is a 3x3 orthonormal (orthogonal vectors, normalized columns) rotation
1002            matrix, and T is a 3x1 vector representing the translation performed by
1003            this matrix.
1004
1005            In this form, the inverse of this matrix is simple to compute and will not
1006            require the calculation of determinants or expensive Gaussian elimination. The
1007            inverse is of form
1008
1009                          [ R^t | R^t(-T) ]
1010                   M^-1 = [-----+---------]
1011                          [  0  |    1    ]
1012
1013            which can be seen by multiplying out M * M^(-1) in block form. Especially the top-
1014            right cell turns out to (remember that R^(-1) == R^t since R is orthonormal)
1015
1016                 R * R^t(-T) + T * 1 == (R * R^t)(-T) + T == -T + T == 0, as expected.
1017
1018            Therefore the inversion requires only two steps: */
1019
1020         // a) Transpose the top-left 3x3 part in-place to produce R^t.
1021         Swap(v[0][1], v[1][0]);
1022         Swap(v[0][2], v[2][0]);
1023         Swap(v[1][2], v[2][1]);
1024
1025         // b) Replace the top-right 3x1 part by computing R^t(-T).
1026         SetTranslatePart(TransformDir(-v[0][3], -v[1][3], -v[2][3]));
1027 #endif
1028 }
1029
1030 void float3x4::Transpose3()
1031 {
1032         ///\todo SSE.
1033         Swap(v[0][1], v[1][0]);
1034         Swap(v[0][2], v[2][0]);
1035         Swap(v[1][2], v[2][1]);
1036 }
1037
1038 float3x4 float3x4::Transposed3() const
1039 {
1040         float3x4 copy = *this;
1041         copy.Transpose3();
1042         return copy;
1043 }
1044
1045 bool float3x4::InverseTranspose()
1046 {
1047         bool success = Inverse();
1048         Transpose3();
1049         // float3x4 cannot represent the translation element as the fourth row after transposing.
1050         // Since inverse transposes are used mainly to transform direction vectors, we can discard the translation component.
1051         SetTranslatePart(0,0,0);
1052         return success;
1053 }
1054
1055 float3x4 float3x4::InverseTransposed() const
1056 {
1057         float3x4 copy = *this;
1058         copy.Transpose3();
1059         copy.Inverse();
1060         // float3x4 cannot represent the translation element as the fourth row after transposing.
1061         // Since inverse transposes are used mainly to transform direction vectors, we can discard the translation component.
1062         copy.SetTranslatePart(0,0,0);
1063         return copy;
1064 }
1065
1066 float float3x4::Trace() const
1067 {
1068         assume(IsFinite());
1069         return v[0][0] + v[1][1] + v[2][2];
1070 }
1071
1072 void float3x4::Orthonormalize(int c0, int c1, int c2)
1073 {
1074         ///\todo SSE.
1075         assume(c0 != c1 && c0 != c2 && c1 != c2);
1076         assume(c0 >= 0 && c1 >= 0 && c2 >= 0 && c0 < Cols && c1 < Cols && c2 < Cols);
1077 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1078         if (c0 == c1 || c0 == c2 || c1 == c2)
1079                 return;
1080 #endif
1081
1082         ///@todo Optimize away copies.
1083         float3 v0 = Col(c0);
1084         float3 v1 = Col(c1);
1085         float3 v2 = Col(c2);
1086         float3::Orthonormalize(v0, v1, v2);
1087         SetCol(c0, v0);
1088         SetCol(c1, v1);
1089         SetCol(c2, v2);
1090 }
1091
1092 void float3x4::RemoveScale()
1093 {
1094         ///\todo SSE.
1095         float x = Row3(0).Normalize();
1096         float y = Row3(1).Normalize();
1097         float z = Row3(2).Normalize();
1098         assume(x != 0 && y != 0 && z != 0 && "float3x4::RemoveScale failed!");
1099         MARK_UNUSED(x);
1100         MARK_UNUSED(y);
1101         MARK_UNUSED(z);
1102 }
1103
1104 float3 float3x4::TransformPos(const float3 &pointVector) const
1105 {
1106 #ifdef MATH_SSE
1107         return mat3x4_mul_vec(row, set_ps(1.f, pointVector.z, pointVector.y, pointVector.x));
1108 #else
1109         return TransformPos(pointVector.x, pointVector.y, pointVector.z);
1110 #endif
1111 }
1112
1113 float3 float3x4::TransformPos(float x, float y, float z) const
1114 {
1115 #ifdef MATH_SSE
1116         return mat3x4_mul_vec(row, set_ps(1, z, y, x));
1117 #else
1118         return float3(DOT3_xyz(v[0], x,y,z) + v[0][3],
1119                                   DOT3_xyz(v[1], x,y,z) + v[1][3],
1120                                   DOT3_xyz(v[2], x,y,z) + v[2][3]);
1121 #endif
1122 }
1123
1124 float3 float3x4::TransformDir(const float3 &directionVector) const
1125 {
1126 #ifdef MATH_SSE
1127         return mat3x4_mul_vec(row, set_ps(0.f, directionVector.z, directionVector.y, directionVector.x));
1128 #else
1129         return TransformDir(directionVector.x, directionVector.y, directionVector.z);
1130 #endif
1131 }
1132
1133 float3 float3x4::TransformDir(float x, float y, float z) const
1134 {
1135 #ifdef MATH_SSE
1136         return mat3x4_mul_vec(row, set_ps(0, z, y, x));
1137 #else
1138         return float3(DOT3_xyz(v[0], x,y,z),
1139                                   DOT3_xyz(v[1], x,y,z),
1140                                   DOT3_xyz(v[2], x,y,z));
1141 #endif
1142 }
1143
1144 float4 float3x4::TransformDir(const float4 &directionVector) const
1145 {
1146         assume(EqualAbs(directionVector.w, 0.f));
1147         return Transform(directionVector);
1148 }
1149
1150 float4 float3x4::Transform(const float4 &vector) const
1151 {
1152 #ifdef MATH_SSE
1153         return float4(mat3x4_mul_sse(row, vector.v));
1154 #else
1155         return float4(DOT4(v[0], vector),
1156                                   DOT4(v[1], vector),
1157                                   DOT4(v[2], vector),
1158                                   vector.w);
1159 #endif
1160 }
1161
1162 void float3x4::BatchTransformPos(float3 *pointArray, int numPoints) const
1163 {
1164         assume(pointArray);
1165 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1166         if (!pointArray)
1167                 return;
1168 #endif
1169         for(int i = 0; i < numPoints; ++i)
1170                 pointArray[i] = MulPos(pointArray[i]);
1171 }
1172
1173 void float3x4::BatchTransformPos(float3 *pointArray, int numPoints, int stride) const
1174 {
1175         assume(pointArray);
1176 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1177         if (!pointArray)
1178                 return;
1179 #endif
1180         assume(stride >= (int)sizeof(float3));
1181         u8 *data = reinterpret_cast<u8*>(pointArray);
1182         for(int i = 0; i < numPoints; ++i)
1183         {
1184                 float3 *v = reinterpret_cast<float3*>(data + stride*i);
1185                 *v = MulPos(*v);
1186         }
1187 }
1188
1189 void float3x4::BatchTransformDir(float3 *dirArray, int numVectors) const
1190 {
1191         assume(dirArray);
1192 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1193         if (!dirArray)
1194                 return;
1195 #endif
1196         for(int i = 0; i < numVectors; ++i)
1197                 dirArray[i] = MulPos(dirArray[i]);
1198 }
1199
1200 void float3x4::BatchTransformDir(float3 *dirArray, int numVectors, int stride) const
1201 {
1202         assume(dirArray);
1203 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1204         if (!dirArray)
1205                 return;
1206 #endif
1207         assume(stride >= (int)sizeof(float3));
1208         u8 *data = reinterpret_cast<u8*>(dirArray);
1209         for(int i = 0; i < numVectors; ++i)
1210         {
1211                 float3 *v = reinterpret_cast<float3*>(data + stride*i);
1212                 *v = MulDir(*v);
1213         }
1214 }
1215
1216 void float3x4::BatchTransform(float4 *vectorArray, int numVectors) const
1217 {
1218         assume(vectorArray);
1219 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1220         if (!vectorArray)
1221                 return;
1222 #endif
1223         for(int i = 0; i < numVectors; ++i)
1224                 vectorArray[i] = *this * vectorArray[i];
1225 }
1226
1227 void float3x4::BatchTransform(float4 *vectorArray, int numVectors, int stride) const
1228 {
1229         assume(vectorArray);
1230 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
1231         if (!vectorArray)
1232                 return;
1233 #endif
1234         assume(stride >= (int)sizeof(float4));
1235         u8 *data = reinterpret_cast<u8*>(vectorArray);
1236         for(int i = 0; i < numVectors; ++i)
1237         {
1238                 float4 *v = reinterpret_cast<float4*>(data + stride*i);
1239                 *v = *this * *v;
1240         }
1241 }
1242
1243 float3x4 float3x4::operator *(const float3x3 &rhs) const
1244 {
1245         ///\todo SSE.
1246         float3x4 r;
1247         const float *c0 = rhs.ptr();
1248         const float *c1 = rhs.ptr() + 1;
1249         const float *c2 = rhs.ptr() + 2;
1250         r[0][0] = DOT3STRIDED(v[0], c0, 3);
1251         r[0][1] = DOT3STRIDED(v[0], c1, 3);
1252         r[0][2] = DOT3STRIDED(v[0], c2, 3);
1253         r[0][3] = v[0][3];
1254
1255         r[1][0] = DOT3STRIDED(v[1], c0, 3);
1256         r[1][1] = DOT3STRIDED(v[1], c1, 3);
1257         r[1][2] = DOT3STRIDED(v[1], c2, 3);
1258         r[1][3] = v[1][3];
1259
1260         r[2][0] = DOT3STRIDED(v[2], c0, 3);
1261         r[2][1] = DOT3STRIDED(v[2], c1, 3);
1262         r[2][2] = DOT3STRIDED(v[2], c2, 3);
1263         r[2][3] = v[2][3];
1264
1265         return r;
1266 }
1267
1268 float3x4 float3x4::operator *(const float3x4 &rhs) const
1269 {
1270         float3x4 r;
1271 #ifdef MATH_SSE
1272         mat3x4_mul_sse(r.row, row, rhs.row);
1273 #else
1274         const float *c0 = rhs.ptr();
1275         const float *c1 = rhs.ptr() + 1;
1276         const float *c2 = rhs.ptr() + 2;
1277         const float *c3 = rhs.ptr() + 3;
1278         r[0][0] = DOT3STRIDED(v[0], c0, 4);
1279         r[0][1] = DOT3STRIDED(v[0], c1, 4);
1280         r[0][2] = DOT3STRIDED(v[0], c2, 4);
1281         r[0][3] = DOT3STRIDED(v[0], c3, 4) + v[0][3];
1282
1283         r[1][0] = DOT3STRIDED(v[1], c0, 4);
1284         r[1][1] = DOT3STRIDED(v[1], c1, 4);
1285         r[1][2] = DOT3STRIDED(v[1], c2, 4);
1286         r[1][3] = DOT3STRIDED(v[1], c3, 4) + v[1][3];
1287
1288         r[2][0] = DOT3STRIDED(v[2], c0, 4);
1289         r[2][1] = DOT3STRIDED(v[2], c1, 4);
1290         r[2][2] = DOT3STRIDED(v[2], c2, 4);
1291         r[2][3] = DOT3STRIDED(v[2], c3, 4) + v[2][3];
1292 #endif
1293
1294         return r;
1295 }
1296
1297 float3x4 float3x4::operator *(const Quat &rhs) const
1298 {
1299         float3x3 rot(rhs);
1300         return *this * rot;
1301 }
1302
1303 float4 float3x4::operator *(const float4 &rhs) const
1304 {
1305         return Transform(rhs);
1306 }
1307
1308 float3x4 float3x4::operator *(float scalar) const
1309 {
1310 #ifdef MATH_SIMD
1311         float3x4 r;
1312         simd4f s = set1_ps(scalar);
1313         r.row[0] = mul_ps(row[0], s);
1314         r.row[1] = mul_ps(row[1], s);
1315         r.row[2] = mul_ps(row[2], s);
1316 #else
1317         float3x4 r = *this;
1318         r *= scalar;
1319 #endif
1320
1321         return r;
1322 }
1323
1324 float3x4 float3x4::operator /(float scalar) const
1325 {
1326         assume(!EqualAbs(scalar, 0));
1327
1328 #ifdef MATH_SIMD
1329         float3x4 r;
1330         simd4f s = set1_ps(scalar);
1331         simd4f one = set1_ps(1.f);
1332         s = div_ps(one, s);
1333         r.row[0] = mul_ps(row[0], s);
1334         r.row[1] = mul_ps(row[1], s);
1335         r.row[2] = mul_ps(row[2], s);
1336 #else
1337         float3x4 r = *this;
1338         r /= scalar;
1339 #endif
1340
1341         return r;
1342 }
1343
1344 float3x4 float3x4::operator +(const float3x4 &rhs) const
1345 {
1346 #ifdef MATH_SIMD
1347         float3x4 r;
1348         r.row[0] = add_ps(row[0], rhs.row[0]);
1349         r.row[1] = add_ps(row[1], rhs.row[1]);
1350         r.row[2] = add_ps(row[2], rhs.row[2]);
1351 #else
1352         float3x4 r = *this;
1353         r += rhs;
1354 #endif
1355
1356         return r;
1357 }
1358
1359 float3x4 float3x4::operator -(const float3x4 &rhs) const
1360 {
1361 #ifdef MATH_SIMD
1362         float3x4 r;
1363         r.row[0] = sub_ps(row[0], rhs.row[0]);
1364         r.row[1] = sub_ps(row[1], rhs.row[1]);
1365         r.row[2] = sub_ps(row[2], rhs.row[2]);
1366 #else
1367         float3x4 r = *this;
1368         r -= rhs;
1369 #endif
1370
1371         return r;
1372 }
1373
1374 float3x4 float3x4::operator -() const
1375 {
1376         float3x4 r;
1377
1378 #ifdef MATH_SIMD
1379         simd4f zero = zero_ps();
1380         r.row[0] = sub_ps(zero, row[0]);
1381         r.row[1] = sub_ps(zero, row[1]);
1382         r.row[2] = sub_ps(zero, row[2]);
1383 #else
1384         for(int y = 0; y < Rows; ++y)
1385                 for(int x = 0; x < Cols; ++x)
1386                         r[y][x] = -v[y][x];
1387 #endif
1388
1389         return r;
1390 }
1391
1392 float3x4 &float3x4::operator *=(float scalar)
1393 {
1394 #ifdef MATH_SIMD
1395         simd4f s = set1_ps(scalar);
1396         row[0] = mul_ps(row[0], s);
1397         row[1] = mul_ps(row[1], s);
1398         row[2] = mul_ps(row[2], s);
1399 #else
1400         for(int y = 0; y < Rows; ++y)
1401                 for(int x = 0; x < Cols; ++x)
1402                         v[y][x] *= scalar;
1403 #endif
1404
1405         return *this;
1406 }
1407
1408 float3x4 &float3x4::operator /=(float scalar)
1409 {
1410         assume(!EqualAbs(scalar, 0));
1411
1412 #ifdef MATH_SIMD
1413         simd4f s = set1_ps(scalar);
1414         simd4f one = set1_ps(1.f);
1415         s = div_ps(one, s);
1416         row[0] = mul_ps(row[0], s);
1417         row[1] = mul_ps(row[1], s);
1418         row[2] = mul_ps(row[2], s);
1419 #else
1420         float invScalar = 1.f / scalar;
1421         for(int y = 0; y < Rows; ++y)
1422                 for(int x = 0; x < Cols; ++x)
1423                         v[y][x] *= invScalar;
1424 #endif
1425
1426         return *this;
1427 }
1428
1429 float3x4 &float3x4::operator +=(const float3x4 &rhs)
1430 {
1431 #ifdef MATH_SIMD
1432         row[0] = add_ps(row[0], rhs.row[0]);
1433         row[1] = add_ps(row[1], rhs.row[1]);
1434         row[2] = add_ps(row[2], rhs.row[2]);
1435 #else
1436         for(int y = 0; y < Rows; ++y)
1437                 for(int x = 0; x < Cols; ++x)
1438                         v[y][x] += rhs[y][x];
1439 #endif
1440
1441         return *this;
1442 }
1443
1444 float3x4 &float3x4::operator -=(const float3x4 &rhs)
1445 {
1446 #ifdef MATH_SIMD
1447         row[0] = sub_ps(row[0], rhs.row[0]);
1448         row[1] = sub_ps(row[1], rhs.row[1]);
1449         row[2] = sub_ps(row[2], rhs.row[2]);
1450 #else
1451         for(int y = 0; y < Rows; ++y)
1452                 for(int x = 0; x < Cols; ++x)
1453                         v[y][x] -= rhs[y][x];
1454 #endif
1455
1456         return *this;
1457 }
1458
1459 bool float3x4::IsFinite() const
1460 {
1461         for(int y = 0; y < Rows; ++y)
1462                 for(int x = 0; x < Cols; ++x)
1463                         if (!MATH_NS::IsFinite(v[y][x]))
1464                                 return false;
1465         return true;
1466 }
1467
1468 bool float3x4::IsIdentity(float epsilon) const
1469 {
1470         for(int y = 0; y < Rows; ++y)
1471                 for(int x = 0; x < Cols; ++x)
1472                         if (!EqualAbs(v[y][x], (x == y) ? 1.f : 0.f, epsilon))
1473                                 return false;
1474
1475         return true;
1476 }
1477
1478 bool float3x4::IsLowerTriangular(float epsilon) const
1479 {
1480         return EqualAbs(v[0][1], 0.f, epsilon)
1481                 && EqualAbs(v[0][2], 0.f, epsilon)
1482                 && EqualAbs(v[0][3], 0.f, epsilon)
1483                 && EqualAbs(v[1][2], 0.f, epsilon)
1484                 && EqualAbs(v[1][3], 0.f, epsilon)
1485                 && EqualAbs(v[2][3], 0.f, epsilon);
1486 }
1487
1488 bool float3x4::IsUpperTriangular(float epsilon) const
1489 {
1490         return EqualAbs(v[1][0], 0.f, epsilon)
1491                 && EqualAbs(v[2][0], 0.f, epsilon)
1492                 && EqualAbs(v[2][1], 0.f, epsilon);
1493 }
1494
1495 bool float3x4::IsInvertible(float epsilon) const
1496 {
1497         float d = Determinant();
1498         bool isSingular = EqualAbs(d, 0.f, epsilon);
1499 #ifdef MATH_ASSERT_CORRECTNESS
1500         float3x3 temp = Float3x3Part();
1501         mathassert(temp.Inverse(epsilon) != isSingular); // IsInvertible() and Inverse() must match!
1502 #endif
1503         return !isSingular;
1504 }
1505
1506 bool float3x4::IsSymmetric(float epsilon) const
1507 {
1508         return EqualAbs(v[0][1], v[1][0], epsilon) &&
1509                 EqualAbs(v[0][2], v[2][0], epsilon) &&
1510                 EqualAbs(v[1][2], v[2][1], epsilon);
1511 }
1512
1513 bool float3x4::IsSkewSymmetric(float epsilon) const
1514 {
1515         return EqualAbs(v[0][0], 0.f, epsilon) &&
1516                 EqualAbs(v[1][1], 0.f, epsilon) &&
1517                 EqualAbs(v[2][2], 0.f, epsilon) &&
1518                 EqualAbs(v[0][1], -v[1][0], epsilon) &&
1519                 EqualAbs(v[0][2], -v[2][0], epsilon) &&
1520                 EqualAbs(v[1][2], -v[2][1], epsilon);
1521 }
1522
1523 bool float3x4::HasUnitaryScale(float epsilon) const
1524 {
1525         float3 scale = ExtractScale();
1526         return scale.Equals(1.f, 1.f, 1.f, epsilon);
1527 }
1528
1529 bool float3x4::HasNegativeScale() const
1530 {
1531         return Determinant() < 0.f;
1532 }
1533
1534 bool float3x4::HasUniformScale(float epsilon) const
1535 {
1536         float3 scale = ExtractScale();
1537         return EqualAbs(scale.x, scale.y, epsilon) && EqualAbs(scale.x, scale.z, epsilon);
1538 }
1539
1540 bool float3x4::IsRowOrthogonal(float epsilon) const
1541 {
1542         return Row(0).IsPerpendicular3(Row(1), epsilon)
1543                 && Row(0).IsPerpendicular3(Row(2), epsilon)
1544                 && Row(1).IsPerpendicular3(Row(2), epsilon);
1545 }
1546
1547 bool float3x4::IsColOrthogonal(float epsilon) const
1548 {
1549         return Col(0).IsPerpendicular(Col(1), epsilon)
1550                 && Col(0).IsPerpendicular(Col(2), epsilon)
1551                 && Col(1).IsPerpendicular(Col(2), epsilon);
1552 }
1553
1554 bool float3x4::IsOrthonormal(float epsilon) const
1555 {
1556         ///@todo Epsilon magnitudes don't match.
1557         return IsColOrthogonal(epsilon) && Row3(0).IsNormalized(epsilon) && Row3(1).IsNormalized(epsilon) && Row3(2).IsNormalized(epsilon);
1558 }
1559
1560 bool float3x4::Equals(const float3x4 &other, float epsilon) const
1561 {
1562         for(int y = 0; y < Rows; ++y)
1563                 for(int x = 0; x < Cols; ++x)
1564                         if (!EqualAbs(v[y][x], other[y][x], epsilon))
1565                                 return false;
1566         return true;
1567 }
1568
1569
1570 #ifdef MATH_ENABLE_STL_SUPPORT
1571 std::string float3x4::ToString() const
1572 {
1573         char str[256];
1574         sprintf(str, "(%.2f, %.2f, %.2f, %.2f) (%.2f, %.2f, %.2f, %.2f) (%.2f, %.2f, %.2f, %.2f)",
1575                 v[0][0], v[0][1], v[0][2], v[0][3],
1576                 v[1][0], v[1][1], v[1][2], v[1][3],
1577                 v[2][0], v[2][1], v[2][2], v[2][3]);
1578
1579         return std::string(str);
1580 }
1581
1582 std::string float3x4::SerializeToString() const
1583 {
1584         char str[512];
1585         char *s = SerializeFloat(v[0][0], str); *s = ','; ++s;
1586         s = SerializeFloat(v[0][1], s); *s = ','; ++s;
1587         s = SerializeFloat(v[0][2], s); *s = ','; ++s;
1588         s = SerializeFloat(v[0][3], s); *s = ','; ++s;
1589         s = SerializeFloat(v[1][0], s); *s = ','; ++s;
1590         s = SerializeFloat(v[1][1], s); *s = ','; ++s;
1591         s = SerializeFloat(v[1][2], s); *s = ','; ++s;
1592         s = SerializeFloat(v[1][3], s); *s = ','; ++s;
1593         s = SerializeFloat(v[2][0], s); *s = ','; ++s;
1594         s = SerializeFloat(v[2][1], s); *s = ','; ++s;
1595         s = SerializeFloat(v[2][2], s); *s = ','; ++s;
1596         s = SerializeFloat(v[2][3], s);
1597         assert(s+1 - str < 512);
1598         MARK_UNUSED(s);
1599         return str;
1600 }
1601
1602 std::string float3x4::ToString2() const
1603 {
1604         char str[256];
1605         sprintf(str, "float3x4(X:(%.2f,%.2f,%.2f) Y:(%.2f,%.2f,%.2f) Z:(%.2f,%.2f,%.2f), Pos:(%.2f,%.2f,%.2f))",
1606                 v[0][0], v[1][0], v[2][0],
1607                 v[0][1], v[1][1], v[2][1],
1608                 v[0][2], v[1][2], v[2][2],
1609                 v[0][3], v[1][3], v[2][3]);
1610
1611         return std::string(str);
1612 }
1613 #endif
1614
1615 float3 float3x4::ToEulerXYX() const { float3 f; ExtractEulerXYX(*this, f[0], f[1], f[2]); return f; }
1616 float3 float3x4::ToEulerXZX() const { float3 f; ExtractEulerXZX(*this, f[0], f[1], f[2]); return f; }
1617 float3 float3x4::ToEulerYXY() const { float3 f; ExtractEulerYXY(*this, f[0], f[1], f[2]); return f; }
1618 float3 float3x4::ToEulerYZY() const { float3 f; ExtractEulerYZY(*this, f[0], f[1], f[2]); return f; }
1619 float3 float3x4::ToEulerZXZ() const { float3 f; ExtractEulerZXZ(*this, f[0], f[1], f[2]); return f; }
1620 float3 float3x4::ToEulerZYZ() const { float3 f; ExtractEulerZYZ(*this, f[0], f[1], f[2]); return f; }
1621 float3 float3x4::ToEulerXYZ() const { float3 f; ExtractEulerXYZ(*this, f[0], f[1], f[2]); return f; }
1622 float3 float3x4::ToEulerXZY() const { float3 f; ExtractEulerXZY(*this, f[0], f[1], f[2]); return f; }
1623 float3 float3x4::ToEulerYXZ() const { float3 f; ExtractEulerYXZ(*this, f[0], f[1], f[2]); return f; }
1624 float3 float3x4::ToEulerYZX() const { float3 f; ExtractEulerYZX(*this, f[0], f[1], f[2]); return f; }
1625 float3 float3x4::ToEulerZXY() const { float3 f; ExtractEulerZXY(*this, f[0], f[1], f[2]); return f; }
1626 float3 float3x4::ToEulerZYX() const { float3 f; ExtractEulerZYX(*this, f[0], f[1], f[2]); return f; }
1627
1628 float3 float3x4::ExtractScale() const
1629 {
1630         return float3(Col(0).Length(), Col(1).Length(), Col(2).Length());
1631 }
1632
1633 void float3x4::Decompose(float3 &translate, Quat &rotate, float3 &scale) const
1634 {
1635         assume(this->IsColOrthogonal());
1636
1637         float3x3 r;
1638         Decompose(translate, r, scale);
1639         rotate = Quat(r);
1640
1641         // Test that composing back yields the original float3x4.
1642         assume(float3x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
1643 }
1644
1645 void float3x4::Decompose(float3 &translate, float3x3 &rotate, float3 &scale) const
1646 {
1647         assume(this->IsColOrthogonal());
1648
1649         translate = Col(3);
1650         rotate = RotatePart();
1651         scale.x = rotate.Col(0).Length();
1652         scale.y = rotate.Col(1).Length();
1653         scale.z = rotate.Col(2).Length();
1654         assume(!EqualAbs(scale.x, 0));
1655         assume(!EqualAbs(scale.y, 0));
1656         assume(!EqualAbs(scale.z, 0));
1657         rotate.ScaleCol(0, 1.f / scale.x);
1658         rotate.ScaleCol(1, 1.f / scale.y);
1659         rotate.ScaleCol(2, 1.f / scale.z);
1660
1661         // Test that composing back yields the original float3x4.
1662         assume(float3x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
1663 }
1664
1665 void float3x4::Decompose(float3 &translate, float3x4 &rotate, float3 &scale) const
1666 {
1667         assume(this->IsColOrthogonal());
1668
1669         float3x3 r;
1670         Decompose(translate, r, scale);
1671         rotate.SetRotatePart(r);
1672         rotate.SetTranslatePart(0,0,0);
1673
1674         // Test that composing back yields the original float3x4.
1675         assume(float3x4::FromTRS(translate, rotate, scale).Equals(*this, 0.1f));
1676 }
1677
1678 #ifdef MATH_ENABLE_STL_SUPPORT
1679 std::ostream &operator <<(std::ostream &out, const float3x4 &rhs)
1680 {
1681         out << rhs.ToString();
1682         return out;
1683 }
1684 #endif
1685
1686 float3x4 operator *(const Quat &lhs, const float3x4 &rhs)
1687 {
1688         return float3x4(lhs) * rhs;
1689 }
1690
1691 float3x4 operator *(const float3x3 &lhs, const float3x4 &rhs)
1692 {
1693         ///\todo SSE.
1694         return float3x4(lhs) * rhs;
1695 }
1696
1697 float4 operator *(const float4 &lhs, const float3x4 &rhs)
1698 {
1699         ///\todo SSE.
1700         return float4(DOT3STRIDED(lhs, rhs.ptr(), 4),
1701                                   DOT3STRIDED(lhs, rhs.ptr()+1, 4),
1702                                   DOT3STRIDED(lhs, rhs.ptr()+2, 4),
1703                                   DOT3STRIDED(lhs, rhs.ptr()+3, 4) + lhs.w);
1704 }
1705
1706 float3x4 float3x4::Mul(const float3x3 &rhs) const { return *this * rhs; }
1707 float3x4 float3x4::Mul(const float3x4 &rhs) const { return *this * rhs; }
1708 float4x4 float3x4::Mul(const float4x4 &rhs) const { return *this * rhs; }
1709 float3x4 float3x4::Mul(const Quat &rhs) const { return *this * rhs; }
1710 float3 float3x4::MulPos(const float3 &pointVector) const { return this->TransformPos(pointVector); }
1711 float4 float3x4::MulPos(const float4 &pointVector) const
1712 {
1713         assume(!EqualAbs(pointVector.w, 0.f));
1714         return this->Transform(pointVector);
1715 }
1716 float3 float3x4::MulDir(const float3 &directionVector) const { return this->TransformDir(directionVector); }
1717 float4 float3x4::MulDir(const float4 &directionVector) const
1718 {
1719         assume(EqualAbs(directionVector.w, 0.f));
1720         return this->TransformDir(directionVector);
1721 }
1722 float4 float3x4::Mul(const float4 &vector) const { return *this * vector; }
1723
1724 const float3x4 float3x4::zero    = float3x4(0,0,0,0, 0,0,0,0, 0,0,0,0);
1725 const float3x4 float3x4::identity = float3x4(1,0,0,0, 0,1,0,0, 0,0,1,0);
1726 const float3x4 float3x4::nan = float3x4(FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN, FLOAT_NAN);
1727
1728 MATH_END_NAMESPACE