//  Rigid_Body.h - a rigid body.
//
//  Copyright (C) 2001--2002 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 _RIGID_BODY_H_
#define _RIGID_BODY_H_

#include "../geometry/Three_Vector.h"
#include "../geometry/Three_Matrix.h"
#include "../geometry/Material.h"
#include "../geometry/Inertia_Tensor.h"
#include "Frame.h"
#include "Contact_Point.h"

#include <vector>

namespace Vamos_Body
{
  struct Contact_Parameters
  {
    Contact_Parameters ();
	Contact_Parameters (Particle* contact_point,
                        const Vamos_Geometry::Three_Vector& impulse,
                        double distance,
                        const Vamos_Geometry::Three_Vector& normal,
                        const Vamos_Geometry::Material& material);

	Particle* mp_contact_point;
    Vamos_Geometry::Three_Vector m_impulse;
	double m_distance;
	Vamos_Geometry::Three_Vector m_normal;
	Vamos_Geometry::Material m_material;
  };

  //* A rigid body.
  class Rigid_Body : public Frame
  {
	// The body's initial position vector, used by reset ().
	Vamos_Geometry::Three_Vector m_initial_position;
	// The body's initial orientation, used by reset ().
    Vamos_Geometry::Three_Matrix m_initial_orientation;

	Vamos_Geometry::Three_Vector m_last_position;

	// The velocity of the center of mass.
	Vamos_Geometry::Three_Vector m_cm_velocity;

	Vamos_Geometry::Three_Vector m_last_cm_velocity;

	Vamos_Geometry::Three_Vector m_last_velocity;

	Vamos_Geometry::Three_Matrix m_last_orientation;

	Vamos_Geometry::Three_Vector m_last_ang_velocity;

	// The acceleration due to gravity, distance/time^2.  The units
	// determine the distance and time units used in the rest of the
	// simulation.
	Vamos_Geometry::Three_Vector m_gravity;

	// The elapsed time since the last time step.
	double m_delta_time;

	// The total mass of the body.
	double m_mass;

	// Common code for the two reset () methods.
	void private_reset ();

	Contact_Parameters m_contact_parameters;

    Contact_Point* mp_temporary_contact_point;

    void remove_temporary_contact_point ();

  protected:
	// The inertia tensor for the body.
	Vamos_Geometry::Inertia_Tensor m_inertia;

	// A vector of pointers to the force and torque producers that
	// make up the body
	std::vector <Particle*> m_particles;

	// The position of the center of mass of the body relative to the
	// origin of the body.
	Vamos_Geometry::Three_Vector m_body_cm;

  public:
	//** Constructors

	// Specify the position and orientation of the body.
	Rigid_Body (const Vamos_Geometry::Three_Vector& pos, 
                const Vamos_Geometry::Three_Matrix& orient);

	// Specify the position, the orientation is the same as the parent.
	Rigid_Body (const Vamos_Geometry::Three_Vector& pos);

	// The body is coincident with the parent.
	Rigid_Body ();

	//** Destructor
	virtual ~Rigid_Body ();

    Vamos_Geometry::Three_Vector center_of_mass () const { return m_body_cm; }

	// Return the position of the center of mass of the body with
	// respect to the world.
	Vamos_Geometry::Three_Vector cm_position ();

	// Provide access to the frame's position method.  Return the
	// origin of the body.
	Vamos_Geometry::Three_Vector position () const 
	{ return Frame::position (); }

	// Return the contact position of the particle with respect to the
	// world.
	Vamos_Geometry::Three_Vector contact_position (Particle* contact_point);
	Vamos_Geometry::Three_Vector position (Particle* contact_point);

	// Return the smallest contact position z-value of the particles.
	double lowest_contact_position () const;

	// Add a particle to the body.
	void add_particle (Particle* const comp) 
	{ m_particles.push_back (comp); } 

	// Calculate the center of mass, the ineritia tensor, and its
	// inverse.
	void update_center_of_mass ();

	// Provide access to the particles.
	std::vector <Particle*>& particles () { return m_particles; }

	void find_forces ();

	// Advance the body in time by TIME.
	void propagate (double time);

	// Undo the last propagation.
	void rewind ();

	// Finish the timestep.
	void end_timestep ();

	// Called by the world to tell the body what the acceleration due
	// to gravity is.
	void gravity (const Vamos_Geometry::Three_Vector& grav) 
	{ m_gravity = grav; }

	// Return the velocity of the particle in the parent frame.
	// With no argument, return the velocity of the origin of the body
	// frame.
	Vamos_Geometry::Three_Vector velocity (Particle* particle);
	Vamos_Geometry::Three_Vector 
    velocity (const Vamos_Geometry::Three_Vector& r);

	// Return the velocity of the center of mass.
	Vamos_Geometry::Three_Vector cm_velocity () const 
    { return m_cm_velocity; }

	// Set the velocity of the center of mass.
	void cm_velocity (Vamos_Geometry::Three_Vector vel) 
	{ m_cm_velocity = vel; }

    Vamos_Geometry::Three_Vector 
    moment (const Vamos_Geometry::Three_Vector& position)
    { return rotate_out (position - m_body_cm); }

	Vamos_Geometry::Inertia_Tensor inertia () const { return m_inertia; }

	// Handle a collision.
	void contact (Particle* contact_point, 
                  const Vamos_Geometry::Three_Vector& impulse,
				  const Vamos_Geometry::Three_Vector& velocity,
				  double distance,
				  const Vamos_Geometry::Three_Vector& normal,
				  const Vamos_Geometry::Material& material);

    void temporary_contact (const Particle& point,
                            const Vamos_Geometry::Three_Vector& position,
                            const Vamos_Geometry::Three_Vector& velocity,
                            double depth,
                            const Vamos_Geometry::Three_Vector& normal,
                            const Vamos_Geometry::Material& material);

	// Transform the wind into the body frame and send it to the 
	// aerodynamic device.
	void wind (Particle* aero_device, 
			   const Vamos_Geometry::Three_Vector& wind_vector, 
			   double density);

	// Return the total mass.
	double mass () const { return m_mass; }

	// This function is defined by subclasses that work with a
	// graphics system.
	virtual void draw () {};

	// Return the body to its initial state at its initial position.
	virtual void reset (double direction);

	// Return the body to its initial state at a particular position and
	// orientation.
	virtual void reset (const Vamos_Geometry::Three_Vector& position, 
						const Vamos_Geometry::Three_Matrix& orientation);
  };
}

#endif // !_BODY_H_
