308 lines
7.7 KiB
C++
308 lines
7.7 KiB
C++
/*************************************************************************
|
|
* *
|
|
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
|
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of EITHER: *
|
|
* (1) The GNU Lesser General Public License as published by the Free *
|
|
* Software Foundation; either version 2.1 of the License, or (at *
|
|
* your option) any later version. The text of the GNU Lesser *
|
|
* General Public License is included with this library in the *
|
|
* file LICENSE.TXT. *
|
|
* (2) The BSD-style license that is included with this library in *
|
|
* the file LICENSE-BSD.TXT. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
|
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
|
* *
|
|
*************************************************************************/
|
|
|
|
// Convex demo.
|
|
// Serves as a test for the convex geometry.
|
|
// By Bram Stolk.
|
|
|
|
#include <assert.h>
|
|
#ifdef HAVE_UNISTD_H
|
|
# include <unistd.h>
|
|
#endif
|
|
#include <ode/ode.h>
|
|
#include <drawstuff/drawstuff.h>
|
|
#include "texturepath.h"
|
|
|
|
#include "halton235_geom.h"
|
|
|
|
#ifdef dDOUBLE
|
|
# define dsDrawConvex dsDrawConvexD
|
|
# define dsDrawLine dsDrawLineD
|
|
#endif
|
|
|
|
|
|
#ifdef _MSC_VER
|
|
# pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
|
|
#endif
|
|
|
|
|
|
// Height at which we drop the composite block.
|
|
const dReal H=4.20;
|
|
|
|
static dWorldID world;
|
|
static dSpaceID space;
|
|
|
|
static dBodyID mbody;
|
|
|
|
static dBodyID hbody[ halton_numc ];
|
|
static dGeomID hgeom[ halton_numc ];
|
|
|
|
static dJointGroupID contactgroup;
|
|
|
|
static bool drawpos=false;
|
|
static bool solidkernel=false;
|
|
|
|
|
|
// this is called by dSpaceCollide when two objects in space are
|
|
// potentially colliding.
|
|
|
|
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
|
|
{
|
|
assert(o1);
|
|
assert(o2);
|
|
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
|
|
{
|
|
// colliding a space with something
|
|
dSpaceCollide2(o1,o2,data,&nearCallback);
|
|
// Note we do not want to test intersections within a space,
|
|
// only between spaces.
|
|
return;
|
|
}
|
|
|
|
const int N = 32;
|
|
dContact contact[N];
|
|
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
|
|
if (n > 0)
|
|
{
|
|
for (int i=0; i<n; i++)
|
|
{
|
|
contact[i].surface.slip1 = 0.7;
|
|
contact[i].surface.slip2 = 0.7;
|
|
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
|
|
contact[i].surface.mu = 500.0; // was: dInfinity
|
|
contact[i].surface.soft_erp = 0.50;
|
|
contact[i].surface.soft_cfm = 0.03;
|
|
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
|
|
dJointAttach
|
|
(
|
|
c,
|
|
dGeomGetBody(contact[i].geom.g1),
|
|
dGeomGetBody(contact[i].geom.g2)
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// start simulation - set viewpoint
|
|
|
|
static void start()
|
|
{
|
|
dAllocateODEDataForThread(dAllocateMaskAll);
|
|
static float xyz[3] = {-8,0,5};
|
|
static float hpr[3] = {0.0f,-29.5000f,0.0000f};
|
|
dsSetViewpoint (xyz,hpr);
|
|
fprintf(stderr,"Press SPACE to reset the simulation.\n");
|
|
}
|
|
|
|
|
|
static void reset()
|
|
{
|
|
dQuaternion q;
|
|
dQSetIdentity(q);
|
|
dBodySetPosition(mbody,0,0,0+H);
|
|
dBodySetQuaternion(mbody, q);
|
|
dBodySetLinearVel(mbody, 0,0,0);
|
|
dBodySetAngularVel(mbody, 0,0,0);
|
|
dBodyEnable(mbody);
|
|
for ( int i=0; i<halton_numc; ++i )
|
|
{
|
|
dBodyID body = hbody[i];
|
|
if ( !body ) continue;
|
|
dBodySetPosition(body, halton_pos[i][0], halton_pos[i][1], halton_pos[i][2]+H);
|
|
dBodySetQuaternion(body, q);
|
|
dBodySetLinearVel(body, 0,0,0);
|
|
dBodySetAngularVel(body, 0,0,0);
|
|
dBodyEnable(body);
|
|
}
|
|
}
|
|
|
|
|
|
// called when a key pressed
|
|
|
|
static void command(int cmd)
|
|
{
|
|
switch (cmd)
|
|
{
|
|
case ' ':
|
|
reset();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static void simLoop(int pause)
|
|
{
|
|
double simstep = 1/240.0;
|
|
double dt = dsElapsedTime();
|
|
|
|
int nrofsteps = (int) ceilf(dt/simstep);
|
|
nrofsteps = nrofsteps > 8 ? 8 : nrofsteps;
|
|
|
|
for (int i=0; i<nrofsteps && !pause; i++)
|
|
{
|
|
dSpaceCollide (space,0,&nearCallback);
|
|
dWorldQuickStep (world, simstep);
|
|
dJointGroupEmpty (contactgroup);
|
|
}
|
|
|
|
dsSetColor (1,1,1);
|
|
// Draw the convex objects.
|
|
for ( int i=0; i<halton_numc; ++i )
|
|
{
|
|
dGeomID geom = hgeom[i];
|
|
dBodyID body = dGeomGetBody(geom);
|
|
//const dReal *pos = dBodyGetPosition(body);
|
|
//const dReal *rot = dBodyGetRotation(body);
|
|
const dReal *pos = dGeomGetPosition(geom);
|
|
const dReal *rot = dGeomGetRotation(geom);
|
|
dsDrawConvex
|
|
(
|
|
pos, rot,
|
|
halton_planes[i],
|
|
halton_numf[i],
|
|
halton_verts[i],
|
|
halton_numv[i],
|
|
halton_faces[i]
|
|
);
|
|
}
|
|
|
|
if (drawpos)
|
|
{
|
|
dsSetColor(1,0,0.2);
|
|
dsSetTexture(DS_NONE);
|
|
const dReal l = 0.35;
|
|
for ( int i=0; i<halton_numc; ++i )
|
|
{
|
|
dBodyID body = hbody[i];
|
|
const dReal *pos = dBodyGetPosition(body);
|
|
dReal x0[3] = { pos[0]-l, pos[1], pos[2] };
|
|
dReal x1[3] = { pos[0]+l, pos[1], pos[2] };
|
|
dReal y0[3] = { pos[0], pos[1]-l, pos[2] };
|
|
dReal y1[3] = { pos[0], pos[1]+l, pos[2] };
|
|
dReal z0[3] = { pos[0], pos[1], pos[2]-l };
|
|
dReal z1[3] = { pos[0], pos[1], pos[2]+l };
|
|
dsDrawLine(x0,x1);
|
|
dsDrawLine(y0,y1);
|
|
dsDrawLine(z0,z1);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
int main (int argc, char **argv)
|
|
{
|
|
dMass m;
|
|
|
|
// setup pointers to drawstuff callback functions
|
|
dsFunctions fn;
|
|
fn.version = DS_VERSION;
|
|
fn.start = &start;
|
|
fn.step = &simLoop;
|
|
fn.command = &command;
|
|
fn.stop = 0;
|
|
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
|
|
|
|
// create world
|
|
dInitODE2(0);
|
|
world = dWorldCreate();
|
|
space = dHashSpaceCreate (0);
|
|
dHashSpaceSetLevels(space, -3, 5);
|
|
dCreatePlane(space,0,0,1,0); // Add a ground plane.
|
|
|
|
contactgroup = dJointGroupCreate (0);
|
|
dWorldSetGravity(world,0,0,-9.8);
|
|
dWorldSetQuickStepNumIterations(world, 32);
|
|
dWorldSetContactMaxCorrectingVel(world, 40);
|
|
dWorldSetMaxAngularSpeed(world, 62.8);
|
|
dWorldSetERP(world, 0.7);
|
|
dWorldSetQuickStepW(world, 0.75); // For increased stability.
|
|
|
|
dWorldSetAutoDisableFlag( world, true );
|
|
dWorldSetAutoDisableLinearThreshold( world, 0.01 );
|
|
dWorldSetAutoDisableAngularThreshold( world, 0.03 );
|
|
dWorldSetAutoDisableTime( world, 0.15f );
|
|
|
|
const float kernelrad = 0.7;
|
|
|
|
mbody = dBodyCreate(world);
|
|
dBodySetPosition(mbody, 0,0,0+H);
|
|
dMassSetSphere( &m, 5, kernelrad );
|
|
dBodySetMass( mbody, &m );
|
|
|
|
for (int i=0; i<halton_numc; ++i )
|
|
{
|
|
dGeomID geom = dCreateConvex
|
|
(
|
|
space,
|
|
halton_planes[i],
|
|
halton_numf[i],
|
|
halton_verts[i],
|
|
halton_numv[i],
|
|
halton_faces[i]
|
|
);
|
|
hgeom[i] = geom;
|
|
const dReal x = halton_pos[i][0];
|
|
const dReal y = halton_pos[i][1];
|
|
const dReal z = halton_pos[i][2];
|
|
const dReal dsqr = x*x + y*y + z*z;
|
|
|
|
if ( dsqr < kernelrad*kernelrad && solidkernel )
|
|
{
|
|
dGeomSetBody(geom, mbody);
|
|
dGeomSetOffsetPosition(geom, x,y,z);
|
|
}
|
|
else
|
|
{
|
|
dBodyID body = dBodyCreate(world);
|
|
hbody[i] = body;
|
|
dBodySetPosition(body, x,y,z+H);
|
|
dReal volu = halton_volu[i];
|
|
dReal rad = pow( volu * 3 / (4*M_PI), (1/3.0) );
|
|
dMassSetSphere( &m,5,rad );
|
|
dBodySetMass( body,&m );
|
|
#if 1
|
|
dBodySetLinearDamping (body, 0.0005);
|
|
dBodySetAngularDamping(body, 0.0300);
|
|
#endif
|
|
dGeomSetBody(geom,body);
|
|
}
|
|
}
|
|
|
|
// run simulation
|
|
const int w=1280;
|
|
const int h=720;
|
|
dsSimulationLoop (argc,argv,w,h,&fn);
|
|
|
|
dJointGroupEmpty (contactgroup);
|
|
dJointGroupDestroy (contactgroup);
|
|
dSpaceDestroy (space);
|
|
dWorldDestroy (world);
|
|
dCloseODE();
|
|
return 0;
|
|
}
|
|
|
|
|