FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2005 Imetric 3D GmbH                                    *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
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.      *
10
 *                                                                         *
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.                  *
15
 *                                                                         *
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                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23

24
#include "PreCompiled.h"
25
#ifndef _PreComp_
26
#include <cstring>
27
#include <sstream>
28
#endif
29

30
#include "Matrix.h"
31
#include "Converter.h"
32

33

34
using namespace Base;
35

36
// clang-format off
37
Matrix4D::Matrix4D()
38
    : dMtrx4D {{1., 0., 0., 0.},
39
               {0., 1., 0., 0.},
40
               {0., 0., 1., 0.},
41
               {0., 0., 0., 1.}}
42
{}
43

44
Matrix4D::Matrix4D(float a11, float a12, float a13, float a14,
45
                   float a21, float a22, float a23, float a24,
46
                   float a31, float a32, float a33, float a34,
47
                   float a41, float a42, float a43, float a44)
48
    : dMtrx4D {{a11, a12, a13, a14},
49
               {a21, a22, a23, a24},
50
               {a31, a32, a33, a34},
51
               {a41, a42, a43, a44}}
52
{}
53

54
Matrix4D::Matrix4D(double a11, double a12, double a13, double a14,
55
                   double a21, double a22, double a23, double a24,
56
                   double a31, double a32, double a33, double a34,
57
                   double a41, double a42, double a43, double a44)
58
    : dMtrx4D {{a11, a12, a13, a14},
59
               {a21, a22, a23, a24},
60
               {a31, a32, a33, a34},
61
               {a41, a42, a43, a44}}
62
{}
63
// clang-format on
64

65
Matrix4D::Matrix4D(const Matrix4D& mat)
66
    : Matrix4D()
67
{
68
    (*this) = mat;
69
}
70

71
Matrix4D::Matrix4D(const Vector3f& rclBase, const Vector3f& rclDir, float fAngle)
72
    : Matrix4D()
73
{
74
    this->rotLine(rclBase, rclDir, fAngle);
75
}
76

77
Matrix4D::Matrix4D(const Vector3d& rclBase, const Vector3d& rclDir, double fAngle)
78
    : Matrix4D()
79
{
80
    this->rotLine(rclBase, rclDir, fAngle);
81
}
82

83
void Matrix4D::setToUnity()
84
{
85
    dMtrx4D[0][0] = 1.0;
86
    dMtrx4D[0][1] = 0.0;
87
    dMtrx4D[0][2] = 0.0;
88
    dMtrx4D[0][3] = 0.0;
89
    dMtrx4D[1][0] = 0.0;
90
    dMtrx4D[1][1] = 1.0;
91
    dMtrx4D[1][2] = 0.0;
92
    dMtrx4D[1][3] = 0.0;
93
    dMtrx4D[2][0] = 0.0;
94
    dMtrx4D[2][1] = 0.0;
95
    dMtrx4D[2][2] = 1.0;
96
    dMtrx4D[2][3] = 0.0;
97
    dMtrx4D[3][0] = 0.0;
98
    dMtrx4D[3][1] = 0.0;
99
    dMtrx4D[3][2] = 0.0;
100
    dMtrx4D[3][3] = 1.0;
101
}
102

103
bool Matrix4D::isUnity() const
104
{
105
    return isUnity(0.0);
106
}
107

108
bool Matrix4D::isUnity(double tol) const
109
{
110
    for (int i = 0; i < 4; i++) {
111
        for (int j = 0; j < 4; j++) {
112
            if (i == j) {
113
                if (fabs(dMtrx4D[i][j] - 1.0) > tol) {
114
                    return false;
115
                }
116
            }
117
            else {
118
                if (fabs(dMtrx4D[i][j]) > tol) {
119
                    return false;
120
                }
121
            }
122
        }
123
    }
124

125
    return true;
126
}
127

128
void Matrix4D::nullify()
129
{
130
    dMtrx4D[0][0] = 0.0;
131
    dMtrx4D[0][1] = 0.0;
132
    dMtrx4D[0][2] = 0.0;
133
    dMtrx4D[0][3] = 0.0;
134
    dMtrx4D[1][0] = 0.0;
135
    dMtrx4D[1][1] = 0.0;
136
    dMtrx4D[1][2] = 0.0;
137
    dMtrx4D[1][3] = 0.0;
138
    dMtrx4D[2][0] = 0.0;
139
    dMtrx4D[2][1] = 0.0;
140
    dMtrx4D[2][2] = 0.0;
141
    dMtrx4D[2][3] = 0.0;
142
    dMtrx4D[3][0] = 0.0;
143
    dMtrx4D[3][1] = 0.0;
144
    dMtrx4D[3][2] = 0.0;
145
    dMtrx4D[3][3] = 0.0;
146
}
147

148
bool Matrix4D::isNull() const
149
{
150
    // NOLINTBEGIN
151
    for (int i = 0; i < 4; i++) {
152
        for (int j = 0; j < 4; j++) {
153
            if (dMtrx4D[i][j] != 0.0) {
154
                return false;
155
            }
156
        }
157
    }
158
    // NOLINTEND
159

160
    return true;
161
}
162

