https://wiki.robotika.sk/robowiki/index.php?title=Source_4&feed=atom&action=history
Source 4 - Revision history
2024-03-28T22:39:54Z
Revision history for this page on the wiki
MediaWiki 1.30.0
https://wiki.robotika.sk/robowiki/index.php?title=Source_4&diff=1652&oldid=prev
Admin: import from robotika.sk
2006-11-30T04:54:56Z
<p>import from robotika.sk</p>
<p><b>New page</b></p><div><pre><nowiki><br />
#include <ode/ode.h><br />
#include <drawstuff/drawstuff.h><br />
<br />
#ifdef _MSC_VER<br />
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints<br />
#endif <br />
<br />
// select correct drawing functions<br />
<br />
#ifdef dDOUBLE<br />
#define dsDrawBox dsDrawBoxD<br />
#define dsDrawSphere dsDrawSphereD<br />
#define dsDrawCylinder dsDrawCylinderD<br />
#define dsDrawCapsule dsDrawCapsuleD<br />
#define dsDrawLine dsDrawLineD<br />
#define dsDrawTriangle dsDrawTriangleD<br />
#endif<br />
<br />
<br />
// some constants<br />
<br />
#define NUM 1000 // max number of objects<br />
#define DENSITY_ROTOR (4.5) // density of rotor<br />
#define DENSITY_AIR (1.2) // density of air<br />
#define MAX_CONTACTS 1024 // maximum number of contact points per body<br />
#define NEW_PER_STEP 10 // must be less then NUM<br />
#define TIMESTEP 0.01<br />
#define MOLECULE_SIZE 0.005<br />
#define INITIAL_FORCE_Y 0.0001<br />
<br />
// dynamics and collision objects<br />
<br />
struct MyObject {<br />
dBodyID body; // the body<br />
dGeomID geom; // geometries representing this body<br />
<br />
// Trimesh only - double buffered matrices for 'last transform' setup<br />
};<br />
<br />
static int num=0; // number of objects in simulation<br />
static int nextobj=0; // next object to recycle if num==NUM<br />
static dWorldID world;<br />
static dSpaceID space;<br />
static MyObject obj[NUM];<br />
static dJointGroupID contactgroup;<br />
static int selected = -1; // selected object<br />
static int show_aabb = 0; // show geom AABBs?<br />
static int show_contacts = 0; // show contact points?<br />
static int random_pos = 1; // drop objects from random position?<br />
<br />
static long int stepp = 0;<br />
<br />
// Bunny mesh ripped from Opcode<br />
const int VertexCount = 74; <br />
const int IndexCount = 18 * 6 * 3;<br />
<br />
typedef dReal dVector3R[3];<br />
<br />
static dGeomID TriMesh1;<br />
static dGeomID TriMesh2;<br />
static dGeomID List1;<br />
static dGeomID List2;<br />
static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data<br />
<br />
static dGeomID Capsule;<br />
static dGeomID blade1_geom;<br />
static dGeomID blade2_geom;<br />
static dGeomID axis_geom;<br />
<br />
static dBodyID rotor;<br />
<br />
<br />
static dJointID lozisko;<br />
<br />
float Vertices[VertexCount * 3] = {<br />
REAL(-0.275), REAL(0.0), REAL(0.1),<br />
REAL(-0.275), REAL(0.0), REAL(0.0),<br />
REAL(-0.275), REAL(0.0), REAL(-0.1),<br />
REAL(-0.25), REAL(0.0), REAL(0.15),<br />
REAL(-0.25), REAL(0.0), REAL(0.05),<br />
REAL(-0.25), REAL(0.0), REAL(-0.05),<br />
REAL(-0.25), REAL(0.0), REAL(-0.15),<br />
REAL(-0.225), REAL(0.0), REAL(0.1),<br />
REAL(-0.225), REAL(0.0), REAL(0.0),<br />
REAL(-0.225), REAL(0.0), REAL(-0.1),<br />
REAL(-0.2), REAL(0.0), REAL(0.15),<br />
REAL(-0.2), REAL(0.0), REAL(0.05),<br />
REAL(-0.2), REAL(0.0), REAL(-0.05),<br />
REAL(-0.2), REAL(0.0), REAL(-0.15),<br />
REAL(-0.175), REAL(0.0), REAL(0.1),<br />
REAL(-0.175), REAL(0.0), REAL(0.0),<br />
REAL(-0.175), REAL(0.0), REAL(-0.1),<br />
REAL(-0.15), REAL(0.0), REAL(0.05),<br />
REAL(-0.15), REAL(0.0), REAL(-0.05),<br />
REAL(-0.125), REAL(0.0), REAL(0.1),<br />
REAL(-0.125), REAL(0.0), REAL(0.0),<br />
REAL(-0.125), REAL(0.0), REAL(-0.1),<br />
REAL(-0.1), REAL(0.0), REAL(0.15),<br />
REAL(-0.1), REAL(0.0), REAL(0.05),<br />
REAL(-0.1), REAL(0.0), REAL(-0.05),<br />
REAL(-0.1), REAL(0.0), REAL(-0.15),<br />
REAL(-0.075), REAL(0.0), REAL(0.1),<br />
REAL(-0.075), REAL(0.0), REAL(0.0),<br />
REAL(-0.075), REAL(0.0), REAL(-0.1),<br />
REAL(-0.05), REAL(0.0), REAL(0.15),<br />
REAL(-0.05), REAL(0.0), REAL(0.05),<br />
REAL(-0.05), REAL(0.0), REAL(-0.05),<br />
REAL(-0.05), REAL(0.0), REAL(-0.15),<br />
REAL(-0.025), REAL(0.0), REAL(0.1),<br />
REAL(-0.025), REAL(0.0), REAL(0.0),<br />
REAL(-0.025), REAL(0.0), REAL(-0.1),<br />
REAL(0.0), REAL(0.0), REAL(0.05),<br />
REAL(0.0), REAL(0.0), REAL(-0.05),<br />
REAL(0.025), REAL(0.0), REAL(0.1),<br />
REAL(0.025), REAL(0.0), REAL(0.0),<br />
REAL(0.025), REAL(0.0), REAL(-0.1),<br />
REAL(0.05), REAL(0.0), REAL(0.15),<br />
REAL(0.05), REAL(0.0), REAL(0.05),<br />
REAL(0.05), REAL(0.0), REAL(-0.05),<br />
REAL(0.05), REAL(0.0), REAL(-0.15),<br />
REAL(0.075), REAL(0.0), REAL(0.1),<br />
REAL(0.075), REAL(0.0), REAL(0.0),<br />
REAL(0.075), REAL(0.0), REAL(-0.1),<br />
REAL(0.1), REAL(0.0), REAL(0.15),<br />
REAL(0.1), REAL(0.0), REAL(0.05),<br />
REAL(0.1), REAL(0.0), REAL(-0.05),<br />
REAL(0.1), REAL(0.0), REAL(-0.15),<br />
REAL(0.125), REAL(0.0), REAL(0.1),<br />
REAL(0.125), REAL(0.0), REAL(0.0),<br />
REAL(0.125), REAL(0.0), REAL(-0.1),<br />
REAL(0.15), REAL(0.0), REAL(0.05),<br />
REAL(0.15), REAL(0.0), REAL(-0.05),<br />
REAL(0.175), REAL(0.0), REAL(0.1),<br />
REAL(0.175), REAL(0.0), REAL(0.0),<br />
REAL(0.175), REAL(0.0), REAL(-0.1),<br />
REAL(0.2), REAL(0.0), REAL(0.15),<br />
REAL(0.2), REAL(0.0), REAL(0.05),<br />
REAL(0.2), REAL(0.0), REAL(-0.05),<br />
REAL(0.2), REAL(0.0), REAL(-0.15),<br />
REAL(0.225), REAL(0.0), REAL(0.1),<br />
REAL(0.225), REAL(0.0), REAL(0.0),<br />
REAL(0.225), REAL(0.0), REAL(-0.1),<br />
REAL(0.25), REAL(0.0), REAL(0.15),<br />
REAL(0.25), REAL(0.0), REAL(0.05),<br />
REAL(0.25), REAL(0.0), REAL(-0.05),<br />
REAL(0.25), REAL(0.0), REAL(-0.15),<br />
REAL(0.275), REAL(0.0), REAL(0.1),<br />
REAL(0.275), REAL(0.0), REAL(0.0),<br />
REAL(0.275), REAL(0.0), REAL(-0.1),<br />
};<br />
<br />
int Indices[IndexCount / 3][3] = {<br />
{0, 3, 7},<br />
{10, 7, 3},<br />
{7, 10, 14},<br />
{7, 14, 11},<br />
{4, 7, 11},<br />
{0, 7, 4},<br />
<br />
{1, 4, 8},<br />
{8, 4, 11},<br />
{8, 11, 15},<br />
{8, 15, 12},<br />
{5, 8, 12},<br />
{5, 1, 8},<br />
<br />
{2, 5, 9},<br />
{9, 5, 12},<br />
{9, 12, 16},<br />
{9, 16, 13},<br />
{6, 9, 13},<br />
{6, 2, 9},<br />
<br />
{11, 14, 17},<br />
{17, 14, 19},<br />
{17, 19, 23},<br />
{17, 23, 20},<br />
{15, 17, 20},<br />
{15, 11, 17},<br />
<br />
{12, 15, 18},<br />
{18, 15, 20},<br />
{18, 20, 24},<br />
{18, 24, 21},<br />
{16, 18, 21},<br />
{16, 12, 18},<br />
<br />
{19, 22, 26},<br />
{26, 22, 29},<br />
{26, 29, 33},<br />
{26, 33, 30},<br />
{23, 26, 30},<br />
{23, 19, 26},<br />
<br />
{20, 23, 27},<br />
{27, 23, 30},<br />
{27, 30, 34},<br />
{31, 27, 34},<br />
{24, 27, 31},<br />
{24, 20, 27},<br />
<br />
{21, 24, 28},<br />
{28, 24, 31},<br />
{28, 31, 35},<br />
{32, 28, 35},<br />
{25, 28, 32},<br />
{25, 21, 28},<br />
<br />
{30, 33, 36},<br />
{36, 33, 38},<br />
{36, 38, 42},<br />
{39, 36, 42},<br />
{34, 36, 39},<br />
{34, 30, 36},<br />
<br />
{31, 34, 37},<br />
{37, 34, 39},<br />
{37, 39, 43},<br />
{40, 37, 43},<br />
{35, 37, 40},<br />
{35, 31, 37},<br />
<br />
{38, 41, 45},<br />
{45, 41, 48},<br />
{45, 48, 52},<br />
{49, 45, 52},<br />
{42, 45, 49},<br />
{42, 38, 45},<br />
<br />
{39, 42, 46},<br />
{46, 42, 49},<br />
{46, 49, 53},<br />
{50, 46, 53},<br />
{43, 46, 50},<br />
{43, 39, 46},<br />
<br />
{40, 43, 47},<br />
{47, 43, 50},<br />
{47, 50, 54},<br />
{51, 47, 54},<br />
{44, 47, 51},<br />
{44, 40, 47},<br />
<br />
{49, 52, 55},<br />
{55, 52, 57},<br />
{55, 57, 61},<br />
{58, 55, 61},<br />
{53, 55, 58},<br />
{53, 49, 55},<br />
<br />
{50, 53, 56},<br />
{56, 53, 58},<br />
{56, 58, 62},<br />
{59, 56, 62},<br />
{54, 56, 59},<br />
{54, 50, 56},<br />
<br />
{57, 60, 64},<br />
{64, 60, 67},<br />
{64, 67, 71},<br />
{68, 64, 71},<br />
{61, 64, 68},<br />
{61, 57, 64},<br />
<br />
{58, 61, 65},<br />
{65, 61, 68},<br />
{65, 68, 72},<br />
{69, 65, 72},<br />
{62, 65, 69},<br />
{62, 58, 65},<br />
<br />
{59, 62, 66},<br />
{66, 62, 69},<br />
{66, 69, 73},<br />
{70, 66, 73},<br />
{63, 66, 70},<br />
{63, 59, 66}<br />
};<br />
<br />
<br />
// this is called by dSpaceCollide when two objects in space are<br />
// potentially colliding.<br />
<br />
static void nearCallback (void *data, dGeomID o1, dGeomID o2)<br />
{<br />
int i;<br />
//if ((o1 == 0) || (o2 == 0)) return;<br />
//if (o1->body && o2->body) return;<br />
if (dGeomGetClass(o1) == dGeomGetClass(o2)) return;<br />
<br />
// exit without doing anything if the two bodies are connected by a joint<br />
dBodyID b1 = dGeomGetBody(o1);<br />
dBodyID b2 = dGeomGetBody(o2);<br />
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;<br />
<br />
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box<br />
for (i=0; i<MAX_CONTACTS; i++) {<br />
contact[i].surface.mode = dContactBounce | dContactSoftCFM;<br />
contact[i].surface.mu = 0;<br />
contact[i].surface.mu2 = 0;<br />
contact[i].surface.bounce = 0.1;<br />
contact[i].surface.bounce_vel = 0.1;<br />
contact[i].surface.soft_cfm = 0.01;<br />
}<br />
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,<br />
sizeof(dContact))) {<br />
dMatrix3 RI;<br />
dRSetIdentity (RI);<br />
const dReal ss[3] = {0.02,0.02,0.02};<br />
for (i=0; i<numc; i++) {<br />
dJointID c = dJointCreateContact (world,contactgroup,contact+i);<br />
dJointAttach (c,b1,b2);<br />
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);<br />
}<br />
}<br />
}<br />
<br />
<br />
// start simulation - set viewpoint<br />
<br />
static void start()<br />
{<br />
static float xyz[3] = {1,0,1};//{2.1640f,-1.3079f,1.7600f};<br />
static float hpr[3] = {180,0,0};//{125.5000f,-17.0000f,0.0000f};<br />
dsSetViewpoint (xyz,hpr);<br />
printf ("To toggle showing the geom AABBs, press a.\n");<br />
printf ("To toggle showing the contact points, press t.\n");<br />
}<br />
<br />
<br />
char locase (char c)<br />
{<br />
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');<br />
else return c;<br />
}<br />
<br />
<br />
// called when a key pressed<br />
<br />
static void command (int cmd)<br />
{<br />
cmd = locase (cmd);<br />
if (cmd == 'a') {<br />
show_aabb ^= 1;<br />
}<br />
else if (cmd == 't') {<br />
show_contacts ^= 1;<br />
}<br />
}<br />
<br />
<br />
// draw a geom<br />
<br />
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)<br />
{<br />
if (!g) return;<br />
if (!pos) pos = dGeomGetPosition (g);<br />
if (!R) R = dGeomGetRotation (g);<br />
<br />
int type = dGeomGetClass (g);<br />
if (type == dBoxClass) {<br />
dVector3 sides;<br />
dGeomBoxGetLengths (g,sides);<br />
dsDrawBox (pos,R,sides);<br />
}<br />
else if (type == dSphereClass) {<br />
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));<br />
}<br />
else if (type == dCapsuleClass) {<br />
dReal radius,length;<br />
dGeomCapsuleGetParams (g,&radius,&length);<br />
dsDrawCapsule (pos,R,length,radius);<br />
}<br />
else if (type == dGeomTransformClass) {<br />
dGeomID g2 = dGeomTransformGetGeom (g);<br />
const dReal *pos2 = dGeomGetPosition (g2);<br />
const dReal *R2 = dGeomGetRotation (g2);<br />
dVector3 actual_pos;<br />
dMatrix3 actual_R;<br />
dMULTIPLY0_331 (actual_pos,R,pos2);<br />
actual_pos[0] += pos[0];<br />
actual_pos[1] += pos[1];<br />
actual_pos[2] += pos[2];<br />
dMULTIPLY0_333 (actual_R,R,R2);<br />
drawGeom (g2,actual_pos,actual_R,0);<br />
} <br />
else if (type == dTriMeshClass) {<br />
int* Indices = (int*)::Indices;<br />
<br />
for (int ii = 0; ii < IndexCount / 3; ii++) {<br />
const dReal v[9] = { // explicit conversion from float to dReal<br />
Vertices[Indices[ii * 3 + 0] * 3 + 0],<br />
Vertices[Indices[ii * 3 + 0] * 3 + 1],<br />
Vertices[Indices[ii * 3 + 0] * 3 + 2],<br />
Vertices[Indices[ii * 3 + 1] * 3 + 0],<br />
Vertices[Indices[ii * 3 + 1] * 3 + 1],<br />
Vertices[Indices[ii * 3 + 1] * 3 + 2],<br />
Vertices[Indices[ii * 3 + 2] * 3 + 0],<br />
Vertices[Indices[ii * 3 + 2] * 3 + 1],<br />
Vertices[Indices[ii * 3 + 2] * 3 + 2]<br />
};<br />
dsDrawTriangle(pos, R, &v[0], &v[3], &v[6], 0);<br />
}<br />
}<br />
if (show_aabb) {<br />
// draw the bounding box for this geom<br />
dReal aabb[6];<br />
dGeomGetAABB (g,aabb);<br />
dVector3 bbpos;<br />
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);<br />
dVector3 bbsides;<br />
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];<br />
dMatrix3 RI;<br />
dRSetIdentity (RI);<br />
dsSetColorAlpha (1,0,0,0.5);<br />
dsDrawBox (bbpos,RI,bbsides);<br />
}<br />
}<br />
<br />
<br />
// simulation loop<br />
<br />
static void simLoop (int pause)<br />
{<br />
dsSetColor (1,0,0);<br />
dSpaceCollide (space,0,&nearCallback);<br />
<br />
printf("%ld\t", stepp++);<br />
<br />
static int start = 500;<br />
int i,k;<br />
dMass m;<br />
dMatrix3 R;<br />
const dReal* a;<br />
dRFromAxisAndAngle (R, 0.0, 0.0, 0.0, 0);<br />
<br />
if (start > 0) start--;<br />
if (!start && !pause)<br />
{<br />
if (num >= NUM - NEW_PER_STEP - 1)<br />
start = -1;<br />
for (k = 0; k < NEW_PER_STEP; k++)<br />
{<br />
if (obj[num+k].body) // this case should be rare<br />
{<br />
continue;<br />
}<br />
obj[num+k].body = dBodyCreate (world);<br />
<br />
dBodySetPosition (obj[num+k].body, dRandReal()-0.5, -1.0 + (dRandReal()-0.5)/10, dRandReal()+0.5);<br />
<br />
dMassSetSphere (&m,DENSITY_AIR, MOLECULE_SIZE);<br />
obj[num+k].geom = dCreateSphere (space, MOLECULE_SIZE);<br />
dGeomSetBody (obj[num+k].geom, obj[num+k].body);<br />
<br />
dBodySetMass (obj[num+k].body,&m);<br />
dBodyAddRelForceAtRelPos (obj[num+k].body, 0.0, INITIAL_FORCE_Y, 0.0, 0.0, 0.0, 0.0);<br />
}<br />
num += NEW_PER_STEP;<br />
}<br />
dsSetColor (1,1,0);<br />
dsSetTexture (DS_WOOD);<br />
for (int i=0; i<num; i++) {<br />
if (obj[i].geom) {<br />
a = dGeomGetPosition (obj[i].geom);<br />
// restart particles flying out of rotor space<br />
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))<br />
{<br />
// particles are flying in a loop when it reaches 0.2 then is restarted around -0.2 with new position<br />
dBodySetPosition (obj[i].body, dRandReal()-0.5, -0.5 + (dRandReal()-0.5)/10,dRandReal()+0.5);<br />
dBodySetLinearVel (obj[i].body, 0.0, 0.0, 0.0);<br />
dBodySetRotation (obj[i].body, R);<br />
dBodyAddRelForceAtRelPos (obj[i].body, 0.0, INITIAL_FORCE_Y, 0.0, 0.0, 0.0, 0.0);<br />
}<br />
// checking the force affecting particles<br />
//a = dBodyGetForce (obj[i].body);<br />
//if (a[0] || a[1] || a[2])<br />
// printf ("obj[%d].body-force = [%5.4f, %5.4f, %5.4f]\n", i, a[0], a[1], a[2]);<br />
<br />
drawGeom (obj[i].geom,0,0,show_aabb);<br />
}<br />
}<br />
dsSetColor (0,0,1);<br />
drawGeom (blade1_geom, 0, 0, show_aabb);<br />
drawGeom (blade2_geom, 0, 0, show_aabb);<br />
drawGeom (axis_geom, 0, 0, show_aabb);<br />
<br />
if (!pause) dWorldStep (world, TIMESTEP);<br />
// if (!pause) dWorldStepFast1 (world,TIMESTEP, 5);<br />
<br />
// remove all contact joints<br />
dJointGroupEmpty (contactgroup);<br />
<br />
}<br />
<br />
<br />
int main (int argc, char **argv)<br />
{<br />
// setup pointers to drawstuff callback functions<br />
dsFunctions fn;<br />
fn.version = DS_VERSION;<br />
fn.start = &start;<br />
fn.step = &simLoop;<br />
fn.command = &command;<br />
fn.stop = 0;<br />
fn.path_to_textures = "../../drawstuff/textures";<br />
if(argc==2)<br />
{<br />
fn.path_to_textures = argv[1];<br />
}<br />
<br />
// create world<br />
<br />
world = dWorldCreate();<br />
<br />
space = dSimpleSpaceCreate(0);<br />
contactgroup = dJointGroupCreate (0);<br />
dWorldSetGravity (world,0,0,0);<br />
dWorldSetCFM (world,1e-5);<br />
dCreatePlane (space,0,0,1,0);<br />
<br />
memset (obj,0,sizeof(obj));<br />
<br />
// note: can't share tridata if intending to trimesh-trimesh collide<br />
TriData1 = dGeomTriMeshDataCreate();<br />
dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (int*)&Indices[0], IndexCount, 3 * sizeof(int));<br />
TriData2 = dGeomTriMeshDataCreate();<br />
dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (int*)&Indices[0], IndexCount, 3 * sizeof(int));<br />
<br />
TriMesh1 = dCreateTriMesh(0, TriData1, 0, 0, 0);<br />
TriMesh2 = dCreateTriMesh(0, TriData2, 0, 0, 0);<br />
<br />
{<br />
dMatrix3 R;<br />
dMass m;<br />
dMass m2;<br />
dMassSetZero (&m);<br />
<br />
// rotor body - comprises 3 GeomTransforms<br />
rotor = dBodyCreate (world);<br />
<br />
// List1 = dCreateBox (0, 0.5, 0.1, 0.001);<br />
//blade1 of Rotor - encapsulating geom<br />
blade1_geom = dCreateGeomTransform (space);<br />
dGeomTransformSetCleanup (blade1_geom,1);<br />
dMassSetBox (&m2,DENSITY_ROTOR,0.5, 0.001, 0.1/*rozmery kocky s rovnakym objemom ako vsetky trojuholniky trimeshu s hrubkou napr. 0.1*/);<br />
dGeomTransformSetGeom (blade1_geom,TriMesh1);<br />
<br />
// set the transformation (adjust the mass too)<br />
dGeomSetPosition (TriMesh1,0.25,0.0,0.0);<br />
dMassTranslate (&m2,0.0,0.0,0.0 );<br />
dMatrix3 Rtx;<br />
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, M_PI / 4);<br />
dGeomSetRotation (TriMesh1,Rtx);<br />
dMassRotate (&m2,Rtx);<br />
// add to the total mass<br />
dMassAdd (&m,&m2);<br />
<br />
// List2 = dCreateBox (0, 0.5, 0.1, 0.001);<br />
//blade2 of Rotor - encapsulating geom<br />
blade2_geom = dCreateGeomTransform (space);<br />
dGeomTransformSetCleanup (blade2_geom,1);<br />
dMassSetBox (&m2,DENSITY_ROTOR,0.5, 0.001, 0.1/*rozmery kocky s rovnakym objemom ako vsetky trojuholniky trimeshu s hrubkou napr. 0.1*/);<br />
dGeomTransformSetGeom (blade2_geom,TriMesh2);<br />
<br />
// set the transformation (adjust the mass too)<br />
dGeomSetPosition (TriMesh2,-0.25,0.0,0.0); <br />
dMassTranslate (&m2,0.0,0.0,0.0 );<br />
<br />
dRFromAxisAndAngle (Rtx, 0.0,0.0,1.0, M_PI);<br />
dQuaternion q1;<br />
dRtoQ (Rtx, q1);<br />
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, M_PI / 4);<br />
dQuaternion q2;<br />
dRtoQ (Rtx, q2);<br />
dQuaternion qr;<br />
dQMultiply0 (qr, q1, q2);<br />
dQtoR (qr, Rtx);<br />
<br />
dGeomSetRotation (TriMesh2,Rtx);<br />
dMassRotate (&m2,Rtx);<br />
// add to the total mass<br />
dMassAdd (&m,&m2);<br />
<br />
//Capsule - axis of rotor - encapsulated geom<br />
Capsule = dCreateCapsule (0, 0.05, 0.1);<br />
<br />
//Axis of Rotor - encapsulating geom<br />
axis_geom = dCreateGeomTransform (space);<br />
dGeomTransformSetCleanup (axis_geom,1);<br />
dMassSetCapsule (&m2, DENSITY_ROTOR, 1, 0.05, 0.1);<br />
dGeomTransformSetGeom (axis_geom,Capsule);<br />
<br />
// set the transformation (adjust the mass too)<br />
dGeomSetPosition (Capsule,0.0, 0.0, 0.0);<br />
dMassTranslate (&m2,0.0, 0.0, 0.0 );<br />
dRFromAxisAndAngle (Rtx, 0.0, 0.0, 0.0, M_PI / 2);<br />
dGeomSetRotation (Capsule,Rtx);<br />
dMassRotate (&m2,Rtx);<br />
// add to the total masss<br />
dMassAdd (&m,&m2);<br />
<br />
// move all encapsulated objects so that the center of mass is (0,0,0)<br />
<br />
dGeomSetPosition (TriMesh1,<br />
0.25-m.c[0], <br />
-m.c[1],<br />
-m.c[2]);<br />
dGeomSetPosition (TriMesh2,<br />
-0.25-m.c[0],<br />
-m.c[1],<br />
-m.c[2]);<br />
dGeomSetPosition (Capsule,<br />
-m.c[0],<br />
-m.c[1],<br />
-m.c[2]);<br />
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);<br />
<br />
if (blade1_geom) dGeomSetBody (blade1_geom,rotor);<br />
if (blade2_geom) dGeomSetBody (blade2_geom,rotor);<br />
if (axis_geom) dGeomSetBody (axis_geom,rotor);<br />
<br />
dBodySetMass (rotor, &m);<br />
dRFromAxisAndAngle (Rtx, 1.0, 0.0, 0.0, M_PI / 2);<br />
dBodySetRotation(rotor, Rtx);<br />
dBodySetPosition(rotor, 0.0, 0.0, 1.0);<br />
<br />
//joint with static environment - center of rotor<br />
lozisko = dJointCreateHinge (world, 0);<br />
dJointAttach (lozisko, 0, rotor);<br />
dJointSetHingeAnchor (lozisko, 0.0, 0.0, 1.0);<br />
dJointSetHingeAxis (lozisko, 0.0, 1.0, 0.0);<br />
}<br />
<br />
// run simulation<br />
dsSimulationLoop (argc,argv,652,488,&fn);<br />
<br />
dJointGroupDestroy (contactgroup);<br />
dSpaceDestroy (space);<br />
dWorldDestroy (world);<br />
<br />
return 0;<br />
}<br />
</nowiki></pre></div>
Admin