diff --git a/df_datalink/src/networking/link_layer/geometry/coord.c b/df_datalink/src/networking/link_layer/geometry/coord.c index 68d4108..1ca9044 100644 --- a/df_datalink/src/networking/link_layer/geometry/coord.c +++ b/df_datalink/src/networking/link_layer/geometry/coord.c @@ -7,7 +7,6 @@ #include #include "coord.h" - const COORD COORD_NIL = {NAN, NAN, NAN}; const COORD COORD_ZERO = {0.0, 0.0, 0.0}; const COORD COORD_ONE = {1.0, 1.0, 1.0}; @@ -15,8 +14,9 @@ const COORD COORD_X_AXIS = {1.0, 0.0, 0.0}; const COORD COORD_Y_AXIS = {0.0, 1.0, 0.0}; const COORD COORD_Z_AXIS = {0.0, 0.0, 1.0}; - /** @brief Subtracts two coordinate vectors. */ -COORD Coord_Sub(COORD a, COORD b) { +/** @brief Subtracts two coordinate vectors. */ +COORD Coord_Sub(COORD a, COORD b) +{ COORD ret; ret.x = a.x - b.x; ret.y = a.y - b.y; @@ -25,7 +25,8 @@ COORD Coord_Sub(COORD a, COORD b) { } /** @brief Adds two coordinate vectors. */ -COORD Coord_Add(COORD a, COORD b) { +COORD Coord_Add(COORD a, COORD b) +{ COORD ret; ret.x = a.x + b.x; ret.y = a.y + b.y; @@ -34,14 +35,16 @@ COORD Coord_Add(COORD a, COORD b) { } /** @brief Multiplies a coordinate vector by a real number. */ -void Coord_ScalarMult(COORD *a, double t) { +void Coord_ScalarMult(COORD *a, double t) +{ a->x *= t; a->y *= t; a->z *= t; } /** @brief Multiplies a coordinate vector by a real number. */ -COORD Coord_GetScalarMult(COORD a, double t) { +COORD Coord_GetScalarMult(COORD a, double t) +{ COORD ret; ret.x = a.x * t; ret.y = a.y * t; @@ -50,14 +53,16 @@ COORD Coord_GetScalarMult(COORD a, double t) { } /** @brief Divides a coordinate vector by a real number. */ -void Coord_ScalarDiv(COORD *a, double t) { +void Coord_ScalarDiv(COORD *a, double t) +{ a->x /= t; a->y /= t; a->z /= t; } /** @brief Divides a coordinate vector by a real number. */ -COORD Coord_GetScalarDiv(COORD a, double t) { +COORD Coord_GetScalarDiv(COORD a, double t) +{ COORD ret; ret.x = a.x / t; ret.y = a.y / t; @@ -66,12 +71,14 @@ COORD Coord_GetScalarDiv(COORD a, double t) { } /** @brief Dot product. */ -inline double Coord_Dot(COORD a, COORD b) { +inline double Coord_Dot(COORD a, COORD b) +{ return (a.x * b.x + a.y * b.y + a.z * b.z); } /** @brief Cross product. */ -COORD Coord_Cross(COORD a, COORD b) { +COORD Coord_Cross(COORD a, COORD b) +{ COORD ret; ret.x = a.y * b.z - a.z * b.y; ret.y = a.z * b.x - a.x * b.z; @@ -80,7 +87,8 @@ COORD Coord_Cross(COORD a, COORD b) { } /** @brief Multiplies two coordinate vector in elementwise. */ -COORD Coord_ElementwiseMult(COORD a, COORD b) { +COORD Coord_ElementwiseMult(COORD a, COORD b) +{ COORD ret; ret.x = a.x * b.x; ret.y = a.y * b.y; @@ -89,7 +97,8 @@ COORD Coord_ElementwiseMult(COORD a, COORD b) { } /** @brief Divides two coordinate vector in elementwise. */ -COORD Coord_ElementwiseDiv(COORD a, COORD b) { +COORD Coord_ElementwiseDiv(COORD a, COORD b) +{ COORD ret; ret.x = a.x / b.x; ret.y = a.y / b.y; @@ -98,45 +107,51 @@ COORD Coord_ElementwiseDiv(COORD a, COORD b) { } /** @brief Calculate a coordinate vector length^2 (omits calling square root). */ -double Coord_SquareLength(COORD a) { - return (a.x*a.x + a.y*a.y + a.z*a.z); +double Coord_SquareLength(COORD a) +{ + return (a.x * a.x + a.y * a.y + a.z * a.z); } /** @brief Calculate a coordinate vector length. */ -double Coord_Length(COORD a) { +double Coord_Length(COORD a) +{ return sqrt(Coord_SquareLength(a)); } /** @brief Calculate two coordinates' distance. */ -double Coord_Distance(COORD a, COORD b) { +double Coord_Distance(COORD a, COORD b) +{ return Coord_Length(Coord_Sub(a, b)); } /** @brief Normalize coordinate vector. */ -void Coord_Normalize(COORD *a) { +void Coord_Normalize(COORD *a) +{ Coord_ScalarDiv(a, Coord_Length(*a)); } /** @brief Normalize coordinate vector. */ -COORD Coord_GetNormalized(COORD a) { +COORD Coord_GetNormalized(COORD a) +{ Coord_Normalize(&a); return a; } /** @brief Returns the angle between the two vectors. */ -double Coord_Angle(COORD a, COORD b) { +double Coord_Angle(COORD a, COORD b) +{ Coord_Normalize(&a); Coord_Normalize(&b); return acos(Coord_Dot(a, b)); } - // Additional Coordinates convert // TODO: Confirm correctness /** @brief Convert coordinate from Launch coordinate system to ECEF. */ -COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) { +COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) +{ COORD ret; - double sinA,cosA, sinB, cosB, sinL, cosL; + double sinA, cosA, sinB, cosB, sinL, cosL; double S11, S12, S13, S21, S22, S23, S31, S32, S33; sinA = sin(A); cosA = cos(A); @@ -145,51 +160,52 @@ COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) { sinL = sin(lambda); cosL = cos(lambda); - S11 = -sinA*sinL - cosA*sinB*cosL; - S12 = cosB*cosL; - S13 = -cosA*sinL + sinA*sinB*cosL; + S11 = -sinA * sinL - cosA * sinB * cosL; + S12 = cosB * cosL; + S13 = -cosA * sinL + sinA * sinB * cosL; - S21 = sinA*cosL - cosA*sinB*sinL; - S22 = cosB*sinL; - S23 = cosA*cosL + sinA*sinB*sinL; + S21 = sinA * cosL - cosA * sinB * sinL; + S22 = cosB * sinL; + S23 = cosA * cosL + sinA * sinB * sinL; - S31 = cosA*cosB; + S31 = cosA * cosB; S32 = sinB; - S33 = -sinA*cosB; + S33 = -sinA * cosB; - ret.x = p.x*(S11 + S21 + S31) + o.x; - ret.y = p.y*(S12 + S22 + S32) + o.y; - ret.z = p.z*(S13 + S23 + S33) + o.z; + ret.x = p.x * (S11 + S21 + S31) + o.x; + ret.y = p.y * (S12 + S22 + S32) + o.y; + ret.z = p.z * (S13 + S23 + S33) + o.z; return ret; } /** @brief Convert coordinate from launch coordinate system to Inertia coordinate system. */ -COORD Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t) { +COORD Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t) +{ COORD ret; double OmgX, OmgY, OmgZ, sinwet, coswet; double M11, M12, M13, M21, M22, M23, M31, M32, M33; - OmgX = cos(A)*cos(B); + OmgX = cos(A) * cos(B); OmgY = sin(B); - OmgZ = -sin(A)*cos(B); - sinwet = sin(we*t); - coswet = cos(we*t); + OmgZ = -sin(A) * cos(B); + sinwet = sin(we * t); + coswet = cos(we * t); - M11 = 1 - (1 - OmgX*OmgX)*(1-coswet); - M12 = OmgX*OmgY*(1-coswet) + OmgZ*sinwet; - M13 = OmgX*OmgZ*(1-coswet)-OmgY*sinwet; + M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet); + M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet; + M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet; - M21 = OmgY*OmgX*(1-coswet) - OmgZ*sinwet; - M22 = 1-(1-OmgY*OmgY)*(1-coswet); - M23 = OmgY*OmgZ*(1-coswet) + OmgX*sinwet; + M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet; + M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet); + M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet; - M31 = OmgZ*OmgX*(1-coswet) + OmgY*sinwet; - M32 = OmgZ*OmgY*(1-coswet) - OmgX*sinwet; - M33 = 1 - (1-OmgZ*OmgZ)*(1-coswet); + M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet; + M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet; + M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet); - ret.x = p.x*(M11 + M21 + M31) + o.x; - ret.y = p.y*(M12 + M22 + M32) + o.y; - ret.z = p.z*(M13 + M23 + M33) + o.z; + ret.x = p.x * (M11 + M21 + M31) + o.x; + ret.y = p.y * (M12 + M22 + M32) + o.y; + ret.z = p.z * (M13 + M23 + M33) + o.z; return ret; } diff --git a/df_datalink/src/networking/link_layer/geometry/coord.h b/df_datalink/src/networking/link_layer/geometry/coord.h index 9bdf7e0..ee8b6a2 100644 --- a/df_datalink/src/networking/link_layer/geometry/coord.h +++ b/df_datalink/src/networking/link_layer/geometry/coord.h @@ -4,7 +4,6 @@ // SPDX-License-Identifier: LGPL-3.0-or-later // - #ifndef _COORD_H #define _COORD_H @@ -16,7 +15,8 @@ * @ingroup utils */ -typedef struct{ +typedef struct +{ double x; double y; double z; @@ -78,4 +78,3 @@ COORD Coord_GetNormalized(COORD a); double Coord_Angle(COORD a, COORD b); #endif - diff --git a/df_datalink/src/networking/link_layer/geometry/eulerangles.c b/df_datalink/src/networking/link_layer/geometry/eulerangles.c index 76755df..8c580ec 100644 --- a/df_datalink/src/networking/link_layer/geometry/eulerangles.c +++ b/df_datalink/src/networking/link_layer/geometry/eulerangles.c @@ -7,51 +7,55 @@ #include #include "eulerangles.h" - const EULERANGLES EULERANGLES_NIL = {NAN, NAN, NAN}; const EULERANGLES EULERANGLES_ZERO = {0.0, 0.0, 0.0}; - /** @brief Add two EulerAngles. */ -EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2) { +EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2) +{ EULERANGLES ret; ret.alpha = e1.alpha + e2.alpha; - ret.beta = e1.beta + e2.beta ; + ret.beta = e1.beta + e2.beta; ret.gamma = e1.gamma + e2.gamma; return ret; } /** @brief Subtracts two EulerAngles. */ -EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2) { +EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2) +{ EULERANGLES ret; ret.alpha = e1.alpha - e2.alpha; - ret.beta = e1.beta - e2.beta ; + ret.beta = e1.beta - e2.beta; ret.gamma = e1.gamma - e2.gamma; return ret; } /** @brief Multiplies EulerAngles by a real number. */ -void EulerAngles_ScalarMult(EULERANGLES *e, double f) { +void EulerAngles_ScalarMult(EULERANGLES *e, double f) +{ e->alpha *= f; - e->beta *= f; + e->beta *= f; e->gamma *= f; } /** @brief Multiplies EulerAngles by a real number. */ -EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f) { +EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f) +{ EulerAngles_ScalarMult(&e, f); return e; } /** @brief Normalize EulerAngles. */ -void EulerAngles_Normalize(EULERANGLES *e) { - e->alpha = fmod(e->alpha, 2*EUL_PI); - e->beta = fmod(e->beta , 2*EUL_PI); - e->gamma = fmod(e->gamma, 2*EUL_PI); +void EulerAngles_Normalize(EULERANGLES *e) +{ + e->alpha = fmod(e->alpha, 2 * EUL_PI); + e->beta = fmod(e->beta, 2 * EUL_PI); + e->gamma = fmod(e->gamma, 2 * EUL_PI); } /** @brief Normalize EulerAngles. */ -EULERANGLES EulerAngles_GetNormalize(EULERANGLES e) { +EULERANGLES EulerAngles_GetNormalize(EULERANGLES e) +{ EulerAngles_Normalize(&e); return e; } diff --git a/df_datalink/src/networking/link_layer/geometry/eulerangles.h b/df_datalink/src/networking/link_layer/geometry/eulerangles.h index 6a82c8f..8db0860 100644 --- a/df_datalink/src/networking/link_layer/geometry/eulerangles.h +++ b/df_datalink/src/networking/link_layer/geometry/eulerangles.h @@ -3,8 +3,8 @@ #define EUL_PI 3.14159265358979323846 -#define DEG2RAD(d) ((EUL_PI/180.0)*d) -#define RAD2DEG(r) ((180.0/EUL_PI)*r) +#define DEG2RAD(d) ((EUL_PI / 180.0) * d) +#define RAD2DEG(r) ((180.0 / EUL_PI) * r) /** * Orientations are represented by 3D double precision Tait-Bryan (Euler) tuples @@ -16,18 +16,20 @@ * rotation along the Y axis rotates Z into X (leans forward), positive rotation * along the X axis rotates Y into Z (leans right). */ -typedef struct { - double alpha; // unit rad - double beta; // unit rad - double gamma; // unit rad +typedef struct +{ + double alpha; // unit rad + double beta; // unit rad + double gamma; // unit rad } EULERANGLES; -typedef enum { +typedef enum +{ EULERANGLES_NONE = 0, EULERANGLES_ZYX = 1, EULERANGLES_ZYZ = 2, EULERANGLES_XYZ = 3, -}EULERANGLES_TYPE; +} EULERANGLES_TYPE; extern const EULERANGLES EULERANGLES_ZERO; extern const EULERANGLES EULERANGLES_NIL; @@ -51,4 +53,3 @@ void EulerAngles_Normalize(EULERANGLES *e); EULERANGLES EulerAngles_GetNormalize(EULERANGLES e); #endif - diff --git a/df_datalink/src/networking/link_layer/geometry/quaternion.c b/df_datalink/src/networking/link_layer/geometry/quaternion.c index b9847d5..be2aa52 100644 --- a/df_datalink/src/networking/link_layer/geometry/quaternion.c +++ b/df_datalink/src/networking/link_layer/geometry/quaternion.c @@ -11,23 +11,26 @@ #include "eulerangles.h" #include "quaternion.h" -typedef union { +typedef union +{ float value; uint32_t raw; } SOFT_FLOAT; -typedef union { +typedef union +{ double value; uint64_t raw; } SOFT_DOUBLE; -const SOFT_FLOAT EPS_FLOAT = {.raw = 0b110100000000000000000000000000}; -const SOFT_DOUBLE EPS_DOUBLE = {.raw = 0b11110010110000000000000000000000000000000000000000000000000000}; +const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000}; +const SOFT_DOUBLE EPS_DOUBLE = {.raw = 0x3CB0000000000000}; -const QUATERNION QUATERNION_IDENTITY = {1.0, 0.0, 0.0 ,0.0}; +const QUATERNION QUATERNION_IDENTITY = {1.0, 0.0, 0.0, 0.0}; const QUATERNION QUATERNION_NIL = {NAN, NAN, NAN, NAN}; /** @brief Subtracts two quaternions. */ -QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2) { +QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2) +{ QUATERNION ret; ret.s = q1.s - q2.s; ret.i = q1.i - q2.i; @@ -37,7 +40,8 @@ QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2) { } /** @brief Adds two quaternions. */ -QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2) { +QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2) +{ QUATERNION ret; ret.s = q1.s + q2.s; ret.i = q1.i + q2.i; @@ -47,7 +51,8 @@ QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2) { } /** @brief Multiplies a quaternion by a real number. */ -void Quaternion_ScalarMult(QUATERNION *q, double t) { +void Quaternion_ScalarMult(QUATERNION *q, double t) +{ q->s *= t; q->i *= t; q->j *= t; @@ -55,13 +60,15 @@ void Quaternion_ScalarMult(QUATERNION *q, double t) { } /** @brief Multiplies a quaternion by a real number. */ -QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t) { +QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t) +{ Quaternion_ScalarMult(&a, t); return a; } /** @brief Divides a quaternion by a real number. */ -void Quaternion_ScalarDiv(QUATERNION *q, double t) { +void Quaternion_ScalarDiv(QUATERNION *q, double t) +{ q->s /= t; q->i /= t; q->j /= t; @@ -69,15 +76,17 @@ void Quaternion_ScalarDiv(QUATERNION *q, double t) { } /** @brief Divides a quaternion by a real number. */ -QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t) { +QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t) +{ Quaternion_ScalarDiv(&q, t); return q; } /** @brief Multiplies two quaternions. */ -QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2) { +QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2) +{ QUATERNION ret; - ret.s = q1.s * q2.s - (q1.i*q2.i + q1.j*q2.j + q1.k*q2.k); + ret.s = q1.s * q2.s - (q1.i * q2.i + q1.j * q2.j + q1.k * q2.k); ret.i = q1.j * q2.k - q1.k * q2.j + q1.s * q2.i + q1.i * q2.s; ret.j = q1.k * q2.i - q1.i * q2.k + q1.s * q2.j + q1.j * q2.s; ret.k = q1.i * q2.j - q1.j * q2.i + q1.s * q2.k + q1.k * q2.s; @@ -85,69 +94,80 @@ QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2) { } /** @brief Divides two quaternions. */ -QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2) { +QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2) +{ Quaternion_Inverse(&q2); return Quaternion_Mult(q1, q2); } /** @brief Dot product. */ -inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2) { - return (q1.s*q2.s + q1.i*q2.i + q1.j*q2.j + q1.k*q2.k); +inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2) +{ + return (q1.s * q2.s + q1.i * q2.i + q1.j * q2.j + q1.k * q2.k); } /** @brief Calculate a quaternion length^2 (omits calling square root). */ -inline double Quaternion_SquareLength(QUATERNION q) { - return (q.s*q.s + q.i*q.i + q.j*q.j + q.k*q.k); +inline double Quaternion_SquareLength(QUATERNION q) +{ + return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k); } /** @brief Calculate a quaternion length. */ -inline double Quaternion_Length(QUATERNION q) { +inline double Quaternion_Length(QUATERNION q) +{ return sqrt(Quaternion_SquareLength(q)); } /** @brief Normalize quaternion. */ -void Quaternion_Normalize(QUATERNION *q) { +void Quaternion_Normalize(QUATERNION *q) +{ Quaternion_ScalarDiv(q, Quaternion_Length(*q)); } /** @brief Normalize quaternion. */ -QUATERNION Quaternion_GetNormalized(QUATERNION q) { +QUATERNION Quaternion_GetNormalized(QUATERNION q) +{ Quaternion_Normalize(&q); return q; } /** @brief Conjugated quaternion. */ -void Quaternion_Conjugated(QUATERNION *q) { +void Quaternion_Conjugated(QUATERNION *q) +{ q->i = -q->i; q->j = -q->j; q->k = -q->k; } /** @brief Conjugated quaternion. */ -QUATERNION Quaternion_GetConjugated(QUATERNION q) { +QUATERNION Quaternion_GetConjugated(QUATERNION q) +{ Quaternion_Conjugated(&q); return q; } /** @brief Inverse quaternion. */ -void Quaternion_Inverse(QUATERNION *q) { +void Quaternion_Inverse(QUATERNION *q) +{ Quaternion_Conjugated(q); Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q)); } /** @brief Inverse quaternion. */ -QUATERNION Quaternion_GeInverse(QUATERNION q) { +QUATERNION Quaternion_GeInverse(QUATERNION q) +{ Quaternion_Inverse(&q); return q; } -inline double Quaternion_GetRotationAngle(QUATERNION q) { +inline double Quaternion_GetRotationAngle(QUATERNION q) +{ return (acos(q.s) * 2); } - /** @brief Get axis of quaternion. */ -COORD Quaternion_GetRotationAxis(QUATERNION q) { +COORD Quaternion_GetRotationAxis(QUATERNION q) +{ double sin_theta_inv; COORD ret; sin_theta_inv = 1.0 / sin(acos(q.s)); @@ -158,7 +178,8 @@ COORD Quaternion_GetRotationAxis(QUATERNION q) { } /** @brief Get the axis and rotate angle of a unit Quaternion */ -void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle) { +void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle) +{ double sin_theta_inv; *angle = acos(q.s); @@ -173,7 +194,8 @@ void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle } /** @brief Decompose the rotation on to 2 parts. */ -void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist) { +void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist) +{ double p; COORD v; v.x = q.i; @@ -191,16 +213,17 @@ void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QU } /** @brief rotates v by this quaternion (quaternion must be unit) */ -COORD Quaternion_Rotate(QUATERNION q, COORD v) { +COORD Quaternion_Rotate(QUATERNION q, COORD v) +{ QUATERNION temp; COORD ret; temp.s = 0; temp.i = v.x; temp.j = v.y; temp.k = v.z; - temp = Quaternion_Mult(q,temp); + temp = Quaternion_Mult(q, temp); Quaternion_Conjugated(&q); - temp = Quaternion_Mult(temp,q); + temp = Quaternion_Mult(temp, q); ret.x = temp.i; ret.y = temp.j; ret.z = temp.k; @@ -208,122 +231,132 @@ COORD Quaternion_Rotate(QUATERNION q, COORD v) { } /** @brief returns the euler angles from a rotation Quaternion */ -EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) { - EULERANGLES ret; - switch (type) { - case EULERANGLES_ZYX : { - /* Covert form matlab 2023a */ - // Cap all inputs to asin to 1, since values >1 produce complex - // results - // Since the quaternion is of unit length, this should never happen, - // but some code generation configuration seem to hit this edge case - // under some circumstances. +EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) +{ + EULERANGLES ret; + switch (type) + { + case EULERANGLES_ZYX: + { + /* Covert form matlab 2023a */ + // Cap all inputs to asin to 1, since values >1 produce complex + // results + // Since the quaternion is of unit length, this should never happen, + // but some code generation configuration seem to hit this edge case + // under some circumstances. - double asinValue; - bool mask1, mask2; + double asinValue; + bool mask1, mask2; - asinValue = -2.0 *(q.i*q.k - q.s*q.j); - mask1 = asinValue >= 1.0 - 10*EPS_DOUBLE.value; - mask2 = asinValue <= -1.0 + 10*EPS_DOUBLE.value; - if (mask1) - asinValue = 1; - if (mask2) - asinValue = -1; + asinValue = -2.0 * (q.i * q.k - q.s * q.j); + mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value; + mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value; + if (mask1) + asinValue = 1; + if (mask2) + asinValue = -1; - if(mask1 || mask2) { - ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s)); - ret.gamma = 0; - } - else { - ret.alpha = atan2(2.0*(q.i*q.j + q.s*q.k), q.s*q.s + q.i*q.i - q.j*q.j - q.k*q.k); - ret.gamma = atan2(2.0*(q.j*q.k + q.s*q.i), q.s*q.s - q.i*q.i - q.j*q.j + q.k*q.k); - } - ret.beta = asin( asinValue ); - break; - } + if (mask1 || mask2) + { + ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s)); + ret.gamma = 0; + } + else + { + ret.alpha = atan2(2.0 * (q.i * q.j + q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k); + ret.gamma = atan2(2.0 * (q.j * q.k + q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k); + } + ret.beta = asin(asinValue); + break; + } - case EULERANGLES_ZYZ : { - // TODO; - ret = EULERANGLES_NIL; - break; - } + case EULERANGLES_ZYZ: + { + // TODO; + ret = EULERANGLES_NIL; + break; + } - case EULERANGLES_XYZ : { - /* Covert form matlab 2023a */ - // Prevent singularities as done in ZYX case - // Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer - // operations - // sin(y) = R13 = 2 * (qx*qz + qy*qw) - // tan(x) = sin(x) / cos(x) = -R23 / R33 - // = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2)) - // = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2) - // tan(z) = sin(z) / cos(z) = -R12 / R11 - // = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2)) - // = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2) + case EULERANGLES_XYZ: + { + /* Covert form matlab 2023a */ + // Prevent singularities as done in ZYX case + // Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer + // operations + // sin(y) = R13 = 2 * (qx*qz + qy*qw) + // tan(x) = sin(x) / cos(x) = -R23 / R33 + // = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2)) + // = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2) + // tan(z) = sin(z) / cos(z) = -R12 / R11 + // = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2)) + // = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2) - double asinValue; - bool mask1, mask2; + double asinValue; + bool mask1, mask2; - asinValue = 2.0 *(q.i*q.k + q.s*q.j); - mask1 = asinValue >= 1.0 - 10*EPS_DOUBLE.value; - mask2 = asinValue <= -1.0 + 10*EPS_DOUBLE.value; - if (mask1) - asinValue = 1; - if (mask2) - asinValue = -1; - if(mask1 || mask2) { - ret.alpha = 0; - ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s)); - } - else { - ret.alpha = atan2(-2.0*(q.j*q.k - q.s*q.i), q.s*q.s - q.i*q.i - q.j*q.j + q.k*q.k); - ret.gamma = atan2(-2.0*(q.i*q.j - q.s*q.k), q.s*q.s + q.i*q.i - q.j*q.j - q.k*q.k); - } - ret.beta = asin( asinValue ); - break; - } + asinValue = 2.0 * (q.i * q.k + q.s * q.j); + mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value; + mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value; + if (mask1) + asinValue = 1; + if (mask2) + asinValue = -1; + if (mask1 || mask2) + { + ret.alpha = 0; + ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s)); + } + else + { + ret.alpha = atan2(-2.0 * (q.j * q.k - q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k); + ret.gamma = atan2(-2.0 * (q.i * q.j - q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k); + } + ret.beta = asin(asinValue); + break; + } - - default: - ret = EULERANGLES_NIL; - } - return ret; + default: + ret = EULERANGLES_NIL; + } + return ret; } /** @brief returns the rotation Quaternion from a euler angles */ -QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type) { - QUATERNION ret; - double cosx, sinx, cosy, siny, cosz, sinz; - cosx = cos(e.alpha * 0.5); +QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type) +{ + QUATERNION ret; + double cosx, sinx, cosy, siny, cosz, sinz; + cosx = cos(e.alpha * 0.5); sinx = sin(e.alpha * 0.5); cosy = cos(e.beta * 0.5); siny = sin(e.beta * 0.5); cosz = cos(e.gamma * 0.5); sinz = sin(e.gamma * 0.5); - switch (type) { - case EULERANGLES_ZYX : - ret.s = cosx * cosy * cosz + sinx * siny * sinz; - ret.i = cosx * cosy * sinz - sinx * siny * cosz; - ret.j = cosx * siny * cosz + sinx * cosy * sinz; - ret.k = sinx * cosy * cosz - cosx * siny * sinz; - break; - case EULERANGLES_ZYZ : - ret.s = cosx * cosy * cosz - sinx * cosy * sinz; - ret.i = cosx * siny * sinz - sinx * siny * cosz; - ret.j = cosx * siny * cosz + sinx * siny * sinz; - ret.k = sinx * cosy * cosz + cosx * cosy * sinz; - break; - case EULERANGLES_XYZ : - ret.s = cosx * cosy * cosz - sinx * siny * sinz; - ret.i = sinx * cosy * cosz + cosx * siny * sinz; - ret.j = -sinx * cosy * sinz + cosx * siny * cosz; - ret.k = cosx * cosy * sinz + sinx * siny * cosz; - break; - default: - ret = QUATERNION_NIL; - break; - } - return ret; + switch (type) + { + case EULERANGLES_ZYX: + ret.s = cosx * cosy * cosz + sinx * siny * sinz; + ret.i = cosx * cosy * sinz - sinx * siny * cosz; + ret.j = cosx * siny * cosz + sinx * cosy * sinz; + ret.k = sinx * cosy * cosz - cosx * siny * sinz; + break; + case EULERANGLES_ZYZ: + ret.s = cosx * cosy * cosz - sinx * cosy * sinz; + ret.i = cosx * siny * sinz - sinx * siny * cosz; + ret.j = cosx * siny * cosz + sinx * siny * sinz; + ret.k = sinx * cosy * cosz + cosx * cosy * sinz; + break; + case EULERANGLES_XYZ: + ret.s = cosx * cosy * cosz - sinx * siny * sinz; + ret.i = sinx * cosy * cosz + cosx * siny * sinz; + ret.j = -sinx * cosy * sinz + cosx * siny * cosz; + ret.k = cosx * cosy * sinz + sinx * siny * cosz; + break; + default: + ret = QUATERNION_NIL; + break; + } + return ret; } /** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */ @@ -338,10 +371,12 @@ QUATERNION Quaternion_RotationFromTo(COORD from, COORD to) c = Coord_Dot(from, to); - if (c >= 1.0) { // If dot == 1, vectors are the same + if (c >= 1.0) + { // If dot == 1, vectors are the same return QUATERNION_IDENTITY; // identity } - else if (c <= -1.0) { // exactly opposite + else if (c <= -1.0) + { // exactly opposite axis = Coord_Cross(COORD_X_AXIS, from); if (Coord_Length(axis) == 0) @@ -367,38 +402,41 @@ QUATERNION Quaternion_RotationFromTo(COORD from, COORD to) return ret; } - /** @brief linear quaternion interpolation */ -QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t) { +QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t) +{ QUATERNION ret; Quaternion_ScalarMult(&q1, 1 - t); Quaternion_ScalarMult(&q1, t); ret = Quaternion_Add(q1, q2); Quaternion_Normalize(&ret); - return ret; + return ret; } - /** @brief spherical linear interpolation */ -QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t) { +QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t) +{ double dot; QUATERNION q3, ret; dot = Quaternion_Dot(q1, q2); /* dot = cos(theta) if (dot < 0), q1 and q2 are more than 90 degrees apart, so we can invert one to reduce spinning */ - if (dot < 0) { + if (dot < 0) + { dot = -dot; q3.s = -q2.s; q3.i = -q2.i; q3.j = -q2.j; q3.k = -q2.k; } - else { - q3 = q2; + else + { + q3 = q2; } - if (dot < 0.95) { + if (dot < 0.95) + { double angle = acos(dot); Quaternion_ScalarMult(&q1, sin(angle * (1 - t))); Quaternion_ScalarMult(&q3, sin(angle * t)); @@ -407,7 +445,8 @@ QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t) { return ret; } // if the angle is small, use linear interpolation - else { + else + { return Quaternion_Lerp(q1, q3, t); } } diff --git a/df_datalink/src/networking/link_layer/geometry/quaternion.h b/df_datalink/src/networking/link_layer/geometry/quaternion.h index c0806a2..7e766c4 100644 --- a/df_datalink/src/networking/link_layer/geometry/quaternion.h +++ b/df_datalink/src/networking/link_layer/geometry/quaternion.h @@ -6,7 +6,8 @@ #include "eulerangles.h" // borrowed and adapted from https://github.com/MegaManSE/willperone/blob/master/Math/quaternion.h -typedef struct { +typedef struct +{ double s; double i; double j; @@ -106,4 +107,3 @@ QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t); QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t); #endif -