163
double Matrix4D::determinant() const
164
{
165
    double fA0 = dMtrx4D[0][0] * dMtrx4D[1][1] - dMtrx4D[0][1] * dMtrx4D[1][0];
166
    double fA1 = dMtrx4D[0][0] * dMtrx4D[1][2] - dMtrx4D[0][2] * dMtrx4D[1][0];
167
    double fA2 = dMtrx4D[0][0] * dMtrx4D[1][3] - dMtrx4D[0][3] * dMtrx4D[1][0];
168
    double fA3 = dMtrx4D[0][1] * dMtrx4D[1][2] - dMtrx4D[0][2] * dMtrx4D[1][1];
169
    double fA4 = dMtrx4D[0][1] * dMtrx4D[1][3] - dMtrx4D[0][3] * dMtrx4D[1][1];
170
    double fA5 = dMtrx4D[0][2] * dMtrx4D[1][3] - dMtrx4D[0][3] * dMtrx4D[1][2];
171
    double fB0 = dMtrx4D[2][0] * dMtrx4D[3][1] - dMtrx4D[2][1] * dMtrx4D[3][0];
172
    double fB1 = dMtrx4D[2][0] * dMtrx4D[3][2] - dMtrx4D[2][2] * dMtrx4D[3][0];
173
    double fB2 = dMtrx4D[2][0] * dMtrx4D[3][3] - dMtrx4D[2][3] * dMtrx4D[3][0];
174
    double fB3 = dMtrx4D[2][1] * dMtrx4D[3][2] - dMtrx4D[2][2] * dMtrx4D[3][1];
175
    double fB4 = dMtrx4D[2][1] * dMtrx4D[3][3] - dMtrx4D[2][3] * dMtrx4D[3][1];
176
    double fB5 = dMtrx4D[2][2] * dMtrx4D[3][3] - dMtrx4D[2][3] * dMtrx4D[3][2];
177
    double fDet = fA0 * fB5 - fA1 * fB4 + fA2 * fB3 + fA3 * fB2 - fA4 * fB1 + fA5 * fB0;
178
    return fDet;
179
}
180

181
double Matrix4D::determinant3() const
182
{
183
    double va = dMtrx4D[0][0] * dMtrx4D[1][1] * dMtrx4D[2][2];
184
    double vb = dMtrx4D[0][1] * dMtrx4D[1][2] * dMtrx4D[2][0];
185
    double vc = dMtrx4D[1][0] * dMtrx4D[2][1] * dMtrx4D[0][2];
186
    double vd = dMtrx4D[0][2] * dMtrx4D[1][1] * dMtrx4D[2][0];
187
    double ve = dMtrx4D[1][0] * dMtrx4D[0][1] * dMtrx4D[2][2];
188
    double vf = dMtrx4D[0][0] * dMtrx4D[2][1] * dMtrx4D[1][2];
189
    double det = (va + vb + vc) - (vd + ve + vf);
190
    return det;
191
}
192

193
void Matrix4D::move(const Vector3f& vec)
194
{
195
    move(convertTo<Vector3d>(vec));
196
}
197

198
void Matrix4D::move(const Vector3d& vec)
199
{
200
    dMtrx4D[0][3] += vec.x;
201
    dMtrx4D[1][3] += vec.y;
202
    dMtrx4D[2][3] += vec.z;
203
}
204

205
void Matrix4D::scale(const Vector3f& vec)
206
{
207
    scale(convertTo<Vector3d>(vec));
208
}
209

210
void Matrix4D::scale(const Vector3d& vec)
211
{
212
    Matrix4D clMat;
213

214
    clMat.dMtrx4D[0][0] = vec.x;
215
    clMat.dMtrx4D[1][1] = vec.y;
216
    clMat.dMtrx4D[2][2] = vec.z;
217
    (*this) = clMat * (*this);
218
}
219

220
void Matrix4D::rotX(double fAngle)
221
{
222
    Matrix4D clMat;
223
    double fsin {};
224
    double fcos {};
225

226
    fsin = sin(fAngle);
227
    fcos = cos(fAngle);
228
    clMat.dMtrx4D[1][1] = fcos;
229
    clMat.dMtrx4D[2][2] = fcos;
230
    clMat.dMtrx4D[1][2] = -fsin;
231
    clMat.dMtrx4D[2][1] = fsin;
232

233
    (*this) = clMat * (*this);
234
}
235

236
void Matrix4D::rotY(double fAngle)
237
{
238
    Matrix4D clMat;
239
    double fsin {};
240
    double fcos {};
241

242
    fsin = sin(fAngle);
243
    fcos = cos(fAngle);
244
    clMat.dMtrx4D[0][0] = fcos;
245
    clMat.dMtrx4D[2][2] = fcos;
246
    clMat.dMtrx4D[2][0] = -fsin;
247
    clMat.dMtrx4D[0][2] = fsin;
248

249
    (*this) = clMat * (*this);
250
}
251

252
void Matrix4D::rotZ(double fAngle)
253
{
254
    Matrix4D clMat;
255
    double fsin {};
256
    double fcos {};
257

258
    fsin = sin(fAngle);
259
    fcos = cos(fAngle);
260
    clMat.dMtrx4D[0][0] = fcos;
261
    clMat.dMtrx4D[1][1] = fcos;
262
    clMat.dMtrx4D[0][1] = -fsin;
263
    clMat.dMtrx4D[1][0] = fsin;
264

265
    (*this) = clMat * (*this);
266
}
267

