Source 2

#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;
}