XiaQingMiao/datalink_nc_mode
1
0
Fork 0
forked from 0xha0/datalink_repo
Code Pull Requests 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
+36 -20
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};
@@ -16,7 +15,8 @@ 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,43 +107,49 @@ 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;
@@ -165,7 +180,8 @@ COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) {
} }
/** @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;
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
+12 -8
View File
@@ -7,13 +7,12 @@
#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;
@@ -22,7 +21,8 @@ EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2) {
} }
/** @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;
@@ -31,27 +31,31 @@ EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2) {
} }
/** @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->alpha = fmod(e->alpha, 2 * EUL_PI);
e->beta = fmod(e->beta, 2 * EUL_PI); e->beta = fmod(e->beta, 2 * EUL_PI);
e->gamma = fmod(e->gamma, 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
+4 -3
View File
@@ -16,13 +16,15 @@
* 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 alpha; // unit rad
double beta; // unit rad double beta; // unit rad
double gamma; // 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,
@@ -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
+88 -49
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,13 +76,15 @@ 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;
@@ -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,7 +213,8 @@ 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;
@@ -208,10 +231,13 @@ 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; EULERANGLES ret;
switch (type) { switch (type)
case EULERANGLES_ZYX : { {
case EULERANGLES_ZYX:
{
/* Covert form matlab 2023a */ /* Covert form matlab 2023a */
// Cap all inputs to asin to 1, since values >1 produce complex // Cap all inputs to asin to 1, since values >1 produce complex
// results // results
@@ -230,11 +256,13 @@ EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) {
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.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
ret.gamma = 0; ret.gamma = 0;
} }
else { 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.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.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);
} }
@@ -242,13 +270,15 @@ EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) {
break; break;
} }
case EULERANGLES_ZYZ : { case EULERANGLES_ZYZ:
{
// TODO; // TODO;
ret = EULERANGLES_NIL; ret = EULERANGLES_NIL;
break; break;
} }
case EULERANGLES_XYZ : { case EULERANGLES_XYZ:
{
/* Covert form matlab 2023a */ /* Covert form matlab 2023a */
// Prevent singularities as done in ZYX case // Prevent singularities as done in ZYX case
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer // Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
@@ -271,11 +301,13 @@ EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) {
asinValue = 1; asinValue = 1;
if (mask2) if (mask2)
asinValue = -1; asinValue = -1;
if(mask1 || mask2) { if (mask1 || mask2)
{
ret.alpha = 0; ret.alpha = 0;
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s)); ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
} }
else { 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.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.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);
} }
@@ -283,7 +315,6 @@ EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) {
break; break;
} }
default: default:
ret = EULERANGLES_NIL; ret = EULERANGLES_NIL;
} }
@@ -291,7 +322,8 @@ EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) {
} }
/** @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; QUATERNION ret;
double cosx, sinx, cosy, siny, cosz, sinz; double cosx, sinx, cosy, siny, cosz, sinz;
cosx = cos(e.alpha * 0.5); cosx = cos(e.alpha * 0.5);
@@ -300,7 +332,8 @@ QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type) {
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: case EULERANGLES_ZYX:
ret.s = cosx * cosy * cosz + sinx * siny * sinz; ret.s = cosx * cosy * cosz + sinx * siny * sinz;
ret.i = cosx * cosy * sinz - sinx * siny * cosz; ret.i = cosx * cosy * sinz - sinx * siny * cosz;
@@ -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