268
void Matrix4D::rotLine(const Vector3d& vec, double fAngle)
269
{
270
    // **** algorithm was taken from a math book
271
    Matrix4D clMA;
272
    Matrix4D clMB;
273
    Matrix4D clMC;
274
    Matrix4D clMRot;
275
    Vector3d clRotAxis(vec);
276
    double fcos {};
277
    double fsin {};
278

279
    // set all entries to "0"
280
    for (short iz = 0; iz < 4; iz++) {
281
        for (short is = 0; is < 4; is++) {
282
            clMA.dMtrx4D[iz][is] = 0;
283
            clMB.dMtrx4D[iz][is] = 0;
284
            clMC.dMtrx4D[iz][is] = 0;
285
        }
286
    }
287

288
    // ** normalize the rotation axis
289
    clRotAxis.Normalize();
290

291
    // ** set the rotation matrix (formula taken from a math book) */
292
    fcos = cos(fAngle);
293
    fsin = sin(fAngle);
294

295
    clMA.dMtrx4D[0][0] = (1 - fcos) * clRotAxis.x * clRotAxis.x;
296
    clMA.dMtrx4D[0][1] = (1 - fcos) * clRotAxis.x * clRotAxis.y;
297
    clMA.dMtrx4D[0][2] = (1 - fcos) * clRotAxis.x * clRotAxis.z;
298
    clMA.dMtrx4D[1][0] = (1 - fcos) * clRotAxis.x * clRotAxis.y;
299
    clMA.dMtrx4D[1][1] = (1 - fcos) * clRotAxis.y * clRotAxis.y;
300
    clMA.dMtrx4D[1][2] = (1 - fcos) * clRotAxis.y * clRotAxis.z;
301
    clMA.dMtrx4D[2][0] = (1 - fcos) * clRotAxis.x * clRotAxis.z;
302
    clMA.dMtrx4D[2][1] = (1 - fcos) * clRotAxis.y * clRotAxis.z;
303
    clMA.dMtrx4D[2][2] = (1 - fcos) * clRotAxis.z * clRotAxis.z;
304

305
    clMB.dMtrx4D[0][0] = fcos;
306
    clMB.dMtrx4D[1][1] = fcos;
307
    clMB.dMtrx4D[2][2] = fcos;
308

309
    clMC.dMtrx4D[0][1] = -fsin * clRotAxis.z;
310
    clMC.dMtrx4D[0][2] = fsin * clRotAxis.y;
311
    clMC.dMtrx4D[1][0] = fsin * clRotAxis.z;
312
    clMC.dMtrx4D[1][2] = -fsin * clRotAxis.x;
313
    clMC.dMtrx4D[2][0] = -fsin * clRotAxis.y;
314
    clMC.dMtrx4D[2][1] = fsin * clRotAxis.x;
315

316
    for (short iz = 0; iz < 3; iz++) {
317
        for (short is = 0; is < 3; is++) {
318
            clMRot.dMtrx4D[iz][is] =
319
                clMA.dMtrx4D[iz][is] + clMB.dMtrx4D[iz][is] + clMC.dMtrx4D[iz][is];
320
        }
321
    }
322

323
    (*this) = clMRot * (*this);
324
}
325

326
void Matrix4D::rotLine(const Vector3f& vec, float fAngle)
327
{
328
    Vector3d tmp = convertTo<Vector3d>(vec);
329
    rotLine(tmp, static_cast<double>(fAngle));
330
}
331

332
void Matrix4D::rotLine(const Vector3d& rclBase, const Vector3d& rclDir, double fAngle)
333
{
334
    Matrix4D clMRot;
335
    clMRot.rotLine(rclDir, fAngle);
336
    transform(rclBase, clMRot);
337
}
338

339
void Matrix4D::rotLine(const Vector3f& rclBase, const Vector3f& rclDir, float fAngle)
340
{
341
    Vector3d pnt = convertTo<Vector3d>(rclBase);
342
    Vector3d dir = convertTo<Vector3d>(rclDir);
343
    rotLine(pnt, dir, static_cast<double>(fAngle));
344
}
345

346
/**
347
 * If this matrix describes a rotation around an arbitrary axis with a translation (in axis
348
 * direction) then the base point of the axis, its direction, the rotation angle and the translation
349
 * part get calculated. In this case the return value is set to true, if this matrix doesn't
350
 * describe a rotation false is returned.
351
 *
352
 * The translation vector can be calculated with \a fTranslation * \a rclDir, whereas the length of
353
 * \a rclDir is normalized to 1.
354
 *
355
 * Note: In case the \a fTranslation part is zero then passing \a rclBase, \a rclDir and \a rfAngle
356
 * to a new matrix object creates an identical matrix.
357
 */
358
bool Matrix4D::toAxisAngle(Vector3f& rclBase,
359
                           Vector3f& rclDir,
360
                           float& rfAngle,
361
                           float& fTranslation) const
362
{
363
    Vector3d pnt = convertTo<Vector3d>(rclBase);
364
    Vector3d dir = convertTo<Vector3d>(rclDir);
365
    double dAngle = static_cast<double>(rfAngle);
366
    double dTranslation = static_cast<double>(fTranslation);
367

368
    bool ok = toAxisAngle(pnt, dir, dAngle, dTranslation);
369
    if (ok) {
370
        rclBase = convertTo<Vector3f>(pnt);
371
        rclDir = convertTo<Vector3f>(dir);
372
        rfAngle = static_cast<float>(dAngle);
373
        fTranslation = static_cast<float>(dTranslation);
374
    }
375

376
    return ok;
377
}
378

379
bool Matrix4D::toAxisAngle(Vector3d& rclBase,
380
                           Vector3d& rclDir,
381
                           double& rfAngle,
382
                           double& fTranslation) const
