void constructWorldForTest (dReal gravity, int bodycount,
/* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z,
/* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z,
/* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z,
/* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z,
/* rotation angles */ dReal a1, dReal a2)
{
// create world
world = dWorldCreate();
dWorldSetERP (world,0.2);
dWorldSetCFM (world,1e-6);
dWorldSetGravity (world,0,0,gravity);
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0], pos1x, pos1y, pos1z);
dQuaternion q;
dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1);
dBodySetQuaternion (body[0],q);
if (bodycount==2) {
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1], pos2x, pos2y, pos2z);
dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2);
dBodySetQuaternion (body[1],q);
}
else body[1] = 0;
}
void command (int cmd)
{
// note: 0.0174532925 radians = 1 degree
dQuaternion q;
dMatrix3 m;
switch(cmd)
{
case 'w':
geom1pos[0]+=0.05;
break;
case 'a':
geom1pos[1]-=0.05;
break;
case 's':
geom1pos[0]-=0.05;
break;
case 'd':
geom1pos[1]+=0.05;
break;
case 'e':
geom1pos[2]-=0.05;
break;
case 'q':
geom1pos[2]+=0.05;
break;
case 'i':
dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'j':
dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'k':
dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'l':
dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'm':
(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
break;
case 'n':
(geoms!=convex)? geoms=convex:geoms=boxes;
break;
default:
dsPrint ("received command %d (`%c')\n",cmd,cmd);
}
#if 0
dGeomSetPosition (geoms[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom1quat, m);
dGeomSetRotation (geoms[1],m);
#endif
DumpInfo=true;
}
EXPORT_C void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal angle)
{
dQuaternion q;
dQFromAxisAndAngle (q,ax,ay,az,angle);
dQtoR (q,R);
}
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);
}
Object::Object()
{
iSize.x = iSize.y = iSize.z = 1;
iPosition.x = iPosition.y = iPosition.z = 0;
iVel.x = iVel.y =iVel.z = 0;
iM = 1;
Mat = new Material();
dQFromAxisAndAngle(q,1,0,0,0);
data = new BodyData();
data->setName("Object");
};
//you should create your own world
void CreateWorld1(Simulator * sim)
{
const double zdim = 2.0;
const double xdim = 20;
const double ydim = 20;
AddBoundaries(sim, xdim, ydim, zdim, 0.2);
dGeomID geom;
dGeomID geom_gara1;
dGeomID geom_gara2;
dQuaternion q; //for rotations
// create a parking garage - 1 st wall
geom_gara1 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara1, -13 + 0.5*(30-xdim), -5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara1, q);
sim->AddGeometryAsObstacle(geom_gara1);
// creat a pakring garage - 2nd wall
geom_gara2 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara2, -13+0.5*(30-xdim), -7.5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara2, q);
sim->AddGeometryAsObstacle(geom_gara2);
geom = dCreateBox(sim->GetSpaceID(), 9.0, 0.2, zdim);
dGeomSetPosition(geom, 6, -4, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, -M_PI * 0.25);
dGeomSetQuaternion(geom, q);
sim->AddGeometryAsObstacle(geom);
geom = dCreateSphere(sim->GetSpaceID(), 0.5 * zdim);
dGeomSetPosition(geom, 0, -4, 0.5 * zdim);
sim->AddGeometryAsObstacle(geom);
}
PMoveable::PMoveable(Kinematic &kinematic, float mass, GeomInfo *info)
: PObject(info), kinematic(kinematic)
{
dQuaternion q;
body = dBodyCreate(Physics::getInstance().getOdeWorld());
info->createMass(&this->mass, mass);
dBodySetMass(body, &this->mass);
dGeomSetBody(geom, body);
kinematicToOde();
dQFromAxisAndAngle(q, 0, 1, 0, kinematic.orientation);
dBodySetQuaternion(body, q);
}
static void reset_state(void)
{
float sx=-4, sy=-4, sz=2;
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
dBodySetPosition (boxbody, sx, sy+1, sz);
dBodySetLinearVel (boxbody, 0,0,0);
dBodySetAngularVel (boxbody, 0,0,0);
dBodySetQuaternion (boxbody, q);
#endif
#ifdef CYL
dBodySetPosition (cylbody, sx, sy, sz);
dBodySetLinearVel (cylbody, 0,0,0);
dBodySetAngularVel (cylbody, 0,0,0);
dBodySetQuaternion (cylbody, q);
#endif
}
void dJointSetScrewAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
setAxes( joint, x, y, z, joint->axis1, joint->axis2 );
joint->computeInitialRelativeRotation();
if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle;
dQuaternion qAngle, qOffset;
dQFromAxisAndAngle(qAngle, x, y, z, dangle);
dQMultiply3(qOffset, qAngle, joint->qrel);
joint->qrel[0] = qOffset[0];
joint->qrel[1] = qOffset[1];
joint->qrel[2] = qOffset[2];
joint->qrel[3] = qOffset[3];
}
int main (int argc, char **argv)
{
// 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();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dQuaternion q;
dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
dBodySetQuaternion (body[0],q);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
dBodySetQuaternion (body[1],q);
hinge = dJointCreateHinge (world,0);
dJointAttach (hinge,body[0],body[1]);
dJointSetHingeAnchor (hinge,0,0,1);
dJointSetHingeAxis (hinge,1,-1,1.41421356);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
Cube::Cube::Cube(bool useVbo)
{
iSize.x = iSize.y = iSize.z = 1;
iPosition.x = iPosition.y = iPosition.z = 0;
iVel.x = iVel.y =iVel.z = 0;
iRotate.x = iRotate.y = iRotate.z = 0.0f;
iM = 1;
Mat = new Material();
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
data = new BodyData();
data->setName("Cube");
vecs = new vec3d[24];
texCoord = new TexCoord2[24];
texture = NULL;
vbo = NULL;
}
AvatarGameObj::AvatarGameObj(const ORE1::ObjType& obj) :
GameObj(obj, Sim::gen_sphere_body(80, 0.5)), // TODO Load mass information from the ORE mission description
_xrot_delta(0.0),
_zrot_delta(0.0),
_ypos_delta(0.0),
_ylvel_delta(0.0),
_xavel_delta(0.0),
_zavel_delta(0.0),
_norm_coll_steptime(0),
_run_coll_steptime(0),
_mesh(MeshAnimation::load("mesh-LIBAvatar")),
_attached(false),
_attached_this_frame(false)
{
// TODO Load volume information from the ORE mission description
_height = 2.25;
_coll_rad = 0.25;
// Set up a geom for detecting regular collisions
get_entity().set_geom(
"physical",
dCreateCapsule(Sim::get_dyn_space(), _coll_rad, _height - 2*_coll_rad),
std::auto_ptr<CollisionHandler>(new AvatarContactHandler(this))
);
// TODO Maybe some missions start off in upright mode?
_uprightness = 0.0;
// Set up a geom at our feet to detect when we can run on a surface
get_entity().set_geom(
"sticky_attach",
dCreateRay(Sim::get_dyn_space(), RUNNING_MAX_DELTA_Y_POS*2),
std::auto_ptr<CollisionHandler>(new StickyAttachmentContactHandler(this))
);
dQuaternion rdq;
dQFromAxisAndAngle(rdq, 1, 0, 0, M_PI_2);
dGeomSetOffsetQuaternion(get_entity().get_geom("sticky_attach"), rdq);
dGeomDisable(get_entity().get_geom("sticky_attach"));
update_geom_offsets();
}
Item* WorldPhysics::generateItem() {
Item* temp;
switch (qrand()%3) {
case 0: {
temp=new BoxItem(world,space,3,3,3,.2);
} break;
case 1: {
dQuaternion q;
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
temp = new WheelItem(world,space,q,2,.2);
} break;
case 2: {
if (enable_complex) temp=new ComplexItem(world,space,3,3,3,.2);
else temp=new BoxItem(world,space,3,3,3,.3);
} break;
}
temp->state=0;
temp->setPosition(qrand()%30+50,qrand()%30+50,qrand()%30+10);
return temp;
}
int main (int argc, char **argv)
{
// 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();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0,0,1);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dQuaternion q;
dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI);
dBodySetPosition (body[1],0.2,0.2,1.2);
dBodySetQuaternion (body[1],q);
slider = dJointCreateSlider (world,0);
dJointAttach (slider,body[0],body[1]);
dJointSetSliderAxis (slider,1,1,1);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
return 0;
}
void Robots::construirRodas(dWorldID world)
{
for (int i=0; i < 2; i++)
{
// Cria objeto e geometria
this->wheel[i] = dBodyCreate(world);
this->cylinder[i] = dCreateCylinder(0,RADIUS,wTHICK);
// Define a posição e orientação do objeto
dQuaternion q;
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
dBodySetQuaternion(this->wheel[i],q);
dBodySetPosition(this->wheel[i],this->pegarX(),this->pegarY()+(1-2*i)*((WIDTH/2)+(wTHICK/2)),STARTZ);
// Define a massa do objeto
dMass m;
dMassSetCylinder(&m,1,3,RADIUS,wTHICK);
dMassAdjust(&m,WMASS);
dBodySetMass(this->wheel[i],&m);
// Associa o objeto à sua geometria
dGeomSetBody(this->cylinder[i],this->wheel[i]);
}
}
void command (int cmd)
{
// note: 0.0174532925 radians = 1 degree
dQuaternion q;
dMatrix3 m;
bool changed = false;
switch(cmd)
{
case 'w':
geom1pos[0]+=0.05;
changed = true;
break;
case 'a':
geom1pos[1]-=0.05;
changed = true;
break;
case 's':
geom1pos[0]-=0.05;
changed = true;
break;
case 'd':
geom1pos[1]+=0.05;
changed = true;
break;
case 'e':
geom1pos[2]-=0.05;
changed = true;
break;
case 'q':
geom1pos[2]+=0.05;
changed = true;
break;
case 'i':
dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'j':
dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'k':
dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'l':
dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'm':
(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
break;
case 'n':
(geoms!=convex)? geoms=convex:geoms=boxes;
if(geoms==convex)
{
printf("CONVEX------------------------------------------------------>\n");
}
else
{
printf("BOX--------------------------------------------------------->\n");
}
break;
default:
dsPrint ("received command %d (`%c')\n",cmd,cmd);
}
#if 0
dGeomSetPosition (geoms[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom1quat, m);
dGeomSetRotation(geoms[1],m);
if(changed)
{
printf("POS: %f,%f,%f\n",geom1pos[0],geom1pos[1],geom1pos[2]);
printf("ROT:\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n",
m[0],m[1],m[2],m[3],
m[4],m[5],m[6],m[7],
m[8],m[9],m[10],m[11]);
}
#endif
DumpInfo=true;
}
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
{
int i;
dMass m;
// chassis body
body[bodyI] = dBodyCreate (world);
dBodySetPosition (body[bodyI],x,y,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (body[bodyI],&m);
box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxI],body[bodyI]);
// wheel bodies
for (i=1; i<=4; i++) {
body[bodyI+i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[bodyI+i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[bodyI+i],&m);
sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
}
dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[jointI+i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1));
dJointSetHinge2Axis2 (joint[jointI+i],0,1,0);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
}
//center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
dBodyID b = dBodyCreate (world);
dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (b,&m);
dJointID j = dJointCreateFixed(world, 0);
dJointAttach(j, body[bodyI], b);
dJointSetFixed(j);
//box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
//dGeomSetBody (box[boxI+1],b);
bodyI += 5;
jointI += 4;
boxI += 1;
sphereI += 4;
}
Simulator::Simulator(const double posx, const double posy)
{
m_collision = false;
//Open Dynamics Engine stuff
m_world = dWorldCreate();
m_space = dHashSpaceCreate(0);
m_contacts = dJointGroupCreate(0);
m_ground = dCreatePlane(m_space, 0, 0, 1, 0);
dGeomSetData(m_ground, (void *) &TYPE_TERRAIN);
dWorldSetGravity(m_world, 0, 0, -0.81);
//create robot
const double LENGTH = 2.50; // chassis length 2.50;
const double WIDTH = 1.00; // chassis width
const double HEIGHT = 0.40; // chassis height
const double RADIUS = 0.45 * WIDTH;//0.45 * WIDTH; // wheel radius
const double STARTZ = 0.05;
dMass m;
dQuaternion q;
double car_center_x= posx + 1.5; //1.5
double car_center_y= posy + 5.3;
// chassis body
m_robotBodyChassis = dBodyCreate(m_world);
dBodySetPosition(m_robotBodyChassis, car_center_x, car_center_y, 0.85 * RADIUS + 0.5 * HEIGHT + STARTZ);
dMassSetBox(&m, 1, LENGTH, WIDTH, HEIGHT);
dBodySetMass(m_robotBodyChassis,&m);
// chassis geometry
m_robotGeomChassis = dCreateBox(m_space, LENGTH, WIDTH, HEIGHT);
dGeomSetBody(m_robotGeomChassis, m_robotBodyChassis);
m_robotBodies.push_back(m_robotBodyChassis);
dGeomSetData(m_robotGeomChassis, (void *) &TYPE_ROBOT);
// wheel bodies
for(int i= 0; i < 3; i++)
{
m_robotBodyWheels[i] = dBodyCreate(m_world);
dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5);
dBodySetQuaternion(m_robotBodyWheels[i], q);
dMassSetSphere(&m, 1, RADIUS);
dBodySetMass(m_robotBodyWheels[i], &m);
m_robotGeomWheels[i] = dCreateSphere(m_space, RADIUS);
dGeomSetBody(m_robotGeomWheels[i], m_robotBodyWheels[i]);
m_robotBodies.push_back(m_robotBodyWheels[i]);
dGeomSetData(m_robotGeomWheels[i], (void *) &TYPE_ROBOT);
}
dBodySetPosition(m_robotBodyWheels[0], car_center_x + 0.5 * LENGTH, car_center_y, RADIUS + STARTZ);
dBodySetPosition(m_robotBodyWheels[1], car_center_x - 0.5 * LENGTH, car_center_y + 0.5 * WIDTH, RADIUS + STARTZ);
dBodySetPosition(m_robotBodyWheels[2], car_center_x - 0.5 * LENGTH, car_center_y - 0.5 * WIDTH, RADIUS + STARTZ);
// front and back wheel hinges
for (int i = 0; i < 3; i++)
{
m_robotJoints[i] = dJointCreateHinge2(m_world, 0); // creat hign-2 joint as wheels
dJointAttach(m_robotJoints[i], m_robotBodyChassis, m_robotBodyWheels[i]);
const dReal *a = dBodyGetPosition(m_robotBodyWheels[i]);
dJointSetHinge2Anchor(m_robotJoints[i], a[0], a[1], a[2]);
dJointSetHinge2Axis1(m_robotJoints[i], 0, 0, 1);
dJointSetHinge2Axis2(m_robotJoints[i], 0, 1, 0);
// set joint suspension
dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionERP, 0.04);
dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionCFM, 0.08);
}
// lock back wheels along the steering axis
for (int i = 1; i < 3; i++)
{
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (m_robotJoints[i], dParamLoStop, 0);
dJointSetHinge2Param (m_robotJoints[i], dParamHiStop, 0);
}
}
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);
}
}
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]);
}
int main(int argc, char **argv)
{
prepDrawStuff();
initOde(2);
setCurrentOdeContext(ALLTHREADS);
odeRandSetSeed(0);
odeSetContactSoftCFM(0);
odeWorldSetGravity(0, 0, -9.81f);
//creating stage
stage.body = odeBodyCreate();
stage.geom = odeCreateBox(0.4f, 0.4f, 0.05f);
odeMassSetBoxTotal(stage.body, 0.94, 0.4f,0.4f, 0.05f);
odeBodySetPosition(stage.body,0,0.0,h_floor_table+h_base+h_sphere+h_support+stage_dim[2]/2);
odeGeomSetBody(stage.geom,stage.body);
printf(" Stage body id %f, geom id %f \n", stage.body, stage.geom);
const dReal *pos;
pos= odeBodyGetPosition(stage.body);
printf("stage position x%f y%f z%f \n",pos[0],pos[1], pos[2]);
//creating support //
//odeMassSetCapsuleTotal(int bodyId, float total_mass, float radius, float length)
support.radius = 0.05f ;
support.length = 0.2f;
support.body = odeBodyCreate();
support.geom = odeCreateCapsule(0, support.radius, support.length-support.radius);
odeMassSetCapsuleTotal(support.body, 0.5f, support.radius, support.length-support.radius);
odeBodySetPosition(support.body,0.0f,0.0f,h_floor_table+h_base+h_sphere+(h_support/2)-stage_dim[2]/2);
odeGeomSetBody(support.geom,support.body);
printf("Support capsule body id %f, geom id %f \n", support.body, support.geom);
pos= odeBodyGetPosition(support.body);
printf("stage position %f \n", pos[2]);
//creating ball link
LinkBall.radius = 0.08f ;
LinkBall.body = odeBodyCreate();
LinkBall.geom = odeCreateSphere( LinkBall.radius);
odeMassSetSphereTotal(LinkBall.body, 0.5,LinkBall.radius);
odeBodySetPosition(LinkBall.body,0,0,h_floor_table+h_base-support.radius);
odeGeomSetBody(LinkBall.geom,LinkBall.body);
printf("Ball link body id %f, geom id %f \n", LinkBall.body, LinkBall.geom);
//creating main link
mainLink.radius= 0.05f;
mainLink.length= 0.12f;
mainLink.body = odeBodyCreate();
mainLink.geom = odeCreateCapsule(0, mainLink.radius, mainLink.length-mainLink.radius);
odeMassSetCapsuleTotal(mainLink.body, 0.5,mainLink.radius, mainLink.length-mainLink.length);
odeBodySetPosition(mainLink.body,(mainLink.length+mainLink.radius),0, h_floor_table+h_base-mainLink.radius);
odeGeomSetBody(mainLink.geom,mainLink.body);
printf("capsule main link link body id %f, geom id %f \n", mainLink.body, mainLink.geom);
dQuaternion q1;
dQFromAxisAndAngle (q1,0,1,0,1.57) ; //dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, dReal angle);
odeGeomSetQuaternion(mainLink.geom,q1);
pos= odeBodyGetPosition(mainLink.body);
printf("main link position x %f y %f z%f \n",pos[0],pos[1], pos[2]);
//creating the first link
firstLink.radius= 0.05f;
firstLink.length= 0.60f;
firstLink.body = odeBodyCreate();
firstLink.geom = odeCreateCapsule(0, firstLink.radius, firstLink.length-firstLink.radius);
odeMassSetCapsuleTotal(firstLink.body, 0.7, firstLink.radius, firstLink.length-firstLink.radius);
odeBodySetPosition(firstLink.body,(firstLink.length-firstLink.radius/2),0,h_floor_table+h_base-firstLink.radius);
odeGeomSetBody(firstLink.geom,firstLink.body);
printf("capsule first link body id %f, geom id %f \n", firstLink.body, firstLink.geom);
dQuaternion q;
dQFromAxisAndAngle (q,0,1,0,1.57) ; //dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, dReal angle);
odeGeomSetQuaternion(firstLink.geom,q);
pos= odeBodyGetPosition(firstLink.body);
printf("First link position x %f y %f z%f \n",pos[0],pos[1], pos[2]);
//creating base
base.radius= 0.05f;
base.length= 0.32f+h_floor_table;
base.body = odeBodyCreate();
base.geom = odeCreateCapsule(0, base.radius, base.length-base.radius);
odeMassSetCapsuleTotal(base.body, 0.7, base.radius, base.length-base.radius);
odeBodySetPosition(base.body,(h_sphere+h_main+h_first+base.radius),0, (base.length+base.radius)/2-firstLink.radius);
odeGeomSetBody(base.geom,base.body);
printf("capsule base id %f, geom id %f \n", base.body, base.geom);
//Creating Ball
ball.radius = 0.03f ;
ball.body = odeBodyCreate();
ball.geom = odeCreateSphere( ball.radius); //odeCreateCapsule(int spaceId, float radius, float length)
odeMassSetSphereTotal(ball.body, 0.04,ball.radius); //odeMassSetCapsuleTotal(int bodyId, float total_mass, float radius, float length)
odeBodySetPosition(ball.body,0.1,0.0,2.2f);
odeGeomSetBody(ball.geom,ball.body);
printf("ball body id %f, geom id %f \n", ball.body, ball.geom);
//creating Obstacle
obs.radius = 0.05f;
obs.body = odeBodyCreate();
obs.geom = odeCreateSphere( obs.radius);
odeMassSetSphereTotal(obs.body, 0.05,obs.radius);
odeBodySetPosition(obs.body,0,0.0,h_floor_table+h_base+h_sphere+h_support+stage_dim[2]+obs.radius);
odeGeomSetBody(obs.geom,obs.body);
printf("capsule obstacle body id %f, geom id %f \n", obs.body, obs.geom);
////FIXING ALL JOINTS//
//base joint with ground
base.joint =odeJointCreateFixed();
odeJointAttach(base.joint,base.body,0);
odeJointSetFixed(base.joint);
//FirstLink with world
firstLink.joint =odeJointCreateFixed();
odeJointAttach(firstLink.joint,firstLink.body,0);
odeJointSetFixed(firstLink.joint);
// Main link and first link
mainLink.joint =odeJointCreateHinge();
odeJointAttach(mainLink.joint,mainLink.body,0);
//odeJointSetHingeAnchor(capsule2.joint[0],-0.5,0,0.60);
odeJointSetHingeAxis(mainLink.joint,1,0,0);
//Ball link and Main link
LinkBall.joint =odeJointCreateHinge();
odeJointAttach(LinkBall.joint,LinkBall.body,mainLink.body);
//odeJointSetHingeAnchor(capsule3.joint[1],-1.1,0.5,0.60);
odeJointSetHingeAxis(LinkBall.joint,0,1,0);
// Support and Ball link
support.joint =odeJointCreateFixed();
odeJointAttach(support.joint,support.body,LinkBall.body);
odeJointSetFixed(support.joint);
//Stage and support link
stage.joint =odeJointCreateFixed();
odeJointAttach(stage.joint,stage.body,support.body);
odeJointSetFixed(stage.joint);
//Stage and Obstacle
obs.joint =odeJointCreateFixed();
odeJointAttach(obs.joint,stage.body,obs.body);
odeJointSetFixed(obs.joint);
dsSimulationLoop (argc,argv,900,600,&fn);
}
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;
}
Cube::Cube()
{
iSize.x = iSize.y = iSize.z = 1;
iPosition.x = iPosition.y = iPosition.z = 0;
iVel.x = iVel.y =iVel.z = 0;
iRotate.x = iRotate.y = iRotate.z = 0.0f;
iM = 1;
Mat = new Material();
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
data = new BodyData();
data->setName("Cube");
vecs = new vec3d[24];
texCoord = new TexCoord2[24];
texture = NULL;
//Make Verte Buffer Object
// Front Face
vecs[0] = {-1.0f, -1.0f, 1.0f};
vecs[1] = {1.0f, -1.0f, 1.0f};
vecs[2] = {1.0f, 1.0f, 1.0f};
vecs[3] = {-1.0f, 1.0f, 1.0f};
// Back Face
vecs[4] = {-1.0f, -1.0f, -1.0f};
vecs[5] = {-1.0f, 1.0f, -1.0f};
vecs[6] = { 1.0f, 1.0f, -1.0f};
vecs[7] = { 1.0f, -1.0f, -1.0f};
// Top Face
vecs[8] = {-1.0f, 1.0f, -1.0f};
vecs[9] = {-1.0f, 1.0f, 1.0f};
vecs[10] = { 1.0f, 1.0f, 1.0f};
vecs[11] = { 1.0f, 1.0f, -1.0f};
// Bottom Face
vecs[12] = {-1.0f, -1.0f, -1.0f};
vecs[13] = { 1.0f, -1.0f, -1.0f};
vecs[14] = { 1.0f, -1.0f, 1.0f};
vecs[15] = {-1.0f, -1.0f, 1.0f};
// Right face
vecs[16] = { 1.0f, -1.0f, -1.0f};
vecs[17] = { 1.0f, 1.0f, -1.0f};
vecs[18] = { 1.0f, 1.0f, 1.0f};
vecs[19] = { 1.0f, -1.0f, 1.0f};
// Left Face
vecs[20] = {-1.0f, -1.0f, -1.0f};
vecs[21] = {-1.0f, -1.0f, 1.0f};
vecs[22] = {-1.0f, 1.0f, 1.0f};
vecs[23] = {-1.0f, 1.0f, -1.0f};
texCoord[0] = {0.0f, 0.0f};
texCoord[1] = {1.0f, 0.0f};
texCoord[2] = {1.0f, 1.0f};
texCoord[3] = {0.0f, 1.0f};
// Back Face
texCoord[4] = {1.0f, 0.0f};
texCoord[5] = {1.0f, 1.0f};
texCoord[6] = {0.0f, 1.0f};
texCoord[7] = {0.0f, 0.0f};
// Top Face
texCoord[8] = {0.0f, 1.0f};
texCoord[9] = {0.0f, 0.0f};
texCoord[10] = {1.0f, 0.0f};
texCoord[11] = {1.0f, 1.0f};
// Bottom Face
texCoord[12] = {1.0f, 1.0f};
texCoord[13] = {0.0f, 1.0f};
texCoord[14] = {0.0f, 0.0f};
texCoord[15] = {1.0f, 0.0f};
// Right face
texCoord[16] = {1.0f, 0.0f};
texCoord[17] = {1.0f, 1.0f};
texCoord[18] = {0.0f, 1.0f};
texCoord[19] = {0.0f, 0.0f};
// Left Face
texCoord[20] = {0.0f, 0.0f};
texCoord[21] = {1.0f, 0.0f};
texCoord[22] = {1.0f, 1.0f};
texCoord[23] = {0.0f, 1.0f};
vbo = new VertexBufferObject();
vbo->glMode = GL_QUADS;
vbo->setVertices(vecs,24);
vbo->setTexCoords(texCoord,24);
}
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, 32);
dCreatePlane (space,0,0,1, 0.0);
cylbody = dBodyCreate (world);
dQuaternion q;
#if 0
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#else
// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0);
dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);
#endif
dBodySetQuaternion (cylbody,q);
dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH);
dGeomSetBody (cylgeom,cylbody);
dBodySetPosition (cylbody, 0, 0, 3);
dSpaceAdd (space, cylgeom);
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,SPHERERADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, SPHERERADIUS);
dGeomSetBody (sphgeom,sphbody);
dBodySetPosition (sphbody, 0, 0, 5.5);
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dGeomDestroy(sphgeom);
dGeomDestroy (cylgeom);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void dJointSetUniversalAxis1Offset( dJointID j, dReal x, dReal y, dReal z,
dReal offset1, dReal offset2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
setAxes( joint, x, y, z, NULL, joint->axis2 );
offset1 = -offset1;
offset2 = -offset2;
}
else
setAxes( joint, x, y, z, joint->axis1, NULL );
joint->computeInitialRelativeRotations();
dVector3 ax2;
getAxis2( joint, ax2, joint->axis2 );
{
dVector3 ax1;
joint->getAxes(ax1, ax2);
}
dQuaternion qAngle;
dQFromAxisAndAngle(qAngle, x, y, z, offset1);
dMatrix3 R;
dRFrom2Axes( R, x, y, z, ax2[0], ax2[1], ax2[2] );
dQuaternion qcross;
dRtoQ( R, qcross );
dQuaternion qOffset;
dQMultiply0(qOffset, qAngle, qcross);
dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
// Calculating the second offset
dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], x, y, z );
dRtoQ( R, qcross );
dQMultiply1(qOffset, qAngle, qcross);
if ( joint->node[1].body )
{
dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
}
else
{
joint->qrel2[0] = qcross[0];
joint->qrel2[1] = qcross[1];
joint->qrel2[2] = qcross[2];
joint->qrel2[3] = qcross[3];
}
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
void dJointSetUniversalAxis2Offset( dJointID j, dReal x, dReal y, dReal z,
dReal offset1, dReal offset2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
setAxes( joint, x, y, z, joint->axis1, NULL );
offset1 = -offset2;
offset2 = -offset1;
}
else
setAxes( joint, x, y, z, NULL, joint->axis2 );
joint->computeInitialRelativeRotations();
// It is easier to retreive the 2 axes here since
// when there is only one body B2 (the axes switch position)
// Doing this way eliminate the need to write the code differently
// for both case.
dVector3 ax1, ax2;
joint->getAxes(ax1, ax2 );
dQuaternion qAngle;
dQFromAxisAndAngle(qAngle, ax1[0], ax1[1], ax1[2], offset1);
dMatrix3 R;
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
dQuaternion qcross;
dRtoQ( R, qcross );
dQuaternion qOffset;
dQMultiply0(qOffset, qAngle, qcross);
dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
// Calculating the second offset
dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
dRtoQ( R, qcross );
dQMultiply1(qOffset, qAngle, qcross);
if ( joint->node[1].body )
{
dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
}
else
{
joint->qrel2[0] = qcross[0];
joint->qrel2[1] = qcross[1];
joint->qrel2[2] = qcross[2];
joint->qrel2[3] = qcross[3];
}
}
