vector<float> projectVectorOnPlane (vector<float> vec,vector<float> axis1,vector<float> axis2)
{
float projAxis1;
float projAxis2;
vector <float> axis1Norm ;
vector <float> axis2Norm ;
vector <float> projAxis1Vec ;
vector <float> projAxis2Vec ;
vector <float> projectedVector ;
projAxis1 = projVector(vec,axis1);
projAxis2 = projVector(vec,axis2);
axis1Norm = normVector(axis1);
axis2Norm = normVector(axis2);
projAxis1Vec =scalarVector(axis1Norm,projAxis1);
projAxis2Vec =scalarVector(axis2Norm,projAxis2);
projectedVector = vectorSum(projAxis1Vec,projAxis2Vec);
return projectedVector;
}
Vector normalized(const Vector& a)
{
Vector normVector(a.size, a.orientation);
double normVal = norm(a);
for(int i = 0; i < a.size; ++i)
normVector.values[i] = a.values[i] / normVal;
return normVector;
}
int main (int argc, char **argv)
{
/* Matrix data. */
CRS_MATRIX *M1, *M2;
double *b1, *x1, *b2, *x2, *r;
int n;
int nvals;
FILE *fp;
if (argc == 1) {
fp = stdin;
}
else if (argc == 2) {
fp = fopen (*argv, "r");
}
else {
fprintf (stderr, "Usage: pardisoTestExample [<fileName>]\n");
exit (1);
}
M1 = readCRSMatrix (fp, /*symmetric=*/1);
n = M1->nrows;
b1 = readVector (fp, n);
x1 = (double*)malloc(n*sizeof(double));
r = (double*)malloc(n*sizeof(double));
// ja -> M1->colIdxs;
// a -> M1->vals;
// ia -> M1->rowOffs
int mtype = -2; /* Real symmetric matrix */
int nrhs = 1; /* Number of right hand sides. */
/* Internal solver memory pointer pt, */
/* 32-bit: int pt[64]; 64-bit: long int pt[64] */
/* or void *pt[64] should be OK on both architectures */
void *pt[64];
/* Pardiso control parameters. */
int iparm[64];
int maxfct, mnum, phase, error, msglvl;
/* Number of processors. */
int num_procs;
/* Auxiliary variables. */
char *var;
int i;
double ddum; /* Double dummy */
int idum; /* Integer dummy. */
double t0, t1;
/* -------------------------------------------------------------------- */
/* .. Setup Pardiso control parameters. */
/* -------------------------------------------------------------------- */
error = 0;
//solver = 0; /* use sparse direct solver */
// do we need this, or will init take care of it?
for (i=0; i<64; i++) {
pt[i] = (void*)0;
}
for (i=0; i<64; i++) {
iparm[i] = 0;
}
iparm[0] = 1; /* Don't use solver default values */
iparm[1] = 3; /* Fill-in reordering from OpenMP METIS */
/* Numbers of processors; if 0, defaults to max number or MKL_NUM_THREADS */
iparm[2] = 0;
iparm[3] = 0; /* No iterative-direct algorithm */
iparm[4] = 0; /* No user fill-in reducing permutation */
iparm[5] = 0; /* Write solution into x */
iparm[6] = 0; /* Not in use */
iparm[7] = 0; /* Max numbers of iterative refinement steps */
iparm[8] = 0; /* Not in use */
iparm[9] = 13; /* Perturb the pivot elements with 1E-13 */
iparm[10] = 1; /* Use nonsymmetric permutation and scaling MPS */
iparm[11] = 0; /* Not in use */
iparm[12] = 0; /* Maximum weighted matching algorithm is switched-off (default for symmetric). Try iparm[12] = 1 in case of inappropriate accuracy */
iparm[13] = 0; /* Output: Number of perturbed pivots */
iparm[14] = 0; /* Not in use */
iparm[15] = 0; /* Not in use */
iparm[16] = 0; /* Not in use */
iparm[17] = -1; /* Output: Number of nonzeros in the factor LU */
iparm[18] = -1; /* Output: Mflops for LU factorization */
iparm[19] = 0; /* Output: Numbers of CG Iterations */
iparm[20] = 1; /* use 1x1 and 2x2 pivoting
//F77_FUNC(pardisoinit) (pt, &mtype, &solver, iparm, &error);
if (error != 0)
{
if (error == -10 )
printf("No license file found \n");
if (error == -11 )
printf("License is expired \n");
if (error == -12 )
printf("Wrong username or hostname \n");
return 1;
}
else
{ printf("PARDISO license check was successful ... \n");
}
/* Numbers of processors, value of OMP_NUM_THREADS */
var = getenv("OMP_NUM_THREADS");
if(var != NULL)
sscanf( var, "%d", &num_procs );
else {
printf("Set environment OMP_NUM_THREADS to 1");
exit(1);
}
iparm[2] = num_procs;
// other special settings
iparm[1] = 3; // 2 for metis, 0 for AMD
iparm[9] = 12;
iparm[10] = 1;
iparm[12] = 1; // setting this to 2 can cause errors sometimes
maxfct = 1; /* Maximum number of numerical factorizations. */
mnum = 1; /* Which factorization to use. */
msglvl = 0; /* Print statistical information */
error = 0; /* Initialize error flag */
/* -------------------------------------------------------------------- */
/* .. Reordering and Symbolic Factorization. This step also allocates */
/* all memory that is necessary for the factorization. */
/* -------------------------------------------------------------------- */
phase = 11;
t0 = currentTimeUsec();
PARDISO (pt, &maxfct, &mnum, &mtype, &phase,
&n, M1->vals, M1->rowOffs, M1->colIdxs, &idum, &nrhs,
iparm, &msglvl, &ddum, &ddum, &error);
t1 = currentTimeUsec();
if (error != 0) {
printf("ERROR during symbolic factorization: %d\n", error);
exit(1);
}
printf("Analyze: msec=%8.1f\n", (t1-t0)/1000.0);
printf("Number of nonzeros in factors = %d\n", iparm[17]);
printf("Number of factorization MFLOPS = %d\n", iparm[18]);
/* -------------------------------------------------------------------- */
/* .. Numerical factorization. */
/* -------------------------------------------------------------------- */
phase = 22;
t0 = currentTimeUsec();
PARDISO (pt, &maxfct, &mnum, &mtype, &phase,
&n, M1->vals, M1->rowOffs, M1->colIdxs, &idum, &nrhs,
iparm, &msglvl, &ddum, &ddum, &error);
t1 = currentTimeUsec();
if (error != 0) {
printf("ERROR during numerical factorization: %d\n", error);
exit(2);
}
printf("Factor: msec=%8.1f\n", (t1-t0)/1000.0);
/* -------------------------------------------------------------------- */
/* .. Back substitution and iterative refinement. */
/* -------------------------------------------------------------------- */
phase = 33;
iparm[7] = 1; /* Max numbers of iterative refinement steps. */
t0 = currentTimeUsec();
PARDISO (pt, &maxfct, &mnum, &mtype, &phase,
&n, M1->vals, M1->rowOffs, M1->colIdxs, &idum, &nrhs,
iparm, &msglvl, b1, x1, &error);
t1 = currentTimeUsec();
if (error != 0) {
printf("ERROR during solution: %d\n", error);
exit(3);
}
mulVector (r, M1, x1);
subVector (r, r, b1, n);
printf("Solve: msec=%8.1f\n\n", (t1-t0)/1000.0);
/* for (i=0; i<n; i++) { */
/* printf ("%g ", x1[i]); */
/* } */
/* printf ("\n"); */
printf("residual=%g\n", normVector(r, n));
#if 0
M2 = readCRSMatrix (fp, /*symmetric=*/1);
n = M2->nrows;
b2 = readVector (fp, n);
x2 = (double*)malloc(n*sizeof(double));
setVector (x2, x1, n);
iparm[7] = 1; /* Max numbers of iterative refinement steps. */
iparm[3] = 102;
printf ("maxfct=%d\n", maxfct);
printf ("mnum=%d\n", mnum);
printf ("mtype=%d\n", mtype);
printf ("phase=%d\n", phase);
printf ("idum=%d\n", idum);
for (i=0; i<64; i++) {
printf ("%d ", iparm[i]);
}
printf ("\n");
t0 = currentTimeUsec();
PARDISO (pt, &maxfct, &mnum, &mtype, &phase,
&n, M2->vals, M2->rowOffs, M2->colIdxs, &idum, &nrhs,
iparm, &msglvl, b2, x2, &error);
t1 = currentTimeUsec();
if (error != 0) {
printf("ERROR during solution: %d\n", error);
exit(3);
}
printf ("num iterations=%d\n", iparm[19]);
for (i=0; i<n; i++) {
printf ("%g ", x2[i]);
}
printf ("\n");
printf("Solve: msec=%8.1f\n\n", (t1-t0)/1000.0);
mulVector (r, M2, x2);
subVector (r, r, b2, n);
printf("residual=%g\n", normVector(r, n));
#endif
/* -------------------------------------------------------------------- */
/* .. Termination and release of memory. */
/* -------------------------------------------------------------------- */
phase = -1; /* Release internal memory. */
PARDISO (pt, &maxfct, &mnum, &mtype, &phase,
&n, &ddum, M1->rowOffs, M1->colIdxs, &idum, &nrhs,
iparm, &msglvl, &ddum, &ddum, &error);
return 0;
}
Vector3 normalizeVector(Vector3 a) {
return divScalarVector(a, normVector(a));
}
MStatus MG_dotProduct::compute(const MPlug& plug,MDataBlock& dataBlock)
{
MStatus returnStatus;
if ((plug==dotProductA)||
(plug==dotProductMax)||
(plug==proj1on2)||
(plug==proj2on1)||
(plug==angleInBetweenAttr)||
(plug==angleX)||
(plug==angleY)||
(plug==angleZ))
/*get time */
{
//creating handles to the input values
MDataHandle vector1DataH = dataBlock.inputValue(vector1);
MFloatPoint vector1V = vector1DataH.asFloatVector();
MDataHandle vector2DataH = dataBlock.inputValue(vector2);
MFloatPoint vector2V = vector2DataH.asFloatVector();
MDataHandle xAxisH = dataBlock.inputValue(projAxisX);
MFloatPoint xAxisData = xAxisH.asFloatVector();
MDataHandle yAxisH = dataBlock.inputValue(projAxisY);
MFloatPoint yAxisData = yAxisH.asFloatVector();
MDataHandle zAxisH = dataBlock.inputValue(projAxisZ);
MFloatPoint zAxisData = zAxisH.asFloatVector();
MDataHandle normData = dataBlock.inputValue(normalize);
bool norm =normData.asBool();
//Creating some neededs variables
float dotResult; // variable that will hold the dot product result
float maxValue; //variable that will hold the dot product max value
float distance1; // variable that will hold the vector 1 lenght
float distance2; //variable that will hold the vector 2 lenght
float angleDeg; //variable that will hold the angle inbetween the two vectors
//float cosRad ; //variable that will hold the cosine value in radiants
//Dot product math
float vec1Array[3] = {vector1V[0],vector1V[1],vector1V[2]};
vector <float> vec1 = makeVector(vec1Array) ;
float vec2Array[3] = {vector2V[0],vector2V[1],vector2V[2]};
vector <float> vec2 = makeVector(vec2Array) ;
dotResult = vecDotProduct(vec1,vec2);
distance1 = vectorLength(vec1);
distance2 = vectorLength(vec2);
maxValue = distance1*distance2;
if (norm == 1)
{
if (maxValue ==0)
{
dotResult=0;
}else{
dotResult = dotResult/maxValue;
}
}
//Projection v2 on v1
float projV2=0; //variable that will hold the value projection of v2 projected on v1
vector <float> v1Norm; // variable that will hold the normalized v1 vector
vector<float> v2Vec; // variable that will hold the projected vector
if (distance1 != 0)
{
projV2 = projVector(vec2,vec1);
v1Norm = normVector(vec1);
v2Vec = scalarVector(v1Norm,projV2);
}else{
//initialize the vector as 0 0 0
float zeroVec2[3]= {0,0,0};
v2Vec=makeVector(zeroVec2);
}
//Projection v1 on v2
float projV1=0; //variable that will hold the value projection of v1 projected on v2
vector <float> v2Norm;// variable that will hold the normalized v2 vector
vector <float> v1Vec;// variable that will hold the projected vector
if (distance2 != 0)
{
projV1 = projVector(vec1,vec2);
v2Norm = normVector(vec2);
v1Vec = scalarVector(v2Norm,projV1);
}else{
//initialize the vector as 0 0 0
float zeroVec1[3]= {0,0,0};
v1Vec=makeVector(zeroVec1);
}
//Angle in between
if ((distance2*distance1)!=0)
{
angleDeg=angleInbetweenVector(vec1,vec2);
}else{
angleDeg=0;
}
//Angle inbetween splitted into X,Y,Z world rotation
//float dotResultV1X;
// splitting inbetween angle into X Y Z rotation
//converting axis from node into vector class
float xAxisArray[3] = {xAxisData[0],xAxisData[1],xAxisData[2]};
vector<float> xAxisVec = makeVector(xAxisArray) ;
float yAxisArray[3] = {yAxisData[0],yAxisData[1],yAxisData[2]};
vector<float> yAxisVec = makeVector(yAxisArray) ;
float zAxisArray[3] = {zAxisData[0],zAxisData[1],zAxisData[2]};
vector<float> zAxisVec = makeVector(zAxisArray) ;
float angleProjXYDeg=0 ;
float angleProjYZDeg=0 ;
float angleProjXZDeg=0 ;
// angle Z
vector<float> projectedV1;
vector<float> projectedV2;
projectedV1= projectVectorOnPlane(vec1,xAxisVec,yAxisVec);
projectedV2= projectVectorOnPlane(vec2,xAxisVec,yAxisVec);
angleProjXYDeg=angleInbetweenVector(projectedV1,projectedV2);
// angle X
projectedV1= projectVectorOnPlane(vec1,zAxisVec,yAxisVec);
projectedV2= projectVectorOnPlane(vec2,zAxisVec,yAxisVec);
angleProjYZDeg=angleInbetweenVector(projectedV1,projectedV2);
// angle Y
projectedV1= projectVectorOnPlane(vec1,zAxisVec,xAxisVec);
projectedV2= projectVectorOnPlane(vec2,zAxisVec,xAxisVec);
angleProjXZDeg=angleInbetweenVector(projectedV1,projectedV2);
//Setting output values
MDataHandle output = dataBlock.outputValue(dotProductA);
MDataHandle outputMax = dataBlock.outputValue(dotProductMax);
MDataHandle projV1Output = dataBlock.outputValue(proj1on2);
MDataHandle projV2Output = dataBlock.outputValue(proj2on1);
MDataHandle angleInBetweenOutput = dataBlock.outputValue(angleInBetweenAttr);
MDataHandle angleXout = dataBlock.outputValue(angleX);
MDataHandle angleYout = dataBlock.outputValue(angleY);
MDataHandle angleZout = dataBlock.outputValue(angleZ);
output.set(dotResult);
outputMax.set(maxValue);
projV1Output.set(v1Vec[0],v1Vec[1],v1Vec[2]);
projV2Output.set(v2Vec[0],v2Vec[1],v2Vec[2]);
angleInBetweenOutput.set(angleDeg);
angleXout.set(angleProjYZDeg);
angleYout.set(angleProjXZDeg);
angleZout.set(angleProjXYDeg);
//SetClean tells maya attribute is update
outputMax.setClean();
output.setClean();
projV1Output.setClean();
projV2Output.setClean();
angleInBetweenOutput.setClean();
angleXout.setClean();
angleYout.setClean();
angleZout.setClean();
}
return MS::kSuccess;
}
/**
* Get the Wilson's B-matrix
*/
void getBMatrix(Real** cartCoords, int numCartesians,
bondCoord** bonds, int numBonds,
angleCoord** angles, int numAngles,
dihedralCoord** dihedrals, int numDihedrals,
improperCoord** impropers, int numImpropers,
Matrix& B) {
#ifdef DEBUG
cout << "\n\ngetBMatrix - Constructing B Matrix\n";
#endif
// Constructing B Matrix
// follows method in chapter 4 of Molecular Vibrations by Wilson, Decius, and Cross
#ifdef DEBUG
int numInternals = numBonds + numAngles + numDihedrals + numImpropers;
cout << "numBonds: " << numBonds << "\n";
cout << "numAngles: " << numAngles << "\n";
cout << "numDihedrals: " << numDihedrals << "\n";
cout << "numImpropers: " << numImpropers << "\n";
cout << "numInternals: " << numInternals << "\n";
#endif
// Load Data
B = 0.0;
int i = 0;
int j = 0;
int index1 = 0;
int index2 = 0;
RowVector tempCoord1(3);
RowVector tempCoord2(3);
Real norm = 0.0;
// Bonds
for (i=0; i<numBonds; i++) {
index1 = bonds[i]->x1;
index2 = bonds[i]->x2;
//norm = bonds[i].val; // Could calculate this, like below.
#ifdef DEBUG
cout << "index1=" << index1 << "index2=" << index2 << "\n";
#endif
for (j=0; j<3; j++) {
tempCoord1(j+1) = cartCoords[index1][j];
tempCoord2(j+1) = cartCoords[index2][j];
}
tempCoord1 << tempCoord1 - tempCoord2;
norm = tempCoord1.NormFrobenius(); // XXX - don't delete
if (norm > 0.0) {
tempCoord1 << tempCoord1 / norm;
}
for (j=1; j<=3; j++) {
B(i+1,((index1)*3)+j) = tempCoord1(j);
B(i+1,((index2)*3)+j) = -tempCoord1(j);
}
}
#ifdef DEBUG
cout << "after bonds\n";
cout << "B:\n";
cout << setw(9) << setprecision(3) << (B);
cout << "\n\n";
#endif
// Angles
int index3 = 0;
RowVector tempCoord3(3);
RowVector tempCoord4(3);
RowVector tempCoord5(3);
RowVector r21(3); // Vector from 2nd to 1st point
RowVector r23(3); // Vector from 2nd to 3rd point
RowVector e21(3); // Unit vector from 2nd to 1st point
RowVector e23(3); // Unit vector from 2nd to 3rd point
Real norm21; // Norm of r21
Real norm23; // Norm of r23
Real angle = 0.0; // Angle in radians
Real cosAngle123 = 0.0;
Real sinAngle123 = 0.0;
//Real pi = 3.14159265;
Real scaleFactor = 0.529178; // Scaling factor (0.529178)
for (i=0; i<numAngles; i++) {
index1 = angles[i]->x1;
index2 = angles[i]->x2;
index3 = angles[i]->x3;
//angle = angles[i].val * (pi/180.0); // Convert to radians.
for (j=0; j<3; j++) {
tempCoord1(j+1) = cartCoords[index1][j];
tempCoord2(j+1) = cartCoords[index2][j];
tempCoord3(j+1) = cartCoords[index3][j];
}
r21 << tempCoord1 - tempCoord2;
r23 << tempCoord3 - tempCoord2;
norm21 = r21.NormFrobenius();
norm23 = r23.NormFrobenius();
e21 << r21;
if (norm21 > 0.0) {
e21 << e21 / norm21;
}
e23 << r23;
if (norm23 > 0.0) {
e23 << e23 / norm23;
}
angle = acos(DotProduct(r21,r23) / (norm21 * norm23));
cosAngle123 = DotProduct(r21,r23) / (norm21 * norm23);
sinAngle123 = sqrt(1 - (cosAngle123 * cosAngle123));
#ifdef DEBUG
cout << "r21: " << (r21) << "\n";
cout << "r23: " << (r23) << "\n";
cout << "norm21: " << norm21 << ", norm23: " << norm23 << "\n\n";
cout << "e21: " << (e21) << "\n";
cout << "e23: " << (e23) << "\n";
cout << "cos(" << angle << "): " << cos(angle) << "\n";
cout << "sin(" << angle << "): " << sin(angle) << "\n";
cout << "angle: " << acos(DotProduct(r21,r23) / (norm21 * norm23)) << "\n";
cout << "cosAngle123: " << cosAngle123 << "\n";
cout << "sinAngle123: " << sinAngle123 << "\n";
#endif
// First elements of coordinate triples
tempCoord4 << ((cosAngle123 * e21) - e23);
tempCoord4 << (tempCoord4 * scaleFactor) / (norm21 * sinAngle123);
for (j=1; j<=3; j++) {
B(i+numBonds+1,((index1)*3)+j) = tempCoord4(j);
}
// Third elements of coordinate triples
tempCoord5 << ((cosAngle123 * e23) - e21);
tempCoord5 << (tempCoord5 * scaleFactor) / (norm23 * sinAngle123);
for (j=1; j<=3; j++) {
B(i+numBonds+1,((index3)*3)+j) = tempCoord5(j);
}
// Second (middle) elements of coordinate triples (depends on 1st and 3rd)
tempCoord4 << -tempCoord4 - tempCoord5;
for (j=1; j<=3; j++) {
B(i+numBonds+1,((index2)*3)+j) = tempCoord4(j);
}
}
#ifdef DEBUG
cout << "after angles\n";
cout << "B:\n";
cout << setw(9) << setprecision(3) << (B);
cout << "\n\n";
#endif
// Dihedrals
RowVector r12(3); // Vector from 1st to 2nd point
RowVector r32(3); // Vector from 3rd to 2nd point
RowVector r34(3); // Vector from 3rd to 2nd point
RowVector r43(3); // Vector from 4th to 3rd point
RowVector e12(3); // Unit vector from 1st to 2nd point
RowVector e32(3); // Unit vector from 3rd to 2nd point
RowVector e34(3); // Unit vector from 3rd to 2nd point
RowVector e43(3); // Unit vector from 4th to 3rd point
Real norm12; // Norm of r12
Real norm32; // Norm of r32
Real norm34; // Norm of r34
Real norm43; // Norm of r43
RowVector cross1223(3); // Cross product of e12 and e23
RowVector cross4332(3); // Cross product of e43 and e32
Real angle123 = 0.0; // Angle in radians
Real angle234 = 0.0; // Angle in radians
Real cosAngle234 = 0.0;
Real sinAngle234 = 0.0;
scaleFactor = 0.529178; // Scaling factor (0.529178)
int index4 = 0;
RowVector tempCoord6(3);
for (i=0; i<numDihedrals; i++) {
index1 = dihedrals[i]->x1;
index2 = dihedrals[i]->x2;
index3 = dihedrals[i]->x3;
index4 = dihedrals[i]->x4;
for (j=0; j<3; j++) {
tempCoord1(j+1) = cartCoords[index1][j];
tempCoord2(j+1) = cartCoords[index2][j];
tempCoord3(j+1) = cartCoords[index3][j];
tempCoord4(j+1) = cartCoords[index4][j];
}
r12 << tempCoord2 - tempCoord1;
r21 << tempCoord1 - tempCoord2;
r23 << tempCoord3 - tempCoord2;
r32 << tempCoord2 - tempCoord3;
r34 << tempCoord4 - tempCoord3;
r43 << tempCoord3 - tempCoord4;
norm12 = r12.NormFrobenius();
norm21 = r21.NormFrobenius();
norm23 = r23.NormFrobenius();
norm32 = r32.NormFrobenius();
norm34 = r34.NormFrobenius();
norm43 = r43.NormFrobenius();
#ifdef DEBUG
cout << "norm12: " << norm12 << "\n";
cout << "norm21: " << norm21 << "\n";
cout << "norm23: " << norm23 << "\n";
cout << "norm32: " << norm32 << "\n";
cout << "norm34: " << norm34 << "\n";
cout << "norm43: " << norm43 << "\n";
#endif
e12 << r12 / norm12;
e21 << r21 / norm21;
e23 << r23 / norm23;
e32 << r32 / norm32;
e34 << r34 / norm34;
e43 << r43 / norm43;
angle123 = acos(DotProduct(r21,r23) / (norm21 * norm23)); // Wilson's angle 2
angle234 = acos(DotProduct(r32,r34) / (norm32 * norm34)); // Wilson's angle 3
cosAngle123 = DotProduct(r21,r23) / (norm21 * norm23);
cosAngle234 = DotProduct(r32,r34) / (norm32 * norm34);
sinAngle123 = sqrt(1 - (cosAngle123 * cosAngle123));
sinAngle234 = sqrt(1 - (cosAngle234 * cosAngle234));
#ifdef DEBUG
cout << "angle123: " << angle123 << ", cos(angle123): " << cos(angle123) << ", sin(angle123): " << sin(angle123) << "\n";
cout << "angle234: " << angle234 << ", cos(angle234): " << cos(angle234) << ", sin(angle234): " << sin(angle234) << "\n";
cout << "cosAngle123: " << cosAngle123 << ", sinAngle123: " << sinAngle123 << "\n";
cout << "cosAngle234: " << cosAngle234 << ", sinAngle234: " << sinAngle234 << "\n";
#endif
cross1223(1) = (e12(2)*e23(3)) - (e12(3)*e23(2));
cross1223(2) = (e12(3)*e23(1)) - (e12(1)*e23(3));
cross1223(3) = (e12(1)*e23(2)) - (e12(2)*e23(1));
cross4332(1) = (e43(2)*e32(3)) - (e43(3)*e32(2));
cross4332(2) = (e43(3)*e32(1)) - (e43(1)*e32(3));
cross4332(3) = (e43(1)*e32(2)) - (e43(2)*e32(1));
#ifdef DEBUG
cout << "cross1223 (norm " << cross1223.NormFrobenius() << "):\n";
cout << setw(9) << setprecision(6) << (cross1223);
cout << "\n\n";
cout << "cross4332 (norm " << cross4332.NormFrobenius() << "):\n";
cout << setw(9) << setprecision(6) << (cross4332);
cout << "\n\n";
#endif
// First elements of coordinate triples
tempCoord5 << -((cross1223 * scaleFactor) / (norm12 * sinAngle123 * sinAngle123));
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+1,((index1)*3)+j) = tempCoord5(j);
}
// Second elements of coordinate triples
tempCoord5 << ((norm23 - (norm12 * cosAngle123)) / (norm23 * norm12 * sinAngle123 * sinAngle123)) * (cross1223);
tempCoord6 << (cosAngle234 / (norm23 * sinAngle234 * sinAngle234)) * (cross4332);
#ifdef DEBUG
cout << "tempCoord5:\n";
cout << setw(9) << setprecision(6) << (tempCoord5);
cout << "tempCoord6:\n";
cout << setw(9) << setprecision(6) << (tempCoord6);
#endif
tempCoord5 << (tempCoord5 + tempCoord6) * scaleFactor;
#ifdef DEBUG
cout << "tempCoord5:\n";
cout << setw(9) << setprecision(6) << (tempCoord5);
#endif
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+1,((index2)*3)+j) = tempCoord5(j);
}
// Third elements of coordinate triples
tempCoord5 << ((norm32 - (norm43 * cosAngle234)) / (norm32 * norm43 * sinAngle234 * sinAngle234)) * (cross4332);
tempCoord6 << (cosAngle123 / (norm32 * sinAngle123 * sinAngle123)) * (cross1223);
tempCoord5 << (tempCoord5 + tempCoord6) * scaleFactor;
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+1,((index3)*3)+j) = tempCoord5(j);
}
// Fourth elements of coordinate triples
tempCoord5 << -((cross4332 * scaleFactor) / (norm43 * sinAngle234 * sinAngle234));
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+1,((index4)*3)+j) = tempCoord5(j);
}
}
#ifdef DEBUG
cout << "B:\n";
cout << setw(9) << setprecision(3) << (B);
cout << "\n\n";
#endif
// Impropers
RowVector r41(3); // Vector from 4th to 1st point
RowVector r42(3); // Vector from 4th to 2nd point
RowVector e41(3); // Unit vector from 4th to 1st point
RowVector e42(3); // Unit vector from 4th to 2nd point
RowVector normVector(3); // Normal to the plane
Real norm41; // Norm of r41
Real norm42; // Norm of r42
Real angle142 = 0.0; // Angle in radians
Real angle143 = 0.0; // Angle in radians
Real angle243 = 0.0; // Angle in radians
Real cosAngle142 = 0.0;
Real cosAngle143 = 0.0;
Real cosAngle243 = 0.0;
Real sinAngle142 = 0.0;
Real sinAngle143 = 0.0;
Real sinAngle243 = 0.0;
Real apexCoeff = 0.0; // Magnitude of central atom displacement
scaleFactor = -0.352313; // Scale factor (-0.352313)
for (i=0; i<numImpropers; i++) {
index1 = impropers[i]->x1;
index2 = impropers[i]->x2;
index3 = impropers[i]->x3;
index4 = impropers[i]->x4;
for (j=0; j<3; j++) {
tempCoord1(j+1) = cartCoords[index1][j];
tempCoord2(j+1) = cartCoords[index2][j];
tempCoord3(j+1) = cartCoords[index3][j];
tempCoord4(j+1) = cartCoords[index4][j];
}
r41 << tempCoord1 - tempCoord4;
r42 << tempCoord2 - tempCoord4;
r43 << tempCoord3 - tempCoord4;
norm41 = r41.NormFrobenius();
norm42 = r42.NormFrobenius();
norm43 = r43.NormFrobenius();
e41 << r41 / norm41;
e42 << r42 / norm42;
e43 << r43 / norm43;
angle142 = acos(DotProduct(r41,r42) / (norm41 * norm42));
angle143 = acos(DotProduct(r41,r43) / (norm41 * norm43));
angle243 = acos(DotProduct(r42,r43) / (norm42 * norm43));
cosAngle142 = DotProduct(r41,r42) / (norm41 * norm42);
cosAngle143 = DotProduct(r41,r43) / (norm41 * norm43);
cosAngle243 = DotProduct(r42,r43) / (norm42 * norm43);
sinAngle142 = sqrt(1 - (cosAngle142 * cosAngle142));
sinAngle143 = sqrt(1 - (cosAngle143 * cosAngle143));
sinAngle243 = sqrt(1 - (cosAngle243 * cosAngle243));
normVector(1) = (r41(2)*r42(3)) - (r41(3)*r42(2));
normVector(2) = (r41(3)*r42(1)) - (r41(1)*r42(3));
normVector(3) = (r41(1)*r42(2)) - (r41(2)*r42(1));
normVector << normVector / normVector.NormFrobenius();
// First elements of coordinate triples
tempCoord5 << normVector * (scaleFactor / norm41);
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+numDihedrals+1,((index1)*3)+j) = tempCoord5(j);
}
// Second elements of coordinate triples
tempCoord5 << normVector * sinAngle143 * scaleFactor;
tempCoord5 << tempCoord5 / (norm42 * sinAngle243);
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+numDihedrals+1,((index2)*3)+j) = tempCoord5(j);
}
// Third elements of coordinate triples
tempCoord5 << normVector * sinAngle142 * scaleFactor;
tempCoord5 << tempCoord5 / (norm43 * sinAngle243);
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+numDihedrals+1,((index3)*3)+j) = tempCoord5(j);
}
// Fourth elements of coordinate triples
apexCoeff = -1.0 / norm42;
apexCoeff -= sinAngle143 / (norm42 * sinAngle243);
apexCoeff -= sinAngle142 / (norm43 * sinAngle243);
tempCoord5 << normVector * apexCoeff * scaleFactor;
for (j=1; j<=3; j++) {
B(i+numBonds+numAngles+numDihedrals+1,((index4)*3)+j) = tempCoord5(j);
}
}
return;
}
int testPlane() {
int numErr = 0;
logMessage(_T("TESTING - class GM_3dPlane ...\n\n"));
// Default constructor, plane must be invalid
GM_3dPlane p;
if (p.isValid()) {
logMessage(_T("\tERROR - Default constructor creates valid plane\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Default constructor creates invalid plane\n"));
}
// Constructor (coeff)
GM_3dPlane p1(getRandomDouble(), getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPlane pNull(0.0, 0.0, 0.0, getRandomDouble());
if (!p1.isValid() || pNull.isValid()) {
logMessage(_T("\tERROR - Coeff. constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Coeff. constructor working\n"));
}
// Copy constructor
GM_3dPlane copyPlane(p1);
if (!copyPlane.isValid() || copyPlane != p1) {
logMessage(_T("\tERROR - Copy constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Copy constructor working\n"));
}
// Constructor (coeff vector)
double pointsVect[4];
for (int i = 0 ; i < 4 ; i++) {
pointsVect[i] = getRandomDouble();
}
GM_3dPlane p2(pointsVect);
if (!p2.isValid()) {
logMessage(_T("\tERROR - Coeff. vector constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Coeff. vector constructor working\n"));
}
// Constructor (points)
GM_3dPoint pt1(getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPoint pt2(getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPoint pt3(getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dLine ln(getRandomDouble(), getRandomDouble(), getRandomDouble(),getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPoint ptLn1 = ln.begin();
GM_3dPoint ptLn2 = ln.center();
GM_3dPoint ptLn3 = ln.end();
GM_3dPlane p3(pt1, pt2, pt3);
GM_3dPlane pLine(ptLn2, ptLn2, ptLn3);
double distPt1 = pt1.x()*p3[0] + pt1.y()*p3[1] + pt1.z()*p3[2] + p3[3];
double distPt2 = pt2.x()*p3[0] + pt2.y()*p3[1] + pt2.z()*p3[2] + p3[3];
double distPt3 = pt3.x()*p3[0] + pt3.y()*p3[1] + pt3.z()*p3[2] + p3[3];
if (!p3.isValid() || pLine.isValid() || fabs(distPt1) > GM_NULL_TOLERANCE || fabs(distPt2) > GM_NULL_TOLERANCE || fabs(distPt3) > GM_NULL_TOLERANCE) {
logMessage(_T("\tERROR - Points constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Points constructor working\n"));
}
// Point distance
GM_3dPlane p4(getRandomDouble(), getRandomDouble(), getRandomDouble(),getRandomDouble());
GM_3dPoint checkPoint(getRandomDouble(), getRandomDouble(), getRandomDouble());
if (fabs(p4[0]) > GM_NULL_TOLERANCE) {
checkPoint.x(-(checkPoint.y()*p4[1] + checkPoint.z()*p4[2] + p4[3]) / p4[0]);
}
else if (fabs(p4[1]) > GM_NULL_TOLERANCE) {
checkPoint.y(-(checkPoint.x()*p4[0] + checkPoint.z()*p4[2] + p4[3]) / p4[1]);
}
else if (fabs(p4[2]) > GM_NULL_TOLERANCE) {
checkPoint.z(-(checkPoint.x()*p4[0] + checkPoint.y()*p4[1] + p4[3]) / p4[2]);
}
else {
p4.invalidate();
}
double checkSignedDist = getRandomDouble();
double checkDist = fabs(checkSignedDist);
GM_3dVector p4Norm = p4.normalVector();
checkPoint = (GM_3dVector)checkPoint + (p4Norm * checkSignedDist);
GM_3dPoint checkPointOnPlane1;
GM_3dPoint checkPointOnPlane2;
double dist = p4.pointDistance(checkPoint);
double signedDist = p4.pointDistanceSgn(checkPoint);
double signedDistOnPlane = p4.pointDistanceSgn(checkPoint, checkPointOnPlane1);
double distOnPlane = p4.pointDistance(checkPoint, checkPointOnPlane2);
distPt1 = checkPointOnPlane1.x()*p4[0] + checkPointOnPlane1.y()*p4[1] + checkPointOnPlane1.z()*p4[2] + p4[3];
distPt2 = checkPointOnPlane2.x()*p4[0] + checkPointOnPlane2.y()*p4[1] + checkPointOnPlane2.z()*p4[2] + p4[3];
if (!p4.isValid() || !checkPointOnPlane1.isValid() || !checkPointOnPlane2.isValid() ||
fabs(distPt1) > GM_NULL_TOLERANCE || fabs(distPt2) > GM_NULL_TOLERANCE ||
fabs(distOnPlane - checkDist) > GM_NULL_TOLERANCE || fabs(signedDistOnPlane - checkSignedDist) > GM_NULL_TOLERANCE ||
fabs(dist - checkDist) > GM_NULL_TOLERANCE || fabs(signedDist - checkSignedDist) > GM_NULL_TOLERANCE) {
logMessage(_T("\tERROR - Point distance computation not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Point distance computation working\n"));
}
// XY Angle
double angle = ((((double)rand()) / ((double)RAND_MAX)) * GM_PI) - (GM_PI / 2.0);
GM_3dVector normVector(angle/* + GM_HALFPI*/);
normVector.z(normVector.y());
normVector.y(0.0);
GM_3dPlane angleP(normVector, GM_3dPoint(getRandomDouble(), getRandomDouble(), getRandomDouble()));
double checkAngle = angleP.xyAngle();
if (checkAngle > GM_PI) {
checkAngle -= 2.0 * GM_PI;
}
if (!angleP.isValid() || fabs(angle - checkAngle) > GM_NULL_TOLERANCE) {
logMessage(_T("\tERROR - XY angle computation not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - XY angle computation working\n"));
}
return numErr;
}
