diff options
| -rw-r--r-- | physics/src/constraint/mod.rs | 11 |
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) } } |