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

Implement multibody joints and the new solver

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-02 14:47:40 +01:00
parent b45d4b5ac2
commit f74b8401ad
182 changed files with 9871 additions and 12645 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src_testbed/box2d_backend.rs
View File

@@ -2,14 +2,12 @@ use std::collections::HashMap;
use na::{Isometry2, Vector2};
use rapier::counters::Counters;
use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::dynamics::{ImpulseJointSet, IntegrationParameters, RigidBodyHandle, RigidBodySet};
use rapier::geometry::{Collider, ColliderSet};
use std::f32;
use wrapped2d::b2;
use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef};
// use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef};
use wrapped2d::user_data::NoUserData;
fn na_vec_to_b2_vec(v: Vector2<f32>) -> b2::Vec2 {
@@ -34,7 +32,7 @@ impl Box2dWorld {
gravity: Vector2<f32>,
bodies: &RigidBodySet,
colliders: &ColliderSet,
joints: &JointSet,
impulse_joints: &ImpulseJointSet,
) -> Self {
let mut world = b2::World::new(&na_vec_to_b2_vec(gravity));
world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled()));
@@ -46,7 +44,7 @@ impl Box2dWorld {
res.insert_bodies(bodies);
res.insert_colliders(colliders);
res.insert_joints(joints);
res.insert_joints(impulse_joints);
res
}
@@ -95,8 +93,9 @@ impl Box2dWorld {
}
}
fn insert_joints(&mut self, joints: &JointSet) {
for joint in joints.iter() {
fn insert_joints(&mut self, _impulse_joints: &ImpulseJointSet) {
/*
for joint in impulse_joints.iter() {
let body_a = self.rapier2box2d[&joint.1.body1];
let body_b = self.rapier2box2d[&joint.1.body2];
@@ -154,6 +153,8 @@ impl Box2dWorld {
}
}
}
*/
}
fn create_fixture(collider: &Collider, body: &mut b2::MetaBody<NoUserData>) {
@@ -225,7 +226,7 @@ impl Box2dWorld {
self.world.step(
params.dt,
params.max_velocity_iterations as i32,
params.max_position_iterations as i32,
params.max_stabilization_iterations as i32,
);
counters.step_completed();
}

45
src_testbed/harness/mod.rs
View File

@@ -3,7 +3,10 @@ use crate::{
TestbedGraphics,
};
use plugin::HarnessPlugin;
use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
use rapier::dynamics::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use rapier::math::Vector;
use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -78,9 +81,14 @@ impl Harness {
}
}
pub fn new(bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) -> Self {
pub fn new(
bodies: RigidBodySet,
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
) -> Self {
let mut res = Self::new_empty();
res.set_world(bodies, colliders, joints);
res.set_world(bodies, colliders, impulse_joints, multibody_joints);
res
}
@@ -100,24 +108,39 @@ impl Harness {
&mut self.physics
}
pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ())
pub fn set_world(
&mut self,
bodies: RigidBodySet,
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
) {
self.set_world_with_params(
bodies,
colliders,
impulse_joints,
multibody_joints,
Vector::y() * -9.81,
(),
)
}
pub fn set_world_with_params(
&mut self,
bodies: RigidBodySet,
colliders: ColliderSet,
joints: JointSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<f32>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
) {
// println!("Num bodies: {}", bodies.len());
// println!("Num joints: {}", joints.len());
// println!("Num impulse_joints: {}", impulse_joints.len());
self.physics.gravity = gravity;
self.physics.bodies = bodies;
self.physics.colliders = colliders;
self.physics.joints = joints;
self.physics.impulse_joints = impulse_joints;
self.physics.multibody_joints = multibody_joints;
self.physics.hooks = Box::new(hooks);
self.physics.islands = IslandManager::new();
@@ -162,7 +185,8 @@ impl Harness {
&mut physics.narrow_phase,
&mut physics.bodies,
&mut physics.colliders,
&mut physics.joints,
&mut physics.impulse_joints,
&mut physics.multibody_joints,
&mut physics.ccd_solver,
&*physics.hooks,
event_handler,
@@ -179,7 +203,8 @@ impl Harness {
&mut self.physics.narrow_phase,
&mut self.physics.bodies,
&mut self.physics.colliders,
&mut self.physics.joints,
&mut self.physics.impulse_joints,
&mut self.physics.multibody_joints,
&mut self.physics.ccd_solver,
&*self.physics.hooks,
&self.event_handler,

10
src_testbed/lib.rs
View File

@@ -1,12 +1,4 @@
extern crate nalgebra as na;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
extern crate ncollide2d as ncollide;
#[cfg(all(feature = "dim3", feature = "other-backends"))]
extern crate ncollide3d as ncollide;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
extern crate nphysics2d as nphysics;
#[cfg(all(feature = "dim3", feature = "other-backends"))]
extern crate nphysics3d as nphysics;
#[cfg(feature = "dim2")]
extern crate parry2d as parry;
#[cfg(feature = "dim3")]
@@ -37,8 +29,6 @@ mod camera2d;
mod camera3d;
mod graphics;
pub mod harness;
#[cfg(feature = "other-backends")]
mod nphysics_backend;
pub mod objects;
pub mod physics;
#[cfg(all(feature = "dim3", feature = "other-backends"))]

248
src_testbed/nphysics_backend.rs
View File

@@ -1,248 +0,0 @@
#[cfg(feature = "dim2")]
use ncollide::shape::ConvexPolygon;
use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle};
use nphysics::force_generator::DefaultForceGeneratorSet;
use nphysics::joint::{
DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint,
};
use nphysics::object::{
BodyPartHandle, ColliderDesc, DefaultBodyHandle, DefaultBodySet, DefaultColliderSet,
RigidBodyDesc,
};
use nphysics::world::{DefaultGeometricalWorld, DefaultMechanicalWorld};
use rapier::counters::Counters;
use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{Collider, ColliderSet};
use rapier::math::Vector;
use std::collections::HashMap;
#[cfg(feature = "dim3")]
use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint};
pub struct NPhysicsWorld {
rapier2nphysics: HashMap<RigidBodyHandle, DefaultBodyHandle>,
mechanical_world: DefaultMechanicalWorld<f32>,
geometrical_world: DefaultGeometricalWorld<f32>,
bodies: DefaultBodySet<f32>,
colliders: DefaultColliderSet<f32>,
joints: DefaultJointConstraintSet<f32>,
force_generators: DefaultForceGeneratorSet<f32>,
}
impl NPhysicsWorld {
pub fn from_rapier(
gravity: Vector<f32>,
bodies: &RigidBodySet,
colliders: &ColliderSet,
joints: &JointSet,
) -> Self {
let mut rapier2nphysics = HashMap::new();
let mechanical_world = DefaultMechanicalWorld::new(gravity);
let geometrical_world = DefaultGeometricalWorld::new();
let mut nphysics_bodies = DefaultBodySet::new();
let mut nphysics_colliders = DefaultColliderSet::new();
let mut nphysics_joints = DefaultJointConstraintSet::new();
let force_generators = DefaultForceGeneratorSet::new();
for (rapier_handle, rb) in bodies.iter() {
// let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0);
let nphysics_rb = RigidBodyDesc::new().position(*rb.position()).build();
let nphysics_rb_handle = nphysics_bodies.insert(nphysics_rb);
rapier2nphysics.insert(rapier_handle, nphysics_rb_handle);
}
for (_, collider) in colliders.iter() {
if let Some(parent_handle) = collider.parent() {
let parent = &bodies[parent_handle];
let nphysics_rb_handle = rapier2nphysics[&parent_handle];
if let Some(collider) =
nphysics_collider_from_rapier_collider(&collider, parent.is_dynamic())
{
let nphysics_collider = collider.build(BodyPartHandle(nphysics_rb_handle, 0));
nphysics_colliders.insert(nphysics_collider);
} else {
eprintln!("Creating shape unknown to the nphysics backend.")
}
}
}
for joint in joints.iter() {
let b1 = BodyPartHandle(rapier2nphysics[&joint.1.body1], 0);
let b2 = BodyPartHandle(rapier2nphysics[&joint.1.body2], 0);
match &joint.1.params {
JointParams::FixedJoint(params) => {
let c = FixedConstraint::new(
b1,
b2,
params.local_frame1.translation.vector.into(),
params.local_frame1.rotation,
params.local_frame2.translation.vector.into(),
params.local_frame2.rotation,
);
nphysics_joints.insert(c);
}
#[cfg(feature = "dim3")]
JointParams::BallJoint(params) => {
let c = BallConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2);
nphysics_joints.insert(c);
}
#[cfg(feature = "dim2")]
JointParams::BallJoint(params) => {
let c =
RevoluteConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2);
nphysics_joints.insert(c);
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(params) => {
let c = RevoluteConstraint::new(
b1,
b2,
params.local_anchor1,
params.local_axis1,
params.local_anchor2,
params.local_axis2,
);
nphysics_joints.insert(c);
}
JointParams::PrismaticJoint(params) => {
let mut c = PrismaticConstraint::new(
b1,
b2,
params.local_anchor1,
params.local_axis1(),
params.local_anchor2,
);
if params.limits_enabled {
c.enable_min_offset(params.limits[0]);
c.enable_max_offset(params.limits[1]);
}
nphysics_joints.insert(c);
} // JointParams::GenericJoint(_) => {
// eprintln!(
// "Joint type currently unsupported by the nphysics backend: GenericJoint."
// )
// }
}
}
Self {
rapier2nphysics,
mechanical_world,
geometrical_world,
bodies: nphysics_bodies,
colliders: nphysics_colliders,
joints: nphysics_joints,
force_generators,
}
}
pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
self.mechanical_world
.integration_parameters
.max_position_iterations = params.max_position_iterations;
self.mechanical_world
.integration_parameters
.max_velocity_iterations = params.max_velocity_iterations;
self.mechanical_world
.integration_parameters
.set_dt(params.dt);
self.mechanical_world.integration_parameters.warmstart_coeff = params.warmstart_coeff;
counters.step_started();
self.mechanical_world.step(
&mut self.geometrical_world,
&mut self.bodies,
&mut self.colliders,
&mut self.joints,
&mut self.force_generators,
);
counters.step_completed();
}
pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
for (rapier_handle, nphysics_handle) in self.rapier2nphysics.iter() {
let rb = bodies.get_mut(*rapier_handle).unwrap();
let ra = self.bodies.rigid_body(*nphysics_handle).unwrap();
let pos = *ra.position();
rb.set_position(pos, false);
for coll_handle in rb.colliders() {
let collider = &mut colliders[*coll_handle];
collider.set_position(
pos * collider.position_wrt_parent().copied().unwrap_or(na::one()),
);
}
}
}
}
fn nphysics_collider_from_rapier_collider(
collider: &Collider,
is_dynamic: bool,
) -> Option<ColliderDesc<f32>> {
let mut margin = ColliderDesc::<f32>::default_margin();
let mut pos = collider.position_wrt_parent().copied().unwrap_or(na::one());
let shape = collider.shape();
let shape = if let Some(cuboid) = shape.as_cuboid() {
ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
} else if let Some(cuboid) = shape.as_round_cuboid() {
margin = cuboid.border_radius;
ShapeHandle::new(Cuboid::new(cuboid.base_shape.half_extents))
} else if let Some(ball) = shape.as_ball() {
ShapeHandle::new(Ball::new(ball.radius - margin))
} else if let Some(capsule) = shape.as_capsule() {
pos *= capsule.transform_wrt_y();
ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
} else if let Some(heightfield) = shape.as_heightfield() {
let heights = heightfield.heights();
let scale = heightfield.scale();
let heightfield = HeightField::new(heights.clone(), *scale);
ShapeHandle::new(heightfield)
} else {
#[cfg(feature = "dim3")]
if let Some(trimesh) = shape.as_trimesh() {
ShapeHandle::new(TriMesh::new(
trimesh.vertices().to_vec(),
trimesh
.indices()
.iter()
.map(|idx| na::point![idx[0] as usize, idx[1] as usize, idx[2] as usize])
.collect(),
None,
))
} else {
return None;
}
#[cfg(feature = "dim2")]
if let Some(polygon) = shape.as_round_convex_polygon() {
margin = polygon.border_radius;
ShapeHandle::new(ConvexPolygon::try_from_points(polygon.base_shape.points()).unwrap())
} else if let Some(polygon) = shape.as_convex_polygon() {
ShapeHandle::new(ConvexPolygon::try_from_points(polygon.points()).unwrap())
} else {
return None;
}
};
let density = if is_dynamic {
collider.density().unwrap_or(0.0)
} else {
0.0
};
Some(
ColliderDesc::new(shape)
.position(pos)
.density(density)
.sensor(collider.is_sensor())
.margin(margin),
)
}