383
{
384
    // First check if the 3x3 submatrix is orthogonal
385
    for (int i = 0; i < 3; i++) {
386
        // length must be one
387
        if (fabs(dMtrx4D[0][i] * dMtrx4D[0][i] + dMtrx4D[1][i] * dMtrx4D[1][i]
388
                 + dMtrx4D[2][i] * dMtrx4D[2][i] - 1.0)
389
            > 0.01) {
390
            return false;
391
        }
392
        // scalar product with other rows must be zero
393
        if (fabs(dMtrx4D[0][i] * dMtrx4D[0][(i + 1) % 3] + dMtrx4D[1][i] * dMtrx4D[1][(i + 1) % 3]
394
                 + dMtrx4D[2][i] * dMtrx4D[2][(i + 1) % 3])
395
            > 0.01) {
396
            return false;
397
        }
398
    }
399

400
    // Okay, the 3x3 matrix is orthogonal.
401
    // Note: The section to get the rotation axis and angle was taken from WildMagic Library.
402
    //
403
    // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
404
    // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
405
    // I is the identity and
406
    //
407
    //       +-        -+
408
    //   P = |  0 -z +y |
409
    //       | +z  0 -x |
410
    //       | -y +x  0 |
411
    //       +-        -+
412
    //
413
    // If A > 0, R represents a counterclockwise rotation about the axis in
414
    // the sense of looking from the tip of the axis vector towards the
415
    // origin.  Some algebra will show that
416
    //
417
    //   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
418
    //
419
    // In the event that A = pi, R-R^t = 0 which prevents us from extracting
420
    // the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
421
    // P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and
422
    // z^2-1.  We can solve these for axis (x,y,z).  Because the angle is pi,
423
    // it does not matter which sign you choose on the square roots.
424
    //
425
    // For more details see also http://www.math.niu.edu/~rusin/known-math/97/rotations
426

427
    double fTrace = dMtrx4D[0][0] + dMtrx4D[1][1] + dMtrx4D[2][2];
428
    double fCos = 0.5 * (fTrace - 1.0);
429
    rfAngle = acos(fCos);  // in [0,PI]
430

431
    if (rfAngle > 0.0) {
432
        if (rfAngle < D_PI) {
433
            rclDir.x = (dMtrx4D[2][1] - dMtrx4D[1][2]);
434
            rclDir.y = (dMtrx4D[0][2] - dMtrx4D[2][0]);
435
            rclDir.z = (dMtrx4D[1][0] - dMtrx4D[0][1]);
436
            rclDir.Normalize();
437
        }
438
        else {
439
            // angle is PI
440
            double fHalfInverse {};
441
            if (dMtrx4D[0][0] >= dMtrx4D[1][1]) {
442
                // r00 >= r11
443
                if (dMtrx4D[0][0] >= dMtrx4D[2][2]) {
444
                    // r00 is maximum diagonal term
445
                    rclDir.x = (0.5 * sqrt(dMtrx4D[0][0] - dMtrx4D[1][1] - dMtrx4D[2][2] + 1.0));
446
                    fHalfInverse = 0.5 / rclDir.x;
447
                    rclDir.y = (fHalfInverse * dMtrx4D[0][1]);
448
                    rclDir.z = (fHalfInverse * dMtrx4D[0][2]);
449
                }
450
                else {
451
                    // r22 is maximum diagonal term
452
                    rclDir.z = (0.5 * sqrt(dMtrx4D[2][2] - dMtrx4D[0][0] - dMtrx4D[1][1] + 1.0));
453
                    fHalfInverse = 0.5 / rclDir.z;
454
                    rclDir.x = (fHalfInverse * dMtrx4D[0][2]);
455
                    rclDir.y = (fHalfInverse * dMtrx4D[1][2]);
456
                }
457
            }
458
            else {
459
                // r11 > r00
460
                if (dMtrx4D[1][1] >= dMtrx4D[2][2]) {
461
                    // r11 is maximum diagonal term
462
                    rclDir.y = (0.5 * sqrt(dMtrx4D[1][1] - dMtrx4D[0][0] - dMtrx4D[2][2] + 1.0));
463
                    fHalfInverse = 0.5 / rclDir.y;
464
                    rclDir.x = (fHalfInverse * dMtrx4D[0][1]);
465
                    rclDir.z = (fHalfInverse * dMtrx4D[1][2]);
466
                }
467
                else {
468
                    // r22 is maximum diagonal term
469
                    rclDir.z = (0.5 * sqrt(dMtrx4D[2][2] - dMtrx4D[0][0] - dMtrx4D[1][1] + 1.0));
470
                    fHalfInverse = 0.5 / rclDir.z;
471
                    rclDir.x = (fHalfInverse * dMtrx4D[0][2]);
472
                    rclDir.y = (fHalfInverse * dMtrx4D[1][2]);
473
                }
474
            }
475
        }
476
    }
477
    else {
478
        // The angle is 0 and the matrix is the identity.  Any axis will
479
        // work, so just use the x-axis.
480
        rclDir.x = 1.0;
481
        rclDir.y = 0.0;
482
        rclDir.z = 0.0;
483
        rclBase.x = 0.0;
484
        rclBase.y = 0.0;
485
        rclBase.z = 0.0;
486
    }
487

488
    // This is the translation part in axis direction
489
    fTranslation = (dMtrx4D[0][3] * rclDir.x + dMtrx4D[1][3] * rclDir.y + dMtrx4D[2][3] * rclDir.z);
490
    Vector3d cPnt(dMtrx4D[0][3], dMtrx4D[1][3], dMtrx4D[2][3]);
491
    cPnt = cPnt - fTranslation * rclDir;
492

493
    // This is the base point of the rotation axis
494
    if (rfAngle > 0.0) {
495
        double factor = 0.5 * (1.0 + fTrace) / sin(rfAngle);
496
        rclBase.x = (0.5 * (cPnt.x + factor * (rclDir.y * cPnt.z - rclDir.z * cPnt.y)));
497
        rclBase.y = (0.5 * (cPnt.y + factor * (rclDir.z * cPnt.x - rclDir.x * cPnt.z)));
498
        rclBase.z = (0.5 * (cPnt.z + factor * (rclDir.x * cPnt.y - rclDir.y * cPnt.x)));
499
    }
500

501
    return true;
502
}
503

