void TrackedVehicle::create() { this->vehicleBody = dBodyCreate(this->environment->world); this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]); this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom"); dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]); //dMassAdjust(&this->vehicleMass, 2.40); dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE); dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN); dBodySetMass(this->vehicleBody, &this->vehicleMass); dGeomSetBody(this->vehicleGeom, this->vehicleBody); dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]); this->leftTrack->create(); this->rightTrack->create(); dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace; dRigidBodyArraySetPosition(leftTrack->bodyArray, -wheelBase/2, -(w - trackWidth)/2, 0); dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2, (w - trackWidth)/2, 0); this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0); this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0); dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody); dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody); dJointSetFixed(this->leftTrackJoint); dJointSetFixed(this->rightTrackJoint); this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody); dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray); dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray); }
OscFixedODE::OscFixedODE(dWorldID odeWorld, dSpaceID odeSpace, const char *name, OscBase* parent, OscObject *object1, OscObject *object2) : OscFixed(name, parent, object1, object2) { if (object2) { dJointID odeJoint = dJointCreateFixed(odeWorld,0); m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace, object1, object2); dJointSetFixed(odeJoint); } else { ODEObject *o = NULL; if (object1) o = dynamic_cast<ODEObject*>(object1->special()); if (o) o->disconnectBody(); m_pSpecial = new ODEConstraint(this, NULL, odeWorld, odeSpace, object1, object2); } printf("[%s] Fixed joint created between %s and %s.\n", simulation()->type_str(), object1->c_name(), object2?object2->c_name():"world"); }
void createInvisibleHead( float* pos ) { dMatrix3 head_orientation; dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0); //position and orientation head.Body = dBodyCreate(World); dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]); dBodySetRotation(head.Body, head_orientation); dBodySetLinearVel(head.Body, 0, 0, 0); dBodySetData(head.Body, (void *)0); //mass dMass head_mass; dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0); dBodySetMass(head.Body, &head_mass); //geometry head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0); dGeomSetBody(head.Geom, head.Body); //fixed joint invis_box_joint = dJointCreateFixed(World, jointgroup); dJointAttach(invis_box_joint, body.Body, head.Body); dJointSetFixed(invis_box_joint); }
/* ================================================================================= createFixedLeg Use parameters to create leg body/geom and attach to body with fixed joint ================================================================================= */ void createFixedLeg(ODEObject &leg, ODEObject &bodyAttachedTo, dJointID& joint, dReal xPos, dReal yPos, dReal zPos, dReal xRot, dReal yRot, dReal zRot, dReal radius, dReal length) { dMatrix3 legOrient; dRFromEulerAngles(legOrient, xRot, yRot, zRot); //position and orientation leg.Body = dBodyCreate(World); dBodySetPosition(leg.Body, xPos, yPos, zPos); dBodySetRotation(leg.Body, legOrient); dBodySetLinearVel(leg.Body, 0, 0, 0); dBodySetData(leg.Body, (void *)0); //mass dMass legMass; dMassSetCapsule(&legMass, 1, 3, radius, length); dBodySetMass(leg.Body, &legMass); //geometry leg.Geom = dCreateCapsule(Space, radius, length); dGeomSetBody(leg.Geom, leg.Body); //fixed joint joint = dJointCreateFixed(World, jointgroup); dJointAttach(joint, bodyAttachedTo.Body, leg.Body); dJointSetFixed(joint); }
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); }
void Spikey::stick(Body* b) { if (hinge_) dJointDestroy(hinge_); hinge_ = dJointCreateFixed(LEVEL->world, 0); dJointAttach(hinge_, body_.body_id(), b ? b->body_id() : 0); dJointSetFixed(hinge_); state_ = STUCK; }
/** * This method is used for activating or deactivating * the Joint. If the Joint was inactive and should be * activated the fixed ODE-joint is removed and the * virtual attachJoint-method is called. * If the Joint was active and should be deactivated * the virtual detachJoint-method is called and then * a fixed ODE-joint is created and attached to the bodies. * @param isActive 0 .. not active * anything else .. active **/ void Joint::setActive(int isActive) { dBodyID body1, body2; if (!active && isActive && joint) { dJointDestroy(joint); body1 = jointInteraction->getBodyWithID(entityID1); body2 = jointInteraction->getBodyWithID(entityID2); attachJoint(body1, body2); } // if else if (active && !isActive && joint) { // first detach joint e.g. for saving axis angles detachJoint(); dJointDestroy(joint); body1 = jointInteraction->getBodyWithID(entityID1); body2 = jointInteraction->getBodyWithID(entityID2); joint = dJointCreateFixed(world, 0); dJointAttach(joint, body1, body2); dJointSetFixed(joint); } // else if if (isActive) this->active = true; else this->active = false; } // setActive
IoODEFixed *IoODEFixed_rawClone(IoODEFixed *proto) { IoObject *self = IoODEJoint_rawClone(proto); if(DATA(proto)->jointGroup) { IoODEJointGroup *jointGroup = DATA(proto)->jointGroup; JOINTGROUP = jointGroup; IoODEJointGroup_addJoint(jointGroup, self); JOINTID = dJointCreateFixed(WORLDID, JOINTGROUPID); } return self; }
/*** ロボットアームの生成 ***/ void makeArm() { dMass mass; // 質量パラメータ dReal x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 x dReal y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 y dReal z[NUM] = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0}; // 重心 z dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50}; // 長さ dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50}; // 質量 dReal r[NUM-1] = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; // 半径 dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 x dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 y dReal c_z[NUM] = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9}; // 関節中心点 z dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0}; // 関節回転軸 x dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0}; // 関節回転軸 y dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1}; // 関節回転軸 z // リンクの生成 for (int i = 0; i < NUM-1; i++) { rlink[i].body = dBodyCreate(world); dBodySetPosition(rlink[i].body, x[i], y[i], z[i]); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]); dBodySetMass(rlink[i].body, &mass); rlink[i].geom = dCreateCapsule(space,r[i],length[i]); dGeomSetBody(rlink[i].geom,rlink[i].body); } rlink[NUM-1].body = dBodyCreate(world); dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]); dMassSetZero(&mass); dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4); dBodySetMass(rlink[NUM-1].body, &mass); rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4); dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body); // ジョイントの生成とリンクへの取り付け joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント dJointAttach(joint[0], rlink[0].body, 0); dJointSetFixed(joint[0]); for (int j = 1; j < NUM; j++) { joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント dJointAttach(joint[j], rlink[j].body, rlink[j-1].body); dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]); dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]); } }
sODEJoint* sODEJoint::AddFixedJoint( int id, dBodyID body1, dBodyID body2 ) { sODEJoint *pJoint = new sODEJoint( ); //modify prev and next pointers of any involved joints, make new joint the new head pJoint->pNextJoint = pODEJointList; if ( pODEJointList ) pODEJointList->pPrevJoint = pJoint; pODEJointList = pJoint; //create the ODE hinge joint pJoint->oJoint = dJointCreateFixed( g_ODEWorld, 0 ); dJointAttach( pJoint->oJoint, body1, body2 ); dJointSetFixed( pJoint->oJoint ); pJoint->iID = id; return pJoint; }
Robot::Robot(PWorld* world,PBall *ball,ConfigWidget* _cfg,float x,float y,float z,float r,float g,float b,int rob_id,int wheeltexid,int dir) { m_r = r; m_g = g; m_b = b; m_x = x; m_y = y; m_z = z; w = world; m_ball = ball; m_dir = dir; cfg = _cfg; m_rob_id = rob_id; space = w->space; chassis = new PCylinder(x,y,z,cfg->robotSettings.RobotRadius,cfg->robotSettings.RobotHeight,cfg->robotSettings.BodyMass*0.99f,r,g,b,rob_id,true); chassis->space = space; w->addObject(chassis); dummy = new PBall(x,y,z,cfg->robotSettings.RobotCenterFromKicker,cfg->robotSettings.BodyMass*0.01f,0,0,0); dummy->setVisibility(false); dummy->space = space; w->addObject(dummy); dummy_to_chassis = dJointCreateFixed(world->world,0); dJointAttach (dummy_to_chassis,chassis->body,dummy->body); kicker = new Kicker(this); wheels[0] = new Wheel(this,0,cfg->robotSettings.Wheel1Angle,cfg->robotSettings.Wheel1Angle,wheeltexid); wheels[1] = new Wheel(this,1,cfg->robotSettings.Wheel2Angle,cfg->robotSettings.Wheel2Angle,wheeltexid); wheels[2] = new Wheel(this,2,cfg->robotSettings.Wheel3Angle,cfg->robotSettings.Wheel3Angle,wheeltexid); wheels[3] = new Wheel(this,3,cfg->robotSettings.Wheel4Angle,cfg->robotSettings.Wheel4Angle,wheeltexid); firsttime=true; on = (this->getID()%5 < 3); if(!on) { this->chassis->setVisibility(false); for(int i=0; i<4; i++) this->wheels[i]->cyl->setVisibility(false); this->kicker->box->setVisibility(false); } }
int main(int argc, char *argv[]) { dsFunctions fn; double x[NUM] = {0.00}, y[NUM] = {0.00}; // Center of gravity double z[NUM] = { 0.05, 0.50, 1.50, 2.55}; double m[NUM] = {10.00, 2.00, 2.00, 2.00}; // mass double anchor_x[NUM] = {0.00}, anchor_y[NUM] = {0.00};// anchors of joints double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00}; double axis_x[NUM] = { 0.00, 0.00, 0.00, 0.00}; // axises of joints double axis_y[NUM] = { 0.00, 0.00, 1.00, 1.00}; double axis_z[NUM] = { 1.00, 1.00, 0.00, 0.00}; fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = &command; fn.path_to_textures = "../../drawstuff/textures"; dInitODE(); // Initialize ODE world = dWorldCreate(); // Create a world dWorldSetGravity(world, 0, 0, -9.8); for (int i = 0; i < NUM; i++) { dMass mass; link[i] = dBodyCreate(world); dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position dMassSetZero(&mass); // Set mass parameter to zero dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]); // Calculate mass parameter dBodySetMass(link[i], &mass); // Set mass } joint[0] = dJointCreateFixed(world, 0); // A fixed joint dJointAttach(joint[0], link[0], 0); // Attach the joint between the ground and the base dJointSetFixed(joint[0]); // Set the fixed joint for (int j = 1; j < NUM; j++) { joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]); dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]); } dsSimulationLoop(argc, argv, 640, 570, &fn); // Simulation loop dCloseODE(); return 0; }
/** * This method is called if the Joint should be attached. * It gets the ODE-bodies from the JointInteraction-class and * then either attaches the Joint by calling the virtual * method attachJoint or attaches a fixed ODE-joint if * the joint is not active. **/ void Joint::attach() { if (alreadyAttached) return; alreadyAttached = true; dBodyID body1, body2; body1 = jointInteraction->getBodyWithID(entityID1); body2 = jointInteraction->getBodyWithID(entityID2); if (active) attachJoint(body1, body2); else { joint = dJointCreateFixed(world, 0); dJointAttach(joint, body1, body2); dJointSetFixed(joint); } // else jointInteraction->attachedJoints.push_back(this); } // attachJoint
/* ------------------------ * 箱の生成,描画 ------------------------ */ void MakeBox() { /* ローカル変数の定義 */ dMass mass; for(int i = 0; i < BOX_NUM; i++) { /* ボディを生成 */ box[i].body = dBodyCreate(world); dMassSetZero(&mass); /* 質量を設定 */ dMassSetBoxTotal(&mass, boxM[i], boxSize[i][0], boxSize[i][1], boxSize[i][2]); dBodySetMass(box[i].body, &mass); /* ジオメトリ生成 */ box[i].geom = dCreateBox(space, boxSize[i][0], boxSize[i][1], boxSize[i][2]); /* ジオメトリをセット */ dGeomSetBody(box[i].geom, box[i].body); dBodySetPosition(box[i].body, boxPos[i][0], boxPos[i][1], boxPos[i][2]); /* 箱と地面の結合 */ fixed[i] = dJointCreateFixed(world, 0); dJointAttach(fixed[i], box[i].body, 0); dJointSetFixed(fixed[i]); } }
//Overloaded Create void JointFixed::Create(World &world, JointGroup &jointGroup) { if(this->_id) dJointDestroy(this->_id); _id = dJointCreateFixed(world.Id(), jointGroup.Id()); }
/** * This method is called if the joint should be attached. * It creates the ODE-joint, calculates the current anchor-position * and axis-orientation and attaches the Joint. * @param obj1 first ODE-object to attach with * @param obj2 second ODE-object to attach with **/ void Hinge2Joint::attachJoint(dBodyID obj1, dBodyID obj2) { TransformationData entityTrans; gmtl::Vec3f newAnchor, newAxis1, newAxis2, scaleVec; gmtl::Quatf entityRot; gmtl::AxisAnglef axAng; joint = dJointCreateHinge2(world, 0); // Attaching the Joint is done after the position and // orientation calculation! // dJointAttach(joint, obj1, obj2); newAxis1 = axis1; newAxis2 = axis2; newAnchor = anchor; if (mainEntity != NULL) { entityTrans = mainEntity->getEnvironmentTransformation(); // get the scale values of the mainEntity // scaleVec[0] = mainEntity->getXScale(); // scaleVec[1] = mainEntity->getYScale(); // scaleVec[2] = mainEntity->getZScale(); scaleVec = entityTrans.scale; // scale Anchor-offset by mainEntity-scale value newAnchor[0] *= scaleVec[0]; newAnchor[1] *= scaleVec[1]; newAnchor[2] *= scaleVec[2]; // scale Axes by mainEntity-scale value because of possible distortion newAxis1[0] *= scaleVec[0]; newAxis1[1] *= scaleVec[1]; newAxis1[2] *= scaleVec[2]; gmtl::normalize(newAxis1); newAxis2[0] *= scaleVec[0]; newAxis2[1] *= scaleVec[1]; newAxis2[2] *= scaleVec[2]; gmtl::normalize(newAxis2); // 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 Axes by mainEntity-rotation newAxis1 *= entityRot; newAxis2 *= entityRot; // rotate Anchor-offset by mainEntity-rotation newAnchor *= entityRot; // transform new Anchor to world coordinates /* newAnchor[0] += mainEntity->getXTrans(); newAnchor[1] += mainEntity->getYTrans(); newAnchor[2] += mainEntity->getZTrans();*/ newAnchor += entityTrans.position; } // if // create a helper body when necessary to avoid the // Segmentation Fault that is thrown by ODE (v0.5) // when attaching a Hinge2Joint to the static environment if (obj1 == 0 || obj2 == 0) { helperBody = dBodyCreate(world); dBodySetPosition(helperBody, newAnchor[0], newAnchor[1], newAnchor[2]); helperJoint = dJointCreateFixed(world, 0); dJointAttach(helperJoint, helperBody, 0); dJointSetFixed(helperJoint); if (obj1 == 0) obj1 = helperBody; else obj2 = helperBody; usedHelperJoint = true; } // if dJointAttach(joint, obj1, obj2); dJointSetHinge2Anchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]); dJointSetHinge2Axis1(joint, newAxis1[0], newAxis1[1], newAxis1[2]); dJointSetHinge2Axis2(joint, newAxis2[0], newAxis2[1], newAxis2[2]); } // attachJoint
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; }
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 }
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; }
void CODEFixedJoint::create(dWorld& world, dJointGroupID groupID) { mID = dJointCreateFixed(world, groupID); }
// ロボットの生成 void create() { // SHIELDの生成(空間に固定) rod[0].body = dBodyCreate(world); dBodySetPosition(rod[0].body, SHIELD_X, SHIELD_Y, SHIELD_Z); dMassSetZero(&mass); dMassSetCylinderTotal(&mass, SHIELD_WEIGHT, 2, SHIELD_RADIUS, SHIELD_LENGTH); dBodySetMass(rod[0].body, &mass); rod[0].geom = dCreateCylinder(space, SHIELD_RADIUS, SHIELD_LENGTH); dGeomSetBody(rod[0].geom, rod[0].body); dRFromAxisAndAngle(R, 1, 0, 0, 0.5 * M_PI); // x軸に90度回転 dBodySetRotation(rod[0].body, R); rod_joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント dJointAttach(rod_joint[0], rod[0].body, 0); dJointSetFixed(rod_joint[0]); // RODの生成(回転ジョイントy軸に回転軸) rod[1].body = dBodyCreate(world); dBodySetPosition(rod[1].body, SHIELD_X, SHIELD_Y, SHIELD_Z); dMassSetZero(&mass); dMassSetBoxTotal(&mass, ROD_WEIGHT, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH); dBodySetMass(rod[1].body, &mass); rod[1].geom = dCreateBox(space, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH); dGeomSetBody(rod[1].geom, rod[1].body); dRFromAxisAndAngle(R, 0, 0, 1, 0.25 * M_PI); // z軸に45度回転 dBodySetRotation(rod[1].body, R); rod_joint[1] = dJointCreateHinge(world, 0); // ヒンジジョイント dJointAttach(rod_joint[1], rod[1].body, rod[0].body); dJointSetHingeAnchor(rod_joint[1], SHIELD_X, SHIELD_Y, SHIELD_Z); dJointSetHingeAxis(rod_joint[1], 0, 1, 0);// y軸ジョイント // BODYの生成(たてておくだけ) rod[2].body = dBodyCreate(world); dBodySetPosition(rod[2].body, BODY_X, BODY_Y, BODY_Z); dMassSetZero(&mass); dMassSetBoxTotal(&mass, BODY_WEIGHT, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT); dBodySetMass(rod[2].body, &mass); rod[2].geom = dCreateBox(space, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT); dGeomSetBody(rod[2].geom, rod[2].body); // BULLETの生成(CANNON中心に初期座標) bullet.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetSphereTotal(&mass, BULLET_WEIGHT, BULLET_RADIUS); dBodySetMass(bullet.body,&mass); dBodySetPosition(bullet.body, CANNON_X, CANNON_Y, CANNON_Z); bullet.geom = dCreateSphere(space, BULLET_RADIUS); dGeomSetBody(bullet.geom, bullet.body); // TARGETの生成 // target.body = dBodyCreate(world); // dMassSetZero(&mass); // dMassSetSphereTotal(&mass, 0.0001, BULLET_RADIUS); // dBodySetMass(target.body,&mass); // dBodySetPosition(target.body, SHIELD_X, SHIELD_Y, SHIELD_Z); // target.geom = dCreateSphere(space, BULLET_RADIUS); // dGeomSetBody(target.geom, target.body); }
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; }
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.setERP (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].create (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]) ); body[W].setMass(m); // Create the dandling body body[D].create(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]) ); body[D].setMass(&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]) { body[W].setPosition(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]) { body[D].setPosition(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 JointFixed::Create(World &world) { if(this->_id) dJointDestroy(this->_id); _id = dJointCreateFixed(world.Id(), 0); }
/** * Creates the ode joint objects and includes it into the ode-engine. * @param body1 * @param body2 * @return */ dJointID ODE_FixedJoint::createJoint(dBodyID body1, dBodyID body2) { dJointID newJoint = dJointCreateFixed(mWorldID, mGeneralJointGroup); dJointAttach(newJoint, body1, body2); dJointSetFixed(newJoint); return newJoint; }