Esempi in C++ (Cpp) per CPose2D::update_cached_cos_sin

Esempio n. 1

File: CPose2D.cpp Progetto: Insane-neal/mrpt

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

Esempio n. 2

File: CPose2D.cpp Progetto: Insane-neal/mrpt

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