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;
}
moDMatrix<Real> moDMatrix<Real>::operator- (const moDMatrix& rkM) const
{
moDMatrix<Real> kDiff(rkM.m_iRows,rkM.m_iCols);
for (int i = 0; i < m_iQuantity; i++)
{
kDiff.m_afData[i] = m_afData[i] - rkM.m_afData[i];
}
return kDiff;
}
