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

Merge branch 'master' into collider-builder-debug

Browse Source
This commit is contained in:
Thierry Berger
2024-06-03 15:20:24 +02:00
parent fe336b9b98 856675032e
commit e1ed90603e
116 changed files with 5370 additions and 1613 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
.github/workflows/rapier-ci-build.yml vendored
View File

@@ -13,7 +13,7 @@ jobs:
check-fmt:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- name: Check formatting
run: cargo fmt -- --check
build-native:
@@ -21,7 +21,7 @@ jobs:
env:
RUSTFLAGS: -D warnings
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- run: sudo apt-get install -y cmake libxcb-composite0-dev
- name: Clippy
run: cargo clippy
@@ -60,7 +60,7 @@ jobs:
env:
RUSTFLAGS: -D warnings
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- run: rustup target add wasm32-unknown-unknown
- name: build rapier2d
run: cd crates/rapier2d && cargo build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
@@ -71,7 +71,7 @@ jobs:
env:
RUSTFLAGS: -D warnings
steps:
- uses: actions/checkout@v3
- uses: actions/checkout@v4
- run: rustup target add wasm32-unknown-emscripten
- name: build rapier2d
run: cd crates/rapier2d && cargo build --verbose --target wasm32-unknown-emscripten;

321
CHANGELOG.md
View File

