//set event
void Joint::Set_Buffer_Event(dReal thres, dReal buff, Script *scr)
{
if (thres > 0 && buff > 0 && scr)
{
printlog(2, "setting Joint event");
feedback=new dJointFeedback;
dJointSetFeedback (joint_id, feedback);
threshold=thres;
buffer=buff;
buffer_script=scr;
//make sure no old event is left
Buffer_Event_List::Remove(this);
buffer_event=true;
}
else
{
printlog(2, "disabling Joint event");
buffer_event=false;
//remove feedback data
if (feedback)
{
delete feedback;
feedback=NULL;
}
Buffer_Event_List::Remove(this);
//disable
dJointSetFeedback(joint_id, 0);
}
}
void CPHFracturesHolder::PhTune(dBodyID body)
{
//iterate through all body's joints and set joints feedbacks where is not already set
//contact feedbacks stored in global storage - ContactFeedBacks wich cleared on each step
//breacable joints already has their feedbacks,
//feedbacks for rest noncontact joints stored in m_feedbacks in runtime in this function and
//and killed by destructor
//int dBodyGetNumJoints (dBodyID b);
//dJointID dBodyGetJoint (dBodyID, int index);
//dJointGetType
//dJointTypeContact
int num=dBodyGetNumJoints(body);
for(int i=0;i<num;++i)
{
dJointID joint=dBodyGetJoint(body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointSetFeedback(joint,ContactFeedBacks.add());
}
else
{
CPHJoint* ph_joint=(CPHJoint*)dJointGetData(joint);
if(!(ph_joint&&ph_joint->JointDestroyInfo())) dJointSetFeedback(joint,ContactFeedBacks.add());
//if(!dJointGetFeedback(joint))
//{
// m_feedbacks.push_back(dJointFeedback());
// dJointSetFeedback(joint,&m_feedbacks.back());
//}
}
}
}
void Joint::build(const Vector3d &v, const Rotation &r, bool dynamics)
{
if (m_bodyNum != BODY_NUM) { return; }
assert(m_world);
m_joint = createJoint(m_bodies[0], m_bodies[1]);
Parts *parts = (Parts*) dBodyGetData(m_bodies[1]);
assert(parts);
double x, y, z;
parts->givePosition(x, y, z);
m_rotv.set(x, y, z);
m_rotv -= m_anchor;
Vector3d av = m_anchor;
av.rotate(r);
av += v;
applyAnchor(av.x(), av.y(), av.z());
if (m_fixed) {
dJointSetFixed(m_joint);
}
if (dynamics) {
m_jfb = new dJointFeedback;
dJointSetFeedback(m_joint, m_jfb);
}
}
StickyObj::StickyObj(const char * string){
name = string;
geomID = dWebotsGetGeomFromDEF(name);
if(geomID){
if(DEBUG)dWebotsConsolePrintf("%s geom has been found !! ", name);
bodyID = dGeomGetBody(geomID);
dMass dmass;
dBodyGetMass(bodyID, &dmass);
mass = dmass.mass;
}else{
dWebotsConsolePrintf("ERROR %s geom has NOT been found !! ERROR ", name);
}
linkJoint = dBodyGetJoint(bodyID,0);
dJointSetFeedback(linkJoint, &linkJointFeedback);
adhesiveForce = Vector3D(0.0,0.0,0.0);
adheringPoints = 0;
surfaceArea = 0.0;
collidingPoints = 0;
state = 0;
elapsedDetachingTimer = 0;
elapsedAttachingTimer = 0;
rho = 1;
}
virtual void PhTune(dReal step)
{
InitValues();
int num=dBodyGetNumJoints(m_body);
for(int i=0;i<num;++i)
{
dJointID joint=dBodyGetJoint(m_body,i);
if(dJointGetType(joint)==dJointTypeContact)
{
dJointSetFeedback(joint,ContactFeedBacks.add());
}
}
}
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++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
if (doFeedback && (b1==obj[selected].body || b2==obj[selected].body))
{
if (fbnum<MAX_FEEDBACKNUM)
{
feedbacks[fbnum].first = b1==obj[selected].body;
dJointSetFeedback (c,&feedbacks[fbnum++].fb);
}
else fbnum++;
}
}
}
}
// This function is implemented to overide Webots collision detection.
// It returns 1 if a specific collision is handled, and 0 otherwise.
int webots_physics_collide(dGeomID g1, dGeomID g2) {
// check if this collision involves the objects which interest us
if ((dAreGeomsSame(g1, box_geom) && dAreGeomsSame(g2, floor_geom)) || (dAreGeomsSame(g2, box_geom) && dAreGeomsSame(g1, floor_geom))) {
dBodyID body = dGeomGetBody(g1);
if (body==NULL) body = dGeomGetBody(g2);
if (body==NULL) return 0;
dWorldID world = dBodyGetWorld(body);
dJointGroupID contact_joint_group = dWebotsGetContactJointGroup();
// see how many collision points there are between theses objects
nContacts = dCollide(g1, g2, MAX_CONTACTS, &contacts[0].geom, sizeof(dContact));
int i;
for (i = 0; i < nContacts; i++) {
// custom parameters for creating the contact joint
// remove or tune these contact parameters to suit your needs
contacts[i].surface.mode = dContactBounce | dContactSoftCFM | dContactApprox1;
contacts[i].surface.mu = 1.5;
contacts[i].surface.bounce = 0.5;
contacts[i].surface.bounce_vel = 0.01;
contacts[i].surface.soft_cfm = 0.001;
// create a contact joint that will prevent the two bodies from intersecting
// note that contact joints are added to the contact_joint_group
pthread_mutex_lock(&mutex);
contact_joints[i] = dJointCreateContact(world, contact_joint_group, &contacts[i]);
// attach joint between the body and the static environment (0)
dJointAttach(contact_joints[i], box_body, 0);
// attach feedback structure to measure the force on the contact joint
dJointSetFeedback(contact_joints[i], &feedbacks[i]);
pthread_mutex_unlock(&mutex);
}
return 1; // collision was handled above
}
return 0; // collision must be handled by webots
}
// implemented to overide Webots collision detection,
// returns 1 if a specific collision is handled, and 0 otherwise
int webots_physics_collide(dGeomID g1, dGeomID g2) {
static dJointID contact_joints[MAX_CONTACTS];
int i, j;
for (i = 0; i < N_FEET; i++) {
// handle any collisions involving a foot and the floor
if ((g1 == foot_geom[i] && g2 == floor_geom) || (g2 == foot_geom[i] && g1 == floor_geom)) {
// see how many collision points there are between the two geometries
nContacts[i] = dCollide(foot_geom[i], floor_geom, MAX_CONTACTS, &foot_contacts[i][0].geom, sizeof(dContact));
for (j = 0; j < nContacts[i]; j++) {
// custom parameters for creating the contact joint
// remove or tune these contact parameters to suit your needs
foot_contacts[i][j].surface.mode = dContactBounce | dContactSoftCFM | dContactApprox1;
foot_contacts[i][j].surface.mu = 1.5;
foot_contacts[i][j].surface.bounce = 0.5;
foot_contacts[i][j].surface.bounce_vel = 0.01;
foot_contacts[i][j].surface.soft_cfm = 0.001;
// create a contact joint that will prevent the two bodies from intersecting
// note that contact joints are added to the contact_joint_group
contact_joints[j] = dJointCreateContact(world, contact_joint_group, &foot_contacts[i][j]);
// attach joint between the body and the static environment (0)
dJointAttach(contact_joints[j], foot_body[i], 0);
// attach feedback structure to measure the force on the contact joint
dJointSetFeedback(contact_joints[j], &foot_feedbacks[i][j]);
}
return 1; // collision was handled above
}
}
return 0; // collision must be handled by webots
}
static void nearCB(void *data, dGeomID o1, dGeomID o2)
{
ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1);
ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2);
/*
SSimRobotEntity *ent1 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o1);
SSimRobotEntity *ent2 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o2);
if(ent1 != NULL && ent2 != NULL && ent1->name() == ent2->name()){
//LOG_MSG(("name (%s, %s)",ent1->name().c_str(), ent2->name().c_str()));
return;
}
*/
SParts *p1 = NULL;
SParts *p2 = NULL;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 == b2) {
return;
}
ODEWorld *world = ODEWorld::get();
dGeomID ground = world->getGround();
if (b1 && b2) {
if (dAreConnected(b1, b2)) {
return;
}
}
if (b1) {
void *d = dBodyGetData(b1);
if (d) {
p1 = (SParts*)d;
if (p1->isBlind()) { return; }
}
}
if (b2) {
void *d = dBodyGetData(b2);
if (d) {
p2 = (SParts*)d;
if (p2->isBlind()) { return; }
}
}
if (p1 && p2 && p1->sameParent(*p2)) { return; }
if (dGeomIsSpace(o1) && dGeomIsSpace(o2)) {
dSpaceCollide2(o1, o2, data, &collideCB);
return;
}
#define F_SCHOLAR(V) sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2])
static dJointFeedback fb;
#define MAX_COLLISION 32
const int N = MAX_COLLISION;
dContact contacts[N];
int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0]));
if (n > 0) {
ODEWorld *world = ODEWorld::get();
for (int i=0; i<n; i++) {
dContact *c = &contacts[i];
dContactGeom &g = c->geom;
if (p1 && p2) {
#if 0
LOG_SYS(("Collision #%d/%d %s(geomID=%d) <-> %s(geomID=%d)", i, n,
p1->getParent()->name(), o1,
p2->getParent()->name(), o2));
LOG_SYS(("\tpos = (%f, %f, %f)", g.pos[0], g.pos[1], g.pos[2]));
LOG_SYS(("\tnormal = (%f, %f, %f)", g.normal[0], g.normal[1], g.normal[2]));
LOG_SYS(("\tfdir = (%f, %f, %f)", c->fdir1[0], c->fdir1[1], c->fdir1[2]));
LOG_SYS(("\tdepth = %f", g.depth));
#endif
const char *name1 = p1->getParent()->name();
const char *name2 = p2->getParent()->name();
std::string key = chash_key(name1, name2);
if (key.length() <= 0) { continue; }
if (chash_find(key)) { continue; }
s_chash[key] = true;
s_collisions.push_back(ODEWorld::Collision(name1, name2, p1->name(), p2->name()));
// Set the collision flag to be ON
p1->setOnCollision(true);
p2->setOnCollision(true);
}
//c->surface.mode = dContactBounce;
c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//c->surface.mode = dContactSlip1 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//
// Reflection of material parameters of the collided object
// Fliction force should be regarded as average of contiguous material (???)
// TODO: Calclation of fliction force sould be considered
//
#if 0
if (odeobj1 && odeobj2) {
c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0;
c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0;
c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0;
c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0;
c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0;
c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0;
c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0;
}
else {
c->surface.bounce_vel = world->getCollisionParam("bounce_vel");
c->surface.bounce = world->getCollisionParam("bounce");
c->surface.mu = world->getCollisionParam("mu");
c->surface.mu2 = world->getCollisionParam("mu2");
c->surface.slip1 = world->getCollisionParam("slip1");
c->surface.slip2 = world->getCollisionParam("slip2");
c->surface.soft_erp = world->getCollisionParam("soft_erp");
c->surface.soft_cfm = world->getCollisionParam("soft_cfm");
}
#else
c->surface.mu = SPARTS_MU1;
c->surface.mu2 = SPARTS_MU2;
c->surface.slip1 = SPARTS_SLIP1;
c->surface.slip2 = SPARTS_SLIP2;
c->surface.soft_erp = SPARTS_ERP; // parameter for modify the error of Joint position (0..1); if it is 1, modification will be perfect
c->surface.soft_cfm = SPARTS_CFM; // If this value=0, joint velocity is strange
c->surface.bounce = SPARTS_BOUNCE; // is a little smaller than ball
c->surface.bounce_vel = 0.0;
#endif
dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c);
//dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2)); //by MSI
dJointAttach(cj, dGeomGetBody(c->geom.g1), dGeomGetBody(c->geom.g2)); // by Demura.net
#if 0
if (p1 && p2) {
dJointSetFeedback(cj, &fb);
dJointFeedback *pfb = dJointGetFeedback(cj);
if (F_SCHOLAR(pfb->f1) > 1.0) {
LOG_SYS(("\tF1 = (%f, %f, %f)", pfb->f1[0], pfb->f1[1], pfb->f1[2]));
LOG_SYS(("\tF2 = (%f, %f, %f)\n", pfb->f2[0], pfb->f2[1], pfb->f2[2]));
}
}
#endif
}
}
}
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
// chassis pin joints
const dReal *pos;
snow->jChassisUp= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body);
dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0);
pos= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jChassisUp,pos[0]
,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]);
const dReal *pos1;
snow->jChassisLow= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body);
dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0);
pos1= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jChassisLow, pos1[0]
, pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]);
// rotor ball joint
/*const dReal *p2;
jRotorUp= dJointCreateBall(world,0);
dJointAttach(jRotorUp, uplink.body, hlink.body);
p2= dBodyGetPosition(uplink.body);
dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]);
const dReal *p3;
jRotorLow= dJointCreateBall(world,0);
dJointAttach(jRotorLow, lowlink.body,hlink.body);
p3= dBodyGetPosition(lowlink.body);
dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/
const dReal *p2;
snow->jRotorUp= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body);
p2= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jRotorUp, p2[0]
,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]);
dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0);
const dReal *p3;
snow->jRotorLow= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body);
p3= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jRotorLow, p3[0]
,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]);
dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0);
// rotor hinge joint
const dReal *pw= dBodyGetPosition(wnow->body);
snow->jRotorMid= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body);
dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0);
dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]);
// strut joint
const dReal *ps1, *ps2;
dReal angle= -shiftSign*strutAngle;
ps1= dBodyGetPosition(snow->upstrut.body);
ps2= dBodyGetPosition(snow->lowstrut.body);
snow->jStrutUp= dJointCreateBall(world,0);
dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body);
//dJointSetFixed(snow->jStrutUp);
dJointSetBallAnchor(snow->jStrutUp, ps1[0]
,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle))
,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle)));
snow->jStrutLow= dJointCreateBall(world,0);
dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body);
dJointSetBallAnchor(snow->jStrutLow, ps2[0]
,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle))
,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle)));
// struct sliding joint
snow->jStrutMid= dJointCreateSlider(world,0);
dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body);
dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle)));
// set joint feedback
wnow->feedback= new dJointFeedback;
dJointSetFeedback(snow->jStrutMid,wnow->feedback);
// suspension slider
/*snow->jLowSpring= dJointCreateLMotor(world,0);
dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body);
dJointSetLMotorNumAxes(snow->jLowSpring, 1);
dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.0);*/
}
int main (int argc, char **argv)
{
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 20);
int i;
for (i=0; i<SEGMCNT; i++)
{
segbodies[i] = dBodyCreate (world);
dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dBodySetMass (segbodies[i], &m);
seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dGeomSetBody (seggeoms[i], segbodies[i]);
dSpaceAdd (space, seggeoms[i]);
}
for (i=0; i<SEGMCNT-1; i++)
{
hinges[i] = dJointCreateHinge (world,0);
dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
dJointSetHingeAxis (hinges[i], 0,1,0);
dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
// NOTE:
// Here we tell ODE where to put the feedback on the forces for this hinge
dJointSetFeedback (hinges[i], jfeedbacks+i);
stress[i]=0;
}
for (i=0; i<STACKCNT; i++)
{
stackbodies[i] = dBodyCreate(world);
dMassSetBox (&m, 2.0, 2, 2, 0.6);
dBodySetMass(stackbodies[i],&m);
stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
dGeomSetBody(stackgeoms[i], stackbodies[i]);
dBodySetPosition(stackbodies[i], 0,0,8+2*i);
dSpaceAdd(space, stackgeoms[i]);
}
sliders[0] = dJointCreateSlider (world,0);
dJointAttach(sliders[0], segbodies[0], 0);
dJointSetSliderAxis (sliders[0], 1,0,0);
dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
sliders[1] = dJointCreateSlider (world,0);
dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
dJointSetSliderAxis (sliders[1], 1,0,0);
dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
groundgeom = dCreatePlane(space, 0,0,1,0);
for (i=0; i<SEGMCNT; i++)
colours[i]=0.0;
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy seggeoms, then space, then the world.
for (i=0; i<SEGMCNT; i++)
dGeomDestroy (seggeoms[i]);
for (i=0; i<STACKCNT; i++)
dGeomDestroy (stackgeoms[i]);
dSpaceDestroy(space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
/**
* \brief In this function the collision handling from ode is performed.
*
* pre:
* - world_init = true
* - o1 and o2 are regular geoms
*
* post:
* - if o1 or o2 was a Space, called SpaceCollide and exit
* - otherwise tested if the geoms collide and created a contact
* joint if so.
*
* A lot of the code is uncommented in this function. This
* code maybe used later to handle sensors or other special cases
* in the simulation.
*/
void WorldPhysics::nearCallback (dGeomID o1, dGeomID o2) {
int i;
int numc;
//up to MAX_CONTACTS contact per Box-box
//dContact contact[MAX_CONTACTS];
dVector3 v1, v;
//dMatrix3 R;
dReal dot;
if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
/// test if a space is colliding with something
dSpaceCollide2(o1,o2,this,& WorldPhysics::callbackForward);
return;
}
/// exit without doing anything if the two bodies are connected by a joint
dBodyID b1=dGeomGetBody(o1);
dBodyID b2=dGeomGetBody(o2);
geom_data* geom_data1 = (geom_data*)dGeomGetData(o1);
geom_data* geom_data2 = (geom_data*)dGeomGetData(o2);
// test if we have a ray sensor:
if(geom_data1->ray_sensor) {
dContact contact;
if(geom_data1->parent_geom == o2) {
return;
}
if(geom_data1->parent_body == dGeomGetBody(o2)) {
return;
}
numc = dCollide(o2, o1, 1|CONTACTS_UNIMPORTANT, &(contact.geom), sizeof(dContact));
if(numc) {
if(contact.geom.depth < geom_data1->value)
geom_data1->value = contact.geom.depth;
ray_collision = 1;
}
return;
}
else if(geom_data2->ray_sensor) {
dContact contact;
if(geom_data2->parent_geom == o1) {
return;
}
if(geom_data2->parent_body == dGeomGetBody(o1)) {
return;
}
numc = dCollide(o2, o1, 1|CONTACTS_UNIMPORTANT, &(contact.geom), sizeof(dContact));
if(numc) {
if(contact.geom.depth < geom_data2->value)
geom_data2->value = contact.geom.depth;
ray_collision = 1;
}
return;
}
if(b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact))
return;
if(!b1 && !b2 && !geom_data1->ray_sensor && !geom_data2->ray_sensor) return;
int maxNumContacts = 0;
if(geom_data1->c_params.max_num_contacts <
geom_data2->c_params.max_num_contacts) {
maxNumContacts = geom_data1->c_params.max_num_contacts;
}
else {
maxNumContacts = geom_data2->c_params.max_num_contacts;
}
dContact *contact = new dContact[maxNumContacts];
//for granular test
//if( (plane != o2) && (plane !=o1)) return ;
/*
/// we use the geomData to handle some special cases
void* geom_data1 = dGeomGetData(o1);
void* geom_data2 = dGeomGetData(o2);
/// one case is, that we don't wont to handle a collision between some special
/// geoms beweet each other and the ground
if((geom_data1 && ((robot_geom*)geom_data1)->type & 16)) {
if(plane == o2) return;
if((geom_data2 && ((robot_geom*)geom_data2)->type & 16)) return;
}
else if((geom_data2 && ((robot_geom*)geom_data2)->type & 16) && (plane == o1)) return;
/// an other case is a ray geom that we use simulate ray sensors
/// this geom has to be handled in a different way
if((geom_data1 && ((robot_geom*)geom_data1)->type & 8) ||
(geom_data2 && ((robot_geom*)geom_data2)->type & 8)) {
int n;
const int N = MAX_CONTACTS;
dContactGeom contact[N];
n = dCollide (o2,o1,N,contact,sizeof(dContactGeom));
if (n > 0) {
//const dReal ss[3] = {1,0.01,0.01};
for (i=0; i<n; i++) {
contact[i].pos[2] += Z_OFFSET;
if(contact[i].depth > 0.01){
if(geom_data1 && ((robot_geom*)geom_data1)->type & 8)
((robot_geom*)geom_data1)->i_length = contact[0].depth;
if(geom_data2 && ((robot_geom*)geom_data2)->type & 8)
((robot_geom*)geom_data2)->i_length = contact[0].depth;
}
}
}
return;
}
*/
// frist we set the softness values:
contact[0].surface.mode = dContactSoftERP | dContactSoftCFM;
contact[0].surface.soft_cfm = (geom_data1->c_params.cfm +
geom_data2->c_params.cfm)/2;
contact[0].surface.soft_erp = (geom_data1->c_params.erp +
geom_data2->c_params.erp)/2;
// then check if one of the geoms want to use the pyramid approximation
if(geom_data1->c_params.approx_pyramid ||
geom_data2->c_params.approx_pyramid)
contact[0].surface.mode |= dContactApprox1;
// Then check the friction for both directions
contact[0].surface.mu = (geom_data1->c_params.friction1 +
geom_data2->c_params.friction1)/2;
contact[0].surface.mu2 = (geom_data1->c_params.friction2 +
geom_data2->c_params.friction2)/2;
if(contact[0].surface.mu != contact[0].surface.mu2)
contact[0].surface.mode |= dContactMu2;
// check if we have to calculate friction direction1
if(geom_data1->c_params.friction_direction1 ||
geom_data2->c_params.friction_direction1) {
// here the calculation becomes more complicated
// maybe we should make some restrictions
// so -> we only use friction motion in friction direction 1
// the friction motion is only set if a local vector for friction
// direction 1 is given
// the steps for the calculation:
// 1. rotate the local vectors to global coordinates
// 2. scale the vectors to the length of the motion if given
// 3. vector 3 = vector 1 - vector 2
// 4. get the length of vector 3
// 5. set vector 3 as friction direction 1
// 6. set motion 1 to the length
contact[0].surface.mode |= dContactFDir1;
if(!geom_data2->c_params.friction_direction1) {
// get the orientation of the geom
//dGeomGetQuaternion(o1, v);
//dRfromQ(R, v);
// copy the friction direction
v1[0] = geom_data1->c_params.friction_direction1->x();
v1[1] = geom_data1->c_params.friction_direction1->y();
v1[2] = geom_data1->c_params.friction_direction1->z();
// translate the friction direction to global coordinates
// and set friction direction for contact
//dMULTIPLY0_331(contact[0].fdir1, R, v1);
contact[0].fdir1[0] = v1[0];
contact[0].fdir1[1] = v1[1];
contact[0].fdir1[2] = v1[2];
if(geom_data1->c_params.motion1) {
contact[0].surface.mode |= dContactMotion1;
contact[0].surface.motion1 = geom_data1->c_params.motion1;
}
}
else if(!geom_data1->c_params.friction_direction1) {
// get the orientation of the geom
//dGeomGetQuaternion(o2, v);
//dRfromQ(R, v);
// copy the friction direction
v1[0] = geom_data2->c_params.friction_direction1->x();
v1[1] = geom_data2->c_params.friction_direction1->y();
v1[2] = geom_data2->c_params.friction_direction1->z();
// translate the friction direction to global coordinates
// and set friction direction for contact
//dMULTIPLY0_331(contact[0].fdir1, R, v1);
contact[0].fdir1[0] = v1[0];
contact[0].fdir1[1] = v1[1];
contact[0].fdir1[2] = v1[2];
if(geom_data2->c_params.motion1) {
contact[0].surface.mode |= dContactMotion1;
contact[0].surface.motion1 = geom_data2->c_params.motion1;
}
}
else {
// the calculation steps as mentioned above
fprintf(stderr, "the calculation for friction directen set for both nodes is not done yet.\n");
}
}
// then check for fds
if(geom_data1->c_params.fds1 || geom_data2->c_params.fds1) {
contact[0].surface.mode |= dContactSlip1;
contact[0].surface.slip1 = (geom_data1->c_params.fds1 +
geom_data2->c_params.fds1);
}
if(geom_data1->c_params.fds2 || geom_data2->c_params.fds2) {
contact[0].surface.mode |= dContactSlip2;
contact[0].surface.slip2 = (geom_data1->c_params.fds2 +
geom_data2->c_params.fds2);
}
if(geom_data1->c_params.bounce || geom_data2->c_params.bounce) {
contact[0].surface.mode |= dContactBounce;
contact[0].surface.bounce = (geom_data1->c_params.bounce +
geom_data2->c_params.bounce);
if(geom_data1->c_params.bounce_vel > geom_data2->c_params.bounce_vel)
contact[0].surface.bounce_vel = geom_data1->c_params.bounce_vel;
else
contact[0].surface.bounce_vel = geom_data2->c_params.bounce_vel;
}
for (i=1;i<maxNumContacts;i++){
contact[i] = contact[0];
}
numc=dCollide(o1,o2, maxNumContacts, &contact[0].geom,sizeof(dContact));
if(numc){
dJointFeedback *fb;
draw_item item;
Vector contact_point;
num_contacts++;
if(create_contacts) {
fb = 0;
item.id = 0;
item.type = DRAW_LINE;
item.draw_state = DRAW_STATE_CREATE;
item.point_size = 10;
item.myColor.r = 1;
item.myColor.g = 0;
item.myColor.b = 0;
item.myColor.a = 1;
item.label = "";
item.t_width = item.t_height = 0;
item.texture = "";
item.get_light = 0;
for(i=0;i<numc;i++){
item.start.x() = contact[i].geom.pos[0];
item.start.y() = contact[i].geom.pos[1];
item.start.z() = contact[i].geom.pos[2];
item.end.x() = contact[i].geom.pos[0] + contact[i].geom.normal[0];
item.end.y() = contact[i].geom.pos[1] + contact[i].geom.normal[1];
item.end.z() = contact[i].geom.pos[2] + contact[i].geom.normal[2];
draw_intern.push_back(item);
if(geom_data1->c_params.friction_direction1 ||
geom_data2->c_params.friction_direction1) {
v[0] = contact[i].geom.normal[0];
v[1] = contact[i].geom.normal[1];
v[2] = contact[i].geom.normal[2];
dot = dDOT(v, contact[i].fdir1);
dOPEC(v, *=, dot);
contact[i].fdir1[0] -= v[0];
contact[i].fdir1[1] -= v[1];
contact[i].fdir1[2] -= v[2];
dNormalize3(contact[0].fdir1);
}
contact[0].geom.depth += (geom_data1->c_params.depth_correction +
geom_data2->c_params.depth_correction);
if(contact[0].geom.depth < 0.0) contact[0].geom.depth = 0.0;
dJointID c=dJointCreateContact(world,contactgroup,contact+i);
dJointAttach(c,b1,b2);
geom_data1->num_ground_collisions += numc;
geom_data2->num_ground_collisions += numc;
contact_point.x() = contact[i].geom.pos[0];
contact_point.y() = contact[i].geom.pos[1];
contact_point.z() = contact[i].geom.pos[2];
geom_data1->contact_ids.push_back(geom_data2->id);
geom_data2->contact_ids.push_back(geom_data1->id);
geom_data1->contact_points.push_back(contact_point);
geom_data2->contact_points.push_back(contact_point);
//if(dGeomGetClass(o1) == dPlaneClass) {
fb = 0;
if(geom_data2->sense_contact_force) {
fb = (dJointFeedback*)malloc(sizeof(dJointFeedback));
dJointSetFeedback(c, fb);
contact_feedback_list.push_back(fb);
geom_data2->ground_feedbacks.push_back(fb);
geom_data2->node1 = false;
}
//else if(dGeomGetClass(o2) == dPlaneClass) {
if(geom_data1->sense_contact_force) {
if(!fb) {
fb = (dJointFeedback*)malloc(sizeof(dJointFeedback));
dJointSetFeedback(c, fb);
contact_feedback_list.push_back(fb);
}
geom_data1->ground_feedbacks.push_back(fb);
geom_data1->node1 = true;
}
}
}
}
void odCable::setupBody(dWorldID *world, dSpaceID space){
int i;
double pi=4.*atan(1.);
double segLen, dL, x;
double dd, s, rs;
dVector3 OP;
odPoint loc1, loc2, d, P, c;
dMass *m;
dGeomID geometry[nSegments];
// dQuaternion align;
dReal align[4];
printf("Setting up cable model\n");
if (body1!=NULL){
loc1=body1->globalPosition(pos1);
}else{
loc1=pos1;
}
if (body2!=NULL){
loc2=body2->globalPosition(pos2);
}else{
loc2=pos2;
}
d=loc2-loc1;
dL=d.mag()/(double)nSegments;
segLen=length/(double)nSegments;
printf("Distance = %lf m\n",d.mag());
printf("Segment length = %lf m\n",segLen);
printf("Allocating memory\n");
element = new dBodyID[nSegments];
joint = new dJointID[nSegments-1];
printf("Creating elements\n");
for (i=0;i<nSegments;i++){
element[i] = dBodyCreate (*world);
dBodySetDamping(element[i], linearDamping, angularDamping);
m=new dMass;
dMassSetBoxTotal(m, weight*segLen, segLen, diameter, diameter);
dMassAdjust (m, weight*segLen);
dBodySetMass (element[i], m);
x=((double)i+.5)*segLen;
dBodySetPosition(element[i], x, 0, 0);
// geometry[i] = dCreateBox(space, length, diameter, diameter);
// dGeomSetBody(geometry[i], element[i]);
}
printf("Creating internal joints\n");
for (i=0;i<nSegments-1;i++){
joint[i] = dJointCreateBall(*world,0);
dJointAttach (joint[i],element[i],element[i+1]);
x = ((double)i+1.)*segLen;
dJointSetBallAnchor (joint[i],x,0,0);
}
/*
c = d.crossProduct(odPoint(0,0,1));
dd = d.dotProduct_natural(odPoint(0,0,1));
s = sqrt((1.0 + dd)*2.0);
rs = 1.0/s;
align[0]=s*0.5; align[1]=c.x*rs; align[2]=c.y*rs; align[3]=c.z*rs;
printf("Moving segments to world positions...\n");
for (i=0;i<nSegments;i++){
P=loc1+d*((double)i)/(nSegments-1);
if (i==0) P.x+=segLen/2.;
if (i==nSegments-1) P.x-=segLen/2.;
dBodySetPosition(element[i], P.x, P.y, P.z);
// dBodySetQuaternion(element[i], align);
}
*/
printf("Creating end joint 1\n");
end1 = dJointCreateBall(*world,0);
if (body1!=NULL){
dJointAttach(end1, body1->odeBody, element[0]);
}else{
dJointAttach(end1, 0, element[0]);
}
// dJointSetBallAnchor (end1, loc1.x, loc1.y, loc1.z);
dJointSetBallAnchor (end1, 0, 0, 0);
end1Feedback = new dJointFeedback;
dJointSetFeedback (end1, end1Feedback);
/*
printf("Creating end joint 2\n");
end2 = dJointCreateBall(*world,0);
if (body2!=NULL){
dJointAttach(end2, body2->odeBody, element[nSegments-1]);
}else{
dJointAttach(end2, 0, element[nSegments-1]);
}
// dJointSetBallAnchor (end2, loc2.x, loc2.y, loc2.z);
dJointSetBallAnchor (end2, nSegments*segLen, 0, 0);
*/
end2Feedback = new dJointFeedback;
dJointSetFeedback (joint[0], end2Feedback);
}
void CPHCapture::PullingUpdate()
{
if(!m_taget_element->isActive()||inl_ph_world().Device().dwTimeGlobal-m_time_start>m_capture_time)
{
Release();
return;
}
Fvector dir;
Fvector capture_bone_position;
//CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
capture_bone_position.set(m_capture_bone->mTransform.c);
m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
m_taget_element->GetGlobalPositionDynamic(&dir);
dir.sub(capture_bone_position,dir);
float dist=dir.magnitude();
if(dist>m_pull_distance)
{
Release();
return;
}
dir.mul(1.f/dist);
if(dist<m_capture_distance)
{
m_back_force=0.f;
m_joint=dJointCreateBall(0,0);
m_island.AddJoint(m_joint);
m_ajoint=dJointCreateAMotor(0,0);
m_island.AddJoint(m_ajoint);
dJointSetAMotorMode (m_ajoint, dAMotorEuler);
dJointSetAMotorNumAxes (m_ajoint, 3);
CreateBody();
dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
VERIFY( smart_cast<CPHElement*>(m_taget_element) );
CPHElement * e = static_cast<CPHElement*>(m_taget_element);
dJointAttach(m_joint,m_body,e->get_body());
dJointAttach(m_ajoint,m_body,e->get_body());
dJointSetFeedback (m_joint, &m_joint_feedback);
dJointSetFeedback (m_ajoint, &m_joint_feedback);
dJointSetBallAnchor(m_joint,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
dJointSetAMotorAxis (m_ajoint, 0, 1, dir.x, dir.y, dir.z);
if(dir.x>EPS)
{
if(dir.y>EPS)
{
float mag=dir.x*dir.x+dir.y*dir.y;
dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.y/mag, dir.x/mag, 0.f);
}
else if(dir.z>EPS)
{
float mag=dir.x*dir.x+dir.z*dir.z;
dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.z/mag,0.f,dir.x/mag);
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 1.f,0.f,0.f);
}
}
else
{
if(dir.y>EPS)
{
if(dir.z>EPS)
{
float mag=dir.y*dir.y+dir.z*dir.z;
dJointSetAMotorAxis (m_ajoint, 2, 2,0.f,-dir.z/mag,dir.y/mag);
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,1.f,0.f);
}
}
else
{
dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,0.f,1.f);
}
}
//float hi=-M_PI/2.f,lo=-hi;
//dJointSetAMotorParam(m_ajoint,dParamLoStop ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop ,hi);
//dJointSetAMotorParam(m_ajoint,dParamLoStop2 ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop2 ,hi);
//dJointSetAMotorParam(m_ajoint,dParamLoStop3 ,lo);
//dJointSetAMotorParam(m_ajoint,dParamHiStop3 ,hi);
dJointSetAMotorParam(m_ajoint,dParamFMax ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel ,0.f);
dJointSetAMotorParam(m_ajoint,dParamFMax2 ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel2 ,0.f);
dJointSetAMotorParam(m_ajoint,dParamFMax3 ,m_capture_force*0.2f);
dJointSetAMotorParam(m_ajoint,dParamVel3 ,0.f);
///////////////////////////////////
float sf=0.1f,df=10.f;
float erp=ERP(world_spring*sf,world_damping*df);
float cfm=CFM(world_spring*sf,world_damping*df);
dJointSetAMotorParam(m_ajoint,dParamStopERP ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM ,cfm);
dJointSetAMotorParam(m_ajoint,dParamStopERP2 ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM2 ,cfm);
dJointSetAMotorParam(m_ajoint,dParamStopERP3 ,erp);
dJointSetAMotorParam(m_ajoint,dParamStopCFM3 ,cfm);
/////////////////////////////////////////////////////////////////////
///dJointSetAMotorParam(m_joint1,dParamFudgeFactor ,0.1f);
//dJointSetAMotorParam(m_joint1,dParamFudgeFactor2 ,0.1f);
//dJointSetAMotorParam(m_joint1,dParamFudgeFactor3 ,0.1f);
/////////////////////////////////////////////////////////////////////////////
sf=0.1f,df=10.f;
erp=ERP(world_spring*sf,world_damping*df);
cfm=CFM(world_spring*sf,world_damping*df);
dJointSetAMotorParam(m_ajoint,dParamCFM ,cfm);
dJointSetAMotorParam(m_ajoint,dParamCFM2 ,cfm);
dJointSetAMotorParam(m_ajoint,dParamCFM3 ,cfm);
///////////////////////////
//dJointSetAMotorParam(m_ajoint,dParamLoStop ,0.f);
//dJointSetAMotorParam(m_ajoint,dParamHiStop ,0.f);
m_taget_element->set_LinearVel ( Fvector().set( 0 ,0, 0 ) );
m_taget_element->set_AngularVel( Fvector().set( 0 ,0, 0 ) );
m_taget_element->set_DynamicLimits();
//m_taget_object->PPhysicsShell()->set_JointResistance()
e_state=cstCaptured;
return;
}
m_taget_element->applyForce(dir,m_pull_force);
}
void ODESimulator::SetupContactResponse(const ODEObjectID& a,const ODEObjectID& b,int feedbackIndex,ODEContactResult& c)
{
dContact contact;
GetSurfaceParameters(a,b,contact.surface);
dBodyID b1 = dGeomGetBody(c.o1);
dBodyID b2 = dGeomGetBody(c.o2);
c.feedback.resize(c.contacts.size());
for(size_t k=0;k<c.contacts.size();k++) {
//add contact joint to joint group
contact.geom = c.contacts[k];
Assert(contact.geom.g1 == c.o1 || contact.geom.g1 == c.o2);
Assert(contact.geom.g2 == c.o1 || contact.geom.g2 == c.o2);
Assert(contact.geom.depth >= 0);
Assert(contact.geom.g1 == c.o1);
dJointID joint = dJointCreateContact(worldID,contactGroupID,&contact);
dJointSetFeedback(joint,&c.feedback[k]);
//if(b2==0)
//dJointAttach(joint,b2,b1);
//else
dJointAttach(joint,b1,b2);
}
//if contact feedback is enabled, do it!
pair<ODEObjectID,ODEObjectID> cindex;
bool reverse = false;
if(b < a) {
cindex.first = b;
cindex.second = a;
reverse = true;
}
else {
cindex.first = a;
cindex.second = b;
}
ODEContactList* cl=NULL;
if(contactList.count(cindex) != 0) {
cl=&contactList[cindex];
}
else {
//check if there's a generic robot feedback list
bool checkRobot=false;
if(cindex.first.type == 1 && cindex.first.bodyIndex != -1) {
cindex.first.bodyIndex=-1;
checkRobot=true;
}
if(cindex.second.type == 1 && cindex.second.bodyIndex != -1) {
cindex.second.bodyIndex=-1;
checkRobot=true;
}
if(checkRobot) {
if(contactList.count(cindex) != 0)
cl=&contactList[cindex];
}
}
if(cl) {
size_t start=cl->points.size();
cl->points.resize(start+c.contacts.size());
for(size_t k=0;k<c.contacts.size();k++) {
Assert(k+start < cl->points.size());
CopyVector(cl->points[k+start].x,c.contacts[k].pos);
CopyVector(cl->points[k+start].n,c.contacts[k].normal);
cl->points[k+start].kFriction = contact.surface.mu;
if(reverse)
cl->points[k+start].n.inplaceNegative();
}
Assert(feedbackIndex >= 0 && feedbackIndex < (int)gContacts.size());
cl->feedbackIndices.push_back(feedbackIndex);
}
}
void SimWorld::collideGeoms(void* data,dGeomID object_1_ID,dGeomID object_2_ID)
{
dContact contact[MAX_CONTACTS];
SimWorld* world_object = static_cast<SimWorld*>(data);
dBodyID body_1 = dGeomGetBody(object_1_ID);
dBodyID body_2 = dGeomGetBody(object_2_ID);
if (body_1 == NULL) {
body_1 = body_2;
body_2 = NULL;
dGeomID object_3_ID = object_1_ID;
object_1_ID = object_2_ID;
object_2_ID = object_3_ID;
}
// Don't use body-body collisions for now.
if (!world_object->self_collide && (body_2!=NULL)) return;
// exit without doing anything if the two bodies are connected by a joint
if (body_1 && body_2 && dAreConnectedExcluding (body_1,body_2,dJointTypeContact)) return;
// If all bodies are kinematic, we don't need to do anything here since
// adding constraints has no effect.
if ( ((body_1==NULL)||dBodyIsKinematic(body_1)) && ((body_2==NULL)||dBodyIsKinematic(body_2))) return;
// Exclude collisions between the upper arm and torso.
if (((object_1_ID==world_object->body->geometries[CapBody::UP_TORSO_BODY])||
(object_2_ID==world_object->body->geometries[CapBody::UP_TORSO_BODY])) &&
((object_1_ID==world_object->body->geometries[CapBody::RUP_ARM_BODY])||
(object_2_ID==world_object->body->geometries[CapBody::RUP_ARM_BODY])||
(object_1_ID==world_object->body->geometries[CapBody::LUP_ARM_BODY])||
(object_2_ID==world_object->body->geometries[CapBody::LUP_ARM_BODY]))) return;
contact[0].surface.mode = dContactSoftCFM | ((body_1&&body_2)?0:dContactApprox1); //Surfaces are just a little squishy
contact[0].surface.soft_cfm = (body_1&&body_2)?0.001:world_object->ground_squish; //
contact[0].surface.bounce_vel = 0.01;
contact[0].surface.bounce = 0.25;
contact[0].surface.mu = (body_1&&body_2)?1:world_object->ground_friction; // Strong friction with ground
for (int ii=1;ii<MAX_CONTACTS;++ii) {
contact[ii].surface.mode = contact[0].surface.mode;
contact[ii].surface.soft_cfm = contact[0].surface.soft_cfm;
contact[ii].surface.bounce_vel = contact[0].surface.bounce_vel;
contact[ii].surface.bounce = contact[0].surface.bounce;
contact[ii].surface.mu = contact[0].surface.mu;
}
int numC = dCollide (object_1_ID,object_2_ID,MAX_CONTACTS,&contact[0].geom,sizeof(dContact) );
for (int ii=0;ii<numC;++ii) {
dJointID cc = dJointCreateContact (world_object->world,world_object->contact_group,&contact[ii]);
dJointAttach(cc,body_1,body_2);
if ((body_2==NULL)&&(world_object->number_active_ground_contacts<MAX_GROUND_FEEDBACK)) {
dJointSetFeedback(cc,&(world_object->ground_feedback[world_object->number_active_ground_contacts]));
dOPE(world_object->contact_point[world_object->number_active_ground_contacts],=,contact[ii].geom.pos);
if ((body_1==world_object->body->body_segments[CapBody::L_HEEL_BODY])
||(body_1==world_object->body->body_segments[CapBody::L_TARSAL_BODY]))
//||(body_1==world_object->body->bodies[CapBody::L_TOE_BODY]))
{
world_object->left_foot_feedback.push_back(world_object->number_active_ground_contacts);
} else if ((body_1==world_object->body->body_segments[CapBody::R_HEEL_BODY])
||(body_1==world_object->body->body_segments[CapBody::R_TARSAL_BODY]))
//||(body_1==world_object->body->bodies[CapBody::R_TOE_BODY]))
{
world_object->right_foot_feedback.push_back(world_object->number_active_ground_contacts);
}
world_object->number_active_ground_contacts+=1;
}
}
/**
* nearCallback method that is called for every geom-pair that collides during the current simulation step. Here the collisions are resolved by creating contact joints with appropriate properties are added to the simulation and all contacts are stored to be reported to the CollisionManager.
* @param data
* @param o1 first ode geom that is involved in the contact.
* @param o2 second ode geom that is involved in the contact.
*/
void ODE_CollisionHandler::collisionCallback(void *data, dGeomID o1, dGeomID o2) {
//TODO verify if this is correct (seems to be copied)
// checks, whether one of the dGeomID-objects is actually a space instead of a geom.
if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
// colliding a space with something :
dSpaceCollide2(o1, o2, data, &ODE_SimulationAlgorithm::nearCallback);
// collide all geoms internal to the space(s)
if(dGeomIsSpace(o1)) {
dSpaceCollide((dSpaceID) o1, data, &ODE_SimulationAlgorithm::nearCallback);
}
if(dGeomIsSpace(o2)) {
dSpaceCollide((dSpaceID) o2, data, &ODE_SimulationAlgorithm::nearCallback);
}
}
else {
// find the CollisionObjects which belong to the ode-objects.
CollisionObject *first = mLookUpTable.value(o1);
CollisionObject *second = mLookUpTable.value(o2);
if(first == 0 || second == 0) {
Core::log("ODE_CollisionHandler: CollisionObject could not be defined.");
return;
}
//check for disabled Pairs
bool collisionAllowed = false;
if(first->areCollisionsDisabled() || second->areCollisionsDisabled()) {
collisionAllowed = true;
}
// else if(mAllowedCollisionPairs.contains(first)) {
// QList<CollisionObject*> partners = mAllowedCollisionPairs.value(first);
// if(partners.indexOf(second) != -1) {
// collisionAllowed = true;
// }
// }
// if(mAllowedCollisionPairs.contains(second)) {
// QList<CollisionObject*> partners = mAllowedCollisionPairs.value(second);
// if(partners.indexOf(first) != -1) {
// collisionAllowed = true;
// }
// }
else {
QList<CollisionObject*> partners = mAllowedCollisionPairs.value(first);
if(partners.contains(second)) {
collisionAllowed = true;
}
else {
QList<CollisionObject*> partners = mAllowedCollisionPairs.value(second);
if(partners.contains(first)) {
collisionAllowed = true;
}
}
}
// colliding two non-space geoms, so generate contact
// points between o1 and o2
// there are reported max: mMaxContactPointsValue->get() contact points.
dContact contact[mMaxContactPoints];
int num_contact = dCollide(o1, o2, mMaxContactPoints,
&contact[0].geom, sizeof(dContact));
// TODO: maybe introduce getElasticity, getStaticFriction Methods to SimBody
double restitution = 0.0;
double staticFriction = 0.0;
double dynamicFriction = 0.0;
int material1 = -1;
int material2 = -1;
SimBody *host1 = 0;
SimBody *host2 = 0;
ODE_Body *odeHost1 = 0;
ODE_Body *odeHost2 = 0;
// determine material properties of the two colliding objects.
if(first != 0) {
material1 = first->getMaterialType();
host1 = first->getHostBody();
odeHost1 = dynamic_cast<ODE_Body*>(host1);
}
if(second != 0) {
material2 = second->getMaterialType();
host2 = second->getHostBody();
odeHost2 = dynamic_cast<ODE_Body*>(host2);
}
//TODO check if parameters should be accessed directly instead of via strings (performance)
if(material1 < 0 || material2 < 0) {
// Determine the friction properties of the colliding SimBody objects,
// in case one of the bodies has a unknown material property value.
if(first != 0 && second != 0) {
restitution = host1->getElasticityValue()->get()
+ host2->getElasticityValue()->get();
staticFriction = host1->getStaticFrictionValue()->get()
+ host2->getStaticFrictionValue()->get();
dynamicFriction = host1->getDynamicFrictionValue()->get()
+ host2->getDynamicFrictionValue()->get();
// DoubleValue *restitutionValueOne =
// dynamic_cast<DoubleValue*>(host1->getParameter("Elasticity"));
// DoubleValue *restitutionValueTwo =
// dynamic_cast<DoubleValue*>(host2->getParameter("Elasticity"));
// if(restitutionValueOne != 0 && restitutionValueTwo != 0) {
// restitution = restitutionValueOne->get()
// * restitutionValueTwo->get();
// }
// DoubleValue *staticFValueOne =
// dynamic_cast<DoubleValue*>(host1->getParameter("StaticFriction"));
// DoubleValue *staticFValueTwo =
// dynamic_cast<DoubleValue*>(host2->getParameter("StaticFriction"));
// if(staticFValueOne != 0 && staticFValueTwo != 0) {
// staticFriction = staticFValueOne->get() + staticFValueTwo->get();
// }
// DoubleValue *dynamicFValueOne =
// dynamic_cast<DoubleValue*>(host1->getParameter("DynamicFriction"));
// DoubleValue *dynamicFValueTwo =
// dynamic_cast<DoubleValue*>(host2->getParameter("DynamicFriction"));
// if(dynamicFValueOne != 0 && dynamicFValueTwo != 0) {
// dynamicFriction = dynamicFValueOne->get() + dynamicFValueTwo->get();
// }
}
}
else {
//Determine the friction properties in case both SimBody objects have valid material types.
if(mGlobalMaterialProperties != 0) {
restitution = mGlobalMaterialProperties->getRestitution(material1, material2);
dynamicFriction = mGlobalMaterialProperties->getDynamicFriction(material1, material2);
staticFriction = mGlobalMaterialProperties->getStaticFriction(material1, material2);
}
}
dynamicFriction = dynamicFriction * mFrictionScalingFactor;
//every notified collision is saved as a contact. Enables the notification of collision rules in every case
Contact newContact(first, second);
QList<Vector3D> contactPoints;
//generate contact joints for all collision points of the two objects which collide.
for(int i = 0; i < num_contact; i++) {
// enable required ode-specific contact-definition properties.
contact[i].surface.mode = dContactBounce
| dContactSoftCFM
| dContactSoftERP
| dContactSlip1
| dContactSlip2
| dContactApprox1
| dContactMu2;
dContactGeom contactGeom = contact[i].geom;
// add contact point to list of contact points.
contactPoints.push_back(Vector3D(contactGeom.pos[0], contactGeom.pos[1], contactGeom.pos[2]));
// If the two collision objects are allowed to collide, no contact joint is created.
if(collisionAllowed) {
break;
}
// set the friction coefficient depending on the velocity in the friction directions
// (TODO: needs to be confirmed)
if(Math::compareDoubles(contact[i].surface.motion1, 0.0, mStaticFrictionAccuracy) == true) {
contact[i].surface.mu = staticFriction;
}
else {
contact[i].surface.mu = dynamicFriction;
}
if(Math::compareDoubles(contact[i].surface.motion2, 0.0, mStaticFrictionAccuracy) == true) {
contact[i].surface.mu2 = staticFriction;
}
else {
contact[i].surface.mu2 = dynamicFriction;
}
contact[i].surface.bounce = restitution;
contact[i].surface.slip1 = mContactSlip;
contact[i].surface.slip2 = mContactSlip;
contact[i].surface.bounce_vel = mBouncingVelocity;
contact[i].surface.soft_cfm = mSoftCFM;
contact[i].surface.soft_erp = mSoftERP;
if(fabs(contact[i].geom.depth) < 0.000001 ) {
continue;
}
dJointID contactJoint = dJointCreateContact(mAlgorithm->getODEWorldID(),
mAlgorithm->getContactJointGroupID(), &contact[i]);
dJointAttach(contactJoint, dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
//add feedback structure if needed.
dJointFeedback *feedbackStructure = 0;
if(odeHost1 != 0 && odeHost1->isFeedbackEnabled()) {
feedbackStructure = new dJointFeedback();
dJointSetFeedback(contactJoint, feedbackStructure);
odeHost1->getOwnedJointFeedbackList().append(feedbackStructure);
odeHost1->getJointFeedbackList().insertMulti(contactGeom.g1, feedbackStructure);
odeHost1->getJointFeedbackList().insertMulti(contactGeom.g2, feedbackStructure);
}
if(odeHost2 != 0 && odeHost2->isFeedbackEnabled()) {
feedbackStructure = new dJointFeedback();
dJointSetFeedback(contactJoint, feedbackStructure);
odeHost2->getOwnedJointFeedbackList().append(feedbackStructure);
odeHost2->getJointFeedbackList().insertMulti(contactGeom.g1, feedbackStructure);
odeHost2->getJointFeedbackList().insertMulti(contactGeom.g2, feedbackStructure);
}
}
// add the contact points to the contact and add the contact to the list of occured contacts,
// if for both CollisionObjects reporting collisions is enabled (default).
if(num_contact != 0) {
if(first->areCollisionsReported() || second->areCollisionsReported()) {
newContact.setContactPoints(contactPoints);
mCurrentContacts.push_back(newContact);
}
}
}
}
void PhysicsJoint::setFeedback( dJointFeedback* feed)
{
dJointSetFeedback(_JointID, feed);
}
