/*---------------------------------------------------------------
	FILE: CPose3D.h
	USE: Declares a class for storing a pose: location+heading

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CPOSE3D_H
#define CPOSE3D_H

#include <MRPT/MRML/CPose.h>

namespace MRML
{
	/** A class used to store a 3D pose.
	 *    A class used to store a 3D pose, including the 3D coordinate
	 *      point and orientation angles. It is used in many situations,
	 *      from defining a robot pose, maps relative poses, sensors,...
	 *		See introduction in documentation for the CPoseOrPoint class.
			<br>For a complete description of Points/Poses, see MRML::CPoseOrPoint<br>

<div align=center>

<table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0
 style='border-collapse:collapse;border:none'>
 <tr style='height:15.8pt'>
  <td width=676 colspan=2 style='width:507.25pt;border:solid windowtext 1.0pt;
  background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'>
  <p   align=center style='text-align:center'>MRML::CPose3D</p>
  </td>
 </tr>
 <tr style='height:15.8pt'>
  <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top:
  none;padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'>
  <p   align=center style='text-align:center'>Homogeneous
  transfomation matrix</p>
  </td>
  <td width=326 style='width:244.6pt;border-top:none;border-left:none;
  border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt;
  padding:0cm 5.4pt 0cm 5.4pt;height:15.8pt'>
  <p   align=center style='text-align:center'>Spatial
  representation</p>
  </td>
 </tr>
 <tr style='height:202.65pt'>
  <td width=350 style='width:262.65pt;border:solid windowtext 1.0pt;border-top:
  none;padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'>
  <div align=center>
  <table  Table border=0 cellspacing=0 cellpadding=0 width=334
   style='width:250.65pt;border-collapse:collapse'>
   <tr style='height:16.65pt'>
    <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'>
    <p   align=center style='text-align:center'>cycp</p>
    </td>
    <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'>
    <p   align=center style='text-align:center'>cyspsr-sycr</p>
    </td>
    <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'>
    <p   align=center style='text-align:center'>cyspcr+sysr</p>
    </td>
    <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:16.65pt'>
    <p   align=center style='text-align:center'>x</p>
    </td>
   </tr>
   <tr style='height:17.25pt'>
    <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'>
    <p   align=center style='text-align:center'>sycp</p>
    </td>
    <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'>
    <p   align=center style='text-align:center'>syspsr+cycr</p>
    </td>
    <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'>
    <p   align=center style='text-align:center'>syspcr-cysr</p>
    </td>
    <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:17.25pt'>
    <p   align=center style='text-align:center'>y</p>
    </td>
   </tr>
   <tr style='height:19.65pt'>
    <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'>
    <p   align=center style='text-align:center'>-sp</p>
    </td>
    <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'>
    <p   align=center style='text-align:center'>cpsr</p>
    </td>
    <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'>
    <p   align=center style='text-align:center'>cpcr</p>
    </td>
    <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:19.65pt'>
    <p   align=center style='text-align:center'>z</p>
    </td>
   </tr>
   <tr style='height:11.0pt'>
    <td width=66 style='width:49.65pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=99 style='width:74.15pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=87 style='width:65.55pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=82 style='width:61.3pt;padding:0cm 5.4pt 0cm 5.4pt;height:11.0pt'>
    <p   align=center style='text-align:center'>1</p>
    </td>
   </tr>
  </table>
  </div>
  <p   align=center style='text-align:center'><span lang=EN-GB>where:</span></p>
  <p   align=center style='text-align:center'><span lang=EN-GB>cy
  = cos Yaw ;  sy = sin Yaw</span></p>
  <p   align=center style='text-align:center'><span lang=EN-GB>cp
  = cos Pitch ; sp = sin Pitch</span></p>
  <p   align=center style='text-align:center'><span lang=EN-GB>cr
  = cos Roll ; sr = sin Roll</span></p>
  </td>
  <td width=326 style='width:244.6pt;border-top:none;border-left:none;
  border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt;
  padding:0cm 5.4pt 0cm 5.4pt;height:202.65pt'>
  <p   align=center style='text-align:center'><span lang=EN-GB><img  src="CPose3D.gif"></span></p>
  </td>
 </tr>
</table>

</div>

	  *
	 * \sa CPoseOrPoint,CPoint3D
	 */
	class CPose3D : public CPose
	{
		friend class CPose;
		friend class CPose2D;
		friend class CPoint;
		friend std::ostream& operator << (std::ostream& o, const CPose3D& p);

		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CPose3D )


	 public:
		 /** Constructor with initilization of the pose; (remember that angles are always given in radians!)
		  */
		 CPose3D(float x=0,float y=0,float z=0,float yaw=0, float pitch=0, float roll=0);

		 /** Constructor from a 4x4 homogeneous matrix:
		  */
		 CPose3D(CMatrix m);

		 /** Constructor from a CPose2D object.
		  */
		 CPose3D(const CPose2D &);

		 /** Constructor from a CPoint3D object.
		  */
		 CPose3D(const CPoint3D &);

		 /** Returns the corresponding 4x4 homogeneous
		   *  transformation matrix for the point(translation),
		   *  or pose (translation+orientation).
		   * \sa getInverseHomogeneousMatrix
		   */
		 UTILS::CMatrix  getHomogeneousMatrix() const;

		 /** Returns the corresponding 4x4 homogeneous
		   *  transformation matrix for the point(translation),
		   *  or pose (translation+orientation).
		   * \sa getInverseHomogeneousMatrix
		   */
		 void  getHomogeneousMatrix(const UTILS::CMatrix *& ptrHM ) const;


		 /** Scalar sum of components: This is diferent from poses
		  *    composition, which is implemented as "+" operators.
		  */
		 void AddComponents(CPose3D &p);

		 /** Scalar multiplication.
		   */
		 void operator *=(float s);


		/** Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians).
		  * \sa getYawPitchRoll
		  */
		void  setFromValues(float x0,float y0, float z0, float yaw=0, float pitch=0, float roll=0);

		/** Returns the three angles (yaw, pitch, roll), in radians, from the homogeneous matrix.
		  * \sa setFromValues, yaw, pitch, roll
		  */
		void  getYawPitchRoll( float &yaw, float &pitch, float &roll );

		/** These READ-ONLY variables are updated every time that the object homogeneous matrix is modified (construction, loading from values, pose composition, etc )
		  */
		float	yaw, pitch, roll;


	 private:
		/** The complete homogeneous matrix must be stored for a generalized 3D pose.
		  */
		UTILS::CMatrix	HM;


	}; // End of class def.

	std::ostream& operator << (std::ostream& o, const CPose3D& p);


} // End of namespace

#endif