@@ -1,8 +1,96 @@
## Unreleased
### Added
- Add `Multibody::inverse_kinematics`, `Multibody::inverse_kinematics_delta`,
and `::inverse_kinematics_delta_with_jacobian`
for running inverse kinematics on a multibody to align one its links pose to the given prescribed pose.
- Add `InverseKinematicsOption` to customize some behaviors of the inverse-kinematics solver.
- Add `Multibody::body_jacobian` to get the jacobian of a specific link.
- Add `Multibody::update_rigid_bodies` to update rigid-bodies based on the multibody links poses.
- Add `Multibody::forward_kinematics_single_link` to run forward-kinematics to compute the new pose and jacobian of a
single link without mutating the multibody. This can take an optional displacement on generalized coordinates that are
taken into account during transform propagation.
### Modified
- The contact constraints regularization parameters have been changed from `erp/damping_ratio` to
`natural_frequency/damping_ratio`. This helps define them in a timestep-length independent way. The new variables
are named `IntegrationParameters::contact_natural_frequency` and `IntegrationParameters::contact_damping_ratio`.
- The `IntegrationParameters::normalized_max_penetration_correction` has been replaced
by `::normalized_max_corrective_velocity`
to make the parameter more timestep-length independent. It is now set to a non-infinite value to eliminate aggressive
"popping effects".
- The `Multibody::forward_kinematics` method will no longer automatically update the poses of the `RigidBody` associated
to each joint. Instead `Multibody::update_rigid_bodies` has to be called explicitly.
- The `Multibody::forward_kinematics` method will automatically adjust the multibody’s degrees of freedom if the root
rigid-body changed type (between dynamic and non-dynamic). It can also optionally apply the root’s rigid-body pose
instead of the root link’s pose (useful for example if you modified the root rigid-body pose externally and wanted
to propagate it to the multibody).
- Remove an internal special-case for contact constraints on fast contacts. The doesn’t seem necessary with the substep
solver.
## v0.19.0 (05 May 2024)
### Fix
- Fix crash when simulating a spring joint between two dynamic bodies.
- Fix kinematic bodies not being affected by gravity after being switched back to dynamic.
- Fix regression on contact force reporting from contact force events.
- Fix kinematic character controller getting stuck against vertical walls.
- Fix joint limits/motors occasionally not being applied properly when one of the attached
rigid-bodies is fixed.
- Fix an issue where contacts would be completely ignored between two convex shapes.
### Added
**Many stability improvements were added as part of this release. To see illustrations of some of these
changes, see [#625](https://github.com/dimforge/rapier/pull/625).**
- Add `RigidBody::predict_position_using_velocity` to predict the next position of the rigid-body
based only on its current velocity.
- Add `Collider::copy_from` to copy most collider attributes to an existing collider.
- Add `RigidBody::copy_from` to copy most rigid-body attributes to an existing rigid-body.
- Add the `BroadPhase` trait and expect an implementor of this trait as input to `PhysicsPipeline::step`.
- Implement a 2D block-solver as well as warmstarting. Significantly improves stacking capabilities. Generally reduces
the "pop" effect that can happen due to penetration corrections.
- Add `RigidBodyBuilder::soft_ccd_prediction` and `RigidBody::set_soft_ccd_prediction` to enable `soft-ccd`: a form of
CCD based on predictive contacts. This is helpful for objects moving moderately fast. This form of CCD is generally
cheaper than the normal (time-dropping) CCD implemented so far. It is possible to combine both soft-ccd and
time-dropping ccd.
- Add a `ColliderBuilder::contact_skin`, `Collider::set_contact_skin`, and `Collider::contact_skin`. This forces the
solver te maintain a gap between colliders with non-zero contact skin, as if they had a slight margin around them.
This helps performance and stability for thin objects (like triangle meshes).
- Internal edges were reworked to avoid dropping contacts that would help with stability, and improve stability of
collisions between two triangle meshes. The `TriMeshFlags::FIX_INTERNAL_EDGES` and
`HeightFieldFlags::FIX_INTERNAL_EDGES` flags were added to enable internal edges handling.
- Add `IntegrationParameters::length_units` to automatically adjust internal thresholds when the user relies on custom
length units (e.g. pixels in 2D).
### Modified
**Many shape-casting related functions/structs were renamed. Check out the CHANGELOG for parry 0.15.0 for
additional details.**
- Renamed `BroadPhase` to `BroadPhaseMultiSap`. The `BroadPhase` is now a trait that can be
implemented for providing a custom broad-phase to rapier. Equivalently, the `DefaultBroadPhase` type
alias can be used in place of `BroadPhaseMultiSap`.
- The kinematic character controller autostepping is now disabled by default.
- Add `KinematicCharacterController::normal_nudge_factor` used to help getting the character unstuck
due to rounding errors.
- Rename `TOI` to `ShapeCastHit`.
- Rename many fields from `toi` to `time_of_impact`.
- The `QueryPipeline::cast_shape` method now takes a `ShapeCastOptions` instead of the `max_toi`
and `stop_at_penetration` parameters. This allows a couple of extra configurations, including the
ability to have the shape-cast target a specific distance instead of actual shape overlap.
## v0.18.0 (24 Jan. 2024)
The main highlight of this release is the implementation of a new non-linear constraints solver for better stability
and increased convergence rates. See [#579](https://github.com/dimforge/rapier/pull/579) for additional information.
In order to adjust the number of iterations of the new solver, simply adjust `IntegrationParameters::num_solver_iterations`.
In order to adjust the number of iterations of the new solver, simply
adjust `IntegrationParameters::num_solver_iterations`.
If recovering the old solver behavior is useful to you, call `IntegrationParameters::switch_to_standard_pgs_solver()`.
It is now possible to specify some additional solver iteration for specific rigid-bodies (and everything interacting
@@ -11,11 +99,15 @@ with it directly or indirectly through contacts and joints): `RigidBodyBuilder::
without affecting performance of the other parts of the simulation.
### Fix
- Fix bug causing angular joint limits and motor to sometimes only take into account half of the angles specified by the user.
- Fix bug causing angular joint limits and motor to sometimes only take into account half of the angles specified by the
user.
- Fix bug where collisions would not be re-computed after a collider was re-enabled.
### Added
- Add a `SpringJoint` and `SpringJointBuilder` for simulating springs with customizable stiffness and damping coefficients.
- Add a `SpringJoint` and `SpringJointBuilder` for simulating springs with customizable stiffness and damping
coefficients.
- Add `SphericalJoint::local_frame1/2`, `::set_local_frame1/2`, and `SphericalJointBuilder::local_frame1/2` to set both
the joint’s anchor and reference orientation.
- Add `EffectiveCharacterMovement::is_sliding_down_slope` to indicate if the character controlled by the kinematic
@@ -27,6 +119,7 @@ without affecting performance of the other parts of the simulation.
- Fix debug-renderer showing moved kinematic rigid-bodies only at their initial position.
### Modified
- Make `Wheel::friction_slip` public to customize the front friction applied to the vehicle controller’s wheels.
- Add the `DebugRenderBackend::filter_object` predicate that can be implemented to apply custom filtering rules
on the objects being rendered.
@@ -35,19 +128,24 @@ without affecting performance of the other parts of the simulation.
- Rename `NarrowPhase::intersections_with` to `NarrowPhase::intersection_pairs_with`.
## v0.17.2 (26 Feb. 2023)
### Fix
- Fix issue with convex polyhedron jitter due to missing contacts.
- Fix character controller getting stuck against vertical walls.
- Fix character controller’s snapping to ground not triggering sometimes.
- Fix character controller’s horizontal offset being mostly ignored and some instances of vertical offset being ignored.
## v0.17.1 (22 Jan. 2023)
### Fix
- Fix bug resulting in dynamic rigid-bodies acting as kinematic bodies after being disabled and then re-enabled.
## v0.17.0 (15 Jan. 2023)
### Added
- Add `RigidBody::set_enabled`, `RigidBody::is_enabled`, `RigidBodyBuilder::enabled` to enable/disable a rigid-body
without having to delete it. Disabling a rigid-body attached to a multibody joint isn’t supported yet.
- Add `Collider::set_enabled`, `Collider::is_enabled`, `ColliderBuilder::enabled` to enable/disable a collider
@@ -61,37 +159,48 @@ without affecting performance of the other parts of the simulation.
- Add `RigidBody::locked_axes` to get the rigid-body axes that were locked by the user.
### Modified
- Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this
argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time as
argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time
as
these other pipelines. In that case, calling `QueryPipeline::update` a `PhysicsPipeline::step` isn’t needed.
- `RigidBody::set_body_type` now takes an extra boolean argument indicating if the rigid-body should be woken-up
(if it becomes dynamic).
- `RigidBody::mass_properties` now also returns the world-space mass-properties of the rigid-body.
### Fix
- Fix bug resulting in rigid-bodies being awakened after they are created, even if they are created sleeping.
## v0.16.1 (10 Nov. 2022)
### Fix
- Fixed docs build on `docs.rs`.
## v0.16.0 (30 Oct. 2022)
### Added
- Implement `Copy` for `CharacterCollision`.
- Implement conversion (`From` trait) between `Group` and `u32`.
- Add `ColliderBuilder::trimesh_with_flags` to build a triangle mesh with specific flags controlling
its initialization.
### Modified
- Rename `AABB` to `Aabb` to comply with Rust’s style guide.
- Switch to `parry 0.11`.
### Fix
- Fix internal edges of 3D triangle meshes or 3D heightfields generating invalid contacts preventing
balls from moving straight.
## v0.15.0 (02 Oct. 2022)
### Added
- Add a **kinematic character** controller implementation. See the `control` module. The character controller currently
supports the following features:
- Slide on uneven terrains
@@ -104,8 +213,8 @@ without affecting performance of the other parts of the simulation.
- Report information on the obstacles it hit on its path.
- Implement `serde` serialization/deserialization for `CollisionEvents` when the `serde-serialize` feature is enabled
### Modified
- The methods `Collider::set_rotation`, `RigidBody::set_rotation`, and `RigidBody::set_next_kinematic_rotation` now
take a rotation (`UnitQuaternion` or `UnitComplex`) instead of a vector/angle.
- The method `QueryFilter::exclude_dynamic` is now a static method (the `self` argument was removed).
@@ -117,7 +226,9 @@ without affecting performance of the other parts of the simulation.
position.
## v0.14.0 (09 July 2022)
### Fixed
- Fix unpredictable broad-phase panic when using small colliders in the simulation.
- Fix collision events being incorrectly generated for any shape that produces multiple
contact manifolds (like triangle meshes).
@@ -125,6 +236,7 @@ without affecting performance of the other parts of the simulation.
to `CollisionPipeline::step`.
### Modified
- The `RigidBodyBuilder::additional_mass` method will now result in the additional angular inertia
being automatically computed based on the shapes of the colliders attached to the rigid-body.
- Remove the deprecated methods `RigidBodyBuilder::mass`, `::principal_angular_inertia`, `::principal_inertia`.
@@ -137,6 +249,7 @@ without affecting performance of the other parts of the simulation.
`RigidBodyBuilder::enabled_rotations` and `RigidBodyBuilder::enabled_translations`.
### Added
- Add `RigidBody::recompute_mass_properties_from_colliders` to force the immediate computation
of a rigid-body’s mass properties (instead of waiting for them to be recomputed during the next
timestep). This is useful to be able to read immediately the result of a change of a rigid-body
@@ -149,7 +262,8 @@ without affecting performance of the other parts of the simulation.
- Add `ColliderBuilder::mass` to set the mass of the collider instead of its density. Its angular
inertia tensor will be automatically computed based on this mass and its shape.
- Add `Collider::mass` and `Collider::volume` to retrieve the mass or volume of a collider.
- Add the `QueryFilter` that is now used by all the scene queries instead of the `CollisionGroups` and `Fn(ColliderHandle) -> bool`
- Add the `QueryFilter` that is now used by all the scene queries instead of the `CollisionGroups`
and `Fn(ColliderHandle) -> bool`
closure. This `QueryFilter` provides easy access to most common filtering strategies (e.g. dynamic bodies only,
excluding one particular collider, etc.) for scene queries.
- Add force reporting based on contact force events. The `EventHandler` trait has been modified to include
@@ -161,12 +275,15 @@ without affecting performance of the other parts of the simulation.
contact force events.
## v0.13.0 (31 May 2022)
### Fixed
- Fix incorrect sensor events being generated after collider removal.
- Fix bug where the CCD thickness wasn’t initialized properly.
- Fix bug where the contact compliance would result in undesired tunneling, despite CCD being enabled.
### Modified
- Add a `wake_up: bool` argument to the `ImpulseJointSet::insert` and `MultibodyJointSet::insert` to
automatically wake-up the rigid-bodies attached to the inserted joint.
- The methods `ImpulseJointSet::remove/remove_joints_attached_to_rigid_body`,
@@ -180,11 +297,14 @@ without affecting performance of the other parts of the simulation.
by default.
### Added
- Debug-renderer: add rendering of contacts, solver contacts, and collider Aabbs
- Add `MultibodyJointSet::attached_joints` to return all the multibody joints attached to a given rigid-body.
## v0.12.0 (30 Apr. 2022)
### Fixed
- Fix the simulation when the `parallel` feature is enabled.
- Fix bug where damping would not be applied properly to some bodies.
- Fix panics caused by various situations (contact or joints) involving rigid-bodies with locked translations/rotations.
@@ -194,6 +314,7 @@ without affecting performance of the other parts of the simulation.
- Fix the broad-phase becoming potentially invalid after a change of collision groups.
### Modified
- Switch to `nalgebra` 0.31.
- Switch to `parry` 0.9.
- Rename `JointHandle` to `ImpulseJointHandle`.
@@ -210,27 +331,30 @@ without affecting performance of the other parts of the simulation.
- The `ActiveEvents::CONTACT_EVENTS` and `ActiveEvents::INTERSECTION_EVENTS` flags have been replaced by a single
flag `ActiveEvents::COLLISION_EVENTS`.
- Joint motors no longer have a `VelocityBased` model. The new choices are `AccelerationBased` and `ForceBased`
which are more stable.
which are more stable.
- Calling the `.build()` function from builders (`RigidBodyBuilder`, `ColliderBuilder`, etc.) is no longer necessary
whan adding them to sets. It is automatically called thanks to `Into<_>` implementations.
when adding them to sets. It is automatically called thanks to `Into<_>` implementations.
- The `ComponentSet` abstractions (and related `_generic` methods like `PhysicsPipeline::step_generic`) have been
removed. Custom storage for colliders and rigid-bodies are no longer possible: use the built-in `RigidBodySet` and
`ColliderSet` instead.
### Semantic modifications
These are changes in the behavior of the physics engine that are not necessarily
reflected by an API change. See [#304](https://github.com/dimforge/rapier/pull/304) for extensive details.
- `RigidBody::set_linvel` and `RigidBody::set_angvel` no longer modify the velocity of static bodies.
- `RigidBody::set_body_type` will reset the velocity of a rigid-body to zero if it is static.
- Don’t automatically clear forces at the end of a timestep.
- Don’t reset the velocity of kinematic bodies to zero at the end of the timestep.
- Events `CollisionEvent::Stopped` are now generated after a collider is removed.
- Events `CollisionEvent::Stopped` are now generated after a collider is removed.
### Added
- Significantly improve the API of joints by adding:
* Builders based on the builder pattern.
* Getters and setters for all joints.
* Method to convert a `GenericJoint` to one of the more specific joint type.
* Builders based on the builder pattern.
* Getters and setters for all joints.
* Method to convert a `GenericJoint` to one of the more specific joint type.
- Improve stability of joint motors.
- Adds a `bool` argument to `RigidBodySet::remove`. If set to `false`, the colliders attached to the rigid-body
won’t be automatically deleted (they will only be detached from the deleted rigid-body instead).
@@ -240,12 +364,16 @@ reflected by an API change. See [#304](https://github.com/dimforge/rapier/pull/3
renderer to debug the state of the physics engine.
## v0.12.0-alpha.0 (2 Jan. 2022)
### Fixed
- Fixed `RigidBody::restrict_rotations` to properly take into account the axes to lock.
### Modified
- All the impulse-based joints have been replaced by a single generic 6-Dofs joint in 3D
(or 3-Dofs joint in 2D) named `ImpulseJoint`. The `RevoluteJoint, PrismaticJoint, FixedJoint`,
and `SphericalJoint` (formely named `BallJoint`) structures still exist but are just convenient
and `SphericalJoint` (formerly named `BallJoint`) structures still exist but are just convenient
ways to initialize the generic `ImpulseJoint`.
- Our constraints solver has been modified. Before we used one velocity-based resolution followed
by one position-based resolution. We are now using two velocity-based resolution: the first one
@@ -253,63 +381,81 @@ reflected by an API change. See [#304](https://github.com/dimforge/rapier/pull/3
code significantly while offering stiffer results.
### Added
- Added multibody joints: joints based on the reduced-coordinates modeling. These joints can’t
- Added multibody joints: joints based on the reduced-coordinates modeling. These joints can’t
violate their positional constraint.
- Implement `Default` for most of the struct that supports it.
## v0.11.1
### Fixed
- Fix a bug causing large moving colliders to miss some collisions after some time.
- Fix invalid forces generated by contacts with position-based kinematic bodies.
- Fix a bug where two colliders without parent would not have their collision computed even if the
appropriate flags were set.
## v0.11.0
Check out the user-guide for the JS/Typescript bindings for rapier. It has been fully rewritten and is now exhaustive!
Check it out on [rapier.rs](https://www.rapier.rs/docs/user_guides/javascript/getting_started_js)
### Added
- Joint limits are now implemented for all joints that can support them (prismatic, revolute, and ball joints).
### Modified
- Switch to `nalgebra 0.29`.
### Fixed
- Fix the build of Rapier when targeting emscripten.
## v0.10.1
### Added
- Add `Collider::set_translation_wrt_parent` to change the translation of a collider with respect to its parent rigid-body.
- Add `Collider::set_translation_wrt_parent` to change the translation of a collider with respect to its parent
rigid-body.
- Add `Collider::set_rotation_wrt_parent` to change the translation of a collider with respect to its parent rigid-body.
## v0.10.0
### Added
- Implement `Clone` for `IslandManager`.
### Modified
- `JointSet::insert` no longer takes the rigid-body set in its arguments.
- Modify the testbed's plugin system to let plugins interact with the rendering.
- Implement `PartialEq` for most collider and rigid-body components.
## v0.9.2
### Added
- Make the method JointSet::remove_joints_attached_to_rigid_body public so that it can can be called externally for
letting component-based Rapier integration call it to cleanup joints after a rigid-body removal.
### Fixed
- Fix a panic that could happen when the same collider is listed twice in the removed_colliders array.
## v0.9.1
### Added
- Add `rapier::prelude::nalgebra` so that the `vector!` and `point!` macros work out-of-the-box after importing
the prelude: `use rapier::prelude::*`
## v0.9.0
The user-guide has been fully rewritten and is now exhaustive! Check it out on [rapier.rs](https://rapier.rs/)
### Added
- A prelude has been added in order to simplify the most common imports. For example: `use rapier3d::prelude::*`
- Add `RigidBody::set_translation` and `RigidBody.translation()`.
- Add `RigidBody::set_rotation` and `RigidBody.rotation()`.
@@ -322,6 +468,7 @@ The user-guide has been fully rewritten and is now exhaustive! Check it out on [
some SIMD code do generate NaNs which are filtered out by lane-wise selection).
### Modified
The use of `RigidBodySet, ColliderSet, RigidBody, Collider` is no longer mandatory. Rigid-bodies and colliders have
been split into multiple components that can be stored in a user-defined set. This is useful for integrating Rapier
with other engines (for example this allows us to use Bevy's Query as our rigid-body/collider sets).
@@ -330,6 +477,7 @@ The `RigidBodySet, ColliderSet, RigidBody, Collider` are still the best option f
provide their own component sets.
#### Rigid-bodies
- Renamed `BodyStatus` to `RigidBodyType`.
- `RigidBodyBuilder::translation` now takes a vector instead of individual components.
- `RigidBodyBuilder::linvel` now takes a vector instead of individual components.
@@ -337,8 +485,9 @@ provide their own component sets.
`RigidBodyBuilder::new_kinematic_velocity_based` constructors.
- The `RigidBodyType::Kinematic` variant has been replaced by two variants: `RigidBodyType::KinematicVelocityBased` and
`RigidBodyType::KinematicPositionBased`.
#### Colliders
- `Colliderbuilder::translation` now takes a vector instead of individual components.
- The way `PhysicsHooks` are enabled changed. Now, a physics hooks is executed if any of the two
colliders involved in the contact/intersection pair contains the related `PhysicsHooksFlag`.
@@ -360,115 +509,139 @@ provide their own component sets.
- Fixed a bug where collision groups were ignored by CCD.
#### Joints
- The fields `FixedJoint::local_anchor1` and `FixedJoint::local_anchor2` have been renamed to
`FixedJoint::local_frame1` and `FixedJoint::local_frame2`.
#### Pipelines and others
- The field `ContactPair::pair` (which contained two collider handles) has been replaced by two
fields: `ContactPair::collider1` and `ContactPair::collider2`.
- The list of active dynamic bodies is now retrieved with `IslandManager::active_dynamic_bodies`
instead of `RigidBodySet::iter_active_dynamic`.
- The list of active kinematic bodies is now retrieved with `IslandManager::active_kinematic_bodies`
instead of `RigidBodySet::iter_active_kinematic`.
- `NarrowPhase::contacts_with` now returns an `impl Iterator<Item = &ContactPair>` instead of
- `NarrowPhase::contacts_with` now returns an `impl Iterator<Item = &ContactPair>` instead of
an `Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>>`. The colliders
handles can be read from the contact-pair itself.
- `NarrowPhase::intersections_with` now returns an iterator directly instead of an `Option<impl Iterator>`.
- Rename `PhysicsHooksFlags` to `ActiveHooks`.
- Add the contact pair as an argument to `EventHandler::handle_contact_event`
## v0.8.0
### Modified
- Switch to nalgebra 0.26.
## v0.7.2
### Added
- Implement `Serialize` and `Deserialize` for the `CCDSolver`.
### Fixed
- Fix a crash that could happen after adding and then removing a collider right away,
before stepping the simulation.
before stepping the simulation.
## v0.7.1
### Fixed
- Fixed a bug in the broad-phase that could cause non-determinism after snapshot restoration.
## v0.7.0
### Added
- Add the support of **Continuous Collision Detection** (CCD) to
make sure that some fast-moving objects (chosen by the user) don't miss any contacts.
This is done by using motion-clamping, i.e., each fast-moving rigid-body with CCD enabled will
be stopped at the time where their first contact happen. This will result in some "time loss" for that
rigid-body. This loss of time can be reduced by increasing the maximum number of CCD substeps executed
(the default being 1).
make sure that some fast-moving objects (chosen by the user) don't miss any contacts.
This is done by using motion-clamping, i.e., each fast-moving rigid-body with CCD enabled will
be stopped at the time where their first contact happen. This will result in some "time loss" for that
rigid-body. This loss of time can be reduced by increasing the maximum number of CCD substeps executed
(the default being 1).
- Add the support of **collider modification**. Now, most of the characteristics of a collider can be
modified after the collider has been created.
modified after the collider has been created.
- We now use an **implicit friction cone** for handling friction, instead of a pyramidal approximation
of the friction cone. This means that friction will now behave in a more isotropic way (i.e. more realistic
Coulomb friction).
of the friction cone. This means that friction will now behave in a more isotropic way (i.e. more realistic
Coulomb friction).
- Add the support of **custom filters** for the `QueryPipeline`. So far, interaction groups (bit masks)
had to be used to exclude from colliders from a query made with the `QueryPipeline`. Now it is also
possible to provide a custom closures to apply arbitrary user-defined filters.
had to be used to exclude from colliders from a query made with the `QueryPipeline`. Now it is also
possible to provide a custom closures to apply arbitrary user-defined filters.
- It is now possible to solve penetrations using the velocity solver instead of (or alongside) the
position solver (this is disabled by default, set `IntegrationParameters::velocity_based_erp` to
position solver (this is disabled by default, set `IntegrationParameters::velocity_based_erp` to
a value `> 0.0` to enable.).
Added the methods:
- `ColliderBuilder::halfspace` to create a collider with an unbounded plane shape.
- `Collider::shape_mut` to get a mutable reference to its shape.
- `Collider::set_shape`, `::set_restitution_combine_rule`, `::set_position_wrt_parent`, `::set_collision_groups`
`::set_solver_groups` to change various properties of a collider after its creation.
`::set_solver_groups` to change various properties of a collider after its creation.
- `RigidBodyBuilder::ccd_enabled` to enable CCD for a rigid-body.
### Modified
- The `target_dist` argument of `QueryPipeline::cast_shape` was removed.
- `RigidBodyBuilder::mass_properties` has been deprecated, replaced by `::additional_mass_properties`.
- `RigidBodyBuilder::mass` has been deprecated, replaced by `::additional_mass`.
- `RigidBodyBuilder::principal_angular_inertia` has been deprecated, replaced by `::additional_principal_angular_inertia`.
- The field `SolveContact::data` has been replaced by the fields `SolverContact::warmstart_impulse`,
- `RigidBodyBuilder::principal_angular_inertia` has been deprecated, replaced
by `::additional_principal_angular_inertia`.
- The field `SolveContact::data` has been replaced by the fields `SolverContact::warmstart_impulse`,
`SolverContact::warmstart_tangent_impulse`, and `SolverContact::prev_rhs`.
- All the fields of `IntegrationParameters` that we don't use have been removed.
- `NarrowPhase::maintain` has been renamed to `NarrowPhase::handle_user_changes`.
- `BroadPhase::maintain` has been removed. Use ` BroadPhase::update` directly.
### Fixed
- The Broad-Phase algorithm has been completely reworked to support large colliders properly (until now
they could result in very large memory and CPU usage).
they could result in very large memory and CPU usage).
## v0.6.1
### Fixed
- Fix a determinism problem that may happen after snapshot restoration, if a rigid-body is sleeping at
the time the snapshot is taken.
## v0.6.0
### Added
- The support of **dominance groups** have been added. Each rigid-body is part of a dominance group in [-127; 127]
(the default is 0). If two rigid-body are in contact, the one with the highest dominance will act as if it has
an infinite mass, making it immune to the forces the other body would apply on it. See [#122](https://github.com/dimforge/rapier/pull/122)
for further details.
(the default is 0). If two rigid-body are in contact, the one with the highest dominance will act as if it has
an infinite mass, making it immune to the forces the other body would apply on it.
See [#122](https://github.com/dimforge/rapier/pull/122)
for further details.
- The support for **contact modification** has been added. This can bee used to simulate conveyor belts,
one-way platforms and other non-physical effects. It can also be used to simulate materials with
variable friction and restitution coefficient on a single collider. See [#120](https://github.com/dimforge/rapier/pull/120)
for further details.
one-way platforms and other non-physical effects. It can also be used to simulate materials with
variable friction and restitution coefficient on a single collider.
See [#120](https://github.com/dimforge/rapier/pull/120)
for further details.
- The support for **joint motors** have been added. This can be used to control the position and/or
velocity of a joint based on a spring-like equation. See [#119](https://github.com/dimforge/rapier/pull/119)
for further details.
velocity of a joint based on a spring-like equation. See [#119](https://github.com/dimforge/rapier/pull/119)
for further details.
### Removed
- The `ContactPairFilter` and `IntersectionPairFilter` traits have been removed. They are both
combined in a single new trait: `PhysicsHooks`.
## v0.5.0
In this release we are dropping `ncollide` and use our new crate [`parry`](https://parry.rs)
instead! This comes with a lot of new features, as well as two new crates: `rapier2d-f64` and
`rapier3d-f64` for physics simulation with 64-bits floats.
### Added
- Added a `RAPIER.version()` function at the root of the package to retrieve the version of Rapier
as a string.
Several geometric queries have been added to the `QueryPipeline`:
- `QueryPipeline::intersections_with_ray`: get all colliders intersecting a ray.
- `QueryPipeline::intersection_with_shape`: get one collider intersecting a shape.
- `QueryPipeline::project_point`: get the projection of a point on the closest collider.
@@ -478,66 +651,83 @@ Several geometric queries have been added to the `QueryPipeline`:
- `QueryPipeline::intersections_with_shape`: get all the colliders intersecting a shape.
Several new shape types are now supported:
- `RoundCuboid`, `Segment`, `Triangle`, `RoundTriangle`, `Polyline`, `ConvexPolygon` (2D only),
`RoundConvexPolygon` (2D only), `ConvexPolyhedron` (3D only), `RoundConvexPolyhedron` (3D only),
`RoundCone` (3D only).
It is possible to build `ColliderDesc` using these new shapes:
- `ColliderBuilder::round_cuboid`, `ColliderBuilder::segment`, `ColliderBuilder::triangle`, `ColliderBuilder::round_triangle`,
`ColliderBuilder::convex_hull`, `ColliderBuilder::round_convex_hull`, `ColliderBuilder::polyline`,
`ColliderBuilder::convex_decomposition`, `ColliderBuilder::round_convex_decomposition`,
`ColliderBuilder::convex_polyline` (2D only), `ColliderBuilder::round_convex_polyline` (2D only),
`ColliderBuilder::convex_mesh` (3D only),`ColliderBuilder::round_convex_mesh` (3D only), `ColliderBuilder::round_cone` (3D only).
`ColliderBuilder::convex_mesh` (3D only),`ColliderBuilder::round_convex_mesh` (3D
only), `ColliderBuilder::round_cone` (3D only).
It is possible to specify different rules for combining friction and restitution coefficients
of the two colliders involved in a contact with:
- `ColliderDesc::friction_combine_rule`, and `ColliderDesc::restitution_combine_rule`.
Various RigidBody-related getter and setters have been added:
- `RigidBodyBuilder::gravity_scale`, `RigidBody::gravity_scale`, `RigidBody::set_gravity_scale` to get/set the scale
factor applied to the gravity affecting a rigid-body. Setting this to 0.0 will make the rigid-body ignore gravity.
- `RigidBody::set_linear_damping` and `RigidBody::set_angular_damping` to set the linear and angular damping of
the rigid-body.
- `RigidBodyBuilder::restrict_rotations` to prevent rotations along specific coordinate axes. This replaces the three
boolean arguments previously passed to `.set_principal_angular_inertia`.
### Breaking changes
Breaking changes related to contacts:
- The way contacts are represented changed. Refer to the documentation of `parry::query::ContactManifold`, `parry::query::TrackedContact`
- The way contacts are represented changed. Refer to the documentation
of `parry::query::ContactManifold`, `parry::query::TrackedContact`
and `rapier::geometry::ContactManifoldData` and `rapier::geometry::ContactData` for details.
Breaking changes related to rigid-bodies:
- The `RigidBodyDesc.setMass` takes only one argument now. Use `RigidBodyDesc.lockTranslations` to lock the translational
- The `RigidBodyDesc.setMass` takes only one argument now. Use `RigidBodyDesc.lockTranslations` to lock the
translational
motion of the rigid-body.
- The `RigidBodyDesc.setPrincipalAngularInertia` no longer have boolean parameters to lock rotations.
Use `RigidBodyDesc.lockRotations` or `RigidBodyDesc.restrictRotations` to lock the rotational motion of the rigid-body.
Use `RigidBodyDesc.lockRotations` or `RigidBodyDesc.restrictRotations` to lock the rotational motion of the
rigid-body.
Breaking changes related to colliders:
- The collider shape type has been renamed from `ColliderShape` to `SharedShape` (now part of the Parry crate).
- The `Polygon` shape no longer exists. For a 2D convex polygon, use a `ConvexPolygon` instead.
- All occurrences of `Trimesh` have been replaced by `TriMesh` (note the change in case).
Breaking changes related to events:
- Rename all occurrences of `Proximity` to `Intersection`.
- The `Proximity` enum has been removed, it's replaced by a boolean.
## v0.4.2
- Fix a bug in angular inertia tensor computation that could cause rotations not to
work properly.
- Add `RigidBody::set_mass_properties` to set the mass properties of an already-constructed
rigid-body.
## v0.4.1
- The `RigidBodyBuilder::principal_inertia` method has been deprecated and renamed to
`principal_angular_inertia` for clarity.
## v0.4.0
- The rigid-body `linvel`, `angvel`, and `position` fields are no longer public. Access using
their corresponding getters/setters. For example: `rb.linvel()`, `rb.set_linvel(vel, true)`.
- Add `RigidBodyBuilder::sleeping(true)` to allow the creation of a rigid-body that is asleep
at initialization-time.
#### Locking translation and rotations of a rigid-body
- Add `RigidBodyBuilder::lock_rotations` to prevent a rigid-body from rotating because of forces.
- Add `RigidBodyBuilder::lock_translations` to prevent a rigid-body from translating because of forces.
- Add `RigidBodyBuilder::principal_inertia` for setting the principal inertia of a rigid-body, and/or
@@ -546,6 +736,7 @@ Breaking changes related to events:
contributions should be taken into account in the future too.
#### Reading contact and proximity information
- Add `NarrowPhase::contacts_with` and `NarrowPhase::proximities_with` to retrieve all the contact
pairs and proximity pairs involving a specific collider.
- Add `NarrowPhase::contact_pair` and `NarrowPhase::proximity_pair` to retrieve one specific contact
@@ -554,6 +745,7 @@ Breaking changes related to events:
proximity pairs detected by the narrow-phase.
## v0.3.2
- Add linear and angular damping. The damping factor can be set with `RigidBodyBuilder::linear_damping` and
`RigidBodyBuilder::angular_damping`.
- Implement `Clone` for almost everything that can be worth cloning.
@@ -562,34 +754,43 @@ Breaking changes related to events:
- The restitution coefficient of colliders is now taken into account by the physics solver.
## v0.3.1
- Fix non-determinism problem when using triangle-meshes, cone, cylinders, or capsules.
- Add `JointSet::remove(...)` to remove a joint from the `JointSet`.
## v0.3.0
- Collider shapes are now trait-objects instead of a `Shape` enum.
- Add a user-defined `u128` to each colliders and rigid-bodies for storing user data.
- Add the support for `Cylinder`, `RoundCylinder`, and `Cone` shapes.
- Added the support for collision filtering based on bit masks (often known as collision groups, collision masks, or
collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for filtering
what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for filtering
collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for
filtering
what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for
filtering
what pair of colliders should have their contact forces computed by the constraints solver.
- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by the `QueryPipeline`.
- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by
the `QueryPipeline`.
- Added collision filters based on user-defined trait-objects. This adds two traits `ContactPairFilter` and
`ProximityPairFilter` that allows user-defined logic for determining if two colliders/sensors are allowed to interact.
- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter` and `&ProximityPairFilter`
for filtering contact and proximity pairs.
- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter`
and `&ProximityPairFilter`
for filtering contact and proximity pairs.
## v0.2.1
- Fix panic in TriMesh construction and QueryPipeline update caused by a stack overflow or a subtraction underflow.
## v0.2.0
The most significant change on this version is the addition of the `QueryPipeline` responsible for performing
scene-wide queries. So far only ray-casting has been implemented.
- Add `ColliderSet::remove(...)` to remove a collider from the `ColliderSet`.
- Replace `PhysicsPipeline::remove_rigid_body` by `RigidBodySet::remove`.
- The `JointSet.iter()` now returns an iterator yielding `(JointHandle, &Joint)` instead of just `&Joint`.
- Add `ColliderDesc::translation(...)` to set the translation of a collider relative to the rigid-body it is attached to.
- Add `ColliderDesc::translation(...)` to set the translation of a collider relative to the rigid-body it is attached
to.
- Add `ColliderDesc::rotation(...)` to set the rotation of a collider relative to the rigid-body it is attached to.
- Add `ColliderDesc::position(...)` to set the position of a collider relative to the rigid-body it is attached to.
- Add `Collider::position_wrt_parent()` to get the position of a collider relative to the rigid-body it is attached to.
@@ -597,8 +798,8 @@ scene-wide queries. So far only ray-casting has been implemented.
- Deprecate `Collider::delta()` in favor of the new `Collider::position_wrt_parent()`.
- Fix multiple issues occurring when having colliders resulting in a non-zero center-of-mass.
- Fix a crash happening when removing a rigid-body with a collider, stepping the simulation, adding another rigid-body
with a collider, and stepping the simulation again.
with a collider, and stepping the simulation again.
- Fix NaN when detection contacts between two polygonal faces where one has a normal perfectly perpendicular to the
separating vector.
separating vector.
- Fix bug collision detection between trimeshes and other shapes. The bug appeared depending on whether the trimesh
collider was added before the other shape's collider or after.
collider was added before the other shape's collider or after.

4
Cargo.toml
View File

@@ -1,6 +1,6 @@
[workspace]
members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
"crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d" ]
members = ["crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
"crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d"]
resolver = "2"
[patch.crates-io]

28
README.md
View File

@@ -35,24 +35,28 @@
## What is Rapier?
Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. These crates
Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. These crates
are `rapier2d`, `rapier3d`, `rapier2d-f64`, and `rapier3d-f64`. They are written with the Rust
programming language, by the [Dimforge](https://dimforge.com) organization. It is forever free
and open-source!
## Roadmap
We update our roadmap at the beginning of each year. Our 2021 roadmap can be seen
[there](https://www.dimforge.com/blog/2021/01/01/physics-simulation-with-rapier-2021-roadmap/#rapier-roadmap-for-2021).
We regularly give updates about our progress on [our blog](https://www.dimforge.com/blog).
## Getting started
The easiest way to get started with Rapier is to:
1. Read the [user-guides](https://www.rapier.rs/docs/).
2. Play with the examples: `cargo run --release --bin all_examples2` and `cargo run --release --bin all_examples3`.
Their source code are available on the `examples2d/` and `examples3d/` directory.
3. Don't hesitate to ask for help on [Discord](https://discord.gg/vt9DJSW), or by opening an issue on GitHub.
## Resources and discussions
- [Dimforge](https://dimforge.com): See all the open-source projects we are working on! Follow our announcements
on our [blog](https://www.dimforge.com/blog).
- [User guide](https://www.rapier.rs/docs/): Learn to use Rapier in your project by reading the official User Guides.
@@ -63,23 +67,3 @@ The easiest way to get started with Rapier is to:
Please make sure to familiarize yourself with our [Code of Conduct](CODE_OF_CONDUCT.md)
and our [Contribution Guidelines](CONTRIBUTING.md) before contributing or participating in
discussions with the community.
## Acknowledgements
Rapier is supported by our **platinum** sponsors:
<p>
<a href="https://embark-studios.com">
<img src="https://www.embark.dev/img/logo_black.png" width="301px">
</a>
</p>
And our gold sponsors:
<p>
<a href="https://fragcolor.com">
<img src="https://dimforge.com/img/fragcolor_logo2_color_black.svg" width="300px">
</a>
<a href="https://resolutiongames.com/">
<img src="https://dimforge.com/img/logo_resolution_games.png" width="300px" />
</a>
</p>

4
benchmarks2d/all_benchmarks2.rs
View File

@@ -17,6 +17,7 @@ mod joint_ball2;
mod joint_fixed2;
mod joint_prismatic2;
mod pyramid2;
mod vertical_stacks2;
fn demo_name_from_command_line() -> Option<String> {
let mut args = std::env::args();
@@ -57,6 +58,7 @@ pub fn main() {
("Convex polygons", convex_polygons2::init_world),
("Heightfield", heightfield2::init_world),
("Pyramid", pyramid2::init_world),
("Verticals stacks", vertical_stacks2::init_world),
("(Stress test) joint ball", joint_ball2::init_world),
("(Stress test) joint fixed", joint_fixed2::init_world),
(
@@ -66,7 +68,7 @@ pub fn main() {
];
// Lexicographic sort, with stress tests moved at the end of the list.
builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with(')')) {
builders.sort_by(|a, b| match (a.0.starts_with('('), b.0.starts_with('(')) {
(true, true) | (false, false) => a.0.cmp(b.0),
(true, false) => Ordering::Greater,
(false, true) => Ordering::Less,

61
benchmarks2d/vertical_stacks2.rs Normal file
View File

@@ -0,0 +1,61 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let num = 80;
let rad = 0.5;
/*
* Ground
*/
let ground_size = num as f32 * rad * 10.0;
let ground_thickness = 1.0;
let rigid_body = RigidBodyBuilder::fixed();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
let shiftx_centerx = [
(rad * 2.0 + 0.0002, -(num as f32) * rad * 2.0 * 1.5),
(rad * 2.0 + rad, num as f32 * rad * 2.0 * 1.5),
];
for (shiftx, centerx) in shiftx_centerx {
let shifty = rad * 2.0;
let centery = shifty / 2.0 + ground_thickness;
for i in 0..num {
for j in 0usize..1 + i * 2 {
let fj = j as f32;
let fi = i as f32;
let x = (fj - fi) * shiftx + centerx;
let y = (num as f32 - fi - 1.0) * shifty + centery;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 5.0);
}

4
benchmarks3d/all_benchmarks3.rs
View File

@@ -20,6 +20,8 @@ mod joint_fixed3;
mod joint_prismatic3;
mod joint_revolute3;
mod keva3;
mod many_pyramids3;
mod many_sleep3;
mod many_static3;
mod pyramid3;
mod stacks3;
@@ -56,6 +58,7 @@ pub fn main() {
("Compound", compound3::init_world),
("Convex polyhedron", convex_polyhedron3::init_world),
("Many static", many_static3::init_world),
("Many sleep", many_sleep3::init_world),
("Heightfield", heightfield3::init_world),
("Stacks", stacks3::init_world),
("Pyramid", pyramid3::init_world),
@@ -64,6 +67,7 @@ pub fn main() {
("ImpulseJoint fixed", joint_fixed3::init_world),
("ImpulseJoint revolute", joint_revolute3::init_world),
("ImpulseJoint prismatic", joint_prismatic3::init_world),
("Many pyramids", many_pyramids3::init_world),
("Keva tower", keva3::init_world),
];

4
benchmarks3d/ccd3.rs
View File

@@ -56,8 +56,8 @@ pub fn init_world(testbed: &mut Testbed) {
1 => ColliderBuilder::ball(rad),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
// 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
// 3 => ColliderBuilder::cone(rad, rad),
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
3 => ColliderBuilder::cone(rad, rad),
_ => ColliderBuilder::capsule_y(rad, rad),
};

80
benchmarks3d/many_pyramids3.rs Normal file
View File

@@ -0,0 +1,80 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
fn create_pyramid(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
offset: Vector<f32>,
stack_height: usize,
rad: f32,
) {
let shift = rad * 2.0;
for i in 0usize..stack_height {
for j in i..stack_height {
let fj = j as f32;
let fi = i as f32;
let x = (fi * shift / 2.0) + (fj - fi) * shift;
let y = fi * shift;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, 0.0] + offset);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, handle, bodies);
}
}
}
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let rad = 0.5;
let pyramid_count = 40;
let spacing = 4.0;
/*
* Ground
*/
let ground_size = 50.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(
ground_size,
ground_height,
pyramid_count as f32 * spacing / 2.0 + ground_size,
);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
for pyramid_index in 0..pyramid_count {
let bottomy = rad;
create_pyramid(
&mut bodies,
&mut colliders,
vector![
0.0,
bottomy,
(pyramid_index as f32 - pyramid_count as f32 / 2.0) * spacing
],
20,
rad,
);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
}

54
benchmarks3d/many_sleep3.rs Normal file
View File

@@ -0,0 +1,54 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Create the balls
*/
let num = 50;
let rad = 1.0;
let shift = rad * 2.0 + 1.0;
let centerx = shift * (num as f32) / 2.0;
let centery = shift / 2.0;
let centerz = shift * (num as f32) / 2.0;
for i in 0..num {
for j in 0usize..num {
for k in 0..num {
let x = i as f32 * shift - centerx;
let y = j as f32 * shift + centery;
let z = k as f32 * shift - centerz;
let status = if j == 0 {
RigidBodyType::Fixed
} else {
RigidBodyType::Dynamic
};
let density = 0.477;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![x, y, z])
.sleeping(true); // j < num - 1);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad).density(density);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
}

48
crates/rapier2d-f64/Cargo.toml
View File

@@ -1,14 +1,14 @@
[package]
name = "rapier2d-f64"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier2d-f64"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "2-dimensional physics engine in Rust."
documentation = "https://docs.rs/rapier2d"
homepage = "https://rapier.rs"
repository = "https://github.com/dimforge/rapier"
readme = "README.md"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,23 +16,23 @@ edition = "2021"
maintenance = { status = "actively-developed" }
[features]
default = [ "dim2", "f64" ]
dim2 = [ ]
f64 = [ ]
parallel = [ "rayon" ]
simd-stable = [ "simba/wide", "simd-is-enabled" ]
simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
default = ["dim2", "f64"]
dim2 = []
f64 = []
parallel = ["rayon"]
simd-stable = ["simba/wide", "simd-is-enabled"]
simd-nightly = ["simba/packed_simd", "simd-is-enabled"]
# Do not enable this feature directly. It is automatically
# enabled with the "simd-stable" or "simd-nightly" feature.
simd-is-enabled = [ "vec_map" ]
wasm-bindgen = [ "instant/wasm-bindgen" ]
serde-serialize = [ "nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ]
enhanced-determinism = [ "simba/libm_force", "parry2d-f64/enhanced-determinism" ]
debug-render = [ ]
profiler = [ "instant" ] # Enables the internal profiler.
simd-is-enabled = ["vec_map"]
wasm-bindgen = ["instant/wasm-bindgen"]
serde-serialize = ["nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde"]
enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"]
debug-render = []
profiler = ["instant"] # Enables the internal profiler.
# Feature used for debugging only.
debug-disable-legitimate-fe-exceptions = [ ]
debug-disable-legitimate-fe-exceptions = []
# Feature used for development and debugging only.
# Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
[lib]
name = "rapier2d_f64"
path = "../../src/lib.rs"
required-features = [ "dim2", "f64" ]
required-features = ["dim2", "f64"]
[dependencies]
vec_map = { version = "0.8", optional = true }
instant = { version = "0.1", features = [ "now" ], optional = true }
instant = { version = "0.1", features = ["now"], optional = true }
num-traits = "0.2"
nalgebra = "0.32"
parry2d-f64 = "0.13.1"
parry2d-f64 = "0.15.0"
simba = "0.8"
approx = "0.5"
rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
arrayvec = "0.7"
bit-vec = "0.6"
rustc-hash = "1"
serde = { version = "1", features = [ "derive" ], optional = true }
serde = { version = "1", features = ["derive"], optional = true }
downcast-rs = "1.2"
num-derive = "0.4"
bitflags = "1"
log = "0.4"
ordered-float = "4"
[dev-dependencies]
bincode = "1"
serde = { version = "1", features = [ "derive" ] }
serde = { version = "1", features = ["derive"] }

48
crates/rapier2d/Cargo.toml
View File

@@ -1,14 +1,14 @@
[package]
name = "rapier2d"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier2d"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "2-dimensional physics engine in Rust."
documentation = "https://docs.rs/rapier2d"
homepage = "https://rapier.rs"
repository = "https://github.com/dimforge/rapier"
readme = "README.md"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,23 +16,23 @@ edition = "2021"
maintenance = { status = "actively-developed" }
[features]
default = [ "dim2", "f32" ]
dim2 = [ ]
f32 = [ ]
parallel = [ "rayon" ]
simd-stable = [ "simba/wide", "simd-is-enabled" ]
simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
default = ["dim2", "f32"]
dim2 = []
f32 = []
parallel = ["rayon"]
simd-stable = ["simba/wide", "simd-is-enabled"]
simd-nightly = ["simba/packed_simd", "simd-is-enabled"]
# Do not enable this feature directly. It is automatically
# enabled with the "simd-stable" or "simd-nightly" feature.
simd-is-enabled = [ "vec_map" ]
wasm-bindgen = [ "instant/wasm-bindgen" ]
serde-serialize = [ "nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde" ]
enhanced-determinism = [ "simba/libm_force", "parry2d/enhanced-determinism" ]
debug-render = [ ]
profiler = [ "instant" ] # Enables the internal profiler.
simd-is-enabled = ["vec_map"]
wasm-bindgen = ["instant/wasm-bindgen"]
serde-serialize = ["nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "bit-vec/serde", "arrayvec/serde"]
enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"]
debug-render = []
profiler = ["instant"] # Enables the internal profiler.
# Feature used for debugging only.
debug-disable-legitimate-fe-exceptions = [ ]
debug-disable-legitimate-fe-exceptions = []
# Feature used for development and debugging only.
# Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
[lib]
name = "rapier2d"
path = "../../src/lib.rs"
required-features = [ "dim2", "f32" ]
required-features = ["dim2", "f32"]
[dependencies]
vec_map = { version = "0.8", optional = true }
instant = { version = "0.1", features = [ "now" ], optional = true }
instant = { version = "0.1", features = ["now"], optional = true }
num-traits = "0.2"
nalgebra = "0.32"
parry2d = "0.13.1"
parry2d = "0.15.0"
simba = "0.8"
approx = "0.5"
rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
arrayvec = "0.7"
bit-vec = "0.6"
rustc-hash = "1"
serde = { version = "1", features = [ "derive" ], optional = true }
serde = { version = "1", features = ["derive"], optional = true }
downcast-rs = "1.2"
num-derive = "0.4"
bitflags = "1"
log = "0.4"
ordered-float = "4"
[dev-dependencies]
bincode = "1"
serde = { version = "1", features = [ "derive" ] }
serde = { version = "1", features = ["derive"] }

46
crates/rapier3d-f64/Cargo.toml
View File

@@ -1,14 +1,14 @@
[package]
name = "rapier3d-f64"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier3d-f64"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "3-dimensional physics engine in Rust."
documentation = "https://docs.rs/rapier3d"
homepage = "https://rapier.rs"
repository = "https://github.com/dimforge/rapier"
readme = "README.md"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,23 +16,23 @@ edition = "2021"
maintenance = { status = "actively-developed" }
[features]
default = [ "dim3", "f64" ]
dim3 = [ ]
f64 = [ ]
parallel = [ "rayon" ]
simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ]
simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
default = ["dim3", "f64"]
dim3 = []
f64 = []
parallel = ["rayon"]
simd-stable = ["parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled"]
simd-nightly = ["parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled"]
# Do not enable this feature directly. It is automatically
# enabled with the "simd-stable" or "simd-nightly" feature.
simd-is-enabled = [ "vec_map" ]
wasm-bindgen = [ "instant/wasm-bindgen" ]
serde-serialize = [ "nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "serde", "bit-vec/serde" ]
enhanced-determinism = [ "simba/libm_force", "parry3d-f64/enhanced-determinism" ]
simd-is-enabled = ["vec_map"]
wasm-bindgen = ["instant/wasm-bindgen"]
serde-serialize = ["nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "serde", "bit-vec/serde"]
enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"]
debug-render = []
profiler = [ "instant" ] # Enables the internal profiler.
profiler = ["instant"] # Enables the internal profiler.
# Feature used for debugging only.
debug-disable-legitimate-fe-exceptions = [ ]
debug-disable-legitimate-fe-exceptions = []
# Feature used for development and debugging only.
# Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
[lib]
name = "rapier3d_f64"
path = "../../src/lib.rs"
required-features = [ "dim3", "f64" ]
required-features = ["dim3", "f64"]
[dependencies]
vec_map = { version = "0.8", optional = true }
instant = { version = "0.1", features = [ "now" ], optional = true }
instant = { version = "0.1", features = ["now"], optional = true }
num-traits = "0.2"
nalgebra = "0.32"
parry3d-f64 = "0.13.1"
parry3d-f64 = "0.15.0"
simba = "0.8"
approx = "0.5"
rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
arrayvec = "0.7"
bit-vec = "0.6"
rustc-hash = "1"
serde = { version = "1", features = [ "derive" ], optional = true }
serde = { version = "1", features = ["derive"], optional = true }
downcast-rs = "1.2"
num-derive = "0.4"
bitflags = "1"
log = "0.4"
ordered-float = "4"
[dev-dependencies]
bincode = "1"
serde = { version = "1", features = [ "derive" ] }
serde = { version = "1", features = ["derive"] }

48
crates/rapier3d/Cargo.toml
View File

@@ -1,14 +1,14 @@
[package]
name = "rapier3d"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier3d"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "3-dimensional physics engine in Rust."
documentation = "https://docs.rs/rapier3d"
homepage = "https://rapier.rs"
repository = "https://github.com/dimforge/rapier"
readme = "README.md"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,23 +16,23 @@ edition = "2021"
maintenance = { status = "actively-developed" }
[features]
default = [ "dim3", "f32" ]
dim3 = [ ]
f32 = [ ]
parallel = [ "rayon" ]
simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ]
simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
default = ["dim3", "f32"]
dim3 = []
f32 = []
parallel = ["rayon"]
simd-stable = ["parry3d/simd-stable", "simba/wide", "simd-is-enabled"]
simd-nightly = ["parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled"]
# Do not enable this feature directly. It is automatically
# enabled with the "simd-stable" or "simd-nightly" feature.
simd-is-enabled = [ "vec_map" ]
wasm-bindgen = [ "instant/wasm-bindgen" ]
serde-serialize = [ "nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde" ]
enhanced-determinism = [ "simba/libm_force", "parry3d/enhanced-determinism" ]
debug-render = [ ]
profiler = [ "instant" ] # Enables the internal profiler.
simd-is-enabled = ["vec_map"]
wasm-bindgen = ["instant/wasm-bindgen"]
serde-serialize = ["nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "bit-vec/serde"]
enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"]
debug-render = []
profiler = ["instant"] # Enables the internal profiler.
# Feature used for debugging only.
debug-disable-legitimate-fe-exceptions = [ ]
debug-disable-legitimate-fe-exceptions = []
# Feature used for development and debugging only.
# Do not enable this unless you are working on the engine internals.
@@ -44,15 +44,15 @@ features = ["parallel", "simd-stable", "serde-serialize", "debug-render"]
[lib]
name = "rapier3d"
path = "../../src/lib.rs"
required-features = [ "dim3", "f32" ]
required-features = ["dim3", "f32"]
[dependencies]
vec_map = { version = "0.8", optional = true }
instant = { version = "0.1", features = [ "now" ], optional = true }
instant = { version = "0.1", features = ["now"], optional = true }
num-traits = "0.2"
nalgebra = "0.32"
parry3d = "0.13.1"
parry3d = "0.15.0"
simba = "0.8"
approx = "0.5"
rayon = { version = "1", optional = true }
@@ -60,11 +60,13 @@ crossbeam = "0.8"
arrayvec = "0.7"
bit-vec = "0.6"
rustc-hash = "1"
serde = { version = "1", features = [ "derive" ], optional = true }
serde = { version = "1", features = ["derive"], optional = true }
downcast-rs = "1.2"
num-derive = "0.4"
bitflags = "1"
log = "0.4"
ordered-float = "4"
[dev-dependencies]
bincode = "1"
serde = { version = "1", features = [ "derive" ] }
serde = { version = "1", features = ["derive"] }

54
crates/rapier_testbed2d-f64/Cargo.toml
View File

@@ -1,12 +1,12 @@
[package]
name = "rapier_testbed2d-f64"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier_testbed2d-f64"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
homepage = "http://rapier.org"
repository = "https://github.com/dimforge/rapier"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,48 +16,48 @@ maintenance = { status = "actively-developed" }
[lib]
name = "rapier_testbed2d"
path = "../../src_testbed/lib.rs"
required-features = [ "dim2" ]
required-features = ["dim2"]
[features]
default = [ "dim2" ]
dim2 = [ ]
parallel = [ "rapier/parallel", "num_cpus" ]
other-backends = [ "wrapped2d" ]
default = ["dim2"]
dim2 = []
parallel = ["rapier/parallel", "num_cpus"]
other-backends = ["wrapped2d"]
[package.metadata.docs.rs]
features = ["parallel", "other-backends"]
[dependencies]
nalgebra = { version = "0.32", features = [ "rand" ] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
crossbeam = "0.8"
bincode = "1"
Inflector = "0.11"
Inflector = "0.11"
md5 = "0.7"
bevy_egui = "0.23"
bevy_ecs = "0.12"
bevy_core_pipeline = "0.12"
bevy_pbr = "0.12"
bevy_sprite = "0.12"
bevy_egui = "0.26"
bevy_ecs = "0.13"
bevy_core_pipeline = "0.13"
bevy_pbr = "0.13"
bevy_sprite = "0.13"
#bevy_prototype_debug_lines = "0.7"
# Dependencies for native only.
[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_asset", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_asset", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
# Dependencies for WASM only.
[target.'cfg(target_arch = "wasm32")'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_asset", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_asset", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
#bevy_webgl2 = "0.5"
[dependencies.rapier]
package = "rapier2d-f64"
path = "../rapier2d-f64"
version = "0.18.0"
features = [ "serde-serialize", "debug-render", "profiler" ]
version = "0.19.0"
features = ["serde-serialize", "debug-render", "profiler"]

54
crates/rapier_testbed2d/Cargo.toml
View File

@@ -1,12 +1,12 @@
[package]
name = "rapier_testbed2d"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier_testbed2d"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
homepage = "http://rapier.org"
repository = "https://github.com/dimforge/rapier"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,48 +16,48 @@ maintenance = { status = "actively-developed" }
[lib]
name = "rapier_testbed2d"
path = "../../src_testbed/lib.rs"
required-features = [ "dim2" ]
required-features = ["dim2"]
[features]
default = [ "dim2" ]
dim2 = [ ]
parallel = [ "rapier/parallel", "num_cpus" ]
other-backends = [ "wrapped2d" ]
default = ["dim2"]
dim2 = []
parallel = ["rapier/parallel", "num_cpus"]
other-backends = ["wrapped2d"]
[package.metadata.docs.rs]
features = ["parallel", "other-backends"]
[dependencies]
nalgebra = { version = "0.32", features = [ "rand" ] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
crossbeam = "0.8"
bincode = "1"
Inflector = "0.11"
Inflector = "0.11"
md5 = "0.7"
bevy_egui = "0.23"
bevy_ecs = "0.12"
bevy_core_pipeline = "0.12"
bevy_pbr = "0.12"
bevy_sprite = "0.12"
bevy_egui = "0.26"
bevy_ecs = "0.13"
bevy_core_pipeline = "0.13"
bevy_pbr = "0.13"
bevy_sprite = "0.13"
#bevy_prototype_debug_lines = "0.7"
# Dependencies for native only.
[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_sprite", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_sprite", "bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
# Dependencies for WASM only.
[target.'cfg(target_arch = "wasm32")'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_sprite", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_sprite", "bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
#bevy_webgl2 = "0.5"
[dependencies.rapier]
package = "rapier2d"
path = "../rapier2d"
version = "0.18.0"
features = [ "serde-serialize", "debug-render", "profiler" ]
version = "0.19.0"
features = ["serde-serialize", "debug-render", "profiler"]

52
crates/rapier_testbed3d-f64/Cargo.toml
View File

@@ -1,12 +1,12 @@
[package]
name = "rapier_testbed3d-f64"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier_testbed3d-f64"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "Testbed for the Rapier 3-dimensional physics engine in Rust."
homepage = "http://rapier.org"
repository = "https://github.com/dimforge/rapier"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,47 +16,47 @@ maintenance = { status = "actively-developed" }
[lib]
name = "rapier_testbed3d"
path = "../../src_testbed/lib.rs"
required-features = [ "dim3" ]
required-features = ["dim3"]
[features]
default = [ "dim3" ]
dim3 = [ ]
parallel = [ "rapier/parallel", "num_cpus" ]
default = ["dim3"]
dim3 = []
parallel = ["rapier/parallel", "num_cpus"]
[package.metadata.docs.rs]
features = ["parallel"]
[dependencies]
nalgebra = { version = "0.32", features = [ "rand" ] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }
bitflags = "1"
num_cpus = { version = "1", optional = true }
crossbeam = "0.8"
bincode = "1"
md5 = "0.7"
Inflector = "0.11"
serde = { version = "1", features = [ "derive" ] }
Inflector = "0.11"
serde = { version = "1", features = ["derive"] }
bevy_egui = "0.23"
bevy_ecs = "0.12"
bevy_core_pipeline = "0.12"
bevy_pbr = "0.12"
bevy_sprite = "0.12"
bevy_egui = "0.26"
bevy_ecs = "0.13"
bevy_core_pipeline = "0.13"
bevy_pbr = "0.13"
bevy_sprite = "0.13"
#bevy_prototype_debug_lines = { version = "0.7", features = [ "3d" ] }
# Dependencies for native only.
[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
# Dependencies for WASM only.
[target.'cfg(target_arch = "wasm32")'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
#bevy_webgl2 = "0.5"
[dependencies.rapier]
package = "rapier3d-f64"
path = "../rapier3d-f64"
version = "0.18.0"
features = [ "serde-serialize", "debug-render", "profiler" ]
version = "0.19.0"
features = ["serde-serialize", "debug-render", "profiler"]

58
crates/rapier_testbed3d/Cargo.toml
View File

@@ -1,12 +1,12 @@
[package]
name = "rapier_testbed3d"
version = "0.18.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
name = "rapier_testbed3d"
version = "0.19.0"
authors = ["Sébastien Crozet <developer@crozet.re>"]
description = "Testbed for the Rapier 3-dimensional physics engine in Rust."
homepage = "http://rapier.org"
repository = "https://github.com/dimforge/rapier"
categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
categories = ["science", "game-development", "mathematics", "simulation", "wasm"]
keywords = ["physics", "dynamics", "rigid", "real-time", "impulse_joints"]
license = "Apache-2.0"
edition = "2021"
@@ -16,51 +16,51 @@ maintenance = { status = "actively-developed" }
[lib]
name = "rapier_testbed3d"
path = "../../src_testbed/lib.rs"
required-features = [ "dim3" ]
required-features = ["dim3"]
[features]
default = [ "dim3" ]
dim3 = [ ]
parallel = [ "rapier/parallel", "num_cpus" ]
other-backends = [ "physx", "physx-sys", "glam" ]
default = ["dim3"]
dim3 = []
parallel = ["rapier/parallel", "num_cpus"]
other-backends = ["physx", "physx-sys", "glam"]
[package.metadata.docs.rs]
features = ["parallel", "other-backends"]
[dependencies]
nalgebra = { version = "0.32", features = [ "rand" ] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
glam = { version = "0.24", optional = true } # For Physx
num_cpus = { version = "1", optional = true }
physx = { version = "0.19", features = [ "glam" ], optional = true }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }
bitflags = "1"
glam = { version = "0.24", optional = true } # For Physx
num_cpus = { version = "1", optional = true }
physx = { version = "0.19", features = ["glam"], optional = true }
physx-sys = { version = "0.11", optional = true }
crossbeam = "0.8"
bincode = "1"
md5 = "0.7"
Inflector = "0.11"
serde = { version = "1", features = [ "derive" ] }
Inflector = "0.11"
serde = { version = "1", features = ["derive"] }
bevy_egui = "0.23"
bevy_ecs = "0.12"
bevy_core_pipeline = "0.12"
bevy_pbr = "0.12"
bevy_sprite = "0.12"
bevy_egui = "0.26"
bevy_ecs = "0.13"
bevy_core_pipeline = "0.13"
bevy_pbr = "0.13"
bevy_sprite = "0.13"
#bevy_prototype_debug_lines = { version = "0.7", features = [ "3d" ] }
# Dependencies for native only.
[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "x11", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
# Dependencies for WASM only.
[target.'cfg(target_arch = "wasm32")'.dependencies]
bevy = {version = "0.12", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"]}
bevy = { version = "0.13", default-features = false, features = ["bevy_winit", "tonemapping_luts", "ktx2", "zstd", "bevy_render", "bevy_pbr", "bevy_gizmos"] }
#bevy_webgl2 = "0.5"
[dependencies.rapier]
package = "rapier3d"
path = "../rapier3d"
version = "0.18.0"
features = [ "serde-serialize", "debug-render", "profiler" ]
version = "0.19.0"
features = ["serde-serialize", "debug-render", "profiler"]

33
examples2d/all_examples2.rs
View File

@@ -15,8 +15,12 @@ mod collision_groups2;
mod convex_polygons2;
mod damping2;
mod debug_box_ball2;
mod debug_compression2;
mod debug_total_overlap2;
mod debug_vertical_column2;
mod drum2;
mod heightfield2;
mod inverse_kinematics2;
mod joint_motor_position2;
mod joints2;
mod locked_rotations2;
@@ -26,6 +30,17 @@ mod polyline2;
mod pyramid2;
mod restitution2;
mod rope_joints2;
mod s2d_arch;
mod s2d_ball_and_chain;
mod s2d_bridge;
mod s2d_card_house;
mod s2d_confined;
mod s2d_far_pyramid;
mod s2d_high_mass_ratio_1;
mod s2d_high_mass_ratio_2;
mod s2d_high_mass_ratio_3;
mod s2d_joint_grid;
mod s2d_pyramid;
mod sensor2;
mod trimesh2;
@@ -70,6 +85,7 @@ pub fn main() {
("Damping", damping2::init_world),
("Drum", drum2::init_world),
("Heightfield", heightfield2::init_world),
("Inverse kinematics", inverse_kinematics2::init_world),
("Joints", joints2::init_world),
("Locked rotations", locked_rotations2::init_world),
("One-way platforms", one_way_platforms2::init_world),
@@ -82,6 +98,23 @@ pub fn main() {
("Trimesh", trimesh2::init_world),
("Joint motor position", joint_motor_position2::init_world),
("(Debug) box ball", debug_box_ball2::init_world),
("(Debug) compression", debug_compression2::init_world),
("(Debug) total overlap", debug_total_overlap2::init_world),
(
"(Debug) vertical column",
debug_vertical_column2::init_world,
),
("(s2d) high mass ratio 1", s2d_high_mass_ratio_1::init_world),
("(s2d) high mass ratio 2", s2d_high_mass_ratio_2::init_world),
("(s2d) high mass ratio 3", s2d_high_mass_ratio_3::init_world),
("(s2d) confined", s2d_confined::init_world),
("(s2d) pyramid", s2d_pyramid::init_world),
("(s2d) card house", s2d_card_house::init_world),
("(s2d) arch", s2d_arch::init_world),
("(s2d) bridge", s2d_bridge::init_world),
("(s2d) ball and chain", s2d_ball_and_chain::init_world),
("(s2d) joint grid", s2d_joint_grid::init_world),
("(s2d) far pyramid", s2d_far_pyramid::init_world),
];
// Lexicographic sort, with stress tests moved at the end of the list.

14
examples2d/character_controller2.rs
View File

@@ -27,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0]);
let character_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.15, 0.3);
let collider = ColliderBuilder::capsule_y(0.3, 0.15);
colliders.insert_with_parent(collider, character_handle, &mut bodies);
/*
@@ -94,14 +94,18 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let wall_angle = PI / 2.;
let wall_size = 2.0;
let wall_pos = vector![
ground_size + slope_size * 2.0 + impossible_slope_size + 0.35,
-ground_height + 2.5 * 2.3
];
let collider = ColliderBuilder::cuboid(wall_size, ground_height)
.translation(vector![
ground_size + slope_size * 2.0 + impossible_slope_size + 0.35,
-ground_height + 2.5 * 2.3
])
.translation(wall_pos)
.rotation(wall_angle);
colliders.insert(collider);
let collider = ColliderBuilder::cuboid(wall_size, ground_height).translation(wall_pos);
colliders.insert(collider);
/*
* Create a moving platform.
*/

75
examples2d/debug_compression2.rs Normal file
View File

@@ -0,0 +1,75 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let width = 75.0;
let thickness = 2.0;
let ys = [-30.0 - thickness, 30.0 + thickness];
for y in ys {
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(width, thickness);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
// Build two compression boxes rigid body.
let half_height = (ys[1] - ys[0]) / 2.0 - thickness;
let xs = [-width + thickness, width - thickness];
let mut handles = [RigidBodyHandle::invalid(); 2];
for i in 0..2 {
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![xs[i], 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(thickness, half_height);
colliders.insert_with_parent(collider, handle, &mut bodies);
handles[i] = handle;
}
// Build the balls.
let num = 8;
let rad = half_height / (num as f32);
for i in 0..num {
for j in 0..num {
let x = i as f32 * rad * 2.0 - num as f32 * rad;
let y = j as f32 * rad * 2.0 - num as f32 * rad + rad;
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
let mut force = vector![0.0, 0.0];
testbed.add_callback(move |_, physics, _, _| {
let left_plank = &mut physics.bodies[handles[0]];
left_plank.reset_forces(true);
left_plank.add_force(force, true);
let right_plank = &mut physics.bodies[handles[1]];
right_plank.reset_forces(true);
right_plank.add_force(-force, true);
force.x += 10000.0;
println!("force: {}", force.x);
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 0.0], 50.0);
}

28
examples2d/debug_total_overlap2.rs Normal file
View File

@@ -0,0 +1,28 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
// Build many balls, all spawned at the same point.
let rad = 0.5;
for _ in 0..100 {
let rigid_body = RigidBodyBuilder::dynamic();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 0.0], 50.0);
}

47
examples2d/debug_vertical_column2.rs Normal file
View File

@@ -0,0 +1,47 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let num = 80;
let rad = 0.5;
/*
* Ground
*/
let ground_size = 1.0;
let ground_thickness = 1.0;
let rigid_body = RigidBodyBuilder::fixed();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).friction(0.3);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
for i in 0..num {
let y = i as f32 * rad * 2.0 + ground_thickness + rad;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).friction(0.3);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
// testbed.harness_mut().physics.gravity.y = -981.0;
testbed.look_at(point![0.0, 2.5], 5.0);
}

96
examples2d/inverse_kinematics2.rs Normal file
View File

@@ -0,0 +1,96 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 1.0;
let ground_height = 0.01;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]);
let floor_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height);
colliders.insert_with_parent(collider, floor_handle, &mut bodies);
/*
* Setup groups
*/
let num_segments = 10;
let body = RigidBodyBuilder::fixed();
let mut last_body = bodies.insert(body);
let mut last_link = MultibodyJointHandle::invalid();
for i in 0..num_segments {
let size = 1.0 / num_segments as f32;
let body = RigidBodyBuilder::dynamic().can_sleep(false);
let new_body = bodies.insert(body);
// NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
// the demo look nicer. IK could be used without cuboid.
let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0)
.density(0.0)
.sensor(true);
colliders.insert_with_parent(collider, new_body, &mut bodies);
let link_ab = RevoluteJointBuilder::new()
.local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32])
.local_anchor2(point![0.0, -size / 2.0])
.build()
.data;
last_link = multibody_joints
.insert(last_body, new_body, link_ab, true)
.unwrap();
last_body = new_body;
}
let mut displacements = DVector::zeros(0);
testbed.add_callback(move |graphics, physics, _, _| {
let Some(graphics) = graphics else { return };
if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
// Ensure our displacement vector has the right number of elements.
if displacements.nrows() < multibody.ndofs() {
displacements = DVector::zeros(multibody.ndofs());
} else {
displacements.fill(0.0);
}
let Some(mouse_point) = graphics.mouse().point else {
return;
};
// We will have the endpoint track the mouse position.
let target_point = mouse_point.coords;
let options = InverseKinematicsOption {
constrained_axes: JointAxesMask::LIN_AXES,
..Default::default()
};
multibody.inverse_kinematics(
&physics.bodies,
link_id,
&options,
&Isometry::from(target_point),
&mut displacements,
);
multibody.apply_displacements(displacements.as_slice());
}
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 0.0], 300.0);
}

109
examples2d/s2d_arch.rs Normal file
View File

@@ -0,0 +1,109 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
#[allow(clippy::excessive_precision)]
let mut ps1 = [
Point::new(16.0, 0.0),
Point::new(14.93803712795643, 5.133601056842984),
Point::new(13.79871746027416, 10.24928069555078),
Point::new(12.56252963284711, 15.34107019122473),
Point::new(11.20040987372525, 20.39856541571217),
Point::new(9.66521217819836, 25.40369899225096),
Point::new(7.87179930638133, 30.3179337000085),
Point::new(5.635199558196225, 35.03820717801641),
Point::new(2.405937953536585, 39.09554102558315),
];
#[allow(clippy::excessive_precision)]
let mut ps2 = [
Point::new(24.0, 0.0),
Point::new(22.33619528222415, 6.02299846205841),
Point::new(20.54936888969905, 12.00964361211476),
Point::new(18.60854610798073, 17.9470321677465),
Point::new(16.46769273811807, 23.81367936585418),
Point::new(14.05325025774858, 29.57079353071012),
Point::new(11.23551045834022, 35.13775818285372),
Point::new(7.752568160730571, 40.30450679009583),
Point::new(3.016931552701656, 44.28891593799322),
];
let scale = 0.25;
let friction = 0.6;
for i in 0..9 {
ps1[i] *= scale;
ps2[i] *= scale;
}
/*
* Ground
*/
let collider = ColliderBuilder::segment(point![-100.0, 0.0], point![100.0, 0.0]).friction(0.6);
colliders.insert(collider);
/*
* Create the arch
*/
for i in 0..8 {
let ps = [ps1[i], ps2[i], ps2[i + 1], ps1[i + 1]];
let rigid_body = RigidBodyBuilder::dynamic();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::convex_hull(&ps)
.unwrap()
.friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
}
for i in 0..8 {
let ps = [
point![-ps2[i].x, ps2[i].y],
point![-ps1[i].x, ps1[i].y],
point![-ps1[i + 1].x, ps1[i + 1].y],
point![-ps2[i + 1].x, ps2[i + 1].y],
];
let rigid_body = RigidBodyBuilder::dynamic();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::convex_hull(&ps)
.unwrap()
.friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
}
{
let ps = [
ps1[8],
ps2[8],
point![-ps1[8].x, ps1[8].y],
point![-ps2[8].x, ps2[8].y],
];
let rigid_body = RigidBodyBuilder::dynamic();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::convex_hull(&ps)
.unwrap()
.friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
}
for i in 0..4 {
let rigid_body =
RigidBodyBuilder::dynamic().translation(vector![0.0, 0.5 + ps2[8].y + 1.0 * i as f32]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(2.0, 0.5).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

73
examples2d/s2d_ball_and_chain.rs Normal file
View File

@@ -0,0 +1,73 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground = bodies.insert(RigidBodyBuilder::fixed());
/*
* Create the bridge.
*/
let count = 40;
let hx = 0.5;
let density = 20.0;
let friction = 0.6;
let capsule = ColliderBuilder::capsule_x(hx, 0.125)
.friction(friction)
.density(density);
let mut prev = ground;
for i in 0..count {
let rigid_body = RigidBodyBuilder::dynamic()
.linear_damping(0.1)
.angular_damping(0.1)
.translation(vector![(1.0 + 2.0 * i as f32) * hx, count as f32 * hx]);
let handle = bodies.insert(rigid_body);
colliders.insert_with_parent(capsule.clone(), handle, &mut bodies);
let pivot = point![(2.0 * i as f32) * hx, count as f32 * hx];
let joint = RevoluteJointBuilder::new()
.local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
.local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
.contacts_enabled(false);
impulse_joints.insert(prev, handle, joint, true);
prev = handle;
}
let radius = 8.0;
let rigid_body = RigidBodyBuilder::dynamic()
.linear_damping(0.1)
.angular_damping(0.1)
.translation(vector![
(1.0 + 2.0 * count as f32) * hx + radius - hx,
count as f32 * hx
]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(radius)
.friction(friction)
.density(density);
colliders.insert_with_parent(collider, handle, &mut bodies);
let pivot = point![(2.0 * count as f32) * hx, count as f32 * hx];
let joint = RevoluteJointBuilder::new()
.local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
.local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
.contacts_enabled(false);
impulse_joints.insert(prev, handle, joint, true);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

56
examples2d/s2d_bridge.rs Normal file
View File

@@ -0,0 +1,56 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground = bodies.insert(RigidBodyBuilder::fixed());
/*
* Create the bridge.
*/
let density = 20.0;
let x_base = -80.0;
let count = 160;
let mut prev = ground;
for i in 0..count {
let rigid_body = RigidBodyBuilder::dynamic()
.linear_damping(0.1)
.angular_damping(0.1)
.translation(vector![x_base + 0.5 + 1.0 * i as f32, 20.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.5, 0.125).density(density);
colliders.insert_with_parent(collider, handle, &mut bodies);
let pivot = point![x_base + 1.0 * i as f32, 20.0];
let joint = RevoluteJointBuilder::new()
.local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
.local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
.contacts_enabled(false);
impulse_joints.insert(prev, handle, joint, true);
prev = handle;
}
let pivot = point![x_base + 1.0 * count as f32, 20.0];
let joint = RevoluteJointBuilder::new()
.local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
.local_anchor2(bodies[ground].position().inverse_transform_point(&pivot))
.contacts_enabled(false);
impulse_joints.insert(prev, ground, joint, true);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

78
examples2d/s2d_card_house.rs Normal file
View File

@@ -0,0 +1,78 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let friction = 0.7;
/*
* Ground
*/
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
let scale = 10.0;
let card_height = 0.2 * scale;
let card_thickness = 0.001 * scale;
let angle0 = 25.0 * std::f32::consts::PI / 180.0;
let angle1 = -25.0 * std::f32::consts::PI / 180.0;
let angle2 = 0.5 * std::f32::consts::PI;
let card_box = ColliderBuilder::cuboid(card_thickness, card_height).friction(friction);
let mut nb = 5;
let mut z0 = 0.0;
let mut y = card_height - 0.02 * scale;
while nb != 0 {
let mut z = z0;
for i in 0..nb {
if i != nb - 1 {
let rigid_body = RigidBodyBuilder::dynamic()
.translation(vector![z + 0.25 * scale, y + card_height - 0.015 * scale])
.rotation(angle2);
let ground_handle = bodies.insert(rigid_body);
colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
}
let rigid_body = RigidBodyBuilder::dynamic()
.translation(vector![z, y])
.rotation(angle1);
let ground_handle = bodies.insert(rigid_body);
colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
z += 0.175 * scale;
let rigid_body = RigidBodyBuilder::dynamic()
.translation(vector![z, y])
.rotation(angle0);
let ground_handle = bodies.insert(rigid_body);
colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
z += 0.175 * scale;
}
y += card_height * 2.0 - 0.03 * scale;
z0 += 0.175 * scale;
nb -= 1;
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

69
examples2d/s2d_confined.rs Normal file
View File

@@ -0,0 +1,69 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let radius = 0.5;
let grid_count = 25;
let friction = 0.6;
let max_count = grid_count * grid_count;
/*
* Ground
*/
let collider =
ColliderBuilder::capsule_from_endpoints(point![-10.5, 0.0], point![10.5, 0.0], radius)
.friction(friction);
colliders.insert(collider);
let collider =
ColliderBuilder::capsule_from_endpoints(point![-10.5, 0.0], point![-10.5, 20.5], radius)
.friction(friction);
colliders.insert(collider);
let collider =
ColliderBuilder::capsule_from_endpoints(point![10.5, 0.0], point![10.5, 20.5], radius)
.friction(friction);
colliders.insert(collider);
let collider =
ColliderBuilder::capsule_from_endpoints(point![-10.5, 20.5], point![10.5, 20.5], radius)
.friction(friction);
colliders.insert(collider);
/*
* Create the spheres
*/
let mut row;
let mut count = 0;
let mut column = 0;
while count < max_count {
row = 0;
for _ in 0..grid_count {
let x = -8.75 + column as f32 * 18.0 / (grid_count as f32);
let y = 1.5 + row as f32 * 18.0 / (grid_count as f32);
let body = RigidBodyBuilder::dynamic()
.translation(vector![x, y])
.gravity_scale(0.0);
let body_handle = bodies.insert(body);
let ball = ColliderBuilder::ball(radius).friction(friction);
colliders.insert_with_parent(ball, body_handle, &mut bodies);
count += 1;
row += 1;
}
column += 1;
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

50
examples2d/s2d_far_pyramid.rs Normal file
View File

@@ -0,0 +1,50 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let origin = vector![100_000.0, -80_000.0];
let friction = 0.6;
/*
* Ground
*/
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0] + origin);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
let base_count = 10;
let h = 0.5;
let shift = 1.25 * h;
for i in 0..base_count {
let y = (2.0 * i as f32 + 1.0) * shift + 0.5;
for j in i..base_count {
let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift
- h * base_count as f32;
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y] + origin);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(h, h).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5] + origin, 20.0);
}

66
examples2d/s2d_high_mass_ratio_1.rs Normal file
View File

@@ -0,0 +1,66 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let extent = 1.0;
let friction = 0.5;
/*
* Ground
*/
let ground_width = 66.0 * extent;
let rigid_body = RigidBodyBuilder::fixed();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::segment(
point![-0.5 * 2.0 * ground_width, 0.0],
point![0.5 * 2.0 * ground_width, 0.0],
)
.friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
for j in 0..3 {
let mut count = 10;
let offset = -20.0 * extent + 2.0 * (count as f32 + 1.0) * extent * j as f32;
let mut y = extent;
while count > 0 {
for i in 0..count {
let coeff = i as f32 - 0.5 * count as f32;
let yy = if count == 1 { y + 2.0 } else { y };
let position = vector![2.0 * coeff * extent + offset, yy];
let rigid_body = RigidBodyBuilder::dynamic().translation(position);
let parent = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(extent, extent)
.density(if count == 1 {
(j as f32 + 1.0) * 100.0
} else {
1.0
})
.friction(friction);
colliders.insert_with_parent(collider, parent, &mut bodies);
}
count -= 1;
y += 2.0 * extent;
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

53
examples2d/s2d_high_mass_ratio_2.rs Normal file
View File

@@ -0,0 +1,53 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let extent = 1.0;
let friction = 0.6;
/*
* Ground
*/
let ground_width = 66.0 * extent;
let rigid_body = RigidBodyBuilder::fixed();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::segment(
point![-0.5 * 2.0 * ground_width, 0.0],
point![0.5 * 2.0 * ground_width, 0.0],
)
.friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

47
examples2d/s2d_high_mass_ratio_3.rs Normal file
View File

@@ -0,0 +1,47 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
let extent = 1.0;
let friction = 0.6;
/*
* Ground
*/
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

63
examples2d/s2d_joint_grid.rs Normal file
View File

@@ -0,0 +1,63 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Create the joint grid.
*/
let rad = 0.4;
let numi = 100;
let numk = 100;
let shift = 1.0;
let mut index = 0;
let mut handles = vec![RigidBodyHandle::invalid(); numi * numk];
for k in 0..numk {
for i in 0..numi {
let body_type = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 {
RigidBodyType::Fixed
} else {
RigidBodyType::Dynamic
};
let rigid_body = RigidBodyBuilder::new(body_type)
.translation(vector![k as f32 * shift, -(i as f32) * shift]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
if i > 0 {
let joint = RevoluteJointBuilder::new()
.local_anchor1(point![0.0, -0.5 * shift])
.local_anchor2(point![0.0, 0.5 * shift])
.contacts_enabled(false);
impulse_joints.insert(handles[index - 1], handle, joint, true);
}
if k > 0 {
let joint = RevoluteJointBuilder::new()
.local_anchor1(point![0.5 * shift, 0.0])
.local_anchor2(point![-0.5 * shift, 0.0])
.contacts_enabled(false);
impulse_joints.insert(handles[index - numi], handle, joint, true);
}
handles[index] = handle;
index += 1;
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

47
examples2d/s2d_pyramid.rs Normal file
View File

@@ -0,0 +1,47 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(0.6);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
* Create the cubes
*/
let base_count = 100;
let h = 0.5;
let shift = 1.0 * h;
for i in 0..base_count {
let y = (2.0 * i as f32 + 1.0) * shift;
for j in i..base_count {
let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift
- h * base_count as f32;
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(h, h).friction(0.6);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 2.5], 20.0);
}

7
examples2d/trimesh2.rs
View File

@@ -41,7 +41,7 @@ pub fn init_world(testbed: &mut Testbed) {
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
* Create the trimeshes from a tesselated SVG.
* Create the trimeshes from a tessellated SVG.
*/
let mut fill_tess = FillTessellator::new();
let opt = usvg::Options::default();
@@ -67,7 +67,7 @@ pub fn init_world(testbed: &mut Testbed) {
&FillOptions::tolerance(0.01),
&mut BuffersBuilder::new(&mut mesh, VertexCtor { prim_id: 0 }),
)
.expect("Tesselation failed.");
.expect("Tessellation failed.");
let angle = transform.get_rotate() as f32;
@@ -84,7 +84,8 @@ pub fn init_world(testbed: &mut Testbed) {
.collect();
for k in 0..5 {
let collider = ColliderBuilder::trimesh(vertices.clone(), indices.clone());
let collider = ColliderBuilder::trimesh(vertices.clone(), indices.clone())
.contact_skin(0.2);
let rigid_body = RigidBodyBuilder::dynamic()
.translation(vector![ith as f32 * 8.0 - 20.0, 20.0 + k as f32 * 11.0])
.rotation(angle);

2
examples3d-f64/debug_serialized3.rs
View File

@@ -4,7 +4,7 @@ use rapier_testbed3d::Testbed;
#[derive(serde::Deserialize)]
struct State {
pub islands: IslandManager,
pub broad_phase: BroadPhase,
pub broad_phase: DefaultBroadPhase,
pub narrow_phase: NarrowPhase,
pub bodies: RigidBodySet,
pub colliders: ColliderSet,

8
examples3d/all_examples3.rs
View File

@@ -23,12 +23,15 @@ mod debug_deserialize3;
mod debug_dynamic_collider_add3;
mod debug_friction3;
mod debug_infinite_fall3;
mod debug_pop3;
mod debug_prismatic3;
mod debug_rollback3;
mod debug_shape_modification3;
mod debug_thin_cube_on_mesh3;
mod debug_triangle3;
mod debug_trimesh3;
mod domino3;
mod dynamic_trimesh3;
mod fountain3;
mod heightfield3;
mod joints3;
@@ -39,6 +42,7 @@ mod debug_cube_high_mass_ratio3;
mod debug_internal_edges3;
mod debug_long_chain3;
mod debug_multibody_ang_motor_pos3;
mod inverse_kinematics3;
mod joint_motor_position3;
mod keva3;
mod locked_rotations3;
@@ -102,8 +106,10 @@ pub fn main() {
("Convex polyhedron", convex_polyhedron3::init_world),
("Damping", damping3::init_world),
("Domino", domino3::init_world),
("Dynamic trimeshes", dynamic_trimesh3::init_world),
("Heightfield", heightfield3::init_world),
("Impulse Joints", joints3::init_world_with_joints),
("Inverse kinematics", inverse_kinematics3::init_world),
("Joint Motor Position", joint_motor_position3::init_world),
("Locked rotations", locked_rotations3::init_world),
("One-way platforms", one_way_platforms3::init_world),
@@ -124,6 +130,7 @@ pub fn main() {
),
("(Debug) big colliders", debug_big_colliders3::init_world),
("(Debug) boxes", debug_boxes3::init_world),
("(Debug) pop", debug_pop3::init_world),
(
"(Debug) dyn. coll. add",
debug_dynamic_collider_add3::init_world,
@@ -141,6 +148,7 @@ pub fn main() {
),
("(Debug) triangle", debug_triangle3::init_world),
("(Debug) trimesh", debug_trimesh3::init_world),
("(Debug) thin cube", debug_thin_cube_on_mesh3::init_world),
("(Debug) cylinder", debug_cylinder3::init_world),
("(Debug) infinite fall", debug_infinite_fall3::init_world),
("(Debug) prismatic", debug_prismatic3::init_world),

77
examples3d/character_controller3.rs
View File

@@ -13,21 +13,37 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Ground
*/
let scale = 1.0; // Set to a larger value to check if it still works with larger units.
let ground_size = 5.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let rigid_body =
RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0] * scale);
let floor_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
let collider = ColliderBuilder::cuboid(
ground_size * scale,
ground_height * scale,
ground_size * scale,
);
colliders.insert_with_parent(collider, floor_handle, &mut bodies);
let rigid_body =
RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, -ground_size] * scale); //.rotation(vector![-0.1, 0.0, 0.0]);
let floor_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(
ground_size * scale,
ground_size * scale,
ground_height * scale,
);
colliders.insert_with_parent(collider, floor_handle, &mut bodies);
/*
* Character we will control manually.
*/
let rigid_body =
RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0, 0.0]);
RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0, 0.0] * scale);
let character_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(0.3, 0.15); // 0.15, 0.3, 0.15);
let collider = ColliderBuilder::capsule_y(0.3 * scale, 0.15 * scale); // 0.15, 0.3, 0.15);
colliders.insert_with_parent(collider, character_handle, &mut bodies);
/*
@@ -47,9 +63,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = j as f32 * shift + centery;
let z = k as f32 * shift + centerx;
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z] * scale);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad);
let collider = ColliderBuilder::cuboid(rad * scale, rad * scale, rad * scale);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
@@ -64,8 +80,12 @@ pub fn init_world(testbed: &mut Testbed) {
let x = i as f32 * stair_width / 2.0;
let y = i as f32 * stair_height * 1.5 + 3.0;
let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0, stair_width)
.translation(vector![x, y, 0.0]);
let collider = ColliderBuilder::cuboid(
stair_width / 2.0 * scale,
stair_height / 2.0 * scale,
stair_width * scale,
)
.translation(vector![x * scale, y * scale, 0.0]);
colliders.insert(collider);
}
@@ -74,9 +94,13 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let slope_angle = 0.2;
let slope_size = 2.0;
let collider = ColliderBuilder::cuboid(slope_size, ground_height, slope_size)
.translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0])
.rotation(Vector::z() * slope_angle);
let collider = ColliderBuilder::cuboid(
slope_size * scale,
ground_height * scale,
slope_size * scale,
)
.translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0] * scale)
.rotation(Vector::z() * slope_angle);
colliders.insert(collider);
/*
@@ -84,22 +108,29 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let impossible_slope_angle = 0.9;
let impossible_slope_size = 2.0;
let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size)
.translation(vector![
let collider = ColliderBuilder::cuboid(
slope_size * scale,
ground_height * scale,
ground_size * scale,
)
.translation(
vector![
ground_size + slope_size * 2.0 + impossible_slope_size - 0.9,
-ground_height + 2.3,
0.0
])
.rotation(Vector::z() * impossible_slope_angle);
] * scale,
)
.rotation(Vector::z() * impossible_slope_angle);
colliders.insert(collider);
/*
* Create a moving platform.
*/
let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]);
let body =
RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0] * scale);
// .rotation(-0.3);
let platform_handle = bodies.insert(body);
let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0);
let collider = ColliderBuilder::cuboid(2.0 * scale, ground_height * scale, 2.0 * scale);
colliders.insert_with_parent(collider, platform_handle, &mut bodies);
/*
@@ -113,18 +144,18 @@ pub fn init_world(testbed: &mut Testbed) {
+ (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos()
});
let collider =
ColliderBuilder::heightfield(heights, ground_size).translation(vector![-8.0, 5.0, 0.0]);
let collider = ColliderBuilder::heightfield(heights, ground_size * scale)
.translation(vector![-8.0, 5.0, 0.0] * scale);
colliders.insert(collider);
/*
* A tilting dynamic body with a limited joint.
*/
let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]);
let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0] * scale);
let ground_handle = bodies.insert(ground);
let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]);
let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0] * scale);
let handle = bodies.insert(body);
let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0);
let collider = ColliderBuilder::cuboid(1.0 * scale, 0.1 * scale, 2.0 * scale);
colliders.insert_with_parent(collider, handle, &mut bodies);
let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]);
impulse_joints.insert(ground_handle, handle, joint, true);
@@ -137,7 +168,7 @@ pub fn init_world(testbed: &mut Testbed) {
(run_state.time * 2.0).sin() * 2.0,
(run_state.time * 5.0).sin() * 1.5,
0.0
];
] * scale;
// let angvel = run_state.time.sin() * 0.5;
// Update the velocity-based kinematic body by setting its velocity.

125
examples3d/convex_decomposition3.rs
View File

@@ -1,128 +1,5 @@
use obj::raw::object::Polygon;
use rapier3d::parry::bounding_volume;
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
use std::fs::File;
use std::io::BufReader;
/*
* NOTE: The `r` macro is only here to convert from f64 to the `N` scalar type.
* This simplifies experimentation with various scalar types (f32, fixed-point numbers, etc.)
*/
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 50.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
* Create the convex decompositions.
*/
let geoms = models();
let ngeoms = geoms.len();
let width = (ngeoms as f32).sqrt() as usize;
let num_duplications = 4;
let shift = 5.0f32;
for (igeom, obj_path) in geoms.into_iter().enumerate() {
let deltas = Isometry::identity();
let mut shapes = Vec::new();
println!("Parsing and decomposing: {}", obj_path);
let input = BufReader::new(File::open(obj_path).unwrap());
if let Ok(model) = obj::raw::parse_obj(input) {
let mut vertices: Vec<_> = model
.positions
.iter()
.map(|v| point![v.0, v.1, v.2])
.collect();
let indices: Vec<_> = model
.polygons
.into_iter()
.flat_map(|p| match p {
Polygon::P(idx) => idx.into_iter(),
Polygon::PT(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
Polygon::PN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
Polygon::PTN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
})
.collect();
// Compute the size of the model, to scale it and have similar size for everything.
let aabb = bounding_volume::details::point_cloud_aabb(&deltas, &vertices);
let center = aabb.center();
let diag = (aabb.maxs - aabb.mins).norm();
vertices
.iter_mut()
.for_each(|p| *p = (*p - center.coords) * 6.0 / diag);
let indices: Vec<_> = indices
.chunks(3)
.map(|idx| [idx[0] as u32, idx[1] as u32, idx[2] as u32])
.collect();
let decomposed_shape = SharedShape::convex_decomposition(&vertices, &indices);
shapes.push(decomposed_shape);
// let compound = SharedShape::compound(compound_parts);
for k in 1..num_duplications + 1 {
let x = (igeom % width) as f32 * shift;
let y = (igeom / width) as f32 * shift + 4.0;
let z = k as f32 * shift;
let body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
let handle = bodies.insert(body);
for shape in &shapes {
let collider = ColliderBuilder::new(shape.clone());
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
}
fn models() -> Vec<String> {
vec![
"assets/3d/camel_decimated.obj".to_string(),
"assets/3d/chair.obj".to_string(),
"assets/3d/cup_decimated.obj".to_string(),
"assets/3d/dilo_decimated.obj".to_string(),
"assets/3d/feline_decimated.obj".to_string(),
"assets/3d/genus3_decimated.obj".to_string(),
"assets/3d/hand2_decimated.obj".to_string(),
"assets/3d/hand_decimated.obj".to_string(),
"assets/3d/hornbug.obj".to_string(),
"assets/3d/octopus_decimated.obj".to_string(),
"assets/3d/rabbit_decimated.obj".to_string(),
// "assets/3d/rust_logo.obj".to_string(),
"assets/3d/rust_logo_simplified.obj".to_string(),
"assets/3d/screwdriver_decimated.obj".to_string(),
"assets/3d/table.obj".to_string(),
"assets/3d/tstTorusModel.obj".to_string(),
// "assets/3d/tstTorusModel2.obj".to_string(),
// "assets/3d/tstTorusModel3.obj".to_string(),
]
crate::dynamic_trimesh3::do_init_world(testbed, true);
}

2
examples3d/debug_deserialize3.rs
View File

@@ -6,7 +6,7 @@ struct PhysicsState {
pub gravity: Vector<f32>,
pub integration_parameters: IntegrationParameters,
pub islands: IslandManager,
pub broad_phase: BroadPhase,
pub broad_phase: DefaultBroadPhase,
pub narrow_phase: NarrowPhase,
pub bodies: RigidBodySet,
pub colliders: ColliderSet,

3
examples3d/debug_internal_edges3.rs
View File

@@ -11,7 +11,8 @@ pub fn init_world(testbed: &mut Testbed) {
let multibody_joints = MultibodyJointSet::new();
let heights = DMatrix::zeros(100, 100);
let heightfield = HeightField::new(heights, vector![60.0, 1.0, 60.0]);
let heightfield =
HeightField::with_flags(heights, vector![60.0, 1.0, 60.0], HeightFieldFlags::all());
let rotation = vector![0.0, 0.0, 0.0]; // vector![-0.1, 0.0, 0.0];
colliders
.insert(ColliderBuilder::new(SharedShape::new(heightfield.clone())).rotation(rotation));

42
examples3d/debug_pop3.rs Normal file
View File

@@ -0,0 +1,42 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 10.0;
let ground_height = 10.0;
for _ in 0..1 {
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
// Build the dynamic box rigid body.
for _ in 0..1 {
let rigid_body = RigidBodyBuilder::dynamic()
// .translation(vector![0.0, 0.1, 0.0])
// .rotation(vector![0.8, 0.2, 0.1])
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0);
colliders.insert_with_parent(collider.clone(), handle, &mut bodies);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
}

56
examples3d/debug_thin_cube_on_mesh3.rs Normal file
View File

@@ -0,0 +1,56 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
// This shows a bug when a cylinder is in contact with a very large
// but very thin cuboid. In this case the EPA returns an incorrect
// contact normal, resulting in the cylinder falling through the floor.
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
// let vertices = vec![
// point![-50.0, 0.0, -50.0],
// point![-50.0, 0.0, 50.0],
// point![50.0, 0.0, 50.0],
// point![50.0, 0.0, -50.0],
// ];
// let indices = vec![[0, 1, 2], [0, 2, 3]];
//
// let collider = ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::all());
// colliders.insert(collider);
let heights = DMatrix::repeat(2, 2, 0.0);
let collider = ColliderBuilder::heightfield_with_flags(
heights,
Vector::new(50.0, 1.0, 50.0),
HeightFieldFlags::FIX_INTERNAL_EDGES,
);
colliders.insert(collider);
/*
* Create the cubes
*/
// Build the rigid body.
let rigid_body = RigidBodyBuilder::dynamic()
.translation(vector![0.0, 5.0, 0.0])
.rotation(vector![0.5, 0.0, 0.5])
.linvel(vector![0.0, -100.0, 0.0])
.soft_ccd_prediction(10.0);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(5.0, 0.015, 5.0);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
}

145
examples3d/dynamic_trimesh3.rs Normal file
View File

@@ -0,0 +1,145 @@
use obj::raw::object::Polygon;
use rapier3d::parry::bounding_volume;
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
use std::fs::File;
use std::io::BufReader;
pub fn init_world(testbed: &mut Testbed) {
do_init_world(testbed, false);
}
pub fn do_init_world(testbed: &mut Testbed, use_convex_decomposition: bool) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
//// OPTION 1: floor made of a single big box.
// let ground_size = 50.0;
// let ground_height = 0.1;
// let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
// let handle = bodies.insert(rigid_body);
// let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
// colliders.insert_with_parent(collider, handle, &mut bodies);
//// OPTION 2: floor made of a wavy mesh.
let nsubdivs = 100;
let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
-(i as f32 * 40.0 / (nsubdivs as f32) / 2.0).cos()
- (j as f32 * 40.0 / (nsubdivs as f32) / 2.0).cos()
});
let heightfield = HeightField::new(heights, vector![100.0, 2.0, 100.0]);
let mut trimesh = TriMesh::from(heightfield);
let _ = trimesh.set_flags(TriMeshFlags::FIX_INTERNAL_EDGES);
colliders.insert(ColliderBuilder::new(SharedShape::new(trimesh.clone())));
/*
* Create the convex decompositions.
*/
let geoms = models();
let ngeoms = geoms.len();
let width = (ngeoms as f32).sqrt() as usize;
let num_duplications = 4;
let shift_y = 8.0f32;
let shift_xz = 9.0f32;
for (igeom, obj_path) in geoms.into_iter().enumerate() {
let deltas = Isometry::identity();
let mut shapes = Vec::new();
println!("Parsing and decomposing: {}", obj_path);
let input = BufReader::new(File::open(obj_path).unwrap());
if let Ok(model) = obj::raw::parse_obj(input) {
let mut vertices: Vec<_> = model
.positions
.iter()
.map(|v| point![v.0, v.1, v.2])
.collect();
let indices: Vec<_> = model
.polygons
.into_iter()
.flat_map(|p| match p {
Polygon::P(idx) => idx.into_iter(),
Polygon::PT(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
Polygon::PN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
Polygon::PTN(idx) => Vec::from_iter(idx.into_iter().map(|i| i.0)).into_iter(),
})
.collect();
// Compute the size of the model, to scale it and have similar size for everything.
let aabb = bounding_volume::details::point_cloud_aabb(&deltas, &vertices);
let center = aabb.center();
let diag = (aabb.maxs - aabb.mins).norm();
vertices
.iter_mut()
.for_each(|p| *p = (*p - center.coords) * 10.0 / diag);
let indices: Vec<_> = indices
.chunks(3)
.map(|idx| [idx[0] as u32, idx[1] as u32, idx[2] as u32])
.collect();
let decomposed_shape = if use_convex_decomposition {
SharedShape::convex_decomposition(&vertices, &indices)
} else {
// SharedShape::trimesh(vertices, indices)
SharedShape::trimesh_with_flags(vertices, indices, TriMeshFlags::FIX_INTERNAL_EDGES)
};
shapes.push(decomposed_shape);
// let compound = SharedShape::compound(compound_parts);
for k in 1..num_duplications + 1 {
let x =
(igeom % width) as f32 * shift_xz - num_duplications as f32 * shift_xz / 2.0;
let y = (igeom / width) as f32 * shift_y + 7.0;
let z = k as f32 * shift_xz - num_duplications as f32 * shift_xz / 2.0;
let body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]);
let handle = bodies.insert(body);
for shape in &shapes {
let collider = ColliderBuilder::new(shape.clone()).contact_skin(0.1);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
}
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
}
fn models() -> Vec<String> {
vec![
"assets/3d/camel_decimated.obj".to_string(),
"assets/3d/chair.obj".to_string(),
"assets/3d/cup_decimated.obj".to_string(),
"assets/3d/dilo_decimated.obj".to_string(),
"assets/3d/tstTorusModel2.obj".to_string(),
"assets/3d/feline_decimated.obj".to_string(),
"assets/3d/genus3_decimated.obj".to_string(),
// "assets/3d/hand2_decimated.obj".to_string(),
// "assets/3d/hand_decimated.obj".to_string(),
"assets/3d/hornbug.obj".to_string(),
"assets/3d/tstTorusModel.obj".to_string(),
"assets/3d/octopus_decimated.obj".to_string(),
"assets/3d/rabbit_decimated.obj".to_string(),
"assets/3d/rust_logo_simplified.obj".to_string(),
"assets/3d/screwdriver_decimated.obj".to_string(),
"assets/3d/table.obj".to_string(),
"assets/3d/tstTorusModel3.obj".to_string(),
]
}

103
examples3d/inverse_kinematics3.rs Normal file
View File

@@ -0,0 +1,103 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 0.2;
let ground_height = 0.01;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let floor_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, floor_handle, &mut bodies);
/*
* Setup groups
*/
let num_segments = 10;
let body = RigidBodyBuilder::fixed();
let mut last_body = bodies.insert(body);
let mut last_link = MultibodyJointHandle::invalid();
for i in 0..num_segments {
let size = 1.0 / num_segments as f32;
let body = RigidBodyBuilder::dynamic().can_sleep(false);
let new_body = bodies.insert(body);
// NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
// the demo look nicer. IK could be used without cuboid.
let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0, size / 8.0)
.density(0.0)
.sensor(true);
colliders.insert_with_parent(collider, new_body, &mut bodies);
let link_ab = SphericalJointBuilder::new()
.local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32, 0.0])
.local_anchor2(point![0.0, -size / 2.0, 0.0])
.build()
.data;
last_link = multibody_joints
.insert(last_body, new_body, link_ab, true)
.unwrap();
last_body = new_body;
}
let mut displacements = DVector::zeros(0);
testbed.add_callback(move |graphics, physics, _, _| {
let Some(graphics) = graphics else { return };
if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
// Ensure our displacement vector has the right number of elements.
if displacements.nrows() < multibody.ndofs() {
displacements = DVector::zeros(multibody.ndofs());
} else {
displacements.fill(0.0);
}
let Some(mouse_ray) = graphics.mouse().ray else {
return;
};
// Cast a ray on a plane aligned with the camera passing through the origin.
let halfspace = HalfSpace {
normal: -UnitVector::new_normalize(graphics.camera_fwd_dir()),
};
let mouse_ray = Ray::new(mouse_ray.0, mouse_ray.1);
let Some(hit) = halfspace.cast_local_ray(&mouse_ray, f32::MAX, false) else {
return;
};
let target_point = mouse_ray.point_at(hit);
let options = InverseKinematicsOption {
constrained_axes: JointAxesMask::LIN_AXES,
..Default::default()
};
multibody.inverse_kinematics(
&physics.bodies,
link_id,
&options,
&Isometry::from(target_point),
&mut displacements,
);
multibody.apply_displacements(displacements.as_slice());
}
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 0.5, 2.5], point![0.0, 0.5, 0.0]);
}

8
examples3d/vehicle_joints3.rs
View File

@@ -138,16 +138,16 @@ pub fn init_world(testbed: &mut Testbed) {
for key in gfx.keys().get_pressed() {
match *key {
KeyCode::Right => {
KeyCode::ArrowRight => {
steering = -1.0;
}
KeyCode::Left => {
KeyCode::ArrowLeft => {
steering = 1.0;
}
KeyCode::Up => {
KeyCode::ArrowUp => {
thrust = -drive_strength;
}
KeyCode::Down => {
KeyCode::ArrowDown => {
thrust = drive_strength;
}
KeyCode::ShiftRight => {

241
src/control/character_controller.rs
View File

@@ -1,11 +1,12 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, TOI};
use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, ShapeCastHit};
use crate::math::{Isometry, Point, Real, UnitVector, Vector};
use crate::pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline};
use crate::utils;
use na::{RealField, Vector2};
use parry::bounding_volume::BoundingVolume;
use parry::math::Translation;
use parry::query::details::ShapeCastOptions;
use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -15,13 +16,13 @@ pub enum CharacterLength {
/// The length is specified relative to some of the character shape’s size.
///
/// For example setting `CharacterAutostep::max_height` to `CharacterLength::Relative(0.1)`
/// for a shape with an height equal to 20.0 will result in a maximum step height
/// for a shape with a height equal to 20.0 will result in a maximum step height
/// of `0.1 * 20.0 = 2.0`.
Relative(Real),
/// The length is specified as an aboslute value, independent from the character shape’s size.
/// The length is specified as an absolute value, independent from the character shape’s size.
///
/// For example setting `CharacterAutostep::max_height` to `CharacterLength::Relative(0.1)`
/// for a shape with an height equal to 20.0 will result in a maximum step height
/// for a shape with a height equal to 20.0 will result in a maximum step height
/// of `0.1` (the shape height is ignored in for this value).
Absolute(Real),
}
@@ -55,6 +56,13 @@ impl CharacterLength {
}
}
#[derive(Debug)]
struct HitInfo {
toi: ShapeCastHit,
is_wall: bool,
is_nonslip_slope: bool,
}
/// Configuration for the auto-stepping character controller feature.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
@@ -77,6 +85,21 @@ impl Default for CharacterAutostep {
}
}
#[derive(Debug)]
struct HitDecomposition {
normal_part: Vector<Real>,
horizontal_tangent: Vector<Real>,
vertical_tangent: Vector<Real>,
// NOTE: we don’t store the penetration part since we don’t really need it
// for anything.
}
impl HitDecomposition {
pub fn unconstrained_slide_part(&self) -> Vector<Real> {
self.normal_part + self.horizontal_tangent + self.vertical_tangent
}
}
/// A collision between the character and its environment during its movement.
#[derive(Copy, Clone, Debug)]
pub struct CharacterCollision {
@@ -89,7 +112,7 @@ pub struct CharacterCollision {
/// The translations that was still waiting to be applied to the character when the hit happens.
pub translation_remaining: Vector<Real>,
/// Geometric information about the hit.
pub toi: TOI,
pub hit: ShapeCastHit,
}
/// A character controller for kinematic bodies.
@@ -105,7 +128,10 @@ pub struct KinematicCharacterController {
pub offset: CharacterLength,
/// Should the character try to slide against the floor if it hits it?
pub slide: bool,
/// Should the character automatically step over small obstacles?
/// Should the character automatically step over small obstacles? (disabled by default)
///
/// Note that autostepping is currently a very computationally expensive feature, so it
/// is disabled by default.
pub autostep: Option<CharacterAutostep>,
/// The maximum angle (radians) between the floor’s normal and the `up` vector that the
/// character is able to climb.
@@ -116,6 +142,15 @@ pub struct KinematicCharacterController {
/// Should the character be automatically snapped to the ground if the distance between
/// the ground and its feed are smaller than the specified threshold?
pub snap_to_ground: Option<CharacterLength>,
/// Increase this number if your character appears to get stuck when sliding against surfaces.
///
/// This is a small distance applied to the movement toward the contact normals of shapes hit
/// by the character controller. This helps shape-casting not getting stuck in an always-penetrating
/// state during the sliding calculation.
///
/// This value should remain fairly small since it can introduce artificial "bumps" when sliding
/// along a flat surface.
pub normal_nudge_factor: Real,
}
impl Default for KinematicCharacterController {
@@ -124,10 +159,11 @@ impl Default for KinematicCharacterController {
up: Vector::y_axis(),
offset: CharacterLength::Relative(0.01),
slide: true,
autostep: Some(CharacterAutostep::default()),
autostep: None,
max_slope_climb_angle: Real::frac_pi_4(),
min_slope_slide_angle: Real::frac_pi_4(),
snap_to_ground: Some(CharacterLength::Relative(0.2)),
normal_nudge_factor: 1.0e-4,
}
}
}
@@ -225,19 +261,22 @@ impl KinematicCharacterController {
}
// 2. Cast towards the movement direction.
if let Some((handle, toi)) = queries.cast_shape(
if let Some((handle, hit)) = queries.cast_shape(
bodies,
colliders,
&(Translation::from(result.translation) * character_pos),
&translation_dir,
character_shape,
translation_dist + offset,
false,
ShapeCastOptions {
target_distance: offset,
stop_at_penetration: false,
max_time_of_impact: translation_dist,
compute_impact_geometry_on_penetration: true,
},
filter,
) {
// We hit something, compute the allowed self.
let allowed_dist =
(toi.toi - (-toi.normal1.dot(&translation_dir)) * offset).max(0.0);
// We hit something, compute and apply the allowed interference-free translation.
let allowed_dist = hit.time_of_impact;
let allowed_translation = *translation_dir * allowed_dist;
result.translation += allowed_translation;
translation_remaining -= allowed_translation;
@@ -247,10 +286,12 @@ impl KinematicCharacterController {
character_pos: Translation::from(result.translation) * character_pos,
translation_applied: result.translation,
translation_remaining,
toi,
hit,
});
// Try to go up stairs.
let hit_info = self.compute_hit_info(hit);
// Try to go upstairs.
if !self.handle_stairs(
bodies,
colliders,
@@ -260,12 +301,18 @@ impl KinematicCharacterController {
&dims,
filter,
handle,
&hit_info,
&mut translation_remaining,
&mut result,
) {
// No stairs, try to move along slopes.
translation_remaining =
self.handle_slopes(&toi, &translation_remaining, &mut result);
translation_remaining = self.handle_slopes(
&hit_info,
&desired_translation,
&translation_remaining,
self.normal_nudge_factor,
&mut result,
);
}
} else {
// No interference along the path.
@@ -319,7 +366,7 @@ impl KinematicCharacterController {
dims: &Vector2<Real>,
filter: QueryFilter,
result: &mut EffectiveCharacterMovement,
) -> Option<(ColliderHandle, TOI)> {
) -> Option<(ColliderHandle, ShapeCastHit)> {
if let Some(snap_distance) = self.snap_to_ground {
if result.translation.dot(&self.up) < -1.0e-5 {
let snap_distance = snap_distance.eval(dims.y);
@@ -330,12 +377,16 @@ impl KinematicCharacterController {
character_pos,
&-self.up,
character_shape,
snap_distance + offset,
false,
ShapeCastOptions {
target_distance: offset,
stop_at_penetration: false,
max_time_of_impact: snap_distance,
compute_impact_geometry_on_penetration: true,
},
filter,
) {
// Apply the snap.
result.translation -= *self.up * (hit.toi - offset).max(0.0);
result.translation -= *self.up * hit.time_of_impact;
result.grounded = true;
return Some((hit_handle, hit));
}
@@ -481,36 +532,40 @@ impl KinematicCharacterController {
fn handle_slopes(
&self,
hit: &TOI,
hit: &HitInfo,
movement_input: &Vector<Real>,
translation_remaining: &Vector<Real>,
normal_nudge_factor: Real,
result: &mut EffectiveCharacterMovement,
) -> Vector<Real> {
let [vertical_translation, horizontal_translation] =
self.split_into_components(translation_remaining);
let slope_translation = subtract_hit(*translation_remaining, hit);
let [_vertical_input, horizontal_input] = self.split_into_components(movement_input);
let horiz_input_decomp = self.decompose_hit(&horizontal_input, &hit.toi);
let input_decomp = self.decompose_hit(movement_input, &hit.toi);
// Check if there is a slope to climb.
let angle_with_floor = self.up.angle(&hit.normal1);
let decomp = self.decompose_hit(translation_remaining, &hit.toi);
// We are climbing if the movement along the slope goes upward, and the angle with the
// floor is smaller than pi/2 (in which case we hit some some sort of ceiling).
//
// NOTE: part of the slope will already be handled by auto-stepping if it was enabled.
// Therefore, `climbing` may not always be `true` when climbing on a slope at
// slow speed.
let climbing = self.up.dot(&slope_translation) >= 0.0 && self.up.dot(&hit.normal1) > 0.0;
// An object is trying to slip if the tangential movement induced by its vertical movement
// points downward.
let slipping_intent = self.up.dot(&horiz_input_decomp.vertical_tangent) < 0.0;
let slipping = self.up.dot(&decomp.vertical_tangent) < 0.0;
if climbing && angle_with_floor >= self.max_slope_climb_angle {
// Prevent horizontal movement from pushing through the slope.
vertical_translation
} else if !climbing && angle_with_floor <= self.min_slope_slide_angle {
// An object is trying to climb if its indirect vertical motion points upward.
let climbing_intent = self.up.dot(&input_decomp.vertical_tangent) > 0.0;
let climbing = self.up.dot(&decomp.vertical_tangent) > 0.0;
let allowed_movement = if hit.is_wall && climbing && !climbing_intent {
// Can’t climb the slope, remove the vertical tangent motion induced by the forward motion.
decomp.horizontal_tangent + decomp.normal_part
} else if hit.is_nonslip_slope && slipping && !slipping_intent {
// Prevent the vertical movement from sliding down.
horizontal_translation
decomp.horizontal_tangent + decomp.normal_part
} else {
// Let it slide
// Let it slide (including climbing the slope).
result.is_sliding_down_slope = true;
slope_translation
}
decomp.unconstrained_slide_part()
};
allowed_movement + *hit.toi.normal1 * normal_nudge_factor
}
fn split_into_components(&self, translation: &Vector<Real>) -> [Vector<Real>; 2] {
@@ -519,6 +574,51 @@ impl KinematicCharacterController {
[vertical_translation, horizontal_translation]
}
fn compute_hit_info(&self, toi: ShapeCastHit) -> HitInfo {
let angle_with_floor = self.up.angle(&toi.normal1);
let is_ceiling = self.up.dot(&toi.normal1) < 0.0;
let is_wall = angle_with_floor >= self.max_slope_climb_angle && !is_ceiling;
let is_nonslip_slope = angle_with_floor <= self.min_slope_slide_angle;
HitInfo {
toi,
is_wall,
is_nonslip_slope,
}
}
fn decompose_hit(&self, translation: &Vector<Real>, hit: &ShapeCastHit) -> HitDecomposition {
let dist_to_surface = translation.dot(&hit.normal1);
let normal_part;
let penetration_part;
if dist_to_surface < 0.0 {
normal_part = Vector::zeros();
penetration_part = dist_to_surface * *hit.normal1;
} else {
penetration_part = Vector::zeros();
normal_part = dist_to_surface * *hit.normal1;
}
let tangent = translation - normal_part - penetration_part;
#[cfg(feature = "dim3")]
let horizontal_tangent_dir = hit.normal1.cross(&self.up);
#[cfg(feature = "dim2")]
let horizontal_tangent_dir = Vector::zeros();
let horizontal_tangent_dir = horizontal_tangent_dir
.try_normalize(1.0e-5)
.unwrap_or_default();
let horizontal_tangent = tangent.dot(&horizontal_tangent_dir) * horizontal_tangent_dir;
let vertical_tangent = tangent - horizontal_tangent;
HitDecomposition {
normal_part,
horizontal_tangent,
vertical_tangent,
}
}
fn compute_dims(&self, character_shape: &dyn Shape) -> Vector2<Real> {
let extents = character_shape.compute_local_aabb().extents();
let up_extent = extents.dot(&self.up.abs());
@@ -536,14 +636,19 @@ impl KinematicCharacterController {
dims: &Vector2<Real>,
mut filter: QueryFilter,
stair_handle: ColliderHandle,
hit: &HitInfo,
translation_remaining: &mut Vector<Real>,
result: &mut EffectiveCharacterMovement,
) -> bool {
let autostep = match self.autostep {
Some(autostep) => autostep,
None => return false,
let Some(autostep) = self.autostep else {
return false;
};
// Only try to autostep on walls.
if !hit.is_wall {
return false;
}
let offset = self.offset.eval(dims.y);
let min_width = autostep.min_width.eval(dims.x) + offset;
let max_height = autostep.max_height.eval(dims.y) + offset;
@@ -565,12 +670,10 @@ impl KinematicCharacterController {
let shifted_character_pos = Translation::from(*self.up * max_height) * character_pos;
let horizontal_dir = match (*translation_remaining
let Some(horizontal_dir) = (*translation_remaining
- *self.up * translation_remaining.dot(&self.up))
.try_normalize(1.0e-5)
{
Some(dir) => dir,
None => return false,
.try_normalize(1.0e-5) else {
return false;
};
if queries
@@ -580,8 +683,12 @@ impl KinematicCharacterController {
character_pos,
&self.up,
character_shape,
max_height,
false,
ShapeCastOptions {
target_distance: offset,
stop_at_penetration: false,
max_time_of_impact: max_height,
compute_impact_geometry_on_penetration: true,
},
filter,
)
.is_some()
@@ -597,8 +704,12 @@ impl KinematicCharacterController {
&shifted_character_pos,
&horizontal_dir,
character_shape,
min_width,
false,
ShapeCastOptions {
target_distance: offset,
stop_at_penetration: false,
max_time_of_impact: min_width,
compute_impact_geometry_on_penetration: true,
},
filter,
)
.is_some()
@@ -615,8 +726,12 @@ impl KinematicCharacterController {
&(Translation::from(horizontal_dir * min_width) * shifted_character_pos),
&-self.up,
character_shape,
max_height,
false,
ShapeCastOptions {
target_distance: offset,
stop_at_penetration: false,
max_time_of_impact: max_height,
compute_impact_geometry_on_penetration: true,
},
filter,
) {
let [vertical_slope_translation, horizontal_slope_translation] = self
@@ -642,11 +757,15 @@ impl KinematicCharacterController {
&(Translation::from(horizontal_dir * min_width) * shifted_character_pos),
&-self.up,
character_shape,
max_height,
false,
ShapeCastOptions {
target_distance: offset,
stop_at_penetration: false,
max_time_of_impact: max_height,
compute_impact_geometry_on_penetration: true,
},
filter,
)
.map(|hit| hit.1.toi)
.map(|hit| hit.1.time_of_impact)
.unwrap_or(max_height);
// Remove the step height from the vertical part of the self.
@@ -681,7 +800,7 @@ impl KinematicCharacterController {
let extents = character_shape.compute_local_aabb().extents();
let up_extent = extents.dot(&self.up.abs());
let movement_to_transfer =
*collision.toi.normal1 * collision.translation_remaining.dot(&collision.toi.normal1);
*collision.hit.normal1 * collision.translation_remaining.dot(&collision.hit.normal1);
let prediction = self.predict_ground(up_extent);
// TODO: allow custom dispatchers.
@@ -748,7 +867,7 @@ impl KinematicCharacterController {
}
}
fn subtract_hit(translation: Vector<Real>, hit: &TOI) -> Vector<Real> {
fn subtract_hit(translation: Vector<Real>, hit: &ShapeCastHit) -> Vector<Real> {
let surface_correction = (-translation).dot(&hit.normal1).max(0.0);
// This fixes some instances of moving through walls
let surface_correction = surface_correction * (1.0 + 1.0e-5);

6
src/control/ray_cast_vehicle_controller.rs
View File

@@ -341,7 +341,7 @@ impl DynamicRayCastVehicleController {
wheel.raycast_info.ground_object = None;
if let Some((collider_hit, mut hit)) = hit {
if hit.toi == 0.0 {
if hit.time_of_impact == 0.0 {
let collider = &colliders[collider_hit];
let up_ray = Ray::new(source + rayvector, -rayvector);
if let Some(hit2) =
@@ -362,7 +362,7 @@ impl DynamicRayCastVehicleController {
wheel.raycast_info.is_in_contact = true;
wheel.raycast_info.ground_object = Some(collider_hit);
let hit_distance = hit.toi * raylen;
let hit_distance = hit.time_of_impact * raylen;
wheel.raycast_info.suspension_length = hit_distance - wheel.radius;
// clamp on max suspension travel
@@ -372,7 +372,7 @@ impl DynamicRayCastVehicleController {
.raycast_info
.suspension_length
.clamp(min_suspension_length, max_suspension_length);
wheel.raycast_info.contact_point_ws = ray.point_at(hit.toi);
wheel.raycast_info.contact_point_ws = ray.point_at(hit.time_of_impact);
let denominator = wheel
.raycast_info

18
src/counters/mod.rs
View File

@@ -17,7 +17,7 @@ mod timer;
/// Aggregation of all the performances counters tracked by rapier.
#[derive(Clone, Copy)]
pub struct Counters {
/// Whether thi counter is enabled or not.
/// Whether this counter is enabled or not.
pub enabled: bool,
/// Timer for a whole timestep.
pub step_time: Timer,
@@ -69,7 +69,7 @@ impl Counters {
}
}
/// Notfy that the time-step has finished.
/// Notify that the time-step has finished.
pub fn step_completed(&mut self) {
if self.enabled {
self.step_time.pause();
@@ -88,7 +88,7 @@ impl Counters {
}
}
/// Notfy that the custom operation has finished.
/// Notify that the custom operation has finished.
pub fn custom_completed(&mut self) {
if self.enabled {
self.custom.pause();
@@ -182,6 +182,12 @@ measure_method!(
stages.solver_time
);
measure_method!(ccd_started, ccd_completed, ccd_time, stages.ccd_time);
measure_method!(
query_pipeline_update_started,
query_pipeline_update_completed,
query_pipeline_update_time,
stages.query_pipeline_time
);
measure_method!(
assembly_started,
@@ -201,12 +207,6 @@ measure_method!(
velocity_update_time,
solver.velocity_update_time
);
measure_method!(
position_resolution_started,
position_resolution_completed,
position_resolution_time,
solver.position_resolution_time
);
measure_method!(
broad_phase_started,
broad_phase_completed,

17
src/counters/solver_counters.rs
View File

@@ -14,10 +14,8 @@ pub struct SolverCounters {
pub velocity_assembly_time: Timer,
/// Time spent for the update of the velocity of the bodies.
pub velocity_update_time: Timer,
/// Time spent for the assembly of all the position constraints.
pub position_assembly_time: Timer,
/// Time spent for the update of the position of the bodies.
pub position_resolution_time: Timer,
/// Time spent to write force back to user-accessible data.
pub velocity_writeback_time: Timer,
}
impl SolverCounters {
@@ -29,8 +27,7 @@ impl SolverCounters {
velocity_assembly_time: Timer::new(),
velocity_resolution_time: Timer::new(),
velocity_update_time: Timer::new(),
position_assembly_time: Timer::new(),
position_resolution_time: Timer::new(),
velocity_writeback_time: Timer::new(),
}
}
@@ -41,8 +38,7 @@ impl SolverCounters {
self.velocity_resolution_time.reset();
self.velocity_assembly_time.reset();
self.velocity_update_time.reset();
self.position_assembly_time.reset();
self.position_resolution_time.reset();
self.velocity_writeback_time.reset();
}
}
@@ -57,11 +53,10 @@ impl Display for SolverCounters {
self.velocity_resolution_time
)?;
writeln!(f, "Velocity update time: {}", self.velocity_update_time)?;
writeln!(f, "Position assembly time: {}", self.position_assembly_time)?;
writeln!(
f,
"Position resolution time: {}",
self.position_resolution_time
"Velocity writeback time: {}",
self.velocity_writeback_time
)
}
}

14
src/counters/stages_counters.rs
View File

@@ -14,10 +14,14 @@ pub struct StagesCounters {
pub solver_time: Timer,
/// Total time spent for CCD and CCD resolution.
pub ccd_time: Timer,
/// Total time spent updating the query pipeline (if provided to `PhysicsPipeline::step`).
pub query_pipeline_time: Timer,
/// Total time spent propagating user changes.
pub user_changes: Timer,
}
impl StagesCounters {
/// Create a new counter intialized to zero.
/// Create a new counter initialized to zero.
pub fn new() -> Self {
StagesCounters {
update_time: Timer::new(),
@@ -25,6 +29,8 @@ impl StagesCounters {
island_construction_time: Timer::new(),
solver_time: Timer::new(),
ccd_time: Timer::new(),
query_pipeline_time: Timer::new(),
user_changes: Timer::new(),
}
}
@@ -35,6 +41,8 @@ impl StagesCounters {
self.island_construction_time.reset();
self.solver_time.reset();
self.ccd_time.reset();
self.query_pipeline_time.reset();
self.user_changes.reset();
}
}
@@ -52,6 +60,8 @@ impl Display for StagesCounters {
self.island_construction_time
)?;
writeln!(f, "Solver time: {}", self.solver_time)?;
writeln!(f, "CCD time: {}", self.ccd_time)
writeln!(f, "CCD time: {}", self.ccd_time)?;
writeln!(f, "Query pipeline time: {}", self.query_pipeline_time)?;
writeln!(f, "User changes time: {}", self.user_changes)
}
}

2
src/data/graph.rs
View File

@@ -370,7 +370,7 @@ impl<N, E> Graph<N, E> {
// indices.
let edge = self.edges.swap_remove(e.index());
let swap = match self.edges.get(e.index()) {
// no elment needed to be swapped.
// no element needed to be swapped.
None => return Some(edge.weight),
Some(ed) => ed.node,
};

24
src/data/pubsub.rs
View File

@@ -103,7 +103,7 @@ impl<T> PubSub<T> {
subscription
}
/// Read the i-th message not yet read by the given subsciber.
/// Read the i-th message not yet read by the given subscriber.
pub fn read_ith(&self, sub: &Subscription<T>, i: usize) -> Option<&T> {
let cursor = &self.cursors[sub.id as usize];
self.messages.get(cursor.next(self.deleted_messages) + i)
@@ -114,11 +114,7 @@ impl<T> PubSub<T> {
let cursor = &self.cursors[sub.id as usize];
let next = cursor.next(self.deleted_messages);
// TODO: use self.queue.range(next..) once it is stabilised.
MessageRange {
queue: &self.messages,
next,
}
self.messages.range(next..)
}
/// Makes the given subscribe acknowledge all the messages in the queue.
@@ -159,19 +155,3 @@ impl<T> PubSub<T> {
}
}
}
struct MessageRange<'a, T> {
queue: &'a VecDeque<T>,
next: usize,
}
impl<'a, T> Iterator for MessageRange<'a, T> {
type Item = &'a T;
#[inline(always)]
fn next(&mut self) -> Option<&'a T> {
let result = self.queue.get(self.next);
self.next += 1;
result
}
}

4
src/dynamics/ccd/toi_entry.rs
View File

@@ -129,7 +129,7 @@ impl TOIEntry {
// .ok();
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
.cast_shapes_nonlinear(
&motion_c1,
co1.shape.as_ref(),
&motion_c2,
@@ -143,7 +143,7 @@ impl TOIEntry {
let toi = res_toi??;
Some(Self::new(
toi.toi,
toi.time_of_impact,
ch1,
co1.parent.map(|p| p.handle),
ch2,

283
src/dynamics/integration_parameters.rs
View File

@@ -1,13 +1,18 @@
use crate::math::Real;
use na::RealField;
use std::num::NonZeroUsize;
// TODO: enabling the block solver in 3d introduces a lot of jitters in
// the 3D domino demo. So for now we dont enable it in 3D.
pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2");
/// Parameters for a time-step of the physics engine.
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct IntegrationParameters {
/// The timestep length (default: `1.0 / 60.0`)
/// The timestep length (default: `1.0 / 60.0`).
pub dt: Real,
/// Minimum timestep size when using CCD with multiple substeps (default `1.0 / 60.0 / 100.0`)
/// Minimum timestep size when using CCD with multiple substeps (default: `1.0 / 60.0 / 100.0`).
///
/// When CCD with multiple substeps is enabled, the timestep is subdivided
/// into smaller pieces. This timestep subdivision won't generate timestep
@@ -19,37 +24,78 @@ pub struct IntegrationParameters {
/// to numerical instabilities.
pub min_ccd_dt: Real,
/// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
/// will be compensated for during the velocity solve.
/// (default `0.8`).
pub erp: Real,
/// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
/// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
/// before stabilization).
/// (default `0.25`).
pub damping_ratio: Real,
/// > 0: the damping ratio used by the springs for contact constraint stabilization.
///
/// Larger values make the constraints more compliant (allowing more visible
/// penetrations before stabilization).
/// (default `5.0`).
pub contact_damping_ratio: Real,
/// 0-1: multiplier for how much of the joint violation
/// will be compensated for during the velocity solve.
/// (default `1.0`).
pub joint_erp: Real,
/// > 0: the natural frequency used by the springs for contact constraint regularization.
///
/// Increasing this value will make it so that penetrations get fixed more quickly at the
/// expense of potential jitter effects due to overshooting. In order to make the simulation
/// look stiffer, it is recommended to increase the [`Self::contact_damping_ratio`] instead of this
/// value.
/// (default: `30.0`).
pub contact_natural_frequency: Real,
/// > 0: the natural frequency used by the springs for joint constraint regularization.
///
/// Increasing this value will make it so that penetrations get fixed more quickly.
/// (default: `1.0e6`).
pub joint_natural_frequency: Real,
/// The fraction of critical damping applied to the joint for constraints regularization.
/// (default `0.25`).
///
/// Larger values make the constraints more compliant (allowing more joint
/// drift before stabilization).
/// (default `1.0`).
pub joint_damping_ratio: Real,
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
pub allowed_linear_error: Real,
/// Maximum amount of penetration the solver will attempt to resolve in one timestep.
pub max_penetration_correction: Real,
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
pub prediction_distance: Real,
/// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the
/// initial solution (instead of 0) at the next simulation step.
///
/// This should generally be set to 1.
///
/// (default `1.0`).
pub warmstart_coefficient: Real,
/// The approximate size of most dynamic objects in the scene.
///
/// This value is used internally to estimate some length-based tolerance. In particular, the
/// values [`IntegrationParameters::allowed_linear_error`],
/// [`IntegrationParameters::max_corrective_velocity`],
/// [`IntegrationParameters::prediction_distance`], [`RigidBodyActivation::linear_threshold`]
/// are scaled by this value implicitly.
///
/// This value can be understood as the number of units-per-meter in your physical world compared
/// to a human-sized world in meter. For example, in a 2d game, if your typical object size is 100
/// pixels, set the [`Self::length_unit`] parameter to 100.0. The physics engine will interpret
/// it as if 100 pixels is equivalent to 1 meter in its various internal threshold.
/// (default `1.0`).
pub length_unit: Real,
/// Amount of penetration the engine won’t attempt to correct (default: `0.001m`).
///
/// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
pub normalized_allowed_linear_error: Real,
/// Maximum amount of penetration the solver will attempt to resolve in one timestep (default: `10.0`).
///
/// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
pub normalized_max_corrective_velocity: Real,
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
///
/// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
pub normalized_prediction_distance: Real,
/// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
pub num_solver_iterations: NonZeroUsize,
/// Number of addition friction resolution iteration run during the last solver sub-step (default: `4`).
/// Number of addition friction resolution iteration run during the last solver sub-step (default: `0`).
pub num_additional_friction_iterations: usize,
/// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`).
pub num_internal_pgs_iterations: usize,
/// The number of stabilization iterations run at each solver iterations (default: `2`).
pub num_internal_stabilization_iterations: usize,
/// Minimum number of dynamic bodies in each active island (default: `128`).
pub min_island_size: usize,
/// Maximum number of substeps performed by the solver (default: `1`).
@@ -57,51 +103,6 @@ pub struct IntegrationParameters {
}
impl IntegrationParameters {
/// Configures the integration parameters to match the old PGS solver
/// from Rapier version <= 0.17.
///
/// This solver was slightly faster than the new one but resulted
/// in less stable joints and worse convergence rates.
///
/// This should only be used for comparison purpose or if you are
/// experiencing problems with the new solver.
///
/// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will
/// still create solver iterations based on the new "small-steps" PGS solver.
/// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
/// [`Self::joint_damping_ratio`] to their former default values.
pub fn switch_to_standard_pgs_solver(&mut self) {
self.num_internal_pgs_iterations *= self.num_solver_iterations.get();
self.num_solver_iterations = NonZeroUsize::new(1).unwrap();
self.erp = 0.8;
self.damping_ratio = 0.25;
self.joint_erp = 1.0;
self.joint_damping_ratio = 1.0;
}
/// Configures the integration parameters to match the new "small-steps" PGS solver
/// from Rapier version >= 0.18.
///
/// The "small-steps" PGS solver is the default one given by [`Self::default()`] so
/// calling this function is generally not needed unless
/// [`Self::switch_to_standard_pgs_solver()`] was called.
///
/// This solver results in more stable joints and significantly better convergence
/// rates but is slightly slower in its default settings.
///
/// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
/// [`Self::joint_damping_ratio`] to their default values.
pub fn switch_to_small_steps_pgs_solver(&mut self) {
self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap();
self.num_internal_pgs_iterations = 1;
let default = Self::default();
self.erp = default.erp;
self.damping_ratio = default.damping_ratio;
self.joint_erp = default.joint_erp;
self.joint_damping_ratio = default.joint_damping_ratio;
}
/// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
///
/// This is zero if `self.dt` is zero.
@@ -134,29 +135,65 @@ impl IntegrationParameters {
}
}
/// The ERP coefficient, multiplied by the inverse timestep length.
pub fn erp_inv_dt(&self) -> Real {
self.erp * self.inv_dt()
/// The contact’s spring angular frequency for constraints regularization.
pub fn contact_angular_frequency(&self) -> Real {
self.contact_natural_frequency * Real::two_pi()
}
/// The joint ERP coefficient, multiplied by the inverse timestep length.
/// The [`Self::contact_erp`] coefficient, multiplied by the inverse timestep length.
pub fn contact_erp_inv_dt(&self) -> Real {
let ang_freq = self.contact_angular_frequency();
ang_freq / (self.dt * ang_freq + 2.0 * self.contact_damping_ratio)
}
/// The effective Error Reduction Parameter applied for calculating regularization forces
/// on contacts.
///
/// This parameter is computed automatically from [`Self::contact_natural_frequency`],
/// [`Self::contact_damping_ratio`] and the substep length.
pub fn contact_erp(&self) -> Real {
self.dt * self.contact_erp_inv_dt()
}
/// The joint’s spring angular frequency for constraint regularization.
pub fn joint_angular_frequency(&self) -> Real {
self.joint_natural_frequency * Real::two_pi()
}
/// The [`Self::joint_erp`] coefficient, multiplied by the inverse timestep length.
pub fn joint_erp_inv_dt(&self) -> Real {
self.joint_erp * self.inv_dt()
let ang_freq = self.joint_angular_frequency();
ang_freq / (self.dt * ang_freq + 2.0 * self.joint_damping_ratio)
}
/// The CFM factor to be used in the constraints resolution.
pub fn cfm_factor(&self) -> Real {
/// The effective Error Reduction Parameter applied for calculating regularization forces
/// on joints.
///
/// This parameter is computed automatically from [`Self::joint_natural_frequency`],
/// [`Self::joint_damping_ratio`] and the substep length.
pub fn joint_erp(&self) -> Real {
self.dt * self.joint_erp_inv_dt()
}
/// The CFM factor to be used in the constraint resolution.
///
/// This parameter is computed automatically from [`Self::contact_natural_frequency`],
/// [`Self::contact_damping_ratio`] and the substep length.
pub fn contact_cfm_factor(&self) -> Real {
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
let inv_erp_minus_one = 1.0 / self.erp - 1.0;
let inv_erp_minus_one = 1.0 / self.contact_erp() - 1.0;
// let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
// let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * inv_erp_minus_one);
// let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
// NOTE: This simplies to cfm = cfm_coefff / projected_mass:
// NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
/ ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio);
/ ((1.0 + inv_erp_minus_one)
* 4.0
* self.contact_damping_ratio
* self.contact_damping_ratio);
// Furthermore, we use this coefficient inside of the impulse resolution.
// Surprisingly, several simplifications happen there.
@@ -173,36 +210,65 @@ impl IntegrationParameters {
// new_impulse = cfm_factor * (old_impulse - m * delta_vel)
//
// The value returned by this function is this cfm_factor that can be used directly
// in the constraints solver.
// in the constraint solver.
1.0 / (1.0 + cfm_coeff)
}
/// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
/// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization.
///
/// This parameter is computed automatically from [`Self::joint_natural_frequency`],
/// [`Self::joint_damping_ratio`] and the substep length.
pub fn joint_cfm_coeff(&self) -> Real {
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
// The logic is similar to `Self::cfm_factor`.
let inv_erp_minus_one = 1.0 / self.joint_erp() - 1.0;
inv_erp_minus_one * inv_erp_minus_one
/ ((1.0 + inv_erp_minus_one)
* 4.0
* self.joint_damping_ratio
* self.joint_damping_ratio)
}
}
impl Default for IntegrationParameters {
fn default() -> Self {
/// Amount of penetration the engine won’t attempt to correct (default: `0.001` multiplied by
/// [`Self::length_unit`]).
pub fn allowed_linear_error(&self) -> Real {
self.normalized_allowed_linear_error * self.length_unit
}
/// Maximum amount of penetration the solver will attempt to resolve in one timestep.
///
/// This is equal to [`Self::normalized_max_corrective_velocity`] multiplied by
/// [`Self::length_unit`].
pub fn max_corrective_velocity(&self) -> Real {
if self.normalized_max_corrective_velocity != Real::MAX {
self.normalized_max_corrective_velocity * self.length_unit
} else {
Real::MAX
}
}
/// The maximal distance separating two objects that will generate predictive contacts
/// (default: `0.002m` multiped by [`Self::length_unit`]).
pub fn prediction_distance(&self) -> Real {
self.normalized_prediction_distance * self.length_unit
}
/// Initialize the simulation parameters with settings matching the TGS-soft solver
/// with warmstarting.
///
/// This is the default configuration, equivalent to [`IntegrationParameters::default()`].
pub fn tgs_soft() -> Self {
Self {
dt: 1.0 / 60.0,
min_ccd_dt: 1.0 / 60.0 / 100.0,
erp: 0.6,
damping_ratio: 1.0,
joint_erp: 1.0,
contact_natural_frequency: 30.0,
contact_damping_ratio: 5.0,
joint_natural_frequency: 1.0e6,
joint_damping_ratio: 1.0,
allowed_linear_error: 0.001,
max_penetration_correction: Real::MAX,
prediction_distance: 0.002,
warmstart_coefficient: 1.0,
num_internal_pgs_iterations: 1,
num_additional_friction_iterations: 4,
num_internal_stabilization_iterations: 2,
num_additional_friction_iterations: 0,
num_solver_iterations: NonZeroUsize::new(4).unwrap(),
// TODO: what is the optimal value for min_island_size?
// It should not be too big so that we don't end up with
@@ -210,7 +276,42 @@ impl Default for IntegrationParameters {
// However we don't want it to be too small and end up with
// tons of islands, reducing SIMD parallelism opportunities.
min_island_size: 128,
normalized_allowed_linear_error: 0.001,
normalized_max_corrective_velocity: 10.0,
normalized_prediction_distance: 0.002,
max_ccd_substeps: 1,
length_unit: 1.0,
}
}
/// Initialize the simulation parameters with settings matching the TGS-soft solver
/// **without** warmstarting.
///
/// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless
/// warmstarting proves to be undesirable for your use-case.
pub fn tgs_soft_without_warmstart() -> Self {
Self {
contact_damping_ratio: 0.25,
warmstart_coefficient: 0.0,
num_additional_friction_iterations: 4,
..Self::tgs_soft()
}
}
/// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17.
///
/// This exists mainly for testing and comparison purpose.
pub fn pgs_legacy() -> Self {
Self {
num_solver_iterations: NonZeroUsize::new(1).unwrap(),
num_internal_pgs_iterations: 4,
..Self::tgs_soft_without_warmstart()
}
}
}
impl Default for IntegrationParameters {
fn default() -> Self {
Self::tgs_soft()
}
}

14
src/dynamics/island_manager.rs
View File

@@ -150,6 +150,7 @@ impl IslandManager {
pub(crate) fn update_active_set_with_contacts(
&mut self,
dt: Real,
length_unit: Real,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
narrow_phase: &NarrowPhase,
@@ -181,7 +182,7 @@ impl IslandManager {
let sq_linvel = rb.vels.linvel.norm_squared();
let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt);
update_energy(length_unit, &mut rb.activation, sq_linvel, sq_angvel, dt);
if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep {
// Mark them as sleeping for now. This will
@@ -324,8 +325,15 @@ impl IslandManager {
}
}
fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) {
if sq_linvel < activation.linear_threshold * activation.linear_threshold.abs()
fn update_energy(
length_unit: Real,
activation: &mut RigidBodyActivation,
sq_linvel: Real,
sq_angvel: Real,
dt: Real,
) {
let linear_threshold = activation.normalized_linear_threshold * length_unit;
if sq_linvel < linear_threshold * linear_threshold.abs()
&& sq_angvel < activation.angular_threshold * activation.angular_threshold.abs()
{
activation.time_since_can_sleep += dt;

18
src/dynamics/joint/generic_joint.rs
View File

@@ -496,6 +496,24 @@ impl GenericJoint {
self.motors[i].damping = damping;
self
}
/// Flips the orientation of the joint, including limits and motors.
pub fn flip(&mut self) {
std::mem::swap(&mut self.local_frame1, &mut self.local_frame2);
let coupled_bits = self.coupled_axes.bits();
for dim in 0..SPATIAL_DIM {
if coupled_bits & (1 << dim) == 0 {
let limit = self.limits[dim];
self.limits[dim].min = -limit.max;
self.limits[dim].max = -limit.min;
}
self.motors[dim].target_vel = -self.motors[dim].target_vel;
self.motors[dim].target_pos = -self.motors[dim].target_pos;
}
}
}
macro_rules! joint_conversion_methods(

2
src/dynamics/joint/multibody_joint/mod.rs
View File

@@ -1,6 +1,7 @@
//! MultibodyJoints using the reduced-coordinates formalism or using constraints.
pub use self::multibody::Multibody;
pub use self::multibody_ik::InverseKinematicsOption;
pub use self::multibody_joint::MultibodyJoint;
pub use self::multibody_joint_set::{
MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
@@ -13,5 +14,6 @@ mod multibody_joint_set;
mod multibody_link;
mod multibody_workspace;
mod multibody_ik;
mod multibody_joint;
mod unit_multibody_joint;

217
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -89,6 +89,7 @@ impl Default for Multibody {
Multibody::new()
}
}
impl Multibody {
/// Creates a new multibody with no link.
pub fn new() -> Self {
@@ -115,6 +116,8 @@ impl Multibody {
pub(crate) fn with_root(handle: RigidBodyHandle) -> Self {
let mut mb = Multibody::new();
// NOTE: we have no way of knowing if the root in fixed at this point, so
// we mark it as dynamic and will fixe later with `Self::update_root_type`.
mb.root_is_dynamic = true;
let joint = MultibodyJoint::free(Isometry::identity());
mb.add_link(None, joint, handle);
@@ -747,6 +750,12 @@ impl Multibody {
self.velocities.rows(0, self.ndofs)
}
/// The body jacobian for link `link_id` calculated by the last call to [`Multibody::forward_kinematics`].
#[inline]
pub fn body_jacobian(&self, link_id: usize) -> &Jacobian<Real> {
&self.body_jacobians[link_id]
}
/// The mutable generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> {
@@ -762,17 +771,27 @@ impl Multibody {
}
/// Apply displacements, in generalized coordinates, to this multibody.
///
/// Note this does **not** updates the link poses, only their generalized coordinates.
/// To update the link poses and associated rigid-bodies, call [`Self::forward_kinematics`]
/// or [`Self::finalize`].
pub fn apply_displacements(&mut self, disp: &[Real]) {
for link in self.links.iter_mut() {
link.joint.apply_displacement(&disp[link.assembly_id..])
}
}
pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
pub(crate) fn update_root_type(&mut self, bodies: &RigidBodySet, take_body_pose: bool) {
if let Some(rb) = bodies.get(self.links[0].rigid_body) {
if rb.is_dynamic() != self.root_is_dynamic {
let root_pose = if take_body_pose {
*rb.position()
} else {
self.links[0].local_to_world
};
if rb.is_dynamic() {
let free_joint = MultibodyJoint::free(*rb.position());
let free_joint = MultibodyJoint::free(root_pose);
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = free_joint;
self.links[0].assembly_id = 0;
@@ -791,7 +810,7 @@ impl Multibody {
assert!(self.damping.len() >= SPATIAL_DIM);
assert!(self.accelerations.len() >= SPATIAL_DIM);
let fixed_joint = MultibodyJoint::fixed(*rb.position());
let fixed_joint = MultibodyJoint::fixed(root_pose);
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = fixed_joint;
self.links[0].assembly_id = 0;
@@ -820,30 +839,86 @@ impl Multibody {
}
// Make sure the positions are properly set to match the rigid-body’s.
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(*rb.position());
if take_body_pose {
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(*rb.position());
} else {
self.links[0].joint.data.local_frame1 = *rb.position();
}
}
}
}
/// Update the rigid-body poses based on this multibody joint poses.
///
/// This is typically called after [`Self::forward_kinematics`] to apply the new joint poses
/// to the rigid-bodies.
pub fn update_rigid_bodies(&self, bodies: &mut RigidBodySet, update_mass_properties: bool) {
self.update_rigid_bodies_internal(bodies, update_mass_properties, false, true)
}
pub(crate) fn update_rigid_bodies_internal(
&self,
bodies: &mut RigidBodySet,
update_mass_properties: bool,
update_next_positions_only: bool,
change_tracking: bool,
) {
// Handle the children. They all have a parent within this multibody.
for link in self.links.iter() {
let rb = if change_tracking {
bodies.get_mut_internal_with_modification_tracking(link.rigid_body)
} else {
self.links[0].joint.data.local_frame1 = *rb.position();
bodies.get_mut_internal(link.rigid_body)
};
if let Some(rb) = rb {
rb.pos.next_position = link.local_to_world;
if !update_next_positions_only {
rb.pos.position = link.local_to_world;
}
if update_mass_properties {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
}
}
// TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
// (i.e. just something used by the velocity solver’s small steps).
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
/// Apply forward-kinematics to this multibody.
///
/// This will update the [`MultibodyLink`] pose information as wall as the body jacobians.
/// This will also ensure that the multibody has the proper number of degrees of freedom if
/// its root node changed between dynamic and non-dynamic.
///
/// Note that this does **not** update the poses of the [`RigidBody`] attached to the joints.
/// Run [`Self::update_rigid_bodies`] to trigger that update.
///
/// This method updates `self` with the result of the forward-kinematics operation.
/// For a non-mutable version running forward kinematics on a single link, see
/// [`Self::forward_kinematics_single_link`].
///
/// ## Parameters
/// - `bodies`: the set of rigid-bodies.
/// - `read_root_pose_from_rigid_body`: if set to `true`, the root joint (either a fixed joint,
/// or a free joint) will have its pose set to its associated-rigid-body pose. Set this to `true`
/// when the root rigid-body pose has been modified and needs to affect the multibody.
pub fn forward_kinematics(
&mut self,
bodies: &RigidBodySet,
read_root_pose_from_rigid_body: bool,
) {
// Be sure the degrees of freedom match and take the root position if needed.
self.update_root_type(bodies, read_root_pose_from_rigid_body);
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
rb.pos.next_position = link.local_to_world;
if update_rb_mass_props {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
}
// Handle the children. They all have a parent within this multibody.
@@ -865,20 +940,11 @@ impl Multibody {
link.shift23 = c3 - c2;
}
let link_rb = bodies.index_mut_internal(link.rigid_body);
link_rb.pos.next_position = link.local_to_world;
assert_eq!(
link_rb.body_type,
bodies[link.rigid_body].body_type,
RigidBodyType::Dynamic,
"A rigid-body that is not at the root of a multibody must be dynamic."
);
if update_rb_mass_props {
link_rb
.mprops
.update_world_mass_properties(&link.local_to_world);
}
}
/*
@@ -887,6 +953,107 @@ impl Multibody {
self.update_body_jacobians();
}
/// Apply forward-kinematics to compute the position of a single link of this multibody.
///
/// If `out_jacobian` is `Some`, this will simultaneously compute the new jacobian of this link.
/// If `displacement` is `Some`, the generalized position considered during transform propagation
/// is the sum of the current position of `self` and this `displacement`.
// TODO: this shares a lot of code with `forward_kinematics` and `update_body_jacobians`, except
// that we are only traversing a single kinematic chain. Could this be refactored?
pub fn forward_kinematics_single_link(
&self,
bodies: &RigidBodySet,
link_id: usize,
displacement: Option<&[Real]>,
mut out_jacobian: Option<&mut Jacobian<Real>>,
) -> Isometry<Real> {
let mut branch = vec![]; // Perf: avoid allocation.
let mut curr_id = Some(link_id);
while let Some(id) = curr_id {
branch.push(id);
curr_id = self.links[id].parent_id();
}
branch.reverse();
if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
if out_jacobian.ncols() != self.ndofs {
*out_jacobian = Jacobian::zeros(self.ndofs);
} else {
out_jacobian.fill(0.0);
}
}
let mut parent_link: Option<MultibodyLink> = None;
for i in branch {
let mut link = self.links[i];
if let Some(displacement) = displacement {
link.joint
.apply_displacement(&displacement[link.assembly_id..]);
}
let parent_to_world;
if let Some(parent_link) = parent_link {
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = parent_link.local_to_world * link.local_to_parent;
{
let parent_rb = &bodies[parent_link.rigid_body];
let link_rb = &bodies[link.rigid_body];
let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
let c2 = link.local_to_world
* Point::from(link.joint.data.local_frame2.translation.vector);
let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
link.shift02 = c2 - c0;
link.shift23 = c3 - c2;
}
parent_to_world = parent_link.local_to_world;
if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
let (mut link_j_v, parent_j_w) =
out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
let shift_tr = (link.shift02).gcross_matrix_tr();
link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
}
} else {
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
parent_to_world = Isometry::identity();
}
if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
let ndofs = link.joint.ndofs();
let mut tmp = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
let mut link_joint_j = tmp.columns_mut(0, ndofs);
let mut link_j_part = out_jacobian.columns_mut(link.assembly_id, ndofs);
link.joint.jacobian(
&(parent_to_world.rotation * link.joint.data.local_frame1.rotation),
&mut link_joint_j,
);
link_j_part += link_joint_j;
{
let (mut link_j_v, link_j_w) =
out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
let shift_tr = link.shift23.gcross_matrix_tr();
link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
}
}
parent_link = Some(link);
}
parent_link
.map(|link| link.local_to_world)
.unwrap_or_else(Isometry::identity)
}
/// The total number of freedoms of this multibody.
#[inline]
pub fn ndofs(&self) -> usize {

238
src/dynamics/joint/multibody_joint/multibody_ik.rs Normal file
View File

@@ -0,0 +1,238 @@
use crate::dynamics::{JointAxesMask, Multibody, RigidBodySet};
use crate::math::{Isometry, Jacobian, Real, ANG_DIM, DIM, SPATIAL_DIM};
use na::{self, DVector, SMatrix};
use parry::math::SpacialVector;
#[derive(Copy, Clone, Debug, PartialEq)]
/// Options for the jacobian-based Inverse Kinematics solver for multibodies.
pub struct InverseKinematicsOption {
/// A damping coefficient.
///
/// Small value can lead to overshooting preventing convergence. Large
/// values can slown down convergence, requiring more iterations to converge.
pub damping: Real,
/// The maximum number of iterations the iterative IK solver can take.
pub max_iters: usize,
/// The axes the IK solver will solve for.
pub constrained_axes: JointAxesMask,
/// The error threshold on the linear error.
///
/// If errors on both linear and angular parts fall below this
/// threshold, the iterative resolution will stop.
pub epsilon_linear: Real,
/// The error threshold on the angular error.
///
/// If errors on both linear and angular parts fall below this
/// threshold, the iterative resolution will stop.
pub epsilon_angular: Real,
}
impl Default for InverseKinematicsOption {
fn default() -> Self {
Self {
damping: 1.0,
max_iters: 10,
constrained_axes: JointAxesMask::all(),
epsilon_linear: 1.0e-3,
epsilon_angular: 1.0e-3,
}
}
}
impl Multibody {
/// Computes the displacement needed to have the link identified by `link_id` move by the
/// desired transform.
///
/// The displacement calculated by this function is added to the `displacement` vector.
pub fn inverse_kinematics_delta(
&self,
link_id: usize,
desired_movement: &SpacialVector<Real>,
damping: Real,
displacements: &mut DVector<Real>,
) {
let body_jacobian = self.body_jacobian(link_id);
Self::inverse_kinematics_delta_with_jacobian(
body_jacobian,
desired_movement,
damping,
displacements,
);
}
/// Computes the displacement needed to have a link with the given jacobian move by the
/// desired transform.
///
/// The displacement calculated by this function is added to the `displacement` vector.
pub fn inverse_kinematics_delta_with_jacobian(
jacobian: &Jacobian<Real>,
desired_movement: &SpacialVector<Real>,
damping: Real,
displacements: &mut DVector<Real>,
) {
let identity = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::identity();
let jj = jacobian * &jacobian.transpose() + identity * (damping * damping);
let inv_jj = jj.pseudo_inverse(1.0e-5).unwrap_or(identity);
displacements.gemv_tr(1.0, jacobian, &(inv_jj * desired_movement), 1.0);
}
/// Computes the displacement needed to have the link identified by `link_id` have a pose
/// equal (or as close as possible) to `target_pose`.
///
/// If `displacement` is given non-zero, the current pose of the rigid-body is considered to be
/// obtained from its current generalized coordinates summed with the `displacement` vector.
///
/// The `displacements` vector is overwritten with the new displacement.
pub fn inverse_kinematics(
&self,
bodies: &RigidBodySet,
link_id: usize,
options: &InverseKinematicsOption,
target_pose: &Isometry<Real>,
displacements: &mut DVector<Real>,
) {
let mut jacobian = Jacobian::zeros(0);
for _ in 0..options.max_iters {
let pose = self.forward_kinematics_single_link(
bodies,
link_id,
Some(displacements.as_slice()),
Some(&mut jacobian),
);
let delta_lin = target_pose.translation.vector - pose.translation.vector;
let delta_ang = (target_pose.rotation * pose.rotation.inverse()).scaled_axis();
#[cfg(feature = "dim2")]
let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang.x];
#[cfg(feature = "dim3")]
let mut delta = na::vector![
delta_lin.x,
delta_lin.y,
delta_lin.z,
delta_ang.x,
delta_ang.y,
delta_ang.z
];
if !options.constrained_axes.contains(JointAxesMask::X) {
delta[0] = 0.0;
}
if !options.constrained_axes.contains(JointAxesMask::Y) {
delta[1] = 0.0;
}
#[cfg(feature = "dim3")]
if !options.constrained_axes.contains(JointAxesMask::Z) {
delta[2] = 0.0;
}
if !options.constrained_axes.contains(JointAxesMask::ANG_X) {
delta[DIM] = 0.0;
}
#[cfg(feature = "dim3")]
if !options.constrained_axes.contains(JointAxesMask::ANG_Y) {
delta[DIM + 1] = 0.0;
}
#[cfg(feature = "dim3")]
if !options.constrained_axes.contains(JointAxesMask::ANG_Z) {
delta[DIM + 2] = 0.0;
}
// TODO: measure convergence on the error variation instead?
if delta.rows(0, DIM).norm() <= options.epsilon_linear
&& delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular
{
break;
}
Self::inverse_kinematics_delta_with_jacobian(
&jacobian,
&delta,
options.damping,
displacements,
);
}
}
}
#[cfg(test)]
mod test {
use crate::dynamics::{
MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder,
RigidBodySet,
};
use crate::math::{Jacobian, Real, Vector};
use approx::assert_relative_eq;
#[test]
fn one_link_fwd_kinematics() {
let mut bodies = RigidBodySet::new();
let mut multibodies = MultibodyJointSet::new();
let num_segments = 10;
let body = RigidBodyBuilder::fixed();
let mut last_body = bodies.insert(body);
let mut last_link = MultibodyJointHandle::invalid();
for _ in 0..num_segments {
let body = RigidBodyBuilder::dynamic().can_sleep(false);
let new_body = bodies.insert(body);
#[cfg(feature = "dim2")]
let builder = RevoluteJointBuilder::new();
#[cfg(feature = "dim3")]
let builder = RevoluteJointBuilder::new(Vector::z_axis());
let link_ab = builder
.local_anchor1((Vector::y() * (0.5 / num_segments as Real)).into())
.local_anchor2((Vector::y() * (-0.5 / num_segments as Real)).into());
last_link = multibodies
.insert(last_body, new_body, link_ab, true)
.unwrap();
last_body = new_body;
}
let (multibody, last_id) = multibodies.get_mut(last_link).unwrap();
multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date.
assert_eq!(multibody.ndofs(), num_segments);
/*
* No displacement.
*/
let mut jacobian2 = Jacobian::zeros(0);
let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
let jacobian1 = multibody.body_jacobian(last_id);
let link_pose2 =
multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2));
assert_eq!(link_pose1, link_pose2);
assert_eq!(jacobian1, &jacobian2);
/*
* Arbitrary displacement.
*/
let niter = 100;
let displacement_part: Vec<_> = (0..multibody.ndofs())
.map(|i| i as Real * -0.1 / niter as Real)
.collect();
let displacement_total: Vec<_> = displacement_part
.iter()
.map(|d| *d * niter as Real)
.collect();
let link_pose2 = multibody.forward_kinematics_single_link(
&bodies,
last_id,
Some(&displacement_total),
Some(&mut jacobian2),
);
for _ in 0..niter {
multibody.apply_displacements(&displacement_part);
multibody.forward_kinematics(&bodies, false);
}
let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
let jacobian1 = multibody.body_jacobian(last_id);
assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5);
assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5);
}
}

11
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -286,6 +286,13 @@ impl MultibodyJointSet {
self.multibodies.get(index.0)
}
/// Gets a mutable reference to a multibody, based on its temporary index.
/// `MultibodyJointSet`.
pub fn get_multibody_mut(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
// TODO: modification tracking.
self.multibodies.get_mut(index.0)
}
/// Gets a mutable reference to a multibody, based on its temporary index.
///
/// This method will bypass any modification-detection automatically done by the
@@ -363,13 +370,13 @@ impl MultibodyJointSet {
let parent1 = link1.parent_id();
if parent1 == Some(id2.id) {
Some((MultibodyJointHandle(rb1.0), mb, &link1))
Some((MultibodyJointHandle(rb1.0), mb, link1))
} else {
let link2 = mb.link(id2.id)?;
let parent2 = link2.parent_id();
if parent2 == Some(id1.id) {
Some((MultibodyJointHandle(rb2.0), mb, &link2))
Some((MultibodyJointHandle(rb2.0), mb, link2))
} else {
None
}

2
src/dynamics/joint/multibody_joint/multibody_link.rs
View File

@@ -6,7 +6,7 @@ use crate::prelude::RigidBodyVelocity;
/// One link of a multibody.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
#[derive(Copy, Clone)]
pub struct MultibodyLink {
// FIXME: make all those private.
pub(crate) internal_id: usize,

159
src/dynamics/rigid_body.rs
View File

@@ -74,6 +74,61 @@ impl RigidBody {
self.ids = Default::default();
}
/// Copy all the characteristics from `other` to `self`.
///
/// If you have a mutable reference to a rigid-body `rigid_body: &mut RigidBody`, attempting to
/// assign it a whole new rigid-body instance, e.g., `*rigid_body = RigidBodyBuilder::dynamic().build()`,
/// will crash due to some internal indices being overwritten. Instead, use
/// `rigid_body.copy_from(&RigidBodyBuilder::dynamic().build())`.
///
/// This method will allow you to set most characteristics of this rigid-body from another
/// rigid-body instance without causing any breakage.
///
/// This method **cannot** be used for editing the list of colliders attached to this rigid-body.
/// Therefore, the list of colliders attached to `self` won’t be replaced by the one attached
/// to `other`.
///
/// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has
/// a parent, its position is directly controlled by the parent rigid-body).
pub fn copy_from(&mut self, other: &RigidBody) {
// NOTE: we deconstruct the rigid-body struct to be sure we don’t forget to
// add some copies here if we add more field to RigidBody in the future.
let RigidBody {
pos,
mprops,
integrated_vels,
vels,
damping,
forces,
ccd,
ids: _ids, // Internal ids must not be overwritten.
colliders: _colliders, // This function cannot be used to edit collider sets.
activation,
changes: _changes, // Will be set to ALL.
body_type,
dominance,
enabled,
additional_solver_iterations,
user_data,
} = other;
self.pos = *pos;
self.mprops = mprops.clone();
self.integrated_vels = *integrated_vels;
self.vels = *vels;
self.damping = *damping;
self.forces = *forces;
self.ccd = *ccd;
self.activation = *activation;
self.body_type = *body_type;
self.dominance = *dominance;
self.enabled = *enabled;
self.additional_solver_iterations = *additional_solver_iterations;
self.user_data = *user_data;
self.changes = RigidBodyChanges::all();
}
/// Set the additional number of solver iterations run for this rigid-body and
/// everything interacting with it.
///
@@ -237,9 +292,9 @@ impl RigidBody {
allow_rotations_z: bool,
wake_up: bool,
) {
if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z
if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) == allow_rotations_x
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z
{
if self.is_dynamic() && wake_up {
self.wake_up(true);
@@ -381,18 +436,40 @@ impl RigidBody {
]
}
/// Enables of disable CCD (continuous collision-detection) for this rigid-body.
/// Enables of disable CCD (Continuous Collision-Detection) for this rigid-body.
///
/// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
pub fn enable_ccd(&mut self, enabled: bool) {
self.ccd.ccd_enabled = enabled;
}
/// Is CCD (continous collision-detection) enabled for this rigid-body?
/// Is CCD (continuous collision-detection) enabled for this rigid-body?
pub fn is_ccd_enabled(&self) -> bool {
self.ccd.ccd_enabled
}
/// Sets the maximum prediction distance Soft Continuous Collision-Detection.
///
/// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of
/// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
/// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
/// performance badly by increasing the work needed from the broad-phase.
///
/// It is a generally cheaper variant of regular CCD (that can be enabled with
/// [`RigidBody::enable_ccd`] since it relies on predictive constraints instead of
/// shape-cast and substeps.
pub fn set_soft_ccd_prediction(&mut self, prediction_distance: Real) {
self.ccd.soft_ccd_prediction = prediction_distance;
}
/// The soft-CCD prediction distance for this rigid-body.
///
/// See the documentation of [`RigidBody::set_soft_ccd_prediction`] for additional details on
/// soft-CCD.
pub fn soft_ccd_prediction(&self) -> Real {
self.ccd.soft_ccd_prediction
}
// This is different from `is_ccd_enabled`. This checks that CCD
// is active for this rigid-body, i.e., if it was seen to move fast
// enough to justify a CCD run.
@@ -810,6 +887,25 @@ impl RigidBody {
}
}
/// Predicts the next position of this rigid-body, by integrating its velocity and forces
/// by a time of `dt`.
pub(crate) fn predict_position_using_velocity_and_forces_with_max_dist(
&self,
dt: Real,
max_dist: Real,
) -> Isometry<Real> {
let new_vels = self.forces.integrate(dt, &self.vels, &self.mprops);
// Compute the clamped dt such that the body doesn’t travel more than `max_dist`.
let linvel_norm = new_vels.linvel.norm();
let clamped_linvel = linvel_norm.min(max_dist * crate::utils::inv(dt));
let clamped_dt = dt * clamped_linvel * crate::utils::inv(linvel_norm);
new_vels.integrate(
clamped_dt,
&self.pos.position,
&self.mprops.local_mprops.local_com,
)
}
/// Predicts the next position of this rigid-body, by integrating its velocity and forces
/// by a time of `dt`.
pub fn predict_position_using_velocity_and_forces(&self, dt: Real) -> Isometry<Real> {
@@ -817,6 +913,17 @@ impl RigidBody {
.integrate_forces_and_velocities(dt, &self.forces, &self.vels, &self.mprops)
}
/// Predicts the next position of this rigid-body, by integrating only its velocity
/// by a time of `dt`.
///
/// The forces that were applied to this rigid-body since the last physics step will
/// be ignored by this function. Use [`Self::predict_position_using_velocity_and_forces`]
/// instead to take forces into account.
pub fn predict_position_using_velocity(&self, dt: Real) -> Isometry<Real> {
self.vels
.integrate(dt, &self.pos.position, &self.mprops.local_mprops.local_com)
}
pub(crate) fn update_world_mass_properties(&mut self) {
self.mprops.update_world_mass_properties(&self.pos.position);
}
@@ -1031,14 +1138,25 @@ pub struct RigidBodyBuilder {
mprops_flags: LockedAxes,
/// The additional mass-properties of the rigid-body being built. See [`RigidBodyBuilder::additional_mass_properties`] for more information.
additional_mass_properties: RigidBodyAdditionalMassProps,
/// Whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
/// Whether the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
pub can_sleep: bool,
/// Whether or not the rigid-body is to be created asleep.
/// Whether the rigid-body is to be created asleep.
pub sleeping: bool,
/// Whether continuous collision-detection is enabled for the rigid-body to be built.
/// Whether Continuous Collision-Detection is enabled for the rigid-body to be built.
///
/// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
pub ccd_enabled: bool,
/// The maximum prediction distance Soft Continuous Collision-Detection.
///
/// When set to 0, soft CCD is disabled. Soft-CCD helps prevent tunneling especially of
/// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
/// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
/// performance badly by increasing the work needed from the broad-phase.
///
/// It is a generally cheaper variant of regular CCD (that can be enabled with
/// [`RigidBodyBuilder::ccd_enabled`] since it relies on predictive constraints instead of
/// shape-cast and substeps.
pub soft_ccd_prediction: Real,
/// The dominance group of the rigid-body to be built.
pub dominance_group: i8,
/// Will the rigid-body being built be enabled?
@@ -1068,6 +1186,7 @@ impl RigidBodyBuilder {
can_sleep: true,
sleeping: false,
ccd_enabled: false,
soft_ccd_prediction: 0.0,
dominance_group: 0,
enabled: true,
user_data: 0,
@@ -1306,13 +1425,13 @@ impl RigidBodyBuilder {
self
}
/// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
/// Sets whether the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
pub fn can_sleep(mut self, can_sleep: bool) -> Self {
self.can_sleep = can_sleep;
self
}
/// Sets whether or not continuous collision-detection is enabled for this rigid-body.
/// Sets whether Continuous Collision-Detection is enabled for this rigid-body.
///
/// CCD prevents tunneling, but may still allow limited interpenetration of colliders.
pub fn ccd_enabled(mut self, enabled: bool) -> Self {
@@ -1320,7 +1439,22 @@ impl RigidBodyBuilder {
self
}
/// Sets whether or not the rigid-body is to be created asleep.
/// Sets the maximum prediction distance Soft Continuous Collision-Detection.
///
/// When set to 0, soft-CCD is disabled. Soft-CCD helps prevent tunneling especially of
/// slow-but-thin to moderately fast objects. The soft CCD prediction distance indicates how
/// far in the object’s path the CCD algorithm is allowed to inspect. Large values can impact
/// performance badly by increasing the work needed from the broad-phase.
///
/// It is a generally cheaper variant of regular CCD (that can be enabled with
/// [`RigidBodyBuilder::ccd_enabled`] since it relies on predictive constraints instead of
/// shape-cast and substeps.
pub fn soft_ccd_prediction(mut self, prediction_distance: Real) -> Self {
self.soft_ccd_prediction = prediction_distance;
self
}
/// Sets whether the rigid-body is to be created asleep.
pub fn sleeping(mut self, sleeping: bool) -> Self {
self.sleeping = sleeping;
self
@@ -1357,13 +1491,14 @@ impl RigidBodyBuilder {
rb.dominance = RigidBodyDominance(self.dominance_group);
rb.enabled = self.enabled;
rb.enable_ccd(self.ccd_enabled);
rb.set_soft_ccd_prediction(self.soft_ccd_prediction);
if self.can_sleep && self.sleeping {
rb.sleep();
}
if !self.can_sleep {
rb.activation.linear_threshold = -1.0;
rb.activation.normalized_linear_threshold = -1.0;
rb.activation.angular_threshold = -1.0;
}

23
src/dynamics/rigid_body_components.rs
View File

@@ -571,10 +571,11 @@ impl RigidBodyVelocity {
/// The approximate kinetic energy of this rigid-body.
///
/// This approximation does not take the rigid-body's mass and angular inertia
/// into account.
/// into account. Some physics engines call this the "mass-normalized kinetic
/// energy".
#[must_use]
pub fn pseudo_kinetic_energy(&self) -> Real {
self.linvel.norm_squared() + self.angvel.gdot(self.angvel)
0.5 * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel))
}
/// Returns the update velocities after applying the given damping.
@@ -594,7 +595,7 @@ impl RigidBodyVelocity {
}
/// Integrate the velocities in `self` to compute obtain new positions when moving from the given
/// inital position `init_pos`.
/// initial position `init_pos`.
#[must_use]
pub fn integrate(
&self,
@@ -821,6 +822,8 @@ pub struct RigidBodyCcd {
pub ccd_active: bool,
/// Is CCD enabled for this rigid-body?
pub ccd_enabled: bool,
/// The soft-CCD prediction distance for this rigid-body.
pub soft_ccd_prediction: Real,
}
impl Default for RigidBodyCcd {
@@ -830,6 +833,7 @@ impl Default for RigidBodyCcd {
ccd_max_dist: 0.0,
ccd_active: false,
ccd_enabled: false,
soft_ccd_prediction: 0.0,
}
}
}
@@ -992,7 +996,10 @@ impl RigidBodyDominance {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct RigidBodyActivation {
/// The threshold linear velocity bellow which the body can fall asleep.
pub linear_threshold: Real,
///
/// The value is "normalized", i.e., the actual threshold applied by the physics engine
/// is equal to this value multiplied by [`IntegrationParameters::length_unit`].
pub normalized_linear_threshold: Real,
/// The angular linear velocity bellow which the body can fall asleep.
pub angular_threshold: Real,
/// The amount of time the rigid-body must remain below the thresholds to be put to sleep.
@@ -1011,7 +1018,7 @@ impl Default for RigidBodyActivation {
impl RigidBodyActivation {
/// The default linear velocity bellow which a body can be put to sleep.
pub fn default_linear_threshold() -> Real {
pub fn default_normalized_linear_threshold() -> Real {
0.4
}
@@ -1029,7 +1036,7 @@ impl RigidBodyActivation {
/// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
pub fn active() -> Self {
RigidBodyActivation {
linear_threshold: Self::default_linear_threshold(),
normalized_linear_threshold: Self::default_normalized_linear_threshold(),
angular_threshold: Self::default_angular_threshold(),
time_until_sleep: Self::default_time_until_sleep(),
time_since_can_sleep: 0.0,
@@ -1040,7 +1047,7 @@ impl RigidBodyActivation {
/// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
pub fn inactive() -> Self {
RigidBodyActivation {
linear_threshold: Self::default_linear_threshold(),
normalized_linear_threshold: Self::default_normalized_linear_threshold(),
angular_threshold: Self::default_angular_threshold(),
time_until_sleep: Self::default_time_until_sleep(),
time_since_can_sleep: Self::default_time_until_sleep(),
@@ -1051,7 +1058,7 @@ impl RigidBodyActivation {
/// Create a new activation status that prevents the rigid-body from sleeping.
pub fn cannot_sleep() -> Self {
RigidBodyActivation {
linear_threshold: -1.0,
normalized_linear_threshold: -1.0,
angular_threshold: -1.0,
..Self::active()
}

2
src/dynamics/rigid_body_set.rs
View File

@@ -209,7 +209,7 @@ impl RigidBodySet {
/// Update colliders positions after rigid-bodies moved.
///
/// When a rigid-body moves, the positions of the colliders attached to it need to be updated.
/// This update is generally automatically done at the beggining and the end of each simulation
/// This update is generally automatically done at the beginning and the end of each simulation
/// step with `PhysicsPipeline::step`. If the positions need to be updated without running a
/// simulation step (for example when using the `QueryPipeline` alone), this method can be called
/// manually.

12
src/dynamics/solver/contact_constraint/contact_constraints_set.rs
View File

@@ -25,6 +25,7 @@ use {
crate::math::SIMD_WIDTH,
};
#[derive(Debug)]
pub struct ConstraintsCounts {
pub num_constraints: usize,
pub num_jacobian_lines: usize,
@@ -441,6 +442,17 @@ impl ContactConstraintsSet {
assert_eq!(curr_start, total_num_constraints);
}
pub fn warmstart(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.warmstart(jac, solver_vels, generic_solver_vels);
}
}
pub fn solve_restitution(
&mut self,
solver_vels: &mut [SolverVel<Real>],

30
src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
View File

@@ -153,8 +153,9 @@ impl GenericOneBodyConstraintBuilder {
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
impulse_accumulator: na::zero(),
r,
r_mat_elts: [0.0; 2],
};
}
@@ -230,13 +231,8 @@ impl GenericOneBodyConstraintBuilder {
.unwrap()
.local_to_world;
self.inner.update_with_positions(
params,
solved_dt,
pos2,
self.ccd_thickness,
&mut constraint.inner,
);
self.inner
.update_with_positions(params, solved_dt, pos2, &mut constraint.inner);
}
}
@@ -258,6 +254,24 @@ impl GenericOneBodyConstraint {
}
}
pub fn warmstart(
&mut self,
jacobians: &DVector<Real>,
generic_solver_vels: &mut DVector<Real>,
) {
let solver_vel2 = self.inner.solver_vel2;
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
OneBodyConstraintElement::generic_warmstart_group(
elements,
jacobians,
self.ndofs2,
self.j_id,
solver_vel2,
generic_solver_vels,
);
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,

86
src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
View File

@@ -7,6 +7,40 @@ use na::DVector;
use na::SimdPartialOrd;
impl OneBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[cfg(feature = "dim3")]
{
let j_step = ndofs2 * 2;
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
);
}
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -71,6 +105,22 @@ impl OneBodyConstraintTangentPart<Real> {
}
impl OneBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -99,6 +149,42 @@ impl OneBodyConstraintNormalPart<Real> {
}
impl OneBodyConstraintElement<Real> {
#[inline]
pub fn generic_warmstart_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
let j_step = ndofs2 * 2 * DIM;
// Solve penetration.
let mut nrm_j_id = j_id;
for element in elements.iter_mut() {
element.normal_part.generic_warmstart(
nrm_j_id,
jacobians,
ndofs2,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
// Solve friction.
let mut tng_j_id = j_id + ndofs2 * 2;
for element in elements.iter_mut() {
let part = &mut element.tangent_part;
part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels);
tng_j_id += j_step;
}
}
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,

62
src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
View File

@@ -201,15 +201,17 @@ impl GenericTwoBodyConstraintBuilder {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
total_impulse: na::zero(),
impulse: na::zero(),
impulse_accumulator: na::zero(),
impulse: manifold_point.warmstart_impulse,
r,
r_mat_elts: [0.0; 2],
};
}
// Tangent parts.
{
constraint.inner.elements[k].tangent_part.impulse = na::zero();
constraint.inner.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -340,14 +342,8 @@ impl GenericTwoBodyConstraintBuilder {
.map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
self.inner.update_with_positions(
params,
solved_dt,
pos1,
pos2,
self.ccd_thickness,
&mut constraint.inner,
);
self.inner
.update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner);
}
}
@@ -373,6 +369,50 @@ impl GenericTwoBodyConstraint {
}
}
pub fn warmstart(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
} else {
GenericRhs::GenericId(self.inner.solver_vel1)
};
let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
} else {
GenericRhs::GenericId(self.inner.solver_vel2)
};
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
TwoBodyConstraintElement::generic_warmstart_group(
elements,
jacobians,
&self.inner.dir1,
#[cfg(feature = "dim3")]
&self.inner.tangent1,
&self.inner.im1,
&self.inner.im2,
self.ndofs1,
self.ndofs2,
self.j_id,
&mut solver_vel1,
&mut solver_vel2,
generic_solver_vels,
);
if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
solver_vels[self.inner.solver_vel1] = solver_vel1;
}
if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
solver_vels[self.inner.solver_vel2] = solver_vel2;
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,

196
src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
View File

@@ -88,6 +88,95 @@ impl GenericRhs {
}
impl TwoBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id: usize,
jacobians: &DVector<Real>,
tangents1: [&Vector<Real>; DIM - 1],
im1: &Vector<Real>,
im2: &Vector<Real>,
ndofs1: usize,
ndofs2: usize,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
#[cfg(feature = "dim3")]
let j_step = j_step(ndofs1, ndofs2);
#[cfg(feature = "dim2")]
{
solver_vel1.apply_impulse(
j_id1,
ndofs1,
self.impulse[0],
jacobians,
tangents1[0],
&self.gcross1[0],
solver_vels,
im1,
);
solver_vel2.apply_impulse(
j_id2,
ndofs2,
self.impulse[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
solver_vels,
im2,
);
}
#[cfg(feature = "dim3")]
{
solver_vel1.apply_impulse(
j_id1,
ndofs1,
self.impulse[0],
jacobians,
tangents1[0],
&self.gcross1[0],
solver_vels,
im1,
);
solver_vel1.apply_impulse(
j_id1 + j_step,
ndofs1,
self.impulse[1],
jacobians,
tangents1[1],
&self.gcross1[1],
solver_vels,
im1,
);
solver_vel2.apply_impulse(
j_id2,
ndofs2,
self.impulse[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
solver_vels,
im2,
);
solver_vel2.apply_impulse(
j_id2 + j_step,
ndofs2,
self.impulse[1],
jacobians,
&-tangents1[1],
&self.gcross2[1],
solver_vels,
im2,
);
}
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -240,6 +329,45 @@ impl TwoBodyConstraintTangentPart<Real> {
}
impl TwoBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id: usize,
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
im1: &Vector<Real>,
im2: &Vector<Real>,
ndofs1: usize,
ndofs2: usize,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
solver_vel1.apply_impulse(
j_id1,
ndofs1,
self.impulse,
jacobians,
dir1,
&self.gcross1,
solver_vels,
im1,
);
solver_vel2.apply_impulse(
j_id2,
ndofs2,
self.impulse,
jacobians,
&-dir1,
&self.gcross2,
solver_vels,
im2,
);
}
#[inline]
pub fn generic_solve(
&mut self,
@@ -290,6 +418,74 @@ impl TwoBodyConstraintNormalPart<Real> {
}
impl TwoBodyConstraintElement<Real> {
#[inline]
pub fn generic_warmstart_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
#[cfg(feature = "dim3")] tangent1: &Vector<Real>,
im1: &Vector<Real>,
im2: &Vector<Real>,
// ndofs is 0 for a non-multibody body, or a multibody with zero
// degrees of freedom.
ndofs1: usize,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
solver_vel1: &mut GenericRhs,
solver_vel2: &mut GenericRhs,
solver_vels: &mut DVector<Real>,
) {
let j_step = j_step(ndofs1, ndofs2) * DIM;
// Solve penetration.
{
let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
element.normal_part.generic_warmstart(
nrm_j_id,
jacobians,
dir1,
im1,
im2,
ndofs1,
ndofs2,
solver_vel1,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
}
// Solve friction.
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
let part = &mut element.tangent_part;
part.generic_warmstart(
tng_j_id,
jacobians,
tangents1,
im1,
im2,
ndofs1,
ndofs2,
solver_vel1,
solver_vel2,
solver_vels,
);
tng_j_id += j_step;
}
}
}
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,

116
src/dynamics/solver/contact_constraint/one_body_constraint.rs
View File

@@ -3,8 +3,10 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
use na::Matrix2;
use parry::math::Isometry;
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
@@ -130,10 +132,11 @@ impl OneBodyConstraintBuilder {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let projected_mass = utils::inv(
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2),
);
let projected_lin_mass =
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1));
let projected_ang_mass = gcross2.gdot(gcross2);
let projected_mass = utils::inv(projected_lin_mass + projected_ang_mass);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
@@ -148,15 +151,17 @@ impl OneBodyConstraintBuilder {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
impulse: manifold_point.warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [0.0; 2],
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross2 = mprops2
@@ -205,6 +210,44 @@ impl OneBodyConstraintBuilder {
builder.infos[k] = infos;
}
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..manifold_points.len() / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let mut r_mat = Matrix2::zeros();
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
r_mat.m12 = force_dir1
.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m21 = r_mat.m12;
r_mat.m11 = utils::inv(r0);
r_mat.m22 = utils::inv(r1);
if let Some(inv) = r_mat.try_inverse() {
constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
} else {
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts =
if manifold_points[k0].dist <= manifold_points[k1].dist {
[r0, 0.0]
} else {
[0.0, r1]
};
constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
}
}
}
}
}
@@ -217,13 +260,7 @@ impl OneBodyConstraintBuilder {
constraint: &mut OneBodyConstraint,
) {
let rb2 = &bodies[constraint.solver_vel2];
self.update_with_positions(
params,
solved_dt,
&rb2.position,
rb2.ccd_thickness,
constraint,
)
self.update_with_positions(params, solved_dt, &rb2.position, constraint)
}
// TODO: this code is SOOOO similar to TwoBodyConstraint::update.
@@ -233,12 +270,11 @@ impl OneBodyConstraintBuilder {
params: &IntegrationParameters,
solved_dt: Real,
rb2_pos: &Isometry<Real>,
ccd_thickness: Real,
constraint: &mut OneBodyConstraint,
) {
let cfm_factor = params.cfm_factor();
let cfm_factor = params.contact_cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.contact_erp_inv_dt();
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
@@ -247,7 +283,6 @@ impl OneBodyConstraintBuilder {
let new_pos1 = self
.vels1
.integrate(solved_dt, &rb1.position, &rb1.local_com);
let mut is_fast_contact = false;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
@@ -266,23 +301,20 @@ impl OneBodyConstraintBuilder {
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = erp_inv_dt
* (dist + params.allowed_linear_error)
.clamp(-params.max_penetration_correction, 0.0);
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= params.warmstart_coefficient;
}
// Tangent part.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= params.warmstart_coefficient;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -291,7 +323,7 @@ impl OneBodyConstraintBuilder {
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
constraint.cfm_factor = cfm_factor;
}
}
@@ -328,6 +360,21 @@ impl OneBodyConstraint {
}
}
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = solver_vels[self.solver_vel2];
OneBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
&mut solver_vel2,
);
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -359,16 +406,11 @@ impl OneBodyConstraint {
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
}
active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
}

165
src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
View File

@@ -1,20 +1,17 @@
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, Vector, DIM};
use crate::math::{AngVector, TangentImpulse, Vector, DIM};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::Vector2;
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
pub rhs_wo_bias: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub total_impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub total_impulse: na::Vector2<N>,
pub impulse: TangentImpulse<N>,
pub impulse_accumulator: TangentImpulse<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
@@ -28,7 +25,7 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
rhs: [na::zero(); DIM - 1],
rhs_wo_bias: [na::zero(); DIM - 1],
impulse: na::zero(),
total_impulse: na::zero(),
impulse_accumulator: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
@@ -36,41 +33,31 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
}
}
/// Total impulse applied across all the solver substeps.
#[inline]
pub fn apply_limit(
pub fn total_impulse(&self) -> TangentImpulse<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
) {
#[cfg(feature = "dim2")]
{
let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
solver_vel2.angular += self.gcross2[0] * self.impulse[0];
}
#[cfg(feature = "dim3")]
{
let new_impulse = self.impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+ tangents1[1].component_mul(im2) * -self.impulse[1];
solver_vel2.angular +=
self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
}
}
@@ -137,8 +124,9 @@ pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub total_impulse: N,
pub impulse_accumulator: N,
pub r: N,
pub r_mat_elts: [N; 2],
}
impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
@@ -148,11 +136,24 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
impulse_accumulator: na::zero(),
r: na::zero(),
r_mat_elts: [N::zero(); 2],
}
}
/// Total impulse applied across all the solver substeps.
#[inline]
pub fn total_impulse(&self) -> N {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(&mut self, dir1: &Vector<N>, im2: &Vector<N>, solver_vel2: &mut SolverVel<N>) {
solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
solver_vel2.angular += self.gcross2 * self.impulse;
}
#[inline]
pub fn solve(
&mut self,
@@ -172,6 +173,44 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2 * dlambda;
}
#[inline]
pub fn solve_pair(
constraint_a: &mut Self,
constraint_b: &mut Self,
cfm_factor: N,
dir1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel_a = -dir1.dot(&solver_vel2.linear)
+ constraint_a.gcross2.gdot(solver_vel2.angular)
+ constraint_a.rhs;
let dvel_b = -dir1.dot(&solver_vel2.linear)
+ constraint_b.gcross2.gdot(solver_vel2.angular)
+ constraint_b.rhs;
let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints(
Vector2::new(dvel_a, dvel_b),
prev_impulse,
constraint_a.r,
constraint_b.r,
constraint_a.r_mat_elts,
constraint_b.r_mat_elts,
cfm_factor,
);
let dlambda = new_impulse - prev_impulse;
constraint_a.impulse = new_impulse.x;
constraint_b.impulse = new_impulse.y;
solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
}
}
#[derive(Copy, Clone, Debug)]
@@ -188,6 +227,25 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
}
}
#[inline]
pub fn warmstart_group(
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
element.normal_part.warmstart(dir1, im2, solver_vel2);
element.tangent_part.warmstart(tangents1, im2, solver_vel2);
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
@@ -210,13 +268,34 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im2, limit, solver_vel2);
if BLOCK_SOLVER_ENABLED {
for elements in elements.chunks_exact_mut(2) {
let [element_a, element_b] = elements else {
unreachable!()
};
OneBodyConstraintNormalPart::solve_pair(
&mut element_a.normal_part,
&mut element_b.normal_part,
cfm_factor,
dir1,
im2,
solver_vel2,
);
}
if elements.len() % 2 == 1 {
let element = elements.last_mut().unwrap();
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
}
} else {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
}
}
}

134
src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
View File

@@ -1,4 +1,5 @@
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::dynamics::{
@@ -7,14 +8,14 @@ use crate::dynamics::{
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::math::SimdBool;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
@@ -118,6 +119,11 @@ impl SimdOneBodyConstraintBuilder {
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let warmstart_impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
[ii][k]
.warmstart_tangent_impulse]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
@@ -153,14 +159,15 @@ impl SimdOneBodyConstraintBuilder {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
impulse: warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [SimdReal::zero(); 2],
};
}
// tangent parts.
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -200,6 +207,47 @@ impl SimdOneBodyConstraintBuilder {
builder.infos[k] = infos;
}
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..num_points / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
let mut r_mat = SdpMatrix2::zero();
r_mat.m12 = force_dir1.dot(&im2.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m11 = utils::simd_inv(r0);
r_mat.m22 = utils::simd_inv(r1);
let (inv, det) = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
r_mat.inverse_and_get_determinant_unchecked()
};
let is_invertible = det.simd_gt(SimdReal::zero());
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts = [
inv.m11.select(is_invertible, r0),
inv.m22.select(is_invertible, SimdReal::zero()),
];
constraint.elements[k1].normal_part.r_mat_elts = [
inv.m12.select(is_invertible, SimdReal::zero()),
r_mat.m12.select(is_invertible, SimdReal::zero()),
];
}
}
}
}
@@ -213,15 +261,14 @@ impl SimdOneBodyConstraintBuilder {
_multibodies: &MultibodyJointSet,
constraint: &mut OneBodyConstraintSimd,
) {
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
let all_infos = &self.infos[..constraint.num_contacts as usize];
@@ -242,7 +289,6 @@ impl SimdOneBodyConstraintBuilder {
constraint.dir1.cross(&constraint.tangent1),
];
let mut is_fast_contact = SimdBool::splat(false);
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -255,24 +301,20 @@ impl SimdOneBodyConstraintBuilder {
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* erp_inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact =
is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= warmstart_coeff;
}
// tangent parts.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= warmstart_coeff;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -281,7 +323,7 @@ impl SimdOneBodyConstraintBuilder {
}
}
constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
constraint.cfm_factor = cfm_factor;
}
}
@@ -301,6 +343,27 @@ pub(crate) struct OneBodyConstraintSimd {
}
impl OneBodyConstraintSimd {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
OneBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
&mut solver_vel2,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -334,26 +397,21 @@ impl OneBodyConstraintSimd {
// FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
#[cfg(feature = "dim2")]
let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
#[cfg(feature = "dim3")]
let tangent_impulses = self.elements[k].tangent_part.impulse;
let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
let tangent_impulses = self.elements[k].tangent_part.total_impulse();
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
active_contact.data.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}

139
src/dynamics/solver/contact_constraint/two_body_constraint.rs
View File

@@ -2,11 +2,12 @@ use super::{ContactConstraintTypes, ContactPointInfos};
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
use na::DVector;
use na::{DVector, Matrix2};
use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
@@ -23,6 +24,25 @@ impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> {
Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
}
}
pub fn warmstart(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.warmstart(solver_vels),
Self::TwoBodies(c) => c.warmstart(solver_vels),
Self::GenericOneBody(c) => c.warmstart(generic_jacobians, generic_solver_vels),
Self::GenericTwoBodies(c) => {
c.warmstart(generic_jacobians, solver_vels, generic_solver_vels)
}
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.warmstart(solver_vels),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.warmstart(solver_vels),
}
}
pub fn solve_restitution(
&mut self,
@@ -222,15 +242,17 @@ impl TwoBodyConstraintBuilder {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
impulse: manifold_point.warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [0.0; 2],
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross1 = mprops1
@@ -284,6 +306,48 @@ impl TwoBodyConstraintBuilder {
builder.infos[k] = infos;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..manifold_points.len() / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let mut r_mat = Matrix2::zeros();
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross1
.gdot(constraint.elements[k1].normal_part.gcross1)
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m21 = r_mat.m12;
r_mat.m11 = utils::inv(r0);
r_mat.m22 = utils::inv(r1);
if let Some(inv) = r_mat.try_inverse() {
constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
} else {
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts =
if manifold_points[k0].dist <= manifold_points[k1].dist {
[r0, 0.0]
} else {
[0.0, r1]
};
constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
}
}
}
}
}
@@ -297,15 +361,7 @@ impl TwoBodyConstraintBuilder {
) {
let rb1 = &bodies[constraint.solver_vel1];
let rb2 = &bodies[constraint.solver_vel2];
let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness;
self.update_with_positions(
params,
solved_dt,
&rb1.position,
&rb2.position,
ccd_thickness,
constraint,
)
self.update_with_positions(params, solved_dt, &rb1.position, &rb2.position, constraint)
}
// Used by both generic and non-generic builders..
@@ -315,18 +371,15 @@ impl TwoBodyConstraintBuilder {
solved_dt: Real,
rb1_pos: &Isometry<Real>,
rb2_pos: &Isometry<Real>,
ccd_thickness: Real,
constraint: &mut TwoBodyConstraint,
) {
let cfm_factor = params.cfm_factor();
let cfm_factor = params.contact_cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.contact_erp_inv_dt();
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
let mut is_fast_contact = false;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
@@ -344,23 +397,20 @@ impl TwoBodyConstraintBuilder {
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = erp_inv_dt
* (dist + params.allowed_linear_error)
.clamp(-params.max_penetration_correction, 0.0);
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= params.warmstart_coefficient;
}
// Tangent part.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= params.warmstart_coefficient;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -369,11 +419,30 @@ impl TwoBodyConstraintBuilder {
}
}
constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
constraint.cfm_factor = cfm_factor;
}
}
impl TwoBodyConstraint {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = solver_vels[self.solver_vel1];
let mut solver_vel2 = solver_vels[self.solver_vel2];
TwoBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
solver_vels[self.solver_vel1] = solver_vel1;
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -408,16 +477,10 @@ impl TwoBodyConstraint {
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse[0];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = self.elements[k].tangent_part.impulse;
}
active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
}

