From 0fc46fc9590912bf6925c899edd02d7a2cdf5f79 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 04:28:53 +0000 Subject: adding ode source to /libraries --- .../GeomTransformGroup/GeomTransformGroup.cpp" | 218 +++++++++++++++++++++ 1 file changed, 218 insertions(+) create mode 100755 "libraries/ode-0.9\\/contrib/GeomTransformGroup/GeomTransformGroup.cpp" (limited to 'libraries/ode-0.9\/contrib/GeomTransformGroup/GeomTransformGroup.cpp') diff --git "a/libraries/ode-0.9\\/contrib/GeomTransformGroup/GeomTransformGroup.cpp" "b/libraries/ode-0.9\\/contrib/GeomTransformGroup/GeomTransformGroup.cpp" new file mode 100755 index 0000000..26b77b0 --- /dev/null +++ "b/libraries/ode-0.9\\/contrib/GeomTransformGroup/GeomTransformGroup.cpp" @@ -0,0 +1,218 @@ + +/* ************************************************************************ */ +/* + grouped and transformed geometry functions + author: Tim Schmidt tisch@uni-paderborn.de +*/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "objects.h" +#include "array.h" +#include "geom_internal.h" + +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + + +// ############################################################################ + +int dGeomTransformGroupClass = -1; +// ############################################################################ + +struct dxGeomTransformGroup { + dArray parts; // all the geoms that make up the group + dVector3 relativePosition; + dMatrix3 relativeRotation; +}; +// ############################################################################ + +void dGeomTransformGroupSetRelativePosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + transformGroup->relativePosition[0] = x; + transformGroup->relativePosition[1] = y; + transformGroup->relativePosition[2] = z; +} +// ############################################################################ + +void dGeomTransformGroupSetRelativeRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + memcpy (transformGroup->relativeRotation,R,sizeof(dMatrix3)); +} +// ############################################################################ + +const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + return transformGroup->relativePosition; +} +// ############################################################################ + +const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + return transformGroup->relativeRotation; +} +// ############################################################################ + +static void computeFinalTransformation (const dxGeom *tg, const dxGeom *part) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(tg); + dMULTIPLY0_331 (part->pos,tg->R,transformGroup->relativePosition); + part->pos[0] += tg->pos[0]; + part->pos[1] += tg->pos[1]; + part->pos[2] += tg->pos[2]; + dMULTIPLY0_333 (part->R,tg->R,transformGroup->relativeRotation); +} +// ############################################################################ + +int dCollideTransformGroup (const dxGeom *o1, const dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(o1); + if (transformGroup->parts.size() == 0) + { + return 0; + } + int numleft = flags & NUMC_MASK; + if (numleft == 0) numleft = 1; + flags &= ~NUMC_MASK; + int num=0, i=0; + while (i < transformGroup->parts.size() && numleft > 0) + { + dUASSERT (transformGroup->parts[i]->spaceid==0, + "GeomTransformGroup encapsulated object must not be in a space"); + dUASSERT (transformGroup->parts[i]->body==0, + "GeomTransformGroup encapsulated object must not be attached to a body"); + if (!o1->space_aabb) + { + computeFinalTransformation (o1, transformGroup->parts[i]); + } + dxBody *bodyBackup = transformGroup->parts[i]->body; + transformGroup->parts[i]->body = o1->body; + int n = dCollide (transformGroup->parts[i],const_cast(o2), + flags | numleft,contact,skip); + transformGroup->parts[i]->body = bodyBackup; + contact = CONTACT (contact,skip*n); + numleft -= n; + num += n; + i++; + } + return num; +} +// ############################################################################ + +static dColliderFn * dGeomTransformGroupColliderFn (int num) +{ + return (dColliderFn *) &dCollideTransformGroup; +} +// ############################################################################ + +static void dGeomTransformGroupAABB (dxGeom *geom, dReal aabb[6]) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom); + aabb[0] = dInfinity; + aabb[1] = -dInfinity; + aabb[2] = dInfinity; + aabb[3] = -dInfinity; + aabb[4] = dInfinity; + aabb[5] = -dInfinity; + int i,j; + for (i=0; i < transformGroup->parts.size(); i++) + { + computeFinalTransformation (geom, transformGroup->parts[i]); + dReal aabb2[6]; + transformGroup->parts[i]->_class->aabb (transformGroup->parts[i],aabb2); + for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j]; + for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j]; + } +} +// ############################################################################ + +static void dGeomTransformGroupDtor (dxGeom *geom) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom); + transformGroup->parts.~dArray(); +} +// ############################################################################ + +dxGeom *dCreateGeomTransformGroup (dSpaceID space) +{ + if (dGeomTransformGroupClass == -1) { + dGeomClass c; + c.bytes = sizeof (dxGeomTransformGroup); + c.collider = &dGeomTransformGroupColliderFn; + c.aabb = &dGeomTransformGroupAABB; + c.aabb_test = 0; + c.dtor = dGeomTransformGroupDtor; + dGeomTransformGroupClass = dCreateGeomClass (&c); + } + dxGeom *g = dCreateGeom (dGeomTransformGroupClass); + if (space) + { + dSpaceAdd (space,g); + } + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + transformGroup->parts.constructor(); + dSetZero (transformGroup->relativePosition,4); + dRSetIdentity (transformGroup->relativeRotation); + return g; +} +// ############################################################################ + +void dGeomTransformGroupAddGeom (dxGeom *g, dxGeom *obj) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + transformGroup->parts.push (obj); +} +// ############################################################################ + +void dGeomTransformGroupRemoveGeom (dxGeom *g, dxGeom *obj) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + for (int i=0; i < transformGroup->parts.size(); i++) { + if (transformGroup->parts[i] == obj) { + transformGroup->parts.remove (i); + return; + } + } +} +// ############################################################################ + +dxGeom * dGeomTransformGroupGetGeom (dxGeom *g, int i) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + dAASSERT (i >= 0 && i < transformGroup->parts.size()); + return transformGroup->parts[i]; +} +// ############################################################################ + +int dGeomTransformGroupGetNumGeoms (dxGeom *g) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + return transformGroup->parts.size(); +} -- cgit v1.1