XiaQingMiao/datalink_nc_mode
1
0
Fork 0
forked from gatsby/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
+66 -50
View File
@@ -7,7 +7,6 @@
#include <math.h>
#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;
}
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
//
#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
df_datalink/src/networking/link_layer/geometry/eulerangles.c
+18 -14
View File
@@ -7,51 +7,55 @@
#include <math.h>
#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;
}
df_datalink/src/networking/link_layer/geometry/eulerangles.h
+10 -9
View File
@@ -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
df_datalink/src/networking/link_layer/geometry/quaternion.c
+184 -145
View File
@@ -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);
}
}
df_datalink/src/networking/link_layer/geometry/quaternion.h
+2 -2
View File
@@ -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