/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#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 (6D) 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>
	 *
	 *  For a complete description of Points/Poses, see MRML::CPoseOrPoint, or refer
	 *    to the <a href="http://babel.isa.uma.es/mrpt/index.php/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki.
	 *

<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(const double& x=0,const double& y=0,const double& z=0,const double& yaw=0, const double& pitch=0, const double& roll=0);

		 /** Copy constructor.
		  */
		 CPose3D( const CPose3D &o);

		 /** Copy operator.
		  */
		 CPose3D & operator=( const CPose3D &o);

		 /** Constructor from a 4x4 homogeneous matrix:
		  */
		 CPose3D(const UTILS::CMatrixDouble &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
		   */
		 const UTILS::CMatrixDouble& getHomogeneousMatrix() const;

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


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

		 /** Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be called after using addComponents)
		  * \sa addComponents
		   */
		 void  normalizeAngles();

		 /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,pi] interval).
		   */
		 void operator *=(const double & s);

        /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
          *  For the coordinate system see the top of this page.
          */
        void sphericalCoordinates(
            const CPoint3D &point,
            double &out_range,
            double &out_yaw,
            double &out_pitch ) const;

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

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

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


		/** The euclidean distance between two poses taken as two 6-length vectors (angles in radians).
		  */
		double distanceEuclidean6D( const CPose3D &o ) const;

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


	}; // End of class def.

	DEFINE_SERIALIZABLE_POST( CPose3D )

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


} // End of namespace

#endif
