/*************************************************************************

 *                                                                       *

 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *

 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

 *                                                                       *

 * This library is free software; you can redistribute it and/or         *

 * modify it under the terms of EITHER:                                  *

 *   (1) The GNU Lesser General Public License as published by the Free  *

 *       Software Foundation; either version 2.1 of the License, or (at  *

 *       your option) any later version. The text of the GNU Lesser      *

 *       General Public License is included with this library in the     *

 *       file LICENSE.TXT.                                               *

 *   (2) The BSD-style license that is included with this library in     *

 *       the file LICENSE-BSD.TXT.                                       *

 *                                                                       *

 * This library 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 files    *

 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

 *                                                                       *

 *************************************************************************/

/*

core collision functions and data structures, plus part of the public API

for geometry objects

*/

#ifndef __WINSCW__

#include <e32std.h>

#endif // !__WINSCW__

#include <ode/common.h>

#include <ode/matrix.h>

#include <ode/rotation.h>

#include <ode/objects.h>

#include <ode/odemath.h>

#include "collision_kernel.h"

#include "collision_util.h"

#include "collision_std.h"

#include "collision_transform.h"

#include <ode/ode.h>

//****************************************************************************

// helper functions for dCollide()ing a space with another geom

// this struct records the parameters passed to dCollideSpaceGeom()

// Allocate and free posr - we cache a single posr to avoid thrashing

dxPosR* dAllocPosr()

{

	dxPosR* retPosR;

	if (GetGlobalData()->s_cachedPosR)

	{

		retPosR = GetGlobalData()->s_cachedPosR;

		GetGlobalData()->s_cachedPosR = 0;

	}

	else

	{

		retPosR = (dxPosR*) dAlloc (sizeof(dxPosR));

	}

	return retPosR;

}

void dFreePosr(dxPosR* oldPosR)

{

	if (oldPosR)

	{

		if (GetGlobalData()->s_cachedPosR)

		{

			dFree(GetGlobalData()->s_cachedPosR, sizeof(dxPosR));

		}

		GetGlobalData()->s_cachedPosR = oldPosR;

	}

}

void dClearPosrCache(void)

{

	if (GetGlobalData()->s_cachedPosR)

	{

		dFree(GetGlobalData()->s_cachedPosR, sizeof(dxPosR));

		GetGlobalData()->s_cachedPosR = 0;

	}

}

struct SpaceGeomColliderData {

  int flags;			// space left in contacts array

  dContactGeom *contact;

  int skip;

};

static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2)

{

  SpaceGeomColliderData *d = (SpaceGeomColliderData*) data;

  if (d->flags & NUMC_MASK) {

    int n = dCollide (o1,o2,d->flags,d->contact,d->skip);

    d->contact = CONTACT (d->contact,d->skip*n);

    d->flags -= n;

  }

}

static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags,

			      dContactGeom *contact, int skip)

{

  SpaceGeomColliderData data;

  data.flags = flags;

  data.contact = contact;

  data.skip = skip;

  dSpaceCollide2 (o1,o2,&data,&space_geom_collider);

  return (flags & NUMC_MASK) - (data.flags & NUMC_MASK);

}

//****************************************************************************

// dispatcher for the N^2 collider functions

// setCollider() will refuse to write over a collider entry once it has

// been written.

static void setCollider (int i, int j, dColliderFn *fn)

{

  if (GetGlobalData()->colliders[i][j].fn == 0) {

    GetGlobalData()->colliders[i][j].fn = fn;

    GetGlobalData()->colliders[i][j].reverse = 0;

  }

  if (GetGlobalData()->colliders[j][i].fn == 0) {

    GetGlobalData()->colliders[j][i].fn = fn;

    GetGlobalData()->colliders[j][i].reverse = 1;

  }

}

static void setAllColliders (int i, dColliderFn *fn)

{

  for (int j=0; j<dGeomNumClasses; j++) setCollider (i,j,fn);

}

