Bugfix #4081: support for OpenBSD platform for scons. Big thanks to Nathan Houghton...
[blender-staging.git] / extern / bullet / LinearMath / SimdMatrix3x3.h
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose, 
7 including commercial applications, and to alter it and redistribute it freely, 
8 subject to the following restrictions:
9
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15
16 #ifndef SimdMatrix3x3_H
17 #define SimdMatrix3x3_H
18
19 #include "SimdScalar.h"
20
21 #include "SimdVector3.h"
22 #include "SimdQuaternion.h"
23
24
25 class SimdMatrix3x3 {
26         public:
27                 SimdMatrix3x3 () {}
28                 
29 //              explicit SimdMatrix3x3(const SimdScalar *m) { setFromOpenGLSubMatrix(m); }
30                 
31                 explicit SimdMatrix3x3(const SimdQuaternion& q) { setRotation(q); }
32                 /*
33                 template <typename SimdScalar>
34                 Matrix3x3(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
35                 { 
36                         setEulerYPR(yaw, pitch, roll);
37                 }
38                 */
39                 SimdMatrix3x3(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
40                                   const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
41                                   const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
42                 { 
43                         setValue(xx, xy, xz, 
44                                          yx, yy, yz, 
45                                          zx, zy, zz);
46                 }
47                 
48                 SimdVector3 getColumn(int i) const
49                 {
50                         return SimdVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
51                 }
52
53                 const SimdVector3& getRow(int i) const
54                 {
55                         return m_el[i];
56                 }
57
58
59                 SIMD_FORCE_INLINE SimdVector3&  operator[](int i)
60                 { 
61                         assert(0 <= i && i < 3);
62                         return m_el[i]; 
63                 }
64                 
65                 const SimdVector3& operator[](int i) const
66                 {
67                         assert(0 <= i && i < 3);
68                         return m_el[i]; 
69                 }
70                 
71                 SimdMatrix3x3& operator*=(const SimdMatrix3x3& m); 
72                 
73         
74         void setFromOpenGLSubMatrix(const SimdScalar *m)
75                 {
76                         m_el[0][0] = (m[0]); 
77                         m_el[1][0] = (m[1]); 
78                         m_el[2][0] = (m[2]);
79                         m_el[0][1] = (m[4]); 
80                         m_el[1][1] = (m[5]); 
81                         m_el[2][1] = (m[6]);
82                         m_el[0][2] = (m[8]); 
83                         m_el[1][2] = (m[9]); 
84                         m_el[2][2] = (m[10]);
85                 }
86
87                 void setValue(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz, 
88                                           const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz, 
89                                           const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
90                 {
91                         m_el[0][0] = SimdScalar(xx); 
92                         m_el[0][1] = SimdScalar(xy); 
93                         m_el[0][2] = SimdScalar(xz);
94                         m_el[1][0] = SimdScalar(yx); 
95                         m_el[1][1] = SimdScalar(yy); 
96                         m_el[1][2] = SimdScalar(yz);
97                         m_el[2][0] = SimdScalar(zx); 
98                         m_el[2][1] = SimdScalar(zy); 
99                         m_el[2][2] = SimdScalar(zz);
100                 }
101   
102                 void setRotation(const SimdQuaternion& q) 
103                 {
104                         SimdScalar d = q.length2();
105                         assert(d != SimdScalar(0.0));
106                         SimdScalar s = SimdScalar(2.0) / d;
107                         SimdScalar xs = q[0] * s,   ys = q[1] * s,   zs = q[2] * s;
108                         SimdScalar wx = q[3] * xs,  wy = q[3] * ys,  wz = q[3] * zs;
109                         SimdScalar xx = q[0] * xs,  xy = q[0] * ys,  xz = q[0] * zs;
110                         SimdScalar yy = q[1] * ys,  yz = q[1] * zs,  zz = q[2] * zs;
111                         setValue(SimdScalar(1.0) - (yy + zz), xy - wz, xz + wy,
112                                          xy + wz, SimdScalar(1.0) - (xx + zz), yz - wx,
113                                          xz - wy, yz + wx, SimdScalar(1.0) - (xx + yy));
114                 }
115                 
116
117
118                 void setEulerYPR(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll) 
119                 {
120
121                         SimdScalar cy(SimdCos(yaw)); 
122                         SimdScalar  sy(SimdSin(yaw)); 
123                         SimdScalar  cp(SimdCos(pitch)); 
124                         SimdScalar  sp(SimdSin(pitch)); 
125                         SimdScalar  cr(SimdCos(roll));
126                         SimdScalar  sr(SimdSin(roll));
127                         SimdScalar  cc = cy * cr; 
128                         SimdScalar  cs = cy * sr; 
129                         SimdScalar  sc = sy * cr; 
130                         SimdScalar  ss = sy * sr;
131                         setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
132                      cp * sr,       cp * cr,      -sp,
133                                          sc + sp * cs, -ss + sp * cc,  cy * cp);
134                 
135                 }
136
137         /**
138          * setEulerZYX
139          * @param euler a const reference to a SimdVector3 of euler angles
140          * These angles are used to produce a rotation matrix. The euler
141          * angles are applied in ZYX order. I.e a vector is first rotated 
142          * about X then Y and then Z
143          **/
144         
145         void setEulerZYX(SimdScalar eulerX,SimdScalar eulerY,SimdScalar eulerZ) {
146                 SimdScalar ci ( SimdCos(eulerX)); 
147                 SimdScalar cj ( SimdCos(eulerY)); 
148                 SimdScalar ch ( SimdCos(eulerZ)); 
149                 SimdScalar si ( SimdSin(eulerX)); 
150                 SimdScalar sj ( SimdSin(eulerY)); 
151                 SimdScalar sh ( SimdSin(eulerZ)); 
152                 SimdScalar cc = ci * ch; 
153                 SimdScalar cs = ci * sh; 
154                 SimdScalar sc = si * ch; 
155                 SimdScalar ss = si * sh;
156                 
157                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
158                                  cj * sh, sj * ss + cc, sj * cs - sc, 
159                                  -sj,      cj * si,      cj * ci);
160         }
161
162                 void setIdentity()
163                 { 
164                         setValue(SimdScalar(1.0), SimdScalar(0.0), SimdScalar(0.0), 
165                                          SimdScalar(0.0), SimdScalar(1.0), SimdScalar(0.0), 
166                                          SimdScalar(0.0), SimdScalar(0.0), SimdScalar(1.0)); 
167                 }
168     
169                 void getOpenGLSubMatrix(SimdScalar *m) const 
170                 {
171                         m[0]  = SimdScalar(m_el[0][0]); 
172                         m[1]  = SimdScalar(m_el[1][0]);
173                         m[2]  = SimdScalar(m_el[2][0]);
174                         m[3]  = SimdScalar(0.0); 
175                         m[4]  = SimdScalar(m_el[0][1]);
176                         m[5]  = SimdScalar(m_el[1][1]);
177                         m[6]  = SimdScalar(m_el[2][1]);
178                         m[7]  = SimdScalar(0.0); 
179                         m[8]  = SimdScalar(m_el[0][2]); 
180                         m[9]  = SimdScalar(m_el[1][2]);
181                         m[10] = SimdScalar(m_el[2][2]);
182                         m[11] = SimdScalar(0.0); 
183                 }
184
185                 void getRotation(SimdQuaternion& q) const
186                 {
187                         SimdScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
188                         
189                         if (trace > SimdScalar(0.0)) 
190                         {
191                                 SimdScalar s = SimdSqrt(trace + SimdScalar(1.0));
192                                 q[3] = s * SimdScalar(0.5);
193                                 s = SimdScalar(0.5) / s;
194                                 
195                                 q[0] = (m_el[2][1] - m_el[1][2]) * s;
196                                 q[1] = (m_el[0][2] - m_el[2][0]) * s;
197                                 q[2] = (m_el[1][0] - m_el[0][1]) * s;
198                         } 
199                         else 
200                         {
201                                 int i = m_el[0][0] < m_el[1][1] ? 
202                                         (m_el[1][1] < m_el[2][2] ? 2 : 1) :
203                                         (m_el[0][0] < m_el[2][2] ? 2 : 0); 
204                                 int j = (i + 1) % 3;  
205                                 int k = (i + 2) % 3;
206                                 
207                                 SimdScalar s = SimdSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + SimdScalar(1.0));
208                                 q[i] = s * SimdScalar(0.5);
209                                 s = SimdScalar(0.5) / s;
210                                 
211                                 q[3] = (m_el[k][j] - m_el[j][k]) * s;
212                                 q[j] = (m_el[j][i] + m_el[i][j]) * s;
213                                 q[k] = (m_el[k][i] + m_el[i][k]) * s;
214                         }
215                 }
216         
217
218                 
219                 void getEuler(SimdScalar& yaw, SimdScalar& pitch, SimdScalar& roll) const
220                 {
221                         pitch = SimdScalar(SimdAsin(-m_el[2][0]));
222                         if (pitch < SIMD_2_PI)
223                         {
224                                 if (pitch > SIMD_2_PI)
225                                 {
226                                         yaw = SimdScalar(SimdAtan2(m_el[1][0], m_el[0][0]));
227                                         roll = SimdScalar(SimdAtan2(m_el[2][1], m_el[2][2]));
228                                 }
229                                 else 
230                                 {
231                                         yaw = SimdScalar(-SimdAtan2(-m_el[0][1], m_el[0][2]));
232                                         roll = SimdScalar(0.0);
233                                 }
234                         }
235                         else
236                         {
237                                 yaw = SimdScalar(SimdAtan2(-m_el[0][1], m_el[0][2]));
238                                 roll = SimdScalar(0.0);
239                         }
240                 }
241
242                 SimdVector3 getScaling() const
243                 {
244                         return SimdVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0],
245                                                                    m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1],
246                                                                    m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
247                 }
248                 
249                 
250                 SimdMatrix3x3 scaled(const SimdVector3& s) const
251                 {
252                         return SimdMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
253                                                                          m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
254                                                                          m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]);
255                 }
256
257                 SimdScalar            determinant() const;
258                 SimdMatrix3x3 adjoint() const;
259                 SimdMatrix3x3 absolute() const;
260                 SimdMatrix3x3 transpose() const;
261                 SimdMatrix3x3 inverse() const; 
262                 
263                 SimdMatrix3x3 transposeTimes(const SimdMatrix3x3& m) const;
264                 SimdMatrix3x3 timesTranspose(const SimdMatrix3x3& m) const;
265                 
266                 SimdScalar tdot(int c, const SimdVector3& v) const 
267                 {
268                         return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
269                 }
270                 
271         protected:
272                 SimdScalar cofac(int r1, int c1, int r2, int c2) const 
273                 {
274                         return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
275                 }
276
277                 SimdVector3 m_el[3];
278         };
279         
280         SIMD_FORCE_INLINE SimdMatrix3x3& 
281         SimdMatrix3x3::operator*=(const SimdMatrix3x3& m)
282         {
283                 setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
284                                  m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
285                                  m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
286                 return *this;
287         }
288         
289         SIMD_FORCE_INLINE SimdScalar 
290         SimdMatrix3x3::determinant() const
291         { 
292                 return triple((*this)[0], (*this)[1], (*this)[2]);
293         }
294         
295
296         SIMD_FORCE_INLINE SimdMatrix3x3 
297         SimdMatrix3x3::absolute() const
298         {
299                 return SimdMatrix3x3(
300                         SimdFabs(m_el[0][0]), SimdFabs(m_el[0][1]), SimdFabs(m_el[0][2]),
301                         SimdFabs(m_el[1][0]), SimdFabs(m_el[1][1]), SimdFabs(m_el[1][2]),
302                         SimdFabs(m_el[2][0]), SimdFabs(m_el[2][1]), SimdFabs(m_el[2][2]));
303         }
304
305         SIMD_FORCE_INLINE SimdMatrix3x3 
306         SimdMatrix3x3::transpose() const 
307         {
308                 return SimdMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
309                                                                  m_el[0][1], m_el[1][1], m_el[2][1],
310                                                                  m_el[0][2], m_el[1][2], m_el[2][2]);
311         }
312         
313         SIMD_FORCE_INLINE SimdMatrix3x3 
314         SimdMatrix3x3::adjoint() const 
315         {
316                 return SimdMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
317                                                                  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
318                                                                  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
319         }
320         
321         SIMD_FORCE_INLINE SimdMatrix3x3 
322         SimdMatrix3x3::inverse() const
323         {
324                 SimdVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
325                 SimdScalar det = (*this)[0].dot(co);
326                 assert(det != SimdScalar(0.0f));
327                 SimdScalar s = SimdScalar(1.0f) / det;
328                 return SimdMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
329                                                                  co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
330                                                                  co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
331         }
332         
333         SIMD_FORCE_INLINE SimdMatrix3x3 
334         SimdMatrix3x3::transposeTimes(const SimdMatrix3x3& m) const
335         {
336                 return SimdMatrix3x3(
337                         m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0],
338                         m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
339                         m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
340                         m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
341                         m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
342                         m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
343                         m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
344                         m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
345                         m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
346         }
347         
348         SIMD_FORCE_INLINE SimdMatrix3x3 
349         SimdMatrix3x3::timesTranspose(const SimdMatrix3x3& m) const
350         {
351                 return SimdMatrix3x3(
352                         m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
353                         m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
354                         m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
355                 
356         }
357
358         SIMD_FORCE_INLINE SimdVector3 
359         operator*(const SimdMatrix3x3& m, const SimdVector3& v) 
360         {
361                 return SimdVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
362         }
363         
364
365         SIMD_FORCE_INLINE SimdVector3
366         operator*(const SimdVector3& v, const SimdMatrix3x3& m)
367         {
368                 return SimdVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
369         }
370
371         SIMD_FORCE_INLINE SimdMatrix3x3 
372         operator*(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2)
373         {
374                 return SimdMatrix3x3(
375                         m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
376                         m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
377                         m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
378         }
379
380
381         SIMD_FORCE_INLINE SimdMatrix3x3 SimdMultTransposeLeft(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2) {
382     return SimdMatrix3x3(
383         m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
384         m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
385         m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
386         m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
387         m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
388         m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
389         m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
390         m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
391         m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
392 }
393
394
395 #endif