504
void Matrix4D::transform(const Vector3f& vec, const Matrix4D& mat)
505
{
506
    move(-vec);
507
    (*this) = mat * (*this);
508
    move(vec);
509
}
510

511
void Matrix4D::transform(const Vector3d& vec, const Matrix4D& mat)
512
{
513
    move(-vec);
514
    (*this) = mat * (*this);
515
    move(vec);
516
}
517

518
void Matrix4D::inverse()
519
{
520
    Matrix4D clInvTrlMat;
521
    Matrix4D clInvRotMat;
522

523
    /**** Herausnehmen und Inversion der TranslationsMatrix
524
    aus der TransformationMatrix                      ****/
525
    for (short iz = 0; iz < 3; iz++) {
526
        clInvTrlMat.dMtrx4D[iz][3] = -dMtrx4D[iz][3];
527
    }
528

529
    /**** Herausnehmen und Inversion der RotationsMatrix
530
    aus der TransformationMatrix                      ****/
531
    for (short iz = 0; iz < 3; iz++) {
532
        for (short is = 0; is < 3; is++) {
533
            clInvRotMat.dMtrx4D[iz][is] = dMtrx4D[is][iz];
534
        }
535
    }
536

537
    /**** inv(M) = inv(Mtrl * Mrot) = inv(Mrot) * inv(Mtrl) ****/
538
    (*this) = clInvRotMat * clInvTrlMat;
539
}
540

541
using Matrix = double*;
542

543
// NOLINTBEGIN
544
void Matrix_gauss(Matrix a, Matrix b)
545
{
546
    int ipiv[4], indxr[4], indxc[4];
547
    int i {}, j {}, k {}, l {}, ll {};
548
    int irow = 0, icol = 0;
549
    double big {}, pivinv {};
550
    double dum {};
551
    for (j = 0; j < 4; j++) {
552
        ipiv[j] = 0;
553
    }
554
    for (i = 0; i < 4; i++) {
555
        big = 0;
556
        for (j = 0; j < 4; j++) {
557
            if (ipiv[j] != 1) {
558
                for (k = 0; k < 4; k++) {
559
                    if (ipiv[k] == 0) {
560
                        if (fabs(a[4 * j + k]) >= big) {
561
                            big = fabs(a[4 * j + k]);
562
                            irow = j;
563
                            icol = k;
564
                        }
565
                    }
566
                    else if (ipiv[k] > 1) {
567
                        return; /* Singular matrix */
568
                    }
569
                }
570
            }
571
        }
572
        ipiv[icol] = ipiv[icol] + 1;
573
        if (irow != icol) {
574
            for (l = 0; l < 4; l++) {
575
                dum = a[4 * irow + l];
576
                a[4 * irow + l] = a[4 * icol + l];
577
                a[4 * icol + l] = dum;
578
            }
579
            for (l = 0; l < 4; l++) {
580
                dum = b[4 * irow + l];
581
                b[4 * irow + l] = b[4 * icol + l];
582
                b[4 * icol + l] = dum;
583
            }
584
        }
585
        indxr[i] = irow;
586
        indxc[i] = icol;
587
        if (a[4 * icol + icol] == 0.0) {
588
            return;
589
        }
590
        pivinv = 1.0 / a[4 * icol + icol];
591
        a[4 * icol + icol] = 1.0;
592
        for (l = 0; l < 4; l++) {
593
            a[4 * icol + l] = a[4 * icol + l] * pivinv;
594
        }
595
        for (l = 0; l < 4; l++) {
596
            b[4 * icol + l] = b[4 * icol + l] * pivinv;
597
        }
598
        for (ll = 0; ll < 4; ll++) {
599
            if (ll != icol) {
600
                dum = a[4 * ll + icol];
601
                a[4 * ll + icol] = 0;
602
                for (l = 0; l < 4; l++) {
603
                    a[4 * ll + l] = a[4 * ll + l] - a[4 * icol + l] * dum;
604
                }
605
                for (l = 0; l < 4; l++) {
606
                    b[4 * ll + l] = b[4 * ll + l] - b[4 * icol + l] * dum;
607
                }
608
            }
609
        }
610
    }
611
    for (l = 3; l >= 0; l--) {
612
        if (indxr[l] != indxc[l]) {
613
            for (k = 0; k < 4; k++) {
614
                dum = a[4 * k + indxr[l]];
615
                a[4 * k + indxr[l]] = a[4 * k + indxc[l]];
616
                a[4 * k + indxc[l]] = dum;
617
            }
618
        }
619
    }
620
}
621
// NOLINTEND
622

