static void _soy_joints_slider_axis_set (soyjointsSlider* self, soyatomsAxis* value) { soyatomsAxis* _tmp0_; soyatomsAxis* _tmp1_; soyatomsAxis* axis; soyscenesScene* _tmp2_; struct dxJoint* _tmp3_; gfloat _tmp4_; gfloat _tmp5_; gfloat _tmp6_; gfloat _tmp7_; gfloat _tmp8_; gfloat _tmp9_; soyscenesScene* _tmp10_; g_return_if_fail (self != NULL); g_return_if_fail (value != NULL); _tmp0_ = value; _tmp1_ = soy_atoms_axis_new_normalize (_tmp0_); axis = _tmp1_; _tmp2_ = ((soyjointsJoint*) self)->scene; g_rw_lock_writer_lock (&_tmp2_->stepLock); _tmp3_ = ((soyjointsJoint*) self)->joint; _tmp4_ = soy_atoms_axis_get_x (axis); _tmp5_ = _tmp4_; _tmp6_ = soy_atoms_axis_get_y (axis); _tmp7_ = _tmp6_; _tmp8_ = soy_atoms_axis_get_z (axis); _tmp9_ = _tmp8_; dJointSetSliderAxis ((struct dxJoint*) _tmp3_, (dReal) _tmp5_, (dReal) _tmp7_, (dReal) _tmp9_); _tmp10_ = ((soyjointsJoint*) self)->scene; g_rw_lock_writer_unlock (&_tmp10_->stepLock); _g_object_unref0 (axis); }
static void soy_joints_slider_real_setup (soyjointsJoint* base, soyatomsPosition* anchor, soyatomsAxis* axis1, soyatomsAxis* axis2) { soyjointsSlider * self; struct dxJoint* _tmp0_; soyatomsAxis* _tmp1_; gfloat _tmp2_; gfloat _tmp3_; soyatomsAxis* _tmp4_; gfloat _tmp5_; gfloat _tmp6_; soyatomsAxis* _tmp7_; gfloat _tmp8_; gfloat _tmp9_; self = (soyjointsSlider*) base; _tmp0_ = ((soyjointsJoint*) self)->joint; _tmp1_ = axis1; _tmp2_ = soy_atoms_axis_get_x (_tmp1_); _tmp3_ = _tmp2_; _tmp4_ = axis1; _tmp5_ = soy_atoms_axis_get_y (_tmp4_); _tmp6_ = _tmp5_; _tmp7_ = axis1; _tmp8_ = soy_atoms_axis_get_z (_tmp7_); _tmp9_ = _tmp8_; dJointSetSliderAxis ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_); }
Rope::Rope(Object* obja, Body* bodya, Object* objb, Body* bodyb) : obja_(obja), objb_(objb), selected_(false) { vec apos = bodya->position(); proxya_.set_position(apos); proxya_.set_velocity(bodya->velocity()); proxya_.set_mass(0.01, 1); hingea_ = dJointCreateHinge(LEVEL->world, 0); dJointAttach(hingea_, proxya_.body_id(), bodya->body_id()); dJointSetHingeAxis(hingea_, 0, 0, 1); vec bpos = bodyb->position(); proxyb_.set_position(bpos); proxyb_.set_velocity(bodyb->velocity()); proxyb_.set_mass(0.01, 1); hingeb_ = dJointCreateHinge(LEVEL->world, 0); dJointAttach(hingeb_, proxyb_.body_id(), bodyb->body_id()); dJointSetHingeAxis(hingeb_, 0, 0, 1); rope_ = dJointCreateSlider(LEVEL->world, 0); vec axis = bpos - apos; dJointAttach(rope_, proxya_.body_id(), proxyb_.body_id()); dJointSetSliderAxis(rope_, axis.x, axis.y, 0); dJointSetSliderParam(rope_, dParamLoStop, 0); dJointSetSliderParam(rope_, dParamStopCFM, 0.25); dJointSetSliderParam(rope_, dParamStopERP, 0.01); ext_ = base_ext_ = axis.norm(); }
void Slider::createPhysics() { ASSERT(axis); ASSERT(axis->motor); // axis->create(); // ::PhysicalObject::createPhysics(); // find bodies to connect Body* parentBody = dynamic_cast<Body*>(parent); ASSERT(!parentBody || parentBody->body); ASSERT(!children.empty()); Body* childBody = dynamic_cast<Body*>(*children.begin()); ASSERT(childBody); ASSERT(childBody->body); // create joint joint = dJointCreateSlider(Simulation::simulation->physicalWorld, 0); dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0); //set Slider joint parameter Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z); dJointSetSliderAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z); if(axis->cfm != -1.f) dJointSetSliderParam(joint, dParamCFM, dReal(axis->cfm)); if(axis->deflection) { const Axis::Deflection& deflection = *axis->deflection; float minSliderLimit = deflection.min; float maxSliderLimit = deflection.max; if(minSliderLimit > maxSliderLimit) minSliderLimit = maxSliderLimit; //Set physical limits to higher values (+10%) to avoid strange Slider effects. //Otherwise, sometimes the motor exceeds the limits. float internalTolerance = (maxSliderLimit - minSliderLimit) * 0.1f; if(dynamic_cast<ServoMotor*>(axis->motor)) { minSliderLimit -= internalTolerance; maxSliderLimit += internalTolerance; } dJointSetSliderParam(joint, dParamLoStop, dReal(minSliderLimit)); dJointSetSliderParam(joint, dParamHiStop, dReal(maxSliderLimit)); // this has to be done due to the way ODE sets joint stops dJointSetSliderParam(joint, dParamLoStop, dReal(minSliderLimit)); if(deflection.stopCFM != -1.f) dJointSetSliderParam(joint, dParamStopCFM, dReal(deflection.stopCFM)); if(deflection.stopERP != -1.f) dJointSetSliderParam(joint, dParamStopERP, dReal(deflection.stopERP)); } // create motor axis->motor->create(this); OpenGLTools::convertTransformation(rotation, translation, transformation); }
void CODESliderJoint::setParams() { dJointSetSliderAxis(mID, mAxisX, mAxisY, mAxisZ); dJointSetSliderParam(mID, dParamLoStop, mLowerLimit); dJointSetSliderParam(mID, dParamHiStop, mUpperLimit); if (mStopERP >= 0) dJointSetSliderParam(mID, dParamStopERP, mStopERP); if (mStopCFM >= 0) dJointSetSliderParam(mID, dParamStopCFM, mStopCFM); }
/** * This method is called if the joint should be attached. * It creates the ODE-joint, calculates the current * axis-orientation and attaches the Joint. * @param obj1 first ODE-object to attach with * @param obj2 second ODE-object to attach with **/ void SliderJoint::attachJoint(dBodyID obj1, dBodyID obj2) { gmtl::Vec3f newAxis; gmtl::Quatf entityRot; gmtl::AxisAnglef axAng; gmtl::Vec3f scale = gmtl::Vec3f(1,1,1); TransformationData entityTrans; joint = dJointCreateSlider(world, 0); dJointAttach(joint, obj1, obj2); newAxis = axis; if (mainEntity != NULL) { entityTrans = mainEntity->getEnvironmentTransformation(); // scale Axis by mainEntity-scale value because of possible distortion /* scale[0] = mainEntity->getXScale(); scale[1] = mainEntity->getYScale(); scale[2] = mainEntity->getZScale();*/ scale = entityTrans.scale; newAxis[0] *= scale[0]; newAxis[1] *= scale[1]; newAxis[2] *= scale[2]; gmtl::normalize(newAxis); // get the Rotation of the mainEntity // axAng[0] = mainEntity->getRotAngle(); // axAng[1] = mainEntity->getXRot(); // axAng[2] = mainEntity->getYRot(); // axAng[3] = mainEntity->getZRot(); // gmtl::set(entityRot, axAng); entityRot = entityTrans.orientation; // rotate Axis by mainEntity-rotation newAxis *= entityRot; } // if dJointSetSliderAxis(joint, newAxis[0], newAxis[1], newAxis[2]); // set the minimum and maximum joint-positions (if set). if (posSet) { gmtl::Vec3f minDistVec = newAxis * minPos; gmtl::Vec3f maxDistVec = newAxis * maxPos; minDistVec[0] *= scale[0]; minDistVec[1] *= scale[1]; minDistVec[2] *= scale[2]; maxDistVec[0] *= scale[0]; maxDistVec[1] *= scale[1]; maxDistVec[2] *= scale[2]; dJointSetSliderParam(joint, dParamLoStop, gmtl::dot(minDistVec,newAxis)); dJointSetSliderParam(joint, dParamHiStop, gmtl::dot(maxDistVec,newAxis)); } // if } // attachJoint
static void set_phys_joint_axis_1(dJointID j, const float v[3]) { switch (dJointGetType(j)) { case dJointTypeHinge: dJointSetHingeAxis (j, v[0], v[1], v[2]); break; case dJointTypeSlider: dJointSetSliderAxis (j, v[0], v[1], v[2]); break; case dJointTypeHinge2: dJointSetHinge2Axis1 (j, v[0], v[1], v[2]); break; case dJointTypeUniversal: dJointSetUniversalAxis1(j, v[0], v[1], v[2]); break; default: break; } }
/** *@brief 姿勢設定の関数 * @param r ロール * @param p ピッチ * @param y ヨー */ void ODEJointObj::SetJointRotation(double r, double p, double y) { ms->mu->lock(); if(JointType == 0) { dJointSetSliderAxis(joint, r, p, y); } else if(JointType == 2) { dJointSetHingeAxis(joint, r, p, y); //SetPosition(bscale_x*px + offx, bscale_y*py + offy, bscale_z*pz + offz); } axisx = r; axisy = p; axisz = y; ms->mu->unlock(); }
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) { fn.path_to_textures = argv[1]; } // create world dInitODE2(0); 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); dCloseODE(); return 0; }
OscSlideODE::OscSlideODE(dWorldID odeWorld, dSpaceID odeSpace, const char *name, OscBase *parent, OscObject *object1, OscObject *object2, double ax, double ay, double az) : OscSlide(name, parent, object1, object2, ax, ay, az) { m_response = new OscResponse("response",this); dJointID odeJoint = dJointCreateSlider(odeWorld,0); m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace, object1, object2); dJointSetSliderAxis(odeJoint, ax, ay, az); /* TODO access to dJointGetSliderPosition */ printf("[%s] Sliding joint created between %s and %s on axis (%f,%f,%f)\n", simulation()->type_str(), object1->c_name(), object2?object2->c_name():"world", ax, ay, az); }
/** * Creates a new IBDS::SliderJoint. * * @param body1 the first body to connect the joint to. * @param body2 the second body to connect the joint to. * @return a new IBDS::SliderJoint. */ dJointID ODE_SliderJoint::createJoint(dBodyID body1, dBodyID body2) { if(mFirstAxisPoint->get().equals(mSecondAxisPoint->get())) { Core::log("Invalid axes for ODE_SliderJoint."); return 0; } //if one of the bodyIDs is null, the joint is connected to a static object. dJointID newJoint = dJointCreateSlider(mWorldID, mGeneralJointGroup); dJointAttach(newJoint, body1, body2); Vector3D axis(mSecondAxisPoint->getX() - mFirstAxisPoint->getX(), mSecondAxisPoint->getY() - mFirstAxisPoint->getY(), mSecondAxisPoint->getZ() - mFirstAxisPoint->getZ()); axis.normalize(); dJointSetSliderAxis(newJoint, axis.getX(), axis.getY(), axis.getZ()); dJointSetSliderParam(newJoint, dParamLoStop, mMinPositionValue->get()); dJointSetSliderParam(newJoint, dParamHiStop, mMaxPositionValue->get()); dJointSetSliderParam(newJoint, dParamVel, 0.0); dJointSetSliderParam(newJoint, dParamFMax, mFrictionValue->get()); return newJoint; }
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign) { // chassis pin joints const dReal *pos; snow->jChassisUp= dJointCreateHinge(world,0); dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body); dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0); pos= dBodyGetPosition(snow->uplink.body); dJointSetHingeAnchor(snow->jChassisUp,pos[0] ,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]); const dReal *pos1; snow->jChassisLow= dJointCreateHinge(world,0); dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body); dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0); pos1= dBodyGetPosition(snow->lowlink.body); dJointSetHingeAnchor(snow->jChassisLow, pos1[0] , pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]); // rotor ball joint /*const dReal *p2; jRotorUp= dJointCreateBall(world,0); dJointAttach(jRotorUp, uplink.body, hlink.body); p2= dBodyGetPosition(uplink.body); dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]); const dReal *p3; jRotorLow= dJointCreateBall(world,0); dJointAttach(jRotorLow, lowlink.body,hlink.body); p3= dBodyGetPosition(lowlink.body); dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/ const dReal *p2; snow->jRotorUp= dJointCreateHinge(world,0); dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body); p2= dBodyGetPosition(snow->uplink.body); dJointSetHingeAnchor(snow->jRotorUp, p2[0] ,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]); dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0); const dReal *p3; snow->jRotorLow= dJointCreateHinge(world,0); dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body); p3= dBodyGetPosition(snow->lowlink.body); dJointSetHingeAnchor(snow->jRotorLow, p3[0] ,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]); dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0); // rotor hinge joint const dReal *pw= dBodyGetPosition(wnow->body); snow->jRotorMid= dJointCreateHinge(world,0); dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body); dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0); dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]); // strut joint const dReal *ps1, *ps2; dReal angle= -shiftSign*strutAngle; ps1= dBodyGetPosition(snow->upstrut.body); ps2= dBodyGetPosition(snow->lowstrut.body); snow->jStrutUp= dJointCreateBall(world,0); dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body); //dJointSetFixed(snow->jStrutUp); dJointSetBallAnchor(snow->jStrutUp, ps1[0] ,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle)) ,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle))); snow->jStrutLow= dJointCreateBall(world,0); dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body); dJointSetBallAnchor(snow->jStrutLow, ps2[0] ,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle)) ,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle))); // struct sliding joint snow->jStrutMid= dJointCreateSlider(world,0); dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body); dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle))); // set joint feedback wnow->feedback= new dJointFeedback; dJointSetFeedback(snow->jStrutMid,wnow->feedback); // suspension slider /*snow->jLowSpring= dJointCreateLMotor(world,0); dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body); dJointSetLMotorNumAxes(snow->jLowSpring, 1); dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.0);*/ }
void Machine::init() { int i; pushtime=0; energy=4; dMass m; for(i=0; i<3; i++) { wheel[i] = dBodyCreate(world); dMassSetSphere(&m, 1, 5); dMassAdjust(&m, 2); dBodySetMass(wheel[i], &m); sphere[i] = dCreateSphere(0, 5); dGeomSetBody(sphere[i], wheel[i]); } dBodySetPosition(wheel[0], 0, 12, 6); dBodySetPosition(wheel[1], -6, -7, 6); dBodySetPosition(wheel[2], 6, -7, 6); body[0] = dBodyCreate(world); dMassSetBox(&m, 1, 20, 80, 5); dMassAdjust(&m, 5); dBodySetMass(body[0], &m); dBodySetPosition(body[0], 0, 0, 6.5); geom[0] = dCreateBox(0, 19, 27, 10); dGeomSetBody(geom[0], body[0]); body[1] = dBodyCreate(world); dMassSetBox(&m, 1, 11, 5, 10); dMassAdjust(&m, 0.3); dBodySetMass(body[1], &m); dBodySetPosition(body[1], 0, 17, 6.5); geom[1] = dCreateBox(0, 11, 5, 10); dGeomSetBody(geom[1], body[1]); joint = dJointCreateSlider(world, 0); dJointAttach(joint, body[0], body[1]); dJointSetSliderAxis(joint, 0, 1, 0); dJointSetSliderParam(joint, dParamLoStop, -9); dJointSetSliderParam(joint, dParamHiStop, 0); for(i=0; i<2; i++) { geom[i+2] = dCreateGeomTransform(0); dGeomTransformSetCleanup(geom[i+2], 1); finE[i] = dCreateBox(0, 7, 5, 10); dGeomSetPosition(finE[i], i==0?-6.3:6.3, -2, 0); dMatrix3 R; dRFromAxisAndAngle(R, 0, 0, 1, i==0?M_PI/4:-M_PI/4); dGeomSetRotation(finE[i], R); dGeomTransformSetGeom(geom[i+2], finE[i]); dGeomSetBody(geom[i+2], body[1]); } for(i=0; i<3; i++) { wheeljoint[i] = dJointCreateHinge2(world, 0); dJointAttach(wheeljoint[i], body[0], wheel[i]); const dReal *wPos = dBodyGetPosition(wheel[i]); dJointSetHinge2Anchor(wheeljoint[i], wPos[0], wPos[1], wPos[2]); dJointSetHinge2Axis1(wheeljoint[i], 0, 0, 1); dJointSetHinge2Axis2(wheeljoint[i], 1, 0, 0); dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0.8); dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0.01); dJointSetHinge2Param(wheeljoint[i], dParamLoStop, 0); dJointSetHinge2Param(wheeljoint[i], dParamHiStop, 0); dJointSetHinge2Param(wheeljoint[i], dParamCFM, 0.0001); dJointSetHinge2Param(wheeljoint[i], dParamStopERP, 0.8); dJointSetHinge2Param(wheeljoint[i], dParamStopCFM, 0.0001); } reset(); }
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, 20); int i; for (i=0; i<SEGMCNT; i++) { segbodies[i] = dBodyCreate (world); dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5); dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]); dBodySetMass (segbodies[i], &m); seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]); dGeomSetBody (seggeoms[i], segbodies[i]); dSpaceAdd (space, seggeoms[i]); } for (i=0; i<SEGMCNT-1; i++) { hinges[i] = dJointCreateHinge (world,0); dJointAttach (hinges[i], segbodies[i],segbodies[i+1]); dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5); dJointSetHingeAxis (hinges[i], 0,1,0); dJointSetHingeParam (hinges[i],dParamFMax, 8000.0); // NOTE: // Here we tell ODE where to put the feedback on the forces for this hinge dJointSetFeedback (hinges[i], jfeedbacks+i); stress[i]=0; } for (i=0; i<STACKCNT; i++) { stackbodies[i] = dBodyCreate(world); dMassSetBox (&m, 2.0, 2, 2, 0.6); dBodySetMass(stackbodies[i],&m); stackgeoms[i] = dCreateBox(0, 2, 2, 0.6); dGeomSetBody(stackgeoms[i], stackbodies[i]); dBodySetPosition(stackbodies[i], 0,0,8+2*i); dSpaceAdd(space, stackgeoms[i]); } sliders[0] = dJointCreateSlider (world,0); dJointAttach(sliders[0], segbodies[0], 0); dJointSetSliderAxis (sliders[0], 1,0,0); dJointSetSliderParam (sliders[0],dParamFMax, 4000.0); dJointSetSliderParam (sliders[0],dParamLoStop, 0.0); dJointSetSliderParam (sliders[0],dParamHiStop, 0.2); sliders[1] = dJointCreateSlider (world,0); dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0); dJointSetSliderAxis (sliders[1], 1,0,0); dJointSetSliderParam (sliders[1],dParamFMax, 4000.0); dJointSetSliderParam (sliders[1],dParamLoStop, 0.0); dJointSetSliderParam (sliders[1],dParamHiStop, -0.2); groundgeom = dCreatePlane(space, 0,0,1,0); for (i=0; i<SEGMCNT; i++) colours[i]=0.0; // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dJointGroupEmpty (contactgroup); dJointGroupDestroy (contactgroup); // First destroy seggeoms, then space, then the world. for (i=0; i<SEGMCNT; i++) dGeomDestroy (seggeoms[i]); for (i=0; i<STACKCNT; i++) dGeomDestroy (stackgeoms[i]); dSpaceDestroy(space); dWorldDestroy (world); dCloseODE(); return 0; }
void makeRobot_Nleg() { for(int segment = 0; segment < BODY_SEGMENTS; ++segment) { dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT; dReal torso_m = 50.0; // Mass of body // torso_m[segment] = 10.0; dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments //for four legs // dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate) // {-cx1,-cx1,-cx1}}; dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate) {0,0,0}}; dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate) {-cy1,-cy1,-cy1}}; dReal z[num_legs][num_links] = { // Position of each link (z coordinate) {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}, {c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} }; dReal r[num_links] = { r1, r2, r3}; // radius of leg segment dReal length[num_links] = { l1, l2, l3}; // Length of leg segment dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment // //for one leg // dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}}; // dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}}; // dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}}; //for four legs dReal axis_x[num_legs][num_links]; dReal axis_y[num_legs][num_links]; dReal axis_z[num_legs][num_links]; for(int i = 0; i < num_legs; ++i) { axis_x[i][0] = 0.0; axis_x[i][1] = 1.0; axis_x[i][2] = 0.0; axis_y[i][0] = 1.0; axis_y[i][1] = 0.0; axis_y[i][2] = 1.0; axis_z[i][0] = 0.0; axis_z[i][1] = 0.0; axis_z[i][2] = 0.0; } // For mation of the body dMass mass; torso[segment].body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass,torso_m, lx, ly, lz); dBodySetMass(torso[segment].body,&mass); torso[segment].geom = dCreateBox(space,lx, ly, lz); dGeomSetBody(torso[segment].geom, torso[segment].body); dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ); // Formation of leg dMatrix3 R; // Revolution queue dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++) { THETA[segment][i][j] = 0; leg[segment][i][j].body = dBodyCreate(world); if (j == 0) dBodySetRotation(leg[segment][i][j].body,R); dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]); dBodySetMass(leg[segment][i][j].body, &mass); //if(i==1 and j==2) //to set the length of one leg differently //leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg //else leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body); } } // Formation of joints (and connecting them up) for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++) { leg[segment][i][j].joint = dJointCreateHinge(world, 0); if (j == 0){ dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop, .50*M_PI); } else dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body); dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]); dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]); } } } #ifdef USE_NLEG_ROBOT // link torsos for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) { dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT; switch (hingeType) { case 1: //Hinge Joint, X axis (back-breaker) torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0); break; case 2: //Hinge Joint, Y axis (???) torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0); break; case 3: //Hinge Joint, Z axis (snake-like) torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0); break; case 4: // Slider, Y axis (??) torso[segment].joint = dJointCreateSlider(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0); break; case 5: // Slider, X axis (extendo) torso[segment].joint = dJointCreateSlider(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0); break; case 6: //Universal Joint torso[segment].joint = dJointCreateUniversal(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0); dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0); break; case 7: //Ball Joint torso[segment].joint = dJointCreateBall(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); break; case 8: // Fixed torso[segment].joint = dJointCreateHinge(world, 0); dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body); dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ); dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0); dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees dJointSetHingeParam(torso[segment].joint, dParamHiStop, 0.00*M_PI); break; default: assert(false); // not a valid hinge type break; } } #endif }
void PhysicsSliderJoint::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { //Do not respond to changes that have a Sync origin if(origin & ChangedOrigin::Sync) { return; } if(whichField & WorldFieldMask) { if(_JointID) { dJointDestroy(_JointID); _JointID = dJointCreateSlider(getWorld()->getWorldID(), 0); } else { _JointID = dJointCreateSlider(getWorld()->getWorldID(), 0); if(!(whichField & HiStopFieldMask)) { setHiStop(dJointGetSliderParam(_JointID,dParamHiStop)); } if(!(whichField & LoStopFieldMask)) { setLoStop(dJointGetSliderParam(_JointID,dParamLoStop)); } if(!(whichField & BounceFieldMask)) { setBounce(dJointGetSliderParam(_JointID,dParamBounce)); } if(!(whichField & CFMFieldMask)) { setCFM(dJointGetSliderParam(_JointID,dParamCFM)); } if(!(whichField & StopCFMFieldMask)) { setStopCFM(dJointGetSliderParam(_JointID,dParamStopCFM)); } if(!(whichField & StopERPFieldMask)) { setStopERP(dJointGetSliderParam(_JointID,dParamStopERP)); } } } Inherited::changed(whichField, origin, details); if((whichField & AxisFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z()); } if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderParam(_JointID, dParamHiStop, getHiStop()); } if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderParam(_JointID, dParamLoStop, getLoStop()); } if((whichField & BounceFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderParam(_JointID, dParamBounce, getBounce()); } if((whichField & CFMFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderParam(_JointID, dParamCFM, getCFM()); } if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderParam(_JointID, dParamStopERP, getStopERP()); } if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask)) { dJointSetSliderParam(_JointID, dParamStopCFM, getStopCFM()); } }
void WorldPhysics::AI() { if (tmp_wait>0) {tmp_wait--;return;} if (bulldozer_state==8) speed=0; const dReal* BPosition=bulldozer->getPosition(); // const dReal* BRotation=bulldozer->getRotation(); dReal minDistance=150; //~100 * sqrt(2) Item* tmp; int currentItemN=-1; if (items.size()==0) return; for (int i=0;i<items.size();i++) { const dReal* ItemPosition=items[i]->getPosition(); if (ItemPosition[2]<0) items[i]->state=2; if (items[i]->state==2) continue; if (items[i]->state==1) {currentItemN=i;break;} if (items[i]->state==0) { dReal tmpmin=sqrt((ItemPosition[0]-BPosition[0])* (ItemPosition[0]-BPosition[0]) + (ItemPosition[1]-BPosition[1])* (ItemPosition[1]-BPosition[1])); if (tmpmin<minDistance) { minDistance=tmpmin; currentItemN=i; } } } if (currentItemN==-1) { //bulldozer_state=0; generateItems(); return; } /* for (int i=0;i<items.size();i++) { const dReal* tmp_pos=items[i]->getPosition(); if (tmp_pos[2]<0) items[i]->state=2; if (items[i]->state==1) { currentItemN=i; break; } if (items[i]->state==2) currentItemN++; } if (currentItemN>=items.size()) return; */ tmp=items[currentItemN]; const dReal* ItemPosition=tmp->getPosition(); switch (tmp->state) { case 0: { // Выбрали нужный кубик tmp->state=1; bulldozer_state=1; // Поворот до кубика } break; case 1: { // Надо к нему подъехать switch (bulldozer_state) { case 1:{ speed=0; if (RotateTo(ItemPosition[0]-BPosition[0],ItemPosition[1]-BPosition[1])) bulldozer_state=2; } break; case 2: { if (RotateTo(ItemPosition[0]-BPosition[0],ItemPosition[1]-BPosition[1])) { speed=3; if (((ItemPosition[0]-BPosition[0])* (ItemPosition[0]-BPosition[0]) + (ItemPosition[1]-BPosition[1])* (ItemPosition[1]-BPosition[1]))<50) { speed=0; bulldozer_state=3; } } else { speed=0; bulldozer_state=1; } } break; case 3: { cheat_joint=dJointCreateSlider(world,0); dJointAttach(cheat_joint,bulldozer->body,tmp->body); dJointSetSliderAxis(cheat_joint,0,0,1); //dJointSetSliderParam (cheat_joint, dParamCFM, 0.5); bulldozer_state=4; }break; case 4: { if (RotateTo(100-BPosition[0],0-BPosition[1])) bulldozer_state=5; }break; case 5: { if (RotateTo(100-BPosition[0],0-BPosition[1])) { speed=3; if (sqrt((100-BPosition[0])* (100-BPosition[0]) + (0-BPosition[1])* (0-BPosition[1]))<(sqrt(2)*25+LENGTH/2+RADIUS)) { speed=0; bulldozer_state=6; } } else { speed=0; bulldozer_state=4; } } break; case 6: { dJointDestroy(cheat_joint); dBodyAddForce(tmp->body,100-BPosition[0],0-BPosition[1], 5); bulldozer_state=7; } break; case 7: { speed=-10; tmp_wait=70; bulldozer_state=8; } break; case 8: { speed=0; tmp->state=2; } break; } } break; } }
int setupTest (int n) { switch (n) { // ********** fixed joint case 0: { // 2 body constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],body[1]); dJointSetFixed (joint); return 1; } case 1: { // 1 body to static env constructWorldForTest (0,1, 0.5*SIDE,0.5*SIDE,1, 0,0,0, 1,0,0, 1,0,0, 0,0); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],0); dJointSetFixed (joint); return 1; } case 2: { // 2 body with relative rotation constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,-0.25*M_PI); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],body[1]); dJointSetFixed (joint); return 1; } case 3: { // 1 body to static env with relative rotation constructWorldForTest (0,1, 0.5*SIDE,0.5*SIDE,1, 0,0,0, 1,0,0, 1,0,0, 0.25*M_PI,0); joint = dJointCreateFixed (world,0); dJointAttach (joint,body[0],0); dJointSetFixed (joint); return 1; } // ********** hinge joint case 200: // 2 body constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,1,-1,1.41421356); return 1; case 220: // hinge angle polarity test case 221: // hinge angle rate test constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,0,1); max_iterations = 50; return 1; case 230: // hinge motor rate (and polarity) test case 231: // ...with stops constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,0,1); dJointSetHingeParam (joint,dParamFMax,1); if (n==231) { dJointSetHingeParam (joint,dParamLoStop,-0.5); dJointSetHingeParam (joint,dParamHiStop,0.5); } return 1; case 250: // limit bounce test (gravity down) case 251: { // ...gravity up constructWorldForTest ((n==251) ? 0.1 : -0.1, 2, 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHingeAnchor (joint,0,0,1); dJointSetHingeAxis (joint,0,1,0); dJointSetHingeParam (joint,dParamLoStop,-0.9); dJointSetHingeParam (joint,dParamHiStop,0.7854); dJointSetHingeParam (joint,dParamBounce,0.5); // anchor 2nd body with a fixed joint dJointID j = dJointCreateFixed (world,0); dJointAttach (j,body[1],0); dJointSetFixed (j); return 1; } // ********** slider case 300: // 2 body constructWorldForTest (0,2, 0,0,1, 0.2,0.2,1.2, 0,0,1, -1,1,0, 0,0.25*M_PI); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,1,1,1); return 1; case 320: // slider angle polarity test case 321: // slider angle rate test constructWorldForTest (0,2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); max_iterations = 50; return 1; case 330: // slider motor rate (and polarity) test case 331: // ...with stops constructWorldForTest (0, 2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); dJointSetSliderParam (joint,dParamFMax,100); if (n==331) { dJointSetSliderParam (joint,dParamLoStop,-0.4); dJointSetSliderParam (joint,dParamHiStop,0.4); } return 1; case 350: // limit bounce tests case 351: { constructWorldForTest ((n==351) ? 0.1 : -0.1, 2, 0,0,1, 0,0,1.2, 1,0,0, 1,0,0, 0,0); joint = dJointCreateSlider (world,0); dJointAttach (joint,body[0],body[1]); dJointSetSliderAxis (joint,0,0,1); dJointSetSliderParam (joint,dParamLoStop,-0.5); dJointSetSliderParam (joint,dParamHiStop,0.5); dJointSetSliderParam (joint,dParamBounce,0.5); // anchor 2nd body with a fixed joint dJointID j = dJointCreateFixed (world,0); dJointAttach (j,body[1],0); dJointSetFixed (j); return 1; } // ********** hinge-2 joint case 420: // hinge-2 steering angle polarity test case 421: // hinge-2 steering angle rate test constructWorldForTest (0,2, 0.5*SIDE,0,1, -0.5*SIDE,0,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge2 (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); dJointSetHinge2Axis1 (joint,0,0,1); dJointSetHinge2Axis2 (joint,1,0,0); max_iterations = 50; return 1; case 430: // hinge 2 steering motor rate (+polarity) test case 431: // ...with stops case 432: // hinge 2 wheel motor rate (+polarity) test constructWorldForTest (0,2, 0.5*SIDE,0,1, -0.5*SIDE,0,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateHinge2 (world,0); dJointAttach (joint,body[0],body[1]); dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1); dJointSetHinge2Axis1 (joint,0,0,1); dJointSetHinge2Axis2 (joint,1,0,0); dJointSetHinge2Param (joint,dParamFMax,1); dJointSetHinge2Param (joint,dParamFMax2,1); if (n==431) { dJointSetHinge2Param (joint,dParamLoStop,-0.5); dJointSetHinge2Param (joint,dParamHiStop,0.5); } return 1; // ********** angular motor joint case 600: // test euler angle calculations constructWorldForTest (0,2, -SIDE*0.5,0,1, SIDE*0.5,0,1, 0,0,1, 0,0,1, 0,0); joint = dJointCreateAMotor (world,0); dJointAttach (joint,body[0],body[1]); dJointSetAMotorNumAxes (joint,3); dJointSetAMotorAxis (joint,0,1, 0,0,1); dJointSetAMotorAxis (joint,2,2, 1,0,0); dJointSetAMotorMode (joint,dAMotorEuler); max_iterations = 200; return 1; // ********** universal joint case 700: // 2 body case 701: case 702: constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI); joint = dJointCreateUniversal (world,0); dJointAttach (joint,body[0],body[1]); dJointSetUniversalAnchor (joint,0,0,1); dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356); dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356); return 1; case 720: // universal transmit torque test case 721: case 722: case 730: // universal torque about axis 1 case 731: case 732: case 740: // universal torque about axis 2 case 741: case 742: constructWorldForTest (0,2, 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1, 1,0,0, 1,0,0, 0,0); joint = dJointCreateUniversal (world,0); dJointAttach (joint,body[0],body[1]); dJointSetUniversalAnchor (joint,0,0,1); dJointSetUniversalAxis1 (joint,0,0,1); dJointSetUniversalAxis2 (joint, 1, -1,0); max_iterations = 100; return 1; } return 0; }
//SetAxis void JointSlider::SetAxis(double x, double y, double z) { dJointSetSliderAxis(this->_id, x, y, z); }
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); } }