File: AUTOJUST\MATRIX.CPP
1 //#include <iostream.h>
2 #include <math.h>
3
4 #include "autojust\matrix.h"
5
6 //--||--\\--||--//--||--\\--||--//--||--\\--||--//--||--\\--||--//--||--\\--||--
7
8 //******************************************************************************
9 // Matrix-Klasse
10 //******************************************************************************
11
12 // *** Klassenkonstruktoren ***************************************************
13 // Standardkonstruktor (ohne Argumente)
14 TMatrix::TMatrix()
15 {
16 arr= NULL;
17 ze= sp= 0;
18 homogene_koordinaten= false;
19 }
20
21 // Konstruktor, der eine m*n-Nullmatrix initialisiert.
22 TMatrix::TMatrix(unsigned m, unsigned n)
23 {
24 ze= m;
25 sp= n;
26 homogene_koordinaten= false;
27 arr= new double[m * n]; // dynamisches Erzeugen des Matrixarrays
28 for (unsigned i= 0; i < m*n; i++)
29 arr[i]= 0; // Array mit Nullen belegen
30 }
31
32 // Klassendestruktor löscht das dynamische Array
33 TMatrix::~TMatrix()
34 {
35 _FREELIST(arr);
36 }
37
38 // überladener copy-Kontruktor (wird nur bei Initialisierung aufgerufen)
39 TMatrix::TMatrix (const TMatrix & mat)
40 {
41 ze= mat.ze;
42 sp= mat.sp;
43 homogene_koordinaten= mat.homogene_koordinaten;
44 arr= new double[mat.ze * mat.sp];
45 for (unsigned i= 0; i < mat.ze * mat.sp; i++)
46 arr[i]= mat.arr[i];
47 }
48
49 // *** überladene Operatoren *************************************************
50 // überladener= Operator : Zuweisungsoperator
51 TMatrix TMatrix::operator= (const TMatrix & mat)
52 {
53 if (this == &mat)
54 return * this; // *this zurückgeben, wenn Selbstzuweisung
55
56 _FREELIST(arr); // Speicherplatz des Matrix-Arrays freigeben
57 ze= mat.ze;
58 sp= mat.sp;
59 homogene_koordinaten= mat.homogene_koordinaten;
60 arr= new double[mat.ze * mat.sp];
61 for (unsigned i= 0; i < mat.ze * mat.sp; i++)
62 arr[i]= mat.arr[i];
63 return *this;
64 }
65
66 // überladener + Operator: addiert zwei Matrizen gleichen Typs
67 TMatrix TMatrix::operator + (const TMatrix & mat)
68 {
69 if (ze != mat.ze || sp != mat.sp)
70 {
71 TraceErr("(M.+)Fehler: Matrizen versch.Typs können nicht addiert werden.\n");
72 return *this;
73 }
74 else
75 {
76 for (unsigned i= 0; i < ze*sp; i++)
77 arr[i] += mat.arr[i];
78 }
79 homogene_koordinaten &= mat.homogene_koordinaten;
80 return *this;
81 }
82
83 // überladener - Operator: subtrahiert zwei Matrizen gleichen Typs
84 TMatrix TMatrix::operator - (const TMatrix & mat)
85 {
86 if (ze != mat.ze || sp != mat.sp)
87 {
88 TraceErr("(M.-)Fehler: Kann Matrizen versch. Typs nicht subtrahieren.\n");
89 return *this;
90 }
91 else
92 {
93 for (unsigned i= 0; i < ze*sp; i++)
94 arr[i] -= mat.arr[i];
95 }
96 homogene_koordinaten &= mat.homogene_koordinaten;
97 return *this;
98 }
99
100 // überladener * Operator Nr.1: reelle Vervielfachung von Matrizen
101 // Version TMatrix-Objekt * double-Wert
102 TMatrix TMatrix::operator *(double & fakt)
103 {
104 for (unsigned i= 0; i < ze*sp; i++)
105 arr[i] *= fakt;
106 return *this;
107 }
108
109 // Version double-Wert * TMatrix-Objekt (friend-Funktion)
110 TMatrix operator *(double fakt, const TMatrix & mat)
111 {
112 TMatrix temp(mat.ze, mat.sp);
113 for (unsigned i= 0; i < mat.ze*mat.sp; i++)
114 temp.arr[i]= fakt * mat.arr[i];
115 temp.homogene_koordinaten= mat.homogene_koordinaten;
116 return temp;
117 }
118
119 // überladener * Operator Nr.2: multipliziert zwei Matrizen
120 TMatrix TMatrix::operator *(const TMatrix & mat)
121 {
122 TMatrix temp(ze, mat.sp);
123 if (sp != mat.ze)
124 {
125 TraceErr("(M.*)Fehler: Spalten der 1. Matrix ungleich Zeilen der 2.\n");
126 temp.ze= temp.sp= 0;
127 temp.arr= NULL;
128 }
129 else
130 {
131 for (unsigned i= 0; i < ze*mat.sp; i++)
132 for (unsigned j= 0; j < sp; j++)
133 temp.arr[i] += arr[j + sp * (i / mat.sp)] * mat.arr[j * mat.sp + i % mat.sp];
134 }
135 temp.homogene_koordinaten= (homogene_koordinaten && mat.homogene_koordinaten);
136 return temp;
137 }
138
139 // *** Matrizen - Funktionen **************************************************
140 // Erstellen einer Einheitsmatrix mit Rang m
141 TMatrix TMatrix::einheitsmatrix(unsigned m)
142 {
143 _FREELIST(arr);
144
145 ze= m;
146 sp= m;
147
148 arr= new double[m * m];
149
150 for (unsigned i= 0; i < ze; i++)
151 {
152 for (unsigned j= 0; j < sp; j++)
153 {
154 if ( i == j)
155 arr[i*sp + j]= 1;
156 else
157 arr[i*sp + j]= 0;
158 }
159 }
160 return *this;
161 }
162
163 // Berechnung der Inversen einer Matrix, sofern sie existiert
164 // Verfahren von Gauss-Jordan (siehe Stoecker S.402)
165 TMatrix TMatrix::invers(void)
166 {
167 double dummy;
168
169 if ( ze != sp || ze == 0 || sp == 0)
170 TraceErr("(M.Inv)Fehler: Matrix kann nicht invertiert werden.\n");
171
172 TMatrix temp;
173
174 // Einheitsmatrix temporär erstellen
175 temp.einheitsmatrix(ze);
176
177 //Berechnung der Inversen
178 for (unsigned k= 0; k < ze; k++)
179 {
180 dummy= arr[k * sp + k];
181 for (unsigned j= 0; j < sp; j++)
182 {
183 arr[k*sp + j]= arr[k * sp + j] / dummy;
184 temp.arr[k*sp + j]= temp.arr[k * sp + j] / dummy;
185 }
186 for (unsigned i= 0; i < ze; i++)
187 {
188 if (i != k)
189 {
190 dummy= arr[i * sp + k];
191 for (unsigned j= 0; j < sp; j++)
192 {
193 arr[i*sp + j]= arr[i * sp + j] - (dummy * arr[k * sp + j]);
194 temp.arr[i*sp + j]= temp.arr[i * sp + j] - (dummy * temp.arr[k * sp + j]);
195 }
196 }
197 }
198 }
199 // Inverse Matrix ist nun in temp
200 return temp;
201 }
202
203 // **** Transformationsmethoden ***********************************************
204 // Verschiebe-(Translations)TMatrix erzeugen
205 // Verschiebung um (Tx,Ty,Tz) Vektor
206 TMatrix TMatrix::verschiebematrix(const TVektor &vekt)
207 {
208 if ( !(vekt.ze == 4 && vekt.ist_homogen() ||
209 vekt.ze == 3 && !vekt.ist_homogen()))
210 {
211 TraceErr("(M.VM)Fehler: Verschiebungsvektor nicht 3-dimensional.\n");
212 return *this;
213 }
214 // Einheitsmatrix 4x4 erzeugen
215 this->einheitsmatrix(4);
216 // Verschiebungsvektor in die letzte Spalte der Matrix eintragen
217 // bei homogenem Vektor wird homogene Komponente mitkopiert (=1)
218 for (unsigned i= 0; i < vekt.ze; i++)
219 {
220 // Zugriff auf 4. Spaltenelement der jeweiligen Zeile
221 arr[i*ze + 3]= vekt.arr[i];
222 }
223 homogene_koordinaten= true;
224 return *this;
225 }
226
227 // Rotationsmatrix erzeugen
228 // Drehung um x-,y-, bzw. z-Achse mit Winkel
229 // Anmerkung: wenn Winkel negativ angegeben, dann handelt es sich um eine
230 // inverse Drehung
231 TMatrix TMatrix::rotationsmatrix_x(double winkel)
232 {
233 this->einheitsmatrix(4);
234 /* Rotation um die X-Achse mit dem Winkel a
235 1 0 0 0
236 0 cos(a) -sin(a) 0
237 0 sin(a) cos(a) 0
238 0 0 0 1
239 */
240 arr[1*4 + 1]= cos(winkel);
241 arr[1*4 + 2]= -sin(winkel);
242 arr[2*4 + 1]= sin(winkel);
243 arr[2*4 + 2]= cos(winkel);
244 homogene_koordinaten= true;
245 return *this;
246 }
247
248 TMatrix TMatrix::rotationsmatrix_y(double winkel)
249 {
250 this->einheitsmatrix(4);
251 /* Rotation um die Y-Achse mit dem Winkel a
252 cos(a) 0 sin(a) 0
253 0 1 0 0
254 -sin(a) 0 cos(a) 0
255 0 0 0 1
256 */
257 arr[0*4 + 0]= cos(winkel);
258 arr[0*4 + 2]= sin(winkel);
259 arr[2*4 + 0]= -sin(winkel);
260 arr[2*4 + 2]= cos(winkel);
261 homogene_koordinaten= true;
262 return *this;
263 }
264
265 TMatrix TMatrix::rotationsmatrix_z(double winkel)
266 {
267 this->einheitsmatrix(4);
268 /* Rotation um die z-Achse mit dem Winkel a
269 cos(a) -sin(a) 0 0
270 sin(a) cos(a) 0 0
271 0 0 1 0
272 0 0 0 1
273 */
274 arr[0*4 + 0]= cos(winkel);
275 arr[0*4 + 1]= -sin(winkel);
276 arr[1*4 + 0]= sin(winkel);
277 arr[1*4 + 1]= cos(winkel);
278 homogene_koordinaten= true;
279 return *this;
280 }
281
282 // Zusammenfassung aller Transformationen X-Y-Z & Verschiebung in einer Funktion
283 TMatrix TMatrix::transformiere(unsigned reihenfolge,
284 const TVektor & verschiebung,
285 double drehung_x, double drehung_y, double drehung_z)
286 {
287 TMatrix tempx, tempy, tempz;
288
289 // Nur 4dim Vektoren inkl. homogener Komponente oder reine 3dim Vektoren
290 if ( !(verschiebung.ze == 4 && verschiebung.ist_homogen() ||
291 verschiebung.ze == 3 && !verschiebung.ist_homogen()) )
292 {
293 TraceErr("(M.TRAFO)Fehler: Verschiebungsvektor nicht 3-dimensional.\n");
294 return *this;
295 }
296
297 // Reihenfolge der Transformationen muss angegeben sein: X->Y->Z oder Z->Y->X
298 if ( reihenfolge != REIHENFOLGE_XYZ && reihenfolge != REIHENFOLGE_ZYX )
299 {
300 TraceErr("(M.TRAFO)Fehler: Keine gültige Angabe der Trafo-Reihenfolge.\n");
301 return *this;
302 }
303 else
304 {
305 // Drehungen in der 4x4 Trafo-Matrix
306 // Erklärung siehe oben bei Einzelrotationen
307 tempx.rotationsmatrix_x(drehung_x);
308 tempy.rotationsmatrix_y(drehung_y);
309 tempz.rotationsmatrix_z(drehung_z);
310 // Verschiebung um Vektor
311 this->verschiebematrix(verschiebung);
312 if (reihenfolge == REIHENFOLGE_ZYX)
313 // aufgrund der Def. d. Matrixmultiplikation (Beginn der Mult. von rechts)
314 // muss die Reihenfolge der Multiplikation umgekehrt notiert werden,
315 // mathematisch gilt folgendes:
316 // R_rueck= Rx*Ry*Rz*T
317 *this= *this * tempz * tempy * tempx;
318 else
319 // aufgrund der Def. d. Matrixmultiplikation (Beginn der Mult. von rechts)
320 // muss die Reihenfolge der Multiplikation umgekehrt notiert werden,
321 // mathematisch gilt folgendes:
322 // R_hin= T*Rz*Ry*Rx
323 *this= tempx * tempy * tempz * *this;
324 homogene_koordinaten= true;
325 }
326 return *this;
327 }
328
329
330 //******************************************************************************
331 // Vektor-Klasse
332 //******************************************************************************
333
334 // Funktion, um aus einer (m,1)-Matrix einen Vektortyp zu machen
335 TVektor::TVektor (const TMatrix & mat)
336 {
337
338 if (mat.ze == 0 || mat.sp == 0)
339 TraceErr("(V)Fehler: Matrixspalten- oder Zeilenanzahl beträgt 0.\n");
340 else if (mat.sp != 1)
341 TraceErr("(V)Fehler: Matrixspaltenanzahl != 1 Keine Umwandlung in Vektor.\n");
342 else
343 {
344 ze= mat.ze;
345 sp= 1;
346 homogene_koordinaten= mat.homogene_koordinaten;
347 arr= new double[ze];
348 for (unsigned i= 0; i < ze; i++)
349 arr[i]= mat.arr[i];
350 }
351 }
352
353
354 // Vektor skalieren
355 TVektor operator *(double fakt, const TVektor & vekt)
356 {
357 TVektor temp(vekt.ze);
358 for (unsigned i= 0; i < vekt.ze; i++)
359 temp.arr[i]= fakt * vekt.arr[i];
360 return temp;
361 }
362
363 // Umwandlung in einen Vektor in homogenen Koordinaten
364 TVektor TVektor::mache_homogen(void)
365 {
366 unsigned i;
367
368 if (ze == 0)
369 {
370 TraceErr("(V.MH)Fehler: Vektor mit Zeilenanzahl 0.\n");
371 return *this;
372 }
373 else if (this->ist_homogen())
374 {
375 TraceErr("(V.MH)Fehler: Vektor schon in homog. Koord. angegeben.\n");
376 return *this;
377 }
378
379 TVektor temp(ze + 1); // temp-Vektor mit einer zusätzlichen homogenen Komponente
380
381 // Kopieren des Ursprungsvektors
382 for (i= 0; i < ze; i++)
383 temp.arr[i]= arr[i];
384 // neue Komponente im homogenen temp-Vektor auf 1 setzen
385 temp.arr[(ze + 1) - 1]= 1;
386
387 // aktuellen Vektor um homogene Komponente erweitern
388 _FREELIST(arr); // Speicherplatz des Matrix-Arrays freigeben
389 ze= ze + 1;
390 arr= new double[ze]; // neues Vektor-Array erzeugen
391 for (i= 0; i < ze; i++)
392 arr[i]= temp.arr[i];
393 homogene_koordinaten= true;
394 return *this;
395 }
396
397 // Umwandlung in einen Vektor in kartesischen Koordinaten
398 TVektor TVektor::mache_kartesisch(void)
399 {
400 unsigned i;
401
402 if (ze == 0)
403 {
404 TraceErr("(V.MK)Fehler: Vektor mit Zeilenanzahl 0.\n");
405 return *this;
406 }
407 else if (!this->ist_homogen())
408 {
409 TraceErr("(V.MK)Fehler: Vektor bereits in kartes. Koord. angegeben.\n");
410 return *this;
411 }
412
413 TVektor temp(ze - 1); // temp-Vektor ohne homogene Komponente
414
415 // Kopieren des Ursprungsvektors
416 // aktueller Vektor ohne homogene Komponente
417 for (i= 0; i < temp.ze; i++)
418 temp.arr[i]= arr[i];
419
420 _FREELIST(arr); // Speicherplatz des Vektor-Arrays freigeben
421 ze= ze - 1;
422 arr= new double[ze]; // neues Vektor-Array erzeugen
423 for (i= 0; i < ze; i++)
424 arr[i]= temp.arr[i];
425 homogene_koordinaten= false;
426 return *this;
427 }
428
429 // Berechnung des Betrags (Länge) eines Vektors
430 double TVektor::vektor_betrag (void) const
431 {
432 double betrag= 0.0;
433 unsigned temp_ze;
434
435 if (ze == 0)
436 {
437 TraceErr("(V.VB)Fehler: Kein echter Vektor (Zeilen == 0).\n");
438 return -1.0;
439 }
440
441 if (this->ist_homogen())
442 temp_ze= ze - 1; // homogene Komponente des Vektors ignorieren
443 else
444 temp_ze= ze; // alle kartesischen Koordinaten betrachten
445
446 for (unsigned i= 0; i < temp_ze; i++)
447 {
448 betrag += arr[i] * arr[i];
449 }
450 return sqrt(betrag);
451 }
452
453 // Berechnung des Skalarproduktes zweier Vektoren
454 double TVektor::skalarprodukt (const TVektor & vekt)
455 {
456 double skalar= 0.0;
457 unsigned temp_ze;
458
459 if (ze == 0 || vekt.ze == 0)
460 {
461 TraceErr("(V.SP)Fehler: Mindestens ein Vektor mit Dimension 0.\n");
462 return -1.0;
463 }
464 else if (ze != vekt.ze)
465 {
466 TraceErr("(V.SP)Fehler: Nur Vektoren gleicher Dimension zulässig.\n");
467 return -1.0;
468 }
469
470 if (this->ist_homogen() && vekt.ist_homogen())
471 temp_ze= ze - 1; // homogene Komponente des Vektors ignorieren
472 else if (!(this->ist_homogen()) && !(vekt.ist_homogen()))
473 temp_ze= ze; // alle kartesischen Koordinaten betrachten
474 else
475 {
476 TraceErr("(V.SP)Fehler: 1 Vektor ist in homog. Koord. angegeben.\n");
477 return -1.0;
478 }
479
480 for (unsigned i= 0; i < temp_ze; i++)
481 {
482 skalar += arr[i] * vekt.arr[i];
483 }
484 return skalar;
485 }
486
487 // Berechnung des Winkels zwischen zwei Vektoren in RADIANT
488 double TVektor::winkel (const TVektor & vekt)
489 {
490 double winkel= -1.0; // falls Rückgabewert -1.0, dann Fehlerfall
491
492 if (ze == 0 || vekt.ze == 0)
493 {
494 TraceErr("(V.Wi)Fehler: Mindestens ein Vektor mit Dimension 0.\n");
495 return winkel;
496 }
497 else if (ze != vekt.ze)
498 {
499 TraceErr("(V.Wi)Fehler: Nur Vektoren gleicher Dimension zulässig.\n");
500 return winkel;
501 }
502 else if ( (this->ist_homogen() && !(vekt.ist_homogen()) ) ||
503 (!(this->ist_homogen()) && vekt.ist_homogen() ) )
504 {
505 TraceErr("(V.Wi)Fehler: Nur 1 Vektor in homogenen Koordinaten angegeben.\n");
506 return winkel;
507 }
508
509 // Berechnung von |a|*|b|
510 if ((winkel= vekt.vektor_betrag() * this->vektor_betrag()) == 0)
511 {
512 TraceErr("(V.Wi)Fehler: Winkel kann nicht berechnet werden (Nullvektor).\n");
513 return winkel;
514 }
515 // Berechnung von a * b / |a|*|b|
516 winkel= this->skalarprodukt(vekt) / winkel;
517 // Berechnung von arcos (a*b / |a|*|b|)= Winkel zw. 2 Vektoren
518 winkel= acos(winkel);
519 return winkel;
520 }
521
522 // Setzen der x,y,z-Koordinaten eines 3dim Vektors
523 bool TVektor::set_XYZ(double x, double y, double z)
524 {
525 if (ze == 3)
526 {
527 arr[0]= x;
528 arr[1]= y;
529 arr[2]= z;
530 return true;
531 }
532 else
533 return false;
534 }
535
536 // Auslesen der x,y,z-Koordinaten eines 3dim Vektors
537 bool TVektor::get_XYZ(double & x, double & y, double & z)
538 {
539 if ((ze == 3) || (ze == 4))
540 {
541 x= arr[0];
542 y= arr[1];
543 z= arr[2];
544 return true;
545 }
546 else
547 return false;
548 }
549
550
551 //******************************************************************************
552 // Matrizenliste-Klasse
553 //******************************************************************************
554 // Liste von Transformationsmatrizen
555 // organisiert als Stack
556 // mit push(element), pop(), ist_leer()
557
558
559 // Listenkonstruktor um eine Liste mit einer bestimmten Anzahl von
560 // Listenelementen zu erzeugen
561 TMatrizenListe::TMatrizenListe(unsigned anzahl)
562 {
563 liste= NULL;
564 akt_elemente= 0;
565 max_elemente= anzahl;
566
567 if (anzahl != 0 )
568 {
569 liste= new TMatrix[max_elemente];
570 }
571 }
572
573 // Matrix zur Liste hinzufügen
574 bool TMatrizenListe::push(const TMatrix & trafo)
575 {
576 if (akt_elemente != max_elemente)
577 {
578 liste[akt_elemente++]= trafo;
579 return true;
580 }
581
582 return false;
583 }
584
585 // letzte Matrix aus Liste entfernen
586 TMatrix TMatrizenListe::pop(void)
587 {
588 TMatrix temp;
589
590 if (akt_elemente)
591 {
592 temp= liste[--akt_elemente];
593 }
594 return temp;
595 }
596
597 TMatrix TMatrizenListe::zeige(unsigned position)
598 {
599 TMatrix temp;
600
601 if (position == 0)
602 {
603 TraceErr("(ML.Zeig)Fehler: Nulltes Listenelement existiert nicht.\n");
604 }
605 else if (akt_elemente && (akt_elemente >= position) )
606 {
607 temp= liste[position - 1];
608 }
609
610 return temp;
611 }
612
613
614
615