void YGEBodyAsset::update(long delta){
if(!hasBody){
createBody();
} else {
double second = delta / 1000000.0f;
double m = 1; //second * 1000;
dBodySetAngularDamping(bodyId, angularDamping * second);
dBodySetLinearDamping(bodyId, linearDamping * second);
dBodyAddTorque(bodyId, torqueAccum.x * m, torqueAccum.y * m, torqueAccum.z * m);
dBodyAddForce(bodyId, forceAccum.x * m, forceAccum.y * m, forceAccum.z * m);
const dReal* pos = dBodyGetPosition (bodyId);
const dReal* rot = dBodyGetQuaternion(bodyId);
parent->setPosition(YGEMath::Vector3(pos[0], pos[1], pos[2]));
parent->setOrientation(YGEMath::Quaternion(rot[0], rot[1], rot[2], rot[3]));
}
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 43-ODE-tool\n");
printf ("Copyright 2003-2011\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Instructions:\n\n");
printf ("- Use haptic device and user switch to manipulate cubes \n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[h] - Display help menu\n");;
printf ("[1] - Enable gravity\n");
printf ("[2] - Disable gravity\n");
printf ("\n");
printf ("[3] - decrease linear haptic gain\n");
printf ("[4] - increase linear haptic gain\n");
printf ("[5] - decrease angular haptic gain\n");
printf ("[6] - increase angular haptic gain\n");
printf ("\n");
printf ("[7] - decrease linear stiffness\n");
printf ("[8] - increase linear stiffness\n");
printf ("[9] - decrease angular stiffness\n");
printf ("[0] - increase angular stiffness\n");
printf ("\n");
printf ("[x] - Exit application\n");
printf ("\n\n");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
// the color is defined by its (R,G,B) components.
world->setBackgroundColor(0.0, 0.0, 0.0);
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (3.0, 0.0, 0.3), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the "up" vector
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
camera->setUseShadowCasting(false);
// create a light source and attach it to the camera
light = new cSpotLight(world);
camera->addChild(light); // attach light to camera
light->setEnabled(true); // enable light source
light->setLocalPos(cVector3d( 0.0, 0.5, 0.0)); // position the light source
light->setDir(cVector3d(-3.0,-0.5, 0.0)); // define the direction of the light beam
light->m_ambient.set(0.6, 0.6, 0.6);
light->m_diffuse.set(0.8, 0.8, 0.8);
light->m_specular.set(0.8, 0.8, 0.8);
frame = new cGenericObject();
//world->addChild(frame);
frame->setShowFrame(true);
line = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
//world->addChild(line);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_frontLayer->addChild(logo);
// load a "chai3d" bitmap image file
logo->setImage (NEW_CHAI3D_LOGO());
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setLocalPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoom(0.25, 0.25);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image->setTransparentColor(0x00, 0x00, 0x00, 0x00);
// enable transparency
logo->setUseTransparency(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
workspaceScaleFactor = 1.5 / info.m_workspaceRadius;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create an ODE world to simulate dynamic bodies
ODEWorld = new cODEWorld(world);
// add ODE world as a node inside world
world->addChild(ODEWorld);
// set some gravity
ODEWorld->setGravity(cVector3d(0.0, 0.0, -9.81));
// create a new ODE object that is automatically added to the ODE world
ODEBody0 = new cODEGenericBody(ODEWorld);
ODEBody1 = new cODEGenericBody(ODEWorld);
ODEBody2 = new cODEGenericBody(ODEWorld);
// create a virtual mesh that will be used for the geometry
// representation of the dynamic body
cMesh* object0 = new cMesh();
cMesh* object1 = new cMesh();
cMesh* object2 = new cMesh();
// crate a cube mesh
double boxSize = 0.4;
createCube(object0, boxSize);
createCube(object1, boxSize);
createCube(object2, boxSize);
// define some material properties for each cube
cMaterial mat0, mat1, mat2;
mat0.m_ambient.set(0.8, 0.1, 0.4);
mat0.m_diffuse.set(1.0, 0.15, 0.5);
mat0.m_specular.set(1.0, 0.2, 0.8);
mat0.setDynamicFriction(0.8);
mat0.setStaticFriction(0.8);
object0->setMaterial(mat0);
mat1.m_ambient.set(0.2, 0.6, 0.0);
mat1.m_diffuse.set(0.2, 0.8, 0.0);
mat1.m_specular.set(0.2, 1.0, 0.0);
mat1.setDynamicFriction(0.8);
mat1.setStaticFriction(0.8);
object1->setMaterial(mat1);
mat2.m_ambient.set(0.0, 0.2, 0.6);
mat2.m_diffuse.set(0.0, 0.2, 0.8);
mat2.m_specular.set(0.0, 0.2, 1.0);
mat2.setDynamicFriction(0.8);
mat2.setStaticFriction(0.8);
object2->setMaterial(mat2);
// add mesh to ODE object
ODEBody0->setImageModel(object0);
ODEBody1->setImageModel(object1);
ODEBody2->setImageModel(object2);
// create a dynamic model of the ODE object. Here we decide to use a box just like
// the object mesh we just defined
ODEBody0->createDynamicBox(boxSize, boxSize, boxSize);
ODEBody1->createDynamicBox(boxSize, boxSize, boxSize, false, cVector3d(1,1,1));
ODEBody2->createDynamicBox(boxSize, boxSize, boxSize);
// define some mass properties for each cube
ODEBody0->setMass(0.05);
ODEBody1->setMass(0.05);
ODEBody2->setMass(0.05);
// set position of each cube
cVector3d tmpvct;
tmpvct = cVector3d(0.0,-0.6, -0.5);
ODEBody0->setPosition(tmpvct);
tmpvct = cVector3d(0.0, 0.6, -0.5);
ODEBody1->setPosition(tmpvct);
tmpvct = cVector3d(0.0, 0.0, -0.5);
ODEBody2->setPosition(tmpvct);
// rotate central cube of 45 degrees (just to show hoe this works!)
cMatrix3d rot;
rot.identity();
rot.rotateAboutGlobalAxisRad(cVector3d(0,0,1), cDegToRad(45));
ODEBody0->setRotation(rot);
// we create 6 static walls to contains the 3 cubes within a limited workspace
ODEGPlane0 = new cODEGenericBody(ODEWorld);
ODEGPlane1 = new cODEGenericBody(ODEWorld);
ODEGPlane2 = new cODEGenericBody(ODEWorld);
ODEGPlane3 = new cODEGenericBody(ODEWorld);
ODEGPlane4 = new cODEGenericBody(ODEWorld);
ODEGPlane5 = new cODEGenericBody(ODEWorld);
double size = 1.0;
ODEGPlane0->createStaticPlane(cVector3d(0.0, 0.0, 2.0 *size), cVector3d(0.0, 0.0 ,-1.0));
ODEGPlane1->createStaticPlane(cVector3d(0.0, 0.0, -size), cVector3d(0.0, 0.0 , 1.0));
ODEGPlane2->createStaticPlane(cVector3d(0.0, size, 0.0), cVector3d(0.0,-1.0, 0.0));
ODEGPlane3->createStaticPlane(cVector3d(0.0, -size, 0.0), cVector3d(0.0, 1.0, 0.0));
ODEGPlane4->createStaticPlane(cVector3d( size, 0.0, 0.0), cVector3d(-1.0,0.0, 0.0));
ODEGPlane5->createStaticPlane(cVector3d(-0.8 * size, 0.0, 0.0), cVector3d( 1.0,0.0, 0.0));
// create a virtual tool
ODETool = new cODEGenericBody(ODEWorld);
cMesh* objectTool = new cMesh();
createCube(objectTool, boxSize);
// define some material properties for each cube
cMaterial matTool;
matTool.m_ambient.set(0.4, 0.4, 0.4);
matTool.m_diffuse.set(0.8, 0.8, 0.8);
matTool.m_specular.set(1.0, 1.0, 1.0);
matTool.setDynamicFriction(0.8);
matTool.setStaticFriction(0.8);
objectTool->setMaterial(matTool);
// add mesh to ODE object
ODETool->setImageModel(objectTool);
ODETool->createDynamicBox(boxSize, boxSize, boxSize);
// define some mass properties for each cube
ODETool->setMass(0.01);
dBodySetAngularDamping(ODETool->m_ode_body, 0.04);
dBodySetLinearDamping(ODETool->m_ode_body, 0.04);
//////////////////////////////////////////////////////////////////////////
// Create some reflexion
//////////////////////////////////////////////////////////////////////////
// we create an intermediate node to which we will attach
// a copy of the object located inside the world
cGenericObject* reflexion = new cGenericObject();
// set this object as a ghost node so that no haptic interactions or
// collision detecting take place within the child nodes added to the
// reflexion node.
reflexion->setGhostEnabled(true);
// add reflexion node to world
world->addChild(reflexion);
// turn off culling on each object (objects now get rendered on both sides)
object0->setUseCulling(false, true);
object1->setUseCulling(false, true);
object2->setUseCulling(false, true);
// create a symmetry rotation matrix (z-plane)
cMatrix3d rotRefexion;
rotRefexion.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
reflexion->setLocalRot(rotRefexion);
reflexion->setLocalPos(0.0, 0.0, -2.005);
// add objects to the world
reflexion->addChild(ODEWorld);
//////////////////////////////////////////////////////////////////////////
// Create a Ground
//////////////////////////////////////////////////////////////////////////
// create mesh to model ground surface
cMesh* ground = new cMesh();
world->addChild(ground);
// create 4 vertices (one at each corner)
double groundSize = 2.0;
int vertices0 = ground->newVertex(-groundSize, -groundSize, 0.0);
int vertices1 = ground->newVertex( groundSize, -groundSize, 0.0);
int vertices2 = ground->newVertex( groundSize, groundSize, 0.0);
int vertices3 = ground->newVertex(-groundSize, groundSize, 0.0);
// compose surface with 2 triangles
ground->newTriangle(vertices0, vertices1, vertices2);
ground->newTriangle(vertices0, vertices2, vertices3);
// compute surface normals
ground->computeAllNormals();
// position ground at the right level
ground->setLocalPos(0.0, 0.0, -1.0);
// define some material properties and apply to mesh
cMaterial matGround;
matGround.setDynamicFriction(0.7);
matGround.setStaticFriction(1.0);
matGround.m_ambient.set(0.0, 0.0, 0.0);
matGround.m_diffuse.set(0.0, 0.0, 0.0);
matGround.m_specular.set(0.0, 0.0, 0.0);
ground->setMaterial(matGround);
// enable and set transparency level of ground
ground->setTransparencyLevel(0.7);
ground->setUseTransparency(true);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutTimerFunc(30, graphicsTimer, 0);
glutMainLoop();
// close everything
close();
// exit
return (0);
}
void PhysicsBody::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_BodyID != 0)
{
dBodyDestroy(_BodyID);
}
if(getWorld() != NULL)
{
_BodyID = dBodyCreate(getWorld()->getWorldID());
}
}
if( ((whichField & PositionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z());
}
if( ((whichField & RotationFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dBodySetRotation(_BodyID, rotation);
}
if( ((whichField & QuaternionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dBodySetQuaternion(_BodyID,q);
}
if( ((whichField & LinearVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z());
}
if( ((whichField & AngularVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z());
}
if( ((whichField & ForceFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z());
}
if( ((whichField & TorqueFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z());
}
if( ((whichField & AutoDisableFlagFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag());
}
if( ((whichField & AutoDisableLinearThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold());
}
if( ((whichField & AutoDisableAngularThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold());
}
if( ((whichField & AutoDisableStepsFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps());
}
if( ((whichField & AutoDisableTimeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableTime(_BodyID, getAutoDisableTime());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationAxisFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z());
}
if( ((whichField & GravityModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetGravityMode(_BodyID, getGravityMode());
}
if( ((whichField & LinearDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDamping(_BodyID, getLinearDamping());
}
if( ((whichField & AngularDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDamping(_BodyID, getAngularDamping());
}
if( ((whichField & LinearDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold());
}
if( ((whichField & AngularDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold());
}
if( ((whichField & MaxAngularSpeedFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getMaxAngularSpeed() >= 0.0)
{
dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed());
}
else
{
dBodySetMaxAngularSpeed(_BodyID, dInfinity);
}
}
if( ((whichField & MassFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
TheMass.mass = getMass();
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassCenterOfGravityFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
//dMass TheMass;
//dBodyGetMass(_BodyID, &TheMass);
////TheMass.c[0] = getMassCenterOfGravity().x();
////TheMass.c[1] = getMassCenterOfGravity().y();
////TheMass.c[2] = getMassCenterOfGravity().z();
//Vec4f v1 = getMassInertiaTensor()[0];
//Vec4f v2 = getMassInertiaTensor()[1];
//Vec4f v3 = getMassInertiaTensor()[2];
//TheMass.I[0] = v1.x();
//TheMass.I[1] = v1.y();
//TheMass.I[2] = v1.z();
//TheMass.I[3] = getMassCenterOfGravity().x();
//TheMass.I[4] = v2.x();
//TheMass.I[5] = v2.y();
//TheMass.I[6] = v2.z();
//TheMass.I[7] = getMassCenterOfGravity().y();
//TheMass.I[8] = v3.x();
//TheMass.I[9] = v3.y();
//TheMass.I[10] = v3.z();
//TheMass.I[11] = getMassCenterOfGravity().z();
//dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassInertiaTensorFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
Vec4f v1 = getMassInertiaTensor()[0];
Vec4f v2 = getMassInertiaTensor()[1];
Vec4f v3 = getMassInertiaTensor()[2];
TheMass.I[0] = v1.x();
TheMass.I[1] = v1.y();
TheMass.I[2] = v1.z();
TheMass.I[3] = 0;
TheMass.I[4] = v2.x();
TheMass.I[5] = v2.y();
TheMass.I[6] = v2.z();
TheMass.I[7] = 0;
TheMass.I[8] = v3.x();
TheMass.I[9] = v3.y();
TheMass.I[10] = v3.z();
TheMass.I[11] = 0;
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & KinematicFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getKinematic())
{
dBodySetKinematic(_BodyID);
}
else
{
dBodySetDynamic(_BodyID);
}
}
}
void Simulation::staticCollisionCallback(Simulation* simulation, dGeomID geomId1, dGeomID geomId2)
{
ASSERT(!dGeomIsSpace(geomId1));
ASSERT(!dGeomIsSpace(geomId2));
#ifndef NDEBUG
{
dBodyID bodyId1 = dGeomGetBody(geomId1);
dBodyID bodyId2 = dGeomGetBody(geomId2);
ASSERT(bodyId1 || bodyId2);
Body* body1 = bodyId1 ? (Body*)dBodyGetData(bodyId1) : 0;
Body* body2 = bodyId2 ? (Body*)dBodyGetData(bodyId2) : 0;
ASSERT(!body1 || !body2 || body1->rootBody != body2->rootBody);
}
#endif
dContact contact[32];
int collisions = dCollide(geomId1, geomId2, 32, &contact[0].geom, sizeof(dContact));
if(collisions <= 0)
return;
Geometry* geometry1 = (Geometry*)dGeomGetData(geomId1);
Geometry* geometry2 = (Geometry*)dGeomGetData(geomId2);
if(geometry1->collisionCallbacks && !geometry2->immaterial)
{
for(std::list<SimRobotCore2::CollisionCallback*>::iterator i = geometry1->collisionCallbacks->begin(), end = geometry1->collisionCallbacks->end(); i != end; ++i)
(*i)->collided(*geometry1, *geometry2);
if(geometry1->immaterial)
return;
}
if(geometry2->collisionCallbacks && !geometry1->immaterial)
{
for(std::list<SimRobotCore2::CollisionCallback*>::iterator i = geometry2->collisionCallbacks->begin(), end = geometry2->collisionCallbacks->end(); i != end; ++i)
(*i)->collided(*geometry2, *geometry1);
if(geometry2->immaterial)
return;
}
dBodyID bodyId1 = dGeomGetBody(geomId1);
dBodyID bodyId2 = dGeomGetBody(geomId2);
ASSERT(bodyId1 || bodyId2);
float friction = 1.f;
if(geometry1->material && geometry2->material)
{
if(!geometry1->material->getFriction(*geometry2->material, friction))
friction = 1.f;
float rollingFriction;
if(bodyId1)
switch(dGeomGetClass(geomId1))
{
case dSphereClass:
case dCCylinderClass:
case dCylinderClass:
if(geometry1->material->getRollingFriction(*geometry2->material, rollingFriction))
{
dBodySetAngularDamping(bodyId1, 0.2);
Vector3<> linearVel;
ODETools::convertVector(dBodyGetLinearVel(bodyId1), linearVel);
linearVel -= Vector3<>(linearVel).normalize(std::min(linearVel.abs(), rollingFriction * simulation->scene->stepLength));
dBodySetLinearVel(bodyId1, linearVel.x, linearVel.y, linearVel.z);
}
break;
}
if(bodyId2)
switch(dGeomGetClass(geomId2))
{
case dSphereClass:
case dCCylinderClass:
case dCylinderClass:
if(geometry2->material->getRollingFriction(*geometry1->material, rollingFriction))
{
dBodySetAngularDamping(bodyId2, 0.2);
Vector3<> linearVel;
ODETools::convertVector(dBodyGetLinearVel(bodyId2), linearVel);
linearVel -= Vector3<>(linearVel).normalize(std::min(linearVel.abs(), rollingFriction * simulation->scene->stepLength));
dBodySetLinearVel(bodyId2, linearVel.x, linearVel.y, linearVel.z);
}
break;
}
}
for(dContact* cont = contact, * end = contact + collisions; cont < end; ++cont)
{
cont->surface.mode = simulation->scene->contactMode | dContactApprox1;
cont->surface.mu = friction;
/*
cont->surface.bounce = 0.f;
cont->surface.bounce_vel = 0.001f;
cont->surface.slip1 = 0.f;
cont->surface.slip2 = 0.f;
*/
cont->surface.soft_erp = simulation->scene->contactSoftERP;
cont->surface.soft_cfm = simulation->scene->contactSoftCFM;
dJointID c = dJointCreateContact(simulation->physicalWorld, simulation->contactGroup, cont);
ASSERT(bodyId1 == dGeomGetBody(cont->geom.g1));
ASSERT(bodyId2 == dGeomGetBody(cont->geom.g2));
dJointAttach(c, bodyId1, bodyId2);
}
++simulation->collisions;
simulation->contactPoints += collisions;
}
