void moQuaternion<Real>::ToRotationMatrix (moVector3<Real> akRotColumn[3]) const
{
moMatrix3<Real> kRot;
ToRotationMatrix(kRot);
for (int iCol = 0; iCol < 3; iCol++)
{
akRotColumn[iCol][0] = kRot(0,iCol);
akRotColumn[iCol][1] = kRot(1,iCol);
akRotColumn[iCol][2] = kRot(2,iCol);
}
}
moQuaternion<Real>& moQuaternion<Real>::FromRotationMatrix (
const moVector3<Real> akRotColumn[3])
{
moMatrix3<Real> kRot;
for (int iCol = 0; iCol < 3; iCol++)
{
kRot(0,iCol) = akRotColumn[iCol][0];
kRot(1,iCol) = akRotColumn[iCol][1];
kRot(2,iCol) = akRotColumn[iCol][2];
}
return FromRotationMatrix(kRot);
}
Real EllipseFit2<Real>::Energy (const Real* afV, void* pvData)
{
EllipseFit2& rkSelf = *(EllipseFit2*)pvData;
// build rotation matrix
Matrix2<Real> kRot(afV[4]);
Ellipse2<Real> kEllipse(Vector2<Real>::ZERO,Vector2<Real>::UNIT_X,
Vector2<Real>::UNIT_Y,afV[0],afV[1]);
// transform the points to the coordinate system of U and R
Real fEnergy = (Real)0.0;
for (int i = 0; i < rkSelf.m_iQuantity; i++)
{
Vector2<Real> kDiff(
rkSelf.m_akPoint[i].X() - afV[2],
rkSelf.m_akPoint[i].Y() - afV[3]);
rkSelf.m_akTemp[i] = kDiff*kRot;
Real fDist =
DistVector2Ellipse2<Real>(rkSelf.m_akTemp[i],kEllipse).Get();
fEnergy += fDist;
}
return fEnergy;
}
