// NOTE: it is important that rigid bodies are added
// (happens in CJoint::Attach()) before joint transforms are set!!!
void CHinge2Joint::Attach(dWorldID WorldID, dJointGroupID GroupID, const matrix44& ParentTfm)
{
ODEJointID = dJointCreateHinge2(WorldID, GroupID);
for (int i = 0; i < 2; i++)
{
const CJointAxis& CurrAxis = AxisParams[i];
if (CurrAxis.IsLoStopEnabled)
dJointSetHinge2Param(ODEJointID, dParamLoStop + dParamGroup * i, CurrAxis.LoStop);
if (CurrAxis.IsHiStopEnabled)
dJointSetHinge2Param(ODEJointID, dParamHiStop + dParamGroup * i, CurrAxis.HiStop);
dJointSetHinge2Param(ODEJointID, dParamVel + dParamGroup * i, CurrAxis.Velocity);
dJointSetHinge2Param(ODEJointID, dParamFMax + dParamGroup * i, CurrAxis.FMax);
dJointSetHinge2Param(ODEJointID, dParamFudgeFactor + dParamGroup * i, CurrAxis.FudgeFactor);
dJointSetHinge2Param(ODEJointID, dParamBounce + dParamGroup * i, CurrAxis.Bounce);
dJointSetHinge2Param(ODEJointID, dParamCFM + dParamGroup * i, CurrAxis.CFM);
dJointSetHinge2Param(ODEJointID, dParamStopERP + dParamGroup * i, CurrAxis.StopERP);
dJointSetHinge2Param(ODEJointID, dParamStopCFM + dParamGroup * i, CurrAxis.StopCFM);
}
dJointSetHinge2Param(ODEJointID, dParamSuspensionERP, SuspensionERP);
dJointSetHinge2Param(ODEJointID, dParamSuspensionCFM, SuspensionCFM);
CJoint::Attach(WorldID, GroupID, ParentTfm);
UpdateTransform(ParentTfm);
}
OscHinge2ODE::OscHinge2ODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase *parent,
OscObject *object1, OscObject *object2,
double x, double y, double z, double a1x,
double a1y, double a1z, double a2x,
double a2y, double a2z)
: OscHinge2(name, parent, object1, object2, x, y, z,
a1x, a1y, a1z, a2x, a2y, a2z)
{
m_response = new OscResponse("response",this);
dJointID odeJoint = dJointCreateHinge2(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetHinge2Anchor(odeJoint, x, y, z);
dJointSetHinge2Axis1(odeJoint, a1x, a1y, a1z);
dJointSetHinge2Axis2(odeJoint, a2x, a2y, a2z);
printf("[%s] Hinge2 joint created between %s and %s at (%f, %f, %f) for axes (%f, %f, %f) and (%f,%f,%f)\n",
simulation()->type_str(),
object1->c_name(), object2?object2->c_name():"world",
x, y, z, a1x, a1y, a1z, a2x, a2y, a2z);
}
static dJointID create_phys_joint(int t)
{
switch (t)
{
case dJointTypeBall: return dJointCreateBall (world, 0);
case dJointTypeHinge: return dJointCreateHinge (world, 0);
case dJointTypeSlider: return dJointCreateSlider (world, 0);
case dJointTypeUniversal: return dJointCreateUniversal(world, 0);
case dJointTypeHinge2: return dJointCreateHinge2 (world, 0);
default: return 0;
}
}
IoODEHinge2 *IoODEHinge2_rawClone(IoODEHinge2 *proto)
{
IoObject *self = IoODEJoint_rawClone(proto);
if(DATA(proto)->jointGroup)
{
IoODEJointGroup *jointGroup = DATA(proto)->jointGroup;
JOINTGROUP = jointGroup;
IoODEJointGroup_addJoint(jointGroup, self);
JOINTID = dJointCreateHinge2(WORLDID, JOINTGROUPID);
}
return self;
}
void WheelPiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix)
{
// set this piece position and rotation
dBodySetPosition(body, x, y, z);
dBodySetRotation(body, rotationMatrix);
// create and connect
connectingJoint = dJointCreateHinge2(world,0);
dJointAttach (connectingJoint,otherBody,body);
// set anchor, axes, and lo and hi stops
const dReal *a = dBodyGetPosition (body);
dJointSetHinge2Anchor (connectingJoint,a[0],a[1],a[2]);
dJointSetHinge2Axis1 (connectingJoint,rotationMatrix[1],rotationMatrix[5],rotationMatrix[9]);
dJointSetHinge2Axis2 (connectingJoint,rotationMatrix[2],rotationMatrix[6],rotationMatrix[10]);
dJointSetHinge2Param (connectingJoint,dParamLoStop,0);
dJointSetHinge2Param (connectingJoint,dParamHiStop,0);
}
sODEJoint* sODEJoint::AddHinge2Joint( int id, dBodyID body1, dBodyID body2, float ax, float ay, float az, float ax2, float ay2, float az2, float x, float y, float z )
{
sODEJoint *pJoint = new sODEJoint( );
//modify prev and next pointers of any involved joints, make new joint the new head
pJoint->pNextJoint = pODEJointList;
if ( pODEJointList ) pODEJointList->pPrevJoint = pJoint;
pODEJointList = pJoint;
//create the ODE hinge joint
pJoint->oJoint = dJointCreateHinge2( g_ODEWorld, 0 );
dJointAttach( pJoint->oJoint, body1, body2 );
dJointSetHinge2Anchor( pJoint->oJoint, x,y,z );
dJointSetHinge2Axis1( pJoint->oJoint, ax,ay,az );
dJointSetHinge2Axis2( pJoint->oJoint, ax2,ay2,az2 );
pJoint->iID = id;
return pJoint;
}
void Robots::conectarChassiRodas(dWorldID world)
{
const dReal *a;
for (int i = 0; i < 2; i++)
{
this->joint[i] = dJointCreateHinge2(world,0);
dJointAttach(this->joint[i],this->body[0],this->wheel[i]);
a = dBodyGetPosition (this->wheel[i]);
dJointSetHinge2Anchor(this->joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1(this->joint[i],0,0,1);
dJointSetHinge2Axis2(this->joint[i],0,1,0);
// set joint suspension
dJointSetHinge2Param(this->joint[i],dParamSuspensionERP,0.4);
//set stops to make sure wheels always stay in alignment
dJointSetHinge2Param(this->joint[i],dParamLoStop,0);
dJointSetHinge2Param(this->joint[i],dParamHiStop,0);
}
}
/**
* This method is called if the joint should be attached.
* It creates the ODE-joint, calculates the current anchor-position
* and axis-orientation and attaches the Joint.
* @param obj1 first ODE-object to attach with
* @param obj2 second ODE-object to attach with
**/
void Hinge2Joint::attachJoint(dBodyID obj1, dBodyID obj2)
{
TransformationData entityTrans;
gmtl::Vec3f newAnchor, newAxis1, newAxis2, scaleVec;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
joint = dJointCreateHinge2(world, 0);
// Attaching the Joint is done after the position and
// orientation calculation!
// dJointAttach(joint, obj1, obj2);
newAxis1 = axis1;
newAxis2 = axis2;
newAnchor = anchor;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
// scaleVec[0] = mainEntity->getXScale();
// scaleVec[1] = mainEntity->getYScale();
// scaleVec[2] = mainEntity->getZScale();
scaleVec = entityTrans.scale;
// scale Anchor-offset by mainEntity-scale value
newAnchor[0] *= scaleVec[0];
newAnchor[1] *= scaleVec[1];
newAnchor[2] *= scaleVec[2];
// scale Axes by mainEntity-scale value because of possible distortion
newAxis1[0] *= scaleVec[0];
newAxis1[1] *= scaleVec[1];
newAxis1[2] *= scaleVec[2];
gmtl::normalize(newAxis1);
newAxis2[0] *= scaleVec[0];
newAxis2[1] *= scaleVec[1];
newAxis2[2] *= scaleVec[2];
gmtl::normalize(newAxis2);
// get the Rotation of the mainEntity
// axAng[0] = mainEntity->getRotAngle();
// axAng[1] = mainEntity->getXRot();
// axAng[2] = mainEntity->getYRot();
// axAng[3] = mainEntity->getZRot();
// gmtl::set(entityRot, axAng);
entityRot = entityTrans.orientation;
// rotate Axes by mainEntity-rotation
newAxis1 *= entityRot;
newAxis2 *= entityRot;
// rotate Anchor-offset by mainEntity-rotation
newAnchor *= entityRot;
// transform new Anchor to world coordinates
/* newAnchor[0] += mainEntity->getXTrans();
newAnchor[1] += mainEntity->getYTrans();
newAnchor[2] += mainEntity->getZTrans();*/
newAnchor += entityTrans.position;
} // if
// create a helper body when necessary to avoid the
// Segmentation Fault that is thrown by ODE (v0.5)
// when attaching a Hinge2Joint to the static environment
if (obj1 == 0 || obj2 == 0)
{
helperBody = dBodyCreate(world);
dBodySetPosition(helperBody, newAnchor[0], newAnchor[1], newAnchor[2]);
helperJoint = dJointCreateFixed(world, 0);
dJointAttach(helperJoint, helperBody, 0);
dJointSetFixed(helperJoint);
if (obj1 == 0)
obj1 = helperBody;
else
obj2 = helperBody;
usedHelperJoint = true;
} // if
dJointAttach(joint, obj1, obj2);
dJointSetHinge2Anchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
dJointSetHinge2Axis1(joint, newAxis1[0], newAxis1[1], newAxis1[2]);
dJointSetHinge2Axis2(joint, newAxis2[0], newAxis2[1], newAxis2[2]);
} // attachJoint
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 resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
void PhysicsHinge2Joint::setWorld(const PhysicsWorldPtr &value )
{
PhysicsHinge2JointPtr tmpPtr(*this);
tmpPtr->id = dJointCreateHinge2(value->getWorldID(), 0);
PhysicsJointBase::setWorld(value);
}
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);
}
}
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]);
}
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 setupTest (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0,
0.25*M_PI,0.25*M_PI);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetFixed (joint);
return 1;
}
case 1: { // 1 body to static env
constructWorldForTest (0,1,
0.5*SIDE,0.5*SIDE,1, 0,0,0,
1,0,0, 1,0,0,
0,0);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],0);
dJointSetFixed (joint);
return 1;
}
case 2: { // 2 body with relative rotation
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0,
0.25*M_PI,-0.25*M_PI);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetFixed (joint);
return 1;
}
case 3: { // 1 body to static env with relative rotation
constructWorldForTest (0,1,
0.5*SIDE,0.5*SIDE,1, 0,0,0,
1,0,0, 1,0,0,
0.25*M_PI,0);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],0);
dJointSetFixed (joint);
return 1;
}
// ********** hinge joint
case 200: // 2 body
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,1,-1,1.41421356);
return 1;
case 220: // hinge angle polarity test
case 221: // hinge angle rate test
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,0,1);
max_iterations = 50;
return 1;
case 230: // hinge motor rate (and polarity) test
case 231: // ...with stops
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,0,1);
dJointSetHingeParam (joint,dParamFMax,1);
if (n==231) {
dJointSetHingeParam (joint,dParamLoStop,-0.5);
dJointSetHingeParam (joint,dParamHiStop,0.5);
}
return 1;
case 250: // limit bounce test (gravity down)
case 251: { // ...gravity up
constructWorldForTest ((n==251) ? 0.1 : -0.1, 2,
0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,1,0);
dJointSetHingeParam (joint,dParamLoStop,-0.9);
dJointSetHingeParam (joint,dParamHiStop,0.7854);
dJointSetHingeParam (joint,dParamBounce,0.5);
// anchor 2nd body with a fixed joint
dJointID j = dJointCreateFixed (world,0);
dJointAttach (j,body[1],0);
dJointSetFixed (j);
return 1;
}
// ********** slider
case 300: // 2 body
constructWorldForTest (0,2,
0,0,1, 0.2,0.2,1.2,
0,0,1, -1,1,0, 0,0.25*M_PI);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,1,1,1);
return 1;
case 320: // slider angle polarity test
case 321: // slider angle rate test
constructWorldForTest (0,2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
max_iterations = 50;
return 1;
case 330: // slider motor rate (and polarity) test
case 331: // ...with stops
constructWorldForTest (0, 2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
dJointSetSliderParam (joint,dParamFMax,100);
if (n==331) {
dJointSetSliderParam (joint,dParamLoStop,-0.4);
dJointSetSliderParam (joint,dParamHiStop,0.4);
}
return 1;
case 350: // limit bounce tests
case 351: {
constructWorldForTest ((n==351) ? 0.1 : -0.1, 2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
dJointSetSliderParam (joint,dParamLoStop,-0.5);
dJointSetSliderParam (joint,dParamHiStop,0.5);
dJointSetSliderParam (joint,dParamBounce,0.5);
// anchor 2nd body with a fixed joint
dJointID j = dJointCreateFixed (world,0);
dJointAttach (j,body[1],0);
dJointSetFixed (j);
return 1;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
case 421: // hinge-2 steering angle rate test
constructWorldForTest (0,2,
0.5*SIDE,0,1, -0.5*SIDE,0,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge2 (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
dJointSetHinge2Axis1 (joint,0,0,1);
dJointSetHinge2Axis2 (joint,1,0,0);
max_iterations = 50;
return 1;
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: // ...with stops
case 432: // hinge 2 wheel motor rate (+polarity) test
constructWorldForTest (0,2,
0.5*SIDE,0,1, -0.5*SIDE,0,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge2 (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
dJointSetHinge2Axis1 (joint,0,0,1);
dJointSetHinge2Axis2 (joint,1,0,0);
dJointSetHinge2Param (joint,dParamFMax,1);
dJointSetHinge2Param (joint,dParamFMax2,1);
if (n==431) {
dJointSetHinge2Param (joint,dParamLoStop,-0.5);
dJointSetHinge2Param (joint,dParamHiStop,0.5);
}
return 1;
// ********** angular motor joint
case 600: // test euler angle calculations
constructWorldForTest (0,2,
-SIDE*0.5,0,1, SIDE*0.5,0,1,
0,0,1, 0,0,1, 0,0);
joint = dJointCreateAMotor (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetAMotorNumAxes (joint,3);
dJointSetAMotorAxis (joint,0,1, 0,0,1);
dJointSetAMotorAxis (joint,2,2, 1,0,0);
dJointSetAMotorMode (joint,dAMotorEuler);
max_iterations = 200;
return 1;
// ********** universal joint
case 700: // 2 body
case 701:
case 702:
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
joint = dJointCreateUniversal (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetUniversalAnchor (joint,0,0,1);
dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
return 1;
case 720: // universal transmit torque test
case 721:
case 722:
case 730: // universal torque about axis 1
case 731:
case 732:
case 740: // universal torque about axis 2
case 741:
case 742:
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateUniversal (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetUniversalAnchor (joint,0,0,1);
dJointSetUniversalAxis1 (joint,0,0,1);
dJointSetUniversalAxis2 (joint, 1, -1,0);
max_iterations = 100;
return 1;
}
return 0;
}
void Machine::init()
{
int i;
pushtime=0;
energy=4;
dMass m;
for(i=0; i<3; i++)
{
wheel[i] = dBodyCreate(world);
dMassSetSphere(&m, 1, 5);
dMassAdjust(&m, 2);
dBodySetMass(wheel[i], &m);
sphere[i] = dCreateSphere(0, 5);
dGeomSetBody(sphere[i], wheel[i]);
}
dBodySetPosition(wheel[0], 0, 12, 6);
dBodySetPosition(wheel[1], -6, -7, 6);
dBodySetPosition(wheel[2], 6, -7, 6);
body[0] = dBodyCreate(world);
dMassSetBox(&m, 1, 20, 80, 5);
dMassAdjust(&m, 5);
dBodySetMass(body[0], &m);
dBodySetPosition(body[0], 0, 0, 6.5);
geom[0] = dCreateBox(0, 19, 27, 10);
dGeomSetBody(geom[0], body[0]);
body[1] = dBodyCreate(world);
dMassSetBox(&m, 1, 11, 5, 10);
dMassAdjust(&m, 0.3);
dBodySetMass(body[1], &m);
dBodySetPosition(body[1], 0, 17, 6.5);
geom[1] = dCreateBox(0, 11, 5, 10);
dGeomSetBody(geom[1], body[1]);
joint = dJointCreateSlider(world, 0);
dJointAttach(joint, body[0], body[1]);
dJointSetSliderAxis(joint, 0, 1, 0);
dJointSetSliderParam(joint, dParamLoStop, -9);
dJointSetSliderParam(joint, dParamHiStop, 0);
for(i=0; i<2; i++)
{
geom[i+2] = dCreateGeomTransform(0);
dGeomTransformSetCleanup(geom[i+2], 1);
finE[i] = dCreateBox(0, 7, 5, 10);
dGeomSetPosition(finE[i], i==0?-6.3:6.3, -2, 0);
dMatrix3 R;
dRFromAxisAndAngle(R, 0, 0, 1, i==0?M_PI/4:-M_PI/4);
dGeomSetRotation(finE[i], R);
dGeomTransformSetGeom(geom[i+2], finE[i]);
dGeomSetBody(geom[i+2], body[1]);
}
for(i=0; i<3; i++)
{
wheeljoint[i] = dJointCreateHinge2(world, 0);
dJointAttach(wheeljoint[i], body[0], wheel[i]);
const dReal *wPos = dBodyGetPosition(wheel[i]);
dJointSetHinge2Anchor(wheeljoint[i], wPos[0], wPos[1], wPos[2]);
dJointSetHinge2Axis1(wheeljoint[i], 0, 0, 1);
dJointSetHinge2Axis2(wheeljoint[i], 1, 0, 0);
dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0.8);
dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0.01);
dJointSetHinge2Param(wheeljoint[i], dParamLoStop, 0);
dJointSetHinge2Param(wheeljoint[i], dParamHiStop, 0);
dJointSetHinge2Param(wheeljoint[i], dParamCFM, 0.0001);
dJointSetHinge2Param(wheeljoint[i], dParamStopERP, 0.8);
dJointSetHinge2Param(wheeljoint[i], dParamStopCFM, 0.0001);
}
reset();
}