623
void Matrix4D::inverseOrthogonal()
624
{
625
    Base::Vector3d vec(dMtrx4D[0][3], dMtrx4D[1][3], dMtrx4D[2][3]);
626
    transpose();
627
    vec = this->operator*(vec);
628
    dMtrx4D[0][3] = -vec.x;
629
    dMtrx4D[3][0] = 0;
630
    dMtrx4D[1][3] = -vec.y;
631
    dMtrx4D[3][1] = 0;
632
    dMtrx4D[2][3] = -vec.z;
633
    dMtrx4D[3][2] = 0;
634
}
635

636
void Matrix4D::inverseGauss()
637
{
638
    double matrix[16];
639
    // clang-format off
640
    double inversematrix[16] = {1, 0, 0, 0,
641
                                0, 1, 0, 0,
642
                                0, 0, 1, 0,
643
                                0, 0, 0, 1};
644
    // clang-format on
645
    getGLMatrix(matrix);
646

647
    Matrix_gauss(matrix, inversematrix);
648

649
    setGLMatrix(inversematrix);
650
}
651

652
void Matrix4D::getMatrix(double dMtrx[16]) const
653
{
654
    for (short iz = 0; iz < 4; iz++) {
655
        for (short is = 0; is < 4; is++) {
656
            dMtrx[4 * iz + is] = dMtrx4D[iz][is];
657
        }
658
    }
659
}
660

661
void Matrix4D::setMatrix(const double dMtrx[16])
662
{
663
    for (short iz = 0; iz < 4; iz++) {
664
        for (short is = 0; is < 4; is++) {
665
            dMtrx4D[iz][is] = dMtrx[4 * iz + is];
666
        }
667
    }
668
}
669

670
void Matrix4D::getGLMatrix(double dMtrx[16]) const
671
{
672
    for (short iz = 0; iz < 4; iz++) {
673
        for (short is = 0; is < 4; is++) {
674
            dMtrx[iz + 4 * is] = dMtrx4D[iz][is];
675
        }
676
    }
677
}
678

679
void Matrix4D::setGLMatrix(const double dMtrx[16])
680
{
681
    for (short iz = 0; iz < 4; iz++) {
682
        for (short is = 0; is < 4; is++) {
683
            dMtrx4D[iz][is] = dMtrx[iz + 4 * is];
684
        }
685
    }
686
}
687

688
unsigned long Matrix4D::getMemSpace()
689
{
690
    return sizeof(Matrix4D);
691
}
692

693
void Matrix4D::Print() const
694
{
695
    // NOLINTBEGIN
696
    for (short i = 0; i < 4; i++) {
697
        printf("%9.3f %9.3f %9.3f %9.3f\n",
698
               dMtrx4D[i][0],
699
               dMtrx4D[i][1],
700
               dMtrx4D[i][2],
701
               dMtrx4D[i][3]);
702
    }
703
    // NOLINTEND
704
}
705

706
void Matrix4D::transpose()
707
{
708
    double dNew[4][4];
709

710
    for (int i = 0; i < 4; i++) {
711
        for (int j = 0; j < 4; j++) {
712
            dNew[j][i] = dMtrx4D[i][j];
713
        }
714
    }
715

716
    memcpy(dMtrx4D, dNew, sizeof(dMtrx4D));
717
}
718

719

720
// write the 12 double of the matrix in a stream
721
std::string Matrix4D::toString() const
722
{
723
    std::stringstream str;
724
    // NOLINTBEGIN
725
    for (int i = 0; i < 4; i++) {
726
        for (int j = 0; j < 4; j++) {
727
            str << dMtrx4D[i][j] << " ";
728
        }
729
    }
730
    // NOLINTEND
731

732
    return str.str();
733
}
734

735
// read the 12 double of the matrix from a stream
736
void Matrix4D::fromString(const std::string& str)
737
{
738
    std::stringstream input;
739
    input.str(str);
740

741
    // NOLINTBEGIN
742
    for (int i = 0; i < 4; i++) {
743
        for (int j = 0; j < 4; j++) {
744
            input >> dMtrx4D[i][j];
745
        }
746
    }
747
    // NOLINTEND
748
}
749

