Thanks. So the turning stuff should come first?
Thanks. So the turning stuff should come first?
iMeshMain = AddMesh("LER8");
SetMeshVisibilityMode(iMeshMain, MESHVIS_ALWAYS);
void LER2016::MoveAround(){
double theta = 0;
Max_Steering_Angle = 360;
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)
{
if (i3 < 0){
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_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){
rot -= 0.005;//animation axle wheel turn rate
if (rot < -.125) rot = -.125;
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
if (vs2.surf_hdg>2 * PI){ vs2.surf_hdg -= 2 * PI; }
}
if (i3 == 0) {// straight so straighten wheels
if (abs(rot) <= 0.005) rot = 0;
else if (rot < 0) rot += 0.005;
else if (rot > 0) rot -= 0.005;
double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
double d_lat = (targetSpeed*oapiGetSimStep()*cos(vs2.surf_hdg) / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sin(vs2.surf_hdg) / each_deg);
//double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
//double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
if (REVERSEgear == 1)
{
if (i3 > 0){
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_hdg;
if (vs2.surf_hdg<0){ vs2.surf_hdg += 2 * PI; }
}
if (i3 < 0){
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
if (vs2.surf_hdg>2 * PI){ vs2.surf_hdg -= 2 * PI; }
}
double d_lat = (targetSpeed*oapiGetSimStep()*cos(vs2.surf_hdg)/each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sin(vs2.surf_hdg) /each_deg);
vs2.surf_lat -= d_lat*RAD;
vs2.surf_lng -= d_lng*RAD;
}
// sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f ANg %2.2f", targetSpeed, vs2.surf_hdg, AngSpeed);
lng = vs2.surf_lng;
lat = vs2.surf_lat;
hdg = vs2.surf_hdg;
{
MATRIX3 rot1 = RotationMatrix(_V(0 * RAD, (90 * RAD - lng), 0 * RAD), TRUE);
MATRIX3 rot2 = RotationMatrix(_V(-lat + 0 * RAD, 0, 0 * RAD), TRUE);
MATRIX3 rot3 = RotationMatrix(_V(0, 0, 180 * RAD + hdg), TRUE);
MATRIX3 rot4 = RotationMatrix(_V(90 * RAD, 0, 0), TRUE);
//MATRIX3 rot4 = RotationMatrix(_V(90 * RAD - pitch_angle, 0, roll_angle), TRUE);
// MATRIX3 rot_pitch = RotationMatrix(_V(pitch_angle, 0, 0));
// MATRIX3 rot_roll = RotationMatrix(_V(0, 0, roll_angle));
MATRIX3 RotMatrix_Def = mul(rot1, mul(rot2, mul(rot3, rot4)));
vs2.arot.x = atan2(RotMatrix_Def.m23, RotMatrix_Def.m33);
vs2.arot.y = -asin(RotMatrix_Def.m13);
vs2.arot.z = atan2(RotMatrix_Def.m12, RotMatrix_Def.m11);
}
vs2.vrot.x = 2.35;
//sprintf(oapiDebugString(), "targetSpeed %2.2f d_lat %2.2f d_lng %2.2f,rt %2.2f degree %2.2f hdg %2.2f", targetSpeed, vs2.surf_lat, vs2.surf_lng, rt, each_deg, vs2.surf_hdg);
//sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f d_hdg %2.2f,hdg %2.2f", targetSpeed, i, d_hdg, vs2.surf_hdg);
DefSetStateEx(&vs2);
return;
}
void LER2016::MoveAround(){
double theta = 0;
Max_Steering_Angle = 360;
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)
{
if (i3 < 0){
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_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){
rot -= 0.005;//animation axle wheel turn rate
if (rot < -.125) rot = -.125;
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
if (vs2.surf_hdg>2 * PI){ vs2.surf_hdg -= 2 * PI; }
}
if (i3 == 0) {// straight so straighten wheels
if (abs(rot) <= 0.005) rot = 0;
else if (rot < 0) rot += 0.005;
else if (rot > 0) rot -= 0.005;
double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
double d_lat = (targetSpeed*oapiGetSimStep()*cos(vs2.surf_hdg) / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sin(vs2.surf_hdg) / each_deg);
//double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
//double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
void LER2016::MoveAround(){
double theta = 0;
Max_Steering_Angle = 360;
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)
{
if (i3 < 0){
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_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){
rot -= 0.005;//animation axle wheel turn rate
if (rot < -.125) rot = -.125;
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_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;
double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
if (TURN_proc == 0){
double d_lat = (targetSpeed*oapiGetSimStep()*cos(vs2.surf_hdg) / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sin(vs2.surf_hdg) / each_deg);
//double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
//double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
}
}
if (REVERSEgear == 1)
{
if (i3 > 0){
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_hdg;
if (vs2.surf_hdg<0){ vs2.surf_hdg += 2 * PI; }
}
if (i3 < 0){
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
if (vs2.surf_hdg>2 * PI){ vs2.surf_hdg -= 2 * PI; }
}
if (i3 == 0) {// straight so straighten wheels
if (abs(rot) <= 0.005) rot = 0;
else if (rot < 0) rot += 0.005;
else if (rot > 0) rot -= 0.005;
double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat -= d_lat*RAD;
vs2.surf_lng -= d_lng*RAD;
}
if (TURN_proc == 0){
double d_lat = (targetSpeed*oapiGetSimStep()*cos(vs2.surf_hdg) / each_deg);
double d_lng = (targetSpeed*oapiGetSimStep()*sin(vs2.surf_hdg) / each_deg);
//double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
//double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat -= d_lat*RAD;
vs2.surf_lng -= d_lng*RAD;
}
}
// sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f ANg %2.2f", targetSpeed, vs2.surf_hdg, AngSpeed);
lng = vs2.surf_lng;
lat = vs2.surf_lat;
hdg = vs2.surf_hdg;
{
MATRIX3 rot1 = RotationMatrix(_V(0 * RAD, (90 * RAD - lng), 0 * RAD), TRUE);
MATRIX3 rot2 = RotationMatrix(_V(-lat + 0 * RAD, 0, 0 * RAD), TRUE);
MATRIX3 rot3 = RotationMatrix(_V(0, 0, 180 * RAD + hdg), TRUE);
MATRIX3 rot4 = RotationMatrix(_V(90 * RAD, 0, 0), TRUE);
//MATRIX3 rot4 = RotationMatrix(_V(90 * RAD - pitch_angle, 0, roll_angle), TRUE);
// MATRIX3 rot_pitch = RotationMatrix(_V(pitch_angle, 0, 0));
// MATRIX3 rot_roll = RotationMatrix(_V(0, 0, roll_angle));
MATRIX3 RotMatrix_Def = mul(rot1, mul(rot2, mul(rot3, rot4)));
vs2.arot.x = atan2(RotMatrix_Def.m23, RotMatrix_Def.m33);
vs2.arot.y = -asin(RotMatrix_Def.m13);
vs2.arot.z = atan2(RotMatrix_Def.m12, RotMatrix_Def.m11);
}
vs2.vrot.x = 2.35;
//sprintf(oapiDebugString(), "targetSpeed %2.2f d_lat %2.2f d_lng %2.2f,rt %2.2f degree %2.2f hdg %2.2f", targetSpeed, vs2.surf_lat, vs2.surf_lng, rt, each_deg, vs2.surf_hdg);
//sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f d_hdg %2.2f,hdg %2.2f", targetSpeed, i, d_hdg, vs2.surf_hdg);
DefSetStateEx(&vs2);
return;
}
void LER2016::MoveAround() {
double theta = 0;
Max_Steering_Angle = 360;
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) {
if (i3 < 0) {
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_hdg;
if (vs2.surf_hdg<0) {
vs2.surf_hdg += 2 * PI;
}
rot += 0.005;//animation axle wheel turn rate
if (rot > .125) rot = .125;
}
else if (i3 > 0) {
rot -= 0.005;//animation axle wheel turn rate
if (rot < -.125) rot = -.125;
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
if (vs2.surf_hdg > 2 * PI) {
vs2.surf_hdg -= 2 * PI;
}
}
else 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;
double d_lat = (targetSpeed * oapiGetSimStep() * cosTurn / each_deg);
double d_lng = (targetSpeed * oapiGetSimStep() * sinTurn / each_deg);
vs2.surf_lat += d_lat * RAD;
vs2.surf_lng += d_lng * RAD;
}
if (TURN_proc == 0) {
double d_lat = (targetSpeed * oapiGetSimStep() * cos(vs2.surf_hdg) / each_deg);
double d_lng = (targetSpeed * oapiGetSimStep() * sin(vs2.surf_hdg) / each_deg);
//double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
//double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat += d_lat * RAD;
vs2.surf_lng += d_lng * RAD;
}
}
if (REVERSEgear == 1) {
if (i3 > 0) {
d_hdg = d_hdg + .000001;//rate of steering change of heading
vs2.surf_hdg -= d_hdg;
if (vs2.surf_hdg < 0) {
vs2.surf_hdg += 2 * PI;
}
}
if (i3 < 0) {
d_hdg = d_hdg - .0000001;//rate of steering change of heading
vs2.surf_hdg += d_hdg;
if (vs2.surf_hdg > 2 * PI) {
vs2.surf_hdg -= 2 * PI;
}
}
if (i3 == 0) {// straight so straighten wheels
if (abs(rot) <= 0.005)
rot = 0;
else if (rot < 0)
rot += 0.005;
else if (rot > 0)
rot -= 0.005;
double d_lat = (targetSpeed * oapiGetSimStep() * cosTurn / each_deg);
double d_lng = (targetSpeed * oapiGetSimStep() * sinTurn / each_deg);
vs2.surf_lat -= d_lat * RAD;
vs2.surf_lng -= d_lng * RAD;
}
if (TURN_proc == 0) {
double d_lat = (targetSpeed * oapiGetSimStep() * cos(vs2.surf_hdg) / each_deg);
double d_lng = (targetSpeed * oapiGetSimStep() * sin(vs2.surf_hdg) / each_deg);
//double d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
//double d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / each_deg);
vs2.surf_lat -= d_lat * RAD;
vs2.surf_lng -= d_lng * RAD;
}
}
//sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f ANg %2.2f", targetSpeed, vs2.surf_hdg, AngSpeed);
lng = vs2.surf_lng;
lat = vs2.surf_lat;
hdg = vs2.surf_hdg;
//????????????????????????????????????????????????????
{
MATRIX3 rot1 = RotationMatrix(_V(0 * RAD, (90 * RAD - lng), 0 * RAD), TRUE);
MATRIX3 rot2 = RotationMatrix(_V(-lat + 0 * RAD, 0, 0 * RAD), TRUE);
MATRIX3 rot3 = RotationMatrix(_V(0, 0, 180 * RAD + hdg), TRUE);
MATRIX3 rot4 = RotationMatrix(_V(90 * RAD, 0, 0), TRUE);
//MATRIX3 rot4 = RotationMatrix(_V(90 * RAD - pitch_angle, 0, roll_angle), TRUE);
// MATRIX3 rot_pitch = RotationMatrix(_V(pitch_angle, 0, 0));
// MATRIX3 rot_roll = RotationMatrix(_V(0, 0, roll_angle));
MATRIX3 RotMatrix_Def = mul(rot1, mul(rot2, mul(rot3, rot4)));
vs2.arot.x = atan2(RotMatrix_Def.m23, RotMatrix_Def.m33);
vs2.arot.y = -asin(RotMatrix_Def.m13);
vs2.arot.z = atan2(RotMatrix_Def.m12, RotMatrix_Def.m11);
}
vs2.vrot.x = 2.35;
//sprintf(oapiDebugString(), "targetSpeed %2.2f d_lat %2.2f d_lng %2.2f,rt %2.2f degree %2.2f hdg %2.2f", targetSpeed, vs2.surf_lat, vs2.surf_lng, rt, each_deg, vs2.surf_hdg);
//sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f d_hdg %2.2f,hdg %2.2f", targetSpeed, i, d_hdg, vs2.surf_hdg);
DefSetStateEx(&vs2);
return;
}
(And can thus expect a higher income as software developer, as scientific studies have shown.)
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)
{
if (i3 < 0){
d_hdg = d_hdg + .00001;//rate of steering change of heading
vs2.surf_hdg -= d_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){
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;
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;
}
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
d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / 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;
}
{
VISHANDLE MainExternalMeshVisual = 0;
static const int ntdvtx = 3;
static TOUCHDOWNVTX tdvtx[ntdvtx] = {
{ _V(0, -2.35, 1.5), 26150.8, 18409.6, 3.2, 0.8 },
{ _V(-2, -2.35, -2.6), 26150.8, 18409.6, 3.2, 0.4 },
{ _V(2, -2.35, -2.6), 26150.8, 18409.6, 3.2, 0.4 }//,
// { _V(-2, -2.164, 1.5), 1e6, 1e5, 3.2, 0 },
// { _V(2, -2.164, 1.5), 1e6, 1e5, 3.2, 0 },
// { _V(-2, 1.6, -2.6), 1e6, 1e5, 3.2, 0 },
// { _V(2, 1.6, -2.6), 1e6, 1e5, 3.2, 0 },
// { _V(-2, 1.6, 1.5), 1e6, 1e5, 3.2, 0 },
// { _V(2, 1.6, 1.5), 1e6, 1e5, 3.2, 0 }
};
void LER2016::clbkPreStep(double simt, double simdt, double mjd) {
rt = oapiGetSize(GetSurfaceRef());
earth_circ = rt * 2 * PI;
each_deg = earth_circ / 360;
grav_acc = GGRAV*oapiGetMass(GetSurfaceRef()) / (rt*rt);
memset(&vs2, 0, sizeof(vs2));
vs2.version = 2;
GetStatusEx(&vs2);
// CurrentSpeed = .5;
MoveAround();
}
void LER2016::clbkSetClassCaps(FILEHANDLE cfg) {
// physical specs
SetSize(6.08);
SetEmptyMass(MASS);
SetCW(0.9, 0.9, 2, 1.4);
SetWingAspect(0.1);
SetWingEffectiveness(0.1);
SetCrossSections(_V(232.84, 1220.32, 166.36));
SetRotDrag(_V(0.1, 0.1, 0.1));
if (GetFlightModel() >= 1) {
SetPitchMomentScale(1e-4);
SetBankMomentScale(1e-4);
}
SetPMI(_V(163.54, 208.04, 76.03));
SetTrimScale(0.05);
SetCameraOffset(_V(0, .7, 3.121));
// SetTouchdownPoints(_V(0, -2.116, 20), _V(-15, -2.116, -20), _V(15, -2.116, -20));;
SetTouchdownPoints(tdvtx, ntdvtx);
...}
MATRIX3 LER2016::RotationMatrix(VECTOR3 angles, bool xyz = FALSE)
{
MATRIX3 m;
MATRIX3 RM_X, RM_Y, RM_Z;
RM_X = _M(1, 0, 0, 0, cos(angles.x), -sin(angles.x), 0, sin(angles.x), cos(angles.x));
RM_Y = _M(cos(angles.y), 0, sin(angles.y), 0, 1, 0, -sin(angles.y), 0, cos(angles.y));
RM_Z = _M(cos(angles.z), -sin(angles.z), 0, sin(angles.z), cos(angles.z), 0, 0, 0, 1);
if (!xyz) {
m = mul(RM_Z, mul(RM_Y, RM_X));
}
else {
m = mul(RM_X, mul(RM_Y, RM_Z));
}
return m;
}
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
d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / 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;
}
if ((LASTGEAR == 1) && (neutralgear == 1)) {
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
d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / 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;
}
if ((LASTGEAR == 2) && (neutralgear == 1)) {
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
d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / 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;
}
if (REVERSEgear == 1)
{
if (i3 > 0){
d_hdg = d_hdg + .00001;//rate of steering change of heading
vs2.surf_hdg -= d_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){
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;
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
d_lat = (targetSpeed*oapiGetSimStep()*cosTurn / each_deg);
d_lng = (targetSpeed*oapiGetSimStep()*sinTurn / 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;
{
MATRIX3 rot1 = RotationMatrix(_V(0 * RAD, (90 * RAD - lng), 0 * RAD), TRUE);
MATRIX3 rot2 = RotationMatrix(_V(-lat + 0 * RAD, 0, 0 * RAD), TRUE);
MATRIX3 rot3 = RotationMatrix(_V(0, 0, 180 * RAD + hdg), TRUE);
MATRIX3 rot4 = RotationMatrix(_V(90 * RAD, 0, 0), TRUE);
//MATRIX3 rot4 = RotationMatrix(_V(90 * RAD - pitch_angle, 0, roll_angle), TRUE);
// MATRIX3 rot_pitch = RotationMatrix(_V(pitch_angle, 0, 0));
// MATRIX3 rot_roll = RotationMatrix(_V(0, 0, roll_angle));
MATRIX3 RotMatrix_Def = mul(rot1, mul(rot2, mul(rot3, rot4)));
vs2.arot.x = atan2(RotMatrix_Def.m23, RotMatrix_Def.m33);
vs2.arot.y = -asin(RotMatrix_Def.m13);
vs2.arot.z = atan2(RotMatrix_Def.m12, RotMatrix_Def.m11);
}
vs2.vrot.x = 2.35;
//sprintf(oapiDebugString(), "targetSpeed %2.2f d_lat %2.2f d_lng %2.2f,rt %2.2f degree %2.2f hdg %2.2f", targetSpeed, vs2.surf_lat, vs2.surf_lng, rt, each_deg, vs2.surf_hdg);
//sprintf(oapiDebugString(), "targetSpeed %2.2f hd %2.2f d_hdg %2.2f,hdg %2.2f", targetSpeed, i, d_hdg, vs2.surf_hdg);
DefSetStateEx(&vs2);
return;
}