From d4b4e07c4d0b78f6fe58859a09e5a32ffd7e9675 Mon Sep 17 00:00:00 2001 From: onox Date: Sat, 23 May 2015 06:58:53 +0200 Subject: [PATCH] submodel: Use quaternions instead of trigonometry Signed-off-by: onox --- src/AIModel/submodel.cxx | 47 +++++++++++++++------------------------- src/AIModel/submodel.hxx | 3 --- 2 files changed, 18 insertions(+), 32 deletions(-) diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index 6638e3986..63d736938 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -394,39 +394,26 @@ void FGSubmodelMgr::transform(submodel *sm) setOffsetPos(); - double yaw_offset = sm->yaw_offset->get_value(); - double pitch_offset = sm->pitch_offset->get_value(); + // Compute initial orientation using yaw and pitch offsets and parent's orientation + const double yaw_offset = sm->yaw_offset->get_value(); + const double pitch_offset = sm->pitch_offset->get_value(); - // Compute azimuth and elevation given the yaw and pitch offsets SGQuatd ic_quat = SGQuatd::fromYawPitchRollDeg(IC.azimuth, IC.elevation, IC.roll); ic_quat *= SGQuatd::fromYawPitchRollDeg(yaw_offset, pitch_offset, 0.0); - double ic_roll; - ic_quat.getEulerDeg(IC.azimuth, IC.elevation, ic_roll); + // Calculate total speed using speeds of submodel and parent + SGVec3d total_speed = SGVec3d(IC.speed_north_fps, IC.speed_east_fps, IC.speed_down_fps); + total_speed += ic_quat.rotate(SGVec3d(sm->speed, 0, 0)); - // Calculate the total speed north - IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) - * cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps; - - // Calculate the total speed east - IC.total_speed_east = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) - * sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps; - - // Calculate the total speed down - IC.total_speed_down = sm->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS) - + IC.speed_down_fps; - - // Re-calculate speed, elevation and azimuth - IC.speed = sqrt(IC.total_speed_north * IC.total_speed_north - + IC.total_speed_east * IC.total_speed_east - + IC.total_speed_down * IC.total_speed_down); - - cout << "sm speed: " << sm->speed << " IC speed: " << IC.speed << endl; - cout << "az1: " << IC.azimuth << " el1: " << IC.elevation << endl; + IC.speed = length(total_speed); // If speeds are low this calculation can become unreliable if (IC.speed > 1) { - IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES; + const double total_speed_north = total_speed.x(); + const double total_speed_east = total_speed.y(); + const double total_speed_down = total_speed.z(); + + IC.azimuth = atan2(total_speed_east, total_speed_north) * SG_RADIANS_TO_DEGREES; // Rationalize the output if (IC.azimuth < 0) @@ -434,12 +421,14 @@ void FGSubmodelMgr::transform(submodel *sm) else if (IC.azimuth >= 360) IC.azimuth -= 360; - IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north - * IC.total_speed_north + IC.total_speed_east * IC.total_speed_east)) + IC.elevation = -atan(total_speed_down / sqrt(total_speed_north + * total_speed_north + total_speed_east * total_speed_east)) * SG_RADIANS_TO_DEGREES; } - - cout << "az2: " << IC.azimuth << " el2: " << IC.elevation << endl; + else { + double ic_roll; + ic_quat.getEulerDeg(IC.azimuth, IC.elevation, ic_roll); + } } void FGSubmodelMgr::updatelat(double lat) diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx index 04b45d732..86e2b1911 100644 --- a/src/AIModel/submodel.hxx +++ b/src/AIModel/submodel.hxx @@ -91,9 +91,6 @@ public: double speed_down_fps; double speed_east_fps; double speed_north_fps; - double total_speed_down; - double total_speed_east; - double total_speed_north; double mass; int id; bool no_roll; -- 2.39.5