Exemplo n.º 1
0
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
	if (!draw_geom){
		return;
	}

  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCapsuleClass) {
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    dsDrawCapsule (pos,R,length,radius);
  }
/*
  // cylinder option not yet implemented
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }
*/
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}
Exemplo n.º 2
0
void drawGeom( dGeomID g, int colored = 0 )
{
	if( !g )		//If the geometry object is missing, end the function.
		return;

	const dReal *position;		//Define pointers to internal positions and orientations.
	const dReal *orientation;	//Pointers to constant objects (so the objects will not change).

	int type = dGeomGetClass( g );				//Get the type of geometry.

	position = dGeomGetPosition( g );		//Then, get the geometry position.
	orientation = dGeomGetRotation( g );	//And get existing geometry orientation.
	
	if( type == dBoxClass )						//Is it a box?
	{		
		dReal sides[3];
		dGeomBoxGetLengths( g, sides );				//Get length of sides.
		renderBox( sides, position, orientation, colored );	//Render the actual box in environment.
	}
	else if (type == dCylinderClass)
	{
		dReal radius, length;
		dGeomCylinderGetParams(g, &radius, &length);
		renderCylinder(radius, length, position, orientation);
	}
	else if (type == dCapsuleClass)
	{
		dReal radius, length;
		dGeomCapsuleGetParams(g, &radius, &length);
		renderCapsule(radius, length, position, orientation);
	}
}
Exemplo n.º 3
0
static void draw_phys_ccylinder(dGeomID geom)
{
    dReal r;
    dReal l;

    dGeomCapsuleGetParams(geom, &r, &l);
    opengl_draw_cap((float) r, (float) l);
}
Exemplo n.º 4
0
static void printCapsule (PrintingContext &c, dxGeom *g)
{
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    c.print ("type","capsule");
    c.print ("radius",radius);
    c.print ("length",length);
}
Exemplo n.º 5
0
static void ccdGeomToCap(const dGeomID g, ccd_cap_t *cap)
{
    dReal r, h;
    ccdGeomToObj(g, (ccd_obj_t *)cap);

    dGeomCapsuleGetParams(g, &r, &h);
    cap->radius = r;
    cap->height = h / 2.;
}
Exemplo n.º 6
0
Arquivo: arm1.cpp Projeto: Ry0/ODE
/*** ロボットアームの描画 ***/
void drawArm()
{
   dReal r,length;

   for (int i = 0; i < NUM; i++ ) {       // カプセルの描画
     dGeomCapsuleGetParams(rlink[i].geom, &r,&length);
     dsDrawCapsule(dBodyGetPosition(rlink[i].body),
     dBodyGetRotation(rlink[i].body),length,r);
   }
}
/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
	if( !g )		//If the geometry object is missing, end the function.
		return;

	if( !position )	//Position was not passed?
		position = dGeomGetPosition( g );		//Then, get the geometry position.

	if( !orientation )	//Orientation was not given?
		orientation = dGeomGetRotation( g );	//And get existing geometry orientation.

	int type = dGeomGetClass( g );				//Get the type of geometry.
	
	if( type == dBoxClass )						//Is it a box?
	{
		dReal sides[3];
		dGeomBoxGetLengths( g, sides );				//Get length of sides.
		renderBox( sides, position, orientation );	//Render the actual box in environment.
	}

	if( type == dSphereClass )					//Is it a sphere?
	{
		dReal radius;
		radius = dGeomSphereGetRadius( g );				//Get the radius.
		renderSphere( radius, position, orientation );	//Render sphere in environment.
	}

	if( type == dCapsuleClass )
	{
		dReal radius;
		dReal length;
		dGeomCapsuleGetParams( g, &radius, &length );	//Get both radius and length.
		renderCapsule( radius, length, position, orientation );	//Render capsule in environment.
	}

	if( type == dGeomTransformClass )					//Is it an embeded geom in a composite body.
	{
		dGeomID g2 = dGeomTransformGetGeom( g );		//Get the actual geometry inside the wrapper.
		const dReal *position2 = dGeomGetPosition( g2 );	//Get position and orientation of wrapped geometry.
		const dReal *orientation2 = dGeomGetRotation( g2 );
		
		dVector3 actualPosition;						//Real world coordinated position and orientation
		dMatrix3 actualOrientation;						//of the wrapped geometry.
		
		dMultiply0_331( actualPosition, orientation, position2 );	//Get world coordinates of geometry position.
		actualPosition[0] += position[0];
		actualPosition[1] += position[1];
		actualPosition[2] += position[2];

		dMultiply0_333( actualOrientation, orientation, orientation2 );	//Get world coordinates of geom orientation.

		drawGeom( g2, actualPosition, actualOrientation );	//Draw embeded geometry.
	}
}
Exemplo n.º 8
0
Arquivo: 6axis4.cpp Projeto: Ry0/ODE
/*** ロボットアームの描画 ***/
void drawArm()
{
   dReal r,length;
   dReal box_length[3] = {0.4, 0.4, 0.4};

   for (int i = 0; i < NUM-1; i++ ) {       // カプセルの描画
     dGeomCapsuleGetParams(rlink[i].geom, &r,&length);
     dsDrawCapsule(dBodyGetPosition(rlink[i].body),
     dBodyGetRotation(rlink[i].body),length,r);
   }
   dGeomBoxGetLengths(rlink[NUM-1].geom, box_length);
   dsDrawBox(dBodyGetPosition(rlink[NUM-1].body), dBodyGetRotation(rlink[NUM-1].body), box_length);
}
Exemplo n.º 9
0
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCapsuleClass) {
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    dsDrawCapsule (pos,R,length,radius);
  } else if (type == dConvexClass) {
   //dVector3 sides={0.50,0.50,0.50};
    dsDrawConvex(pos,R,planes,
                 planecount,
                 points,
                 pointcount,
                 polygons);
  }
