aselimov/zigsdof
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Compare commits

base: aselimov:8ab8af992a92b0ef8f931a2e802bc55c36d91a5a
Branches Tags
aselimov:master
..
compare: aselimov:3a47ce938345df72cee2672191b843d2f03cca28
Branches Tags
aselimov:master

2 commits
8ab8af992a ... 3a47ce9383

Author SHA1 Message Date
Alex Selimov
3a47ce9383 Fix issue with drag calculation and update expected final value 2026-08-20 00:06:28 -04:00
Alex Selimov
6332e3d770 Fix bugs in determinant and inverse 2026-08-20 00:02:05 -04:00
3 changed files with 44 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/forces.zig
View file

@ -34,9 +34,9 @@ pub fn drag(
velocity: vec3.Vec3F64, velocity: vec3.Vec3F64,
) vec3.Vec3F64 { ) vec3.Vec3F64 {
return .init( return .init(
velocity.x() * drag_coefficient, -velocity.x() * drag_coefficient,
velocity.y() * drag_coefficient, -velocity.y() * drag_coefficient,
velocity.z() * drag_coefficient, -velocity.z() * drag_coefficient,
); );
} }
@ -73,7 +73,7 @@ test "drag is correct" {
const velocity = vec3.Vec3F64.init(0, 1, 1); const velocity = vec3.Vec3F64.init(0, 1, 1);
const f_drag = drag(0.5, velocity); const f_drag = drag(0.5, velocity);
try std.testing.expectApproxEqAbs(0, f_drag.x(), 1e-11); try std.testing.expectApproxEqAbs(-0.0, f_drag.x(), 1e-11);
try std.testing.expectApproxEqAbs(0.5, f_drag.y(), 1e-11); try std.testing.expectApproxEqAbs(-0.5, f_drag.y(), 1e-11);
try std.testing.expectApproxEqAbs(0.5, f_drag.z(), 1e-11); try std.testing.expectApproxEqAbs(-0.5, f_drag.z(), 1e-11);
} }

23
src/integration.zig
View file

@ -28,14 +28,14 @@ pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *dro
); );
s.omega = vec3.vec3Add(s.omega, vec3.vec3Scale(omega_dot, dt)); s.omega = vec3.vec3Add(s.omega, vec3.vec3Scale(omega_dot, dt));
const half_omege_quat: vec3.QuatF64 = .init( const half_omega_quat: vec3.QuatF64 = .init(
s.omega.x() * 0.5, s.omega.x() * 0.5,
s.omega.y() * 0.5, s.omega.y() * 0.5,
s.omega.z() * 0.5, s.omega.z() * 0.5,
0, 0,
); );
const qdot = vec3.quatMul(half_omege_quat, s.q); const qdot = vec3.quatMul(half_omega_quat, s.q);
s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)).normalized(); s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)).normalized();
return; return;
} }
@ -85,4 +85,23 @@ test "Validate timesteps" {
try std.testing.expectApproxEqAbs(0.12403234937465502, state.q.y(), 1e-12); try std.testing.expectApproxEqAbs(0.12403234937465502, state.q.y(), 1e-12);
try std.testing.expectApproxEqAbs(0.006201617468732751, state.q.z(), 1e-11); try std.testing.expectApproxEqAbs(0.006201617468732751, state.q.z(), 1e-11);
try std.testing.expectApproxEqAbs(0.9922587949972401, state.q.w(), 1e-11); try std.testing.expectApproxEqAbs(0.9922587949972401, state.q.w(), 1e-11);
timestep(0.005, d, &props, &state);
try std.testing.expectApproxEqAbs(-1.23072190e+01, state.v.x(), 1e-7);
try std.testing.expectApproxEqAbs(-7.69201185e-02, state.v.y(), 1e-8);
try std.testing.expectApproxEqAbs(-9.83635311e+01, state.v.z(), 1e-7);
try std.testing.expectApproxEqAbs(-6.15360950e-02, state.x_cg.x(), 1e-9);
try std.testing.expectApproxEqAbs(-3.84600592e-04, state.x_cg.y(), 1e-9);
try std.testing.expectApproxEqAbs(-7.41572489e-01, state.x_cg.z(), 1e-9);
try std.testing.expectApproxEqAbs(-0.46875, state.omega.x(), 1e-12);
try std.testing.expectApproxEqAbs(100, state.omega.y(), 1e-12);
try std.testing.expectApproxEqAbs(5, state.omega.z(), 1e-11);
try std.testing.expectApproxEqAbs(-0.0011280012096230429, state.q.x(), 1e-12);
try std.testing.expectApproxEqAbs(0.36096743708693396, state.q.y(), 1e-12);
try std.testing.expectApproxEqAbs(0.01790701920276581, state.q.z(), 1e-12);
try std.testing.expectApproxEqAbs(0.9324057998744074, state.q.w(), 1e-12);
} }

34
src/vec3.zig
View file