static void initColliders()

{

  int i,j;

  if (GetGlobalData()->colliders_initialized) return;

  GetGlobalData()->colliders_initialized = 1;

  memset (GetGlobalData()->colliders,0,sizeof(GetGlobalData()->colliders));

  // setup space colliders

  for (i=dFirstSpaceClass; i <= dLastSpaceClass; i++) {

    for (j=0; j < dGeomNumClasses; j++) {

      setCollider (i,j,&dCollideSpaceGeom);

    }

  }

  setCollider (dSphereClass,dSphereClass,&dCollideSphereSphere);

  setCollider (dSphereClass,dBoxClass,&dCollideSphereBox);

  setCollider (dSphereClass,dPlaneClass,&dCollideSpherePlane);

  setCollider (dBoxClass,dBoxClass,&dCollideBoxBox);

  setCollider (dBoxClass,dPlaneClass,&dCollideBoxPlane);

  setCollider (dCapsuleClass,dSphereClass,&dCollideCapsuleSphere);

  setCollider (dCapsuleClass,dBoxClass,&dCollideCapsuleBox);

  setCollider (dCapsuleClass,dCapsuleClass,&dCollideCapsuleCapsule);

  setCollider (dCapsuleClass,dPlaneClass,&dCollideCapsulePlane);

  setCollider (dRayClass,dSphereClass,&dCollideRaySphere);

  setCollider (dRayClass,dBoxClass,&dCollideRayBox);

  setCollider (dRayClass,dCapsuleClass,&dCollideRayCapsule);

  setCollider (dRayClass,dPlaneClass,&dCollideRayPlane);

  setCollider (dRayClass,dCylinderClass,&dCollideRayCylinder);

  setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox);

  setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere);

  setCollider (dCylinderClass,dPlaneClass,&dCollideCylinderPlane);

  //setCollider (dCylinderClass,dCylinderClass,&dCollideCylinderCylinder);

//--> Convex Collision

  setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane);

  setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex);

  setCollider (dConvexClass,dBoxClass,&dCollideConvexBox);

  setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule);

  setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex);

  setCollider (dRayClass,dConvexClass,&dCollideRayConvex);

//<-- Convex Collision

//--> dHeightfield Collision

  setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield);

  setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield);

  setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield);

  setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield);

  setCollider (dHeightfieldClass,dCylinderClass,&dCollideHeightfield);

  setCollider (dHeightfieldClass,dConvexClass,&dCollideHeightfield);

#if dTRIMESH_ENABLED

  setCollider (dHeightfieldClass,dTriMeshClass,&dCollideHeightfield);

#endif

//<-- dHeightfield Collision

  setAllColliders (dGeomTransformClass,&dCollideTransform);

}

EXPORT_C int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,

	      int skip)

{

  // no contacts if both geoms are the same

  if (o1 == o2) return 0;

  // no contacts if both geoms on the same body, and the body is not 0

  if (o1->body == o2->body && o1->body) return 0;

  o1->recomputePosr();

  o2->recomputePosr();

  dColliderEntry *ce = &GetGlobalData()->colliders[o1->type][o2->type];

  int count = 0;

  if (ce->fn) {

    if (ce->reverse) {

      count = (*ce->fn) (o2,o1,flags,contact,skip);

      for (int i=0; i<count; i++) {

	dContactGeom *c = CONTACT(contact,skip*i);

	c->normal[0] = -c->normal[0];

	c->normal[1] = -c->normal[1];

	c->normal[2] = -c->normal[2];

	dxGeom *tmp = c->g1;

	c->g1 = c->g2;

	c->g2 = tmp;

	int tmpint = c->side1;

	c->side1 = c->side2;

	c->side2 = tmpint;

      }

    }

    else {

      count = (*ce->fn) (o1,o2,flags,contact,skip);

    }

  }

  return count;

}

