void LER2016::MoveAround(){
memset(&vs2, 0, sizeof(vs2));
vs2.version = 2;
GetStatusEx(&vs2);
double sinTurn = sin(2 * PI * TURN_proc);
double cosTurn = cos(2 * PI * TURN_proc);
if (FORWARDgear==1)//MOVING FORWARD
{
if (i3 < 0){//LEFT TURN
d_hdg = d_hdg + .00001;//rate of steering change of heading
vs2.surf_hdg -= d_hdg;
//sprintf(oapiDebugString(), "turn %d SURFHDG %2.2f", i3, vs2.surf_hdg);
if (vs2.surf_hdg<0){ vs2.surf_hdg += 2 * PI; }
rot += 0.005;//animation axle wheel turn rate
if (rot > .125) rot = .125;
}
if (i3 > 0){//RIGHT TURN
rot -= 0.005;//animation axle wheel turn rate
if (rot < -.125) rot = -.125;
d_hdg = d_hdg + .00001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
//sprintf(oapiDebugString(), "turn %d SURFHDG %2.2f", i3, vs2.surf_hdg);
if (vs2.surf_hdg>2 * PI){ vs2.surf_hdg -= 2 * PI; }
}
if (i3 == 0) {// straight so straighten wheels
// sprintf (oapiDebugString (), "turn %2.2f cos %2.2f sin %2.2f", TURN_proc, cosTurn, sinTurn );
if (abs(rot) <= 0.005) rot = 0;
else if (rot < 0) rot += 0.005;
else if (rot > 0) rot -= 0.005;
d_hdg = 0;
}
if (TURN_proc == 0){//wheels are straight so move with heading
d_lat = (targetSpeed*oapiGetSimStep()*cos(vs2.surf_hdg) / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*sin(vs2.surf_hdg) / each_deg);
}
else {//wheels are not straight so move no matter the heading move to the left forward
d_lat = (targetSpeed*oapiGetSimStep()*(cosTurn*1) / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*(sinTurn*1) / each_deg);
}
//sprintf(oapiDebugString(), "turn %2.2f cos %2.2f sin %2.2f lat %2.2f lng %2.2f", TURN_proc, cosTurn, sinTurn, d_lat, d_lng);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
lng = vs2.surf_lng;
lat = vs2.surf_lat;
hdg = vs2.surf_hdg;
DefSetStateEx(&vs2);
return;
}