From 4fd05a87b79f653c82deb2aa0867cb09789f5c7b Mon Sep 17 00:00:00 2001 From: Mikhail Grushinskiy Date: Sat, 31 Aug 2024 20:33:01 -0400 Subject: [PATCH] Create Quaternion.h --- bbn_wave_freq_m5atomS3/Quaternion.h | 371 ++++++++++++++++++++++++++++ 1 file changed, 371 insertions(+) create mode 100644 bbn_wave_freq_m5atomS3/Quaternion.h diff --git a/bbn_wave_freq_m5atomS3/Quaternion.h b/bbn_wave_freq_m5atomS3/Quaternion.h new file mode 100644 index 0000000..1fe0bad --- /dev/null +++ b/bbn_wave_freq_m5atomS3/Quaternion.h @@ -0,0 +1,371 @@ +#ifndef Quaternion_h +#define Quaternion_h + +// Copyright (C) 2022 Martin Weigel +// +// Permission to use, copy, modify, and/or distribute this software for any +// purpose with or without fee is hereby granted, provided that the above +// copyright notice and this permission notice appear in all copies. +// +// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +// ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +// ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +// OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +// See: https://github.com/MartinWeigel/Quaternion + +#include +#include +#include + +/** + * Maximum floating point difference that is considered as equal. + */ +#define QUATERNION_EPS (1e-4) + +/** + * Data structure to hold a quaternion. + */ +typedef struct Quaternion { + float w; /**< Scalar part */ + float v[3]; /**< Vector part */ +} Quaternion; + + +/** + * Sets the given values to the output quaternion. + */ +void Quaternion_set(float w, float v1, float v2, float v3, Quaternion* output); + +/** + * Sets quaternion to its identity. + */ +void Quaternion_setIdentity(Quaternion* q); + +/** + * Copies one quaternion to another. + */ +void Quaternion_copy(Quaternion* q, Quaternion* output); + +/** + * Tests if all quaternion values are equal (using QUATERNION_EPS). + */ +bool Quaternion_equal(Quaternion* q1, Quaternion* q2); + +/** + * Set the quaternion to the equivalent of axis-angle rotation. + * @param axis + * The axis of the rotation (should be normalized). + * @param angle + * Rotation angle in radians. + */ +void Quaternion_fromAxisAngle(float axis[3], float angle, Quaternion* output); + +/** + * Calculates the rotation vector and angle of a quaternion. + * @param output + * A 3D vector of the quaternion rotation axis. + * @return + * The rotation angle in radians. + */ +float Quaternion_toAxisAngle(Quaternion* q, float output[3]); + +/** + * Set the quaternion to the equivalent of euler angles. + * @param eulerZYX + * Euler angles in ZYX, but stored in array as [x'', y', z]. + */ +void Quaternion_fromEulerZYX(float eulerZYX[3], Quaternion* output); + +/** + * Calculates the euler angles of a quaternion. + * @param output + * Euler angles in ZYX, but stored in array as [x'', y', z]. + */ +void Quaternion_toEulerZYX(Quaternion* q, float output[3]); + +/** + * Set the quaternion to the equivalent a rotation around the X-axis. + * @param angle + * Rotation angle in radians. + */ +void Quaternion_fromXRotation(float angle, Quaternion* output); + +/** + * Set the quaternion to the equivalent a rotation around the Y-axis. + * @param angle + * Rotation angle in radians. + */ +void Quaternion_fromYRotation(float angle, Quaternion* output); + +/** + * Set the quaternion to the equivalent a rotation around the Z-axis. + * @param angle + * Rotation angle in radians. + */ +void Quaternion_fromZRotation(float angle, Quaternion* output); + +/** + * Calculates the norm of a given quaternion: + * norm = sqrt(w*w + v1*v1 + v2*v2 + v3*v3) + */ +float Quaternion_norm(Quaternion* q); + +/** + * Normalizes the quaternion. + */ +void Quaternion_normalize(Quaternion* q, Quaternion* output); + +/** + * Calculates the conjugate of the quaternion: (w, -v) + */ +void Quaternion_conjugate(Quaternion* q, Quaternion* output); + +/** + * Multiplies two quaternions: output = q1 * q2 + * @param q1 + * The rotation to apply on q2. + * @param q2 + * The orientation to be rotated. + */ +void Quaternion_multiply(Quaternion* q1, Quaternion* q2, Quaternion* output); + +/** + * Applies quaternion rotation to a given vector. + */ +void Quaternion_rotate(Quaternion* q, float v[3], float output[3]); + +/** + * Interpolates between two quaternions. + * @param t + * Interpolation between the two quaternions [0, 1]. + * 0 is equal with q1, 1 is equal with q2, 0.5 is the middle between q1 and q2. + */ +void Quaternion_slerp(Quaternion* q1, Quaternion* q2, float t, Quaternion* output); + +void Quaternion_set(float w, float v1, float v2, float v3, Quaternion* output) { + assert(output != NULL); + output->w = w; + output->v[0] = v1; + output->v[1] = v2; + output->v[2] = v3; +} + +void Quaternion_setIdentity(Quaternion* q) { + assert(q != NULL); + Quaternion_set(1, 0, 0, 0, q); +} + +void Quaternion_copy(Quaternion* q, Quaternion* output) { + Quaternion_set(q->w, q->v[0], q->v[1], q->v[2], output); +} + +bool Quaternion_equal(Quaternion* q1, Quaternion* q2) { + bool equalW = fabs(q1->w - q2->w) <= QUATERNION_EPS; + bool equalV0 = fabs(q1->v[0] - q2->v[0]) <= QUATERNION_EPS; + bool equalV1 = fabs(q1->v[1] - q2->v[1]) <= QUATERNION_EPS; + bool equalV2 = fabs(q1->v[2] - q2->v[2]) <= QUATERNION_EPS; + return equalW && equalV0 && equalV1 && equalV2; +} + +void Quaternion_fromAxisAngle(float axis[3], float angle, Quaternion* output) { + assert(output != NULL); + // Formula from http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/ + output->w = cos(angle / 2.0); + float c = sin(angle / 2.0); + output->v[0] = c * axis[0]; + output->v[1] = c * axis[1]; + output->v[2] = c * axis[2]; +} + +float Quaternion_toAxisAngle(Quaternion* q, float output[3]) { + assert(output != NULL); + // Formula from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/ + float angle = 2.0 * acos(q->w); + float divider = sqrt(1.0 - q->w * q->w); + + if (divider != 0.0) { + // Calculate the axis + output[0] = q->v[0] / divider; + output[1] = q->v[1] / divider; + output[2] = q->v[2] / divider; + } else { + // Arbitrary normalized axis + output[0] = 1; + output[1] = 0; + output[2] = 0; + } + return angle; +} + +void Quaternion_fromXRotation(float angle, Quaternion* output) { + assert(output != NULL); + float axis[3] = {1.0, 0, 0}; + Quaternion_fromAxisAngle(axis, angle, output); +} + +void Quaternion_fromYRotation(float angle, Quaternion* output) { + assert(output != NULL); + float axis[3] = {0, 1.0, 0}; + Quaternion_fromAxisAngle(axis, angle, output); +} + +void Quaternion_fromZRotation(float angle, Quaternion* output) { + assert(output != NULL); + float axis[3] = {0, 0, 1.0}; + Quaternion_fromAxisAngle(axis, angle, output); +} + +void Quaternion_fromEulerZYX(float eulerZYX[3], Quaternion* output) { + assert(output != NULL); + // Based on https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + float cy = cos(eulerZYX[2] * 0.5); + float sy = sin(eulerZYX[2] * 0.5); + float cr = cos(eulerZYX[0] * 0.5); + float sr = sin(eulerZYX[0] * 0.5); + float cp = cos(eulerZYX[1] * 0.5); + float sp = sin(eulerZYX[1] * 0.5); + + output->w = cy * cr * cp + sy * sr * sp; + output->v[0] = cy * sr * cp - sy * cr * sp; + output->v[1] = cy * cr * sp + sy * sr * cp; + output->v[2] = sy * cr * cp - cy * sr * sp; +} + +void Quaternion_toEulerZYX(Quaternion* q, float output[3]) { + assert(output != NULL); + + // Roll (x-axis rotation) + float sinr_cosp = +2.0 * (q->w * q->v[0] + q->v[1] * q->v[2]); + float cosr_cosp = +1.0 - 2.0 * (q->v[0] * q->v[0] + q->v[1] * q->v[1]); + output[0] = atan2(sinr_cosp, cosr_cosp); + + // Pitch (y-axis rotation) + float sinp = +2.0 * (q->w * q->v[1] - q->v[2] * q->v[0]); + if (fabs(sinp) >= 1) + output[1] = copysign(M_PI / 2, sinp); // use 90 degrees if out of range + else + output[1] = asin(sinp); + + // Yaw (z-axis rotation) + float siny_cosp = +2.0 * (q->w * q->v[2] + q->v[0] * q->v[1]); + float cosy_cosp = +1.0 - 2.0 * (q->v[1] * q->v[1] + q->v[2] * q->v[2]); + output[2] = atan2(siny_cosp, cosy_cosp); +} + +void Quaternion_conjugate(Quaternion* q, Quaternion* output) { + assert(output != NULL); + output->w = q->w; + output->v[0] = -q->v[0]; + output->v[1] = -q->v[1]; + output->v[2] = -q->v[2]; +} + +float Quaternion_norm(Quaternion* q) { + assert(q != NULL); + return sqrt(q->w*q->w + q->v[0]*q->v[0] + q->v[1]*q->v[1] + q->v[2]*q->v[2]); +} + +void Quaternion_normalize(Quaternion* q, Quaternion* output) { + assert(output != NULL); + float len = Quaternion_norm(q); + Quaternion_set( + q->w / len, + q->v[0] / len, + q->v[1] / len, + q->v[2] / len, + output); +} + +void Quaternion_multiply(Quaternion* q1, Quaternion* q2, Quaternion* output) { + assert(output != NULL); + Quaternion result; + + /* + Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm + a*e - b*f - c*g - d*h + + i (b*e + a*f + c*h- d*g) + + j (a*g - b*h + c*e + d*f) + + k (a*h + b*g - c*f + d*e) + */ + result.w = q1->w *q2->w - q1->v[0]*q2->v[0] - q1->v[1]*q2->v[1] - q1->v[2]*q2->v[2]; + result.v[0] = q1->v[0]*q2->w + q1->w *q2->v[0] + q1->v[1]*q2->v[2] - q1->v[2]*q2->v[1]; + result.v[1] = q1->w *q2->v[1] - q1->v[0]*q2->v[2] + q1->v[1]*q2->w + q1->v[2]*q2->v[0]; + result.v[2] = q1->w *q2->v[2] + q1->v[0]*q2->v[1] - q1->v[1]*q2->v[0] + q1->v[2]*q2->w ; + + *output = result; +} + +void Quaternion_rotate(Quaternion* q, float v[3], float output[3]) { + assert(output != NULL); + float result[3]; + + float ww = q->w * q->w; + float xx = q->v[0] * q->v[0]; + float yy = q->v[1] * q->v[1]; + float zz = q->v[2] * q->v[2]; + float wx = q->w * q->v[0]; + float wy = q->w * q->v[1]; + float wz = q->w * q->v[2]; + float xy = q->v[0] * q->v[1]; + float xz = q->v[0] * q->v[2]; + float yz = q->v[1] * q->v[2]; + + // Formula from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm + // p2.x = w*w*p1.x + 2*y*w*p1.z - 2*z*w*p1.y + x*x*p1.x + 2*y*x*p1.y + 2*z*x*p1.z - z*z*p1.x - y*y*p1.x; + // p2.y = 2*x*y*p1.x + y*y*p1.y + 2*z*y*p1.z + 2*w*z*p1.x - z*z*p1.y + w*w*p1.y - 2*x*w*p1.z - x*x*p1.y; + // p2.z = 2*x*z*p1.x + 2*y*z*p1.y + z*z*p1.z - 2*w*y*p1.x - y*y*p1.z + 2*w*x*p1.y - x*x*p1.z + w*w*p1.z; + + result[0] = ww*v[0] + 2*wy*v[2] - 2*wz*v[1] + + xx*v[0] + 2*xy*v[1] + 2*xz*v[2] - + zz*v[0] - yy*v[0]; + result[1] = 2*xy*v[0] + yy*v[1] + 2*yz*v[2] + + 2*wz*v[0] - zz*v[1] + ww*v[1] - + 2*wx*v[2] - xx*v[1]; + result[2] = 2*xz*v[0] + 2*yz*v[1] + zz*v[2] - + 2*wy*v[0] - yy*v[2] + 2*wx*v[1] - + xx*v[2] + ww*v[2]; + + // Copy result to output + output[0] = result[0]; + output[1] = result[1]; + output[2] = result[2]; +} + +void Quaternion_slerp(Quaternion* q1, Quaternion* q2, float t, Quaternion* output) { + Quaternion result; + + // Based on http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm + float cosHalfTheta = q1->w*q2->w + q1->v[0]*q2->v[0] + q1->v[1]*q2->v[1] + q1->v[2]*q2->v[2]; + + // if q1=q2 or qa=-q2 then theta = 0 and we can return qa + if (fabs(cosHalfTheta) >= 1.0) { + Quaternion_copy(q1, output); + return; + } + + float halfTheta = acos(cosHalfTheta); + float sinHalfTheta = sqrt(1.0 - cosHalfTheta*cosHalfTheta); + // If theta = 180 degrees then result is not fully defined + // We could rotate around any axis normal to q1 or q2 + if (fabs(sinHalfTheta) < QUATERNION_EPS) { + result.w = (q1->w * 0.5 + q2->w * 0.5); + result.v[0] = (q1->v[0] * 0.5 + q2->v[0] * 0.5); + result.v[1] = (q1->v[1] * 0.5 + q2->v[1] * 0.5); + result.v[2] = (q1->v[2] * 0.5 + q2->v[2] * 0.5); + } else { + // Default quaternion calculation + float ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; + float ratioB = sin(t * halfTheta) / sinHalfTheta; + result.w = (q1->w * ratioA + q2->w * ratioB); + result.v[0] = (q1->v[0] * ratioA + q2->v[0] * ratioB); + result.v[1] = (q1->v[1] * ratioA + q2->v[1] * ratioB); + result.v[2] = (q1->v[2] * ratioA + q2->v[2] * ratioB); + } + *output = result; +} + +#endif