/*
  // cylinder option not yet implemented
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }
*/

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}
Exemplo n.º 10
0
Arquivo: dm6.cpp Projeto: Ry0/ODE
void dmDraw(dmObject *obj) /*** 物体の描画 ***/
{
	if (!obj->geom) return;
	obj->p = (double *) dGeomGetPosition(obj->geom);
	obj->R = (double *) dGeomGetRotation(obj->geom);

	dsSetColor(obj->color[0],obj->color[1],obj->color[2]);  // 色の設定(r,g,b)
	// printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]);

	int type = dGeomGetClass(obj->geom);

	switch (type)
	{
	case dBoxClass:
	{
		dVector3 sides;
		dGeomBoxGetLengths(obj->geom,sides);
		dsDrawBox(obj->p,obj->R,sides);
	}
	break;

	case dSphereClass:
		dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom));
		break;

	case  dCapsuleClass:
	{
		dReal radius,length;
		dGeomCapsuleGetParams(obj->geom,&radius,&length);
		dsDrawCapsule(obj->p,obj->R,length,radius);
	}
	break;

	case dCylinderClass:
	{
		dReal radius,length;
		dGeomCylinderGetParams(obj->geom,&radius,&length);
		dsDrawCylinder(obj->p,obj->R,length,radius);
	}
	break;

	default:
		printf("Bad geometry type \n");
	}
}
void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule, 
                                                      int flags, int skip)
{
    const dMatrix3* pRot = (const dMatrix3*)dGeomGetRotation(Capsule);
    memcpy(m_mCapsuleRotation, pRot, sizeof(dMatrix3));

    const dVector3* pDst = (const dVector3*)dGeomGetPosition(Capsule);
    memcpy(m_vCapsulePosition, pDst, sizeof(dVector3));

    m_vCapsuleAxis[0] = m_mCapsuleRotation[0*4 + nCAPSULE_AXIS];
    m_vCapsuleAxis[1] = m_mCapsuleRotation[1*4 + nCAPSULE_AXIS];
    m_vCapsuleAxis[2] = m_mCapsuleRotation[2*4 + nCAPSULE_AXIS];

    // Get size of Capsule
    dGeomCapsuleGetParams(Capsule, &m_vCapsuleRadius, &m_fCapsuleSize);
    m_fCapsuleSize += 2*m_vCapsuleRadius;

    const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
    memcpy(m_mTriMeshRot, pTriRot, sizeof(dMatrix3));

    const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
    memcpy(m_mTriMeshPos, pTriPos, sizeof(dVector3));

    // global info for contact creation
    m_iStride			=skip;
    m_iFlags			=flags;

    // reset contact counter
    m_ctContacts = 0;	

    // reset best depth
    m_fBestDepth  = - MAX_REAL;
    m_fBestCenter = 0;
    m_fBestrt     = 0;

    // reset collision normal
    m_vNormal[0] = REAL(0.0);
    m_vNormal[1] = REAL(0.0);
    m_vNormal[2] = REAL(0.0);
}
Exemplo n.º 12
0
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
  int i;
	
  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCapsuleClass) {
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    dsDrawCapsule (pos,R,length,radius);
  }
  //<---- Convex Object
  else if (type == dConvexClass) 
    {
#if 0
      dsDrawConvex(pos,R,planes,
		   planecount,
		   points,
		   pointcount,
		   polygons);
#else
      dsDrawConvex(pos,R,
       Sphere_planes,
		   Sphere_planecount,
		   Sphere_points,
		   Sphere_pointcount,
		   Sphere_polygons);
#endif
    }
  //----> Convex Object
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }
  if (show_body) {
    dBodyID body = dGeomGetBody(g);
    if (body) {
      const dReal *bodypos = dBodyGetPosition (body); 
      const dReal *bodyr = dBodyGetRotation (body); 
      dReal bodySides[3] = { 0.1, 0.1, 0.1 };
      dsSetColorAlpha(0,1,0,1);
      dsDrawBox(bodypos,bodyr,bodySides); 
    }
  }
  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}
