void CPHShell::RunSimulation(bool place_current_forms/*true*/)
{
if(!CPHObject::is_active()) vis_update_deactivate();
EnableObject(0);
dSpaceSetCleanup(m_space,0);
{
ELEMENT_I i=elements.begin(),e=elements.end();
if(place_current_forms) for(;i!=e;++i)(*i)->RunSimulation(mXFORM);
}
{
JOINT_I i=joints.begin(),e=joints.end();
for(;i!=e;++i) (*i)->RunSimulation();
}
spatial_register();
}
void PhysicsSpace::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & CleanupFieldMask)
{
dSpaceSetCleanup(_SpaceID, getCleanup() ? 1 : 0);
}
if(whichField & SublevelFieldMask)
{
dSpaceSetSublevel(_SpaceID, getSublevel());
}
}
void Bola::construir(dWorldID world, dSpaceID space)
{
// Cria objeto e geometria
this->corpo = dBodyCreate(world);
this->geometria = dCreateSphere(0,BOLA_RAIO);
// Define a posição do objeto
dBodySetPosition(this->corpo, this->pegarX(), this->pegarY(), STARTZ);
// Define a massa do objeto
dMass m;
dMassSetSphere(&m,1,BOLA_RAIO);
dMassAdjust(&m,BOLA_MASSA);
dBodySetMass(this->corpo,&m);
// Associa o objeto à geometria
dGeomSetBody(this->geometria,this->corpo);
// Cria um espaço para a bola e a adiciona
this->espaco = dSimpleSpaceCreate(space);
dSpaceSetCleanup(this->espaco,0);
dSpaceAdd(this->espaco,this->geometria);
}
WorldPhysics::WorldPhysics() {
enable_complex=0;
bulldozer_state=1;
tmp_scalar=0;
tmp_wait=0;
qsrand(QTime::currentTime().msec());
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.81);
//ground_cheat = dCreatePlane (space,0,0,1,0);
wall1=dCreatePlane (space,-1,0,0,-100);
wall2=dCreatePlane (space,1,0,0,0);
wall3=dCreatePlane (space,0,-1,0,-100);
wall4=dCreatePlane (space,0,1,0,0);
// our heightfield floor
dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
// Create an finite heightfield.
dGeomHeightfieldDataBuildCallback( heightid, NULL, near_heightfield_callback,
HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );
// Give some very bounds which, while conservative,
// makes AABB computation more accurate than +/-INF.
//dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );
gheight = dCreateHeightfield( space, heightid, 1 );
// Rotate so Z is up, not Y (which is the default orientation)
dMatrix3 R;
dRSetIdentity( R );
dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
dGeomSetRotation( gheight, R );
dGeomSetPosition( gheight, 50,50,0 );
// for (int i=0;i<MAX_ITEMS;i++) {
// items.push_back(generateItem());
//}
generateItems();
bulldozer_space = dSimpleSpaceCreate(space);
dSpaceSetCleanup (bulldozer_space,0);
bulldozer=new BoxItem(world,bulldozer_space,LENGTH,WIDTH,HEIGHT,CMASS);
bulldozer->setPosition(STARTX,STARTY,STARTZ);
bulldozer_cabin=new BoxItem(world,bulldozer_space,LENGTH/2,WIDTH/2,2*HEIGHT,CMASS/3);
bulldozer_cabin->setPosition(-LENGTH/4+STARTX,STARTY,3.0/2.0*HEIGHT+STARTZ);
bulldozer_bucket_c=new BoxItem(world,bulldozer_space,BUCKET_LENGTH,BUCKET_WIDTH,BUCKET_HEIGHT,CMASS/10);
bulldozer_bucket_c->setPosition(LENGTH/2+BUCKET_LENGTH/2+RADIUS+STARTX,STARTY,STARTZ);
bulldozer_bucket_l=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20);
bulldozer_bucket_l->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,-BUCKET_WIDTH/2+BUCKET_LENGTH/2+STARTY,STARTZ);
bulldozer_bucket_r=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20);
bulldozer_bucket_r->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,BUCKET_WIDTH/2-BUCKET_LENGTH/2+STARTY,STARTZ);
for (int i=0; i<4; i++) {
dQuaternion q;
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
wheels[i] = new WheelItem(world,bulldozer_space,q,RADIUS,WMASS);
}
dBodySetPosition (wheels[0]->body,0.5*LENGTH+STARTX,WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
dBodySetPosition (wheels[1]->body,0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
dBodySetPosition (wheels[2]->body,-0.5*LENGTH+STARTX, WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
dBodySetPosition (wheels[3]->body,-0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
cabin_joint=dJointCreateSlider(world,0);
dJointAttach(cabin_joint,bulldozer->body,bulldozer_cabin->body);
dJointSetSliderAxis(cabin_joint,0,0,1);
dJointSetSliderParam(cabin_joint,dParamLoStop,0);
dJointSetSliderParam(cabin_joint,dParamHiStop,0);
bucket_joint_c=dJointCreateSlider(world,0);
dJointAttach(bucket_joint_c,bulldozer->body,bulldozer_bucket_c->body);
dJointSetSliderAxis(bucket_joint_c,0,0,1);
dJointSetSliderParam(bucket_joint_c,dParamLoStop,0);
dJointSetSliderParam(bucket_joint_c,dParamHiStop,0);
bucket_joint_l=dJointCreateSlider(world,0);
dJointAttach(bucket_joint_l,bulldozer->body,bulldozer_bucket_l->body);
dJointSetSliderAxis(bucket_joint_l,0,0,1);
dJointSetSliderParam(bucket_joint_l,dParamLoStop,0);
dJointSetSliderParam(bucket_joint_l,dParamHiStop,0);
bucket_joint_r=dJointCreateSlider(world,0);
dJointAttach(bucket_joint_r,bulldozer->body,bulldozer_bucket_r->body);
dJointSetSliderAxis(bucket_joint_r,0,0,1);
dJointSetSliderParam(bucket_joint_r,dParamLoStop,0);
dJointSetSliderParam(bucket_joint_r,dParamHiStop,0);
// front and back wheel hinges
for (int i=0; i<4; i++) {
wheelJoints[i] = dJointCreateHinge2 (world,0);
dJointAttach (wheelJoints[i],bulldozer->body,wheels[i]->body);
const dReal *a = dBodyGetPosition (wheels[i]->body);
dJointSetHinge2Anchor (wheelJoints[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (wheelJoints[i],0,0,1);
dJointSetHinge2Axis2 (wheelJoints[i],0,1,0);
}
// seeting ERP & CRM
for (int i=0; i<4; i++) {
dJointSetHinge2Param (wheelJoints[i],dParamSuspensionERP,0.5);
dJointSetHinge2Param (wheelJoints[i],dParamSuspensionCFM,0.8);
}
// block back axis !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
for (int i=0; i<2; i++) {
dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0);
dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0);
}
}
int main (int argc, char **argv)
{
int i;
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/textures";
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
ground = dCreatePlane (space,0,0,1,0);
// chassis body
body[0] = dBodyCreate (world);
dBodySetPosition (body[0],0,0,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[0],&m);
box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[0],body[0]);
// wheel bodies
for (i=1; i<=3; i++) {
body[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[i],&m);
sphere[i-1] = dCreateSphere (0,RADIUS);
dGeomSetBody (sphere[i-1],body[i]);
}
dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front wheel hinge
/*
joint[0] = dJointCreateHinge2 (world,0);
dJointAttach (joint[0],body[0],body[1]);
const dReal *a = dBodyGetPosition (body[1]);
dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[0],0,0,1);
dJointSetHinge2Axis2 (joint[0],0,1,0);
*/
// front and back wheel hinges
for (i=0; i<3; i++) {
joint[i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[i],body[0],body[i+1]);
const dReal *a = dBodyGetPosition (body[i+1]);
dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[i],0,0,1);
dJointSetHinge2Axis2 (joint[i],0,1,0);
// breakable joints contribution
// the wheels can break
dJointSetBreakable (joint[i], 1);
// the wheels wil break at a specific force
dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE);
// specify the force for the first body connected to the joint ...
dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5);
// and for the second body
dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5);
}
// set joint suspension
for (i=0; i<3; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
}
// lock back wheels along the steering axis
for (i=1; i<3; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i],dParamLoStop,0);
dJointSetHinge2Param (joint[i],dParamHiStop,0);
// the following alternative method is no good as the wheels may get out
// of alignment:
// dJointSetHinge2Param (joint[i],dParamVel,0);
// dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
// environment
ground_box = dCreateBox (space,2,1.5,1);
dMatrix3 R;
dRFromAxisAndAngle (R,0,1,0,-0.15);
dGeomSetPosition (ground_box,2,0,-0.34);
dGeomSetRotation (ground_box,R);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dGeomDestroy (box[0]);
dGeomDestroy (sphere[0]);
dGeomDestroy (sphere[1]);
dGeomDestroy (sphere[2]);
return 0;
}
void Buggy::embody(dWorldID world, dSpaceID space)
{
dMass m;
float LENGTH = 10;
float WIDTH = 5;
float HEIGHT = 5;
float RADIUS=4; // wheel radius
float STARTZ=1; // starting height of chassis
me = dBodyCreate(world);
dBodySetPosition (me,pos[0],pos[1],pos[2]);
dMassSetBox (&m,1,WIDTH,HEIGHT,LENGTH);
dMassAdjust (&m,CMASS);
dBodySetMass (me,&m);
box[0] = dCreateBox (0,WIDTH,HEIGHT,LENGTH);
dGeomSetBody(box[0], me);
// wheel bodies
for (int i=1; i<=4; i++) {
carbody[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,0);
dBodySetQuaternion (carbody[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (carbody[i],&m);
sphere[i-1] = dCreateSphere (0,RADIUS);
dGeomSetBody (sphere[i-1],carbody[i]);
}
//
dBodySetPosition (carbody[1],WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,+0.5*LENGTH);
dBodySetPosition (carbody[2],-WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,+0.5*LENGTH);
dBodySetPosition (carbody[3],WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,-0.5*LENGTH);
dBodySetPosition (carbody[4],-WIDTH*0.5 ,STARTZ/**-HEIGHT*0.5**/,-0.5*LENGTH);
//
//
// // front and back wheel hinges
for (int i=0; i<4; i++) {
carjoint[i] = dJointCreateHinge2 (world,0);
dJointAttach (carjoint[i],me,carbody[i+1]);
const dReal *a = dBodyGetPosition (carbody[i+1]);
dJointSetHinge2Anchor (carjoint[i],a[0],a[1],a[2]);
dJointSetHinge2Axes (carjoint[i], zunit, yunit);
//dJointSetHinge2Axis1 (carjoint[i],0,1,0); // Axis 1 that comes from the structure
//dJointSetHinge2Axis2 (carjoint[i],0,0,1); // Axis 2 where the wheels spin
}
//
// // set joint suspension
for (int i=0; i<4; i++) {
dJointSetHinge2Param (carjoint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (carjoint[i],dParamSuspensionCFM,0.8);
}
// lock back wheels along the steering axis
for (int i=1; i<4; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (carjoint[i],dParamLoStop,0);
dJointSetHinge2Param (carjoint[i],dParamHiStop,0);
// // the following alternative method is no good as the wheels may get out
// // of alignment:
// // dJointSetHinge2Param (joint[i],dParamVel,0);
// // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
dSpaceAdd (car_space,sphere[3]);
}
