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

Fix some solver issues

Browse Source 
- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier
This commit is contained in:
Sébastien Crozet
2022-01-16 16:40:59 +01:00
parent 4454a845e9
commit 0703e5527f
43 changed files with 936 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src_testbed/camera2d.rs
View File

@@ -39,10 +39,8 @@ impl OrbitCameraPlugin {
mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
) {
for (camera, mut transform) in query.iter_mut() {
if camera.enabled {
transform.translation = camera.center;
transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
}
transform.translation = camera.center;
transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
}
}

10
src_testbed/camera3d.rs
View File

@@ -50,12 +50,10 @@ impl OrbitCameraPlugin {
mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
) {
for (camera, mut transform) in query.iter_mut() {
if camera.enabled {
let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
* Quat::from_axis_angle(-Vec3::X, camera.y);
transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
transform.look_at(camera.center, Vec3::Y);
}
let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
* Quat::from_axis_angle(-Vec3::X, camera.y);
transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
transform.look_at(camera.center, Vec3::Y);
}
}

30
src_testbed/graphics.rs
View File

@@ -2,10 +2,10 @@ use bevy::prelude::*;
use na::{point, Point3};
use crate::math::Isometry;
use crate::objects::node::EntityWithGraphics;
use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
use rapier::math::{Isometry, Real};
//use crate::objects::capsule::Capsule;
//#[cfg(feature = "dim3")]
//use crate::objects::mesh::Mesh;
@@ -301,8 +301,8 @@ impl GraphicsManager {
handle: Option<ColliderHandle>,
shape: &dyn Shape,
sensor: bool,
pos: &Isometry<f32>,
delta: &Isometry<f32>,
pos: &Isometry<Real>,
delta: &Isometry<Real>,
color: Point3<f32>,
out: &mut Vec<EntityWithGraphics>,
) {
@@ -347,18 +347,24 @@ impl GraphicsManager {
_bodies: &RigidBodySet,
colliders: &ColliderSet,
components: &mut Query<(&mut Transform,)>,
_materials: &mut Assets<BevyMaterial>,
) {
for (_, ns) in self.b2sn.iter_mut() {
for n in ns.iter_mut() {
// if let Some(co) = colliders.get(n.collider()) {
// let bo = &_bodies[co.parent()];
//
// if bo.is_dynamic() {
// if bo.is_ccd_active() {
// n.set_color(point![1.0, 0.0, 0.0]);
// } else {
// n.set_color(point![0.0, 1.0, 0.0]);
// }
// if let Some(bo) = n
// .collider
// .and_then(|h| bodies.get(colliders.get(h)?.parent()?))
// {
// if bo.activation().time_since_can_sleep
// >= RigidBodyActivation::default_time_until_sleep()
// {
// n.set_color(materials, point![1.0, 0.0, 0.0]);
// }
// /* else if bo.activation().energy < bo.activation().threshold {
// n.set_color(materials, point![0.0, 0.0, 1.0]);
// } */
// else {
// n.set_color(materials, point![0.0, 1.0, 0.0]);
// }
// }

6
src_testbed/harness/mod.rs
View File

@@ -8,7 +8,7 @@ use rapier::dynamics::{
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use rapier::math::Vector;
use rapier::math::{Real, Vector};
use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
pub mod plugin;
@@ -131,7 +131,7 @@ impl Harness {
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<f32>,
gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
) {
// println!("Num bodies: {}", bodies.len());
@@ -235,7 +235,7 @@ impl Harness {
self.events.poll_all();
self.state.time += self.physics.integration_parameters.dt;
self.state.time += self.physics.integration_parameters.dt as f32;
self.state.timestep_id += 1;
}

8
src_testbed/lib.rs
View File

@@ -1,12 +1,4 @@
extern crate nalgebra as na;
#[cfg(feature = "dim2")]
extern crate parry2d as parry;
#[cfg(feature = "dim3")]
extern crate parry3d as parry;
#[cfg(feature = "dim2")]
extern crate rapier2d as rapier;
#[cfg(feature = "dim3")]
extern crate rapier3d as rapier;
#[macro_use]
extern crate bitflags;

78
src_testbed/objects/node.rs
View File

@@ -10,7 +10,7 @@ use bevy::render::render_resource::PrimitiveTopology;
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
#[cfg(feature = "dim3")]
use rapier::geometry::{Cone, Cylinder};
use rapier::math::Isometry;
use rapier::math::{Isometry, Real};
use crate::graphics::BevyMaterial;
#[cfg(feature = "dim2")]
@@ -26,7 +26,7 @@ pub struct EntityWithGraphics {
pub color: Point3<f32>,
pub base_color: Point3<f32>,
pub collider: Option<ColliderHandle>,
pub delta: Isometry<f32>,
pub delta: Isometry<Real>,
pub opacity: f32,
material: Handle<BevyMaterial>,
}
@@ -39,8 +39,8 @@ impl EntityWithGraphics {
prefab_meshs: &HashMap<ShapeType, Handle<Mesh>>,
shape: &dyn Shape,
collider: Option<ColliderHandle>,
collider_pos: Isometry<f32>,
delta: Isometry<f32>,
collider_pos: Isometry<Real>,
delta: Isometry<Real>,
color: Point3<f32>,
sensor: bool,
) -> Self {
@@ -56,16 +56,16 @@ impl EntityWithGraphics {
let bevy_color = Color::rgba(color.x, color.y, color.z, opacity);
let shape_pos = collider_pos * delta;
let mut transform = Transform::from_scale(scale);
transform.translation.x = shape_pos.translation.vector.x;
transform.translation.y = shape_pos.translation.vector.y;
transform.translation.x = shape_pos.translation.vector.x as f32;
transform.translation.y = shape_pos.translation.vector.y as f32;
#[cfg(feature = "dim3")]
{
transform.translation.z = shape_pos.translation.vector.z;
transform.translation.z = shape_pos.translation.vector.z as f32;
transform.rotation = Quat::from_xyzw(
shape_pos.rotation.i,
shape_pos.rotation.j,
shape_pos.rotation.k,
shape_pos.rotation.w,
shape_pos.rotation.i as f32,
shape_pos.rotation.j as f32,
shape_pos.rotation.k as f32,
shape_pos.rotation.w as f32,
);
}
#[cfg(feature = "dim2")]
@@ -73,7 +73,7 @@ impl EntityWithGraphics {
if sensor {
transform.translation.z = -10.0;
}
transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle());
transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle() as f32);
}
#[cfg(feature = "dim2")]
@@ -172,21 +172,21 @@ impl EntityWithGraphics {
if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
let co_pos = co.position() * self.delta;
pos.translation.x = co_pos.translation.vector.x;
pos.translation.y = co_pos.translation.vector.y;
pos.translation.x = co_pos.translation.vector.x as f32;
pos.translation.y = co_pos.translation.vector.y as f32;
#[cfg(feature = "dim3")]
{
pos.translation.z = co_pos.translation.vector.z;
pos.translation.z = co_pos.translation.vector.z as f32;
pos.rotation = Quat::from_xyzw(
co_pos.rotation.i,
co_pos.rotation.j,
co_pos.rotation.k,
co_pos.rotation.w,
co_pos.rotation.i as f32,
co_pos.rotation.j as f32,
co_pos.rotation.k as f32,
co_pos.rotation.w as f32,
);
}
#[cfg(feature = "dim2")]
{
pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle());
pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle() as f32);
}
}
}
@@ -266,7 +266,7 @@ impl EntityWithGraphics {
}
#[cfg(feature = "dim2")]
fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
fn bevy_mesh_from_polyline(vertices: Vec<Point2<Real>>) -> Mesh {
let n = vertices.len();
let idx = (1..n as u32 - 1).map(|i| [0, i, i + 1]).collect();
let vtx = vertices
@@ -277,7 +277,7 @@ fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
}
#[cfg(feature = "dim2")]
fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
let (vtx, idx) = buffers;
// let mut normals: Vec<[f32; 3]> = vec![];
let mut vertices: Vec<[f32; 3]> = vec![];
@@ -287,11 +287,11 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
let a = vtx[idx[0] as usize];
let b = vtx[idx[1] as usize];
vertices.push([a.x, a.y, 0.0]);
vertices.push([b.x, b.y, 0.0]);
vertices.push([a.x as f32, a.y as f32, 0.0]);
vertices.push([b.x as f32, b.y as f32, 0.0]);
}
} else {
vertices = vtx.iter().map(|v| [v.x, v.y, 0.0]).collect();
vertices = vtx.iter().map(|v| [v.x as f32, v.y as f32, 0.0]).collect();
}
let indices: Vec<_> = (0..vertices.len() as u32).collect();
@@ -310,7 +310,7 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
mesh
}
fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
let (vtx, idx) = buffers;
let mut normals: Vec<[f32; 3]> = vec![];
let mut vertices: Vec<[f32; 3]> = vec![];
@@ -320,9 +320,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
let b = vtx[idx[1] as usize];
let c = vtx[idx[2] as usize];
vertices.push(a.into());
vertices.push(b.into());
vertices.push(c.into());
vertices.push(a.cast::<f32>().into());
vertices.push(b.cast::<f32>().into());
vertices.push(c.cast::<f32>().into());
}
for vtx in vertices.chunks(3) {
@@ -330,9 +330,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
let b = Point3::from(vtx[1]);
let c = Point3::from(vtx[2]);
let n = (b - a).cross(&(c - a)).normalize();
normals.push(n.into());
normals.push(n.into());
normals.push(n.into());
normals.push(n.cast::<f32>().into());
normals.push(n.cast::<f32>().into());
normals.push(n.cast::<f32>().into());
}
normals
@@ -358,36 +358,36 @@ fn collider_mesh_scale(co_shape: &dyn Shape) -> Vec3 {
#[cfg(feature = "dim2")]
ShapeType::Cuboid => {
let c = co_shape.as_cuboid().unwrap();
Vec3::new(c.half_extents.x, c.half_extents.y, 1.0)
Vec3::new(c.half_extents.x as f32, c.half_extents.y as f32, 1.0)
}
ShapeType::Ball => {
let b = co_shape.as_ball().unwrap();
Vec3::new(b.radius, b.radius, b.radius)
Vec3::new(b.radius as f32, b.radius as f32, b.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::Cuboid => {
let c = co_shape.as_cuboid().unwrap();
Vec3::from_slice(c.half_extents.as_slice())
Vec3::from_slice(c.half_extents.cast::<f32>().as_slice())
}
#[cfg(feature = "dim3")]
ShapeType::Cylinder => {
let c = co_shape.as_cylinder().unwrap();
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::RoundCylinder => {
let c = &co_shape.as_round_cylinder().unwrap().base_shape;
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::Cone => {
let c = co_shape.as_cone().unwrap();
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::RoundCone => {
let c = &co_shape.as_round_cone().unwrap().base_shape;
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
_ => Vec3::ONE,
}

4
src_testbed/physics/mod.rs
View File

@@ -4,7 +4,7 @@ use rapier::dynamics::{
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
use rapier::math::Vector;
use rapier::math::{Real, Vector};
use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
pub struct PhysicsSnapshot {
@@ -82,7 +82,7 @@ pub struct PhysicsState {
pub pipeline: PhysicsPipeline,
pub query_pipeline: QueryPipeline,
pub integration_parameters: IntegrationParameters,
pub gravity: Vector<f32>,
pub gravity: Vector<Real>,
pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>,
}

18
src_testbed/testbed.rs
View File

@@ -16,7 +16,7 @@ use rapier::dynamics::{
use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
#[cfg(feature = "dim3")]
use rapier::geometry::{InteractionGroups, Ray};
use rapier::math::Vector;
use rapier::math::{Real, Vector};
use rapier::pipeline::PhysicsHooks;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -487,7 +487,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<f32>,
gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
) {
self.harness.set_world_with_params(
@@ -1129,12 +1129,15 @@ fn update_testbed(
{
if state.flags.contains(TestbedStateFlags::SLEEP) {
for (_, body) in harness.physics.bodies.iter_mut() {
body.activation_mut().threshold = RigidBodyActivation::default_threshold();
body.activation_mut().linear_threshold =
RigidBodyActivation::default_linear_threshold();
body.activation_mut().angular_threshold =
RigidBodyActivation::default_angular_threshold();
}
} else {
for (_, body) in harness.physics.bodies.iter_mut() {
body.wake_up(true);
body.activation_mut().threshold = -1.0;
body.activation_mut().linear_threshold = -1.0;
}
}
}
@@ -1226,6 +1229,7 @@ fn update_testbed(
&harness.physics.bodies,
&harness.physics.colliders,
&mut gfx_components,
&mut *materials,
);
for plugin in &mut plugins.0 {
@@ -1299,14 +1303,14 @@ fn highlight_hovered_body(
let ray_pt1 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
let ray_pt2 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
let ray_dir = ray_pt2 - ray_pt1;
let ray_origin = Point3::new(ray_pt1.x, ray_pt1.y, ray_pt1.z);
let ray_dir = Vector3::new(ray_dir.x, ray_dir.y, ray_dir.z);
let ray_origin = Point3::new(ray_pt1.x as Real, ray_pt1.y as Real, ray_pt1.z as Real);
let ray_dir = Vector3::new(ray_dir.x as Real, ray_dir.y as Real, ray_dir.z as Real);
let ray = Ray::new(ray_origin, ray_dir);
let hit = physics.query_pipeline.cast_ray(
&physics.colliders,
&ray,
f32::MAX,
Real::MAX,
true,
InteractionGroups::all(),
None,

3
src_testbed/ui.rs
View File

@@ -1,4 +1,5 @@
use rapier::counters::Counters;
use rapier::math::Real;
use crate::harness::Harness;
use crate::testbed::{
@@ -147,7 +148,7 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
);
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);
integration_parameters.set_inv_dt(frequency as Real);
let mut sleep = state.flags.contains(TestbedStateFlags::SLEEP);
// let mut contact_points = state.flags.contains(TestbedStateFlags::CONTACT_POINTS);