Rope::Rope(Object* obja, Body* bodya, Object* objb, Body* bodyb)
: obja_(obja), objb_(objb), selected_(false)
{
vec apos = bodya->position();
proxya_.set_position(apos);
proxya_.set_velocity(bodya->velocity());
proxya_.set_mass(0.01, 1);
hingea_ = dJointCreateHinge(LEVEL->world, 0);
dJointAttach(hingea_, proxya_.body_id(), bodya->body_id());
dJointSetHingeAxis(hingea_, 0, 0, 1);
vec bpos = bodyb->position();
proxyb_.set_position(bpos);
proxyb_.set_velocity(bodyb->velocity());
proxyb_.set_mass(0.01, 1);
hingeb_ = dJointCreateHinge(LEVEL->world, 0);
dJointAttach(hingeb_, proxyb_.body_id(), bodyb->body_id());
dJointSetHingeAxis(hingeb_, 0, 0, 1);
rope_ = dJointCreateSlider(LEVEL->world, 0);
vec axis = bpos - apos;
dJointAttach(rope_, proxya_.body_id(), proxyb_.body_id());
dJointSetSliderAxis(rope_, axis.x, axis.y, 0);
dJointSetSliderParam(rope_, dParamLoStop, 0);
dJointSetSliderParam(rope_, dParamStopCFM, 0.25);
dJointSetSliderParam(rope_, dParamStopERP, 0.01);
ext_ = base_ext_ = axis.norm();
}
void Slider::createPhysics()
{
ASSERT(axis);
ASSERT(axis->motor);
//
axis->create();
//
::PhysicalObject::createPhysics();
// find bodies to connect
Body* parentBody = dynamic_cast<Body*>(parent);
ASSERT(!parentBody || parentBody->body);
ASSERT(!children.empty());
Body* childBody = dynamic_cast<Body*>(*children.begin());
ASSERT(childBody);
ASSERT(childBody->body);
// create joint
joint = dJointCreateSlider(Simulation::simulation->physicalWorld, 0);
dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
//set Slider joint parameter
Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
dJointSetSliderAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
if(axis->cfm != -1.f)
dJointSetSliderParam(joint, dParamCFM, dReal(axis->cfm));
if(axis->deflection)
{
const Axis::Deflection& deflection = *axis->deflection;
float minSliderLimit = deflection.min;
float maxSliderLimit = deflection.max;
if(minSliderLimit > maxSliderLimit)
minSliderLimit = maxSliderLimit;
//Set physical limits to higher values (+10%) to avoid strange Slider effects.
//Otherwise, sometimes the motor exceeds the limits.
float internalTolerance = (maxSliderLimit - minSliderLimit) * 0.1f;
if(dynamic_cast<ServoMotor*>(axis->motor))
{
minSliderLimit -= internalTolerance;
maxSliderLimit += internalTolerance;
}
dJointSetSliderParam(joint, dParamLoStop, dReal(minSliderLimit));
dJointSetSliderParam(joint, dParamHiStop, dReal(maxSliderLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetSliderParam(joint, dParamLoStop, dReal(minSliderLimit));
if(deflection.stopCFM != -1.f)
dJointSetSliderParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
if(deflection.stopERP != -1.f)
dJointSetSliderParam(joint, dParamStopERP, dReal(deflection.stopERP));
}
// create motor
axis->motor->create(this);
OpenGLTools::convertTransformation(rotation, translation, transformation);
}
void CubeBasePiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix)
{
dBodySetPosition(body,x,y,z);
dJointID connectingJoint = dJointCreateSlider(world,jointGroup);
dJointAttach(connectingJoint,otherBody,body);
dJointSetSliderParam(connectingJoint,dParamLoStop,0);
dJointSetSliderParam(connectingJoint,dParamHiStop,0);
}
/**
* This method is called if the joint should be attached.
* It creates the ODE-joint, calculates the current
* axis-orientation and attaches the Joint.
* @param obj1 first ODE-object to attach with
* @param obj2 second ODE-object to attach with
**/
void SliderJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
gmtl::Vec3f newAxis;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
gmtl::Vec3f scale = gmtl::Vec3f(1,1,1);
TransformationData entityTrans;
joint = dJointCreateSlider(world, 0);
dJointAttach(joint, obj1, obj2);
newAxis = axis;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// scale Axis by mainEntity-scale value because of possible distortion
/* scale[0] = mainEntity->getXScale();
scale[1] = mainEntity->getYScale();
scale[2] = mainEntity->getZScale();*/
scale = entityTrans.scale;
newAxis[0] *= scale[0];
newAxis[1] *= scale[1];
newAxis[2] *= scale[2];
gmtl::normalize(newAxis);
// 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 Axis by mainEntity-rotation
newAxis *= entityRot;
} // if
dJointSetSliderAxis(joint, newAxis[0], newAxis[1], newAxis[2]);
// set the minimum and maximum joint-positions (if set).
if (posSet)
{
gmtl::Vec3f minDistVec = newAxis * minPos;
gmtl::Vec3f maxDistVec = newAxis * maxPos;
minDistVec[0] *= scale[0];
minDistVec[1] *= scale[1];
minDistVec[2] *= scale[2];
maxDistVec[0] *= scale[0];
maxDistVec[1] *= scale[1];
maxDistVec[2] *= scale[2];
dJointSetSliderParam(joint, dParamLoStop, gmtl::dot(minDistVec,newAxis));
dJointSetSliderParam(joint, dParamHiStop, gmtl::dot(maxDistVec,newAxis));
} // if
} // attachJoint
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;
}
}
static void soy_joints_slider_real_create (soyjointsJoint* base) {
soyjointsSlider * self;
soyscenesScene* _tmp0_;
struct dxWorld* _tmp1_;
struct dxJoint* _tmp2_;
self = (soyjointsSlider*) base;
_tmp0_ = ((soyjointsJoint*) self)->scene;
_tmp1_ = _tmp0_->world;
_tmp2_ = dJointCreateSlider (_tmp1_, NULL);
_dJointDestroy0 (((soyjointsJoint*) self)->joint);
((soyjointsJoint*) self)->joint = _tmp2_;
}
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;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE2(0);
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);
dCloseODE();
return 0;
}
OscSlideODE::OscSlideODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase *parent,
OscObject *object1, OscObject *object2,
double ax, double ay, double az)
: OscSlide(name, parent, object1, object2, ax, ay, az)
{
m_response = new OscResponse("response",this);
dJointID odeJoint = dJointCreateSlider(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetSliderAxis(odeJoint, ax, ay, az);
/* TODO access to dJointGetSliderPosition */
printf("[%s] Sliding joint created between %s and %s on axis (%f,%f,%f)\n",
simulation()->type_str(),
object1->c_name(), object2?object2->c_name():"world",
ax, ay, az);
}
/**
* Creates a new IBDS::SliderJoint.
*
* @param body1 the first body to connect the joint to.
* @param body2 the second body to connect the joint to.
* @return a new IBDS::SliderJoint.
*/
dJointID ODE_SliderJoint::createJoint(dBodyID body1, dBodyID body2) {
if(mFirstAxisPoint->get().equals(mSecondAxisPoint->get())) {
Core::log("Invalid axes for ODE_SliderJoint.");
return 0;
}
//if one of the bodyIDs is null, the joint is connected to a static object.
dJointID newJoint = dJointCreateSlider(mWorldID, mGeneralJointGroup);
dJointAttach(newJoint, body1, body2);
Vector3D axis(mSecondAxisPoint->getX() - mFirstAxisPoint->getX(),
mSecondAxisPoint->getY() - mFirstAxisPoint->getY(),
mSecondAxisPoint->getZ() - mFirstAxisPoint->getZ());
axis.normalize();
dJointSetSliderAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
dJointSetSliderParam(newJoint, dParamLoStop, mMinPositionValue->get());
dJointSetSliderParam(newJoint, dParamHiStop, mMaxPositionValue->get());
dJointSetSliderParam(newJoint, dParamVel, 0.0);
dJointSetSliderParam(newJoint, dParamFMax, mFrictionValue->get());
return newJoint;
}
void JointSlider::Create(World &world)
{
if(this->_id) dJointDestroy(this->_id);
_id = dJointCreateSlider(world.Id(), 0);
}
//Overloaded Create
void JointSlider::Create(World &world, JointGroup &jointGroup)
{
if(this->_id) dJointDestroy(this->_id);
_id = dJointCreateSlider(world.Id(), jointGroup.Id());
}
void CODESliderJoint::create(dWorld& world, dJointGroupID groupID)
{
mID = dJointCreateSlider(world, groupID);
}
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();
}
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
// chassis pin joints
const dReal *pos;
snow->jChassisUp= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body);
dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0);
pos= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jChassisUp,pos[0]
,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]);
const dReal *pos1;
snow->jChassisLow= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body);
dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0);
pos1= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jChassisLow, pos1[0]
, pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]);
// rotor ball joint
/*const dReal *p2;
jRotorUp= dJointCreateBall(world,0);
dJointAttach(jRotorUp, uplink.body, hlink.body);
p2= dBodyGetPosition(uplink.body);
dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]);
const dReal *p3;
jRotorLow= dJointCreateBall(world,0);
dJointAttach(jRotorLow, lowlink.body,hlink.body);
p3= dBodyGetPosition(lowlink.body);
dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/
const dReal *p2;
snow->jRotorUp= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body);
p2= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jRotorUp, p2[0]
,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]);
dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0);
const dReal *p3;
snow->jRotorLow= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body);
p3= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jRotorLow, p3[0]
,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]);
dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0);
// rotor hinge joint
const dReal *pw= dBodyGetPosition(wnow->body);
snow->jRotorMid= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body);
dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0);
dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]);
// strut joint
const dReal *ps1, *ps2;
dReal angle= -shiftSign*strutAngle;
ps1= dBodyGetPosition(snow->upstrut.body);
ps2= dBodyGetPosition(snow->lowstrut.body);
snow->jStrutUp= dJointCreateBall(world,0);
dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body);
//dJointSetFixed(snow->jStrutUp);
dJointSetBallAnchor(snow->jStrutUp, ps1[0]
,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle))
,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle)));
snow->jStrutLow= dJointCreateBall(world,0);
dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body);
dJointSetBallAnchor(snow->jStrutLow, ps2[0]
,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle))
,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle)));
// struct sliding joint
snow->jStrutMid= dJointCreateSlider(world,0);
dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body);
dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle)));
// set joint feedback
wnow->feedback= new dJointFeedback;
dJointSetFeedback(snow->jStrutMid,wnow->feedback);
// suspension slider
/*snow->jLowSpring= dJointCreateLMotor(world,0);
dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body);
dJointSetLMotorNumAxes(snow->jLowSpring, 1);
dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.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;
}
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, 20);
int i;
for (i=0; i<SEGMCNT; i++)
{
segbodies[i] = dBodyCreate (world);
dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dBodySetMass (segbodies[i], &m);
seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dGeomSetBody (seggeoms[i], segbodies[i]);
dSpaceAdd (space, seggeoms[i]);
}
for (i=0; i<SEGMCNT-1; i++)
{
hinges[i] = dJointCreateHinge (world,0);
dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
dJointSetHingeAxis (hinges[i], 0,1,0);
dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
// NOTE:
// Here we tell ODE where to put the feedback on the forces for this hinge
dJointSetFeedback (hinges[i], jfeedbacks+i);
stress[i]=0;
}
for (i=0; i<STACKCNT; i++)
{
stackbodies[i] = dBodyCreate(world);
dMassSetBox (&m, 2.0, 2, 2, 0.6);
dBodySetMass(stackbodies[i],&m);
stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
dGeomSetBody(stackgeoms[i], stackbodies[i]);
dBodySetPosition(stackbodies[i], 0,0,8+2*i);
dSpaceAdd(space, stackgeoms[i]);
}
sliders[0] = dJointCreateSlider (world,0);
dJointAttach(sliders[0], segbodies[0], 0);
dJointSetSliderAxis (sliders[0], 1,0,0);
dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
sliders[1] = dJointCreateSlider (world,0);
dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
dJointSetSliderAxis (sliders[1], 1,0,0);
dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
groundgeom = dCreatePlane(space, 0,0,1,0);
for (i=0; i<SEGMCNT; i++)
colours[i]=0.0;
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy seggeoms, then space, then the world.
for (i=0; i<SEGMCNT; i++)
dGeomDestroy (seggeoms[i]);
for (i=0; i<STACKCNT; i++)
dGeomDestroy (stackgeoms[i]);
dSpaceDestroy(space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void makeRobot_Nleg()
{
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
dReal torso_m = 50.0; // Mass of body
// torso_m[segment] = 10.0;
dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments
//for four legs
// dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
// {-cx1,-cx1,-cx1}};
dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
{0,0,0}};
dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
{-cy1,-cy1,-cy1}};
dReal z[num_legs][num_links] = { // Position of each link (z coordinate)
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
dReal r[num_links] = { r1, r2, r3}; // radius of leg segment
dReal length[num_links] = { l1, l2, l3}; // Length of leg segment
dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment
// //for one leg
// dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
// dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
// dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
//for four legs
dReal axis_x[num_legs][num_links];
dReal axis_y[num_legs][num_links];
dReal axis_z[num_legs][num_links];
for(int i = 0; i < num_legs; ++i) {
axis_x[i][0] = 0.0;
axis_x[i][1] = 1.0;
axis_x[i][2] = 0.0;
axis_y[i][0] = 1.0;
axis_y[i][1] = 0.0;
axis_y[i][2] = 1.0;
axis_z[i][0] = 0.0;
axis_z[i][1] = 0.0;
axis_z[i][2] = 0.0;
}
// For mation of the body
dMass mass;
torso[segment].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
dBodySetMass(torso[segment].body,&mass);
torso[segment].geom = dCreateBox(space,lx, ly, lz);
dGeomSetBody(torso[segment].geom, torso[segment].body);
dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ);
// Formation of leg
dMatrix3 R; // Revolution queue
dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
THETA[segment][i][j] = 0;
leg[segment][i][j].body = dBodyCreate(world);
if (j == 0)
dBodySetRotation(leg[segment][i][j].body,R);
dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
dBodySetMass(leg[segment][i][j].body, &mass);
//if(i==1 and j==2) //to set the length of one leg differently
//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
//else
leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
}
}
// Formation of joints (and connecting them up)
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[segment][i][j].joint = dJointCreateHinge(world, 0);
if (j == 0){
dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop, .50*M_PI);
}
else
dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
}
}
}
#ifdef USE_NLEG_ROBOT
// link torsos
for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
switch (hingeType) {
case 1: //Hinge Joint, X axis (back-breaker)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
break;
case 2: //Hinge Joint, Y axis (???)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
break;
case 3: //Hinge Joint, Z axis (snake-like)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
break;
case 4: // Slider, Y axis (??)
torso[segment].joint = dJointCreateSlider(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
break;
case 5: // Slider, X axis (extendo)
torso[segment].joint = dJointCreateSlider(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);
break;
case 6: //Universal Joint
torso[segment].joint = dJointCreateUniversal(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);
break;
case 7: //Ball Joint
torso[segment].joint = dJointCreateBall(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
break;
case 8: // Fixed
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(torso[segment].joint, dParamHiStop, 0.00*M_PI);
break;
default:
assert(false); // not a valid hinge type
break;
}
}
#endif
}
void PhysicsSliderJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
_JointID = dJointCreateSlider(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateSlider(getWorld()->getWorldID(), 0);
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetSliderParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetSliderParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetSliderParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetSliderParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetSliderParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetSliderParam(_JointID,dParamStopERP));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamStopCFM, getStopCFM());
}
}
void WorldPhysics::AI() {
if (tmp_wait>0) {tmp_wait--;return;}
if (bulldozer_state==8) speed=0;
const dReal* BPosition=bulldozer->getPosition();
// const dReal* BRotation=bulldozer->getRotation();
dReal minDistance=150; //~100 * sqrt(2)
Item* tmp;
int currentItemN=-1;
if (items.size()==0) return;
for (int i=0;i<items.size();i++) {
const dReal* ItemPosition=items[i]->getPosition();
if (ItemPosition[2]<0) items[i]->state=2;
if (items[i]->state==2) continue;
if (items[i]->state==1) {currentItemN=i;break;}
if (items[i]->state==0) {
dReal tmpmin=sqrt((ItemPosition[0]-BPosition[0])*
(ItemPosition[0]-BPosition[0]) +
(ItemPosition[1]-BPosition[1])*
(ItemPosition[1]-BPosition[1]));
if (tmpmin<minDistance) {
minDistance=tmpmin;
currentItemN=i;
}
}
}
if (currentItemN==-1) {
//bulldozer_state=0;
generateItems();
return;
}
/*
for (int i=0;i<items.size();i++) {
const dReal* tmp_pos=items[i]->getPosition();
if (tmp_pos[2]<0) items[i]->state=2;
if (items[i]->state==1) {
currentItemN=i;
break;
}
if (items[i]->state==2) currentItemN++;
}
if (currentItemN>=items.size()) return;
*/
tmp=items[currentItemN];
const dReal* ItemPosition=tmp->getPosition();
switch (tmp->state) {
case 0: { // Выбрали нужный кубик
tmp->state=1;
bulldozer_state=1; // Поворот до кубика
} break;
case 1: { // Надо к нему подъехать
switch (bulldozer_state) {
case 1:{
speed=0;
if (RotateTo(ItemPosition[0]-BPosition[0],ItemPosition[1]-BPosition[1])) bulldozer_state=2;
} break;
case 2: {
if (RotateTo(ItemPosition[0]-BPosition[0],ItemPosition[1]-BPosition[1])) {
speed=3;
if (((ItemPosition[0]-BPosition[0])*
(ItemPosition[0]-BPosition[0]) +
(ItemPosition[1]-BPosition[1])*
(ItemPosition[1]-BPosition[1]))<50) {
speed=0;
bulldozer_state=3;
}
} else {
speed=0;
bulldozer_state=1;
}
} break;
case 3: {
cheat_joint=dJointCreateSlider(world,0);
dJointAttach(cheat_joint,bulldozer->body,tmp->body);
dJointSetSliderAxis(cheat_joint,0,0,1);
//dJointSetSliderParam (cheat_joint, dParamCFM, 0.5);
bulldozer_state=4;
}break;
case 4: {
if (RotateTo(100-BPosition[0],0-BPosition[1])) bulldozer_state=5;
}break;
case 5: {
if (RotateTo(100-BPosition[0],0-BPosition[1])) {
speed=3;
if (sqrt((100-BPosition[0])*
(100-BPosition[0]) +
(0-BPosition[1])*
(0-BPosition[1]))<(sqrt(2)*25+LENGTH/2+RADIUS)) {
speed=0;
bulldozer_state=6;
}
} else {
speed=0;
bulldozer_state=4;
}
} break;
case 6: {
dJointDestroy(cheat_joint);
dBodyAddForce(tmp->body,100-BPosition[0],0-BPosition[1], 5);
bulldozer_state=7;
} break;
case 7: {
speed=-10;
tmp_wait=70;
bulldozer_state=8;
} break;
case 8: {
speed=0;
tmp->state=2;
} break;
}
} break;
}
}
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);
}
}
