typedef struct { float w, x, y, z; } Quaternion;
static Quaternion quaternion_multiply(Quaternion a, Quaternion b) { Quaternion q = { a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z, a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y, a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x, a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w }; return q; }
static Quaternion quaternion_axis_angle(float ax, float ay, float az, float angle) { const float half = angle * 0.5f; const float s = sinf(half); Quaternion q = {cosf(half), ax*s, ay*s, az*s}; return q; }
/* * Convert a world vector to body coordinates using the full vehicle attitude. * Convention/assumption: vehicle-to-world attitude is * q = q_yaw(Y) * q_pitch(X) * q_roll(Z), applied roll, then pitch, then yaw. * With Hamilton quaternions, world->vehicle is q^-1 * v * q. */ static void world_to_vehicle_local_quaternion(float yaw, float pitch, float roll, float wx, float wy, float wz, float *lx, float *ly, float *lz) { const Quaternion q_yaw = quaternion_axis_angle(0.0f, 1.0f, 0.0f, yaw); const Quaternion q_pitch = quaternion_axis_angle(1.0f, 0.0f, 0.0f, pitch); const Quaternion q_roll = quaternion_axis_angle(0.0f, 0.0f, 1.0f, roll); const Quaternion q = quaternion_multiply( quaternion_multiply(q_yaw, q_pitch), q_roll); const Quaternion q_inverse = {q.w, -q.x, -q.y, -q.z}; const Quaternion v_world = {0.0f, wx, wy, wz}; const Quaternion v_local = quaternion_multiply( quaternion_multiply(q_inverse, v_world), q); *lx = v_local.x; *ly = v_local.y; *lz = v_local.z; } |