243
src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
View File

@@ -1,6 +1,9 @@
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, Vector, DIM};
use crate::math::{AngVector, TangentImpulse, Vector, DIM};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::Vector2;
use simba::simd::SimdValue;
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
@@ -13,9 +16,9 @@ pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub total_impulse: na::Vector1<N>,
pub impulse_accumulator: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub total_impulse: na::Vector2<N>,
pub impulse_accumulator: na::Vector2<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
@@ -30,7 +33,7 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
rhs: [na::zero(); DIM - 1],
rhs_wo_bias: [na::zero(); DIM - 1],
impulse: na::zero(),
total_impulse: na::zero(),
impulse_accumulator: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
@@ -38,13 +41,18 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
}
}
/// Total impulse applied across all the solver substeps.
#[inline]
pub fn apply_limit(
pub fn total_impulse(&self) -> TangentImpulse<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
@@ -52,37 +60,24 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
{
#[cfg(feature = "dim2")]
{
let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0];
solver_vel1.angular += self.gcross1[0] * self.impulse[0];
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
solver_vel2.angular += self.gcross2[0] * self.impulse[0];
}
#[cfg(feature = "dim3")]
{
let new_impulse = self.impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]
+ tangents1[1].component_mul(im1) * self.impulse[1];
solver_vel1.angular +=
self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1];
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+ tangents1[1].component_mul(im1) * dlambda[1];
solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+ tangents1[1].component_mul(im2) * -self.impulse[1];
solver_vel2.angular +=
self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
}
}
@@ -166,8 +161,13 @@ pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub total_impulse: N,
pub impulse_accumulator: N,
pub r: N,
// For coupled constraint pairs, even constraints store the
// diagonal of the projected mass matrix. Odd constraints
// store the off-diagonal element of the projected mass matrix,
// as well as the off-diagonal element of the inverse projected mass matrix.
pub r_mat_elts: [N; 2],
}
impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
@@ -178,11 +178,34 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
total_impulse: na::zero(),
impulse_accumulator: na::zero(),
r: na::zero(),
r_mat_elts: [N::zero(); 2],
}
}
/// Total impulse applied across all the solver substeps.
#[inline]
pub fn total_impulse(&self) -> N {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(
&mut self,
dir1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) {
solver_vel1.linear += dir1.component_mul(im1) * self.impulse;
solver_vel1.angular += self.gcross1 * self.impulse;
solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
solver_vel2.angular += self.gcross2 * self.impulse;
}
#[inline]
pub fn solve(
&mut self,
@@ -209,6 +232,83 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2 * dlambda;
}
#[inline(always)]
pub(crate) fn solve_mlcp_two_constraints(
dvel: Vector2<N>,
prev_impulse: Vector2<N>,
r_a: N,
r_b: N,
[r_mat11, r_mat22]: [N; 2],
[r_mat12, r_mat_inv12]: [N; 2],
cfm_factor: N,
) -> Vector2<N> {
let r_dvel = Vector2::new(
r_mat11 * dvel.x + r_mat12 * dvel.y,
r_mat12 * dvel.x + r_mat22 * dvel.y,
);
let new_impulse0 = prev_impulse - r_dvel;
let new_impulse1 = Vector2::new(prev_impulse.x - r_a * dvel.x, N::zero());
let new_impulse2 = Vector2::new(N::zero(), prev_impulse.y - r_b * dvel.y);
let new_impulse3 = Vector2::new(N::zero(), N::zero());
let keep0 = new_impulse0.x.simd_ge(N::zero()) & new_impulse0.y.simd_ge(N::zero());
let keep1 = new_impulse1.x.simd_ge(N::zero())
& (dvel.y + r_mat_inv12 * new_impulse1.x).simd_ge(N::zero());
let keep2 = new_impulse2.y.simd_ge(N::zero())
& (dvel.x + r_mat_inv12 * new_impulse2.y).simd_ge(N::zero());
let keep3 = dvel.x.simd_ge(N::zero()) & dvel.y.simd_ge(N::zero());
let selected3 = (new_impulse3 * cfm_factor).select(keep3, prev_impulse);
let selected2 = (new_impulse2 * cfm_factor).select(keep2, selected3);
let selected1 = (new_impulse1 * cfm_factor).select(keep1, selected2);
(new_impulse0 * cfm_factor).select(keep0, selected1)
}
#[inline]
pub fn solve_pair(
constraint_a: &mut Self,
constraint_b: &mut Self,
cfm_factor: N,
dir1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel_lin = dir1.dot(&solver_vel1.linear) - dir1.dot(&solver_vel2.linear);
let dvel_a = dvel_lin
+ constraint_a.gcross1.gdot(solver_vel1.angular)
+ constraint_a.gcross2.gdot(solver_vel2.angular)
+ constraint_a.rhs;
let dvel_b = dvel_lin
+ constraint_b.gcross1.gdot(solver_vel1.angular)
+ constraint_b.gcross2.gdot(solver_vel2.angular)
+ constraint_b.rhs;
let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
let new_impulse = Self::solve_mlcp_two_constraints(
Vector2::new(dvel_a, dvel_b),
prev_impulse,
constraint_a.r,
constraint_b.r,
constraint_a.r_mat_elts,
constraint_b.r_mat_elts,
cfm_factor,
);
let dlambda = new_impulse - prev_impulse;
constraint_a.impulse = new_impulse.x;
constraint_b.impulse = new_impulse.y;
solver_vel1.linear += dir1.component_mul(im1) * (dlambda.x + dlambda.y);
solver_vel1.angular += constraint_a.gcross1 * dlambda.x + constraint_b.gcross1 * dlambda.y;
solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
}
}
#[derive(Copy, Clone, Debug)]
@@ -225,6 +325,31 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
}
}
#[inline]
pub fn warmstart_group(
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
element
.normal_part
.warmstart(dir1, im1, im2, solver_vel1, solver_vel2);
element
.tangent_part
.warmstart(tangents1, im1, im2, solver_vel1, solver_vel2);
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
@@ -236,31 +361,53 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
solve_normal: bool,
solve_restitution: bool,
solve_friction: bool,
) where
Vector<N>: SimdBasis,
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
if solve_restitution {
if BLOCK_SOLVER_ENABLED {
for elements in elements.chunks_exact_mut(2) {
let [element_a, element_b] = elements else {
unreachable!()
};
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
TwoBodyConstraintNormalPart::solve_pair(
&mut element_a.normal_part,
&mut element_b.normal_part,
cfm_factor,
dir1,
im1,
im2,
solver_vel1,
solver_vel2,
);
}
// There is one constraint left to solve if there isn’t an even number.
if elements.len() % 2 == 1 {
let element = elements.last_mut().unwrap();
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
}
} else {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
}
}
}
// Solve friction.
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;

