1
/***************************************************************************
2
* Copyright (c) 2005 Imetric 3D GmbH *
4
* This file is part of the FreeCAD CAx development system. *
6
* This library is free software; you can redistribute it and/or *
7
* modify it under the terms of the GNU Library General Public *
8
* License as published by the Free Software Foundation; either *
9
* version 2 of the License, or (at your option) any later version. *
11
* This library is distributed in the hope that it will be useful, *
12
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
13
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14
* GNU Library General Public License for more details. *
16
* You should have received a copy of the GNU Library General Public *
17
* License along with this library; see the file COPYING.LIB. If not, *
18
* write to the Free Software Foundation, Inc., 59 Temple Place, *
19
* Suite 330, Boston, MA 02111-1307, USA *
21
***************************************************************************/
24
#include "PreCompiled.h"
36
Matrix4D::Matrix4D (void)
41
Matrix4D::Matrix4D (float a11, float a12, float a13, float a14,
42
float a21, float a22, float a23, float a24,
43
float a31, float a32, float a33, float a34,
44
float a41, float a42, float a43, float a44 )
46
dMtrx4D[0][0] = a11; dMtrx4D[0][1] = a12; dMtrx4D[0][2] = a13; dMtrx4D[0][3] = a14;
47
dMtrx4D[1][0] = a21; dMtrx4D[1][1] = a22; dMtrx4D[1][2] = a23; dMtrx4D[1][3] = a24;
48
dMtrx4D[2][0] = a31; dMtrx4D[2][1] = a32; dMtrx4D[2][2] = a33; dMtrx4D[2][3] = a34;
49
dMtrx4D[3][0] = a41; dMtrx4D[3][1] = a42; dMtrx4D[3][2] = a43; dMtrx4D[3][3] = a44;
52
Matrix4D::Matrix4D (double a11, double a12, double a13, double a14,
53
double a21, double a22, double a23, double a24,
54
double a31, double a32, double a33, double a34,
55
double a41, double a42, double a43, double a44 )
57
dMtrx4D[0][0] = a11; dMtrx4D[0][1] = a12; dMtrx4D[0][2] = a13; dMtrx4D[0][3] = a14;
58
dMtrx4D[1][0] = a21; dMtrx4D[1][1] = a22; dMtrx4D[1][2] = a23; dMtrx4D[1][3] = a24;
59
dMtrx4D[2][0] = a31; dMtrx4D[2][1] = a32; dMtrx4D[2][2] = a33; dMtrx4D[2][3] = a34;
60
dMtrx4D[3][0] = a41; dMtrx4D[3][1] = a42; dMtrx4D[3][2] = a43; dMtrx4D[3][3] = a44;
64
Matrix4D::Matrix4D (const Matrix4D& rclMtrx)
69
Matrix4D::Matrix4D (const Vector3f& rclBase, const Vector3f& rclDir, float fAngle)
72
this->rotLine(rclBase,rclDir,fAngle);
75
void Matrix4D::setToUnity (void)
77
dMtrx4D[0][0] = 1.0; dMtrx4D[0][1] = 0.0; dMtrx4D[0][2] = 0.0; dMtrx4D[0][3] = 0.0;
78
dMtrx4D[1][0] = 0.0; dMtrx4D[1][1] = 1.0; dMtrx4D[1][2] = 0.0; dMtrx4D[1][3] = 0.0;
79
dMtrx4D[2][0] = 0.0; dMtrx4D[2][1] = 0.0; dMtrx4D[2][2] = 1.0; dMtrx4D[2][3] = 0.0;
80
dMtrx4D[3][0] = 0.0; dMtrx4D[3][1] = 0.0; dMtrx4D[3][2] = 0.0; dMtrx4D[3][3] = 1.0;
83
double Matrix4D::determinant() const
85
double fA0 = dMtrx4D[0][0]*dMtrx4D[1][1] - dMtrx4D[0][1]*dMtrx4D[1][0];
86
double fA1 = dMtrx4D[0][0]*dMtrx4D[1][2] - dMtrx4D[0][2]*dMtrx4D[1][0];
87
double fA2 = dMtrx4D[0][0]*dMtrx4D[1][3] - dMtrx4D[0][3]*dMtrx4D[1][0];
88
double fA3 = dMtrx4D[0][1]*dMtrx4D[1][2] - dMtrx4D[0][2]*dMtrx4D[1][1];
89
double fA4 = dMtrx4D[0][1]*dMtrx4D[1][3] - dMtrx4D[0][3]*dMtrx4D[1][1];
90
double fA5 = dMtrx4D[0][2]*dMtrx4D[1][3] - dMtrx4D[0][3]*dMtrx4D[1][2];
91
double fB0 = dMtrx4D[2][0]*dMtrx4D[3][1] - dMtrx4D[2][1]*dMtrx4D[3][0];
92
double fB1 = dMtrx4D[2][0]*dMtrx4D[3][2] - dMtrx4D[2][2]*dMtrx4D[3][0];
93
double fB2 = dMtrx4D[2][0]*dMtrx4D[3][3] - dMtrx4D[2][3]*dMtrx4D[3][0];
94
double fB3 = dMtrx4D[2][1]*dMtrx4D[3][2] - dMtrx4D[2][2]*dMtrx4D[3][1];
95
double fB4 = dMtrx4D[2][1]*dMtrx4D[3][3] - dMtrx4D[2][3]*dMtrx4D[3][1];
96
double fB5 = dMtrx4D[2][2]*dMtrx4D[3][3] - dMtrx4D[2][3]*dMtrx4D[3][2];
97
double fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
102
void Matrix4D::setMoveX (float fMove)
106
clMat.dMtrx4D[0][3] = fMove;
110
void Matrix4D::setMoveY (float fMove)
114
clMat.dMtrx4D[1][3] = fMove;
118
void Matrix4D::setMoveZ (float fMove)
122
clMat.dMtrx4D[2][3] = fMove;
126
void Matrix4D::move (const Vector3f& rclVct)
130
clMat.dMtrx4D[0][3] = rclVct.x;
131
clMat.dMtrx4D[1][3] = rclVct.y;
132
clMat.dMtrx4D[2][3] = rclVct.z;
135
void Matrix4D::move (const Vector3d& rclVct)
139
clMat.dMtrx4D[0][3] = rclVct.x;
140
clMat.dMtrx4D[1][3] = rclVct.y;
141
clMat.dMtrx4D[2][3] = rclVct.z;
145
void Matrix4D::setScaleX (float fScale)
149
clMat.dMtrx4D[0][0] = fScale;
154
void Matrix4D::setScaleY (float fScale)
158
clMat.dMtrx4D[1][1] = fScale;
162
void Matrix4D::setScaleZ (float fScale)
166
clMat.dMtrx4D[2][2] = fScale;
171
void Matrix4D::scale (const Vector3f& rclVct)
175
clMat.dMtrx4D[0][0] = rclVct.x;
176
clMat.dMtrx4D[1][1] = rclVct.y;
177
clMat.dMtrx4D[2][2] = rclVct.z;
180
void Matrix4D::scale (const Vector3d& rclVct)
184
clMat.dMtrx4D[0][0] = rclVct.x;
185
clMat.dMtrx4D[1][1] = rclVct.y;
186
clMat.dMtrx4D[2][2] = rclVct.z;
190
void Matrix4D::rotX (double fAngle)
197
clMat.dMtrx4D[1][1] = fcos; clMat.dMtrx4D[2][2] = fcos;
198
clMat.dMtrx4D[1][2] = -fsin; clMat.dMtrx4D[2][1] = fsin;
203
void Matrix4D::rotY (double fAngle)
210
clMat.dMtrx4D[0][0] = fcos; clMat.dMtrx4D[2][2] = fcos;
211
clMat.dMtrx4D[2][0] = -fsin; clMat.dMtrx4D[0][2] = fsin;
216
void Matrix4D::rotZ (double fAngle)
223
clMat.dMtrx4D[0][0] = fcos; clMat.dMtrx4D[1][1] = fcos;
224
clMat.dMtrx4D[0][1] = -fsin; clMat.dMtrx4D[1][0] = fsin;
229
void Matrix4D::rotLine (const Vector3d& rclVct, double fAngle)
231
// **** algorithm was taken from a math book
232
Matrix4D clMA, clMB, clMC, clMRot;
233
Vector3d clRotAxis(rclVct);
237
// set all entries to "0"
238
for (iz = 0; iz < 4; iz++)
239
for (is = 0; is < 4; is++) {
240
clMA.dMtrx4D[iz][is] = 0;
241
clMB.dMtrx4D[iz][is] = 0;
242
clMC.dMtrx4D[iz][is] = 0;
245
// ** normalize the rotation axis
246
clRotAxis.Normalize();
248
// ** set the rotation matrix (formula taken from a math book) */
252
clMA.dMtrx4D[0][0] = (1-fcos) * clRotAxis.x * clRotAxis.x;
253
clMA.dMtrx4D[0][1] = (1-fcos) * clRotAxis.x * clRotAxis.y;
254
clMA.dMtrx4D[0][2] = (1-fcos) * clRotAxis.x * clRotAxis.z;
255
clMA.dMtrx4D[1][0] = (1-fcos) * clRotAxis.x * clRotAxis.y;
256
clMA.dMtrx4D[1][1] = (1-fcos) * clRotAxis.y * clRotAxis.y;
257
clMA.dMtrx4D[1][2] = (1-fcos) * clRotAxis.y * clRotAxis.z;
258
clMA.dMtrx4D[2][0] = (1-fcos) * clRotAxis.x * clRotAxis.z;
259
clMA.dMtrx4D[2][1] = (1-fcos) * clRotAxis.y * clRotAxis.z;
260
clMA.dMtrx4D[2][2] = (1-fcos) * clRotAxis.z * clRotAxis.z;
262
clMB.dMtrx4D[0][0] = fcos;
263
clMB.dMtrx4D[1][1] = fcos;
264
clMB.dMtrx4D[2][2] = fcos;
266
clMC.dMtrx4D[0][1] = -fsin * clRotAxis.z;
267
clMC.dMtrx4D[0][2] = fsin * clRotAxis.y;
268
clMC.dMtrx4D[1][0] = fsin * clRotAxis.z;
269
clMC.dMtrx4D[1][2] = -fsin * clRotAxis.x;
270
clMC.dMtrx4D[2][0] = -fsin * clRotAxis.y;
271
clMC.dMtrx4D[2][1] = fsin * clRotAxis.x;
273
for (iz = 0; iz < 3; iz++)
274
for (is = 0; is < 3; is++)
275
clMRot.dMtrx4D[iz][is] = clMA.dMtrx4D[iz][is] + clMB.dMtrx4D[iz][is] +
276
clMC.dMtrx4D[iz][is];
280
void Matrix4D::rotLine (const Vector3f& rclVct, float fAngle)
282
Vector3d tmp(rclVct.x,rclVct.y,rclVct.z);
286
void Matrix4D::rotLine(const Vector3d& rclBase, const Vector3d& rclDir, double fAngle)
288
Matrix4D clMT, clMRot, clMInvT, clM;
289
Vector3d clBase(rclBase);
291
clMT.move(clBase); // Translation
292
clMInvT.move(clBase *= (-1.0f)); // inverse Translation
293
clMRot.rotLine(rclDir, fAngle);
295
clM = clMRot * clMInvT;
300
void Matrix4D::rotLine (const Vector3f& rclBase, const Vector3f& rclDir, float fAngle)
302
Matrix4D clMT, clMRot, clMInvT, clM;
303
Vector3f clBase(rclBase);
305
clMT.move(clBase); // Translation
306
clMInvT.move(clBase *= (-1.0f)); // inverse Translation
307
clMRot.rotLine(rclDir, fAngle);
309
clM = clMRot * clMInvT;
315
* If this matrix describes a rotation around an arbitrary axis with a translation (in axis direction)
316
* then the base point of the axis, its direction, the rotation angle and the translation part get calculated.
317
* In this case the return value is set to true, if this matrix doesn't describe a rotation false is returned.
319
* The translation vector can be calculated with \a fTranslation * \a rclDir, whereas the length of \a rclDir
320
* is normalized to 1.
322
* Note: In case the \a fTranslation part is zero then passing \a rclBase, \a rclDir and \a rfAngle to a new
323
* matrix object creates an identical matrix.
325
bool Matrix4D::toAxisAngle (Vector3f& rclBase, Vector3f& rclDir, float& rfAngle, float& fTranslation) const
327
// First check if the 3x3 submatrix is orthogonal
328
for ( int i=0; i<3; i++ ) {
329
// length must be one
330
if ( fabs(dMtrx4D[0][i]*dMtrx4D[0][i]+dMtrx4D[1][i]*dMtrx4D[1][i]+dMtrx4D[2][i]*dMtrx4D[2][i]-1.0) > 0.01 )
332
// scalar product with other rows must be zero
333
if ( fabs(dMtrx4D[0][i]*dMtrx4D[0][(i+1)%3]+dMtrx4D[1][i]*dMtrx4D[1][(i+1)%3]+dMtrx4D[2][i]*dMtrx4D[2][(i+1)%3]) > 0.01 )
337
// Okay, the 3x3 matrix is orthogonal.
338
// Note: The section to get the rotation axis and angle was taken from WildMagic Library.
340
// Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
341
// The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
342
// I is the identity and
350
// If A > 0, R represents a counterclockwise rotation about the axis in
351
// the sense of looking from the tip of the axis vector towards the
352
// origin. Some algebra will show that
354
// cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P
356
// In the event that A = pi, R-R^t = 0 which prevents us from extracting
357
// the axis through P. Instead note that R = I+2*P^2 when A = pi, so
358
// P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and
359
// z^2-1. We can solve these for axis (x,y,z). Because the angle is pi,
360
// it does not matter which sign you choose on the square roots.
362
// For more details see also http://www.math.niu.edu/~rusin/known-math/97/rotations
364
double fTrace = dMtrx4D[0][0] + dMtrx4D[1][1] + dMtrx4D[2][2];
365
double fCos = 0.5*(fTrace-1.0);
366
rfAngle = (float)acos(fCos); // in [0,PI]
368
if ( rfAngle > 0.0f )
370
if ( rfAngle < F_PI )
372
rclDir.x = (float)(dMtrx4D[2][1]-dMtrx4D[1][2]);
373
rclDir.y = (float)(dMtrx4D[0][2]-dMtrx4D[2][0]);
374
rclDir.z = (float)(dMtrx4D[1][0]-dMtrx4D[0][1]);
381
if ( dMtrx4D[0][0] >= dMtrx4D[1][1] )
384
if ( dMtrx4D[0][0] >= dMtrx4D[2][2] )
386
// r00 is maximum diagonal term
387
rclDir.x = (float)(0.5*sqrt(dMtrx4D[0][0] - dMtrx4D[1][1] - dMtrx4D[2][2] + 1.0));
388
fHalfInverse = 0.5/rclDir.x;
389
rclDir.y = (float)(fHalfInverse*dMtrx4D[0][1]);
390
rclDir.z = (float)(fHalfInverse*dMtrx4D[0][2]);
394
// r22 is maximum diagonal term
395
rclDir.z = (float)(0.5*sqrt(dMtrx4D[2][2] - dMtrx4D[0][0] - dMtrx4D[1][1] + 1.0));
396
fHalfInverse = 0.5/rclDir.z;
397
rclDir.x = (float)(fHalfInverse*dMtrx4D[0][2]);
398
rclDir.y = (float)(fHalfInverse*dMtrx4D[1][2]);
404
if ( dMtrx4D[1][1] >= dMtrx4D[2][2] )
406
// r11 is maximum diagonal term
407
rclDir.y = (float)(0.5*sqrt(dMtrx4D[1][1] - dMtrx4D[0][0] - dMtrx4D[2][2] + 1.0));
408
fHalfInverse = 0.5/rclDir.y;
409
rclDir.x = (float)(fHalfInverse*dMtrx4D[0][1]);
410
rclDir.z = (float)(fHalfInverse*dMtrx4D[1][2]);
414
// r22 is maximum diagonal term
415
rclDir.z = (float)(0.5*sqrt(dMtrx4D[2][2] - dMtrx4D[0][0] - dMtrx4D[1][1] + 1.0));
416
fHalfInverse = 0.5/rclDir.z;
417
rclDir.x = (float)(fHalfInverse*dMtrx4D[0][2]);
418
rclDir.y = (float)(fHalfInverse*dMtrx4D[1][2]);
425
// The angle is 0 and the matrix is the identity. Any axis will
426
// work, so just use the x-axis.
435
// This is the translation part in axis direction
436
fTranslation = (float)(dMtrx4D[0][3]*rclDir.x+dMtrx4D[1][3]*rclDir.y+dMtrx4D[2][3]*rclDir.z);
437
Vector3f cPnt((float)dMtrx4D[0][3],(float)dMtrx4D[1][3],(float)dMtrx4D[2][3]);
438
cPnt = cPnt - fTranslation * rclDir;
440
// This is the base point of the rotation axis
441
if ( rfAngle > 0.0f )
443
double factor = 0.5*(1.0+fTrace)/sin(rfAngle);
444
rclBase.x = (float)(0.5*(cPnt.x+factor*(rclDir.y*cPnt.z-rclDir.z*cPnt.y)));
445
rclBase.y = (float)(0.5*(cPnt.y+factor*(rclDir.z*cPnt.x-rclDir.x*cPnt.z)));
446
rclBase.z = (float)(0.5*(cPnt.z+factor*(rclDir.x*cPnt.y-rclDir.y*cPnt.x)));
452
void Matrix4D::transform (const Vector3f& rclVct, const Matrix4D& rclMtrx)
458
void Matrix4D::transform (const Vector3d& rclVct, const Matrix4D& rclMtrx)
465
void Matrix4D::inverse (void)
467
Matrix4D clInvTrlMat, clInvRotMat;
470
/**** Herausnehmen und Inversion der TranslationsMatrix
471
aus der TransformationMatrix ****/
472
for (iz = 0 ;iz < 3; iz++)
473
clInvTrlMat.dMtrx4D[iz][3] = -dMtrx4D[iz][3];
475
/**** Herausnehmen und Inversion der RotationsMatrix
476
aus der TransformationMatrix ****/
477
for (iz = 0 ;iz < 3; iz++)
478
for (is = 0 ;is < 3; is++)
479
clInvRotMat.dMtrx4D[iz][is] = dMtrx4D[is][iz];
481
/**** inv(M) = inv(Mtrl * Mrot) = inv(Mrot) * inv(Mtrl) ****/
482
(*this) = clInvRotMat * clInvTrlMat;
485
typedef double * Matrix;
487
void Matrix_gauss(Matrix a, Matrix b)
489
int ipiv[4], indxr[4], indxc[4];
494
for (j = 0; j < 4; j++)
496
for (i = 0; i < 4; i++) {
498
for (j = 0; j < 4; j++) {
500
for (k = 0; k < 4; k++) {
502
if (fabs(a[4*j+k]) >= big) {
503
big = fabs(a[4*j+k]);
507
} else if (ipiv[k] > 1)
508
return; /* Singular matrix */
512
ipiv[icol] = ipiv[icol]+1;
514
for (l = 0; l < 4; l++) {
516
a[4*irow+l] = a[4*icol+l];
519
for (l = 0; l < 4; l++) {
521
b[4*irow+l] = b[4*icol+l];
527
if (a[4*icol+icol] == 0) return;
528
pivinv = 1.0/a[4*icol+icol];
529
a[4*icol+icol] = 1.0;
530
for (l = 0; l < 4; l++)
531
a[4*icol+l] = a[4*icol+l]*pivinv;
532
for (l = 0; l < 4; l++)
533
b[4*icol+l] = b[4*icol+l]*pivinv;
534
for (ll = 0; ll < 4; ll++) {
538
for (l = 0; l < 4; l++)
539
a[4*ll+l] = a[4*ll+l] - a[4*icol+l]*dum;
540
for (l = 0; l < 4; l++)
541
b[4*ll+l] = b[4*ll+l] - b[4*icol+l]*dum;
545
for (l = 3; l >= 0; l--) {
546
if (indxr[l] != indxc[l]) {
547
for (k = 0; k < 4; k++) {
548
dum = a[4*k+indxr[l]];
549
a[4*k+indxr[l]] = a[4*k+indxc[l]];
550
a[4*k+indxc[l]] = dum;
555
/* ------------------------------------------------------------------------
556
Matrix_identity(Matrix a)
558
Puts an identity matrix in matrix a
559
------------------------------------------------------------------------ */
561
void Matrix_identity (Matrix a)
564
for (i = 0; i < 16; i++) a[i] = 0;
571
/* ------------------------------------------------------------------------
572
Matrix_invert(Matrix a, Matrix inva)
574
Inverts Matrix a and places the result in inva.
575
Relies on the Gaussian Elimination code above. (See Numerical recipes).
576
------------------------------------------------------------------------ */
577
void Matrix_invert (Matrix a, Matrix inva)
583
for (i = 0; i < 16; i++)
585
Matrix_identity(inva);
586
Matrix_gauss(temp,inva);
589
void Matrix4D::inverseGauss (void)
592
double inversematrix [16] = { 1 ,0 ,0 ,0 ,
598
// Matrix_invert(matrix, inversematrix);
599
Matrix_gauss(matrix,inversematrix);
601
setGLMatrix(inversematrix);
604
void Matrix4D::getGLMatrix (double dMtrx[16]) const
608
for (iz = 0; iz < 4; iz++)
609
for (is = 0; is < 4; is++)
610
dMtrx[ iz + 4*is ] = dMtrx4D[iz][is];
613
void Matrix4D::setGLMatrix (const double dMtrx[16])
617
for (iz = 0; iz < 4; iz++)
618
for (is = 0; is < 4; is++)
619
dMtrx4D[iz][is] = dMtrx[ iz + 4*is ];
622
unsigned long Matrix4D::getMemSpace (void)
624
return sizeof(Matrix4D);
627
void Matrix4D::Print (void) const
631
for (i = 0; i < 4; i++)
632
printf("%9.3f %9.3f %9.3f %9.3f\n", dMtrx4D[i][0], dMtrx4D[i][1], dMtrx4D[i][2], dMtrx4D[i][3]);
635
void Matrix4D::transpose (void)
639
for (int i = 0; i < 4; i++)
641
for (int j = 0; j < 4; j++)
642
dNew[j][i] = dMtrx4D[i][j];
645
memcpy(dMtrx4D, dNew, sizeof(dMtrx4D));
650
// write the 12 double of the matrix in a stream
651
std::string Matrix4D::toString(void) const
653
std::stringstream str;
654
for (int i = 0; i < 4; i++)
656
for (int j = 0; j < 4; j++)
657
str << dMtrx4D[i][j] << " ";
663
// read the 12 double of the matrix from a stream
664
void Matrix4D::fromString(const std::string &str)
666
std::stringstream input;
669
for (int i = 0; i < 4; i++)
671
for (int j = 0; j < 4; j++)
672
input >> dMtrx4D[i][j];
added
removed
src/Base/Matrix.cpp
