void vmWishboneCar::simForward(dReal mphSpeed)
{
dReal dsteer, realSpeed;
//dsteer = 0.0 - dJointGetHinge2Angle1(frWheel.joint); // target at zero steer
// calculate wheel real speed
dReal siSpeed= mphSpeed*1.6/3600.0*1000; // convert ot m/s
realSpeed = -siSpeed/frWheel.radius;
// steer
/*dJointSetHinge2Param(frWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01);
dJointSetHinge2Param(frWheel.joint, dParamFMax, 1000.0);
dJointSetHinge2Param (frWheel.joint,dParamLoStop,-40.0*M_PI/180.0);
dJointSetHinge2Param (frWheel.joint,dParamHiStop,40.0*M_PI/180.0);
dJointSetHinge2Param (frWheel.joint,dParamFudgeFactor,0.1);
dJointSetHinge2Param(flWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01);
dJointSetHinge2Param(flWheel.joint, dParamFMax, 1000.0);
dJointSetHinge2Param (flWheel.joint,dParamLoStop,-40.0*M_PI/180.0);
dJointSetHinge2Param (flWheel.joint,dParamHiStop,40.0*M_PI/180.0);
dJointSetHinge2Param (flWheel.joint,dParamFudgeFactor,0.1);*/
// speed
if (brakeYes)
{
dReal factor= 0.1;
dJointSetHingeParam(rrSuspension.jRotorMid, dParamVel, dJointGetHingeParam(rrSuspension.jRotorMid,dParamVel)*factor);
dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 1000.0);
dJointSetHingeParam(rlSuspension.jRotorMid, dParamVel, dJointGetHingeParam(rlSuspension.jRotorMid,dParamVel)*factor);
dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 1000.0);
dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(frSuspension.jRotorMid,dParamVel2)*factor);
dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 1000.0);
dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(flSuspension.jRotorMid,dParamVel2)*factor);
dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 1000.0);
}
else
{
// turn off rear wheels
dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 0.0);
dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 0.0);
// set up front wheels
dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed);
dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 500.0);
dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed);
dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 500.0);
}
}
static float get_phys_joint_attr(dJointID j, int p)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: return (float) dJointGetHingeParam (j, p);
case dJointTypeSlider: return (float) dJointGetSliderParam (j, p);
case dJointTypeHinge2: return (float) dJointGetHinge2Param (j, p);
case dJointTypeUniversal: return (float) dJointGetUniversalParam(j, p);
default: return 0.0f;
}
}
void vmWishboneCar::simControl()
{
// auto align steering angle
if (manualYes)
steerGain= 10;
else
{
steerGain= 10;
steer*= 0.98;
speed*= 0.999;
}
dReal dsteer, realSpeed;
//dsteer = bounded(steer,-40.0,40.0) - dJointGetHingeAngle(frWheel.joint);
realSpeed = -bounded(speed, -14*M_PI, 30*M_PI);
// steer
/*dJointSetHingeParam(frWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01);
dJointSetHingeParam(frWheel.joint, dParamFMax, 1000.0);
dJointSetHingeParam (frWheel.joint,dParamLoStop,-40.0*M_PI/180.0);
dJointSetHingeParam (frWheel.joint,dParamHiStop,40.0*M_PI/180.0);
dJointSetHingeParam (frWheel.joint,dParamFudgeFactor,0.1);
dJointSetHinge2Param(flWheel.joint, dParamVel, steerGain*dsteer+0.01*dsteer/0.01);
dJointSetHinge2Param(flWheel.joint, dParamFMax, 1000.0);
dJointSetHinge2Param (flWheel.joint,dParamLoStop,-40.0*M_PI/180.0);
dJointSetHinge2Param (flWheel.joint,dParamHiStop,40.0*M_PI/180.0);
dJointSetHinge2Param (flWheel.joint,dParamFudgeFactor,0.1);*/
// speed
if (brakeYes)
{
dReal factor= 0.1;
dJointSetHingeParam(rrSuspension.jRotorMid, dParamVel
,dJointGetHingeParam(rrSuspension.jRotorMid,dParamVel)*factor);
dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 1000.0);
dJointSetHingeParam(rlSuspension.jRotorMid, dParamVel
,dJointGetHingeParam(rlSuspension.jRotorMid,dParamVel)*factor);
dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 1000.0);
dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(frWheel.joint,dParamVel2)*factor);
dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 1000.0);
dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed);//dJointGetHinge2Param(flWheel.joint,dParamVel2)*factor);
dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 1000.0);
}
else
{
// turn off rear wheels
dJointSetHingeParam(rrSuspension.jRotorMid, dParamFMax, 0.0);
dJointSetHingeParam(rlSuspension.jRotorMid, dParamFMax, 0.0);
// set up front wheels
dJointSetHingeParam(frSuspension.jRotorMid, dParamVel, realSpeed);
dJointSetHingeParam(frSuspension.jRotorMid, dParamFMax, 500.0);
dJointSetHingeParam(flSuspension.jRotorMid, dParamVel, realSpeed);
dJointSetHingeParam(flSuspension.jRotorMid, dParamFMax, 500.0);
//printf("real speed: %f", realSpeed);
}
// reset manual mode flag
manualYes= 0;
}
void PhysicsHingeJoint::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 = dJointCreateHinge(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateHinge(getWorld()->getWorldID(), 0);
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetHingeParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetHingeParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetHingeParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetHingeParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetHingeParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetHingeParam(_JointID,dParamStopERP));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & AnchorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeAnchor(_JointID, getAnchor().x(), getAnchor().y(), getAnchor().z());
}
if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetHingeParam(_JointID, dParamStopCFM, getStopCFM());
}
}
void VelocityMotor::VelocitySensor::updateValue()
{
data.floatValue = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeParam(joint->joint, dParamVel)
: dJointGetSliderParam(joint->joint, dParamVel);
}
