//description
//Default behavior.
//Default collision of bodies
void ODEBaseScene::Collide(dGeomID g1, dGeomID g2)
{
int n;
dBodyID b1 = dGeomGetBody(g1);
dBodyID b2 = dGeomGetBody(g2);
if (b1 && b2 && dAreConnected(b1, b2))
return;
const int N = 4;
dContact contact[N];
n = dCollide (g1,g2,N,&contact[0].geom,sizeof(dContact));
if (n > 0)
{
// printf("Body %d hits body %d.\n", (size_t) dGeomGetData(g1),(size_t) dGeomGetData(g2));
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 (m_domain->getWorld(), m_domain->getContactGroup(), contact+i);
dJointAttach(j, b1, b2);
}
}
}
void nearCallback(void *data, dGeomID o1, dGeomID o2) {
State* state = (State*)data;
if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
dSpaceCollide2(o1, o2, data, &nearCallback);
if(dGeomIsSpace(o1))
dSpaceCollide((dSpaceID)o1, data, &nearCallback);
if(dGeomIsSpace(o2))
dSpaceCollide((dSpaceID)o2, data, &nearCallback);
} else {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
const int MAX_CONTACTS = 18;
dContact contact[MAX_CONTACTS];
for(int i = 0; i < MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = 2000;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.15;
}
if(int numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) {
for(int i = 0; i < numc; i++) {
dJointID c = dJointCreateContact(state->world, state->physicsContactgroup, &contact[i]);
dJointAttach(c, b1, b2);
}
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i,n;
// only collide things with the ground
int g1 = (o1 == ground || o1 == ground_box);
int g2 = (o2 == ground || o2 == ground_box);
if (!(g1 ^ g2)) return;
const int N = 10;
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;
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = 0.1;
contact[i].surface.slip2 = 0.1;
contact[i].surface.soft_erp = 0.5;
contact[i].surface.soft_cfm = 0.3;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
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);
}
}
}
//collision detection
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i,n;
dBodyID b1 = dGeomGetBody (o1);
dBodyID b2 = dGeomGetBody (o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return;
const int N = 10;
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);
contact[i].surface.mu = 0.1;
contact[i].surface.slip1 = 0.02;
contact[i].surface.slip2 = 0.02;
contact[i].surface.soft_erp = 0.1;
contact[i].surface.soft_cfm = 0.0001;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
}
}
}
void CDynamics3DEngine::ManageCloseGeomsAddingContactJoints(SGeomCheckData* ps_data, dGeomID t_geom1, dGeomID t_geom2) {
/*
* The passed geometries can be spaces or shapes.
* Depending on the type of each passed geometry, we do something different
*/
if(dGeomIsSpace(t_geom1) || dGeomIsSpace(t_geom2)) {
/*
* At least one of the two geometries is a space.
* We need to open up the spaces to get to the basic elements.
*/
dSpaceCollide2(t_geom1, t_geom2, ps_data, &ManageCloseGeomsAddingContactJointsCallback);
}
else {
/* Both geoms are shapes. Let's check for collisions among them */
size_t unContacts = dCollide(t_geom1, t_geom2, m_unMaxContacts, &m_ptContacts->geom, sizeof(dContact));
/* If no collision is detected, we're done */
if(unContacts == 0) return;
/* Otherwise, let's add contact joints in each contact point detected */
/* Get the body ids */
dBodyID tBody1 = dGeomGetBody(t_geom1);
dBodyID tBody2 = dGeomGetBody(t_geom2);
/* Create a buffer for the contact joints */
dJointID tContactJoint;
/* Go through the contact points and create the joints */
for(UInt32 i = 0; i < unContacts; ++i) {
tContactJoint = dJointCreateContact(m_tWorldID, m_tContactJointGroupID, m_ptContacts+i);
dJointAttach(tContactJoint, tBody1, tBody2);
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", o1,o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = 100.0;
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.02;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
void ODEEngine::near_callback(dGeomID o1, dGeomID o2) {
ODECollidable* c1 = (ODECollidable*) dGeomGetData(o1);
ODECollidable* c2 = (ODECollidable*) dGeomGetData(o2);
dContact contact[MAX_CONTACTS];
int collision_count = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
for(int32_t i = 0; i < collision_count; ++i) {
//Calculate combined surface values to build contact joints
contact[i].surface.mode = dContactBounce;
contact[i].surface.bounce = c1->bounciness() * c2->bounciness();
if(c1->has_infinite_friction() || c2->has_infinite_friction()) {
contact[i].surface.mu = dInfinity;
} else {
contact[i].surface.mu = c1->friction() * c2->friction();
}
dBodyID body1 = dGeomGetBody(contact[i].geom.g1);
dBodyID body2 = dGeomGetBody(contact[i].geom.g2);
if(body1 == body2) {
//Don't collide the same body against itself
continue;
}
if(i == 0) {
//Fire the collision signal on the first loop only
c1->signal_collided_(*c2);
c2->signal_collided_(*c1);
//Fire any combined signals
fire_collision_signals_for(*c1, *c2);
}
//Check to see if we should create response joints, or just ignore the collision
if(c1->is_ghost() || c2->is_ghost()) {
//Don't respond if we're not supposed to (e.g. we're a hit zone or something)
continue;
}
if(c1->should_respond_callback_ && !c1->should_respond_callback_(*c2)) {
continue;
}
if(c2->should_respond_callback_ && !c2->should_respond_callback_(*c1)) {
continue;
}
dJointID c = dJointCreateContact(world(), contact_group_, &contact[i]);
dJointAttach(c, body1, body2);
}
}
//------------------------------------------------------------------------------
void OdeSimulator::addContactJoint(const dContact & contact,
const OdeRigidBody * body1,
const OdeRigidBody * body2)
{
dJointID joint_id = dJointCreateContact(world_id_, contact_group_id_, &contact);
dJointAttach(joint_id,
body1 ? body1->getId() : NULL,
body2 ? body2->getId() : NULL);
}
// main collision handling function
void PhysicsServer::checkCollision(void *data, dGeomID o1, dGeomID o2)
{
int i,n;
if( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) )
{
dSpaceCollide2( o1, o2, data, &collisionCallback );
}
else
{
int mode = 0;
double slip1 = 0;
double slip2 = 0;
// get bodies
dBodyID body1 = dGeomGetBody(o1);
dBodyID body2 = dGeomGetBody(o2);
// get geom data
PhysGeomData* pData1 = (PhysGeomData*)dGeomGetData(o1);
PhysGeomData* pData2 = (PhysGeomData*)dGeomGetData(o2);
// set contact params according to geom data
if (pData1 != NULL)
slip1 = pData1->slip;
if (pData2 != NULL)
slip2 = pData2->slip;
// set mode
if (slip1 != 0)
mode |= dContactSlip1;
if (slip2 != 0)
mode |= dContactSlip2;
static const int N = 8; // max number of contact points
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 = mode; /*dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM;*/
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = slip1;
contact[i].surface.slip2 = slip2;
// contact[i].surface.soft_erp = 0.7;
// contact[i].surface.soft_cfm = 0.1;
dJointID c = dJointCreateContact (_world, _contactgroup, &contact[i]);
dJointAttach (c, body1, body2);
}
}
}
}
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact;
contact.surface.mode = 0;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world,contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}
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);
}
}
static void collideCB(void *data, dGeomID o1, dGeomID o2)
{
ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1);
ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2);
// const int N = 32;
const int N = 10; // TODO: Magic number should be removed
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];
c->surface.mode = dContactSlip1 | dContactSlip2 | 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 (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.mu = SPARTS_MU1;
c->surface.mu2 = SPARTS_MU2;
c->surface.slip1 = SPARTS_SLIP1;
c->surface.slip2 = SPARTS_SLIP2;
c->surface.soft_erp = SPARTS_ERP;
c->surface.soft_cfm = SPARTS_CFM;
c->surface.bounce = SPARTS_BOUNCE;
}
dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c);
dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2));
}
}
}
//All pairs of geoms that are potentially intersecting will be passed by dSpaceCollide to this function.
//Here we can determine which objects are actually colliding by making a call to dCollide and change the
//behavior of the joints before adding them to the joint group. The first parameter would be the user data
//pointer passed to dSpaceCollide if we had provided it. The second and third parameters are the two potentially intersecting geoms.
void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
//Temporary index for each contact
int i;
int numc = 0;
//Get the dynamics body for each geom
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
//Create an array of dContact objects to hold the contact joints
dContact contact[MAX_CONTACTS];
// Now we set the joint properties of each contact. Going into the full details here would require a tutorial of its
// own. I'll just say that the members of the dContact structure control the joint behaviour, such as friction,
// velocity and bounciness. See section 7.3.7 of the ODE manual and have fun experimenting to learn more.
for (i = 0; i < MAX_CONTACTS; i++)
{
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 5000;
contact[i].surface.bounce = 0.98;
contact[i].surface.bounce_vel = 0.5;
contact[i].surface.soft_cfm = 0.01;
}
// Here we do the actual collision test by calling dCollide. It returns the number of actual contact points or zero
// if there were none. As well as the geom IDs, max number of contacts we also pass the address of a dContactGeom
// as the fourth parameter. dContactGeom is a substructure of a dContact object so we simply pass the address of
// the first dContactGeom from our array of dContact objects and then pass the offset to the next dContactGeom
// as the fifth paramater, which is the size of a dContact structure. That made sense didn't it?
numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
if (numc > 0)
{
// To add each contact point found to our joint group we call dJointCreateContact which is just one of the many
// different joint types available.
for (i = 0; i < numc; i++)
{
//dJointCreateContact needs to know which world and joint group to work with as well as the dContact
//object itself. It returns a new dJointID which we then use with dJointAttach to finally create the
//temporary contact joint between the two geom bodies.
dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
dJointAttach(c, b1, b2);
}
}
}
void StaticEnvironment ( bool& do_colide, bool bo1, dContact& c, SGameMtl* material_1, SGameMtl* material_2 )
{
dJointID contact_joint = dJointCreateContact(0, ContactGroup, &c);
if(bo1)
{
((CPHActivationShape*)(retrieveGeomUserData(c.geom.g1)->callback_data))->DActiveIsland()->ConnectJoint(contact_joint);
dJointAttach (contact_joint, dGeomGetBody(c.geom.g1), 0);
}
else
{
((CPHActivationShape*)(retrieveGeomUserData(c.geom.g2)->callback_data))->DActiveIsland()->ConnectJoint(contact_joint);
dJointAttach (contact_joint, 0, dGeomGetBody(c.geom.g2));
}
do_colide=false;
}
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
const int N = 10;
dContact contact[N];
int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
for (int i = 0; i < n; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = 0.0;
contact[i].surface.bounce = 0.7;
contact[i].surface.bounce_vel = 0.01;
dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
dJointAttach(c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
// 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;
// @@@ it's still more convenient to use the C interface here.
dContact contact;
contact.surface.mode = 0;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
dJointAttach (c,b1,b2);
}
}
static void nearCallback (void *, 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 nearCallback(void *data, dGeomID o1, dGeomID o2)
{
static const int N = 10; // 接触点数の最大値
dContact contact[N]; // 接触点
// 衝突情報の生成 nは衝突点数
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
for (int i = 0; i < n; i++){
contact[i].surface.mode = dContactBounce; // 接触面の反発性を設定
// ボールとバーが接触したら反発係数を1.5に設定
if(((ball.geom == o1) && (bar.geom == o2)) || ((ball.geom == o2) && (bar.geom == o1))){
contact[i].surface.bounce = 1.2; // 反発係数(実際の世界では0.0から1.0)
}
// ボールと床が接触したら反発係数を0.5に設定
else if (((ball.geom == o1) && (field.geom == o2)) || ((ball.geom == o2) && (field.geom == o1))){
contact[i].surface.bounce = 0.5;
}
// ボールとブロック１が接触したら。block1_hitを１に設定
else if (((ball.geom == o1) && (block1.geom == o2)) || ((ball.geom == o2) && (block1.geom == o1))){
block1_hit = 1;
}
// ボールとブロック2が接触したら。block2_hitを１に設定
else if (((ball.geom == o1) && (block2.geom == o2)) || ((ball.geom == o2) && (block2.geom == o1))){
block2_hit = 1;
}
// ボールとブロック3が接触したら。block2_hitを１に設定
else if (((ball.geom == o1) && (block3.geom == o2)) || ((ball.geom == o2) && (block3.geom == o1))){
block3_hit = 1;
}
// その他は反発係数を0.8に設定
else{
contact[i].surface.bounce = 0.8;
}
//contact[i].surface.bounce_vel = 0.05; // 反発に必要な最低速度
contact[i].surface.mu = 0.0;// dInfinity; // 摩擦係数
// 接触ジョイントの生成
dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
// 接触している２つの剛体を接触ジョイントにより拘束
dJointAttach(c,dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
/* exit without doing anything if the two bodies are connected by a joint */
dBodyID b1,b2;
dContact contact;
b1 = dGeomGetBody(o1);
b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
contact.surface.mode = 0;
contact.surface.mu = 0.1;
contact.surface.mu2 = 0;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world,contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = 0;
contact[i].surface.mu = 50.0; // was: dInfinity
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
if (show_contacts)
{
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.12,0.12,0.12};
dsSetColorAlpha (0,0,1,0.5);
dsDrawBox (contact[i].geom.pos,RI,ss);
dReal *pos = contact[i].geom.pos;
dReal depth = contact[i].geom.depth;
dReal *norm = contact[i].geom.normal;
dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]};
dsSetColorAlpha (1,1,1,1);
dsDrawLine (contact[i].geom.pos, endp);
}
}
}
}
void odeCollisionCallback(void *data, dGeomID o1, dGeomID o2) {
RobogenConfig *config = static_cast<RobogenConfig*>(data);
// 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.slip1 = 0.01;
contact[i].surface.slip2 = 0.01;
contact[i].surface.mode = dContactSoftERP |
dContactSoftCFM |
dContactApprox1 |
dContactSlip1 | dContactSlip2;
contact[i].surface.mu = config->getTerrainConfig()->getFriction();
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.01;
}
int collisionCounts = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom,
sizeof(dContact));
if (collisionCounts != 0) {
for (int i = 0; i < collisionCounts; i++) {
dJointID c = dJointCreateContact(odeWorld, odeContactGroup,
contact + i);
dJointAttach(c, b1, b2);
}
}
}
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1,b2;
dBodyID test;
assert(o1);
assert(o2);
b1 = dGeomGetBody(o1);
b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
if (o1 == floorplane || o2 == floorplane)
{
if (o1==floorplane)
test=b2;
if (o2==floorplane)
test=b1;
//test should equal the body that is colliding with floor
for (int x=0; x<creatures.size(); x++)
{
int bsize=creatures[x]->bodies.size();
for (int y=0; y<bsize; y++)
if (test==creatures[x]->bodies[y])
creatures[x]->onground[y]=true;
}
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = 0;
contact[i].surface.mu = dInfinity; //50.0; // was: dInfinity
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
}
}
}
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact;
contact.surface.mode = dContactBounce | dContactSoftCFM;
// friction parameter
contact.surface.mu = dInfinity;
// bounce is the amount of "bouncyness".
contact.surface.bounce = 1.0;
// bounce_vel is the minimum incoming velocity to cause a bounce
contact.surface.bounce_vel = 0.1; //0.1
// constraint force mixing parameter
contact.surface.soft_cfm = 0.001;
if (int numc = dCollide (o1,o2,1,&contact.geom,sizeof(dContact))) {
dJointID c = dJointCreateContact (world,contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}
void Resolve_Contact(dGeomID o1, dGeomID o2) {
int i,n;
const int N = 10;
// const int N = 1;
// N is the flags field in dCollide (collision_kernel.cpp)
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
// Two objects that shouldn't interpenetrate have
// collided with one another.
if (n > 0) {
Set_Touch_Sensor(o1);
Set_Touch_Sensor(o2);
for (i=0; i<n; i++) {
contact[i].surface.slip1 = 0.01;
contact[i].surface.slip2 = 0.01;
contact[i].surface.mode = dContactSoftERP |
dContactSoftCFM |
dContactApprox1 |
dContactSlip1 | dContactSlip2;
contact[i].surface.mu = 50.0;
double kp = 100.0/STEP_SIZE;
double kd = 0.0;
contact[i].surface.soft_erp = STEP_SIZE*kp/
(STEP_SIZE*kp+kd);
contact[i].surface.soft_cfm = 1.0/(STEP_SIZE*kp+kd);
dJointID c = dJointCreateContact (world,
contactgroup,
&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
const int MAX_CONTACTS = 8;
dContact contact[MAX_CONTACTS];
int numc = dCollide (o1, o2, MAX_CONTACTS,
&contact[0].geom,
sizeof(dContact));
for (int i=0; i<numc; i++) {
contact[i].surface.mode = dContactApprox1;
contact[i].surface.mu = 5;
dJointID c = dJointCreateContact (world, contactgroup, contact+i);
dJointAttach (c, b1, b2);
}
}
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
const int N = 10;
dContact contact[N];
int isGround = ((ground == o1) || (ground == o2));
int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
if (isGround) {
for (int i = 0; i < n; i++) {
contact[i].surface.mu = dInfinity;
contact[i].surface.mode = dContactBounce;
contact[i].surface.bounce = 1.0; // (0.0~1.0)
contact[i].surface.bounce_vel = 0.01;
dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
dJointAttach(c, dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", o1,o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
// Paranoia <-- not working for some people, temporarily removed for 0.6
//dIASSERT(dVALIDVEC3(contact[i].geom.pos));
//dIASSERT(dVALIDVEC3(contact[i].geom.normal));
//dIASSERT(!dIsNan(contact[i].geom.depth));
contact[i].surface.slip1 = 0.7;
contact[i].surface.slip2 = 0.7;
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
contact[i].surface.mu = 50.0; // was: dInfinity
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.04;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
void PhysicsSim::ode_nearCallback (void *data, dGeomID o1, dGeomID o2)
{
PhysicsSim *me = static_cast<PhysicsSim*>(data);
int i;
// 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)))
{
OscObject *p1 = static_cast<OscObject*>(dGeomGetData(o1));
OscObject *p2 = static_cast<OscObject*>(dGeomGetData(o2));
if (p1 && p2) {
bool co1 = p1->collidedWith(p2, me->m_counter);
bool co2 = p2->collidedWith(p1, me->m_counter);
if ( (co1 || co2) && me->m_collide.m_value ) {
lo_send(address_send, "/world/collide", "ssf",
p1->c_name(), p2->c_name(),
(double)(p1->m_velocity - p2->m_velocity).length());
}
// TODO: this strategy will NOT work for multiple collisions between same objects!!
}
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (me->m_odeWorld, me->m_odeContactGroup, contact+i);
dJointAttach (c,b1,b2);
}
}
}
void PWorld::handleCollisions(dGeomID o1, dGeomID o2)
{
PSurface* sur;
int j=sur_matrix[*((int*)(dGeomGetData(o1)))][*((int*)(dGeomGetData(o2)))];
if (j!=-1)
{
const int N = 10;
dContact contact[N];
int n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
sur = surfaces[j];
sur->contactPos [0] = contact[0].geom.pos[0];
sur->contactPos [1] = contact[0].geom.pos[1];
sur->contactPos [2] = contact[0].geom.pos[2];
sur->contactNormal[0] = contact[0].geom.normal[0];
sur->contactNormal[1] = contact[0].geom.normal[1];
sur->contactNormal[2] = contact[0].geom.normal[2];
bool flag=true;
if (sur->callback!=NULL) flag = sur->callback(o1,o2,sur);
if (flag)
for (int i=0; i<n; i++) {
contact[i].surface = sur->surface;
if (sur->usefdir1)
{
contact[i].fdir1[0] = sur->fdir1[0];
contact[i].fdir1[1] = sur->fdir1[1];
contact[i].fdir1[2] = sur->fdir1[2];
contact[i].fdir1[3] = sur->fdir1[3];
}
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
}
