Exemplo n.º 1
0
void BodyVerifier::collisionCallback(void *data, dGeomID o1, dGeomID o2) {

	CollisionData *collisionData = (CollisionData *) data;
	const int MAX_CONTACTS = 32; // maximum number of contact points per body
	// TODO will it work with just 1 point? Probably yes.

	// exit without doing anything if the two bodies are connected by a joint
	dBodyID b1 = dGeomGetBody(o1);
	dBodyID b2 = dGeomGetBody(o2);

	if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) {
		return;
	}

	dContact contact[MAX_CONTACTS];
	for (int i = 0; i < MAX_CONTACTS; i++) {
		contact[i].surface.mode = 0;
	}


	if (dGeomGetClass(o1) == dCylinderClass &&
			dGeomGetClass(o2) == dCylinderClass) {
		collisionData->cylinders.push_back(o1);
		collisionData->cylinders.push_back(o2);
	}
	int collisionCounts = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom,
				sizeof(dContact));

	if (collisionCounts != 0) {
		collisionData->offendingBodies.push_back(std::pair<dBodyID,
				dBodyID>(b1, b2));
	}
}
Exemplo n.º 2
0
void CProtoHapticDoc::nearCallback(dGeomID o1, dGeomID o2)
{
	int index1= (int)dGeomGetData(o1);
	int index2= (int)dGeomGetData(o2);

	if(m_shapes[index1]->isCollisionDynamic() || 
	   m_shapes[index2]->isCollisionDynamic()) {

		int n, i;
		const int N = 50;
		dContact contact[N];
		n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
		if (n > 0) {
			OutputDebugString("Collision\n");
			for (i=0; i<n; i++) {
				contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
				if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
					contact[i].surface.mu = 20;
				else
					contact[i].surface.mu = 0.5;
				contact[i].surface.slip1 = 1.0 - (m_shapes[index1]->getSurfaceFriction());
				contact[i].surface.slip2 = 1.0 - (m_shapes[index2]->getSurfaceFriction());
				contact[i].surface.soft_erp = 0.8;
				contact[i].surface.soft_cfm = 0.01;
				dJointID c = dJointCreateContact (m_worldID,m_jointGroup,contact+i);
				dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
			}
		}
	}

}
Exemplo n.º 3
0
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
	int i,n;
	
	dBodyID b1 = dGeomGetBody(o1);
	dBodyID b2 = dGeomGetBody(o2);
	if (b1 && b2 && dAreConnected(b1, b2))
		return;
	
	const int N = 4;
	dContact contact[N];
	n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
	if (n > 0) {
		for (i=0; i<n; i++) {
			contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
			if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
				contact[i].surface.mu = 20;
			else
				contact[i].surface.mu = 0.5;
			contact[i].surface.slip1 = 0.0;
			contact[i].surface.slip2 = 0.0;
			contact[i].surface.soft_erp = 0.8;
			contact[i].surface.soft_cfm = 0.01;
			dJointID c = dJointCreateContact (world,contactgroup,contact+i);
			dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
		}
	}
}
Exemplo n.º 4
0
void draw_phys_geom(dGeomID geom)
{
    glPushAttrib(GL_ENABLE_BIT);
    {
        glDisable(GL_LIGHTING);
        glDisable(GL_TEXTURE_2D);

        glEnable(GL_COLOR_MATERIAL);
        glColor4f(1.0f, 1.0f, 0.0f, 0.5f);

        if (dGeomGetClass(geom) == dPlaneClass)
            draw_phys_plane(geom);
        else
        {
            switch (dGeomGetClass(geom))
            {
                case dBoxClass:     draw_phys_box      (geom); break;
                case dSphereClass:  draw_phys_sphere   (geom); break;
                case dCapsuleClass: draw_phys_ccylinder(geom); break;
                case dRayClass:     draw_phys_ray      (geom); break;
            }
        }
    }
    glPopAttrib();
}
Exemplo n.º 5
0
//Verena ?
void Test_ODEBodies::testODEBody() {
	Core::resetCore();
	ODE_SimulationAlgorithm *algorithm = new ODE_SimulationAlgorithm();
	Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(algorithm);
	ODE_BodyAdapter *body = new ODE_BodyAdapter("Body1");
	algorithm->resetPhysics();
	
	QVERIFY(body->getRigidBodyID() == 0);
	body->setup();
	QVERIFY(body->getRigidBodyID() != 0);
	QCOMPARE(body->getCollisionObjects().size(), 0);
	body->addCollisionObject(new CollisionObject(BoxGeom(body, 1, 1, 1), body));
	QCOMPARE(body->getCollisionObjects().size(), 1);
	// Test create Collision Objects: Creates a dGeomID if a valid Geom exists
	QVERIFY(body->createODECollisionObjects());
	CollisionObject *boxCollisionObject = body->getCollisionObjects().at(0);
	QVERIFY(boxCollisionObject->getNativeCollisionObject() != 0);
	// A valid dGeomID was created and added to the collision object as native collision object.
	QVERIFY(dGeomGetBody(static_cast<dGeomID>(boxCollisionObject->getNativeCollisionObject())) == body->getRigidBodyID());
	// create fails, if an invalid SimulationAlgorithm is used
	Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(0);
	QVERIFY(!body->createODECollisionObjects());
	Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(algorithm);
	QVERIFY(body->createODECollisionObjects());
	body->clear();
	// create fails, if no dRigidBody exists and the object is dynamic.
	QVERIFY(body->getRigidBodyID() == 0);
	QVERIFY(!body->createODECollisionObjects());
	// create succeeds, if the object is static.
	body->getParameter("Dynamic")->setValueFromString("False");
	QVERIFY(body->createODECollisionObjects());
	QVERIFY(body->getCollisionObjects().at(0)->getNativeCollisionObject() != 0);
	QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 1);
	body->removeCollisionObject(boxCollisionObject);
	// Check whether the ode-type of the created objects is correct.
	body->addCollisionObject(new CollisionObject(SphereGeom(body, 1), body));
	QVERIFY(body->createODECollisionObjects());
	CollisionObject *sphereCollisionObject = body->getCollisionObjects().at(0);
	QVERIFY(sphereCollisionObject->getNativeCollisionObject() != 0);
	QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 0);
	body->removeCollisionObject(sphereCollisionObject);
	body->addCollisionObject(new CollisionObject(CylinderGeom(body, 1, 2), body));
	QVERIFY(body->createODECollisionObjects());
	CollisionObject *cylinderCollisionObject = body->getCollisionObjects().at(0);
	QVERIFY(cylinderCollisionObject ->getNativeCollisionObject() != 0);
	QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 3);
	body->removeCollisionObject(cylinderCollisionObject);
	body->addCollisionObject(new CollisionObject(TriangleGeom(body), body));
	QVERIFY(body->createODECollisionObjects());
	CollisionObject *trimeshCollisionObject = body->getCollisionObjects().at(0);
	QVERIFY(trimeshCollisionObject->getNativeCollisionObject() != 0);
	QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 8);

}
Exemplo n.º 6
0
void TurbulentDispersionScene::Collide(dGeomID g1, dGeomID g2)
{
    dBodyID b1 = dGeomGetBody(g1);
    dBodyID b2 = dGeomGetBody(g2);
    
    dContact contact[MAX_CONTACTS];
    int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
     
    if(n>0)
    {
        //Particle g1 collides with ground g2
        if ((dGeomGetClass(g1) == dSphereClass)&&(g2==ground_wall))
        {                                                    
            unsigned int index=(size_t) dGeomGetData(g1);
            //bodies_prts_indexes_to_remove.push_back(FindPrtsIndexByGlobalId(index)); 
            Add_prts_body_index_to_remove(FindPrtsIndexByGlobalId(index));
            
            return;
        }

        //Particle g2 collides with ground g1
        if((dGeomGetClass(g2) == dSphereClass)&&(g1==ground_wall))
        {            
            unsigned int index=(size_t) dGeomGetData(g2);
            //bodies_prts_indexes_to_remove.push_back(FindPrtsIndexByGlobalId(index));                        
            Add_prts_body_index_to_remove(FindPrtsIndexByGlobalId(index));
            
            return;
        }
    }
    
    for (int i=0; i<n; ++i)
    {
        // contact[i].surface.mode = dContactBounce | dContactSoftCFM;
        contact[i].surface.mode = dContactBounce;        
        //contact[i].surface.mu = dInfinity;
        contact[i].surface.mu = 0.5;
        //contact[i].surface.mu2 = 0.5;
        contact[i].surface.bounce = 0.000999990;
        //contact[i].surface.bounce_vel = 0.1;
        //contact[i].surface.soft_cfm = 0.001;

        //contact[i].surface.mode = dContactBounce|dContactSoftERP|dContactSoftCFM;
        contact[i].surface.soft_erp = 1.0; //1.0;
        contact[i].surface.soft_cfm = 1e-10;
        dJointID j = dJointCreateContact (domain->getWorld(), domain->getContactGroup(), contact+i);
        dJointAttach(j, b1, b2);
    }
}
Exemplo n.º 7
0
void DrawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
    // If the geom ID is missing then return immediately.
    if (!g)
    {
        return;
    }

    // If there was no position vector supplied then get the existing position.
    if (!pos)
    {
        pos = dGeomGetPosition (g);
    }

    // If there was no rotation matrix given then get the existing rotation.
    if (!R)
    {
        R = dGeomGetRotation (g);
    }

    // Get the geom's class type.
    int type = dGeomGetClass (g);

    if (type == dBoxClass)
    {
        // Create a temporary array of floats to hold the box dimensions.
        dReal sides[3];
        dGeomBoxGetLengths(g, sides);

        // Now to actually render the box we make a call to DrawBox, passing the geoms dimensions, position vector and
        // rotation matrix. And this function is the subject of our next discussion.
       // DrawBox(sides, pos, R);
    }
}
Exemplo n.º 8
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);
  }
}
Exemplo n.º 9
0
bool PObject::isPlaceable()
{
    int c = dGeomGetClass(geom);
    if (c == dPlaneClass)
        return false;
    return true;
}
Exemplo n.º 10
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.º 11
0
void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i,j,n;
  const int N = 100;
  dContactGeom contact[N];

  if (dGeomGetClass (o2) == dRayClass) {
    n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom));
  }
  else {
    n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom));
  }
  if (n > 0) {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.01,0.01,0.01};
    for (i=0; i<n; i++) {
      contact[i].pos[2] += Z_OFFSET;
      dsDrawBox (contact[i].pos,RI,ss);
      dVector3 n;
      for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j];
      dsDrawLine (contact[i].pos,n);
    }
  }
}
Exemplo n.º 12
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.º 13
0
//Auxiliary function to print info on a space
void OdeInit::printInfoOnSpace(dSpaceID my_space,const std::string & my_space_name)
{
   int num_geoms = 0;
   dGeomID geom_temp;
   int geom_class_temp = 0;
   dReal aabb[6];
   dBodyID body_temp;
   
   num_geoms = dSpaceGetNumGeoms(my_space);
   printf("\nSpace: %s: ID: %p. sublevel: %d, nr. geoms: %d. \n",my_space_name.c_str(),my_space,dSpaceGetSublevel(my_space),num_geoms);
   for (int i=0;i<=(num_geoms-1);i++){
      geom_temp = dSpaceGetGeom (my_space, i);
      geom_class_temp = dGeomGetClass(geom_temp);
      printGeomClassAndNr(geom_class_temp,i);
      if (!dGeomIsSpace(geom_temp)){
        if (dGeomGetBody(geom_temp)!=NULL){ // i.e. a placeable geom
            printf("            ID: %p, Coordinates:",geom_temp); 
            ICubSim::printPositionOfGeom(geom_temp);
            dGeomGetAABB(geom_temp,aabb);
            printf("            Bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]); 
            if (geom_class_temp==8){ // trimesh
               body_temp = dGeomGetBody(geom_temp);
               printf("          Coordinates of associated body are:"); 
               ICubSim::printPositionOfBody(body_temp);
            } 
        } else {
            dGeomGetAABB(geom_temp,aabb);
            printf("            ID: %p; bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",geom_temp,aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]); 
        }
      }
   }
}
Exemplo n.º 14
0
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.1;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
			   sizeof(dContact))) {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
		if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
			dMatrix3 Rotation;
			dRSetIdentity(Rotation);
			dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
			
			dVector3 End;
			End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth);
			End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth);
			End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth);
			End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth);
			
			dsDrawLine(contact[i].geom.pos, End);
			continue;
		}
		
      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
      if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
    }
  }
}
Exemplo n.º 15
0
void TSRODERigidBody::DebugRender()
{
    Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
    TSRMatrix4 bodyTransform;
    TSRMatrix4 geomTransform;
    const float* pBodyPosition = dBodyGetPosition( m_BodyID );
    const float* pBodyRotation = dBodyGetRotation( m_BodyID );
    ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
    TSRGlobalConstants.PushMatrix();
    TSRGlobalConstants.MultMatrix( bodyTransform.d );
    TSRDebugDraw::RenderAxis( 1.0f );
    TSRDebugDraw::RenderSphere( 0.25f );

    for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
    {
        dGeomID currGeomTransformID = m_GeomIDs[ i ];
        dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );

        const float* pGeomPosition = dGeomGetPosition( geomID );
        const float* pGeomRotation = dGeomGetRotation( geomID );
        ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
        TSRGlobalConstants.PushMatrix();
        TSRGlobalConstants.MultMatrix( bodyTransform.d );
        switch( dGeomGetClass( geomID ) )
        {
        case dBoxClass:
            {
                dVector3 extents;
                dGeomBoxGetLengths( geomID, extents );
                TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
                TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
                aabbMin *= 0.5f;
                aabbMax *= 0.5f;
                TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
            }
            break;

        case dSphereClass:
            {
                float radius;
                radius = dGeomSphereGetRadius( geomID );
                TSRDebugDraw::RenderSphere( radius );
            }
            break;

        case dCylinderClass:
            {
                float radius,length;
                dGeomCylinderGetParams( geomID, &radius, &length );
                TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
            }
            break;
        }
		TSRGlobalConstants.PopMatrix();
    }
    TSRGlobalConstants.PopMatrix();
    Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
