MATRIX3 GeneralVehicle::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;
}
/////then the calculation part
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-pitch_angle, 0, roll_angle), TRUE);
//This last roation matrix is used to set the vehicle parallel to ground, if you need a tail sitter you should be able to skip this
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);