From 59495dcf54390cd761e03636cf5acc727b9491dd Mon Sep 17 00:00:00 2001 From: sjplimp Date: Tue, 19 Apr 2011 18:52:48 +0000 Subject: [PATCH] git-svn-id: svn://svn.icms.temple.edu/lammps-ro/trunk@5975 f3b2605a-c512-4ea7-a41b-209d697bcdaa --- src/ASPHERE/fix_nve_asphere.cpp | 6 +- src/fix_rigid.cpp | 58 +++-- src/math_extra.cpp | 436 ++++++++++++++++++++++++++++++++ src/math_extra.h | 227 ++++++++--------- 4 files changed, 588 insertions(+), 139 deletions(-) create mode 100644 src/math_extra.cpp diff --git a/src/ASPHERE/fix_nve_asphere.cpp b/src/ASPHERE/fix_nve_asphere.cpp index 9f1769b7f0..c0cbdd05ea 100755 --- a/src/ASPHERE/fix_nve_asphere.cpp +++ b/src/ASPHERE/fix_nve_asphere.cpp @@ -150,7 +150,7 @@ void FixNVEAsphere::richardson(double *q, double *m, double *moments) // compute omega at 1/2 step from m at 1/2 step and q at 0 double w[3]; - omega_from_mq(q,m,moments,w); + omega_from_mq(m,q,moments,w); // full update from dq/dt = 1/2 w q @@ -176,7 +176,7 @@ void FixNVEAsphere::richardson(double *q, double *m, double *moments) // re-compute omega at 1/2 step from m at 1/2 step and q at 1/2 step // recompute wq - omega_from_mq(qhalf,m,moments,w); + omega_from_mq(m,qhalf,moments,w); MathExtra::vecquat(w,qhalf,wq); // 2nd half of update from dq/dt = 1/2 w q @@ -204,7 +204,7 @@ void FixNVEAsphere::richardson(double *q, double *m, double *moments) and divide by principal moments ------------------------------------------------------------------------- */ -void FixNVEAsphere::omega_from_mq(double *q, double *m, double *moments, +void FixNVEAsphere::omega_from_mq(double *m, double *q, double *moments, double *w) { double rot[3][3]; diff --git a/src/fix_rigid.cpp b/src/fix_rigid.cpp index 989196cea5..15cb67dd84 100644 --- a/src/fix_rigid.cpp +++ b/src/fix_rigid.cpp @@ -589,7 +589,6 @@ void FixRigid::init() pre_neighbor(); // compute 6 moments of inertia of each body - // stored as 6-vector in Voigt ordering // dx,dy,dz = coords relative to center-of-mass double dx,dy,dz,rad; @@ -633,7 +632,8 @@ void FixRigid::init() // extended particles may contribute extra terms to moments of inertia if (extended) { - double ivec[6]; + double ex[3],ey[3],ez[3],idiag[3]; + double p[3][3],ptrans[3][3],ispace[3][3],itemp[3][3]; double *shape,*quatatom; for (i = 0; i < nlocal; i++) { @@ -652,13 +652,19 @@ void FixRigid::init() if (eflags[i] & INERTIA_ELLIPSOID) { shape = ebonus[ellipsoid[i]].shape; quatatom = ebonus[ellipsoid[i]].quat; - MathExtra::inertia_ellipsoid(shape,quatatom,massone,ivec); - sum[ibody][0] += ivec[0]; - sum[ibody][1] += ivec[1]; - sum[ibody][2] += ivec[2]; - sum[ibody][3] += ivec[3]; - sum[ibody][4] += ivec[4]; - sum[ibody][5] += ivec[5]; + MathExtra::quat_to_mat(quatatom,p); + MathExtra::quat_to_mat_trans(quatatom,ptrans); + idiag[0] = 0.2*massone * (shape[1]*shape[1]+shape[2]*shape[2]); + idiag[1] = 0.2*massone * (shape[0]*shape[0]+shape[2]*shape[2]); + idiag[2] = 0.2*massone * (shape[0]*shape[0]+shape[1]*shape[1]); + MathExtra::diag_times3(idiag,ptrans,itemp); + MathExtra::times3(p,itemp,ispace); + sum[ibody][0] += ispace[0][0]; + sum[ibody][1] += ispace[1][1]; + sum[ibody][2] += ispace[2][2]; + sum[ibody][3] += ispace[0][1]; + sum[ibody][4] += ispace[1][2]; + sum[ibody][5] += ispace[0][2]; } } } @@ -669,8 +675,9 @@ void FixRigid::init() // ex_space,ey_space,ez_space = 3 eigenvectors = principal axes of rigid body double tensor[3][3],evectors[3][3]; - double ez0,ez1,ez2; + int ierror; + double ez0,ez1,ez2; for (ibody = 0; ibody < nbody; ibody++) { tensor[0][0] = all[ibody][0]; @@ -792,11 +799,10 @@ void FixRigid::init() // test for valid principal moments & axes // recompute moments of inertia around new axes - // stored as 6-vector in Voigt ordering // 3 diagonal moments should equal principal moments // 3 off-diagonal moments should be 0.0 // extended particles may contribute extra terms to moments of inertia - + for (ibody = 0; ibody < nbody; ibody++) for (i = 0; i < 6; i++) sum[ibody][i] = 0.0; @@ -818,7 +824,8 @@ void FixRigid::init() } if (extended) { - double ivec[6]; + double ex[3],ey[3],ez[3],idiag[3]; + double p[3][3],ptrans[3][3],ispace[3][3],itemp[3][3]; double *shape; for (i = 0; i < nlocal; i++) { @@ -836,13 +843,19 @@ void FixRigid::init() } if (eflags[i] & INERTIA_ELLIPSOID) { shape = ebonus[ellipsoid[i]].shape; - MathExtra::inertia_ellipsoid(shape,qorient[i],massone,ivec); - sum[ibody][0] += ivec[0]; - sum[ibody][1] += ivec[1]; - sum[ibody][2] += ivec[2]; - sum[ibody][3] += ivec[3]; - sum[ibody][4] += ivec[4]; - sum[ibody][5] += ivec[5]; + MathExtra::quat_to_mat(qorient[i],p); + MathExtra::quat_to_mat_trans(qorient[i],ptrans); + idiag[0] = 0.2*massone * (shape[1]*shape[1]+shape[2]*shape[2]); + idiag[1] = 0.2*massone * (shape[0]*shape[0]+shape[2]*shape[2]); + idiag[2] = 0.2*massone * (shape[0]*shape[0]+shape[1]*shape[1]); + MathExtra::diag_times3(idiag,ptrans,itemp); + MathExtra::times3(p,itemp,ispace); + sum[ibody][0] += ispace[0][0]; + sum[ibody][1] += ispace[1][1]; + sum[ibody][2] += ispace[2][2]; + sum[ibody][3] += ispace[0][1]; + sum[ibody][4] += ispace[1][2]; + sum[ibody][5] += ispace[0][2]; } } } @@ -1220,6 +1233,7 @@ void FixRigid::richardson(double *q, double *w, qfull[1] = q[1] + dtq * wq[1]; qfull[2] = q[2] + dtq * wq[2]; qfull[3] = q[3] + dtq * wq[3]; + MathExtra::qnormalize(qfull); // 1st half update from dq/dt = 1/2 w q @@ -1229,6 +1243,7 @@ void FixRigid::richardson(double *q, double *w, qhalf[1] = q[1] + 0.5*dtq * wq[1]; qhalf[2] = q[2] + 0.5*dtq * wq[2]; qhalf[3] = q[3] + 0.5*dtq * wq[3]; + MathExtra::qnormalize(qhalf); // udpate ex,ey,ez from qhalf @@ -1245,6 +1260,7 @@ void FixRigid::richardson(double *q, double *w, qhalf[1] += 0.5*dtq * wq[1]; qhalf[2] += 0.5*dtq * wq[2]; qhalf[3] += 0.5*dtq * wq[3]; + MathExtra::qnormalize(qhalf); // corrected Richardson update @@ -1253,8 +1269,8 @@ void FixRigid::richardson(double *q, double *w, q[1] = 2.0*qhalf[1] - qfull[1]; q[2] = 2.0*qhalf[2] - qfull[2]; q[3] = 2.0*qhalf[3] - qfull[3]; - MathExtra::qnormalize(q); + MathExtra::qnormalize(q); MathExtra::q_to_exyz(q,ex,ey,ez); } diff --git a/src/math_extra.cpp b/src/math_extra.cpp new file mode 100644 index 0000000000..905625973f --- /dev/null +++ b/src/math_extra.cpp @@ -0,0 +1,436 @@ +/* ---------------------------------------------------------------------- + LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator + http://lammps.sandia.gov, Sandia National Laboratories + Steve Plimpton, sjplimp@sandia.gov + + Copyright (2003) Sandia Corporation. Under the terms of Contract + DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains + certain rights in this software. This software is distributed under + the GNU General Public License. + + See the README file in the top-level LAMMPS directory. +------------------------------------------------------------------------- */ + +/* ---------------------------------------------------------------------- + Contributing author: Mike Brown (SNL) +------------------------------------------------------------------------- */ + +#include "stdio.h" +#include "string.h" +#include "math_extra.h" + +#define MAXJACOBI 50 + +namespace MathExtra { + +/* ---------------------------------------------------------------------- + output a matrix +------------------------------------------------------------------------- */ + +void MathExtra::write3(const double mat[3][3]) +{ + for (unsigned i = 0; i < 3; i++) { + for (unsigned j = 0; j < 3; j++) printf("%g ",mat[i][j]); + printf("\n"); + } +} + +/* ---------------------------------------------------------------------- + solve Ax = b or M ans = v + use gaussian elimination & partial pivoting on matrix +------------------------------------------------------------------------- */ + +int MathExtra::mldivide3(const double m[3][3], const double *v, double *ans) +{ + // create augmented matrix for pivoting + + double aug[3][4]; + for (unsigned i = 0; i < 3; i++) { + aug[i][3] = v[i]; + for (unsigned j = 0; j < 3; j++) aug[i][j] = m[i][j]; + } + + for (unsigned i = 0; i < 2; i++) { + unsigned p = i; + for (unsigned j = i+1; j < 3; j++) { + if (fabs(aug[j][i]) > fabs(aug[i][i])) { + double tempv[4]; + memcpy(tempv,aug[i],4*sizeof(double)); + memcpy(aug[i],aug[j],4*sizeof(double)); + memcpy(aug[j],tempv,4*sizeof(double)); + } + } + + while (aug[p][i] == 0.0 && p < 3) p++; + + if (p == 3) return 1; + else + if (p != i) { + double tempv[4]; + memcpy(tempv,aug[i],4*sizeof(double)); + memcpy(aug[i],aug[p],4*sizeof(double)); + memcpy(aug[p],tempv,4*sizeof(double)); + } + + for (unsigned j = i+1; j < 3; j++) { + double m = aug[j][i]/aug[i][i]; + for (unsigned k=i+1; k<4; k++) aug[j][k]-=m*aug[i][k]; + } + } + + if (aug[2][2] == 0.0) return 1; + + // back substitution + + ans[2] = aug[2][3]/aug[2][2]; + for (int i = 1; i >= 0; i--) { + double sumax = 0.0; + for (unsigned j = i+1; j < 3; j++) sumax += aug[i][j]*ans[j]; + ans[i] = (aug[i][3]-sumax) / aug[i][i]; + } + + return 0; +} + +/* ---------------------------------------------------------------------- + compute evalues and evectors of 3x3 real symmetric matrix + based on Jacobi rotations + adapted from Numerical Recipes jacobi() function +------------------------------------------------------------------------- */ + +int MathExtra::jacobi(double matrix[3][3], double *evalues, + double evectors[3][3]) +{ + int i,j,k; + double tresh,theta,tau,t,sm,s,h,g,c,b[3],z[3]; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) evectors[i][j] = 0.0; + evectors[i][i] = 1.0; + } + for (i = 0; i < 3; i++) { + b[i] = evalues[i] = matrix[i][i]; + z[i] = 0.0; + } + + for (int iter = 1; iter <= MAXJACOBI; iter++) { + sm = 0.0; + for (i = 0; i < 2; i++) + for (j = i+1; j < 3; j++) + sm += fabs(matrix[i][j]); + if (sm == 0.0) return 0; + + if (iter < 4) tresh = 0.2*sm/(3*3); + else tresh = 0.0; + + for (i = 0; i < 2; i++) { + for (j = i+1; j < 3; j++) { + g = 100.0*fabs(matrix[i][j]); + if (iter > 4 && fabs(evalues[i])+g == fabs(evalues[i]) + && fabs(evalues[j])+g == fabs(evalues[j])) + matrix[i][j] = 0.0; + else if (fabs(matrix[i][j]) > tresh) { + h = evalues[j]-evalues[i]; + if (fabs(h)+g == fabs(h)) t = (matrix[i][j])/h; + else { + theta = 0.5*h/(matrix[i][j]); + t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta)); + if (theta < 0.0) t = -t; + } + c = 1.0/sqrt(1.0+t*t); + s = t*c; + tau = s/(1.0+c); + h = t*matrix[i][j]; + z[i] -= h; + z[j] += h; + evalues[i] -= h; + evalues[j] += h; + matrix[i][j] = 0.0; + for (k = 0; k < i; k++) rotate(matrix,k,i,k,j,s,tau); + for (k = i+1; k < j; k++) rotate(matrix,i,k,k,j,s,tau); + for (k = j+1; k < 3; k++) rotate(matrix,i,k,j,k,s,tau); + for (k = 0; k < 3; k++) rotate(evectors,k,i,k,j,s,tau); + } + } + } + + for (i = 0; i < 3; i++) { + evalues[i] = b[i] += z[i]; + z[i] = 0.0; + } + } + return 1; +} + +/* ---------------------------------------------------------------------- + perform a single Jacobi rotation +------------------------------------------------------------------------- */ + +void MathExtra::rotate(double matrix[3][3], int i, int j, int k, int l, + double s, double tau) +{ + double g = matrix[i][j]; + double h = matrix[k][l]; + matrix[i][j] = g-s*(h+g*tau); + matrix[k][l] = h+s*(g-h*tau); +} + +/* ---------------------------------------------------------------------- + compute rotation matrix from quaternion + quat = [w i j k] +------------------------------------------------------------------------- */ + +void MathExtra::quat_to_mat(const double *quat, double mat[3][3]) +{ + double w2 = quat[0]*quat[0]; + double i2 = quat[1]*quat[1]; + double j2 = quat[2]*quat[2]; + double k2 = quat[3]*quat[3]; + double twoij = 2.0*quat[1]*quat[2]; + double twoik = 2.0*quat[1]*quat[3]; + double twojk = 2.0*quat[2]*quat[3]; + double twoiw = 2.0*quat[1]*quat[0]; + double twojw = 2.0*quat[2]*quat[0]; + double twokw = 2.0*quat[3]*quat[0]; + + mat[0][0] = w2+i2-j2-k2; + mat[0][1] = twoij-twokw; + mat[0][2] = twojw+twoik; + + mat[1][0] = twoij+twokw; + mat[1][1] = w2-i2+j2-k2; + mat[1][2] = twojk-twoiw; + + mat[2][0] = twoik-twojw; + mat[2][1] = twojk+twoiw; + mat[2][2] = w2-i2-j2+k2; +} + +/* ---------------------------------------------------------------------- + compute rotation matrix from quaternion conjugate + quat = [w i j k] +------------------------------------------------------------------------- */ + +void MathExtra::quat_to_mat_trans(const double *quat, double mat[3][3]) +{ + double w2 = quat[0]*quat[0]; + double i2 = quat[1]*quat[1]; + double j2 = quat[2]*quat[2]; + double k2 = quat[3]*quat[3]; + double twoij = 2.0*quat[1]*quat[2]; + double twoik = 2.0*quat[1]*quat[3]; + double twojk = 2.0*quat[2]*quat[3]; + double twoiw = 2.0*quat[1]*quat[0]; + double twojw = 2.0*quat[2]*quat[0]; + double twokw = 2.0*quat[3]*quat[0]; + + mat[0][0] = w2+i2-j2-k2; + mat[1][0] = twoij-twokw; + mat[2][0] = twojw+twoik; + + mat[0][1] = twoij+twokw; + mat[1][1] = w2-i2+j2-k2; + mat[2][1] = twojk-twoiw; + + mat[0][2] = twoik-twojw; + mat[1][2] = twojk+twoiw; + mat[2][2] = w2-i2-j2+k2; +} + +/* ---------------------------------------------------------------------- + compute omega from angular momentum, both in space frame + only know Idiag so need to do M = Iw in body frame + ex,ey,ez are column vectors of rotation matrix P + Mbody = P_transpose Mspace + wbody = Mbody / Idiag + wspace = P wbody + set wbody component to 0.0 if inertia component is 0.0 + otherwise body can spin easily around that axis +------------------------------------------------------------------------- */ + +void MathExtra::angmom_to_omega(double *m, double *ex, double *ey, double *ez, + double *idiag, double *w) +{ + double wbody[3]; + + if (idiag[0] == 0.0) wbody[0] = 0.0; + else wbody[0] = (m[0]*ex[0] + m[1]*ex[1] + m[2]*ex[2]) / idiag[0]; + if (idiag[1] == 0.0) wbody[1] = 0.0; + else wbody[1] = (m[0]*ey[0] + m[1]*ey[1] + m[2]*ey[2]) / idiag[1]; + if (idiag[2] == 0.0) wbody[2] = 0.0; + else wbody[2] = (m[0]*ez[0] + m[1]*ez[1] + m[2]*ez[2]) / idiag[2]; + + w[0] = wbody[0]*ex[0] + wbody[1]*ey[0] + wbody[2]*ez[0]; + w[1] = wbody[0]*ex[1] + wbody[1]*ey[1] + wbody[2]*ez[1]; + w[2] = wbody[0]*ex[2] + wbody[1]*ey[2] + wbody[2]*ez[2]; +} + +/* ---------------------------------------------------------------------- + compute angular momentum from omega, both in space frame + only know Idiag so need to do M = Iw in body frame + ex,ey,ez are column vectors of rotation matrix P + wbody = P_transpose wspace + Mbody = Idiag wbody + Mspace = P Mbody +------------------------------------------------------------------------- */ + +void MathExtra::omega_to_angmom(double *w, + double *ex, double *ey, double *ez, + double *idiag, double *m) +{ + double mbody[3]; + + mbody[0] = (w[0]*ex[0] + w[1]*ex[1] + w[2]*ex[2]) * idiag[0]; + mbody[1] = (w[0]*ey[0] + w[1]*ey[1] + w[2]*ey[2]) * idiag[1]; + mbody[2] = (w[0]*ez[0] + w[1]*ez[1] + w[2]*ez[2]) * idiag[2]; + + m[0] = mbody[0]*ex[0] + mbody[1]*ey[0] + mbody[2]*ez[0]; + m[1] = mbody[0]*ex[1] + mbody[1]*ey[1] + mbody[2]*ez[1]; + m[2] = mbody[0]*ex[2] + mbody[1]*ey[2] + mbody[2]*ez[2]; +} + +/* ---------------------------------------------------------------------- + create unit quaternion from space-frame ex,ey,ez + ex,ey,ez are columns of a rotation matrix +------------------------------------------------------------------------- */ + +void MathExtra::exyz_to_q(double *ex, double *ey, double *ez, double *q) +{ + // squares of quaternion components + + double q0sq = 0.25 * (ex[0] + ey[1] + ez[2] + 1.0); + double q1sq = q0sq - 0.5 * (ey[1] + ez[2]); + double q2sq = q0sq - 0.5 * (ex[0] + ez[2]); + double q3sq = q0sq - 0.5 * (ex[0] + ey[1]); + + // some component must be greater than 1/4 since they sum to 1 + // compute other components from it + + if (q0sq >= 0.25) { + q[0] = sqrt(q0sq); + q[1] = (ey[2] - ez[1]) / (4.0*q[0]); + q[2] = (ez[0] - ex[2]) / (4.0*q[0]); + q[3] = (ex[1] - ey[0]) / (4.0*q[0]); + } else if (q1sq >= 0.25) { + q[1] = sqrt(q1sq); + q[0] = (ey[2] - ez[1]) / (4.0*q[1]); + q[2] = (ey[0] + ex[1]) / (4.0*q[1]); + q[3] = (ex[2] + ez[0]) / (4.0*q[1]); + } else if (q2sq >= 0.25) { + q[2] = sqrt(q2sq); + q[0] = (ez[0] - ex[2]) / (4.0*q[2]); + q[1] = (ey[0] + ex[1]) / (4.0*q[2]); + q[3] = (ez[1] + ey[2]) / (4.0*q[2]); + } else if (q3sq >= 0.25) { + q[3] = sqrt(q3sq); + q[0] = (ex[1] - ey[0]) / (4.0*q[3]); + q[1] = (ez[0] + ex[2]) / (4.0*q[3]); + q[2] = (ez[1] + ey[2]) / (4.0*q[3]); + } + + qnormalize(q); +} + +/* ---------------------------------------------------------------------- + compute space-frame ex,ey,ez from current quaternion q + ex,ey,ez = space-frame coords of 1st,2nd,3rd principal axis + operation is ex = q' d q = Q d, where d is (1,0,0) = 1st axis in body frame +------------------------------------------------------------------------- */ + +void MathExtra::q_to_exyz(double *q, double *ex, double *ey, double *ez) +{ + ex[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]; + ex[1] = 2.0 * (q[1]*q[2] + q[0]*q[3]); + ex[2] = 2.0 * (q[1]*q[3] - q[0]*q[2]); + + ey[0] = 2.0 * (q[1]*q[2] - q[0]*q[3]); + ey[1] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3]; + ey[2] = 2.0 * (q[2]*q[3] + q[0]*q[1]); + + ez[0] = 2.0 * (q[1]*q[3] + q[0]*q[2]); + ez[1] = 2.0 * (q[2]*q[3] - q[0]*q[1]); + ez[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; +} + +/* ---------------------------------------------------------------------- + compute space-frame inertia tensor of an ellipsoid + quat = orientiation quaternion of ellipsoid + radii = 3 radii of ellipsoid + return symmetric inertia tensor as 6-vector in Voigt notation +------------------------------------------------------------------------- */ + +void MathExtra::inertia_ellipsoid(double *radii, double *quat, double mass, + double *inertia) +{ + double p[3][3],ptrans[3][3],itemp[3][3],tensor[3][3]; + double idiag[3]; + + quat_to_mat(quat,p); + quat_to_mat_trans(quat,ptrans); + idiag[0] = 0.2*mass * (radii[1]*radii[1] + radii[2]*radii[2]); + idiag[1] = 0.2*mass * (radii[0]*radii[0] + radii[2]*radii[2]); + idiag[2] = 0.2*mass * (radii[0]*radii[0] + radii[1]*radii[1]); + diag_times3(idiag,ptrans,itemp); + times3(p,itemp,tensor); + inertia[0] = tensor[0][0]; + inertia[1] = tensor[1][1]; + inertia[2] = tensor[2][2]; + inertia[3] = tensor[1][2]; + inertia[4] = tensor[0][2]; + inertia[5] = tensor[0][1]; +} + +/* ---------------------------------------------------------------------- + compute space-frame inertia tensor of a triangle + v0,v1,v2 = 3 vertices of triangle + from http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle: + inertia tensor = a/24 (v0^2 + v1^2 + v2^2 + (v0+v1+v2)^2) I - a Vt S V + a = 2*area of tri = |(v1-v0) x (v2-v0)| + I = 3x3 identity matrix + V = 3x3 matrix with v0,v1,v2 as rows + Vt = 3x3 matrix with v0,v1,v2 as columns + S = 1/24 [2 1 1] + [1 2 1] + [1 1 2] + return symmetric inertia tensor as 6-vector in Voigt notation +------------------------------------------------------------------------- */ + +void MathExtra::inertia_triangle(double *v0, double *v1, double *v2, + double mass, double *inertia) +{ + double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}}; + double v[3][3],sv[3][3],vtsv[3][3]; + double vvv[3],v1mv0[3],v2mv0[3],normal[3]; + + v[0][0] = v0[0]; v[0][1] = v0[2]; v[0][2] = v0[3]; + v[1][0] = v1[0]; v[1][1] = v1[2]; v[1][2] = v1[3]; + v[2][0] = v2[0]; v[2][1] = v2[2]; v[2][2] = v2[3]; + + times3(s,v,sv); + transpose_times3(v,sv,vtsv); + + double sum = lensq3(v0) + lensq3(v1) + lensq3(v2); + vvv[0] = v0[0] + v1[0] + v2[0]; + vvv[1] = v0[1] + v1[1] + v2[1]; + vvv[2] = v0[2] + v1[2] + v2[2]; + sum += lensq3(vvv); + + sub3(v1,v0,v1mv0); + sub3(v2,v0,v2mv0); + cross3(v1mv0,v2mv0,normal); + double a = len3(normal); + double inv24 = 1.0/24.0; + + // NOTE: use mass + + inertia[0] = inv24*a * (sum-vtsv[0][0]); + inertia[1] = inv24*a * (sum-vtsv[1][1]); + inertia[2] = inv24*a * (sum-vtsv[2][2]); + inertia[3] = -inv24*a*vtsv[1][2]; + inertia[4] = -inv24*a*vtsv[0][2]; + inertia[5] = -inv24*a*vtsv[0][1]; +} + +/* ---------------------------------------------------------------------- */ + +} diff --git a/src/math_extra.h b/src/math_extra.h index 26c6c7ed2d..29aaf942be 100755 --- a/src/math_extra.h +++ b/src/math_extra.h @@ -19,8 +19,9 @@ #define LMP_MATH_EXTRA_H #include "math.h" - -// short methods are inlined, others are in math_extra.cpp +#include "stdio.h" +#include "string.h" +#include "error.h" namespace MathExtra { @@ -67,22 +68,23 @@ namespace MathExtra { int jacobi(double matrix[3][3], double *evalues, double evectors[3][3]); void rotate(double matrix[3][3], int i, int j, int k, int l, double s, double tau); - + // shape matrix operations // upper-triangular 3x3 matrix stored in Voigt notation as 6-vector inline void multiply_shape_shape(const double *one, const double *two, double *ans); + // quaternion operations - - inline void axisangle_to_quat(const double *v, const double angle, - double *quat); + + inline void qnormalize(double *q); + inline void qconjugate(double *q, double *qc); inline void vecquat(double *a, double *b, double *c); inline void quatvec(double *a, double *b, double *c); inline void quatquat(double *a, double *b, double *c); inline void invquatvec(double *a, double *b, double *c); - inline void qconjugate(double *q, double *qc); - inline void qnormalize(double *q); + inline void axisangle_to_quat(const double *v, const double angle, + double *quat); inline void matvec_rows(double *x, double *y, double *z, double *b, double *c); inline void matvec_cols(double *x, double *y, double *z, @@ -96,6 +98,8 @@ namespace MathExtra { void q_to_exyz(double *q, double *ex, double *ey, double *ez); void quat_to_mat(const double *quat, double mat[3][3]); void quat_to_mat_trans(const double *quat, double mat[3][3]); + void quat_to_mat(const double *quat, double mat[3][3]); + void quat_to_mat_trans(const double *quat, double mat[3][3]); // rotation operations @@ -109,6 +113,7 @@ namespace MathExtra { double *inertia); void inertia_triangle(double *v0, double *v1, double *v2, double mass, double *inertia); + } /* ---------------------------------------------------------------------- @@ -193,7 +198,7 @@ double MathExtra::lensq3(const double *v) double MathExtra::dot3(const double *v1, const double *v2) { - return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; + return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]; } /* ---------------------------------------------------------------------- @@ -202,9 +207,9 @@ double MathExtra::dot3(const double *v1, const double *v2) void MathExtra::cross3(const double *v1, const double *v2, double *ans) { - ans[0] = v1[1]*v2[2] - v1[2]*v2[1]; - ans[1] = v1[2]*v2[0] - v1[0]*v2[2]; - ans[2] = v1[0]*v2[1] - v1[1]*v2[0]; + ans[0] = v1[1]*v2[2]-v1[2]*v2[1]; + ans[1] = v1[2]*v2[0]-v1[0]*v2[2]; + ans[2] = v1[0]*v2[1]-v1[1]*v2[0]; } /* ---------------------------------------------------------------------- @@ -213,8 +218,7 @@ void MathExtra::cross3(const double *v1, const double *v2, double *ans) double MathExtra::det3(const double m[3][3]) { - double ans = - m[0][0]*m[1][1]*m[2][2] - m[0][0]*m[1][2]*m[2][1] - + double ans = m[0][0]*m[1][1]*m[2][2] - m[0][0]*m[1][2]*m[2][1] - m[1][0]*m[0][1]*m[2][2] + m[1][0]*m[0][2]*m[2][1] + m[2][0]*m[0][1]*m[1][2] - m[2][0]*m[0][2]*m[1][1]; return ans; @@ -245,15 +249,15 @@ void MathExtra::diag_times3(const double *d, const double m[3][3], void MathExtra::plus3(const double m[3][3], const double m2[3][3], double ans[3][3]) { - ans[0][0] = m[0][0] + m2[0][0]; - ans[0][1] = m[0][1] + m2[0][1]; - ans[0][2] = m[0][2] + m2[0][2]; - ans[1][0] = m[1][0] + m2[1][0]; - ans[1][1] = m[1][1] + m2[1][1]; - ans[1][2] = m[1][2] + m2[1][2]; - ans[2][0] = m[2][0] + m2[2][0]; - ans[2][1] = m[2][1] + m2[2][1]; - ans[2][2] = m[2][2] + m2[2][2]; + ans[0][0] = m[0][0]+m2[0][0]; + ans[0][1] = m[0][1]+m2[0][1]; + ans[0][2] = m[0][2]+m2[0][2]; + ans[1][0] = m[1][0]+m2[1][0]; + ans[1][1] = m[1][1]+m2[1][1]; + ans[1][2] = m[1][2]+m2[1][2]; + ans[2][0] = m[2][0]+m2[2][0]; + ans[2][1] = m[2][1]+m2[2][1]; + ans[2][2] = m[2][2]+m2[2][2]; } /* ---------------------------------------------------------------------- @@ -263,15 +267,15 @@ void MathExtra::plus3(const double m[3][3], const double m2[3][3], void MathExtra::times3(const double m[3][3], const double m2[3][3], double ans[3][3]) { - ans[0][0] = m[0][0]*m2[0][0] + m[0][1]*m2[1][0] + m[0][2]*m2[2][0]; - ans[0][1] = m[0][0]*m2[0][1] + m[0][1]*m2[1][1] + m[0][2]*m2[2][1]; - ans[0][2] = m[0][0]*m2[0][2] + m[0][1]*m2[1][2] + m[0][2]*m2[2][2]; - ans[1][0] = m[1][0]*m2[0][0] + m[1][1]*m2[1][0] + m[1][2]*m2[2][0]; - ans[1][1] = m[1][0]*m2[0][1] + m[1][1]*m2[1][1] + m[1][2]*m2[2][1]; - ans[1][2] = m[1][0]*m2[0][2] + m[1][1]*m2[1][2] + m[1][2]*m2[2][2]; - ans[2][0] = m[2][0]*m2[0][0] + m[2][1]*m2[1][0] + m[2][2]*m2[2][0]; - ans[2][1] = m[2][0]*m2[0][1] + m[2][1]*m2[1][1] + m[2][2]*m2[2][1]; - ans[2][2] = m[2][0]*m2[0][2] + m[2][1]*m2[1][2] + m[2][2]*m2[2][2]; + ans[0][0] = m[0][0]*m2[0][0]+m[0][1]*m2[1][0]+m[0][2]*m2[2][0]; + ans[0][1] = m[0][0]*m2[0][1]+m[0][1]*m2[1][1]+m[0][2]*m2[2][1]; + ans[0][2] = m[0][0]*m2[0][2]+m[0][1]*m2[1][2]+m[0][2]*m2[2][2]; + ans[1][0] = m[1][0]*m2[0][0]+m[1][1]*m2[1][0]+m[1][2]*m2[2][0]; + ans[1][1] = m[1][0]*m2[0][1]+m[1][1]*m2[1][1]+m[1][2]*m2[2][1]; + ans[1][2] = m[1][0]*m2[0][2]+m[1][1]*m2[1][2]+m[1][2]*m2[2][2]; + ans[2][0] = m[2][0]*m2[0][0]+m[2][1]*m2[1][0]+m[2][2]*m2[2][0]; + ans[2][1] = m[2][0]*m2[0][1]+m[2][1]*m2[1][1]+m[2][2]*m2[2][1]; + ans[2][2] = m[2][0]*m2[0][2]+m[2][1]*m2[1][2]+m[2][2]*m2[2][2]; } /* ---------------------------------------------------------------------- @@ -281,15 +285,15 @@ void MathExtra::times3(const double m[3][3], const double m2[3][3], void MathExtra::transpose_times3(const double m[3][3], const double m2[3][3], double ans[3][3]) { - ans[0][0] = m[0][0]*m2[0][0] + m[1][0]*m2[1][0] + m[2][0]*m2[2][0]; - ans[0][1] = m[0][0]*m2[0][1] + m[1][0]*m2[1][1] + m[2][0]*m2[2][1]; - ans[0][2] = m[0][0]*m2[0][2] + m[1][0]*m2[1][2] + m[2][0]*m2[2][2]; - ans[1][0] = m[0][1]*m2[0][0] + m[1][1]*m2[1][0] + m[2][1]*m2[2][0]; - ans[1][1] = m[0][1]*m2[0][1] + m[1][1]*m2[1][1] + m[2][1]*m2[2][1]; - ans[1][2] = m[0][1]*m2[0][2] + m[1][1]*m2[1][2] + m[2][1]*m2[2][2]; - ans[2][0] = m[0][2]*m2[0][0] + m[1][2]*m2[1][0] + m[2][2]*m2[2][0]; - ans[2][1] = m[0][2]*m2[0][1] + m[1][2]*m2[1][1] + m[2][2]*m2[2][1]; - ans[2][2] = m[0][2]*m2[0][2] + m[1][2]*m2[1][2] + m[2][2]*m2[2][2]; + ans[0][0] = m[0][0]*m2[0][0]+m[1][0]*m2[1][0]+m[2][0]*m2[2][0]; + ans[0][1] = m[0][0]*m2[0][1]+m[1][0]*m2[1][1]+m[2][0]*m2[2][1]; + ans[0][2] = m[0][0]*m2[0][2]+m[1][0]*m2[1][2]+m[2][0]*m2[2][2]; + ans[1][0] = m[0][1]*m2[0][0]+m[1][1]*m2[1][0]+m[2][1]*m2[2][0]; + ans[1][1] = m[0][1]*m2[0][1]+m[1][1]*m2[1][1]+m[2][1]*m2[2][1]; + ans[1][2] = m[0][1]*m2[0][2]+m[1][1]*m2[1][2]+m[2][1]*m2[2][2]; + ans[2][0] = m[0][2]*m2[0][0]+m[1][2]*m2[1][0]+m[2][2]*m2[2][0]; + ans[2][1] = m[0][2]*m2[0][1]+m[1][2]*m2[1][1]+m[2][2]*m2[2][1]; + ans[2][2] = m[0][2]*m2[0][2]+m[1][2]*m2[1][2]+m[2][2]*m2[2][2]; } /* ---------------------------------------------------------------------- @@ -299,20 +303,20 @@ void MathExtra::transpose_times3(const double m[3][3], const double m2[3][3], void MathExtra::times3_transpose(const double m[3][3], const double m2[3][3], double ans[3][3]) { - ans[0][0] = m[0][0]*m2[0][0] + m[0][1]*m2[0][1] + m[0][2]*m2[0][2]; - ans[0][1] = m[0][0]*m2[1][0] + m[0][1]*m2[1][1] + m[0][2]*m2[1][2]; - ans[0][2] = m[0][0]*m2[2][0] + m[0][1]*m2[2][1] + m[0][2]*m2[2][2]; - ans[1][0] = m[1][0]*m2[0][0] + m[1][1]*m2[0][1] + m[1][2]*m2[0][2]; - ans[1][1] = m[1][0]*m2[1][0] + m[1][1]*m2[1][1] + m[1][2]*m2[1][2]; - ans[1][2] = m[1][0]*m2[2][0] + m[1][1]*m2[2][1] + m[1][2]*m2[2][2]; - ans[2][0] = m[2][0]*m2[0][0] + m[2][1]*m2[0][1] + m[2][2]*m2[0][2]; - ans[2][1] = m[2][0]*m2[1][0] + m[2][1]*m2[1][1] + m[2][2]*m2[1][2]; - ans[2][2] = m[2][0]*m2[2][0] + m[2][1]*m2[2][1] + m[2][2]*m2[2][2]; + ans[0][0] = m[0][0]*m2[0][0]+m[0][1]*m2[0][1]+m[0][2]*m2[0][2]; + ans[0][1] = m[0][0]*m2[1][0]+m[0][1]*m2[1][1]+m[0][2]*m2[1][2]; + ans[0][2] = m[0][0]*m2[2][0]+m[0][1]*m2[2][1]+m[0][2]*m2[2][2]; + ans[1][0] = m[1][0]*m2[0][0]+m[1][1]*m2[0][1]+m[1][2]*m2[0][2]; + ans[1][1] = m[1][0]*m2[1][0]+m[1][1]*m2[1][1]+m[1][2]*m2[1][2]; + ans[1][2] = m[1][0]*m2[2][0]+m[1][1]*m2[2][1]+m[1][2]*m2[2][2]; + ans[2][0] = m[2][0]*m2[0][0]+m[2][1]*m2[0][1]+m[2][2]*m2[0][2]; + ans[2][1] = m[2][0]*m2[1][0]+m[2][1]*m2[1][1]+m[2][2]*m2[1][2]; + ans[2][2] = m[2][0]*m2[2][0]+m[2][1]*m2[2][1]+m[2][2]*m2[2][2]; } /* ---------------------------------------------------------------------- invert a matrix - does NOT check for singular or badly scaled matrix + does NOT checks for singular or badly scaled matrix ------------------------------------------------------------------------- */ void MathExtra::invert3(const double m[3][3], double ans[3][3]) @@ -339,9 +343,9 @@ void MathExtra::invert3(const double m[3][3], double ans[3][3]) void MathExtra::times_column3(const double m[3][3], const double *v, double *ans) { - ans[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; - ans[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; - ans[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; + ans[0] = m[0][0]*v[0]+m[0][1]*v[1]+m[0][2]*v[2]; + ans[1] = m[1][0]*v[0]+m[1][1]*v[1]+m[1][2]*v[2]; + ans[2] = m[2][0]*v[0]+m[2][1]*v[1]+m[2][2]*v[2]; } /* ---------------------------------------------------------------------- @@ -351,9 +355,9 @@ void MathExtra::times_column3(const double m[3][3], const double *v, void MathExtra::transpose_times_column3(const double m[3][3], const double *v, double *ans) { - ans[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2]; - ans[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2]; - ans[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2]; + ans[0] = m[0][0]*v[0]+m[1][0]*v[1]+m[2][0]*v[2]; + ans[1] = m[0][1]*v[0]+m[1][1]*v[1]+m[2][1]*v[2]; + ans[2] = m[0][2]*v[0]+m[1][2]*v[1]+m[2][2]*v[2]; } /* ---------------------------------------------------------------------- @@ -381,9 +385,9 @@ void MathExtra::transpose_times_diag3(const double m[3][3], void MathExtra::row_times3(const double *v, const double m[3][3], double *ans) { - ans[0] = m[0][0]*v[0] + v[1]*m[1][0] + v[2]*m[2][0]; - ans[1] = v[0]*m[0][1] + m[1][1]*v[1] + v[2]*m[2][1]; - ans[2] = v[0]*m[0][2] + v[1]*m[1][2] + m[2][2]*v[2]; + ans[0] = m[0][0]*v[0]+v[1]*m[1][0]+v[2]*m[2][0]; + ans[1] = v[0]*m[0][1]+m[1][1]*v[1]+v[2]*m[2][1]; + ans[2] = v[0]*m[0][2]+v[1]*m[1][2]+m[2][2]*v[2]; } /* ---------------------------------------------------------------------- @@ -392,20 +396,14 @@ void MathExtra::row_times3(const double *v, const double m[3][3], inline void MathExtra::scalar_times3(const double f, double m[3][3]) { - m[0][0] *= f; - m[0][1] *= f; - m[0][2] *= f; - m[1][0] *= f; - m[1][1] *= f; - m[1][2] *= f; - m[2][0] *= f; - m[2][1] *= f; - m[2][2] *= f; + m[0][0] *= f; m[0][1] *= f; m[0][2] *= f; + m[1][0] *= f; m[1][1] *= f; m[1][2] *= f; + m[2][0] *= f; m[2][1] *= f; m[2][2] *= f; } /* ---------------------------------------------------------------------- - multiply 2 shape matrices to yield a 3rd shape matrix - upper-triangular 3x3 matrices stored in Voigt notation as 6-vectors + multiply 2 shape matrices + upper-triangular 3x3, stored as 6-vector in Voigt notation ------------------------------------------------------------------------- */ void MathExtra::multiply_shape_shape(const double *one, const double *two, @@ -420,19 +418,29 @@ void MathExtra::multiply_shape_shape(const double *one, const double *two, } /* ---------------------------------------------------------------------- - compute quaternion from axis-angle rotation - v MUST be a unit vector + normalize a quaternion ------------------------------------------------------------------------- */ -void MathExtra::axisangle_to_quat(const double *v, const double angle, - double *quat) +void MathExtra::qnormalize(double *q) { - double halfa = 0.5*angle; - double sina = sin(halfa); - quat[0] = cos(halfa); - quat[1] = v[0]*sina; - quat[2] = v[1]*sina; - quat[3] = v[2]*sina; + double norm = 1.0 / sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); + q[0] *= norm; + q[1] *= norm; + q[2] *= norm; + q[3] *= norm; +} + +/* ---------------------------------------------------------------------- + conjugate of a quaternion: qc = conjugate of q + assume q is of unit length +------------------------------------------------------------------------- */ + +void MathExtra::qconjugate(double *q, double *qc) +{ + qc[0] = q[0]; + qc[1] = -q[1]; + qc[2] = -q[2]; + qc[3] = -q[3]; } /* ---------------------------------------------------------------------- @@ -461,7 +469,6 @@ void MathExtra::quatvec(double *a, double *b, double *c) /* ---------------------------------------------------------------------- quaternion-quaternion multiply: c = a*b - NOT a commutative operation ------------------------------------------------------------------------- */ void MathExtra::quatquat(double *a, double *b, double *c) @@ -487,29 +494,19 @@ void MathExtra::invquatvec(double *a, double *b, double *c) } /* ---------------------------------------------------------------------- - conjugate of a quaternion: qc = conjugate of q - assume q is of unit length + compute quaternion from axis-angle rotation + v MUST be a unit vector ------------------------------------------------------------------------- */ -void MathExtra::qconjugate(double *q, double *qc) +void MathExtra::axisangle_to_quat(const double *v, const double angle, + double *quat) { - qc[0] = q[0]; - qc[1] = -q[1]; - qc[2] = -q[2]; - qc[3] = -q[3]; -} - -/* ---------------------------------------------------------------------- - normalize a quaternion -------------------------------------------------------------------------- */ - -void MathExtra::qnormalize(double *q) -{ - double norm = 1.0 / sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); - q[0] *= norm; - q[1] *= norm; - q[2] *= norm; - q[3] *= norm; + double halfa = 0.5*angle; + double sina = sin(halfa); + quat[0] = cos(halfa); + quat[1] = v[0]*sina; + quat[2] = v[1]*sina; + quat[3] = v[2]*sina; } /* ---------------------------------------------------------------------- @@ -537,54 +534,54 @@ void MathExtra::matvec_cols(double *x, double *y, double *z, } /* ---------------------------------------------------------------------- - apply principal rotation generator about x to rotation matrix m + Apply principal rotation generator about x to rotation matrix m ------------------------------------------------------------------------- */ void MathExtra::rotation_generator_x(const double m[3][3], double ans[3][3]) { - ans[0][0] = 0.0; + ans[0][0] = 0; ans[0][1] = -m[0][2]; ans[0][2] = m[0][1]; - ans[1][0] = 0.0; + ans[1][0] = 0; ans[1][1] = -m[1][2]; ans[1][2] = m[1][1]; - ans[2][0] = 0.0; + ans[2][0] = 0; ans[2][1] = -m[2][2]; ans[2][2] = m[2][1]; } /* ---------------------------------------------------------------------- - apply principal rotation generator about y to rotation matrix m + Apply principal rotation generator about y to rotation matrix m ------------------------------------------------------------------------- */ void MathExtra::rotation_generator_y(const double m[3][3], double ans[3][3]) { ans[0][0] = m[0][2]; - ans[0][1] = 0.0; + ans[0][1] = 0; ans[0][2] = -m[0][0]; ans[1][0] = m[1][2]; - ans[1][1] = 0.0; + ans[1][1] = 0; ans[1][2] = -m[1][0]; ans[2][0] = m[2][2]; - ans[2][1] = 0.0; + ans[2][1] = 0; ans[2][2] = -m[2][0]; } /* ---------------------------------------------------------------------- - apply principal rotation generator about z to rotation matrix m + Apply principal rotation generator about z to rotation matrix m ------------------------------------------------------------------------- */ void MathExtra::rotation_generator_z(const double m[3][3], double ans[3][3]) { ans[0][0] = -m[0][1]; ans[0][1] = m[0][0]; - ans[0][2] = 0.0; + ans[0][2] = 0; ans[1][0] = -m[1][1]; ans[1][1] = m[1][0]; - ans[1][2] = 0.0; + ans[1][2] = 0; ans[2][0] = -m[2][1]; ans[2][1] = m[2][0]; - ans[2][2] = 0.0; + ans[2][2] = 0; } - + #endif