diff --git a/src/ASPHERE/compute_erotate_asphere.cpp b/src/ASPHERE/compute_erotate_asphere.cpp index 0b63ba0ca7..31d500c4a3 100644 --- a/src/ASPHERE/compute_erotate_asphere.cpp +++ b/src/ASPHERE/compute_erotate_asphere.cpp @@ -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]; diff --git a/src/ASPHERE/compute_temp_asphere.cpp b/src/ASPHERE/compute_temp_asphere.cpp index 9de378f2ec..e4e1177a7e 100755 --- a/src/ASPHERE/compute_temp_asphere.cpp +++ b/src/ASPHERE/compute_temp_asphere.cpp @@ -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]; diff --git a/src/ASPHERE/pair_gayberne.cpp b/src/ASPHERE/pair_gayberne.cpp index 4ea696e901..ad4c334c49 100755 --- a/src/ASPHERE/pair_gayberne.cpp +++ b/src/ASPHERE/pair_gayberne.cpp @@ -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; diff --git a/src/ASPHERE/pair_resquared.cpp b/src/ASPHERE/pair_resquared.cpp index 7980fd7bda..8241e1a6c7 100755 --- a/src/ASPHERE/pair_resquared.cpp +++ b/src/ASPHERE/pair_resquared.cpp @@ -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; diff --git a/src/fix_rigid.cpp b/src/fix_rigid.cpp index 9a3427e6f8..1099f09d7a 100644 --- a/src/fix_rigid.cpp +++ b/src/fix_rigid.cpp @@ -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 diff --git a/src/math_extra.cpp b/src/math_extra.cpp index a56b526a0e..535a1e576b 100644 --- a/src/math_extra.cpp +++ b/src/math_extra.cpp @@ -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 diff --git a/src/math_extra.h b/src/math_extra.h index fa6541258f..c9a117fe03 100755 --- a/src/math_extra.h +++ b/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]; } /* ----------------------------------------------------------------------