LightWrap++
AC++wrapperfortheLightWave3DSDK
matrix4x4.h
Go to the documentation of this file.
1 #ifndef MATRIX4X4_H
2 #define MATRIX4X4_H
3 
4 #include <lwpp/Point3d.h>
5 #include <limits>
6 
7 namespace lwpp
8 {
10  template <typename T>
11  class Matrix4x4
12  {
13  public:
14  T m[4][4];
16  void SetIdentity() {
17  for (int i = 0; i < 4; i++)
18  for (int j = 0; j < 4; j++)
19  m[i][j] = (i == j) ? 1.0 : 0.0;
20  }
22  Matrix4x4 () {
23  SetIdentity();
24  }
26  Matrix4x4 (const Vector3<T> &right, const Vector3<T> &up, const Vector3<T> &forward)
27  {
28 
29  m[0][0] = right.x;
30  m[1][0] = right.y;
31  m[2][0] = right.z;
32  m[3][0] = 0.0;
33 
34  m[0][1] = up.x;
35  m[1][1] = up.y;
36  m[2][1] = up.z;
37  m[3][1] = 0.0;
38 
39  m[0][2] = forward.x;
40  m[1][2] = forward.y;
41  m[2][2] = forward.z;
42  m[3][2] = 0.0;
43 
44  m[0][3] = m[1][3] = m[2][3] = 0.0;
45  m[3][3] = 1.0;
46  }
47 
48  Matrix4x4 (Vector3<T> &right, Vector3<T> &up, Vector3<T> &forward, Point3<T> &pos)
49  {
50  m[0][0] = right.x;
51  m[0][1] = right.y;
52  m[0][2] = right.z;
53  m[0][3] = 0.0;
54 
55  m[1][0] = up.x;
56  m[1][1] = up.y;
57  m[1][2] = up.z;
58  m[1][3] = 0.0;
59 
60  m[2][0] = forward.x;
61  m[2][1] = forward.y;
62  m[2][2] = forward.z;
63  m[2][3] = 0.0;
64 
65  m[3][0] = pos.x;
66  m[3][1] = pos.y;
67  m[3][2] = pos.z;
68  m[3][3] = 1.0;
69  }
70 
72  Matrix4x4 (const T v[9]) {
73 
74  m[0][0] = v[0];
75  m[0][1] = v[1];
76  m[0][2] = v[2];
77  m[0][3] = 0.0;
78 
79  m[1][0] = v[3];
80  m[1][1] = v[4];
81  m[1][2] = v[5];
82  m[1][3] = 0.0;
83 
84  m[2][0] = v[6];
85  m[2][1] = v[7];
86  m[2][2] = v[8];
87  m[2][3] = 0.0;
88 
89  m[0][3] = m[1][3] = m[2][3] = 0.0;
90  m[3][3] = 1.0;
91 
92 /*
93  m[0][0] = v[0];
94  m[1][0] = v[1];
95  m[2][0] = v[2];
96 
97  m[0][1] = v[3];
98  m[1][1] = v[4];
99  m[2][1] = v[5];
100 
101  m[0][2] = v[6];
102  m[1][2] = v[7];
103  m[2][2] = v[8];
104  */
105  }
106 
107  Matrix4x4 (const Matrix4x4 &v)
108  {
109  for (int i = 0; i < 4; ++i)
110  {
111  m[i][0] = v.m[i][0];
112  m[i][1] = v.m[i][1];
113  m[i][2] = v.m[i][2];
114  m[i][3] = v.m[i][3];
115  }
116  }
117 
118  Matrix4x4 &operator=(const Matrix4x4 &v)
119  {
120  if (this != &v)
121  {
122  for (int i = 0; i < 4; ++i)
123  {
124  m[i][0] = v.m[i][0];
125  m[i][1] = v.m[i][1];
126  m[i][2] = v.m[i][2];
127  m[i][3] = v.m[i][3];
128  }
129  }
130  return *this;
131  }
132 
133 
134  inline void RotateP(T angle)
135  {
136  T s = sin(angle);
137  T c = cos(angle);
138  m[1][1] = c;
139  // the following two have been swapped
140  m[1][2] = s;
141  m[2][1] = -s;
142  m[2][2] = c;
143  }
144  inline void RotateH(T angle)
145  {
146  T s = sin(angle);
147  T c = cos(angle);
148  m[0][0] = c;
149  m[2][0] = s;
150  m[0][2] = -s;
151  m[2][2] = c;
152  }
153  inline void RotateB(T angle)
154  {
155  T s = sin(angle);
156  T c = cos(angle);
157  m[0][0] = c;
158  m[1][0] = -s;
159  m[0][1] = s;
160  m[1][1] = c;
161  }
162 
163  inline void SetRotationHPB(T heading, T pitch, T bank)
164  {
165  T sinH = sin(heading);
166  T cosH = cos(heading);
167  T sinP = sin(pitch);
168  T cosP = cos(pitch);
169  T sinB = sin(bank);
170  T cosB = cos(bank);
171  m[0][0] = cosH * cosB - sinH * sinP * sinB;
172  m[1][0] = -cosP * sinB;
173  m[2][0] = sinH * cosB + cosH * sinP * sinB;
174 
175  m[0][1] = cosH * sinB + sinH * sinP * cosB;
176  m[1][1] = cosP * cosB;
177  m[2][1] = sinH * sinB + cosH * sinP * cosB;
178 
179  m[0][2] = -sinH * cosP;
180  m[1][2] = sinP;
181  m[2][2] = cosH * cosP;
182  }
183 
184  inline void SetRotationBHP(T heading, T pitch, T bank)
185  {
186  T sinH = sin(heading);
187  T cosH = cos(heading);
188  T sinP = sin(pitch);
189  T cosP = cos(pitch);
190  T sinB = sin(bank);
191  T cosB = cos(bank);
192  m[0][0] = cosB * cosH;
193  m[1][0] = -sinB * cosH;
194  m[2][0] = sinH;
195 
196  m[0][1] = sinB * cosP + cosB * sinH * sinP;
197  m[1][1] = cosB * cosP - sinH * sinB * sinP;
198  m[2][1] = -cosH * sinP;
199 
200  m[0][2] = sinB * sinP - cosB * sinH * cosP;
201  m[1][2] = cosB * sinP + sinH * sinB * cosP;
202  m[2][2] = cosH * cosP;
203  }
204 
205  Matrix4x4 (T m00, T m01, T m02, T m03,
206  T m10, T m11, T m12, T m13,
207  T m20, T m21, T m22, T m23,
208  T m30, T m31, T m32, T m33)
209  {
210  m[0][0] = m00; m[0][1] = m01; m[0][2] = m02; m[0][3] = m03;
211  m[1][0] = m10; m[1][1] = m11; m[1][2] = m12; m[1][3] = m13;
212  m[2][0] = m20; m[2][1] = m21; m[2][2] = m22; m[2][3] = m23;
213  m[3][0] = m30; m[3][1] = m31; m[3][2] = m32; m[3][3] = m33;
214  }
215  /*
216  For a homogeneous geometrical transformation matrix, you can get the roll, pitch and yaw angles, following the TRPY convention, using the following formulas:
217 
218  roll (rotation around z) : atan2(xy, xx)
219  pitch (rotation around y) : -arcsin(xz)
220  yaw (rotation around x) : atan2(yz,zz)
221 
222  where the matrix is defined in the form:
223 
224  [
225  xx, yx, zx, px;
226  xy, yy, zy, py;
227  xz, yz, zz, pz;
228  0, 0, 0, 1
229  ]
230 
231  Do use the full atan2 function so that you can get values from full trigonometric circle (ie. don't just use atan).
232  */
233 
234  Matrix4x4 (T v[4][4])
235  {
236  for (int i = 0; i < 4; ++i)
237  {
238  for (int j = 0; j < 4; ++j)
239  {
240  m[i][j] = v[i][j];
241  }
242  }
243  }
244 
246  inline Matrix4x4 Mul(const Matrix4x4 &m1)
247  {
248  T r[4][4];
249  for (int i = 0; i < 4; ++i)
250  {
251  for (int j = 0; j < 4; ++j)
252  {
253  r[i][j] = m[i][0] * m1.m[0][j] +
254  m[i][1] * m1.m[1][j] +
255  m[i][2] * m1.m[2][j] +
256  m[i][3] * m1.m[3][j];
257  }
258  }
259  return Matrix4x4(r);
260  }
261 
263  Matrix4x4 &Inverse()
264  {
265  int indxc[4], indxr[4];
266  int ipiv[4] = { 0, 0, 0, 0 };
267  Matrix4x4 minv(*this);
268  //memcpy(minv, m, 4*4*sizeof(double));
269  int j;
270  for (int i = 0; i < 4; i++)
271  {
272  int irow = -1, icol = -1;
273  T big = 0.;
274  // Choose pivot
275  for (j = 0; j < 4; j++)
276  {
277  if (ipiv[j] != 1)
278  {
279  for (int k = 0; k < 4; k++)
280  {
281  if (ipiv[k] == 0)
282  {
283  if (abs(minv.m[j][k]) >= big)
284  {
285  big = abs(minv.m[j][k]);
286  irow = j;
287  icol = k;
288  }
289  }
290  else if (ipiv[k] > 1) return *this; // Error("Singular matrix in MatrixInvert");
291  }
292  }
293  }
294  ++ipiv[icol];
295  // Swap rows _irow_ and _icol_ for pivot
296  if (irow != icol)
297  {
298  for (int k = 0; k < 4; ++k) Swap(minv.m[irow][k], minv.m[icol][k]);
299  }
300  indxr[i] = irow;
301  indxc[i] = icol;
302  if (minv.m[icol][icol] == 0.) return *this; // Error("Singular matrix in MatrixInvert");
303  // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
304  T pivinv = 1.f / minv.m[icol][icol];
305  minv.m[icol][icol] = 1.f;
306  for (j = 0; j < 4; j++) minv.m[icol][j] *= pivinv;
307  // Subtract this row from others to zero out their columns
308  for (j = 0; j < 4; j++)
309  {
310  if (j != icol)
311  {
312  T save = minv.m[j][icol];
313  minv.m[j][icol] = 0;
314  for (int k = 0; k < 4; k++)
315  minv.m[j][k] -= minv.m[icol][k]*save;
316  }
317  }
318  }
319  // Swap columns to reflect permutation
320  for (j = 3; j >= 0; j--)
321  {
322  if (indxr[j] != indxc[j])
323  {
324  for (int k = 0; k < 4; k++)
325  Swap(minv.m[k][indxr[j]], minv.m[k][indxc[j]]);
326  }
327  }
328  *this = minv;
329  return *this;
330  }
335  inline Point3<T> operator()(const Point3<T> &v) const
336  {
337  T xp = v.x*m[0][0] + v.y*m[1][0] + v.z*m[2][0] + m[3][0];
338  T yp = v.x*m[0][1] + v.y*m[1][1] + v.z*m[2][1] + m[3][1];
339  T zp = v.x*m[0][2] + v.y*m[1][2] + v.z*m[2][2] + m[3][2];
340 /*
341  T wp = v.x*m[0][3] + v.y*m[1][3] + v.z*m[2][3] + m[3][3];
342 
343  if (wp > std::numeric_limits<T>::denorm_min())
344  {
345  return Point3<T>(xp, yp, zp) / wp;
346  }
347  */
348  return Point3<T>(xp, yp, zp);
349  }
350 
355  inline Point3<T> &Mul3x3 (Point3<T> &v) const
356  {
357  T xp = v.x*m[0][0] + v.y*m[1][0] + v.z*m[2][0];
358  T yp = v.x*m[0][1] + v.y*m[1][1] + v.z*m[2][1];
359  T zp = v.x*m[0][2] + v.y*m[1][2] + v.z*m[2][2];
360  v.x = xp;
361  v.y = yp;
362  v.z = zp;
363  return v;
364  }
369  inline T operator()(Point3<T> *v) const
370  {
371  T x = v->x;
372  T y = v->y;
373  T z = v->z;
374  v->x = x*m[0][0] + y*m[1][0] + z*m[2][0] + m[3][0];
375  v->y = x*m[0][1] + y*m[1][1] + z*m[2][1] + m[3][1];
376  v->z = x*m[0][2] + y*m[1][2] + z*m[2][2] + m[3][2];
377  T w = x*m[0][3] + y*m[1][3] + z*m[2][3] + m[3][3];
378  //if (w > std::numeric_limits<T>::denorm_min())
379  if (w != 0.0)
380  {
381  *v /= w;
382  }
383  return w;
384  }
389  inline Vector3<T> transform(const Vector3<T> &v) const
390  {
391  return Vector3<T>( v.x*m[0][0] + v.y*m[1][0] + v.z*m[2][0],
392  v.x*m[0][1] + v.y*m[1][1] + v.z*m[2][1],
393  v.x*m[0][2] + v.y*m[1][2] + v.z*m[2][2]);
394  }
395  inline Vector3<T> operator()(const Vector3<T> &v) const
396  {
397  return transform(v);
398  }
399 
400  };
401 
402  typedef Vector3<double> Vector3d;
403  typedef Vector3<float> Vector3f;
404 
405  typedef Matrix4x4<double> Matrix4x4d;
406  typedef Matrix4x4<float> Matrix4x4f;
407 }
408 #endif // MATRIX4X4_H