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));
}
}
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));
}
}
}
}
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));
}
}
}
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();
}
//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);
}
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);
}
}
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);
}
}
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);
}
}
bool PObject::isPlaceable()
{
int c = dGeomGetClass(geom);
if (c == dPlaneClass)
return false;
return true;
}
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);
}
}
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);
}
}
}
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);
}
}
//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]);
}
}
}
}
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);
}
}
}
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 );
}
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.
}
}
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);
}
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);
}
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));
}
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);
}
}
}
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);
}
}
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;
}
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:
{}
}
}
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;
}
void dmDraw(dmObject *obj) /*** 物体の描画 ***/
{
if (!obj->geom) return;
obj->p = (double *) dGeomGetPosition(obj->geom);
obj->R = (double *) dGeomGetRotation(obj->geom);
dsSetColor(obj->color[0],obj->color[1],obj->color[2]); // 色の設定(r,g,b)
// printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]);
int type = dGeomGetClass(obj->geom);
switch (type)
{
case dBoxClass:
{
dVector3 sides;
dGeomBoxGetLengths(obj->geom,sides);
dsDrawBox(obj->p,obj->R,sides);
}
break;
case dSphereClass:
dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom));
break;
case dCapsuleClass:
{
dReal radius,length;
dGeomCapsuleGetParams(obj->geom,&radius,&length);
dsDrawCapsule(obj->p,obj->R,length,radius);
}
break;
case dCylinderClass:
{
dReal radius,length;
dGeomCylinderGetParams(obj->geom,&radius,&length);
dsDrawCylinder(obj->p,obj->R,length,radius);
}
break;
default:
printf("Bad geometry type \n");
}
}
void 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);
}
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
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);
}}}
}
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);
}
}
}
}