Esempio n. 1
0
/*** 車輪の生成 ***/
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]);
    }
}
Esempio n. 2
0
int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  dInitODE2(0);
  world = dWorldCreate();
 
  space = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // note: can't share tridata if intending to trimesh-trimesh collide
  TriData1 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  TriData2 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  
  TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
  TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
  dGeomSetData(TriMesh1, TriData1);
  dGeomSetData(TriMesh2, TriData2);
  
  {dGeomSetPosition(TriMesh1, 0, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh1, Rotation);}

  {dGeomSetPosition(TriMesh2, 1, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh2, Rotation);}
  
  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
void place_cards()
{
    ncards = getncards(levels);
    // destroy removed cards (if any)
    int oldcards = cards.size();
    for (int i=ncards; i<oldcards; ++i)
        delete cards[i];
    cards.resize(ncards);
    // construct new cards (if any)
    for (int i=oldcards; i<ncards; ++i)
        cards[i] = new Card;
    
    // for each level
    int c = 0;
    dMatrix3 right, left, hrot;
    dReal angle = 20*M_PI/180.;
    dRFromAxisAndAngle(right, 1, 0, 0, -angle);
    dRFromAxisAndAngle(left, 1, 0, 0, angle);

    dRFromAxisAndAngle(hrot, 1, 0, 0, 91*M_PI/180.);
    
    dReal eps = 0.05;
    dReal vstep = cos(angle)*clength + eps;
    dReal hstep = sin(angle)*clength + eps;
    
    for (int lvl=0; lvl<levels; ++lvl) {
        // there are 3*(levels-lvl)-1 cards in each level, except last
        int n = (levels-lvl);
        dReal height = (lvl)*vstep + vstep/2;
        // inclined cards
        for (int i=0; i<2*n; ++i, ++c) {
            dBodySetPosition(cards[c]->body, 
                    0,
                    -n*hstep + hstep*i,
                    height
                    );
            if (i%2)
                dBodySetRotation(cards[c]->body, left);
            else
                dBodySetRotation(cards[c]->body, right);
        }
        
        if (n==1) // top of the house
            break;
        
        // horizontal cards
        for (int i=0; i<n-1; ++i, ++c) {
            dBodySetPosition(cards[c]->body,
                    0,
                    -(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i,
                    height + vstep/2);
            dBodySetRotation(cards[c]->body, hrot);
        }
    }
    
}
Esempio n. 4
0
void Machine::reset(void)
{
	dMatrix3 R;
	if(this==&machine[0])
	{

		dRFromAxisAndAngle(R, 0, 0, 1, -M_PI/2.0);
		dBodySetPosition(body[0], -court.x/2, 0, 26.5);
		dBodySetRotation(body[0], R);
		dBodySetPosition(body[1], -court.x/2+17, 0, 26.5);
		dBodySetRotation(body[1], R);
		int i;
		for(i=0; i<3; i++)
			dBodySetRotation(wheel[i], R);
		dBodySetPosition(wheel[0], 12-court.x/2, 0, 26);
		dBodySetPosition(wheel[1], -7-court.x/2, 6, 26);
		dBodySetPosition(wheel[2], -7-court.x/2, -6, 26);
	}
	else
	{
		dRFromAxisAndAngle(R, 0, 0, 1, M_PI/2.0);
		dBodySetPosition(body[0], court.x/2, 0, 26.5);
		dBodySetRotation(body[0], R);
		dBodySetPosition(body[1], court.x/2-17, 0, 26.5);
		dBodySetRotation(body[1], R);
		int i;
		for(i=0; i<3; i++)
			dBodySetRotation(wheel[i], R);
		dBodySetPosition(wheel[0], -12+court.x/2, 0, 26);
		dBodySetPosition(wheel[1], 7+court.x/2, -6, 26);
		dBodySetPosition(wheel[2], 7+court.x/2, 6, 26);
	}
	steer=0;
	pushtime=0;
	energy=4;
	adjuststeer();
	dBodySetLinearVel(body[0], 0, 0, 0);
	dBodySetLinearVel(body[1], 0, 0, 0);
	dBodySetAngularVel(body[0], 0, 0, 0);
	dBodySetAngularVel(body[1], 0, 0, 0);
	int i;
	for(i=0; i<3; i++)
	{
		dBodySetLinearVel(wheel[i], 0, 0, 0);
		dBodySetAngularVel(wheel[i], 0, 0, 0);
	}
	upsidedowntime=0;

	shieldcount=0;
	meshatekcount=0;

	powerupammo=0;
	powerupcount=0;
	powerupcharge=0;
}
Esempio n. 5
0
void Object::setRotation(float x,float y,float z)
{
	dMatrix3 R,R0,R1,R2,R3;

	dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(x));
	dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(y));
	dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(z));
	dMultiply0 (R0,R1,R2,3,3,3);
	dMultiply0 (R, R0,R3,3,3,3);

	dGeomSetRotation(iGeom,R);
	dBodySetRotation(iBody,R);
}
Esempio n. 6
0
void Object::MakeGeom(dSpaceID space)
{
	iGeom = dCreateBox(space,iSize.x,iSize.y,iSize.z);

	dMatrix3 R,R0,R1,R2,R3;

	dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(iRotate.x));
	dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(iRotate.y));
	dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(iRotate.z));
	dMultiply0 (R0,R1,R2,3,3,3);
	dMultiply0 (R, R0,R3,3,3,3);

	dGeomSetRotation(iGeom,R);
	dGeomSetPosition(iGeom, iPosition.x, iPosition.y, iPosition.z);
};
Esempio n. 7
0
int test_dBoxTouchesBox()
{
  int k,bt1,bt2;
  dVector3 p1,p2,side1,side2;
  dMatrix3 R1,R2;

  dSimpleSpace space(0);
  dGeomID box1 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box1);
  dGeomID box2 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box2);

  dMakeRandomVector (p1,3,0.5);
  dMakeRandomVector (p2,3,0.5);
  for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
  for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
  dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
  dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);

  dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
  dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
  dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
  dGeomSetRotation (box1,R1);
  dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
  dGeomSetRotation (box2,R2);
  draw_all_objects (space);

  int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
  int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
  bt1 = t1 || t2;
  bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);

  if (bt1 != bt2) FAILED();

  /*
    // some more debugging info if necessary
    if (bt1 && bt2) printf ("agree - boxes touch\n");
    if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
    if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
			     "says no\n");
    if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
			     "says yes\n");
  */

  PASSED();
}
static void simLoop (int pause)
{
  // stop after a given number of iterations, as long as we are not in
  // interactive mode
  if (cmd_graphics && !cmd_interactive &&
      (iteration >= max_iterations)) {
    dsStop();
    return;
  }
  iteration++;

  if (!pause) {
    // do stuff for this test and check to see if the joint is behaving well
    dReal error = doStuffAndGetError (test_num);
    if (error > max_error) max_error = error;
    if (cmd_interactive && error < dInfinity) {
      printf ("scaled error = %.4e\n",error);
    }

    // take a step
    dWorldStep (world,STEPSIZE);

    // occasionally re-orient the first body to create a deliberate error.
    if (cmd_occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[0]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[0],R3);

	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[0]);
	dBodySetPosition (body[0],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }
  }

  if (cmd_graphics) {
    dReal sides1[3] = {SIDE,SIDE,SIDE};
    dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
    dsSetTexture (DS_WOOD);
    dsSetColor (1,1,0);
    dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
    if (body[1]) {
      dsSetColor (0,1,1);
      dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
    }
  }
}
Esempio n. 9
0
void AvatarGameObj::update_geom_offsets() {
  // Update the orientation of our physical geom to match _uprightness
  dMatrix3 grot;
  dRFromAxisAndAngle(grot, 1, 0, 0, _uprightness*M_PI_2);
  dGeomSetOffsetRotation(get_entity().get_geom("physical"), grot);
  
  // Update the position of the sticky_attach geom to match _uprightness
  dGeomSetOffsetPosition(get_entity().get_geom("sticky_attach"), 0, -sin(_uprightness*M_PI_2) + RUNNING_MAX_DELTA_Y_POS, 0);
}
static void simLoop (int pause)
{
  const dReal kd = -0.3;	// angular damping constant
  const dReal ks = 0.5;	// spring constant
  if (!pause) {
    // add an oscillating torque to body 0, and also damp its rotational motion
    static dReal a=0;
    const dReal *w = dBodyGetAngularVel (body[0]);
    dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
    a += 0.01;

    // add a spring force to keep the bodies together, otherwise they will
    // fly apart along the slider axis.
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
		   ks*(p2[2]-p1[2]));
    dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
		   ks*(p1[2]-p2[2]));

    // occasionally re-orient one of the bodies to create a deliberate error.
    if (occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[0]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[0],R3);

	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[0]);
	dBodySetPosition (body[0],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }

    dWorldStep (world,0.05);
  }

  dReal sides1[3] = {SIDE,SIDE,SIDE};
  dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
  dsSetTexture (DS_WOOD);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
