forked from lijiext/lammps
git-svn-id: svn://svn.icms.temple.edu/lammps-ro/trunk@5981 f3b2605a-c512-4ea7-a41b-209d697bcdaa
This commit is contained in:
sjplimp
parent
18701fe232
commit
b482468fb1
|
|
@ -96,7 +96,7 @@ double ComputeERotateAsphere::compute_scalar()
|
|||
// wbody = angular velocity in body frame
|
||||
|
||||
MathExtra::quat_to_mat(quat,rot);
|
||||
MathExtra::transpose_times_column3(rot,angmom[i],wbody);
|
||||
MathExtra::transpose_matvec(rot,angmom[i],wbody);
|
||||
wbody[0] /= inertia[0];
|
||||
wbody[1] /= inertia[1];
|
||||
wbody[2] /= inertia[2];
|
||||
|
|
|
|||
|
|
@ -186,7 +186,7 @@ double ComputeTempAsphere::compute_scalar()
|
|||
// wbody = angular velocity in body frame
|
||||
|
||||
MathExtra::quat_to_mat(quat,rot);
|
||||
MathExtra::transpose_times_column3(rot,angmom[i],wbody);
|
||||
MathExtra::transpose_matvec(rot,angmom[i],wbody);
|
||||
wbody[0] /= inertia[0];
|
||||
wbody[1] /= inertia[1];
|
||||
wbody[2] /= inertia[2];
|
||||
|
|
@ -255,7 +255,7 @@ void ComputeTempAsphere::compute_vector()
|
|||
// wbody = angular velocity in body frame
|
||||
|
||||
MathExtra::quat_to_mat(quat,rot);
|
||||
MathExtra::transpose_times_column3(rot,angmom[i],wbody);
|
||||
MathExtra::transpose_matvec(rot,angmom[i],wbody);
|
||||
wbody[0] /= inertia[0];
|
||||
wbody[1] /= inertia[1];
|
||||
wbody[2] /= inertia[2];
|
||||
|
|
|
|||
|
|
@ -620,18 +620,18 @@ double PairGayBerne::gayberne_analytic(const int i,const int j,double a1[3][3],
|
|||
tempv[0] = -uslj_rsq*kappa[0];
|
||||
tempv[1] = -uslj_rsq*kappa[1];
|
||||
tempv[2] = -uslj_rsq*kappa[2];
|
||||
MathExtra::row_times3(kappa,g1,tempv2);
|
||||
MathExtra::vecmat(kappa,g1,tempv2);
|
||||
MathExtra::cross3(tempv,tempv2,dUr);
|
||||
double dUr2[3];
|
||||
|
||||
if (newton_pair || j < nlocal) {
|
||||
MathExtra::row_times3(kappa,g2,tempv2);
|
||||
MathExtra::vecmat(kappa,g2,tempv2);
|
||||
MathExtra::cross3(tempv,tempv2,dUr2);
|
||||
}
|
||||
|
||||
// compute d_chi
|
||||
|
||||
MathExtra::row_times3(iota,b1,tempv);
|
||||
MathExtra::vecmat(iota,b1,tempv);
|
||||
MathExtra::cross3(tempv,iota,dchi);
|
||||
temp1 = -4.0/rsq;
|
||||
dchi[0] *= temp1;
|
||||
|
|
@ -640,7 +640,7 @@ double PairGayBerne::gayberne_analytic(const int i,const int j,double a1[3][3],
|
|||
double dchi2[3];
|
||||
|
||||
if (newton_pair || j < nlocal) {
|
||||
MathExtra::row_times3(iota,b2,tempv);
|
||||
MathExtra::vecmat(iota,b2,tempv);
|
||||
MathExtra::cross3(tempv,iota,dchi2);
|
||||
dchi2[0] *= temp1;
|
||||
dchi2[1] *= temp1;
|
||||
|
|
@ -806,12 +806,12 @@ double PairGayBerne::gayberne_lj(const int i,const int j,double a1[3][3],
|
|||
tempv[0] = -uslj_rsq*kappa[0];
|
||||
tempv[1] = -uslj_rsq*kappa[1];
|
||||
tempv[2] = -uslj_rsq*kappa[2];
|
||||
MathExtra::row_times3(kappa,g1,tempv2);
|
||||
MathExtra::vecmat(kappa,g1,tempv2);
|
||||
MathExtra::cross3(tempv,tempv2,dUr);
|
||||
|
||||
// compute d_chi
|
||||
|
||||
MathExtra::row_times3(iota,b1,tempv);
|
||||
MathExtra::vecmat(iota,b1,tempv);
|
||||
MathExtra::cross3(tempv,iota,dchi);
|
||||
temp1 = -4.0/rsq;
|
||||
dchi[0] *= temp1;
|
||||
|
|
|
|||
|
|
@ -615,8 +615,8 @@ double PairRESquared::resquared_analytic(const int i, const int j,
|
|||
if (ierror) error->all("Bad matrix inversion in mldivide3");
|
||||
|
||||
sigma12 = 1.0/sqrt(0.5*MathExtra::dot3(s,rhat));
|
||||
MathExtra::times_column3(wi.A,rhat,z1);
|
||||
MathExtra::times_column3(wj.A,rhat,z2);
|
||||
MathExtra::matvec(wi.A,rhat,z1);
|
||||
MathExtra::matvec(wj.A,rhat,z2);
|
||||
v1[0] = z1[0]/shape2[type[i]][0];
|
||||
v1[1] = z1[1]/shape2[type[i]][1];
|
||||
v1[2] = z1[2]/shape2[type[i]][2];
|
||||
|
|
@ -737,8 +737,8 @@ double PairRESquared::resquared_analytic(const int i, const int j,
|
|||
u[0] /= rnorm;
|
||||
u[1] /= rnorm;
|
||||
u[2] /= rnorm;
|
||||
MathExtra::times_column3(wi.A,u,u1);
|
||||
MathExtra::times_column3(wj.A,u,u2);
|
||||
MathExtra::matvec(wi.A,u,u1);
|
||||
MathExtra::matvec(wj.A,u,u2);
|
||||
dsigma1=MathExtra::dot3(u1,vsigma1);
|
||||
dsigma2=MathExtra::dot3(u2,vsigma2);
|
||||
dH12[0][0] = dsigma1*gsigma1[0][0]+dsigma2*gsigma2[0][0];
|
||||
|
|
@ -763,10 +763,10 @@ double PairRESquared::resquared_analytic(const int i, const int j,
|
|||
|
||||
// torque on i
|
||||
|
||||
MathExtra::row_times3(fourw,wi.aTe,fwae);
|
||||
MathExtra::vecmat(fourw,wi.aTe,fwae);
|
||||
|
||||
for (int i=0; i<3; i++) {
|
||||
MathExtra::times_column3(wi.lA[i],rhat,p);
|
||||
MathExtra::matvec(wi.lA[i],rhat,p);
|
||||
dsigma1 = MathExtra::dot3(p,vsigma1);
|
||||
dH12[0][0] = wi.lAsa[i][0][0]/sigma1+dsigma1*gsigma1[0][0];
|
||||
dH12[0][1] = wi.lAsa[i][0][1]/sigma1+dsigma1*gsigma1[0][1];
|
||||
|
|
@ -781,9 +781,9 @@ double PairRESquared::resquared_analytic(const int i, const int j,
|
|||
deta = tsig1sig2*dsigma1-tdH*ddH;
|
||||
deta -= teta1*dsigma1;
|
||||
double tempv[3];
|
||||
MathExtra::times_column3(wi.lA[i],w,tempv);
|
||||
MathExtra::matvec(wi.lA[i],w,tempv);
|
||||
dchi = -MathExtra::dot3(fwae,tempv);
|
||||
MathExtra::times_column3(wi.lAtwo[i],spr,tempv);
|
||||
MathExtra::matvec(wi.lAtwo[i],spr,tempv);
|
||||
dh12 = -MathExtra::dot3(s,tempv);
|
||||
|
||||
dUa = pbsu*(eta*dchi + deta*chi)-dh12*dspu;
|
||||
|
|
@ -796,10 +796,10 @@ double PairRESquared::resquared_analytic(const int i, const int j,
|
|||
if (!(force->newton_pair || j < atom->nlocal))
|
||||
return Ua+Ur;
|
||||
|
||||
MathExtra::row_times3(fourw,wj.aTe,fwae);
|
||||
MathExtra::vecmat(fourw,wj.aTe,fwae);
|
||||
|
||||
for (int i=0; i<3; i++) {
|
||||
MathExtra::times_column3(wj.lA[i],rhat,p);
|
||||
MathExtra::matvec(wj.lA[i],rhat,p);
|
||||
dsigma2 = MathExtra::dot3(p,vsigma2);
|
||||
dH12[0][0] = wj.lAsa[i][0][0]/sigma2+dsigma2*gsigma2[0][0];
|
||||
dH12[0][1] = wj.lAsa[i][0][1]/sigma2+dsigma2*gsigma2[0][1];
|
||||
|
|
@ -814,9 +814,9 @@ double PairRESquared::resquared_analytic(const int i, const int j,
|
|||
deta = tsig1sig2*dsigma2-tdH*ddH;
|
||||
deta -= teta2*dsigma2;
|
||||
double tempv[3];
|
||||
MathExtra::times_column3(wj.lA[i],w,tempv);
|
||||
MathExtra::matvec(wj.lA[i],w,tempv);
|
||||
dchi = -MathExtra::dot3(fwae,tempv);
|
||||
MathExtra::times_column3(wj.lAtwo[i],spr,tempv);
|
||||
MathExtra::matvec(wj.lAtwo[i],spr,tempv);
|
||||
dh12 = -MathExtra::dot3(s,tempv);
|
||||
|
||||
dUa = pbsu*(eta*dchi + deta*chi)-dh12*dspu;
|
||||
|
|
@ -973,14 +973,14 @@ double PairRESquared::resquared_lj(const int i, const int j,
|
|||
// torque on i
|
||||
|
||||
if (calc_torque) {
|
||||
MathExtra::row_times3(fourw,wi.aTe,fwae);
|
||||
MathExtra::vecmat(fourw,wi.aTe,fwae);
|
||||
|
||||
for (int i=0; i<3; i++) {
|
||||
MathExtra::times_column3(wi.lA[i],rhat,p);
|
||||
MathExtra::matvec(wi.lA[i],rhat,p);
|
||||
double tempv[3];
|
||||
MathExtra::times_column3(wi.lA[i],w,tempv);
|
||||
MathExtra::matvec(wi.lA[i],w,tempv);
|
||||
dchi = -MathExtra::dot3(fwae,tempv);
|
||||
MathExtra::times_column3(lAtwo[i],spr,tempv);
|
||||
MathExtra::matvec(lAtwo[i],spr,tempv);
|
||||
dh12 = -MathExtra::dot3(s,tempv);
|
||||
|
||||
dUa = pbsu*dchi-dh12*dspu;
|
||||
|
|
|
|||
|
|
@ -732,7 +732,7 @@ void FixRigid::init()
|
|||
// set displace = 0.0 for atoms not in any rigid body
|
||||
// for extended particles, set their orientation wrt to rigid body
|
||||
|
||||
double qc[4];
|
||||
double qc[4],delta[3];
|
||||
double *quatatom;
|
||||
|
||||
for (i = 0; i < nlocal; i++) {
|
||||
|
|
@ -757,25 +757,16 @@ void FixRigid::init()
|
|||
zunwrap = x[i][2] + zbox*zprd;
|
||||
}
|
||||
|
||||
dx = xunwrap - xcm[ibody][0];
|
||||
dy = yunwrap - xcm[ibody][1];
|
||||
dz = zunwrap - xcm[ibody][2];
|
||||
|
||||
displace[i][0] = dx*ex_space[ibody][0] + dy*ex_space[ibody][1] +
|
||||
dz*ex_space[ibody][2];
|
||||
displace[i][1] = dx*ey_space[ibody][0] + dy*ey_space[ibody][1] +
|
||||
dz*ey_space[ibody][2];
|
||||
displace[i][2] = dx*ez_space[ibody][0] + dy*ez_space[ibody][1] +
|
||||
dz*ez_space[ibody][2];
|
||||
|
||||
delta[0] = xunwrap - xcm[ibody][0];
|
||||
delta[1] = yunwrap - xcm[ibody][1];
|
||||
delta[2] = zunwrap - xcm[ibody][2];
|
||||
MathExtra::transpose_matvec(ex_space[ibody],ey_space[ibody],
|
||||
ez_space[ibody],delta,displace[i]);
|
||||
|
||||
if (extended) {
|
||||
if (eflags[i] & ORIENT_DIPOLE) {
|
||||
dorient[i][0] = mu[i][0]*ex_space[ibody][0] +
|
||||
mu[i][1]*ex_space[ibody][1] + mu[i][2]*ex_space[ibody][2];
|
||||
dorient[i][1] = mu[i][0]*ey_space[ibody][0] +
|
||||
mu[i][1]*ey_space[ibody][1] + mu[i][2]*ey_space[ibody][2];
|
||||
dorient[i][2] = mu[i][0]*ez_space[ibody][0] +
|
||||
mu[i][1]*ez_space[ibody][1] + mu[i][2]*ez_space[ibody][2];
|
||||
MathExtra::transpose_matvec(ex_space[ibody],ey_space[ibody],
|
||||
ez_space[ibody],mu[i],dorient[i]);
|
||||
MathExtra::snormalize3(mu[i][3],dorient[i],dorient[i]);
|
||||
} else if (dorientflag)
|
||||
dorient[i][0] = dorient[i][1] = dorient[i][2] = 0.0;
|
||||
|
|
@ -1498,15 +1489,8 @@ void FixRigid::set_xv()
|
|||
// x = displacement from center-of-mass, based on body orientation
|
||||
// v = vcm + omega around center-of-mass
|
||||
|
||||
x[i][0] = ex_space[ibody][0]*displace[i][0] +
|
||||
ey_space[ibody][0]*displace[i][1] +
|
||||
ez_space[ibody][0]*displace[i][2];
|
||||
x[i][1] = ex_space[ibody][1]*displace[i][0] +
|
||||
ey_space[ibody][1]*displace[i][1] +
|
||||
ez_space[ibody][1]*displace[i][2];
|
||||
x[i][2] = ex_space[ibody][2]*displace[i][0] +
|
||||
ey_space[ibody][2]*displace[i][1] +
|
||||
ez_space[ibody][2]*displace[i][2];
|
||||
MathExtra::matvec(ex_space[ibody],ey_space[ibody],
|
||||
ez_space[ibody],displace[i],x[i]);
|
||||
|
||||
v[i][0] = omega[ibody][1]*x[i][2] - omega[ibody][2]*x[i][1] +
|
||||
vcm[ibody][0];
|
||||
|
|
@ -1571,7 +1555,7 @@ void FixRigid::set_xv()
|
|||
|
||||
if (eflags[i] & ORIENT_DIPOLE) {
|
||||
MathExtra::quat_to_mat(quat[ibody],p);
|
||||
MathExtra::times_column3(p,dorient[i],mu[i]);
|
||||
MathExtra::matvec(p,dorient[i],mu[i]);
|
||||
MathExtra::snormalize3(mu[i][3],mu[i],mu[i]);
|
||||
}
|
||||
if (eflags[i] & ORIENT_QUAT) {
|
||||
|
|
@ -1611,7 +1595,7 @@ void FixRigid::set_v()
|
|||
double dx,dy,dz;
|
||||
double x0,x1,x2,v0,v1,v2,fc0,fc1,fc2,massone;
|
||||
double xy,xz,yz;
|
||||
double ione[3],exone[3],eyone[3],ezone[3],vr[6];
|
||||
double ione[3],exone[3],eyone[3],ezone[3],delta[3],vr[6];
|
||||
|
||||
double **x = atom->x;
|
||||
double **v = atom->v;
|
||||
|
|
@ -1637,15 +1621,8 @@ void FixRigid::set_v()
|
|||
if (body[i] < 0) continue;
|
||||
ibody = body[i];
|
||||
|
||||
dx = ex_space[ibody][0]*displace[i][0] +
|
||||
ey_space[ibody][0]*displace[i][1] +
|
||||
ez_space[ibody][0]*displace[i][2];
|
||||
dy = ex_space[ibody][1]*displace[i][0] +
|
||||
ey_space[ibody][1]*displace[i][1] +
|
||||
ez_space[ibody][1]*displace[i][2];
|
||||
dz = ex_space[ibody][2]*displace[i][0] +
|
||||
ey_space[ibody][2]*displace[i][1] +
|
||||
ez_space[ibody][2]*displace[i][2];
|
||||
MathExtra::matvec(ex_space[ibody],ey_space[ibody],
|
||||
ez_space[ibody],displace[i],delta);
|
||||
|
||||
// save old velocities for virial
|
||||
|
||||
|
|
@ -1655,9 +1632,12 @@ void FixRigid::set_v()
|
|||
v2 = v[i][2];
|
||||
}
|
||||
|
||||
v[i][0] = omega[ibody][1]*dz - omega[ibody][2]*dy + vcm[ibody][0];
|
||||
v[i][1] = omega[ibody][2]*dx - omega[ibody][0]*dz + vcm[ibody][1];
|
||||
v[i][2] = omega[ibody][0]*dy - omega[ibody][1]*dx + vcm[ibody][2];
|
||||
v[i][0] = omega[ibody][1]*delta[2] - omega[ibody][2]*delta[1] +
|
||||
vcm[ibody][0];
|
||||
v[i][1] = omega[ibody][2]*delta[0] - omega[ibody][0]*delta[2] +
|
||||
vcm[ibody][1];
|
||||
v[i][2] = omega[ibody][0]*delta[1] - omega[ibody][1]*delta[0] +
|
||||
vcm[ibody][2];
|
||||
|
||||
// virial = unwrapped coords dotted into body constraint force
|
||||
// body constraint force = implied force due to v change minus f external
|
||||
|
|
|
|||
|
|
@ -226,68 +226,6 @@ void richardson(double *q, double *m, double *w, double *moments, double dtq)
|
|||
MathExtra::qnormalize(q);
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
compute rotation matrix from quaternion
|
||||
quat = [w i j k]
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void 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 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
|
||||
|
|
@ -330,14 +268,14 @@ void mq_to_omega(double *m, double *q, double *moments, double *w)
|
|||
double rot[3][3];
|
||||
|
||||
MathExtra::quat_to_mat(q,rot);
|
||||
MathExtra::transpose_times_column3(rot,m,wbody);
|
||||
MathExtra::transpose_matvec(rot,m,wbody);
|
||||
if (moments[0] == 0.0) wbody[0] = 0.0;
|
||||
else wbody[0] /= moments[0];
|
||||
if (moments[1] == 0.0) wbody[1] = 0.0;
|
||||
else wbody[1] /= moments[1];
|
||||
if (moments[2] == 0.0) wbody[2] = 0.0;
|
||||
else wbody[2] /= moments[2];
|
||||
MathExtra::times_column3(rot,wbody,w);
|
||||
MathExtra::matvec(rot,wbody,w);
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -426,6 +364,68 @@ void q_to_exyz(double *q, double *ex, double *ey, double *ez)
|
|||
ez[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
compute rotation matrix from quaternion
|
||||
quat = [w i j k]
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void 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 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 space-frame inertia tensor of an ellipsoid
|
||||
quat = orientiation quaternion of ellipsoid
|
||||
|
|
|
|||
127
src/math_extra.h
127
src/math_extra.h
|
|
@ -53,14 +53,17 @@ namespace MathExtra {
|
|||
const double mat2[3][3],
|
||||
double ans[3][3]);
|
||||
inline void invert3(const double mat[3][3], double ans[3][3]);
|
||||
inline void times_column3(const double mat[3][3], const double*vec,
|
||||
double *ans);
|
||||
inline void transpose_times_column3(const double mat[3][3],const double*vec,
|
||||
double *ans);
|
||||
inline void transpose_times_diag3(const double mat[3][3],const double*vec,
|
||||
inline void matvec(const double mat[3][3], const double*vec, double *ans);
|
||||
inline void matvec(const double *ex, const double *ey, const double *ez,
|
||||
const double *vec, double *ans);
|
||||
inline void transpose_matvec(const double mat[3][3], const double*vec,
|
||||
double *ans);
|
||||
inline void transpose_matvec(const double *ex, const double *ey,
|
||||
const double *ez, const double *v,
|
||||
double *ans);
|
||||
inline void transpose_times_diag3(const double mat[3][3], const double*vec,
|
||||
double ans[3][3]);
|
||||
inline void row_times3(const double *v, const double m[3][3],
|
||||
double *ans);
|
||||
inline void vecmat(const double *v, const double m[3][3], double *ans);
|
||||
inline void scalar_times3(const double f, double m[3][3]);
|
||||
|
||||
void write3(const double mat[3][3]);
|
||||
|
|
@ -100,8 +103,6 @@ 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
|
||||
|
||||
|
|
@ -115,7 +116,6 @@ namespace MathExtra {
|
|||
double *inertia);
|
||||
void inertia_triangle(double *v0, double *v1, double *v2,
|
||||
double mass, double *inertia);
|
||||
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -269,15 +269,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];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -287,15 +287,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];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -305,15 +305,15 @@ 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];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -342,24 +342,48 @@ void MathExtra::invert3(const double m[3][3], double ans[3][3])
|
|||
matrix times vector
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::times_column3(const double m[3][3], const double *v,
|
||||
double *ans)
|
||||
void MathExtra::matvec(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];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
matrix times vector
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::matvec(const double *ex, const double *ey, const double *ez,
|
||||
const double *v, double *ans)
|
||||
{
|
||||
ans[0] = ex[0]*v[0] + ey[0]*v[1] + ez[0]*v[2];
|
||||
ans[1] = ex[1]*v[0] + ey[1]*v[1] + ez[1]*v[2];
|
||||
ans[2] = ex[2]*v[0] + ey[2]*v[1] + ez[2]*v[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
transposed matrix times vector
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::transpose_times_column3(const double m[3][3], const double *v,
|
||||
double *ans)
|
||||
void MathExtra::transpose_matvec(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];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
transposed matrix times vector
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::transpose_matvec(const double *ex, const double *ey,
|
||||
const double *ez, const double *v,
|
||||
double *ans)
|
||||
{
|
||||
ans[0] = ex[0]*v[0] + ex[1]*v[1] + ex[2]*v[2];
|
||||
ans[1] = ey[0]*v[0] + ey[1]*v[1] + ey[2]*v[2];
|
||||
ans[2] = ez[0]*v[0] + ez[1]*v[1] + ez[2]*v[2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
@ -384,12 +408,11 @@ void MathExtra::transpose_times_diag3(const double m[3][3],
|
|||
row vector times matrix
|
||||
------------------------------------------------------------------------- */
|
||||
|
||||
void MathExtra::row_times3(const double *v, const double m[3][3],
|
||||
double *ans)
|
||||
void MathExtra::vecmat(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] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];
|
||||
ans[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];
|
||||
ans[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------------------------
|
||||
|
|
|
|||
Loading…
Reference in New Issue