//! file="amma/Surf3d/RigidT/Homtm.hh"
//! lib=RigidT
//! userlevel=Default
//! author="Andrew Stoddart"
//! date="30/07/98"
//! rcsid="$Id: Homtm.hh,v 1.11 1999/10/05 16:42:01 ees1dv Exp $"
//! docentry="default.Andrew Stoddart"
// -----------------------------------------------------------------------------
// *********************************** HomtmC **************** ***********
// -----------------------------------------------------------------------------
//
// File Name   : Homtm.hh
// Author      : Paris Lyritis
// Last Change : 04/11/1997
// Synopsis    : The HomtmC represents the 4x4 Homogeneous Transformation Matrix
// -----------------------------------------------------------------------------
//
// Modifications :
//         ajs 4/5/98
//        (1) Use of scale and perspective in this class is deprecated
//            It is seen as a RigidBodyTransform class.
//            This means that Perspective related classes may be removed.
//        (2) All angles will be converted to radians except for i/o
//            to be consistent Surf3d practice
//
// Class Description
// .LP
// Suppose that we have a fixed reference frame (OXYZ) which contains a 
// moving (rotating or translating) frame (OUVW). The later frame may move with
// respect to itself or to the OXYZ.Let me clarify that we are talking about right-
// handed frames.
// .LP
// Homogeneous Transformation Matrices (Htm) help us map coordinates from
// one frame to another. They are particularly helpful when we know the
// position of a point in a local frame, which (frame) moves with respect
// to some fixed world frame. Usually OUVW is attached on a rigid body which
// is expressed with local coordinates. The vector/point (pu,pv,pw)of the rigid body
// can be transformed to (px,py,pz), by multiplying the appropriate Htm with it.
// .LP
// Usually such a vector is represented with a Vector3dC, but the provision for a
// Vector4dC representation has also been made. We can express a 3D vector (x,y,z) 
// in homogeneous coordinates, just by adding a scale value s, ie. (x/s,y/s,z/s,s).
// When a Vector3dC is used we add to it a scale equal to 1.
// .LP
// So, we use the HomtmC to describe a (rotation/translation/perspective/scale). We can
// store that information by storing the HomtmC itself. Extracting the translation from
// such a matrix is easy. But how can we extract the angles from an Htm ?
// In this case we use the QuarternC to extract the rotation angles of our HomtmC.
// We wanted to avoid keeping a dual representation, i.e. three additional RealT 
// variables in the HomtmC to denote the angles (which angles and in what sequence?)
// Instead, every time we want to find the rotation that an HomtmC represents, we set a
// QuarternC with the rotation submatrix, and we export from it the vector and the angle
// of rotation about it.
// .LP
// Basically the HomtmC class is an "enchanced" Matrix4d4C class. Most os its overloaded
// operators are just executing the according Matrix4d4C operators.

// -----------------------------------------------------------------------------
#ifndef HOMTRANSMATC_HH
#define HOMTRANSMATC_HH

// -----------------------------------------------------------------------------

#include "amma/Matrix4d.hh"
#include "amma/Vector3d.hh"
#include "amma/Vector4d.hh"
#include "amma/Quartern.hh"


// -----------------------------------------------------------------------------

enum HtmOperT  { RotNone, RotX, RotY, RotZ };


// -----------------------------------------------------------------------------

class HomtmC
{

public:

  Matrix4d4C T;
  // The class data is public to allow full access to its member functions.
  // We suppose that Matrix4d4C is sufficiently "protected" by itself and
  // that the programmer is "carefull?".
      
  // Constructors
  // ------------

  HomtmC();
  // Null constructor, sets to identity transform

  HomtmC(RealT, RealT, RealT, RealT,
       RealT, RealT, RealT, RealT,
       RealT, RealT, RealT, RealT,
       RealT, RealT, RealT, RealT);
  // Constructor with direct value initialisation

  HomtmC(JointT t, const Vector3dC& angles, BooleanT premult=TRUE);
  // constructs a Homtm which is a pure rotation using the 1-3 angles
  // stored in the Vector. If premult is TRUE the rotations
  // are applied in the order 3 2 1 otherwise 1 2 3 

  HomtmC(HtmOperT t, double a);
  // specific rotations, eg RX, theta in radians

  HomtmC(HtmOperT t0, HtmOperT t1, HtmOperT t2, 
         const Vector3dC& angles, BooleanT premult=TRUE);
  // specific rotations, eg RX, theta in radians

  HomtmC(Vector3dC axis, double theta);
  // Axis and angle, axis will be normalised!!

  HomtmC(double roll, double pitch, double yaw);
  // roll, pitch, yaw

  HomtmC(const Vector3dC & trans);
  // Constructor from a translation

  HomtmC(const Matrix4d4C &);
  // Constructor from a Matrix4d4C

  HomtmC(const Matrix3d3C &);
  // Constructor from a Matrix3d3C

  HomtmC(const QuarternC &);
  // Constructor from a quarternion

  HomtmC(const HomtmC &);
  // Copy constructor

  ~HomtmC();
  // Destructor


  // Access/ Action Functions
  // ------------------------

  Vector3dC & Transform(Vector3dC & v);
  // transform the supplied vector, the vector is changed!

  void Dump() const;
  // Outputs the contents of the matrix to stdout in a comprehensive format

  void DumpLong() const;
  // Outputs the contents of the matrix to stdout in a comprehensive format

