Admin: import from robotika.sk

2006-11-30T04:54:56Z

import from robotika.sk

New page

<pre><nowiki>
#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 // must be less then NUM
#define TIMESTEP 0.01
#define MOLECULE_SIZE 0.005
#define INITIAL_FORCE_Y 0.0001

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

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 = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?

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);
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);
}
else if (type == dTriMeshClass) {
int* Indices = (int*)::Indices;

for (int ii = 0; ii < IndexCount / 3; ii++) {
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(pos, R, &v[0], &v[3], &v[6], 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);

printf("%ld\t", stepp++);

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 && !pause)
{
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, -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);
}
// 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));

// 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);

{
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.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);

// 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.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);
}

// run simulation
dsSimulationLoop (argc,argv,652,488,&fn);

dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);

return 0;
}
</nowiki></pre>

Source 4 - Revision history

Admin: import from robotika.sk