osaODEServoMotor::osaODEServoMotor(dWorldID world,
dBodyID body1,
dBodyID body2,
const vctFixedSizeVector<double,3>& axis,
double vwmax,
double ftmax,
dJointType motortype ) :
vwmax( fabs( vwmax ) ),
ftmax( fabs( ftmax ) ) {
if( motortype == dJointTypeHinge ){
motorid = dJointCreateAMotor( world, 0 ); // create the motor
dJointAttach( MotorID(), body1, body2 ); // attach the joint
dJointSetAMotorMode( MotorID(), dAMotorUser ); // motor is in user mode
dJointSetAMotorNumAxes( MotorID(), 1 ); // only 1 axis
dJointSetAMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );
SetVelocity( 0.0 ); // idle the motor
}
if( motortype == dJointTypeSlider ){
motorid = dJointCreateLMotor( world, 0 ); // create the motor
dJointAttach( MotorID(), body1, body2 ); // attach the joint
dJointSetLMotorNumAxes( MotorID(), 1 ); // 1 axis
dJointSetLMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );
SetVelocity( 0.0 ); // idle the motor
}
}
void ComponentMovement::createMotorJoints() {
ASSERT(physicsEngine, "Null pointer: physicsEngine");
dBodyID body = getBodyID();
ASSERT(body, "Cannot create motor joints: No physics body available");
amotor = dJointCreateAMotor(physicsEngine->getWorld(), 0);
dJointAttach(amotor, body, 0);
dJointSetAMotorNumAxes(amotor, 3);
dJointSetAMotorAxis (amotor, 0, 1, 1, 0, 0);
dJointSetAMotorAxis (amotor, 1, 1, 0, 1, 0);
dJointSetAMotorAxis (amotor, 2, 1, 0, 0, 1);
dJointSetAMotorParam(amotor, dParamFMax, maxForce);
dJointSetAMotorParam(amotor, dParamFMax2, maxForce);
dJointSetAMotorParam(amotor, dParamFMax3, maxForce);
dJointSetAMotorParam(amotor, dParamVel, 0);
dJointSetAMotorParam(amotor, dParamVel2, 0);
dJointSetAMotorParam(amotor, dParamVel3, 0);
lmotor = dJointCreateLMotor(physicsEngine->getWorld(), 0);
dJointAttach(lmotor, body, 0);
dJointSetLMotorNumAxes(lmotor, 2);
dJointSetLMotorAxis (lmotor, 0, 1, 1, 0, 0);
dJointSetLMotorAxis (lmotor, 1, 1, 0, 1, 0);
dJointSetLMotorParam(lmotor, dParamFMax, maxForce);
dJointSetLMotorParam(lmotor, dParamFMax2, maxForce);
dJointSetLMotorParam(lmotor, dParamVel, 0);
dJointSetLMotorParam(lmotor, dParamVel2, 0);
}
// ang2 = position angle
// ang = rotation angle
Robot::Wheel::Wheel(Robot* robot,int _id,float ang,float ang2,int wheeltexid)
{
id = _id;
rob = robot;
float rad = rob->cfg->robotSettings.RobotRadius - rob->cfg->robotSettings.WheelThickness / 2.0;
ang *= M_PI/180.0f;
ang2 *= M_PI/180.0f;
float x = rob->m_x;
float y = rob->m_y;
float z = rob->m_z;
float centerx = x+rad*cos(ang2);
float centery = y+rad*sin(ang2);
float centerz = z-rob->cfg->robotSettings.RobotHeight*0.5+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight;
cyl = new PCylinder(centerx,centery,centerz,rob->cfg->robotSettings.WheelRadius,rob->cfg->robotSettings.WheelThickness,rob->cfg->robotSettings.WheelMass,0.9,0.9,0.9,wheeltexid);
cyl->setRotation(-sin(ang),cos(ang),0,M_PI*0.5);
cyl->setBodyRotation(-sin(ang),cos(ang),0,M_PI*0.5,true); //set local rotation matrix
cyl->setBodyPosition(centerx-x,centery-y,centerz-z,true); //set local position vector
cyl->space = rob->space;
rob->w->addObject(cyl);
joint = dJointCreateHinge (rob->w->world,0);
dJointAttach (joint,rob->chassis->body,cyl->body);
const dReal *a = dBodyGetPosition (cyl->body);
dJointSetHingeAxis (joint,cos(ang),sin(ang),0);
dJointSetHingeAnchor (joint,a[0],a[1],a[2]);
motor = dJointCreateAMotor(rob->w->world,0);
dJointAttach(motor,rob->chassis->body,cyl->body);
dJointSetAMotorNumAxes(motor,1);
dJointSetAMotorAxis(motor,0,1,cos(ang),sin(ang),0);
dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax);
speed = 0;
}
void TrackedVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
//dMassAdjust(&this->vehicleMass, 2.40);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);
this->leftTrack->create();
this->rightTrack->create();
dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
dRigidBodyArraySetPosition(leftTrack->bodyArray, -wheelBase/2, -(w - trackWidth)/2, 0);
dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2, (w - trackWidth)/2, 0);
this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
dJointSetFixed(this->leftTrackJoint);
dJointSetFixed(this->rightTrackJoint);
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}
Rope::Rope(Object* obja, Body* bodya, Object* objb, Body* bodyb)
: obja_(obja), objb_(objb), selected_(false)
{
vec apos = bodya->position();
proxya_.set_position(apos);
proxya_.set_velocity(bodya->velocity());
proxya_.set_mass(0.01, 1);
hingea_ = dJointCreateHinge(LEVEL->world, 0);
dJointAttach(hingea_, proxya_.body_id(), bodya->body_id());
dJointSetHingeAxis(hingea_, 0, 0, 1);
vec bpos = bodyb->position();
proxyb_.set_position(bpos);
proxyb_.set_velocity(bodyb->velocity());
proxyb_.set_mass(0.01, 1);
hingeb_ = dJointCreateHinge(LEVEL->world, 0);
dJointAttach(hingeb_, proxyb_.body_id(), bodyb->body_id());
dJointSetHingeAxis(hingeb_, 0, 0, 1);
rope_ = dJointCreateSlider(LEVEL->world, 0);
vec axis = bpos - apos;
dJointAttach(rope_, proxya_.body_id(), proxyb_.body_id());
dJointSetSliderAxis(rope_, axis.x, axis.y, 0);
dJointSetSliderParam(rope_, dParamLoStop, 0);
dJointSetSliderParam(rope_, dParamStopCFM, 0.25);
dJointSetSliderParam(rope_, dParamStopERP, 0.01);
ext_ = base_ext_ = axis.norm();
}
/*** コールバック関数 ***/
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
dVector3 tmp_fdir = {0, 0, 0, 0};
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;
int wheel_flag = 0;
for (int j = 0; j < WHEEL_NUM; j++)
{
if ((o1 == wheel[j].geom)||(o2 == wheel[j].geom))
{
wheel_flag = 1;
dJointGetHingeAxis(wheel[j].joint,tmp_fdir);
break;
}
}
static const int N = 10;
dContact contact[N];
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0)
{
if (wheel_flag == 1)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactFDir1| dContactMu2 | dContactSoftERP | dContactSoftCFM;
contact[i].fdir1[0] = tmp_fdir[0]; // 第１摩擦方向の設定(x軸成分)
contact[i].fdir1[1] = tmp_fdir[1]; // 第１摩擦方向の設定(y軸成分)
contact[i].fdir1[2] = tmp_fdir[2]; // 第１摩擦方向の設定(z軸成分)
contact[i].surface.mu = 0.1; // 車軸方向の摩擦係数
contact[i].surface.mu2 = dInfinity; // 車輪方向の摩擦係数
contact[i].surface.soft_erp = 0.9;
contact[i].surface.soft_cfm = 0.001;
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach(c,b1,b2);
}
}
else
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 1e-5;
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach(c,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));
}
}
}
}
void createInvisibleHead( float* pos )
{
dMatrix3 head_orientation;
dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);
//position and orientation
head.Body = dBodyCreate(World);
dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
dBodySetRotation(head.Body, head_orientation);
dBodySetLinearVel(head.Body, 0, 0, 0);
dBodySetData(head.Body, (void *)0);
//mass
dMass head_mass;
dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
dBodySetMass(head.Body, &head_mass);
//geometry
head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
dGeomSetBody(head.Geom, head.Body);
//fixed joint
invis_box_joint = dJointCreateFixed(World, jointgroup);
dJointAttach(invis_box_joint, body.Body, head.Body);
dJointSetFixed(invis_box_joint);
}
// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
void Robots::construirChassi(dWorldID world)
{
for (int i=0; i < 2; i++)
{
// Cria objeto e geometria
this->body[i] = dBodyCreate(world);
this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);
// Define a posição do objeto
dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);
// Se o robô for do segundo time, deve ser rotacionado em 180 graus
if ((this->id == 3) || (this->id == 4) || (this->id == 5))
{
dQuaternion q;
dQFromAxisAndAngle(q,0,0,1,M_PI);
dBodySetQuaternion(this->body[i],q);
}
// Define a massa do objeto
dMass m;
dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT); // O segundo bloco é mais curto
dMassAdjust(&m,CMASS*(1+i*2)); // O segundo bloco é mais pesado
dBodySetMass(this->body[i],&m);
// Associa o objeto à sua geometria
dGeomSetBody(this->box[i],this->body[i]);
}
// O chassis é composto por dois blocos que são fixos entre si
dJointID fixed = dJointCreateFixed(world,0);
dJointAttach(fixed,this->body[1],this->body[0]);
dJointSetFixed(fixed);
}
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 Spikey::stick(Body* b) {
if (hinge_) dJointDestroy(hinge_);
hinge_ = dJointCreateFixed(LEVEL->world, 0);
dJointAttach(hinge_, body_.body_id(), b ? b->body_id() : 0);
dJointSetFixed(hinge_);
state_ = STUCK;
}
void PhysicsWorld::collide(void *_data, dGeomID _o1, dGeomID _o2)
{
int i;
//std::cout<<"collide\n";
// 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;
for (i=0; i<m_maxContacts; i++)
{
m_contact[i].surface.mode = dContactBounce;// | dContactSoftCFM;
m_contact[i].surface.mu = dInfinity;
m_contact[i].surface.mu2 = 0;
m_contact[i].surface.bounce = 0.3;
m_contact[i].surface.bounce_vel = 0.1;
m_contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide (_o1,_o2,m_maxContacts,&m_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 (m_world,m_contactgroup,m_contact+i);
dJointAttach (c,b1,b2);
}
}
}
Robot::Kicker::Kicker(Robot* robot)
{
rob = robot;
float x = rob->m_x;
float y = rob->m_y;
float z = rob->m_z;
float centerx = x+(rob->cfg->robotSettings.RobotCenterFromKicker+rob->cfg->robotSettings.KickerThickness);
float centery = y;
float centerz = z-(rob->cfg->robotSettings.RobotHeight)*0.5f+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight+rob->cfg->robotSettings.KickerZ;
box = new PBox(centerx,centery,centerz,rob->cfg->robotSettings.KickerThickness,rob->cfg->robotSettings.KickerWidth,rob->cfg->robotSettings.KickerHeight,rob->cfg->robotSettings.KickerMass,0.9,0.9,0.9);
box->setBodyPosition(centerx-x,centery-y,centerz-z,true);
box->space = rob->space;
rob->w->addObject(box);
joint = dJointCreateHinge (rob->w->world,0);
dJointAttach (joint,rob->chassis->body,box->body);
const dReal *aa = dBodyGetPosition (box->body);
dJointSetHingeAnchor (joint,aa[0],aa[1],aa[2]);
dJointSetHingeAxis (joint,0,-1,0);
dJointSetHingeParam (joint,dParamVel,0);
dJointSetHingeParam (joint,dParamLoStop,0);
dJointSetHingeParam (joint,dParamHiStop,0);
rolling = 0;
kicking = false;
}
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);
}
}
}
}
//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 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;
}
void WorldPhysics::handleCollisionBetween(dGeomID o0, dGeomID o1) {
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;
// for(int i=0;i<4;i++) {
// if (((o0==bulldozer->geom) && (o1==wheels[i]->geom)) ||
// ((o0==wheels[i]->geom) && (o1==bulldozer->geom))) return;
// }
const int N = 100;
dContact contact[N];
n = dCollide (o0,o1,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 = 1.7;
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 inspectJoints(void)
{
const dReal forcelimit = 2000.0;
int i;
for (i=0; i<SEGMCNT-1; i++)
{
if (dJointGetBody(hinges[i], 0))
{
// This joint has not snapped already... inspect it.
dReal l0 = dCalcVectorLength3(jfeedbacks[i].f1);
dReal l1 = dCalcVectorLength3(jfeedbacks[i].f2);
colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit;
colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit;
if (l0 > forcelimit || l1 > forcelimit)
stress[i]++;
else
stress[i]=0;
if (stress[i]>4)
{
// Low-pass filter the noisy feedback data.
// Only after 4 consecutive timesteps with excessive load, snap.
fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i);
dJointAttach (hinges[i], 0, 0);
}
}
}
}
ODEConstraint::ODEConstraint(OscConstraint *c, dJointID odeJoint,
dWorldID odeWorld, dSpaceID odeSpace,
OscObject *object1, OscObject *object2)
{
m_constraint = c;
m_odeWorld = odeWorld;
m_odeSpace = odeSpace;
m_odeBody1 = 0;
m_odeBody2 = 0;
m_odeJoint = odeJoint;
ODEObject *o = NULL;
if (object1)
o = dynamic_cast<ODEObject*>(object1->special());
if (o)
m_odeBody1 = o->m_odeBody;
if (object2) {
o = NULL;
if (object2)
o = dynamic_cast<ODEObject*>(object2->special());
if (o)
m_odeBody2 = o->m_odeBody;
}
else {
printf("constraint created with bodies %#x and world.\n", m_odeBody1);
}
if (m_odeJoint)
dJointAttach(m_odeJoint, m_odeBody1, m_odeBody2);
printf("constraint created with bodies %#x and %#x.\n", m_odeBody1, m_odeBody2);
}
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));
}
}
}
dJointID HingeJoint::createJoint(dBodyID b1, dBodyID b2)
{
dJointID j = dJointCreateHinge(m_world, 0);
dJointAttach(j, b1, b2);
return j;
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
// for drawing the contact points
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
int i;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact[MAX_CONTACTS];
int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact));
for (i=0; i<numc; i++) {
contact[i].surface.mode = dContactApprox1;
contact[i].surface.mu = 2;
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) );
}
}
}
static void nearCallback(void *data, dGeomID o1, dGeomID o2) //o1,o2: geometries likely to collide
{
// inner collision detecting
innerCollision = innerCollision || (o1 != ground && o2 != ground);
const int N = 30;
dContact contact[N];
// Don’t do 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;
int isGround = ((ground == o1) || (ground == o2)); //Collision with ground
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact)); //n: Number of collision points
if (isGround) {//If there is a collision with the ground
for (int i = 0; i < n; i++) {
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM;
contact[i].surface.mu = dInfinity; //Coulomb friction coef
contact[i].surface.soft_erp = 0.2; // 1.0 ideal
contact[i].surface.soft_cfm = 1e-4; // 0.0 ideal
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,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));
}
}
}
/**
* This method is used for activating or deactivating
* the Joint. If the Joint was inactive and should be
* activated the fixed ODE-joint is removed and the
* virtual attachJoint-method is called.
* If the Joint was active and should be deactivated
* the virtual detachJoint-method is called and then
* a fixed ODE-joint is created and attached to the bodies.
* @param isActive 0 .. not active
* anything else .. active
**/
void Joint::setActive(int isActive)
{
dBodyID body1, body2;
if (!active && isActive && joint)
{
dJointDestroy(joint);
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
attachJoint(body1, body2);
} // if
else if (active && !isActive && joint)
{
// first detach joint e.g. for saving axis angles
detachJoint();
dJointDestroy(joint);
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
joint = dJointCreateFixed(world, 0);
dJointAttach(joint, body1, body2);
dJointSetFixed(joint);
} // else if
if (isActive)
this->active = true;
else
this->active = false;
} // setActive
/***********************************************************
hanlde collision
***********************************************************/
void ODEPhysicHandler::handleCollisionBetween(dGeomID o0, dGeomID o1)
{
// Create an array of dContact objects to hold the contact joints
static const int MAX_CONTACTS = 10;
dContact contact[MAX_CONTACTS];
for (int 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.8;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide(o0, o1, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)))
{
// Get the dynamics body for each geom
dBodyID b1 = dGeomGetBody(o0);
dBodyID b2 = dGeomGetBody(o1);
// 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 (int 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);
}
}
}
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);
}
}
}
/*
=================================================================================
createFixedLeg
Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius,
dReal length)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//fixed joint
joint = dJointCreateFixed(World, jointgroup);
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetFixed(joint);
}