aboutsummaryrefslogtreecommitdiff
path: root/src_testbed
diff options
context:
space:
mode:
Diffstat (limited to 'src_testbed')
-rw-r--r--src_testbed/box2d_backend.rs19
-rw-r--r--src_testbed/harness/mod.rs45
-rw-r--r--src_testbed/lib.rs10
-rw-r--r--src_testbed/nphysics_backend.rs248
-rw-r--r--src_testbed/physics/mod.rs25
-rw-r--r--src_testbed/physx_backend.rs383
-rw-r--r--src_testbed/testbed.rs382
-rw-r--r--src_testbed/ui.rs84
8 files changed, 513 insertions, 683 deletions
diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index f0bffa3..2a143fb 100644
--- a/src_testbed/box2d_backend.rs
+++ b/src_testbed/box2d_backend.rs
@@ -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();
}
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index 0bc08d0..3358a70 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -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,
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index a3e3a00..b9a8822 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -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"))]
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
deleted file mode 100644
index c7d6c0e..0000000
--- a/src_testbed/nphysics_backend.rs
+++ /dev/null
@@ -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),
- )
-}
diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 29e9a46..13bf915 100644
--- a/src_testbed/physics/mod.rs
+++ b/src_testbed/physics/mod.rs
@@ -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(),
diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs
index 6c3155f..9eec3ff 100644
--- a/src_testbed/physx_backend.rs
+++ b/src_testbed/physx_backend.rs
@@ -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);
@@ -212,6 +219,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();
-
- 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 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 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,
- );
+ let px_frame1 = joint.1.data.local_frame1.into_physx();
+ let px_frame2 = joint.1.data.local_frame2.into_physx();