Exemplo n.º 13
0
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
    int i;

    if (!g) return;

    if (dGeomIsSpace(g))
    {
        displaySpace((dSpaceID)g);
        return;
    }
    int type = dGeomGetClass (g);
    if (type == dBoxClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);

        dVector3 sides;
        dGeomBoxGetLengths (g,sides);
        dsDrawBox (pos,R,sides);
    }
    else if (type == dSphereClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
    }
    else if (type == dCapsuleClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCapsuleGetParams (g,&radius,&length);
        dsDrawCapsule (pos,R,length,radius);
    }
    else if (type == dCylinderClass)
    {

        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCylinderGetParams (g,&radius,&length);
        dsDrawCylinder (pos,R,length,radius);
    }
        else
            show_aabb = 0;

    if (show_aabb)
    {
        // draw the bounding box for this geom
        dReal aabb[6];
        dGeomGetAABB (g,aabb);
        dVector3 bbpos;
        for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
        dVector3 bbsides;
        for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
        dMatrix3 RI;
        dRSetIdentity (RI);
        dsSetColorAlpha (1,0,0,0.5);
        dsDrawBox (bbpos,RI,bbsides);
    }
}
Exemplo n.º 14
0
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);
    }
}
Exemplo n.º 15
0
// simulation loop
static void simLoop (int pause)
{
    const dReal *rot;
    dVector3 ax;
    dReal l=0;

    switch (joint->getType() )
    {
    case dJointTypeSlider :
        ( (dSliderJoint *) joint)->getAxis (ax);
        l = ( (dSliderJoint *) joint)->getPosition();
        break;
    case dJointTypePiston :
        ( (dPistonJoint *) joint)->getAxis (ax);
        l = ( (dPistonJoint *) joint)->getPosition();
        break;
    default:
    {} // keep the compiler happy
    }


    if (!pause)
    {
        double simstep = 0.01; // 1ms simulation steps
        double dt = dsElapsedTime();

        int nrofsteps = (int) ceilf (dt/simstep);
        if (!nrofsteps)
            nrofsteps = 1;

        for (int i=0; i<nrofsteps && !pause; i++)
        {
            dSpaceCollide (space,0,&nearCallback);
            dWorldStep (world, simstep);

            dJointGroupEmpty (contactgroup);
        }

        update();


        dReal radius, length;

        dsSetTexture (DS_WOOD);

        drawBox (geom[BODY2], 1,1,0);

        drawBox (geom[RECT], 0,0,1);

        if ( geom[BODY1] )
        {
            const dReal *pos = dGeomGetPosition (geom[BODY1]);
            rot = dGeomGetRotation (geom[BODY1]);
            dsSetColor (0,0,1);

            dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
            dsDrawCapsule (pos, rot, length, radius);
        }


        drawBox (geom[OBS], 1,0,1);


        // Draw the prismatic axis
        if ( geom[BODY1] )
        {
            const dReal *pos = dGeomGetPosition (geom[BODY1]);
            rot = dGeomGetRotation (geom[BODY2]);
            dVector3 p;
            p[X] = pos[X] - l*ax[X];
            p[Y] = pos[Y] - l*ax[Y];
            p[Z] = pos[Z] - l*ax[Z];
            dsSetColor (1,0,0);
            dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
        }


        if (joint->getType() == dJointTypePiston )
        {
            dVector3 anchor;
            dJointGetPistonAnchor(joint->id(), anchor);

            // Draw the rotoide axis
            rot = dGeomGetRotation (geom[BODY2]);
            dsSetColor (1,0.5,0);
            dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);


            dsSetColor (0,1,1);
            rot = dGeomGetRotation (geom[BODY1]);
            dsDrawSphere (anchor, rot, 1.5*RADIUS);
        }

    }
}
Exemplo n.º 16
0
void drawRobot_Nleg()
{
#ifndef NOVIZ
	if(visualize){
		for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
			dReal r,length;
			dVector3 sides;
			
			// Drawing of body
			// Drawing of body
			dsSetTexture(1);
			dsSetColor(2.5,2.5,2.5);
			dGeomBoxGetLengths(torso[segment].geom,sides);
			dsDrawBox(dBodyGetPosition(torso[segment].body),
								dBodyGetRotation(torso[segment].body),sides);
			
			// Drawing oflegs
			dsSetTexture(0);
			dsSetColor(1.3,0.0,0.0);
			
			for (int i = 0; i < num_legs; i++) {
				for (int j = 0; j < num_links; j++ ) { 
					dGeomCapsuleGetParams(leg[segment][i][j].geom, &r,&length);
					if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body),
																	 dBodyGetRotation(leg[segment][i][j].body),0.5*length,1.2*r);
					else       dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body),
																	 dBodyGetRotation(leg[segment][i][j].body),length,r);
				}
			}
		}
		//		int middleBodySegment = BODY_SEGMENTS/2;
		//		cerr <<dBodyGetPosition(torso.body)[0]<<" " <<dBodyGetPosition(torso.body)[1]<< " " << dBodyGetPosition(torso.body)[2]<<endl;
		
		//FROM SIDE
		//		hpr[0] = 90.0f;
		//		hpr[1] = 0.0f;//-1.2f;
		//		hpr[2] = 0.0f;  // point of view[m]
		//		xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0];
		//		xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1]-ceil(log2(BODY_SEGMENTS));
		//		xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f;  // gaze[°] 
		
		//FROM BACK		
		//		hpr[0] = 1.0f;
		//		hpr[1] = 0.0f;//-1.2f;
		//		hpr[2] = 0.0f;  // point of view[m]
		//		xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-ceil(log2(BODY_SEGMENTS));
		//		xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1];
		//		xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f;  // gaze[°] 
		
		
		//FROM BELOW and BACK
		hpr[0] = 1.0f;
		hpr[1] = -1.2f;
		hpr[2] = 0.5f;  // point of view[m]
		
		xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-BODY_SEGMENTS/2.0f;
		xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1];
		xyz[2] = 0.1; // gaze[°]
 		dsSetViewpoint(xyz,hpr);		
	}