750
// Analyse the a transformation Matrix and describe the transformation
751
std::string Matrix4D::analyse() const
752
{
753
    const double eps = 1.0e-06;
754
    bool hastranslation = (dMtrx4D[0][3] != 0.0 || dMtrx4D[1][3] != 0.0 || dMtrx4D[2][3] != 0.0);
755
    const Base::Matrix4D unityMatrix = Base::Matrix4D();
756
    std::string text;
757
    if (*this == unityMatrix) {
758
        text = "Unity Matrix";
759
    }
760
    else {
761
        if (dMtrx4D[3][0] != 0.0 || dMtrx4D[3][1] != 0.0 || dMtrx4D[3][2] != 0.0
762
            || dMtrx4D[3][3] != 1.0) {
763
            text = "Projection";
764
        }
765
        else  // translation and affine
766
        {
767
            if (dMtrx4D[0][1] == 0.0 && dMtrx4D[0][2] == 0.0 && dMtrx4D[1][0] == 0.0
768
                && dMtrx4D[1][2] == 0.0 && dMtrx4D[2][0] == 0.0 && dMtrx4D[2][1] == 0.0)  // scaling
769
            {
770
                std::ostringstream stringStream;
771
                stringStream << "Scale [" << dMtrx4D[0][0] << ", " << dMtrx4D[1][1] << ", "
772
                             << dMtrx4D[2][2] << "]";
773
                text = stringStream.str();
774
            }
775
            else {
776
                Base::Matrix4D sub;
777
                sub[0][0] = dMtrx4D[0][0];
778
                sub[0][1] = dMtrx4D[0][1];
779
                sub[0][2] = dMtrx4D[0][2];
780
                sub[1][0] = dMtrx4D[1][0];
781
                sub[1][1] = dMtrx4D[1][1];
782
                sub[1][2] = dMtrx4D[1][2];
783
                sub[2][0] = dMtrx4D[2][0];
784
                sub[2][1] = dMtrx4D[2][1];
785
                sub[2][2] = dMtrx4D[2][2];
786

787
                Base::Matrix4D trp = sub;
788
                trp.transpose();
789
                trp = trp * sub;
790
                bool ortho = true;
791
                for (unsigned short i = 0; i < 4 && ortho; i++) {
792
                    for (unsigned short j = 0; j < 4 && ortho; j++) {
793
                        if (i != j) {
794
                            if (fabs(trp[i][j]) > eps) {
795
                                ortho = false;
796
                                break;
797
                            }
798
                        }
799
                    }
800
                }
801

802
                double determinant = sub.determinant();
803
                if (ortho) {
804
                    if (fabs(determinant - 1.0) < eps)  // rotation
805
                    {
806
                        text = "Rotation Matrix";
807
                    }
808
                    else {
809
                        if (fabs(determinant + 1.0) < eps)  // rotation
810
                        {
811
                            text = "Rotinversion Matrix";
812
                        }
813
                        else  // scaling with rotation
814
                        {
815
                            std::ostringstream stringStream;
816
                            stringStream << "Scale and Rotate ";
817
                            if (determinant < 0.0) {
818
                                stringStream << "and Invert ";
819
                            }
820
                            stringStream << "[ " << sqrt(trp[0][0]) << ", " << sqrt(trp[1][1])
821
                                         << ", " << sqrt(trp[2][2]) << "]";
822
                            text = stringStream.str();
823
                        }
824
                    }
825
                }
826
                else {
827
                    std::ostringstream stringStream;
828
                    stringStream << "Affine with det= " << determinant;
829
                    text = stringStream.str();
830
                }
831
            }
832
        }
833
        if (hastranslation) {
834
            text += " with Translation";
835
        }
836
    }
837
    return text;
838
}
839

840
Matrix4D& Matrix4D::Outer(const Vector3f& rV1, const Vector3f& rV2)
841
{
842
    setToUnity();
843

844
    Outer(convertTo<Vector3d>(rV1), convertTo<Vector3d>(rV2));
845

846
    return *this;
847
}
848

849
Matrix4D& Matrix4D::Outer(const Vector3d& rV1, const Vector3d& rV2)
850
{
851
    setToUnity();
852

853
    dMtrx4D[0][0] = rV1.x * rV2.x;
854
    dMtrx4D[0][1] = rV1.x * rV2.y;
855
    dMtrx4D[0][2] = rV1.x * rV2.z;
856

857
    dMtrx4D[1][0] = rV1.y * rV2.x;
858
    dMtrx4D[1][1] = rV1.y * rV2.y;
859
    dMtrx4D[1][2] = rV1.y * rV2.z;
860

861
    dMtrx4D[2][0] = rV1.z * rV2.x;
862
    dMtrx4D[2][1] = rV1.z * rV2.y;
863
    dMtrx4D[2][2] = rV1.z * rV2.z;
864

865
    return *this;
866
}
867

868
Matrix4D& Matrix4D::Hat(const Vector3f& rV)
869
{
870
    setToUnity();
871

872
    Hat(convertTo<Vector3d>(rV));
873

874
    return *this;
875
}
876

877
Matrix4D& Matrix4D::Hat(const Vector3d& rV)
878
{
879
    setToUnity();
880

881
    dMtrx4D[0][0] = 0.0;
882
    dMtrx4D[0][1] = -rV.z;
883
    dMtrx4D[0][2] = rV.y;
884

885
    dMtrx4D[1][0] = rV.z;
886
    dMtrx4D[1][1] = 0.0;
887
    dMtrx4D[1][2] = -rV.x;
888

889
    dMtrx4D[2][0] = -rV.y;
890
    dMtrx4D[2][1] = rV.x;
891
    dMtrx4D[2][2] = 0.0;
892

893
    return *this;
894
}
895

896
ScaleType Matrix4D::hasScale(double tol) const
897
{
898
    const double defaultTolerance = 1e-9;
899
    // check for uniform scaling
900
    //
901
    // For a scaled rotation matrix it matters whether
902
    // the scaling was applied from the left or right side.
903
    // Only in case of uniform scaling it doesn't make a difference.
904
    if (tol == 0.0) {
905
        tol = defaultTolerance;
906
    }
907

908
    // check if the absolute values are proportionally close or equal
909
    auto closeAbs = [&](double val_a, double val_b) {
910
        double abs_a = fabs(val_a);
911
        double abs_b = fabs(val_b);
912
        if (abs_b > abs_a) {
913
            return (abs_b - abs_a) / abs_b <= tol;
914
        }
915
        if (abs_a > abs_b) {
916
            return (abs_a - abs_b) / abs_a <= tol;
917
        }
918
        return true;
919
    };
920

921
    // get column vectors
922
    double dx = getCol(0).Sqr();
923
    double dy = getCol(1).Sqr();
924
    double dz = getCol(2).Sqr();
925
    double dxyz = sqrt(dx * dy * dz);
926

927
    // get row vectors
928
    double du = getRow(0).Sqr();
929
    double dv = getRow(1).Sqr();
930
    double dw = getRow(2).Sqr();
931
    double duvw = sqrt(du * dv * dw);
932

933
    double d3 = determinant3();
934

935
    // This could be e.g. a projection, a shearing,... matrix
936
    if (!closeAbs(dxyz, d3) && !closeAbs(duvw, d3)) {
937
        return ScaleType::Other;
938
    }
939

940
    if (closeAbs(duvw, d3) && (!closeAbs(du, dv) || !closeAbs(dv, dw))) {
941
        return ScaleType::NonUniformLeft;
942
    }
943

944
    if (closeAbs(dxyz, d3) && (!closeAbs(dx, dy) || !closeAbs(dy, dz))) {
945
        return ScaleType::NonUniformRight;
946
    }
947

948
    if (fabs(d3 - 1.0) > tol) {
949
        return ScaleType::Uniform;
950
    }
951

952
    return ScaleType::NoScaling;
953
}
954