Esempio n. 11
0
void Wheel::disposePhysics(Utils::Xml &x) {
    dMatrix3 R;
    dRFromAxisAndAngle(R, 0.0, 1.0, 0.0, 
		       Ogre::Degree(x.mustOReal("rotation.y")).valueRadians());
    dBodySetRotation(ph.body, R);
    
    Utils::Xml w("../xml/car.xml", "car");
    dGeomSetPosition(ph.geom, 
		     w.mustOReal("global-position.x") + x.mustOReal("position.x"), 
		     w.mustOReal("global-position.y") + x.mustOReal("position.y"), 
		     w.mustOReal("global-position.z") + x.mustOReal("position.z")
		     );
}
Esempio n. 12
0
void StaticWorldObject::AddLeaf(ssgLeaf *leaf, sgVec3 initialpos)
{
  // traverse the triangles
  int cnt = leaf->getNumTriangles() ;
  int nv  = leaf->getNumVertices() ;
//  int nn  = leaf->getNumNormals() ;

  float *vertices = new float[3*nv];
  int   *indices  = new int[3*cnt];

  int i;
  for (i=0; i<nv; i++)
  {
    float *v = leaf->getVertex( i ) ;
    assert(v);
    memcpy(vertices+3*i, v, 3*sizeof(float));
  }
  for (i=0; i<cnt; i++)
  {
    short idx0, idx1, idx2 ;
    leaf->getTriangle( i, &idx0, &idx1, &idx2 ) ;
    indices[3*i+0]=idx0;
    indices[3*i+1]=idx1;
    indices[3*i+2]=idx2;
  }

  dTriMeshDataID data = dGeomTriMeshDataCreate();
  dataids.push_back(data);
  dGeomTriMeshDataBuildSingle
  (
    data, 
    vertices,
    3*sizeof(float), 
    nv, 
    indices,
    cnt*3, 
    3*sizeof(int)
  );
  //fprintf(stderr,"Adding trimesh with %d verts, %d indices\n", nv, cnt*3);
  dGeomID trimesh = dCreateTriMesh(space, data, 0,0,0);
  geomids.push_back(trimesh);
  dGeomSetPosition(trimesh, initialpos[0], initialpos[1], initialpos[2]);
  dMatrix3 R;
  dRFromAxisAndAngle (R, 0,1,0, 0.0);
  dGeomSetRotation (trimesh, R);
  dGeomSetData(trimesh, this);
}
Esempio n. 13
0
void construirQuinas()
{
    // Walls
    // 0 front      1 back
    // 2 right top  3 right back
    // 4 left top   5 left back
    int n[7] = {1, 1, -1, -1, 1, 1, -1};
    dMatrix3 R;

    for(int i=0; i < 4; i++)
    {
        triangle[i] = dCreateBox(space,TRIANGLE_THICK,TRIANGLE_LENGTH,HEIGHT);
        dGeomSetPosition(triangle[i],n[i]*TRIANGLE_X,n[i+1]*TRIANGLE_Y,0);
        dRFromAxisAndAngle (R,0,0,1,n[i*2]*M_PI_4);
        dGeomSetRotation (triangle[i], R);
    }
}
Esempio n. 14
0
void Robot::drawLabel()
{
    glPushMatrix();
    dVector3 pos;
    float fr_r,fr_b,fr_n;w->g->getFrustum(fr_r,fr_b,fr_n);
    const float txtWidth = 12.0f*fr_r/(float)w->g->getWidth();
    const float txtHeight = 24.0f*fr_b/(float)w->g->getHeight();
    pos[0] = dBodyGetPosition(chassis->body)[0];
    pos[1] = dBodyGetPosition(chassis->body)[1];
    pos[2] = dBodyGetPosition(chassis->body)[2];    
    float xyz[3],hpr[3];
    w->g->getViewpoint(xyz,hpr);
    float ax = -pos[0]+xyz[0];
    float ay = -pos[1]+xyz[1];
    float az = -pos[2]+xyz[2];
    float fx,fy,fz;
    float rx,ry,rz;
    w->g->getCameraForward(fx,fy,fz);
    w->g->getCameraRight(rx,ry,rz);
    normalizeVector(fx,fy,fz);
    normalizeVector(rx,ry,rz);
    float zz = fx*ax + fy*ay + fz*az;
    float zfact = zz/fr_n;
    pos[2] += cfg->robotSettings.RobotHeight*0.5f + cfg->robotSettings.BottomHeight + cfg->robotSettings.WheelRadius + txtHeight*zfact;
    dMatrix3 rot;
    dRFromAxisAndAngle(rot,0,0,0,0);
    float tx = fy*rz-ry*fz;
    float ty = rx*fz-fx*rz;
    float tz = fx*ry-fy*rx;
    w->g->setTransform(pos,rot);
    w->g->useTexture((m_rob_id-1) + 11 + 10*((on)?0:1));
    glShadeModel (GL_FLAT);
    glDisable(GL_LIGHTING);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
    glBegin(GL_QUADS);
    glTexCoord2f(1,1);glVertex3f( txtWidth*rx*zfact +txtHeight*tx*zfact, txtWidth*ry*zfact +txtHeight*ty*zfact, txtWidth*rz*zfact +txtHeight*tz*zfact);
    glTexCoord2f(0,1);glVertex3f(-txtWidth*rx*zfact +txtHeight*tx*zfact,-txtWidth*ry*zfact +txtHeight*ty*zfact,-txtWidth*rz*zfact +txtHeight*tz*zfact);
    glTexCoord2f(0,0);glVertex3f(-txtWidth*rx*zfact -txtHeight*tx*zfact,-txtWidth*ry*zfact -txtHeight*ty*zfact,-txtWidth*rz*zfact -txtHeight*tz*zfact);
    glTexCoord2f(1,0);glVertex3f( txtWidth*rx*zfact -txtHeight*tx*zfact, txtWidth*ry*zfact -txtHeight*ty*zfact, txtWidth*rz*zfact -txtHeight*tz*zfact);
    glEnd();
    glDisable(GL_BLEND);
    w->g->noTexture();
    glPopMatrix();
}
Esempio n. 15
0
int test_sphere_point_depth()
{
  int j;
  dVector3 p,q;
  dMatrix3 R;
  dReal r,d;

  dSimpleSpace space(0);
  dGeomID sphere = dCreateSphere (0,1);
  dSpaceAdd (space,sphere);

  // ********** make a random sphere of radius r at position p

  r = dRandReal()+0.1;
  dGeomSphereSetRadius (sphere,r);
  dMakeRandomVector (p,3,1.0);
  dGeomSetPosition (sphere,p[0],p[1],p[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (sphere,R);

  // ********** test center point has depth r

  if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();

  // ********** test point on surface has depth 0

  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();

  // ********** test point at random depth

  d = (dRandReal()*2-1) * r;
  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();

  PASSED();
}
Esempio n. 16
0
void HarrierSim::makeHarrier()
{
  dMass mass;
  itsHarrierBody = dBodyCreate(world);
  //pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;

  dBodySetPosition(itsHarrierBody,0,0.2*5,4.94);

  dMatrix3 R;
  dRFromAxisAndAngle (R,1,0,0,0);
  dBodySetRotation(itsHarrierBody, R);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, itsHarrierWeight,itsHarrierWidth, itsHarrierLength, 0.5);
  //dMassSetCappedCylinderTotal(&mass,itsHarrierWeight,3,itsHarrierWidth,itsHarrierLength/2);
  dMassRotate(&mass, R);
  dBodySetMass(itsHarrierBody,&mass);
  itsHarrierGeom = dCreateBox(space, itsHarrierWidth, itsHarrierLength, 0.5);
  dGeomSetRotation(itsHarrierGeom, R);
  dGeomSetBody(itsHarrierGeom, itsHarrierBody);
}
Esempio n. 17
0
int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

  // create world
  dInitODE2(0);
  world = dWorldCreate();
 
  space = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // note: can't share tridata if intending to trimesh-trimesh collide
  const unsigned preprocessFlags = (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES) | (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES);
  TriData1 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  dGeomTriMeshDataPreprocess2(TriData1, preprocessFlags, NULL);
  TriData2 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  dGeomTriMeshDataPreprocess2(TriData2, preprocessFlags, NULL);
  
  TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
  TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
  dGeomSetData(TriMesh1, TriData1);
  dGeomSetData(TriMesh2, TriData2);
  
  {dGeomSetPosition(TriMesh1, 0, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh1, Rotation);}

  {dGeomSetPosition(TriMesh2, 1, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh2, Rotation);}
  
  dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
  dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
  dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
  // dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
  dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dThreadingImplementationShutdownProcessing(threading);
  dThreadingFreeThreadPool(pool);
  dWorldSetStepThreadingImplementation(world, NULL, NULL);
  dThreadingFreeImplementation(threading);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
static void command (int cmd)
{
  size_t i;
  int j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*) i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCCylinder (0,radius,length);
	  dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
  else if (cmd == '1') {
    write_world = 1;
  }
}
Esempio n. 19
0
void vmWishboneCar::buildWheelJointLinkage(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
    // some parameters
    dMass m1;
    dMatrix3 rmat;
    const dReal *wheelPos= dBodyGetPosition(wnow->body);

    // uplink
    snow->uplink.body= dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetCylinderTotal(&m1,snow->uplink.mass,3
                          ,snow->uplink.radius
                          ,snow->uplink.width);
    dBodySetMass(snow->uplink.body,&m1);
    dBodySetPosition(snow->uplink.body, wheelPos[0]
            ,wheelPos[1]+0.5*shiftSign*(snow->uplink.width)
            ,wheelPos[2]+0.5*snow->hlink.width);
    dRFromAxisAndAngle(rmat,1.0,0.0,0.0, 90*M_PI/180.0);
    dBodySetRotation(snow->uplink.body,rmat);

    // hlink
    snow->hlink.body= dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetCylinderTotal(&m1,snow->hlink.mass,3
                          ,snow->hlink.radius
                          ,snow->hlink.width);
    dBodySetMass(snow->hlink.body,&m1);
    dBodySetPosition(snow->hlink.body, wheelPos[0]
            ,wheelPos[1],wheelPos[2]);

    // lowlink
    snow->lowlink.body= dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetCylinderTotal(&m1,snow->lowlink.mass,3
                          ,snow->lowlink.radius
                          ,snow->lowlink.width);
    dBodySetMass(snow->lowlink.body,&m1);
    dBodySetPosition(snow->lowlink.body, wheelPos[0]
            ,wheelPos[1]+0.5*shiftSign*(snow->lowlink.width)
            ,wheelPos[2]-0.5*snow->hlink.width);
    dRFromAxisAndAngle(rmat,1.0,0.0,0.0, 90*M_PI/180.0);
    dBodySetRotation(snow->lowlink.body,rmat);

    // lowstrut
    dReal y,z,angle;
    angle= -shiftSign*strutAngle;
    y= wheelPos[1]+0.1*shiftSign*(snow->lowlink.width)
            +0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle));
    z= wheelPos[2]-0.5*snow->hlink.width+ 0.5*snow->lowstrut.width*cos(angle);

    snow->lowstrut.body= dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetCylinderTotal(&m1,snow->lowstrut.mass,3
                          ,snow->lowstrut.radius,snow->uplink.width);
    dBodySetMass(snow->lowstrut.body, &m1);
    dBodySetPosition(snow->lowstrut.body,wheelPos[0],y,z);
    dRFromAxisAndAngle(rmat,1.0,0.0,0.0, angle);
    dBodySetRotation(snow->lowstrut.body,rmat);

    // upstrut
    snow->upstrut.body= dBodyCreate(world);
    dMassSetZero(&m1);
    dMassSetCylinderTotal(&m1,snow->upstrut.mass,3
                          ,snow->upstrut.radius,snow->uplink.width);
    dBodySetMass(snow->upstrut.body, &m1);
    y= wheelPos[1]+0.1*shiftSign*(snow->lowlink.width)
            +shiftSign*snow->lowstrut.width*fabs(sin(angle))
            +0.5*shiftSign*snow->upstrut.width*fabs(sin(angle));
    z= wheelPos[2]-0.5*snow->hlink.width
            +1.0*snow->lowstrut.width*cos(angle)
            +0.5*snow->upstrut.width*cos(angle);
    dBodySetPosition(snow->upstrut.body,wheelPos[0],y,z);
    dRFromAxisAndAngle(rmat,1.0,0.0,0.0, angle);
    dBodySetRotation(snow->upstrut.body,rmat);



}
int main (int argc, char **argv)
{
    dInitODE2(0);
    bool fixed  = true;

    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = 0;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

    dVector3 offset;
    dSetZero (offset, 4);

    // Default test case

    if (argc >= 2 )
    {
        for (int i=1; i < argc; ++i)
        {
            //static int tata = 0;

            if (1)
            {
                if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
                    Help (argv);

                if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
                    type = dJointTypeSlider;

                if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
                {
                    int j = i+1;
                    if ( j+1 > argc      ||  // Check if we have enough arguments
                            argv[j] == '\0' ||  // We should have a path here
                            argv[j][0] == '-' ) // We should have a path not a command line
                        Help (argv);
                    else
                        fn.path_to_textures = argv[++i]; // Increase i since we use this argument
                }
            }


            if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
                tc = 1;

            if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
                tc = 2;

            if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
                tc = 3;

            if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
                fixed = false;
        }
    }

    world = dWorldCreate();
    dWorldSetERP (world, 0.8);

    space = dSimpleSpaceCreate (0);
    contactgroup = dJointGroupCreate (0);
    geom[GROUND] = dCreatePlane (space, 0,0,1,0);
    dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
    dGeomSetCollideBits (geom[GROUND], catBits[ALL]);

    dMass m;
    dMatrix3 R;


    // Create the Obstacle
    geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
    dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
    dGeomSetCollideBits (geom[OBS], catBits[ALL]);
    //Rotation of 45deg around y
    dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
    dGeomSetRotation (geom[OBS], R);
    dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);


    //Rotation of 90deg around y
    // Will orient the Z axis along X
    dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);


    // Create Body2 (Wiil be attached to the world)
    body[BODY2] = dBodyCreate (world);
    // Main axis of cylinder is along X=1
    dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
    dMassAdjust (&m, Mass1);
    geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
    dGeomSetBody (geom[BODY2], body[BODY2]);
    dGeomSetOffsetRotation (geom[BODY2], R);
    dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
    dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
    dBodySetMass (body[BODY2], &m);


    // Create Body 1 (Slider on the prismatic axis)
    body[BODY1] = dBodyCreate (world);
    // Main axis of capsule is along X=1
    dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
    dMassAdjust (&m, Mass1);
    geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
    dGeomSetBody (geom[BODY1], body[BODY1]);
    dGeomSetOffsetRotation (geom[BODY1], R);
    dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
    dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);

    dMass mRect;
    dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
    dMassAdd (&m, &mRect);
    // TODO: translate m?
    geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
    dGeomSetBody (geom[RECT], body[BODY1]);
    dGeomSetOffsetPosition (geom[RECT],
                            (BODY1_LENGTH-RECT_SIDES[0]) /2.0,
                            0.0,
                            -RADIUS -RECT_SIDES[2]/2.0);
    dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
    dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );

    dBodySetMass (body[BODY1], &m);



    setPositionBodies (tc);


    if ( fixed )
    {
        // Attache external cylinder to the world
        dJointID fixed = dJointCreateFixed (world,0);
        dJointAttach (fixed , NULL, body[BODY2]);
        dJointSetFixed (fixed );
        dWorldSetGravity (world,0,0,-0.8);
    }
    else
    {
        dWorldSetGravity (world,0,0,0);
    }




    // The static is here only to help debugging
    switch (type)
    {
    case dJointTypeSlider :
    {
        dSliderJoint *sj = new dSliderJoint (world, 0);
        sj->attach (body[BODY1], body[BODY2]);
        sj->setAxis (1, 0, 0);
        joint = sj;
    }
    break;

    case dJointTypePiston : // fall through default
    default:
    {
        dPistonJoint *pj = new dPistonJoint (world, 0);
        pj->attach (body[BODY1], body[BODY2]);
        pj->setAxis (1, 0, 0);

        dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);

        joint = pj;
    }
    break;
    };


    // run simulation
    dsSimulationLoop (argc,argv,400,300,&fn);

    delete joint;
    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}
