void vmWishboneCar::drawLinkage(vm::WheelLoc loc)
{
// select wheel
vmWishbone *snow;
switch (loc) {
case vm::WheelLoc::FR:
snow= &frSuspension;
break;
case vm::WheelLoc::FL:
snow= &flSuspension;
break;
case vm::WheelLoc::RR:
snow= &rrSuspension;
break;
case vm::WheelLoc::RL:
snow= &rlSuspension;
break;
default:
break;
}
// setup linkage
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
const dReal *pos4,*R4,*pos5,*R5;
dsSetColorAlpha(0.0,0.0,1.0, 0.5);
pos1 = dBodyGetPosition(snow->uplink.body);
R1 = dBodyGetRotation(snow->uplink.body);
dsDrawCylinderD(pos1,R1,snow->uplink.width,snow->uplink.radius);
dsSetColor(0.0,1.0,0.0);
pos2 = dBodyGetPosition(snow->hlink.body);
R2 = dBodyGetRotation(snow->hlink.body);
dsDrawCylinderD(pos2,R2,snow->hlink.width,snow->hlink.radius);
dsSetColorAlpha(1.0,0.0,1.0, 0.5);
pos3 = dBodyGetPosition(snow->lowlink.body);
R3 = dBodyGetRotation(snow->lowlink.body);
dsDrawCylinderD(pos3,R3,snow->lowlink.width,snow->lowlink.radius);
// setup spring and damper
dsSetColorAlpha(0.1,0.7,1.0, 0.5);
pos4= dBodyGetPosition(snow->upstrut.body);
R4= dBodyGetRotation(snow->upstrut.body);
dsDrawCylinderD(pos4,R4,snow->upstrut.width,snow->upstrut.radius);
dsSetColorAlpha(1.0,0.7,1.0, 0.5);
pos5= dBodyGetPosition(snow->lowstrut.body);
R5= dBodyGetRotation(snow->lowstrut.body);
dsDrawCylinderD(pos5,R5,snow->lowstrut.width,snow->lowstrut.radius);
}
void vmWishboneCar::simDraw()
{
// draw suspension linkages
drawLinkage(vm::FR);
drawLinkage(vm::FL);
drawLinkage(vm::RR);
drawLinkage(vm::RL);
// draw wheels
dsSetColorAlpha(1.0,1.0,0.0,0.5);
dsDrawCylinderD(dBodyGetPosition(frWheel.body),dBodyGetRotation(frWheel.body),frWheel.length,frWheel.radius);
dsDrawCylinderD(dBodyGetPosition(flWheel.body),dBodyGetRotation(frWheel.body),flWheel.length,flWheel.radius);
dsDrawCylinderD(dBodyGetPosition(rrWheel.body),dBodyGetRotation(rrWheel.body),rrWheel.length,rrWheel.radius);
dsDrawCylinderD(dBodyGetPosition(rlWheel.body),dBodyGetRotation(rlWheel.body),rlWheel.length,rlWheel.radius);
// draw chassis
dsSetColorAlpha(1.0,0.0,0.0,0.5); // red
dsDrawBoxD(dBodyGetPosition(chassis.body),dBodyGetRotation(chassis.body)
,chassis.sides);
}
void CManipulator::drawCylinder (dGeomID id, float R, float G, float B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dReal l,r;
dGeomCylinderGetParams(id,&r,&l);
dsDrawCylinderD(pos, rot, (float)l,(float)r);
}
void CManipulator::drawCylinderWithBall (dGeomID idGeom, float R, float G, float B)
{
if (!idGeom)
return;
dVector3 pos1;
const dReal *pos = dGeomGetPosition (idGeom);
const dReal *rot = dGeomGetRotation (idGeom);
dsSetColor (R,G,B);
dReal l,r;
dGeomCylinderGetParams(idGeom,&r,&l);
dsDrawCylinderD(pos, rot, (float)l,(float)r);
pos1[0]=pos[0]+(l/2.0)*rot[2]*-1;
pos1[1]=pos[1]+(l/2.0)*rot[6]*-1;
pos1[2]=pos[2]+(l/2.0)*rot[10]*-1;
dsDrawSphereD(pos1,rot,float(r+BORDER));
pos1[0]=pos[0]+(l/2.0)*rot[2];
pos1[1]=pos[1]+(l/2.0)*rot[6];
pos1[2]=pos[2]+(l/2.0)*rot[10];
dsDrawSphereD(pos1,rot,float(r+BORDER));
}
void SkidSteeringVehicle::draw() {
{
dsSetColor(0, 0, 1);
const dReal *pos = dGeomGetPosition(this->vehicleGeom);
const dReal *R = dGeomGetRotation(this->vehicleGeom);
dReal sides[3];
dGeomBoxGetLengths(this->vehicleGeom, sides);
dsDrawBoxD(pos, R, sides);
}
dsSetColor(1, 1, 0);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
const dReal *pos = dGeomGetPosition(this->wheelGeom[fr][lr]);
const dReal *R = dGeomGetRotation(this->wheelGeom[fr][lr]);
dReal radius, length;
dGeomCylinderGetParams(this->wheelGeom[fr][lr], &radius, &length);
dsDrawCylinderD(pos, R, length, radius);
}
}
}