#endif
}
Exemplo n.º 17
0
/*** Drawing of robot ***/
void drawRobot()
{
#ifndef NOVIZ
	if(visualize){ 
		dReal r,length;
		dVector3 sides;
		
		// Drawing of body
		dsSetTexture(1);
		dsSetColor(2.5,2.5,2.5);
		dGeomBoxGetLengths(torso[0].geom,sides);
		dsDrawBox(dBodyGetPosition(torso[0].body),
							dBodyGetRotation(torso[0].body),sides);
		
		// Drawing oflegs
		dsSetTexture(0);
		dsSetColor(1.3,0.0,0.0);
		
		for (int i = 0; i < num_legs; i++) {
			for (int j = 0; j < num_links; j++ ) { 
				dGeomCapsuleGetParams(leg[0][i][j].geom, &r,&length);
				if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body),
																 dBodyGetRotation(leg[0][i][j].body),0.5*length,1.2*r);
				else       dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body),
																 dBodyGetRotation(leg[0][i][j].body),length,r);
			}
		}
		//		hpr[0] = 0.0f;
		//		hpr[1] = -20.0f;
		//		hpr[2] = 0.0f;  // point of view[m]
		//		//cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl;
		//		xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f;
		//		xyz[1] = dBodyGetPosition(torso[0].body)[1];
		//		xyz[2] = dBodyGetPosition(torso[0].body)[2]+0.05f;  // gaze[°] 


		hpr[0] = 1.0f;
		hpr[1] = -1.2f;
		hpr[2] = 0.5f;  // point of view[m]
		
		//		hpr[0] = dBodyGetRotation(torso[0].body)[0];
		//		hpr[1] = dBodyGetRotation(torso[0].body)[1];
		//		hpr[2] = dBodyGetRotation(torso[0].body)[2];
		//cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl;
		
		
		xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f;
		xyz[1] = dBodyGetPosition(torso[0].body)[1];
		xyz[2] = 0.1; //dBodyGetPosition(torso[0].body)[2]-0.15f;  // gaze[°] 

		//overrides to look from the other side of the robot
		//hpr[0] = -180.0f; //slides view to the right
		//xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f;

		//		hpr[1] = -30.2f; //slides down/up
