Ejemplo n.º 1
0
static void simLoop (int pause)
{
    const dReal stepsize = 0.02;

    dsSetColor (0,0,2);
    dSpaceCollide (space,0,&nearCallback);
    if (!pause) {
        
        if (mov_type == 1)
            moveplat_1(stepsize);
        else
            moveplat_2(stepsize);

        dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]);
        updatecam();
        dWorldQuickStep (world,stepsize);
        //dWorldStep (world,stepsize);
    }

    if (write_world) {
        FILE *f = fopen ("state.dif","wt");
        if (f) {
            dWorldExportDIF (world,f,"X");
            fclose (f);
        }
        write_world = 0;
    }
  
    // remove all contact joints
    dJointGroupEmpty (contactgroup);

    dsSetColor (1,1,0);
    dsSetTexture (DS_WOOD);
    for (int i=0; i<num; i++) {
        for (int j=0; j < GPB; j++) {
            if (! dBodyIsEnabled (obj[i].body)) {
                dsSetColor (1,0.8,0);
            }
            else {
                dsSetColor (1,1,0);
            }
            drawGeom (obj[i].geom[j],0,0,show_aabb);
        }
    }
    dsSetColor (1,0,0);
    drawGeom (platform,0,0,show_aabb);
    //usleep(5000);
}
Ejemplo n.º 2
0
void simLoop (int pause)
{
    if (!pause) {

        const dReal step = 0.02;
        const unsigned nsteps = 1;

        for (unsigned i=0; i<nsteps; ++i) {
            dSpaceCollide(space, 0, nearCallback);
            dWorldQuickStep(world, step);
            dJointGroupEmpty(contact_group);
        }
    } else {
        dSpaceCollide(space, 0, nearCallback);
        dJointGroupEmpty(contact_group);
    }
    
    // now we draw everything
    unsigned ngeoms = dSpaceGetNumGeoms(space);
    for (unsigned i=0; i<ngeoms; ++i) {
        dGeomID g = dSpaceGetGeom(space, i);

        if (g == ground)
            continue; // drawstuff is already drawing it for us

        drawGeom(g);
    }
    
    if (dBodyGetPosition(ball1_body)[0] < -track_len)
        resetSim();
}
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.05);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }
  
  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}
Ejemplo n.º 4
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.02);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }


  if (doFeedback)
  {
    if (fbnum>MAX_FEEDBACKNUM)
      printf("joint feedback buffer overflow!\n");
    else
    {
      dVector3 sum = {0, 0, 0};
      printf("\n");
      for (int i=0; i<fbnum; i++) {
        dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
        printf("%f %f %f\n", f[0], f[1], f[2]);
        sum[0] += f[0];
        sum[1] += f[1];
        sum[2] += f[2];
      }
      printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
      dMass m;
      dBodyGetMass(obj[selected].body, &m);
      printf("Object G=%f\n", GRAVITY*m.mass);
    }
    doFeedback = 0;
    fbnum = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}
