/* see ref [] */ ReturnMatrix rotk(Real theta, const ColumnVector & k) { Matrix rot(4,4); Real c, s, vers, kx, ky, kz; rot << fourbyfourident; /* identity matrix */ vers = SumSquare(k.SubMatrix(1,3,1,1)); if (vers != 0.0) { /* compute the rotation if the vector norm is not 0.0 */ vers = sqrt(1/vers); kx = k(1)*vers; ky = k(2)*vers; kz = k(3)*vers; s = sin(theta); c = cos(theta); vers = 1-c; rot(1,1) = kx*kx*vers+c; rot(1,2) = kx*ky*vers-kz*s; rot(1,3) = kx*kz*vers+ky*s; rot(2,1) = kx*ky*vers+kz*s; rot(2,2) = ky*ky*vers+c; rot(2,3) = ky*kz*vers-kx*s; rot(3,1) = kx*kz*vers-ky*s; rot(3,2) = ky*kz*vers+kx*s; rot(3,3) = kz*kz*vers+c; } rot.Release(); return rot; }
bool MLE_D_FI::NextPoint(ColumnVector& Adj, Real& test) { Tracer tr("MLE_D_FI::NextPoint"); SymmetricMatrix FI = LL.FI(); LT = Cholesky(FI); ColumnVector Adj1 = LT.i() * Derivs; Adj = LT.t().i() * Adj1; test = SumSquare(Adj1); cout << " " << setw(20) << setprecision(10) << test; return (test < Criterion); }