/*************************************************************************
 *                                                                       *
 * 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

*/

#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 "collision_trimesh_internal.h"

#ifdef _MSC_VER
#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
#endif

//****************************************************************************
// 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
static dxPosR* s_cachedPosR = 0;

dxPosR* dAllocPosr()
{
	dxPosR* retPosR;
	if (s_cachedPosR)
	{
		retPosR = s_cachedPosR;
		s_cachedPosR = 0;
	}
	else
	{
		retPosR = (dxPosR*) dAlloc (sizeof(dxPosR));
	}
	return retPosR;
}

void dFreePosr(dxPosR* oldPosR)
{
	if (oldPosR)
	{
		if (s_cachedPosR)
		{
			dFree(s_cachedPosR, sizeof(dxPosR));
		}
		s_cachedPosR = oldPosR;
	}
}

void dClearPosrCache(void)
{
	if (s_cachedPosR)
	{
		dFree(s_cachedPosR, sizeof(dxPosR));
		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

// function pointers and modes for n^2 class collider functions

struct dColliderEntry {
  dColliderFn *fn;	// collider function, 0 = no function available
  int reverse;		// 1 = reverse o1 and o2
};
static dColliderEntry colliders[dGeomNumClasses][dGeomNumClasses];
static int colliders_initialized = 0;


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

static void setCollider (int i, int j, dColliderFn *fn)
{
  if (colliders[i][j].fn == 0) {
    colliders[i][j].fn = fn;
    colliders[i][j].reverse = 0;
  }
  if (colliders[j][i].fn == 0) {
    colliders[j][i].fn = fn;
    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 (colliders_initialized) return;
  colliders_initialized = 1;

  memset (colliders,0,sizeof(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 (dCCylinderClass,dSphereClass,&dCollideCCylinderSphere);
  setCollider (dCCylinderClass,dBoxClass,&dCollideCCylinderBox);
  setCollider (dCCylinderClass,dCCylinderClass,&dCollideCCylinderCCylinder);
  setCollider (dCCylinderClass,dPlaneClass,&dCollideCCylinderPlane);
  setCollider (dRayClass,dSphereClass,&dCollideRaySphere);
  setCollider (dRayClass,dBoxClass,&dCollideRayBox);
  setCollider (dRayClass,dCCylinderClass,&dCollideRayCCylinder);
  setCollider (dRayClass,dPlaneClass,&dCollideRayPlane);
#ifdef dTRIMESH_ENABLED
  setCollider (dTriMeshClass,dSphereClass,&dCollideSTL);
  setCollider (dTriMeshClass,dBoxClass,&dCollideBTL);
  setCollider (dTriMeshClass,dRayClass,&dCollideRTL);
  setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL);
  setCollider (dTriMeshClass,dCCylinderClass,&dCollideCCTL);
#ifdef dCYLINDER_ENABLED
  setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh);
#endif
#endif
#ifdef dCYLINDER_ENABLED
  setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox);
  setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere);
  //setCollider (dCylinderClass,dCylinderClass,&dCollideCylinderCylinder);
#endif
  setAllColliders (dGeomTransformClass,&dCollideTransform);
}


int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
	      int skip)
{
  dAASSERT(o1 && o2 && contact);
  dUASSERT(colliders_initialized,"colliders array not initialized");
  dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number");
  dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number");

  // 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 = &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;
      }
    }
    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) 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
  dIASSERT(offset_posr);  
  dIASSERT(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

#define CHECK_NOT_LOCKED(space) \
  dUASSERT (!(space && space->lock_count), \
	    "invalid operation for geom in locked space");


void dGeomDestroy (dxGeom *g)
{
  dAASSERT (g);
  delete g;
}


void dGeomSetData (dxGeom *g, void *data)
{
  dAASSERT (g);
  g->data = data;
}


void *dGeomGetData (dxGeom *g)
{
  dAASSERT (g);
  return g->data;
}


void dGeomSetBody (dxGeom *g, dxBody *b)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  CHECK_NOT_LOCKED (g->parent_space);

  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.
  }
}


dBodyID dGeomGetBody (dxGeom *g)
{
  dAASSERT (g);
  return g->body;
}


void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  CHECK_NOT_LOCKED (g->parent_space);
  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);
  }
}


void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
{
  dAASSERT (g && R);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  CHECK_NOT_LOCKED (g->parent_space);
  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);
  }
}


void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
{
  dAASSERT (g && quat);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  CHECK_NOT_LOCKED (g->parent_space);
  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);
  }
}


const dReal * dGeomGetPosition (dxGeom *g)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  g->recomputePosr();
  return g->final_posr->pos;
}


const dReal * dGeomGetRotation (dxGeom *g)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  g->recomputePosr();
  return g->final_posr->R;
}


