jianmu
/
lammps
forked from lijiext/lammps
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
lammps/lib/poems/fastmatrixops.cpp

1034 lines
36 KiB
C++
Raw Blame History

/*
*_________________________________________________________________________*
* 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];
}
Reference in New Issue View Git Blame Copy Permalink