151
src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
View File

@@ -1,4 +1,5 @@
use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::dynamics::{
@@ -7,14 +8,14 @@ use crate::dynamics::{
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
MAX_MANIFOLD_POINTS, SIMD_WIDTH,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::math::SimdBool;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
@@ -101,6 +102,11 @@ impl TwoBodyConstraintBuilderSimd {
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let warmstart_impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
[ii][k]
.warmstart_tangent_impulse]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
@@ -137,14 +143,15 @@ impl TwoBodyConstraintBuilderSimd {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: SimdReal::splat(0.0),
total_impulse: SimdReal::splat(0.0),
impulse: warmstart_impulse,
impulse_accumulator: SimdReal::splat(0.0),
r: projected_mass,
r_mat_elts: [SimdReal::zero(); 2],
};
}
// tangent parts.
constraint.elements[k].tangent_part.impulse = na::zero();
constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -186,6 +193,52 @@ impl TwoBodyConstraintBuilderSimd {
builder.infos[k] = infos;
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..num_points / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let imsum = im1 + im2;
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
let mut r_mat = SdpMatrix2::zero();
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross1
.gdot(constraint.elements[k1].normal_part.gcross1)
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m11 = utils::simd_inv(r0);
r_mat.m22 = utils::simd_inv(r1);
let (inv, det) = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
r_mat.inverse_and_get_determinant_unchecked()
};
let is_invertible = det.simd_gt(SimdReal::zero());
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts = [
inv.m11.select(is_invertible, r0),
inv.m22.select(is_invertible, SimdReal::zero()),
];
constraint.elements[k1].normal_part.r_mat_elts = [
inv.m12.select(is_invertible, SimdReal::zero()),
r_mat.m12.select(is_invertible, SimdReal::zero()),
];
}
}
}
}
@@ -197,19 +250,16 @@ impl TwoBodyConstraintBuilderSimd {
_multibodies: &MultibodyJointSet,
constraint: &mut TwoBodyConstraintSimd,
) {
let cfm_factor = SimdReal::splat(params.cfm_factor());
let dt = SimdReal::splat(params.dt);
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness])
+ SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
@@ -224,7 +274,6 @@ impl TwoBodyConstraintBuilderSimd {
constraint.dir1.cross(&constraint.tangent1),
];
let mut is_fast_contact = SimdBool::splat(false);
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -237,24 +286,20 @@ impl TwoBodyConstraintBuilderSimd {
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = (dist + allowed_lin_err)
.simd_clamp(-max_penetration_correction, SimdReal::zero())
* erp_inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
let total_impulse = element.normal_part.total_impulse + element.normal_part.impulse;
is_fast_contact =
is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.total_impulse = total_impulse;
element.normal_part.impulse = na::zero();
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= warmstart_coeff;
}
// tangent parts.
{
element.tangent_part.total_impulse += element.tangent_part.impulse;
element.tangent_part.impulse = na::zero();
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= warmstart_coeff;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -263,7 +308,7 @@ impl TwoBodyConstraintBuilderSimd {
}
}
constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
constraint.cfm_factor = cfm_factor;
}
}
@@ -285,6 +330,38 @@ pub(crate) struct TwoBodyConstraintSimd {
}
impl TwoBodyConstraintSimd {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]),
};
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
TwoBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii);
}
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
@@ -328,26 +405,20 @@ impl TwoBodyConstraintSimd {
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
#[cfg(feature = "dim2")]
let tangent_impulses: [_; SIMD_WIDTH] = self.elements[k].tangent_part.impulse[0].into();
#[cfg(feature = "dim3")]
let tangent_impulses = self.elements[k].tangent_part.impulse;
let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
let tangent_impulses = self.elements[k].tangent_part.total_impulse();
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contact.data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}

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