Ejemplo n.º 5
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);
  }
}
Ejemplo n.º 6
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}
Ejemplo n.º 7
0
void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
{
  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 == dCCylinderClass) {
    dReal radius,length;
    dGeomCCylinderGetParams (g,&radius,&length);
    dsDrawCappedCylinder (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);
  }
}
void SceneObject::draw() {

	setupMaterials();
	glPushMatrix();
	glTranslated( pos[0], pos[1], pos[2] );
	drawGeom();
	glPopMatrix();
}
/*******************************************************************************
Function to draw all geometries associated to object's body.
*******************************************************************************/
void GOdeObject::drawGeometries() const
{
	GDrawing::setColor( R, G, B );				//Draw geometries using object's color.
	for( int I = 0; I < geometries.size(); I++ )
	{
		drawGeom( geometries[I], NULL, NULL );	//Iterate over all geometries and draw them.
	}
}
Ejemplo n.º 10
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }

  /*{
    for (int i = 1; i < IndexCount; i++) {
      dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
    }
  }*/

  {const dReal* Pos = dGeomGetPosition(TriMesh);
  const dReal* Rot = dGeomGetRotation(TriMesh);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
	const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
	const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
	dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
  }}}

  if (Ray){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Ray, Origin, Direction);
	  
	  dReal Length = dGeomRayGetLength(Ray);
	  
	  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);
	  
	  dsDrawLine(Origin, End);
  }
}
/*******************************************************************************
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.
	}
}
Ejemplo n.º 12
0
void DisplayOpenDESpaces::displaySpace(std::vector<dGeomID> g, std::vector<Tmesh> *tm)
{
    int ngeoms = g.size();

    for (int i = 0 ; i < ngeoms-1 ; ++i)
    {
        dGeomID geom = g[i];
        std::map<dGeomID, Color>::const_iterator it = m_gcolors.find(geom);
        if (it != m_gcolors.end())
            dsSetColor(it->second.r,it->second.g,it->second.b);
        else
            dsSetColor(m_activeColor.r, m_activeColor.g, m_activeColor.b);

        if(tm==NULL)
        drawGeom(geom, NULL, NULL, 0);
        else
        drawGeom(geom, NULL, NULL, 0,tm->at(i));

    }
    drawGeom(g[ngeoms-1], NULL, NULL, 0);
}
Ejemplo n.º 13
0
void DisplayOpenDESpaces::displaySpace(dSpaceID space)
{
    int ngeoms = dSpaceGetNumGeoms(space);
    for (int i = 0 ; i < ngeoms ; ++i)
    {
        dGeomID geom = dSpaceGetGeom(space, i);
        std::map<dGeomID, Color>::const_iterator it = m_gcolors.find(geom);
        if (it != m_gcolors.end())
            dsSetColor(it->second.r,it->second.g,it->second.b);
        else
            dsSetColor(m_activeColor.r, m_activeColor.g, m_activeColor.b);
        drawGeom(geom, nullptr, nullptr, 0);
    }
}
Ejemplo n.º 14
0
static void simLoop (int pause)
{
  int i;

  double simstep = 0.002; // 2ms simulation steps
  double dt = dsElapsedTime();
  int nrofsteps = (int) ceilf(dt/simstep);
  for (i=0; i<nrofsteps && !pause; i++)
  {
    dSpaceCollide (space,0,&nearCallback);
    dWorldQuickStep (world, simstep);
    dJointGroupEmpty (contactgroup);
    inspectJoints();
  }

  for (i=0; i<SEGMCNT; i++)
  {
    float r=0,g=0,b=0.2;
    float v = colours[i];
    if (v>1.0) v=1.0;
    if (v<0.5) 
    {
      r=2*v;
      g=1.0;
    }
    else
    {
      r=1.0;
      g=2*(1.0-v);
    }
    dsSetColor (r,g,b);
    drawGeom(seggeoms[i]);
  }
  dsSetColor (1,1,1);
  for (i=0; i<STACKCNT; i++)
    drawGeom(stackgeoms[i]);
}
Ejemplo n.º 15
0
void PhysicsVisualization::draw(int pause)
{
	UNUSED(pause);
	if (nullptr != g_drawingThread && nullptr != g_envToDraw)
	{
		g_envToDraw->pauseSimulation();
	    // now we draw everything
		dSpaceID collisionSpace = g_envToDraw->getCollisionSpaceID();
		dSpaceID visualsSpace = g_envToDraw->getVisualsSpaceID();

	    uint ngeoms = dSpaceGetNumGeoms(collisionSpace);
	    for (uint i = 0; i < ngeoms; ++i) {
	        dGeomID g = dSpaceGetGeom(collisionSpace, i);
	        drawGeom(g);
	    }

	    ngeoms = dSpaceGetNumGeoms(visualsSpace);
		for (uint i = 0; i < ngeoms; ++i) {
			dGeomID g = dSpaceGetGeom(visualsSpace, i);
			drawGeom(g);
		}
		g_envToDraw->unPauseSimulation();
	}
}
Ejemplo n.º 16
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    int color_changed = 0;
    if (i==selected) {
      dsSetColor (0,0.7,1);
      color_changed = 1;
    }
    else if (! dBodyIsEnabled (obj[i].body)) {
      dsSetColor (1,0,0);
      color_changed = 1;
    }
    for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
    if (color_changed) dsSetColor (1,1,0);
  }

  {for (int i = 0; i < RayCount; i++){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Rays[i], Origin, Direction);
  
	  dReal Length = dGeomRayGetLength(Rays[i]);

	  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);
  
	  dsDrawLine(Origin, End);
  }}
}
Ejemplo n.º 17
0
static void simLoop (int pause)
{
  int i,j;
  
  dsSetColor (0,0,2);
  
  dSpaceCollide (space,0,&nearCallback);
  
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldQuickStep (world,0.05);
  if (!pause) dWorldStepFast1 (world,0.05, 5);


  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);



	const dReal* pReal = dGeomGetPosition( gheight );

	const dReal* RReal = dGeomGetRotation( gheight );

	//
	// Draw Heightfield
	//

	// Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
	int ox = (int) ( -HFIELD_WIDTH/2 );
	int oz = (int) ( -HFIELD_DEPTH/2 );

//	for ( int tx = -1; tx < 2; ++tx )
//	for ( int tz = -1; tz < 2; ++tz )
	{
		dsSetColorAlpha (0.5,1,0.5,0.5);
		dsSetTexture( DS_WOOD );

		for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
		for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
		{
			dReal a[3], b[3], c[3], d[3];

			a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
			a[ 1 ] = heightfield_callback( NULL, i, j );
			a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;

			b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
			b[ 1 ] = heightfield_callback( NULL, i + 1, j );
			b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;

			c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
			c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
			c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;

			d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
			d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
			d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;

			dsDrawTriangle( pReal, RReal, a, c, b, 1 );
			dsDrawTriangle( pReal, RReal, b, c, d, 1 );
		}
	}





	dsSetColor (1,1,0);
	dsSetTexture (DS_WOOD);
	for (i=0; i<num; i++)
	{
		for (j=0; j < GPB; j++)
		{
			if (i==selected)
			{
				dsSetColor (0,0.7,1);
			}
			else if (! dBodyIsEnabled (obj[i].body))
			{
				dsSetColor (1,0.8,0);
			}
			else 
			{
				dsSetColor (1,1,0);
			}

			if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
			{
				dTriIndex* Indices = (dTriIndex*)::Indices;

				// assume all trimeshes are drawn as bunnies
				const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
				const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);

				for (int ii = 0; ii < IndexCount / 3; ii++)
				{
					const dReal v[9] = { // explicit conversion from float to dReal
						Vertices[Indices[ii * 3 + 0] * 3 + 0],
							Vertices[Indices[ii * 3 + 0] * 3 + 1],
							Vertices[Indices[ii * 3 + 0] * 3 + 2],
							Vertices[Indices[ii * 3 + 1] * 3 + 0],
							Vertices[Indices[ii * 3 + 1] * 3 + 1],
							Vertices[Indices[ii * 3 + 1] * 3 + 2],
							Vertices[Indices[ii * 3 + 2] * 3 + 0],
							Vertices[Indices[ii * 3 + 2] * 3 + 1],
							Vertices[Indices[ii * 3 + 2] * 3 + 2]
					};
					dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
				}

				// tell the tri-tri collider the current transform of the trimesh --
				// this is fairly important for good results.

				// Fill in the (4x4) matrix.
				dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );

				p_matrix[ 0 ] = Rot[ 0 ];	p_matrix[ 1 ] = Rot[ 1 ];	p_matrix[ 2 ] = Rot[ 2 ];	p_matrix[ 3 ] = 0;
				p_matrix[ 4 ] = Rot[ 4 ];	p_matrix[ 5 ] = Rot[ 5 ];	p_matrix[ 6 ] = Rot[ 6 ];	p_matrix[ 7 ] = 0;
				p_matrix[ 8 ] = Rot[ 8 ];	p_matrix[ 9 ] = Rot[ 9 ];	p_matrix[10 ] = Rot[10 ];	p_matrix[11 ] = 0;
				p_matrix[12 ] = Pos[ 0 ];	p_matrix[13 ] = Pos[ 1 ];	p_matrix[14 ] = Pos[ 2 ];	p_matrix[15 ] = 1;

				// Flip to other matrix.
				obj[i].last_matrix_index = !obj[i].last_matrix_index;

				// Apply the 'other' matrix which is the oldest.
				dGeomTriMeshSetLastTransform( obj[i].geom[j], 
					*(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
			}
			else
			{
				drawGeom (obj[i].geom[j],0,0,show_aabb);
			}
		}
	}

	if ( show_aabb )
	{
		// draw the bounding box for this geom
		dReal aabb[6];
		dGeomGetAABB (gheight,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);
	}
}
Ejemplo n.º 18
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);
  }
}
Ejemplo n.º 19
0
void drawContext::drawView()
{
  OrthofFromGModel();

  glMatrixMode(GL_MODELVIEW);
  // fill the background
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  if(CTX::instance()->bgGradient){
    glPushMatrix();
    glLoadIdentity();
    const GLfloat squareVertices[] = {
      (GLfloat)_top,	(GLfloat)_left, 2*_far,
      (GLfloat)_top,	(GLfloat)_right, 2*_far,
      (GLfloat)_bottom,	(GLfloat)_left, 2*_far,
      (GLfloat)_bottom,	(GLfloat)_right, 2*_far,
    };
    const GLubyte squareColors[] = {
      255, 255, 255, 255,
      255, 255, 255, 255,
      190, 200, 255, 255,
      190, 200, 255, 255,
    };
    glVertexPointer(3, GL_FLOAT, 0, squareVertices);
    glEnableClientState(GL_VERTEX_ARRAY);
    glColorPointer(4, GL_UNSIGNED_BYTE, 0, squareColors);
    glEnableClientState(GL_COLOR_ARRAY);
    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
    glDisableClientState(GL_COLOR_ARRAY);
    glDisableClientState(GL_VERTEX_ARRAY);
    glPopMatrix();
  }
  checkGlError("Draw background");

  glLoadIdentity();
  glScalef(_scale[0], _scale[1], _scale[2]);
  glTranslatef(_translate[0], _translate[1], _translate[2]);

  if(CTX::instance()->rotationCenterCg)
    glTranslatef(CTX::instance()->cg[0],
                 CTX::instance()->cg[1],
                 CTX::instance()->cg[2]);
  else
    glTranslatef(CTX::instance()->rotationCenter[0],
                 CTX::instance()->rotationCenter[1],
                 CTX::instance()->rotationCenter[2]);

  buildRotationMatrix();
  glMultMatrixf(_rotatef);

  if(CTX::instance()->rotationCenterCg)
    glTranslatef(-CTX::instance()->cg[0],
                 -CTX::instance()->cg[1],
                 -CTX::instance()->cg[2]);
  else
    glTranslatef(-CTX::instance()->rotationCenter[0],
                 -CTX::instance()->rotationCenter[1],
                 -CTX::instance()->rotationCenter[2]);

  checkGlError("Initialize position");

  glEnable(GL_DEPTH_TEST);

  drawMesh();
  checkGlError("Draw mesh");
  drawGeom();
  checkGlError("Draw geometry");
  drawPost();
  checkGlError("Draw post-pro");
  glDisable(GL_DEPTH_TEST);
  drawScale();
  checkGlError("Draw scales");
  drawAxes();
  checkGlError("Draw axes");
}
static void simLoop (int pause)


{


	dsSetColor (0,0,2);


	dSpaceCollide (space,0,&nearCallback);


	if (!pause) dWorldStep (world,0.05);





	dAASSERT(terrainY);


	dAASSERT(terrainZ);


	dsSetColor (0,1,0);


	dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));


	dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));





	if (show_aabb) 


	{


		dReal aabb[6];


		dGeomGetAABB (terrainY,aabb);


		dVector3 bbpos;


		int i;


		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);





		dGeomGetAABB (terrainZ,aabb);


		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


		dsDrawBox (bbpos,RI,bbsides);


	}





	dsSetColor (1,1,0);


	


	// remove all contact joints


	dJointGroupEmpty (contactgroup);


	


	dsSetColor (1,1,0);


	dsSetTexture (DS_WOOD);


	for (int i=0; i<num; i++) {


		for (int j=0; j < GPB; j++) {


			if (i==selected) {


				dsSetColor (0,0.7,1);


			}


			else if (! dBodyIsEnabled (obj[i].body)) {


				dsSetColor (1,0,0);


			}


			else {


				dsSetColor (1,1,0);


			}


			drawGeom (obj[i].geom[j],0,0,show_aabb);


		}


	}


}
Ejemplo n.º 21
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);
    }
}
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);


#if 1
  // What is this for??? - Bram
  if (!pause) 
  {
    for (int i=0; i<num; i++)
      for (int j=0; j < GPB; j++)
        if (obj[i].geom[j])
          if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
            setCurrentTransform(obj[i].geom[j]);
 
    setCurrentTransform(TriMesh1);
    setCurrentTransform(TriMesh2);
  }
#endif

  //if (!pause) dWorldStep (world,0.05);
  if (!pause) dWorldQuickStep (world,0.05);

  for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
	  dSpaceGetGeom(space, j);
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (obj[i].geom[j]) {
        if (i==selected) {
          dsSetColor (0,0.7,1);
        }
        else if (! dBodyIsEnabled (obj[i].body)) {
          dsSetColor (1,0,0);
        }
        else {
          dsSetColor (1,1,0);
        }
      
        if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
          dTriIndex* Indices = (dTriIndex*)::Indices;

          // assume all trimeshes are drawn as bunnies
          const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
          const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
        
          for (int ii = 0; ii < IndexCount / 3; ii++) {
            const dReal v[9] = { // explicit conversion from float to dReal
              Vertices[Indices[ii * 3 + 0] * 3 + 0],
              Vertices[Indices[ii * 3 + 0] * 3 + 1],
              Vertices[Indices[ii * 3 + 0] * 3 + 2],
              Vertices[Indices[ii * 3 + 1] * 3 + 0],
              Vertices[Indices[ii * 3 + 1] * 3 + 1],
              Vertices[Indices[ii * 3 + 1] * 3 + 2],
              Vertices[Indices[ii * 3 + 2] * 3 + 0],
              Vertices[Indices[ii * 3 + 2] * 3 + 1],
              Vertices[Indices[ii * 3 + 2] * 3 + 2]
            };
            dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
          }

          // tell the tri-tri collider the current transform of the trimesh --
          // this is fairly important for good results.
          
		  // Fill in the (4x4) matrix.
		  dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );

		  p_matrix[ 0 ] = Rot[ 0 ];	p_matrix[ 1 ] = Rot[ 1 ];	p_matrix[ 2 ] = Rot[ 2 ];	p_matrix[ 3 ] = 0;
		  p_matrix[ 4 ] = Rot[ 4 ];	p_matrix[ 5 ] = Rot[ 5 ];	p_matrix[ 6 ] = Rot[ 6 ];	p_matrix[ 7 ] = 0;
		  p_matrix[ 8 ] = Rot[ 8 ];	p_matrix[ 9 ] = Rot[ 9 ];	p_matrix[10 ] = Rot[10 ];	p_matrix[11 ] = 0;
		  p_matrix[12 ] = Pos[ 0 ];	p_matrix[13 ] = Pos[ 1 ];	p_matrix[14 ] = Pos[ 2 ];	p_matrix[15 ] = 1;

		  // Flip to other matrix.
		  obj[i].last_matrix_index = !obj[i].last_matrix_index;

		  dGeomTriMeshSetLastTransform( obj[i].geom[j], 
			  *(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
  
        } else {
          drawGeom (obj[i].geom[j],0,0,show_aabb);
        }
      }
    }
  }

  dTriIndex* Indices = (dTriIndex*)::Indices;

  {const dReal* Pos = dGeomGetPosition(TriMesh1);
  const dReal* Rot = dGeomGetRotation(TriMesh1);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dReal v[9] = { // explicit conversion from float to dReal
      Vertices[Indices[i * 3 + 0] * 3 + 0],
      Vertices[Indices[i * 3 + 0] * 3 + 1],
      Vertices[Indices[i * 3 + 0] * 3 + 2],
      Vertices[Indices[i * 3 + 1] * 3 + 0],
      Vertices[Indices[i * 3 + 1] * 3 + 1],
      Vertices[Indices[i * 3 + 1] * 3 + 2],
      Vertices[Indices[i * 3 + 2] * 3 + 0],
      Vertices[Indices[i * 3 + 2] * 3 + 1],
      Vertices[Indices[i * 3 + 2] * 3 + 2]
    };
    dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
  }}}

  {const dReal* Pos = dGeomGetPosition(TriMesh2);
  const dReal* Rot = dGeomGetRotation(TriMesh2);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dReal v[9] = { // explicit conversion from float to dReal
      Vertices[Indices[i * 3 + 0] * 3 + 0],
      Vertices[Indices[i * 3 + 0] * 3 + 1],
      Vertices[Indices[i * 3 + 0] * 3 + 2],
      Vertices[Indices[i * 3 + 1] * 3 + 0],
      Vertices[Indices[i * 3 + 1] * 3 + 1],
      Vertices[Indices[i * 3 + 1] * 3 + 2],
      Vertices[Indices[i * 3 + 2] * 3 + 0],
      Vertices[Indices[i * 3 + 2] * 3 + 1],
      Vertices[Indices[i * 3 + 2] * 3 + 2]
    };
    dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
  }}}
}
Ejemplo n.º 23
0
/*******************************************************************************
Function that gets called by the event handler to draw the scene.
*******************************************************************************/
void display(void)
{
	//glClearColor (red, green, blue, alpha)
	glClearColor(1.0f, 1.0f, 1.0f, 1.0f);		//Set the background color.
	
	//OK, now clear the screen with the background color.
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	//Load initial matrix transformation.
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	//Locate the camera.
	gluLookAt (g_eye[X], g_eye[Y], g_eye[Z], g_ref[X], g_ref[Y], g_ref[Z], 0.0, 1.0, 0.0);

	HMatrix arcball_rot;
	Ball_Value(g_arcBall,arcball_rot);
	glMultMatrixf((float *)arcball_rot);

	//Scale the scene in response to mouse commands.
	glScalef(g_zoom, g_zoom, g_zoom); 

	////////////////////// Draw the geometries in World ////////////////////////

	//drawGeom( Object.Geom );
	//drawGeom( Rod.Geom );
	drawGeom(body.Geom);
	drawGeom(middleRightLeg.Geom);
	drawGeom(middleLeftLeg.Geom);
	drawGeom(middleLeftOuterLeg.Geom);
	drawGeom(middleRightOuterLeg.Geom);
	drawGeom(brLeg.Geom);
	drawGeom(backRightOuterLeg.Geom);
	drawGeom(blLeg.Geom);
	drawGeom(backLeftOuterLeg.Geom);
	drawGeom(frLeg.Geom);
	drawGeom(frontRightOuterLeg.Geom);
	drawGeom(flLeg.Geom);
	drawGeom(frontLeftOuterLeg.Geom);

	for (auto i = 0; i < animator.foodParticles.size(); ++i)
	{
		drawGeom(animator.foodParticles.at(i).odeObject.Geom, animator.foodParticles.at(i).colored );

	}

	drawGeom(target.Geom);

	glPushMatrix();						//Draw the collision plane.
	glTranslated( 0.0, -0.05, 0.0 );
	glScaled( 140.0, 0.1, 140.0 );
	GDrawing::setColor( 0.4, 0.4, 1.0 );
	GDrawing::drawCube();
	glPopMatrix();

	////////////////////////////////////////////////////////////////////////////

	glutSwapBuffers();
	if( g_recording == 1)
		g_frameSaver.DumpPPM(g_width,g_height);
}