void nearCallback(void *data, dGeomID g1, dGeomID g2)
{
if (g1 == ground->id()) {
queueRemoval(g2);
return;
}
if (g2 == ground->id()) {
queueRemoval(g1);
return;
}
dBodyID b1 = dGeomGetBody(g1);
dBodyID b2 = dGeomGetBody(g2);
if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
const int MAX_CONTACTS = 10;
dContact contact[MAX_CONTACTS];
int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
for (int i=0; i<n; ++i) {
contact[i].surface.mode = 0;
contact[i].surface.mu = 1;
dJointID j = dJointCreateContact (*world, joints.id(), contact+i);
dJointAttach(j, b1, b2);
}
}
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 = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
int i;
contactgroup.create ();
world.setGravity (0,0,-0.5);
dWorldSetCFM (world.id(),1e-5);
dPlane plane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i].create (world);
dReal k = i*SIDE;
body[i].setPosition (k,k,k+0.4);
dMass m;
m.setBox (1,SIDE,SIDE,SIDE);
m.adjust (MASS);
body[i].setMass (&m);
body[i].setData ((void*)(size_t)i);
box[i].create (space,SIDE,SIDE,SIDE);
box[i].setBody (body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i].create (world);
joint[i].attach (body[i],body[i+1]);
dReal k = (i+0.5)*SIDE;
joint[i].setAnchor (k,k,k+0.4);
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dCloseODE();
return 0;
}
void
simLoop(int pause)
{
if (!pause) {
const dReal timestep = 0.005;
// this does a hard-coded circular motion animation
static float t=0;
t += timestep/4;
if (t > 2*M_PI)
t = 0;
dReal px = cos(t);
dReal py = sin(t);
dReal vx = -sin(t)/4;
dReal vy = cos(t)/4;
kbody->setPosition(px, py, .5);
kbody->setLinearVel(vx, vy, 0);
// end of hard-coded animation
space->collide(0, nearCallback);
removeQueued();
world->quickStep(timestep);
joints.clear();
}
dVector3 lengths;
// the moving platform
kbox->getLengths(lengths);
dsSetTexture(DS_WOOD);
dsSetColor(.3, .3, 1);
dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths);
dReal length, radius;
kpole->getParams(&radius, &length);
dsSetTexture(DS_CHECKERED);
dsSetColor(1, 1, 0);
dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius);
// the matraca
matraca_geom->getLengths(lengths);
dsSetColor(1,0,0);
dsSetTexture(DS_WOOD);
dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths);
// and the boxes
for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw));
}
void
simLoop(int pause)
{
if (!pause) {
const dReal timestep = 0.04;
// this does a hard-coded circular motion animation
static float t=0;
t += timestep/4;
if (t > 2*M_PI)
t = 0;
dVector3 next_pos = { cos(t), sin(t), 0.5};
dVector3 vel;
// vel = (next_pos - cur_pos) / timestep
dSubtractVectors3(vel, next_pos, kbody->getPosition());
dScaleVector3(vel, 1/timestep);
kbody->setLinearVel(vel);
// end of hard-coded animation
space->collide(0, nearCallback);
removeQueued();
world->quickStep(timestep);
joints.clear();
}
dVector3 lengths;
// the moving platform
kbox->getLengths(lengths);
dsSetTexture(DS_WOOD);
dsSetColor(.3, .3, 1);
dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths);
dReal length, radius;
kpole->getParams(&radius, &length);
dsSetTexture(DS_CHECKERED);
dsSetColor(1, 1, 0);
dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius);
// the matraca
matraca_geom->getLengths(lengths);
dsSetColor(1,0,0);
dsSetTexture(DS_WOOD);
dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths);
// and the boxes
for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw));
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
// @@@ it's still more convenient to use the C interface here.
dContact contact;
contact.surface.mode = 0;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
dJointAttach (c,b1,b2);
}
}
static void simLoop (int pause)
{
if (!pause) {
static double angle = 0;
angle += 0.05;
body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0));
space.collide (0,&nearCallback);
world.step (0.05);
// remove all contact joints
contactgroup.empty();
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<NUM; i++)
dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE();
world = new dWorld();
world->setGravity(0,0,-0.5f);
world->setCFM(1e-5f);
world->setLinearDamping(0.00001f);
world->setAngularDamping(0.0001f);
space = new dSimpleSpace(0);
dPlane *floor = new dPlane(*space, 0,0,1,0);
top1 = new dBody(*world);
top2 = new dBody(*world);
dMass m;
m.setCylinderTotal(1, 3, topradius, toplength);
top1->setMass(m);
top2->setMass(m);
dGeom *g1, *g2, *pin1, *pin2;
g1 = new dCylinder(*space, topradius, toplength);
g1->setBody(*top1);
g2 = new dCylinder(*space, topradius, toplength);
g2->setBody(*top2);
pin1 = new dCapsule(*space, pinradius, pinlength);
pin1->setBody(*top1);
pin2 = new dCapsule(*space, pinradius, pinlength);
pin2->setBody(*top2);
top2->setGyroscopicMode(false);
reset();
// run simulation
dsSimulationLoop (argc,argv,512,384,&fn);
delete g1;
delete g2;
delete pin1;
delete pin2;
delete floor;
contactgroup.empty();
delete top1;
delete top2;
delete space;
delete world;
dCloseODE();
}
//===========================================================================================
//! \brief シミュレーションオブジェクトを作成
void create_world( void )
//===========================================================================================
{
// acrobot の回転軸（以下，支柱）は ここでは 直方体(Box)にしています．
const dReal param_h0 = 0.05; // 支柱（直方体）の高さ[m]
const dReal param_wx0 = 0.05; // 同幅(x)
const dReal param_wy0 = 0.80; // 同幅(y)
const dReal param_z0 = 1.20; // 支柱の垂直位置[m]
const dReal param_l1 = 0.50; // 第1リンク（支柱に近いリンク）の長さ[m]
const dReal param_d1 = 0.15; // 同直径[m]
const dReal param_l2 = 0.50; // 第2リンク（支柱に近いリンク）の長さ[m]
const dReal param_d2 = 0.15; // 同直径[m]
const dReal density = 1000.0; // 各リンクの密度[kg/m^3]. 参考(?)`人体の密度' は 900~1100 kg/m^3 (wikipedia)
int i;
contactgroup.create (0);
world.setGravity (0,0,-9.8); // 重力 [m/s^2]
dWorldSetCFM (world.id(),1e-5);
plane.create (space,0,0,1,0); // 地面（平面）．
i=0; {
body[i].create (world);
body[i].setPosition (0.0, 0.0, param_z0); // 支柱の中心座標
dReal xx=param_wx0, yy=param_wy0, zz=param_h0;
dMass m;
m.setBox (density,xx,yy,zz);
body[i].setMass (&m);
LinkBase.create (space,xx,yy,zz);
LinkBase.setBody (body[i]);
}
i=1; {
body[i].create (world);
body[i].setPosition (0.0, 0.0, param_z0-0.5*param_l1); // リンク1の中心座標
dReal rad=0.5*param_d1, len=param_l1-2.0*rad;
dMass m;
m.setCappedCylinder (density,3,rad,len); // direction(3): z-axis
body[i].setMass (&m);
Link1.create (space,rad,len);
Link1.setBody (body[i]);
}
i=2; {
body[i].create (world);
body[i].setPosition (0.0, 0.0, param_z0-param_l1-0.5*param_l2); // リンク2の中心座標
dReal rad=0.5*param_d2, len=param_l2-2.0*rad;
dMass m;
m.setCappedCylinder (density,3,rad,len); // direction(3): z-axis
body[i].setMass (&m);
Link2.create (space,rad,len);
Link2.setBody (body[i]);
}
i=0; {
const dReal *pos = body[0].getPosition();
joint[i].create (world);
joint[i].attach (body[0],body[1]);
joint[i].setAnchor (pos[0],pos[1],pos[2]); // 回転中心=支柱の中心(=原点)
joint[i].setAxis (0.0,1.0,0.0); // 回転軸=y軸
// joint[i].setParam (dParamHiStop, +0.5*M_PI); // 関節の可動範囲を制約するときに使う
// joint[i].setParam (dParamLoStop, -0.5*M_PI); // acrobot の場合は省略
}
i=1; {
const dReal *pos = body[1].getPosition();
joint[i].create (world);
joint[i].attach (body[1],body[2]);
joint[i].setAnchor (pos[0],pos[1],pos[2]-0.5*param_l1); // 回転中心=リンク1とリンク2の間
joint[i].setAxis (0.0,1.0,0.0); // 回転軸=y軸
}
base_joint.create(world);
base_joint.attach(body[0].id(),0); // 支柱(body[0]) と 平面(0)の間の固定リンク．支柱が固定される．
base_joint.set();
}