« back to all changes in this revision
Viewing changes to src/geometry/Matrix.cpp
- browse files at revision 1
- compare with another revision
- download diff
- download tarball
- view history from revision 1
- Committer: Dan Heeks
- Date: 2008-07-17 14:54:00 UTC
- Revision ID: git-v1:5b3ee80cd3c0bee835afb056556f883a80f2b6e0
Initial adding of all the HeeksCNC files
- files added:
- bitmaps
- icons
- src
- src/geometry
- src/messages
added
removed
src/geometry/Matrix.cpp
1
////////////////////////////////////////////////////////////////////////////////////////////////
2
// 3d geometry classes - implements some peps 3d stuff
3
//
4
// g.j.hawkesford August 2003
5
////////////////////////////////////////////////////////////////////////////////////////////////
6
7
#include "geometry.h"
8
9
#ifdef PEPSDLL
10
#include "vdm.h"
11
#include "pepsdll.h"
12
#include "realds.h"
13
#endif
14
////////////////////////////////////////////////////////////////////////////////////////////////
15
// matrix
16
////////////////////////////////////////////////////////////////////////////////////////////////
17
namespace geoff_geometry {
18
19
Matrix::Matrix(){
20
Unit();
21
}
22
Matrix::Matrix(double m[16]) {
23
memcpy(e, m, sizeof(e));
24
this->IsUnit();
25
this->IsMirrored();
26
}
27
Matrix::Matrix( Matrix& m)
28
{
29
*this = m;
30
}
31
32
#if 0
33
const Matrix& Matrix::operator=( Matrix &m) {
34
for(int i = 0; i < 16; i++) e[i] = m.e[i];
35
m_unit = m.m_unit;
36
m_mirrored = m.m_mirrored;
37
return *this;
38
}
39
#endif
40
void Matrix::Unit()
41
{
42
// homogenous matrix - set as unit matrix
43
for( int i = 0; i < 16; i ++ ) e[i] = 0;
44
e[0] = e[5] = e[10] = e[15] = 1;
45
m_unit = true;
46
m_mirrored = false;
47
}
48
49
void Matrix::Get(double* p) const
50
{
51
// copy the matrix
52
for( int i = 0; i < 16; i ++ ) p[i] = e[i];
53
}
54
void Matrix::Put(double* p)
55
{
56
// assign the matrix
57
for( int i = 0; i < 16; i ++ ) e[i] = p[i];
58
m_unit = false; // don't know
59
m_mirrored = -1; // don't know
60
61
}
62
void Matrix::Translate(double x, double y, double z)
63
{
64
// translation
65
e[3] += x;
66
e[7] += y;
67
e[11] += z;
68
m_unit = false;
69
}
70
71
void Matrix::Rotate(double angle, Vector3d *rotAxis) {
72
/// Rotation about rotAxis with angle
73
Rotate(sin(angle), cos(angle), rotAxis);
74
}
75
76
void Matrix::Rotate(double sinang, double cosang, Vector3d *rotAxis) {
77
/// Rotation about rotAxis with cp & dp
78
Matrix rotate;
79
double oneminusc = 1.0 - cosang;
80
81
rotate.e[0] = rotAxis->getx() * rotAxis->getx() * oneminusc + cosang;
82
rotate.e[1] = rotAxis->getx() * rotAxis->gety() * oneminusc - rotAxis->getz() * sinang;
83
rotate.e[2] = rotAxis->getx() * rotAxis->getz() * oneminusc + rotAxis->gety() * sinang;
84
85
rotate.e[4] = rotAxis->getx() * rotAxis->gety() * oneminusc + rotAxis->getz() * sinang;
86
rotate.e[5] = rotAxis->gety() * rotAxis->gety() * oneminusc + cosang;
87
rotate.e[6] = rotAxis->gety() * rotAxis->getz() * oneminusc - rotAxis->getx() * sinang;
88
89
rotate.e[8] = rotAxis->getx() * rotAxis->getz() * oneminusc - rotAxis->gety() * sinang;
90
rotate.e[9] = rotAxis->gety() * rotAxis->getz() * oneminusc + rotAxis->getx() * sinang;
91
rotate.e[10] = rotAxis->getz() * rotAxis->getz() * oneminusc + cosang;
92
Multiply(rotate); // concatinate rotation with this matrix
93
m_unit = false;
94
m_mirrored = -1; // don't know
95
}
96
97
98
void Matrix::Rotate(double angle, int Axis)
99
{ // Rotation (Axis 1 = x , 2 = y , 3 = z
100
Rotate(sin(angle), cos(angle), Axis);
101
}
102
103
void Matrix::Rotate(double sinang, double cosang, int Axis)
104
{ // Rotation (Axis 1 = x , 2 = y , 3 = z
105
Matrix rotate;
106
rotate.Unit();
107
108
switch(Axis)
109
{
110
case 1:
111
// about x axis
112
rotate.e[5] = rotate.e[10] = cosang;
113
rotate.e[6] = -sinang;
114
rotate.e[9] = sinang;
115
break;
116
case 2:
117
// about y axis
118
rotate.e[0] = rotate.e[10] = cosang;
119
rotate.e[2] = sinang;
120
rotate.e[8] = -sinang;
121
break;
122
case 3:
123
// about z axis
124
rotate.e[0] = rotate.e[5] = cosang;
125
rotate.e[1] = -sinang;
126
rotate.e[4] = sinang;
127
break;
128
}
129
Multiply(rotate); // concatinate rotation with this matrix
130
m_unit = false;
131
m_mirrored = -1; // don't know
132
}
133
134
void Matrix::Scale(double scale)
135
{
136
// add a scale
137
Scale(scale, scale, scale);
138
}
139
140
void Matrix::Scale(double scalex, double scaley, double scalez)
141
{
142
// add a scale
143
Matrix temp;
144
temp.Unit();
145
146
temp.e[0] = scalex;
147
temp.e[5] = scaley;
148
temp.e[10] = scalez;
149
Multiply(temp);
150
m_unit = false;
151
m_mirrored = -1; // don't know
152
}
153
void Matrix::Multiply(Matrix& m)
154
{
155
// multiply this by give matrix - concatinate
156
int i, k, l;
157
Matrix ret;
158
159
for (i = 0; i < 16; i++)
160
{
161
l = i - (k = (i % 4));
162
ret.e[i] = m.e[l] * e[k] + m.e[l+1] * e[k+4] + m.e[l+2] * e[k+8] + m.e[l+3] * e[k+12];
163
}
164
*this = ret;
165
}
166
167
void Matrix::Transform(double p0[3], double p1[3]) const
168
{
169
// transform p0 thro' this matrix
170
if(m_unit)
171
memcpy(p1, p0, 3 * sizeof(double));
172
else {
173
p1[0] = p0[0] * e[0] + p0[1] * e[1] + p0[2] * e[2] + e[3];
174
p1[1] = p0[0] * e[4] + p0[1] * e[5] + p0[2] * e[6] + e[7];
175
p1[2] = p0[0] * e[8] + p0[1] * e[9] + p0[2] * e[10] + e[11];
176
}
177
}
178
void Matrix::Transform2d(double p0[2], double p1[2]) const
179
{
180
// transform p0 thro' this matrix (2d only)
181
if(m_unit)
182
memcpy(p1, p0, 2 * sizeof(double));
183
else {
184
p1[0] = p0[0] * e[0] + p0[1] * e[1] + e[3];
185
p1[1] = p0[0] * e[4] + p0[1] * e[5] + e[7];
186
}
187
}
188
189
void Matrix::Transform(double p0[3]) const
190
{
191
double p1[3];
192
if(!m_unit) {
193
Transform(p0, p1);
194
memcpy(p0, p1, 3 * sizeof(double));
195
}
196
}
197
198
int Matrix::IsMirrored()
199
{
200
// returns true if matrix has a mirror
201
if(m_unit)
202
m_mirrored = false;
203
else if(m_mirrored == -1) {
204
205
m_mirrored = ((e[0] * (e[5] * e[10] - e[6] * e[9])
206
- e[1] * (e[4] * e[10] - e[6] * e[8])
207
+ e[2] * (e[4] * e[9] - e[5] * e[8])) < 0);
208
}
209
return m_mirrored;
210
}
211
int Matrix::IsUnit() {
212
// returns true if unit matrix
213
for(int i = 0; i < 16; i++) {
214
if(i == 0 || i == 5 || i == 10 || i == 15) {
215
if(e[i] != 1) return m_unit = false;
216
}
217
else {
218
if(e[i] != 0) return m_unit = false;
219
}
220
}
221
m_mirrored = false;
222
return m_unit = true;
223
}
224
225
void Matrix::GetTranslate(double& x, double& y, double& z) const
226
{
227
// return translation
228
x = e[3];
229
y = e[7];
230
z = e[11];
231
}
232
void Matrix::GetScale(double& sx, double& sy, double& sz) const
233
{
234
// return the scale
235
if(m_unit) {
236
sx = sy = sz = 1;
237
}
238
else {
239
sx = sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);
240
sy = sqrt(e[4] * e[4] + e[5] * e[5] + e[6] * e[6]);
241
sz = sqrt(e[8] * e[8] + e[9] * e[9] + e[10] * e[10]);
242
}
243
}
244
bool Matrix::GetScale(double& sx) const
245
{
246
// return a uniform scale (false if differential)
247
double sy, sz;
248
if(m_unit) {
249
sx = 1;
250
return true;
251
}
252
GetScale(sx, sy, sz);
253
return (fabs(fabs(sx) - fabs(sy)) < 0.000001)?true : false;
254
}
255
void Matrix::GetRotation(double& ax, double& ay, double& az) const
256
{
257
// return the rotations
258
if(m_unit) {
259
ax = ay = az = 0;
260
return;
261
}
262
double a; /* cos(bx) */
263
double b; /* sin(bx) */
264
double c; /* cos(by) */
265
double d; /* sin(by) */
266
double ee; /* cos(bz) */
267
double f; /* sin(bz) */
268
double sx, sy, sz;
269
GetScale(sx, sy, sz);
270
if(this->m_mirrored == -1) FAILURE(L"Don't know mirror - use IsMirrored method on object");
271
if(this->m_mirrored) sx = -sx;
272
273
// solve for d and decide case and solve for a, b, c, e and f
274
d = - e[8] / sz;
275
if((c = (1 - d) * (1 + d)) > 0.001)
276
{
277
// case 1
278
c = sqrt( c );
279
a = e[10] / sz / c;
280
b = e[9] / sz / c;
281
ee = e[0] / sx / c;
282
f = e[4] / sy / c;
283
}
284
else
285
{
286
// case 2
287
double coef;
288
double p, q;
289
290
d = ( d < 0 ) ? -1 : 1 ;
291
c = 0 ;
292
p = d * e[5] / sy - e[2] / sx;
293
q = d * e[6] / sy + e[1] / sx;
294
if((coef = sqrt( p * p + q * q )) > 0.001) {
295
a = q / coef;
296
b = p / coef;
297
ee = b;
298
f = -d * b;
299
}
300
else
301
{
302
/* dependent pairs */
303
a = e[5] / sy;
304
b = -e[6] / sy;
305
ee = 1 ;
306
f = 0 ;
307
}
308
}
309
310
// solve and return ax, ay and az
311
ax = atan2( b, a );
312
ay = atan2( d, c );
313
az = atan2( f, ee );
314
}
315
316
Matrix Matrix::Inverse()
317
{
318
// matrix inversion routine
319
320
// a is input matrix destroyed & replaced by inverse
321
// method used is gauss-jordan (ref ibm applications)
322
323
double hold , biga ;
324
int i , j , k , nk , kk , ij , iz ;
325
int ki , ji , jp , jk , kj , jq , jr , ik;
326
327
int n = 4; // 4 x 4 matrix only
328
Matrix a = *this;
329
int l[4], m[4];
330
331
if(a.m_unit) return a; // unit matrix
332
333
// search for largest element
334
nk = - n ;
335
for ( k = 0 ; k < n ; k++ ) {
336
nk += n ;
337
l [ k ] = m [ k ] = k ;
338
kk = nk + k ;
339
biga = a.e[ kk ] ;
340
341
for ( j = k ; j < n ; j++ ) {
342
iz = n * j ;
343
for ( i = k ; i < n ; i++ ) {
344
ij = iz + i ;
345
if ( fabs ( biga ) < fabs ( a.e[ ij ] ) ) {
346
biga = a.e[ ij ] ;
347
l[ k ] = i ;
348
m[ k ] = j ;
349
}
350
}
351
}
352
353
354
// interchange rows
355
j = l[ k ] ;
356
if ( j > k ) {
357
ki = k - n ;
358
359
for ( i = 0 ; i < n ; i++ ) {
360
ki += n ;
361
hold = - a.e[ ki ] ;
362
ji = ki - k + j ;
363
a.e[ ki ] = a.e[ ji ] ;
364
a.e[ ji ] = hold ;
365
}
366
}
367
368
// interchange columns
369
i = m[ k ] ;
370
if ( i > k ) {
371
jp = n * i ;
372
for ( j = 0 ; j < n ; j++ ) {
373
jk = nk + j ;
374
ji = jp + j ;
375
hold = - a.e[ jk ] ;
376
a.e[ jk ] = a.e[ ji ] ;
377
a.e[ ji ] = hold ;
378
}
379
}
380
381
// divide columns by minus pivot (value of pivot element is contained in biga)
382
if ( fabs ( biga ) < 1.0e-10 )FAILURE(getMessage(L"Singular Matrix - Inversion failure",GEOMETRY_ERROR_MESSAGES, -1)); // singular matrix
383
384
for ( i = 0 ; i < n ; i++ ) {
385
if ( i != k ) {
386
ik = nk + i ;
387
a.e[ ik ] = - a.e[ ik ] /biga ;
388
}
389
}
390
391
// reduce matrix
392
for ( i = 0 ; i < n ; i++ ) {
393
ik = nk + i ;
394
hold = a.e[ ik ] ;
395
ij = i - n ;
396
397
for ( j = 0 ; j < n ; j++ ) {
398
ij = ij + n ;
399
if ( i != k && j != k ) {
400
kj = ij - i + k ;
401
a.e[ ij ] = hold * a.e[ kj ] + a.e[ ij ] ;
402
}
403
}
404
}
405
406
// divide row by pivot
407
kj = k - n ;
408
for ( j = 0 ; j < n ; j++ ) {
409
kj = kj + n ;
410
if ( j != k ) a.e[ kj] = a.e[ kj ] /biga ;
411
}
412
413
// replace pivot by reciprocal
414
a.e[ kk ] = 1 / biga ;
415
}
416
417
// final row and column interchange
418
k = n - 1 ;
419
420
while ( k > 0 ) {
421
i = l[ --k ] ;
422
if ( i > k ) {
423
jq = n * k ;
424
jr = n * i ;
425
426
for ( j = 0 ; j < n ; j++ ) {
427
jk = jq + j ;
428
hold = a.e[jk] ;
429
ji = jr + j ;
430
a.e[jk] = - a.e[ji] ;
431
a.e[ji] = hold ;
432
}
433
}
434
435
j = m[ k ] ;
436
if ( j > k ) {
437
ki = k - n ;
438
439
for ( i = 1 ; i <= n ; i ++ ) {
440
ki = ki + n ;
441
hold = a.e[ ki ] ;
442
ji = ki - k + j ;
443
a.e[ ki ] = - a.e[ ji ] ;
444
a.e[ ji ] = hold ;
445
}
446
}
447
}
448
449
return a;
450
}
451
452
#ifdef PEPSDLL
453
void Matrix::ToPeps(int id)
454
{
455
int set = PepsVdmMake(id, VDM_MATRIX_TYPE , VDM_LOCAL);
456
if(set < 0) FAILURE(L"Failed to create Matrix VDM");
457
struct kgm_header pepsm;
458
459
Get(pepsm.matrix);
460
pepsm.off_rad = 0;
461
pepsm.off_dir = pepsm.origin_id = 0;
462
463
PepsVdmWriteTmx(set , &pepsm );
464
465
PepsVdmClose(set);
466
467
}
468
469
void Matrix::FromPeps(int id)
470
{
471
// if(id) {
472
int set = PepsVdmOpen(id, VDM_MATRIX_TYPE , VDM_READ_ONLY | VDM_LOCAL);
473
if(set < 0) FAILURE(L"Failed to open Matrix VDM");
474
475
struct kgm_header pepsm;
476
PepsVdmReadTmx(set , &pepsm);
477
memcpy(e, pepsm.matrix, sizeof(pepsm.matrix));
478
m_unit = true;
479
for(int i = 0; i < 16; i++) {
480
// copy over matrix and check for unit matrix
481
if(i == 0 || i == 5 || i == 10 || i == 15) {
482
if((e[i] = pepsm.matrix[i]) != 1) m_unit = false;
483
}
484
else {
485
if((e[i] = pepsm.matrix[i]) != 0) m_unit = false;
486
}
487
}
488
PepsVdmClose(set);
489
m_mirrored = IsMirrored();
490
// }
491
}
492
#endif
493
494
Matrix UnitMatrix; // a global unit matrix
495
496
497
// vector
498
Vector2d::Vector2d(const Vector3d &v){
499
if(FEQZ(v.getz())) FAILURE(L"Converting Vector3d to Vector2d illegal");
500
dx = v.getx();
501
dy = v.gety();
502
}
503
504
bool Vector2d::operator==(const Vector2d &v)const {
505
return FEQ(dx, v.getx(), 1.0e-06) && FEQ(dy, v.gety(), 1.0e-06);
506
}
507
508
void Vector2d::Transform(const Matrix& m) {
509
// transform vector
510
if(m.m_unit == false) {
511
double dxt = dx * m.e[0] + dy * m.e[1];
512
double dyt = dx * m.e[4] + dy * m.e[5];
513
dx = dxt;
514
dy = dyt;
515
}
516
this->normalise();
517
}
518
519
void Vector3d::Transform(const Matrix& m) {
520
// transform vector
521
if(m.m_unit == false) {
522
double dxt = dx * m.e[0] + dy * m.e[1] + dz * m.e[2];
523
double dyt = dx * m.e[4] + dy * m.e[5] + dz * m.e[6];
524
double dzt = dx * m.e[8] + dy * m.e[9] + dz * m.e[10];
525
dx = dxt;
526
dy = dyt;
527
dz = dzt;
528
}
529
this->normalise();
530
}
531
532
void Vector3d::arbitrary_axes(Vector3d& x, Vector3d& y){
533
// arbitrary axis algorithm - acad method of generating an arbitrary but
534
// consistant set of axes from a single normal ( z )
535
// arbitrary x & y axes
536
537
if ( ( fabs ( this->getx() ) < 1.0/64.0 ) && (fabs(this->gety()) < 1.0/64.0))
538
x = Y_VECTOR ^ *this;
539
else
540
x = Z_VECTOR ^ *this;
541
542
y = *this ^ x;
543
}
544
545
int Vector3d::setCartesianAxes(Vector3d& b, Vector3d& c) {
546
#define a *this
547
// computes a RH triad of Axes (Cartesian) starting from a (normalised)
548
// if a & b are perpendicular then c = a ^ b
549
// if a & c are perpendicular then b = c ^ a
550
// if neither are perpendicular to a, then return arbitrary axes from a
551
552
// calling sequence for RH cartesian
553
// x y z
554
// y z x
555
// z x y
556
if(a == NULL_VECTOR) FAILURE(L"SetAxes given a NULL Vector");
557
double epsilon = 1.0e-09;
558
bool bNull = (b == NULL_VECTOR);
559
bool cNull = (c == NULL_VECTOR);
560
bool abPerp = !bNull;
561
if(abPerp) abPerp = (fabs(a * b) < epsilon);
562
563
bool acPerp = !cNull;
564
if(acPerp) acPerp = (fabs(a * c) < epsilon);
565
566
if(abPerp) {
567
c = a ^ b;
568
return 1;
569
}
570
571
if(acPerp) {
572
b = c ^ a;
573
return 1;
574
}
575
576
arbitrary_axes(b, c);
577
b.normalise();
578
c.normalise();
579
return 2;
580
}
581
582
583
584
}
b'\\ No newline at end of file'
Loggerhead is a web-based interface for Breezy
Version: 2.0.1