25
src_testbed/physics/mod.rs
View File

@@ -1,5 +1,8 @@
use crossbeam::channel::Receiver;
use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet};
use rapier::dynamics::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
use rapier::math::Vector;
use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -10,7 +13,7 @@ pub struct PhysicsSnapshot {
narrow_phase: Vec<u8>,
bodies: Vec<u8>,
colliders: Vec<u8>,
joints: Vec<u8>,
impulse_joints: Vec<u8>,
}
impl PhysicsSnapshot {
@@ -20,7 +23,7 @@ impl PhysicsSnapshot {
narrow_phase: &NarrowPhase,
bodies: &RigidBodySet,
colliders: &ColliderSet,
joints: &JointSet,
impulse_joints: &ImpulseJointSet,
) -> bincode::Result<Self> {
Ok(Self {
timestep_id,
@@ -28,7 +31,7 @@ impl PhysicsSnapshot {
narrow_phase: bincode::serialize(narrow_phase)?,
bodies: bincode::serialize(bodies)?,
colliders: bincode::serialize(colliders)?,
joints: bincode::serialize(joints)?,
impulse_joints: bincode::serialize(impulse_joints)?,
})
}
@@ -40,7 +43,7 @@ impl PhysicsSnapshot {
NarrowPhase,
RigidBodySet,
ColliderSet,
JointSet,
ImpulseJointSet,
)> {
Ok((
self.timestep_id,
@@ -48,7 +51,7 @@ impl PhysicsSnapshot {
bincode::deserialize(&self.narrow_phase)?,
bincode::deserialize(&self.bodies)?,
bincode::deserialize(&self.colliders)?,
bincode::deserialize(&self.joints)?,
bincode::deserialize(&self.impulse_joints)?,
))
}
@@ -57,13 +60,13 @@ impl PhysicsSnapshot {
+ self.narrow_phase.len()
+ self.bodies.len()
+ self.colliders.len()
+ self.joints.len();
+ self.impulse_joints.len();
println!("Snapshot length: {}B", total);
println!("|_ broad_phase: {}B", self.broad_phase.len());
println!("|_ narrow_phase: {}B", self.narrow_phase.len());
println!("|_ bodies: {}B", self.bodies.len());
println!("|_ colliders: {}B", self.colliders.len());
println!("|_ joints: {}B", self.joints.len());
println!("|_ impulse_joints: {}B", self.impulse_joints.len());
}
}
@@ -73,7 +76,8 @@ pub struct PhysicsState {
pub narrow_phase: NarrowPhase,
pub bodies: RigidBodySet,
pub colliders: ColliderSet,
pub joints: JointSet,
pub impulse_joints: ImpulseJointSet,
pub multibody_joints: MultibodyJointSet,
pub ccd_solver: CCDSolver,
pub pipeline: PhysicsPipeline,
pub query_pipeline: QueryPipeline,
@@ -90,7 +94,8 @@ impl PhysicsState {
narrow_phase: NarrowPhase::new(),
bodies: RigidBodySet::new(),
colliders: ColliderSet::new(),
joints: JointSet::new(),
impulse_joints: ImpulseJointSet::new(),
multibody_joints: MultibodyJointSet::new(),
ccd_solver: CCDSolver::new(),
pipeline: PhysicsPipeline::new(),
query_pipeline: QueryPipeline::new(),

381
src_testbed/physx_backend.rs
View File

@@ -1,9 +1,7 @@
#![allow(dead_code)]
use na::{
Isometry3, Matrix3, Matrix4, Point3, Quaternion, Rotation3, Translation3, Unit, UnitQuaternion,
Vector3,
};
use na::{Isometry3, Matrix4, Point3, Quaternion, Translation3, Unit, UnitQuaternion, Vector3};
use physx::articulation_joint_base::JointMap;
use physx::cooking::{
ConvexMeshCookingResult, PxConvexMeshDesc, PxCooking, PxCookingParams, PxHeightFieldDesc,
PxTriangleMeshDesc, TriangleMeshCookingResult,
@@ -13,15 +11,16 @@ use physx::prelude::*;
use physx::scene::FrictionType;
use physx::traits::Class;
use physx_sys::{
FilterShaderCallbackInfo, PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags,
PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor,
FilterShaderCallbackInfo, PxArticulationLink_getInboundJoint, PxBitAndByte, PxConvexFlags,
PxConvexMeshGeometryFlags, PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new,
PxRigidActor,
};
use rapier::counters::Counters;
use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{Collider, ColliderSet};
use rapier::utils::WBasis;
use rapier::prelude::JointAxesMask;
use std::collections::HashMap;
trait IntoNa {
@@ -145,7 +144,8 @@ impl PhysxWorld {
integration_parameters: &IntegrationParameters,
bodies: &RigidBodySet,
colliders: &ColliderSet,
joints: &JointSet,
impulse_joints: &ImpulseJointSet,
multibody_joints: &MultibodyJointSet,
use_two_friction_directions: bool,
num_threads: usize,
) -> Self {
@@ -181,6 +181,7 @@ impl PhysxWorld {
let mut scene: Owner<PxScene> = physics.create(scene_desc).unwrap();
let mut rapier2dynamic = HashMap::new();
let mut rapier2static = HashMap::new();
let mut rapier2link = HashMap::new();
let cooking_params =
PxCookingParams::new(&*physics).expect("Failed to init PhysX cooking.");
let mut cooking = PxCooking::new(physics.foundation_mut(), &cooking_params)
@@ -192,6 +193,10 @@ impl PhysxWorld {
*
*/
for (rapier_handle, rb) in bodies.iter() {
if multibody_joints.rigid_body_link(rapier_handle).is_some() {
continue;
};
let pos = rb.position().into_physx();
if rb.is_dynamic() {
let mut actor = physics.create_dynamic(&pos, rapier_handle).unwrap();
@@ -200,8 +205,10 @@ impl PhysxWorld {
actor.set_linear_velocity(&linvel, true);
actor.set_angular_velocity(&angvel, true);
actor.set_solver_iteration_counts(
integration_parameters.max_position_iterations as u32,
integration_parameters.max_velocity_iterations as u32,
// Use our number of velocity iterations as their number of position iterations.
integration_parameters.max_velocity_iterations.max(1) as u32,
// Use our number of velocity stabilization iterations as their number of velocity iterations.
integration_parameters.max_stabilization_iterations.max(1) as u32,
);
rapier2dynamic.insert(rapier_handle, actor);
@@ -211,6 +218,79 @@ impl PhysxWorld {
}
}
/*
* Articulations.
*/
for multibody in multibody_joints.multibodies() {
let mut articulation: Owner<PxArticulationReducedCoordinate> =
physics.create_articulation_reduced_coordinate(()).unwrap();
let mut parent = None;
for link in multibody.links() {
let is_root = parent.is_none();
let rb_handle = link.rigid_body_handle();
let rb = bodies.get(rb_handle).unwrap();
if is_root && rb.is_static() {
articulation.set_articulation_flag(ArticulationFlag::FixBase, true);
}
let link_pose = rb.position().into_physx();
let px_link = articulation
.create_link(parent.take(), &link_pose, rb_handle)
.unwrap();
// TODO: there is no get_inbound_joint_mut?
if let Some(px_inbound_joint) = unsafe {
(PxArticulationLink_getInboundJoint(px_link.as_ptr())
as *mut physx_sys::PxArticulationJointBase
as *mut JointMap)
.as_mut()
} {
let frame1 = link.joint().data.local_frame1.into_physx();
let frame2 = link.joint().data.local_frame2.into_physx();
px_inbound_joint.set_parent_pose(&frame1);
px_inbound_joint.set_child_pose(&frame2);
/*
let px_joint = px_inbound_joint
.as_articulation_joint_reduced_coordinate()
.unwrap();
if let Some(_) = link
.articulation()
.downcast_ref::<SphericalMultibodyJoint>()
{
px_joint.set_joint_type(ArticulationJointType::Spherical);
px_joint.set_motion(ArticulationAxis::Swing1, ArticulationMotion::Free);
px_joint.set_motion(ArticulationAxis::Swing2, ArticulationMotion::Free);
px_joint.set_motion(ArticulationAxis::Twist, ArticulationMotion::Free);
} else if let Some(_) =
link.articulation().downcast_ref::<RevoluteMultibodyJoint>()
{
px_joint.set_joint_type(ArticulationJointType::Revolute);
px_joint.set_motion(ArticulationAxis::Swing1, ArticulationMotion::Free);
px_joint.set_motion(ArticulationAxis::Swing2, ArticulationMotion::Free);
px_joint.set_motion(ArticulationAxis::Twist, ArticulationMotion::Free);
}
*/
}
// FIXME: we are using transmute here in order to erase the lifetime of
// the &mut ref behind px_link (which is tied to the lifetime of the
// multibody_joint). This looks necessary because we need
// that mutable ref to create the next link. Yet, the link creation
// methods also requires a mutable ref to the multibody_joint.
rapier2link.insert(rb_handle, px_link as *mut PxArticulationLink);
parent = Some(unsafe { std::mem::transmute(px_link as *mut _) });
}
scene.add_articulation(articulation);
}
/*
*
* Colliders
@@ -223,7 +303,15 @@ impl PhysxWorld {
if let Some(parent_handle) = collider.parent() {
let parent_body = &bodies[parent_handle];
if !parent_body.is_dynamic() {
if let Some(link) = rapier2link.get_mut(&parent_handle) {
unsafe {
physx_sys::PxRigidActor_attachShape_mut(
*link as *mut PxRigidActor,
px_shape.as_mut_ptr(),
);
}
} else if !parent_body.is_dynamic() {
println!("Ground collider");
let actor = rapier2static.get_mut(&parent_handle).unwrap();
actor.attach_shape(&mut px_shape);
} else {
@@ -246,8 +334,8 @@ impl PhysxWorld {
}
// Update mass properties and CCD flags.
for (rapier_handle, actor) in rapier2dynamic.iter_mut() {
let rb = &bodies[*rapier_handle];
for (rapier_handle, _rb) in bodies.iter() {
let rb = &bodies[rapier_handle];
let densities: Vec<_> = rb
.colliders()
.iter()
@@ -255,8 +343,16 @@ impl PhysxWorld {
.collect();
unsafe {
let actor = if let Some(actor) = rapier2dynamic.get_mut(&rapier_handle) {
std::mem::transmute(actor.as_mut())
} else if let Some(actor) = rapier2link.get_mut(&rapier_handle) {
*actor as *mut _
} else {
continue;
};
physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
std::mem::transmute(actor.as_mut()),
actor,
densities.as_ptr(),
densities.len() as u32,
std::ptr::null(),
@@ -265,19 +361,10 @@ impl PhysxWorld {
if rb.is_ccd_enabled() {
physx_sys::PxRigidBody_setRigidBodyFlag_mut(
std::mem::transmute(actor.as_mut()),
actor,
RigidBodyFlag::EnableCCD as u32,
true,
);
// physx_sys::PxRigidBody_setMinCCDAdvanceCoefficient_mut(
// std::mem::transmute(actor.as_mut()),
// 0.0,
// );
// physx_sys::PxRigidBody_setRigidBodyFlag_mut(
// std::mem::transmute(actor.as_mut()),
// RigidBodyFlag::EnableCCDFriction as u32,
// true,
// );
}
}
}
@@ -289,9 +376,10 @@ impl PhysxWorld {
*/
Self::setup_joints(
&mut physics,
joints,
impulse_joints,
&mut rapier2static,
&mut rapier2dynamic,
&mut rapier2link,
);
for (_, actor) in rapier2static {
@@ -312,18 +400,22 @@ impl PhysxWorld {
fn setup_joints(
physics: &mut PxPhysicsFoundation,
joints: &JointSet,
impulse_joints: &ImpulseJointSet,
rapier2static: &mut HashMap<RigidBodyHandle, Owner<PxRigidStatic>>,
rapier2dynamic: &mut HashMap<RigidBodyHandle, Owner<PxRigidDynamic>>,
rapier2link: &mut HashMap<RigidBodyHandle, *mut PxArticulationLink>,
) {
unsafe {
for joint in joints.iter() {
for joint in impulse_joints.iter() {
let actor1 = rapier2static
.get_mut(&joint.1.body1)
.map(|act| &mut **act as *mut PxRigidStatic as *mut PxRigidActor)
.or(rapier2dynamic
.get_mut(&joint.1.body1)
.map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor))
.or(rapier2link
.get_mut(&joint.1.body1)
.map(|lnk| *lnk as *mut PxRigidActor))
.unwrap();
let actor2 = rapier2static
.get_mut(&joint.1.body2)
@@ -331,150 +423,83 @@ impl PhysxWorld {
.or(rapier2dynamic
.get_mut(&joint.1.body2)
.map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor))
.or(rapier2link
.get_mut(&joint.1.body2)
.map(|lnk| *lnk as *mut PxRigidActor))
.unwrap();
match &joint.1.params {
JointParams::BallJoint(params) => {
let frame1 = Isometry3::new(params.local_anchor1.coords, na::zero())
.into_physx()
.into();
let frame2 = Isometry3::new(params.local_anchor2.coords, na::zero())
.into_physx()
.into();
let px_frame1 = joint.1.data.local_frame1.into_physx();
let px_frame2 = joint.1.data.local_frame2.into_physx();
physx_sys::phys_PxSphericalJointCreate(
physics.as_mut_ptr(),
actor1,
&frame1 as *const _,
actor2,
&frame2 as *const _,
);
}
JointParams::RevoluteJoint(params) => {
// NOTE: orthonormal_basis() returns the two basis vectors.
// However we only use one and recompute the other just to
// make sure our basis is right-handed.
let basis1a = params.local_axis1.orthonormal_basis()[0];
let basis2a = params.local_axis2.orthonormal_basis()[0];
let basis1b = params.local_axis1.cross(&basis1a);
let basis2b = params.local_axis2.cross(&basis2a);
let px_joint = physx_sys::phys_PxD6JointCreate(
physics.as_mut_ptr(),
actor1,
px_frame1.as_ptr(),
actor2,
px_frame2.as_ptr(),
);
let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
params.local_axis1.into_inner(),
basis1a,
basis1b,
]));
let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
params.local_axis2.into_inner(),
basis2a,
basis2b,
]));
let axisangle1 = rotmat1.scaled_axis();
let axisangle2 = rotmat2.scaled_axis();
let motion_x = if joint.1.data.limit_axes.contains(JointAxesMask::X) {
physx_sys::PxD6Motion::eLIMITED
} else if !joint.1.data.locked_axes.contains(JointAxesMask::X) {
physx_sys::PxD6Motion::eFREE
} else {
physx_sys::PxD6Motion::eLOCKED
};
let motion_y = if joint.1.data.limit_axes.contains(JointAxesMask::Y) {
physx_sys::PxD6Motion::eLIMITED
} else if !joint.1.data.locked_axes.contains(JointAxesMask::Y) {
physx_sys::PxD6Motion::eFREE
} else {
physx_sys::PxD6Motion::eLOCKED
};
let motion_z = if joint.1.data.limit_axes.contains(JointAxesMask::Z) {
physx_sys::PxD6Motion::eLIMITED
} else if !joint.1.data.locked_axes.contains(JointAxesMask::Z) {
physx_sys::PxD6Motion::eFREE
} else {
physx_sys::PxD6Motion::eLOCKED
};
let motion_ax = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_X) {
physx_sys::PxD6Motion::eLIMITED
} else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_X) {
physx_sys::PxD6Motion::eFREE
} else {
physx_sys::PxD6Motion::eLOCKED
};
let motion_ay = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_Y) {
physx_sys::PxD6Motion::eLIMITED
} else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_Y) {
physx_sys::PxD6Motion::eFREE
} else {
physx_sys::PxD6Motion::eLOCKED
};
let motion_az = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_Z) {
physx_sys::PxD6Motion::eLIMITED
} else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_Z) {
physx_sys::PxD6Motion::eFREE
} else {
physx_sys::PxD6Motion::eLOCKED
};
let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1)
.into_physx()
.into();
let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2)
.into_physx()
.into();
let revolute_joint = physx_sys::phys_PxRevoluteJointCreate(
physics.as_mut_ptr(),
actor1,
&frame1 as *const _,
actor2,
&frame2 as *const _,
);
physx_sys::PxRevoluteJoint_setDriveVelocity_mut(
revolute_joint,
params.motor_target_vel,
true,
);
if params.motor_damping != 0.0 {
physx_sys::PxRevoluteJoint_setRevoluteJointFlag_mut(
revolute_joint,
physx_sys::PxRevoluteJointFlag::eDRIVE_ENABLED,
true,
);
}
}
JointParams::PrismaticJoint(params) => {
// NOTE: orthonormal_basis() returns the two basis vectors.
// However we only use one and recompute the other just to
// make sure our basis is right-handed.
let basis1a = params.local_axis1().orthonormal_basis()[0];
let basis2a = params.local_axis2().orthonormal_basis()[0];
let basis1b = params.local_axis1().cross(&basis1a);
let basis2b = params.local_axis2().cross(&basis2a);
let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
params.local_axis1().into_inner(),
basis1a,
basis1b,
]));
let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[
params.local_axis2().into_inner(),
basis2a,
basis2b,
]));
let axisangle1 = rotmat1.scaled_axis();
let axisangle2 = rotmat2.scaled_axis();
let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1)
.into_physx()
.into();
let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2)
.into_physx()
.into();
let joint = physx_sys::phys_PxPrismaticJointCreate(
physics.as_mut_ptr(),
actor1,
&frame1 as *const _,
actor2,
&frame2 as *const _,
);
if params.limits_enabled {
let limits = physx_sys::PxJointLinearLimitPair {
restitution: 0.0,
bounceThreshold: 0.0,
stiffness: 0.0,
damping: 0.0,
contactDistance: 0.01,
lower: params.limits[0],
upper: params.limits[1],
};
physx_sys::PxPrismaticJoint_setLimit_mut(joint, &limits);
physx_sys::PxPrismaticJoint_setPrismaticJointFlag_mut(
joint,
physx_sys::PxPrismaticJointFlag::eLIMIT_ENABLED,
true,
);
}
}
JointParams::FixedJoint(params) => {
let frame1 = params.local_frame1.into_physx().into();
let frame2 = params.local_frame2.into_physx().into();
physx_sys::phys_PxFixedJointCreate(
physics.as_mut_ptr(),
actor1,
&frame1 as *const _,
actor2,
&frame2 as *const _,
);
} // JointParams::GenericJoint(_) => {
// eprintln!(
// "Joint type currently unsupported by the PhysX backend: GenericJoint."
// )
// }
}
physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eX, motion_x);
physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eY, motion_y);
physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eZ, motion_z);
physx_sys::PxD6Joint_setMotion_mut(
px_joint,
physx_sys::PxD6Axis::eTWIST,
motion_ax,
);
physx_sys::PxD6Joint_setMotion_mut(
px_joint,
physx_sys::PxD6Axis::eSWING1,
motion_ay,
);
physx_sys::PxD6Joint_setMotion_mut(
px_joint,
physx_sys::PxD6Axis::eSWING2,
motion_az,
);
}
}
}
@@ -497,9 +522,7 @@ impl PhysxWorld {
}
pub fn sync(&mut self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
for actor in self.scene.as_mut().unwrap().get_dynamic_actors() {
let handle = actor.get_user_data();
let pos = actor.get_global_pose().into_na();
let mut sync_pos = |handle: &RigidBodyHandle, pos: Isometry3<f32>| {
let rb = &mut bodies[*handle];
rb.set_position(pos, false);
@@ -509,6 +532,22 @@ impl PhysxWorld {
pos * collider.position_wrt_parent().copied().unwrap_or(na::one()),
);
}
};
for actor in self.scene.as_mut().unwrap().get_dynamic_actors() {
let handle = actor.get_user_data();
let pos = actor.get_global_pose().into_na();
sync_pos(handle, pos);
}
for articulation in self.scene.as_mut().unwrap().get_articulations() {
if let Some(articulation) = articulation.as_articulation_reduced_coordinate() {
for link in articulation.get_links() {
let handle = link.get_user_data();
let pos = link.get_global_pose().into_na();
sync_pos(handle, pos);
}
}
}
}
}
@@ -673,7 +712,7 @@ fn physx_collider_from_rapier_collider(
type PxPhysicsFoundation = PhysicsFoundation<DefaultAllocator, PxShape>;
type PxMaterial = physx::material::PxMaterial<()>;
type PxShape = physx::shape::PxShape<(), PxMaterial>;
type PxArticulationLink = physx::articulation_link::PxArticulationLink<(), PxShape>;
type PxArticulationLink = physx::articulation_link::PxArticulationLink<RigidBodyHandle, PxShape>;
type PxRigidStatic = physx::rigid_static::PxRigidStatic<(), PxShape>;
type PxRigidDynamic = physx::rigid_dynamic::PxRigidDynamic<RigidBodyHandle, PxShape>;
type PxArticulation = physx::articulation::PxArticulation<(), PxArticulationLink>;

382
src_testbed/testbed.rs
View File

@@ -11,7 +11,8 @@ use crate::{graphics::GraphicsManager, harness::RunState};
use na::{self, Point2, Point3, Vector3};
use rapier::dynamics::{
IntegrationParameters, JointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet,
ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation,
RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
#[cfg(feature = "dim3")]
@@ -22,8 +23,6 @@ use rapier::pipeline::PhysicsHooks;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
use crate::box2d_backend::Box2dWorld;
use crate::harness::Harness;
#[cfg(feature = "other-backends")]
use crate::nphysics_backend::NPhysicsWorld;
#[cfg(all(feature = "dim3", feature = "other-backends"))]
use crate::physx_backend::PhysxWorld;
use bevy::render::camera::Camera;
@@ -38,12 +37,10 @@ use crate::camera3d::{OrbitCamera, OrbitCameraPlugin};
use bevy::render::pipeline::PipelineDescriptor;
const RAPIER_BACKEND: usize = 0;
#[cfg(feature = "other-backends")]
const NPHYSICS_BACKEND: usize = 1;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
const BOX2D_BACKEND: usize = 2;
pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 2;
pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 3;
const BOX2D_BACKEND: usize = 1;
pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 1;
pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 2;
#[derive(PartialEq)]
pub enum RunMode {
@@ -128,7 +125,6 @@ struct OtherBackends {
box2d: Option<Box2dWorld>,
#[cfg(feature = "dim3")]
physx: Option<PhysxWorld>,
nphysics: Option<NPhysicsWorld>,
}
struct Plugins(Vec<Box<dyn TestbedPlugin>>);
@@ -169,8 +165,6 @@ impl TestbedApp {
#[allow(unused_mut)]
let mut backend_names = vec!["rapier"];
#[cfg(feature = "other-backends")]
backend_names.push("nphysics");
#[cfg(all(feature = "dim2", feature = "other-backends"))]
backend_names.push("box2d");
#[cfg(all(feature = "dim3", feature = "other-backends"))]
@@ -207,7 +201,6 @@ impl TestbedApp {
box2d: None,
#[cfg(feature = "dim3")]
physx: None,
nphysics: None,
};
TestbedApp {
@@ -271,28 +264,15 @@ impl TestbedApp {
for (backend_id, backend) in backend_names.iter().enumerate() {
println!("|_ using backend {}", backend);
self.state.selected_backend = backend_id;
if cfg!(feature = "dim3")
&& (backend_id == PHYSX_BACKEND_PATCH_FRICTION
|| backend_id == PHYSX_BACKEND_TWO_FRICTION_DIR)
{
self.harness
.physics
.integration_parameters
.max_velocity_iterations = 1;
self.harness
.physics
.integration_parameters
.max_position_iterations = 4;
} else {
self.harness
.physics
.integration_parameters
.max_velocity_iterations = 4;
self.harness
.physics
.integration_parameters
.max_position_iterations = 1;
}
self.harness
.physics
.integration_parameters
.max_velocity_iterations = 4;
self.harness
.physics
.integration_parameters
.max_stabilization_iterations = 1;
// Init world.
let mut testbed = Testbed {
graphics: None,
@@ -341,20 +321,6 @@ impl TestbedApp {
);
}
}
#[cfg(feature = "other-backends")]
{
if self.state.selected_backend == NPHYSICS_BACKEND {
self.other_backends.nphysics.as_mut().unwrap().step(
&mut self.harness.physics.pipeline.counters,
&self.harness.physics.integration_parameters,
);
self.other_backends.nphysics.as_mut().unwrap().sync(
&mut self.harness.physics.bodies,
&mut self.harness.physics.colliders,
);
}
}
}
// Skip the first update.
@@ -498,20 +464,40 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
&mut self.harness
}
pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ())
pub fn set_world(
&mut self,
bodies: RigidBodySet,
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
) {
self.set_world_with_params(
bodies,
colliders,
impulse_joints,
multibody_joints,
Vector::y() * -9.81,
(),
)
}
pub fn set_world_with_params(
&mut self,
bodies: RigidBodySet,
colliders: ColliderSet,
joints: JointSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<f32>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
) {
self.harness
.set_world_with_params(bodies, colliders, joints, gravity, hooks);
self.harness.set_world_with_params(
bodies,
colliders,
impulse_joints,
multibody_joints,
gravity,
hooks,
);
self.state
.action_flags
@@ -526,7 +512,7 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
self.harness.physics.gravity,
&self.harness.physics.bodies,
&self.harness.physics.colliders,
&self.harness.physics.joints,
&self.harness.physics.impulse_joints,
));
}
}
@@ -541,24 +527,13 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
&self.harness.physics.integration_parameters,
&self.harness.physics.bodies,
&self.harness.physics.colliders,
&self.harness.physics.joints,
&self.harness.physics.impulse_joints,
&self.harness.physics.multibody_joints,
self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR,
self.harness.state.num_threads,
));
}
}
#[cfg(feature = "other-backends")]
{
if self.state.selected_backend == NPHYSICS_BACKEND {
self.other_backends.nphysics = Some(NPhysicsWorld::from_rapier(
self.harness.physics.gravity,
&self.harness.physics.bodies,
&self.harness.physics.colliders,
&self.harness.physics.joints,
));
}
}
}
#[cfg(feature = "dim2")]
@@ -634,85 +609,134 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> {
self.plugins.0.push(Box::new(plugin));
}
// fn handle_common_event<'b>(&mut self, event: Event<'b>) -> Event<'b> {
// match event.value {
// WindowEvent::Key(Key::T, Action::Release, _) => {
// if self.state.running == RunMode::Stop {
// self.state.running = RunMode::Running;
// } else {
// self.state.running = RunMode::Stop;
// }
// }
// WindowEvent::Key(Key::S, Action::Release, _) => self.state.running = RunMode::Step,
// WindowEvent::Key(Key::R, Action::Release, _) => self
// .state
// .action_flags
// .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
// WindowEvent::Key(Key::C, Action::Release, _) => {
// // Delete 1 collider of 10% of the remaining dynamic bodies.
// let mut colliders: Vec<_> = self
// .harness
// .physics
// .bodies
// .iter()
// .filter(|e| e.1.is_dynamic())
// .filter(|e| !e.1.colliders().is_empty())
// .map(|e| e.1.colliders().to_vec())
// .collect();
// colliders.sort_by_key(|co| -(co.len() as isize));
//
// let num_to_delete = (colliders.len() / 10).max(1);
// for to_delete in &colliders[..num_to_delete] {
// self.harness.physics.colliders.remove(
// to_delete[0],
// &mut self.harness.physics.islands,
// &mut self.harness.physics.bodies,
// true,
// );
// }
// }
// WindowEvent::Key(Key::D, Action::Release, _) => {
// // Delete 10% of the remaining dynamic bodies.
// let dynamic_bodies: Vec<_> = self
// .harness
// .physics
// .bodies
// .iter()
// .filter(|e| !e.1.is_static())
// .map(|e| e.0)
// .collect();
// let num_to_delete = (dynamic_bodies.len() / 10).max(1);
// for to_delete in &dynamic_bodies[..num_to_delete] {
// self.harness.physics.bodies.remove(
// *to_delete,
// &mut self.harness.physics.islands,
// &mut self.harness.physics.colliders,
// &mut self.harness.physics.joints,
// );
// }
// }
// WindowEvent::Key(Key::J, Action::Release, _) => {
// // Delete 10% of the remaining joints.
// let joints: Vec<_> = self.harness.physics.joints.iter().map(|e| e.0).collect();
// let num_to_delete = (joints.len() / 10).max(1);
// for to_delete in &joints[..num_to_delete] {
// self.harness.physics.joints.remove(
// *to_delete,
// &mut self.harness.physics.islands,
// &mut self.harness.physics.bodies,
// true,
// );
// }
// }
// WindowEvent::CursorPos(x, y, _) => {
// self.state.cursor_pos.x = x as f32;
// self.state.cursor_pos.y = y as f32;
// }
// _ => {}
// }
//
// event
// }
fn handle_common_events(&mut self, events: &Input<KeyCode>) {
for key in events.get_just_released() {
match *key {
KeyCode::T => {
if self.state.running == RunMode::Stop {
self.state.running = RunMode::Running;
} else {
self.state.running = RunMode::Stop;
}
}
KeyCode::S => self.state.running = RunMode::Step,
KeyCode::R => self
.state
.action_flags
.set(TestbedActionFlags::EXAMPLE_CHANGED, true),
KeyCode::C => {
// Delete 1 collider of 10% of the remaining dynamic bodies.
let mut colliders: Vec<_> = self
.harness
.physics
.bodies
.iter()
.filter(|e| e.1.is_dynamic())
.filter(|e| !e.1.colliders().is_empty())
.map(|e| e.1.colliders().to_vec())
.collect();
colliders.sort_by_key(|co| -(co.len() as isize));
let num_to_delete = (colliders.len() / 10).max(1);
for to_delete in &colliders[..num_to_delete] {
if let Some(graphics) = self.graphics.as_mut() {
graphics.remove_collider(to_delete[0], &self.harness.physics.colliders);
}
self.harness.physics.colliders.remove(
to_delete[0],
&mut self.harness.physics.islands,
&mut self.harness.physics.bodies,
true,
);
}
}
KeyCode::D => {
// Delete 10% of the remaining dynamic bodies.
let dynamic_bodies: Vec<_> = self
.harness
.physics
.bodies
.iter()
.filter(|e| !e.1.is_static())
.map(|e| e.0)
.collect();
let num_to_delete = (dynamic_bodies.len() / 10).max(1);
for to_delete in &dynamic_bodies[..num_to_delete] {
if let Some(graphics) = self.graphics.as_mut() {
graphics.remove_body(*to_delete);
}
self.harness.physics.bodies.remove(
*to_delete,
&mut self.harness.physics.islands,
&mut self.harness.physics.colliders,
&mut self.harness.physics.impulse_joints,
&mut self.harness.physics.multibody_joints,
);
}
}
KeyCode::J => {
// Delete 10% of the remaining impulse_joints.
let impulse_joints: Vec<_> = self
.harness
.physics
.impulse_joints
.iter()
.map(|e| e.0)
.collect();
let num_to_delete = (impulse_joints.len() / 10).max(1);
for to_delete in &impulse_joints[..num_to_delete] {
self.harness.physics.impulse_joints.remove(
*to_delete,
&mut self.harness.physics.islands,
&mut self.harness.physics.bodies,
true,
);
}
}
KeyCode::A => {
// Delete 10% of the remaining multibody_joints.
let multibody_joints: Vec<_> = self
.harness
.physics
.multibody_joints
.iter()
.map(|e| e.0)
.collect();
let num_to_delete = (multibody_joints.len() / 10).max(1);
for to_delete in &multibody_joints[..num_to_delete] {
self.harness.physics.multibody_joints.remove(
*to_delete,
&mut self.harness.physics.islands,
&mut self.harness.physics.bodies,
true,
);
}
}
KeyCode::M => {
// Delete one remaining multibody.
let to_delete = self
.harness
.physics
.multibody_joints
.iter()
.next()
.map(|a| a.2.rigid_body_handle());
if let Some(to_delete) = to_delete {
self.harness
.physics
.multibody_joints
.remove_multibody_articulations(
to_delete,
&mut self.harness.physics.islands,
&mut self.harness.physics.bodies,
true,
);
}
}
_ => {}
}
}
}
// #[cfg(feature = "dim2")]
// fn handle_special_event(&mut self) {}
@@ -878,11 +902,37 @@ fn update_testbed(
ui_context: Res<EguiContext>,
mut gfx_components: Query<(&mut Transform,)>,
mut cameras: Query<(&Camera, &GlobalTransform, &mut OrbitCamera)>,
keys: Res<Input<KeyCode>>,
) {
let meshes = &mut *meshes;
let materials = &mut *materials;
let prev_example = state.selected_example;
// Handle inputs
{
let graphics_context = TestbedGraphics {
pipelines: &mut *pipelines,
shaders: &mut *shaders,
graphics: &mut *graphics,
commands: &mut commands,
meshes: &mut *meshes,
materials: &mut *materials,
components: &mut gfx_components,
camera: &mut cameras.iter_mut().next().unwrap().2,
};
let mut testbed = Testbed {
graphics: Some(graphics_context),
state: &mut *state,
harness: &mut *harness,
#[cfg(feature = "other-backends")]
other_backends: &mut *other_backends,
plugins: &mut *plugins,
};
testbed.handle_common_events(&*keys);
}
// Update UI
{
let harness = &mut *harness;
@@ -943,28 +993,6 @@ fn update_testbed(
if state.selected_example != prev_example {
harness.physics.integration_parameters = IntegrationParameters::default();
if cfg!(feature = "dim3")
&& (state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION
|| state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR)
{
let max_position_iterations = harness
.physics
.integration_parameters
.max_position_iterations;
let max_velocity_iterations = harness
.physics
.integration_parameters
.max_velocity_iterations;
harness
.physics
.integration_parameters
.max_velocity_iterations = max_position_iterations;
harness
.physics
.integration_parameters
.max_position_iterations = max_velocity_iterations;
}
}
let selected_example = state.selected_example;
@@ -1009,7 +1037,7 @@ fn update_testbed(
&harness.physics.narrow_phase,
&harness.physics.bodies,
&harness.physics.colliders,
&harness.physics.joints,
&harness.physics.impulse_joints,
)
.ok();
@@ -1172,22 +1200,6 @@ fn update_testbed(
}
}
#[cfg(feature = "other-backends")]
{
if state.selected_backend == NPHYSICS_BACKEND {
let harness = &mut *harness;
other_backends.nphysics.as_mut().unwrap().step(
&mut harness.physics.pipeline.counters,
&harness.physics.integration_parameters,
);
other_backends
.nphysics
.as_mut()
.unwrap()
.sync(&mut harness.physics.bodies, &mut harness.physics.colliders);
}
}
for plugin in &mut plugins.0 {
plugin.run_callbacks(&mut harness);
}

84
src_testbed/ui.rs
View File

@@ -1,7 +1,10 @@
use rapier::counters::Counters;
use crate::harness::Harness;
use crate::testbed::{RunMode, TestbedActionFlags, TestbedState, TestbedStateFlags};
use crate::testbed::{
RunMode, TestbedActionFlags, TestbedState, TestbedStateFlags, PHYSX_BACKEND_PATCH_FRICTION,
PHYSX_BACKEND_TWO_FRICTION_DIR,
};
use crate::PhysicsState;
use bevy_egui::egui::Slider;
@@ -10,8 +13,6 @@ use bevy_egui::{egui, EguiContext};
pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &mut Harness) {
egui::Window::new("Parameters").show(ui_context.ctx(), |ui| {
if state.backend_names.len() > 1 && !state.example_names.is_empty() {
#[cfg(all(feature = "dim3", feature = "other-backends"))]
let prev_selected_backend = state.selected_backend;
let mut changed = false;
egui::ComboBox::from_label("backend")
.width(150.0)
@@ -29,30 +30,6 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
state
.action_flags
.set(TestbedActionFlags::BACKEND_CHANGED, true);
#[cfg(all(feature = "dim3", feature = "other-backends"))]
fn is_physx(id: usize) -> bool {
id == crate::testbed::PHYSX_BACKEND_PATCH_FRICTION
|| id == crate::testbed::PHYSX_BACKEND_TWO_FRICTION_DIR
}
#[cfg(all(feature = "dim3", feature = "other-backends"))]
if (is_physx(state.selected_backend) && !is_physx(prev_selected_backend))
|| (!is_physx(state.selected_backend) && is_physx(prev_selected_backend))
{
// PhysX defaults (4 position iterations, 1 velocity) are the
// opposite of rapier's (4 velocity iterations, 1 position).
std::mem::swap(
&mut harness
.physics
.integration_parameters
.max_position_iterations,
&mut harness
.physics
.integration_parameters
.max_velocity_iterations,
);
}
}
ui.separator();
@@ -113,14 +90,47 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
});
let integration_parameters = &mut harness.physics.integration_parameters;
ui.add(
Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200)
.text("vels. iters."),
);
ui.add(
Slider::new(&mut integration_parameters.max_position_iterations, 0..=200)
.text("pos. iters."),
ui.checkbox(
&mut integration_parameters.interleave_restitution_and_friction_resolution,
"interleave friction resolution",
);
if state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION
|| state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR
{
ui.add(
Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200)
.text("pos. iters."),
);
ui.add(
Slider::new(
&mut integration_parameters.max_stabilization_iterations,
0..=200,
)
.text("vel. iters."),
);
} else {
ui.add(
Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200)
.text("vel. rest. iters."),
);
ui.add(
Slider::new(
&mut integration_parameters.max_velocity_friction_iterations,
0..=200,
)
.text("vel. frict. iters."),
);
ui.add(
Slider::new(
&mut integration_parameters.max_stabilization_iterations,
0..=200,
)
.text("vel. stab. iters."),
);
}
#[cfg(feature = "parallel")]
{
ui.add(
@@ -135,10 +145,6 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
Slider::new(&mut integration_parameters.min_island_size, 1..=10_000)
.text("min island size"),
);
ui.add(
Slider::new(&mut integration_parameters.warmstart_coeff, 0.0..=1.0)
.text("warmstart coeff"),
);
let mut frequency = integration_parameters.inv_dt().round() as u32;
ui.add(Slider::new(&mut frequency, 0..=240).text("frequency (Hz)"));
integration_parameters.set_inv_dt(frequency as f32);
@@ -247,7 +253,7 @@ fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String {
let cs = bincode::serialize(&physics.colliders).unwrap();
// println!("cs: {}", instant::now() - t);
// let t = instant::now();
let js = bincode::serialize(&physics.joints).unwrap();
let js = bincode::serialize(&physics.impulse_joints).unwrap();
// println!("js: {}", instant::now() - t);
let serialization_time = instant::now() - t;
let hash_bf = md5::compute(&bf);