@ -90,7 +90,7 @@ pub fn mat3Mul(a: Mat3F64, b: Mat3F64) Mat3F64 {
pub fn mat3Det(a: Mat3F64) f64 { pub fn mat3Det(a: Mat3F64) f64 {
return a.row1.x() * (a.row2.y() * a.row3.z() - a.row2.z() * a.row3.y()) - return a.row1.x() * (a.row2.y() * a.row3.z() - a.row2.z() * a.row3.y()) -
a.row1.y() * (a.row2.x() * a.row3.z() - a.row2.z() * a.row3.x()) + a.row1.y() * (a.row2.x() * a.row3.z() - a.row2.z() * a.row3.x()) +
a.row1.y() * (a.row2.x() * a.row3.y() - a.row2.y() * a.row3.x()); a.row1.z() * (a.row2.x() * a.row3.y() - a.row2.y() * a.row3.x());
} }
pub fn mat3Inv(a: Mat3F64) Mat3F64 { pub fn mat3Inv(a: Mat3F64) Mat3F64 {
@ -105,7 +105,7 @@ pub fn mat3Inv(a: Mat3F64) Mat3F64 {
); );
const row2 = vec3Scale( const row2 = vec3Scale(
.init( .init(
a.row2.z() * a.row3.x() + a.row2.x() * a.row3.z(), a.row2.z() * a.row3.x() - a.row2.x() * a.row3.z(),
a.row1.x() * a.row3.z() - a.row1.z() * a.row3.x(), a.row1.x() * a.row3.z() - a.row1.z() * a.row3.x(),
a.row1.z() * a.row2.x() - a.row1.x() * a.row2.z(), a.row1.z() * a.row2.x() - a.row1.x() * a.row2.z(),
), ),
@ -113,7 +113,7 @@ pub fn mat3Inv(a: Mat3F64) Mat3F64 {
); );
const row3 = vec3Scale( const row3 = vec3Scale(
.init( .init(
a.row2.x() * a.row3.y() + a.row2.y() * a.row3.x(), a.row2.x() * a.row3.y() - a.row2.y() * a.row3.x(),
a.row1.y() * a.row3.x() - a.row1.x() * a.row3.y(), a.row1.y() * a.row3.x() - a.row1.x() * a.row3.y(),
a.row1.x() * a.row2.y() - a.row1.y() * a.row2.x(), a.row1.x() * a.row2.y() - a.row1.y() * a.row2.x(),
), ),
@ -321,30 +321,30 @@ test "Mat3 mul works" {
test "mat determinant works" { test "mat determinant works" {
const a: Mat3F64 = .{ const a: Mat3F64 = .{
.row1 = Vec3F64.init(1, 2, 3), .row1 = Vec3F64.init(1, 2, 3),
.row2 = Vec3F64.init(0, 1, 5), .row2 = Vec3F64.init(4, 1, 5),
.row3 = Vec3F64.init(0, 0, 9), .row3 = Vec3F64.init(3, 1, 9),
}; };
try std.testing.expectApproxEqAbs(9, mat3Det(a), 1e-12); try std.testing.expectApproxEqAbs(-35, mat3Det(a), 1e-12);
} }
test "mat inverse works" { test "mat inverse works" {
const a: Mat3F64 = .{ const a: Mat3F64 = .{
.row1 = Vec3F64.init(1, 2, 3), .row1 = Vec3F64.init(1, 2, 3),
.row2 = Vec3F64.init(0, 1, 5), .row2 = Vec3F64.init(4, 1, 5),
.row3 = Vec3F64.init(0, 0, 9), .row3 = Vec3F64.init(3, 1, 9),
}; };
const ainv = mat3Inv(a); const ainv = mat3Inv(a);
try std.testing.expectApproxEqAbs(1, ainv.row1.x(), 1e-12); try std.testing.expectApproxEqAbs(-4.0 / 35.0, ainv.row1.x(), 1e-12);
try std.testing.expectApproxEqAbs(-2, ainv.row1.y(), 1e-12); try std.testing.expectApproxEqAbs(3.0 / 7.0, ainv.row1.y(), 1e-12);
try std.testing.expectApproxEqAbs(7.0 / 9.0, ainv.row1.z(), 1e-12); try std.testing.expectApproxEqAbs(-1.0 / 5.0, ainv.row1.z(), 1e-12);
try std.testing.expectApproxEqAbs(0, ainv.row2.x(), 1e-12); try std.testing.expectApproxEqAbs(3.0 / 5.0, ainv.row2.x(), 1e-12);
try std.testing.expectApproxEqAbs(1, ainv.row2.y(), 1e-12); try std.testing.expectApproxEqAbs(0, ainv.row2.y(), 1e-12);
try std.testing.expectApproxEqAbs(-5.0 / 9.0, ainv.row2.z(), 1e-12); try std.testing.expectApproxEqAbs(-1.0 / 5.0, ainv.row2.z(), 1e-12);
try std.testing.expectApproxEqAbs(0, ainv.row3.x(), 1e-12); try std.testing.expectApproxEqAbs(-1.0 / 35.0, ainv.row3.x(), 1e-12);
try std.testing.expectApproxEqAbs(0, ainv.row3.y(), 1e-12); try std.testing.expectApproxEqAbs(-1.0 / 7.0, ainv.row3.y(), 1e-12);
try std.testing.expectApproxEqAbs(1.0 / 9.0, ainv.row3.z(), 1e-12); try std.testing.expectApproxEqAbs(1.0 / 5.0, ainv.row3.z(), 1e-12);
} }
test "vec3 mul mat3 works" { test "vec3 mul mat3 works" {
const b: Mat3F64 = .{ const b: Mat3F64 = .{