/***************************************************************************
                          body.cpp  -  description
                             -------------------
    begin                : Mit Sep 25 13:11:41 CEST 2002
    copyright            : (C) 2002 by Harald Krippel
    email                : neuro.harald@surfeu.at
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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.                                   *
 *                                                                         *
 ***************************************************************************/
#include <body.h>

Body::Body(void)
{
  int a=0;

  lvel[0]=0;
  lvel[1]=0;
  lvel[2]=0;
  
  avel[0]=0;
  avel[1]=0;
  avel[2]=0;

  for(a=0;a<MAX_BODY_OBJ;a++)
  {
    objtrans[a] = NULL;
    odgeom[a]   = NULL;
    odbody[a]   = NULL;
  }
  boxobjtrans = NULL;
  oldtyp=-1;
}

void Body::createOdeBody(dWorldID odworld, dSpaceID odspace,int typ)
{
   // some constants
 
#define MASS 0.1f	



    // body

   odbody[0] = dBodyCreate (odworld);
   dBodySetPosition (odbody[0],0,0,0);
   dMassSetBox(&odm,1,1,1,1);
   dMassTranslate (&odm, 0, 0, 0);  // HEIGHT
   dMassAdjust(&odm,MASS); //Mass of body
   dBodySetMass (odbody[0],&odm);
   if(odgeom[0] != NULL)
      dGeomDestroy(odgeom[0]);
   if(typ == 0)  // Box
   {
    odgeom[0] = dCreateBox (odspace,1,1,1);
   }
   if(typ == 1)  // Cylinder
   {
    odgeom[0] = dCreateCCylinder (odspace,0.5,1);
   }
   if(typ == 2)  // Sphere
   {
    odgeom[0] = dCreateSphere (odspace,1);
   }
   dGeomSetBody (odgeom[0],odbody[0]);
   oldtyp=typ;
}

void Body::typ(dSpaceID odspace,int typ)
{
  if(oldtyp != typ)
  {
   if(odgeom[0] != NULL)
      dGeomDestroy(odgeom[0]);
   if(typ == 0)  // Box
   {
    odgeom[0] = dCreateBox (odspace,1,1,1);
   }
   if(typ == 1)  // Cylinder
   {
    odgeom[0] = dCreateCCylinder (odspace,0.5,1);
   }
   if(typ == 2)  // Sphere
   {
    odgeom[0] = dCreateSphere (odspace,1);
   }
   dGeomSetBody (odgeom[0],odbody[0]);
   oldtyp=typ;
  }
}

void Body::adjustMass(float tmpmass)
{
   dMassAdjust(&odm,tmpmass); //Mass of body
   dBodySetMass (odbody[0],&odm);
}

void Body::Scale(float x, float y, float z)
{
    if(dGeomGetClass(odgeom[0]) == dBoxClass)
    {
      dGeomBoxSetLengths (odgeom[0], x, y, z);
    }
    if(dGeomGetClass(odgeom[0]) == dSphereClass)
    {
      dGeomSphereSetRadius (odgeom[0], x/2.0f);
    }
    if(dGeomGetClass(odgeom[0]) == dCCylinderClass)
    {
      dGeomCCylinderSetParams (odgeom[0], x/2.0f,z);
    }
}

void Body::update(ssgRoot *scene, float dt)
{
  int a=0;
  dVector3 dxyz;

  // positon body from ode
  for(a=0;a<MAX_BODY_OBJ;a++)
  {
    if(objtrans[a] != NULL)
    {
      if(dGeomGetClass(odgeom[a]) == dBoxClass)
      {
        dGeomBoxGetLengths (odgeom[a], dxyz);
      }
      if(dGeomGetClass(odgeom[a]) == dSphereClass)
      {
        dxyz[0]= dGeomSphereGetRadius (odgeom[a])*2;
        dxyz[1] = dxyz[2] = dxyz[0];
      }
      if(dGeomGetClass(odgeom[a]) == dCCylinderClass)
      {
        dGeomCCylinderGetParams (odgeom[a],&dxyz[0],&dxyz[2]);
        dxyz[0] *=2;
        dxyz[1]=dxyz[0];
      }
      setTransform(objtrans[a],dBodyGetPosition(odbody[a]),dBodyGetRotation(odbody[a]),dxyz);
    }
  }
}