//****************************************************************************

// dxGeom

dxGeom::dxGeom (dSpaceID _space, int is_placeable)

{

  initColliders();

  // setup body vars. invalid type of -1 must be changed by the constructor.

  type = -1;

  gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;

  if (is_placeable) gflags |= GEOM_PLACEABLE;

  data = 0;

  body = 0;

  body_next = 0;

  if (is_placeable) {

	final_posr = dAllocPosr();

    dSetZero (final_posr->pos,4);

    dRSetIdentity (final_posr->R);

  }

  else {

    final_posr = 0;

  }

  offset_posr = 0;

  // setup space vars

  next = 0;

  tome = 0;

  parent_space = 0;

  dSetZero (aabb,6);

  category_bits = ~0;

  collide_bits = ~0;

  // put this geom in a space if required

  if (_space) dSpaceAdd (_space,this);

}

dxGeom::~dxGeom()

{

   if (parent_space) dSpaceRemove (parent_space,this);

   if ((gflags & GEOM_PLACEABLE) && (!body || (body && offset_posr)))

     dFreePosr(final_posr);

   if (offset_posr) dFreePosr(offset_posr);

   bodyRemove();

}

int dxGeom::AABBTest (dxGeom */*o*/, dReal /*aabb*/[6])

{

  return 1;

}

void dxGeom::bodyRemove()

{

  if (body) {

    // delete this geom from body list

    dxGeom **last = &body->geom, *g = body->geom;

    while (g) {

      if (g == this) {

	*last = g->body_next;

	break;

      }

      last = &g->body_next;

      g = g->body_next;

    }

    body = 0;

    body_next = 0;

  }

}

inline void myswap(dReal& a, dReal& b) { dReal t=b; b=a; a=t; }

inline void matrixInvert(const dMatrix3& inMat, dMatrix3& outMat)

{

	memcpy(outMat, inMat, sizeof(dMatrix3));

	// swap _12 and _21

	myswap(outMat[0 + 4*1], outMat[1 + 4*0]);

	// swap _31 and _13

	myswap(outMat[2 + 4*0], outMat[0 + 4*2]);

	// swap _23 and _32

	myswap(outMat[1 + 4*2], outMat[2 + 4*1]);

}

void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr)

{

	dMatrix3 inv_offset;

	matrixInvert(offset_posr.R, inv_offset);

	dMULTIPLY0_333(body_posr.R, final_posr.R, inv_offset);

	dVector3 world_offset;

	dMULTIPLY0_331(world_offset, body_posr.R, offset_posr.pos);

	body_posr.pos[0] = final_posr.pos[0] - world_offset[0];

	body_posr.pos[1] = final_posr.pos[1] - world_offset[1];

	body_posr.pos[2] = final_posr.pos[2] - world_offset[2];

}

void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr)

{

	dMatrix3 inv_body;

	matrixInvert(body_posr.R, inv_body);

	dMULTIPLY0_333(offset_posr.R, inv_body, world_posr.R);

	dVector3 world_offset;

	world_offset[0] = world_posr.pos[0] - body_posr.pos[0];

	world_offset[1] = world_posr.pos[1] - body_posr.pos[1];

	world_offset[2] = world_posr.pos[2] - body_posr.pos[2];

	dMULTIPLY0_331(offset_posr.pos, inv_body, world_offset);

}

void dxGeom::computePosr()

{

  // should only be recalced if we need to - ie offset from a body

  dMULTIPLY0_331 (final_posr->pos,body->posr.R,offset_posr->pos);

  final_posr->pos[0] += body->posr.pos[0];

  final_posr->pos[1] += body->posr.pos[1];

  final_posr->pos[2] += body->posr.pos[2];

  dMULTIPLY0_333 (final_posr->R,body->posr.R,offset_posr->R);

}

//****************************************************************************

// misc

dxGeom *dGeomGetBodyNext (dxGeom *geom)

