00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include "geometry.hh"
00026 #include "vector.hh"
00027
00028 #include <cassert>
00029 #include <cmath>
00030
00031 INLINE
00032 CTransformationMatrix::CTransformationMatrix(TMatrixType AType)
00033 {
00034 switch (AType)
00035 {
00036 case AnyMatrix : break;
00037 case NullMatrix : setToNull (); break;
00038 case IdentityMatrix: setToIdentity(); break;
00039 }
00040 }
00041
00042 INLINE
00043 CTransformationMatrix::
00044 CTransformationMatrix(const CTransformationMatrix& AMatrix)
00045 {
00046 *this = AMatrix;
00047 }
00048
00049 INLINE
00050 CTransformationMatrix::CTransformationMatrix(TCoordinate A00, TCoordinate A01,
00051 TCoordinate A02, TCoordinate A03,
00052 TCoordinate A10, TCoordinate A11,
00053 TCoordinate A12, TCoordinate A13,
00054 TCoordinate A20, TCoordinate A21,
00055 TCoordinate A22, TCoordinate A23,
00056 TCoordinate A30, TCoordinate A31,
00057 TCoordinate A32, TCoordinate A33)
00058 {
00059 FElements[0][0] = A00; FElements[0][1] = A01;
00060 FElements[0][2] = A02; FElements[0][3] = A03;
00061
00062 FElements[1][0] = A10; FElements[1][1] = A11;
00063 FElements[1][2] = A12; FElements[1][3] = A13;
00064
00065 FElements[2][0] = A20; FElements[2][1] = A21;
00066 FElements[2][2] = A22; FElements[2][3] = A23;
00067
00068 FElements[3][0] = A30; FElements[3][1] = A31;
00069 FElements[3][2] = A32; FElements[3][3] = A33;
00070 }
00071
00072 INLINE
00073 CTransformationMatrix::CTransformationMatrix(const CVertex& AVertex1,
00074 const CVertex& AVertex2,
00075 const CVertex& AVertex3)
00076 {
00077 FElements[0][0] = AVertex1.getX();
00078 FElements[0][1] = AVertex1.getY();
00079 FElements[0][2] = AVertex1.getZ();
00080 FElements[0][3] = 0;
00081
00082 FElements[1][0] = AVertex2.getX();
00083 FElements[1][1] = AVertex2.getY();
00084 FElements[1][2] = AVertex2.getZ();
00085 FElements[1][3] = 0;
00086
00087 FElements[2][0] = AVertex3.getX();
00088 FElements[2][1] = AVertex3.getY();
00089 FElements[2][2] = AVertex3.getZ();
00090 FElements[2][3] = 0;
00091
00092 FElements[3][0] = 0;
00093 FElements[3][1] = 0;
00094 FElements[3][2] = 0;
00095 FElements[3][3] = 0;
00096 }
00097
00098 INLINE
00099 void CTransformationMatrix::setToNull()
00100 {
00101 for (int row=0; row<4; ++row)
00102 for (int col=0; col<4; ++col)
00103 (*this)[row][col] = 0;
00104 }
00105
00106 INLINE
00107 void CTransformationMatrix::setToIdentity()
00108 {
00109 for(int row=0; row<4; ++row)
00110 for(int col=0; col<4; ++col)
00111 (*this)[row][col] = (row==col) ? 1 : 0;
00112 }
00113
00114 INLINE
00115 CTransformationMatrix& CTransformationMatrix::
00116 operator=(const CTransformationMatrix& AMatrix)
00117 {
00118 if (&AMatrix != this)
00119 {
00120 for (int row=0; row<4; ++row)
00121 for (int col=0; col<4; ++col)
00122 (*this)[row][col] = AMatrix[row][col];
00123 }
00124
00125 return *this;
00126 }
00127
00128 INLINE
00129 TCoordinate* CTransformationMatrix::operator[](int ARow) const
00130 {
00131 assert(0 <= ARow && ARow <= 3);
00132
00133 return (TCoordinate *) FElements[ARow];
00134 }
00135
00136 INLINE
00137 CTransformationMatrix
00138 CTransformationMatrix::operator*(const CTransformationMatrix& AMatrix) const
00139 {
00140 CTransformationMatrix temp(NullMatrix);
00141
00142 for (int row=0; row<4; ++row)
00143 for (int col=0; col<4; ++col)
00144 for (int k=0; k<4; ++k)
00145 temp[row][col] +=
00146 (*this)[row][k] * AMatrix[k][col];
00147
00148 return temp;
00149 }
00150
00151 INLINE
00152 CVector CTransformationMatrix::operator*(const CVector& AVector) const
00153 {
00154 CVector temp(CVector::NullVector);
00155
00156 for (int row=0; row<4; ++row)
00157 for (int col=0; col<4; ++col)
00158 temp[row] +=
00159 (*this)[row][col] * AVector[col];
00160
00161 return temp;
00162 }
00163
00164 INLINE
00165 void CTransformationMatrix::translate(const CVertex& AVector)
00166 {
00167 CTransformationMatrix translationMatrix(IdentityMatrix);
00168
00169 translationMatrix[0][3] = AVector.getX();
00170 translationMatrix[1][3] = AVector.getY();
00171 translationMatrix[2][3] = AVector.getZ();
00172
00173 (*this) = translationMatrix * (*this);
00174 }
00175
00176 INLINE
00177 void CTransformationMatrix::rotate(const CVertex& AAxeDirection,
00178 TCoordinate AAngle)
00179 {
00180 assert(!AAxeDirection.isNull());
00181
00182 CTransformationMatrix rotationMatrix(IdentityMatrix);
00183
00184 CVertex u = AAxeDirection / AAxeDirection.norm();
00185
00186 TCoordinate ca = cos(AAngle / 180.0 * M_PI);
00187 TCoordinate sa = sin(AAngle / 180.0 * M_PI);
00188
00189 TCoordinate ux = u.getX();
00190 TCoordinate uy = u.getY();
00191 TCoordinate uz = u.getZ();
00192
00193 TCoordinate ux2 = ux * ux;
00194 TCoordinate uy2 = uy * uy;
00195 TCoordinate uz2 = uz * uz;
00196
00197 rotationMatrix[0][0] = ux2 + ca*(1-ux2);
00198 rotationMatrix[0][1] = ux*uy*(1-ca) - uz*sa;
00199 rotationMatrix[0][2] = uz*ux*(1-ca) + uy*sa;
00200
00201 rotationMatrix[1][0] = ux*uy*(1-ca) + uz*sa;
00202 rotationMatrix[1][1] = uy2 + ca*(1-uy2);
00203 rotationMatrix[1][2] = uy*uz*(1-ca) - ux*sa;
00204
00205 rotationMatrix[2][0] = uz*ux*(1-ca) - uy*sa;
00206 rotationMatrix[2][1] = uy*uz*(1-ca) + ux*sa;
00207 rotationMatrix[2][2] = uz2 + ca*(1-uz2);
00208
00209 (*this) = rotationMatrix * (*this);
00210 }
00211
00212 INLINE
00213 void CTransformationMatrix::rotate(const CVertex& AAxeVertex,
00214 const CVertex& AAxeDirection,
00215 TCoordinate AAngle)
00216 {
00217 translate(-AAxeVertex);
00218 rotate(AAxeDirection, AAngle);
00219 translate(+AAxeVertex);
00220 }
00221
00222 INLINE
00223 void CTransformationMatrix::rotate(const CVertex& AAngles)
00224 {
00225 CVertex Y(OY), Z(OZ);
00226
00227 CTransformationMatrix rotationMatrix(CTransformationMatrix::IdentityMatrix);
00228
00229 rotationMatrix.rotate(OX, AAngles.getX()); rotationMatrix.applyOn(Y);
00230 rotationMatrix.rotate( Y, AAngles.getY()); rotationMatrix.applyOn(Z);
00231 rotationMatrix.rotate( Z, AAngles.getZ());
00232
00233 (*this) = rotationMatrix * (*this);
00234 }
00235
00236 INLINE
00237 void CTransformationMatrix::rotate180(const CVertex& AAxeDirection)
00238 {
00239 assert(!AAxeDirection.isNull());
00240
00241 CTransformationMatrix rotation180Matrix(IdentityMatrix);
00242
00243 CVertex u = AAxeDirection / AAxeDirection.norm();
00244
00245 TCoordinate ux = u.getX();
00246 TCoordinate uy = u.getY();
00247 TCoordinate uz = u.getZ();
00248
00249 rotation180Matrix[0][0] = 2*ux*ux - 1;
00250 rotation180Matrix[0][1] = 2*ux*uy;
00251 rotation180Matrix[0][2] = 2*uz*ux;
00252
00253 rotation180Matrix[1][0] = 2*ux*uy;
00254 rotation180Matrix[1][1] = 2*uy*uy - 1;
00255 rotation180Matrix[1][2] = 2*uy*uz;
00256
00257 rotation180Matrix[2][0] = 2*uz*ux;
00258 rotation180Matrix[2][1] = 2*uy*uz;
00259 rotation180Matrix[2][2] = 2*uz*uz - 1;
00260
00261 (*this) = rotation180Matrix * (*this);
00262 }
00263
00264 INLINE
00265 void CTransformationMatrix::scale(TCoordinate ACoef)
00266 {
00267 scale(CVertex(ACoef,ACoef,ACoef));
00268 }
00269
00270 INLINE
00271 void CTransformationMatrix::scale(const CVertex& ACoef)
00272 {
00273 CTransformationMatrix scaleMatrix(IdentityMatrix);
00274
00275 scaleMatrix[0][0] = ACoef.getX();
00276 scaleMatrix[1][1] = ACoef.getY();
00277 scaleMatrix[2][2] = ACoef.getZ();
00278
00279 (*this) = scaleMatrix * (*this);
00280 }
00281
00282 INLINE
00283 void CTransformationMatrix::scale(const CVertex& ACenter, TCoordinate ACoef)
00284 {
00285 CTransformationMatrix scaleMatrix(IdentityMatrix);
00286
00287 scaleMatrix.translate(-ACenter);
00288 scaleMatrix.scale(ACoef);
00289 scaleMatrix.translate(+ACenter);
00290
00291 (*this) = scaleMatrix * (*this);
00292 }
00293
00294 INLINE
00295 void CTransformationMatrix::scale(const CVertex& ACenter, const CVertex& ACoef)
00296 {
00297 CTransformationMatrix scaleMatrix(IdentityMatrix);
00298
00299 scaleMatrix.translate(-ACenter);
00300 scaleMatrix.scale(ACoef);
00301 scaleMatrix.translate(+ACenter);
00302
00303 (*this) = scaleMatrix * (*this);
00304 }
00305
00306 INLINE
00307 void CTransformationMatrix::axialScale(const CVertex& AAxeDirection,
00308 TCoordinate ACoef)
00309 {
00310 assert(!AAxeDirection.isNull());
00311
00312 CVertex axeNormal = CGeometry::getNormalVector(AAxeDirection);
00313 CTransformationMatrix scaleMatrix(IdentityMatrix);
00314
00315 scaleMatrix.orientate(AAxeDirection, axeNormal, OX, OZ);
00316 scaleMatrix.scale(CVertex(1,ACoef,ACoef));
00317 scaleMatrix.orientate(OX, OZ, AAxeDirection, axeNormal);
00318
00319 (*this) = scaleMatrix * (*this);
00320 }
00321
00322 INLINE
00323 void CTransformationMatrix::axialScale(const CVertex& AAxeVertex,
00324 const CVertex& AAxeDirection,
00325 TCoordinate ACoef)
00326 {
00327 assert(!AAxeDirection.isNull());
00328
00329 CTransformationMatrix scaleMatrix(IdentityMatrix);
00330
00331 scaleMatrix.translate(-AAxeVertex);
00332 scaleMatrix.axialScale(AAxeDirection, ACoef);
00333 scaleMatrix.translate(+AAxeVertex);
00334
00335 (*this) = scaleMatrix * (*this);
00336 }
00337
00338 INLINE
00339 void CTransformationMatrix::planarScale(const CVertex& APlaneNormal,
00340 TCoordinate ACoef)
00341 {
00342 assert(!APlaneNormal.isNull());
00343
00344 CVertex planeVector = CGeometry::getNormalVector(APlaneNormal);
00345 CTransformationMatrix scaleMatrix(IdentityMatrix);
00346
00347 scaleMatrix.orientate(planeVector, APlaneNormal, OX, OZ);
00348 scaleMatrix.scale(CVertex(1,1,ACoef));
00349 scaleMatrix.orientate(OX, OZ, planeVector, APlaneNormal);
00350
00351 (*this) = scaleMatrix * (*this);
00352 }
00353
00354 INLINE
00355 void CTransformationMatrix::planarScale(const CVertex& APlaneVertex,
00356 const CVertex& APlaneNormal,
00357 TCoordinate ACoef)
00358 {
00359 assert(!APlaneNormal.isNull());
00360
00361 CTransformationMatrix scaleMatrix(IdentityMatrix);
00362
00363 scaleMatrix.translate(-APlaneVertex);
00364 scaleMatrix.planarScale(APlaneNormal, ACoef);
00365 scaleMatrix.translate(+APlaneVertex);
00366
00367 (*this) = scaleMatrix * (*this);
00368 }
00369
00370 INLINE
00371 void CTransformationMatrix::orientate(const CVertex& AVector1,
00372 const CVertex& AVector2)
00373 {
00374 assert(!AVector1.isNull());
00375 assert(!AVector2.isNull());
00376
00377 CVertex axe = AVector1*AVector2;
00378
00379 if (axe.isNull())
00380 {
00381 if (AVector1.dot(AVector2) < 0)
00382 rotate(CGeometry::getNormalVector(AVector1), 180.0);
00383
00384 return;
00385 }
00386
00387 rotate(axe, CGeometry::getAngle(AVector2, AVector1));
00388 }
00389
00390 INLINE
00391 void CTransformationMatrix::orientate(const CVertex& ACenter,
00392 const CVertex& AVector1,
00393 const CVertex& AVector2)
00394 {
00395 translate(-ACenter);
00396 orientate(AVector1, AVector2);
00397 translate(+ACenter);
00398 }
00399
00400 INLINE
00401 void CTransformationMatrix::orientate(const CVertex& AVectorU1,
00402 const CVertex& AVectorV1,
00403 const CVertex& AVectorU2,
00404 const CVertex& AVectorV2)
00405 {
00406 assert(!AVectorU1.isNull());
00407 assert(!AVectorV1.isNull());
00408
00409 CVertex U1 = AVectorU1;
00410 CVertex const& U2 = AVectorU2;
00411
00412 CVertex V1 = CGeometry::orthoProjectVertexOnPlane(AVectorV1, ORIGIN, U1);
00413 CVertex V2 = CGeometry::orthoProjectVertexOnPlane(AVectorV2, ORIGIN, U2);
00414
00415 assert(!V1.isNull());
00416 assert(!V2.isNull());
00417
00418 CTransformationMatrix
00419 orientationMatrix(CTransformationMatrix::IdentityMatrix);
00420
00421 orientationMatrix.rotate180(CGeometry::getBissectrix(U1,-U2));
00422 orientationMatrix.applyOn(V1);
00423 orientationMatrix.rotate180(CGeometry::getBissectrix(V1,V2, U2));
00424
00425 (*this) = orientationMatrix * (*this);
00426 }
00427
00428 INLINE
00429 void CTransformationMatrix::transform(const CVertex& AOrigin1 ,
00430 const CVertex& AVectorU1,
00431 const CVertex& AVectorV1,
00432 const CVertex& AOrigin2 ,
00433 const CVertex& AVectorU2,
00434 const CVertex& AVectorV2,
00435 const CVertex& ACoef)
00436 {
00437 CTransformationMatrix
00438 transformationMatrix(CTransformationMatrix::IdentityMatrix);
00439
00440 transformationMatrix.translate(-AOrigin1);
00441 transformationMatrix.orientate(AVectorU1, AVectorV1, OZ,OX);
00442 transformationMatrix.scale(ACoef);
00443 transformationMatrix.orientate(OZ,OX, AVectorU2, AVectorV2);
00444 transformationMatrix.translate(+AOrigin2);
00445
00446 (*this) = transformationMatrix * (*this);
00447 }
00448
00449 INLINE
00450 void CTransformationMatrix::applyOn(CVertex& AVertex) const
00451 {
00452 applyOn(AVertex, AVertex);
00453 }
00454
00455 INLINE
00456 void CTransformationMatrix::applyOn(const CVertex& AVertex,
00457 CVertex& AResult) const
00458 {
00459 TCoordinate vertex[4] = { AVertex.getX(), AVertex.getY(), AVertex.getZ(), 1 };
00460 TCoordinate result[3] = { 0,0,0 };
00461
00462 for (int row=0; row<3; ++row)
00463 for (int col=0; col<4; ++col)
00464 result[row] += (*this)[row][col] * vertex[col];
00465
00466 AResult.setXYZ(result[0], result[1], result[2]);
00467 }
00468
00469 INLINE
00470 std::ostream& operator<<(std::ostream& AStream,
00471 const CTransformationMatrix& AMatrix)
00472 {
00473 AStream << std::endl;
00474
00475 for (int row=0; row<4; ++row)
00476 {
00477 AStream << "[";
00478
00479 for (int col=0; col<4; ++col)
00480 AStream << AMatrix[row][col] << "\t";
00481
00482
00483 AStream << "]" << std::endl;
00484 }
00485
00486 return AStream;
00487 }
00488