Esempio n. 21
0
void ODE_Particle::setOrientation(dReal ax, dReal ay, dReal az,dReal rad)
{        
    dMatrix3 R;
    dRFromAxisAndAngle(R,ax,ay,az,rad);
    dBodySetRotation(body,R);
}
Esempio n. 22
0
static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;
  bool setBody = false;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' 
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*)(size_t)i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
 
            setBody = true;
            // start accumulating masses for the composite geometries
            dMass m2;
            dMassSetZero (&m);

            dReal dpos[GPB][3];	// delta-positions for composite geometries
            dMatrix3 drot[GPB];
      
            // set random delta positions
            for (j=0; j<GPB; j++)
                for (k=0; k<3; k++)
                    dpos[j][k] = dRandReal()*0.3-0.15;
    
            for (k=0; k<GPB; k++) {
                if (k==0) {
                    dReal radius = dRandReal()*0.25+0.05;
                    obj[i].geom[k] = dCreateSphere (space,radius);
                    dMassSetSphere (&m2,DENSITY,radius);
                } else if (k==1) {
                    obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
                    dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
                } else {
                    dReal radius = dRandReal()*0.1+0.05;
                    dReal length = dRandReal()*1.0+0.1;
                    obj[i].geom[k] = dCreateCapsule(space,radius,length);
                    dMassSetCapsule(&m2,DENSITY,3,radius,length);
                }

                dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
                                   dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
                dMassRotate(&m2,drot[k]);
		
                dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);

                // add to the total mass
                dMassAdd(&m,&m2);

            }
            for (k=0; k<GPB; k++) {
                dGeomSetBody(obj[i].geom[k],obj[i].body);
                dGeomSetOffsetPosition(obj[i].geom[k],
                                       dpos[k][0]-m.c[0],
                                       dpos[k][1]-m.c[1],
                                       dpos[k][2]-m.c[2]);
                dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
            }
            dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
            dBodySetMass(obj[i].body,&m);

        } else if (cmd == 'v') {

            dMassSetBox (&m,DENSITY,0.25,0.25,0.25);

            obj[i].geom[0] = dCreateConvex(space,
                                           planes,
                                           planecount,
                                           points,
                                           pointcount,
                                           polygons);
        }

        if (!setBody) { // avoid calling for composite geometries
            for (k=0; k < GPB; k++)
                if (obj[i].geom[k])
                    dGeomSetBody(obj[i].geom[k],obj[i].body);

            dBodySetMass(obj[i].body,&m);
        }
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}
static void simLoop (int pause)
{
	int i, j;
		
	dsSetTexture (DS_WOOD);

	if (!pause) {
#ifdef BOX
		dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
		for (j = 0; j < joints; j++)
		{
			dReal curturn = dJointGetHinge2Angle1 (joint[j]);
			//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
			dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
			dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
			dJointSetHinge2Param(joint[j],dParamVel2,speed);
			dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
			dBodyEnable(dJointGetBody(joint[j],0));
			dBodyEnable(dJointGetBody(joint[j],1));
		}		
		if (doFast)
		{
			dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
			dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
			dWorldStepFast1 (world,0.05,ITERS);
#endif
			dJointGroupEmpty (contactgroup);
		}
		else
		{
			dSpaceCollide (space,0,&nearCallback);
			dWorldStep (world,0.05);
			dJointGroupEmpty (contactgroup);
		}
		
		for (i = 0; i < wb; i++)
		{
			b = dGeomGetBody(wall_boxes[i]);
			if (dBodyIsEnabled(b)) 
			{
				bool disable = true;
				const dReal *lvel = dBodyGetLinearVel(b);
				dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
				if (lspeed > DISABLE_THRESHOLD)
					disable = false;
				const dReal *avel = dBodyGetAngularVel(b);
				dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
				if (aspeed > DISABLE_THRESHOLD)
					disable = false;
				
				if (disable)
					wb_stepsdis[i]++;
				else
					wb_stepsdis[i] = 0;
				
				if (wb_stepsdis[i] > DISABLE_STEPS)
				{
					dBodyDisable(b);
					dsSetColor(0.5,0.5,1);
				}
				else
					dsSetColor(1,1,1);

			}
			else
				dsSetColor(0.4,0.4,0.4);
			dVector3 ss;
			dGeomBoxGetLengths (wall_boxes[i], ss);
			dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
		}
	}
	else
	{
		for (i = 0; i < wb; i++)
		{
			b = dGeomGetBody(wall_boxes[i]);
			if (dBodyIsEnabled(b))
				dsSetColor(1,1,1);
			else
				dsSetColor(0.4,0.4,0.4);
			dVector3 ss;
			dGeomBoxGetLengths (wall_boxes[i], ss);
			dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
		}
	}
	
	dsSetColor (0,1,1);
	dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
	for (i = 0; i < boxes; i++)
		dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
	dsSetColor (1,1,1);
	for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
				   dGeomGetRotation(sphere[i]),RADIUS);
	
	// draw the cannon
	dsSetColor (1,1,0);
	dMatrix3 R2,R3,R4;
	dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
	dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
	dMultiply0 (R4,R2,R3,3,3,3);
	dReal cpos[3] = {CANNON_X,CANNON_Y,1};
	dReal csides[3] = {2,2,2};
	dsDrawBox (cpos,R2,csides);
	for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
	dsDrawCylinder (cpos,R4,3,0.5);
	
	// draw the cannon ball
	dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
		      CANNON_BALL_RADIUS);
}
static void command (int cmd)
{
	switch (cmd) {
	case 'a': case 'A':
		speed += 0.3;
		break;
	case 'z': case 'Z':
		speed -= 0.3;
		break;
	case ',':
		turn += 0.1;
		if (turn > 0.3)
			turn = 0.3;
		break;
	case '.':
		turn -= 0.1;
		if (turn < -0.3)
			turn = -0.3;
		break;
	case ' ':
		speed = 0;
		turn = 0;
		break;
	case 'f': case 'F':
		doFast = !doFast;
		break;
	case '+':
		dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1);
		break;
	case '-':
		dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1);
		break;
	case 'r': case 'R':
		resetSimulation();
		break;
	case '[':
		cannon_angle += 0.1;
		break;
	case ']':
		cannon_angle -= 0.1;
		break;
	case '1':
		cannon_elevation += 0.1;
		break;
	case '2':
		cannon_elevation -= 0.1;
		break;
	case 'x': case 'X': {
		dMatrix3 R2,R3,R4;
		dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
		dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
		dMultiply0 (R4,R2,R3,3,3,3);
		dReal cpos[3] = {CANNON_X,CANNON_Y,1};
		for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2];
		dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]);
		dReal force = 10;
		dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]);
		dBodySetAngularVel (cannon_ball_body,0,0,0);
		break;
	}
	}
}
Esempio n. 25
0
static void command (int cmd)
{
    int i,j,k;
    dReal sides[3];
    dMass m;
    bool setBody = false;

    cmd = locase (cmd);
    if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') {
        if (num < NUM) {
            i = num;
            num++;
        }
        else {
            i = nextobj;
            nextobj++;
            if (nextobj >= num) nextobj = 0;

            // destroy the body and geoms for slot i
            dBodyDestroy (obj[i].body);
            for (k=0; k < GPB; k++) {
                if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
            }
            memset (&obj[i],0,sizeof(obj[i]));
        }

        obj[i].body = dBodyCreate (world);
        for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

        dMatrix3 R;
        if (random_pos) {
            dBodySetPosition (obj[i].body,
                              dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
            dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
                                dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
        }
        else {
            dReal maxheight = 0;
            for (k=0; k<num; k++) {
                const dReal *pos = dBodyGetPosition (obj[k].body);
                if (pos[2] > maxheight) maxheight = pos[2];
            }
            dBodySetPosition (obj[i].body, 0,0,maxheight+1);
            dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
        }
        dBodySetRotation (obj[i].body,R);
        dBodySetData (obj[i].body,(void*)(size_t)i);

        if (cmd == 'b') {
            dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
            obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
        }
        else if (cmd == 'c') {
            sides[0] *= 0.5;
            dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
            obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
        } else if (cmd == 'v') {

            dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
            obj[i].geom[0] = dCreateConvex(space,
                                           planes,
                                           planecount,
                                           points,
                                           pointcount,
                                           polygons);
        }
        else if (cmd == 'y') {
            sides[1] *= 0.5;
            dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
            obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
        }
	else if (cmd == 's') {
            sides[0] *= 0.5;
            dMassSetSphere (&m,DENSITY,sides[0]);
            obj[i].geom[0] = dCreateSphere (space,sides[0]);
        }
        else if (cmd == 'm') {
            dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
            dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, 
                                        (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
            dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);


            obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);

            // remember the mesh's dTriMeshDataID on its userdata for convenience.
            dGeomSetData(obj[i].geom[0], new_tmdata);

            dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
            printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
            dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
            dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
        }
        else if (cmd == 'x') {

            setBody = true;
            // start accumulating masses for the composite geometries
            dMass m2;
            dMassSetZero (&m);

            dReal dpos[GPB][3];	// delta-positions for composite geometries
            dMatrix3 drot[GPB];
      
            // set random delta positions
            for (j=0; j<GPB; j++)
                for (k=0; k<3; k++)
                    dpos[j][k] = dRandReal()*0.3-0.15;
    
            for (k=0; k<GPB; k++) {
                if (k==0) {
                    dReal radius = dRandReal()*0.25+0.05;
                    obj[i].geom[k] = dCreateSphere (space,radius);
                    dMassSetSphere (&m2,DENSITY,radius);
                } else if (k==1) {
                    obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
                    dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
                } else {
                    dReal radius = dRandReal()*0.1+0.05;
                    dReal length = dRandReal()*1.0+0.1;
                    obj[i].geom[k] = dCreateCapsule(space,radius,length);
                    dMassSetCapsule(&m2,DENSITY,3,radius,length);
                }

                dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
                                   dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
                dMassRotate(&m2,drot[k]);
		
                dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);

                // add to the total mass
                dMassAdd(&m,&m2);

            }
            for (k=0; k<GPB; k++) {
                dGeomSetBody(obj[i].geom[k],obj[i].body);
                dGeomSetOffsetPosition(obj[i].geom[k],
                                       dpos[k][0]-m.c[0],
                                       dpos[k][1]-m.c[1],
                                       dpos[k][2]-m.c[2]);
                dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
            }
            dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
            dBodySetMass(obj[i].body,&m);

        }

        if (!setBody) { // avoid calling for composite geometries
            for (k=0; k < GPB; k++)
                if (obj[i].geom[k])
                    dGeomSetBody(obj[i].geom[k],obj[i].body);

            dBodySetMass(obj[i].body,&m);
        }
    }

    if (cmd == ' ') {
        selected++;
        if (selected >= num) selected = 0;
        if (selected < 0) selected = 0;
    }
    else if (cmd == 'd' && selected >= 0 && selected < num) {
        dBodyDisable (obj[selected].body);
    }
    else if (cmd == 'e' && selected >= 0 && selected < num) {
        dBodyEnable (obj[selected].body);
    }
    else if (cmd == 'a') {
        show_aabb ^= 1;
    }
    else if (cmd == 't') {
        show_contacts ^= 1;
    }
    else if (cmd == 'r') {
        random_pos ^= 1;
    }
}
Esempio n. 26
0
void  makeRobot_Nleg() 
{ 
	for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		dReal torso_m = 50.0;                    // Mass of body
		//	torso_m[segment] = 10.0;
		
		dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
		
		//for four legs
		//		dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		//																		{-cx1,-cx1,-cx1}};
		dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
			{0,0,0}};
		
		dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
			{-cy1,-cy1,-cy1}};
		dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
		
		dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
		dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
		dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
		
		//  //for one leg
		//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
		//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
		//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
		
		//for four legs
		dReal axis_x[num_legs][num_links];
		dReal axis_y[num_legs][num_links];
		dReal axis_z[num_legs][num_links];
		for(int i = 0; i < num_legs; ++i) {
			axis_x[i][0] = 0.0;
			axis_x[i][1] = 1.0;
			axis_x[i][2] = 0.0;
			
			axis_y[i][0] = 1.0;
			axis_y[i][1] = 0.0;
			axis_y[i][2] = 1.0;
			
			axis_z[i][0] = 0.0;
			axis_z[i][1] = 0.0;
			axis_z[i][2] = 0.0;
		}
		// For mation of the body
		dMass mass; 
		torso[segment].body  = dBodyCreate(world);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
		dBodySetMass(torso[segment].body,&mass);
		torso[segment].geom = dCreateBox(space,lx, ly, lz); 
		dGeomSetBody(torso[segment].geom, torso[segment].body);
		dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ); 
		
		// Formation of leg
		dMatrix3 R;                          // Revolution queue
		dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) {
				THETA[segment][i][j] = 0;
				
				leg[segment][i][j].body = dBodyCreate(world);
				if (j == 0)
					dBodySetRotation(leg[segment][i][j].body,R);
				dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);				
				dMassSetZero(&mass);
				dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
				dBodySetMass(leg[segment][i][j].body, &mass);
				//if(i==1 and j==2)                                         //to set the length of one leg differently
				//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
				//else
				leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
				dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
			}
		}
		
		// Formation of joints (and connecting them up)
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) { 
				leg[segment][i][j].joint = dJointCreateHinge(world, 0);
				if (j == 0){ 
					dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
					dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
					dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop,  .50*M_PI);
				}
				else
					dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
				
				dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
				dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
			} 
		}
	}
	
	
