From 1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:22:23 +0000 Subject: trying to fix my screwup, please hold on --- libraries/ode-0.9/ode/src/collision_kernel.cpp | 1103 ------------------------ 1 file changed, 1103 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/collision_kernel.cpp (limited to 'libraries/ode-0.9\/ode/src/collision_kernel.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_kernel.cpp b/libraries/ode-0.9/ode/src/collision_kernel.cpp deleted file mode 100644 index b885603..0000000 --- a/libraries/ode-0.9/ode/src/collision_kernel.cpp +++ /dev/null @@ -1,1103 +0,0 @@ -/************************************************************************* - * * - * 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 -#include -#include -#include -#include -#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 -#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 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); -} - - -/* - * 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,"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"); - // 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; inormal[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 -} -- cgit v1.1