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

Start implementing ray-casting.

Browse Source 
This adds a QueryPipeline structure responsible for scene queries.
Currently this structure is able to perform a brute-force ray-cast.
This commit also includes the beginning of implementation of a SIMD-based acceleration structure which will be used for these scene queries in the future.
This commit is contained in:
Sébastien Crozet
2020-09-08 21:18:17 +02:00
committed by Crozet Sébastien
parent 99f28ba4b4
commit 3c85a6ac41
10 changed files with 528 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
src/geometry/collider.rs
View File

@@ -1,11 +1,12 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
Proximity, Triangle, Trimesh,
Proximity, Ray, RayIntersection, Triangle, Trimesh,
};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
use ncollide::query::RayCast;
use num::Zero;
#[derive(Clone)]
@@ -97,6 +98,46 @@ impl Shape {
Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
}
}
/// Computes the first intersection point between a ray in this collider.
///
/// Some shapes are not supported yet and will always return `None`.
///
/// # Parameters
/// - `position`: the position of this shape.
/// - `ray`: the ray to cast.
/// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
pub fn cast_ray(
&self,
position: &Isometry<f32>,
ray: &Ray,
max_toi: f32,
) -> Option<RayIntersection> {
match self {
Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true),
Shape::Polygon(_poly) => None,
Shape::Capsule(caps) => {
let pos = position * caps.transform_wrt_y();
let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius);
caps.toi_and_normal_with_ray(&pos, ray, max_toi, true)
}
Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true),
#[cfg(feature = "dim2")]
Shape::Triangle(triangle) => {
// This is not implemented yet in 2D.
None
}
#[cfg(feature = "dim3")]
Shape::Triangle(triangle) => {
triangle.toi_and_normal_with_ray(position, ray, max_toi, true)
}
Shape::Trimesh(_trimesh) => None,
Shape::HeightField(heightfield) => {
heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
}
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -353,6 +394,12 @@ impl ColliderBuilder {
self
}
/// Sets the restitution coefficient of the collider this builder will build.
pub fn restitution(mut self, restitution: f32) -> Self {
self.restitution = restitution;
self
}
/// Sets the density of the collider this builder will build.
pub fn density(mut self, density: f32) -> Self {
self.density = Some(density);

6
src/geometry/mod.rs
View File

@@ -36,6 +36,10 @@ pub type AABB = ncollide::bounding_volume::AABB<f32>;
pub type ContactEvent = ncollide::pipeline::ContactEvent<ColliderHandle>;
/// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not).
pub type ProximityEvent = ncollide::pipeline::ProximityEvent<ColliderHandle>;
/// A ray that can be cast against colliders.
pub type Ray = ncollide::query::Ray<f32>;
/// The intersection between a ray and a collider.
pub type RayIntersection = ncollide::query::RayIntersection<f32>;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::ball::WBall;
@@ -48,7 +52,6 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma
pub(crate) use self::narrow_phase::ContactManifoldIndex;
#[cfg(feature = "dim3")]
pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::waabb::WAABB;
//pub(crate) use self::z_order::z_cmp_floats;
@@ -75,6 +78,5 @@ mod proximity_detector;
pub(crate) mod sat;
pub(crate) mod triangle;
mod trimesh;
#[cfg(feature = "simd-is-enabled")]
mod waabb;
//mod z_order;

93
src/geometry/waabb.rs
View File

@@ -1,15 +1,27 @@
#[cfg(feature = "serde-serialize")]
use crate::math::DIM;
use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH};
use crate::math::{Point, SIMD_WIDTH};
use ncollide::bounding_volume::AABB;
use simba::simd::{SimdPartialOrd, SimdValue};
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SimdBool, SimdFloat},
simba::simd::{SimdPartialOrd, SimdValue},
};
#[derive(Debug, Copy, Clone)]
#[cfg(feature = "simd-is-enabled")]
pub(crate) struct WAABB {
pub mins: Point<SimdFloat>,
pub maxs: Point<SimdFloat>,
}
#[derive(Debug, Copy, Clone)]
#[cfg(not(feature = "simd-is-enabled"))]
pub(crate) struct WAABB {
pub mins: [Point<f32>; SIMD_WIDTH],
pub maxs: [Point<f32>; SIMD_WIDTH],
}
#[cfg(feature = "serde-serialize")]
impl serde::Serialize for WAABB {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
@@ -18,16 +30,24 @@ impl serde::Serialize for WAABB {
{
use serde::ser::SerializeStruct;
#[cfg(feature = "simd-is-enabled")]
let mins: Point<[f32; SIMD_WIDTH]> = Point::from(
self.mins
.coords
.map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
);
#[cfg(feature = "simd-is-enabled")]
let maxs: Point<[f32; SIMD_WIDTH]> = Point::from(
self.maxs
.coords
.map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
);
#[cfg(not(feature = "simd-is-enabled"))]
let mins = self.mins;
#[cfg(not(feature = "simd-is-enabled"))]
let maxs = self.maxs;
let mut waabb = serializer.serialize_struct("WAABB", 2)?;
waabb.serialize_field("mins", &mins)?;
waabb.serialize_field("maxs", &maxs)?;
@@ -52,6 +72,7 @@ impl<'de> serde::Deserialize<'de> for WAABB {
)
}
#[cfg(feature = "simd-is-enabled")]
fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
@@ -66,17 +87,36 @@ impl<'de> serde::Deserialize<'de> for WAABB {
let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e)));
Ok(WAABB { mins, maxs })
}
#[cfg(not(feature = "simd-is-enabled"))]
fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
let mins = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
let maxs = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(1, &self))?;
Ok(WAABB { mins, maxs })
}
}
deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {})
}
}
#[cfg(feature = "simd-is-enabled")]
impl WAABB {
pub fn new(mins: Point<SimdFloat>, maxs: Point<SimdFloat>) -> Self {
Self { mins, maxs }
}
pub fn new_invalid() -> Self {
Self::splat(AABB::new_invalid())
}
pub fn splat(aabb: AABB<f32>) -> Self {
Self {
mins: Point::splat(aabb.mins),
@@ -103,6 +143,7 @@ impl WAABB {
}
}
#[cfg(feature = "simd-is-enabled")]
impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
fn from(aabbs: [AABB<f32>; SIMD_WIDTH]) -> Self {
let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
@@ -114,3 +155,51 @@ impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
}
}
}
#[cfg(not(feature = "simd-is-enabled"))]
impl WAABB {
pub fn new_invalid() -> Self {
Self::splat(AABB::new_invalid())
}
pub fn splat(aabb: AABB<f32>) -> Self {
Self {
mins: [aabb.mins; SIMD_WIDTH],
maxs: [aabb.maxs; SIMD_WIDTH],
}
}
#[cfg(feature = "dim2")]
pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] {
array![|ii|
self.mins[ii].x <= other.maxs[ii].x
&& other.mins[ii].x <= self.maxs[ii].x
&& self.mins[ii].y <= other.maxs[ii].y
&& other.mins[ii].y <= self.maxs[ii].y
; SIMD_WIDTH
]
}
#[cfg(feature = "dim3")]
pub fn intersects_lanewise(&self, other: &WAABB) -> [bool; SIMD_WIDTH] {
array![|ii|
self.mins[ii].x <= other.maxs[ii].x
&& other.mins[ii].x <= self.maxs[ii].x
&& self.mins[ii].y <= other.maxs[ii].y
&& other.mins[ii].y <= self.maxs[ii].y
&& self.mins[ii].z <= other.maxs[ii].z
&& other.mins[ii].z <= self.maxs[ii].z
; SIMD_WIDTH
]
}
}
#[cfg(not(feature = "simd-is-enabled"))]
impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
fn from(aabbs: [AABB<f32>; SIMD_WIDTH]) -> Self {
let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH];
WAABB { mins, maxs }
}
}