// Create a ball and a pole void createBallandPole() { dMass m1; dReal x0 = 0.0, y0 = 0.0, z0 = 2.5; // ball ball.radius = 0.2; ball.mass = 1.0; ball.body = dBodyCreate(world); dMassSetZero(&m1); dMassSetSphereTotal(&m1, ball.mass, ball.radius); dBodySetMass(ball.body, &m1); dBodySetPosition(ball.body, x0, y0, z0); ball.geom = dCreateSphere(space, ball.radius); dGeomSetBody(ball.geom, ball.body); // pole pole.radius = 0.025; pole.length = 1.0; pole.mass = 1.0; pole.body = dBodyCreate(world); dMassSetZero(&m1); dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length); dBodySetMass(pole.body, &m1); dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length); pole.geom = dCreateCCylinder(space, pole.radius, pole.length); dGeomSetBody(pole.geom, pole.body); // hinge joint joint = dJointCreateHinge(world, 0); dJointAttach(joint, ball.body, pole.body); dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius); dJointSetHingeAxis(joint, 1, 0, 0); }
void constructWorldForTest (dReal gravity, int bodycount, /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z, /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z, /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z, /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z, /* rotation angles */ dReal a1, dReal a2) { // create world world = dWorldCreate(); dWorldSetERP (world,0.2); dWorldSetCFM (world,1e-6); dWorldSetGravity (world,0,0,gravity); dMass m; dMassSetBox (&m,1,SIDE,SIDE,SIDE); dMassAdjust (&m,MASS); body[0] = dBodyCreate (world); dBodySetMass (body[0],&m); dBodySetPosition (body[0], pos1x, pos1y, pos1z); dQuaternion q; dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1); dBodySetQuaternion (body[0],q); if (bodycount==2) { body[1] = dBodyCreate (world); dBodySetMass (body[1],&m); dBodySetPosition (body[1], pos2x, pos2y, pos2z); dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2); dBodySetQuaternion (body[1],q); } else body[1] = 0; }
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.stop = 0; fn.command = 0; fn.path_to_textures = "../../drawstuff/textures"; dInitODE (); // create world world = dWorldCreate (); space = dHashSpaceCreate (0); dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2 dWorldSetCFM (world,1e-5); dCreatePlane (space,0,0,1,0); contactgroup = dJointGroupCreate (0); // create object sphere0 = dBodyCreate (world); sphere0_geom = dCreateSphere (space,0.5); dMassSetSphere (&m,1,0.5); dBodySetMass (sphere0,&m); dGeomSetBody (sphere0_geom,sphere0); sphere1 = dBodyCreate (world); sphere1_geom = dCreateSphere (space,0.5); dMassSetSphere (&m,1,0.5); dBodySetMass (sphere1,&m); dGeomSetBody (sphere1_geom,sphere1); sphere2 = dBodyCreate (world); sphere2_geom = dCreateSphere (space,0.5); dMassSetSphere (&m,1,0.5); dBodySetMass (sphere2,&m); dGeomSetBody (sphere2_geom,sphere2); // set initial position dBodySetPosition (sphere0,0,0,4); dBodySetPosition (sphere1,5,0,4); dBodySetPosition (sphere2,-2,0,4); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); // clean up dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
int main (int argc, char **argv) { // set for drawing dsFunctions fn; fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = NULL; fn.stop = NULL; fn.path_to_textures = "../textures"; dInitODE(); // init ODE world = dWorldCreate(); // create a dynamic world dWorldSetGravity(world,0,0,-0.1); dMass m; // a parameter for mass dMassSetZero (&m); // initialize the parameter //@a sphere sphere.body = dBodyCreate (world); // create a rigid body dReal radius = 0.5; // radius [m] dMassSetSphere (&m,DENSITY,radius); // calculate a mass parameter for a sphere dBodySetMass (sphere.body,&m); // set the mass parameter to the body dBodySetPosition (sphere.body,0,1, 1); // set the position of the body //@a box box.body = dBodyCreate (world); dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); dBodySetMass (box.body,&m); dBodySetPosition (box.body,0,2,1); // a capsule capsule.body = dBodyCreate (world); dMassSetCapsule(&m,DENSITY,3,radius,length); dBodySetMass (capsule.body,&m); dBodySetPosition (capsule.body,0,4,1); // a cylinder cylinder.body = dBodyCreate (world); dMassSetCylinder(&m,DENSITY,3,radius,length); dBodySetMass (cylinder.body,&m); dBodySetPosition (cylinder.body,0,3,1); // do the simulation dsSimulationLoop (argc,argv,960,480,&fn); dWorldDestroy (world); // destroy the world dCloseODE(); // close ODE return 0; }
void CRigidCapsule::setMass(F32 mass){ if(!mBodyID) return; dMass m; dMassSetCappedCylinderTotal(&m, TO_WORLD(mass), 3, TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y)); dBodySetMass(mBodyID, &m); }
GameObject::GameObject(GameWorld& gw, const ObjectPrototype& proto, double x, double y, double z, int id) : m_meshName(proto.m_meshName), m_sceneEntity(nullptr), m_sceneNode(nullptr), m_lockRotation(proto.m_lockRotation), m_isKinematic(proto.m_isKinematic), m_maxTurn(proto.m_maxTurnAngle), m_maxForward(proto.m_maxForward), m_maxBackward(proto.m_maxBackward), m_hitPoints(proto.m_maxHitPoints), m_collisionAccum(0), m_totalDamage(0), m_hasAgent(proto.m_hasAgent), m_gw(gw), m_id(id), m_oldGS(nullptr), m_newGS(nullptr) { m_sceneEntity = gw.GetScene()->createEntity(gw.GetMesh(m_meshName)); m_sceneNode = gw.GetScene()->getRootSceneNode()->createChildSceneNode(); m_sceneNode->attachObject(m_sceneEntity); Ogre::Vector3 size = m_sceneEntity->getBoundingBox().getSize(); m_body = dBodyCreate(gw.GetPhysicsWorld()); m_geom = dCreateBox(gw.GetPhysicsSpace(), size.x, size.y, size.z); dMassSetBox(&m_mass, proto.m_density, size.x, size.y, size.z); dBodySetMass(m_body, &m_mass); dGeomSetBody(m_geom, m_body); dBodySetPosition(m_body, x, y, z); // automagically disable things when the body is still for long enough dBodySetAutoDisableFlag(m_body, 1); dBodySetAutoDisableLinearThreshold(m_body, 0.1f); dBodySetAutoDisableAngularThreshold(m_body, 0.25f); dBodySetAutoDisableSteps(m_body, 1); // improve simulation accuracy dBodySetDamping(m_body, 0.0f, 0.1f); if (proto.m_registerCollisions) { gw.RegisterForCollisions(this); } if (proto.m_isKinematic) { dBodySetKinematic(m_body); } }
void PhysicsActor::postLoad(){ dMass m; if (type==CUBESHAPE) { geom = dCreateBox(space,shape.x,shape.y,shape.z); dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z); } if (type==CAPSULESHAPE) { geom = dCreateCapsule(space,shape.x,shape.y); dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y); //the '3' means align on z-axis and density is shape.z generateCapsuleList(); } dMassAdjust(&m,mass); dBodySetMass(body,&m); dGeomSetBody(geom,body); //initialise position if (base){ Matrix4f bGlobal= base->baseMatrix * renderer->inverseCameraMatrix; dBodySetPosition(body,bGlobal.data[12] + originalMatrix.data[12] + transformMatrix.data[12],bGlobal.data[13] + originalMatrix.data[13] + transformMatrix.data[13],bGlobal.data[14] + originalMatrix.data[14] + transformMatrix.data[14]); }else{ dBodySetPosition(body,originalMatrix.data[12] + transformMatrix.data[12],originalMatrix.data[13] + transformMatrix.data[13],originalMatrix.data[14] + transformMatrix.data[14]); } dBodySetDamping(body, linearDamp, angleDamp); bInit=true; }
void CRigidBox::setDensity(F32 density){ if(!mBodyID) return; dMass m; dMassSetBox(&m, TO_WORLD(density), TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y), TO_PHYSICS(mDimentions.z)); dBodySetMass(mBodyID, &m); }
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); }
void RigidBodyEnvironment::createWorld(void) { // BEGIN SETTING UP AN OPENDE ENVIRONMENT // *********************************** bodyWorld = dWorldCreate(); space = dHashSpaceCreate(0); dWorldSetGravity(bodyWorld, 0, 0, -0.981); double lx = 0.2; double ly = 0.2; double lz = 0.1; dMassSetBox(&m, 1, lx, ly, lz); boxGeom = dCreateBox(space, lx, ly, lz); boxBody = dBodyCreate(bodyWorld); dBodySetMass(boxBody, &m); dGeomSetBody(boxGeom, boxBody); // ********************************* // END SETTING UP AN OPENDE ENVIRONMENT setPlanningParameters(); }
/* ================================================================================= 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 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); }
void CRigidCapsule::setDensity(F32 density){ if(!mBodyID) return; dMass m; dMassSetCappedCylinder(&m, TO_WORLD(density), 3, TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y)); dBodySetMass(mBodyID, &m); }
ODEObject::ODEObject(OscObject *obj, dGeomID odeGeom, dWorldID odeWorld, dSpaceID odeSpace) : m_odeWorld(odeWorld), m_odeSpace(odeSpace) { m_object = obj; m_odeGeom = odeGeom; m_odeBody = NULL; m_odeBody = dBodyCreate(m_odeWorld); assert(m_odeGeom!=NULL); dBodySetPosition(m_odeBody, 0, 0, 0); dGeomSetPosition(m_odeGeom, 0, 0, 0); // note: owners must override this by setting the density. can't // do it here because obj->m_pSpecial is not yet // initialized. dMassSetSphere(&m_odeMass, 1, 1); dBodySetMass(m_odeBody, &m_odeMass); dGeomSetBody(m_odeGeom, m_odeBody); dGeomSetData(m_odeGeom, obj); if (!obj) return; obj->m_rotation.setSetCallback(ODEObject::on_set_rotation, this); obj->m_position.setSetCallback(ODEObject::on_set_position, this); obj->m_velocity.setSetCallback(ODEObject::on_set_velocity, this); obj->m_accel.setSetCallback(ODEObject::on_set_accel, this); obj->m_force.setSetCallback(ODEObject::on_set_force, this); obj->addHandler("push", "ffffff", ODEObject::push_handler); }
void PCylinder::setMass(dReal mass) { m_mass = mass; dMass m; dMassSetCylinderTotal (&m,m_mass,1,m_radius,m_length); dBodySetMass (body,&m); }
/** sets full mass specification. \b cg is center of gravity vector \b I are parts of the 3x3 interia tensor */ void Primitive::setMass(double mass, double cgx, double cgy, double cgz, double I11, double I22, double I33, double I12, double I13, double I23){ dMass mass0; dMassSetParameters(&mass0, mass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23); dBodySetMass(body, &mass0); }
void CRigidBox::setMass(F32 mass){ if(!mBodyID) return; dMass m; dMassSetBoxTotal(&m, mass, TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y), TO_PHYSICS(mDimentions.z)); dBodySetMass(mBodyID, &m); }
void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity ) { TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface; dMass totalMass; dBodyGetMass( m_BodyID, &totalMass ); if ( m_GeomIDs.size() == 0 ) { dMassSetZero( &totalMass ); } dMatrix4 R; dVector3 P; Matrix4ToODE( _bodyToGeomTransform, R, P ); dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID ); dGeomID encapsulatedGeom = 0; dMass currMass; dMassSetZero( &currMass ); encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength ); dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength ); dMassRotate( &currMass, R ); //dMassTranslate(&currMass,P[0],P[1],P[2]); dMassAdd( &totalMass, &currMass ); dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] ); dGeomSetRotation( encapsulatedGeom, R ); dGeomTransformSetCleanup( geomTransform, 1 ); dGeomTransformSetGeom( geomTransform, encapsulatedGeom ); dGeomSetBody( geomTransform, m_BodyID ); m_GeomIDs.push_back( geomTransform ); dBodySetMass( m_BodyID, &totalMass ); }
WheelPiece::WheelPiece(dWorldID& world,dSpaceID& space, float x, float y, float z) { dBodyCreate(world); body = dBodyCreate(world); radius = .35; thickness = .25; activationDirection = 0; geom = dCreateCylinder(space, radius, thickness); dGeomSetBody(geom, body); dGeomSetData(geom, this); dMass mass; mass.setCylinder(.5, 1, radius, thickness); dBodySetMass(body, &mass); const dMatrix3 rotationMatrix = {1,0,0,0, 0,1,0,0, 0,0,1,0 }; dBodySetRotation(body, rotationMatrix); dBodySetPosition(body,x,y,z); attachmentOffset = thickness + .25; color[0] = .5; color[1] = 1; color[2] = 1; }
CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z) { body = dBodyCreate(world); geom = dCreateBox(space, sides[0], sides[1], sides[2]); dGeomSetBody(geom, body); dGeomSetData(geom, this); dMass mass; mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]); dBodySetMass(body, &mass); const dMatrix3 rotationMatrix = {1,0,0,0, 0,1,0,0, 0,0,1,0}; dBodySetRotation(body, rotationMatrix); dBodySetPosition(body,x,y,z); for(int i = 0 ; i < 6 ; i++) attachedPieces[i] = NULL; // initialize attached piece array to all null pointers color[0] = 1; color[1] = 1; color[2] = 1; }
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 CProtoHapticDoc::UpdateDynamics() { for(int i= 0; i<m_shapeCount; i++) { dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(), m_shapes[i]->getSizeY(), m_shapes[i]->getSizeZ()); dGeomSetPosition (m_geoms[i], m_shapes[i]->getLocationX(), m_shapes[i]->getLocationY(), m_shapes[i]->getLocationZ()); dGeomSetRotation (m_geoms[i], dBodyGetRotation(bodies[i])); dBodySetPosition (bodies[i], m_shapes[i]->getLocationX(), m_shapes[i]->getLocationY(), m_shapes[i]->getLocationZ()); float *rotation= m_shapes[i]->getRotation(); const dReal rot[12]= { rotation[0], rotation[4], rotation[8], rotation[12], rotation[1], rotation[5], rotation[9], rotation[13], rotation[2], rotation[6], rotation[10], rotation[14] }; dBodySetRotation (bodies[i], rot); dMass mass; dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(), m_shapes[i]->getSizeY(), m_shapes[i]->getSizeZ()); dBodySetMass (bodies[i], &mass); } }
void CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16 flags) { VERIFY(ref_obj); R_ASSERT(_valid( start_pos ) ); R_ASSERT( _valid( start_size ) ); m_body = dBodyCreate (0) ; dMass m; dMassSetSphere(&m,1.f,100000.f); dMassAdjust(&m,1.f); dBodySetMass(m_body,&m); switch(_type) { case etBox: m_geom = dCreateBox (0,start_size.x,start_size.y,start_size.z) ; break; case etSphere: m_geom = dCreateSphere (0,start_size.x); break; }; dGeomCreateUserData (m_geom) ; dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback) ; dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj) ; dGeomSetBody (m_geom,m_body) ; dBodySetPosition (m_body,start_pos.x,start_pos.y,start_pos.z) ; Island() .AddBody (m_body) ; dBodyEnable (m_body) ; m_safe_state .create(m_body) ; spatial_register () ; m_flags.set(flags,TRUE); }
void ODERigidObject::Create(dWorldID worldID,dSpaceID space,bool useBoundaryLayer) { Clear(); spaceID = space; bodyID = dBodyCreate(worldID); dMass mass; mass.mass = obj.mass; //NOTE: in ODE, COM must be zero vector! -- we take care of this by shifting geometry //CopyVector3(mass.c,obj.com); mass.c[0] = mass.c[1] = mass.c[2] = 0; mass.c[3] = 1.0; CopyMatrix(mass.I,obj.inertia); int res=dMassCheck(&mass); if(res != 1) { fprintf(stderr,"Uh... mass is not considered to be valid by ODE?\n"); std::cerr<<"Inertia: "<<obj.inertia<<std::endl; } dBodySetMass(bodyID,&mass); geometry = new ODEGeometry; geometry->Create(&obj.geometry,spaceID,-obj.com,useBoundaryLayer); dGeomSetBody(geometry->geom(),bodyID); dGeomSetData(geometry->geom(),(void*)-1); geometry->SetPadding(defaultPadding); geometry->surf().kRestitution = obj.kRestitution; geometry->surf().kFriction = obj.kFriction; geometry->surf().kStiffness = obj.kStiffness; geometry->surf().kDamping = obj.kDamping; SetTransform(obj.T); }
void VStateSphereMass::Apply() { VState::Parent pParent = GetParent(); dMassSetSphere (&m_Mass, GetDensity(), m_fRadius); dMassAdjust (&m_Mass, GetMass()); dBodySetMass (pParent->GetBodyID(),&m_Mass); }
void createPart(const dWorldID & _world , dPart & part, dReal * pos3){ dMass mass; part.body = dBodyCreate(_world); dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position dMassSetZero(&mass); // Set mass parameter to zero dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length); // Calculate mass parameter dBodySetMass(part.body, &mass); // Set mass }
void PCylinder::setMass(float mass) { m_mass = mass; dMass m; dMassSetCylinder (&m,1,1,m_radius,m_length); dMassAdjust (&m,m_mass); dBodySetMass (body,&m); }
void OscSphereODE::on_density() { ODEObject *ode_object = static_cast<ODEObject*>(special()); dMassSetSphere(&ode_object->mass(), m_density.m_value, m_radius.m_value); dBodySetMass(ode_object->body(), &ode_object->mass()); m_mass.m_value = ode_object->mass().mass; }
/*** ロボットアームの生成 ***/ 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]); } }
/*** 車輪の生成 ***/ void makeWheel() { dMass mass; dReal r = 0.1; dReal w = 0.024; dReal m = 0.15; dReal d = 0.01; dReal tmp_z = Z; dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0}; dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)}; dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z}; dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0}; dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2}; dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z}; for (int i = 0; i < WHEEL_NUM; i++) { wheel[i].v = 0.0; // make body, geom and set geom to body. // The position and orientation of geom is abondoned. wheel[i].body = dBodyCreate(world); wheel[i].geom = dCreateCylinder(space,r, w); dGeomSetBody(wheel[i].geom,wheel[i].body); // set configure of body dMassSetZero(&mass); if (i < 2) { dMassSetCylinderTotal(&mass,m, 1, r, w); } else { dMassSetCylinderTotal(&mass,m, 2, r, w); } dBodySetMass(wheel[i].body,&mass); // set position and orientation of body. dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]); dMatrix3 R; if (i >= 2) { dRFromAxisAndAngle(R,1,0,0,M_PI/2.0); dBodySetRotation(wheel[i].body,R); } else { dRFromAxisAndAngle(R,0,1,0,M_PI/2.0); dBodySetRotation(wheel[i].body,R); } // make joint. wheel[i].joint = dJointCreateHinge(world,0); dJointAttach(wheel[i].joint,base.body,wheel[i].body); if (i < 2) { dJointSetHingeAxis(wheel[i].joint, 1, 0, 0); } else { dJointSetHingeAxis(wheel[i].joint, 0, -1, 0); } dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]); } }