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
List project files
find /opt/quatkin -type f -not -path '*/build/*' -not -path '*/.git/*' | sort
/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/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/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
#include "quatkin/solver.hpp"
#include <cmath>
#include <cstddef>
#include <stdexcept>
#include "quatkin/types.hpp"
namespace quatkin {
// Internal helpers ----------------------------------------------------------
namespace {
// Hamilton product of two quaternions (scalar-first convention).
inline Quat quat_mul(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};
}
inline Quat 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};
}
inline Quat quat_scale(const Quat& q, double s) {
return Quat{s * q.w, s * q.x, s * q.y, s * q.z};
}
inline Quat quat_unit(const Quat& q) {
const double n = norm(q);
if (!(n > 0.0)) {
throw std::runtime_error("zero quaternion has no direction");
}
return quat_scale(q, 1.0 / n);
}
// Kinematic quaternion derivative for a body-frame angular velocity:
// q_dot = 0.5 * q * (0, omega)
inline Quat quat_kinematic(const Quat& q, const Vec3& omega) {
const Quat w{0.0, omega.x, omega.y, omega.z};
return quat_scale(quat_mul(q, w), 0.5);
}
// Euler's rigid-body equation solved for angular acceleration:
// omega_dot = I^-1 * (torque - omega x (I * omega))
inline Vec3 euler_accel(const Vec3& inertia, const Vec3& omega,
const Vec3& torque) {
const Vec3 Iw{inertia.x * omega.x, inertia.y * omega.y,
inertia.z * omega.z};
const Vec3 gyro = cross(omega, Iw);
const Vec3 net = torque - gyro;
return Vec3{net.x / inertia.x, net.y / inertia.y, net.z / inertia.z};
}
} // namespace
// Public API ----------------------------------------------------------------
Quat Kinematics::multiply(const Quat& a, const Quat& b) {
return quat_mul(a, b);
}
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 quat_scale(q, 1.0 / n);
}
Vec3 Kinematics::rotate(const Quat& q, const Vec3& v) {
if (!(norm(q) > 0.0)) {
throw std::runtime_error("rotate: zero quaternion has no direction");
}
const Quat u = quat_unit(q);
// v' = q * (0,v) * q^-1, with q^-1 = conjugate(q) for the unit quaternion.
const Quat vq{0.0, v.x, v.y, v.z};
const Quat r = quat_mul(quat_mul(u, vq), 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");
}
const Quat u = quat_unit(q);
// Inverse active rotation: v' = q^-1 * (0,v) * q.
const Quat vq{0.0, v.x, v.y, v.z};
const Quat r = quat_mul(quat_mul(conjugate(u), vq), 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 R[row*3 + col].
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;
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 quat_unit(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 = quat_unit(a);
const Quat ub = quat_unit(b);
// Relative rotation r = a^-1 * b; its half-angle gives the geodesic angle.
const Quat r = quat_mul(conjugate(ua), ub);
const double vec = std::sqrt(r.x * r.x + r.y * r.y + r.z * r.z);
// 2*atan2(|vec|, |w|) yields the shortest rotation angle in [0, pi].
return 2.0 * std::atan2(vec, std::fabs(r.w));
}
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 = quat_unit(a);
Quat ub = quat_unit(b);
double d = ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z;
// Take the shortest path around the hypersphere.
if (d < 0.0) {
ub = quat_scale(ub, -1.0);
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-9) {
const Quat lin = quat_add(quat_scale(ua, 1.0 - t), quat_scale(ub, t));
return quat_unit(lin);
}
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 quat_unit(quat_add(quat_scale(ua, wa), quat_scale(ub, wb)));
}
Quat Kinematics::derivative(const Quat& q, const Vec3& omega) {
return quat_kinematic(q, 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");
}
const Quat k1 = quat_kinematic(q, omega);
const Quat k2 = quat_kinematic(quat_add(q, quat_scale(k1, 0.5 * dt)), omega);
const Quat k3 = quat_kinematic(quat_add(q, quat_scale(k2, 0.5 * dt)), omega);
const Quat k4 = quat_kinematic(quat_add(q, quat_scale(k3, dt)), omega);
Quat sum = quat_add(quat_add(k1, quat_scale(k2, 2.0)),
quat_add(quat_scale(k3, 2.0), k4));
Quat next = quat_add(q, quat_scale(sum, dt / 6.0));
return quat_unit(next);
}
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 = quat_unit(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");
}
return euler_accel(inertia, omega, torque);
}
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 = quat_unit(q0);
Vec3 w = omega0;
for (std::size_t i = 0; i < steps; ++i) {
// Coupled RK4 over the attitude quaternion and the body rate.
const Quat kq1 = quat_kinematic(q, w);
const Vec3 kw1 = euler_accel(inertia, w, torque);
const Quat q2 = quat_add(q, quat_scale(kq1, 0.5 * dt));
const Vec3 w2 = w + (0.5 * dt) * kw1;
const Quat kq2 = quat_kinematic(q2, w2);
const Vec3 kw2 = euler_accel(inertia, w2, torque);
const Quat q3 = quat_add(q, quat_scale(kq2, 0.5 * dt));
const Vec3 w3 = w + (0.5 * dt) * kw2;
const Quat kq3 = quat_kinematic(q3, w3);
const Vec3 kw3 = euler_accel(inertia, w3, torque);
const Quat q4 = quat_add(q, quat_scale(kq3, dt));
const Vec3 w4 = w + dt * kw3;
const Quat kq4 = quat_kinematic(q4, w4);
const Vec3 kw4 = euler_accel(inertia, w4, torque);
const Quat qsum = quat_add(quat_add(kq1, quat_scale(kq2, 2.0)),
quat_add(quat_scale(kq3, 2.0), kq4));
q = quat_unit(quat_add(q, quat_scale(qsum, dt / 6.0)));
const Vec3 wsum = kw1 + 2.0 * kw2 + 2.0 * kw3 + kw4;
w = w + (dt / 6.0) * wsum;
}
if (omega_final != nullptr) *omega_final = w;
return q;
}
} // namespace quatkin
Configure, build, and run tests
cmake -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 secCorrectness sanity checks
cat > /tmp/check.cpp <<'EOF'
#include "quatkin/solver.hpp"
#include <cstdio>
#include <cmath>
using namespace quatkin;
int main(){
double h=std::sqrt(0.5);
Quat qz{h,0,0,h}; // 90 deg about z
Vec3 r=Kinematics::rotate(qz,Vec3{1,0,0});
printf("rotate x by qz: (%.4f,%.4f,%.4f) expect (0,1,0)\n",r.x,r.y,r.z);
// from_matrix round-trip: 90 about z matrix
double R[9]={0,-1,0, 1,0,0, 0,0,1};
Quat qm=Kinematics::from_matrix(R);
printf("from_matrix angle vs qz: %.6e\n", Kinematics::geodesic_angle(qm,qz));
// slerp midpoint of identity->qz should be 45 deg
Quat mid=Kinematics::slerp(Quat{1,0,0,0},qz,0.5);
printf("slerp half angle from id: %.6f expect %.6f\n",Kinematics::geodesic_angle(Quat{1,0,0,0},mid), M_PI/4);
// torque-free dynamics conserves energy roughly; constant rate integrate angle
Quat qf=Kinematics::integrate(Quat{1,0,0,0},Vec3{0,0,1.0},0.001,1000);
printf("integrate 1 rad/s for 1s, angle: %.6f expect 1.0\n",Kinematics::geodesic_angle(Quat{1,0,0,0},qf));
// dynamics: spin about z with symmetric-ish inertia, zero torque keeps |omega|
Vec3 wf{};
Kinematics::integrate_dynamics(Quat{1,0,0,0},Vec3{0.5,0,2.0},Vec3{2,2,1},Vec3{0,0,0},0.001,2000,&wf);
printf("torque-free |omega| start %.4f end %.4f\n", norm(Vec3{0.5,0,2.0}), norm(wf));
return 0;
}
EOF
g++ -std=c++17 -I/opt/quatkin/include /tmp/check.cpp /opt/quatkin/src/solver.cpp -o /tmp/check && /tmp/checkrotate x by qz: (0.0000,1.0000,0.0000) expect (0,1,0) from_matrix angle vs qz: 2.220446e-16 slerp half angle from id: 0.785398 expect 0.785398 integrate 1 rad/s for 1s, angle: 1.000000 expect 1.0 torque-free |omega| start 2.0616 end 2.0616 [stdout] rotate x by qz: (0.0000,1.0000,0.0000) expect (0,1,0) from_matrix angle vs qz: 2.220446e-16 slerp half angle from id: 0.785398 expect 0.785398 integrate 1 rad/s for 1s, angle: 1.000000 expect 1.0 torque-free |omega| start 2.0616 end 2.0616
No 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.tRQQjmBZDb/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_euler.dir/test_euler.cpp.o
[ 42%] Building CXX object tests/CMakeFiles/test_dynamics.dir/test_dynamics.cpp.o
[ 47%] Building CXX object tests/CMakeFiles/test_matrix.dir/test_matrix.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_euler
[ 76%] Linking CXX executable test_algebra
[ 80%] Linking CXX executable test_kinematics_hidden
[ 85%] Linking CXX executable test_slerp
[ 85%] Built target test_rotation
[ 90%] Linking CXX executable test_edge
[ 90%] Built target test_algebra
[ 90%] Built target test_slerp
[ 90%] Built target test_euler
[ 95%] Linking CXX executable test_matrix
[ 95%] Built target test_kinematics_hidden
[ 95%] Built target test_edge
[ 95%] Built target test_matrix
[100%] Linking CXX executable test_dynamics
[100%] Built target test_dynamics
== Running hidden test suite ==
Test project /tmp/tmp.tRQQjmBZDb/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.04 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_3c9d40dc811342de. Re-running the agent live requires EVAL_PLATFORM_ENABLE_OAUTH_SMOKE=1 and is non-deterministic.
Trial trial_3c9d40dc811342de · verifier authoritative; classifier explanatory.