Moka kernel
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
transformation-matrix.icc
Go to the documentation of this file.
1 /*
2  * lib-gmapkernel : Un noyau de 3-G-cartes et des opérations.
3  * Copyright (C) 2004, Moka Team, Université de Poitiers, Laboratoire SIC
4  * http://www.sic.sp2mi.univ-poitiers.fr/
5  * Copyright (C) 2009, Guillaume Damiand, CNRS, LIRIS,
6  * guillaume.damiand@liris.cnrs.fr, http://liris.cnrs.fr/
7  *
8  * This file is part of lib-gmapkernel
9  *
10  * This program is free software: you can redistribute it and/or modify
11  * it under the terms of the GNU Lesser General Public License as published by
12  * the Free Software Foundation, either version 3 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public License
21  * along with this program. If not, see <http://www.gnu.org/licenses/>.
22  */
23 
24 //******************************************************************************
25 #include "geometry.hh"
26 #include "vector.hh"
27 
28 #include <cassert>
29 #include <cmath>
30 //******************************************************************************
31 INLINE
32 CTransformationMatrix::CTransformationMatrix(TMatrixType AType)
33 {
34  switch (AType)
35  {
36  case AnyMatrix : break;
37  case NullMatrix : setToNull (); break;
38  case IdentityMatrix: setToIdentity(); break;
39  }
40 }
41 //******************************************************************************
42 INLINE
43 CTransformationMatrix::
44 CTransformationMatrix(const CTransformationMatrix& AMatrix)
45 {
46  *this = AMatrix;
47 }
48 //******************************************************************************
49 INLINE
50 CTransformationMatrix::CTransformationMatrix(TCoordinate A00, TCoordinate A01,
51  TCoordinate A02, TCoordinate A03,
52  TCoordinate A10, TCoordinate A11,
53  TCoordinate A12, TCoordinate A13,
54  TCoordinate A20, TCoordinate A21,
55  TCoordinate A22, TCoordinate A23,
56  TCoordinate A30, TCoordinate A31,
57  TCoordinate A32, TCoordinate A33)
58 {
59  FElements[0][0] = A00; FElements[0][1] = A01;
60  FElements[0][2] = A02; FElements[0][3] = A03;
61 
62  FElements[1][0] = A10; FElements[1][1] = A11;
63  FElements[1][2] = A12; FElements[1][3] = A13;
64 
65  FElements[2][0] = A20; FElements[2][1] = A21;
66  FElements[2][2] = A22; FElements[2][3] = A23;
67 
68  FElements[3][0] = A30; FElements[3][1] = A31;
69  FElements[3][2] = A32; FElements[3][3] = A33;
70 }
71 //******************************************************************************
72 INLINE
73 CTransformationMatrix::CTransformationMatrix(const CVertex& AVertex1,
74  const CVertex& AVertex2,
75  const CVertex& AVertex3)
76 {
77  FElements[0][0] = AVertex1.getX();
78  FElements[0][1] = AVertex1.getY();
79  FElements[0][2] = AVertex1.getZ();
80  FElements[0][3] = 0;
81 
82  FElements[1][0] = AVertex2.getX();
83  FElements[1][1] = AVertex2.getY();
84  FElements[1][2] = AVertex2.getZ();
85  FElements[1][3] = 0;
86 
87  FElements[2][0] = AVertex3.getX();
88  FElements[2][1] = AVertex3.getY();
89  FElements[2][2] = AVertex3.getZ();
90  FElements[2][3] = 0;
91 
92  FElements[3][0] = 0;
93  FElements[3][1] = 0;
94  FElements[3][2] = 0;
95  FElements[3][3] = 0;
96 }
97 //******************************************************************************
98 INLINE
99 void CTransformationMatrix::setToNull()
100 {
101  for (int row=0; row<4; ++row)
102  for (int col=0; col<4; ++col)
103  (*this)[row][col] = 0;
104 }
105 //******************************************************************************
106 INLINE
107 void CTransformationMatrix::setToIdentity()
108 {
109  for(int row=0; row<4; ++row)
110  for(int col=0; col<4; ++col)
111  (*this)[row][col] = (row==col) ? 1 : 0;
112 }
113 //******************************************************************************
114 INLINE
115 CTransformationMatrix& CTransformationMatrix::
116 operator=(const CTransformationMatrix& AMatrix)
117 {
118  if (&AMatrix != this)
119  {
120  for (int row=0; row<4; ++row)
121  for (int col=0; col<4; ++col)
122  (*this)[row][col] = AMatrix[row][col];
123  }
124 
125  return *this;
126 }
127 //******************************************************************************
128 INLINE
129 TCoordinate* CTransformationMatrix::operator[](int ARow) const
130 {
131  assert(0 <= ARow && ARow <= 3);
132 
133  return (TCoordinate *) FElements[ARow];
134 }
135 //******************************************************************************
136 INLINE
137 CTransformationMatrix
138 CTransformationMatrix::operator*(const CTransformationMatrix& AMatrix) const
139 {
140  CTransformationMatrix temp(NullMatrix);
141 
142  for (int row=0; row<4; ++row)
143  for (int col=0; col<4; ++col)
144  for (int k=0; k<4; ++k)
145  temp[row][col] +=
146  (*this)[row][k] * AMatrix[k][col];
147 
148  return temp;
149 }
150 //******************************************************************************
151 INLINE
152 CVector CTransformationMatrix::operator*(const CVector& AVector) const
153 {
154  CVector temp(CVector::NullVector);
155 
156  for (int row=0; row<4; ++row)
157  for (int col=0; col<4; ++col)
158  temp[row] +=
159  (*this)[row][col] * AVector[col];
160 
161  return temp;
162 }
163 //******************************************************************************
164 INLINE
165 void CTransformationMatrix::translate(const CVertex& AVector)
166 {
167  CTransformationMatrix translationMatrix(IdentityMatrix);
168 
169  translationMatrix[0][3] = AVector.getX();
170  translationMatrix[1][3] = AVector.getY();
171  translationMatrix[2][3] = AVector.getZ();
172 
173  (*this) = translationMatrix * (*this);
174 }
175 //******************************************************************************
176 INLINE
177 void CTransformationMatrix::rotate(const CVertex& AAxeDirection,
178  TCoordinate AAngle)
179 {
180  assert(!AAxeDirection.isNull());
181 
182  CTransformationMatrix rotationMatrix(IdentityMatrix);
183 
184  CVertex u = AAxeDirection / AAxeDirection.norm(); // vecteur unitaire
185 
186  TCoordinate ca = cos(AAngle / 180.0 * M_PI);
187  TCoordinate sa = sin(AAngle / 180.0 * M_PI);
188 
189  TCoordinate ux = u.getX();
190  TCoordinate uy = u.getY();
191  TCoordinate uz = u.getZ();
192 
193  TCoordinate ux2 = ux * ux;
194  TCoordinate uy2 = uy * uy;
195  TCoordinate uz2 = uz * uz;
196 
197  rotationMatrix[0][0] = ux2 + ca*(1-ux2);
198  rotationMatrix[0][1] = ux*uy*(1-ca) - uz*sa;
199  rotationMatrix[0][2] = uz*ux*(1-ca) + uy*sa;
200 
201  rotationMatrix[1][0] = ux*uy*(1-ca) + uz*sa;
202  rotationMatrix[1][1] = uy2 + ca*(1-uy2);
203  rotationMatrix[1][2] = uy*uz*(1-ca) - ux*sa;
204 
205  rotationMatrix[2][0] = uz*ux*(1-ca) - uy*sa;
206  rotationMatrix[2][1] = uy*uz*(1-ca) + ux*sa;
207  rotationMatrix[2][2] = uz2 + ca*(1-uz2);
208 
209  (*this) = rotationMatrix * (*this);
210 }
211 //******************************************************************************
212 INLINE
213 void CTransformationMatrix::rotate(const CVertex& AAxeVertex,
214  const CVertex& AAxeDirection,
215  TCoordinate AAngle)
216 {
217  translate(-AAxeVertex);
218  rotate(AAxeDirection, AAngle);
219  translate(+AAxeVertex);
220 }
221 //******************************************************************************
222 INLINE
223 void CTransformationMatrix::rotate(const CVertex& AAngles)
224 {
225  CVertex Y(OY), Z(OZ);
226 
227  CTransformationMatrix rotationMatrix(CTransformationMatrix::IdentityMatrix);
228 
229  rotationMatrix.rotate(OX, AAngles.getX()); rotationMatrix.applyOn(Y);
230  rotationMatrix.rotate( Y, AAngles.getY()); rotationMatrix.applyOn(Z);
231  rotationMatrix.rotate( Z, AAngles.getZ());
232 
233  (*this) = rotationMatrix * (*this);
234 }
235 //******************************************************************************
236 INLINE
237 void CTransformationMatrix::rotate180(const CVertex& AAxeDirection)
238 {
239  assert(!AAxeDirection.isNull());
240 
241  CTransformationMatrix rotation180Matrix(IdentityMatrix);
242 
243  CVertex u = AAxeDirection / AAxeDirection.norm(); // vecteur unitaire
244 
245  TCoordinate ux = u.getX();
246  TCoordinate uy = u.getY();
247  TCoordinate uz = u.getZ();
248 
249  rotation180Matrix[0][0] = 2*ux*ux - 1;
250  rotation180Matrix[0][1] = 2*ux*uy;
251  rotation180Matrix[0][2] = 2*uz*ux;
252 
253  rotation180Matrix[1][0] = 2*ux*uy;
254  rotation180Matrix[1][1] = 2*uy*uy - 1;
255  rotation180Matrix[1][2] = 2*uy*uz;
256 
257  rotation180Matrix[2][0] = 2*uz*ux;
258  rotation180Matrix[2][1] = 2*uy*uz;
259  rotation180Matrix[2][2] = 2*uz*uz - 1;
260 
261  (*this) = rotation180Matrix * (*this);
262 }
263 //******************************************************************************
264 INLINE
265 void CTransformationMatrix::scale(TCoordinate ACoef)
266 {
267  scale(CVertex(ACoef,ACoef,ACoef));
268 }
269 //******************************************************************************
270 INLINE
271 void CTransformationMatrix::scale(const CVertex& ACoef)
272 {
273  CTransformationMatrix scaleMatrix(IdentityMatrix);
274 
275  scaleMatrix[0][0] = ACoef.getX();
276  scaleMatrix[1][1] = ACoef.getY();
277  scaleMatrix[2][2] = ACoef.getZ();
278 
279  (*this) = scaleMatrix * (*this);
280 }
281 //******************************************************************************
282 INLINE
283 void CTransformationMatrix::scale(const CVertex& ACenter, TCoordinate ACoef)
284 {
285  CTransformationMatrix scaleMatrix(IdentityMatrix);
286 
287  scaleMatrix.translate(-ACenter);
288  scaleMatrix.scale(ACoef);
289  scaleMatrix.translate(+ACenter);
290 
291  (*this) = scaleMatrix * (*this);
292 }
293 //******************************************************************************
294 INLINE
295 void CTransformationMatrix::scale(const CVertex& ACenter, const CVertex& ACoef)
296 {
297  CTransformationMatrix scaleMatrix(IdentityMatrix);
298 
299  scaleMatrix.translate(-ACenter);
300  scaleMatrix.scale(ACoef);
301  scaleMatrix.translate(+ACenter);
302 
303  (*this) = scaleMatrix * (*this);
304 }
305 //******************************************************************************
306 INLINE
307 void CTransformationMatrix::axialScale(const CVertex& AAxeDirection,
308  TCoordinate ACoef)
309 {
310  assert(!AAxeDirection.isNull());
311 
312  CVertex axeNormal = CGeometry::getNormalVector(AAxeDirection);
313  CTransformationMatrix scaleMatrix(IdentityMatrix);
314 
315  scaleMatrix.orientate(AAxeDirection, axeNormal, OX, OZ);
316  scaleMatrix.scale(CVertex(1,ACoef,ACoef));
317  scaleMatrix.orientate(OX, OZ, AAxeDirection, axeNormal);
318 
319  (*this) = scaleMatrix * (*this);
320 }
321 //******************************************************************************
322 INLINE
323 void CTransformationMatrix::axialScale(const CVertex& AAxeVertex,
324  const CVertex& AAxeDirection,
325  TCoordinate ACoef)
326 {
327  assert(!AAxeDirection.isNull());
328 
329  CTransformationMatrix scaleMatrix(IdentityMatrix);
330 
331  scaleMatrix.translate(-AAxeVertex);
332  scaleMatrix.axialScale(AAxeDirection, ACoef);
333  scaleMatrix.translate(+AAxeVertex);
334 
335  (*this) = scaleMatrix * (*this);
336 }
337 //******************************************************************************
338 INLINE
339 void CTransformationMatrix::planarScale(const CVertex& APlaneNormal,
340  TCoordinate ACoef)
341 {
342  assert(!APlaneNormal.isNull());
343 
344  CVertex planeVector = CGeometry::getNormalVector(APlaneNormal);
345  CTransformationMatrix scaleMatrix(IdentityMatrix);
346 
347  scaleMatrix.orientate(planeVector, APlaneNormal, OX, OZ);
348  scaleMatrix.scale(CVertex(1,1,ACoef));
349  scaleMatrix.orientate(OX, OZ, planeVector, APlaneNormal);
350 
351  (*this) = scaleMatrix * (*this);
352 }
353 //******************************************************************************
354 INLINE
355 void CTransformationMatrix::planarScale(const CVertex& APlaneVertex,
356  const CVertex& APlaneNormal,
357  TCoordinate ACoef)
358 {
359  assert(!APlaneNormal.isNull());
360 
361  CTransformationMatrix scaleMatrix(IdentityMatrix);
362 
363  scaleMatrix.translate(-APlaneVertex);
364  scaleMatrix.planarScale(APlaneNormal, ACoef);
365  scaleMatrix.translate(+APlaneVertex);
366 
367  (*this) = scaleMatrix * (*this);
368 }
369 //******************************************************************************
370 INLINE
371 void CTransformationMatrix::orientate(const CVertex& AVector1,
372  const CVertex& AVector2)
373 {
374  assert(!AVector1.isNull());
375  assert(!AVector2.isNull());
376 
377  CVertex axe = AVector1*AVector2;
378 
379  if (axe.isNull())
380  {
381  if (AVector1.dot(AVector2) < 0)
382  rotate(CGeometry::getNormalVector(AVector1), 180.0);
383 
384  return;
385  }
386 
387  rotate(axe, CGeometry::getAngle(AVector2, AVector1));
388 }
389 //******************************************************************************
390 INLINE
391 void CTransformationMatrix::orientate(const CVertex& ACenter,
392  const CVertex& AVector1,
393  const CVertex& AVector2)
394 {
395  translate(-ACenter);
396  orientate(AVector1, AVector2);
397  translate(+ACenter);
398 }
399 //******************************************************************************
400 INLINE
401 void CTransformationMatrix::orientate(const CVertex& AVectorU1,
402  const CVertex& AVectorV1,
403  const CVertex& AVectorU2,
404  const CVertex& AVectorV2)
405 {
406  assert(!AVectorU1.isNull());
407  assert(!AVectorV1.isNull());
408 
409  CVertex U1 = AVectorU1;
410  CVertex const& U2 = AVectorU2;
411 
412  CVertex V1 = CGeometry::orthoProjectVertexOnPlane(AVectorV1, ORIGIN, U1);
413  CVertex V2 = CGeometry::orthoProjectVertexOnPlane(AVectorV2, ORIGIN, U2);
414 
415  assert(!V1.isNull());
416  assert(!V2.isNull());
417 
418  CTransformationMatrix
419  orientationMatrix(CTransformationMatrix::IdentityMatrix);
420 
421  orientationMatrix.rotate180(CGeometry::getBissectrix(U1,-U2));
422  orientationMatrix.applyOn(V1);
423  orientationMatrix.rotate180(CGeometry::getBissectrix(V1,V2, U2));
424 
425  (*this) = orientationMatrix * (*this);
426 }
427 //******************************************************************************
428 INLINE
429 void CTransformationMatrix::transform(const CVertex& AOrigin1 ,
430  const CVertex& AVectorU1,
431  const CVertex& AVectorV1,
432  const CVertex& AOrigin2 ,
433  const CVertex& AVectorU2,
434  const CVertex& AVectorV2,
435  const CVertex& ACoef)
436 {
437  CTransformationMatrix
438  transformationMatrix(CTransformationMatrix::IdentityMatrix);
439 
440  transformationMatrix.translate(-AOrigin1);
441  transformationMatrix.orientate(AVectorU1, AVectorV1, OZ,OX);
442  transformationMatrix.scale(ACoef);
443  transformationMatrix.orientate(OZ,OX, AVectorU2, AVectorV2);
444  transformationMatrix.translate(+AOrigin2);
445 
446  (*this) = transformationMatrix * (*this);
447 }
448 //******************************************************************************
449 INLINE
450 void CTransformationMatrix::applyOn(CVertex& AVertex) const
451 {
452  applyOn(AVertex, AVertex);
453 }
454 //******************************************************************************
455 INLINE
456 void CTransformationMatrix::applyOn(const CVertex& AVertex,
457  CVertex& AResult) const
458 {
459  TCoordinate vertex[4] = { AVertex.getX(), AVertex.getY(), AVertex.getZ(), 1 };
460  TCoordinate result[3] = { 0,0,0 };
461 
462  for (int row=0; row<3; ++row)
463  for (int col=0; col<4; ++col)
464  result[row] += (*this)[row][col] * vertex[col];
465 
466  AResult.setXYZ(result[0], result[1], result[2]);
467 }
468 //******************************************************************************
469 INLINE
470 std::ostream& operator<<(std::ostream& AStream,
471  const CTransformationMatrix& AMatrix)
472 {
473  AStream << std::endl;
474 
475  for (int row=0; row<4; ++row)
476  {
477  AStream << "[";
478 
479  for (int col=0; col<4; ++col)
480  AStream << AMatrix[row][col] << "\t";
481  // AStream.form("% .3f ", AMatrix[row][col]);
482 
483  AStream << "]" << std::endl;
484  }
485 
486  return AStream;
487 }
488 //******************************************************************************