https://wiki.robotika.sk/robowiki/index.php?title=Source_2&feed=atom&action=history
Source 2 - Revision history
2024-03-29T10:21:51Z
Revision history for this page on the wiki
MediaWiki 1.30.0
https://wiki.robotika.sk/robowiki/index.php?title=Source_2&diff=1650&oldid=prev
Admin: import from robotika.sk
2006-11-30T04:53:36Z
<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<br />
#define TIMESTEP 0.005<br />
#define MOLECULE_SIZE 0.01<br />
#define INITIAL_FORCE_Y 0.001<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 />
// dReal matrix_dblbuff[ 16 * 2 ];<br />
// int last_matrix_index;<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 = 1; // show contact points?<br />
static int random_pos = 1; // drop objects from random position?<br />
<br />
// Bunny mesh ripped from Opcode<br />
const int VertexCount = 3; //452<br />
const int IndexCount = 1 * 3; // 901 * 3<br />
<br />
typedef dReal dVector3R[3];<br />
<br />
static dGeomID List1;<br />
static dGeomID List2;<br />
static dGeomID Capsule;<br />
<br />
static dGeomID blade1_geom;<br />
static dGeomID blade2_geom;<br />
static dGeomID axis_geom;<br />
<br />
static dBodyID rotor;<br />
<br />
static dJointID lozisko;<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->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 | dContactSoftERP | dContactSoftCFM;<br />
contact[i].surface.mu = 0;//dInfinity;<br />
contact[i].surface.mu2 = 0;//dInfinity;<br />
contact[i].surface.bounce = 1;<br />
contact[i].surface.bounce_vel = 0.0;<br />
contact[i].surface.soft_cfm = 0.01;<br />
contact[i].surface.soft_erp = 0.2;<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 />
<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 />
<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 />
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)<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, -1.0 + (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 />
{<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.1, 0.001);<br />
dGeomTransformSetGeom (blade1_geom,List1);<br />
<br />
// set the transformation (adjust the mass too)<br />
dGeomSetPosition (List1,0.25,0.0,0.0);<br />
dMassTranslate (&m2,0.25,0.0,0.0 );<br />
dMatrix3 Rtx;<br />
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, M_PI / 4);<br />
dGeomSetRotation (List1,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.1, 0.001);<br />
dGeomTransformSetGeom (blade2_geom,List2);<br />
<br />
// set the transformation (adjust the mass too)<br />
dGeomSetPosition (List2,-0.25,0.0,0.0); <br />
dMassTranslate (&m2,-0.25,0.0,0.0 );<br />
dRFromAxisAndAngle (Rtx, 1.0,0.0,0.0, -M_PI / 4);<br />
dGeomSetRotation (List2,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 (List1/*TriMes1*/,<br />
0.25-m.c[0],<br />
-m.c[1],<br />
-m.c[2]);<br />
dGeomSetPosition (List2/*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