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/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/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
find /opt/quatkin -type f -not -path '*/build/*' -not -path '*/.git/*' | head -50
/opt/quatkin/CMakeLists.txt /opt/quatkin/examples/spin_z.rotation /opt/quatkin/include/quatkin/io.hpp /opt/quatkin/include/quatkin/solver.hpp /opt/quatkin/include/quatkin/types.hpp /opt/quatkin/src/io.cpp /opt/quatkin/src/main.cpp /opt/quatkin/src/solver.cpp /opt/quatkin/tests/CMakeLists.txt /opt/quatkin/tests/test_smoke.cpp /opt/quatkin/tests/test_util.hpp [stdout] /opt/quatkin/CMakeLists.txt /opt/quatkin/examples/spin_z.rotation /opt/quatkin/include/quatkin/io.hpp /opt/quatkin/include/quatkin/solver.hpp /opt/quatkin/include/quatkin/types.hpp /opt/quatkin/src/io.cpp /opt/quatkin/src/main.cpp /opt/quatkin/src/solver.cpp /opt/quatkin/tests/CMakeLists.txt /opt/quatkin/tests/test_smoke.cpp /opt/quatkin/tests/test_util.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/src/io.cpp
1 #include "quatkin/io.hpp"
2
3 #include <cmath>
4 #include <fstream>
5 #include <iomanip>
6 #include <optional>
7 #include <sstream>
8 #include <stdexcept>
9 #include <string>
10
11 namespace quatkin {
12
13 namespace {
14
15 [[noreturn]] void fail(std::size_t line_no, const std::string& msg) {
16 std::ostringstream oss;
17 oss << "parse_scenario: line " << line_no << ": " << msg;
18 throw std::runtime_error(oss.str());
19 }
20
21 } // namespace
22
23 Scenario parse_scenario(std::istream& in) {
24 Quat q0{1.0, 0.0, 0.0, 0.0};
25 std::optional<Vec3> omega;
26 std::optional<double> dt;
27 std::optional<long long> steps;
28
29 std::string line;
30 std::size_t line_no = 0;
31 while (std::getline(in, line)) {
32 ++line_no;
33 const auto hash = line.find('#');
34 if (hash != std::string::npos) line.erase(hash);
35 std::istringstream ls(line);
36 std::string tag;
37 if (!(ls >> tag)) continue;
38
39 if (tag == "q0") {
40 Quat q;
41 if (!(ls >> q.w >> q.x >> q.y >> q.z))
42 fail(line_no, "record 'q0' needs w x y z");
43 q0 = q;
44 } else if (tag == "omega") {
45 Vec3 w;
46 if (!(ls >> w.x >> w.y >> w.z))
47 fail(line_no, "record 'omega' needs wx wy wz");
48 omega = w;
49 } else if (tag == "dt") {
50 double v;
51 if (!(ls >> v)) fail(line_no, "record 'dt' needs a value");
52 dt = v;
53 } else if (tag == "steps") {
54 long long v;
55 if (!(ls >> v)) fail(line_no, "record 'steps' needs an integer");
56 steps = v;
57 } else {
58 fail(line_no, "unknown record '" + tag + "'");
59 }
60 }
61
62 if (!omega) fail(line_no, "missing required record: omega");
63 if (!dt) fail(line_no, "missing required record: dt");
64 if (!steps) fail(line_no, "missing required record: steps");
65 if (*steps <= 0) fail(line_no, "steps must be positive");
66
67 Scenario sc{q0, *omega, *dt, static_cast<std::size_t>(*steps)};
68 return sc;
69 }
70
71 Scenario parse_scenario_file(const std::string& path) {
72 std::ifstream in(path);
73 if (!in) {
74 throw std::runtime_error("parse_scenario_file: cannot open '" + path +
75 "'");
76 }
77 return parse_scenario(in);
78 }
79
80 void write_report(std::ostream& out, const Scenario& scenario) {
81 const Quat qf = Kinematics::integrate(scenario.q0, scenario.omega,
82 scenario.dt, scenario.steps);
83 // Recovered rotation angle theta from the scalar part of the unit
84 // quaternion: w = cos(theta/2).
85 const double w_clamped = std::fmax(-1.0, std::fmin(1.0, qf.w));
86 const double angle = 2.0 * std::acos(w_clamped);
87
88 out << std::scientific << std::setprecision(6);
89 out << "# quaternion attitude-propagation report\n";
90 out << "omega=(" << scenario.omega.x << ", " << scenario.omega.y << ", "
91 << scenario.omega.z << ") t_final=" << scenario.dt * scenario.steps
92 << "\n";
93 out << "q_final=(" << qf.w << ", " << qf.x << ", " << qf.y << ", " << qf.z
94 << ")\n";
95 out << "norm=" << norm(qf) << " angle_rad=" << angle << "\n";
96 }
97
98 } // namespace quatkin
99
/opt/quatkin/src/main.cpp
1 #include <cstdlib>
2 #include <iostream>
3
4 #include "quatkin/io.hpp"
5 #include "quatkin/solver.hpp"
6 #include "quatkin/types.hpp"
7
8 // Command-line demo driver: read a .rotation scenario, propagate the attitude,
9 // and print the final quaternion and recovered rotation angle.
10 // quatkin_cli <scenario.rotation>
11 int main(int argc, char** argv) {
12 if (argc != 2) {
13 std::cerr << "usage: " << argv[0] << " <scenario.rotation>\n";
14 return 2;
15 }
16 try {
17 quatkin::Scenario scenario = quatkin::parse_scenario_file(argv[1]);
18 quatkin::write_report(std::cout, scenario);
19 } catch (const std::exception& ex) {
20 std::cerr << "error: " << ex.what() << "\n";
21 return 1;
22 }
23 return 0;
24 }
25
/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 {
// Implements Hamilton quaternion algebra with the {w, x, y, z} (scalar-first)
// convention used throughout the engine. Unit quaternions encode active
// rotations of body-frame vectors into the world frame via the sandwich
// product v' = q (0, v) q*, and the scalar part satisfies w = cos(theta/2).
// Attitude kinematics use the body-rate convention q_dot = 0.5 q (0, omega).
namespace {
// Component-wise quaternion addition and scaling, used to combine RK4 slopes.
Quat add(const Quat& a, const Quat& b) {
return Quat{a.w + b.w, a.x + b.x, a.y + b.y, a.z + b.z};
}
Quat scale(const Quat& q, double s) {
return Quat{s * q.w, s * q.x, s * q.y, s * q.z};
}
} // namespace
Quat Kinematics::multiply(const Quat& a, const Quat& 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 on the unit quaternion.
const Quat u = normalize(q);
const Quat p{0.0, v.x, v.y, v.z};
const Quat r = multiply(multiply(u, p), conjugate(u));
return Vec3{r.x, r.y, r.z};
}
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 uses the conjugate: v' = q* (0, v) q.
const Quat u = normalize(q);
const Quat p{0.0, v.x, v.y, v.z};
const Quat r = multiply(multiply(conjugate(u), p), u);
return Vec3{r.x, r.y, r.z};
}
Quat Kinematics::from_matrix(const double R[9]) {
if (R == nullptr) {
throw std::runtime_error("from_matrix: null matrix");
}
// Row-major rotation matrix entries.
const double r00 = R[0], r01 = R[1], r02 = R[2];
const double r10 = R[3], r11 = R[4], r12 = R[5];
const double r20 = R[6], r21 = R[7], r22 = R[8];
const double trace = r00 + r11 + r22;
Quat q;
// Shepperd's method: pick the largest divisor for numerical stability.
if (trace > 0.0) {
double s = std::sqrt(trace + 1.0) * 2.0; // s = 4*w
q.w = 0.25 * s;
q.x = (r21 - r12) / s;
q.y = (r02 - r20) / s;
q.z = (r10 - r01) / s;
} else if (r00 > r11 && r00 > r22) {
double s = std::sqrt(1.0 + r00 - r11 - r22) * 2.0; // s = 4*x
q.w = (r21 - r12) / s;
q.x = 0.25 * s;
q.y = (r01 + r10) / s;
q.z = (r02 + r20) / s;
} else if (r11 > r22) {
double s = std::sqrt(1.0 + r11 - r00 - r22) * 2.0; // s = 4*y
q.w = (r02 - r20) / s;
q.x = (r01 + r10) / s;
q.y = 0.25 * s;
q.z = (r12 + r21) / s;
} else {
double s = std::sqrt(1.0 + r22 - r00 - r11) * 2.0; // s = 4*z
q.w = (r10 - r01) / s;
q.x = (r02 + r20) / s;
q.y = (r12 + r21) / 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 orientation, so use |dot| for the shortest arc.
double d = std::fabs(ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z);
d = std::fmin(1.0, std::fmax(-1.0, d));
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");
}
const Quat ua = normalize(a);
Quat ub = normalize(b);
double cos_theta = ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z;
// Take the shortest path on the double cover.
if (cos_theta < 0.0) {
ub = scale(ub, -1.0);
cos_theta = -cos_theta;
}
cos_theta = std::fmin(1.0, cos_theta);
// Nearly parallel: fall back to normalized linear interpolation.
if (cos_theta > 1.0 - 1e-12) {
return normalize(add(scale(ua, 1.0 - t), scale(ub, t)));
}
const double theta = std::acos(cos_theta);
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(add(scale(ua, wa), scale(ub, wb)));
}
Quat Kinematics::derivative(const Quat& q, const Vec3& omega) {
// Body-rate kinematics: q_dot = 0.5 * q * (0, omega).
const Quat w{0.0, omega.x, omega.y, omega.z};
return scale(multiply(q, w), 0.5);
}
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");
}
// Classical RK4 on q_dot = derivative(q, omega) with constant omega.
const Quat k1 = derivative(q, omega);
const Quat k2 = derivative(add(q, scale(k1, dt * 0.5)), omega);
const Quat k3 = derivative(add(q, scale(k2, dt * 0.5)), omega);
const Quat k4 = derivative(add(q, scale(k3, dt)), omega);
const Quat incr =
scale(add(add(k1, scale(k2, 2.0)), add(scale(k3, 2.0), k4)), dt / 6.0);
return normalize(add(q, incr));
}
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 about principal axes:
// I omega_dot = torque - omega x (I omega).
const Vec3 Iw{inertia.x * omega.x, inertia.y * omega.y,
inertia.z * omega.z};
const Vec3 gyro = cross(omega, Iw);
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");
}
// Coupled RK4 over the attitude/rate state (q, omega): the attitude obeys
// q_dot = 0.5 q (0, omega) while omega obeys Euler's equations.
Quat q = normalize(q0);
Vec3 w = omega0;
for (std::size_t i = 0; i < steps; ++i) {
const Quat kq1 = derivative(q, w);
const Vec3 kw1 = body_angular_accel(inertia, w, torque);
const Quat q2 = add(q, scale(kq1, dt * 0.5));
const Vec3 w2 = w + (dt * 0.5) * kw1;
const Quat kq2 = derivative(q2, w2);
const Vec3 kw2 = body_angular_accel(inertia, w2, torque);
const Quat q3 = add(q, scale(kq2, dt * 0.5));
const Vec3 w3 = w + (dt * 0.5) * kw2;
const Quat kq3 = derivative(q3, w3);
const Vec3 kw3 = body_angular_accel(inertia, w3, torque);
const Quat q4 = add(q, scale(kq3, dt));
const Vec3 w4 = w + dt * kw3;
const Quat kq4 = derivative(q4, w4);
const Vec3 kw4 = body_angular_accel(inertia, w4, torque);
const Quat dq = scale(
add(add(kq1, scale(kq2, 2.0)), add(scale(kq3, 2.0), kq4)),
dt / 6.0);
q = normalize(add(q, dq));
w = w + (dt / 6.0) * (kw1 + 2.0 * kw2 + 2.0 * kw3 + kw4);
}
if (omega_final != nullptr) *omega_final = w;
return q;
}
} // namespace quatkincmake -S /opt/quatkin -B /opt/quatkin/build -DCMAKE_BUILD_TYPE=Release >/dev/null 2>&1 && cmake --build /opt/quatkin/build --parallel 2>&1 | tail -5 && cd /opt/quatkin/build && ctest --output-on-failure
[ 71%] Building CXX object CMakeFiles/quatkin_cli.dir/src/main.cpp.o
[ 85%] Linking CXX executable quatkin_cli
[ 85%] Built target quatkin_cli
[100%] Linking CXX executable test_smoke
[100%] Built target test_smoke
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]
[ 71%] Building CXX object CMakeFiles/quatkin_cli.dir/src/main.cpp.o
[ 85%] Linking CXX executable quatkin_cli
[ 85%] Built target quatkin_cli
[100%] Linking CXX executable test_smoke
[100%] Built target test_smoke
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.9M1cLaS9ma/quatkin/build_grade
== Building ==
[ 9%] 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_algebra.dir/test_algebra.cpp.o
[ 28%] Building CXX object tests/CMakeFiles/test_kinematics_hidden.dir/test_kinematics_hidden.cpp.o
[ 33%] Building CXX object tests/CMakeFiles/test_rotation.dir/test_rotation.cpp.o
[ 38%] Building CXX object tests/CMakeFiles/test_matrix.dir/test_matrix.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_kinematics_hidden
[ 71%] Linking CXX executable test_euler
[ 76%] Linking CXX executable test_edge
[ 80%] Linking CXX executable test_rotation
[ 85%] Linking CXX executable test_matrix
[ 85%] Built target test_kinematics_hidden
[ 90%] Linking CXX executable test_algebra
[ 90%] Built target test_rotation
[ 90%] Built target test_euler
[ 95%] Linking CXX executable test_slerp
[ 95%] Built target test_edge
[ 95%] Built target test_matrix
[ 95%] Built target test_algebra
[ 95%] Built target test_slerp
[100%] Linking CXX executable test_dynamics
[100%] Built target test_dynamics
== Running hidden test suite ==
Test project /tmp/tmp.9M1cLaS9ma/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_3e5f8c7d029f4b8e. Re-running the agent live requires EVAL_PLATFORM_ENABLE_OAUTH_SMOKE=1 and is non-deterministic.
Trial trial_3e5f8c7d029f4b8e · verifier authoritative; classifier explanatory.