@@ -41,14 +41,12 @@ impl IslandSolver {
joint_indices: &[JointIndex],
multibodies: &mut MultibodyJointSet,
) {
counters.solver.velocity_resolution_time.resume();
counters.solver.velocity_assembly_time.resume();
let num_solver_iterations = base_params.num_solver_iterations.get()
+ islands.active_island_additional_solver_iterations(island_id);
let mut params = *base_params;
params.dt /= num_solver_iterations as Real;
params.damping_ratio /= num_solver_iterations as Real;
// params.joint_damping_ratio /= num_solver_iterations as Real;
/*
*
@@ -76,8 +74,10 @@ impl IslandSolver {
&mut self.contact_constraints,
&mut self.joint_constraints,
);
counters.solver.velocity_assembly_time.pause();
// SOLVE
counters.solver.velocity_resolution_time.resume();
self.velocity_solver.solve_constraints(
&params,
num_solver_iterations,
@@ -86,8 +86,10 @@ impl IslandSolver {
&mut self.contact_constraints,
&mut self.joint_constraints,
);
counters.solver.velocity_resolution_time.pause();
// WRITEBACK
counters.solver.velocity_writeback_time.resume();
self.joint_constraints.writeback_impulses(impulse_joints);
self.contact_constraints.writeback_impulses(manifolds);
self.velocity_solver.writeback_bodies(
@@ -98,6 +100,6 @@ impl IslandSolver {
bodies,
multibodies,
);
counters.solver.velocity_resolution_time.pause();
counters.solver.velocity_writeback_time.pause();
}
}

78
src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

@@ -198,7 +198,7 @@ impl JointOneBodyConstraintBuilder {
if flipped {
std::mem::swap(&mut handle1, &mut handle2);
std::mem::swap(&mut joint_data.local_frame1, &mut joint_data.local_frame2);
joint_data.flip();
};
let rb1 = &bodies[handle1];
@@ -551,6 +551,82 @@ impl<N: SimdRealCopy> JointTwoBodyConstraintHelper<N> {
constraint
}
pub fn motor_linear_coupled<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &JointSolverBody<N, LANES>,
body2: &JointSolverBody<N, LANES>,
coupled_axes: u8,
motor_params: &MotorParameters<N>,
limits: Option<[N; 2]>,
writeback_id: WritebackId,
) -> JointTwoBodyConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = na::zero();
for i in 0..DIM {
if coupled_axes & (1 << i) != 0 {
let coeff = self.basis.column(i).dot(&self.lin_err);
lin_jac += self.basis.column(i) * coeff;
#[cfg(feature = "dim2")]
{
ang_jac1 += self.cmat1_basis[i] * coeff;
ang_jac2 += self.cmat2_basis[i] * coeff;
}
#[cfg(feature = "dim3")]
{
ang_jac1 += self.cmat1_basis.column(i) * coeff;
ang_jac2 += self.cmat2_basis.column(i) * coeff;
}
}
}
let dist = lin_jac.norm();
let inv_dist = crate::utils::simd_inv(dist);
lin_jac *= inv_dist;
ang_jac1 *= inv_dist;
ang_jac2 *= inv_dist;
let mut rhs_wo_bias = N::zero();
if motor_params.erp_inv_dt != N::zero() {
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
target_vel =
target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
};
rhs_wo_bias += -target_vel;
ang_jac1 = body1.sqrt_ii * ang_jac1;
ang_jac2 = body2.sqrt_ii * ang_jac2;
JointTwoBodyConstraint {
joint_id,
solver_vel1: body1.solver_vel,
solver_vel2: body2.solver_vel,
im1: body1.im,
im2: body2.im,
impulse: N::zero(),
impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse],
lin_jac,
ang_jac1,
ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
rhs: rhs_wo_bias,
rhs_wo_bias,
writeback_id,
}
}
pub fn lock_linear<const LANES: usize>(
&self,
params: &IntegrationParameters,

22
src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
View File

@@ -319,7 +319,6 @@ pub struct JointGenericVelocityOneBodyExternalConstraintBuilder {
joint_id: JointIndex,
joint: GenericJoint,
j_id: usize,
flipped: bool,
constraint_id: usize,
// These are solver body for both joints, except that
// the world_com is actually in local-space.
@@ -337,21 +336,20 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
jacobians: &mut DVector<Real>,
out_constraint_id: &mut usize,
) {
let mut joint_data = joint.data;
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let flipped = !bodies[handle2].is_dynamic();
let local_frame1 = if flipped {
if flipped {
std::mem::swap(&mut handle1, &mut handle2);
joint.data.local_frame2
} else {
joint.data.local_frame1
};
joint_data.flip();
}
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let frame1 = rb1.pos.position * local_frame1;
let frame1 = rb1.pos.position * joint_data.local_frame1;
let starting_j_id = *j_id;
@@ -394,11 +392,10 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
body1,
link2,
joint_id,
joint: joint.data,
joint: joint_data,
j_id: starting_j_id,
frame1,
local_body2,
flipped,
constraint_id: *out_constraint_id,
});
@@ -417,12 +414,7 @@ impl JointGenericVelocityOneBodyExternalConstraintBuilder {
// constraints. Could we make this more incremental?
let mb2 = &multibodies[self.link2.multibody];
let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world;
let frame2 = pos2
* if self.flipped {
self.joint.local_frame1
} else {
self.joint.local_frame2
};
let frame2 = pos2 * self.joint.local_frame2;
let joint_body2 = JointSolverBody {
world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space.

43
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -217,28 +217,26 @@ impl JointTwoBodyConstraint<Real, 1> {
}
if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
// if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 {
// let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 {
// Some([
// joint.limits[first_coupled_lin_axis_id].min,
// joint.limits[first_coupled_lin_axis_id].max,
// ])
// } else {
// None
// };
//
// out[len] = builder.motor_linear_coupled
// params,
// [joint_id],
// body1,
// body2,
// coupled_axes,
// &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
// limits,
// WritebackId::Motor(first_coupled_lin_axis_id),
// );
// len += 1;
// }
let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
Some([
joint.limits[first_coupled_lin_axis_id].min,
joint.limits[first_coupled_lin_axis_id].max,
])
} else {
None
};
out[len] = builder.motor_linear_coupled(
params,
[joint_id],
body1,
body2,
coupled_axes,
&joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
limits,
WritebackId::Motor(first_coupled_lin_axis_id),
);
len += 1;
}
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
@@ -350,6 +348,7 @@ impl JointTwoBodyConstraint<Real, 1> {
}
}
}
#[cfg(feature = "simd-is-enabled")]
impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
pub fn lock_axes(

3
src/dynamics/solver/solver_constraints_set.rs
View File

@@ -25,6 +25,7 @@ pub(crate) trait ConstraintTypes {
type SimdBuilderTwoBodies;
}
#[derive(Debug)]
pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> {
OneBody(&'a mut Constraints::OneBody),
TwoBodies(&'a mut Constraints::TwoBodies),
@@ -95,7 +96,7 @@ impl<Constraints: ConstraintTypes> SolverConstraintsSet<Constraints> {
}
}
#[allow(dead_code)] // Useful for debuging.
#[allow(dead_code)] // Useful for debugging.
pub fn print_counts(&self) {
println!("Solver constraints:");
println!(

9
src/dynamics/solver/solver_vel.rs
View File

@@ -1,7 +1,7 @@
use crate::math::{AngVector, Vector, SPATIAL_DIM};
use crate::utils::SimdRealCopy;
use na::{DVectorView, DVectorViewMut, Scalar};
use std::ops::{AddAssign, Sub};
use std::ops::{AddAssign, Sub, SubAssign};
#[derive(Copy, Clone, Debug, Default)]
#[repr(C)]
@@ -47,6 +47,13 @@ impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
}
}
impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
fn sub_assign(&mut self, rhs: Self) {
self.linear -= rhs.linear;
self.angular -= rhs.angular;
}
}
impl<N: SimdRealCopy> Sub for SolverVel<N> {
type Output = Self;

25
src/dynamics/solver/velocity_solver.rs
View File

@@ -178,6 +178,10 @@ impl VelocitySolver {
joint_constraints.update(params, multibodies, &self.solver_bodies);
contact_constraints.update(params, substep_id, multibodies, &self.solver_bodies);
if params.warmstart_coefficient != 0.0 {
contact_constraints.warmstart(&mut self.solver_vels, &mut self.generic_solver_vels);
}
for _ in 0..params.num_internal_pgs_iterations {
joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
@@ -201,9 +205,19 @@ impl VelocitySolver {
/*
* Resolution without bias.
*/
joint_constraints.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
.solve_restitution_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
if params.num_internal_stabilization_iterations > 0 {
for _ in 0..params.num_internal_stabilization_iterations {
joint_constraints
.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints.solve_restitution_wo_bias(
&mut self.solver_vels,
&mut self.generic_solver_vels,
);
}
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
}
}
}
@@ -239,8 +253,9 @@ impl VelocitySolver {
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities.copy_from(&solver_vels);
multibody.integrate(params.dt);
// PERF: we could have a mode where it doesn’t write back to the `bodies` yet.
multibody.forward_kinematics(bodies, !is_last_substep);
// PERF: don’t write back to the rigid-body poses `bodies` before the last step?
multibody.forward_kinematics(bodies, false);
multibody.update_rigid_bodies_internal(bodies, !is_last_substep, true, false);
if !is_last_substep {
// These are very expensive and not needed if we don’t

50
src/geometry/broad_phase.rs Normal file
View File

@@ -0,0 +1,50 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderSet};
use parry::math::Real;
/// An internal index stored in colliders by some broad-phase algorithms.
pub type BroadPhaseProxyIndex = u32;
/// Trait implemented by broad-phase algorithms supported by Rapier.
///
/// The task of a broad-phase algorithm is to detect potential collision pairs, usually based on
/// bounding volumes. The pairs must be conservative: it is OK to create a collision pair if
/// two objects don’t actually touch, but it is incorrect to remove a pair between two objects
/// that are still touching. In other words, it can have false-positive (though these induce
/// some computational overhead on the narrow-phase), but cannot have false-negative.
pub trait BroadPhase: Send + Sync + 'static {
/// Updates the broad-phase.
///
/// The results must be output through the `events` struct. The broad-phase algorithm is only
/// required to generate new events (i.e. no need to re-send an `AddPair` event if it was already
/// sent previously and no `RemovePair` happened since then). Sending redundant events is allowed
/// but can result in a slight computational overhead.
///
/// The `colliders` set is mutable only to provide access to
/// [`collider.set_internal_broad_phase_proxy_index`]. Other properties of the collider should
/// **not** be modified during the broad-phase update.
///
/// # Parameters
/// - `prediction_distance`: colliders that are not exactly touching, but closer to this
/// distance must form a collision pair.
/// - `colliders`: the set of colliders. Change detection with `collider.needs_broad_phase_update()`
/// can be relied on at this stage.
/// - `modified_colliders`: colliders that are know to be modified since the last update.
/// - `removed_colliders`: colliders that got removed since the last update. Any associated data
/// in the broad-phase should be removed by this call to `update`.
/// - `events`: the broad-phase’s output. They indicate what collision pairs need to be created
/// and what pairs need to be removed. It is OK to create pairs for colliders that don’t
/// actually collide (though this can increase computational overhead in the narrow-phase)
/// but it is important not to indicate removal of a collision pair if the underlying colliders
/// are still touching or closer than `prediction_distance`.
fn update(
&mut self,
dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
);
}

