1 /*
2  *_________________________________________________________________________*
3  *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
4  *      DESCRIPTION: SEE READ-ME                                           *
5  *      FILE NAME: fastmatrixops.cpp                                       *
6  *      AUTHORS: See Author List                                           *
7  *      GRANTS: See Grants List                                            *
8  *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
9  *      LICENSE: Please see License Agreement                              *
10  *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
11  *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
12  *                     Computational Dynamics Lab                          *
13  *                     Rensselaer Polytechnic Institute                    *
14  *                     110 8th St. Troy NY 12180                           *
15  *      CONTACT:        anderk5@rpi.edu                                    *
16  *_________________________________________________________________________*/
17 
18 #include <iostream>
19 #include "fastmatrixops.h"
20 #include <cmath>
21 
22 using namespace std;
23 
24 //
25 // Cross Product (friend of Vect3)
26 //
27 
FastCross(Vect3 & a,Vect3 & b,Vect3 & c)28 void FastCross(Vect3& a, Vect3& b, Vect3& c){
29   c.elements[0] = a.elements[1]*b.elements[2] - a.elements[2]*b.elements[1];
30   c.elements[1] = a.elements[2]*b.elements[0] - a.elements[0]*b.elements[2];
31   c.elements[2] = a.elements[0]*b.elements[1] - a.elements[1]*b.elements[0];
32 }
33 
34 //
35 //  Simple Rotation (friend of Vect3 and Mat3x3)
36 //
37 
FastSimpleRotation(Vect3 & v,double q,Mat3x3 & C)38 void FastSimpleRotation(Vect3& v, double q, Mat3x3& C){
39   // intermediate quantities
40   double cq = cos(q);
41   double sq = sin(q);
42   double one_m_cq = 1-cq;
43   double l12 = v.elements[0]*v.elements[1]*one_m_cq;
44   double l13 = v.elements[0]*v.elements[2]*one_m_cq;
45   double l23 = v.elements[1]*v.elements[2]*one_m_cq;
46 
47   // the transformation
48   C.elements[0][0] = v.elements[0]*v.elements[0]*one_m_cq+cq;
49   C.elements[0][1] = l12-v.elements[2]*sq;
50   C.elements[0][2] = l13+v.elements[1]*sq;
51   C.elements[1][0] = l12+v.elements[2]*sq;
52   C.elements[1][1] = v.elements[1]*v.elements[1]*one_m_cq+cq;
53   C.elements[1][2] = l23-v.elements[0]*sq;
54   C.elements[2][0] = l13-v.elements[1]*sq;
55   C.elements[2][1] = l23+v.elements[0]*sq;
56   C.elements[2][2] = v.elements[2]*v.elements[2]*one_m_cq+cq;
57 }
58 
59 //
60 // Quaternion Functions
61 //
62 
FastQuaternions(ColMatrix & q,Mat3x3 & C)63 void FastQuaternions(ColMatrix& q, Mat3x3& C){
64   double* e = q.elements;
65 
66   // normalize the quaternions
67   double length = e[0]*e[0] + e[1]*e[1] + e[2]*e[2] + e[3]*e[3];
68   length = sqrt(length);
69   e[0] = e[0]/length;
70   e[1] = e[1]/length;
71   e[2] = e[2]/length;
72   e[3] = e[3]/length;
73 
74   // compute the transformation
75   C.elements[0][0] = e[0]*e[0] + e[1]*e[1] - e[2]*e[2] - e[3]*e[3];
76   C.elements[1][1] = e[0]*e[0] - e[1]*e[1] + e[2]*e[2] - e[3]*e[3];
77   C.elements[2][2] = e[0]*e[0] - e[1]*e[1] - e[2]*e[2] + e[3]*e[3];
78 
79   C.elements[0][1] = 2 * (e[1]*e[2] - e[0]*e[3]);
80   C.elements[0][2] = 2 * (e[1]*e[3] + e[0]*e[2]);
81   C.elements[1][2] = 2 * (e[2]*e[3] - e[0]*e[1]);
82 
83   C.elements[1][0] = 2 * (e[1]*e[2] + e[0]*e[3]);
84   C.elements[2][0] = 2 * (e[1]*e[3] - e[0]*e[2]);
85   C.elements[2][1] = 2 * (e[2]*e[3] + e[0]*e[1]);
86 }
87 
FastQuaternionDerivatives(ColMatrix & q,ColMatrix & omega,ColMatrix & qdot)88 void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot){
89   double* w = omega.elements;
90   double* e = q.elements;
91 
92   qdot.elements[0] = 0.5 * (-w[0]*e[1] - w[1]*e[2] - w[2]*e[3]);
93   qdot.elements[1] = 0.5 * ( w[0]*e[0] + w[2]*e[2] - w[1]*e[3]);
94   qdot.elements[2] = 0.5 * ( w[1]*e[0] - w[2]*e[1] + w[0]*e[3]);
95   qdot.elements[3] = 0.5 * ( w[2]*e[0] + w[1]*e[1] - w[0]*e[2]);
96 }
97 
FastInvQuaternions(Mat3x3 & C,ColMatrix & q)98 void FastInvQuaternions(Mat3x3& C, ColMatrix& q){
99 }
100 
101 //
102 // Inverse
103 //
104 
105 // friend of Matrix
106 //void FastInverse(Matrix& A, Matrix& C){ // C = A^(-1)
107 //  C.rows[0][0] = 1/A.rows[0][0];
108 //}
109 
110 //
111 // LDL^T Decomposition (from Golub and Van Loan)
112 //
113 
114 // friend of Matrix
FastLDLT(Matrix & A,Matrix & C)115 void FastLDLT(Matrix& A, Matrix& C){ // C is the LD of the LDL^T decomposition of A (SPD)
116   double Lv;
117   int n = A.numrows;
118 
119   for(int j=0;j<n;j++){
120     Lv = 0.0;
121     for(int i=0;i<j;i++){
122       C.rows[i][j] = C.rows[j][i]*C.rows[i][i];
123       Lv += C.rows[j][i]*C.rows[i][j];
124     }
125 
126     C.rows[j][j] = A.rows[j][j] - Lv;
127     for(int i=j+1;i<n;i++){
128       Lv = 0.0;
129       for(int k=0;k<j;k++) Lv += C.rows[i][k]*C.rows[k][j];
130       C.rows[i][j] = (A.rows[i][j] - Lv)/C.rows[j][j];
131     }
132   }
133 }
134 
135 
136 // friend of Mat6x6
FastLDLT(Mat6x6 & A,Mat6x6 & C)137 void FastLDLT(Mat6x6& A, Mat6x6& C){ // C is the LD of the LDL^T decomposition of A (SPD)
138   double v[6];
139   double Lv;
140 
141   for(int j=0;j<6;j++){
142     Lv = 0.0;
143     for(int i=0;i<j;i++){
144       v[i] = C.elements[j][i]*C.elements[i][i];
145       Lv += C.elements[j][i]*v[i];
146     }
147 
148     v[j] = A.elements[j][j] - Lv;
149     C.elements[j][j] = v[j];
150     for(int i=j+1;i<6;i++){
151       Lv = 0.0;
152       for(int k=0;k<j;k++) Lv += C.elements[i][k]*v[k];
153       C.elements[i][j] = (A.elements[i][j] - Lv)/v[j];
154     }
155   }
156 }
157 
158 // friend of Matrix
FastLDLTSubs(Matrix & LD,Matrix & B,Matrix & C)159 void FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C){
160   int n = B.numrows;
161   int c = B.numcols;
162   double temp;
163 
164   for(int k=0;k<c;k++){
165     for(int i=0;i<n;i++){
166       temp = 0.0;
167       for(int j=0;j<i;j++){
168         temp += C.rows[j][k] * LD.rows[i][j];
169       }
170       C.rows[i][k] = B.rows[i][k] - temp;
171     }
172     for(int i=n-1;i>-1;i--){
173       C.rows[i][k] = C.rows[i][k] / LD.rows[i][i];
174       temp = 0.0;
175       for(int j=n-1;j>i;j--){
176         temp += C.rows[j][k] * LD.rows[j][i];
177       }
178       C.rows[i][k] = C.rows[i][k] - temp;
179     }
180   }
181 }
182 
183 // friend of Matrix
FastLDLTSubsLH(Matrix & B,Matrix & LD,Matrix & C)184 void FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C){
185   int n = B.numcols;
186   int c = B.numrows;
187   double temp;
188 
189   for(int k=0;k<c;k++){
190     for(int i=0;i<n;i++){
191       temp = 0.0;
192       for(int j=0;j<i;j++){
193         temp += C.rows[k][j] * LD.rows[i][j];
194       }
195       C.rows[k][i] = B.rows[k][i] - temp;
196     }
197     for(int i=n-1;i>-1;i--){
198       C.rows[k][i] = C.rows[k][i] / LD.rows[i][i];
199       temp = 0.0;
200       for(int j=n-1;j>i;j--){
201         temp += C.rows[k][j] * LD.rows[j][i];
202       }
203       C.rows[k][i] = C.rows[k][i] - temp;
204     }
205   }
206 }
207 
208 // friend of Mat6x6
FastLDLTSubs(Mat6x6 & LD,Mat6x6 & B,Mat6x6 & C)209 void FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C){
210   double temp;
211 
212   for(int k=0;k<6;k++){
213     for(int i=0;i<6;i++){
214       temp = 0.0;
215       for(int j=0;j<i;j++){
216         temp += C.elements[j][k] * LD.elements[i][j];
217       }
218       C.elements[i][k] = B.elements[i][k] - temp;
219     }
220     for(int i=5;i>-1;i--){
221       C.elements[i][k] = C.elements[i][k] / LD.elements[i][i];
222       temp = 0.0;
223       for(int j=5;j>i;j--){
224         temp += C.elements[j][k] * LD.elements[j][i];
225       }
226       C.elements[i][k] = C.elements[i][k] - temp;
227     }
228   }
229 }
230 
231 // friend of Mat6x6 & Vect6
FastLDLTSubs(Mat6x6 & LD,Vect6 & B,Vect6 & C)232 void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C){
233   double temp;
234 
235   for(int i=0;i<6;i++){
236     temp = 0.0;
237     for(int j=0;j<i;j++){
238       temp += C.elements[j] * LD.elements[i][j];
239     }
240     C.elements[i] = B.elements[i] - temp;
241   }
242   for(int i=5;i>-1;i--){
243     C.elements[i] = C.elements[i] / LD.elements[i][i];
244     temp = 0.0;
245     for(int j=5;j>i;j--){
246       temp += C.elements[j] * LD.elements[j][i];
247     }
248     C.elements[i] = C.elements[i] - temp;
249   }
250 }
251 
252 // friend of Matrix
FastLU(Matrix & A,Matrix & LU,int * indx)253 void FastLU(Matrix& A, Matrix& LU, int *indx){ // LU is the LU decomposition of A
254   int i,imax=0,j,k;
255   int n = A.numrows;
256   double big, dum, sum, temp;
257   double vv[10000];
258 
259   LU = A;
260   for (i=0;i<n;i++){
261     big=0.0;
262     for (j=0;j<n;j++){
263       temp=fabs(LU.rows[i][j]);
264       if (temp > big) big=temp;
265       }
266     vv[i]=1.0/big;
267   }
268   for (j=0;j<n;j++){
269     for (i=0;i<j;i++){
270       sum=LU.rows[i][j];
271       for (k=0;k<i;k++) sum -= LU.rows[i][k]*LU.rows[k][j];
272       LU.rows[i][j]=sum;
273     }
274     big=0.0;
275     for (i=j;i<n;i++){
276       sum=LU.rows[i][j];
277       for (k=0;k<j;k++)
278         sum -= LU.rows[i][k]*LU.rows[k][j];
279         LU.rows[i][j]=sum;
280         if ((dum=vv[i]*fabs(sum)) >= big) {
281         big=dum;
282         imax=i;
283       }
284     }
285     if (j != imax) {
286       for (k=0;k<n;k++) {
287         dum=LU.rows[imax][k];
288         LU.rows[imax][k]=LU.rows[j][k];
289         LU.rows[j][k]=dum;
290       }
291       vv[imax]=vv[j];
292     }
293     indx[j]=imax;
294     // if (LU.rows[j][j] == 0.0) LU.rows[j][j]=1.0e-20;
295     if (j != n-1) {
296       dum=1.0/(LU.rows[j][j]);
297       for (i=j+1;i<n;i++) LU.rows[i][j] *= dum;
298     }
299   }
300 }
301 
302 // friend of Mat3x3
FastLU(Mat3x3 & A,Mat3x3 & LU,int * indx)303 void FastLU(Mat3x3& A, Mat3x3& LU, int *indx){ // LU is the LU decomposition of A
304   int i,imax=0,j,k;
305   double big, dum, sum, temp;
306   double vv[10000];
307 
308   LU = A;
309   for (i=0;i<3;i++){
310     big=0.0;
311     for (j=0;j<3;j++){
312       temp=fabs(LU.BasicGet(i,j));
313       if (temp > big) big=temp;
314       }
315     vv[i]=1.0/big;
316   }
317   for (j=0;j<3;j++){
318     for (i=0;i<j;i++){
319       sum=LU.BasicGet(i,j);
320       for (k=0;k<i;k++) sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
321       LU.BasicSet(i,j,sum);
322     }
323     big=0.0;
324     for (i=j;i<3;i++){
325       sum=LU.BasicGet(i,j);
326       for (k=0;k<j;k++)
327         sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
328         LU.BasicSet(i,j,sum);
329         if ((dum=vv[i]*fabs(sum)) >= big) {
330         big=dum;
331         imax=i;
332       }
333     }
334     if (j != imax) {
335       for (k=0;k<3;k++) {
336         dum=LU.BasicGet(imax,k);
337         LU.BasicSet(imax,k,LU.BasicGet(j,k));
338         LU.BasicSet(j,k,dum);
339       }
340       vv[imax]=vv[j];
341     }
342     indx[j]=imax;
343     if (j != 3-1) {
344       dum=1.0/(LU.BasicGet(j,j));
345       for (i=j+1;i<3;i++) LU.BasicSet(i,j,LU.BasicGet(i,j)*dum);
346     }
347   }
348 }
349 
350 // friend of Mat4x4
FastLU(Mat4x4 & A,Mat4x4 & LU,int * indx)351 void FastLU(Mat4x4& A, Mat4x4& LU, int *indx){ // LU is the LU decomposition of A
352   int i,imax=0,j,k;
353   double big, dum, sum, temp;
354   double vv[10000];
355 
356   LU = A;
357   for (i=0;i<4;i++){
358     big=0.0;
359     for (j=0;j<4;j++){
360       temp=fabs(LU.BasicGet(i,j));
361       if (temp > big) big=temp;
362       }
363     vv[i]=1.0/big;
364   }
365   for (j=0;j<4;j++){
366     for (i=0;i<j;i++){
367       sum=LU.BasicGet(i,j);
368       for (k=0;k<i;k++) sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
369       LU.BasicSet(i,j,sum);
370     }
371     big=0.0;
372     for (i=j;i<4;i++){
373       sum=LU.BasicGet(i,j);
374       for (k=0;k<j;k++)
375         sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
376         LU.BasicSet(i,j,sum);
377         if ((dum=vv[i]*fabs(sum)) >= big) {
378         big=dum;
379         imax=i;
380       }
381     }
382     if (j != imax) {
383       for (k=0;k<4;k++) {
384         dum=LU.BasicGet(imax,k);
385         LU.BasicSet(imax,k,LU.BasicGet(j,k));
386         LU.BasicSet(j,k,dum);
387       }
388       vv[imax]=vv[j];
389     }
390     indx[j]=imax;
391     if (j != 4-1) {
392       dum=1.0/(LU.BasicGet(j,j));
393       for (i=j+1;i<4;i++) LU.BasicSet(i,j,LU.BasicGet(i,j)*dum);
394     }
395   }
396 }
397 
398 // friend of Mat6x6
FastLU(Mat6x6 & A,Mat6x6 & LU,int * indx)399 void FastLU(Mat6x6& A, Mat6x6& LU, int *indx){ // LU is the LU decomposition of A
400   int i,imax=0,j,k;
401   double big, dum, sum, temp;
402   double vv[10000];
403 
404   LU = A;
405   for (i=0;i<6;i++){
406     big=0.0;
407     for (j=0;j<6;j++){
408       temp=fabs(LU.BasicGet(i,j));
409       if (temp > big) big=temp;
410       }
411     vv[i]=1.0/big;
412   }
413   for (j=0;j<6;j++){
414     for (i=0;i<j;i++){
415       sum=LU.BasicGet(i,j);
416       for (k=0;k<i;k++) sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
417       LU.BasicSet(i,j,sum);
418     }
419     big=0.0;
420     for (i=j;i<6;i++){
421       sum=LU.BasicGet(i,j);
422       for (k=0;k<j;k++)
423         sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
424         LU.BasicSet(i,j,sum);
425         if ((dum=vv[i]*fabs(sum)) >= big) {
426         big=dum;
427         imax=i;
428       }
429     }
430     if (j != imax) {
431       for (k=0;k<6;k++) {
432         dum=LU.BasicGet(imax,k);
433         LU.BasicSet(imax,k,LU.BasicGet(j,k));
434         LU.BasicSet(j,k,dum);
435       }
436       vv[imax]=vv[j];
437     }
438     indx[j]=imax;
439     if (j != 6-1) {
440       dum=1.0/(LU.BasicGet(j,j));
441       for (i=j+1;i<6;i++) LU.BasicSet(i,j,LU.BasicGet(i,j)*dum);
442     }
443   }
444 }
445 
446 // friend of Matrix
FastLUSubs(Matrix & LU,Matrix & B,Matrix & C,int * indx)447 void FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
448   int i,ip,j,k;
449   int n = B.numrows;
450   int c = B.numcols;
451   double temp;
452 
453   C = B;
454   for (k=0;k<c;k++){
455 	for (i=0;i<n;i++){
456 		ip=indx[i];
457 		temp=C.rows[ip][k];
458 		C.rows[ip][k]=C.rows[i][k];
459 		for (j=0;j<i;j++) temp -= LU.rows[i][j]*C.rows[j][k];
460 		C.rows[i][k]=temp;
461 	}
462 	for (i=n-1;i>=0;i--){
463 		temp=C.rows[i][k];
464 		for (j=i+1;j<n;j++) temp -= LU.rows[i][j]*C.rows[j][k];
465 		C.rows[i][k]=temp/LU.rows[i][i];
466 	}
467   }
468 }
469 
470 // friend of Matrix and Mat3x3
FastLUSubs(Mat3x3 & LU,Matrix & B,Matrix & C,int * indx)471 void FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
472   int i,ip,j,k;
473   int n = B.numrows;
474   int c = B.numcols;
475   double temp;
476 
477   C = B;
478   for (k=0;k<c;k++){
479 	for (i=0;i<n;i++){
480 		ip=indx[i];
481 		temp=C.rows[ip][k];
482 		C.rows[ip][k]=C.rows[i][k];
483 		for (j=0;j<i;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
484 		C.rows[i][k]=temp;
485 	}
486 	for (i=n-1;i>=0;i--){
487 		temp=C.rows[i][k];
488 		for (j=i+1;j<n;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
489 		C.rows[i][k]=temp/LU.BasicGet(i,i);
490 	}
491   }
492 }
493 
494 // friend of Matrix and Mat4x4
FastLUSubs(Mat4x4 & LU,Matrix & B,Matrix & C,int * indx)495 void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
496   int i,ip,j,k;
497   int n = B.numrows;
498   int c = B.numcols;
499   double temp;
500 
501   C = B;
502   for (k=0;k<c;k++){
503 	for (i=0;i<n;i++){
504 		ip=indx[i];
505 		temp=C.rows[ip][k];
506 		C.rows[ip][k]=C.rows[i][k];
507 		for (j=0;j<i;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
508 		C.rows[i][k]=temp;
509 	}
510 	for (i=n-1;i>=0;i--){
511 		temp=C.rows[i][k];
512 		for (j=i+1;j<n;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
513 		C.rows[i][k]=temp/LU.BasicGet(i,i);
514 	}
515   }
516 }
517 
518 // friend of Matrix and Mat6x6
FastLUSubs(Mat6x6 & LU,Matrix & B,Matrix & C,int * indx)519 void FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
520   int i,ip,j,k;
521   int n = B.numrows;
522   int c = B.numcols;
523   double temp;
524 
525   C = B;
526   for (k=0;k<c;k++){
527 	for (i=0;i<n;i++){
528 		ip=indx[i];
529 		temp=C.rows[ip][k];
530 		C.rows[ip][k]=C.rows[i][k];
531 		for (j=0;j<i;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
532 		C.rows[i][k]=temp;
533 	}
534 	for (i=n-1;i>=0;i--){
535 		temp=C.rows[i][k];
536 		for (j=i+1;j<n;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
537 		C.rows[i][k]=temp/LU.BasicGet(i,i);
538 	}
539   }
540 }
541 
542 
543 // The following LUSubsLH routine is incomplete at the moment.
544 // friend of Matrix
FastLUSubsLH(Matrix & LU,Matrix & B,Matrix & C,int * indx)545 void FastLUSubsLH(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
546   int i,ip,j,k;
547   int n = B.numcols;
548   int c = B.numrows;
549   double temp;
550   Matrix C_temp;
551 
552   C_temp = B;
553   for (k=0;k<c;k++){
554 	for (i=0;i<n;i++){
555 		ip=indx[k];
556 	    temp=C_temp.rows[ip][i];
557 		C_temp.rows[ip][i]=C_temp.rows[k][i];
558 		for (j=0;j<i;j++) temp -= LU.rows[i][j]*C_temp.rows[k][j];
559 		C_temp.rows[k][i]=temp;
560 	}
561 	for (i=n-1;i>=0;i--){
562 		temp=C_temp.rows[k][i];
563 		for (j=i+1;j<n;j++) temp -= LU.rows[i][j]*C_temp.rows[k][j];
564 		C_temp.rows[k][i]=temp/LU.rows[i][i];
565 	}
566   }
567   C = C_temp;
568 }
569 
570 
571 
572 //
573 // Triple sum
574 //
575 
FastTripleSum(Vect3 & a,Vect3 & b,Vect3 & c,Vect3 & d)576 void FastTripleSum(Vect3& a, Vect3& b, Vect3& c, Vect3& d){ // d = a+b+c
577   d.elements[0] = a.elements[0]+b.elements[0]+c.elements[0];
578   d.elements[1] = a.elements[1]+b.elements[1]+c.elements[1];
579   d.elements[2] = a.elements[2]+b.elements[2]+c.elements[2];
580 }
581 
FastTripleSumPPM(Vect3 & a,Vect3 & b,Vect3 & c,Vect3 & d)582 void FastTripleSumPPM(Vect3& a, Vect3& b, Vect3& c, Vect3& d){ // d = a+b-c
583   d.elements[0] = a.elements[0]+b.elements[0]-c.elements[0];
584   d.elements[1] = a.elements[1]+b.elements[1]-c.elements[1];
585   d.elements[2] = a.elements[2]+b.elements[2]-c.elements[2];
586 }
587 
588 //
589 // Multiplications
590 //
591 
592 // friend of matrix
FastMult(Matrix & A,Matrix & B,Matrix & C)593 void FastMult(Matrix& A, Matrix& B, Matrix& C){  // C = A*B
594   // assumes dimensions are already correct!
595   int r = A.numrows;
596   int ca = A.numcols;
597   int cb = B.numcols;
598 
599   int i,j,k;
600   for(i=0;i<r;i++)
601     for(j=0;j<cb;j++){
602       C.rows[i][j] = 0.0;
603       for(k=0;k<ca;k++)
604         C.rows[i][j] += A.rows[i][k] * B.rows[k][j];
605     }
606 }
607 
608 // friend of matrix
FastTMult(Matrix & A,Matrix & B,Matrix & C)609 void FastTMult(Matrix& A, Matrix& B, Matrix& C){  // C = A*B
610   // assumes dimensions are already correct!
611   int r = A.numcols;
612   int ca = A.numrows;
613   int cb = B.numcols;
614 
615   int i,j,k;
616   for(i=0;i<r;i++)
617     for(j=0;j<cb;j++){
618       C.rows[i][j] = A.rows[0][i] * B.rows[0][j];
619       for(k=1;k<ca;k++)
620         C.rows[i][j] += A.rows[k][i] * B.rows[k][j];
621     }
622 }
623 
624 // friend of Mat3x3 & Vect3
FastMult(Mat3x3 & A,Vect3 & B,Vect3 & C)625 void FastMult(Mat3x3& A, Vect3& B, Vect3& C){  // C = A*B
626   C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2];
627   C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2];
628   C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
629 }
630 
631 // friend of Mat3x3, ColMatrix, & Vect3
FastMult(Mat3x3 & A,ColMatrix & B,Vect3 & C)632 void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C){  // C = A*B
633   C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2];
634   C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2];
635   C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
636 }
637 
638 
639 // friend of Mat3x3, ColMatrix, & Vect3
FastMult(Mat3x3 & A,Vect3 & B,ColMatrix & C)640 void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C){  // C = A*B
641   C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2];
642   C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2];
643   C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
644 }
645 
646 // friend of Mat3x3 & Vect3
FastTMult(Mat3x3 & A,Vect3 & B,Vect3 & C)647 void FastTMult(Mat3x3& A, Vect3& B, Vect3& C){  // C = A^T*B
648   C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[1][0]*B.elements[1] + A.elements[2][0]*B.elements[2];
649   C.elements[1] = A.elements[0][1]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[2][1]*B.elements[2];
650   C.elements[2] = A.elements[0][2]*B.elements[0] + A.elements[1][2]*B.elements[1] + A.elements[2][2]*B.elements[2];
651 }
652 
653 // friend of Mat3x3 & Vect3
FastNegMult(Mat3x3 & A,Vect3 & B,Vect3 & C)654 void FastNegMult(Mat3x3& A, Vect3& B, Vect3& C){  // C = -A*B
655   C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[0][1]*B.elements[1] - A.elements[0][2]*B.elements[2];
656   C.elements[1] = -A.elements[1][0]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[1][2]*B.elements[2];
657   C.elements[2] = -A.elements[2][0]*B.elements[0] - A.elements[2][1]*B.elements[1] - A.elements[2][2]*B.elements[2];
658 }
659 
660 // friend of Mat3x3 & Vect3
FastNegTMult(Mat3x3 & A,Vect3 & B,Vect3 & C)661 void FastNegTMult(Mat3x3& A, Vect3& B, Vect3& C){  // C = -A^T*B
662   C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[1][0]*B.elements[1] - A.elements[2][0]*B.elements[2];
663   C.elements[1] = -A.elements[0][1]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[2][1]*B.elements[2];
664   C.elements[2] = -A.elements[0][2]*B.elements[0] - A.elements[1][2]*B.elements[1] - A.elements[2][2]*B.elements[2];
665 }
666 
667 // friend of Vect3
FastMult(double a,Vect3 & B,Vect3 & C)668 void FastMult(double a, Vect3& B, Vect3& C){  // C = a*B
669   C.elements[0] = a*B.elements[0];
670   C.elements[1] = a*B.elements[1];
671   C.elements[2] = a*B.elements[2];
672 }
673 
674 // friend of Mat4x4 & Vect4
FastMult(Mat4x4 & A,Vect4 & B,Vect4 & C)675 void FastMult(Mat4x4& A, Vect4& B, Vect4& C){  // C = A*B
676   C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2] + A.elements[0][3]*B.elements[3];
677   C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2] + A.elements[1][3]*B.elements[3];
678   C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2] + A.elements[2][3]*B.elements[3];
679   C.elements[3] = A.elements[3][0]*B.elements[0] + A.elements[3][1]*B.elements[1] + A.elements[3][2]*B.elements[2] + A.elements[3][3]*B.elements[3];
680 }
681 
682 // friend of Mat4x4 & Vect4
FastTMult(Mat4x4 & A,Vect4 & B,Vect4 & C)683 void FastTMult(Mat4x4& A, Vect4& B, Vect4& C){  // C = A^T*B
684   C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[1][0]*B.elements[1] + A.elements[2][0]*B.elements[2] + A.elements[3][0]*B.elements[3];
685   C.elements[1] = A.elements[0][1]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[2][1]*B.elements[2] + A.elements[3][1]*B.elements[3];
686   C.elements[2] = A.elements[0][2]*B.elements[0] + A.elements[1][2]*B.elements[1] + A.elements[2][2]*B.elements[2] + A.elements[3][2]*B.elements[3];
687   C.elements[3] = A.elements[0][3]*B.elements[0] + A.elements[1][3]*B.elements[1] + A.elements[2][3]*B.elements[2] + A.elements[3][3]*B.elements[3];
688 }
689 
690 // friend of Mat4x4 & Vect4
FastNegMult(Mat4x4 & A,Vect4 & B,Vect4 & C)691 void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C){  // C = -A*B
692   C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[0][1]*B.elements[1] - A.elements[0][2]*B.elements[2] - A.elements[0][3]*B.elements[3];
693   C.elements[1] = -A.elements[1][0]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[1][2]*B.elements[2] - A.elements[1][3]*B.elements[3];
694   C.elements[2] = -A.elements[2][0]*B.elements[0] - A.elements[2][1]*B.elements[1] - A.elements[2][2]*B.elements[2] - A.elements[2][3]*B.elements[3];
695   C.elements[3] = -A.elements[3][0]*B.elements[0] - A.elements[3][1]*B.elements[1] - A.elements[3][2]*B.elements[2] - A.elements[3][3]*B.elements[3];
696 }
697 
698 // friend of Mat4x4 & Vect4
FastNegTMult(Mat4x4 & A,Vect4 & B,Vect4 & C)699 void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C){  // C = -A^T*B
700   C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[1][0]*B.elements[1] - A.elements[2][0]*B.elements[2] - A.elements[3][0]*B.elements[3];
701   C.elements[1] = -A.elements[0][1]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[2][1]*B.elements[2] - A.elements[3][1]*B.elements[3];
702   C.elements[2] = -A.elements[0][2]*B.elements[0] - A.elements[1][2]*B.elements[1] - A.elements[2][2]*B.elements[2] - A.elements[3][2]*B.elements[3];
703   C.elements[3] = -A.elements[0][3]*B.elements[0] - A.elements[1][3]*B.elements[1] - A.elements[2][3]*B.elements[2] - A.elements[3][3]*B.elements[3];
704 }
705 
706 // friend of Vect4
FastMult(double a,Vect4 & B,Vect4 & C)707 void FastMult(double a, Vect4& B, Vect4& C){  // C = a*B
708   C.elements[0] = a*B.elements[0];
709   C.elements[1] = a*B.elements[1];
710   C.elements[2] = a*B.elements[2];
711   C.elements[3] = a*B.elements[3];
712 }
713 
714 // friend of Matrix & Mat6x6
FastMultT(Matrix & A,Matrix & B,Mat6x6 & C)715 void FastMultT(Matrix& A, Matrix& B, Mat6x6& C){  // C = A*B^T
716   int i,j,k,n;
717   n = A.numcols;
718 
719   for(i=0;i<6;i++)
720     for(j=0;j<6;j++){
721       C.elements[i][j] = 0.0;
722       for(k=0;k<n;k++)
723         C.elements[i][j] += A.rows[i][k] * B.rows[j][k];
724     }
725 }
726 
727 // friend Matrix, Vect6, ColMatrix
FastMult(Matrix & A,ColMatrix & B,Vect6 & C)728 void FastMult(Matrix& A, ColMatrix& B, Vect6& C){
729   int ca = A.numcols;
730 
731   int i,k;
732   for(i=0;i<6;i++){
733     C.elements[i] = 0.0;
734     for(k=0;k<ca;k++)
735       C.elements[i] += A.rows[i][k] * B.elements[k];
736   }
737 }
738 
739 // friend of Matrix & Mat6x6
FastMult(Mat6x6 & A,Matrix & B,Matrix & C)740 void FastMult(Mat6x6& A, Matrix& B, Matrix& C){  // C = A*B
741   // assumes dimensions are already correct!
742   int cb = B.numcols;
743 
744   int i,j,k;
745   for(i=0;i<6;i++)
746     for(j=0;j<cb;j++){
747       C.rows[i][j] = 0.0;
748       for(k=0;k<6;k++)
749         C.rows[i][j] += A.elements[i][k] * B.rows[k][j];
750     }
751 }
752 
753 // friend Matrix, Vect6, ColMatrix
FastTMult(Matrix & A,Vect6 & B,ColMatrix & C)754 void FastTMult(Matrix& A, Vect6& B, ColMatrix& C){  // C = A^T*B
755   int n = C.numrows;
756   int i,k;
757   for(i=0;i<n;i++){
758     C.elements[i] = 0.0;
759     for(k=0;k<6;k++)
760       C.elements[i] += A.rows[k][i] * B.elements[k];
761   }
762 }
763 
764 // friend of Mat3x3
FastMult(Mat3x3 & A,Mat3x3 & B,Mat3x3 & C)765 void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C){  // C = A*B
766   C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[1][0] + A.elements[0][2]*B.elements[2][0];
767   C.elements[0][1] = A.elements[0][0]*B.elements[0][1] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[2][1];
768   C.elements[0][2] = A.elements[0][0]*B.elements[0][2] + A.elements[0][1]*B.elements[1][2] + A.elements[0][2]*B.elements[2][2];
769 
770   C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[1][0] + A.elements[1][2]*B.elements[2][0];
771   C.elements[1][1] = A.elements[1][0]*B.elements[0][1] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[2][1];
772   C.elements[1][2] = A.elements[1][0]*B.elements[0][2] + A.elements[1][1]*B.elements[1][2] + A.elements[1][2]*B.elements[2][2];
773 
774   C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[1][0] + A.elements[2][2]*B.elements[2][0];
775   C.elements[2][1] = A.elements[2][0]*B.elements[0][1] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[2][1];
776   C.elements[2][2] = A.elements[2][0]*B.elements[0][2] + A.elements[2][1]*B.elements[1][2] + A.elements[2][2]*B.elements[2][2];
777 }
778 
779 // friend of Mat3x3
FastMultT(Mat3x3 & A,Mat3x3 & B,Mat3x3 & C)780 void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C){  // C = A*B^T
781   C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[0][1] + A.elements[0][2]*B.elements[0][2];
782   C.elements[0][1] = A.elements[0][0]*B.elements[1][0] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[1][2];
783   C.elements[0][2] = A.elements[0][0]*B.elements[2][0] + A.elements[0][1]*B.elements[2][1] + A.elements[0][2]*B.elements[2][2];
784 
785   C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[0][1] + A.elements[1][2]*B.elements[0][2];
786   C.elements[1][1] = A.elements[1][0]*B.elements[1][0] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[1][2];
787   C.elements[1][2] = A.elements[1][0]*B.elements[2][0] + A.elements[1][1]*B.elements[2][1] + A.elements[1][2]*B.elements[2][2];
788 
789   C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[0][1] + A.elements[2][2]*B.elements[0][2];
790   C.elements[2][1] = A.elements[2][0]*B.elements[1][0] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[1][2];
791   C.elements[2][2] = A.elements[2][0]*B.elements[2][0] + A.elements[2][1]*B.elements[2][1] + A.elements[2][2]*B.elements[2][2];
792 }
793 
794 // friend of Mat4x4
FastMult(Mat4x4 & A,Mat4x4 & B,Mat4x4 & C)795 void FastMult(Mat4x4& A, Mat4x4& B, Mat4x4& C){  // C = A*B
796   C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[1][0] + A.elements[0][2]*B.elements[2][0] + A.elements[0][3]*B.elements[3][0];
797   C.elements[0][1] = A.elements[0][0]*B.elements[0][1] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[2][1] + A.elements[0][3]*B.elements[3][1];
798   C.elements[0][2] = A.elements[0][0]*B.elements[0][2] + A.elements[0][1]*B.elements[1][2] + A.elements[0][2]*B.elements[2][2] + A.elements[0][3]*B.elements[3][2];
799   C.elements[0][3] = A.elements[0][0]*B.elements[0][3] + A.elements[0][1]*B.elements[1][3] + A.elements[0][2]*B.elements[2][3] + A.elements[0][3]*B.elements[3][3];
800 
801   C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[1][0] + A.elements[1][2]*B.elements[2][0] + A.elements[1][3]*B.elements[3][0];
802   C.elements[1][1] = A.elements[1][0]*B.elements[0][1] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[2][1] + A.elements[1][3]*B.elements[3][1];
803   C.elements[1][2] = A.elements[1][0]*B.elements[0][2] + A.elements[1][1]*B.elements[1][2] + A.elements[1][2]*B.elements[2][2] + A.elements[1][3]*B.elements[3][2];
804   C.elements[1][3] = A.elements[1][0]*B.elements[0][3] + A.elements[1][1]*B.elements[1][3] + A.elements[1][2]*B.elements[2][3] + A.elements[1][3]*B.elements[3][3];
805 
806   C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[1][0] + A.elements[2][2]*B.elements[2][0] + A.elements[2][3]*B.elements[3][0];
807   C.elements[2][1] = A.elements[2][0]*B.elements[0][1] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[2][1] + A.elements[2][3]*B.elements[3][1];
808   C.elements[2][2] = A.elements[2][0]*B.elements[0][2] + A.elements[2][1]*B.elements[1][2] + A.elements[2][2]*B.elements[2][2] + A.elements[2][3]*B.elements[3][2];
809   C.elements[2][3] = A.elements[2][0]*B.elements[0][3] + A.elements[2][1]*B.elements[1][3] + A.elements[2][2]*B.elements[2][3] + A.elements[2][3]*B.elements[3][3];
810 
811   C.elements[3][0] = A.elements[3][0]*B.elements[0][0] + A.elements[3][1]*B.elements[1][0] + A.elements[3][2]*B.elements[2][0] + A.elements[3][3]*B.elements[3][0];
812   C.elements[3][1] = A.elements[3][0]*B.elements[0][1] + A.elements[3][1]*B.elements[1][1] + A.elements[3][2]*B.elements[2][1] + A.elements[3][3]*B.elements[3][1];
813   C.elements[3][2] = A.elements[3][0]*B.elements[0][2] + A.elements[3][1]*B.elements[1][2] + A.elements[3][2]*B.elements[2][2] + A.elements[3][3]*B.elements[3][2];
814   C.elements[3][3] = A.elements[3][0]*B.elements[0][3] + A.elements[3][1]*B.elements[1][3] + A.elements[3][2]*B.elements[2][3] + A.elements[3][3]*B.elements[3][3];
815 }
816 
817 // friend of Mat4x4
FastMultT(Mat4x4 & A,Mat4x4 & B,Mat4x4 & C)818 void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C){  // C = A*B^T
819   C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[0][1] + A.elements[0][2]*B.elements[0][2] + A.elements[0][3]*B.elements[0][3];
820   C.elements[0][1] = A.elements[0][0]*B.elements[1][0] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[1][2] + A.elements[0][3]*B.elements[1][3];
821   C.elements[0][2] = A.elements[0][0]*B.elements[2][0] + A.elements[0][1]*B.elements[2][1] + A.elements[0][2]*B.elements[2][2] + A.elements[0][3]*B.elements[2][3];
822   C.elements[0][3] = A.elements[0][0]*B.elements[3][0] + A.elements[0][1]*B.elements[3][1] + A.elements[0][2]*B.elements[3][2] + A.elements[0][3]*B.elements[3][3];
823 
824   C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[0][1] + A.elements[1][2]*B.elements[0][2] + A.elements[1][3]*B.elements[0][3];
825   C.elements[1][1] = A.elements[1][0]*B.elements[1][0] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[1][2] + A.elements[1][3]*B.elements[1][3];
826   C.elements[1][2] = A.elements[1][0]*B.elements[2][0] + A.elements[1][1]*B.elements[2][1] + A.elements[1][2]*B.elements[2][2] + A.elements[1][3]*B.elements[2][3];
827   C.elements[1][3] = A.elements[1][0]*B.elements[3][0] + A.elements[1][1]*B.elements[3][1] + A.elements[1][2]*B.elements[3][2] + A.elements[1][3]*B.elements[3][3];
828 
829   C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[0][1] + A.elements[2][2]*B.elements[0][2] + A.elements[2][3]*B.elements[0][3];
830   C.elements[2][1] = A.elements[2][0]*B.elements[1][0] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[1][2] + A.elements[2][3]*B.elements[1][3];
831   C.elements[2][2] = A.elements[2][0]*B.elements[2][0] + A.elements[2][1]*B.elements[2][1] + A.elements[2][2]*B.elements[2][2] + A.elements[2][3]*B.elements[2][3];
832   C.elements[2][3] = A.elements[2][0]*B.elements[3][0] + A.elements[2][1]*B.elements[3][1] + A.elements[2][2]*B.elements[3][2] + A.elements[2][3]*B.elements[3][3];
833 
834   C.elements[3][0] = A.elements[3][0]*B.elements[0][0] + A.elements[3][1]*B.elements[0][1] + A.elements[3][2]*B.elements[0][2] + A.elements[3][3]*B.elements[0][3];
835   C.elements[3][1] = A.elements[3][0]*B.elements[1][0] + A.elements[3][1]*B.elements[1][1] + A.elements[3][2]*B.elements[1][2] + A.elements[3][3]*B.elements[1][3];
836   C.elements[3][2] = A.elements[3][0]*B.elements[2][0] + A.elements[3][1]*B.elements[2][1] + A.elements[3][2]*B.elements[2][2] + A.elements[3][3]*B.elements[2][3];
837   C.elements[3][3] = A.elements[3][0]*B.elements[3][0] + A.elements[3][1]*B.elements[3][1] + A.elements[3][2]*B.elements[3][2] + A.elements[3][3]*B.elements[3][3];
838   C.elements[2][2] = A.elements[2][0]*B.elements[2][0] + A.elements[2][1]*B.elements[2][1] + A.elements[2][2]*B.elements[2][2];
839 }
840 
841 // friend of Mat6x6
FastMult(Mat6x6 & A,Mat6x6 & B,Mat6x6 & C)842 void FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){  // C = A*B
843   int i,j,k;
844   for(i=0;i<6;i++)
845     for(j=0;j<6;j++){
846       C.elements[i][j] = 0.0;
847       for(k=0;k<6;k++)
848         C.elements[i][j] += A.elements[i][k]*B.elements[k][j];
849     }
850 }
851 
852 // friend of Mat6x6
FastMultT(Mat6x6 & A,Mat6x6 & B,Mat6x6 & C)853 void FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C){  // C = A*B
854   int i,j,k;
855   for(i=0;i<6;i++)
856     for(j=0;j<6;j++){
857       C.elements[i][j] = 0.0;
858       for(k=0;k<6;k++)
859         C.elements[i][j] += A.elements[i][k]*B.elements[j][k];
860     }
861 }
862 
863 // friend of Mat6x6
FastTMult(Mat6x6 & A,Mat6x6 & B,Mat6x6 & C)864 void FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){  // C = A^T*B
865   int i,j,k;
866   for(i=0;i<6;i++)
867     for(j=0;j<6;j++){
868       C.elements[i][j] = 0.0;
869       for(k=0;k<6;k++)
870         C.elements[i][j] += A.elements[k][i]*B.elements[k][j];
871     }
872 }
873 
874 // friend of Mat6x6 & Vect6
FastMult(Mat6x6 & A,Vect6 & B,Vect6 & C)875 void FastMult(Mat6x6& A, Vect6& B, Vect6& C){  // C = A*B
876   for(int i=0;i<6;i++)
877     C.elements[i] = A.elements[i][0]*B.elements[0] + A.elements[i][1]*B.elements[1] + A.elements[i][2]*B.elements[2] + A.elements[i][3]*B.elements[3] + A.elements[i][4]*B.elements[4] + A.elements[i][5]*B.elements[5];
878 }
879 
880 // friend of Mat6x6 & Vect6
FastTMult(Mat6x6 & A,Vect6 & B,Vect6 & C)881 void FastTMult(Mat6x6& A, Vect6& B, Vect6& C){  // C = A^T*B
882   for(int i=0;i<6;i++)
883     C.elements[i] = A.elements[0][i]*B.elements[0] + A.elements[1][i]*B.elements[1] + A.elements[2][i]*B.elements[2] + A.elements[3][i]*B.elements[3] + A.elements[4][i]*B.elements[4] + A.elements[5][i]*B.elements[5];
884 }
885 
886 //
887 // Additions
888 //
889 
890 // friend of Vect3
FastAdd(Vect3 & A,Vect3 & B,Vect3 & C)891 void FastAdd(Vect3& A, Vect3& B, Vect3& C){ // C = A+B
892   C.elements[0] = A.elements[0] + B.elements[0];
893   C.elements[1] = A.elements[1] + B.elements[1];
894   C.elements[2] = A.elements[2] + B.elements[2];
895 }
896 
897 // friend of Vect4
FastAdd(Vect4 & A,Vect4 & B,Vect4 & C)898 void FastAdd(Vect4& A, Vect4& B, Vect4& C){ // C = A+B
899   C.elements[0] = A.elements[0] + B.elements[0];
900   C.elements[1] = A.elements[1] + B.elements[1];
901   C.elements[2] = A.elements[2] + B.elements[2];
902   C.elements[3] = A.elements[3] + B.elements[3];
903 }
904 
FastAdd(Mat6x6 & A,Mat6x6 & B,Mat6x6 & C)905 void FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C){  // C = A+B
906   int i,j;
907   for(i=0;i<6;i++)
908     for(j=0;j<6;j++)
909       C.elements[i][j] = A.elements[i][j] + B.elements[i][j];
910 }
911 
912 // friend of Vect6
FastAdd(Vect6 & A,Vect6 & B,Vect6 & C)913 void FastAdd(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
914   C.elements[0] = A.elements[0] + B.elements[0];
915   C.elements[1] = A.elements[1] + B.elements[1];
916   C.elements[2] = A.elements[2] + B.elements[2];
917   C.elements[3] = A.elements[3] + B.elements[3];
918   C.elements[4] = A.elements[4] + B.elements[4];
919   C.elements[5] = A.elements[5] + B.elements[5];
920 }
921 
922 //
923 // Subtractions
924 //
925 
926 // friend of Vect3
FastSubt(Vect3 & A,Vect3 & B,Vect3 & C)927 void FastSubt(Vect3& A, Vect3& B, Vect3& C){ // C = A-B
928   C.elements[0] = A.elements[0] - B.elements[0];
929   C.elements[1] = A.elements[1] - B.elements[1];
930   C.elements[2] = A.elements[2] - B.elements[2];
931 }
932 
933 // friend of Vect4
FastSubt(Vect4 & A,Vect4 & B,Vect4 & C)934 void FastSubt(Vect4& A, Vect4& B, Vect4& C){ // C = A-B
935   C.elements[0] = A.elements[0] - B.elements[0];
936   C.elements[1] = A.elements[1] - B.elements[1];
937   C.elements[2] = A.elements[2] - B.elements[2];
938   C.elements[3] = A.elements[3] - B.elements[3];
939 }
940 
FastSubt(Mat6x6 & A,Mat6x6 & B,Mat6x6 & C)941 void FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C){  // C = A-B
942   int i,j;
943   for(i=0;i<6;i++)
944     for(j=0;j<6;j++)
945       C.elements[i][j] = A.elements[i][j] - B.elements[i][j];
946 }
947 
948 // friend of Vect6
FastSubt(Vect6 & A,Vect6 & B,Vect6 & C)949 void FastSubt(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
950   C.elements[0] = A.elements[0] - B.elements[0];
951   C.elements[1] = A.elements[1] - B.elements[1];
952   C.elements[2] = A.elements[2] - B.elements[2];
953   C.elements[3] = A.elements[3] - B.elements[3];
954   C.elements[4] = A.elements[4] - B.elements[4];
955   C.elements[5] = A.elements[5] - B.elements[5];
956 }
957 
958 // friend of ColMatMap
FastAssign(ColMatMap & A,ColMatMap & C)959 void FastAssign(ColMatMap& A, ColMatMap& C){
960   for(int i=0;i<C.numrows;i++)
961     *(C.elements[i]) = *(A.elements[i]);
962 }
963 
964 // friend of ColMatrix
FastAssign(ColMatrix & A,ColMatrix & C)965 void FastAssign(ColMatrix& A, ColMatrix& C){ //C = A
966   for(int i=0;i<C.numrows;i++)
967     C.elements[i] = A.elements[i];
968 }
969 
970 // friend of Vect3
FastAssign(Vect3 & A,Vect3 & C)971 void FastAssign(Vect3& A, Vect3& C){ //C = A
972     C.elements[0] = A.elements[0];
973     C.elements[1] = A.elements[1];
974     C.elements[2] = A.elements[2];
975 }
976 
977 // friend of Vect3 & ColMatrix
FastAssign(ColMatrix & A,Vect3 & C)978 void FastAssign(ColMatrix& A, Vect3& C){ //C = A
979     C.elements[0] = A.elements[0];
980     C.elements[1] = A.elements[1];
981     C.elements[2] = A.elements[2];
982 }
983 
984 // friend of Vect4
FastAssign(Vect4 & A,Vect4 & C)985 void FastAssign(Vect4& A, Vect4& C){ //C = A
986     C.elements[0] = A.elements[0];
987     C.elements[1] = A.elements[1];
988     C.elements[2] = A.elements[2];
989     C.elements[3] = A.elements[3];
990 }
991 
992 // friend of Mat3x3
FastAssignT(Mat3x3 & A,Mat3x3 & C)993 void FastAssignT(Mat3x3& A, Mat3x3& C){
994   C.elements[0][0] = A.elements[0][0];
995   C.elements[1][1] = A.elements[1][1];
996   C.elements[2][2] = A.elements[2][2];
997 
998   C.elements[0][1] = A.elements[1][0];
999   C.elements[1][0] = A.elements[0][1];
1000 
1001   C.elements[0][2] = A.elements[2][0];
1002   C.elements[2][0] = A.elements[0][2];
1003 
1004   C.elements[1][2] = A.elements[2][1];
1005   C.elements[2][1] = A.elements[1][2];
1006 }
1007 
1008 // friend of Mat4x4
FastAssignT(Mat4x4 & A,Mat4x4 & C)1009 void FastAssignT(Mat4x4& A, Mat4x4& C){
1010   C.elements[0][0] = A.elements[0][0];
1011   C.elements[1][1] = A.elements[1][1];
1012   C.elements[2][2] = A.elements[2][2];
1013   C.elements[3][3] = A.elements[3][3];
1014 
1015   C.elements[0][1] = A.elements[1][0];
1016   C.elements[1][0] = A.elements[0][1];
1017 
1018   C.elements[0][2] = A.elements[2][0];
1019   C.elements[2][0] = A.elements[0][2];
1020 
1021   C.elements[0][3] = A.elements[3][0];
1022   C.elements[3][0] = A.elements[0][3];
1023 
1024   C.elements[1][2] = A.elements[2][1];
1025   C.elements[2][1] = A.elements[1][2];
1026 
1027   C.elements[1][3] = A.elements[3][1];
1028   C.elements[3][1] = A.elements[1][3];
1029 
1030   C.elements[2][3] = A.elements[3][2];
1031   C.elements[3][2] = A.elements[2][3];
1032 }
1033 
1034