// 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); }
IoObject *IoODEMass_setSphereMass(IoODEMass *self, IoObject *locals, IoMessage *m) { const double totalMass = IoMessage_locals_doubleArgAt_(m, locals, 0); const double radius = IoMessage_locals_doubleArgAt_(m, locals, 1); dMassSetSphereTotal(DATA(self), totalMass, radius); return self; }
void OscSphereODE::on_mass() { ODEObject *ode_object = static_cast<ODEObject*>(special()); dMassSetSphereTotal(&ode_object->mass(), m_mass.m_value, m_radius.m_value); dBodySetMass(ode_object->body(), &ode_object->mass()); dReal volume = 4*M_PI*m_radius.m_value*m_radius.m_value*m_radius.m_value/3; m_density.m_value = m_mass.m_value / volume; }
void PhySphere::SetMass(float mass, float radius) { if (mBodyId != NULL) { dMass m; dMassSetZero(&m); dMassSetSphereTotal(&m, mass, radius); dBodySetMass(mOdeBody, &m); } }
void sphere::set_mass(double mass) { if(body_id) { dMass dmass; dMassSetZero(&dmass); dMassSetSphereTotal(&dmass,mass,radius); dBodySetMass (body_id, &dmass); material.mass = mass; } }
void Sphere::MakeBody(dWorldID world) { iBody = dBodyCreate(world); dMassSetSphereTotal(&iMass,iM,iRadius); dBodySetMass(iBody,&iMass); dBodySetPosition(iBody,iPosition.x,iPosition.y,iPosition.z); dBodySetLinearVel(iBody,iVel.x,iVel.y,iVel.z); disabledSteps = 0; dBodySetData(iBody,data); }
void capsule::set_mass(double mass) { if(body_id) { dMass dmass; dMassSetZero(&dmass); dMassSetSphereTotal(&dmass,mass,radius); dMassSetCapsuleTotal(&dmass,mass,3,radius,length); dBodySetMass (body_id, &dmass); material.mass = mass; } }
void cPhysicsObject::CreateSphere(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot) { geom = dCreateSphere(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(), fRadius); InitCommon(pWorld, pos, rot); if (bBody) { dMass mass; dMassSetSphereTotal(&mass, fMassKg, 2.0f * fRadius); dBodySetMass(body, &mass); } }
void Wheel::createPhysics(Utils::Xml &x, dSpaceID space) { if (x.mustString("nature") == "sphere") { ph.geom = dCreateSphere(space, x.mustOReal("radius")); dMassSetSphereTotal(&ph.mass, x.mustOReal("mass"), x.mustOReal("radius")); } else { ph.geom = dCreateCylinder(space, x.mustOReal("radius"), x.mustOReal("width")); dMassSetCylinderTotal(&ph.mass, x.mustOReal("mass"), 1, x.mustOReal("radius"), x.mustOReal("width")); } ph.body = World::getSingletonPtr()->add(ph.geom, &ph.mass); }
dMass KinematicMass::getODEMass(arma::mat44 coordinateFrame) const { dMass mass; arma::colvec4 helper = arma::zeros(4); helper(3) = 1.; helper.rows(0, 2) = m_position; helper = coordinateFrame * helper; dMassSetZero(&mass); dMassSetSphereTotal(&mass, m_massGrams / 1000., 1); dMassTranslate(&mass, helper(0), helper(1), helper(2)); return mass; }
//set mass and radius by available total mass void ODE_Particle::setMassTotal(dReal total_mass, dReal rad) { dMass m; dMassSetZero(&m); if (getShapeType() == dSphereClass) { dMassSetSphereTotal(&m,total_mass,rad); // set a given mass to sphere dBodySetMass(body,&m); } else { printf("ERROR in ODE_Particle.cpp: Setting totalMass for non spherical object"); exit(0); } }
/*** ボールの生成 ***/ static void makeBall() { dMass mass; ball.m = 0.45; ball.r = 0.11; ball.x = 1.0; ball.y = 0.0; ball.z = 0.14 + offset_z; ball.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetSphereTotal(&mass,ball.m,ball.r); dBodySetMass(ball.body,&mass); ball.geom = dCreateSphere(space,ball.r); dGeomSetBody(ball.geom,ball.body); dBodySetPosition(ball.body,ball.x, ball.y, ball.z); }
void dmCreateSphere(dmObject *obj, double p[2], double R[12], double m, double r, double color[3]) { obj->body = dBodyCreate(world); // ボールの生成 obj->m = m; obj->r = r; obj->R = R; obj->p = p; obj->color = color; dMass mass; // 構造体massの宣言 dMassSetZero(&mass); // 構造体massの初期化 dMassSetSphereTotal(&mass,obj->m,obj->r); // 構造体massに質量を設定 dBodySetMass(obj->body,&mass); // ボールにmassを設定 dBodySetPosition(obj->body, obj->p[0], obj->p[1], obj->p[2]); // ボールの位置(x,y,z)を設定 dBodySetRotation(obj->body, obj->R); obj->geom = dCreateSphere(space,obj->r); // 球ジオメトリの生成 dGeomSetBody(obj->geom, obj->body); // ボディとジオメトリの関連付け }
void CBall::Init() { static float dif_orange[4] = {0.75,0.4,0.2,1.0}; static float amb_orange[4] = {0.5,0.2,0.1,1.0}; static float spec[4] = {1.0,1.0,1.0,1.0}; glPushMatrix(); glLoadIdentity(); glTranslatef(0.000, 0.000, 1.100); glGetDoublev(GL_MODELVIEW_MATRIX, position); glPopMatrix(); material.SetDiffuse(dif_orange); material.SetAmbient(amb_orange); material.SetSpecular(spec); material.SetShininess(300); radius = 0.040; //body設定 bBall = dBodyCreate(world); dBodySetAutoDisableFlag(bBall,0); dBodySetPosition(bBall, position[12+0], position[12+1], position[12+2]); dMass mass; dMassSetSphereTotal(&(mass),0.010,0.040); dBodySetMass (bBall ,&(mass)); //geom設定 transBall = dCreateGeomTransform(space); gBall = dCreateSphere (0,0.040); dGeomSetPosition(gBall, 0 ,0, 0.002);//重心が3mmずれてる dGeomTransformSetGeom(transBall, gBall); dGeomSetBody(transBall, bBall); }
// ロボットの生成 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); }
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3], const float r[16]) { dVector3 v; dMatrix3 M; dReal rad; dReal len; dMass add; if (r) set_rotation(M, r); if (dGeomGetClass(geom) != dPlaneClass) { dReal m = get_data(geom)->mass; /* Create a new mass for the given geom. */ switch (dGeomGetClass(geom)) { case dBoxClass: dGeomBoxGetLengths(geom, v); dMassSetBoxTotal(&add, m, v[0], v[1], v[2]); break; case dSphereClass: rad = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&add, m, rad); break; case dCapsuleClass: dGeomCapsuleGetParams(geom, &rad, &len); dMassSetCapsuleTotal(&add, m, 3, rad, len); break; default: dMassSetZero(&add); break; } /* Transform the geom and mass to the given position and rotation. */ if(dGeomGetBody(geom)) { if (p) { dGeomSetOffsetPosition(geom, p[0], p[1], p[2]); dMassTranslate (&add, p[0], p[1], p[2]); } if (r) { dGeomSetOffsetRotation(geom, M); dMassRotate (&add, M); } } else { if (p) dGeomSetPosition(geom, p[0], p[1], p[2]); if (r) dGeomSetRotation(geom, M); } /* Accumulate the new mass with the body's existing mass. */ dMassAdd(mass, &add); } }
void PhysicsBody::setSphereMassTotal( Real32 totalMass, Real32 radius ) { dMass mass; dMassSetSphereTotal(&mass, totalMass, radius); setMassStruct(mass); }
void MyDeformableObjectNode::createPhysicsBody(dWorldID odeWorld, dReal totalMass) { if (!odeWorld) { throw dcollide::NullPointerException("dWorldID odeWorld"); } if (!mProxy->getShape()) { std::cerr << "WARNING: cannot create physics body for a shapeless Proxy."<<std::endl; return; } if (!mPhysicsObjects.empty()) { std::cerr << "WARNING: previously created a physics body for this node." << std::endl; return; } const std::vector<dcollide::Vertex*> vertices = mProxy->getShape()->getMesh()->getVertices(); int verticesCount = vertices.size(); // This object is physically equipped/enabled mUseODE = true; // Distribute the given mass to the whole body dReal vertexRadius = 0.5; dReal vertexMass = totalMass / verticesCount; // Create a physics mid point for the object when its a volume object dcollide::Vector3 meshCenter; if (mProxy->getProxyType() & dcollide::PROXYTYPE_CLOSEDHULL) { mCenterPoint = new MyODETestAppGeom(this); mCenterPoint->setMoveNodeOnBodyMoved(false); dMass* mass = new dMass(); dBodyID body = dBodyCreate(odeWorld); dMassSetSphereTotal(mass, vertexMass, vertexRadius); //dBodySetMass(body, mass); dGeomSetBody((dGeomID)mCenterPoint, body); meshCenter = mProxy->getShape()->getMesh()->getMidPoint(); dBodySetPosition(mCenterPoint->getBody(), meshCenter[0], meshCenter[1], meshCenter[2]); } // Create an physics body for each vertex of the mesh mPhysicsObjects.reserve(verticesCount); for (int index = 0; index < verticesCount; ++index) { mPhysicsObjects.push_back( new MyODEDeformableTestAppGeom(this, index) ); dMass* mass = new dMass(); dBodyID body = dBodyCreate(odeWorld); dMassSetSphereTotal(mass, vertexMass, vertexRadius); //dBodySetMass(body, mass); dGeomSetBody((dGeomID)mPhysicsObjects[index], body); // Adjust the ODE body position mPhysicsObjects[index]->setMoveNodeOnBodyMoved(false); const dcollide::Vector3& position = vertices[index]->getWorldPosition(); dBodySetPosition(body, position[0], position[1], position[2]); mPhysicsObjects[index]->setMoveNodeOnBodyMoved(true); } // Connect the created physics bodies through joints // Create a joint groups (useful for static deformations) mJoints = dJointGroupCreate(0); // Set which avoids double creation std::set<dcollide::MultiMapElement<dcollide::Vertex, dJointID> > finished; std::set<dcollide::MultiMapElement<dcollide::Vertex, dJointID> >::iterator finished_pos; // Create the actual joints for (std::vector<dcollide::Vertex*>::const_iterator one = vertices.begin(); one != vertices.end(); ++one) { dcollide::Vertex* v1 = (*one); dBodyID body1 = mPhysicsObjects[v1->getVertexIndex()]->getBody(); //const dReal* v1Pos = dBodyGetPosition(body1); //dcollide::debug() << "Body of vertex1 (worldPosition="<< v1->getWorldPosition()<<") is at (" << v1Pos[0] << ", " << v1Pos[1] << ", " << v1Pos[2] << ")"; const std::list<dcollide::Vertex*>& list = (*one)->getAdjacentVertices(); for (std::list<dcollide::Vertex*>::const_iterator two = list.begin(); two != list.end(); ++two) { dcollide::Vertex* v2 = (*two); dBodyID body2 = mPhysicsObjects[v2->getVertexIndex()]->getBody(); finished_pos = finished.find(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v1, v2)); if (finished_pos == finished.end()) { dcollide::Vector3 center = (v1->getWorldPosition() + v2->getWorldPosition()) / 2; dJointID joint = dJointCreateBall(odeWorld, mJoints); //dJointID joint = dJointCreateFixed(odeWorld, mJoints); //dJointID joint = dJointCreateUniversal(odeWorld, mJoints); dJointAttach(joint, body1, body2); dJointSetBallAnchor(joint, center.getX(), center.getY(), center.getZ()); //dJointSetFixed(joint); /* dcollide::Vector3 axisOne = (v1->getWorldPosition() - v2->getWorldPosition()); dcollide::Vector3 axisTwo = v1->getAdjacentTriangles().front()->getWorldCoordinatesNormalVector(); dJointSetUniversalAnchor(joint, center.getX(), center.getY(), center.getZ()); dJointSetUniversalAxis1(joint, axisOne.getX(), axisOne.getY(), axisOne.getZ()); dJointSetUniversalAxis2(joint, axisTwo.getX(), axisTwo.getY(), axisTwo.getZ()); */ finished.insert(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v1, v2, joint)); /* // Create the so called bending joints std::list<dcollide::Vertex*>::const_iterator adjecent_pos; const std::list<dcollide::Vertex*>& farneigbours = v2->getAdjacentVertices(); for (std::list<dcollide::Vertex*>::const_iterator neighbour = farneigbours.begin(); neighbour != farneigbours.end(); ++neighbour) { dcollide::Vertex* v3 = (*neighbour); adjecent_pos = find(list.begin(), list.end(), v3); if (adjecent_pos == list.end()) { // The current node is a far neighbour and thus should be connected finished_pos = finished.find(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v2, v3)); // If it wasn't previously created -> create it! if (finished_pos == finished.end()) { dcollide::Vector3 center = (v2->getWorldPosition() + v3->getWorldPosition()) / 2; dJointID joint = dJointCreateBall(odeWorld, mJoints); dJointAttach(joint, body1, body2); dJointSetBallAnchor(joint, center.getX(), center.getY(), center.getZ()); finished.insert(dcollide::MultiMapElement<dcollide::Vertex, dJointID>(v2, v3, joint)); } } } */ } } // Connect all vertex "bodies" with the mid point if (mCenterPoint != 0) { dcollide::Vector3 center = (v1->getWorldPosition() + meshCenter) / 2; dcollide::Vector3 direction = v1->getWorldPosition() - meshCenter; dJointID joint = dJointCreateBall(odeWorld, mJoints); //dJointID joint = dJointCreateSlider(odeWorld, mJoints); dJointAttach(joint, body1, mCenterPoint->getBody()); //dJointSetFixed(joint); dJointSetBallAnchor(joint, center.getX(), center.getY(), center.getZ()); //dJointSetSliderAxis(joint, direction.getX(), direction.getY(), direction.getZ()); } } }
void SphereInfo::createMass(dMass * mass, float massVal) { dMassSetSphereTotal(mass, massVal, radius); }
void resetSimulation() { int i; i = 0; // destroy world if it exists if (bodies) { dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); } for (i = 0; i < 1000; i++) wb_stepsdis[i] = 0; // recreate world world = dWorldCreate(); // space = dHashSpaceCreate( 0 ); // space = dSimpleSpaceCreate( 0 ); space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ ); contactgroup = dJointGroupCreate (0); dWorldSetGravity (world,0,0,-1.5); dWorldSetCFM (world, 1e-5); dWorldSetERP (world, 0.8); dWorldSetQuickStepNumIterations (world,ITERS); ground = dCreatePlane (space,0,0,1,0); bodies = 0; joints = 0; boxes = 0; spheres = 0; wb = 0; #ifdef CARS for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS) for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2) makeCar(x, y, bodies, joints, boxes, spheres); #endif #ifdef WALL bool offset = false; for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE) { offset = !offset; for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE) { wall_bodies[wb] = dBodyCreate (world); dBodySetPosition (wall_bodies[wb],-20,y,z); dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE); dMassAdjust (&m, WALLMASS); dBodySetMass (wall_bodies[wb],&m); wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE); dGeomSetBody (wall_boxes[wb],wall_bodies[wb]); //dBodyDisable(wall_bodies[wb++]); wb++; } } dMessage(0,"wall boxes: %i", wb); #endif #ifdef BALLS for (dReal x = -7; x <= -4; x+=1) for (dReal y = -1.5; y <= 1.5; y+=1) for (dReal z = 1; z <= 4; z+=1) { b = dBodyCreate (world); dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, BALLMASS); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); } #endif #ifdef ONEBALL b = dBodyCreate (world); dBodySetPosition (b,0,0,2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, 1); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); #endif #ifdef BALLSTACK for (dReal z = 1; z <= 6; z+=1) { b = dBodyCreate (world); dBodySetPosition (b,0,0,z*RADIUS*2); dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m, 0.1); dBodySetMass (b,&m); sphere[spheres] = dCreateSphere (space,RADIUS); dGeomSetBody (sphere[spheres++],b); } #endif #ifdef CENTIPEDE dBodyID lastb = 0; for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1) { // chassis body b = body[bodies] = dBodyCreate (world); dBodySetPosition (body[bodies],-15,y,STARTZ); dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT); dMassAdjust (&m,CMASS); dBodySetMass (body[bodies],&m); box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT); dGeomSetBody (box[boxes++],body[bodies++]); for (dReal x = -17; x > -20; x-=RADIUS*2) { body[bodies] = dBodyCreate (world); dBodySetPosition(body[bodies], x, y, STARTZ); dMassSetSphere(&m, 1, RADIUS); dMassAdjust(&m, WMASS); dBodySetMass(body[bodies], &m); sphere[spheres] = dCreateSphere (space, RADIUS); dGeomSetBody (sphere[spheres++], body[bodies]); joint[joints] = dJointCreateHinge2 (world,0); if (x == -17) dJointAttach (joint[joints],b,body[bodies]); else dJointAttach (joint[joints],body[bodies-2],body[bodies]); const dReal *a = dBodyGetPosition (body[bodies++]); dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]); dJointSetHinge2Axis1 (joint[joints],0,0,1); dJointSetHinge2Axis2 (joint[joints],1,0,0); dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[joints],dParamLoStop,0); dJointSetHinge2Param (joint[joints],dParamHiStop,0); dJointSetHinge2Param (joint[joints],dParamVel2,-10.0); dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); body[bodies] = dBodyCreate (world); dBodySetPosition(body[bodies], -30 - x, y, STARTZ); dMassSetSphere(&m, 1, RADIUS); dMassAdjust(&m, WMASS); dBodySetMass(body[bodies], &m); sphere[spheres] = dCreateSphere (space, RADIUS); dGeomSetBody (sphere[spheres++], body[bodies]); joint[joints] = dJointCreateHinge2 (world,0); if (x == -17) dJointAttach (joint[joints],b,body[bodies]); else dJointAttach (joint[joints],body[bodies-2],body[bodies]); const dReal *b = dBodyGetPosition (body[bodies++]); dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]); dJointSetHinge2Axis1 (joint[joints],0,0,1); dJointSetHinge2Axis2 (joint[joints],1,0,0); dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0); dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5); dJointSetHinge2Param (joint[joints],dParamLoStop,0); dJointSetHinge2Param (joint[joints],dParamHiStop,0); dJointSetHinge2Param (joint[joints],dParamVel2,10.0); dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX); } if (lastb) { dJointID j = dJointCreateFixed(world,0); dJointAttach (j, b, lastb); dJointSetFixed(j); } lastb = b; } #endif #ifdef BOX body[bodies] = dBodyCreate (world); dBodySetPosition (body[bodies],0,0,HEIGHT/2); dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m, 1); dBodySetMass (body[bodies],&m); box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT); dGeomSetBody (box[boxes++],body[bodies++]); #endif #ifdef CANNON cannon_ball_body = dBodyCreate (world); cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS); dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS); dBodySetMass (cannon_ball_body,&m); dGeomSetBody (cannon_ball_geom,cannon_ball_body); dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS); #endif }
//Assistance with ode from http://www.alsprogrammingresource.com/basic_ode.html //and from the ODE user manual, not a lot of resources out there! void initODE() { int i; int j = 0;//For placing skyPanels int k = 0;//For placing sphere in box int q = 0;//For placing target boxes int r = 0;//For placing targetBoxes int s = 0;//FOr placing targetBoxes dReal radius = 0.5;//For sphere's in scene that bounce around/ get shot dMass m;//For mass of sphere dMass mp;//For mass of platform dMass md;//For mass of doorway dMass mpy;//For mass of pyramid dMass mbox;//For box of balls dMass msp;//For skyPanels dMass mtb;//For targetBox // Create a new, empty world and assign its ID number to World. Most applications will only need one world. world = dWorldCreate(); // Create a new collision space and assign its ID number to Space, passing 0 instead of an existing dSpaceID. // There are three different types of collision spaces we could create here depending on the number of objects // in the world but dSimpleSpaceCreate is fine for a small number of objects. If there were more objects we // would be using dHashSpaceCreate or dQuadTreeSpaceCreate (look these up in the ODE d spacePhy = dSimpleSpaceCreate(0); // Create a joint group object and assign its ID number to contactgroup. dJointGroupCreate used to have a // max_size parameter but it is no longer used so we just pass 0 as its argument. contactgroup = dJointGroupCreate(0); //Set the gravity of the world where y is up dWorldSetGravity(world, 0, -0.1, 0); // These next two functions control how much error correcting and constraint force mixing occurs in the world. // Don't worry about these for now as they are set to the default values and we could happily delete them from // this example. Different values, however, can drastically change the behaviour of the objects colliding, so // I suggest you look up the full info on them in the ODE docs. dWorldSetERP(world, 0.2); dWorldSetCFM(world, 1e-5); // This function sets the velocity that inter-penetrating objects will separate at. The default value is infinity. dWorldSetContactMaxCorrectingVel(world, 0.9); // This function sets the depth of the surface layer around the world objects. Contacts are allowed to sink into // each other up to this depth. Setting it to a small value reduces the amount of jittering between contacting // objects, the default value is 0. dWorldSetContactSurfaceLayer(world, 0.0); // To save some CPU time we set the auto disable flag to 1. This means that objects that have come to rest (based // on their current linear and angular velocity) will no longer participate in the simulation, unless acted upon // by a moving object. If you do not want to use this feature then set the flag to 0. You can also manually enable // or disable objects using dBodyEnable and dBodyDisable, see the docs for more info on this. dWorldSetAutoDisableFlag(world, 1); //This brings us to the end of the world settings, now we have to initialize the objects themselves. //****************Ball 1 // Create a new body for our object in the world and get its ID. ball_body = dBodyCreate(world); //Set ball1 geometries dMassSetZero(&ball_mass); dMassSetSphereTotal(&ball_mass, DENSITY, 2.0); dBodySetMass(ball_body, &ball_mass); ball_geom = dCreateSphere(spacePhy, radius); dGeomSetData(ball_geom, (void *)"ball"); dGeomSetBody(ball_geom, ball_body); dBodyEnable(ball_body); //Next we set the position of the new body dBodySetPosition(ball_body, 5, 100, 15); //****************Ball 2 // Create a new body for our object in the world and get its ID. ball_body2 = dBodyCreate(world); //Set ball1 geometries dMassSetZero(&ball_mass2); dMassSetSphereTotal(&ball_mass2, DENSITY, 2.0); dBodySetMass(ball_body2, &ball_mass2); ball_geom2 = dCreateSphere(spacePhy, radius); dGeomSetData(ball_geom2, (void *)"ball2"); dGeomSetBody(ball_geom2, ball_body2); dBodySetGravityMode (ball_body2, 0); //******End of ball 2 //*************Ball 3, a new way of creating a ball with physical properties ball_body3 = dBodyCreate(world); dMatrix3 R; dBodySetPosition(ball_body3, -5, 100, 15); dRFromAxisAndAngle(R, 1, 0, 0, -1.57); dBodySetRotation(ball_body3, R); // Here we use dMassSetSphere instead of dMassSetBox // and we pass the local radius variable as the third parameter dMassSetSphere(&m, DENSITY, radius); // To create the sphere object we use dCreateSphere and pass it the same local radius variable ball_geom3 = dCreateSphere(spacePhy, radius); dGeomSetBody(ball_geom3, ball_body3); dBodySetMass(ball_body3, &m); //*****End of ball3 //*************Box o balls properties for (i = 0; i < 61; i++) { // Create a new body for our object in the world and get its ID. balls_body[i] = dBodyCreate(world); //Set ball1 geometries dMassSetZero(&balls_mass[i]); dMassSetSphereTotal(&balls_mass[i], DENSITY, 2.0); dBodySetMass(balls_body[i], &balls_mass[i]); balls_geom[i] = dCreateSphere(spacePhy, radius); dGeomSetData(balls_geom[i], (void *)"ball"); dGeomSetBody(balls_geom[i], balls_body[i]); dBodyDisable(balls_body[i]);//Disable bodies, enabled auto after hit //Box 1 if (i <31) { dBodySetPosition(balls_body[i], (-20), 28.5, 7.0);//(xpos b/w -20 and -12, ypos b/w 28 and 32, zpos between 0 and 16) } //Box 2 if (i >= 31) { dBodySetPosition(balls_body[i], (25), 38.5, -14.0);//(xpos b/w -20 and -12, ypos b/w 28 and 32, zpos between 0 and 16) } k++; } //******End of Box o Balls //**************platform geometries //create 5 platform's with physics things for (i = 0; i < 15; i++) { platform_body[i] = dBodyCreate(world);//Add the platform body to the world //Set the geom platform // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&mp, DENSITY*1500, 4.5, 0.5, 18.0); //It works better if the invisible physics box is slightly larger than the object we're drawing platform_geom[i] = dCreateBox(spacePhy, 4.5, 0.5, 18.0); dGeomSetBody(platform_geom[i], platform_body[i]); dBodySetMass(platform_body[i], &mp); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (platform_body[i], 0); } //*******End of platform geometries //*******Set the doorway properties, 18 platforms make all 3 doors for (i = 0; i < 18; i++) { doorway_body[i] = dBodyCreate(world);//Add the platform body to the world //Set the geom platform // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&md, DENSITY*1500, 4.0, 0.25, 2.0); //It works better if the invisible physics box is slightly larger than the object we're drawing doorway_geom[i] = dCreateBox(spacePhy, 4.0, 0.25, 2.0); dGeomSetBody(doorway_geom[i], doorway_body[i]); dBodySetMass(doorway_body[i], &md); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (doorway_body[i], 0); } //*******End of doorway properties //*******Set the Box o' Balls properties, 6 sides total make the larger box for (i = 0; i < 12; i++) { boxSide_body[i] = dBodyCreate(world); } //Box 1 //Need to set position here to affect the box with gravity dBodySetPosition(boxSide_body[0], -20.0, 28.0, 16);//Front, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[1], -12.25, 28.0, 8.0);//Right, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[2], -20.0, 28.0, 0);//Back, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[3], -27.75, 28.0, 8.0);//Left, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[4], -20.0, 32.0, -0.23);//Top, rotated 90 deg. on x axis dBodySetPosition(boxSide_body[5], -20.0, 28.0, -0.23);//Bottom, rotated 90 deg. on x axis //Need to set position here to affect the box with gravity //Box 2 dBodySetPosition(boxSide_body[6], 30.0, 38.0, -16);//Front, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[7], 22.25, 38.0, -8.0);//Right, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[8], 30.0, 38.0, 0);//Back, rotated 0 deg. on y axis dBodySetPosition(boxSide_body[9], 37.75, 38.0, -8.0);//Left, rotated 90 deg. on y axis dBodySetPosition(boxSide_body[10], 30.0, 42.0, -16.23);//Top, rotated 90 deg. on x axis*****Unsure about z here dBodySetPosition(boxSide_body[11], 30.0, 38.0, -16.23);//Bottom, rotated 90 deg. on x axis //Box 1 for (i = 0; i < 6; i++) { if (i < 4)//Small sides { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 1, 0.25); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 1, 0.25); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } if (i >=4)//Large top and bottom { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 16, 0.5); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 16, 0.5); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } //Turn off colliding object force effects on this geom so that it is stationary (less density = greater effect on objects colliding into this one) dBodyDisable(boxSide_body[i]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (boxSide_body[i], 1); } //Box 2 for (; i < 12; i++) { if (i < 10)//Small sides { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 1, 0.5); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 1, 0.5); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } if (i >=10)//Large top and bottom { // Here we use dMassSetBox, we want a lower density here to be able to destroy the box dMassSetBox(&mbox, DENSITY, 16, 16, 1.0); //It works better if the invisible physics box is slightly larger than the object we're drawing boxSide_geom[i] = dCreateBox(spacePhy, 16, 16, 1.0); dGeomSetBody(boxSide_geom[i], boxSide_body[i]); dBodySetMass(boxSide_body[i], &mbox); } //Turn off colliding object force effects on this geom so that it is stationary (less density = greater effect on objects colliding into this one) dBodyDisable(boxSide_body[i]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (boxSide_body[i], 1); } //*******End of Box o' Balls properties //*******Set the pyramid properties, 120 pyramids make diamond excluding inverted that fill holes (gotta cheat) pyramid_body = dBodyCreate(world);//Add the platform body to the world //Set the geom platform // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&mpy, DENSITY*1500, 2.6, 4.5, 2.6); //It works better if the invisible physics box is slightly larger than the object we're drawing pyramid_geom = dCreateBox(spacePhy, 2.6, 4.5, 2.6); dGeomSetBody(pyramid_geom, pyramid_body); dBodySetMass(pyramid_body, &mpy); dBodyDisable(pyramid_body); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (pyramid_body, 0); //*******End of pyramid properties //*******skyPanels physics for(j = 0; j < 60; j++) { skyPanel_body[j] = dBodyCreate(world);//Add the skypanel body to the world //Set the geom skypanel // Here we use dMassSetBox //set DENSITY to be very high to prevent it from being moved from balls dMassSetBox(&msp, DENSITY*1500, 3.0, 7.0, 1.0); //It works better if the invisible physics box is slightly larger than the object we're drawing skyPanel_geom[j] = dCreateBox(spacePhy, 3.0, 7.0, 1.0); dGeomSetBody(skyPanel_geom[j], skyPanel_body[j]);//Bind the geom to the body so that we only need to place the body to also set the geom dBodySetMass(skyPanel_body[j], &msp); dBodyDisable(skyPanel_body[j]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (skyPanel_body[j], 0);//Gravity off } //*******End of skyPanels physics //*******Set targetBox physics for(q = 0; q < 114; q++) { targetBox_body[q] = dBodyCreate(world);//Add the skypanel body to the world //Set the geom target box // Here we use dMassSetBox dMassSetBox(&mtb, (0.05), 4.0, 4.0, 4.0); //It works better if the invisible physics box is slightly larger than the object we're drawing targetBox_geom[q] = dCreateBox(spacePhy, 4.0, 4.0, 4.0); dGeomSetBody(targetBox_geom[q], targetBox_body[q]);//Bind the geom to the body so that we only need to place the body to also set the geom dBodySetMass(targetBox_body[q], &mtb); dBodyDisable(targetBox_body[q]); //Set/get whether the body is influenced by the world's gravity or not. If mode is nonzero it is, if mode is zero, it isn't. Newly created bodies are always influenced by the world's gravity. dBodySetGravityMode (targetBox_body[q], 1);//Gravity on //Pile 1 if(q < 22) { dBodySetPosition(targetBox_body[q], (-60 + (s * 4)), (2 + r), -30);//Set position here, otherwise unaffected by gravity s++;//update x axis if (q == 8) { r += 4; s = 0; } if (q == 14) { r += 4; s = 0; } if (q == 18) { r += 4; s = 0; } if (q == 20) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 21) { r = 0; s = 0; } } //Pile 2 if((q > 21) && (q < 43)) { dBodySetPosition(targetBox_body[q], (-30 + (s * 4)), (2 + r), 90); s++;//Update x pos if (q == 29) { r += 4; s = 0; } if (q == 35) { r += 4; s = 0; } if (q == 39) { r += 4; s = 0; } if (q == 41) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 42) { r = 0; s = 0; } } //Pile 3 if((q > 42) && (q < 64)) { dBodySetPosition(targetBox_body[q], (110), (2 + r), (0 + (s * 4))); s++;//Update x pos if (q == 50) { r += 4; s = 0; } if (q == 56) { r += 4; s = 0; } if (q == 60) { r += 4; s = 0; } if (q == 62) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 63) { r = 0; s = 0; } } //Pile 4 if((q > 63) && (q < 85)) { dBodySetPosition(targetBox_body[q], (-90), (2 + r), (0 + (s * 4))); s++;//Update x pos if (q == 71) { r += 4; s = 0; } if (q == 77) { r += 4; s = 0; } if (q == 81) { r += 4; s = 0; } if (q == 83) { r += 4; s = 0; } //Reset values for next pile of boxes if (q == 84) { r = 0; s = 0; } } //Box tower 1 if((q > 84) && (q < 94)) { dBodySetPosition(targetBox_body[q], (-60), (2 + r), (30)); r+=1;//Put boxes slightly inside each other if (q == 93) { r = 0;//Reset r } } //Box tower 2 if((q > 93) && (q < 104)) { dBodySetPosition(targetBox_body[q], (70), (2 + r), (-90)); r+=1;//Put boxes slightly inside each other if (q == 103) { r = 0;//Reset r } } //Box tower 3 if((q > 103) && (q < 114)) { dBodySetPosition(targetBox_body[q], (60), (2 + r), (90)); r+=1;//Put boxes slightly inside each other if (q == 103) { r = 0;//Reset r } } } //*******End of targetBox Physics //Set the ground location: //First three param's set the normal vector, last param sets the distance according to the plane equation equation a*x+b*y+c*z=d and must have length 1 ground = dCreatePlane(spacePhy, 0, 2.0, 0, 0);//Plane where I have most objects sitting, y =2 // Here I have set the initial linear velocity to stationary and let gravity do the work on our spheres, but you can experiment // with the velocity vector to change the starting behavior. You can also set the rotational velocity for the new // body using dBodySetAngularVel which takes the same parameters. tempVect.x = 0.0; tempVect.y = 0.0; tempVect.z = 0.0; dBodySetLinearVel(ball_body, tempVect.x, tempVect.y, tempVect.z); dBodySetLinearVel(ball_body2, tempVect.x, tempVect.y, tempVect.z); dBodySetLinearVel(ball_body3, tempVect.x, tempVect.y, tempVect.z); dWorldSetLinearDamping(world, 0.00001); dWorldSetAngularDamping(world, 0.005); dWorldSetMaxAngularSpeed(world, 200); }
//! setting of mass void SSimRobotEntity::setMass(SSimObjParts *parts, double mass) { // if a part is already added int geomNum = parts->geoms.size(); if (geomNum != 0) { // mass per each part double ms = mass / geomNum; // refer geometry and body dBodyID body = parts->body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // Refer the type of geometry dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } // const dReal *pos = dGeomGetPosition(geom); //LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2])); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } // adjustment of the gap between CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // seeting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
//! add ODE parts with fixed joint void SSimEntity::setMass(double mass) { m_parts.mass = mass; int geomNum = getGeomNum(); if (geomNum != 0) { // if a part is already added // mass per part double ms = mass / geomNum; // refer geometry and body dBodyID body = m_parts.body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // refer the type of geometory dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } const dReal *pos = dGeomGetPosition(geom); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } /* for (int i = 0; i < geomNum; i++) { dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); const dReal *pos = dGeomGetPosition(geom); // Change to a relative position from CoG dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]); } // keep the CoG position //m_parts.pos.set(m.c[0], m.c[1], m.c[2]); */ // Adjustment of the gap from CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // Setting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
void dMassSetSphere (dMass *m, dReal density, dReal radius) { dMassSetSphereTotal (m, (dReal) ((REAL(4.0)/REAL(3.0)) * M_PI * radius*radius*radius * density), radius); }
int main (int argc, char **argv) { initscr(); dInitODE(); // setup world world = dWorldCreate(); dWorldSetGravity(world, 0, 0, -9.8); dSpaceID space = dHashSpaceCreate(0); contactgroup = dJointGroupCreate(0); // grand plane (for collision) dGeomID ground = dCreatePlane(space, 0, 0, 1, 0); // body dBodyID ball = dBodyCreate(world); // mass dMass m; dMassSetZero(&m); const dReal radius = 0.2; // 20cm const dReal mass = 1.0; // 1kg dMassSetSphereTotal(&m, mass, radius); dBodySetMass(ball, &m); dBodySetPosition(ball, 0.0, 0.0, 10); // x=0m, y=0m, z=10m dGeomID geom = dCreateSphere(space, radius); dGeomSetBody(geom, ball); // simulation loop (1000 step) dReal stepsize = 0.01; // 0.01ms for (int i = 0; i < 1000; ++i) { dSpaceCollide(space, 0, &nearCallback); dWorldStep(world, 0.01); dJointGroupEmpty(contactgroup); // draw erase(); const dReal *pos = dBodyGetPosition(ball); const dReal *R = dBodyGetRotation(ball); mvprintw((int)(12-pos[2]), 5, "*"); // ball mvprintw(12, 0, "============================"); // ground // draw move(0, 0); printw("t=%f, pos=(%f, %f, %f) \n", stepsize * i, pos[0], pos[1], pos[2]); refresh(); usleep(10 * 1000); } // cleanup dWorldDestroy(world); dCloseODE(); endwin(); return 0; }