// 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); }
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; }
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(); }
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 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 Balaenidae::embody(dWorldID world, dSpaceID space) { me = dBodyCreate(world); embody(me); geom = dCreateBox( space, 100.0f, 40, 500.0f); // scale 50 dGeomSetBody(geom, me); }
BarrelGeom2(Barrel *b) : OdeGeom(dCreateBox(gDynamicSpace, b->radius_ * 1.4f, b->radius_ * 1.4f, b->height_)), b_(b) { dGeomSetBody(id_, b->body_->id_); dGeomSetData(id_, this); }
BarrelGeom(Barrel *b) : OdeGeom(dCreateCylinder(gDynamicSpace, b->radius_, b->height_)), b_(b) { dGeomSetBody(id_, b->body_->id_); dGeomSetData(id_, this); }
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 ); }
void cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot) { math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight); rotation.LoadIdentity(); rotation.SetFromAngles(math::DegreesToRadians(rot)); const math::cMat4 m = rotation.GetMatrix(); dMatrix3 r; r[0] = m[0]; r[1] = m[4]; r[2] = m[8]; r[3] = 0; r[4] = m[1]; r[5] = m[5]; r[6] = m[9]; r[7] = 0; r[8] = m[2]; r[9] = m[6]; r[10] = m[10]; r[11] = 0; position = pos; dGeomSetPosition(geom, position.x, position.y, position.z); dGeomSetRotation(geom, r); if (bBody) { body = dBodyCreate(pWorld->GetWorld()); dBodySetPosition(body, position.x, position.y, position.z); dBodySetRotation(body, r); dBodySetAutoDisableFlag(body, 1); dGeomSetBody(geom, body); pWorld->AddPhysicsObject(shared_from_this()); } }
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; }
// Universal method for all specific ODE geom types, which add the // geom to the collide space, using an ODE proxy geom to offset the // geom by the provided transformation matrix. The geom will also // be attached to the rigid body, if any is set. void CShape::AttachGeom(dGeomID GeomId, dSpaceID SpaceID) { n_assert(GeomId); n_assert(!IsAttached()); // set the geom's local Transform const vector3& Pos = Transform.pos_component(); dGeomSetPosition(GeomId, Pos.x, Pos.y, Pos.z); dMatrix3 ODERotation; CPhysicsServer::Matrix44ToOde(Transform, ODERotation); dGeomSetRotation(GeomId, ODERotation); // if attached to rigid body, create a geom Transform "proxy" object && attach it to the rigid body // else directly set Transform and rotation if (pRigidBody) { ODEGeomID = dCreateGeomTransform(0); dGeomTransformSetCleanup(ODEGeomID, 1); dGeomTransformSetGeom(ODEGeomID, GeomId); dGeomSetBody(ODEGeomID, pRigidBody->GetODEBodyID()); } else ODEGeomID = GeomId; dGeomSetCategoryBits(ODEGeomID, CatBits); dGeomSetCollideBits(ODEGeomID, CollBits); dGeomSetData(ODEGeomID, this); AttachToSpace(SpaceID); }
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 cylinder::create_physical_body( double x, double y, double z, double radius, double length, double mass, manager& mgr) { this->radius = radius; this->length = length; world_id = mgr.ode_world(); space_id = mgr.ode_space(); //create and position the geom to represent the physical shape of the rigid body geom_id = dCreateCylinder(mgr.ode_space(),radius, length); object::set_geom_data(geom_id); dGeomSetPosition (geom_id, x, y, z); if(mass > 0) { object::create_rigid_body(x, y, z, mgr); dGeomSetBody (geom_id, body_id); set_mass(mass); } }
ODE_Particle::ODE_Particle(dBodyID &b, dGeomID &g) { geom=g; body=b; dGeomSetBody(geom,body); id=object_counter; dGeomSetData(geom, (void*)(size_t)id); object_counter++; is_drawable=true; is_aabb_drawable=false; force[0]=0;force[1]=0;force[2]=0; TurbVel[0]=0;TurbVel[1]=0;TurbVel[2]=0; tTurb=0; tTurbInt=0; //TODO //dBodySetAutoDisableFlag(body, true); //dBodySetAutoDisableSteps (body,1); //dBodySetAutoDisableLinearThreshold (body,100); //dBodySetAutoDisableAngularThreshold (body,100); /*dReal t= dBodyGetAutoDisableSteps (body); printf("%f\n",t); fflush(stdout);*/ //printf("Particle=%u constructor!\n",id); //fflush(stdout); }
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 box::create_physical_body( double x, double y, double z, double size_x, double size_y, double size_z, double mass, manager& mgr) { world_id = mgr.ode_world(); space_id = mgr.ode_space(); //create and position the geom to represent the pysical shape of the rigid body geom_id = dCreateBox (mgr.ode_space(), size_x, size_y, size_z); object::set_geom_data(geom_id); dGeomSetPosition (geom_id, x, y, z); size[0] = size_x; size[1] = size_y; size[2] = size_z; if(mass > 0) { object::create_rigid_body(x, y, z, mgr); set_mass(mass); dGeomSetBody (geom_id, body_id); } }
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 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 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); }
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); } }
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; }
void RigidBody::addGeom(Geom &_g) { m_geom=_g; dGeomSetBody(m_geom.getID(),m_id); m_type=m_geom.getType(); }
void Rigid::_init() { mBodyID = dBodyCreate(RigidManager::getSingleton().getWorldID()); //默认质量密度 mMassDensity = 1; if(mGeom) dGeomSetBody(mGeom->getGeomID(), mBodyID); }
IoObject *IoODEBox_setBody(IoODEBox *self, IoObject *locals, IoMessage *m) { dBodyID body = IoMessage_locals_odeBodyIdArgAt_(m, locals, 0); IoODEBox_assertHasBoxId(self, locals, m); dGeomSetBody(GEOMID, body); 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]); } }
void LaserBeam::embody(dWorldID world, dSpaceID space) { me = dBodyCreate(world); embody(me); //geom = dCreateSphere( space, 2.64f); geom = dCreateBox( space, Gunshot::width, Gunshot::height, Gunshot::length); dBodySetGravityMode(me,0); dGeomSetBody(geom, me); }
//=========================================================================== void cODEGenericBody::createDynamicBox(const double a_lengthX, const double a_lengthY, const double a_lengthZ, bool a_staticObject, const cVector3d& a_offsetPos, const cMatrix3d& a_offsetRot) { // create ode dynamic body if object is non static if (!a_staticObject) { m_ode_body = dBodyCreate(m_ODEWorld->m_ode_world); // store pointer to current object dBodySetData (m_ode_body, this); } m_static = a_staticObject; // build box m_ode_geom = dCreateBox(m_ODEWorld->m_ode_space, a_lengthX, a_lengthY, a_lengthZ); // adjust position offset dGeomSetPosition (m_ode_geom, a_offsetPos.x, a_offsetPos.y, a_offsetPos.z); // adjust orientation offset dMatrix3 R; R[0] = a_offsetRot.m[0][0]; R[1] = a_offsetRot.m[0][1]; R[2] = a_offsetRot.m[0][2]; R[4] = a_offsetRot.m[1][0]; R[5] = a_offsetRot.m[1][1]; R[6] = a_offsetRot.m[1][2]; R[8] = a_offsetRot.m[2][0]; R[9] = a_offsetRot.m[2][1]; R[10] = a_offsetRot.m[2][2]; dGeomSetRotation (m_ode_geom, R); // set inertia properties if (!m_static) { dMassSetBox(&m_ode_mass, 1.0, a_lengthX, a_lengthY, a_lengthZ); dMassAdjust(&m_ode_mass, m_mass); dBodySetMass(m_ode_body,&m_ode_mass); // attach body and geometry together dGeomSetBody(m_ode_geom, m_ode_body); } // store dynamic model type m_typeDynamicCollisionModel = ODE_MODEL_BOX; // store dynamic model parameters m_paramDynColModel0 = a_lengthX; m_paramDynColModel1 = a_lengthY; m_paramDynColModel2 = a_lengthZ; m_posOffsetDynColModel = a_offsetPos; m_rotOffsetDynColModel = a_offsetRot; }
/*** 車輪の生成 ***/ 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]); } }