tests/test.sh exit code (0 → resolved); the classification below is post-hoc and cannot change it./opt/quatkin/include/quatkin/solver.hpp
1 #ifndef QUATKIN_SOLVER_HPP
2 #define QUATKIN_SOLVER_HPP
3
4 #include <cstddef>
5
6 #include "quatkin/types.hpp"
7
8 namespace quatkin {
9
10
11
12
13
14
15 class Kinematics {
16 public:
17
18
19 static Quat multiply(const Quat& a, const Quat& b);
20
21
22
23 static Quat normalize(const Quat& q);
24
25
26
27 static Vec3 rotate(const Quat& q, const Vec3& v);
28
29
30
31 static Vec3 rotate_inv(const Quat& q, const Vec3& v);
32
33
34
35
36 static Quat from_matrix(const double R[9]);
37
38
39
40
41 static double geodesic_angle(const Quat& a, const Quat& b);
42
43
44
45 static Quat slerp(const Quat& a, const Quat& b, double t);
46
47
48
49 static Quat derivative(const Quat& q, const Vec3& omega);
50
51
52
53
54 static Quat rk4_step(const Quat& q, const Vec3& omega, double dt);
55
56
57
58
59 static Quat integrate(const Quat& q0, const Vec3& omega, double dt,
60 std::size_t steps);
61
62
63
64
65 static Vec3 body_angular_accel(const Vec3& inertia, const Vec3& omega,
66 const Vec3& torque);
67
68
69
70
71
72
73 static Quat integrate_dynamics(const Quat& q0, const Vec3& omega0,
74 const Vec3& inertia, const Vec3& torque,
75 double dt, std::size_t steps,
76 Vec3* omega_final);
77 };
78
79 }
80
81 #endif
82
/opt/quatkin/src/solver.cpp
1 #include "quatkin/solver.hpp"
2
3 #include <cmath>
4 #include <cstddef>
5 #include <stdexcept>
6
7 #include "quatkin/types.hpp"
8
9 namespace quatkin {
10
11 // TODO(candidate): implement the quaternion algebra, active/inverse rotations,
12 // matrix reconstruction, geodesic distance, SLERP, constant-rate attitude RK4,
13 // body-frame Euler angular acceleration, and coupled attitude/rate dynamics.
14 // Keep the public signatures fixed and use the support types in types.hpp.
15
16 Quat Kinematics::multiply(const Quat& a, const Quat& b) {
17 (void)a;
18 (void)b;
19 return Quat{1.0, 0.0, 0.0, 0.0};
20 }
21
22 Quat Kinematics::normalize(const Quat& q) {
23 const double n = norm(q);
24 if (!(n > 0.0)) {
25 throw std::runtime_error("normalize: zero quaternion has no direction");
26 }
27 return q;
28 }
29
30 Vec3 Kinematics::rotate(const Quat& q, const Vec3& v) {
31 if (!(norm(q) > 0.0)) {
32 throw std::runtime_error("rotate: zero quaternion has no direction");
33 }
34 (void)q;
35 return v;
36 }
37
38 Vec3 Kinematics::rotate_inv(const Quat& q, const Vec3& v) {
39 if (!(norm(q) > 0.0)) {
40 throw std::runtime_error("rotate_inv: zero quaternion has no direction");
41 }
42 (void)q;
43 return v;
44 }
45
46 Quat Kinematics::from_matrix(const double R[9]) {
47 if (R == nullptr) {
48 throw std::runtime_error("from_matrix: null matrix");
49 }
50 (void)R;
51 return Quat{1.0, 0.0, 0.0, 0.0};
52 }
53
54 double Kinematics::geodesic_angle(const Quat& a, const Quat& b) {
55 if (!(norm(a) > 0.0) || !(norm(b) > 0.0)) {
56 throw std::runtime_error("geodesic_angle: zero quaternion input");
57 }
58 (void)a;
59 (void)b;
60 return 0.0;
61 }
62
63 Quat Kinematics::slerp(const Quat& a, const Quat& b, double t) {
64 if (!(norm(a) > 0.0) || !(norm(b) > 0.0)) {
65 throw std::runtime_error("slerp: zero quaternion input");
66 }
67 (void)b;
68 (void)t;
69 return a;
70 }
71
72 Quat Kinematics::derivative(const Quat& q, const Vec3& omega) {
73 (void)q;
74 (void)omega;
75 return Quat{0.0, 0.0, 0.0, 0.0};
76 }
77
78 Quat Kinematics::rk4_step(const Quat& q, const Vec3& omega, double dt) {
79 if (!(dt > 0.0)) {
80 throw std::invalid_argument("rk4_step: dt must be positive");
81 }
82 (void)omega;
83 return q;
84 }
85
86 Quat Kinematics::integrate(const Quat& q0, const Vec3& omega, double dt,
87 std::size_t steps) {
88 if (!(dt > 0.0)) {
89 throw std::invalid_argument("integrate: dt must be positive");
90 }
91 if (steps == 0) {
92 throw std::invalid_argument("integrate: steps must be positive");
93 }
94 (void)omega;
95 return q0;
96 }
97
98 Vec3 Kinematics::body_angular_accel(const Vec3& inertia, const Vec3& omega,
99 const Vec3& torque) {
100 if (!(inertia.x > 0.0) || !(inertia.y > 0.0) || !(inertia.z > 0.0)) {
101 throw std::invalid_argument(
102 "body_angular_accel: inertia components must be positive");
103 }
104 (void)omega;
105 (void)torque;
106 return Vec3{0.0, 0.0, 0.0};
107 }
108
109 Quat Kinematics::integrate_dynamics(const Quat& q0, const Vec3& omega0,
110 const Vec3& inertia, const Vec3& torque,
111 double dt, std::size_t steps,
112 Vec3* omega_final) {
113 if (!(dt > 0.0)) {
114 throw std::invalid_argument("integrate_dynamics: dt must be positive");
115 }
116 if (steps == 0) {
117 throw std::invalid_argument(
118 "integrate_dynamics: steps must be positive");
119 }
120 if (!(inertia.x > 0.0) || !(inertia.y > 0.0) || !(inertia.z > 0.0)) {
121 throw std::invalid_argument(
122 "integrate_dynamics: inertia components must be positive");
123 }
124 if (!(norm(q0) > 0.0)) {
125 throw std::runtime_error(
126 "integrate_dynamics: zero quaternion has no direction");
127 }
128 (void)torque;
129 if (omega_final != nullptr) *omega_final = omega0;
130 return q0;
131 }
132
133 } // namespace quatkin
134
/opt/quatkin/include/quatkin/types.hpp
1 #ifndef QUATKIN_TYPES_HPP
2 #define QUATKIN_TYPES_HPP
3
4 #include <cmath>
5
6 namespace quatkin {
7
8
9
10 struct Vec3 {
11 double x{0.0};
12 double y{0.0};
13 double z{0.0};
14 };
15
16 inline Vec3 operator+(const Vec3& a, const Vec3& b) {
17 return Vec3{a.x + b.x, a.y + b.y, a.z + b.z};
18 }
19 inline Vec3 operator-(const Vec3& a, const Vec3& b) {
20 return Vec3{a.x - b.x, a.y - b.y, a.z - b.z};
21 }
22 inline Vec3 operator*(double s, const Vec3& a) {
23 return Vec3{s * a.x, s * a.y, s * a.z};
24 }
25 inline Vec3 operator*(const Vec3& a, double s) { return s * a; }
26
27 inline double dot(const Vec3& a, const Vec3& b) {
28 return a.x * b.x + a.y * b.y + a.z * b.z;
29 }
30 inline Vec3 cross(const Vec3& a, const Vec3& b) {
31 return Vec3{a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z,
32 a.x * b.y - a.y * b.x};
33 }
34 inline double norm(const Vec3& a) { return std::sqrt(dot(a, a)); }
35
36
37
38
39
40
41 struct Quat {
42 double w{1.0};
43 double x{0.0};
44 double y{0.0};
45 double z{0.0};
46 };
47
48
49 inline double norm_sq(const Quat& q) {
50 return q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
51 }
52
53
54 inline double norm(const Quat& q) { return std::sqrt(norm_sq(q)); }
55
56
57
58 inline Quat conjugate(const Quat& q) {
59 return Quat{q.w, -q.x, -q.y, -q.z};
60 }
61
62 }
63
64 #endif
65
/opt/quatkin/**/*
CMakeLists.txt examples/spin_z.rotation src/io.cpp src/main.cpp tests/test_util.hpp src/solver.cpp tests/CMakeLists.txt tests/test_smoke.cpp include/quatkin/solver.hpp include/quatkin/io.hpp include/quatkin/types.hpp
/opt/quatkin/tests/test_smoke.cpp
1 #include <cmath>
2 #include <stdexcept>
3
4 #include "quatkin/solver.hpp"
5 #include "quatkin/types.hpp"
6 #include "test_util.hpp"
7
8 using namespace quatkin;
9
10 namespace {
11
12 bool finite(const Quat& q) {
13 return std::isfinite(q.w) && std::isfinite(q.x) && std::isfinite(q.y) &&
14 std::isfinite(q.z);
15 }
16
17 bool finite(const Vec3& v) {
18 return std::isfinite(v.x) && std::isfinite(v.y) && std::isfinite(v.z);
19 }
20
21 double vec_error(const Vec3& a, const Vec3& b) {
22 return norm(Vec3{a.x - b.x, a.y - b.y, a.z - b.z});
23 }
24
25 } // namespace
26
27 QUATKIN_TEST("attitude_smoke") {
28 const double half = std::sqrt(0.5);
29 const Quat qz{half, 0.0, 0.0, half};
30 const Vec3 x{1.0, 0.0, 0.0};
31 const Vec3 rx = Kinematics::rotate(qz, x);
32 quatkin_test::expect(finite(rx), "rotate returned finite vector");
33 quatkin_test::expect(rx.x < 1.0 - 1e-6,
34 "nontrivial rotation moves x off itself");
35
36 const Vec3 v{0.2, -0.7, 1.1};
37 const Vec3 round = Kinematics::rotate_inv(qz, Kinematics::rotate(qz, v));
38 quatkin_test::expect(vec_error(round, v) < 1e-6,
39 "inverse rotation round-trips a vector");
40
41 const Quat qi{1.0, 0.0, 0.0, 0.0};
42 const Quat qf = Kinematics::integrate(qi, Vec3{0.3, 0.4, -0.2}, 0.01, 20);
43 quatkin_test::expect(finite(qf), "integrate returned finite quaternion");
44 quatkin_test::expect(Kinematics::geodesic_angle(qi, qf) > 1e-6,
45 "nonzero rate changes attitude");
46
47 Vec3 wf{};
48 const Quat qd = Kinematics::integrate_dynamics(
49 qi, Vec3{0.2, 0.1, -0.3}, Vec3{1.0, 1.3, 1.8},
50 Vec3{0.0, 0.0, 0.0}, 0.01, 10, &wf);
51 quatkin_test::expect(finite(qd) && finite(wf),
52 "integrate_dynamics returned finite state");
53 quatkin_test::expect(std::fabs(norm(qd) - 1.0) < 1e-6,
54 "integrate_dynamics returns unit attitude");
55
56 const Quat mid = Kinematics::slerp(qi, qz, 0.5);
57 quatkin_test::expect(finite(mid), "slerp returned finite quaternion");
58 quatkin_test::expect(std::fabs(norm(mid) - 1.0) < 1e-6,
59 "slerp result is unit norm");
60
61 double R[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
62 const Quat qm = Kinematics::from_matrix(R);
63 quatkin_test::expect(finite(qm), "from_matrix returned finite quaternion");
64 }
65
66 QUATKIN_TEST("obvious_exceptions") {
67 bool norm_threw = false;
68 try {
69 (void)Kinematics::normalize(Quat{0.0, 0.0, 0.0, 0.0});
70 } catch (const std::runtime_error&) {
71 norm_threw = true;
72 }
73 quatkin_test::expect(norm_threw, "normalize zero throws runtime_error");
74
75 bool integrate_threw = false;
76 try {
77 (void)Kinematics::integrate(Quat{1.0, 0.0, 0.0, 0.0},
78 Vec3{1.0, 0.0, 0.0}, 0.0, 1);
79 } catch (const std::invalid_argument&) {
80 integrate_threw = true;
81 }
82 quatkin_test::expect(integrate_threw,
83 "integrate with nonpositive dt throws invalid_argument");
84 }
85
86 QUATKIN_TEST_MAIN()
87
/opt/quatkin/tests/test_util.hpp
1 #ifndef QUATKIN_TEST_UTIL_HPP
2 #define QUATKIN_TEST_UTIL_HPP
3
4 // Minimal dependency-free test harness for the quatkin suite.
5 //
6 // Each test executable defines test functions and registers them with
7 // QUATKIN_TEST(name) { ... }. main() (provided by QUATKIN_TEST_MAIN) runs every
8 // registered test and returns non-zero if any assertion fails, which CTest
9 // interprets as a failure.
10
11 #include <cmath>
12 #include <cstddef>
13 #include <functional>
14 #include <iostream>
15 #include <string>
16 #include <vector>
17
18 namespace quatkin_test {
19
20 struct Case {
21 std::string name;
22 std::function<void()> fn;
23 };
24
25 inline std::vector<Case>& registry() {
26 static std::vector<Case> cases;
27 return cases;
28 }
29
30 struct Registrar {
31 Registrar(const std::string& name, std::function<void()> fn) {
32 registry().push_back(Case{name, std::move(fn)});
33 }
34 };
35
36 // Thrown by an assertion failure to abort the current test case.
37 struct AssertionError {
38 std::string message;
39 };
40
41 inline void expect(bool cond, const std::string& what) {
42 if (!cond) {
43 throw AssertionError{what};
44 }
45 }
46
47 inline void expect_near(double got, double want, double tol, const std::string& what) {
48 if (std::fabs(got - want) > tol) {
49 throw AssertionError{what + " (got " + std::to_string(got) +
50 ", want " + std::to_string(want) +
51 ", tol " + std::to_string(tol) + ")"};
52 }
53 }
54
55 // Relative-error check (with an absolute floor) for quantities whose scale
56 // varies widely across cases.
57 inline void expect_rel(double got, double want, double rel_tol,
58 const std::string& what) {
59 const double denom = std::fabs(want) > 1e-300 ? std::fabs(want) : 1.0;
60 if (std::fabs(got - want) / denom > rel_tol) {
61 throw AssertionError{what + " (got " + std::to_string(got) +
62 ", want " + std::to_string(want) +
63 ", rel_tol " + std::to_string(rel_tol) + ")"};
64 }
65 }
66
67 inline int run_all() {
68 int failures = 0;
69 for (const auto& c : registry()) {
70 try {
71 c.fn();
72 std::cout << "[ PASS ] " << c.name << "\n";
73 } catch (const AssertionError& e) {
74 std::cout << "[ FAIL ] " << c.name << ": " << e.message << "\n";
75 ++failures;
76 } catch (const std::exception& e) {
77 std::cout << "[ FAIL ] " << c.name
78 << ": unexpected exception: " << e.what() << "\n";
79 ++failures;
80 } catch (...) {
81 std::cout << "[ FAIL ] " << c.name << ": unknown exception\n";
82 ++failures;
83 }
84 }
85 std::cout << "----\n"
86 << (registry().size() - failures) << "/" << registry().size()
87 << " tests passed\n";
88 return failures == 0 ? 0 : 1;
89 }
90
91 } // namespace quatkin_test
92
93 #define QUATKIN_CONCAT_INNER(a, b) a##b
94 #define QUATKIN_CONCAT(a, b) QUATKIN_CONCAT_INNER(a, b)
95
96 #define QUATKIN_TEST(NAME) \
97 static void QUATKIN_CONCAT(quatkin_test_fn_, __LINE__)(); \
98 static ::quatkin_test::Registrar QUATKIN_CONCAT(quatkin_test_reg_, \
99 __LINE__){ \
100 NAME, &QUATKIN_CONCAT(quatkin_test_fn_, __LINE__)}; \
101 static void QUATKIN_CONCAT(quatkin_test_fn_, __LINE__)()
102
103 #define QUATKIN_TEST_MAIN() \
104 int main() { return ::quatkin_test::run_all(); }
105
106 #endif // QUATKIN_TEST_UTIL_HPP
107
/opt/quatkin/tests/CMakeLists.txt
1 add_executable(test_smoke test_smoke.cpp)
2 target_link_libraries(test_smoke PRIVATE quatkin)
3 target_include_directories(test_smoke PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
4 add_test(NAME smoke COMMAND test_smoke)
5
/opt/quatkin/include/quatkin/io.hpp
1 #ifndef QUATKIN_IO_HPP
2 #define QUATKIN_IO_HPP
3
4 #include <cstddef>
5 #include <iosfwd>
6 #include <string>
7
8 #include "quatkin/solver.hpp"
9 #include "quatkin/types.hpp"
10
11 namespace quatkin {
12
13
14
15 struct Scenario {
16 Quat q0{1.0, 0.0, 0.0, 0.0};
17 Vec3 omega{};
18 double dt{0.0};
19 std::size_t steps{0};
20 };
21
22
23
24
25
26
27
28
29
30
31
32 Scenario parse_scenario(std::istream& in);
33
34
35 Scenario parse_scenario_file(const std::string& path);
36
37
38
39 void write_report(std::ostream& out, const Scenario& scenario);
40
41 }
42
43 #endif
44
/opt/quatkin/src/solver.cpp
namespace quatkin {
// TODO(candidate): implement the quaternion algebra, active/inverse rotations,
// matrix reconstruction, geodesic distance, SLERP, constant-rate attitude RK4,
// body-frame Euler angular acceleration, and coupled attitude/rate dynamics.
// Keep the public signatures fixed and use the support types in types.hpp.
Quat Kinematics::multiply(const Quat& a, const Quat& b) {
(void)a;
(void)b;
return Quat{1.0, 0.0, 0.0, 0.0};
}
Quat Kinematics::normalize(const Quat& q) {
const double n = norm(q);
if (!(n > 0.0)) {
throw std::runtime_error("normalize: zero quaternion has no direction");
}
return q;
}
Vec3 Kinematics::rotate(const Quat& q, const Vec3& v) {
if (!(norm(q) > 0.0)) {
throw std::runtime_error("rotate: zero quaternion has no direction");
}
(void)q;
return v;
}
Vec3 Kinematics::rotate_inv(const Quat& q, const Vec3& v) {
if (!(norm(q) > 0.0)) {
throw std::runtime_error("rotate_inv: zero quaternion has no direction");
}
(void)q;
return v;
}
Quat Kinematics::from_matrix(const double R[9]) {
if (R == nullptr) {
throw std::runtime_error("from_matrix: null matrix");
}
(void)R;
return Quat{1.0, 0.0, 0.0, 0.0};
}
double Kinematics::geodesic_angle(const Quat& a, const Quat& b) {
if (!(norm(a) > 0.0) || !(norm(b) > 0.0)) {
throw std::runtime_error("geodesic_angle: zero quaternion input");
}
(void)a;
(void)b;
return 0.0;
}
Quat Kinematics::slerp(const Quat& a, const Quat& b, double t) {
if (!(norm(a) > 0.0) || !(norm(b) > 0.0)) {
throw std::runtime_error("slerp: zero quaternion input");
}
(void)b;
(void)t;
return a;
}
Quat Kinematics::derivative(const Quat& q, const Vec3& omega) {
(void)q;
(void)omega;
return Quat{0.0, 0.0, 0.0, 0.0};
}
Quat Kinematics::rk4_step(const Quat& q, const Vec3& omega, double dt) {
if (!(dt > 0.0)) {
throw std::invalid_argument("rk4_step: dt must be positive");
}
(void)omega;
return q;
}
Quat Kinematics::integrate(const Quat& q0, const Vec3& omega, double dt,
std::size_t steps) {
if (!(dt > 0.0)) {
throw std::invalid_argument("integrate: dt must be positive");
}
if (steps == 0) {
throw std::invalid_argument("integrate: steps must be positive");
}
(void)omega;
return q0;
}
Vec3 Kinematics::body_angular_accel(const Vec3& inertia, const Vec3& omega,
const Vec3& torque) {
if (!(inertia.x > 0.0) || !(inertia.y > 0.0) || !(inertia.z > 0.0)) {
throw std::invalid_argument(
"body_angular_accel: inertia components must be positive");
}
(void)omega;
(void)torque;
return Vec3{0.0, 0.0, 0.0};
}
Quat Kinematics::integrate_dynamics(const Quat& q0, const Vec3& omega0,
const Vec3& inertia, const Vec3& torque,
double dt, std::size_t steps,
Vec3* omega_final) {
if (!(dt > 0.0)) {
throw std::invalid_argument("integrate_dynamics: dt must be positive");
}
if (steps == 0) {
throw std::invalid_argument(
"integrate_dynamics: steps must be positive");
}
if (!(inertia.x > 0.0) || !(inertia.y > 0.0) || !(inertia.z > 0.0)) {
throw std::invalid_argument(
"integrate_dynamics: inertia components must be positive");
}
if (!(norm(q0) > 0.0)) {
throw std::runtime_error(
"integrate_dynamics: zero quaternion has no direction");
}
(void)torque;
if (omega_final != nullptr) *omega_final = omega0;
return q0;
}
} // namespace quatkinnamespace quatkin {
namespace {
// Componentwise quaternion sum and scalar scaling, used to combine the RK4
// stage slopes. These intentionally operate on raw (unnormalized) quaternions
// since the derivative of a unit quaternion is not itself a unit quaternion.
inline Quat qadd(const Quat& a, const Quat& b) {
return Quat{a.w + b.w, a.x + b.x, a.y + b.y, a.z + b.z};
}
inline Quat qscale(double s, const Quat& q) {
return Quat{s * q.w, s * q.x, s * q.y, s * q.z};
}
// Pure-quaternion form of a body-frame angular rate, used by the kinematic
// derivative q_dot = 1/2 * q (x) (0, omega).
inline Quat pure(const Vec3& v) { return Quat{0.0, v.x, v.y, v.z}; }
} // namespace
Quat Kinematics::multiply(const Quat& a, const Quat& b) {
// Hamilton product a (x) b.
return Quat{
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w};
}
Quat Kinematics::normalize(const Quat& q) {
const double n = norm(q);
if (!(n > 0.0)) {
throw std::runtime_error("normalize: zero quaternion has no direction");
}
const double inv = 1.0 / n;
return Quat{q.w * inv, q.x * inv, q.y * inv, q.z * inv};
}
Vec3 Kinematics::rotate(const Quat& q, const Vec3& v) {
if (!(norm(q) > 0.0)) {
throw std::runtime_error("rotate: zero quaternion has no direction");
}
// Active rotation v' = q (0,v) q*, evaluated with the unit quaternion via
// t = 2 (u x v); v' = v + w t + u x t, where u = (x,y,z).
const Quat u = normalize(q);
const Vec3 uvec{u.x, u.y, u.z};
const Vec3 t = 2.0 * cross(uvec, v);
return v + u.w * t + cross(uvec, t);
}
Vec3 Kinematics::rotate_inv(const Quat& q, const Vec3& v) {
if (!(norm(q) > 0.0)) {
throw std::runtime_error("rotate_inv: zero quaternion has no direction");
}
// Inverse rotation is the active rotation by the conjugate.
return rotate(conjugate(normalize(q)), v);
}
Quat Kinematics::from_matrix(const double R[9]) {
if (R == nullptr) {
throw std::runtime_error("from_matrix: null matrix");
}
// Row-major rotation matrix: R[3*r + c]. Shepperd's method picks the
// largest divisor among the four candidate forms for numerical stability.
const double m00 = R[0], m01 = R[1], m02 = R[2];
const double m10 = R[3], m11 = R[4], m12 = R[5];
const double m20 = R[6], m21 = R[7], m22 = R[8];
const double trace = m00 + m11 + m22;
Quat q;
if (trace > 0.0) {
double s = std::sqrt(trace + 1.0) * 2.0; // s = 4*w
q.w = 0.25 * s;
q.x = (m21 - m12) / s;
q.y = (m02 - m20) / s;
q.z = (m10 - m01) / s;
} else if (m00 > m11 && m00 > m22) {
double s = std::sqrt(1.0 + m00 - m11 - m22) * 2.0; // s = 4*x
q.w = (m21 - m12) / s;
q.x = 0.25 * s;
q.y = (m01 + m10) / s;
q.z = (m02 + m20) / s;
} else if (m11 > m22) {
double s = std::sqrt(1.0 + m11 - m00 - m22) * 2.0; // s = 4*y
q.w = (m02 - m20) / s;
q.x = (m01 + m10) / s;
q.y = 0.25 * s;
q.z = (m12 + m21) / s;
} else {
double s = std::sqrt(1.0 + m22 - m00 - m11) * 2.0; // s = 4*z
q.w = (m10 - m01) / s;
q.x = (m02 + m20) / s;
q.y = (m12 + m21) / s;
q.z = 0.25 * s;
}
return normalize(q);
}
double Kinematics::geodesic_angle(const Quat& a, const Quat& b) {
if (!(norm(a) > 0.0) || !(norm(b) > 0.0)) {
throw std::runtime_error("geodesic_angle: zero quaternion input");
}
const Quat ua = normalize(a);
const Quat ub = normalize(b);
// q and -q encode the same attitude, so use |a.b| to take the shorter arc.
double d = std::fabs(ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z);
if (d > 1.0) d = 1.0; // guard against round-off above 1
return 2.0 * std::acos(d);
}
Quat Kinematics::slerp(const Quat& a, const Quat& b, double t) {
if (!(norm(a) > 0.0) || !(norm(b) > 0.0)) {
throw std::runtime_error("slerp: zero quaternion input");
}
Quat ua = normalize(a);
Quat ub = normalize(b);
double d = ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z;
// Choose the sign that interpolates along the shorter arc.
if (d < 0.0) {
ub = qscale(-1.0, ub);
d = -d;
}
if (d > 1.0) d = 1.0;
// For nearly-parallel inputs fall back to normalized linear interpolation
// to avoid dividing by sin(theta) ~ 0.
if (d > 1.0 - 1e-12) {
return normalize(qadd(qscale(1.0 - t, ua), qscale(t, ub)));
}
const double theta = std::acos(d);
const double sin_theta = std::sin(theta);
const double wa = std::sin((1.0 - t) * theta) / sin_theta;
const double wb = std::sin(t * theta) / sin_theta;
return normalize(qadd(qscale(wa, ua), qscale(wb, ub)));
}
Quat Kinematics::derivative(const Quat& q, const Vec3& omega) {
// Body-frame kinematics: q_dot = 1/2 * q (x) (0, omega).
return qscale(0.5, multiply(q, pure(omega)));
}
Quat Kinematics::rk4_step(const Quat& q, const Vec3& omega, double dt) {
if (!(dt > 0.0)) {
throw std::invalid_argument("rk4_step: dt must be positive");
}
// Classic RK4 on the attitude with a constant body rate. The slope depends
// only on the current quaternion, so the stages evaluate derivative() at
// displaced quaternion states.
const Quat k1 = derivative(q, omega);
const Quat k2 = derivative(qadd(q, qscale(0.5 * dt, k1)), omega);
const Quat k3 = derivative(qadd(q, qscale(0.5 * dt, k2)), omega);
const Quat k4 = derivative(qadd(q, qscale(dt, k3)), omega);
const Quat increment =
qscale(dt / 6.0, qadd(qadd(k1, qscale(2.0, k2)),
qadd(qscale(2.0, k3), k4)));
return normalize(qadd(q, increment));
}
Quat Kinematics::integrate(const Quat& q0, const Vec3& omega, double dt,
std::size_t steps) {
if (!(dt > 0.0)) {
throw std::invalid_argument("integrate: dt must be positive");
}
if (steps == 0) {
throw std::invalid_argument("integrate: steps must be positive");
}
Quat q = normalize(q0);
for (std::size_t i = 0; i < steps; ++i) {
q = rk4_step(q, omega, dt);
}
return q;
}
Vec3 Kinematics::body_angular_accel(const Vec3& inertia, const Vec3& omega,
const Vec3& torque) {
if (!(inertia.x > 0.0) || !(inertia.y > 0.0) || !(inertia.z > 0.0)) {
throw std::invalid_argument(
"body_angular_accel: inertia components must be positive");
}
// Euler's rigid-body equations with a diagonal inertia tensor:
// I omega_dot = torque - omega x (I omega).
const Vec3 I_omega{inertia.x * omega.x, inertia.y * omega.y,
inertia.z * omega.z};
const Vec3 gyro = cross(omega, I_omega);
return Vec3{(torque.x - gyro.x) / inertia.x,
(torque.y - gyro.y) / inertia.y,
(torque.z - gyro.z) / inertia.z};
}
Quat Kinematics::integrate_dynamics(const Quat& q0, const Vec3& omega0,
const Vec3& inertia, const Vec3& torque,
double dt, std::size_t steps,
Vec3* omega_final) {
if (!(dt > 0.0)) {
throw std::invalid_argument("integrate_dynamics: dt must be positive");
}
if (steps == 0) {
throw std::invalid_argument(
"integrate_dynamics: steps must be positive");
}
if (!(inertia.x > 0.0) || !(inertia.y > 0.0) || !(inertia.z > 0.0)) {
throw std::invalid_argument(
"integrate_dynamics: inertia components must be positive");
}
if (!(norm(q0) > 0.0)) {
throw std::runtime_error(
"integrate_dynamics: zero quaternion has no direction");
}
Quat q = normalize(q0);
Vec3 w = omega0;
// Coupled RK4 over the joint attitude/rate state (q, omega): the attitude
// slope depends on omega, and the rate slope follows Euler's equations.
for (std::size_t i = 0; i < steps; ++i) {
const Quat k1q = derivative(q, w);
const Vec3 k1w = body_angular_accel(inertia, w, torque);
const Quat q2 = qadd(q, qscale(0.5 * dt, k1q));
const Vec3 w2 = w + (0.5 * dt) * k1w;
const Quat k2q = derivative(q2, w2);
const Vec3 k2w = body_angular_accel(inertia, w2, torque);
const Quat q3 = qadd(q, qscale(0.5 * dt, k2q));
const Vec3 w3 = w + (0.5 * dt) * k2w;
const Quat k3q = derivative(q3, w3);
const Vec3 k3w = body_angular_accel(inertia, w3, torque);
const Quat q4 = qadd(q, qscale(dt, k3q));
const Vec3 w4 = w + dt * k3w;
const Quat k4q = derivative(q4, w4);
const Vec3 k4w = body_angular_accel(inertia, w4, torque);
const Quat dq =
qscale(dt / 6.0, qadd(qadd(k1q, qscale(2.0, k2q)),
qadd(qscale(2.0, k3q), k4q)));
q = normalize(qadd(q, dq));
w = w + (dt / 6.0) * (k1w + 2.0 * k2w + 2.0 * k3w + k4w);
}
if (omega_final != nullptr) *omega_final = w;
return q;
}
} // namespace quatkinConfigure and build the project
cmake -S /opt/quatkin -B /opt/quatkin/build -DCMAKE_BUILD_TYPE=Release 2>&1 | tail -3 && cmake --build /opt/quatkin/build --parallel 2>&1 | tail -15
-- Configuring done -- Generating done -- Build files have been written to: /opt/quatkin/build [ 14%] Building CXX object CMakeFiles/quatkin.dir/src/solver.cpp.o [ 28%] Building CXX object CMakeFiles/quatkin.dir/src/io.cpp.o [ 42%] Linking CXX static library libquatkin.a [ 42%] Built target quatkin [ 71%] Building CXX object CMakeFiles/quatkin_cli.dir/src/main.cpp.o [ 71%] Building CXX object tests/CMakeFiles/test_smoke.dir/test_smoke.cpp.o [ 85%] Linking CXX executable quatkin_cli [ 85%] Built target quatkin_cli [100%] Linking CXX executable test_smoke [100%] Built target test_smoke [stdout] -- Configuring done -- Generating done -- Build files have been written to: /opt/quatkin/build [ 14%] Building CXX object CMakeFiles/quatkin.dir/src/solver.cpp.o [ 28%] Building CXX object CMakeFiles/quatkin.dir/src/io.cpp.o [ 42%] Linking CXX static library libquatkin.a [ 42%] Built target quatkin [ 71%] Building CXX object CMakeFiles/quatkin_cli.dir/src/main.cpp.o [ 71%] Building CXX object tests/CMakeFiles/test_smoke.dir/test_smoke.cpp.o [ 85%] Linking CXX executable quatkin_cli [ 85%] Built target quatkin_cli [100%] Linking CXX executable test_smoke [100%] Built target test_smoke
Run the test suite
cd /opt/quatkin/build && ctest --output-on-failure 2>&1 | tail -20
Test project /opt/quatkin/build
Start 1: smoke
1/1 Test #1: smoke ............................ Passed 0.00 sec
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.01 sec
[stdout]
Test project /opt/quatkin/build
Start 1: smoke
1/1 Test #1: smoke ............................ Passed 0.00 sec
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.01 secNo file changes, the agent left the workspace untouched.
== Configuring (cmake) ==
-- The CXX compiler identification is GNU 11.4.0
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++ - skipped
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Configuring done
-- Generating done
-- Build files have been written to: /tmp/tmp.p7UM4orig2/quatkin/build_grade
== Building ==
[ 4%] Building CXX object CMakeFiles/quatkin.dir/src/solver.cpp.o
[ 9%] Building CXX object CMakeFiles/quatkin.dir/src/io.cpp.o
[ 14%] Linking CXX static library libquatkin.a
[ 14%] Built target quatkin
[ 19%] Building CXX object CMakeFiles/quatkin_cli.dir/src/main.cpp.o
[ 23%] Building CXX object tests/CMakeFiles/test_kinematics_hidden.dir/test_kinematics_hidden.cpp.o
[ 28%] Building CXX object tests/CMakeFiles/test_rotation.dir/test_rotation.cpp.o
[ 33%] Building CXX object tests/CMakeFiles/test_matrix.dir/test_matrix.cpp.o
[ 38%] Building CXX object tests/CMakeFiles/test_algebra.dir/test_algebra.cpp.o
[ 42%] Building CXX object tests/CMakeFiles/test_euler.dir/test_euler.cpp.o
[ 47%] Building CXX object tests/CMakeFiles/test_dynamics.dir/test_dynamics.cpp.o
[ 52%] Building CXX object tests/CMakeFiles/test_slerp.dir/test_slerp.cpp.o
[ 57%] Building CXX object tests/CMakeFiles/test_edge.dir/test_edge.cpp.o
[ 61%] Linking CXX executable quatkin_cli
[ 61%] Built target quatkin_cli
[ 66%] Linking CXX executable test_rotation
[ 71%] Linking CXX executable test_kinematics_hidden
[ 76%] Linking CXX executable test_euler
[ 80%] Linking CXX executable test_edge
[ 85%] Linking CXX executable test_algebra
[ 85%] Built target test_kinematics_hidden
[ 85%] Built target test_rotation
[ 85%] Built target test_euler
[ 90%] Linking CXX executable test_slerp
[ 90%] Built target test_edge
[ 95%] Linking CXX executable test_matrix
[ 95%] Built target test_algebra
[ 95%] Built target test_slerp
[ 95%] Built target test_matrix
[100%] Linking CXX executable test_dynamics
[100%] Built target test_dynamics
== Running hidden test suite ==
Test project /tmp/tmp.p7UM4orig2/quatkin/build_grade
Start 1: test_algebra
1/8 Test #1: test_algebra ..................... Passed 0.00 sec
Start 2: test_rotation
2/8 Test #2: test_rotation .................... Passed 0.00 sec
Start 3: test_matrix
3/8 Test #3: test_matrix ......................***Failed 0.00 sec
[ FAIL ] from_matrix_round_trip_random: from_matrix sign-pinned round trip w (got -0.035791, want 0.035791, tol 0.000000)
[ FAIL ] from_matrix_inverts_rodrigues: from_matrix inverts Rodrigues matrix
[ PASS ] from_matrix_180_degree_branches
[ FAIL ] from_matrix_trace_boundary_sweep: from_matrix across trace branch boundary
[ FAIL ] from_matrix_trace_near_minus_one_off_axis: near-180 off-axis matrix recovered across branch crossover
[ FAIL ] from_matrix_rejects_non_rotation: reflection matrix rejected
----
1/6 tests passed
Start 4: test_kinematics_hidden
4/8 Test #4: test_kinematics_hidden ........... Passed 0.00 sec
Start 5: test_euler
5/8 Test #5: test_euler ....................... Passed 0.00 sec
Start 6: test_dynamics
6/8 Test #6: test_dynamics .................... Passed 0.01 sec
Start 7: test_slerp
7/8 Test #7: test_slerp ....................... Passed 0.00 sec
Start 8: test_edge
8/8 Test #8: test_edge ........................***Failed 0.00 sec
[ PASS ] geodesic_angle_self_zero_and_double_cover
[ PASS ] geodesic_angle_range
[ FAIL ] exceptions_contract: from_matrix non-rotation runtime_error
----
2/3 tests passed
75% tests passed, 2 tests failed out of 8
Label Time Summary:
hidden = 0.03 sec*proc (8 tests)
Total Test time (real) = 0.03 sec
The following tests FAILED:
3 - test_matrix (Failed)
8 - test_edge (Failed)
Errors while running CTest
FAIL: one or more hidden tests failedReproduce this trial: git checkout 2f94510 && PYTHONPATH=src python3 scripts/build_site.py , then open trial/trial_27720bdd72dc4b35. Re-running the agent live requires EVAL_PLATFORM_ENABLE_OAUTH_SMOKE=1 and is non-deterministic.
Trial trial_27720bdd72dc4b35 · verifier authoritative; classifier explanatory.