CPose3D CPose2D::operator -(const CPose3D& b) const
{
CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
b.getInverseHomogeneousMatrix( B_INV );
CMatrixDouble44 HM(UNINITIALIZED_MATRIX);
this->getHomogeneousMatrix(HM);
CMatrixDouble44 RES(UNINITIALIZED_MATRIX);
RES.multiply(B_INV,HM);
return CPose3D( RES );
}
/*---------------------------------------------------------------
point3D = point3D - pose3D
---------------------------------------------------------------*/
CPoint3D CPoint3D::operator - (const CPose3D& b) const
{
// JLBC: 7-FEB-2008: Why computing the whole matrix multiplication?? ;-)
// 5.7us -> 4.1us -> 3.1us (with optimization of HM matrices by reference)
// JLBC: 10-APR-2009: Usage of fixed-size 4x4 matrix, should be even faster now.
CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
b.getInverseHomogeneousMatrix( B_INV );
return CPoint3D(
B_INV.get_unsafe(0,0) * m_coords[0] + B_INV.get_unsafe(0,1) * m_coords[1] + B_INV.get_unsafe(0,2) * m_coords[2] + B_INV.get_unsafe(0,3),
B_INV.get_unsafe(1,0) * m_coords[0] + B_INV.get_unsafe(1,1) * m_coords[1] + B_INV.get_unsafe(1,2) * m_coords[2] + B_INV.get_unsafe(1,3),
B_INV.get_unsafe(2,0) * m_coords[0] + B_INV.get_unsafe(2,1) * m_coords[1] + B_INV.get_unsafe(2,2) * m_coords[2] + B_INV.get_unsafe(2,3) );
}