void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  if (g->body && !g->offset_posr) {
    const dReal * body_quat = dBodyGetQuaternion (g->body);
    quat[0] = body_quat[0];
    quat[1] = body_quat[1];
    quat[2] = body_quat[2];
    quat[3] = body_quat[3];
  }
  else {
    g->recomputePosr();
    dRtoQ (g->final_posr->R, quat);
  }
}


void dGeomGetAABB (dxGeom *g, dReal aabb[6])
{
  dAASSERT (g);
  dAASSERT (aabb);
  g->recomputeAABB();
  memcpy (aabb,g->aabb,6 * sizeof(dReal));
}


int dGeomIsSpace (dxGeom *g)
{
  dAASSERT (g);
  return IS_SPACE(g);
}


dSpaceID dGeomGetSpace (dxGeom *g)
{
  dAASSERT (g);
  return g->parent_space;
}


int dGeomGetClass (dxGeom *g)
{
  dAASSERT (g);
  return g->type;
}


void dGeomSetCategoryBits (dxGeom *g, unsigned long bits)
{
  dAASSERT (g);
  CHECK_NOT_LOCKED (g->parent_space);
  g->category_bits = bits;
}


void dGeomSetCollideBits (dxGeom *g, unsigned long bits)
{
  dAASSERT (g);
  CHECK_NOT_LOCKED (g->parent_space);
  g->collide_bits = bits;
}


unsigned long dGeomGetCategoryBits (dxGeom *g)
{
  dAASSERT (g);
  return g->category_bits;
}


unsigned long dGeomGetCollideBits (dxGeom *g)
{
  dAASSERT (g);
  return g->collide_bits;
}


void dGeomEnable (dxGeom *g)
{
	dAASSERT (g);
	g->gflags |= GEOM_ENABLED;
}

void dGeomDisable (dxGeom *g)
{
	dAASSERT (g);
	g->gflags &= ~GEOM_ENABLED;
}

int dGeomIsEnabled (dxGeom *g)
{
	dAASSERT (g);
	return (g->gflags & GEOM_ENABLED) != 0;
}


//****************************************************************************
// C interface that lets the user make new classes. this interface is a lot
// more cumbersome than C++ subclassing, which is what is used internally
// in ODE. this API is mainly to support legacy code.

static int num_user_classes = 0;
static dGeomClass user_classes [dMaxUserClasses];


struct dxUserGeom : public dxGeom {
  void *user_data;

  dxUserGeom (int class_num);
  ~dxUserGeom();
  void computeAABB();
  int AABBTest (dxGeom *o, dReal aabb[6]);
};


dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1)
{
  type = class_num;
  int size = user_classes[type-dFirstUserClass].bytes;
  user_data = dAlloc (size);
  memset (user_data,0,size);
}


dxUserGeom::~dxUserGeom()
{
  dGeomClass *c = &user_classes[type-dFirstUserClass];
  if (c->dtor) c->dtor (this);
  dFree (user_data,c->bytes);
}


void dxUserGeom::computeAABB()
{
  user_classes[type-dFirstUserClass].aabb (this,aabb);
}


int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6])
{
  dGeomClass *c = &user_classes[type-dFirstUserClass];
  if (c->aabb_test) return c->aabb_test (this,o,aabb);
  else return 1;
}


static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags,
				     dContactGeom *contact, int skip)
{
  // this generic collider function is called the first time that a user class
  // tries to collide against something. it will find out the correct collider
  // function and then set the colliders array so that the correct function is
  // called directly the next time around.

  int t1 = o1->type;	// note that o1 is a user geom
  int t2 = o2->type;	// o2 *may* be a user geom

  // find the collider function to use. if o1 does not know how to collide with
  // o2, then o2 might know how to collide with o1 (provided that it is a user
  // geom).
  dColliderFn *fn = user_classes[t1-dFirstUserClass].collider (t2);
  int reverse = 0;
  if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) {
    fn = user_classes[t2-dFirstUserClass].collider (t1);
    reverse = 1;
  }

  // set the colliders array so that the correct function is called directly
  // the next time around. note that fn can be 0 here if no collider was found,
  // which means that dCollide() will always return 0 for this case.
  colliders[t1][t2].fn = fn;
  colliders[t1][t2].reverse = reverse;
  colliders[t2][t1].fn = fn;
  colliders[t2][t1].reverse = !reverse;

  // now call the collider function indirectly through dCollide(), so that
  // contact reversing is properly handled.
  return dCollide (o1,o2,flags,contact,skip);
}


int dCreateGeomClass (const dGeomClass *c)
{
  dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class");

  if (num_user_classes >= dMaxUserClasses) {
    dDebug (0,"too many user classes, you must increase the limit and "
	      "recompile ODE");
  }
  user_classes[num_user_classes] = *c;
  int class_number = num_user_classes + dFirstUserClass;
  initColliders();
  setAllColliders (class_number,&dCollideUserGeomWithGeom);

  num_user_classes++;
  return class_number;
}


