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