From 9a19677c53dd8d3735027d03c6bc923b3fd9c3e6 Mon Sep 17 00:00:00 2001 From: eug-vs Date: Mon, 16 Dec 2024 22:22:09 +0100 Subject: fix: compute Jacobian by probing in both +- directions --- physics/src/constraint/mod.rs | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) (limited to 'physics/src/constraint/mod.rs') diff --git a/physics/src/constraint/mod.rs b/physics/src/constraint/mod.rs index e9997f5..5c5034c 100644 --- a/physics/src/constraint/mod.rs +++ b/physics/src/constraint/mod.rs @@ -21,19 +21,18 @@ pub trait Constraint { fn partial_derivative(&self, q: &DVector) -> RowDVector { let dq = 0.00001; - let c_original = self.c(&q); let mut result = RowDVector::zeros(q.len()); // The only non-zero components of derivative vector are // the particles that this constraint affects for particle_id in self.get_particles() { for i in 0..N { let index = particle_id * N + i; - let mut q_partial = q.clone(); - q_partial[index] += dq; + let mut a = q.clone(); + let mut b = q.clone(); + a[index] += dq; + b[index] -= dq; - let c = self.c(&q_partial); - - result[index] = (c - c_original) / dq + result[index] = (self.c(&a) - self.c(&b)) / (2.0 * dq) } } -- cgit v1.2.3