Source 2
From RoboWiki
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
#endif
// some constants
#define NUM 1000 // max number of objects
#define DENSITY_ROTOR (4.5) // density of rotor
#define DENSITY_AIR (1.2) // density of air
#define MAX_CONTACTS 1024 // maximum number of contact points per body
#define NEW_PER_STEP 10
#define TIMESTEP 0.005
#define MOLECULE_SIZE 0.01
#define INITIAL_FORCE_Y 0.001
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom; // geometries representing this body
// Trimesh only - double buffered matrices for 'last transform' setup
// dReal matrix_dblbuff[ 16 * 2 ];
// int last_matrix_index;
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 1; // show contact points?
static int random_pos = 1; // drop objects from random position?
// Bunny mesh ripped from Opcode
const int VertexCount = 3; //452
const int IndexCount = 1 * 3; // 901 * 3
typedef dReal dVector3R[3];
static dGeomID List1;
static dGeomID List2;
static dGeomID Capsule;
static dGeomID blade1_geom;
static dGeomID blade2_geom;
static dGeomID axis_geom;
static dBodyID rotor;
static dJointID lozisko;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i;
//if (o1->body && o2->body) return;
if (dGeomGetClass(o1) == dGeomGetClass(o2)) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftERP | dContactSoftCFM;
contact[i].surface.mu = 0;//dInfinity;
contact[i].surface.mu2 = 0;//dInfinity;
contact[i].surface.bounce = 1;
contact[i].surface.bounce_vel = 0.0;
contact[i].surface.soft_cfm = 0.01;
contact[i].surface.soft_erp = 0.2;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
// start simulation - set viewpoint
static void start()
{
static float xyz[3] = {1,0,1};//{2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {180,0,0};//{125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
}
char locase (char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command (int cmd)
{
cmd = locase (cmd);
if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (1,0,0);
dSpaceCollide (space,0,&nearCallback);
static int start = 500;
int i,k;
dMass m;
dMatrix3 R;
const dReal* a;
dRFromAxisAndAngle (R, 0.0, 0.0, 0.0, 0);
if (start > 0) start--;
if (!start)
{
if (num >= NUM - NEW_PER_STEP - 1)
start = -1;
for (k = 0; k < NEW_PER_STEP; k++)
{
if (obj[num+k].body) // this case should be rare
{
continue;
}
obj[num+k].body = dBodyCreate (world);
dBodySetPosition (obj[num+k].body, dRandReal()-0.5, -1.0 + (dRandReal()-0.5)/10, dRandReal()+0.5);
dMassSetSphere (&m,DENSITY_AIR, MOLECULE_SIZE);
obj[num+k].geom = dCreateSphere (space, MOLECULE_SIZE);
dGeomSetBody (obj[num+k].geom, obj[num+k].body);
dBodySetMass (obj[num+k].body,&m);
dBodyAddRelForceAtRelPos (obj[num+k].body, 0.0, INITIAL_FORCE_Y, 0.0, 0.0, 0.0, 0.0);
}
num += NEW_PER_STEP;
}
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
if (obj[i].geom) {
a = dGeomGetPosition (obj[i].geom);
// restart particles flying out of rotor space
if ((a[0] < -0.5) || (a[0] > 0.5) || (a[2] < 0.5) || (a[2] > 1.5) || (a[1] > 0.5) || (a[1] < -1.0))
{
// particles are flying in a loop when it reaches 0.2 then is restarted around -0.2 with new position
dBodySetPosition (obj[i].body, dRandReal()-0.5, -1.0 + (dRandReal()-0.5)/10,dRandReal()+0.5);
dBodySetLinearVel (obj[i].body, 0.0, 0.0, 0.0);
dBodySetRotation (obj[i].body, R);
dBodyAddRelForceAtRelPos (obj[i].body, 0.0, INITIAL_FORCE_Y, 0.0, 0.0, 0.0, 0.0);
}
// checking the force affecting particles
//a = dBodyGetForce (obj[i].body);
//if (a[0] || a[1] || a[2])
// printf ("obj[%d].body-force = [%5.4f, %5.4f, %5.4f]\n", i, a[0], a[1], a[2]);
drawGeom (obj[i].geom,0,0,show_aabb);
}
}
dsSetColor (0,0,1);
drawGeom (blade1_geom, 0, 0, show_aabb);
drawGeom (blade2_geom, 0, 0, show_aabb);
drawGeom (axis_geom, 0, 0, show_aabb);
if (!pause) dWorldStep (world, TIMESTEP);
// if (!pause) dWorldStepFast1 (world,TIMESTEP, 5);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
int main (int argc, char **argv)
{
// 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/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,0);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
{
dMatrix3 R;
dMass m;
dMass m2;
dMassSetZero (&m);
// rotor body - comprises 3 GeomTransforms
rotor = dBodyCreate (world);
List1 = dCreateBox (0, 0.5, 0.1, 0.001);
//blade1 of Rotor - encapsulating geom
blade1_geom = dCreateGeomTransform (space);
dGeomTransformSetCleanup (blade1_geom,1);
dMassSetBox (&m2,DENSITY_ROTOR,0.5,0.1, 0.001);
dGeomTransformSetGeom (blade1_geom,List1);
// set the transformation (adjust the mass too)
dGeomSetPosition (List1,0.25,0.0,0.0);
dMassTranslate (&m2,0.25,0.0,0.0 );
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, M_PI / 4);
dGeomSetRotation (List1,Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
List2 = dCreateBox (0, 0.5, 0.1, 0.001);
//blade2 of Rotor - encapsulating geom
blade2_geom = dCreateGeomTransform (space);
dGeomTransformSetCleanup (blade2_geom,1);
dMassSetBox (&m2,DENSITY_ROTOR,0.5,0.1, 0.001);
dGeomTransformSetGeom (blade2_geom,List2);
// set the transformation (adjust the mass too)
dGeomSetPosition (List2,-0.25,0.0,0.0);
dMassTranslate (&m2,-0.25,0.0,0.0 );
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, -M_PI / 4);
dGeomSetRotation (List2,Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
//Capsule - axis of rotor - encapsulated geom
Capsule = dCreateCapsule (0, 0.05, 0.1);
//Axis of Rotor - encapsulating geom
axis_geom = dCreateGeomTransform (space);
dGeomTransformSetCleanup (axis_geom,1);
dMassSetCapsule (&m2, DENSITY_ROTOR, 1, 0.05, 0.1);
dGeomTransformSetGeom (axis_geom,Capsule);
// set the transformation (adjust the mass too)
dGeomSetPosition (Capsule,0.0, 0.0, 0.0);
dMassTranslate (&m2,0.0, 0.0, 0.0 );
dRFromAxisAndAngle (Rtx, 0.0, 0.0, 0.0, M_PI / 2);
dGeomSetRotation (Capsule,Rtx);
dMassRotate (&m2,Rtx);
// add to the total masss
dMassAdd (&m,&m2);
// move all encapsulated objects so that the center of mass is (0,0,0)
dGeomSetPosition (List1/*TriMes1*/,
0.25-m.c[0],
-m.c[1],
-m.c[2]);
dGeomSetPosition (List2/*TriMesh2*/,
-0.25-m.c[0],
-m.c[1],
-m.c[2]);
dGeomSetPosition (Capsule,
-m.c[0],
-m.c[1],
-m.c[2]);
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
if (blade1_geom) dGeomSetBody (blade1_geom,rotor);
if (blade2_geom) dGeomSetBody (blade2_geom,rotor);
if (axis_geom) dGeomSetBody (axis_geom,rotor);
dBodySetMass (rotor, &m);
dRFromAxisAndAngle (Rtx, 1.0, 0.0, 0.0, M_PI / 2);
dBodySetRotation(rotor, Rtx);
dBodySetPosition(rotor, 0.0, 0.0, 1.0);
//joint with static environment - center of rotor
lozisko = dJointCreateHinge (world, 0);
dJointAttach (lozisko, 0, rotor);
dJointSetHingeAnchor (lozisko, 0.0, 0.0, 1.0);
dJointSetHingeAxis (lozisko, 0.0, 1.0, 0.0);
}
// run simulation
dsSimulationLoop (argc,argv,652,488,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
