MATRIX3 SLSCRAWLER::RotationMatrix(VECTOR3 angles, bool xyz)
{
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 SLSCRAWLER::MoveAround(bool FWD, bool BWD, bool LFT, bool RGT)
{
if (FWD)
{
VESSELSTATUS2 vs2;
memset(&vs2, 0, sizeof(vs2));
vs2.version = 2;
GetStatusEx(&vs2);
double d_lat = (Speed*oapiGetSimStep()*cos(vs2.surf_hdg)) / each_deg;
double d_lng = (Speed*oapiGetSimStep()*sin(vs2.surf_hdg)) / each_deg;
vs2.surf_lat += d_lat*RAD;
vs2.surf_lng += d_lng*RAD;
MATRIX3 rot1 = RotationMatrix(_V(0, (90 - Long)*RAD, 0 * RAD), TRUE);
MATRIX3 rot2 = RotationMatrix(_V((-Lat)*RAD, 0, 0 * RAD), TRUE);
MATRIX3 RotMatrix_Def = mul(rot1, rot2);
vs.arot.x = atan2(RotMatrix_Def.m23, RotMatrix_Def.m33);
vs.arot.y = -asin(RotMatrix_Def.m13);
vs.arot.z = atan2(RotMatrix_Def.m12, RotMatrix_Def.m11);
DefSetStateEx(&vs2);
return;
}