gmueller
/
bluecore
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
bluecore/ode/src/collision_kernel.cpp

1092 lines
27 KiB
C++
Raw Permalink Blame History

/*************************************************************************
* *
* 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"
#if dTRIMESH_GIMPACT
#include <GIMPACT/gimpact.h>
#endif
#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 (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);
#if dTRIMESH_ENABLED
setCollider (dTriMeshClass,dSphereClass,&dCollideSTL);
setCollider (dTriMeshClass,dBoxClass,&dCollideBTL);
setCollider (dTriMeshClass,dRayClass,&dCollideRTL);
setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL);
setCollider (dTriMeshClass,dCapsuleClass,&dCollideCCTL);
setCollider (dTriMeshClass,dPlaneClass,&dCollideTrimeshPlane);
setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh);
#endif
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);
}
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;
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
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 (b == NULL || (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;
}
void dGeomCopyPosition(dxGeom *g, dVector3 pos)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
g->recomputePosr();
const dReal* src = g->final_posr->pos;
pos[0] = src[0];
pos[1] = src[1];
pos[2] = src[2];
}
const dReal * dGeomGetRotation (dxGeom *g)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
g->recomputePosr();
return g->final_posr->R;
}
void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
g->recomputePosr();
const dReal* src = g->final_posr->R;
R[0] = src[0];
R[1] = src[1];
R[2] = src[2];
R[4] = src[4];
R[5] = src[5];
R[6] = src[6];
R[8] = src[8];
R[9] = src[9];
R[10] = src[10];
}
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;
}
void dGeomCopyOffsetPosition (dxGeom *g, dVector3 pos)
{
dAASSERT (g);
if (g->offset_posr)
{
const dReal* src = g->offset_posr->pos;
pos[0] = src[0];
pos[1] = src[1];
pos[2] = src[2];
}
else
{
pos[0] = 0;
pos[1] = 0;
pos[2] = 0;
}
}
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 dGeomCopyOffsetRotation (dxGeom *g, dMatrix3 R)
{
dAASSERT (g);
if (g->offset_posr)
{
const dReal* src = g->final_posr->R;
R[0] = src[0];
R[1] = src[1];
R[2] = src[2];
R[4] = src[4];
R[5] = src[5];
R[6] = src[6];
R[8] = src[8];
R[9] = src[9];
R[10] = src[10];
}
else
{
R[0] = OFFSET_ROTATION_ZERO[0];
R[1] = OFFSET_ROTATION_ZERO[1];
R[2] = OFFSET_ROTATION_ZERO[2];
R[4] = OFFSET_ROTATION_ZERO[4];
R[5] = OFFSET_ROTATION_ZERO[5];
R[6] = OFFSET_ROTATION_ZERO[6];
R[8] = OFFSET_ROTATION_ZERO[8];
R[9] = OFFSET_ROTATION_ZERO[9];
R[10] = OFFSET_ROTATION_ZERO[10];
}
}
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;
}
}
//****************************************************************************
// initialization and shutdown routines - allocate and initialize data,
// cleanup before exiting
extern void opcode_collider_cleanup();
void dInitODE()
{
#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
gimpact_init();
#endif
}
void dCloseODE()
{
colliders_initialized = 0;
num_user_classes = 0;
dClearPosrCache();
#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
gimpact_terminate();
#endif
#if dTRIMESH_ENABLED && dTRIMESH_OPCODE
// Free up static allocations in opcode
opcode_collider_cleanup();
#endif
}
Reference in New Issue View Git Blame Copy Permalink