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