11/*
2- Copyright (c) 2021-2023 . Sylvain Guillet (sylvain.guillet@tutamail.com)
2+ Copyright (c) 2021-2024 . Sylvain Guillet (sylvain.guillet@tutamail.com)
33 */
44#include < Quaternion.h>
55#include < cmath>
@@ -14,101 +14,104 @@ IO::Astrodynamics::Math::Quaternion::Quaternion(double q0, double q1, double q2,
1414
1515IO::Astrodynamics::Math::Quaternion::Quaternion (const IO::Astrodynamics::Math::Vector3D &axis, const double angle)
1616{
17- double c{std::cos (angle / 2 )};
18- double s{std::sin (angle / 2 )};
19- const_cast <double &>(m_q0) = c;
20- const_cast <double &>(m_q1) = s * axis.GetX ();
21- const_cast <double &>(m_q2) = s * axis.GetY ();
22- const_cast <double &>(m_q3) = s * axis.GetZ ();
17+ double c{std::cos (angle / 2 )};
18+ double s{std::sin (angle / 2 )};
19+ const_cast <double &>(m_q0) = c;
20+ const_cast <double &>(m_q1) = s * axis.GetX ();
21+ const_cast <double &>(m_q2) = s * axis.GetY ();
22+ const_cast <double &>(m_q3) = s * axis.GetZ ();
2323}
2424
2525IO::Astrodynamics::Math::Quaternion::Quaternion (const IO::Astrodynamics::Math::Matrix &mtx)
2626{
27- SpiceDouble m[3 ][3 ]{};
28-
29- for (size_t i = 0 ; i < 3 ; i++)
30- {
31- for (size_t j = 0 ; j < 3 ; j++)
32- {
33- m[i][j] = mtx.GetValue (i, j);
34- }
35- }
36-
37- SpiceDouble q[4 ];
38- m2q_c (m, q);
39-
40- const_cast <double &>(m_q0) = q[0 ];
41- const_cast <double &>(m_q1) = q[1 ];
42- const_cast <double &>(m_q2) = q[2 ];
43- const_cast <double &>(m_q3) = q[3 ];
27+ SpiceDouble m[3 ][3 ]{};
28+
29+ for (size_t i = 0 ; i < 3 ; i++)
30+ {
31+ for (size_t j = 0 ; j < 3 ; j++)
32+ {
33+ m[i][j] = mtx.GetValue (i, j);
34+ }
35+ }
36+
37+ SpiceDouble q[4 ];
38+ m2q_c (m, q);
39+
40+ const_cast <double &>(m_q0) = q[0 ];
41+ const_cast <double &>(m_q1) = q[1 ];
42+ const_cast <double &>(m_q2) = q[2 ];
43+ const_cast <double &>(m_q3) = q[3 ];
4444}
4545
46- IO::Astrodynamics::Math::Quaternion::Quaternion (const IO::Astrodynamics::Math::Quaternion &quaternion):Quaternion(quaternion.GetQ0(),quaternion.GetQ1(),quaternion.GetQ2(),quaternion.GetQ3())
46+ IO::Astrodynamics::Math::Quaternion::Quaternion (const IO::Astrodynamics::Math::Quaternion &quaternion) : Quaternion(quaternion.GetQ0(), quaternion.GetQ1(), quaternion.GetQ2(),
47+ quaternion.GetQ3())
4748{
4849
4950}
5051
5152IO::Astrodynamics::Math::Quaternion IO::Astrodynamics::Math::Quaternion::Multiply (const Quaternion &quaternion) const
5253{
53- return *this * quaternion;
54+ return *this * quaternion;
5455}
5556
5657IO::Astrodynamics::Math::Quaternion IO::Astrodynamics::Math::Quaternion::operator *(const Quaternion &quaternion) const
5758{
58- ConstSpiceDouble _this[4 ] = {m_q0, m_q1, m_q2, m_q3};
59- ConstSpiceDouble other[4 ] = {quaternion.m_q0 , quaternion.m_q1 , quaternion.m_q2 , quaternion.m_q3 };
60- SpiceDouble res[4 ];
61- qxq_c (_this, other, res);
62- return Quaternion{res[0 ], res[1 ], res[2 ], res[3 ]};
59+ ConstSpiceDouble _this[4 ] = {m_q0, m_q1, m_q2, m_q3};
60+ ConstSpiceDouble other[4 ] = {quaternion.m_q0 , quaternion.m_q1 , quaternion.m_q2 , quaternion.m_q3 };
61+ SpiceDouble res[4 ];
62+ qxq_c (_this, other, res);
63+ return Quaternion{res[0 ], res[1 ], res[2 ], res[3 ]};
6364}
6465
6566IO::Astrodynamics::Math::Matrix IO::Astrodynamics::Math::Quaternion::GetMatrix () const
6667{
67- SpiceDouble mtx[3 ][3 ];
68- ConstSpiceDouble q[4 ] = {m_q0, m_q1, m_q2, m_q3};
69- q2m_c (q, mtx);
70-
71- double **exportMtx = new double *[3 ];
72- for (int i = 0 ; i < 3 ; i++)
73- {
74- exportMtx[i] = new double [3 ]{};
75- }
76-
77- for (size_t i = 0 ; i < 3 ; i++)
78- {
79- for (size_t j = 0 ; j < 3 ; j++)
80- {
81- exportMtx[i][j] = mtx[i][j];
82- }
83- }
84-
85- return IO::Astrodynamics::Math::Matrix{3 , 3 , exportMtx};
68+ SpiceDouble mtx[3 ][3 ];
69+ ConstSpiceDouble q[4 ] = {m_q0, m_q1, m_q2, m_q3};
70+ q2m_c (q, mtx);
71+ double exportMtx_data[3 ][3 ]{};
72+ double *exportMtx[3 ];
73+
74+ // Assign pointers to the rows
75+ for (int i = 0 ; i < 3 ; ++i)
76+ {
77+ exportMtx[i] = exportMtx_data[i];
78+ }
79+
80+ // Copy data from mtx to exportMtx
81+ for (size_t i = 0 ; i < 3 ; ++i)
82+ {
83+ for (size_t j = 0 ; j < 3 ; ++j)
84+ {
85+ exportMtx[i][j] = mtx[i][j];
86+ }
87+ }
88+ return IO::Astrodynamics::Math::Matrix{3 , 3 , exportMtx};
8689}
8790
8891double IO::Astrodynamics::Math::Quaternion::Magnitude () const
8992{
90- return std::sqrt (m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3);
93+ return std::sqrt (m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3);
9194}
9295
9396IO::Astrodynamics::Math::Quaternion IO::Astrodynamics::Math::Quaternion::Normalize () const
9497{
95- auto magnitude = Magnitude ();
96- return IO::Astrodynamics::Math::Quaternion{m_q0 / magnitude, m_q1 / magnitude, m_q2 / magnitude, m_q3 / magnitude};
98+ auto magnitude = Magnitude ();
99+ return IO::Astrodynamics::Math::Quaternion{m_q0 / magnitude, m_q1 / magnitude, m_q2 / magnitude, m_q3 / magnitude};
97100}
98101
99102IO::Astrodynamics::Math::Quaternion IO::Astrodynamics::Math::Quaternion::Conjugate () const
100103{
101- return IO::Astrodynamics::Math::Quaternion{m_q0, -m_q1, -m_q2, -m_q3};
104+ return IO::Astrodynamics::Math::Quaternion{m_q0, -m_q1, -m_q2, -m_q3};
102105}
103106
104107IO::Astrodynamics::Math::Quaternion &IO::Astrodynamics::Math::Quaternion::operator =(const IO::Astrodynamics::Math::Quaternion &quaternion)
105108{
106- if (this != &quaternion) // not a self-assignment
107- {
108- const_cast <double &>(m_q0) = quaternion.m_q0 ;
109- const_cast <double &>(m_q1) = quaternion.m_q1 ;
110- const_cast <double &>(m_q2) = quaternion.m_q2 ;
111- const_cast <double &>(m_q3) = quaternion.m_q3 ;
112- }
113- return *this ;
109+ if (this != &quaternion) // not a self-assignment
110+ {
111+ const_cast <double &>(m_q0) = quaternion.m_q0 ;
112+ const_cast <double &>(m_q1) = quaternion.m_q1 ;
113+ const_cast <double &>(m_q2) = quaternion.m_q2 ;
114+ const_cast <double &>(m_q3) = quaternion.m_q3 ;
115+ }
116+ return *this ;
114117}
0 commit comments