nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First working version of non-linear CCD based on single-substep motion-clamping.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-26 18:16:27 +01:00
parent 326469a1df
commit 97157c9423
29 changed files with 696 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/dynamics/solver/island_solver.rs
View File

@@ -59,7 +59,7 @@ impl IslandSolver {
counters.solver.velocity_update_time.resume();
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.integrate(params.dt)
rb.integrate_next_position(params.dt, true)
});
counters.solver.velocity_update_time.pause();
@@ -77,7 +77,7 @@ impl IslandSolver {
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
// Since we didn't run the velocity solver we need to integrate the accelerations here
rb.integrate_accelerations(params.dt);
rb.integrate(params.dt);
rb.integrate_next_position(params.dt, true);
});
counters.solver.velocity_update_time.pause();
}

4
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
View File

@@ -114,7 +114,7 @@ impl BallPositionGroundConstraint {
// are the local_anchors. The rb1 and rb2 have
// already been flipped by the caller.
Self {
anchor1: rb1.predicted_position * cparams.local_anchor2,
anchor1: rb1.next_position * cparams.local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor1,
@@ -123,7 +123,7 @@ impl BallPositionGroundConstraint {
}
} else {
Self {
anchor1: rb1.predicted_position * cparams.local_anchor1,
anchor1: rb1.next_position * cparams.local_anchor1,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor2,

2
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint {
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
let anchor1 = position1
* Point::from(array![|ii| if flipped[ii] {
cparams[ii].local_anchor2

4
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint {
let local_anchor2;
if flipped {
anchor1 = rb1.predicted_position * cparams.local_anchor2;
anchor1 = rb1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
} else {
anchor1 = rb1.predicted_position * cparams.local_anchor1;
anchor1 = rb1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
};

8
src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
View File

@@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint {
let local_axis2;
if flipped {
frame1 = rb1.predicted_position * cparams.local_frame2();
frame1 = rb1.next_position * cparams.local_frame2();
local_frame2 = cparams.local_frame1();
axis1 = rb1.predicted_position * cparams.local_axis2;
axis1 = rb1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
} else {
frame1 = rb1.predicted_position * cparams.local_frame1();
frame1 = rb1.next_position * cparams.local_frame1();
local_frame2 = cparams.local_frame2();
axis1 = rb1.predicted_position * cparams.local_axis1;
axis1 = rb1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
};

16
src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
View File

@@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint {
let local_basis2;
if flipped {
anchor1 = rb1.predicted_position * cparams.local_anchor2;
anchor1 = rb1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
axis1 = rb1.predicted_position * cparams.local_axis2;
axis1 = rb1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
basis1 = [
rb1.predicted_position * cparams.basis2[0],
rb1.predicted_position * cparams.basis2[1],
rb1.next_position * cparams.basis2[0],
rb1.next_position * cparams.basis2[1],
];
local_basis2 = cparams.basis1;
} else {
anchor1 = rb1.predicted_position * cparams.local_anchor1;
anchor1 = rb1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
axis1 = rb1.predicted_position * cparams.local_axis1;
axis1 = rb1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
basis1 = [
rb1.predicted_position * cparams.basis1[0],
rb1.predicted_position * cparams.basis1[1],
rb1.next_position * cparams.basis1[0],
rb1.next_position * cparams.basis1[1],
];
local_basis2 = cparams.basis2;
};

4
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -277,7 +277,7 @@ impl ParallelIslandSolver {
rb.linvel += dvel.linear;
rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);
rb.integrate(params.dt);
positions[rb.active_set_offset] = rb.position;
positions[rb.active_set_offset] = rb.next_position;
}
}
@@ -298,7 +298,7 @@ impl ParallelIslandSolver {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies[handle.0];
rb.set_position_internal(positions[rb.active_set_offset]);
rb.set_next_position(positions[rb.active_set_offset]);
}
}
})

4
src/dynamics/solver/position_solver.rs
View File

@@ -25,7 +25,7 @@ impl PositionSolver {
self.positions.extend(
bodies
.iter_active_island(island_id)
.map(|(_, b)| b.position),
.map(|(_, b)| b.next_position),
);
for _ in 0..params.max_position_iterations {
@@ -39,7 +39,7 @@ impl PositionSolver {
}
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.set_position_internal(self.positions[rb.active_set_offset])
rb.set_next_position(self.positions[rb.active_set_offset])
});
}
}