void constructWorldForTest (dReal gravity, int bodycount, /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z, /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z, /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z, /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z, /* rotation angles */ dReal a1, dReal a2) { // create world world = dWorldCreate(); dWorldSetERP (world,0.2); dWorldSetCFM (world,1e-6); dWorldSetGravity (world,0,0,gravity); dMass m; dMassSetBox (&m,1,SIDE,SIDE,SIDE); dMassAdjust (&m,MASS); body[0] = dBodyCreate (world); dBodySetMass (body[0],&m); dBodySetPosition (body[0], pos1x, pos1y, pos1z); dQuaternion q; dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1); dBodySetQuaternion (body[0],q); if (bodycount==2) { body[1] = dBodyCreate (world); dBodySetMass (body[1],&m); dBodySetPosition (body[1], pos2x, pos2y, pos2z); dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2); dBodySetQuaternion (body[1],q); } else body[1] = 0; }
void command (int cmd) { // note: 0.0174532925 radians = 1 degree dQuaternion q; dMatrix3 m; switch(cmd) { case 'w': geom1pos[0]+=0.05; break; case 'a': geom1pos[1]-=0.05; break; case 's': geom1pos[0]-=0.05; break; case 'd': geom1pos[1]+=0.05; break; case 'e': geom1pos[2]-=0.05; break; case 'q': geom1pos[2]+=0.05; break; case 'i': dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925); dQMultiply0(geom1quat,geom1quat,q); break; case 'j': dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925); dQMultiply0(geom1quat,geom1quat,q); break; case 'k': dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925); dQMultiply0(geom1quat,geom1quat,q); break; case 'l': dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925); dQMultiply0(geom1quat,geom1quat,q); break; case 'm': (drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME; break; case 'n': (geoms!=convex)? geoms=convex:geoms=boxes; break; default: dsPrint ("received command %d (`%c')\n",cmd,cmd); } #if 0 dGeomSetPosition (geoms[1], geom1pos[0], geom1pos[1], geom1pos[2]); dQtoR (geom1quat, m); dGeomSetRotation (geoms[1],m); #endif DumpInfo=true; }
EXPORT_C void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, dReal angle) { dQuaternion q; dQFromAxisAndAngle (q,ax,ay,az,angle); dQtoR (q,R); }
void Robots::construirChassi(dWorldID world) { for (int i=0; i < 2; i++) { // Cria objeto e geometria this->body[i] = dBodyCreate(world); this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT); // Define a posição do objeto dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i); // Se o robô for do segundo time, deve ser rotacionado em 180 graus if ((this->id == 3) || (this->id == 4) || (this->id == 5)) { dQuaternion q; dQFromAxisAndAngle(q,0,0,1,M_PI); dBodySetQuaternion(this->body[i],q); } // Define a massa do objeto dMass m; dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT); // O segundo bloco é mais curto dMassAdjust(&m,CMASS*(1+i*2)); // O segundo bloco é mais pesado dBodySetMass(this->body[i],&m); // Associa o objeto à sua geometria dGeomSetBody(this->box[i],this->body[i]); } // O chassis é composto por dois blocos que são fixos entre si dJointID fixed = dJointCreateFixed(world,0); dJointAttach(fixed,this->body[1],this->body[0]); dJointSetFixed(fixed); }
Object::Object() { iSize.x = iSize.y = iSize.z = 1; iPosition.x = iPosition.y = iPosition.z = 0; iVel.x = iVel.y =iVel.z = 0; iM = 1; Mat = new Material(); dQFromAxisAndAngle(q,1,0,0,0); data = new BodyData(); data->setName("Object"); };
//you should create your own world void CreateWorld1(Simulator * sim) { const double zdim = 2.0; const double xdim = 20; const double ydim = 20; AddBoundaries(sim, xdim, ydim, zdim, 0.2); dGeomID geom; dGeomID geom_gara1; dGeomID geom_gara2; dQuaternion q; //for rotations // create a parking garage - 1 st wall geom_gara1 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim); dGeomSetPosition(geom_gara1, -13 + 0.5*(30-xdim), -5, 0.5 * zdim); dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1); dGeomSetQuaternion(geom_gara1, q); sim->AddGeometryAsObstacle(geom_gara1); // creat a pakring garage - 2nd wall geom_gara2 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim); dGeomSetPosition(geom_gara2, -13+0.5*(30-xdim), -7.5, 0.5 * zdim); dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1); dGeomSetQuaternion(geom_gara2, q); sim->AddGeometryAsObstacle(geom_gara2); geom = dCreateBox(sim->GetSpaceID(), 9.0, 0.2, zdim); dGeomSetPosition(geom, 6, -4, 0.5 * zdim); dQFromAxisAndAngle(q, 0, 0, 1, -M_PI * 0.25); dGeomSetQuaternion(geom, q); sim->AddGeometryAsObstacle(geom); geom = dCreateSphere(sim->GetSpaceID(), 0.5 * zdim); dGeomSetPosition(geom, 0, -4, 0.5 * zdim); sim->AddGeometryAsObstacle(geom); }
PMoveable::PMoveable(Kinematic &kinematic, float mass, GeomInfo *info) : PObject(info), kinematic(kinematic) { dQuaternion q; body = dBodyCreate(Physics::getInstance().getOdeWorld()); info->createMass(&this->mass, mass); dBodySetMass(body, &this->mass); dGeomSetBody(geom, body); kinematicToOde(); dQFromAxisAndAngle(q, 0, 1, 0, kinematic.orientation); dBodySetQuaternion(body, q); }
static void reset_state(void) { float sx=-4, sy=-4, sz=2; dQuaternion q; dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); #ifdef BOX dBodySetPosition (boxbody, sx, sy+1, sz); dBodySetLinearVel (boxbody, 0,0,0); dBodySetAngularVel (boxbody, 0,0,0); dBodySetQuaternion (boxbody, q); #endif #ifdef CYL dBodySetPosition (cylbody, sx, sy, sz); dBodySetLinearVel (cylbody, 0,0,0); dBodySetAngularVel (cylbody, 0,0,0); dBodySetQuaternion (cylbody, q); #endif }
void dJointSetScrewAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle ) { dxJointScrew* joint = ( dxJointScrew* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Screw ); setAxes( joint, x, y, z, joint->axis1, joint->axis2 ); joint->computeInitialRelativeRotation(); if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle; dQuaternion qAngle, qOffset; dQFromAxisAndAngle(qAngle, x, y, z, dangle); dQMultiply3(qOffset, qAngle, joint->qrel); joint->qrel[0] = qOffset[0]; joint->qrel[1] = qOffset[1]; joint->qrel[2] = qOffset[2]; joint->qrel[3] = qOffset[3]; }
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_TEXTURE_PATH; // create world dInitODE2(0); world = dWorldCreate(); dMass m; dMassSetBox (&m,1,SIDE,SIDE,SIDE); dMassAdjust (&m,MASS); dQuaternion q; dQFromAxisAndAngle (q,1,1,0,0.25*M_PI); body[0] = dBodyCreate (world); dBodySetMass (body[0],&m); dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1); dBodySetQuaternion (body[0],q); body[1] = dBodyCreate (world); dBodySetMass (body[1],&m); dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1); dBodySetQuaternion (body[1],q); hinge = dJointCreateHinge (world,0); dJointAttach (hinge,body[0],body[1]); dJointSetHingeAnchor (hinge,0,0,1); dJointSetHingeAxis (hinge,1,-1,1.41421356); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dWorldDestroy (world); dCloseODE(); return 0; }
Cube::Cube::Cube(bool useVbo) { iSize.x = iSize.y = iSize.z = 1; iPosition.x = iPosition.y = iPosition.z = 0; iVel.x = iVel.y =iVel.z = 0; iRotate.x = iRotate.y = iRotate.z = 0.0f; iM = 1; Mat = new Material(); dQFromAxisAndAngle(q,1,0,0,M_PI*0.5); data = new BodyData(); data->setName("Cube"); vecs = new vec3d[24]; texCoord = new TexCoord2[24]; texture = NULL; vbo = NULL; }
AvatarGameObj::AvatarGameObj(const ORE1::ObjType& obj) : GameObj(obj, Sim::gen_sphere_body(80, 0.5)), // TODO Load mass information from the ORE mission description _xrot_delta(0.0), _zrot_delta(0.0), _ypos_delta(0.0), _ylvel_delta(0.0), _xavel_delta(0.0), _zavel_delta(0.0), _norm_coll_steptime(0), _run_coll_steptime(0), _mesh(MeshAnimation::load("mesh-LIBAvatar")), _attached(false), _attached_this_frame(false) { // TODO Load volume information from the ORE mission description _height = 2.25; _coll_rad = 0.25; // Set up a geom for detecting regular collisions get_entity().set_geom( "physical", dCreateCapsule(Sim::get_dyn_space(), _coll_rad, _height - 2*_coll_rad), std::auto_ptr<CollisionHandler>(new AvatarContactHandler(this)) ); // TODO Maybe some missions start off in upright mode? _uprightness = 0.0; // Set up a geom at our feet to detect when we can run on a surface get_entity().set_geom( "sticky_attach", dCreateRay(Sim::get_dyn_space(), RUNNING_MAX_DELTA_Y_POS*2), std::auto_ptr<CollisionHandler>(new StickyAttachmentContactHandler(this)) ); dQuaternion rdq; dQFromAxisAndAngle(rdq, 1, 0, 0, M_PI_2); dGeomSetOffsetQuaternion(get_entity().get_geom("sticky_attach"), rdq); dGeomDisable(get_entity().get_geom("sticky_attach")); update_geom_offsets(); }
Item* WorldPhysics::generateItem() { Item* temp; switch (qrand()%3) { case 0: { temp=new BoxItem(world,space,3,3,3,.2); } break; case 1: { dQuaternion q; dQFromAxisAndAngle(q,1,0,0,M_PI*0.5); temp = new WheelItem(world,space,q,2,.2); } break; case 2: { if (enable_complex) temp=new ComplexItem(world,space,3,3,3,.2); else temp=new BoxItem(world,space,3,3,3,.3); } break; } temp->state=0; temp->setPosition(qrand()%30+50,qrand()%30+50,qrand()%30+10); return temp; }
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"; // create world world = dWorldCreate(); dMass m; dMassSetBox (&m,1,SIDE,SIDE,SIDE); dMassAdjust (&m,MASS); body[0] = dBodyCreate (world); dBodySetMass (body[0],&m); dBodySetPosition (body[0],0,0,1); body[1] = dBodyCreate (world); dBodySetMass (body[1],&m); dQuaternion q; dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI); dBodySetPosition (body[1],0.2,0.2,1.2); dBodySetQuaternion (body[1],q); slider = dJointCreateSlider (world,0); dJointAttach (slider,body[0],body[1]); dJointSetSliderAxis (slider,1,1,1); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dWorldDestroy (world); return 0; }
void Robots::construirRodas(dWorldID world) { for (int i=0; i < 2; i++) { // Cria objeto e geometria this->wheel[i] = dBodyCreate(world); this->cylinder[i] = dCreateCylinder(0,RADIUS,wTHICK); // Define a posição e orientação do objeto dQuaternion q; dQFromAxisAndAngle(q,1,0,0,M_PI*0.5); dBodySetQuaternion(this->wheel[i],q); dBodySetPosition(this->wheel[i],this->pegarX(),this->pegarY()+(1-2*i)*((WIDTH/2)+(wTHICK/2)),STARTZ); // Define a massa do objeto dMass m; dMassSetCylinder(&m,1,3,RADIUS,wTHICK); dMassAdjust(&m,WMASS); dBodySetMass(this->wheel[i],&m); // Associa o objeto à sua geometria dGeomSetBody(this->cylinder[i],this->wheel[i]); } }
void command (int cmd) { // note: 0.0174532925 radians = 1 degree dQuaternion q; dMatrix3 m; bool changed = false; switch(cmd) { case 'w': geom1pos[0]+=0.05; changed = true; break; case 'a': geom1pos[1]-=0.05; changed = true; break; case 's': geom1pos[0]-=0.05; changed = true; break; case 'd': geom1pos[1]+=0.05; changed = true; break; case 'e': geom1pos[2]-=0.05; changed = true; break; case 'q': geom1pos[2]+=0.05; changed = true; break; case 'i': dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925); dQMultiply0(geom1quat,geom1quat,q); changed = true; break; case 'j': dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925); dQMultiply0(geom1quat,geom1quat,q); changed = true; break; case 'k': dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925); dQMultiply0(geom1quat,geom1quat,q); changed = true; break; case 'l': dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925); dQMultiply0(geom1quat,geom1quat,q); changed = true; break; case 'm': (drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME; break; case 'n': (geoms!=convex)? geoms=convex:geoms=boxes; if(geoms==convex) { printf("CONVEX------------------------------------------------------>\n"); } else { printf("BOX--------------------------------------------------------->\n"); } break; default: dsPrint ("received command %d (`%c')\n",cmd,cmd); } #if 0 dGeomSetPosition (geoms[1], geom1pos[0], geom1pos[1], geom1pos[2]); dQtoR (geom1quat, m); dGeomSetRotation(geoms[1],m); if(changed) { printf("POS: %f,%f,%f\n",geom1pos[0],geom1pos[1],geom1pos[2]); printf("ROT:\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n", m[0],m[1],m[2],m[3], m[4],m[5],m[6],m[7], m[8],m[9],m[10],m[11]); } #endif DumpInfo=true; }
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI) { int i; dMass m; // chassis body body[bodyI] = dBodyCreate (world); dBodySetPosition (body[bodyI],x,y,STARTZ); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m,CMASS/2.0); dBodySetMass (body[bodyI],&m); box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); dGeomSetBody (box[boxI],body[bodyI]); // wheel bodies for (i=1; i<=4; i++) { body[bodyI+i] = dBodyCreate (world); dQuaternion q; dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); dBodySetQuaternion (body[bodyI+i],q); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m,WMASS); dBodySetMass (body[bodyI+i],&m); sphere[sphereI+i-1] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]); } dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5); // front and back wheel hinges for (i=0; i<4; i++) { joint[jointI+i] = dJointCreateHinge2 (world,0); dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]); const dReal *a = dBodyGetPosition (body[bodyI+i+1]); dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1)); dJointSetHinge2Axis2 (joint[jointI+i],0,1,0); dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8); dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[jointI+i],dParamVel2,0); dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX); } //center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint) dBodyID b = dBodyCreate (world); dBodySetPosition (b,x,y,STARTZ+COMOFFSET); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m,CMASS/2.0); dBodySetMass (b,&m); dJointID j = dJointCreateFixed(world, 0); dJointAttach(j, body[bodyI], b); dJointSetFixed(j); //box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT); //dGeomSetBody (box[boxI+1],b); bodyI += 5; jointI += 4; boxI += 1; sphereI += 4; }
Simulator::Simulator(const double posx, const double posy) { m_collision = false; //Open Dynamics Engine stuff m_world = dWorldCreate(); m_space = dHashSpaceCreate(0); m_contacts = dJointGroupCreate(0); m_ground = dCreatePlane(m_space, 0, 0, 1, 0); dGeomSetData(m_ground, (void *) &TYPE_TERRAIN); dWorldSetGravity(m_world, 0, 0, -0.81); //create robot const double LENGTH = 2.50; // chassis length 2.50; const double WIDTH = 1.00; // chassis width const double HEIGHT = 0.40; // chassis height const double RADIUS = 0.45 * WIDTH;//0.45 * WIDTH; // wheel radius const double STARTZ = 0.05; dMass m; dQuaternion q; double car_center_x= posx + 1.5; //1.5 double car_center_y= posy + 5.3; // chassis body m_robotBodyChassis = dBodyCreate(m_world); dBodySetPosition(m_robotBodyChassis, car_center_x, car_center_y, 0.85 * RADIUS + 0.5 * HEIGHT + STARTZ); dMassSetBox(&m, 1, LENGTH, WIDTH, HEIGHT); dBodySetMass(m_robotBodyChassis,&m); // chassis geometry m_robotGeomChassis = dCreateBox(m_space, LENGTH, WIDTH, HEIGHT); dGeomSetBody(m_robotGeomChassis, m_robotBodyChassis); m_robotBodies.push_back(m_robotBodyChassis); dGeomSetData(m_robotGeomChassis, (void *) &TYPE_ROBOT); // wheel bodies for(int i= 0; i < 3; i++) { m_robotBodyWheels[i] = dBodyCreate(m_world); dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5); dBodySetQuaternion(m_robotBodyWheels[i], q); dMassSetSphere(&m, 1, RADIUS); dBodySetMass(m_robotBodyWheels[i], &m); m_robotGeomWheels[i] = dCreateSphere(m_space, RADIUS); dGeomSetBody(m_robotGeomWheels[i], m_robotBodyWheels[i]); m_robotBodies.push_back(m_robotBodyWheels[i]); dGeomSetData(m_robotGeomWheels[i], (void *) &TYPE_ROBOT); } dBodySetPosition(m_robotBodyWheels[0], car_center_x + 0.5 * LENGTH, car_center_y, RADIUS + STARTZ); dBodySetPosition(m_robotBodyWheels[1], car_center_x - 0.5 * LENGTH, car_center_y + 0.5 * WIDTH, RADIUS + STARTZ); dBodySetPosition(m_robotBodyWheels[2], car_center_x - 0.5 * LENGTH, car_center_y - 0.5 * WIDTH, RADIUS + STARTZ); // front and back wheel hinges for (int i = 0; i < 3; i++) { m_robotJoints[i] = dJointCreateHinge2(m_world, 0); // creat hign-2 joint as wheels dJointAttach(m_robotJoints[i], m_robotBodyChassis, m_robotBodyWheels[i]); const dReal *a = dBodyGetPosition(m_robotBodyWheels[i]); dJointSetHinge2Anchor(m_robotJoints[i], a[0], a[1], a[2]); dJointSetHinge2Axis1(m_robotJoints[i], 0, 0, 1); dJointSetHinge2Axis2(m_robotJoints[i], 0, 1, 0); // set joint suspension dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionERP, 0.04); dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionCFM, 0.08); } // lock back wheels along the steering axis for (int i = 1; i < 3; i++) { // set stops to make sure wheels always stay in alignment dJointSetHinge2Param (m_robotJoints[i], dParamLoStop, 0); dJointSetHinge2Param (m_robotJoints[i], dParamHiStop, 0); } }
WorldPhysics::WorldPhysics() { enable_complex=0; bulldozer_state=1; tmp_scalar=0; tmp_wait=0; qsrand(QTime::currentTime().msec()); dInitODE(); world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-9.81); //ground_cheat = dCreatePlane (space,0,0,1,0); wall1=dCreatePlane (space,-1,0,0,-100); wall2=dCreatePlane (space,1,0,0,0); wall3=dCreatePlane (space,0,-1,0,-100); wall4=dCreatePlane (space,0,1,0,0); // our heightfield floor dHeightfieldDataID heightid = dGeomHeightfieldDataCreate(); // Create an finite heightfield. dGeomHeightfieldDataBuildCallback( heightid, NULL, near_heightfield_callback, HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP, REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 ); // Give some very bounds which, while conservative, // makes AABB computation more accurate than +/-INF. //dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) ); gheight = dCreateHeightfield( space, heightid, 1 ); // Rotate so Z is up, not Y (which is the default orientation) dMatrix3 R; dRSetIdentity( R ); dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 ); dGeomSetRotation( gheight, R ); dGeomSetPosition( gheight, 50,50,0 ); // for (int i=0;i<MAX_ITEMS;i++) { // items.push_back(generateItem()); //} generateItems(); bulldozer_space = dSimpleSpaceCreate(space); dSpaceSetCleanup (bulldozer_space,0); bulldozer=new BoxItem(world,bulldozer_space,LENGTH,WIDTH,HEIGHT,CMASS); bulldozer->setPosition(STARTX,STARTY,STARTZ); bulldozer_cabin=new BoxItem(world,bulldozer_space,LENGTH/2,WIDTH/2,2*HEIGHT,CMASS/3); bulldozer_cabin->setPosition(-LENGTH/4+STARTX,STARTY,3.0/2.0*HEIGHT+STARTZ); bulldozer_bucket_c=new BoxItem(world,bulldozer_space,BUCKET_LENGTH,BUCKET_WIDTH,BUCKET_HEIGHT,CMASS/10); bulldozer_bucket_c->setPosition(LENGTH/2+BUCKET_LENGTH/2+RADIUS+STARTX,STARTY,STARTZ); bulldozer_bucket_l=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20); bulldozer_bucket_l->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,-BUCKET_WIDTH/2+BUCKET_LENGTH/2+STARTY,STARTZ); bulldozer_bucket_r=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20); bulldozer_bucket_r->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,BUCKET_WIDTH/2-BUCKET_LENGTH/2+STARTY,STARTZ); for (int i=0; i<4; i++) { dQuaternion q; dQFromAxisAndAngle(q,1,0,0,M_PI*0.5); wheels[i] = new WheelItem(world,bulldozer_space,q,RADIUS,WMASS); } dBodySetPosition (wheels[0]->body,0.5*LENGTH+STARTX,WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); dBodySetPosition (wheels[1]->body,0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); dBodySetPosition (wheels[2]->body,-0.5*LENGTH+STARTX, WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); dBodySetPosition (wheels[3]->body,-0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5); cabin_joint=dJointCreateSlider(world,0); dJointAttach(cabin_joint,bulldozer->body,bulldozer_cabin->body); dJointSetSliderAxis(cabin_joint,0,0,1); dJointSetSliderParam(cabin_joint,dParamLoStop,0); dJointSetSliderParam(cabin_joint,dParamHiStop,0); bucket_joint_c=dJointCreateSlider(world,0); dJointAttach(bucket_joint_c,bulldozer->body,bulldozer_bucket_c->body); dJointSetSliderAxis(bucket_joint_c,0,0,1); dJointSetSliderParam(bucket_joint_c,dParamLoStop,0); dJointSetSliderParam(bucket_joint_c,dParamHiStop,0); bucket_joint_l=dJointCreateSlider(world,0); dJointAttach(bucket_joint_l,bulldozer->body,bulldozer_bucket_l->body); dJointSetSliderAxis(bucket_joint_l,0,0,1); dJointSetSliderParam(bucket_joint_l,dParamLoStop,0); dJointSetSliderParam(bucket_joint_l,dParamHiStop,0); bucket_joint_r=dJointCreateSlider(world,0); dJointAttach(bucket_joint_r,bulldozer->body,bulldozer_bucket_r->body); dJointSetSliderAxis(bucket_joint_r,0,0,1); dJointSetSliderParam(bucket_joint_r,dParamLoStop,0); dJointSetSliderParam(bucket_joint_r,dParamHiStop,0); // front and back wheel hinges for (int i=0; i<4; i++) { wheelJoints[i] = dJointCreateHinge2 (world,0); dJointAttach (wheelJoints[i],bulldozer->body,wheels[i]->body); const dReal *a = dBodyGetPosition (wheels[i]->body); dJointSetHinge2Anchor (wheelJoints[i],a[0],a[1],a[2]); dJointSetHinge2Axis1 (wheelJoints[i],0,0,1); dJointSetHinge2Axis2 (wheelJoints[i],0,1,0); } // seeting ERP & CRM for (int i=0; i<4; i++) { dJointSetHinge2Param (wheelJoints[i],dParamSuspensionERP,0.5); dJointSetHinge2Param (wheelJoints[i],dParamSuspensionCFM,0.8); } // block back axis !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! for (int i=0; i<2; i++) { dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0); dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0); } }
void Buggy::embody(dWorldID world, dSpaceID space) { dMass m; float LENGTH = 10; float WIDTH = 5; float HEIGHT = 5; float RADIUS=4; // wheel radius float STARTZ=1; // starting height of chassis me = dBodyCreate(world); dBodySetPosition (me,pos[0],pos[1],pos[2]); dMassSetBox (&m,1,WIDTH,HEIGHT,LENGTH); dMassAdjust (&m,CMASS); dBodySetMass (me,&m); box[0] = dCreateBox (0,WIDTH,HEIGHT,LENGTH); dGeomSetBody(box[0], me); // wheel bodies for (int i=1; i<=4; i++) { carbody[i] = dBodyCreate (world); dQuaternion q; dQFromAxisAndAngle (q,1,0,0,0); dBodySetQuaternion (carbody[i],q); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m,WMASS); dBodySetMass (carbody[i],&m); sphere[i-1] = dCreateSphere (0,RADIUS); dGeomSetBody (sphere[i-1],carbody[i]); } // dBodySetPosition (carbody[1],WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,+0.5*LENGTH); dBodySetPosition (carbody[2],-WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,+0.5*LENGTH); dBodySetPosition (carbody[3],WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,-0.5*LENGTH); dBodySetPosition (carbody[4],-WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,-0.5*LENGTH); // // // // front and back wheel hinges for (int i=0; i<4; i++) { carjoint[i] = dJointCreateHinge2 (world,0); dJointAttach (carjoint[i],me,carbody[i+1]); const dReal *a = dBodyGetPosition (carbody[i+1]); dJointSetHinge2Anchor (carjoint[i],a[0],a[1],a[2]); dJointSetHinge2Axes (carjoint[i], zunit, yunit); //dJointSetHinge2Axis1 (carjoint[i],0,1,0); // Axis 1 that comes from the structure //dJointSetHinge2Axis2 (carjoint[i],0,0,1); // Axis 2 where the wheels spin } // // // set joint suspension for (int i=0; i<4; i++) { dJointSetHinge2Param (carjoint[i],dParamSuspensionERP,0.4); dJointSetHinge2Param (carjoint[i],dParamSuspensionCFM,0.8); } // lock back wheels along the steering axis for (int i=1; i<4; i++) { // set stops to make sure wheels always stay in alignment dJointSetHinge2Param (carjoint[i],dParamLoStop,0); dJointSetHinge2Param (carjoint[i],dParamHiStop,0); // // the following alternative method is no good as the wheels may get out // // of alignment: // // dJointSetHinge2Param (joint[i],dParamVel,0); // // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity); } // create car space and add it to the top level space car_space = dSimpleSpaceCreate (space); dSpaceSetCleanup (car_space,0); dSpaceAdd (car_space,box[0]); dSpaceAdd (car_space,sphere[0]); dSpaceAdd (car_space,sphere[1]); dSpaceAdd (car_space,sphere[2]); dSpaceAdd (car_space,sphere[3]); }
int main(int argc, char **argv) { prepDrawStuff(); initOde(2); setCurrentOdeContext(ALLTHREADS); odeRandSetSeed(0); odeSetContactSoftCFM(0); odeWorldSetGravity(0, 0, -9.81f); //creating stage stage.body = odeBodyCreate(); stage.geom = odeCreateBox(0.4f, 0.4f, 0.05f); odeMassSetBoxTotal(stage.body, 0.94, 0.4f,0.4f, 0.05f); odeBodySetPosition(stage.body,0,0.0,h_floor_table+h_base+h_sphere+h_support+stage_dim[2]/2); odeGeomSetBody(stage.geom,stage.body); printf(" Stage body id %f, geom id %f \n", stage.body, stage.geom); const dReal *pos; pos= odeBodyGetPosition(stage.body); printf("stage position x%f y%f z%f \n",pos[0],pos[1], pos[2]); //creating support // //odeMassSetCapsuleTotal(int bodyId, float total_mass, float radius, float length) support.radius = 0.05f ; support.length = 0.2f; support.body = odeBodyCreate(); support.geom = odeCreateCapsule(0, support.radius, support.length-support.radius); odeMassSetCapsuleTotal(support.body, 0.5f, support.radius, support.length-support.radius); odeBodySetPosition(support.body,0.0f,0.0f,h_floor_table+h_base+h_sphere+(h_support/2)-stage_dim[2]/2); odeGeomSetBody(support.geom,support.body); printf("Support capsule body id %f, geom id %f \n", support.body, support.geom); pos= odeBodyGetPosition(support.body); printf("stage position %f \n", pos[2]); //creating ball link LinkBall.radius = 0.08f ; LinkBall.body = odeBodyCreate(); LinkBall.geom = odeCreateSphere( LinkBall.radius); odeMassSetSphereTotal(LinkBall.body, 0.5,LinkBall.radius); odeBodySetPosition(LinkBall.body,0,0,h_floor_table+h_base-support.radius); odeGeomSetBody(LinkBall.geom,LinkBall.body); printf("Ball link body id %f, geom id %f \n", LinkBall.body, LinkBall.geom); //creating main link mainLink.radius= 0.05f; mainLink.length= 0.12f; mainLink.body = odeBodyCreate(); mainLink.geom = odeCreateCapsule(0, mainLink.radius, mainLink.length-mainLink.radius); odeMassSetCapsuleTotal(mainLink.body, 0.5,mainLink.radius, mainLink.length-mainLink.length); odeBodySetPosition(mainLink.body,(mainLink.length+mainLink.radius),0, h_floor_table+h_base-mainLink.radius); odeGeomSetBody(mainLink.geom,mainLink.body); printf("capsule main link link body id %f, geom id %f \n", mainLink.body, mainLink.geom); dQuaternion q1; dQFromAxisAndAngle (q1,0,1,0,1.57) ; //dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, dReal angle); odeGeomSetQuaternion(mainLink.geom,q1); pos= odeBodyGetPosition(mainLink.body); printf("main link position x %f y %f z%f \n",pos[0],pos[1], pos[2]); //creating the first link firstLink.radius= 0.05f; firstLink.length= 0.60f; firstLink.body = odeBodyCreate(); firstLink.geom = odeCreateCapsule(0, firstLink.radius, firstLink.length-firstLink.radius); odeMassSetCapsuleTotal(firstLink.body, 0.7, firstLink.radius, firstLink.length-firstLink.radius); odeBodySetPosition(firstLink.body,(firstLink.length-firstLink.radius/2),0,h_floor_table+h_base-firstLink.radius); odeGeomSetBody(firstLink.geom,firstLink.body); printf("capsule first link body id %f, geom id %f \n", firstLink.body, firstLink.geom); dQuaternion q; dQFromAxisAndAngle (q,0,1,0,1.57) ; //dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, dReal angle); odeGeomSetQuaternion(firstLink.geom,q); pos= odeBodyGetPosition(firstLink.body); printf("First link position x %f y %f z%f \n",pos[0],pos[1], pos[2]); //creating base base.radius= 0.05f; base.length= 0.32f+h_floor_table; base.body = odeBodyCreate(); base.geom = odeCreateCapsule(0, base.radius, base.length-base.radius); odeMassSetCapsuleTotal(base.body, 0.7, base.radius, base.length-base.radius); odeBodySetPosition(base.body,(h_sphere+h_main+h_first+base.radius),0, (base.length+base.radius)/2-firstLink.radius); odeGeomSetBody(base.geom,base.body); printf("capsule base id %f, geom id %f \n", base.body, base.geom); //Creating Ball ball.radius = 0.03f ; ball.body = odeBodyCreate(); ball.geom = odeCreateSphere( ball.radius); //odeCreateCapsule(int spaceId, float radius, float length) odeMassSetSphereTotal(ball.body, 0.04,ball.radius); //odeMassSetCapsuleTotal(int bodyId, float total_mass, float radius, float length) odeBodySetPosition(ball.body,0.1,0.0,2.2f); odeGeomSetBody(ball.geom,ball.body); printf("ball body id %f, geom id %f \n", ball.body, ball.geom); //creating Obstacle obs.radius = 0.05f; obs.body = odeBodyCreate(); obs.geom = odeCreateSphere( obs.radius); odeMassSetSphereTotal(obs.body, 0.05,obs.radius); odeBodySetPosition(obs.body,0,0.0,h_floor_table+h_base+h_sphere+h_support+stage_dim[2]+obs.radius); odeGeomSetBody(obs.geom,obs.body); printf("capsule obstacle body id %f, geom id %f \n", obs.body, obs.geom); ////FIXING ALL JOINTS// //base joint with ground base.joint =odeJointCreateFixed(); odeJointAttach(base.joint,base.body,0); odeJointSetFixed(base.joint); //FirstLink with world firstLink.joint =odeJointCreateFixed(); odeJointAttach(firstLink.joint,firstLink.body,0); odeJointSetFixed(firstLink.joint); // Main link and first link mainLink.joint =odeJointCreateHinge(); odeJointAttach(mainLink.joint,mainLink.body,0); //odeJointSetHingeAnchor(capsule2.joint[0],-0.5,0,0.60); odeJointSetHingeAxis(mainLink.joint,1,0,0); //Ball link and Main link LinkBall.joint =odeJointCreateHinge(); odeJointAttach(LinkBall.joint,LinkBall.body,mainLink.body); //odeJointSetHingeAnchor(capsule3.joint[1],-1.1,0.5,0.60); odeJointSetHingeAxis(LinkBall.joint,0,1,0); // Support and Ball link support.joint =odeJointCreateFixed(); odeJointAttach(support.joint,support.body,LinkBall.body); odeJointSetFixed(support.joint); //Stage and support link stage.joint =odeJointCreateFixed(); odeJointAttach(stage.joint,stage.body,support.body); odeJointSetFixed(stage.joint); //Stage and Obstacle obs.joint =odeJointCreateFixed(); odeJointAttach(obs.joint,stage.body,obs.body); odeJointSetFixed(obs.joint); dsSimulationLoop (argc,argv,900,600,&fn); }
int main (int argc, char **argv) { int i; dMass m; // 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"; // create world world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-0.5); ground = dCreatePlane (space,0,0,1,0); // chassis body body[0] = dBodyCreate (world); dBodySetPosition (body[0],0,0,STARTZ); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m,CMASS); dBodySetMass (body[0],&m); box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); dGeomSetBody (box[0],body[0]); // wheel bodies for (i=1; i<=3; i++) { body[i] = dBodyCreate (world); dQuaternion q; dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); dBodySetQuaternion (body[i],q); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m,WMASS); dBodySetMass (body[i],&m); sphere[i-1] = dCreateSphere (0,RADIUS); dGeomSetBody (sphere[i-1],body[i]); } dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); // front wheel hinge /* joint[0] = dJointCreateHinge2 (world,0); dJointAttach (joint[0],body[0],body[1]); const dReal *a = dBodyGetPosition (body[1]); dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[0],0,0,1); dJointSetHinge2Axis2 (joint[0],0,1,0); */ // front and back wheel hinges for (i=0; i<3; i++) { joint[i] = dJointCreateHinge2 (world,0); dJointAttach (joint[i],body[0],body[i+1]); const dReal *a = dBodyGetPosition (body[i+1]); dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[i],0,0,1); dJointSetHinge2Axis2 (joint[i],0,1,0); // breakable joints contribution // the wheels can break dJointSetBreakable (joint[i], 1); // the wheels wil break at a specific force dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE); // specify the force for the first body connected to the joint ... dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5); // and for the second body dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5); } // set joint suspension for (i=0; i<3; i++) { dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); } // lock back wheels along the steering axis for (i=1; i<3; i++) { // set stops to make sure wheels always stay in alignment dJointSetHinge2Param (joint[i],dParamLoStop,0); dJointSetHinge2Param (joint[i],dParamHiStop,0); // the following alternative method is no good as the wheels may get out // of alignment: // dJointSetHinge2Param (joint[i],dParamVel,0); // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity); } // create car space and add it to the top level space car_space = dSimpleSpaceCreate (space); dSpaceSetCleanup (car_space,0); dSpaceAdd (car_space,box[0]); dSpaceAdd (car_space,sphere[0]); dSpaceAdd (car_space,sphere[1]); dSpaceAdd (car_space,sphere[2]); // environment ground_box = dCreateBox (space,2,1.5,1); dMatrix3 R; dRFromAxisAndAngle (R,0,1,0,-0.15); dGeomSetPosition (ground_box,2,0,-0.34); dGeomSetRotation (ground_box,R); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dGeomDestroy (box[0]); dGeomDestroy (sphere[0]); dGeomDestroy (sphere[1]); dGeomDestroy (sphere[2]); return 0; }
Cube::Cube() { iSize.x = iSize.y = iSize.z = 1; iPosition.x = iPosition.y = iPosition.z = 0; iVel.x = iVel.y =iVel.z = 0; iRotate.x = iRotate.y = iRotate.z = 0.0f; iM = 1; Mat = new Material(); dQFromAxisAndAngle(q,1,0,0,M_PI*0.5); data = new BodyData(); data->setName("Cube"); vecs = new vec3d[24]; texCoord = new TexCoord2[24]; texture = NULL; //Make Verte Buffer Object // Front Face vecs[0] = {-1.0f, -1.0f, 1.0f}; vecs[1] = {1.0f, -1.0f, 1.0f}; vecs[2] = {1.0f, 1.0f, 1.0f}; vecs[3] = {-1.0f, 1.0f, 1.0f}; // Back Face vecs[4] = {-1.0f, -1.0f, -1.0f}; vecs[5] = {-1.0f, 1.0f, -1.0f}; vecs[6] = { 1.0f, 1.0f, -1.0f}; vecs[7] = { 1.0f, -1.0f, -1.0f}; // Top Face vecs[8] = {-1.0f, 1.0f, -1.0f}; vecs[9] = {-1.0f, 1.0f, 1.0f}; vecs[10] = { 1.0f, 1.0f, 1.0f}; vecs[11] = { 1.0f, 1.0f, -1.0f}; // Bottom Face vecs[12] = {-1.0f, -1.0f, -1.0f}; vecs[13] = { 1.0f, -1.0f, -1.0f}; vecs[14] = { 1.0f, -1.0f, 1.0f}; vecs[15] = {-1.0f, -1.0f, 1.0f}; // Right face vecs[16] = { 1.0f, -1.0f, -1.0f}; vecs[17] = { 1.0f, 1.0f, -1.0f}; vecs[18] = { 1.0f, 1.0f, 1.0f}; vecs[19] = { 1.0f, -1.0f, 1.0f}; // Left Face vecs[20] = {-1.0f, -1.0f, -1.0f}; vecs[21] = {-1.0f, -1.0f, 1.0f}; vecs[22] = {-1.0f, 1.0f, 1.0f}; vecs[23] = {-1.0f, 1.0f, -1.0f}; texCoord[0] = {0.0f, 0.0f}; texCoord[1] = {1.0f, 0.0f}; texCoord[2] = {1.0f, 1.0f}; texCoord[3] = {0.0f, 1.0f}; // Back Face texCoord[4] = {1.0f, 0.0f}; texCoord[5] = {1.0f, 1.0f}; texCoord[6] = {0.0f, 1.0f}; texCoord[7] = {0.0f, 0.0f}; // Top Face texCoord[8] = {0.0f, 1.0f}; texCoord[9] = {0.0f, 0.0f}; texCoord[10] = {1.0f, 0.0f}; texCoord[11] = {1.0f, 1.0f}; // Bottom Face texCoord[12] = {1.0f, 1.0f}; texCoord[13] = {0.0f, 1.0f}; texCoord[14] = {0.0f, 0.0f}; texCoord[15] = {1.0f, 0.0f}; // Right face texCoord[16] = {1.0f, 0.0f}; texCoord[17] = {1.0f, 1.0f}; texCoord[18] = {0.0f, 1.0f}; texCoord[19] = {0.0f, 0.0f}; // Left Face texCoord[20] = {0.0f, 0.0f}; texCoord[21] = {1.0f, 0.0f}; texCoord[22] = {1.0f, 1.0f}; texCoord[23] = {0.0f, 1.0f}; vbo = new VertexBufferObject(); vbo->glMode = GL_QUADS; vbo->setVertices(vecs,24); vbo->setTexCoords(texCoord,24); }
int main (int argc, char **argv) { dMass m; // 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_TEXTURE_PATH; // create world dInitODE2(0); world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-9.8); dWorldSetQuickStepNumIterations (world, 32); dCreatePlane (space,0,0,1, 0.0); cylbody = dBodyCreate (world); dQuaternion q; #if 0 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); #else // dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0); dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77); #endif dBodySetQuaternion (cylbody,q); dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH); dBodySetMass (cylbody,&m); cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH); dGeomSetBody (cylgeom,cylbody); dBodySetPosition (cylbody, 0, 0, 3); dSpaceAdd (space, cylgeom); sphbody = dBodyCreate (world); dMassSetSphere (&m,1,SPHERERADIUS); dBodySetMass (sphbody,&m); sphgeom = dCreateSphere(0, SPHERERADIUS); dGeomSetBody (sphgeom,sphbody); dBodySetPosition (sphbody, 0, 0, 5.5); dSpaceAdd (space, sphgeom); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dJointGroupEmpty (contactgroup); dJointGroupDestroy (contactgroup); dGeomDestroy(sphgeom); dGeomDestroy (cylgeom); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
void dJointSetUniversalAxis1Offset( dJointID j, dReal x, dReal y, dReal z, dReal offset1, dReal offset2 ) { dxJointUniversal* joint = ( dxJointUniversal* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Universal ); if ( joint->flags & dJOINT_REVERSE ) { setAxes( joint, x, y, z, NULL, joint->axis2 ); offset1 = -offset1; offset2 = -offset2; } else setAxes( joint, x, y, z, joint->axis1, NULL ); joint->computeInitialRelativeRotations(); dVector3 ax2; getAxis2( joint, ax2, joint->axis2 ); { dVector3 ax1; joint->getAxes(ax1, ax2); } dQuaternion qAngle; dQFromAxisAndAngle(qAngle, x, y, z, offset1); dMatrix3 R; dRFrom2Axes( R, x, y, z, ax2[0], ax2[1], ax2[2] ); dQuaternion qcross; dRtoQ( R, qcross ); dQuaternion qOffset; dQMultiply0(qOffset, qAngle, qcross); dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset ); // Calculating the second offset dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2); dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], x, y, z ); dRtoQ( R, qcross ); dQMultiply1(qOffset, qAngle, qcross); if ( joint->node[1].body ) { dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset ); } else { joint->qrel2[0] = qcross[0]; joint->qrel2[1] = qcross[1]; joint->qrel2[2] = qcross[2]; joint->qrel2[3] = qcross[3]; } }
// simulation loop static void simLoop (int pause) { static bool todo = false; if ( todo ) { // DEBUG static int cnt = 0; ++cnt; if (cnt == 5) command ( 'q' ); if (cnt == 10) dsStop(); } if (!pause) { double simstep = 0.01; // 10ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf (dt/simstep); if (!nrofsteps) nrofsteps = 1; for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldStep (world, simstep); dJointGroupEmpty (contactgroup); } update(); dReal radius, length; dsSetTexture (DS_WOOD); drawBox (geom[W], 1,1,0); drawBox (geom[EXT], 0,1,0); dVector3 anchorPos; dReal ang1 = 0; dReal ang2 = 0; dVector3 axisP, axisR1, axisR2; if ( dJointTypePU == type ) { dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); ang1 = pu->getAngle1(); ang2 = pu->getAngle2(); pu->getAxis1 (axisR1); pu->getAxis2 (axisR2); pu->getAxisP (axisP); dJointGetPUAnchor (pu->id(), anchorPos); } else if ( dJointTypePR == type ) { dPRJoint *pr = dynamic_cast<dPRJoint *> (joint); pr->getAxis1 (axisP); pr->getAxis2 (axisR1); dJointGetPRAnchor (pr->id(), anchorPos); } // Draw the axisR if ( geom[INT] ) { dsSetColor (1,0,1); dVector3 l; dGeomBoxGetLengths (geom[INT], l); const dReal *rotBox = dGeomGetRotation (geom[W]); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i]; dsDrawBox (pos, rotBox, l); } dsSetTexture (DS_CHECKERED); if ( geom[AXIS1] ) { dQuaternion q, qAng; dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dGeomGetQuaternion (geom[AXIS1], q); dQuaternion qq; dQMultiply1 (qq, qAng, q); dMatrix3 R; dQtoR (qq,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length); dsSetColor (1,0,0); dsDrawCylinder (anchorPos, R, length, radius); } if ( dJointTypePU == type && geom[AXIS2] ) { //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint); dQuaternion q, qAng, qq, qq1; dGeomGetQuaternion (geom[AXIS2], q); dQFromAxisAndAngle (qAng, 0, 1, 0, ang2); dQMultiply1 (qq, qAng, q); dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1); dQMultiply1 (qq1, qAng, qq); dMatrix3 R; dQtoR (qq1,R); dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length); dsSetColor (0,0,1); dsDrawCylinder (anchorPos, R, length, radius); } dsSetTexture (DS_WOOD); // Draw the anchor if ( geom[ANCHOR] ) { dsSetColor (1,1,1); dVector3 l; dGeomBoxGetLengths (geom[ANCHOR], l); const dReal *rotBox = dGeomGetRotation (geom[D]); const dReal *posBox = dGeomGetPosition (geom[D]); dVector3 e; for (int i=0; i<3; ++i) e[i] = posBox[i] - anchorPos[i]; dNormalize3 (e); dVector3 pos; for (int i=0; i<3; ++i) pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i]; dsDrawBox (pos, rotBox, l); } drawBox (geom[D], 1,1,0); } }
void dJointSetUniversalAxis2Offset( dJointID j, dReal x, dReal y, dReal z, dReal offset1, dReal offset2 ) { dxJointUniversal* joint = ( dxJointUniversal* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Universal ); if ( joint->flags & dJOINT_REVERSE ) { setAxes( joint, x, y, z, joint->axis1, NULL ); offset1 = -offset2; offset2 = -offset1; } else setAxes( joint, x, y, z, NULL, joint->axis2 ); joint->computeInitialRelativeRotations(); // It is easier to retreive the 2 axes here since // when there is only one body B2 (the axes switch position) // Doing this way eliminate the need to write the code differently // for both case. dVector3 ax1, ax2; joint->getAxes(ax1, ax2 ); dQuaternion qAngle; dQFromAxisAndAngle(qAngle, ax1[0], ax1[1], ax1[2], offset1); dMatrix3 R; dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); dQuaternion qcross; dRtoQ( R, qcross ); dQuaternion qOffset; dQMultiply0(qOffset, qAngle, qcross); dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset ); // Calculating the second offset dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2); dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); dRtoQ( R, qcross ); dQMultiply1(qOffset, qAngle, qcross); if ( joint->node[1].body ) { dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset ); } else { joint->qrel2[0] = qcross[0]; joint->qrel2[1] = qcross[1]; joint->qrel2[2] = qcross[2]; joint->qrel2[3] = qcross[3]; } }