one more for the gipper

1 files changed, 218 insertions, 0 deletions

diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
new file mode 100644
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 <ode/common.h>
+#include <ode/geom.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include <ode/memory.h>
+#include <ode/misc.h>
+#include <ode/objects.h>
+#include <ode/matrix.h>
+#include <ode/GeomTransformGroup.h>
+#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<dxGeom*> 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<dxGeom*>(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();
+}