#ifdef USE_NLEG_ROBOT
	// link torsos
	for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		switch (hingeType) {
			case 1: //Hinge Joint, X axis  (back-breaker)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
				break;
			case 2: //Hinge Joint, Y axis (???)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 3: //Hinge Joint, Z axis (snake-like)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
				break;
			case 4: // Slider, Y axis  (??)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 5: // Slider, X axis  (extendo)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				break;
			case 6: //Universal Joint
				torso[segment].joint = dJointCreateUniversal(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
				dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);				
				break;
			case 7: //Ball Joint
				torso[segment].joint = dJointCreateBall(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				break;
			case 8:  // Fixed
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(torso[segment].joint, dParamHiStop,  0.00*M_PI);
				break;
			default:
				assert(false); // not a valid hinge type
				break;
		}
	}
#endif
}
Esempio n. 27
0
int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

  if (argc >= 2 ) {
    for (int i=1; i < argc; ++i) {
      if (  0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
        Help (argv);

      if (  0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
        type = dJointTypePR;

      if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
        int j = i+1;
        if ( j+1 > argc      ||  // Check if we have enough arguments
             argv[j] == '\0' ||  // We should have a path here
             argv[j][0] == '-' ) // We should have a path not a command line
          Help (argv);
        else
          fn.path_to_textures = argv[++i]; // Increase i since we use this argument
      }
    }
  }

  dInitODE2(0);

  world.setERP (0.8);

  space = dSimpleSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  geom[GROUND] = dCreatePlane (space, 0,0,1,0);
  dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
  dGeomSetCollideBits (geom[GROUND], catBits[ALL]);

  dMass m;

  // Create the body attached to the World
  body[W].create (world);
  // Main axis of cylinder is along X=1
  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
  m.adjust (Mass1);
  geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
  dGeomSetBody (geom[W], body[W]);
  dGeomSetCategoryBits (geom[W], catBits[W]);
  dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
  body[W].setMass(m);





  // Create the dandling body
  body[D].create(world);
  // Main axis of capsule is along X=1
  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
  m.adjust (Mass1);
  geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
  dGeomSetBody (geom[D], body[D]);
  dGeomSetCategoryBits (geom[D], catBits[D]);
  dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
  body[D].setMass(&m);


  // Create the external part of the slider joint
  geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
  dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
  dGeomSetCollideBits (geom[EXT],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );

  // Create the internal part of the slider joint
  geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
                          INT_EXT_RATIO*extDim[Y],
                          INT_EXT_RATIO*extDim[Z]);
  dGeomSetCategoryBits (geom[INT], catBits[INT]);
  dGeomSetCollideBits (geom[INT],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );


  dMatrix3 R;
  dGeomID id;
  // Create the first axis of the universal joi9nt
  geom[AXIS1] = dCreateGeomTransform (space);
  //Rotation of 90deg around y
  dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
  dGeomSetRotation (geom[AXIS1], R);
  dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
  dGeomSetCollideBits (geom[AXIS1],
                       catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
  id = geom[AXIS1];
  dGeomTransformSetGeom (geom[AXIS1],  dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );


  // Create the second axis of the universal joint
  geom[AXIS2] = dCreateGeomTransform (space);
  //Rotation of 90deg around y
  dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
  dGeomSetRotation (geom[AXIS2], R);
  dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
  dGeomSetCollideBits (geom[AXIS2],
                       catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
  id = geom[AXIS2];
  dGeomTransformSetGeom (geom[AXIS2],  dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );


  // Create the anchor
  geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
  dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
  dGeomSetCollideBits (geom[ANCHOR],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );



  if (body[W]) {
    body[W].setPosition(0, 0, 5);
  }


  if (geom[EXT]) {
    dGeomSetPosition (geom[EXT], 0,0,3.8);
  }
  if (geom[INT]) {
    dGeomSetPosition (geom[INT], 0,0,2.6);
  }
  if (geom[AXIS1]) {
    dGeomSetPosition (geom[AXIS1], 0,0,2.5);
  }
  if (geom[AXIS2]) {
    dGeomSetPosition (geom[AXIS2], 0,0,2.5);
  }

  if (geom[ANCHOR]) {
    dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
  }

  if (body[D]) {
    body[D].setPosition(0,0,1.5);
  }



  // Attache the upper box to the world
  dJointID fixed = dJointCreateFixed (world,0);
  dJointAttach (fixed , NULL, body[W]);
  dJointSetFixed (fixed );

  if (type == dJointTypePR) {
    dPRJoint *pr = new dPRJoint (world, 0);
    pr->attach (body[W], body[D]);
    pr->setAxis1 (0, 0, -1);
    pr->setAxis2 (1, 0, 0);
    joint = pr;

    dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
  }
  else {
    dPUJoint *pu = new dPUJoint (world, 0);
    pu->attach (body[W], body[D]);
    pu->setAxis1 (1, 0, 0);
    pu->setAxis2 (0, 1, 0);
    pu->setAxisP (0, 0, -1);
    joint = pu;

    dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
  }


  // run simulation
  dsSimulationLoop (argc,argv,400,300,&fn);

  delete joint;
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
Esempio n. 28
0
// ロボットの生成
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);

}
static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*)(size_t)i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
    else if (cmd == 'y') {
      sides[1] *= 0.5;
      dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
	else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'm') {
      dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
      dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, 
		  (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));

      obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);

      // remember the mesh's dTriMeshDataID on its userdata for convenience.
      dGeomSetData(obj[i].geom[0], new_tmdata);

      dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
      printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
      dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
      dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCapsule (0,radius,length);
	  dMassSetCapsule (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}
