From 3f26b46196531cfca10ae487bb242e8bb3f84f65 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Wed, 17 Feb 2021 19:15:52 +0100 Subject: Implement ball wide --- .../joint_constraint/ball_velocity_constraint_wide.rs | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index 95b0bb5..ff5e001 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -62,12 +62,15 @@ impl WBallVelocityConstraint { let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); - let anchor1 = position1 * local_anchor1 - world_com1; - let anchor2 = position2 * local_anchor2 - world_com2; + let anchor_world1 = position1 * local_anchor1; + let anchor_world2 = position2 * local_anchor2; + let anchor1 = anchor_world1 - world_com1; + let anchor2 = anchor_world2 - world_com2; let vel1: Vector = linvel1 + angvel1.gcross(anchor1); let vel2: Vector = linvel2 + angvel2.gcross(anchor2); - let rhs = -(vel1 - vel2); + let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction) + + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt()); let lhs; let cmat1 = anchor1.gcross_matrix(); @@ -239,12 +242,15 @@ impl WBallVelocityGroundConstraint { ); let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); - let anchor1 = position1 * local_anchor1 - world_com1; - let anchor2 = position2 * local_anchor2 - world_com2; + let anchor_world1 = position1 * local_anchor1; + let anchor_world2 = position2 * local_anchor2; + let anchor1 = anchor_world1 - world_com1; + let anchor2 = anchor_world2 - world_com2; let vel1: Vector = linvel1 + angvel1.gcross(anchor1); let vel2: Vector = linvel2 + angvel2.gcross(anchor2); - let rhs = vel2 - vel1; + let rhs = (vel2 - vel1) * SimdReal::splat(params.velocity_solve_fraction) + + (anchor_world2 - anchor_world1) * SimdReal::splat(params.velocity_based_erp_inv_dt()); let lhs; let cmat2 = anchor2.gcross_matrix(); -- cgit