Difference between revisions of "Source 6"
From RoboWiki
(import from robotika.sk) |
(No difference)
|
Latest revision as of 05:55, 30 November 2006
#include <ode/ode.h>
// some constants
#define NUM 1000 // max number of objects
#define DENSITY_ROTOR (0.1) // density of rotor
#define DENSITY_AIR (10000000.0) // density of air
#define MAX_CONTACTS 256 // maximum number of contact points per body
#define NEW_PER_STEP 100 // must be less then NUM
#define TIMESTEP 0.01
#define MOLECULE_SIZE 0.00001
#define INITIAL_FORCE_Y 0.0001
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom; // geometries representing this body
};
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 long int stepp = 0;
// Bunny mesh ripped from Opcode
const int VertexCount = 74;
const int IndexCount = 18 * 6 * 3;
typedef dReal dVector3R[3];
static dGeomID TriMesh1;
static dGeomID TriMesh2;
static dGeomID List1;
static dGeomID List2;
static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data
static dGeomID Capsule;
static dGeomID blade1_geom;
static dGeomID blade2_geom;
static dGeomID axis_geom;
static dBodyID rotor;
static dJointID lozisko;
float Vertices[VertexCount * 3] = {
REAL(-0.275), REAL(0.0), REAL(0.1),
REAL(-0.275), REAL(0.0), REAL(0.0),
REAL(-0.275), REAL(0.0), REAL(-0.1),
REAL(-0.25), REAL(0.0), REAL(0.15),
REAL(-0.25), REAL(0.0), REAL(0.05),
REAL(-0.25), REAL(0.0), REAL(-0.05),
REAL(-0.25), REAL(0.0), REAL(-0.15),
REAL(-0.225), REAL(0.0), REAL(0.1),
REAL(-0.225), REAL(0.0), REAL(0.0),
REAL(-0.225), REAL(0.0), REAL(-0.1),
REAL(-0.2), REAL(0.0), REAL(0.15),
REAL(-0.2), REAL(0.0), REAL(0.05),
REAL(-0.2), REAL(0.0), REAL(-0.05),
REAL(-0.2), REAL(0.0), REAL(-0.15),
REAL(-0.175), REAL(0.0), REAL(0.1),
REAL(-0.175), REAL(0.0), REAL(0.0),
REAL(-0.175), REAL(0.0), REAL(-0.1),
REAL(-0.15), REAL(0.0), REAL(0.05),
REAL(-0.15), REAL(0.0), REAL(-0.05),
REAL(-0.125), REAL(0.0), REAL(0.1),
REAL(-0.125), REAL(0.0), REAL(0.0),
REAL(-0.125), REAL(0.0), REAL(-0.1),
REAL(-0.1), REAL(0.0), REAL(0.15),
REAL(-0.1), REAL(0.0), REAL(0.05),
REAL(-0.1), REAL(0.0), REAL(-0.05),
REAL(-0.1), REAL(0.0), REAL(-0.15),
REAL(-0.075), REAL(0.0), REAL(0.1),
REAL(-0.075), REAL(0.0), REAL(0.0),
REAL(-0.075), REAL(0.0), REAL(-0.1),
REAL(-0.05), REAL(0.0), REAL(0.15),
REAL(-0.05), REAL(0.0), REAL(0.05),
REAL(-0.05), REAL(0.0), REAL(-0.05),
REAL(-0.05), REAL(0.0), REAL(-0.15),
REAL(-0.025), REAL(0.0), REAL(0.1),
REAL(-0.025), REAL(0.0), REAL(0.0),
REAL(-0.025), REAL(0.0), REAL(-0.1),
REAL(0.0), REAL(0.0), REAL(0.05),
REAL(0.0), REAL(0.0), REAL(-0.05),
REAL(0.025), REAL(0.0), REAL(0.1),
REAL(0.025), REAL(0.0), REAL(0.0),
REAL(0.025), REAL(0.0), REAL(-0.1),
REAL(0.05), REAL(0.0), REAL(0.15),
REAL(0.05), REAL(0.0), REAL(0.05),
REAL(0.05), REAL(0.0), REAL(-0.05),
REAL(0.05), REAL(0.0), REAL(-0.15),
REAL(0.075), REAL(0.0), REAL(0.1),
REAL(0.075), REAL(0.0), REAL(0.0),
REAL(0.075), REAL(0.0), REAL(-0.1),
REAL(0.1), REAL(0.0), REAL(0.15),
REAL(0.1), REAL(0.0), REAL(0.05),
REAL(0.1), REAL(0.0), REAL(-0.05),
REAL(0.1), REAL(0.0), REAL(-0.15),
REAL(0.125), REAL(0.0), REAL(0.1),
REAL(0.125), REAL(0.0), REAL(0.0),
REAL(0.125), REAL(0.0), REAL(-0.1),
REAL(0.15), REAL(0.0), REAL(0.05),
REAL(0.15), REAL(0.0), REAL(-0.05),
REAL(0.175), REAL(0.0), REAL(0.1),
REAL(0.175), REAL(0.0), REAL(0.0),
REAL(0.175), REAL(0.0), REAL(-0.1),
REAL(0.2), REAL(0.0), REAL(0.15),
REAL(0.2), REAL(0.0), REAL(0.05),
REAL(0.2), REAL(0.0), REAL(-0.05),
REAL(0.2), REAL(0.0), REAL(-0.15),
REAL(0.225), REAL(0.0), REAL(0.1),
REAL(0.225), REAL(0.0), REAL(0.0),
REAL(0.225), REAL(0.0), REAL(-0.1),
REAL(0.25), REAL(0.0), REAL(0.15),
REAL(0.25), REAL(0.0), REAL(0.05),
REAL(0.25), REAL(0.0), REAL(-0.05),
REAL(0.25), REAL(0.0), REAL(-0.15),
REAL(0.275), REAL(0.0), REAL(0.1),
REAL(0.275), REAL(0.0), REAL(0.0),
REAL(0.275), REAL(0.0), REAL(-0.1),
};
int Indices[IndexCount / 3][3] = {
{0, 3, 7},
{10, 7, 3},
{7, 10, 14},
{7, 14, 11},
{4, 7, 11},
{0, 7, 4},
{1, 4, 8},
{8, 4, 11},
{8, 11, 15},
{8, 15, 12},
{5, 8, 12},
{5, 1, 8},
{2, 5, 9},
{9, 5, 12},
{9, 12, 16},
{9, 16, 13},
{6, 9, 13},
{6, 2, 9},
{11, 14, 17},
{17, 14, 19},
{17, 19, 23},
{17, 23, 20},
{15, 17, 20},
{15, 11, 17},
{12, 15, 18},
{18, 15, 20},
{18, 20, 24},
{18, 24, 21},
{16, 18, 21},
{16, 12, 18},
{19, 22, 26},
{26, 22, 29},
{26, 29, 33},
{26, 33, 30},
{23, 26, 30},
{23, 19, 26},
{20, 23, 27},
{27, 23, 30},
{27, 30, 34},
{31, 27, 34},
{24, 27, 31},
{24, 20, 27},
{21, 24, 28},
{28, 24, 31},
{28, 31, 35},
{32, 28, 35},
{25, 28, 32},
{25, 21, 28},
{30, 33, 36},
{36, 33, 38},
{36, 38, 42},
{39, 36, 42},
{34, 36, 39},
{34, 30, 36},
{31, 34, 37},
{37, 34, 39},
{37, 39, 43},
{40, 37, 43},
{35, 37, 40},
{35, 31, 37},
{38, 41, 45},
{45, 41, 48},
{45, 48, 52},
{49, 45, 52},
{42, 45, 49},
{42, 38, 45},
{39, 42, 46},
{46, 42, 49},
{46, 49, 53},
{50, 46, 53},
{43, 46, 50},
{43, 39, 46},
{40, 43, 47},
{47, 43, 50},
{47, 50, 54},
{51, 47, 54},
{44, 47, 51},
{44, 40, 47},
{49, 52, 55},
{55, 52, 57},
{55, 57, 61},
{58, 55, 61},
{53, 55, 58},
{53, 49, 55},
{50, 53, 56},
{56, 53, 58},
{56, 58, 62},
{59, 56, 62},
{54, 56, 59},
{54, 50, 56},
{57, 60, 64},
{64, 60, 67},
{64, 67, 71},
{68, 64, 71},
{61, 64, 68},
{61, 57, 64},
{58, 61, 65},
{65, 61, 68},
{65, 68, 72},
{69, 65, 72},
{62, 65, 69},
{62, 58, 65},
{59, 62, 66},
{66, 62, 69},
{66, 69, 73},
{70, 66, 73},
{63, 66, 70},
{63, 59, 66}
};
// 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 == 0) || (o2 == 0)) return;
//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 | dContactSoftCFM;
contact[i].surface.mu = 0;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
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);
}
}
}
int main (int argc, char **argv)
{
int i,k;
dMass m;
dMatrix3 R;
const dReal* a;
dRFromAxisAndAngle (R, 0.0, 0.0, 0.0, 0);
// 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));
// note: can't share tridata if intending to trimesh-trimesh collide
TriData1 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (int*)&Indices[0], IndexCount, 3 * sizeof(int));
TriData2 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (int*)&Indices[0], IndexCount, 3 * sizeof(int));
TriMesh1 = dCreateTriMesh(0, TriData1, 0, 0, 0);
TriMesh2 = dCreateTriMesh(0, TriData2, 0, 0, 0);
{
dMass m2;
dMassSetZero (&m);
// rotor body - comprises 3 GeomTransforms
rotor = dBodyCreate (world);
//blade1 of Rotor - encapsulating geom
blade1_geom = dCreateGeomTransform (space);
dGeomTransformSetCleanup (blade1_geom,1);
dMassSetBox (&m2,DENSITY_ROTOR,0.5, 0.001, 0.1/*rozmery kocky s rovnakym objemom ako vsetky trojuholniky trimeshu s hrubkou napr. 0.1*/);
dGeomTransformSetGeom (blade1_geom,TriMesh1);
// set the transformation (adjust the mass too)
dGeomSetPosition (TriMesh1,0.25,0.0,0.0);
dMassTranslate (&m2,0.0,0.0,0.0 );
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, M_PI / 4);
dGeomSetRotation (TriMesh1,Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
//blade2 of Rotor - encapsulating geom
blade2_geom = dCreateGeomTransform (space);
dGeomTransformSetCleanup (blade2_geom,1);
dMassSetBox (&m2,DENSITY_ROTOR,0.5, 0.001, 0.1/*rozmery kocky s rovnakym objemom ako vsetky trojuholniky trimeshu s hrubkou napr. 0.1*/);
dGeomTransformSetGeom (blade2_geom,TriMesh2);
// set the transformation (adjust the mass too)
dGeomSetPosition (TriMesh2,-0.25,0.0,0.0);
dMassTranslate (&m2,0.0,0.0,0.0 );
dRFromAxisAndAngle (Rtx, 0.0,0.0,1.0, M_PI);
dQuaternion q1;
dRtoQ (Rtx, q1);
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, M_PI / 4);
dQuaternion q2;
dRtoQ (Rtx, q2);
dQuaternion qr;
dQMultiply0 (qr, q1, q2);
dQtoR (qr, Rtx);
dGeomSetRotation (TriMesh2,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 (TriMesh1,
0.25-m.c[0],
-m.c[1],
-m.c[2]);
dGeomSetPosition (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);
}
while (stepp++ < 1000)
{
dSpaceCollide (space,0,&nearCallback);
if (num < NUM - NEW_PER_STEP - 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;
}
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, -0.5 + (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);
}
}
}
dWorldStep (world, TIMESTEP);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
a = dBodyGetAngularVel (rotor);
printf ("Angular velocity of rotor: %f\n", a[1]); // Rotating along Y-axis
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