955
std::array<Matrix4D, 4> Matrix4D::decompose() const
956
{
957
    // decompose the matrix to shear, scale, rotation and move
958
    // so that matrix = move * rotation * scale * shear
959
    // return an array of matrices
960
    Matrix4D moveMatrix;
961
    Matrix4D rotationMatrix;
962
    Matrix4D scaleMatrix;
963
    Matrix4D residualMatrix(*this);
964
    // extract transform
965
    moveMatrix.move(residualMatrix.getCol(3));
966
    residualMatrix.setCol(3, Vector3d());
967
    // find and extract rotation
968
    int prim_dir = -1;
969
    std::array<Vector3d, 3> dirs = {Vector3d(1., 0., 0.),
970
                                    Vector3d(0., 1., 0.),
971
                                    Vector3d(0., 0., 1.)};
972
    for (int i = 0; i < 3; i++) {
973
        if (residualMatrix.getCol(i).IsNull()) {
974
            continue;
975
        }
976
        if (prim_dir < 0) {
977
            dirs[i] = residualMatrix.getCol(i);
978
            dirs[i].Normalize();
979
            prim_dir = i;
980
            continue;
981
        }
982

983
        Vector3d cross = dirs[prim_dir].Cross(residualMatrix.getCol(i));
984
        if (cross.IsNull()) {
985
            continue;
986
        }
987
        cross.Normalize();
988
        int last_dir = 3 - i - prim_dir;
989
        if (i - prim_dir == 1) {
990
            dirs[last_dir] = cross;
991
            dirs[i] = cross.Cross(dirs[prim_dir]);
992
        }
993
        else {
994
            dirs[last_dir] = -cross;
995
            dirs[i] = dirs[prim_dir].Cross(-cross);
996
        }
997
        prim_dir = -2;  // done
998
        break;
999
    }
1000
    if (prim_dir >= 0) {
1001
        // handle case with only one valid direction
1002
        Vector3d cross = dirs[prim_dir].Cross(Vector3d(0., 0., 1.));
1003
        if (cross.IsNull()) {
1004
            cross = dirs[prim_dir].Cross(Vector3d(0., 1., 0.));
1005
        }
1006
        dirs[(prim_dir + 1) % 3] = cross;
1007
        dirs[(prim_dir + 2) % 3] = dirs[prim_dir].Cross(cross);
1008
    }
1009
    rotationMatrix.setCol(0, dirs[0]);
1010
    rotationMatrix.setCol(1, dirs[1]);
1011
    rotationMatrix.setCol(2, dirs[2]);
1012
    rotationMatrix.inverseGauss();
1013
    residualMatrix = rotationMatrix * residualMatrix;
1014
    // To keep signs of the scale factors equal
1015
    if (residualMatrix.determinant() < 0) {
1016
        rotationMatrix.rotZ(D_PI);
1017
        residualMatrix.rotZ(D_PI);
1018
    }
1019
    rotationMatrix.inverseGauss();
1020
    // extract scale
1021
    double xScale = residualMatrix.dMtrx4D[0][0];
1022
    double yScale = residualMatrix.dMtrx4D[1][1];
1023
    double zScale = residualMatrix.dMtrx4D[2][2];
1024
    scaleMatrix.dMtrx4D[0][0] = xScale;
1025
    scaleMatrix.dMtrx4D[1][1] = yScale;
1026
    scaleMatrix.dMtrx4D[2][2] = zScale;
1027
    // The remaining shear
1028
    residualMatrix.scale(xScale != 0 ? 1.0 / xScale : 1.0,
1029
                         yScale != 0 ? 1.0 / yScale : 1.0,
1030
                         zScale != 0 ? 1.0 / zScale : 1.0);
1031
    // Restore trace in shear matrix
1032
    residualMatrix.setDiagonal(Vector3d(1.0, 1.0, 1.0));
1033
    // Remove values close to zero
1034
    for (int i = 0; i < 3; i++) {
1035
        if (std::abs(scaleMatrix.dMtrx4D[i][i]) < 1e-15) {
1036
            scaleMatrix.dMtrx4D[i][i] = 0.0;
1037
        }
1038
        for (int j = 0; j < 3; j++) {
1039
            if (std::abs(residualMatrix.dMtrx4D[i][j]) < 1e-15) {
1040
                residualMatrix.dMtrx4D[i][j] = 0.0;
1041
            }
1042
            if (std::abs(rotationMatrix.dMtrx4D[i][j]) < 1e-15) {
1043
                rotationMatrix.dMtrx4D[i][j] = 0.0;
1044
            }
1045
        }
1046
    }
1047
    return std::array<Matrix4D, 4> {residualMatrix, scaleMatrix, rotationMatrix, moveMatrix};
1048
}
1049