forked from lijiext/lammps
409 lines
8.1 KiB
C++
409 lines
8.1 KiB
C++
/*
|
|
*_________________________________________________________________________*
|
|
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
|
|
* DESCRIPTION: SEE READ-ME *
|
|
* FILE NAME: matrixfun.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 "matrixfun.h"
|
|
#include <math.h>
|
|
#include "fastmatrixops.h"
|
|
#include <cstdlib>
|
|
|
|
using namespace std;
|
|
|
|
//
|
|
// Create a new matrix
|
|
//
|
|
|
|
VirtualMatrix* NewMatrix(int type){
|
|
switch( MatrixType(type) )
|
|
{
|
|
case MATRIX : return new Matrix;
|
|
case COLMATRIX : return new ColMatrix;
|
|
case ROWMATRIX : return new RowMatrix;
|
|
case MAT3X3 : return new Mat3x3;
|
|
case MAT4X4 : return new Mat4x4;
|
|
case VECT3 : return new Vect3;
|
|
case VECT4 : return new Vect4;
|
|
default : return 0; // error!
|
|
}
|
|
}
|
|
|
|
//
|
|
// Transpose
|
|
//
|
|
|
|
Matrix T(const VirtualMatrix& A){
|
|
int numrows = A.GetNumRows();
|
|
int numcols = A.GetNumCols();
|
|
Matrix C(numcols,numrows);
|
|
for(int i=0;i<numcols;i++)
|
|
for(int j=0;j<numrows;j++)
|
|
C.BasicSet(i,j,A.BasicGet(j,i));
|
|
return C;
|
|
}
|
|
|
|
Mat3x3 T(const 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[0][2] = A.elements[2][0];
|
|
C.elements[1][2] = A.elements[2][1];
|
|
|
|
C.elements[1][0] = A.elements[0][1];
|
|
C.elements[2][0] = A.elements[0][2];
|
|
C.elements[2][1] = A.elements[1][2];
|
|
return C;
|
|
}
|
|
|
|
Mat6x6 T(const Mat6x6& A){
|
|
Mat6x6 C;
|
|
int i,j;
|
|
for(i=0;i<6;i++)
|
|
for(j=0;j<6;j++)
|
|
C.elements[i][j] = A.elements[j][i];
|
|
|
|
return C;
|
|
}
|
|
|
|
Matrix T(const Vect3& A){
|
|
Matrix C(1,3);
|
|
C.BasicSet(0,0,A.elements[0]);
|
|
C.BasicSet(0,1,A.elements[1]);
|
|
C.BasicSet(0,2,A.elements[2]);
|
|
|
|
return C;
|
|
}
|
|
|
|
Matrix T(const Vect6& A){
|
|
Matrix C(1,6);
|
|
C.BasicSet(0,0,A.elements[0]);
|
|
C.BasicSet(0,1,A.elements[1]);
|
|
C.BasicSet(0,2,A.elements[2]);
|
|
C.BasicSet(0,3,A.elements[3]);
|
|
C.BasicSet(0,4,A.elements[4]);
|
|
C.BasicSet(0,5,A.elements[5]);
|
|
|
|
return C;
|
|
}
|
|
|
|
RowMatrix T(const VirtualColMatrix &A){
|
|
int numele = A.GetNumRows();
|
|
RowMatrix C(numele);
|
|
for(int i=0;i<numele;i++)
|
|
C.BasicSet(i,A.BasicGet(i));
|
|
return C;
|
|
}
|
|
|
|
ColMatrix T(const VirtualRowMatrix &A){
|
|
int numele = A.GetNumCols();
|
|
ColMatrix C(numele);
|
|
for(int i=0;i<numele;i++)
|
|
C.BasicSet(i,A.BasicGet(i));
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// Symmetric Inverse
|
|
//
|
|
|
|
Matrix SymInverse(Matrix &A){
|
|
int r = A.GetNumRows();
|
|
Matrix C(r,r);
|
|
Matrix LD(r,r);
|
|
Matrix I(r,r);
|
|
I.Zeros();
|
|
for(int i=0;i<r;i++) I.BasicSet(i,i,1.0);
|
|
FastLDLT(A,LD);
|
|
FastLDLTSubs(LD,I,C);
|
|
return C;
|
|
}
|
|
|
|
Mat6x6 SymInverse(Mat6x6 &A){
|
|
Mat6x6 C;
|
|
Mat6x6 LD;
|
|
Mat6x6 I;
|
|
I.Zeros();
|
|
for(int i=0;i<6;i++) I.BasicSet(i,i,1.0);
|
|
FastLDLT(A,LD);
|
|
FastLDLTSubs(LD,I,C);
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// Inverse
|
|
//
|
|
|
|
Matrix Inverse(Matrix& A){
|
|
int r = A.GetNumRows();
|
|
int indx[10000];
|
|
Matrix LU(r,r);
|
|
Matrix I(r,r);
|
|
Matrix C(r,r);
|
|
I.Zeros();
|
|
for(int i=0;i<r;i++) I.BasicSet(i,i,1.0);
|
|
FastLU(A,LU,indx);
|
|
FastLUSubs(LU,I,C,indx);
|
|
return C;
|
|
}
|
|
|
|
Mat3x3 Inverse(Mat3x3& A){
|
|
int indx[10000];
|
|
Mat3x3 LU;
|
|
Matrix I(3,3);
|
|
Matrix C(3,3);
|
|
I.Zeros();
|
|
for(int i=0;i<3;i++) I.BasicSet(i,i,1.0);
|
|
FastLU(A,LU,indx);
|
|
FastLUSubs(LU,I,C,indx);
|
|
return C;
|
|
}
|
|
|
|
Mat4x4 Inverse(Mat4x4& A){
|
|
int indx[10000];
|
|
Mat4x4 LU;
|
|
Matrix I(4,4);
|
|
Matrix C(4,4);
|
|
I.Zeros();
|
|
for(int i=0;i<4;i++) I.BasicSet(i,i,1.0);
|
|
FastLU(A,LU,indx);
|
|
FastLUSubs(LU,I,C,indx);
|
|
return C;
|
|
}
|
|
|
|
Mat6x6 Inverse(Mat6x6& A){
|
|
int indx[10000];
|
|
Mat6x6 LU;
|
|
Matrix I(6,6);
|
|
Matrix C(6,6);
|
|
I.Zeros();
|
|
for(int i=0;i<6;i++) I.BasicSet(i,i,1.0);
|
|
FastLU(A,LU,indx);
|
|
FastLUSubs(LU,I,C,indx);
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// overloaded addition
|
|
//
|
|
|
|
Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B){ // addition
|
|
int Arows,Acols,Brows,Bcols;
|
|
Arows = A.GetNumRows();
|
|
Acols = A.GetNumCols();
|
|
Brows = B.GetNumRows();
|
|
Bcols = B.GetNumCols();
|
|
|
|
if( !((Arows == Brows) && (Acols == Bcols)) ){
|
|
cerr << "Dimesion mismatch in matrix addition" << endl;
|
|
exit(1);
|
|
}
|
|
|
|
Matrix C(Arows,Acols);
|
|
|
|
for(int i=0;i<Arows;i++)
|
|
for(int j=0;j<Acols;j++)
|
|
C.BasicSet(i,j,A.BasicGet(i,j) + B.BasicGet(i,j));
|
|
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// overloaded subtraction
|
|
//
|
|
|
|
Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B){ // subtraction
|
|
int Arows,Acols,Brows,Bcols;
|
|
Arows = A.GetNumRows();
|
|
Acols = A.GetNumCols();
|
|
Brows = B.GetNumRows();
|
|
Bcols = B.GetNumCols();
|
|
|
|
if( !((Arows == Brows) && (Acols == Bcols)) ){
|
|
cerr << "Dimesion mismatch in matrix addition" << endl;
|
|
exit(1);
|
|
}
|
|
|
|
Matrix C(Arows,Acols);
|
|
|
|
for(int i=0;i<Arows;i++)
|
|
for(int j=0;j<Acols;j++)
|
|
C.BasicSet(i,j,A.BasicGet(i,j) - B.BasicGet(i,j));
|
|
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// overloaded matrix multiplication
|
|
//
|
|
|
|
Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B){ // multiplication
|
|
int Arows,Acols,Brows,Bcols;
|
|
Arows = A.GetNumRows();
|
|
Acols = A.GetNumCols();
|
|
Brows = B.GetNumRows();
|
|
Bcols = B.GetNumCols();
|
|
|
|
if(Acols != Brows){
|
|
cerr << "Dimesion mismatch in matrix multiplication" << endl;
|
|
exit(1);
|
|
}
|
|
|
|
Matrix C(Arows,Bcols);
|
|
C.Zeros();
|
|
|
|
for(int i=0;i<Arows;i++)
|
|
for(int j=0;j<Bcols;j++)
|
|
for(int k=0;k<Brows;k++)
|
|
C.BasicIncrement(i,j, A.BasicGet(i,k) * B.BasicGet(k,j) );
|
|
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// overloaded scalar multiplication
|
|
//
|
|
|
|
Matrix operator* (const VirtualMatrix &A, double b){ // multiplication
|
|
Matrix C = A;
|
|
C *= b;
|
|
return C;
|
|
}
|
|
|
|
Matrix operator* (double b, const VirtualMatrix &A){ // multiplication
|
|
Matrix C = A;
|
|
C *= b;
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// overloaded negative
|
|
//
|
|
|
|
Matrix operator- (const VirtualMatrix& A){ // negative
|
|
int r = A.GetNumRows();
|
|
int c = A.GetNumCols();
|
|
Matrix C(r,c);
|
|
|
|
for(int i=0;i<r;i++)
|
|
for(int j=0;j<c;j++)
|
|
C.BasicSet(i,j,-A.BasicGet(i,j));
|
|
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// Cross product (friend of Vect3)
|
|
//
|
|
|
|
Vect3 Cross(Vect3& a, Vect3& b){
|
|
return CrossMat(a)*b;
|
|
}
|
|
|
|
//
|
|
// Cross Matrix (friend of Vect3 & Mat3x3)
|
|
//
|
|
|
|
Mat3x3 CrossMat(Vect3& a){
|
|
Mat3x3 C;
|
|
C.Zeros();
|
|
C.elements[0][1] = -a.elements[2];
|
|
C.elements[1][0] = a.elements[2];
|
|
C.elements[0][2] = a.elements[1];
|
|
C.elements[2][0] = -a.elements[1];
|
|
C.elements[1][2] = -a.elements[0];
|
|
C.elements[2][1] = a.elements[0];
|
|
|
|
return C;
|
|
}
|
|
|
|
//
|
|
// Stack
|
|
//
|
|
|
|
Matrix Stack(VirtualMatrix& A, VirtualMatrix& B){
|
|
int m,na,nb;
|
|
m = A.GetNumCols();
|
|
if( m != B.GetNumCols()){
|
|
cerr << "Error: cannot stack matrices of differing column dimension" << endl;
|
|
exit(0);
|
|
}
|
|
na = A.GetNumRows();
|
|
nb = B.GetNumRows();
|
|
|
|
Matrix C(na+nb,m);
|
|
|
|
for(int i=0;i<na;i++){
|
|
for(int j=0;j<m;j++){
|
|
C.BasicSet(i,j,A.BasicGet(i,j));
|
|
}
|
|
}
|
|
|
|
for(int i=0;i<nb;i++){
|
|
for(int j=0;j<m;j++){
|
|
C.BasicSet(i+na,j,B.BasicGet(i,j));
|
|
}
|
|
}
|
|
|
|
return C;
|
|
}
|
|
|
|
//Hstack
|
|
|
|
Matrix HStack(VirtualMatrix& A, VirtualMatrix& B){
|
|
int m,na,nb;
|
|
m = A.GetNumRows();
|
|
if( m != B.GetNumRows()){
|
|
cerr << "Error: cannot stack matrices of differing row dimension" << endl;
|
|
exit(0);
|
|
}
|
|
na = A.GetNumCols();
|
|
nb = B.GetNumCols();
|
|
|
|
Matrix C(m,na+nb);
|
|
|
|
for(int i=0;i<m;i++){
|
|
for(int j=0;j<na;j++){
|
|
C.BasicSet(i,j,A.BasicGet(i,j));
|
|
}
|
|
}
|
|
|
|
for(int i=0;i<m;i++){
|
|
for(int j=0;j<nb;j++){
|
|
C.BasicSet(i,j+na,B.BasicGet(i,j));
|
|
}
|
|
}
|
|
|
|
return C;
|
|
}
|
|
|
|
//
|
|
//
|
|
|
|
void Set6DAngularVector(Vect6& v6, Vect3& v3){
|
|
v6.elements[0] = v3.elements[0];
|
|
v6.elements[1] = v3.elements[1];
|
|
v6.elements[2] = v3.elements[2];
|
|
}
|
|
|
|
void Set6DLinearVector(Vect6& v6, Vect3& v3){
|
|
v6.elements[3] = v3.elements[0];
|
|
v6.elements[4] = v3.elements[1];
|
|
v6.elements[5] = v3.elements[2];
|
|
}
|
|
|