--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/collision_transform.cpp Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,209 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 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. *
+ * *
+ *************************************************************************/
+
+/*
+
+geom transform
+
+*/
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_transform.h"
+#include "collision_util.h"
+
+//****************************************************************************
+// dxGeomTransform class
+
+struct dxGeomTransform : public dxGeom {
+ dxGeom *obj; // object that is being transformed
+ int cleanup; // 1 to destroy obj when destroyed
+ int infomode; // 1 to put Tx geom in dContactGeom g1
+
+ // cached final object transform (body tx + relative tx). this is set by
+ // computeAABB(), and it is valid while the AABB is valid.
+ dxPosR transform_posr;
+
+ dxGeomTransform (dSpaceID space);
+ ~dxGeomTransform();
+ void computeAABB();
+ void computeFinalTx();
+};
+
+
+dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1)
+{
+ type = dGeomTransformClass;
+ obj = 0;
+ cleanup = 0;
+ infomode = 0;
+ dSetZero (transform_posr.pos,4);
+ dRSetIdentity (transform_posr.R);
+}
+
+
+dxGeomTransform::~dxGeomTransform()
+{
+ if (obj && cleanup) delete obj;
+}
+
+
+void dxGeomTransform::computeAABB()
+{
+ if (!obj) {
+ dSetZero (aabb,6);
+ return;
+ }
+
+ // backup the relative pos and R pointers of the encapsulated geom object
+ dxPosR* posr_bak = obj->final_posr;
+
+ // compute temporary pos and R for the encapsulated geom object
+ computeFinalTx();
+ obj->final_posr = &transform_posr;
+
+ // compute the AABB
+ obj->computeAABB();
+ memcpy (aabb,obj->aabb,6*sizeof(dReal));
+
+ // restore the pos and R
+ obj->final_posr = posr_bak;
+}
+
+
+// utility function for dCollideTransform() : compute final pos and R
+// for the encapsulated geom object
+
+void dxGeomTransform::computeFinalTx()
+{
+ dMULTIPLY0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos);
+ transform_posr.pos[0] += final_posr->pos[0];
+ transform_posr.pos[1] += final_posr->pos[1];
+ transform_posr.pos[2] += final_posr->pos[2];
+ dMULTIPLY0_333 (transform_posr.R,final_posr->R,obj->final_posr->R);
+}
+
+//****************************************************************************
+// collider function:
+// this collides a transformed geom with another geom. the other geom can
+// also be a transformed geom, but this case is not handled specially.
+
+int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
+ dContactGeom *contact, int skip)
+{
+
+
+ dxGeomTransform *tr = (dxGeomTransform*) o1;
+ if (!tr->obj) return 0;
+
+
+ // backup the relative pos and R pointers of the encapsulated geom object,
+ // and the body pointer
+ dxPosR *posr_bak = tr->obj->final_posr;
+ dxBody *bodybak = tr->obj->body;
+
+ // compute temporary pos and R for the encapsulated geom object.
+ // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag,
+ // because computeFinalTx() will have already been called in
+ // dxGeomTransform::computeAABB()
+
+ if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx();
+ tr->obj->final_posr = &tr->transform_posr;
+ tr->obj->body = o1->body;
+
+ // do the collision
+ int n = dCollide (tr->obj,o2,flags,contact,skip);
+
+ // if required, adjust the 'g1' values in the generated contacts so that
+ // thay indicated the GeomTransform object instead of the encapsulated
+ // object.
+ if (tr->infomode) {
+ for (int i=0; i<n; i++) {
+ dContactGeom *c = CONTACT(contact,skip*i);
+ c->g1 = o1;
+ }
+ }
+
+ // restore the pos, R and body
+ tr->obj->final_posr = posr_bak;
+ tr->obj->body = bodybak;
+ return n;
+}
+
+//****************************************************************************
+// public API
+
+EXPORT_C dGeomID dCreateGeomTransform (dSpaceID space)
+{
+ return new dxGeomTransform (space);
+}
+
+
+EXPORT_C void dGeomTransformSetGeom (dGeomID g, dGeomID obj)
+{
+
+ dxGeomTransform *tr = (dxGeomTransform*) g;
+ if (tr->obj && tr->cleanup) delete tr->obj;
+ tr->obj = obj;
+}
+
+
+EXPORT_C dGeomID dGeomTransformGetGeom (dGeomID g)
+{
+
+ dxGeomTransform *tr = (dxGeomTransform*) g;
+ return tr->obj;
+}
+
+
+EXPORT_C void dGeomTransformSetCleanup (dGeomID g, int mode)
+{
+
+ dxGeomTransform *tr = (dxGeomTransform*) g;
+ tr->cleanup = mode;
+}
+
+
+EXPORT_C int dGeomTransformGetCleanup (dGeomID g)
+{
+
+ dxGeomTransform *tr = (dxGeomTransform*) g;
+ return tr->cleanup;
+}
+
+
+EXPORT_C void dGeomTransformSetInfo (dGeomID g, int mode)
+{
+
+ dxGeomTransform *tr = (dxGeomTransform*) g;
+ tr->infomode = mode;
+}
+
+
+EXPORT_C int dGeomTransformGetInfo (dGeomID g)
+{
+
+ dxGeomTransform *tr = (dxGeomTransform*) g;
+ return tr->infomode;
+}