Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F66216342
fastmatrixops.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sun, Jun 9, 01:54
Size
35 KB
Mime Type
text/x-c
Expires
Tue, Jun 11, 01:54 (2 d)
Engine
blob
Format
Raw Data
Handle
16675243
Attached To
rLAMMPS lammps
fastmatrixops.cpp
View Options
/*
*_________________________________________________________________________*
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: fastmatrixops.cpp *
* AUTHORS: See Author List *
* GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement *
* DOWNLOAD: Free at www.rpi.edu/~anderk5 *
* ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab *
* Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/
#include <iostream>
#include "fastmatrixops.h"
#include <cmath>
using
namespace
std
;
//
// Cross Product (friend of Vect3)
//
void
FastCross
(
Vect3
&
a
,
Vect3
&
b
,
Vect3
&
c
){
c
.
elements
[
0
]
=
a
.
elements
[
1
]
*
b
.
elements
[
2
]
-
a
.
elements
[
2
]
*
b
.
elements
[
1
];
c
.
elements
[
1
]
=
a
.
elements
[
2
]
*
b
.
elements
[
0
]
-
a
.
elements
[
0
]
*
b
.
elements
[
2
];
c
.
elements
[
2
]
=
a
.
elements
[
0
]
*
b
.
elements
[
1
]
-
a
.
elements
[
1
]
*
b
.
elements
[
0
];
}
//
// Simple Rotation (friend of Vect3 and Mat3x3)
//
void
FastSimpleRotation
(
Vect3
&
v
,
double
q
,
Mat3x3
&
C
){
// intermediate quantities
double
cq
=
cos
(
q
);
double
sq
=
sin
(
q
);
double
one_m_cq
=
1
-
cq
;
double
l12
=
v
.
elements
[
0
]
*
v
.
elements
[
1
]
*
one_m_cq
;
double
l13
=
v
.
elements
[
0
]
*
v
.
elements
[
2
]
*
one_m_cq
;
double
l23
=
v
.
elements
[
1
]
*
v
.
elements
[
2
]
*
one_m_cq
;
// the transformation
C
.
elements
[
0
][
0
]
=
v
.
elements
[
0
]
*
v
.
elements
[
0
]
*
one_m_cq
+
cq
;
C
.
elements
[
0
][
1
]
=
l12
-
v
.
elements
[
2
]
*
sq
;
C
.
elements
[
0
][
2
]
=
l13
+
v
.
elements
[
1
]
*
sq
;
C
.
elements
[
1
][
0
]
=
l12
+
v
.
elements
[
2
]
*
sq
;
C
.
elements
[
1
][
1
]
=
v
.
elements
[
1
]
*
v
.
elements
[
1
]
*
one_m_cq
+
cq
;
C
.
elements
[
1
][
2
]
=
l23
-
v
.
elements
[
0
]
*
sq
;
C
.
elements
[
2
][
0
]
=
l13
-
v
.
elements
[
1
]
*
sq
;
C
.
elements
[
2
][
1
]
=
l23
+
v
.
elements
[
0
]
*
sq
;
C
.
elements
[
2
][
2
]
=
v
.
elements
[
2
]
*
v
.
elements
[
2
]
*
one_m_cq
+
cq
;
}
//
// Quaternion Functions
//
void
FastQuaternions
(
ColMatrix
&
q
,
Mat3x3
&
C
){
double
*
e
=
q
.
elements
;
// normalize the quaternions
double
length
=
e
[
0
]
*
e
[
0
]
+
e
[
1
]
*
e
[
1
]
+
e
[
2
]
*
e
[
2
]
+
e
[
3
]
*
e
[
3
];
length
=
sqrt
(
length
);
e
[
0
]
=
e
[
0
]
/
length
;
e
[
1
]
=
e
[
1
]
/
length
;
e
[
2
]
=
e
[
2
]
/
length
;
e
[
3
]
=
e
[
3
]
/
length
;
// compute the transformation
C
.
elements
[
0
][
0
]
=
e
[
0
]
*
e
[
0
]
+
e
[
1
]
*
e
[
1
]
-
e
[
2
]
*
e
[
2
]
-
e
[
3
]
*
e
[
3
];
C
.
elements
[
1
][
1
]
=
e
[
0
]
*
e
[
0
]
-
e
[
1
]
*
e
[
1
]
+
e
[
2
]
*
e
[
2
]
-
e
[
3
]
*
e
[
3
];
C
.
elements
[
2
][
2
]
=
e
[
0
]
*
e
[
0
]
-
e
[
1
]
*
e
[
1
]
-
e
[
2
]
*
e
[
2
]
+
e
[
3
]
*
e
[
3
];
C
.
elements
[
0
][
1
]
=
2
*
(
e
[
1
]
*
e
[
2
]
-
e
[
0
]
*
e
[
3
]);
C
.
elements
[
0
][
2
]
=
2
*
(
e
[
1
]
*
e
[
3
]
+
e
[
0
]
*
e
[
2
]);
C
.
elements
[
1
][
2
]
=
2
*
(
e
[
2
]
*
e
[
3
]
-
e
[
0
]
*
e
[
1
]);
C
.
elements
[
1
][
0
]
=
2
*
(
e
[
1
]
*
e
[
2
]
+
e
[
0
]
*
e
[
3
]);
C
.
elements
[
2
][
0
]
=
2
*
(
e
[
1
]
*
e
[
3
]
-
e
[
0
]
*
e
[
2
]);
C
.
elements
[
2
][
1
]
=
2
*
(
e
[
2
]
*
e
[
3
]
+
e
[
0
]
*
e
[
1
]);
}
void
FastQuaternionDerivatives
(
ColMatrix
&
q
,
ColMatrix
&
omega
,
ColMatrix
&
qdot
){
double
*
w
=
omega
.
elements
;
double
*
e
=
q
.
elements
;
qdot
.
elements
[
0
]
=
0.5
*
(
-
w
[
0
]
*
e
[
1
]
-
w
[
1
]
*
e
[
2
]
-
w
[
2
]
*
e
[
3
]);
qdot
.
elements
[
1
]
=
0.5
*
(
w
[
0
]
*
e
[
0
]
+
w
[
2
]
*
e
[
2
]
-
w
[
1
]
*
e
[
3
]);
qdot
.
elements
[
2
]
=
0.5
*
(
w
[
1
]
*
e
[
0
]
-
w
[
2
]
*
e
[
1
]
+
w
[
0
]
*
e
[
3
]);
qdot
.
elements
[
3
]
=
0.5
*
(
w
[
2
]
*
e
[
0
]
+
w
[
1
]
*
e
[
1
]
-
w
[
0
]
*
e
[
2
]);
}
void
FastInvQuaternions
(
Mat3x3
&
C
,
ColMatrix
&
q
){
}
//
// Inverse
//
// friend of Matrix
//void FastInverse(Matrix& A, Matrix& C){ // C = A^(-1)
// C.rows[0][0] = 1/A.rows[0][0];
//}
//
// LDL^T Decomposition (from Golub and Van Loan)
//
// friend of Matrix
void
FastLDLT
(
Matrix
&
A
,
Matrix
&
C
){
// C is the LD of the LDL^T decomposition of A (SPD)
double
Lv
;
int
n
=
A
.
numrows
;
for
(
int
j
=
0
;
j
<
n
;
j
++
){
Lv
=
0.0
;
for
(
int
i
=
0
;
i
<
j
;
i
++
){
C
.
rows
[
i
][
j
]
=
C
.
rows
[
j
][
i
]
*
C
.
rows
[
i
][
i
];
Lv
+=
C
.
rows
[
j
][
i
]
*
C
.
rows
[
i
][
j
];
}
C
.
rows
[
j
][
j
]
=
A
.
rows
[
j
][
j
]
-
Lv
;
for
(
int
i
=
j
+
1
;
i
<
n
;
i
++
){
Lv
=
0.0
;
for
(
int
k
=
0
;
k
<
j
;
k
++
)
Lv
+=
C
.
rows
[
i
][
k
]
*
C
.
rows
[
k
][
j
];
C
.
rows
[
i
][
j
]
=
(
A
.
rows
[
i
][
j
]
-
Lv
)
/
C
.
rows
[
j
][
j
];
}
}
}
// friend of Mat6x6
void
FastLDLT
(
Mat6x6
&
A
,
Mat6x6
&
C
){
// C is the LD of the LDL^T decomposition of A (SPD)
double
v
[
6
];
double
Lv
;
for
(
int
j
=
0
;
j
<
6
;
j
++
){
Lv
=
0.0
;
for
(
int
i
=
0
;
i
<
j
;
i
++
){
v
[
i
]
=
C
.
elements
[
j
][
i
]
*
C
.
elements
[
i
][
i
];
Lv
+=
C
.
elements
[
j
][
i
]
*
v
[
i
];
}
v
[
j
]
=
A
.
elements
[
j
][
j
]
-
Lv
;
C
.
elements
[
j
][
j
]
=
v
[
j
];
for
(
int
i
=
j
+
1
;
i
<
6
;
i
++
){
Lv
=
0.0
;
for
(
int
k
=
0
;
k
<
j
;
k
++
)
Lv
+=
C
.
elements
[
i
][
k
]
*
v
[
k
];
C
.
elements
[
i
][
j
]
=
(
A
.
elements
[
i
][
j
]
-
Lv
)
/
v
[
j
];
}
}
}
// friend of Matrix
void
FastLDLTSubs
(
Matrix
&
LD
,
Matrix
&
B
,
Matrix
&
C
){
int
n
=
B
.
numrows
;
int
c
=
B
.
numcols
;
double
temp
;
for
(
int
k
=
0
;
k
<
c
;
k
++
){
for
(
int
i
=
0
;
i
<
n
;
i
++
){
temp
=
0.0
;
for
(
int
j
=
0
;
j
<
i
;
j
++
){
temp
+=
C
.
rows
[
j
][
k
]
*
LD
.
rows
[
i
][
j
];
}
C
.
rows
[
i
][
k
]
=
B
.
rows
[
i
][
k
]
-
temp
;
}
for
(
int
i
=
n
-
1
;
i
>-
1
;
i
--
){
C
.
rows
[
i
][
k
]
=
C
.
rows
[
i
][
k
]
/
LD
.
rows
[
i
][
i
];
temp
=
0.0
;
for
(
int
j
=
n
-
1
;
j
>
i
;
j
--
){
temp
+=
C
.
rows
[
j
][
k
]
*
LD
.
rows
[
j
][
i
];
}
C
.
rows
[
i
][
k
]
=
C
.
rows
[
i
][
k
]
-
temp
;
}
}
}
// friend of Matrix
void
FastLDLTSubsLH
(
Matrix
&
B
,
Matrix
&
LD
,
Matrix
&
C
){
int
n
=
B
.
numcols
;
int
c
=
B
.
numrows
;
double
temp
;
for
(
int
k
=
0
;
k
<
c
;
k
++
){
for
(
int
i
=
0
;
i
<
n
;
i
++
){
temp
=
0.0
;
for
(
int
j
=
0
;
j
<
i
;
j
++
){
temp
+=
C
.
rows
[
k
][
j
]
*
LD
.
rows
[
i
][
j
];
}
C
.
rows
[
k
][
i
]
=
B
.
rows
[
k
][
i
]
-
temp
;
}
for
(
int
i
=
n
-
1
;
i
>-
1
;
i
--
){
C
.
rows
[
k
][
i
]
=
C
.
rows
[
k
][
i
]
/
LD
.
rows
[
i
][
i
];
temp
=
0.0
;
for
(
int
j
=
n
-
1
;
j
>
i
;
j
--
){
temp
+=
C
.
rows
[
k
][
j
]
*
LD
.
rows
[
j
][
i
];
}
C
.
rows
[
k
][
i
]
=
C
.
rows
[
k
][
i
]
-
temp
;
}
}
}
// friend of Mat6x6
void
FastLDLTSubs
(
Mat6x6
&
LD
,
Mat6x6
&
B
,
Mat6x6
&
C
){
double
temp
;
for
(
int
k
=
0
;
k
<
6
;
k
++
){
for
(
int
i
=
0
;
i
<
6
;
i
++
){
temp
=
0.0
;
for
(
int
j
=
0
;
j
<
i
;
j
++
){
temp
+=
C
.
elements
[
j
][
k
]
*
LD
.
elements
[
i
][
j
];
}
C
.
elements
[
i
][
k
]
=
B
.
elements
[
i
][
k
]
-
temp
;
}
for
(
int
i
=
5
;
i
>-
1
;
i
--
){
C
.
elements
[
i
][
k
]
=
C
.
elements
[
i
][
k
]
/
LD
.
elements
[
i
][
i
];
temp
=
0.0
;
for
(
int
j
=
5
;
j
>
i
;
j
--
){
temp
+=
C
.
elements
[
j
][
k
]
*
LD
.
elements
[
j
][
i
];
}
C
.
elements
[
i
][
k
]
=
C
.
elements
[
i
][
k
]
-
temp
;
}
}
}
// friend of Mat6x6 & Vect6
void
FastLDLTSubs
(
Mat6x6
&
LD
,
Vect6
&
B
,
Vect6
&
C
){
double
temp
;
for
(
int
i
=
0
;
i
<
6
;
i
++
){
temp
=
0.0
;
for
(
int
j
=
0
;
j
<
i
;
j
++
){
temp
+=
C
.
elements
[
j
]
*
LD
.
elements
[
i
][
j
];
}
C
.
elements
[
i
]
=
B
.
elements
[
i
]
-
temp
;
}
for
(
int
i
=
5
;
i
>-
1
;
i
--
){
C
.
elements
[
i
]
=
C
.
elements
[
i
]
/
LD
.
elements
[
i
][
i
];
temp
=
0.0
;
for
(
int
j
=
5
;
j
>
i
;
j
--
){
temp
+=
C
.
elements
[
j
]
*
LD
.
elements
[
j
][
i
];
}
C
.
elements
[
i
]
=
C
.
elements
[
i
]
-
temp
;
}
}
// friend of Matrix
void
FastLU
(
Matrix
&
A
,
Matrix
&
LU
,
int
*
indx
){
// LU is the LU decomposition of A
int
i
,
imax
=
0
,
j
,
k
;
int
n
=
A
.
numrows
;
double
big
,
dum
,
sum
,
temp
;
double
vv
[
10000
];
LU
=
A
;
for
(
i
=
0
;
i
<
n
;
i
++
){
big
=
0.0
;
for
(
j
=
0
;
j
<
n
;
j
++
){
temp
=
fabs
(
LU
.
rows
[
i
][
j
]);
if
(
temp
>
big
)
big
=
temp
;
}
vv
[
i
]
=
1.0
/
big
;
}
for
(
j
=
0
;
j
<
n
;
j
++
){
for
(
i
=
0
;
i
<
j
;
i
++
){
sum
=
LU
.
rows
[
i
][
j
];
for
(
k
=
0
;
k
<
i
;
k
++
)
sum
-=
LU
.
rows
[
i
][
k
]
*
LU
.
rows
[
k
][
j
];
LU
.
rows
[
i
][
j
]
=
sum
;
}
big
=
0.0
;
for
(
i
=
j
;
i
<
n
;
i
++
){
sum
=
LU
.
rows
[
i
][
j
];
for
(
k
=
0
;
k
<
j
;
k
++
)
sum
-=
LU
.
rows
[
i
][
k
]
*
LU
.
rows
[
k
][
j
];
LU
.
rows
[
i
][
j
]
=
sum
;
if
((
dum
=
vv
[
i
]
*
fabs
(
sum
))
>=
big
)
{
big
=
dum
;
imax
=
i
;
}
}
if
(
j
!=
imax
)
{
for
(
k
=
0
;
k
<
n
;
k
++
)
{
dum
=
LU
.
rows
[
imax
][
k
];
LU
.
rows
[
imax
][
k
]
=
LU
.
rows
[
j
][
k
];
LU
.
rows
[
j
][
k
]
=
dum
;
}
vv
[
imax
]
=
vv
[
j
];
}
indx
[
j
]
=
imax
;
// if (LU.rows[j][j] == 0.0) LU.rows[j][j]=1.0e-20;
if
(
j
!=
n
-
1
)
{
dum
=
1.0
/
(
LU
.
rows
[
j
][
j
]);
for
(
i
=
j
+
1
;
i
<
n
;
i
++
)
LU
.
rows
[
i
][
j
]
*=
dum
;
}
}
}
// friend of Mat3x3
void
FastLU
(
Mat3x3
&
A
,
Mat3x3
&
LU
,
int
*
indx
){
// LU is the LU decomposition of A
int
i
,
imax
=
0
,
j
,
k
;
double
big
,
dum
,
sum
,
temp
;
double
vv
[
10000
];
LU
=
A
;
for
(
i
=
0
;
i
<
3
;
i
++
){
big
=
0.0
;
for
(
j
=
0
;
j
<
3
;
j
++
){
temp
=
fabs
(
LU
.
BasicGet
(
i
,
j
));
if
(
temp
>
big
)
big
=
temp
;
}
vv
[
i
]
=
1.0
/
big
;
}
for
(
j
=
0
;
j
<
3
;
j
++
){
for
(
i
=
0
;
i
<
j
;
i
++
){
sum
=
LU
.
BasicGet
(
i
,
j
);
for
(
k
=
0
;
k
<
i
;
k
++
)
sum
-=
LU
.
BasicGet
(
i
,
k
)
*
LU
.
BasicGet
(
k
,
j
);
LU
.
BasicSet
(
i
,
j
,
sum
);
}
big
=
0.0
;
for
(
i
=
j
;
i
<
3
;
i
++
){
sum
=
LU
.
BasicGet
(
i
,
j
);
for
(
k
=
0
;
k
<
j
;
k
++
)
sum
-=
LU
.
BasicGet
(
i
,
k
)
*
LU
.
BasicGet
(
k
,
j
);
LU
.
BasicSet
(
i
,
j
,
sum
);
if
((
dum
=
vv
[
i
]
*
fabs
(
sum
))
>=
big
)
{
big
=
dum
;
imax
=
i
;
}
}
if
(
j
!=
imax
)
{
for
(
k
=
0
;
k
<
3
;
k
++
)
{
dum
=
LU
.
BasicGet
(
imax
,
k
);
LU
.
BasicSet
(
imax
,
k
,
LU
.
BasicGet
(
j
,
k
));
LU
.
BasicSet
(
j
,
k
,
dum
);
}
vv
[
imax
]
=
vv
[
j
];
}
indx
[
j
]
=
imax
;
if
(
j
!=
3
-
1
)
{
dum
=
1.0
/
(
LU
.
BasicGet
(
j
,
j
));
for
(
i
=
j
+
1
;
i
<
3
;
i
++
)
LU
.
BasicSet
(
i
,
j
,
LU
.
BasicGet
(
i
,
j
)
*
dum
);
}
}
}
// friend of Mat4x4
void
FastLU
(
Mat4x4
&
A
,
Mat4x4
&
LU
,
int
*
indx
){
// LU is the LU decomposition of A
int
i
,
imax
=
0
,
j
,
k
;
double
big
,
dum
,
sum
,
temp
;
double
vv
[
10000
];
LU
=
A
;
for
(
i
=
0
;
i
<
4
;
i
++
){
big
=
0.0
;
for
(
j
=
0
;
j
<
4
;
j
++
){
temp
=
fabs
(
LU
.
BasicGet
(
i
,
j
));
if
(
temp
>
big
)
big
=
temp
;
}
vv
[
i
]
=
1.0
/
big
;
}
for
(
j
=
0
;
j
<
4
;
j
++
){
for
(
i
=
0
;
i
<
j
;
i
++
){
sum
=
LU
.
BasicGet
(
i
,
j
);
for
(
k
=
0
;
k
<
i
;
k
++
)
sum
-=
LU
.
BasicGet
(
i
,
k
)
*
LU
.
BasicGet
(
k
,
j
);
LU
.
BasicSet
(
i
,
j
,
sum
);
}
big
=
0.0
;
for
(
i
=
j
;
i
<
4
;
i
++
){
sum
=
LU
.
BasicGet
(
i
,
j
);
for
(
k
=
0
;
k
<
j
;
k
++
)
sum
-=
LU
.
BasicGet
(
i
,
k
)
*
LU
.
BasicGet
(
k
,
j
);
LU
.
BasicSet
(
i
,
j
,
sum
);
if
((
dum
=
vv
[
i
]
*
fabs
(
sum
))
>=
big
)
{
big
=
dum
;
imax
=
i
;
}
}
if
(
j
!=
imax
)
{
for
(
k
=
0
;
k
<
4
;
k
++
)
{
dum
=
LU
.
BasicGet
(
imax
,
k
);
LU
.
BasicSet
(
imax
,
k
,
LU
.
BasicGet
(
j
,
k
));
LU
.
BasicSet
(
j
,
k
,
dum
);
}
vv
[
imax
]
=
vv
[
j
];
}
indx
[
j
]
=
imax
;
if
(
j
!=
4
-
1
)
{
dum
=
1.0
/
(
LU
.
BasicGet
(
j
,
j
));
for
(
i
=
j
+
1
;
i
<
4
;
i
++
)
LU
.
BasicSet
(
i
,
j
,
LU
.
BasicGet
(
i
,
j
)
*
dum
);
}
}
}
// friend of Mat6x6
void
FastLU
(
Mat6x6
&
A
,
Mat6x6
&
LU
,
int
*
indx
){
// LU is the LU decomposition of A
int
i
,
imax
=
0
,
j
,
k
;
double
big
,
dum
,
sum
,
temp
;
double
vv
[
10000
];
LU
=
A
;
for
(
i
=
0
;
i
<
6
;
i
++
){
big
=
0.0
;
for
(
j
=
0
;
j
<
6
;
j
++
){
temp
=
fabs
(
LU
.
BasicGet
(
i
,
j
));
if
(
temp
>
big
)
big
=
temp
;
}
vv
[
i
]
=
1.0
/
big
;
}
for
(
j
=
0
;
j
<
6
;
j
++
){
for
(
i
=
0
;
i
<
j
;
i
++
){
sum
=
LU
.
BasicGet
(
i
,
j
);
for
(
k
=
0
;
k
<
i
;
k
++
)
sum
-=
LU
.
BasicGet
(
i
,
k
)
*
LU
.
BasicGet
(
k
,
j
);
LU
.
BasicSet
(
i
,
j
,
sum
);
}
big
=
0.0
;
for
(
i
=
j
;
i
<
6
;
i
++
){
sum
=
LU
.
BasicGet
(
i
,
j
);
for
(
k
=
0
;
k
<
j
;
k
++
)
sum
-=
LU
.
BasicGet
(
i
,
k
)
*
LU
.
BasicGet
(
k
,
j
);
LU
.
BasicSet
(
i
,
j
,
sum
);
if
((
dum
=
vv
[
i
]
*
fabs
(
sum
))
>=
big
)
{
big
=
dum
;
imax
=
i
;
}
}
if
(
j
!=
imax
)
{
for
(
k
=
0
;
k
<
6
;
k
++
)
{
dum
=
LU
.
BasicGet
(
imax
,
k
);
LU
.
BasicSet
(
imax
,
k
,
LU
.
BasicGet
(
j
,
k
));
LU
.
BasicSet
(
j
,
k
,
dum
);
}
vv
[
imax
]
=
vv
[
j
];
}
indx
[
j
]
=
imax
;
if
(
j
!=
6
-
1
)
{
dum
=
1.0
/
(
LU
.
BasicGet
(
j
,
j
));
for
(
i
=
j
+
1
;
i
<
6
;
i
++
)
LU
.
BasicSet
(
i
,
j
,
LU
.
BasicGet
(
i
,
j
)
*
dum
);
}
}
}
// friend of Matrix
void
FastLUSubs
(
Matrix
&
LU
,
Matrix
&
B
,
Matrix
&
C
,
int
*
indx
){
// Appropriate Forward and Back Substitution
int
i
,
ip
,
j
,
k
;
int
n
=
B
.
numrows
;
int
c
=
B
.
numcols
;
double
temp
;
C
=
B
;
for
(
k
=
0
;
k
<
c
;
k
++
){
for
(
i
=
0
;
i
<
n
;
i
++
){
ip
=
indx
[
i
];
temp
=
C
.
rows
[
ip
][
k
];
C
.
rows
[
ip
][
k
]
=
C
.
rows
[
i
][
k
];
for
(
j
=
0
;
j
<
i
;
j
++
)
temp
-=
LU
.
rows
[
i
][
j
]
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
;
}
for
(
i
=
n
-
1
;
i
>=
0
;
i
--
){
temp
=
C
.
rows
[
i
][
k
];
for
(
j
=
i
+
1
;
j
<
n
;
j
++
)
temp
-=
LU
.
rows
[
i
][
j
]
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
/
LU
.
rows
[
i
][
i
];
}
}
}
// friend of Matrix and Mat3x3
void
FastLUSubs
(
Mat3x3
&
LU
,
Matrix
&
B
,
Matrix
&
C
,
int
*
indx
){
// Appropriate Forward and Back Substitution
int
i
,
ip
,
j
,
k
;
int
n
=
B
.
numrows
;
int
c
=
B
.
numcols
;
double
temp
;
C
=
B
;
for
(
k
=
0
;
k
<
c
;
k
++
){
for
(
i
=
0
;
i
<
n
;
i
++
){
ip
=
indx
[
i
];
temp
=
C
.
rows
[
ip
][
k
];
C
.
rows
[
ip
][
k
]
=
C
.
rows
[
i
][
k
];
for
(
j
=
0
;
j
<
i
;
j
++
)
temp
-=
LU
.
BasicGet
(
i
,
j
)
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
;
}
for
(
i
=
n
-
1
;
i
>=
0
;
i
--
){
temp
=
C
.
rows
[
i
][
k
];
for
(
j
=
i
+
1
;
j
<
n
;
j
++
)
temp
-=
LU
.
BasicGet
(
i
,
j
)
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
/
LU
.
BasicGet
(
i
,
i
);
}
}
}
// friend of Matrix and Mat4x4
void
FastLUSubs
(
Mat4x4
&
LU
,
Matrix
&
B
,
Matrix
&
C
,
int
*
indx
){
// Appropriate Forward and Back Substitution
int
i
,
ip
,
j
,
k
;
int
n
=
B
.
numrows
;
int
c
=
B
.
numcols
;
double
temp
;
C
=
B
;
for
(
k
=
0
;
k
<
c
;
k
++
){
for
(
i
=
0
;
i
<
n
;
i
++
){
ip
=
indx
[
i
];
temp
=
C
.
rows
[
ip
][
k
];
C
.
rows
[
ip
][
k
]
=
C
.
rows
[
i
][
k
];
for
(
j
=
0
;
j
<
i
;
j
++
)
temp
-=
LU
.
BasicGet
(
i
,
j
)
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
;
}
for
(
i
=
n
-
1
;
i
>=
0
;
i
--
){
temp
=
C
.
rows
[
i
][
k
];
for
(
j
=
i
+
1
;
j
<
n
;
j
++
)
temp
-=
LU
.
BasicGet
(
i
,
j
)
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
/
LU
.
BasicGet
(
i
,
i
);
}
}
}
// friend of Matrix and Mat6x6
void
FastLUSubs
(
Mat6x6
&
LU
,
Matrix
&
B
,
Matrix
&
C
,
int
*
indx
){
// Appropriate Forward and Back Substitution
int
i
,
ip
,
j
,
k
;
int
n
=
B
.
numrows
;
int
c
=
B
.
numcols
;
double
temp
;
C
=
B
;
for
(
k
=
0
;
k
<
c
;
k
++
){
for
(
i
=
0
;
i
<
n
;
i
++
){
ip
=
indx
[
i
];
temp
=
C
.
rows
[
ip
][
k
];
C
.
rows
[
ip
][
k
]
=
C
.
rows
[
i
][
k
];
for
(
j
=
0
;
j
<
i
;
j
++
)
temp
-=
LU
.
BasicGet
(
i
,
j
)
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
;
}
for
(
i
=
n
-
1
;
i
>=
0
;
i
--
){
temp
=
C
.
rows
[
i
][
k
];
for
(
j
=
i
+
1
;
j
<
n
;
j
++
)
temp
-=
LU
.
BasicGet
(
i
,
j
)
*
C
.
rows
[
j
][
k
];
C
.
rows
[
i
][
k
]
=
temp
/
LU
.
BasicGet
(
i
,
i
);
}
}
}
// The following LUSubsLH routine is incomplete at the moment.
// friend of Matrix
void
FastLUSubsLH
(
Matrix
&
LU
,
Matrix
&
B
,
Matrix
&
C
,
int
*
indx
){
// Appropriate Forward and Back Substitution
int
i
,
ip
,
j
,
k
;
int
n
=
B
.
numcols
;
int
c
=
B
.
numrows
;
double
temp
;
Matrix
C_temp
;
C_temp
=
B
;
for
(
k
=
0
;
k
<
c
;
k
++
){
for
(
i
=
0
;
i
<
n
;
i
++
){
ip
=
indx
[
k
];
temp
=
C_temp
.
rows
[
ip
][
i
];
C_temp
.
rows
[
ip
][
i
]
=
C_temp
.
rows
[
k
][
i
];
for
(
j
=
0
;
j
<
i
;
j
++
)
temp
-=
LU
.
rows
[
i
][
j
]
*
C_temp
.
rows
[
k
][
j
];
C_temp
.
rows
[
k
][
i
]
=
temp
;
}
for
(
i
=
n
-
1
;
i
>=
0
;
i
--
){
temp
=
C_temp
.
rows
[
k
][
i
];
for
(
j
=
i
+
1
;
j
<
n
;
j
++
)
temp
-=
LU
.
rows
[
i
][
j
]
*
C_temp
.
rows
[
k
][
j
];
C_temp
.
rows
[
k
][
i
]
=
temp
/
LU
.
rows
[
i
][
i
];
}
}
C
=
C_temp
;
}
//
// Triple sum
//
void
FastTripleSum
(
Vect3
&
a
,
Vect3
&
b
,
Vect3
&
c
,
Vect3
&
d
){
// d = a+b+c
d
.
elements
[
0
]
=
a
.
elements
[
0
]
+
b
.
elements
[
0
]
+
c
.
elements
[
0
];
d
.
elements
[
1
]
=
a
.
elements
[
1
]
+
b
.
elements
[
1
]
+
c
.
elements
[
1
];
d
.
elements
[
2
]
=
a
.
elements
[
2
]
+
b
.
elements
[
2
]
+
c
.
elements
[
2
];
}
void
FastTripleSumPPM
(
Vect3
&
a
,
Vect3
&
b
,
Vect3
&
c
,
Vect3
&
d
){
// d = a+b-c
d
.
elements
[
0
]
=
a
.
elements
[
0
]
+
b
.
elements
[
0
]
-
c
.
elements
[
0
];
d
.
elements
[
1
]
=
a
.
elements
[
1
]
+
b
.
elements
[
1
]
-
c
.
elements
[
1
];
d
.
elements
[
2
]
=
a
.
elements
[
2
]
+
b
.
elements
[
2
]
-
c
.
elements
[
2
];
}
//
// Multiplications
//
// friend of matrix
void
FastMult
(
Matrix
&
A
,
Matrix
&
B
,
Matrix
&
C
){
// C = A*B
// assumes dimensions are already correct!
int
r
=
A
.
numrows
;
int
ca
=
A
.
numcols
;
int
cb
=
B
.
numcols
;
int
i
,
j
,
k
;
for
(
i
=
0
;
i
<
r
;
i
++
)
for
(
j
=
0
;
j
<
cb
;
j
++
){
C
.
rows
[
i
][
j
]
=
0.0
;
for
(
k
=
0
;
k
<
ca
;
k
++
)
C
.
rows
[
i
][
j
]
+=
A
.
rows
[
i
][
k
]
*
B
.
rows
[
k
][
j
];
}
}
// friend of matrix
void
FastTMult
(
Matrix
&
A
,
Matrix
&
B
,
Matrix
&
C
){
// C = A*B
// assumes dimensions are already correct!
int
r
=
A
.
numcols
;
int
ca
=
A
.
numrows
;
int
cb
=
B
.
numcols
;
int
i
,
j
,
k
;
for
(
i
=
0
;
i
<
r
;
i
++
)
for
(
j
=
0
;
j
<
cb
;
j
++
){
C
.
rows
[
i
][
j
]
=
A
.
rows
[
0
][
i
]
*
B
.
rows
[
0
][
j
];
for
(
k
=
1
;
k
<
ca
;
k
++
)
C
.
rows
[
i
][
j
]
+=
A
.
rows
[
k
][
i
]
*
B
.
rows
[
k
][
j
];
}
}
// friend of Mat3x3 & Vect3
void
FastMult
(
Mat3x3
&
A
,
Vect3
&
B
,
Vect3
&
C
){
// C = A*B
C
.
elements
[
0
]
=
A
.
elements
[
0
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
0
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
0
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
1
]
=
A
.
elements
[
1
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
1
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
1
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
2
]
=
A
.
elements
[
2
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
2
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
2
][
2
]
*
B
.
elements
[
2
];
}
// friend of Mat3x3, ColMatrix, & Vect3
void
FastMult
(
Mat3x3
&
A
,
ColMatrix
&
B
,
Vect3
&
C
){
// C = A*B
C
.
elements
[
0
]
=
A
.
elements
[
0
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
0
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
0
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
1
]
=
A
.
elements
[
1
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
1
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
1
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
2
]
=
A
.
elements
[
2
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
2
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
2
][
2
]
*
B
.
elements
[
2
];
}
// friend of Mat3x3, ColMatrix, & Vect3
void
FastMult
(
Mat3x3
&
A
,
Vect3
&
B
,
ColMatrix
&
C
){
// C = A*B
C
.
elements
[
0
]
=
A
.
elements
[
0
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
0
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
0
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
1
]
=
A
.
elements
[
1
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
1
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
1
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
2
]
=
A
.
elements
[
2
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
2
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
2
][
2
]
*
B
.
elements
[
2
];
}
// friend of Mat3x3 & Vect3
void
FastTMult
(
Mat3x3
&
A
,
Vect3
&
B
,
Vect3
&
C
){
// C = A^T*B
C
.
elements
[
0
]
=
A
.
elements
[
0
][
0
]
*
B
.
elements
[
0
]
+
A
.
elements
[
1
][
0
]
*
B
.
elements
[
1
]
+
A
.
elements
[
2
][
0
]
*
B
.
elements
[
2
];
C
.
elements
[
1
]
=
A
.
elements
[
0
][
1
]
*
B
.
elements
[
0
]
+
A
.
elements
[
1
][
1
]
*
B
.
elements
[
1
]
+
A
.
elements
[
2
][
1
]
*
B
.
elements
[
2
];
C
.
elements
[
2
]
=
A
.
elements
[
0
][
2
]
*
B
.
elements
[
0
]
+
A
.
elements
[
1
][
2
]
*
B
.
elements
[
1
]
+
A
.
elements
[
2
][
2
]
*
B
.
elements
[
2
];
}
// friend of Mat3x3 & Vect3
void
FastNegMult
(
Mat3x3
&
A
,
Vect3
&
B
,
Vect3
&
C
){
// C = -A*B
C
.
elements
[
0
]
=
-
A
.
elements
[
0
][
0
]
*
B
.
elements
[
0
]
-
A
.
elements
[
0
][
1
]
*
B
.
elements
[
1
]
-
A
.
elements
[
0
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
1
]
=
-
A
.
elements
[
1
][
0
]
*
B
.
elements
[
0
]
-
A
.
elements
[
1
][
1
]
*
B
.
elements
[
1
]
-
A
.
elements
[
1
][
2
]
*
B
.
elements
[
2
];
C
.
elements
[
2
]
=
-
A
.
elements
[
2
][
0
]
*
B
.
elements
[
0
]
-
A
.
elements
[
2
][
1
]
*
B
.
elements
[
1
]
-
A
.
elements
[
2
][
2
]
*
B
.
elements
[
2
];
}
// friend of Mat3x3 & Vect3
void
FastNegTMult
(
Mat3x3
&
A
,
Vect3
&
B
,
Vect3
&
C
){
// C = -A^T*B
C
.
elements
[
0
]
=
-
A
.
elements
[
0
][
0
]
*
B
.
elements
[
0
]
-
A
.
elements
[
1
][
0
]
*
B
.
elements
[
1
]
-
A
.
elements
[
2
][
0
]
*
B
.
elements
[
2
];
C
.
elements
[
1
]
=
-
A
.
elements
[
0
][
1
]
*
B
.
elements
[
0
]
-
A
.
elements
[
1
][
1
]
*
B
.
elements
[
1
]
-
A
.
elements
[
2
][
1
]
*
B
.
elements
[
2
];
C
.
elements
[
2
]
=
-
A
.
elements
[
0
][
2
]
*
B
.
elements
[
0
]
-
A
.
elements
[
1
][
2
]
*
B
.
elements
[
1
]
-
A
.
elements
[
2
][
2
]
*
B
.
elements
[
2
];
}
// friend of Vect3
void
FastMult
(
double
a
,
Vect3
&
B
,
Vect3
&
C
){
// C = a*B
C
.
elements
[
0
]
=
a
*
B
.
elements
[
0
];
C
.
elements
[
1
]
=
a
*
B
.
elements
[
1
];
C
.
elements
[
2
]
=
a
*
B
.
elements
[
2
];
}
// friend of Mat4x4 & Vect4
void
FastMult
(
Mat4x4
&
A
,
Vect4
&
B
,
Vect4
&
C
){
// C = A*B
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
];
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
];
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
];
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
];
}
// friend of Mat4x4 & Vect4
void
FastTMult
(
Mat4x4
&
A
,
Vect4
&
B
,
Vect4
&
C
){
// C = A^T*B
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
];
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
];
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
];
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
];
}
// friend of Mat4x4 & Vect4
void
FastNegMult
(
Mat4x4
&
A
,
Vect4
&
B
,
Vect4
&
C
){
// C = -A*B
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
];
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
];
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
];
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
];
}
// friend of Mat4x4 & Vect4
void
FastNegTMult
(
Mat4x4
&
A
,
Vect4
&
B
,
Vect4
&
C
){
// C = -A^T*B
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
];
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
];
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
];
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
];
}
// friend of Vect4
void
FastMult
(
double
a
,
Vect4
&
B
,
Vect4
&
C
){
// C = a*B
C
.
elements
[
0
]
=
a
*
B
.
elements
[
0
];
C
.
elements
[
1
]
=
a
*
B
.
elements
[
1
];
C
.
elements
[
2
]
=
a
*
B
.
elements
[
2
];
C
.
elements
[
3
]
=
a
*
B
.
elements
[
3
];
}
// friend of Matrix & Mat6x6
void
FastMultT
(
Matrix
&
A
,
Matrix
&
B
,
Mat6x6
&
C
){
// C = A*B^T
int
i
,
j
,
k
,
n
;
n
=
A
.
numcols
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
6
;
j
++
){
C
.
elements
[
i
][
j
]
=
0.0
;
for
(
k
=
0
;
k
<
n
;
k
++
)
C
.
elements
[
i
][
j
]
+=
A
.
rows
[
i
][
k
]
*
B
.
rows
[
j
][
k
];
}
}
// friend Matrix, Vect6, ColMatrix
void
FastMult
(
Matrix
&
A
,
ColMatrix
&
B
,
Vect6
&
C
){
int
ca
=
A
.
numcols
;
int
i
,
k
;
for
(
i
=
0
;
i
<
6
;
i
++
){
C
.
elements
[
i
]
=
0.0
;
for
(
k
=
0
;
k
<
ca
;
k
++
)
C
.
elements
[
i
]
+=
A
.
rows
[
i
][
k
]
*
B
.
elements
[
k
];
}
}
// friend of Matrix & Mat6x6
void
FastMult
(
Mat6x6
&
A
,
Matrix
&
B
,
Matrix
&
C
){
// C = A*B
// assumes dimensions are already correct!
int
cb
=
B
.
numcols
;
int
i
,
j
,
k
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
cb
;
j
++
){
C
.
rows
[
i
][
j
]
=
0.0
;
for
(
k
=
0
;
k
<
6
;
k
++
)
C
.
rows
[
i
][
j
]
+=
A
.
elements
[
i
][
k
]
*
B
.
rows
[
k
][
j
];
}
}
// friend Matrix, Vect6, ColMatrix
void
FastTMult
(
Matrix
&
A
,
Vect6
&
B
,
ColMatrix
&
C
){
// C = A^T*B
int
n
=
C
.
numrows
;
int
i
,
k
;
for
(
i
=
0
;
i
<
n
;
i
++
){
C
.
elements
[
i
]
=
0.0
;
for
(
k
=
0
;
k
<
6
;
k
++
)
C
.
elements
[
i
]
+=
A
.
rows
[
k
][
i
]
*
B
.
elements
[
k
];
}
}
// friend of Mat3x3
void
FastMult
(
Mat3x3
&
A
,
Mat3x3
&
B
,
Mat3x3
&
C
){
// C = A*B
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
}
// friend of Mat3x3
void
FastMultT
(
Mat3x3
&
A
,
Mat3x3
&
B
,
Mat3x3
&
C
){
// C = A*B^T
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
}
// friend of Mat4x4
void
FastMult
(
Mat4x4
&
A
,
Mat4x4
&
B
,
Mat4x4
&
C
){
// C = A*B
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
}
// friend of Mat4x4
void
FastMultT
(
Mat4x4
&
A
,
Mat4x4
&
B
,
Mat4x4
&
C
){
// C = A*B^T
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
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
];
}
// friend of Mat6x6
void
FastMult
(
Mat6x6
&
A
,
Mat6x6
&
B
,
Mat6x6
&
C
){
// C = A*B
int
i
,
j
,
k
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
6
;
j
++
){
C
.
elements
[
i
][
j
]
=
0.0
;
for
(
k
=
0
;
k
<
6
;
k
++
)
C
.
elements
[
i
][
j
]
+=
A
.
elements
[
i
][
k
]
*
B
.
elements
[
k
][
j
];
}
}
// friend of Mat6x6
void
FastMultT
(
Mat6x6
&
A
,
Mat6x6
&
B
,
Mat6x6
&
C
){
// C = A*B
int
i
,
j
,
k
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
6
;
j
++
){
C
.
elements
[
i
][
j
]
=
0.0
;
for
(
k
=
0
;
k
<
6
;
k
++
)
C
.
elements
[
i
][
j
]
+=
A
.
elements
[
i
][
k
]
*
B
.
elements
[
j
][
k
];
}
}
// friend of Mat6x6
void
FastTMult
(
Mat6x6
&
A
,
Mat6x6
&
B
,
Mat6x6
&
C
){
// C = A^T*B
int
i
,
j
,
k
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
6
;
j
++
){
C
.
elements
[
i
][
j
]
=
0.0
;
for
(
k
=
0
;
k
<
6
;
k
++
)
C
.
elements
[
i
][
j
]
+=
A
.
elements
[
k
][
i
]
*
B
.
elements
[
k
][
j
];
}
}
// friend of Mat6x6 & Vect6
void
FastMult
(
Mat6x6
&
A
,
Vect6
&
B
,
Vect6
&
C
){
// C = A*B
for
(
int
i
=
0
;
i
<
6
;
i
++
)
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
];
}
// friend of Mat6x6 & Vect6
void
FastTMult
(
Mat6x6
&
A
,
Vect6
&
B
,
Vect6
&
C
){
// C = A^T*B
for
(
int
i
=
0
;
i
<
6
;
i
++
)
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
];
}
//
// Additions
//
// friend of Vect3
void
FastAdd
(
Vect3
&
A
,
Vect3
&
B
,
Vect3
&
C
){
// C = A+B
C
.
elements
[
0
]
=
A
.
elements
[
0
]
+
B
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
]
+
B
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
]
+
B
.
elements
[
2
];
}
// friend of Vect4
void
FastAdd
(
Vect4
&
A
,
Vect4
&
B
,
Vect4
&
C
){
// C = A+B
C
.
elements
[
0
]
=
A
.
elements
[
0
]
+
B
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
]
+
B
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
]
+
B
.
elements
[
2
];
C
.
elements
[
3
]
=
A
.
elements
[
3
]
+
B
.
elements
[
3
];
}
void
FastAdd
(
Mat6x6
&
A
,
Mat6x6
&
B
,
Mat6x6
&
C
){
// C = A+B
int
i
,
j
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
6
;
j
++
)
C
.
elements
[
i
][
j
]
=
A
.
elements
[
i
][
j
]
+
B
.
elements
[
i
][
j
];
}
// friend of Vect6
void
FastAdd
(
Vect6
&
A
,
Vect6
&
B
,
Vect6
&
C
){
// C = A-B
C
.
elements
[
0
]
=
A
.
elements
[
0
]
+
B
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
]
+
B
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
]
+
B
.
elements
[
2
];
C
.
elements
[
3
]
=
A
.
elements
[
3
]
+
B
.
elements
[
3
];
C
.
elements
[
4
]
=
A
.
elements
[
4
]
+
B
.
elements
[
4
];
C
.
elements
[
5
]
=
A
.
elements
[
5
]
+
B
.
elements
[
5
];
}
//
// Subtractions
//
// friend of Vect3
void
FastSubt
(
Vect3
&
A
,
Vect3
&
B
,
Vect3
&
C
){
// C = A-B
C
.
elements
[
0
]
=
A
.
elements
[
0
]
-
B
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
]
-
B
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
]
-
B
.
elements
[
2
];
}
// friend of Vect4
void
FastSubt
(
Vect4
&
A
,
Vect4
&
B
,
Vect4
&
C
){
// C = A-B
C
.
elements
[
0
]
=
A
.
elements
[
0
]
-
B
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
]
-
B
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
]
-
B
.
elements
[
2
];
C
.
elements
[
3
]
=
A
.
elements
[
3
]
-
B
.
elements
[
3
];
}
void
FastSubt
(
Mat6x6
&
A
,
Mat6x6
&
B
,
Mat6x6
&
C
){
// C = A-B
int
i
,
j
;
for
(
i
=
0
;
i
<
6
;
i
++
)
for
(
j
=
0
;
j
<
6
;
j
++
)
C
.
elements
[
i
][
j
]
=
A
.
elements
[
i
][
j
]
-
B
.
elements
[
i
][
j
];
}
// friend of Vect6
void
FastSubt
(
Vect6
&
A
,
Vect6
&
B
,
Vect6
&
C
){
// C = A-B
C
.
elements
[
0
]
=
A
.
elements
[
0
]
-
B
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
]
-
B
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
]
-
B
.
elements
[
2
];
C
.
elements
[
3
]
=
A
.
elements
[
3
]
-
B
.
elements
[
3
];
C
.
elements
[
4
]
=
A
.
elements
[
4
]
-
B
.
elements
[
4
];
C
.
elements
[
5
]
=
A
.
elements
[
5
]
-
B
.
elements
[
5
];
}
// friend of ColMatMap
void
FastAssign
(
ColMatMap
&
A
,
ColMatMap
&
C
){
for
(
int
i
=
0
;
i
<
C
.
numrows
;
i
++
)
*
(
C
.
elements
[
i
])
=
*
(
A
.
elements
[
i
]);
}
// friend of ColMatrix
void
FastAssign
(
ColMatrix
&
A
,
ColMatrix
&
C
){
//C = A
for
(
int
i
=
0
;
i
<
C
.
numrows
;
i
++
)
C
.
elements
[
i
]
=
A
.
elements
[
i
];
}
// friend of Vect3
void
FastAssign
(
Vect3
&
A
,
Vect3
&
C
){
//C = A
C
.
elements
[
0
]
=
A
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
];
}
// friend of Vect3 & ColMatrix
void
FastAssign
(
ColMatrix
&
A
,
Vect3
&
C
){
//C = A
C
.
elements
[
0
]
=
A
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
];
}
// friend of Vect4
void
FastAssign
(
Vect4
&
A
,
Vect4
&
C
){
//C = A
C
.
elements
[
0
]
=
A
.
elements
[
0
];
C
.
elements
[
1
]
=
A
.
elements
[
1
];
C
.
elements
[
2
]
=
A
.
elements
[
2
];
C
.
elements
[
3
]
=
A
.
elements
[
3
];
}
// friend of Mat3x3
void
FastAssignT
(
Mat3x3
&
A
,
Mat3x3
&
C
){
C
.
elements
[
0
][
0
]
=
A
.
elements
[
0
][
0
];
C
.
elements
[
1
][
1
]
=
A
.
elements
[
1
][
1
];
C
.
elements
[
2
][
2
]
=
A
.
elements
[
2
][
2
];
C
.
elements
[
0
][
1
]
=
A
.
elements
[
1
][
0
];
C
.
elements
[
1
][
0
]
=
A
.
elements
[
0
][
1
];
C
.
elements
[
0
][
2
]
=
A
.
elements
[
2
][
0
];
C
.
elements
[
2
][
0
]
=
A
.
elements
[
0
][
2
];
C
.
elements
[
1
][
2
]
=
A
.
elements
[
2
][
1
];
C
.
elements
[
2
][
1
]
=
A
.
elements
[
1
][
2
];
}
// friend of Mat4x4
void
FastAssignT
(
Mat4x4
&
A
,
Mat4x4
&
C
){
C
.
elements
[
0
][
0
]
=
A
.
elements
[
0
][
0
];
C
.
elements
[
1
][
1
]
=
A
.
elements
[
1
][
1
];
C
.
elements
[
2
][
2
]
=
A
.
elements
[
2
][
2
];
C
.
elements
[
3
][
3
]
=
A
.
elements
[
3
][
3
];
C
.
elements
[
0
][
1
]
=
A
.
elements
[
1
][
0
];
C
.
elements
[
1
][
0
]
=
A
.
elements
[
0
][
1
];
C
.
elements
[
0
][
2
]
=
A
.
elements
[
2
][
0
];
C
.
elements
[
2
][
0
]
=
A
.
elements
[
0
][
2
];
C
.
elements
[
0
][
3
]
=
A
.
elements
[
3
][
0
];
C
.
elements
[
3
][
0
]
=
A
.
elements
[
0
][
3
];
C
.
elements
[
1
][
2
]
=
A
.
elements
[
2
][
1
];
C
.
elements
[
2
][
1
]
=
A
.
elements
[
1
][
2
];
C
.
elements
[
1
][
3
]
=
A
.
elements
[
3
][
1
];
C
.
elements
[
3
][
1
]
=
A
.
elements
[
1
][
3
];
C
.
elements
[
2
][
3
]
=
A
.
elements
[
3
][
2
];
C
.
elements
[
3
][
2
]
=
A
.
elements
[
2
][
3
];
}
Event Timeline
Log In to Comment