/***************************************************************************************
*****                        CLASS FRAME IMPLEMENTATION                            *****
****************************************************************************************
** Author: Peter Stuart
** Date:   3/7/98
** Description: This class represents a coordinate frame. The frame is made up of three
**   orthogonal vectors u, v, w, and origin point o. Methods include finding the 
**   matrix to trasform coordinates from frame space to cartesian space, changing
**   orientation and position in both frame and cartesian coordinates, and the 
**   standard relational, assignment, and output stream methods.
**
** Note: All of these graphics classes use a right-handed coordinate system.
**
***************************************************************************************/

#include "Frame.hpp"

/* Constructor ************
** Default values for private variables:
** _u: <1.0, 0.0, 0.0>
** _v: <0.0, 1.0, 0.0>
** _w: <0.0, 0.0, 1.0>
** _o: <0.0, 0.0, 0.0>
*/

Frame::Frame (const Vector& u, const Vector& v, const Vector& w, const Point& o)
{
   _u = u;
   _v = v;
   _w = w;
   _o = o;
}

// Copy Constructor
Frame::Frame (const Frame& fr)
{
   _u = fr._u;
   _v = fr._v;
   _w = fr._w;
   _o = fr._o;
}
	
// Destructor
Frame::~Frame (void)
{
   ;
}

// BASIC OPERATIONS ///////////////

// Transform Frame using matrix
// NOTE: Since the frame's origin is a separate variable, it is necessary to 
//   break the matrix into its rotation and translation parts. The rotation
//   part is then used on the vectors of the frame, and the translation
//   part is used on the frame's position. It was done this way so that
//   the vectors of the frame would stay unit vectors.

void Frame::transformFrame (const Matrix4D& mat)
{
   Matrix4D rot, move;
   Point opt;

   rot = mat;
   rot.setElement(3, 0, 0.0);
   rot.setElement(3, 1, 0.0);
   rot.setElement(3, 2, 0.0);

   move = move.identity();
   move.setElement(3, 0, mat.getElement(3, 0));
   move.setElement(3, 1, mat.getElement(3, 1));
   move.setElement(3, 2, mat.getElement(3, 2));

   _o = _o*move;
   _u = (_u*rot).direction();
   _v = (_v*rot).direction();
   _w = (_w*rot).direction();
}

// Translate frame with respect to its own coordinate system
void     Frame::translateFrame (const double& x, const double& y, const double& z)
{
   Matrix4D move, trans;
   trans.setElement(0, 0, _u.x());
   trans.setElement(0, 1, _v.x());
   trans.setElement(0, 2, _w.x());
   trans.setElement(1, 0, _u.y());
   trans.setElement(1, 1, _v.y());
   trans.setElement(1, 2, _w.y());
   trans.setElement(2, 0, _u.z());
   trans.setElement(2, 1, _v.z());
   trans.setElement(2, 2, _w.z());
   trans.setElement(3, 3, 1.0);

   move = translate(x, y, z);
   move = trans*move*trans.inverse();
   _o = _o*move;
}

// rotate frame with respect to its own coordinate system
void Frame::rotateFrame (const double& x, const double& y, const double& z)
{
   Matrix4D trans, rot;
   trans.setElement(0, 0, _u.x());
   trans.setElement(0, 1, _v.x());
   trans.setElement(0, 2, _w.x());
   trans.setElement(1, 0, _u.y());
   trans.setElement(1, 1, _v.y());
   trans.setElement(1, 2, _w.y());
   trans.setElement(2, 0, _u.z());
   trans.setElement(2, 1, _v.z());
   trans.setElement(2, 2, _w.z());
   trans.setElement(3, 3, 1.0);

   rot = rotate(x, y, z);
   rot = trans*rot*trans.inverse();

   _u = (_u*rot).direction();
   _v = (_v*rot).direction();
   _w = (_w*rot).direction();
}

// Create frame to cartesian frame coordinate transformation matrix
Matrix4D Frame::frameTransform(void)
{
   Matrix4D temp;  // returning matrix

   // set matrix to transform coordinates to the cartesian frame
   temp.setElement(0, 0, _u.x());
   temp.setElement(0, 1, _v.x());
   temp.setElement(0, 2, _w.x());
   temp.setElement(1, 0, _u.y());
   temp.setElement(1, 1, _v.y());
   temp.setElement(1, 2, _w.y());
   temp.setElement(2, 0, _u.z());
   temp.setElement(2, 1, _v.z());
   temp.setElement(2, 2, _w.z());
   temp.setElement(3, 0, _o.x());
   temp.setElement(3, 1, _o.y());
   temp.setElement(3, 2, _o.z());
   temp.setElement(3, 3, 1.0);

   return temp;
}

// Create cartesian frame to frame coordinate transformation matrix
Matrix4D Frame::cartTransform(void)
{
   Matrix4D temp;   // returning matrix
   Vector   j;      // vector from origin to frame position

   j = zeroPoint - _o;

   temp.setElement(0, 0, _u.x());
   temp.setElement(0, 1, _v.x());
   temp.setElement(0, 2, _w.x());
   temp.setElement(1, 0, _u.y());
   temp.setElement(1, 1, _v.y());
   temp.setElement(1, 2, _w.y());
   temp.setElement(2, 0, _u.z());
   temp.setElement(2, 1, _v.z());
   temp.setElement(2, 2, _w.z());
   temp.setElement(3, 0, _u.dot(j));
   temp.setElement(3, 1, _v.dot(j));
   temp.setElement(3, 2, _w.dot(j));
   temp.setElement(3, 3, 1.0);

   return temp;
}

// ELEMENTARY OPEARTORS //////////////////////////

// Assignment operator
Frame&   Frame::operator =  (const Frame& fr)
{
   if (this == &fr)
      return (*this);

   _u = fr._u;
   _v = fr._v;
   _w = fr._w;
   _o = fr._o;

   return (*this);
}

// Relational operators
int operator == (const Frame& fr1, const Frame& fr2)
{
   if (fr1._u != fr2._u)
      return FALSE;
   if (fr1._v != fr2._v)
      return FALSE;
   if (fr1._w != fr2._w)
      return FALSE;
   if (fr1._o != fr2._o)
      return FALSE;

   return TRUE;
}

int operator != (const Frame& fr1, const Frame& fr2)
{
   if (fr1._u != fr2._u)
      return TRUE;
   if (fr1._v != fr2._v)
      return TRUE;
   if (fr1._w != fr2._w)
      return TRUE;
   if (fr1._o != fr2._o)
      return TRUE;

   return FALSE;
}

// output stream
ostream& operator << (ostream& co, const Frame& fr)
{
   co << "\nu: " << fr._u
      << "\nv: " << fr._v
      << "\nw: " << fr._w
      << "\no: " << fr._o;

   return co;
}