Esempio n. 30
0
/*** Formation of robot ***/
void  makeRobot() 
{ 

  dReal torso_m = 10.0;                    // Mass of body
  dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
	
  //for four legs
  dReal x[num_legs][num_links] = {{ cx1, cx1, cx1},{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		{-cx1,-cx1,-cx1},{ cx1, cx1, cx1}};
  dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},{ cy1, cy1, cy1},// Position of each link (y coordinate)
		{-cy1,-cy1,-cy1},{-cy1,-cy1,-cy1}};
  dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}};
	
	
  dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
  dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
  dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
	
	//  //for one leg
	//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
	//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
	//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
	
  //for four legs
  dReal axis_x[num_legs][num_links] = {{ 0,1, 0},{ 0,1,0},{ 0, 1, 0},{ 0, 1, 0}};
  dReal axis_y[num_legs][num_links] = {{ 1,0, 1},{ 1,0,1},{ 1, 0, 1},{ 1, 0, 1}};
  dReal axis_z[num_legs][num_links] = {{ 0,0, 0},{ 0,0,0},{ 0, 0, 0},{ 0, 0, 0}};
  
  // For mation of the body
  dMass mass; 
  torso[0].body  = dBodyCreate(world);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
  dBodySetMass(torso[0].body,&mass);
  torso[0].geom = dCreateBox(space, lx, ly, lz); 
  dGeomSetBody(torso[0].geom, torso[0].body); 
  dBodySetPosition(torso[0].body, SX, SY, SZ); 
	
  // Formation of leg
  dMatrix3 R;                          // Revolution queue
  dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
  for (int i = 0; i < num_legs; i++) { 
    for (int j = 0; j < num_links; j++) { 
      leg[0][i][j].body = dBodyCreate(world);
      if (j == 0) dBodySetRotation(leg[0][i][j].body,R);
      dBodySetPosition(leg[0][i][j].body, SX+x[i][j], SY+y[i][j], SZ+z[i][j]);
      dMassSetZero(&mass);
      dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
      dBodySetMass(leg[0][i][j].body, &mass);
      //if(i==1 and j==2)                                         //to set the length of one leg differently
      //leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
      //else
      leg[0][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
      dGeomSetBody(leg[0][i][j].geom,leg[0][i][j].body);
    }
  }
	
  // Formation of joints (and connecting them up)
  for (int i = 0; i < num_legs; i++) { 
    for (int j = 0; j < num_links; j++) { 
      leg[0][i][j].joint = dJointCreateHinge(world, 0);
      if (j == 0){ 
				dJointAttach(leg[0][i][j].joint, torso[0].body, leg[0][i][j].body); //connects hip to the environment
				dJointSetHingeParam(leg[0][i][j].joint, dParamLoStop, -.5*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(leg[0][i][j].joint, dParamHiStop,  .5*M_PI);
      }
      else
        dJointAttach(leg[0][i][j].joint, leg[0][i][j-1].body, leg[0][i][j].body);
      dJointSetHingeAnchor(leg[0][i][j].joint, SX+c_x[i][j], SY+c_y[i][j],SZ+c_z[i][j]);
      dJointSetHingeAxis(leg[0][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
    } 
  } 
  
}