//	Contact_Point.cc - a particle that responds to collisions.
//
//  Copyright (C) 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

#include <vamos/body/Contact_Point.h>

//* Constructors

Vamos_Body::Contact_Point::
Contact_Point (double mass, const Vamos_Geometry::Three_Vector& position, 
			   const Vamos_Geometry::Three_Matrix& orientation,
			   Vamos_Geometry::Material::Material_Type type,
			   double friction, double restitution) 
  : Particle (mass, position, orientation),
	m_contact (false)
{
  m_material = new Vamos_Geometry::Material (type, friction, restitution);
}

// Take the parent's orientation.
Vamos_Body::Contact_Point::
Contact_Point (double mass, const Vamos_Geometry::Three_Vector& position,
			   Vamos_Geometry::Material::Material_Type type,
			   double friction, double restitution ) 
  : Particle (mass, position),
	m_contact (false)
{
  m_material = new Vamos_Geometry::Material (type, friction, restitution);
}

// Default constructor.
Vamos_Body::Contact_Point::
Contact_Point ()
{
}

// Handle collisions.  The return value is how much the particle has
// moved as a result of the contact.  For a Particle, this is always
// 0.  But, for derived classes that model moving parts, a non-zero
// value may be returned.
double Vamos_Body::Contact_Point::
contact (const Vamos_Geometry::Three_Vector& position,
		 const Vamos_Geometry::Inertia_Tensor& inertia,
		 const Vamos_Geometry::Three_Vector& velocity, 
		 double, // distance,
		 const Vamos_Geometry::Three_Vector& normal,
		 const Vamos_Geometry::Three_Vector& ang_velocity,
		 Vamos_Geometry::Material_Handle material)
{
  // POSITION - the world-frame vector from the body's center of mass to
  //   the particle.
  // INERTIA - the body's inertia tensor.
  // VELOCITY - the particle's velocity normal to the surface in the
  //   body frame.
  // ANG_VELOCITY - the angular velocity about the normal to the
  //   surface. 
  // NORM - the normal vector in the body frame.
  // MATERIAL - the material properies of the thing collided with.

   Vamos_Geometry::Three_Vector v_perp = rotate_in (velocity.project (normal));

  // Find the effective mass.
  double meff = inertia.inertia (position, normal);
  m_impulse = -(1.0 + (m_material->restitution_factor () 
					   * material->restitution_factor ())) * meff * v_perp;

  Vamos_Geometry::Three_Vector v_par = rotate_in (velocity) - v_perp;
  m_impulse -= v_par.unit () * m_material->friction_factor ()
 	* material->friction_factor () * m_impulse.abs ();

  m_contact = true;
  
  return 0;
}


// Find and store the forces and torques for the current
// configuration.
void Vamos_Body::Contact_Point::
find_forces ()
{
  if (!m_contact)
	{
	  m_force.zero ();
	  m_impulse.zero ();
	  m_torque.zero ();
	}
}

// Do any neccary cleanup at the end of a time step.
void Vamos_Body::Contact_Point::
end_timestep ()
{
  m_contact = false;
}
