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);
}
