/*---------------------------------------------------------------
The operator D="this"-b is the pose inverse compounding operator.
The resulting pose "D" is the diference between this pose and "b"
---------------------------------------------------------------*/
void CPose2D::inverseComposeFrom(const CPose2D& A, const CPose2D& B )
{
B.update_cached_cos_sin();
m_coords[0] = (A.m_coords[0] - B.m_coords[0]) * B.m_cosphi + (A.m_coords[1] - B.m_coords[1]) * B.m_sinphi;
m_coords[1] =-(A.m_coords[0] - B.m_coords[0]) * B.m_sinphi + (A.m_coords[1] - B.m_coords[1]) * B.m_cosphi;
m_phi = math::wrapToPi(A.m_phi - B.m_phi);
m_cossin_uptodate=false;
}
/*---------------------------------------------------------------
composeFrom
---------------------------------------------------------------*/
void CPose2D::composeFrom(const CPose2D &A, const CPose2D &B)
{
A.update_cached_cos_sin();
// Use temporary variables for the cases (A==this) or (B==this)
const double new_x = A.m_coords[0] + B.m_coords[0] * A.m_cosphi - B.m_coords[1] * A.m_sinphi;
const double new_y = A.m_coords[1] + B.m_coords[0] * A.m_sinphi + B.m_coords[1] * A.m_cosphi;
m_coords[0] = new_x;
m_coords[1] = new_y;
m_phi = math::wrapToPi(A.m_phi + B.m_phi);
m_cossin_uptodate=false;
}