// Frame.h - a coordinate system.
//
//  Copyright (C) 2001--2004 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  This program 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 2 of the License, or
//  (at your option) any later version.
//
//  This program 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 this program; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#ifndef _FRAME_H_
#define _FRAME_H_

#include <vamos/geometry/Three_Vector.h>
#include <vamos/geometry/Three_Matrix.h>

namespace Vamos_Body
{
  //* A Frame describes a coordinate system.
  class Frame
  {
  protected:
    // The position of the origin of this frame relative to the parent frame.
    Vamos_Geometry::Three_Vector m_position;

    // The orientation of this frame relative to the parent frame.  The columns
    // of the matrix are the unit vectors of the x, y, and z axes.  This is
    // also the transformation matrix for rotating vectors into and out of
    // this frame.  See rotate_in () and rotate_out ().
    Vamos_Geometry::Three_Matrix m_orientation;

    // The position of the origin of this frame relative to the parent
    // frame.  This is the vector from the parent frame's origin to
    // this frame's origin, expressed in the coordinates of the parent
    // frame.
    Vamos_Geometry::Three_Vector m_velocity;

    // The angular velocity vector of this frame expressed in the
    // coordinates of the parent's frame.
    Vamos_Geometry::Three_Vector m_ang_velocity;

  public:
    //** Constructors

    // Specify the position and orientation.
    Frame (const Vamos_Geometry::Three_Vector& position, 
           const Vamos_Geometry::Three_Matrix& orientation);

    // Take the parent's orientation.
    Frame (const Vamos_Geometry::Three_Vector& position);

    // Make a frame that's coincident with the parent frame.
    Frame ();

    // VEC is a vector in the parent's frame.  The representation of
    // VEC in this frame is returned.
    Vamos_Geometry::Three_Vector 
    transform_in (const Vamos_Geometry::Three_Vector& vec) const;

    // VEC is a vector in this frame.  The representation of VEC in
    // the parent's frame is returned.
    Vamos_Geometry::Three_Vector 
    transform_out (const Vamos_Geometry::Three_Vector& vec) const;

    // Same as transform_out (VEC) above, except that the rotation is
    // about the point PIVOT expressed in this frame's coordinates.
    Vamos_Geometry::Three_Vector 
    transform_out (const Vamos_Geometry::Three_Vector& vec,
                   const Vamos_Geometry::Three_Vector& pivot) const;

    // Same as transform_in (VEC) above, except that translation is
    // not performed.
    Vamos_Geometry::Three_Vector 
    rotate_in (const Vamos_Geometry::Three_Vector& vec) const
    { return m_orientation.transpose () * vec; }

    // Same as transform_out (VEC) above, except that translation is
    // not performed.
    Vamos_Geometry::Three_Vector 
    rotate_out (const Vamos_Geometry::Three_Vector& vec) const
    { return m_orientation * vec; }

    // Change the position by DELTA_R.
    void translate (const Vamos_Geometry::Three_Vector& delta_r) 
    { m_position += delta_r; }

    // Rotate the frame about the vector delta_theta, by an angle equal to
    // the magnitude of DELTA_THETA.
    void rotate (const Vamos_Geometry::Three_Vector& delta_theta);

    // Put this frame's origin at NEW_POSITION in the parent's
    // coordinates.
    void place (const Vamos_Geometry::Three_Vector& new_position) 
    { m_position = new_position; }

    // Give this frame an absolute orientation of NEW_ORIENTATION.
    void orient (const Vamos_Geometry::Three_Matrix& new_orientation)
    { m_orientation = new_orientation; }

    // Express the orientation of this frame as a vector in the parent
    // frame and a rotation about that vector.  ANGLE holds the
    // rotation angle when the function returns.  The returned vector
    // has a magnitude of sin (ANGLE).  The values returned are
    // suitable for use with the glRotate functions.
    Vamos_Geometry::Three_Vector axis_angle (double* angle) const;

    // Return the position of the origin in the parent frame.
    Vamos_Geometry::Three_Vector position () const { return m_position; };

    // Return the orientation matrix of the frame relative to the
    // parent frame.
    Vamos_Geometry::Three_Matrix orientation () const { return m_orientation; }

    // Return the velocity of the origin relative to the parent frame.
    Vamos_Geometry::Three_Vector velocity () const { return m_velocity; }

    // Return the angular velocity of the frame relative to the parent
    // frame.
    Vamos_Geometry::Three_Vector ang_velocity () const 
    { return m_ang_velocity; }
	
	void set_ang_velocity(Vamos_Geometry::Three_Vector newangvel) {m_ang_velocity = newangvel;}
	void set_velocity(Vamos_Geometry::Three_Vector newvel) {m_velocity = newvel;}
  };
}

#endif // not _FRAME_H_
