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