void CPHWorld::Create() { dWorldID phWorld=0; if (psDeviceFlags.test(mtPhysics)) Device.seqFrameMT.Add (this,REG_PRIORITY_HIGH); else Device.seqFrame.Add (this,REG_PRIORITY_LOW); m_commander =xr_new<CPHCommander>(); //dVector3 extensions={2048,256,2048}; /* Fbox level_box = Level().ObjectSpace.GetBoundingVolume(); Fvector level_size,level_center; level_box . getsize (level_size); level_box . getcenter (level_center); dVector3 extensions = { level_size.x ,256.f,level_size.z}; dVector3 center = {level_center.x,0.f,level_center.z}; */ #ifdef ODE_SLOW_SOLVER #else dWorldSetAutoEnableDepthSF1(phWorld, 100000000); ///dWorldSetContactSurfaceLayer(phWorld,0.f); //phWorld->contactp.min_depth =0.f; #endif ContactGroup = dJointGroupCreate(0); dWorldSetGravity (phWorld, 0,-Gravity(), 0);//-2.f*9.81f Mesh.Create (0,phWorld); #ifdef PH_PLAIN plane=dCreatePlane(Space,0,1,0,0.3f); #endif //const dReal k_p=2400000.f;//550000.f;///1000000.f; //const dReal k_d=200000.f; dWorldSetERP(phWorld, ERP(world_spring,world_damping) ); dWorldSetCFM(phWorld, CFM(world_spring,world_damping)); //dWorldSetERP(phWorld, 0.2f); //dWorldSetCFM(phWorld, 0.000001f); disable_count=0; m_motion_ray=dCreateRayMotions(0); phBoundaries.set(Level().ObjectSpace.GetBoundingVolume()); phBoundaries.y1-=30.f; CPHCollideValidator::Init(); b_exist=true; }
void create_world(Controller* controller,bool log,bool bMoviePlay) { // create world dRandSetSeed(10); dInitODE(); creatures.clear(); world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-9.8); floorplane = dCreatePlane (space,0,0,1, 0.0); dWorldSetERP(world,0.1); dWorldSetCFM(world,1E-4); Biped* biped = new Biped(log,bMoviePlay); dVector3 pos={0.0,0.0,0.0}; biped->Create(world,space,pos,controller); creatures.push_back(biped); }
ODEWorld::ODEWorld(const Gravity &g, double erp) : m_timeStep(0.01), m_itrCnt(0), m_nowtime(0.0), m_quickStep(false), m_mu (SPARTS_MU1), m_mu2 (SPARTS_MU2), m_slip1 (SPARTS_SLIP1), m_slip2 (SPARTS_SLIP2), m_soft_erp(SPARTS_ERP), m_soft_cfm(SPARTS_CFM), m_bounce (SPARTS_BOUNCE), m_bounce_vel(0.0) { m_world = dWorldCreate(); dWorldSetGravity(m_world, g.x, g.y, g.z); // default dWorldSetERP(m_world, SPARTS_ERP); dWorldSetCFM(m_world, SPARTS_CFM); m_space = dHashSpaceCreate(0); m_jgroup = dJointGroupCreate(0); // modified by inamura }
/*** メイン関数 ***/ int main(int argc, char *argv[]) { dInitODE(); // ODEの初期化 setDrawStuff(); // 描画関数の設定 world = dWorldCreate(); // ワールドの生成 space = dHashSpaceCreate(0); // スペースの生成 contactgroup = dJointGroupCreate(0); // 接触点グループの生成 ground = dCreatePlane(space,0,0,1,0); // 地面の生成 dWorldSetGravity(world, 0.0, 0.0, - 9.8); // 重力加速度の設定 dWorldSetCFM(world,1e-3); // CFMの設定 dWorldSetERP(world,0.8); // ERPの設定 makeOmni(); // 全方向移動ロボットの生成 makeBall(); // ボールの生成 makeGoal(); // ゴールの生成 dsSimulationLoop(argc,argv,640,480,&fn); // シミュレーションループ dJointGroupDestroy(contactgroup); // 接触点グループの破壊 dSpaceDestroy(space); // スペースの破壊 dWorldDestroy(world); // ワールドの破壊 dCloseODE(); // ODEの終了 return 0; }
bool Simulation::loadFile(const std::string& filename, std::list<std::string>& errors) { ASSERT(scene == 0); Parser parser; if(!parser.parse(filename, errors)) return false; ASSERT(scene); dInitODE(); physicalWorld = dWorldCreate(); rootSpace = dHashSpaceCreate(0); staticSpace = dHashSpaceCreate(rootSpace); movableSpace = dHashSpaceCreate(rootSpace); contactGroup = dJointGroupCreate(0); dWorldSetGravity(physicalWorld, 0, 0, scene->gravity); if(scene->erp != -1.f) dWorldSetERP(physicalWorld, scene->erp); if(scene->cfm != -1.f) dWorldSetCFM(physicalWorld, scene->cfm); if(scene->quickSolverIterations != -1) dWorldSetQuickStepNumIterations(physicalWorld, scene->quickSolverIterations); #ifdef MULTI_THREADING threading = dThreadingAllocateMultiThreadedImplementation(); pool = dThreadingAllocateThreadPool(std::thread::hardware_concurrency(), 0, dAllocateFlagBasicData, nullptr); dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading); dWorldSetStepThreadingImplementation(physicalWorld, dThreadingImplementationGetFunctions(threading), threading); #endif scene->createPhysics(); renderer.init(); return true; }
int main (int argc, char **argv) { setDrawStuff(); // Initialize Drawstuff "fn" parameters dInitODE(); // Initialize ODE world = dWorldCreate(); // Create a dynamic world space = dHashSpaceCreate(0); // Create a 3D space contactgroup = dJointGroupCreate(0); // Create a Joint group dWorldSetGravity(world,0,0,-9.8); // Set gravity (x,y,z) dWorldSetERP (world, 0.95); // ERP: good: [0.8,1.0> dWorldSetCFM(world,1e-5); // CFM ground = dCreatePlane(space,0,0,1,0); // Create ground: plane (space,a,b,c,d) Model::rCreateRobot(world,space,jointR,jointL); // Create robot dsSimulationLoop(argc,argv,600,400,&fn);// Simulation using Drawstuff "fn" parameters dJointGroupDestroy(contactgroup); // Destroy Joint group dSpaceDestroy(space); // Destroy space dWorldDestroy (world); // Destroy the world dCloseODE(); // Close ODE return 0; }
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; if (argc >= 2 ) { for (int i=1; i < argc; ++i) { if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) Help (argv); if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) ) type = dJointTypePR; if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) { int j = i+1; if ( j+1 > argc || // Check if we have enough arguments argv[j] == '\0' || // We should have a path here argv[j][0] == '-' ) // We should have a path not a command line Help (argv); else fn.path_to_textures = argv[++i]; // Increase i since we use this argument } } } dInitODE2(0); world = dWorldCreate(); dWorldSetERP (world, 0.8); space = dSimpleSpaceCreate (0); contactgroup = dJointGroupCreate (0); geom[GROUND] = dCreatePlane (space, 0,0,1,0); dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); dGeomSetCollideBits (geom[GROUND], catBits[ALL]); dMass m; // Create the body attached to the World body[W] = dBodyCreate (world); // Main axis of cylinder is along X=1 m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]); m.adjust (Mass1); geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]); dGeomSetBody (geom[W], body[W]); dGeomSetCategoryBits (geom[W], catBits[W]); dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) ); dBodySetMass (body[W], &m); // Create the dandling body body[D] = dBodyCreate (world); // Main axis of capsule is along X=1 m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]); m.adjust (Mass1); geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]); dGeomSetBody (geom[D], body[D]); dGeomSetCategoryBits (geom[D], catBits[D]); dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) ); dBodySetMass (body[D], &m); // Create the external part of the slider joint geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]); dGeomSetCategoryBits (geom[EXT], catBits[EXT]); dGeomSetCollideBits (geom[EXT], catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); // Create the internal part of the slider joint geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X], INT_EXT_RATIO*extDim[Y], INT_EXT_RATIO*extDim[Z]); dGeomSetCategoryBits (geom[INT], catBits[INT]); dGeomSetCollideBits (geom[INT], catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); dMatrix3 R; dGeomID id; // Create the first axis of the universal joi9nt geom[AXIS1] = dCreateGeomTransform (space); //Rotation of 90deg around y dRFromAxisAndAngle (R, 0,1,0, 0.5*PI); dGeomSetRotation (geom[AXIS1], R); dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]); dGeomSetCollideBits (geom[AXIS1], catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]); id = geom[AXIS1]; dGeomTransformSetGeom (geom[AXIS1], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) ); // Create the second axis of the universal joint geom[AXIS2] = dCreateGeomTransform (space); //Rotation of 90deg around y dRFromAxisAndAngle (R, 1,0,0, 0.5*PI); dGeomSetRotation (geom[AXIS2], R); dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]); dGeomSetCollideBits (geom[AXIS2], catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]); id = geom[AXIS2]; dGeomTransformSetGeom (geom[AXIS2], dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) ); // Create the anchor geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]); dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]); dGeomSetCollideBits (geom[ANCHOR], catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) ); if (body[W]) { dBodySetPosition(body[W], 0, 0, 5); } if (geom[EXT]) { dGeomSetPosition (geom[EXT], 0,0,3.8); } if (geom[INT]) { dGeomSetPosition (geom[INT], 0,0,2.6); } if (geom[AXIS1]) { dGeomSetPosition (geom[AXIS1], 0,0,2.5); } if (geom[AXIS2]) { dGeomSetPosition (geom[AXIS2], 0,0,2.5); } if (geom[ANCHOR]) { dGeomSetPosition (geom[ANCHOR], 0,0,2.25); } if (body[D]) { dBodySetPosition (body[D], 0,0,1.5); } // Attache the upper box to the world dJointID fixed = dJointCreateFixed (world,0); dJointAttach (fixed , NULL, body[W]); dJointSetFixed (fixed ); if (type == dJointTypePR) { dPRJoint *pr = new dPRJoint (world, 0); pr->attach (body[W], body[D]); pr->setAxis1 (0, 0, -1); pr->setAxis2 (1, 0, 0); joint = pr; dJointSetPRAnchor (pr->id(), 0, 0, 2.5); } else { dPUJoint *pu = new dPUJoint (world, 0); pu->attach (body[W], body[D]); pu->setAxis1 (1, 0, 0); pu->setAxis2 (0, 1, 0); pu->setAxisP (0, 0, -1); joint = pu; dJointSetPUAnchor (pu->id(), 0, 0, 2.5); } // run simulation dsSimulationLoop (argc,argv,400,300,&fn); delete joint; dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
void ODEWorld::setERP(double value) { dWorldSetERP(m_world, value); }
/******************************************************************************* Function to initialize ODE. *******************************************************************************/ void initODE() { ///////////////// Initializing the ODE general features //////////////////// dInitODE(); //Initialize library. World = dWorldCreate(); //Crate a new dynamics, empty world. Space = dSimpleSpaceCreate(0); //Create a new space for collision (independent). ContactGroup = dJointGroupCreate(0); //Create a joints container, without specifying size. dWorldSetGravity( World, 0.0, -9.81, 0 ); //Add gravity to this World. //Define error conrrection constants. dWorldSetERP( World, 0.2 ); dWorldSetCFM( World, 1e-5 ); //Set the velocity that interpenetrating objects will separate at. dWorldSetContactMaxCorrectingVel( World, 0.9 ); //Set the safety area for contacts to be considered at rest. dWorldSetContactSurfaceLayer( World, 0.001 ); //Automatically disable objects that have come to a rest. dWorldSetAutoDisableFlag( World, false ); /////////////// Initializing the rigid bodies in the world ///////////////// //Create a collision plane and add it to space. The next parameters are the //literal components of ax + by + cz = d. dCreatePlane( Space, 0.0, 1.0, 0.0, 0.0 ); const dReal xPos = 0; const dReal yPos = 5; const dReal zPos = 0; const dReal xRot = 0; const dReal yRot = 0; const dReal zRot = 0; const dReal radius = .75; const dReal length = 4; const dReal sides[3] = { 2, 2, 2 }; //Create body body.Body = dBodyCreate(World); dBodySetPosition(body.Body, xPos, yPos, zPos); dMatrix3 Orient3; //dRFromAxisAndAngle(Orient3, 0, 0, 1, 3.14/4); dRFromEulerAngles(Orient3, xRot, yRot, zRot); //dRFromEulerAngles(Orient3, 0, 0, 0); dBodySetRotation(body.Body, Orient3); dBodySetLinearVel(body.Body, 0, 0, 0); dBodySetData(body.Body, (void *)0); dMass bodyMass; dMassSetCapsule(&bodyMass, 1.0, 3, radius, length); //dMassSetBox(&bodyMass, 10, sides[0], sides[1], sides[2]); dBodySetMass(body.Body, &bodyMass); body.Geom = dCreateCapsule(Space, radius, length); //body.Geom = dCreateBox(Space, sides[0], sides[1], sides[2]); dGeomSetBody(body.Geom, body.Body); float head_pos[ 3 ] = { 0.0f, 5.0f, -1.0f }; createInvisibleHead( head_pos ); createFrontLegs(); createMiddleLegs(); createBackLegs(); rng_engine.seed( std::chrono::duration_cast< std::chrono::microseconds >( std::chrono::system_clock::now().time_since_epoch() ).count() ); const int foodPrize = 10; dReal position[3] = { 10.0f, 0.0f, -20.0f }; addFoodParticle( position, foodPrize, &World, &Space ); }
int main (int argc, char **argv) { dInitODE2(0); bool fixed = true; // 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; dVector3 offset; dSetZero (offset, 4); // Default test case if (argc >= 2 ) { for (int i=1; i < argc; ++i) { //static int tata = 0; if (1) { if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) ) Help (argv); if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) ) type = dJointTypeSlider; if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) { int j = i+1; if ( j+1 > argc || // Check if we have enough arguments argv[j] == '\0' || // We should have a path here argv[j][0] == '-' ) // We should have a path not a command line Help (argv); else fn.path_to_textures = argv[++i]; // Increase i since we use this argument } } if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) ) tc = 1; if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) ) tc = 2; if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) ) tc = 3; if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) ) fixed = false; } } world = dWorldCreate(); dWorldSetERP (world, 0.8); space = dSimpleSpaceCreate (0); contactgroup = dJointGroupCreate (0); geom[GROUND] = dCreatePlane (space, 0,0,1,0); dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]); dGeomSetCollideBits (geom[GROUND], catBits[ALL]); dMass m; dMatrix3 R; // Create the Obstacle geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]); dGeomSetCategoryBits (geom[OBS], catBits[OBS]); dGeomSetCollideBits (geom[OBS], catBits[ALL]); //Rotation of 45deg around y dRFromAxisAndAngle (R, 1,1,0, -0.25*PI); dGeomSetRotation (geom[OBS], R); dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5); //Rotation of 90deg around y // Will orient the Z axis along X dRFromAxisAndAngle (R, 0,1,0, -0.5*PI); // Create Body2 (Wiil be attached to the world) body[BODY2] = dBodyCreate (world); // Main axis of cylinder is along X=1 dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); dMassAdjust (&m, Mass1); geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]); dGeomSetBody (geom[BODY2], body[BODY2]); dGeomSetOffsetRotation (geom[BODY2], R); dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]); dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) ); dBodySetMass (body[BODY2], &m); // Create Body 1 (Slider on the prismatic axis) body[BODY1] = dBodyCreate (world); // Main axis of capsule is along X=1 dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH); dMassAdjust (&m, Mass1); geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH); dGeomSetBody (geom[BODY1], body[BODY1]); dGeomSetOffsetRotation (geom[BODY1], R); dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]); dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]); dMass mRect; dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); dMassAdd (&m, &mRect); // TODO: translate m? geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]); dGeomSetBody (geom[RECT], body[BODY1]); dGeomSetOffsetPosition (geom[RECT], (BODY1_LENGTH-RECT_SIDES[0]) /2.0, 0.0, -RADIUS -RECT_SIDES[2]/2.0); dGeomSetCategoryBits (geom[RECT], catBits[RECT]); dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) ); dBodySetMass (body[BODY1], &m); setPositionBodies (tc); if ( fixed ) { // Attache external cylinder to the world dJointID fixed = dJointCreateFixed (world,0); dJointAttach (fixed , NULL, body[BODY2]); dJointSetFixed (fixed ); dWorldSetGravity (world,0,0,-0.8); } else { dWorldSetGravity (world,0,0,0); } // The static is here only to help debugging switch (type) { case dJointTypeSlider : { dSliderJoint *sj = new dSliderJoint (world, 0); sj->attach (body[BODY1], body[BODY2]); sj->setAxis (1, 0, 0); joint = sj; } break; case dJointTypePiston : // fall through default default: { dPistonJoint *pj = new dPistonJoint (world, 0); pj->attach (body[BODY1], body[BODY2]); pj->setAxis (1, 0, 0); dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]); joint = pj; } break; }; // run simulation dsSimulationLoop (argc,argv,400,300,&fn); delete joint; dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
signed ODE_Init() { Quit = SDL_FALSE; dInitODE2(dInitFlagManualThreadCleanup); dSetMessageHandler(Error); dSetDebugHandler(Error); dSetErrorHandler(Error); World = dWorldCreate(); Space = dHashSpaceCreate(0); Group = dJointGroupCreate(0); Step = 1.0/50.0; lua_getglobal(State, "World"); int table = lua_gettop(State); if (!lua_isnil(State, table)) { lua_pushnil(State); while (lua_next(State, table)) { const char *key = lua_tostring(State, -2); #define tointeger lua_tointeger(State, -1) #define toboolean lua_toboolean(State, -1) #define tonumber lua_tonumber(State, -1) if (!SDL_strcasecmp(key, "FPS")) { Step = 1.0/tonumber; } else if (!SDL_strcasecmp(key, "ERP")) { dWorldSetERP(World, tonumber); } else if (!SDL_strcasecmp(key, "CFM")) { dWorldSetCFM(World, tonumber); } else if (!SDL_strcasecmp(key, "LINEAR_DAMPING")) { dWorldSetLinearDamping(World, tonumber); } else if (!SDL_strcasecmp(key, "LINEAR_DAMPING_THRESHOLD")) { dWorldSetLinearDampingThreshold(World, tonumber); } else if (!SDL_strcasecmp(key, "ANGULAR_DAMPING")) { dWorldSetAngularDamping(World, tonumber); } else if (!SDL_strcasecmp(key, "ANGULAR_DAMPING_THRESHOLD")) { dWorldSetAngularDampingThreshold(World, tonumber); } else if (!SDL_strcasecmp(key, "MAX_ANGULAR_SPEED")) { dWorldSetMaxAngularSpeed(World, tonumber); } else if (!SDL_strcasecmp(key, "CONTACT_MAX_CORRECTING_VELOCITY")) { dWorldSetContactMaxCorrectingVel(World, tonumber); } else if (!SDL_strcasecmp(key, "CONTACT_SURFACE_LAYER")) { dWorldSetContactSurfaceLayer(World, tonumber); } else if (!SDL_strcasecmp(key, "AUTO_DISABLE")) { dWorldSetAutoDisableFlag(World, toboolean); } else if (!SDL_strcasecmp(key, "AUTO_DISABLE_LINEAR_THRESHOLD")) { dWorldSetAutoDisableLinearThreshold(World, tonumber); } else if (!SDL_strcasecmp(key, "AUTO_DISABLE_ANGULAR_THRESHOLD")) { dWorldSetAutoDisableAngularThreshold(World, tonumber); } else if (!SDL_strcasecmp(key, "AUTO_DISABLE_STEPS")) { dWorldSetAutoDisableSteps(World, tointeger); } else if (!SDL_strcasecmp(key, "AUTO_DISABLE_TIME")) { dWorldSetAutoDisableTime(World, tonumber); } else { SDL_Log("World: %s does not match", key); } lua_pop(State, 1); } } lua_pop(State, 1); Cond = SDL_CreateCond(); if (!Cond) { dWorldDestroy(World); dSpaceDestroy(Space); dJointGroupDestroy(Group); SDL_perror("SDL_CreateCond"); return SDL_SetError("cannot create simulation signal"); } Mutex = SDL_CreateMutex(); if (!Mutex) { dWorldDestroy(World); dSpaceDestroy(Space); dJointGroupDestroy(Group); SDL_DestroyCond(Cond); SDL_perror("SDL_CreateMutex"); return SDL_SetError("cannot create simulation mutex"); } Thread = SDL_CreateThread(SimulationThread, "ODE", NULL); if (!Thread) { dWorldDestroy(World); dSpaceDestroy(Space); dJointGroupDestroy(Group); SDL_DestroyCond(Cond); SDL_DestroyMutex(Mutex); SDL_perror("SDL_CreateThread"); return SDL_SetError("cannot create simulation thread"); } TimerID = SDL_AddTimer(Uint32(1000*Step), SimulationTimer, NULL); if (!TimerID) { dWorldDestroy(World); dSpaceDestroy(Space); dJointGroupDestroy(Group); SDL_DestroyCond(Cond); SDL_DestroyMutex(Mutex); SDL_perror("SDL_AddTimer"); return SDL_SetError("cannot create simulation timer"); } return 0; }
void ODESimulator::initData(SimulatorData data) { Simulator::initData(data); // We can only init ODE once. if (0 == mInitCounter) { dInitODE(); } ++mInitCounter; // Create ODE world. mWorldID = dWorldCreate(); // Set default gravity. setGravity(defaults::gravity); // Create the root ODE space. //mRootSpaceID = dSimpleSpaceCreate(0); //dVector3 center = {0, 0, 0}; //dVector3 extents = {200, 100, 200}; //mRootSpaceID = dQuadTreeSpaceCreate(0, center, extents, 5); if (data.useOctreeInsteadHash) { dVector3 center = { data.worldCenter[0], data.worldCenter[1], data.worldCenter[2] }; dVector3 extents = { data.worldSize[0], data.worldSize[1], data.worldSize[2] }; mRootSpaceID = dQuadTreeSpaceCreate(0, center, extents, data.octreeDepth); } else { mRootSpaceID = dHashSpaceCreate(0); dHashSpaceSetLevels(mRootSpaceID, data.hashMinLevel, data.hashMaxLevel); } mRootSpace = new ODESpace(mRootSpaceID); // Create the ODE contact joint group. mContactJointGroupID = dJointGroupCreate(0); // Set the ODE global CFM value that will be used by all Joints // (including contacts). This affects normal Joint constraint // operation and Joint limits. The user cannot adjust CFM, but // they can adjust ERP (a.k.a. bounciness/restitution) for materials // (i.e. contact settings) and Joint limits. dWorldSetCFM(mWorldID, defaults::ode::globalCFM); // Set the ODE global ERP value. This will only be used for Joints // under normal conditions, not at their limits. Also, it will not // affect contacts at all since they use material properties to // calculate ERP. dWorldSetERP(mWorldID, (real) 0.5 * (defaults::ode::maxERP + defaults::ode::minERP)); dWorldSetContactSurfaceLayer(mWorldID, defaults::ode::surfaceLayer); setSolverAccuracy(defaults::solverAccuracy); mCollisionCount = 0; // "mRaycastResult" is initialized in its own constructor. mSensorSolid = NULL; mRayContactGroup = defaults::shape::contactGroup; }
SimWorld::SimWorld(QObject *parent) : QObject(parent) { dInitODE(); world = dWorldCreate(); space = dHashSpaceCreate(0); contact_group = dJointGroupCreate(0); // Setup gravity dWorldSetGravity (world,0,0,-9.8); // Setup constraint satisfaction parameters. dWorldSetERP (world, 0.2); // How hard the world pushes to fix unsatisfied constraints dWorldSetCFM(world, 0.000001); // Constraint force mixing dWorldSetContactSurfaceLayer(world,.005); // How deeply an object can penetrate (in meters) dWorldSetMaxAngularSpeed(world,15); // A hard limit on how fast anything can spin //dWorldSetLinearDamping(world,.02); // Linear Friction //dWorldSetAngularDamping(world,.02); // Angular friction stepsize=1/60.0; // Create the ground plane ground_plane = dCreatePlane (space,0,0,1,0.0); body = new CapBody(this); body->createBody(world,space); marker_space = dSimpleSpaceCreate(0); target_space = dSimpleSpaceCreate(0); // //markData = new SwingData(world,markSpace,this); //markData = new LiveMarkerData(world,markSpace,this); std::cout << "[Simworld] Loading marker_data..." << std::endl; #if defined( BOARD_DATA ) markData = new BoardMarkerData(world,markSpace,this); #elif defined( POKE_DATA ) markData = new PokeMarkerData(world,markSpace,this); #else marker_data = new MarkerData(world,marker_space,this); #endif marker_data->body_pointer=body; body->changeMarkLinks(marker_data->marker_count); std::cout << "marker_data created/loaded" << std::endl; #if defined( BOARD_DATA ) board0 = dCreateBox(space,.5,.5,.05); board1 = dCreateBox(space,.5,.5,.05); dGeomSetPosition(board0,0,.5,0.025); dGeomSetPosition(board1,0,-.5,0.025); #endif paused=true; single_step=false; angle_sequence = new Sequence(this); torque_sequence = new Sequence(this); self_collide=false; follow_sequence_source_state=0; ground_squish = .00001; ground_friction = 1; main_file = fopen("totals.txt","w"); }
void ODESimulator::SetERP(double erp) { settings.errorReductionParameter = erp; dWorldSetERP(worldID,erp); }
/** * \brief This function handles the calculation of a step in the world. * * pre: * - world_init = true * - step_size > 0 * * post: * - handled the collisions * - step the world for step_size seconds * - the contactgroup should be empty */ void WorldPhysics::stepTheWorld(void) { MutexLocker locker(&iMutex); std::vector<dJointFeedback*>::iterator iter; geom_data* data; int i; // if world_init = false or step_size <= 0 debug something if(world_init && step_size > 0) { if(old_gravity != world_gravity) { old_gravity = world_gravity; dWorldSetGravity(world, world_gravity.x(), world_gravity.y(), world_gravity.z()); } if(old_cfm != world_cfm) { old_cfm = world_cfm; dWorldSetCFM(world, (dReal)world_cfm); } if(old_erp != world_erp) { old_erp = world_erp; dWorldSetERP(world, (dReal)world_erp); } // printf("now WorldPhysics.cpp..stepTheWorld(void)....1 : dSpaceGetNumGeoms: %d\n",dSpaceGetNumGeoms(space)); /// first clear the collision counters of all geoms for(i=0; i<dSpaceGetNumGeoms(space); i++) { data = (geom_data*)dGeomGetData(dSpaceGetGeom(space, i)); data->num_ground_collisions = 0; data->contact_ids.clear(); data->contact_points.clear(); data->ground_feedbacks.clear(); } for(iter = contact_feedback_list.begin(); iter != contact_feedback_list.end(); iter++) { free((*iter)); } contact_feedback_list.clear(); draw_intern.clear(); /// then we have to clear the contacts dJointGroupEmpty(contactgroup); /// first check for collisions num_contacts = log_contacts = 0; create_contacts = 1; dSpaceCollide(space,this, &WorldPhysics::callbackForward); drawLock.lock(); draw_extern.swap(draw_intern); drawLock.unlock(); // then calculate the next state for a time of step_size seconds try { if(fast_step) dWorldQuickStep(world, step_size); else dWorldStep(world, step_size); } catch (...) { control->sim->handleError(PHYSICS_UNKNOWN); } if(WorldPhysics::error) { control->sim->handleError(WorldPhysics::error); WorldPhysics::error = PHYSICS_NO_ERROR; } } }
void resetSimulation() { int i; i = 0; // destroy world if it exists if (bodies) { dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); } for (i = 0; i < 1000; i++) wb_stepsdis[i] = 0; // recreate world world = dWorldCreate(); // space = dHashSpaceCreate( 0 ); // space = dSimpleSpaceCreate( 0 ); space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ ); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-1.5); dWorldSetCFM (world, 1e-5); dWorldSetERP (world, 0.8); dWorldSetQuickStepNumIterations (world,ITERS); ground = dCreatePlane (space,0,0,1,0); bodies = 0; joints = 0; boxes = 0; spheres = 0; wb = 0; #ifdef CARS for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS) for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2) makeCar(x, y, bodies, joints, boxes, spheres); #endif #ifdef WALL bool offset = false; for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE) { offset = !offset; for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE) { wall_bodies[wb] = dBodyCreate (world); dBodySetPosition (wall_bodies[wb],-20,y,z); dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE); dMassAdjust (&m, WALLMASS); dBodySetMass (wall_bodies[wb],&m); wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE); dGeomSetBody (wall_boxes[wb],wall_bodies[wb]); //dBodyDisable(wall_bodies[wb++]); wb++; } } dMessage(0,"wall boxes: %i", wb); #endif #ifdef BALLS for (dReal x = -7; x <= -4; x+=1) for (dReal y = -1.5; y <= 1.5; y+=1) for (dReal z = 1; z <= 4; z+=1) { b = dBodyCreate (world); dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, BALLMASS); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); } #endif #ifdef ONEBALL b = dBodyCreate (world); dBodySetPosition (b,0,0,2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, 1); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); #endif #ifdef BALLSTACK for (dReal z = 1; z <= 6; z+=1) { b = dBodyCreate (world); dBodySetPosition (b,0,0,z*RADIUS*2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, 0.1); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); } #endif #ifdef CENTIPEDE dBodyID lastb = 0; for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1) { // chassis body b = body[bodies] = dBodyCreate (world); dBodySetPosition (body[bodies],-15,y,STARTZ); dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT); dMassAdjust (&m,CMASS); dBodySetMass (body[bodies],&m); box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT); dGeomSetBody (box[boxes++],body[bodies++]); for (dReal x = -17; x > -20; x-=RADIUS*2) { body[bodies] = dBodyCreate (world); dBodySetPosition(body[bodies], x, y, STARTZ); dMassSetSphere(&m, 1, RADIUS); dMassAdjust(&m, WMASS); dBodySetMass(body[bodies], &m); sphere[spheres] = dCreateSphere (space, RADIUS); dGeomSetBody (sphere[spheres++], body[bodies]); joint[joints] = dJointCreateHinge2 (world,0); if (x == -17) dJointAttach (joint[joints],b,body[bodies]); else dJointAttach (joint[joints],body[bodies-2],body[bodies]); const dReal *a = dBodyGetPosition (body[bodies++]); dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[joints],0,0,1); dJointSetHinge2Axis2 (joint[joints],1,0,0); dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[joints],dParamLoStop,0); dJointSetHinge2Param (joint[joints],dParamHiStop,0); dJointSetHinge2Param (joint[joints],dParamVel2,-10.0); dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); body[bodies] = dBodyCreate (world); dBodySetPosition(body[bodies], -30 - x, y, STARTZ); dMassSetSphere(&m, 1, RADIUS); dMassAdjust(&m, WMASS); dBodySetMass(body[bodies], &m); sphere[spheres] = dCreateSphere (space, RADIUS); dGeomSetBody (sphere[spheres++], body[bodies]); joint[joints] = dJointCreateHinge2 (world,0); if (x == -17) dJointAttach (joint[joints],b,body[bodies]); else dJointAttach (joint[joints],body[bodies-2],body[bodies]); const dReal *b = dBodyGetPosition (body[bodies++]); dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]); dJointSetHinge2Axis1 (joint[joints],0,0,1); dJointSetHinge2Axis2 (joint[joints],1,0,0); dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[joints],dParamLoStop,0); dJointSetHinge2Param (joint[joints],dParamHiStop,0); dJointSetHinge2Param (joint[joints],dParamVel2,10.0); dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); } if (lastb) { dJointID j = dJointCreateFixed(world,0); dJointAttach (j, b, lastb); dJointSetFixed(j); } lastb = b; } #endif #ifdef BOX body[bodies] = dBodyCreate (world); dBodySetPosition (body[bodies],0,0,HEIGHT/2); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m, 1); dBodySetMass (body[bodies],&m); box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); dGeomSetBody (box[boxes++],body[bodies++]); #endif #ifdef CANNON cannon_ball_body = dBodyCreate (world); cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS); dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS); dBodySetMass (cannon_ball_body,&m); dGeomSetBody (cannon_ball_geom,cannon_ball_body); dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS); #endif }
//Assistance with ode from http://www.alsprogrammingresource.com/basic_ode.html //and from the ODE user manual, not a lot of resources out there! void initODE() { int i; int j = 0;//For placing skyPanels int k = 0;//For placing sphere in box int q = 0;//For placing target boxes int r = 0;//For placing targetBoxes int s = 0;//FOr placing targetBoxes dReal radius = 0.5;//For sphere's in scene that bounce around/ get shot dMass m;//For mass of sphere dMass mp;//For mass of platform dMass md;//For mass of doorway dMass mpy;//For mass of pyramid dMass mbox;//For box of balls dMass msp;//For skyPanels dMass mtb;//For targetBox // Create a new, empty world and assign its ID number to World. Most applications will only need one world. world = dWorldCreate(); // Create a new collision space and assign its ID number to Space, passing 0 instead of an existing dSpaceID. // There are three different types of collision spaces we could create here depending on the number of objects // in the world but dSimpleSpaceCreate is fine for a small number of objects. If there were more objects we // would be using dHashSpaceCreate or dQuadTreeSpaceCreate (look these up in the ODE d spacePhy = dSimpleSpaceCreate(0); // Create a joint group object and assign its ID number to contactgroup. dJointGroupCreate used to have a // max_size parameter but it is no longer used so we just pass 0 as its argument. contactgroup = dJointGroupCreate(0); //Set the gravity of the world where y is up dWorldSetGravity(world, 0, -0.1, 0); // These next two functions control how much error correcting and constraint force mixing occurs in the world. // Don't worry about these for now as they are set to the default values and we could happily delete them from // this example. Different values, however, can drastically change the behaviour of the objects colliding, so // I suggest you look up the full info on them in the ODE docs. dWorldSetERP(world, 0.2); dWorldSetCFM(world, 1e-5); // This function sets the velocity that inter-penetrating objects will separate at. The default value is infinity. dWorldSetContactMaxCorrectingVel(world, 0.9); // This function sets the depth of the surface layer around the world objects. Contacts are allowed to sink into // each other up to this depth. Setting it to a small value reduces the amount of jittering between contacting // objects, the default value is 0. dWorldSetContactSurfaceLayer(world, 0.0); // To save some CPU time we set the auto disable flag to 1. This means that objects that have come to rest (based // on their current linear and angular velocity) will no longer participate in the simulation, unless acted upon // by a moving object. If you do not want to use this feature then set the flag to 0. You can also manually enable // or disable objects using dBodyEnable and dBodyDisable, see the docs for more info on this. dWorldSetAutoDisableFlag(world, 1); //This brings us to the end of the world settings, now we have to initialize the objects themselves. //****************Ball 1 // Create a new body for our object in the world and get its ID. ball_body = dBodyCreate(world); //Set ball1 geometries dMassSetZero(&ball_mass); dMassSetSphereTotal(&ball_mass, DENSITY, 2.0); dBodySetMass(ball_body, &ball_mass); ball_geom = dCreateSphere(spacePhy, radius); dGeomSetData(ball_geom, (void *)"ball"); dGeomSetBody(ball_geom, ball_body); dBodyEnable(ball_body); //Next we set the position of the new body dBodySetPosition(ball_body, 5, 100, 15); //****************Ball 2 // Create a new body for our object in the world and get its ID. ball_body2 = dBodyCreate(world); //Set ball1 geometries dMassSetZero(&ball_mass2); dMassSetSphereTotal(&ball_mass2, DENSITY, 2.0); dBodySetMass(ball_body2, &ball_mass2); ball_geom2 = dCreateSphere(spacePhy, radius); dGeomSetData(ball_geom2, (void *)"ball2"); dGeomSetBody(ball_geom2, ball_body2); dBodySetGravityMode (ball_body2, 0); //******End of ball 2 //*************Ball 3, a new way of creating a ball with physical properties ball_body3 = dBodyCreate(world); dMatrix3 R; dBodySetPosition(ball_body3, -5, 100, 15); dRFromAxisAndAngle(R, 1, 0, 0, -1.57); dBodySetRotation(ball_body3, R); // Here we use dMassSetSphere instead of dMassSetBox // and we pass the local radius variable as the third parameter dMassSetSphere(&m, DENSITY, radius); // To create the sphere object we use dCreateSphere and pass it the same local radius variable ball_geom3 = dCreateSphere(spacePhy, radius); dGeomSetBody(ball_geom3, ball_body3); dBodySetMass(ball_body3, &m); //*****End of ball3 //*************Box o balls properties for (i = 0; i < 61; i++) { // Create a new body for our object in the world and get its ID. balls_body[i] = dBodyCreate(world); //Set ball1 geometries dMassSetZero(&balls_mass[i]); dMassSetSphereTotal(&balls_mass[i], DENSITY, 2.0); dBodySetMass(balls_body[i], &balls_mass[i]); balls_geom[i] = dCreateSphere(spacePhy, radius); dGeomSetData(balls_geom[i], (void *)"ball"); dGeomSetBody(balls_geom[i], balls_body[i]); dBodyDisable(balls_body[i]);//Disable bodies, enabled auto after hit //Box 1 if (i <31) { dBodySetPosition(balls_body[i], (-20), 28.5, 7.0);//(xpos b/w -20 and -12, ypos b/w 28 and 32, zpos between 0 and 16) } //Box 2 if (i >= 31) { dBodySetPosition(balls_body[i], (25), 38.5, -14.0);//(xpos b/w -20 and -12, ypos b/w 28 and 32, zpos between 0 and 16) } k++; } //******End of Box o Balls //**************platform geometries //create 5 platform's with physics things for (i = 0; i < 15; i++) { platform_body[i] = dBodyCreate(world);//Add the platform body to the world //Set the geom platform // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&mp, DENSITY*1500, 4.5, 0.5, 18.0); //It works better if the invisible physics box is slightly larger than the object we're drawing platform_geom[i] = dCreateBox(spacePhy, 4.5, 0.5, 18.0); dGeomSetBody(platform_geom[i], platform_body[i]); dBodySetMass(platform_body[i], &mp); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (platform_body[i], 0); } //*******End of platform geometries //*******Set the doorway properties, 18 platforms make all 3 doors for (i = 0; i < 18; i++) { doorway_body[i] = dBodyCreate(world);//Add the platform body to the world //Set the geom platform // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&md, DENSITY*1500, 4.0, 0.25, 2.0); //It works better if the invisible physics box is slightly larger than the object we're drawing doorway_geom[i] = dCreateBox(spacePhy, 4.0, 0.25, 2.0); dGeomSetBody(doorway_geom[i], doorway_body[i]); dBodySetMass(doorway_body[i], &md); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (doorway_body[i], 0); } //*******End of doorway properties //*******Set the Box o' Balls properties, 6 sides total make the larger box for (i = 0; i < 12; i++) { boxSide_body[i] = dBodyCreate(world); } //Box 1 //Need to set position here to affect the box with gravity dBodySetPosition(boxSide_body[0], -20.0, 28.0, 16);//Front, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[1], -12.25, 28.0, 8.0);//Right, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[2], -20.0, 28.0, 0);//Back, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[3], -27.75, 28.0, 8.0);//Left, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[4], -20.0, 32.0, -0.23);//Top, rotated 90 deg. on x axis dBodySetPosition(boxSide_body[5], -20.0, 28.0, -0.23);//Bottom, rotated 90 deg. on x axis //Need to set position here to affect the box with gravity //Box 2 dBodySetPosition(boxSide_body[6], 30.0, 38.0, -16);//Front, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[7], 22.25, 38.0, -8.0);//Right, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[8], 30.0, 38.0, 0);//Back, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[9], 37.75, 38.0, -8.0);//Left, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[10], 30.0, 42.0, -16.23);//Top, rotated 90 deg. on x axis*****Unsure about z here dBodySetPosition(boxSide_body[11], 30.0, 38.0, -16.23);//Bottom, rotated 90 deg. on x axis //Box 1 for (i = 0; i < 6; i++) { if (i < 4)//Small sides { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 1, 0.25); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 1, 0.25); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } if (i >=4)//Large top and bottom { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 16, 0.5); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 16, 0.5); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } //Turn off colliding object force effects on this geom so that it is stationary (less density = greater effect on objects colliding into this one) dBodyDisable(boxSide_body[i]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (boxSide_body[i], 1); } //Box 2 for (; i < 12; i++) { if (i < 10)//Small sides { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 1, 0.5); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 1, 0.5); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } if (i >=10)//Large top and bottom { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 16, 1.0); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 16, 1.0); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } //Turn off colliding object force effects on this geom so that it is stationary (less density = greater effect on objects colliding into this one) dBodyDisable(boxSide_body[i]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (boxSide_body[i], 1); } //*******End of Box o' Balls properties //*******Set the pyramid properties, 120 pyramids make diamond excluding inverted that fill holes (gotta cheat) pyramid_body = dBodyCreate(world);//Add the platform body to the world //Set the geom platform // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&mpy, DENSITY*1500, 2.6, 4.5, 2.6); //It works better if the invisible physics box is slightly larger than the object we're drawing pyramid_geom = dCreateBox(spacePhy, 2.6, 4.5, 2.6); dGeomSetBody(pyramid_geom, pyramid_body); dBodySetMass(pyramid_body, &mpy); dBodyDisable(pyramid_body); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (pyramid_body, 0); //*******End of pyramid properties //*******skyPanels physics for(j = 0; j < 60; j++) { skyPanel_body[j] = dBodyCreate(world);//Add the skypanel body to the world //Set the geom skypanel // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&msp, DENSITY*1500, 3.0, 7.0, 1.0); //It works better if the invisible physics box is slightly larger than the object we're drawing skyPanel_geom[j] = dCreateBox(spacePhy, 3.0, 7.0, 1.0); dGeomSetBody(skyPanel_geom[j], skyPanel_body[j]);//Bind the geom to the body so that we only need to place the body to also set the geom dBodySetMass(skyPanel_body[j], &msp); dBodyDisable(skyPanel_body[j]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (skyPanel_body[j], 0);//Gravity off } //*******End of skyPanels physics //*******Set targetBox physics for(q = 0; q < 114; q++) { targetBox_body[q] = dBodyCreate(world);//Add the skypanel body to the world //Set the geom target box // Here we use dMassSetBox dMassSetBox(&mtb, (0.05), 4.0, 4.0, 4.0); //It works better if the invisible physics box is slightly larger than the object we're drawing targetBox_geom[q] = dCreateBox(spacePhy, 4.0, 4.0, 4.0); dGeomSetBody(targetBox_geom[q], targetBox_body[q]);//Bind the geom to the body so that we only need to place the body to also set the geom dBodySetMass(targetBox_body[q], &mtb); dBodyDisable(targetBox_body[q]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (targetBox_body[q], 1);//Gravity on //Pile 1 if(q < 22) { dBodySetPosition(targetBox_body[q], (-60 + (s * 4)), (2 + r), -30);//Set position here, otherwise unaffected by gravity s++;//update x axis if (q == 8) { r += 4; s = 0; } if (q == 14) { r += 4; s = 0; } if (q == 18) { r += 4; s = 0; } if (q == 20) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 21) { r = 0; s = 0; } } //Pile 2 if((q > 21) && (q < 43)) { dBodySetPosition(targetBox_body[q], (-30 + (s * 4)), (2 + r), 90); s++;//Update x pos if (q == 29) { r += 4; s = 0; } if (q == 35) { r += 4; s = 0; } if (q == 39) { r += 4; s = 0; } if (q == 41) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 42) { r = 0; s = 0; } } //Pile 3 if((q > 42) && (q < 64)) { dBodySetPosition(targetBox_body[q], (110), (2 + r), (0 + (s * 4))); s++;//Update x pos if (q == 50) { r += 4; s = 0; } if (q == 56) { r += 4; s = 0; } if (q == 60) { r += 4; s = 0; } if (q == 62) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 63) { r = 0; s = 0; } } //Pile 4 if((q > 63) && (q < 85)) { dBodySetPosition(targetBox_body[q], (-90), (2 + r), (0 + (s * 4))); s++;//Update x pos if (q == 71) { r += 4; s = 0; } if (q == 77) { r += 4; s = 0; } if (q == 81) { r += 4; s = 0; } if (q == 83) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 84) { r = 0; s = 0; } } //Box tower 1 if((q > 84) && (q < 94)) { dBodySetPosition(targetBox_body[q], (-60), (2 + r), (30)); r+=1;//Put boxes slightly inside each other if (q == 93) { r = 0;//Reset r } } //Box tower 2 if((q > 93) && (q < 104)) { dBodySetPosition(targetBox_body[q], (70), (2 + r), (-90)); r+=1;//Put boxes slightly inside each other if (q == 103) { r = 0;//Reset r } } //Box tower 3 if((q > 103) && (q < 114)) { dBodySetPosition(targetBox_body[q], (60), (2 + r), (90)); r+=1;//Put boxes slightly inside each other if (q == 103) { r = 0;//Reset r } } } //*******End of targetBox Physics //Set the ground location: //First three param's set the normal vector, last param sets the distance according to the plane equation equation a*x+b*y+c*z=d and must have length 1 ground = dCreatePlane(spacePhy, 0, 2.0, 0, 0);//Plane where I have most objects sitting, y =2 // Here I have set the initial linear velocity to stationary and let gravity do the work on our spheres, but you can experiment // with the velocity vector to change the starting behavior. You can also set the rotational velocity for the new // body using dBodySetAngularVel which takes the same parameters. tempVect.x = 0.0; tempVect.y = 0.0; tempVect.z = 0.0; dBodySetLinearVel(ball_body, tempVect.x, tempVect.y, tempVect.z); dBodySetLinearVel(ball_body2, tempVect.x, tempVect.y, tempVect.z); dBodySetLinearVel(ball_body3, tempVect.x, tempVect.y, tempVect.z); dWorldSetLinearDamping(world, 0.00001); dWorldSetAngularDamping(world, 0.005); dWorldSetMaxAngularSpeed(world, 200); }
void PhysicsServer::SetERP( float val ) { m_ERP = val; if (m_WorldID) dWorldSetERP( m_WorldID, val ); } // PhysicsServer::SetERP
OdeInit::OdeInit(RobotConfig *config) : mutex(1), robot_config(config) { //create the world parameters world = dWorldCreate(); space = dHashSpaceCreate (0); contactgroup = dJointGroupCreate (0); verbose = false; dWorldSetGravity (world,0,-9.8,0); dWorldSetERP(world, config->getWorldERP()); // error reduction parameter: in [0.1,0.8], the higher, the more springy constraints are dWorldSetCFM(world, config->getWorldCFM()); // constraint force mixing: in [1e-9,1], the higher, the softer constraints are // Maximum correcting velocity the contacts are allowed to generate. Default value is infinity. // Reducing it can help prevent "popping" of deeply embedded objects dWorldSetContactMaxCorrectingVel(world, config->getMaxContactCorrectingVel()); // Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. // The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering // problems due to contacts being repeatedly made and broken. dWorldSetContactSurfaceLayer(world, config->getContactSurfaceLayer()); ground = dCreatePlane (space,0, 1, 0, 0); //feedback = new dJointFeedback; //feedback1 = new dJointFeedback; //feedback_mat = new dJointFeedback; _iCub = new ICubSim(world, space, 0,0,0, *robot_config); _wrld = new worldSim(world, space, 0,0,0, *robot_config); _controls = new iCubSimulationControl*[MAX_PART]; // // info on spaces created - relevant in actSelfCol mode // printf("ICubSim::init(): overview of the spaces created: \n"); // std::string s("space"); // printInfoOnSpace(space,s); // s="iCub"; // printInfoOnSpace(_iCub->iCub,s); // s="iCubHeadSpace"; // printInfoOnSpace(_iCub->iCubHeadSpace,s); // s="iCubTorsoSpace"; // printInfoOnSpace(_iCub->iCubTorsoSpace,s); // s="iCubLeftArmSpace"; // printInfoOnSpace(_iCub->iCubLeftArmSpace,s); // s="iCubRightArmSpace"; // printInfoOnSpace(_iCub->iCubRightArmSpace,s); // s="iCubLegsSpace"; // printInfoOnSpace(_iCub->iCubLegsSpace,s); // initialize at NULL for (int i=0; i<MAX_PART; i++) { _controls[i] = NULL; } _wrld->OBJNUM = 0; _wrld->waitOBJ = 0; _wrld->S_OBJNUM = 0; _wrld->SPHNUM = 0; _wrld->waitSPH = 0; _wrld->S_SPHNUM = 0; _wrld->cylOBJNUM = 0; _wrld->waitOBJ1 = 0; _wrld->S_cylOBJNUM = 0; _wrld->waitMOD = 0; _wrld->MODEL_NUM = 0; _wrld->s_waitMOD = 0; _wrld->s_MODEL_NUM = 0; }