182
src/geometry/broad_phase_multi_sap/broad_phase.rs → src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
View File

@@ -1,12 +1,12 @@
use super::{
BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
};
use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
use crate::geometry::{
ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet,
ColliderShape,
BroadPhaseProxyIndex, Collider, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
ColliderSet,
};
use crate::math::Real;
use crate::math::{Isometry, Real};
use crate::prelude::{BroadPhase, RigidBodySet};
use crate::utils::IndexMut2;
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::HashMap;
@@ -74,7 +74,7 @@ use parry::utils::hashmap::HashMap;
/// broad-phase, as well as the Aabbs of all the regions part of this broad-phase.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhase {
pub struct BroadPhaseMultiSap {
proxies: SAPProxies,
layers: Vec<SAPLayer>,
smallest_layer: u8,
@@ -90,7 +90,7 @@ pub struct BroadPhase {
// Another alternative would be to remove ColliderProxyId and
// just use a Coarena. But this seems like it could use too
// much memory.
colliders_proxy_ids: HashMap<ColliderHandle, SAPProxyIndex>,
colliders_proxy_ids: HashMap<ColliderHandle, BroadPhaseProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
region_pool: SAPRegionPool, // To avoid repeated allocations.
// We could think serializing this workspace is useless.
@@ -114,16 +114,16 @@ pub struct BroadPhase {
reporting: HashMap<(u32, u32), bool>, // Workspace
}
impl Default for BroadPhase {
impl Default for BroadPhaseMultiSap {
fn default() -> Self {
Self::new()
}
}
impl BroadPhase {
impl BroadPhaseMultiSap {
/// Create a new empty broad-phase.
pub fn new() -> Self {
BroadPhase {
BroadPhaseMultiSap {
proxies: SAPProxies::new(),
layers: Vec::new(),
smallest_layer: 0,
@@ -138,7 +138,7 @@ impl BroadPhase {
///
/// For each colliders marked as removed, we make their containing layer mark
/// its proxy as pre-deleted. The actual proxy removal will happen at the end
/// of the `BroadPhase::update`.
/// of the `BroadPhaseMultiSap::update`.
fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
// For each removed collider, remove the corresponding proxy.
for removed in removed_colliders {
@@ -156,7 +156,7 @@ impl BroadPhase {
/// remove, the `complete_removal` method MUST be called to
/// complete the removal of these proxies, by actually removing them
/// from all the relevant layers/regions/axes.
fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) {
fn predelete_proxy(&mut self, proxy_index: BroadPhaseProxyIndex) {
if proxy_index == crate::INVALID_U32 {
// This collider has not been added to the broad-phase yet.
return;
@@ -353,13 +353,18 @@ impl BroadPhase {
prediction_distance: Real,
handle: ColliderHandle,
proxy_index: &mut u32,
collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
collider: &Collider,
next_position: Option<&Isometry<Real>>,
) -> bool {
let (co_pos, co_shape, co_changes) = collider;
let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
let mut aabb = co_shape
.compute_aabb(co_pos)
.loosened(prediction_distance / 2.0);
if let Some(next_position) = next_position {
let next_aabb = collider
.shape
.compute_aabb(next_position)
.loosened(collider.contact_skin() + prediction_distance / 2.0);
aabb.merge(&next_aabb);
}
if aabb.mins.coords.iter().any(|e| !e.is_finite())
|| aabb.maxs.coords.iter().any(|e| !e.is_finite())
@@ -378,7 +383,7 @@ impl BroadPhase {
prev_aabb = proxy.aabb;
proxy.aabb = aabb;
if co_changes.contains(ColliderChanges::SHAPE) {
if collider.changes.contains(ColliderChanges::SHAPE) {
// If the shape was changed, then we need to see if this proxy should be
// migrated to a larger layer. Indeed, if the shape was replaced by
// a much larger shape, we need to promote the proxy to a bigger layer
@@ -449,65 +454,6 @@ impl BroadPhase {
!layer.created_regions.is_empty()
}
/// Updates the broad-phase, taking into account the new collider positions.
pub fn update(
&mut self,
prediction_distance: Real,
colliders: &mut ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
if let Some(co) = colliders.get_mut_internal(*handle) {
if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
continue;
}
let mut new_proxy_id = co.bf_data.proxy_index;
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
(&co.pos, &co.shape, &co.changes),
) {
need_region_propagation = true;
}
if co.bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Make sure we have the new proxy index in case
// the collider was added for the first time.
co.bf_data = ColliderBroadPhaseData {
proxy_index: new_proxy_id,
};
}
}
}
// Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
if need_region_propagation {
self.propagate_created_regions();
}
// Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
self.update_layers_and_find_pairs(events);
// Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
// layers to possible remove regions from larger layers that would become empty that way.
self.complete_removals(colliders, removed_colliders);
}
/// Propagate regions from the smallest layers up to the larger layers.
///
/// Whenever a region is created on a layer `n`, then its Aabb must be
@@ -618,16 +564,90 @@ impl BroadPhase {
}
}
impl BroadPhase for BroadPhaseMultiSap {
/// Updates the broad-phase, taking into account the new collider positions.
fn update(
&mut self,
dt: Real,
prediction_distance: Real,
colliders: &mut ColliderSet,
bodies: &RigidBodySet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
// Phase 1: pre-delete the collisions that have been deleted.
self.handle_removed_colliders(removed_colliders);
let mut need_region_propagation = false;
// Phase 2: pre-delete the collisions that have been deleted.
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
if let Some(co) = colliders.get_mut_internal(*handle) {
if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
continue;
}
let mut new_proxy_id = co.bf_data.proxy_index;
let next_pos = co.parent.and_then(|p| {
let parent = bodies.get(p.handle)?;
(parent.soft_ccd_prediction() > 0.0).then(|| {
parent.predict_position_using_velocity_and_forces_with_max_dist(
dt,
parent.soft_ccd_prediction(),
) * p.pos_wrt_parent
})
});
if self.handle_modified_collider(
prediction_distance,
*handle,
&mut new_proxy_id,
co,
next_pos.as_ref(),
) {
need_region_propagation = true;
}
if co.bf_data.proxy_index != new_proxy_id {
self.colliders_proxy_ids.insert(*handle, new_proxy_id);
// Make sure we have the new proxy index in case
// the collider was added for the first time.
co.bf_data = ColliderBroadPhaseData {
proxy_index: new_proxy_id,
};
}
}
}
// Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
if need_region_propagation {
self.propagate_created_regions();
}
// Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
self.update_layers_and_find_pairs(events);
// Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
// layers to possible remove regions from larger layers that would become empty that way.
self.complete_removals(colliders, removed_colliders);
}
}
#[cfg(test)]
mod test {
use crate::dynamics::{
ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyBuilder, RigidBodySet,
};
use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
use crate::geometry::{BroadPhase, BroadPhaseMultiSap, ColliderBuilder, ColliderSet};
#[test]
fn test_add_update_remove() {
let mut broad_phase = BroadPhase::new();
let mut broad_phase = BroadPhaseMultiSap::new();
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
@@ -640,7 +660,7 @@ mod test {
let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
let mut events = Vec::new();
broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
bodies.remove(
hrb,
@@ -650,7 +670,7 @@ mod test {
&mut multibody_joints,
true,
);
broad_phase.update(0.0, &mut colliders, &[], &[coh], &mut events);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[], &[coh], &mut events);
// Create another body.
let rb = RigidBodyBuilder::dynamic().build();
@@ -659,6 +679,6 @@ mod test {
let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
// Make sure the proxy handles is recycled properly.
broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
}
}

5
src/geometry/broad_phase_multi_sap/mod.rs
View File

@@ -1,6 +1,5 @@
pub use self::broad_phase::BroadPhase;
pub use self::broad_phase_multi_sap::BroadPhaseMultiSap;
pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair};
pub use self::sap_proxy::SAPProxyIndex;
use self::sap_axis::*;
use self::sap_endpoint::*;
@@ -9,7 +8,7 @@ use self::sap_proxy::*;
use self::sap_region::*;
use self::sap_utils::*;
mod broad_phase;
mod broad_phase_multi_sap;
mod broad_phase_pair_event;
mod sap_axis;
mod sap_endpoint;

4
src/geometry/broad_phase_multi_sap/sap_axis.rs
View File

@@ -1,6 +1,6 @@
use super::{SAPEndpoint, SAPProxies, NUM_SENTINELS};
use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
use crate::geometry::SAPProxyIndex;
use crate::geometry::BroadPhaseProxyIndex;
use crate::math::Real;
use bit_vec::BitVec;
use parry::bounding_volume::BoundingVolume;
@@ -39,7 +39,7 @@ impl SAPAxis {
pub fn batch_insert(
&mut self,
dim: usize,
new_proxies: &[SAPProxyIndex],
new_proxies: &[BroadPhaseProxyIndex],
proxies: &SAPProxies,
reporting: Option<&mut HashMap<(u32, u32), bool>>,
) {

20
src/geometry/broad_phase_multi_sap/sap_layer.rs
View File

@@ -1,6 +1,6 @@
use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool};
use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
use crate::geometry::{Aabb, SAPProxyIndex};
use crate::geometry::{Aabb, BroadPhaseProxyIndex};
use crate::math::{Point, Real};
use parry::bounding_volume::BoundingVolume;
use parry::utils::hashmap::{Entry, HashMap};
@@ -13,11 +13,11 @@ pub(crate) struct SAPLayer {
pub smaller_layer: Option<u8>,
pub larger_layer: Option<u8>,
region_width: Real,
pub regions: HashMap<Point<i32>, SAPProxyIndex>,
pub regions: HashMap<Point<i32>, BroadPhaseProxyIndex>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
regions_to_potentially_remove: Vec<Point<i32>>, // Workspace
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub created_regions: Vec<SAPProxyIndex>,
pub created_regions: Vec<BroadPhaseProxyIndex>,
}
impl SAPLayer {
@@ -71,7 +71,7 @@ impl SAPLayer {
///
/// This method must be called in a bottom-up loop, propagating new regions from the
/// smallest layer, up to the largest layer. That loop is done by the Phase 3 of the
/// BroadPhase::update.
/// BroadPhaseMultiSap::update.
pub fn propagate_created_regions(
&mut self,
larger_layer: &mut Self,
@@ -103,7 +103,7 @@ impl SAPLayer {
/// one region on its parent "larger" layer.
fn register_subregion(
&mut self,
proxy_id: SAPProxyIndex,
proxy_id: BroadPhaseProxyIndex,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) {
@@ -140,7 +140,7 @@ impl SAPLayer {
fn unregister_subregion(
&mut self,
proxy_id: SAPProxyIndex,
proxy_id: BroadPhaseProxyIndex,
proxy_region: &SAPRegion,
proxies: &mut SAPProxies,
) {
@@ -182,7 +182,7 @@ impl SAPLayer {
/// If the region with the given region key does not exist yet, it is created.
/// When a region is created, it creates a new proxy for that region, and its
/// proxy ID is added to `self.created_region` so it can be propagated during
/// the Phase 3 of `BroadPhase::update`.
/// the Phase 3 of `BroadPhaseMultiSap::update`.
///
/// This returns the proxy ID of the already existing region if it existed, or
/// of the new region if it did not exist and has been created by this method.
@@ -191,7 +191,7 @@ impl SAPLayer {
region_key: Point<i32>,
proxies: &mut SAPProxies,
pool: &mut SAPRegionPool,
) -> SAPProxyIndex {
) -> BroadPhaseProxyIndex {
match self.regions.entry(region_key) {
// Yay, the region already exists!
Entry::Occupied(occupied) => *occupied.get(),
@@ -266,7 +266,7 @@ impl SAPLayer {
}
}
pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: SAPProxyIndex) {
pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: BroadPhaseProxyIndex) {
// Discretize the Aabb to find the regions that need to be invalidated.
let proxy_aabb = &mut proxies[proxy_index].aabb;
let start = super::point_key(proxy_aabb.mins, self.region_width);
@@ -379,7 +379,7 @@ impl SAPLayer {
pub fn proper_proxy_moved_to_bigger_layer(
&mut self,
proxies: &mut SAPProxies,
proxy_id: SAPProxyIndex,
proxy_id: BroadPhaseProxyIndex,
) {
for (point, region_id) in &self.regions {
let region = &mut proxies[*region_id].data.as_region_mut();

24
src/geometry/broad_phase_multi_sap/sap_proxy.rs
View File

@@ -1,11 +1,9 @@
use super::NEXT_FREE_SENTINEL;
use crate::geometry::broad_phase_multi_sap::SAPRegion;
use crate::geometry::ColliderHandle;
use crate::geometry::{BroadPhaseProxyIndex, ColliderHandle};
use parry::bounding_volume::Aabb;
use std::ops::{Index, IndexMut};
pub type SAPProxyIndex = u32;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub enum SAPProxyData {
@@ -49,7 +47,7 @@ impl SAPProxyData {
pub struct SAPProxy {
pub data: SAPProxyData,
pub aabb: Aabb,
pub next_free: SAPProxyIndex,
pub next_free: BroadPhaseProxyIndex,
// TODO: pack the layer_id and layer_depth into a single u16?
pub layer_id: u8,
pub layer_depth: i8,
@@ -81,7 +79,7 @@ impl SAPProxy {
#[derive(Clone)]
pub struct SAPProxies {
pub elements: Vec<SAPProxy>,
pub first_free: SAPProxyIndex,
pub first_free: BroadPhaseProxyIndex,
}
impl Default for SAPProxies {
@@ -98,7 +96,7 @@ impl SAPProxies {
}
}
pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex {
pub fn insert(&mut self, proxy: SAPProxy) -> BroadPhaseProxyIndex {
if self.first_free != NEXT_FREE_SENTINEL {
let proxy_id = self.first_free;
self.first_free = self.elements[proxy_id as usize].next_free;
@@ -110,31 +108,31 @@ impl SAPProxies {
}
}
pub fn remove(&mut self, proxy_id: SAPProxyIndex) {
pub fn remove(&mut self, proxy_id: BroadPhaseProxyIndex) {
let proxy = &mut self.elements[proxy_id as usize];
proxy.next_free = self.first_free;
self.first_free = proxy_id;
}
// NOTE: this must not take holes into account.
pub fn get_mut(&mut self, i: SAPProxyIndex) -> Option<&mut SAPProxy> {
pub fn get_mut(&mut self, i: BroadPhaseProxyIndex) -> Option<&mut SAPProxy> {
self.elements.get_mut(i as usize)
}
// NOTE: this must not take holes into account.
pub fn get(&self, i: SAPProxyIndex) -> Option<&SAPProxy> {
pub fn get(&self, i: BroadPhaseProxyIndex) -> Option<&SAPProxy> {
self.elements.get(i as usize)
}
}
impl Index<SAPProxyIndex> for SAPProxies {
impl Index<BroadPhaseProxyIndex> for SAPProxies {
type Output = SAPProxy;
fn index(&self, i: SAPProxyIndex) -> &SAPProxy {
fn index(&self, i: BroadPhaseProxyIndex) -> &SAPProxy {
self.elements.index(i as usize)
}
}
impl IndexMut<SAPProxyIndex> for SAPProxies {
fn index_mut(&mut self, i: SAPProxyIndex) -> &mut SAPProxy {
impl IndexMut<BroadPhaseProxyIndex> for SAPProxies {
fn index_mut(&mut self, i: BroadPhaseProxyIndex) -> &mut SAPProxy {
self.elements.index_mut(i as usize)
}
}

18
src/geometry/broad_phase_multi_sap/sap_region.rs
View File

@@ -1,5 +1,5 @@
use super::{SAPAxis, SAPProxies};
use crate::geometry::SAPProxyIndex;
use crate::geometry::BroadPhaseProxyIndex;
use crate::math::DIM;
use bit_vec::BitVec;
use parry::bounding_volume::Aabb;
@@ -13,8 +13,8 @@ pub struct SAPRegion {
pub axes: [SAPAxis; DIM],
pub existing_proxies: BitVec,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub to_insert: Vec<SAPProxyIndex>, // Workspace
pub subregions: Vec<SAPProxyIndex>,
pub to_insert: Vec<BroadPhaseProxyIndex>, // Workspace
pub subregions: Vec<BroadPhaseProxyIndex>,
pub id_in_parent_subregion: u32,
pub update_count: u8,
pub needs_update_after_subregion_removal: bool,
@@ -90,7 +90,7 @@ impl SAPRegion {
/// If this region contains the given proxy, this will decrement this region's proxy count.
///
/// Returns `true` if this region contained the proxy. Returns `false` otherwise.
pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: SAPProxyIndex) -> bool {
pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: BroadPhaseProxyIndex) -> bool {
if self.existing_proxies.get(proxy_id as usize) == Some(true) {
// NOTE: we are just registering the fact that that proxy isn't a
// subproper proxy anymore. But it is still part of this region
@@ -142,7 +142,7 @@ impl SAPRegion {
self.subproper_proxy_count -= num_deleted_subregion_endpoints[0] / 2;
}
pub fn predelete_proxy(&mut self, _proxy_id: SAPProxyIndex) {
pub fn predelete_proxy(&mut self, _proxy_id: BroadPhaseProxyIndex) {
// We keep the proxy_id as argument for uniformity with the "preupdate"
// method. However we don't actually need it because the deletion will be
// handled transparently during the next update.
@@ -153,14 +153,18 @@ impl SAPRegion {
self.update_count = self.update_count.max(1);
}
pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize {
pub fn register_subregion(&mut self, proxy_id: BroadPhaseProxyIndex) -> usize {
let subregion_index = self.subregions.len();
self.subregions.push(proxy_id);
self.preupdate_proxy(proxy_id, true);
subregion_index
}
pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool {
pub fn preupdate_proxy(
&mut self,
proxy_id: BroadPhaseProxyIndex,
is_subproper_proxy: bool,
) -> bool {
let mask_len = self.existing_proxies.len();
if proxy_id as usize >= mask_len {
self.existing_proxies

13
src/geometry/broad_phase_qbvh.rs
View File

@@ -1,5 +1,4 @@
use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
use parry::bounding_volume::BoundingVolume;
use parry::math::Real;
use parry::partitioning::Qbvh;
use parry::partitioning::QbvhUpdateWorkspace;
@@ -7,20 +6,20 @@ use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhase {
pub struct BroadPhaseQbvh {
qbvh: Qbvh<ColliderHandle>,
stack: Vec<(u32, u32)>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
workspace: QbvhUpdateWorkspace,
}
impl Default for BroadPhase {
impl Default for BroadPhaseQbvh {
fn default() -> Self {
Self::new()
}
}
impl BroadPhase {
impl BroadPhaseQbvh {
pub fn new() -> Self {
Self {
qbvh: Qbvh::new(),
@@ -59,7 +58,7 @@ impl BroadPhase {
colliders.iter().map(|(handle, collider)| {
(
handle,
collider.compute_aabb().loosened(prediction_distance / 2.0),
collider.compute_collision_aabb(prediction_distance / 2.0),
)
}),
margin,
@@ -76,9 +75,7 @@ impl BroadPhase {
}
let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
colliders[*handle]
.compute_aabb()
.loosened(prediction_distance / 2.0)
colliders[*handle].compute_collision_aabb(prediction_distance / 2.0)
});
self.qbvh
.traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);

143
src/geometry/collider.rs
View File

@@ -1,17 +1,20 @@
use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
use crate::geometry::{
ActiveCollisionTypes, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition, ColliderShape,
ColliderType, InteractionGroups, SharedShape,
ActiveCollisionTypes, BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges,
ColliderFlags, ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition,
ColliderShape, ColliderType, InteractionGroups, SharedShape,
};
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
use crate::parry::transformation::vhacd::VHACDParameters;
use crate::pipeline::{ActiveEvents, ActiveHooks};
use crate::prelude::ColliderEnabled;
use na::Unit;
use parry::bounding_volume::Aabb;
use parry::bounding_volume::{Aabb, BoundingVolume};
use parry::shape::{Shape, TriMeshFlags};
#[cfg(feature = "dim3")]
use crate::geometry::HeightFieldFlags;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
/// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
@@ -27,6 +30,7 @@ pub struct Collider {
pub(crate) material: ColliderMaterial,
pub(crate) flags: ColliderFlags,
pub(crate) bf_data: ColliderBroadPhaseData,
contact_skin: Real,
contact_force_event_threshold: Real,
/// User-defined data associated to this collider.
pub user_data: u128,
@@ -50,6 +54,21 @@ impl Collider {
}
}
/// An internal index associated to this collider by the broad-phase algorithm.
pub fn internal_broad_phase_proxy_index(&self) -> BroadPhaseProxyIndex {
self.bf_data.proxy_index
}
/// Sets the internal index associated to this collider by the broad-phase algorithm.
///
/// This must **not** be called, unless you are implementing your own custom broad-phase
/// that require storing an index in the collider struct.
/// Modifying that index outside of a custom broad-phase code will most certainly break
/// the physics engine.
pub fn set_internal_broad_phase_proxy_index(&mut self, id: BroadPhaseProxyIndex) {
self.bf_data.proxy_index = id;
}
/// The rigid body this collider is attached to.
pub fn parent(&self) -> Option<RigidBodyHandle> {
self.parent.map(|parent| parent.handle)
@@ -60,6 +79,55 @@ impl Collider {
self.coll_type.is_sensor()
}
/// Copy all the characteristics from `other` to `self`.
///
/// If you have a mutable reference to a collider `collider: &mut Collider`, attempting to
/// assign it a whole new collider instance, e.g., `*collider = ColliderBuilder::ball(0.5).build()`,
/// will crash due to some internal indices being overwritten. Instead, use
/// `collider.copy_from(&ColliderBuilder::ball(0.5).build())`.
///
/// This method will allow you to set most characteristics of this collider from another
/// collider instance without causing any breakage.
///
/// This method **cannot** be used for reparenting a collider. Therefore, the parent of the
/// `other` (if any), as well as its relative position to that parent will not be copied into
/// `self`.
///
/// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has
/// a parent, its position is directly controlled by the parent rigid-body).
pub fn copy_from(&mut self, other: &Collider) {
// NOTE: we deconstruct the collider struct to be sure we don’t forget to
// add some copies here if we add more field to Collider in the future.
let Collider {
coll_type,
shape,
mprops,
changes: _changes, // Will be set to ALL.
parent: _parent, // This function cannot be used to reparent the collider.
pos,
material,
flags,
bf_data: _bf_data, // Internal ids must not be overwritten.
contact_force_event_threshold,
user_data,
contact_skin,
} = other;
if self.parent.is_none() {
self.pos = *pos;
}
self.coll_type = *coll_type;
self.shape = shape.clone();
self.mprops = mprops.clone();
self.material = *material;
self.contact_force_event_threshold = *contact_force_event_threshold;
self.user_data = *user_data;
self.flags = *flags;
self.changes = ColliderChanges::all();
self.contact_skin = *contact_skin;
}
/// The physics hooks enabled for this collider.
pub fn active_hooks(&self) -> ActiveHooks {
self.flags.active_hooks
@@ -90,6 +158,20 @@ impl Collider {
self.flags.active_collision_types = active_collision_types;
}
/// The contact skin of this collider.
///
/// See the documentation of [`ColliderBuilder::contact_skin`] for details.
pub fn contact_skin(&self) -> Real {
self.contact_skin
}
/// Sets the contact skin of this collider.
///
/// See the documentation of [`ColliderBuilder::contact_skin`] for details.
pub fn set_contact_skin(&mut self, skin_thickness: Real) {
self.contact_skin = skin_thickness;
}
/// The friction coefficient of this collider.
pub fn friction(&self) -> Real {
self.material.friction
@@ -224,7 +306,7 @@ impl Collider {
}
}
/// Sets the rotational part of this collider's rotaiton relative to its parent rigid-body.
/// Sets the rotational part of this collider's rotation relative to its parent rigid-body.
pub fn set_rotation_wrt_parent(&mut self, rotation: AngVector<Real>) {
if let Some(parent) = self.parent.as_mut() {
self.changes.insert(ColliderChanges::PARENT);
@@ -372,10 +454,21 @@ impl Collider {
}
/// Compute the axis-aligned bounding box of this collider.
///
/// This AABB doesn’t take into account the collider’s contact skin.
/// [`Collider::contact_skin`].
pub fn compute_aabb(&self) -> Aabb {
self.shape.compute_aabb(&self.pos)
}
/// Compute the axis-aligned bounding box of this collider, taking into account the
/// [`Collider::contact_skin`] and prediction distance.
pub fn compute_collision_aabb(&self, prediction: Real) -> Aabb {
self.shape
.compute_aabb(&self.pos)
.loosened(self.contact_skin + prediction)
}
/// Compute the axis-aligned bounding box of this collider moving from its current position
/// to the given `next_position`
pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> Aabb {
@@ -430,6 +523,8 @@ pub struct ColliderBuilder {
pub enabled: bool,
/// The total force magnitude beyond which a contact force event can be emitted.
pub contact_force_event_threshold: Real,
/// An extra thickness around the collider shape to keep them further apart when colliding.
pub contact_skin: Real,
}
impl ColliderBuilder {
@@ -452,6 +547,7 @@ impl ColliderBuilder {
active_events: ActiveEvents::empty(),
enabled: true,
contact_force_event_threshold: 0.0,
contact_skin: 0.0,
}
}
@@ -518,6 +614,15 @@ impl ColliderBuilder {
Self::new(SharedShape::round_cuboid(hx, hy, border_radius))
}
/// Initialize a new collider builder with a capsule defined from its endpoints.
///
/// See also [`ColliderBuilder::capsule_x`], [`ColliderBuilder::capsule_y`], and
/// [`ColliderBuilder::capsule_z`], for a simpler way to build capsules with common
/// orientations.
pub fn capsule_from_endpoints(a: Point<Real>, b: Point<Real>, radius: Real) -> Self {
Self::new(SharedShape::capsule(a, b, radius))
}
/// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
pub fn capsule_x(half_height: Real, radius: Real) -> Self {
Self::new(SharedShape::capsule_x(half_height, radius))
@@ -698,6 +803,17 @@ impl ColliderBuilder {
Self::new(SharedShape::heightfield(heights, scale))
}
/// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
/// factor along each coordinate axis.
#[cfg(feature = "dim3")]
pub fn heightfield_with_flags(
heights: na::DMatrix<Real>,
scale: Vector<Real>,
flags: HeightFieldFlags,
) -> Self {
Self::new(SharedShape::heightfield_with_flags(heights, scale, flags))
}
/// The default friction coefficient used by the collider builder.
pub fn default_friction() -> Real {
0.5
@@ -705,7 +821,7 @@ impl ColliderBuilder {
/// The default density used by the collider builder.
pub fn default_density() -> Real {
1.0
100.0
}
/// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
@@ -861,6 +977,20 @@ impl ColliderBuilder {
self
}
/// Sets the contact skin of the collider.
///
/// The contact skin acts as if the collider was enlarged with a skin of width `skin_thickness`
/// around it, keeping objects further apart when colliding.
///
/// A non-zero contact skin can increase performance, and in some cases, stability. However
/// it creates a small gap between colliding object (equal to the sum of their skin). If the
/// skin is sufficiently small, this might not be visually significant or can be hidden by the
/// rendering assets.
pub fn contact_skin(mut self, skin_thickness: Real) -> Self {
self.contact_skin = skin_thickness;
self
}
/// Enable or disable the collider after its creation.
pub fn enabled(mut self, enabled: bool) -> Self {
self.enabled = enabled;
@@ -908,6 +1038,7 @@ impl ColliderBuilder {
flags,
coll_type,
contact_force_event_threshold: self.contact_force_event_threshold,
contact_skin: self.contact_skin,
user_data: self.user_data,
}
}

4
src/geometry/collider_components.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle, RigidBodyType};
use crate::geometry::{InteractionGroups, SAPProxyIndex, Shape, SharedShape};
use crate::geometry::{BroadPhaseProxyIndex, InteractionGroups, Shape, SharedShape};
use crate::math::{Isometry, Real};
use crate::parry::partitioning::IndexedData;
use crate::pipeline::{ActiveEvents, ActiveHooks};
@@ -118,7 +118,7 @@ impl ColliderType {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Data associated to a collider that takes part to a broad-phase algorithm.
pub struct ColliderBroadPhaseData {
pub(crate) proxy_index: SAPProxyIndex,
pub(crate) proxy_index: BroadPhaseProxyIndex,
}
impl Default for ColliderBroadPhaseData {

44
src/geometry/contact_pair.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold};
use crate::math::{Point, Real, Vector};
use crate::math::{Point, Real, TangentImpulse, Vector};
use crate::pipeline::EventHandler;
use crate::prelude::CollisionEventFlags;
use parry::query::ContactManifoldsWorkspace;
@@ -33,12 +33,11 @@ pub struct ContactData {
pub impulse: Real,
/// The friction impulse along the vector orthonormal to the contact normal, applied to the first
/// collider's rigid-body.
#[cfg(feature = "dim2")]
pub tangent_impulse: Real,
/// The friction impulses along the basis orthonormal to the contact normal, applied to the first
/// collider's rigid-body.
#[cfg(feature = "dim3")]
pub tangent_impulse: na::Vector2<Real>,
pub tangent_impulse: TangentImpulse<Real>,
/// The impulse retained for warmstarting the next simulation step.
pub warmstart_impulse: Real,
/// The friction impulse retained for warmstarting the next simulation step.
pub warmstart_tangent_impulse: TangentImpulse<Real>,
}
impl Default for ContactData {
@@ -46,6 +45,8 @@ impl Default for ContactData {
Self {
impulse: 0.0,
tangent_impulse: na::zero(),
warmstart_impulse: 0.0,
warmstart_tangent_impulse: na::zero(),
}
}
}
@@ -57,14 +58,14 @@ pub struct IntersectionPair {
/// Are the colliders intersecting?
pub intersecting: bool,
/// Was a `CollisionEvent::Started` emitted for this collider?
pub(crate) start_event_emited: bool,
pub(crate) start_event_emitted: bool,
}
impl IntersectionPair {
pub(crate) fn new() -> Self {
Self {
intersecting: false,
start_event_emited: false,
start_event_emitted: false,
}
}
@@ -76,7 +77,7 @@ impl IntersectionPair {
collider2: ColliderHandle,
events: &dyn EventHandler,
) {
self.start_event_emited = true;
self.start_event_emitted = true;
events.handle_collision_event(
bodies,
colliders,
@@ -93,7 +94,7 @@ impl IntersectionPair {
collider2: ColliderHandle,
events: &dyn EventHandler,
) {
self.start_event_emited = false;
self.start_event_emitted = false;
events.handle_collision_event(
bodies,
colliders,
@@ -114,11 +115,14 @@ pub struct ContactPair {
/// The set of contact manifolds between the two colliders.
///
/// All contact manifold contain themselves contact points between the colliders.
/// Note that contact points in the contact manifold do not take into account the
/// [`Collider::contact_skin`] which only affects the constraint solver and the
/// [`SolverContact`].
pub manifolds: Vec<ContactManifold>,
/// Is there any active contact in this contact pair?
pub has_any_active_contact: bool,
/// Was a `CollisionEvent::Started` emitted for this collider?
pub(crate) start_event_emited: bool,
pub(crate) start_event_emitted: bool,
pub(crate) workspace: Option<ContactManifoldsWorkspace>,
}
@@ -129,7 +133,7 @@ impl ContactPair {
collider2,
has_any_active_contact: false,
manifolds: Vec::new(),
start_event_emited: false,
start_event_emitted: false,
workspace: None,
}
}
@@ -206,7 +210,7 @@ impl ContactPair {
colliders: &ColliderSet,
events: &dyn EventHandler,
) {
self.start_event_emited = true;
self.start_event_emitted = true;
events.handle_collision_event(
bodies,
@@ -222,7 +226,7 @@ impl ContactPair {
colliders: &ColliderSet,
events: &dyn EventHandler,
) {
self.start_event_emited = false;
self.start_event_emitted = false;
events.handle_collision_event(
bodies,
@@ -299,6 +303,10 @@ pub struct SolverContact {
pub tangent_velocity: Vector<Real>,
/// Whether or not this contact existed during the last timestep.
pub is_new: bool,
/// Impulse used to warmstart the solve for the normal constraint.
pub warmstart_impulse: Real,
/// Impulse used to warmstart the solve for the friction constraints.
pub warmstart_tangent_impulse: TangentImpulse<Real>,
}
impl SolverContact {
@@ -351,16 +359,10 @@ impl ContactManifoldData {
pub trait ContactManifoldExt {
/// Computes the sum of all the impulses applied by contacts from this contact manifold.
fn total_impulse(&self) -> Real;
/// Computes the maximum impulse applied by contacts from this contact manifold.
fn max_impulse(&self) -> Real;
}
impl ContactManifoldExt for ContactManifold {
fn total_impulse(&self) -> Real {
self.points.iter().map(|pt| pt.data.impulse).sum()
}
fn max_impulse(&self) -> Real {
self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse))
}
}

17
src/geometry/mod.rs
View File

@@ -1,9 +1,7 @@
//! Structures related to geometry: colliders, shapes, etc.
pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
pub use self::broad_phase_multi_sap::BroadPhase;
// pub use self::broad_phase_qbvh::BroadPhase;
pub use self::broad_phase::BroadPhase;
pub use self::broad_phase_multi_sap::{BroadPhaseMultiSap, BroadPhasePairEvent, ColliderPair};
pub use self::collider_components::*;
pub use self::contact_pair::{
ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -51,10 +49,12 @@ pub type Aabb = parry::bounding_volume::Aabb;
pub type Ray = parry::query::Ray;
/// The intersection between a ray and a collider.
pub type RayIntersection = parry::query::RayIntersection;
/// The the projection of a point on a collider.
/// The projection of a point on a collider.
pub type PointProjection = parry::query::PointProjection;
/// The the time of impact between two shapes.
pub type TOI = parry::query::TOI;
/// The result of a shape-cast between two shapes.
pub type ShapeCastHit = parry::query::ShapeCastHit;
/// The default broad-phase implementation recommended for general-purpose usage.
pub type DefaultBroadPhase = BroadPhaseMultiSap;
bitflags::bitflags! {
/// Flags providing more information regarding a collision event.
@@ -180,7 +180,7 @@ impl ContactForceEvent {
}
}
pub(crate) use self::broad_phase_multi_sap::SAPProxyIndex;
pub(crate) use self::broad_phase::BroadPhaseProxyIndex;
pub(crate) use self::narrow_phase::ContactManifoldIndex;
pub(crate) use parry::partitioning::Qbvh;
pub use parry::shape::*;
@@ -203,6 +203,7 @@ mod interaction_graph;
mod interaction_groups;
mod narrow_phase;
mod broad_phase;
mod broad_phase_qbvh;
mod collider;
mod collider_set;

124
src/geometry/narrow_phase.rs
View File

@@ -8,9 +8,10 @@ use crate::dynamics::{
RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex,
BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle,
ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData,
ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
TemporaryInteractionIndex,
};
use crate::math::{Real, Vector};
use crate::pipeline::{
@@ -342,7 +343,7 @@ impl NarrowPhase {
islands.wake_up(bodies, parent.handle, true)
}
if pair.start_event_emited {
if pair.start_event_emitted {
events.handle_collision_event(
bodies,
colliders,
@@ -354,7 +355,7 @@ impl NarrowPhase {
} else {
// If there is no island, don’t wake-up bodies, but do send the Stopped collision event.
for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
if pair.start_event_emited {
if pair.start_event_emitted {
events.handle_collision_event(
bodies,
colliders,
@@ -370,7 +371,7 @@ impl NarrowPhase {
.intersection_graph
.interactions_with(intersection_graph_id)
{
if pair.start_event_emited {
if pair.start_event_emitted {
events.handle_collision_event(
bodies,
colliders,
@@ -709,7 +710,6 @@ impl NarrowPhase {
let co1 = &colliders[handle1];
let co2 = &colliders[handle2];
// TODO: remove the `loop` once labels on blocks is stabilized.
'emit_events: {
if !co1.changes.needs_narrow_phase_update()
&& !co2.changes.needs_narrow_phase_update()
@@ -767,7 +767,6 @@ impl NarrowPhase {
edge.weight.intersecting = query_dispatcher
.intersection_test(&pos12, &*co1.shape, &*co2.shape)
.unwrap_or(false);
break 'emit_events;
}
let active_events = co1.flags.active_events | co2.flags.active_events;
@@ -789,6 +788,7 @@ impl NarrowPhase {
pub(crate) fn compute_contacts(
&mut self,
prediction_distance: Real,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
impulse_joints: &ImpulseJointSet,
@@ -810,7 +810,6 @@ impl NarrowPhase {
let co1 = &colliders[pair.collider1];
let co2 = &colliders[pair.collider2];
// TODO: remove the `loop` once labels on blocks are supported.
'emit_events: {
if !co1.changes.needs_narrow_phase_update()
&& !co2.changes.needs_narrow_phase_update()
@@ -819,17 +818,11 @@ impl NarrowPhase {
return;
}
// TODO: avoid lookup into bodies.
let mut rb_type1 = RigidBodyType::Fixed;
let mut rb_type2 = RigidBodyType::Fixed;
let rb1 = co1.parent.map(|co_parent1| &bodies[co_parent1.handle]);
let rb2 = co2.parent.map(|co_parent2| &bodies[co_parent2.handle]);
if let Some(co_parent1) = &co1.parent {
rb_type1 = bodies[co_parent1.handle].body_type;
}
if let Some(co_parent2) = &co2.parent {
rb_type2 = bodies[co_parent2.handle].body_type;
}
let rb_type1 = rb1.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed);
let rb_type2 = rb2.map(|rb| rb.body_type).unwrap_or(RigidBodyType::Fixed);
// Deal with contacts disabled between bodies attached by joints.
if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) {
@@ -901,11 +894,37 @@ impl NarrowPhase {
}
let pos12 = co1.pos.inv_mul(&co2.pos);
let contact_skin_sum = co1.contact_skin() + co2.contact_skin();
let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0);
let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 {
let aabb1 = co1.compute_collision_aabb(0.0);
let aabb2 = co2.compute_collision_aabb(0.0);
let inv_dt = crate::utils::inv(dt);
let linvel1 = rb1.map(|rb| rb.linvel()
.cap_magnitude(soft_ccd_prediction1 * inv_dt)).unwrap_or_default();
let linvel2 = rb2.map(|rb| rb.linvel()
.cap_magnitude(soft_ccd_prediction2 * inv_dt)).unwrap_or_default();
if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) {
pair.clear();
break 'emit_events;
}
prediction_distance.max(
dt * (linvel1 - linvel2).norm()) + contact_skin_sum
} else {
prediction_distance + contact_skin_sum
};
let _ = query_dispatcher.contact_manifolds(
&pos12,
&*co1.shape,
&*co2.shape,
prediction_distance,
effective_prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
);
@@ -924,14 +943,8 @@ impl NarrowPhase {
);
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co1
.parent
.map(|p1| bodies[p1.handle].dominance)
.unwrap_or(zero);
let dominance2 = co2
.parent
.map(|p2| bodies[p2.handle].dominance)
.unwrap_or(zero);
let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero);
let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero);
pair.has_any_active_contact = false;
@@ -948,12 +961,22 @@ impl NarrowPhase {
// Generate solver contacts.
for (contact_id, contact) in manifold.points.iter().enumerate() {
assert!(
contact_id <= u8::MAX as usize,
"A contact manifold cannot contain more than 255 contacts currently."
);
if contact_id > u8::MAX as usize {
log::warn!("A contact manifold cannot contain more than 255 contacts currently, dropping contact in excess.");
break;
}
if contact.dist < prediction_distance {
let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin();
let keep_solver_contact = effective_contact_dist < prediction_distance || {
let world_pt1 = world_pos1 * contact.local_p1;
let world_pt2 = world_pos2 * contact.local_p2;
let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default();
let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default();
effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance
};
if keep_solver_contact {
// Generate the solver contact.
let world_pt1 = world_pos1 * contact.local_p1;
let world_pt2 = world_pos2 * contact.local_p2;
@@ -962,11 +985,13 @@ impl NarrowPhase {
let solver_contact = SolverContact {
contact_id: contact_id as u8,
point: effective_point,
dist: contact.dist,
dist: effective_contact_dist,
friction,
restitution,
tangent_velocity: Vector::zeros(),
is_new: contact.data.impulse == 0.0,
warmstart_impulse: contact.data.warmstart_impulse,
warmstart_tangent_impulse: contact.data.warmstart_tangent_impulse,
};
manifold.data.solver_contacts.push(solver_contact);
@@ -1000,9 +1025,36 @@ impl NarrowPhase {
manifold.data.normal = modifiable_normal;
manifold.data.user_data = modifiable_user_data;
}
}
break 'emit_events;
/*
* TODO: When using the block solver in 3D, I’d expect this sort to help, but
* it makes the domino demo worse. Needs more investigation.
fn sort_solver_contacts(mut contacts: &mut [SolverContact]) {
while contacts.len() > 2 {
let first = contacts[0];
let mut furthest_id = 1;
let mut furthest_dist = na::distance(&first.point, &contacts[1].point);
for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) {
let candidate_dist = na::distance(&first.point, &candidate.point);
if candidate_dist > furthest_dist {
furthest_dist = candidate_dist;
furthest_id = candidate_id;
}
}
if furthest_id > 1 {
contacts.swap(1, furthest_id);
}
contacts = &mut contacts[2..];
}
}
sort_solver_contacts(&mut manifold.data.solver_contacts);
*/
}
}
let active_events = co1.flags.active_events | co2.flags.active_events;

9
src/lib.rs
View File

@@ -15,6 +15,7 @@
#![allow(clippy::too_many_arguments)]
#![allow(clippy::needless_range_loop)] // TODO: remove this? I find that in the math code using indices adds clarity.
#![allow(clippy::module_inception)]
#![allow(unexpected_cfgs)] // This happens due to the dim2/dim3/f32/f64 cfg.
#[cfg(all(feature = "dim2", feature = "f32"))]
pub extern crate parry2d as parry;
@@ -166,6 +167,10 @@ pub mod math {
#[cfg(feature = "dim2")]
pub type JacobianViewMut<'a, N> = na::MatrixViewMut3xX<'a, N>;
/// The type of impulse applied for friction constraints.
#[cfg(feature = "dim2")]
pub type TangentImpulse<N> = na::Vector1<N>;
/// The maximum number of possible rotations and translations of a rigid body.
#[cfg(feature = "dim2")]
pub const SPATIAL_DIM: usize = 3;
@@ -195,6 +200,10 @@ pub mod math {
#[cfg(feature = "dim3")]
pub type JacobianViewMut<'a, N> = na::MatrixViewMut6xX<'a, N>;
/// The type of impulse applied for friction constraints.
#[cfg(feature = "dim3")]
pub type TangentImpulse<N> = na::Vector2<N>;
/// The maximum number of possible rotations and translations of a rigid body.
#[cfg(feature = "dim3")]
pub const SPATIAL_DIM: usize = 6;

15
src/pipeline/collision_pipeline.rs
View File

@@ -43,7 +43,7 @@ impl CollisionPipeline {
fn detect_collisions(
&mut self,
prediction_distance: Real,
broad_phase: &mut BroadPhase,
broad_phase: &mut dyn BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
@@ -58,8 +58,10 @@ impl CollisionPipeline {
self.broadphase_collider_pairs.clear();
broad_phase.update(
0.0,
prediction_distance,
colliders,
bodies,
modified_colliders,
removed_colliders,
&mut self.broad_phase_events,
@@ -80,6 +82,7 @@ impl CollisionPipeline {
narrow_phase.register_pairs(None, colliders, bodies, &self.broad_phase_events, events);
narrow_phase.compute_contacts(
prediction_distance,
0.0,
bodies,
colliders,
&ImpulseJointSet::new(),
@@ -107,7 +110,7 @@ impl CollisionPipeline {
pub fn step(
&mut self,
prediction_distance: Real,
broad_phase: &mut BroadPhase,
broad_phase: &mut dyn BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
@@ -192,13 +195,13 @@ mod tests {
let _ = collider_set.insert(collider_b);
let integration_parameters = IntegrationParameters::default();
let mut broad_phase = BroadPhase::new();
let mut broad_phase = BroadPhaseMultiSap::new();
let mut narrow_phase = NarrowPhase::new();
let mut collision_pipeline = CollisionPipeline::new();
let physics_hooks = ();
collision_pipeline.step(
integration_parameters.prediction_distance,
integration_parameters.prediction_distance(),
&mut broad_phase,
&mut narrow_phase,
&mut rigid_body_set,
@@ -244,13 +247,13 @@ mod tests {
let _ = collider_set.insert(collider_b);
let integration_parameters = IntegrationParameters::default();
let mut broad_phase = BroadPhase::new();
let mut broad_phase = BroadPhaseMultiSap::new();
let mut narrow_phase = NarrowPhase::new();
let mut collision_pipeline = CollisionPipeline::new();
let physics_hooks = ();
collision_pipeline.step(
integration_parameters.prediction_distance,
integration_parameters.prediction_distance(),
&mut broad_phase,
&mut narrow_phase,
&mut rigid_body_set,

12
src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
View File

@@ -9,6 +9,7 @@ use crate::math::{Isometry, Point, Real, Vector, DIM};
use crate::pipeline::debug_render_pipeline::debug_render_backend::DebugRenderObject;
use crate::pipeline::debug_render_pipeline::DebugRenderStyle;
use crate::utils::SimdBasis;
use parry::utils::IsometryOpt;
use std::any::TypeId;
use std::collections::HashMap;
@@ -115,16 +116,19 @@ impl DebugRenderPipeline {
if backend.filter_object(object) {
for manifold in &pair.manifolds {
for contact in manifold.contacts() {
let world_subshape_pos1 =
manifold.subshape_pos1.prepend_to(co1.position());
backend.draw_line(
object,
co1.position() * contact.local_p1,
co2.position() * contact.local_p2,
world_subshape_pos1 * contact.local_p1,
manifold.subshape_pos2.prepend_to(co2.position())
* contact.local_p2,
self.style.contact_depth_color,
);
backend.draw_line(
object,
co1.position() * contact.local_p1,
co1.position()
world_subshape_pos1 * contact.local_p1,
world_subshape_pos1
* (contact.local_p1
+ manifold.local_n1 * self.style.contact_normal_length),
self.style.contact_normal_color,

Some files were not shown because too many files have changed in this diff Show More