0xha0/datalink_repo
2
2
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

formatted.

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