void ComputeAngVel (int n, VecR *w) { Quat qt, qvt; qvt = mol[n].qv; qvt.u4 *= -1.; QMul (qt, qvt, mol[n].q); QScale (qt, 2.); VSet (*w, qt.u1, qt.u2, qt.u3); }
void InitAngVels () { Quat qe; VecR e; real f; int n; DO_MOL { VRand (&e); QSet (qe, e.x, e.y, e.z, 0.); QMul (mol[n].qv, mol[n].q, qe); f = 0.5 * velMag / sqrt (VWLenSq (mInert, e)); QScale (mol[n].qv, f); } }
void ComputeAccelsQ () { Quat qs; VecR w; int n; DO_MOL { ComputeAngVel (n, &w); QSet (qs, (mol[n].torq.x + (mInert.y - mInert.z) * w.y * w.z) / mInert.x, (mol[n].torq.y + (mInert.z - mInert.x) * w.z * w.x) / mInert.y, (mol[n].torq.z + (mInert.x - mInert.y) * w.x * w.y) / mInert.z, - 2. * QLenSq (mol[n].qv)); QMul (mol[n].qa, mol[n].q, qs); QScale (mol[n].qa, 0.5); } }
void QRDecomposition<T>::leastSquares(const VectorT& b, VectorT& x, VectorT& residual) const { if(x.n == 0) x.resize(QR.n); Assert(QR.m >= QR.n); Assert(QR.m == b.n); Assert(QR.n == x.n); Assert(QR.m == residual.n); MatrixT R; R.setRef(QR,0,0,1,1,QR.n,QR.n); VectorT c; c.setRef(residual,0,1,QR.n); /* compute rhs = Q^T b */ QtMul(b,residual); /* Solve R x = rhs */ UBackSubstitute(R,c,x); /* Compute residual = b - A x = Q (Q^T b - R x) */ c.setZero(); QMul(residual,residual); }