//		hpr[2] = 20.5f;  // point of view[m]

		//different override for other corner
		xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f;
		xyz[1] = dBodyGetPosition(torso[0].body)[1]-.50;
		hpr[0] = -220.0f; //slides view to the right

		
		dsSetViewpoint(xyz,hpr);		
	}
#endif
}
Exemplo n.º 18
0
// copied from an OpenDE demo program
//todo:pass trimesh as argument to this function
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb, Tmesh tm)
{
    int i;

    if (!g) return;

    if (dGeomIsSpace(g))
    {
        displaySpace((dSpaceID)g);
        return;
    }
    int type = dGeomGetClass (g);
    if (type == dBoxClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);

        dVector3 sides;
        dGeomBoxGetLengths (g,sides);
        dsDrawBox (pos,R,sides);
    }
    else if (type == dSphereClass)
    {

        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
    }
    else if (type == dCapsuleClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCapsuleGetParams (g,&radius,&length);
        dsDrawCapsule (pos,R,length,radius);
    }
    else if (type == dCylinderClass)
    {

        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCylinderGetParams (g,&radius,&length);
        dsDrawCylinder (pos,R,length,radius);
    }

    else if (type == dTriMeshClass)
    {
        //dTriIndex* Indices = DISP.tmd[i].indices;

        const dReal* Pos = dGeomGetPosition(g);
        const dReal* Rot = dGeomGetRotation(g);

        for (int ii = 0; ii < (tm.indexSize/3); ii++)
        {

            const dReal v[9] = { // explicit conversion from float to dReal
                                 tm.vertices[tm.indices[ii * 3 + 0] * 3 + 0],
                                 tm.vertices[tm.indices[ii * 3 + 0] * 3 + 1],
                                 tm.vertices[tm.indices[ii * 3 + 0] * 3 + 2],
                                 tm.vertices[tm.indices[ii * 3 + 1] * 3 + 0],
                                 tm.vertices[tm.indices[ii * 3 + 1] * 3 + 1],
                                 tm.vertices[tm.indices[ii * 3 + 1] * 3 + 2],
                                 tm.vertices[tm.indices[ii * 3 + 2] * 3 + 0],
                                 tm.vertices[tm.indices[ii * 3 + 2] * 3 + 1],
                                 tm.vertices[tm.indices[ii * 3 + 2] * 3 + 2]
                               };
            dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
        }
        //std::cout<<"done once"<<std::endl;
    }
    else
        if (type == dRayClass)
        {
            dVector3 Origin, Direction;
            dGeomRayGet(g, Origin, Direction);

            dReal Length = dGeomRayGetLength(g);

            dVector3 End;
            End[0] = Origin[0] + (Direction[0] * Length);
            End[1] = Origin[1] + (Direction[1] * Length);
            End[2] = Origin[2] + (Direction[2] * Length);
            End[3] = Origin[3] + (Direction[3] * Length);
            double *ori = new double[3];
            double *end = new double[4];
            ori[0]=Origin[0];
            ori[1]=Origin[1];
            ori[2]=Origin[2];
            end[0]=End[0];
            end[1]=End[1];
            end[2]=End[2];
            end[3]=End[3];


            dsDrawLine(ori, end);
        }
        else
            show_aabb = 0;

    if (show_aabb)
    {
        // draw the bounding box for this geom
        dReal aabb[6];
        dGeomGetAABB (g,aabb);
        dVector3 bbpos;
        for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
        dVector3 bbsides;
        for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
        dMatrix3 RI;
        dRSetIdentity (RI);
        dsSetColorAlpha (1,0,0,0.5);
        dsDrawBox (bbpos,RI,bbsides);
    }
}
Exemplo n.º 19
0
void draw_all_objects (dSpaceID space)
{
  int i, j;

  draw_all_objects_called = 1;
  if (!graphical_test) return;
  int n = dSpaceGetNumGeoms (space);

  // draw all contact points
  dsSetColor (0,1,1);
  dSpaceCollide (space,0,&nearCallback);

  // draw all rays
  for (i=0; i<n; i++) {
    dGeomID g = dSpaceGetGeom (space,i);
    if (dGeomGetClass (g) == dRayClass) {
      dsSetColor (1,1,1);
      dVector3 origin,dir;
      dGeomRayGet (g,origin,dir);
      origin[2] += Z_OFFSET;
      dReal length = dGeomRayGetLength (g);
      for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j];
      dsDrawLine (origin,dir);
      dsSetColor (0,0,1);
      dsDrawSphere (origin,dGeomGetRotation(g),0.01);
    }
  }

  // draw all other objects
  for (i=0; i<n; i++) {
    dGeomID g = dSpaceGetGeom (space,i);
    dVector3 pos;
    if (dGeomGetClass (g) != dPlaneClass) {
      memcpy (pos,dGeomGetPosition(g),sizeof(pos));
      pos[2] += Z_OFFSET;
    }

    switch (dGeomGetClass (g)) {

    case dSphereClass: {
      dsSetColorAlpha (1,0,0,0.8);
      dReal radius = dGeomSphereGetRadius (g);
      dsDrawSphere (pos,dGeomGetRotation(g),radius);
      break;
    }

    case dBoxClass: {
      dsSetColorAlpha (1,1,0,0.8);
      dVector3 sides;
      dGeomBoxGetLengths (g,sides);
      dsDrawBox (pos,dGeomGetRotation(g),sides);
      break;
    }

    case dCapsuleClass: {
      dsSetColorAlpha (0,1,0,0.8);
      dReal radius,length;
      dGeomCapsuleGetParams (g,&radius,&length);
      dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
      break;
    }
    case dCylinderClass: {
      dsSetColorAlpha (0,1,0,0.8);
      dReal radius,length;
      dGeomCylinderGetParams (g,&radius,&length);
      dsDrawCylinder (pos,dGeomGetRotation(g),length,radius);
      break;
    }

    case dPlaneClass: {
      dVector4 n;
      dMatrix3 R,sides;
      dVector3 pos2;
      dGeomPlaneGetParams (g,n);
      dRFromZAxis (R,n[0],n[1],n[2]);
      for (j=0; j<3; j++) pos[j] = n[j]*n[3];
      pos[2] += Z_OFFSET;
      sides[0] = 2;
      sides[1] = 2;
      sides[2] = 0.001;
      dsSetColor (1,0,1);
      for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
      dsDrawLine (pos,pos2);
      dsSetColorAlpha (1,0,1,0.8);
      dsDrawBox (pos,R,sides);
      break;
    }

    }
  }
}
Exemplo n.º 20
0
// copied from an OpenDE demo program
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
    int i;

    if (g == nullptr) return;

    if (dGeomIsSpace(g) != 0)
    {
        displaySpace((dSpaceID)g);
        return;
    }

    int type = dGeomGetClass (g);
    if (type == dBoxClass)
    {
        if (pos == nullptr) pos = dGeomGetPosition (g);
        if (R == nullptr) R = dGeomGetRotation (g);

        dVector3 sides;
        dGeomBoxGetLengths (g,sides);
        dsDrawBox (pos,R,sides);
    }
    else if (type == dSphereClass)
    {
        if (pos == nullptr) pos = dGeomGetPosition (g);
        if (R == nullptr) R = dGeomGetRotation (g);
        dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
    }
    else if (type == dCapsuleClass)
    {
        if (pos == nullptr) pos = dGeomGetPosition (g);
        if (R == nullptr) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCapsuleGetParams (g,&radius,&length);
        dsDrawCapsule (pos,R,length,radius);
    }
    else if (type == dCylinderClass)
    {
        if (pos == nullptr) pos = dGeomGetPosition (g);
        if (R == nullptr) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCylinderGetParams (g,&radius,&length);
        dsDrawCylinder (pos,R,length,radius);
    }
    else if (type == dGeomTransformClass)
    {
        if (pos == nullptr) pos = dGeomGetPosition (g);
        if (R == nullptr) R = dGeomGetRotation (g);

        dGeomID g2 = dGeomTransformGetGeom (g);
        const dReal *pos2 = dGeomGetPosition (g2);
        const dReal *R2 = dGeomGetRotation (g2);
        dVector3 actual_pos;
        dMatrix3 actual_R;
        dMULTIPLY0_331 (actual_pos,R,pos2);
        actual_pos[0] += pos[0];
        actual_pos[1] += pos[1];
        actual_pos[2] += pos[2];
        dMULTIPLY0_333 (actual_R,R,R2);
        drawGeom (g2,actual_pos,actual_R,0);
    }
    else
        show_aabb = 0;

    if (show_aabb != 0)
    {
        // draw the bounding box for this geom
        dReal aabb[6];
        dGeomGetAABB (g,aabb);
        dVector3 bbpos;
        for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
        dVector3 bbsides;
        for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
        dMatrix3 RI;
        dRSetIdentity (RI);
        dsSetColorAlpha (1,0,0,0.5);
        dsDrawBox (bbpos,RI,bbsides);
    }
}