Exemplo n.º 16
0
void GetSphereExtensions(dGeomID sphere,const dReal* axis,
						 const dReal	*pos,
						 float center_prg,dReal* lo_ext,dReal* hi_ext)
{
	R_ASSERT2(dGeomGetClass(sphere)==dSphereClass,"is not a sphere");
	dReal radius=dGeomSphereGetRadius(sphere);
	dReal dif=dDOT(pos,axis)-center_prg;
	*lo_ext=-radius+dif;
	*hi_ext=radius+dif;
}
/*******************************************************************************
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.º 18
0
void TransformedGeometryExtensionLocalParams(dGeomID geom_transform,const dReal* axis,float center_prg,dReal* local_axis,dReal& local_center_prg)
{
	R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
	const dReal* rot=dGeomGetRotation(geom_transform);
	const dReal* pos=dGeomGetPosition(geom_transform);
	dVector3	local_pos;

	dMULTIPLY1_331(local_axis,rot,axis);
	dMULTIPLY1_331(local_pos,rot,pos);
	local_center_prg=center_prg-dDOT(local_pos,local_axis);
}
Exemplo n.º 19
0
Particle::Particle(dBodyID &b, dGeomID &g):ODE_Particle(b,g)
{    
    shape_type=dGeomGetClass(g);
    enabled=true;
    
    position[0]=0;position[1]=0;position[2]=0;
    velocity[0]=0;velocity[1]=0;velocity[2]=0;
    ang_velocity[0]=0;ang_velocity[1]=0;ang_velocity[2]=0;
    
    //printf("Particle constructor!\n");
    //fflush(stdout);
}
Exemplo n.º 20
0
static void computeFinalTx(dGeomID geom_transform,dReal* final_pos,dReal* final_R)
{
	R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
	dGeomID obj=dGeomTransformGetGeom(geom_transform);
	const	dReal	*R	=dGeomGetRotation(geom_transform);
	const	dReal	*pos=dGeomGetPosition(geom_transform);
	dMULTIPLY0_331 (final_pos,R,dGeomGetPosition(obj));
	final_pos[0] += pos[0];
	final_pos[1] += pos[1];
	final_pos[2] += pos[2];
	dMULTIPLY0_333 (final_R,R,dGeomGetRotation(obj));
}
Exemplo n.º 21
0
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return;

  dContact contact[32];			// up to 3 contacts per box
  for (i=0; i<32; i++) {
    contact[i].surface.mode = dContactBounce; //dContactMu2;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.5;
    contact[i].surface.bounce_vel = 0.1;
  }
  if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
     dMatrix3 RI;
     dRSetIdentity (RI);
     const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
		if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
			dMatrix3 Rotation;
			dRSetIdentity(Rotation);
			dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
			continue;
		}

      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
       //dsDrawBox (contact[i].geom.pos,RI,ss);

	  
    }
  }
}
Exemplo n.º 22
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.º 23
0
void GetBoxExtensions(dGeomID box,const dReal* axis,
					  const dReal	*pos,	const dReal	*rot,
					  float center_prg,dReal* lo_ext,dReal* hi_ext)
{
	R_ASSERT2(dGeomGetClass(box)==dBoxClass,"is not a box");
	dVector3 length;
	dGeomBoxGetLengths(box,length);
	dReal dif=dDOT(pos,axis)-center_prg;
	dReal ful_ext=dFabs(dDOT14(axis,rot+0))*length[0]
	+dFabs(dDOT14(axis,rot+1))*length[1]
	+dFabs(dDOT14(axis,rot+2))*length[2];
	ful_ext/=2.f;
	*lo_ext=-ful_ext+dif;
	*hi_ext=ful_ext+dif;
}
Exemplo n.º 24
0
void drawGeom(dGeomID g)
{
    int gclass = dGeomGetClass(g);
    const dReal *pos = dGeomGetPosition(g);
    const dReal *rot = dGeomGetRotation(g);

    switch (gclass) {
        case dSphereClass:
            dsSetColorAlpha(0, 0.75, 0.5, 0.5);
            dsSetTexture (DS_CHECKERED);
            dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
            break;
        case dBoxClass:
        {
            dVector3 lengths;
            dsSetColorAlpha(1, 1, 0, 0.5);
            dsSetTexture (DS_WOOD);
            dGeomBoxGetLengths(g, lengths);
            dsDrawBox(pos, rot, lengths);
            break;
        }
        case dTriMeshClass: 
        {
            int numi = dGeomTriMeshGetTriangleCount(g);

            for (int i=0; i<numi; ++i) {
                dVector3 v0, v1, v2;
                dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2);

                dsSetTexture (DS_WOOD);

                dsSetDrawMode(DS_WIREFRAME);
                dsSetColorAlpha(0, 0, 0, 1.0);
                dsDrawTriangle(pos, rot, v0, v1, v2, true);

                dsSetDrawMode(DS_POLYFILL);
                dsSetColorAlpha(1, 1, 0, 0.5);
                dsDrawTriangle(pos, rot, v0, v1, v2, true);
            }
            break;
        }
        
        default:
        {}
    }
}
Exemplo n.º 25
0
void GetCylinderExtensions(dGeomID cyl,const dReal* axis,
						   const dReal	*pos,	const dReal	*rot,
						   float center_prg,dReal* lo_ext,dReal* hi_ext)
{
	R_ASSERT2(dGeomGetClass(cyl)==dCylinderClassUser,"is not a cylinder");
	dReal radius,length;
	dGeomCylinderGetParams(cyl,&radius,&length);
	dReal dif=dDOT(pos,axis)-center_prg;
	dReal _cos=dFabs(dDOT14(axis,rot+1));
	dReal cos1=dDOT14(axis,rot+0);
	dReal cos3=dDOT14(axis,rot+2);
	dReal _sin=_sqrt(cos1*cos1+cos3*cos3);
	length/=2.f;
	dReal ful_ext=_cos*length+_sin*radius;
	*lo_ext=-ful_ext+dif;
	*hi_ext=ful_ext+dif;
}
Exemplo n.º 26
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");
	}
}
Exemplo n.º 27
0
void drawGeom (dGeomID g)
{
  const dReal *pos = dGeomGetPosition(g);
  const dReal *R   = dGeomGetRotation(g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass)
  {
    dVector3 sides;
    dGeomBoxGetLengths (g, sides);
    dsDrawBox (pos,R,sides);
  }
  if (type == dCylinderClass)
  {
    dReal r,l;
    dGeomCylinderGetParams(g, &r, &l);
    dsDrawCylinder (pos, R, l, r);
  }
}
Exemplo n.º 28
0
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
CollisionShape::CollisionShapeType
CollisionShape::getType()
{
    switch(dGeomGetClass(m_geometryId))
    {
    case dSphereClass:
        return SPHERE_SHAPE;
    case dBoxClass:
        return BOX_SHAPE;
    case dCapsuleClass:
        return CAPSULE_SHAPE;
    case dCylinderClass:
        return CYLINDER_SHAPE;
    case dHeightfieldClass:
        return HEIGHTFIELD_SHAPE;
    case dPlaneClass:
    case dRayClass:
    case dConvexClass:
    case dGeomTransformClass:
    case dTriMeshClass:
    default:
        return NULL_SHAPE;
    };
}
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);
  }}}
}
Exemplo n.º 30
0
static void callback(void *data, dGeomID o1, dGeomID o2)
{
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);

    if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
        return;

    /* Two geoms associated with the same body do not collide. */

    if (b1 == 0 || b2 == 0 || b1 != b2)
    {
        dContact contact[MAX_CONTACTS];
        size_t sz = sizeof (dContact);
        int i, n;

        /* Find and enumerate all collision points. */

        if ((n = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sz)))
        {
            struct geom_data *d1 = get_data(o1);
            struct geom_data *d2 = get_data(o2);

            /* Compute collision parameters from geom parameters. */

            dReal bounce    = MAX(d1->bounce,   d2->bounce);
            dReal friction  = MIN(d1->friction, d2->friction);
            dReal soft_erp  = MIN(d1->soft_erp, d2->soft_erp);
            dReal soft_cfm  = MAX(d1->soft_cfm, d2->soft_cfm);

            unsigned long category1 = dGeomGetCategoryBits(o1);
            unsigned long category2 = dGeomGetCategoryBits(o2);

            if (b1) dBodyEnable(b1);
            if (b2) dBodyEnable(b2);

            /* Create a contact joint for each collision, if requsted. */

            if ((category1 & d2->response) ||
                (category2 & d1->response))
                for (i = 0; i < n; ++i)
                    if (dGeomGetClass(contact[0].geom.g1) != dRayClass &&
                        dGeomGetClass(contact[0].geom.g2) != dRayClass)
                    {
                        dJointID c;

                        contact[i].surface.mode = dContactBounce
                                                | dContactSoftCFM
                                                | dContactSoftERP;

                        contact[i].surface.mu         = friction;
                        contact[i].surface.bounce     = bounce;
                        contact[i].surface.soft_cfm   = soft_cfm;
                        contact[i].surface.soft_erp   = soft_erp;
                        contact[i].surface.bounce_vel = (dReal) 0.10;

                        c = dJointCreateContact(world, group, contact + i);
                        dJointAttach(c, b1, b2);
                    }

            /* Report contacts to the Lua script, if requested. */

            if ((category1 & d2->callback) ||
                (category2 & d1->callback))
                for (i = 0; i < n; ++i)
                {
                    float p[3];
                    float v[3];

                    p[0] = (float) contact[i].geom.pos[0];
                    p[1] = (float) contact[i].geom.pos[1];
                    p[2] = (float) contact[i].geom.pos[2];

                    v[0] = (float) contact[i].geom.normal[0];
                    v[1] = (float) contact[i].geom.normal[1];
                    v[2] = (float) contact[i].geom.normal[2];

                    do_contact_script(d1->entity, d2->entity,
                                      p, v, (float) contact[i].geom.depth);
                }
        }
    }
}