/*
 *   GRacer
 *
 *   Copyright (C) 1999 Takashi Matsuda <matsu@users.sourceforge.net>
 *
 * 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 <math.h>
#include <string.h>
#include "gr_physics.h"


void
gr_physics_inertial_move (GrInertial *value, double h)
{
  double estimate;

  if (fabs(value->target - value->current) < h * fabs(value->speed) &&
      fabs(value->speed) <= value->accel) {
    value->current = value->target;
    value->speed = 0.0;

    return;
  }
  else
    value->current += value->speed * h;

  estimate = value->current;
  estimate += 0.5 * value->speed * fabs (value->speed) / value->accel;

  if (estimate > value->target)
    value->speed -= value->accel;
  else if (estimate < value->target)
    value->speed += value->accel;
}


void
gr_rigid_body_integral (GrRigidBody *rb, int from, double h)
{
  GrVertex force;
  GrVertex acc;
  GrVertex spd;
#if 0
  GrVertex tmp;
  GrQuaternion quat;
#endif
  int to = (from + 1) % 2;

  /* linear */
  gr_matrix_mult_vertex (&rb->state[from].m_zyx,
      			 &rb->state[from].force,
			 &force);

  gr_vertex_scale (&acc, &force, h / rb->mass);
  gr_vertex_scale (&spd, &acc, 0.5);
  gr_vertex_add_a (&spd, &rb->state[from].spd);
  gr_vertex_scale_a (&spd, h);

  gr_vertex_add (&rb->state[to].pos, &rb->state[from].pos, &spd);
  gr_vertex_add (&rb->state[to].spd, &rb->state[from].spd, &acc);

  /* rotation */
  gr_matrix_mult_vertex (&rb->m_j, &rb->state[from].m, &acc);

  gr_vertex_scale_a (&acc, h);
  gr_vertex_scale (&spd, &acc, 0.5);
  gr_vertex_add_a (&spd, &rb->state[from].omg);
  gr_vertex_scale_a (&spd, h);

  gr_vertex_add (&rb->state[to].rot, &rb->state[from].rot, &spd);
  gr_vertex_add (&rb->state[to].omg, &rb->state[from].omg, &acc);

  /*
   * d(quat)/dt = f(quat, omg)
   */
#if 0
  quat.q0 = - 0.5 * (  spd.f[0] * rb->state[from].quat.q.f[0]
  		     + spd.f[1] * rb->state[from].quat.q.f[1]
  		     + spd.f[2] * rb->state[from].quat.q.f[2]);

  quat.q.f[0] = 0.5 * (  spd.f[0] * rb->state[from].quat.q0
		       - spd.f[1] * rb->state[from].quat.q.f[2]
		       + spd.f[2] * rb->state[from].quat.q.f[1]);

  quat.q.f[1] = 0.5 * (  spd.f[0] * rb->state[from].quat.q.f[2]
		       + spd.f[1] * rb->state[from].quat.q0
		       - spd.f[2] * rb->state[from].quat.q.f[0]);

  quat.q.f[2] = 0.5 * (- spd.f[0] * rb->state[from].quat.q.f[1]
		       + spd.f[1] * rb->state[from].quat.q.f[0]
		       + spd.f[2] * rb->state[from].quat.q0);

  gr_quaternion_normalize (&quat, &quat);
  printf (" Omg: %f %f %f\n",
      spd.f[0],
      spd.f[1],
      spd.f[2]);

  printf ("Quat: %f %f %f %f\n",
      quat.q0,
      quat.q.f[0],
      quat.q.f[1],
      quat.q.f[2]);

  printf ("From: %f %f %f %f\n",
      rb->state[from].quat.q0,
      rb->state[from].quat.q.f[0],
      rb->state[from].quat.q.f[1],
      rb->state[from].quat.q.f[2]);

  gr_quaternion_add (&rb->state[to].quat, &quat, &rb->state[from].quat);

  printf ("  To: %f %f %f %f\n",
      rb->state[to].quat.q0,
      rb->state[to].quat.q.f[0],
      rb->state[to].quat.q.f[1],
      rb->state[to].quat.q.f[2]);
#endif

  rb->state[to].rot.c.x = fmod (rb->state[to].rot.c.x, 2.0 * M_PI);
  rb->state[to].rot.c.y = fmod (rb->state[to].rot.c.y, 2.0 * M_PI);
  rb->state[to].rot.c.z = fmod (rb->state[to].rot.c.z, 2.0 * M_PI);

  gr_rigid_body_reset (rb, to);
}

void
gr_rigid_body_reset (GrRigidBody *rb, int which)
{
  GrVertexd *rot;
  GrMatrix *m;
  double sx, cx;
  double sy, cy;
  double sz, cz;
#if 0
  GrQuaternion quat;
#endif

  memset (&rb->state[which].m, 0, sizeof(GrVertex));
  memset (&rb->state[which].force, 0, sizeof(GrVertex));

  rot = &rb->state[which].rot;

  sx = sin(rot->c.x);
  cx = cos(rot->c.x);

  sy = sin(rot->c.y);
  cy = cos(rot->c.y);

  sz = sin(rot->c.z);
  cz = cos(rot->c.z);

  m = &rb->state[which].m_zyx;
  gr_matrix_rotate_x (gr_matrix_identity, m, sx, cx);
  gr_matrix_rotate_y (m, m, sy, cy);
  gr_matrix_rotate_z (m, m, sz, cz);

  m = &rb->state[which].m_rzyx;
  gr_matrix_rotate_z (gr_matrix_identity, m, -sz, cz);
  gr_matrix_rotate_y (m, m, -sy, cy);
  gr_matrix_rotate_x (m, m, -sx, cx);

#if 0
  //gr_quaternion_make_matrix (&rb->state[which].quat, &rb->state[which].m_zyx);

  quat.q0 = rb->state[which].quat.q0;
  quat.q.f[0] = -rb->state[which].quat.q.f[0];
  quat.q.f[1] = -rb->state[which].quat.q.f[1];
  quat.q.f[2] = -rb->state[which].quat.q.f[2];

  //gr_quaternion_make_matrix (&quat, &rb->state[which].m_rzyx);
#endif
}


void
gr_rigid_body_add_force_relative (GrRigidBody *rb,
    				  int which,
				  GrVertex *point,
				  GrVertex *force)
{
  GrVertex tmp;

  /* accumulate force */
  gr_vertex_add_a (&rb->state[which].force, force);

  /* accumulate rotation momentum */
  gr_vertex_cross (&tmp, point, force);
  gr_vertex_add_a (&rb->state[which].m, &tmp);
}


void
gr_rigid_body_add_force (GrRigidBody *rb,
    			 int which,
			 GrVertex *point,
			 GrVertex *force)
{
  GrVertex tp, tf;

  gr_matrix_mult_vertex (&rb->state[which].m_rzyx, point, &tp);
  gr_matrix_mult_vertex (&rb->state[which].m_rzyx, force, &tf);

  gr_rigid_body_add_force_relative (rb, which, &tp, &tf);
}