void Body::setTransform (ssgTransform *objtrans, const dReal bodypos[3], const dReal R[12],dVector3 dxyz)
{
  sgMat4 m;
  
  m[0][0] = R[0] ;
  m[0][1] = R[4] ;
  m[0][2] = R[8] ;
  m[0][3] = SG_ZERO ; //x

  m[1][0] = R[1] ;
  m[1][1] = R[5] ;
  m[1][2] = R[9] ;
  m[1][3] = SG_ZERO ;  // y

  m[2][0] = R[2] ;
  m[2][1] = R[6] ;
  m[2][2] = R[10] ;
  m[2][3] = SG_ZERO ;  // z

  m[3][0] =  bodypos [0] ;
  m[3][1] =  bodypos [1] ;
  m[3][2] =  bodypos [2] ;
  m[3][3] =  SG_ONE ;

  sgScaleVec3(m[0], dxyz[0]);
  sgScaleVec3(m[1], dxyz[1]);
  sgScaleVec3(m[2], dxyz[2]);
          
  objtrans->setTransform(m);
  sgSetCoord  ( &pos, m ) ;
}

void Body::GetPosition ( sgCoord  *bodypos )
{
   sgCopyCoord ( bodypos, &pos ) ;
}

void Body::SetPosition ()
{
  dMatrix3 odR;

  dBodySetPosition (odbody[0],pos.xyz[0],pos.xyz[1],pos.xyz[2]);
  dRFromEulerAngles(odR,-pos.hpr[1] * SG_DEGREES_TO_RADIANS
                      ,-pos.hpr[2] * SG_DEGREES_TO_RADIANS
                      ,-pos.hpr[0] * SG_DEGREES_TO_RADIANS);
  dBodySetRotation (odbody[0],odR);

  dBodySetLinearVel (odbody[0],lvel[0],lvel[1],lvel[2]);
  dBodySetAngularVel (odbody[0],avel[0],avel[1],avel[2]);
}

void Body::SetRotation ()
{
  dMatrix3 odR;

  dRFromEulerAngles(odR,-pos.hpr[1] * SG_DEGREES_TO_RADIANS
                      ,-pos.hpr[2] * SG_DEGREES_TO_RADIANS
                      ,-pos.hpr[0] * SG_DEGREES_TO_RADIANS);
  dBodySetRotation (odbody[0],odR);
  dBodySetLinearVel (odbody[0],lvel[0],lvel[1],lvel[2]);
  dBodySetAngularVel (odbody[0],avel[0],avel[1],avel[2]);
}

void Body::Rotation (float h, float p, float r)
{

  dMatrix3 odR;
  sgMat4 oglM;
  const dReal *odeM;
  sgMat4 mh ;

  sgMakeCoordMat4 ( mh, 0, 0, 0, h, p, r ) ;
  odeM = dBodyGetRotation(odbody[0]);

oglM[0][0]  = odeM[0];
oglM[0][1]  = odeM[4];
oglM[0][2]  = odeM[8];
oglM[0][3]  = 0;
oglM[1][0]  = odeM[1];
oglM[1][1]  = odeM[5];
oglM[1][2]  = odeM[9];
oglM[1][3]  = 0;
oglM[2][0]  = odeM[2];
oglM[2][1]  = odeM[6];
oglM[2][2] = odeM[10];
oglM[2][3] = 0;
oglM[3][0] = 0;
oglM[3][1] = 0;
oglM[3][2] = 0;
oglM[3][3] = 1;

  sgPreMultMat4  ( oglM, mh ) ;

  odR[0]=oglM[0][0];      
  odR[4]=oglM[0][1];
  odR[8]=oglM[0][2];

  odR[1]=oglM[1][0];
  odR[5]=oglM[1][1];
  odR[9]=oglM[1][2];

  odR[2]=oglM[2][0];
  odR[6]=oglM[2][1];
  odR[10]=oglM[2][2];
    
  dBodySetRotation (odbody[0],odR);
}

void Body::SetLinearVel ()
{
  dBodySetLinearVel (odbody[0],lvel[0],lvel[1],lvel[2]);
}
void Body::AddRelForce ()
{
  dBodyAddRelForce (odbody[0],lvel[0],lvel[1],lvel[2]);
}

void Body::SetAngularVel ()
{
  dBodySetAngularVel (odbody[0],lvel[0],lvel[1],lvel[2]);
}
void Body::AddRelTorque ()
{
  dBodyAddRelTorque (odbody[0],lvel[0],lvel[1],lvel[2]);
}

Body::~Body()
{
}
