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
List project files
find /opt/quatkin -type f -not -path '*/build/*' | 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/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/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
#include "quatkin/solver.hpp"
#include <cmath>
#include <cstddef>
#include <stdexcept>
#include "quatkin/types.hpp"
namespace quatkin {
namespace {
// Quaternion vector-space helpers (the public API keeps Quat free of operators).
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};
}
Quat qscale(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) {
// Hamilton product, scalar-first convention.
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* using a unit quaternion.
const Quat u = normalize(q);
const Quat vq{0.0, v.x, v.y, v.z};
const Quat r = multiply(multiply(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");
}
// Inverse (passive) rotation v' = q* (0,v) q.
const Quat u = normalize(q);
const Quat vq{0.0, v.x, v.y, v.z};
const Quat r = multiply(multiply(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");
}
// R is row-major: R[3*row + col]. Shepperd's method for numerical safety.
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);
double d = ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z;
// q and -q are the same orientation; take the shorter arc.
d = std::fabs(d);
if (d > 1.0) d = 1.0;
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 cos_theta = ua.w * ub.w + ua.x * ub.x + ua.y * ub.y + ua.z * ub.z;
// Choose the shorter arc by flipping one endpoint if needed.
if (cos_theta < 0.0) {
ub = qscale(ub, -1.0);
cos_theta = -cos_theta;
}
if (cos_theta > 1.0) cos_theta = 1.0;
// Nearly colinear: fall back to normalized linear interpolation.
if (cos_theta > 1.0 - 1e-12) {
return normalize(qadd(qscale(ua, 1.0 - t), qscale(ub, t)));
}
const double theta = std::acos(cos_theta);
const double sin_theta = std::sin(theta);
const double sa = std::sin((1.0 - t) * theta) / sin_theta;
const double sb = std::sin(t * theta) / sin_theta;
return normalize(qadd(qscale(ua, sa), qscale(ub, sb)));
}
Quat Kinematics::derivative(const Quat& q, const Vec3& omega) {
// Body-frame rate kinematics: q_dot = 0.5 * q * (0, omega).
const Quat omega_q{0.0, omega.x, omega.y, omega.z};
return qscale(multiply(q, omega_q), 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");
}
// Classic RK4 with constant angular velocity over the step.
const Quat k1 = derivative(q, omega);
const Quat k2 = derivative(qadd(q, qscale(k1, 0.5 * dt)), omega);
const Quat k3 = derivative(qadd(q, qscale(k2, 0.5 * dt)), omega);
const Quat k4 = derivative(qadd(q, qscale(k3, dt)), omega);
Quat sum = qadd(qadd(k1, qscale(k2, 2.0)), qadd(qscale(k3, 2.0), k4));
return normalize(qadd(q, qscale(sum, dt / 6.0)));
}
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 equation: I*w_dot = tau - w x (I*w).
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");
}
Quat q = normalize(q0);
Vec3 w = omega0;
// Coupled RK4 over the joint state (attitude, body rate).
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 = qadd(q, qscale(kq1, 0.5 * dt));
const Vec3 w2 = w + (0.5 * dt) * kw1;
const Quat kq2 = derivative(q2, w2);
const Vec3 kw2 = body_angular_accel(inertia, w2, torque);
const Quat q3 = qadd(q, qscale(kq2, 0.5 * dt));
const Vec3 w3 = w + (0.5 * dt) * kw2;
const Quat kq3 = derivative(q3, w3);
const Vec3 kw3 = body_angular_accel(inertia, w3, torque);
const Quat q4 = qadd(q, qscale(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 qsum =
qadd(qadd(kq1, qscale(kq2, 2.0)), qadd(qscale(kq3, 2.0), kq4));
q = normalize(qadd(q, qscale(qsum, dt / 6.0)));
w = w + (dt / 6.0) * (kw1 + 2.0 * kw2 + 2.0 * kw3 + kw4);
}
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 2>&1 | tail -20
[ 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.01 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.01 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.Inr96WTNwg/quatkin/build_grade
== Building ==
[ 4%] Building CXX object CMakeFiles/quatkin.dir/src/io.cpp.o
[ 9%] Building CXX object CMakeFiles/quatkin.dir/src/solver.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_matrix.dir/test_matrix.cpp.o
[ 38%] Building CXX object tests/CMakeFiles/test_euler.dir/test_euler.cpp.o
[ 42%] Building CXX object tests/CMakeFiles/test_rotation.dir/test_rotation.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_algebra
[ 71%] Linking CXX executable test_edge
[ 76%] Linking CXX executable test_kinematics_hidden
[ 80%] Linking CXX executable test_slerp
[ 85%] Linking CXX executable test_euler
[ 90%] Linking CXX executable test_rotation
[ 90%] Built target test_kinematics_hidden
[ 90%] Built target test_slerp
[ 95%] Linking CXX executable test_matrix
[ 95%] Built target test_edge
[ 95%] Built target test_algebra
[ 95%] Built target test_euler
[ 95%] Built target test_rotation
[ 95%] Built target test_matrix
[100%] Linking CXX executable test_dynamics
[100%] Built target test_dynamics
== Running hidden test suite ==
Test project /tmp/tmp.Inr96WTNwg/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_b2ae917fc4464d12. Re-running the agent live requires EVAL_PLATFORM_ENABLE_OAUTH_SMOKE=1 and is non-deterministic.
Trial trial_b2ae917fc4464d12 · verifier authoritative; classifier explanatory.