one more for the gipper

3 files changed, 395 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();
+}
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h
new file mode 100644
index 0000000..705fdb9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h
@@ -0,0 +1,29 @@
+ 
+/* ************************************************************************ */
+/* 
+   grouped and transformed geometry functions 
+   author: Tim Schmidt tisch@uni-paderborn.de
+*/
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+extern int dGeomTransformGroupClass;
+
+void dGeomTransformGroupSetRelativePosition (dGeomID g, dReal x, dReal y, dReal z);
+void dGeomTransformGroupSetRelativeRotation (dGeomID g, const dMatrix3 R);
+const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g);
+const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g);
+dGeomID dCreateGeomTransformGroup (dSpaceID space);
+void dGeomTransformGroupAddGeom    (dGeomID tg, dGeomID obj);
+void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj);
+dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i);
+int dGeomTransformGroupGetNumGeoms (dGeomID tg);
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt
new file mode 100644
index 0000000..bca0e66
--- /dev/null
+++ b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt
@@ -0,0 +1,148 @@
+README for GeomTransformGroup by Tim Schmidt.
+---------------------------------------------
+
+This is a patch to add the dGeomTransformGroup object to the list of geometry 
+objects.
+
+It should work with the cvs version of the ode library from 07/24/2002.
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+comment by russ smith: this code is easy to use with the rest of ODE.
+simply copy GeomTransformGroup.cpp to ode/src and copy GeomTransformGroup.h
+to include/ode. then add GeomTransformGroup.cpp to the ODE_SRC variable
+in the makefile. rebuild, and you're done! of course i could have done all
+this for you, but i prefer to keep GeomTransformGroup separated from the
+rest of ODE for now while other issues with the collision system are
+resolved.
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+
+Description:
+
+The dGeomTransformGroup is an adaption of the TransformGroup known from 
+Java3D (and maybe other libraries with a similar scene graph representation).
+It can be used to build an arbitrarily structured tree of objects that are 
+each positioned relative to the particular parent node.
+
+If you have a plane for example, there is one root node associated with the 
+plane's body and another three transformgroups placed 'under' this node. One 
+with the fuselage (cappedcylinder) under it and two with the underlying wings 
+(flat boxes). And if you want to add engines, simply put them 'next to' the 
+wings under another two transformgroups.
+
+bodyTG ---> associated with dBody
+    |
+    +--fuselageTG
+    |   |
+    |   +--fuselageCylinder
+    |
+    +--leftwingTG
+    |   |
+    |   +--wingBox
+    |   |
+    |   +--leftengineTG
+    |         |
+    |         +--leftengineCylinder
+    | 
+    +--rightwingTG
+        |
+        +--wingBox
+        |
+        +--rightengineTG
+              |
+              +--rightengineCylinder
+
+This is a method to easily compose objects without the necessity of always 
+calculating global coordinates. But apart from this there is something else 
+that makes dGeomTransformGroups very valuable.
+
+Maybe you remember that some users reported the problem of acquiring the 
+correct bodies to be attached by a contactjoint in the nearCallback when 
+using dGeomGroups and dGeomTransforms at the same time. This results from the 
+fact that dGeomGroups are not associated with bodies while all other 
+geometries are.
+
+So, as you can see in the nearCallback of the the test_buggy demo you have to 
+attach the contactjoint with the bodies that you get from the geometries that 
+are stored in the contact struct (-> dGeomGetBody(contacts[i].geom.g1)). 
+Normally you would do this by asking o1 and o2 directly with dGeomGetBody(o1) 
+and dGeomGetBody(o2) respectively.
+
+As a first approach you can overcome that problem by testing o1 and o2 if 
+they are groups or not to find out how to get the corresponding bodies.
+
+However this will fail if you want grouped transforms that are constructed 
+out of dGeomTransforms encapsulated in a dGeomGroup. According to the test 
+you use contacts[i].geom.g1 to get the right body. Unfortunately g1 is 
+encapsulated in a transform and therefore not attached to any body. In this 
+case the dGeomTransform 'in the middle' would have been the right object to 
+be asked for the body.
+
+You may now conclude that it is a good idea to unwrap the group encapsulated 
+geoms at the beginning of the nearcallback and use dGeomGetBody(o1) 
+consistently. But keep in mind that this also means not to invoke 
+dCollide(..) on groups at all and therefore not to expoit the capability of 
+dGeomGroups to speed up collision detection by the creation of bounding boxes 
+around the encapsulated geometry.
+
+Everything becomes even worse if you create a dGeomTransform that contains a 
+dGeomGroup of geoms. The function that cares about the collision of 
+transforms with other objects uses the position and rotation of the 
+respective encapsulated object to compute its final position and orientation. 
+Unfortunately dGeomGroups do not have a position and rotation, so the result 
+will not be what you have expected.
+
+Here the dGeomTransformGroups comes into operation, because it combines the 
+advantages and capabilities of the dGeomGroup and the dGeomTransform.
+And as an effect of synergy it is now even possible to set the position of a 
+group of geoms with one single command.
+Even nested encapsulations of dGeomTransformGroups in dGeomTransformGroups 
+should be possible (to be honest, I have not tried that so far ;-) ).
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+API:
+
+dGeomID dCreateGeomTransformGroup (dSpaceID space);
+  - create a GeomTransformGroup 
+    
+void dGeomTransformGroupAddGeom    (dGeomID tg, dGeomID obj);
+  - Comparable to dGeomTransformSetGeom or dGeomGroupAdd
+  - add objects to this group
+   
+void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj);
+  - remove objects from this group
+
+void dGeomTransformGroupSetRelativePosition
+            (dGeomID g, dReal x, dReal y, dReal z);
+void dGeomTransformGroupSetRelativeRotation
+            (dGeomID g, const dMatrix3 R);
+  - Comparable to setting the position and rotation of all the
+    dGeomTransform encapsulated geometry. The difference 
+    is that it is global with respect to this group and therefore
+    affects all geoms in this group.
+  - The relative position and rotation are attributes of the 
+    transformgroup, so the position and rotation of the individual
+    geoms are not changed 
+
+const dReal * dGeomTransformGroupGetRelativePosition (dGeomID g);
+const dReal * dGeomTransformGroupGetRelativeRotation (dGeomID g);
+  - get the relative position and rotation
+  
+dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i);
+  - Comparable to dGeomGroupGetGeom
+  - get a specific geom of the group
+  
+int dGeomTransformGroupGetNumGeoms (dGeomID tg);
+  - Comparable to dGeomGroupGetNumGeoms
+  - get the number of geoms in the group
+  
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+Tim Schmidt
+student of computer science
+University of Paderborn, Germany
+tisch@uni-paderborn.de