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