The following is example code that would be used to determine collision in a complicated envirnment. In this case there is a large array of pendulums disrupted by a falling wood block.
This is a simulation for only one set of objects, you can imagine how complex this would get if you had an entire environment to provide reactons for.
/*
Copyright MathEngine PLC 2000
www.mathengine.com
$Name: AUTO_TEMP_MeTKMakeRelease_1_299 $
$Id: ManyPendulums.c,v 1.16 2000/08/31 13:52:45 lyndonh Exp $
*/
/*
This example shows the collision detection, Dynamics Event Manager
and Kea Dynamics working on a complicated environment.
*/
#pragma warning( disable : 4305 )
#include "MePrecision.h"
#include "McdFrame.h"
#include "McdPrimitives.h"
#include "McdDtBridge.h"
#include "Mdt.h"
#include "MeMath.h"
#include "MeViewer.h"
extern struct MeMemoryAPI MeMemoryAPIMalloc;
struct MeMemoryOptions opts;
/* Global declarations */
/* Number of pendulums down one side of grid */
#define NSIDE 20
#define NPENDULUMS (NSIDE * NSIDE)
#define penRadius (MeReal)0.5
#define penHeight (MeReal)2.0
#define penJoint (MeReal)5.0
#define penDensity (MeReal)8.0
#define penSpacing (MeReal)3.0
/* MeReal penRadius = (MeReal)0.5; */
/* MeReal penHeight = (MeReal)2.0; */
/* MeReal penJoint = (MeReal)5.0; */
/* MeReal penDensity = (MeReal)8.0; */
/* MeReal penSpacing = (MeReal)3.0; */
MeReal thingDensity = 1.0;
MeVector3 thingDim = {3.8, 3.8, 3.8};
MeVector3 thingStart = {0.8, 20.0, 0.01};
/* color of pendulums in the corners */
MeVector3 topLeftColor = {1, 0, 0};
MeVector3 topRightColor = {1, 1, 0};
MeVector3 bottomLeftColor = {0, 1, 0};
MeVector3 bottomRightColor = {0, 0, 1};
/* Force applied to the box to drag it around */
MeReal thingForce = 1000;
/* World for the Dynamics Toolkit simulation */
MdtWorldID world;
/* Collision */
McdDtBridge *cdHandler;
McdSpaceID space;
/* Kea solver workspace */
void *KeaMemoryArea;
/* Physics representations */
MdtBodyID thing;
MdtBodyID pen[NPENDULUMS];
MdtBSJointID joint[NPENDULUMS];
/* Graphical representations */
RGraphic *groundG;
RGraphic *thingG;
RGraphic *penG[NPENDULUMS];
RGraphic *lineG[NPENDULUMS];
/* Collision reps */
McdModelID groundCM;
McdModelID thingCM;
McdModelID penCM[NPENDULUMS];
McdGeometryID plane_prim, box_prim, pen_prim[NPENDULUMS];
MeReal gravity[3] = { 0, -10, 0 };
/* Render context */
RRender *rc;
/* Timestep size */
MeReal step = (MeReal)(0.04);
MeReal groundTransform[4][4] =
{
{1, 0, 0, 0},
{0, 0, -1, 0},
{0, 1, 0, 0},
{0, -1, 0, 1}
};
MeReal groundRenderTransform[16] =
{
1, 0, 0, 0,
0, 0, -1, 0,
0, 1, 0, 0,
0, -1.5, 0, 1
};
/* Functions to add forces to block to drag it around. */
void increaseXForce(void)
{
MdtBodyEnable(thing);
MdtBodyAddForce(thing, thingForce, 0, 0);
}
void decreaseXForce(void)
{
MdtBodyEnable(thing);
MdtBodyAddForce(thing, -thingForce, 0, 0);
}
void increaseZForce(void)
{
MdtBodyEnable(thing);
MdtBodyAddForce(thing, 0, 0, thingForce);
}
void decreaseZForce(void)
{
MdtBodyEnable(thing);
MdtBodyAddForce(thing, 0, 0, -thingForce);
}
void ThingCameraTrack()
{
MeVector3 pos;
MdtBodyGetPosition(thing, pos);
rc->m_cameraLookAt[0] = pos[0];
rc->m_cameraLookAt[1] = pos[1];
rc->m_cameraLookAt[2] = pos[2];
RUpdateCamera();
}
/*
Tick() is a callback function called from the renderer's main loop
to evolve the world by 'step' seconds
*/
void tick(RRender * rc)
{
/*
These timer calls are for the OpenGL performance bar.
*/
RStopTimer(kRRender);
/* Update collision */
RStartTimer(kRCollision);
McdDtBridgeUpdateAllFreezeFlags(space);
McdPairHandlerUpdate();
RStopTimer(kRCollision);
/* Update dynamics */
RStartTimer(kRDynamics);
MdtWorldStep(world, step);
RStopTimer(kRDynamics);
RStartTimer(kRRender);
ThingCameraTrack();
}
/*
Reset boxes and pendulums to initial positions
*/
void reset(void)
{
int i,j;
/* Offset to start making pendulums from */
MeReal start = (NSIDE-1) * -penSpacing * (MeReal)0.5;
MdtBodySetPosition(thing, thingStart[0], thingStart[1], thingStart[2]);
MdtBodySetQuaternion(thing, 1, 0, 0, 0);
MdtBodySetLinearVelocity(thing, 0, 0, 0);
MdtBodySetAngularVelocity(thing, 0, 0, 0);
MdtBodyEnable(thing);
for (i = 0; i < NSIDE; i++)
{
for(j=0; j < NSIDE; j++)
{
MdtBodySetPosition(pen[(i*NSIDE)+j], start + i * penSpacing, penHeight, start + j * penSpacing);
MdtBodySetQuaternion(pen[(i*NSIDE)+j], 1, 0, 0, 0);
MdtBodySetLinearVelocity(pen[(i*NSIDE)+j], 0, 0, 0);
MdtBodySetAngularVelocity(pen[(i*NSIDE)+j], 0, 0, 0);
MdtBodyEnable(pen[(i*NSIDE)+j]);
}
}
}
void cleanup(void)
{
int i;
McdGeometryDestroy(plane_prim);
McdModelDestroy(groundCM);
McdGeometryDestroy(box_prim);
McdModelDestroy(thingCM);
for (i = 0; i < NPENDULUMS; i++)
{
McdGeometryDestroy(pen_prim[i]);
McdModelDestroy(penCM[i]);
}
McdSpaceDestroy(space);
McdDtBridgeDestroy(cdHandler);
McdDtBridgeTerm();
McdTerm();
MdtWorldDestroy(world);
free(KeaMemoryArea);
RDeleteRenderContext(rc);
}
/*
Main Routine
*/
int main(int argc, const char **argv)
{
const RRenderType render = RParseRenderType(&argc, &argv);
#ifndef PS2
static char *help[] =
{
"Left Mouse Drag - move camera",
" Enter - reset",
" w,a,s,d - drag block around"
};
#else
static char *help[] =
{
" Start - reset",
" Directional pad - drag block around"
};
#endif
const int helpNum = sizeof (help) / sizeof (help[0]);
int i, j, k, id;
float color[3];
short material1;
MeReal mass, start;
MeMatrix3 I;
AcmeReal lineStart[3] = { 0.0, penRadius, 0.0};
AcmeReal lineEnd[3] = { 0.0, penJoint - penHeight, 0.0};
MeVector3 tempColorTop, tempColorBottom;
MeReal propI, propJ;
MdtContactParamsID props;
if (render == kRD3D) {
MeInfo(0,"Direct3D not supported for this example.\n");
exit(1);
}
MeMemorySetDefaults(&opts);
(*MeMemoryAPI.setOptions)(&opts);
/*
Initialise dynamics
*/
KeaMemoryArea = malloc(2000000);
world = MdtWorldCreate(NPENDULUMS + 1, NPENDULUMS + 100, KeaMemoryArea, 2000000);
MdtWorldSetGravity(world, gravity[0], gravity[1], gravity[2]);
/*
In this demo, a lot of objects are stationary a lot of the time.
To speed things up we tell Mdt to AutoDisable objects when they
come to rest. Disabled MdtBodies are automatically re-Enabled
when an Enabled body contacts them.
*/
MdtWorldSetDEMMode(world, MdtDEMModePartitionAutoDisable);
/*
THING:
*/
thing = MdtBodyCreate(world);
mass = thingDensity * thingDim[0] * thingDim[1] * thingDim[2];
MdtMakeInertiaTensorBox(mass, thingDim[0], thingDim[1], thingDim[2], I);
MdtBodySetMass(thing, mass);
MdtBodySetInertiaTensor(thing, I);
/*
Add a little (angular)velocity damping
*/
MdtBodySetAngularVelocityDamping(thing, 0.03);
MdtBodySetLinearVelocityDamping(thing, 0.02);
/*
PENDULUMS:
*/
mass = PI * penRadius * penRadius * penDensity;
MdtMakeInertiaTensorSphere(mass, penRadius, I);
for (i = 0; i < NPENDULUMS; i++)
{
pen[i] = MdtBodyCreate(world);
MdtBodySetMass(pen[i], mass);
MdtBodySetInertiaTensor(pen[i], I);
MdtBodySetLinearVelocityDamping(pen[i], (MeReal)(0.2));
MdtBodySetAngularVelocityDamping(pen[i], (MeReal)(0.1));
}
/* reset pendulums to correct positions.. */
reset();
/* offset to start making pendulums from */
start = (NSIDE-1) * -penSpacing * (MeReal)0.5;
/* now make all the joints */
for (i = 0; i < NSIDE; i++)
{
for(j=0; j < NSIDE; j++)
{
joint[(i*NSIDE)+j] = MdtBSJointCreate(world, pen[(i*NSIDE)+j], 0);
MdtBSJointSetPosition(joint[(i*NSIDE)+j], start + i * penSpacing, penJoint, start + j * penSpacing);
MdtBSJointEnable(joint[(i*NSIDE)+j]);
}
}
/*
Collision detection
*/
McdInit(McdPrimitivesGetTypeCount());
McdPrimitivesRegisterTypes();
McdPrimitivesRegisterInteractions();
McdDtBridgeInit(0);
/* max objects and pairs */
space = McdSpaceAxisSortCreate(McdAllAxes, NPENDULUMS + 2, 150);
McdPairHandlerRegisterSpace(space);
cdHandler = McdDtBridgeCreate();
/* max pairs at a time */
McdDtBridgeSetPairListMaxCount(cdHandler, 150);
/*
Set parameters for contacts.
*/
material1 = McdDtBridgeGetDefaultMaterialID();
props = McdDtBridgeGetContactParams(material1, material1);
MdtContactParamsSetType(props, MdtContactTypeFriction2D);
MdtContactParamsSetFriction(props, 2.0);
MdtContactParamsSetRestitution(props, 0.3);
/*
Only use 3 contacts per pair.
*/
id = McdIntersectGetDefaultRequestID();
McdIntersectRequestSetContactMaxCount(id, id, 3);
plane_prim = McdPlaneCreate();
groundCM = McdModelCreate(plane_prim);
McdSpaceInsertModel(space, groundCM);
McdDtBridgeSetBody(cdHandler, groundCM, 0);
McdModelSetTransformPtr(groundCM, groundTransform);
McdModelFreezeInSpace(groundCM);
box_prim = McdBoxCreate(thingDim[0],
thingDim[1], thingDim[2]);
thingCM = McdModelCreate(box_prim);
McdDtBridgeSetBody(cdHandler, thingCM, thing);
McdSpaceInsertModel(space, thingCM);
for (i = 0; i < NPENDULUMS; i++)
{
pen_prim[i] = McdBoxCreate(2 * penRadius,
2 * penRadius, 2 * penRadius);
penCM[i] = McdModelCreate(pen_prim[i]);
McdDtBridgeSetBody(cdHandler, penCM[i], pen[i]);
McdSpaceInsertModel(space, penCM[i]);
}
McdSpaceBuild(space);
/*
Initialise rendering attributes
*/
rc = RNewRenderContext(render, kRQualitySmooth);
rc->m_cameraOffset = 30;
RUpdateCamera();
/*
GROUND:
*/
color[0] = 0.0f;
color[1] = 0.75f;
color[2] = 0.1f;
groundG =
RCreateCube(rc, 84.0f, 84.0f, 0.5f, color,
groundRenderTransform);
#ifndef PS2
RSetTexture(groundG, "checkerboard");
#endif
color[0] = 0.1;
color[1] = 0.1;
color[2] = 0.8;
thingG =
RCreateCube(rc, thingDim[0], thingDim[1], thingDim[2],
color, MdtBodyGetTransformPtr(thing));
#ifndef PS2
RSetTexture(thingG, "wood1");
#endif
/* make graphics (pretty colours) */
for (i = 0; i < NSIDE; i++)
{
for(j=0; j < NSIDE; j++)
{
propI = (MeReal)i/((MeReal)NSIDE-1);
propJ = (MeReal)j/((MeReal)NSIDE-1);
for(k=0; k<3; k++)
{
tempColorTop[k] =
(propI * (topRightColor[k] - topLeftColor[k])) + topLeftColor[k];
tempColorBottom[k] =
(propI * (bottomRightColor[k] - bottomLeftColor[k])) + bottomLeftColor[k];
color[k] =
(propJ * (tempColorTop[k] - tempColorBottom[k])) + tempColorBottom[k];
}
penG[(i*NSIDE)+j] =
RCreateCube(rc, 2*penRadius, 2*penRadius, 2*penRadius,
color, MdtBodyGetTransformPtr(pen[(i*NSIDE)+j]));
lineG[(i*NSIDE)+j] =
RCreateLine(rc, lineStart, lineEnd,
color, MdtBodyGetTransformPtr(pen[(i*NSIDE)+j]));
}
}
/*
KEYS:
*/
#ifndef PS2
RUseKey('w', increaseZForce);
RUseKey('s', decreaseZForce);
RUseKey('a', decreaseXForce);
RUseKey('d', increaseXForce);
RUseKey('\r', reset);
#else
RUsePadKey(PADLup, increaseZForce);
RUsePadKey(PADLdown, decreaseZForce);
RUsePadKey(PADLright, decreaseXForce);
RUsePadKey(PADLleft, increaseXForce);
RUsePadKey(PADstart, reset);
#endif
/* selects a fixed-width font */
RSetOGLFont(3);
RCreateUserHelp(help, helpNum);
rc->m_title = "ManyPendulums example";
/*
Cleanup after simulation.
*/
#ifndef PS2
atexit(cleanup);
#endif
/*
Run the Simulation.
*/
/*
RRun() executes the main loop.
Pseudocode: while no exit-request { Handle user input call Tick() to
evolve the simulation and update graphic transforms Draw graphics }
*/
RRun(rc, tick);
return 0;
}