  HomtmC Inv();
  // Returns an HomtmC which is the inverse of this Htm.
  // It does not modify this instance. Additionally, this is not a typical 4x4
  // inverse function and therefore cannot be applied to derive the inverse
  // of any 4x4 matrix. It only works for Htms with their perspective 
  // sub-matrix equal to [0 0 0] and their scale equal to [1]. 
  // So, the result is hard-coded to return an Htm with such characteristics.


  Matrix3d3C RotMat() const;
  // Returns the rotation matrix which is described by the 3x3 upper left

  Vector3dC Pos() const;
  // Returns the position vector which is described by the 3x1 upper right
  // sub-matrix of this Htm.

  void SetPos(const Vector3dC &);  
  void SetPos(RealT, RealT, RealT);
  // Sets the position sub-matrix of the Htm.

  /*
  Vector3dC Perspective();
  // Returns the perspective vector which is the lower-left 1X3 submatric

  void SetPerspective(Vector3dC&);  
  void SetPerspective(RealT, RealT, RealT);
  // Sets the perspective sub-matrix of the Htm. Should be kept [0 0 0]
 
  RealT Scale();
  // Returns the scale the 3,3 element

  void SetScale(RealT);  
  // Sets the scale element of the Htm. (Normally should be 1)
  */

  QuarternC ExportQuartern() const;
  // Returns the quaternion which describes this Htm's rotation.

  Vector3dC ExportRotVector() const;
  // Exports the unit vector of the rotation axis.

  RealT ExportRotAngle() const;
  // Exports the angle of rotation (in radians) about the previous rotation axis.

  Vector3dC ExportRotation() const;
  // Exports the represented rotation in the form of a single vector r(rx,ry,rz).
  // Recall, that if |r| is the length of the vector (i.e. sqrt(rx^2+ry^2+rz^2)
  // then the rotation is theta=|r|, about a unit vector u=(rx,ry,rz)/|r|.

  void  ImportQuartern(const QuarternC &);
  // Sets the rotation sub-matrix from a given quarternion

  void ImportRotation(const Matrix3d3C & );
  // Import the rotation represented by the given 3x3 matrix

  void ImportRotation(Vector3dC & );
  // Import the rotation represented by the given vector, into the rot. sub-matrix.
  // Rotation expressed in radians

  //Vector3dC GetEuler(JointT t) const;
  //removed
  // calculates and gives the euler angles of the rotation matrix homtm
  // Warning - not available for all rotation types
  // Warning - not tested over a wide range of angles, eg near 0 and 180
  // It is the users responsibility to make a request compatible with the
  // actual contents of the Homtm, eg dont ask for RXRY if general rotation!
  // The exact scheme for Euler angle conventions will be documented later 
  // by Dorien van de Belt

  // Overloaded Operators
  // --------------------

  HomtmC & operator=(const HomtmC& );
  // Assignment 

  HomtmC & operator=(const Matrix4d4C& );
  // Assignment to a Matrix4d4C

  /*
  HomtmC operator +(const HomtmC & ); 
  // HomtmC + HomtmC = HomtmC (Breaks down to Matrix4d4C +)
  // deprecated - (3,3) component should be 1

  HomtmC operator -(const HomtmC & ); 
  // HomtmC - HomtmC = HomtmC (Breaks down to Matrix4d4C -)
  // deprecated
  */

  HomtmC operator *(const HomtmC&) const;  
  // HomtmC * HomtmC = HomtmC (Breaks down to Matrix4d4C *)

  // -------------  Shoemake's Euler Angle Conversion, ----------------------
  // -------------  Graphic Gems IV, pp. 222 - 229 --------------------------

  Vector3dC ExportEuler(JointT jtype=RXRYRZ, BooleanT premult=TRUE) const;
  // the angles correspond to rotations around static axes XYZ (in radians)

  static Vector3dC  RotToEuler(const Matrix3d3C & m, JointT jtype=RXRYRZ, BooleanT premult=TRUE);
  // the angles correspond to rotations around static axes XYZ (in radians)

  static Vector3dC  RotToEuler(const Matrix4d4C & m, JointT jtype=RXRYRZ, BooleanT premult=TRUE);  
  // the angles correspond to rotations around static axes XYZ (in radians)

  // -------------  End of Shoemake's Euler Angle Conversion ---------------
  
};

// Externally defined operators 
// ----------------------------

HomtmC operator *(const RealT&, const HomtmC&); 
// RealT * HomtmC = HomtmC

Vector3dC operator *(const HomtmC&, const Vector3dC&);  
// HomtmC * Vector3dC = Vector3dC
// Ok. This is not mathematically correct : you can't multiply a 4x4 matrix
// with a 3d vector. Here, we make the assumption that the 3x3 vector has a
// 4th scale component which is equal to 1.

Vector4dC operator *(const HomtmC&, const Vector4dC&);  
// Vector4dC <- HomtmC * Vector4dC

HomtmC operator +(const HomtmC &, const Vector3dC &);  
// HomtmC <- HomtmC + Vector3dC

HomtmC operator -(const HomtmC &, const Vector3dC &);  
// HomtmC <- HomtmC - Vector3dC

ostream & operator<<(ostream& , const HomtmC& );
// ouput stream operator

istream & operator>>(istream &, HomtmC&);
// input stream operator

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------

#endif

// -----------------------------------------------------------------------------