LOST 0.0.1
LOST: Open-source Star Tracker
Loading...
Searching...
No Matches
attitude-utils.cpp
Go to the documentation of this file.
1#include "attitude-utils.hpp"
2
3#include <math.h>
4#include <assert.h>
5#include <cmath>
6#include <iostream>
7
8#include "decimal.hpp"
10
11namespace lost {
12
15 return Quaternion(
16 real*other.real - i*other.i - j*other.j - k*other.k,
17 real*other.i + other.real*i + j*other.k - k*other.j,
18 real*other.j + other.real*j + k*other.i - i*other.k,
19 real*other.k + other.real*k + i*other.j - j*other.i);
20}
21
24 return Quaternion(real, -i, -j, -k);
25}
26
29 return { i, j, k };
30}
31
33void Quaternion::SetVector(const Vec3 &vec) {
34 i = vec.x;
35 j = vec.y;
36 k = vec.z;
37}
38
41 real = 0;
42 SetVector(input);
43}
44
46Quaternion::Quaternion(const Vec3 &input, decimal theta) {
47 real = DECIMAL_COS(theta/2);
48 // the compiler will optimize it. Right?
49 i = input.x * DECIMAL_SIN(theta/2);
50 j = input.y * DECIMAL_SIN(theta/2);
51 k = input.z * DECIMAL_SIN(theta/2);
52}
53
55Vec3 Quaternion::Rotate(const Vec3 &input) const {
56 // TODO: optimize
57 return ((*this)*Quaternion(input)*Conjugate()).Vector();
58}
59
62 if (real <= -1) {
63 return 0; // 180*2=360=0
64 }
65 // TODO: we shouldn't need this nonsense, right? how come acos sometimes gives nan? (same as in AngleUnit)
66 return (real >= 1 ? 0 : DECIMAL_ACOS(real))*2;
67}
68
70 decimal rawAngle = Angle();
71 return rawAngle > DECIMAL_M_PI
72 ? 2*DECIMAL_M_PI - rawAngle
73 : rawAngle;
74}
75
77 real = DECIMAL_COS(newAngle/2);
78 SetVector(Vector().Normalize() * DECIMAL_SIN(newAngle/2));
79}
80
82 // Working out these equations would be a pain in the ass. Thankfully, this wikipedia page:
83 // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion
84 // uses almost exactly the same euler angle scheme that we do, so we copy their equations almost
85 // wholesale. The only differences are that 1, we use de and roll in the opposite directions,
86 // and 2, we store the conjugate of the quaternion (double check why?), which means we need to
87 // invert the final de and roll terms, as well as negate all the terms involving a mix between
88 // the real and imaginary parts.
89 decimal ra = DECIMAL_ATAN2(2*(-real*k+i*j), 1-2*(j*j+k*k));
90 if (ra < 0)
91 ra += 2*DECIMAL_M_PI;
92 decimal de = -DECIMAL_ASIN(2*(-real*j-i*k)); // allow de to be positive or negaive, as is convention
93 decimal roll = -DECIMAL_ATAN2(2*(-real*i+j*k), 1-2*(i*i+j*j));
94 if (roll < 0)
95 roll += 2*DECIMAL_M_PI;
96
97 return EulerAngles(ra, de, roll);
98}
99
101 assert(roll >= DECIMAL(0.0) && roll <= 2*DECIMAL_M_PI);
102 assert(ra >= DECIMAL(0.0) && ra <= 2*DECIMAL_M_PI);
103 assert(dec >= -DECIMAL_M_PI && dec <= DECIMAL_M_PI);
104
105 // when we are modifying the coordinate axes, the quaternion multiplication works so that the
106 // rotations are applied from left to right. This is the opposite as for modifying vectors.
107
108 // It is indeed correct that a positive rotation in our right-handed coordinate frame is in the
109 // clockwise direction when looking down/through the axis of rotation. Just like the right hand
110 // rule for magnetic field around a current-carrying conductor.
111 Quaternion a = Quaternion({ 0, 0, 1 }, ra);
112 Quaternion b = Quaternion({ 0, 1, 0 }, -dec);
113 Quaternion c = Quaternion({ 1, 0, 0 }, -roll);
114 Quaternion result = (a*b*c).Conjugate();
115 assert(result.IsUnit(0.00001));
116 return result;
117}
118
120bool Quaternion::IsUnit(decimal tolerance) const {
121 return abs(i*i+j*j+k*k+real*real - 1) < tolerance;
122}
123
128 if (real >= 0) {
129 return *this;
130 }
131
132 return Quaternion(-real, -i, -j, -k);
133}
134
137 return {
138 DECIMAL_COS(ra)*DECIMAL_COS(de),
139 DECIMAL_SIN(ra)*DECIMAL_COS(de),
140 DECIMAL_SIN(de),
141 };
142}
143
145void SpatialToSpherical(const Vec3 &vec, decimal *ra, decimal *de) {
146 *ra = DECIMAL_ATAN2(vec.y, vec.x);
147 if (*ra < 0)
148 *ra += DECIMAL_M_PI*2;
149 *de = DECIMAL_ASIN(vec.z);
150}
151
153 return rad*DECIMAL(180.0)/DECIMAL_M_PI;
154}
155
157 return deg/DECIMAL(180.0)*DECIMAL_M_PI;
158}
159
161 return RadToDeg(rad) * DECIMAL(3600.0);
162}
163
165 return DegToRad(arcSec / DECIMAL(3600.0));
166}
167
169 // first but not last chatgpt generated code in lost:
170 decimal result = x - mod * DECIMAL_FLOOR(x / mod);
171 return result >= 0 ? result : result + mod;
172}
173
174std::ostream &operator<<(std::ostream &output, const Vec2 &vec) {
175 output << "Vec2(x: " << vec.x << ", y: " << vec.y << ")";
176 return output;
177}
178
179std::ostream &operator<<(std::ostream &output, const Vec3 &vec) {
180 output << "Vec3(x: " << vec.x << ", y: " << vec.y << ", z: " << vec.z << ")";
181 return output;
182}
183
186 return x*x + y*y + z*z;
187}
188
191 return x*x + y*y;
192}
193
195 return DECIMAL_HYPOT(DECIMAL_HYPOT(x, y), z); // not sure if this is faster than a simple sqrt, but it does have less error?
196}
197
199 return DECIMAL_HYPOT(x, y);
200}
201
204 decimal mag = Magnitude();
205 return {
206 x/mag, y/mag, z/mag,
207 };
208}
209
211decimal Vec3::operator*(const Vec3 &other) const {
212 return x*other.x + y*other.y + z*other.z;
213}
214
216Vec2 Vec2::operator*(const decimal &other) const {
217 return { x*other, y*other };
218}
219
221Vec3 Vec3::operator*(const decimal &other) const {
222 return { x*other, y*other, z*other };
223}
224
226Vec2 Vec2::operator+(const Vec2 &other) const {
227 return {x + other.x, y + other.y };
228}
229
231Vec2 Vec2::operator-(const Vec2 &other) const {
232 return { x - other.x, y - other.y };
233}
234
236Vec3 Vec3::operator-(const Vec3 &other) const {
237 return { x - other.x, y - other.y, z - other.z };
238}
239
240Vec3 Vec3::operator+(const Vec3 &other) const {
241 return {x + other.x, y + other.y, z + other.z};
242}
243
245Vec3 Vec3::CrossProduct(const Vec3 &other) const {
246 return {
247 y*other.z - z*other.y,
248 -(x*other.z - z*other.x),
249 x*other.y - y*other.x,
250 };
251}
252
254Mat3 Vec3::OuterProduct(const Vec3 &other) const {
255 return {
256 x*other.x, x*other.y, x*other.z,
257 y*other.x, y*other.y, y*other.z,
258 z*other.x, z*other.y, z*other.z
259 };
260}
261
263Vec3 Vec3::operator*(const Mat3 &other) const {
264 return {
265 x*other.At(0,0) + y*other.At(0,1) + z*other.At(0,2),
266 x*other.At(1,0) + y*other.At(1,1) + z*other.At(1,2),
267 x*other.At(2,0) + y*other.At(2,1) + z*other.At(2,2),
268 };
269}
270
272decimal Mat3::At(int i, int j) const {
273 return x[3*i+j];
274}
275
277Vec3 Mat3::Column(int j) const {
278 return {At(0,j), At(1,j), At(2,j)};
279}
280
282Vec3 Mat3::Row(int i) const {
283 return {At(i,0), At(i,1), At(i,2)};
284}
285
287Mat3 Mat3::operator+(const Mat3 &other) const {
288 return {
289 At(0,0)+other.At(0,0), At(0,1)+other.At(0,1), At(0,2)+other.At(0,2),
290 At(1,0)+other.At(1,0), At(1,1)+other.At(1,1), At(1,2)+other.At(1,2),
291 At(2,0)+other.At(2,0), At(2,1)+other.At(2,1), At(2,2)+other.At(2,2)
292 };
293}
294
296Mat3 Mat3::operator*(const Mat3 &other) const {
297#define _MATMUL_ENTRY(row, col) At(row,0)*other.At(0,col) + At(row,1)*other.At(1,col) + At(row,2)*other.At(2,col)
298 return {
302 };
303#undef _MATMUL_ENTRY
304}
305
307Vec3 Mat3::operator*(const Vec3 &vec) const {
308 return {
309 vec.x*At(0,0) + vec.y*At(0,1) + vec.z*At(0,2),
310 vec.x*At(1,0) + vec.y*At(1,1) + vec.z*At(1,2),
311 vec.x*At(2,0) + vec.y*At(2,1) + vec.z*At(2,2),
312 };
313}
314
316Mat3 Mat3::operator*(const decimal &s) const {
317 return {
318 s*At(0,0), s*At(0,1), s*At(0,2),
319 s*At(1,0), s*At(1,1), s*At(1,2),
320 s*At(2,0), s*At(2,1), s*At(2,2)
321 };
322}
323
326 return {
327 At(0,0), At(1,0), At(2,0),
328 At(0,1), At(1,1), At(2,1),
329 At(0,2), At(1,2), At(2,2),
330 };
331}
332
335 return At(0,0) + At(1,1) + At(2,2);
336}
337
340 return (At(0,0) * (At(1,1)*At(2,2) - At(2,1)*At(1,2))) - (At(0,1) * (At(1,0)*At(2,2) - At(2,0)*At(1,2))) + (At(0,2) * (At(1,0)*At(2,1) - At(2,0)*At(1,1)));
341}
342
345 // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/
346 decimal scalar = 1 / Det();
347
348 Mat3 res = {
349 At(1,1)*At(2,2) - At(1,2)*At(2,1), At(0,2)*At(2,1) - At(0,1)*At(2,2), At(0,1)*At(1,2) - At(0,2)*At(1,1),
350 At(1,2)*At(2,0) - At(1,0)*At(2,2), At(0,0)*At(2,2) - At(0,2)*At(2,0), At(0,2)*At(1,0) - At(0,0)*At(1,2),
351 At(1,0)*At(2,1) - At(1,1)*At(2,0), At(0,1)*At(2,0) - At(0,0)*At(2,1), At(0,0)*At(1,1) - At(0,1)*At(1,0)
352 };
353
354 return res * scalar;
355}
356
358 const Mat3 kIdentityMat3 = {1,0,0,
359 0,1,0,
360 0,0,1};
361
363 : type(AttitudeType::QuaternionType), quaternion(quat) {}
364
366 : type(AttitudeType::DCMType), dcm(matrix) {}
367
370 Vec3 x = quat.Rotate({1, 0, 0});
371 Vec3 y = quat.Rotate({0, 1, 0});
372 Vec3 z = quat.Rotate({0, 0, 1});
373 return {
374 x.x, y.x, z.x,
375 x.y, y.y, z.y,
376 x.z, y.z, z.z,
377 };
378}
379
382 // Make a quaternion that rotates the reference frame X-axis into the dcm's X-axis, just like
383 // the DCM itself does
384 Vec3 oldXAxis = Vec3({1, 0, 0});
385 Vec3 newXAxis = dcm.Column(0); // this is where oldXAxis is mapped to
386 assert(DECIMAL_ABS(newXAxis.Magnitude()-1) < DECIMAL(0.001));
387 Vec3 xAlignAxis = oldXAxis.CrossProduct(newXAxis).Normalize();
388 decimal xAlignAngle = AngleUnit(oldXAxis, newXAxis);
389 Quaternion xAlign(xAlignAxis, xAlignAngle);
390
391 // Make a quaternion that will rotate the Y-axis into place
392 Vec3 oldYAxis = xAlign.Rotate({0, 1, 0});
393 Vec3 newYAxis = dcm.Column(1);
394 // we still need to take the cross product, because acos returns a value in [0,pi], and thus we
395 // need to know which direction to rotate before we rotate. We do this by checking if the cross
396 // product of old and new y axes is in the same direction as the new X axis.
397 bool rotateClockwise = oldYAxis.CrossProduct(newYAxis) * newXAxis > 0; // * is dot product
398 Quaternion yAlign({1, 0, 0}, AngleUnit(oldYAxis, newYAxis) * (rotateClockwise ? 1 : -1));
399
400 // We're done! There's no need to worry about the Z-axis because the handed-ness of the
401 // coordinate system is always preserved, which means the Z-axis is uniquely determined as the
402 // cross product of the X- and Y-axes. This goes to show that DCMs store redundant information.
403
404 // we want the y alignment to have memory of X, which means we put its multiplication on the
405 // right
406 return xAlign*yAlign;
407}
408
411 switch (type) {
412 case AttitudeType::QuaternionType:
413 return quaternion;
414 case AttitudeType::DCMType:
415 return DCMToQuaternion(dcm);
416 default:
417 assert(false);
418 }
419}
420
423 switch (type) {
424 case AttitudeType::DCMType:
425 return dcm;
426 case AttitudeType::QuaternionType:
427 return QuaternionToDCM(quaternion);
428 default:
429 assert(false);
430 }
431}
432
434Vec3 Attitude::Rotate(const Vec3 &vec) const {
435 switch (type) {
436 case AttitudeType::DCMType:
437 return dcm*vec;
438 case AttitudeType::QuaternionType:
439 return quaternion.Rotate(vec);
440 default:
441 assert(false);
442 }
443}
444
447 switch (type) {
448 case AttitudeType::DCMType:
449 return GetQuaternion().ToSpherical();
450 case AttitudeType::QuaternionType:
451 return quaternion.ToSpherical();
452 default:
453 assert(false);
454 }
455}
456
457bool Attitude::IsKnown() const {
458 switch (type) {
459 case AttitudeType::DCMType:
460 case AttitudeType::QuaternionType:
461 return true;
462 default:
463 return false;
464 }
465}
466
468void SerializeVec3(SerializeContext *ser, const Vec3 &vec) {
472}
473
475 Vec3 result = {
479 };
480 return result;
481}
482
484decimal Angle(const Vec3 &vec1, const Vec3 &vec2) {
485 return AngleUnit(vec1.Normalize(), vec2.Normalize());
486}
487
493decimal AngleUnit(const Vec3 &vec1, const Vec3 &vec2) {
494 decimal dot = vec1*vec2;
495 // TODO: we shouldn't need this nonsense, right? how come acos sometimes gives nan?
496 return dot >= 1 ? 0 : dot <= -1 ? DECIMAL_M_PI-DECIMAL(0.0000001) : DECIMAL_ACOS(dot);
497}
498
499}
#define _MATMUL_ENTRY(row, col)
EulerAngles ToSpherical() const
Get the euler angles from the attitude, converting from whatever format is stored.
Vec3 Rotate(const Vec3 &) const
Convert a vector from the reference frame to the body frame.
Mat3 GetDCM() const
Get the rotation matrix (direction cosine matrix) representing the attitude, converting from whatever...
Attitude()=default
constructs unknown attitude:
Quaternion GetQuaternion() const
Get the quaternion representing the attitude, converting from whatever format is stored.
bool IsKnown() const
A "human-readable" way to represent a 3d rotation or orientation.
3x3 vector with decimaling point components
Mat3 Inverse() const
Inverse of a matrix.
Vec3 Column(int) const
Get the column at index j.
Vec3 Row(int) const
Get the row at index i.
decimal Det() const
Determinant of a matrix.
Mat3 Transpose() const
Transpose of a matrix.
decimal Trace() const
Trace of a matrix.
decimal At(int i, int j) const
Access the i,j-th element of the matrix.
Mat3 operator+(const Mat3 &) const
Normal matrix addition.
decimal x[9]
Mat3 operator*(const Mat3 &) const
Naive matrix multiplication.
A quaternion is a common way to represent a 3d rotation.
Quaternion Canonicalize() const
Ensure that the quaternion's real part is nonnegative.
Vec3 Vector() const
The vector formed by imaginary components of the quaternion. The axis of the represented rotation.
decimal Angle() const
How many radians the rotation represented by this quaternion has.
EulerAngles ToSpherical() const
Vec3 Rotate(const Vec3 &) const
Rotate a 3d vector according to the rotation represented by the quaternion.
Quaternion Conjugate() const
Effectively computes a quaternion representing the inverse rotation of the original.
void SetVector(const Vec3 &)
Set imaginary components.
Quaternion()=default
Quaternion operator*(const Quaternion &other) const
Multiply two quaternions using the usual definition of quaternion multiplication (effectively compose...
void SetAngle(decimal)
bool IsUnit(decimal tolerance) const
Whether the quaternion is a unit quaternion. All quaternions representing rotations should be units.
decimal SmallestAngle() const
Returns the smallest angle that can be used to represent the rotation represented by the quaternion.
Three dimensional vector with decimaling point components.
Vec3 Normalize() const
Create a vector pointing in the same direction with magnitude 1.
decimal MagnitudeSq() const
The square of the magnitude.
Mat3 OuterProduct(const Vec3 &) const
The outer product of two vectors.
decimal operator*(const Vec3 &) const
Dot product.
Vec3 operator+(const Vec3 &) const
Vec3 CrossProduct(const Vec3 &) const
Usual vector cross product.
decimal Magnitude() const
Vec3 operator-(const Vec3 &) const
Usual vector subtraction.
double decimal
Definition decimal.hpp:8
#define DECIMAL_ASIN(x)
Definition decimal.hpp:52
#define DECIMAL(x)
Definition decimal.hpp:18
#define DECIMAL_M_PI
Definition decimal.hpp:26
#define DECIMAL_HYPOT(x, y)
Definition decimal.hpp:58
#define DECIMAL_SIN(x)
Definition decimal.hpp:49
#define DECIMAL_ABS(x)
Definition decimal.hpp:46
#define DECIMAL_ATAN2(x, y)
Definition decimal.hpp:55
#define DECIMAL_COS(x)
Definition decimal.hpp:50
#define DECIMAL_ACOS(x)
Definition decimal.hpp:53
#define DECIMAL_FLOOR(x)
Definition decimal.hpp:45
LOST starting point.
Mat3 QuaternionToDCM(const Quaternion &quat)
Convert a quaternion to a rotation matrix (Direction Cosine Matrix).
decimal RadToArcSec(decimal rad)
decimal ArcSecToRad(decimal arcSec)
void SpatialToSpherical(const Vec3 &vec, decimal *ra, decimal *de)
Convert from a 3d point on the unit sphere to right ascension & declination.
decimal AngleUnit(const Vec3 &vec1, const Vec3 &vec2)
Calculate the inner angle, in radians, between two /unit/ vectors.
decimal DegToRad(decimal deg)
Quaternion SphericalToQuaternion(decimal ra, decimal dec, decimal roll)
Return a quaternion that will reorient the coordinate axes so that the x-axis points at the given rig...
Vec3 SphericalToSpatial(decimal ra, decimal de)
Convert from right ascension & declination to a 3d point on the unit sphere.
decimal RadToDeg(decimal rad)
const Mat3 kIdentityMat3
3x3 identity matrix
Quaternion DCMToQuaternion(const Mat3 &dcm)
Convert a rotation matrix (Direction Cosine Matrix) to a quaternion representing the same rotation.
decimal Angle(const Vec3 &vec1, const Vec3 &vec2)
Calculate the inner angle, in radians, between two vectors.
T DeserializePrimitive(DeserializeContext *des)
decimal DecimalModulo(decimal x, decimal mod)
Given a decimal, find it "modulo" another decimal, in the true mathematical sense (not remainder).
void SerializePrimitive(SerializeContext *ser, const T &val)
void SerializeVec3(SerializeContext *ser, const Vec3 &vec)
Serialize a Vec3 to buffer. Takes up space according to SerializeLengthVec3.
Vec3 DeserializeVec3(DeserializeContext *des)
std::ostream & operator<<(std::ostream &output, const Vec2 &vec)
A two dimensional vector with decimaling point components.
decimal MagnitudeSq() const
The square of the magnitude.
Vec2 operator-(const Vec2 &) const
Usual vector subtraction.
decimal operator*(const Vec2 &) const
Vec2 operator+(const Vec2 &) const
Usual vector addition.
decimal Magnitude() const