fix: compute Jacobian by probing in both +- directions

1 files changed, 5 insertions, 6 deletions

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<Scalar>) -> RowDVector<Scalar> {
         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)
             }
         }