diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_kernel.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/collision_kernel.cpp | 1247 |
1 files changed, 1247 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_kernel.cpp b/libs/ode-0.16.1/ode/src/collision_kernel.cpp new file mode 100644 index 0000000..527941a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_kernel.cpp @@ -0,0 +1,1247 @@ +/************************************************************************* + * * + * 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/rotation.h> +#include <ode/objects.h> +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_kernel.h" +#include "collision_util.h" +#include "collision_std.h" +#include "collision_transform.h" +#include "collision_trimesh_internal.h" +#include "collision_space_internal.h" +#include "odeou.h" + +#ifdef dLIBCCD_ENABLED +# include "collision_libccd.h" +#endif /* dLIBCCD_ENABLED */ + + +#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() + +#if dATOMICS_ENABLED +static volatile atomicptr s_cachedPosR = 0; // dxPosR * +#endif // dATOMICS_ENABLED + +static inline dxPosR* dAllocPosr() +{ + dxPosR *retPosR; + +#if dATOMICS_ENABLED + retPosR = (dxPosR *)AtomicExchangePointer(&s_cachedPosR, NULL); + + if (!retPosR) +#endif + { + retPosR = (dxPosR*) dAlloc (sizeof(dxPosR)); + } + + return retPosR; +} + +static inline void dFreePosr(dxPosR *oldPosR) +{ +#if dATOMICS_ENABLED + if (!AtomicCompareExchangePointer(&s_cachedPosR, NULL, (atomicptr)oldPosR)) +#endif + { + dFree(oldPosR, sizeof(dxPosR)); + } +} + +/*extern */void dClearPosrCache(void) +{ +#if dATOMICS_ENABLED + // No threads should be accessing ODE at this time already, + // hence variable may be read directly. + dxPosR *existingPosR = (dxPosR *)s_cachedPosR; + + if (existingPosR) + { + dFree(existingPosR, sizeof(dxPosR)); + + s_cachedPosR = 0; + } +#endif +} + +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); +} + +/*extern */void dInitColliders() +{ + dIASSERT(!colliders_initialized); + colliders_initialized = 1; + + memset (colliders,0,sizeof(colliders)); + + int i,j; + + // 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); + setCollider (dConvexClass,dTriMeshClass,&dCollideConvexTrimesh); +#endif + +#ifdef dLIBCCD_BOX_CYL + setCollider (dBoxClass,dCylinderClass,&dCollideBoxCylinderCCD); +#else + setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox); +#endif + setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere); + setCollider (dCylinderClass,dPlaneClass,&dCollideCylinderPlane); + +#ifdef dLIBCCD_CYL_CYL + setCollider (dCylinderClass, dCylinderClass, &dCollideCylinderCylinder); +#endif +#ifdef dLIBCCD_CAP_CYL + setCollider (dCapsuleClass, dCylinderClass, &dCollideCapsuleCylinder); +#endif + + //--> Convex Collision +#ifdef dLIBCCD_CONVEX_BOX + setCollider (dConvexClass, dBoxClass, &dCollideConvexBoxCCD); +#else + setCollider (dConvexClass,dBoxClass,&dCollideConvexBox); +#endif + +#ifdef dLIBCCD_CONVEX_CAP + setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsuleCCD); +#else + setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule); +#endif + +#ifdef dLIBCCD_CONVEX_CYL + setCollider (dConvexClass,dCylinderClass,&dCollideConvexCylinderCCD); +#endif + +#ifdef dLIBCCD_CONVEX_SPHERE + setCollider (dConvexClass,dSphereClass,&dCollideConvexSphereCCD); +#else + setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex); +#endif + +#ifdef dLIBCCD_CONVEX_CONVEX + setCollider (dConvexClass,dConvexClass,&dCollideConvexConvexCCD); +#else + setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex); +#endif + + setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane); + 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); +} + +/*extern */void dFinitColliders() +{ + colliders_initialized = 0; +} + +void dSetColliderOverride (int i, int j, dColliderFn *fn) +{ + dIASSERT( colliders_initialized ); + dAASSERT( i < dGeomNumClasses ); + dAASSERT( j < dGeomNumClasses ); + + colliders[i][j].fn = fn; + colliders[i][j].reverse = 0; + colliders[j][i].fn = fn; + colliders[j][i].reverse = 1; +} + +/* +* NOTE! +* If it is necessary to add special processing mode without contact generation +* use NULL contact parameter value as indicator, not zero in flags. +*/ +int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dAASSERT(o1 && o2 && contact); + dUASSERT(colliders_initialized,"Please call ODE initialization (dInitODE() or similar) before using the library"); + dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number"); + dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number"); + // Even though comparison for greater or equal to one is used in all the + // other places, here it is more logical to check for greater than zero + // because function does not require any specific number of contact slots - + // it must be just a positive. + dUASSERT((flags & NUMC_MASK) > 0, "no contacts requested"); + + // Extra precaution for zero contact count in parameters + if ((flags & NUMC_MASK) == 0) return 0; + // 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) +{ + // 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; + next_ex = 0; + tome_ex = 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(); +} + +unsigned dxGeom::getParentSpaceTLSKind() const +{ + return parent_space ? parent_space->tls_kind : dSPACE_TLS_KIND_INIT_VALUE; +} + +int dxGeom::AABBTest (dxGeom *, dReal [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); +} + +bool dxGeom::controlGeometry(int /*controlClass*/, int /*controlCode*/, void * /*dataValue*/, int *dataSize) +{ + dAASSERT(false && "Control class/code is not supported for current geom"); + + *dataSize = 0; + return false; +} + +//**************************************************************************** +// misc + +dxGeom *dGeomGetBodyNext (dxGeom *geom) +{ + return geom->body_next; +} + +//**************************************************************************** +// public API for geometry objects + +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"); + + return g->buildUpdatedPosition(); +} + + +void dGeomCopyPosition(dxGeom *g, dVector3 pos) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + const dVector3 &src = g->buildUpdatedPosition(); + pos[0] = src[dV3E_X]; + pos[1] = src[dV3E_Y]; + pos[2] = src[dV3E_Z]; +} + + +const dReal * dGeomGetRotation (dxGeom *g) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + return g->buildUpdatedRotation(); +} + + +void dGeomCopyRotation(dxGeom *g, dMatrix3 R) +{ + dAASSERT (g); + dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable"); + + const dMatrix3 &src = g->buildUpdatedRotation(); + R[0] = src[dM3E_XX]; + R[1] = src[dM3E_XY]; + R[2] = src[dM3E_XZ]; + R[4] = src[dM3E_YX]; + R[5] = src[dM3E_YY]; + R[6] = src[dM3E_YZ]; + R[8] = src[dM3E_ZX]; + R[9] = src[dM3E_ZY]; + R[10] = src[dM3E_ZZ]; +} + + +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; +} + + +void dGeomGetRelPointPos (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMultiply0_331 (p,g->final_posr->R,prel); + result[0] = p[0] + g->final_posr->pos[0]; + result[1] = p[1] + g->final_posr->pos[1]; + result[2] = p[2] + g->final_posr->pos[2]; +} + + +void dGeomGetPosRelPoint (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 prel; + prel[0] = px - g->final_posr->pos[0]; + prel[1] = py - g->final_posr->pos[1]; + prel[2] = pz - g->final_posr->pos[2]; + prel[3] = 0; + dMultiply1_331 (result,g->final_posr->R,prel); +} + + +void dGeomVectorToWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMultiply0_331 (result,g->final_posr->R,p); +} + + +void dGeomVectorFromWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result) +{ + dAASSERT (g); + if ((g->gflags & GEOM_PLACEABLE) == 0) { + result[0] = px; + result[1] = py; + result[2] = pz; + return; + } + + g->recomputePosr(); + + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMultiply1_331 (result,g->final_posr->R,p); +} + + + +int dGeomLowLevelControl (dxGeom *g, int controlClass, int controlCode, void *dataValue, int *dataSize) +{ + dAASSERT (g); + dAASSERT (dataSize); + + if (!dataSize) { + return false; + } + + bool result = g->controlGeometry(controlClass, controlCode, dataValue, dataSize); + return result; +} + +//**************************************************************************** +// 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; + setAllColliders (class_number,&dCollideUserGeomWithGeom); + + num_user_classes++; + return class_number; +} + +/*extern */void dFinitUserClasses() +{ + num_user_classes = 0; +} + +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->offset_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; + } +} + + |