{

  return geom->body_next;

}

//****************************************************************************

// public API for geometry objects

EXPORT_C void dGeomDestroy (dxGeom *g)

{

  delete g;

}

EXPORT_C void dGeomSetData (dxGeom *g, void *data)

{

  g->data = data;

}

EXPORT_C void *dGeomGetData (dxGeom *g)

{

  return g->data;

}

EXPORT_C void dGeomSetBody (dxGeom *g, dxBody *b)

{

  if (b) {

    if (!g->body) dFreePosr(g->final_posr);

    if (g->body != b) {

      if (g->offset_posr) {

        dFreePosr(g->offset_posr);

        g->offset_posr = 0;

      }

      g->final_posr = &b->posr;

      g->bodyRemove();

      g->bodyAdd (b);

    }

    dGeomMoved (g);

  }

  else {

    if (g->body) {

      if (g->offset_posr)

      {

        // if we're offset, we already have our own final position, make sure its updated

        g->recomputePosr();

        dFreePosr(g->offset_posr);

        g->offset_posr = 0;

      }

      else

      {

        g->final_posr = dAllocPosr();

        memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3));

        memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3));

      }

      g->bodyRemove();

    }

    // dGeomMoved() should not be called if the body is being set to 0, as the

    // new position of the geom is set to the old position of the body, so the

    // effective position of the geom remains unchanged.

  }

}

EXPORT_C dBodyID dGeomGetBody (dxGeom *g)

{

  return g->body;

}

EXPORT_C void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)

{

  if (g->offset_posr) {

    // move body such that body+offset = position

	dVector3 world_offset;

	dMULTIPLY0_331(world_offset, g->body->posr.R, g->offset_posr->pos);

	dBodySetPosition(g->body,

	    x - world_offset[0],

	    y - world_offset[1],

	    z - world_offset[2]);

  }

  else if (g->body) {

    // this will call dGeomMoved (g), so we don't have to

    dBodySetPosition (g->body,x,y,z);

  }

  else {

    g->final_posr->pos[0] = x;

    g->final_posr->pos[1] = y;

    g->final_posr->pos[2] = z;

    dGeomMoved (g);

  }

}

EXPORT_C void dGeomSetRotation (dxGeom *g, const dMatrix3 R)

{

  if (g->offset_posr) {

    g->recomputePosr();

    // move body such that body+offset = rotation

    dxPosR new_final_posr;

    dxPosR new_body_posr;

    memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));

    memcpy(new_final_posr.R, R, sizeof(dMatrix3));

    getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);

    dBodySetRotation(g->body, new_body_posr.R);

    dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);

  }

  else if (g->body) {

    // this will call dGeomMoved (g), so we don't have to

    dBodySetRotation (g->body,R);

  }

  else {

    memcpy (g->final_posr->R,R,sizeof(dMatrix3));

    dGeomMoved (g);

  }

}

EXPORT_C void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)

{

  if (g->offset_posr) {

    g->recomputePosr();

    // move body such that body+offset = rotation

    dxPosR new_final_posr;

    dxPosR new_body_posr;

    dQtoR (quat, new_final_posr.R);

    memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));

    getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);

    dBodySetRotation(g->body, new_body_posr.R);

    dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);

  }

  if (g->body) {

    // this will call dGeomMoved (g), so we don't have to

    dBodySetQuaternion (g->body,quat);

  }

  else {

    dQtoR (quat, g->final_posr->R);

    dGeomMoved (g);

  }

}

EXPORT_C const dReal * dGeomGetPosition (dxGeom *g)

{

  g->recomputePosr();

  return g->final_posr->pos;

}

EXPORT_C void dGeomCopyPosition(dxGeom *g, dVector3 pos)

{

  g->recomputePosr();

  const dReal* src = g->final_posr->pos;

  pos[0] = src[0];

  pos[1] = src[1];

  pos[2] = src[2];

}