219 lines
7.4 KiB
C++
219 lines
7.4 KiB
C++
|
|
/* ************************************************************************ */
|
|
/*
|
|
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();
|
|
}
|