void * dGeomGetClassData (dxGeom *g)
{
  dUASSERT (g && g->type >= dFirstUserClass &&
	    g->type <= dLastUserClass,"not a custom class");
  dxUserGeom *user = (dxUserGeom*) g;
  return user->user_data;
}


dGeomID dCreateGeom (int classnum)
{
  dUASSERT (classnum >= dFirstUserClass &&
	    classnum <= dLastUserClass,"not a custom class");
  return new dxUserGeom (classnum);
}



/* ************************************************************************ */
/* geom offset from body */

void dGeomCreateOffset (dxGeom *g)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  if (g->offset_posr)
  {
	return; // already created
  }
  dIASSERT (g->final_posr == &g->body->posr);
  
  g->final_posr = dAllocPosr();
  g->offset_posr = dAllocPosr();
  dSetZero (g->offset_posr->pos,4);
  dRSetIdentity (g->offset_posr->R);
  
  g->gflags |= GEOM_POSR_BAD;
}

void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  CHECK_NOT_LOCKED (g->parent_space);
  if (!g->offset_posr) 
  {
	dGeomCreateOffset(g);
  }
  g->offset_posr->pos[0] = x;
  g->offset_posr->pos[1] = y;
  g->offset_posr->pos[2] = z;
  dGeomMoved (g);
}

void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R)
{
  dAASSERT (g && R);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  CHECK_NOT_LOCKED (g->parent_space);
  if (!g->offset_posr) 
  {
	dGeomCreateOffset (g);
  }
  memcpy (g->offset_posr->R,R,sizeof(dMatrix3));
  dGeomMoved (g);
}

void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat)
{
  dAASSERT (g && quat);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  CHECK_NOT_LOCKED (g->parent_space);
  if (!g->offset_posr) 
  {
	dGeomCreateOffset (g);
  }
  dQtoR (quat, g->offset_posr->R);
  dGeomMoved (g);
}

void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  CHECK_NOT_LOCKED (g->parent_space);
  if (!g->offset_posr) 
  {
	dGeomCreateOffset(g);
  }
  dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos);
  dGeomMoved (g);
}

void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R)
{
  dAASSERT (g && R);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  CHECK_NOT_LOCKED (g->parent_space);
  if (!g->offset_posr) 
  {
	dGeomCreateOffset (g);
  }
  g->recomputePosr();
  
  dxPosR new_final_posr;
  memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
  memcpy(new_final_posr.R, R, sizeof(dMatrix3));
  
  getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
  dGeomMoved (g);
}

void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat)
{
  dAASSERT (g && quat);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  dUASSERT (g->body, "geom must be on a body");  
  CHECK_NOT_LOCKED (g->parent_space);
  if (!g->offset_posr) 
  {
	dGeomCreateOffset (g);
  }

  g->recomputePosr();
  
  dxPosR new_final_posr;
  memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
  dQtoR (quat, new_final_posr.R);
  
  getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
  dGeomMoved (g);
}

void dGeomClearOffset(dxGeom *g)
{
  dAASSERT (g);
  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
  if (g->offset_posr)
  {
    dIASSERT(g->body);
    // no longer need an offset posr
	dFreePosr(g->offset_posr);
	g->offset_posr = 0;
    // the geom will now share the position of the body
    dFreePosr(g->final_posr);
    g->final_posr = &g->body->posr;
    // geom has moved
    g->gflags &= ~GEOM_POSR_BAD;
    dGeomMoved (g);
  }
}

int dGeomIsOffset(dxGeom *g)
{
  dAASSERT (g);
  return ((0 != g->offset_posr) ? 1 : 0);
}

static const dVector3 OFFSET_POSITION_ZERO = { 0.0f, 0.0f, 0.0f, 0.0f };

const dReal * dGeomGetOffsetPosition (dxGeom *g)
{
  dAASSERT (g);
  if (g->offset_posr)
  {
    return g->offset_posr->pos;
  }
  return OFFSET_POSITION_ZERO;
}

static const dMatrix3 OFFSET_ROTATION_ZERO = 
{ 
	1.0f, 0.0f, 0.0f, 0.0f, 
	0.0f, 1.0f, 0.0f, 0.0f, 
	0.0f, 0.0f, 1.0f, 0.0f, 
};

const dReal * dGeomGetOffsetRotation (dxGeom *g)
{
  dAASSERT (g);
  if (g->offset_posr)
  {
    return g->offset_posr->R;
  }
  return OFFSET_ROTATION_ZERO;
}

void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result)
{
  dAASSERT (g);
  if (g->offset_posr)
  {
    dRtoQ (g->offset_posr->R, result);
  }
  else
  {
    dSetZero (result,4);
    result[0] = 1;
  }
}

//****************************************************************************
// here is where we deallocate any memory that has been globally
// allocated, or free other global resources.

void dCloseODE()
{
  colliders_initialized = 0;
  num_user_classes = 0;
  dClearPosrCache();
}