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

Merge branch 'master' into infinite_fall_memory

Browse Source
This commit is contained in:
Robert Hrusecky
2020-10-06 14:22:26 -05:00
parent 0c1b210109 6b1cd9cd40
commit dd8e25bc47
39 changed files with 1781 additions and 707 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/data/mod.rs
View File

@@ -2,3 +2,4 @@
pub mod arena;
pub(crate) mod graph;
pub mod pubsub;

175
src/data/pubsub.rs Normal file
View File

@@ -0,0 +1,175 @@
//! Publish-subscribe mechanism for internal events.
use std::collections::VecDeque;
use std::marker::PhantomData;
/// A permanent subscription to a pub-sub queue.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct Subscription<T> {
// Position on the cursor array.
id: u32,
_phantom: PhantomData<T>,
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct PubSubCursor {
// Position on the offset array.
id: u32,
// Index of the next message to read.
// NOTE: Having this here is not actually necessary because
// this value is supposed to be equal to `offsets[self.id]`.
// However, we keep it because it lets us avoid one lookup
// on the `offsets` array inside of message-polling loops
// based on `read_ith`.
next: u32,
}
impl PubSubCursor {
fn id(&self, num_deleted: u32) -> usize {
(self.id - num_deleted) as usize
}
fn next(&self, num_deleted: u32) -> usize {
(self.next - num_deleted) as usize
}
}
/// A pub-sub queue.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct PubSub<T> {
deleted_messages: u32,
deleted_offsets: u32,
messages: VecDeque<T>,
offsets: VecDeque<u32>,
cursors: Vec<PubSubCursor>,
}
impl<T> PubSub<T> {
/// Create a new empty pub-sub queue.
pub fn new() -> Self {
Self {
deleted_offsets: 0,
deleted_messages: 0,
messages: VecDeque::new(),
offsets: VecDeque::new(),
cursors: Vec::new(),
}
}
fn reset_shifts(&mut self) {
for offset in &mut self.offsets {
*offset -= self.deleted_messages;
}
for cursor in &mut self.cursors {
cursor.id -= self.deleted_offsets;
cursor.next -= self.deleted_messages;
}
self.deleted_offsets = 0;
self.deleted_messages = 0;
}
/// Publish a new message.
pub fn publish(&mut self, message: T) {
if self.offsets.is_empty() {
// No subscribers, drop the message.
return;
}
self.messages.push_back(message);
}
/// Subscribe to the queue.
///
/// A subscription cannot be cancelled.
#[must_use]
pub fn subscribe(&mut self) -> Subscription<T> {
let cursor = PubSubCursor {
next: self.messages.len() as u32 + self.deleted_messages,
id: self.offsets.len() as u32 + self.deleted_offsets,
};
let subscription = Subscription {
id: self.cursors.len() as u32,
_phantom: PhantomData,
};
self.offsets.push_back(cursor.next);
self.cursors.push(cursor);
subscription
}
/// Read the i-th message not yet read by the given subsciber.
pub fn read_ith(&self, sub: &Subscription<T>, i: usize) -> Option<&T> {
let cursor = &self.cursors[sub.id as usize];
self.messages
.get(cursor.next(self.deleted_messages) as usize + i)
}
/// Get the messages not yet read by the given subscriber.
pub fn read(&self, sub: &Subscription<T>) -> impl Iterator<Item = &T> {
let cursor = &self.cursors[sub.id as usize];
let next = cursor.next(self.deleted_messages);
// TODO: use self.queue.range(next..) once it is stabilised.
MessageRange {
queue: &self.messages,
next,
}
}
/// Makes the given subscribe acknowledge all the messages in the queue.
///
/// A subscriber cannot read acknowledged messages any more.
pub fn ack(&mut self, sub: &Subscription<T>) {
// Update the cursor.
let cursor = &mut self.cursors[sub.id as usize];
self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX;
cursor.id = self.offsets.len() as u32 + self.deleted_offsets;
cursor.next = self.messages.len() as u32 + self.deleted_messages;
self.offsets.push_back(cursor.next);
// Now clear the messages we don't need to
// maintain in memory anymore.
while self.offsets.front() == Some(&u32::MAX) {
self.offsets.pop_front();
self.deleted_offsets += 1;
}
// There must be at least one offset otherwise
// that would mean we have no subscribers.
let next = self.offsets.front().unwrap();
let num_to_delete = *next - self.deleted_messages;
for _ in 0..num_to_delete {
self.messages.pop_front();
}
self.deleted_messages += num_to_delete;
if self.deleted_messages > u32::MAX / 2 || self.deleted_offsets > u32::MAX / 2 {
// Don't let the deleted_* shifts grow indefinitely otherwise
// they will end up overflowing, breaking everything.
self.reset_shifts();
}
}
}
struct MessageRange<'a, T> {
queue: &'a VecDeque<T>,
next: usize,
}
impl<'a, T> Iterator for MessageRange<'a, T> {
type Item = &'a T;
#[inline(always)]
fn next(&mut self) -> Option<&'a T> {
let result = self.queue.get(self.next);
self.next += 1;
result
}
}

2
src/dynamics/integration_parameters.rs
View File

@@ -2,7 +2,7 @@
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct IntegrationParameters {
/// The timestep (default: `1.0 / 60.0`)
/// The timestep length (default: `1.0 / 60.0`)
dt: f32,
/// The inverse of `dt`.
inv_dt: f32,

16
src/dynamics/joint/joint_set.rs
View File

@@ -66,17 +66,21 @@ impl JointSet {
}
/// Iterates through all the joint on this set.
pub fn iter(&self) -> impl Iterator<Item = &Joint> {
self.joint_graph.graph.edges.iter().map(|e| &e.weight)
pub fn iter(&self) -> impl Iterator<Item = (JointHandle, &Joint)> {
self.joint_graph
.graph
.edges
.iter()
.map(|e| (e.weight.handle, &e.weight))
}
/// Iterates mutably through all the joint on this set.
pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Joint> {
pub fn iter_mut(&mut self) -> impl Iterator<Item = (JointHandle, &mut Joint)> {
self.joint_graph
.graph
.edges
.iter_mut()
.map(|e| &mut e.weight)
.map(|e| (e.weight.handle, &mut e.weight))
}
// /// The set of joints as an array.
@@ -202,8 +206,8 @@ impl JointSet {
}
// Wake up the attached bodies.
bodies.wake_up(h1);
bodies.wake_up(h2);
bodies.wake_up(h1, true);
bodies.wake_up(h2, true);
}
if let Some(other) = self.joint_graph.remove_node(deleted_id) {

34
src/dynamics/rigid_body.rs
View File

@@ -137,6 +137,15 @@ impl RigidBody {
crate::utils::inv(self.mass_properties.inv_mass)
}
/// The predicted position of this rigid-body.
///
/// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position`
/// method and is used for estimating the kinematic body velocity at the next timestep.
/// For non-kinematic bodies, this value is currently unspecified.
pub fn predicted_position(&self) -> &Isometry<f32> {
&self.predicted_position
}
/// Adds a collider to this rigid-body.
pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
let mass_properties = coll
@@ -172,10 +181,13 @@ impl RigidBody {
}
/// Wakes up this rigid body if it is sleeping.
pub fn wake_up(&mut self) {
///
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, strong: bool) {
self.activation.sleeping = false;
if self.activation.energy == 0.0 && self.is_dynamic() {
if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
self.activation.energy = self.activation.threshold.abs() * 2.0;
}
}
@@ -189,9 +201,18 @@ impl RigidBody {
/// Is this rigid body sleeping?
pub fn is_sleeping(&self) -> bool {
// TODO: should we:
// - return false for static bodies.
// - return true for non-sleeping dynamic bodies.
// - return true only for kinematic bodies with non-zero velocity?
self.activation.sleeping
}
/// Is the velocity of this body not zero?
pub fn is_moving(&self) -> bool {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
let com = &self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);
@@ -201,12 +222,17 @@ impl RigidBody {
self.position = self.integrate_velocity(dt) * self.position;
}
/// Sets the position of this rigid body.
/// Sets the position and `next_kinematic_position` of this rigid body.
///
/// This will teleport the rigid-body to the specified position/orientation,
/// completely ignoring any physics rule. If this body is kinematic, this will
/// also set the next kinematic position to the same value, effectively
/// resetting to zero the next interpolated velocity of the kinematic body.
pub fn set_position(&mut self, pos: Isometry<f32>) {
self.position = pos;
// TODO: update the predicted position for dynamic bodies too?
if self.is_static() {
if self.is_static() || self.is_kinematic() {
self.predicted_position = pos;
}
}

160
src/dynamics/rigid_body_set.rs
View File

@@ -2,8 +2,8 @@
use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{Joint, RigidBody};
use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
use crossbeam::channel::{Receiver, Sender};
use std::ops::{Deref, DerefMut, Index, IndexMut};
@@ -128,9 +128,23 @@ impl RigidBodySet {
pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
let mut rb = &mut self.bodies[handle];
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
match rb.body_status {
// XXX: this case should only concern the dynamic bodies.
// For static bodies we should use the modified_inactive_set, or something
// similar. Right now we do this for static bodies as well so the broad-phase
// takes them into account the first time they are inserted.
BodyStatus::Dynamic | BodyStatus::Static => {
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
}
BodyStatus::Kinematic => {
if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
}
}
}
@@ -143,13 +157,14 @@ impl RigidBodySet {
pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
let handle = self.bodies.insert(rb);
let rb = &mut self.bodies[handle];
rb.active_set_id = self.active_dynamic_set.len();
if !rb.is_sleeping() && rb.is_dynamic() {
rb.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
}
if rb.is_kinematic() {
rb.active_set_id = self.active_kinematic_set.len();
self.active_kinematic_set.push(handle);
}
@@ -160,29 +175,57 @@ impl RigidBodySet {
handle
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
/// Removes a rigid-body, and all its attached colliders and joints, from these sets.
pub fn remove(
&mut self,
handle: RigidBodyHandle,
colliders: &mut ColliderSet,
joints: &mut JointSet,
) -> Option<RigidBody> {
let rb = self.bodies.remove(handle)?;
// Remove this body from the active dynamic set.
if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
self.active_dynamic_set.swap_remove(rb.active_set_id);
/*
* Update active sets.
*/
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) {
self.bodies[*replacement].active_set_id = rb.active_set_id;
for active_set in &mut active_sets {
if active_set.get(rb.active_set_id) == Some(&handle) {
active_set.swap_remove(rb.active_set_id);
if let Some(replacement) = active_set.get(rb.active_set_id) {
self.bodies[*replacement].active_set_id = rb.active_set_id;
}
}
}
/*
* Remove colliders attached to this rigid-body.
*/
for collider in &rb.colliders {
colliders.remove(*collider, self);
}
/*
* Remove joints attached to this rigid-body.
*/
joints.remove_rigid_body(rb.joint_graph_index, self);
Some(rb)
}
pub(crate) fn num_islands(&self) -> usize {
self.active_islands.len() - 1
}
/// Forces the specified rigid-body to wake up if it is dynamic.
pub fn wake_up(&mut self, handle: RigidBodyHandle) {
///
/// If `strong` is `true` then it is assured that the rigid-body will
/// remain awake during multiple subsequent timesteps.
pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
if let Some(rb) = self.bodies.get_mut(handle) {
// TODO: what about kinematic bodies?
if rb.is_dynamic() {
rb.wake_up();
rb.wake_up(strong);
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
rb.active_set_id = self.active_dynamic_set.len();
@@ -214,8 +257,11 @@ impl RigidBodySet {
///
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not
/// suffer form the ABA problem.
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
self.bodies.get_unknown_gen_mut(i)
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> {
let sender = &self.activation_channel.0;
self.bodies
.get_unknown_gen_mut(i)
.map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
}
/// Gets the rigid-body with the given handle.
@@ -256,6 +302,16 @@ impl RigidBodySet {
.map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
}
/// Iter through all the active kinematic rigid-bodies on this set.
pub fn iter_active_kinematic<'a>(
&'a self,
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
let bodies: &'a _ = &self.bodies;
self.active_kinematic_set
.iter()
.filter_map(move |h| Some((*h, bodies.get(*h)?)))
}
/// Iter through all the active dynamic rigid-bodies on this set.
pub fn iter_active_dynamic<'a>(
&'a self,
@@ -425,6 +481,45 @@ impl RigidBodySet {
}
}
// Read all the contacts and push objects touching touching this rigid-body.
#[inline(always)]
fn push_contacting_colliders(
rb: &RigidBody,
colliders: &ColliderSet,
contact_graph: &InteractionGraph<ContactPair>,
stack: &mut Vec<ColliderHandle>,
) {
for collider_handle in &rb.colliders {
let collider = &colliders[*collider_handle];
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other =
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
let other_body = colliders[other].parent;
stack.push(other_body);
break;
}
}
}
}
}
// Now iterate on all active kinematic bodies and push all the bodies
// touching them to the stack so they can be woken up.
for h in self.active_kinematic_set.iter() {
let rb = &self.bodies[*h];
if !rb.is_moving() {
// If the kinematic body does not move, it does not have
// to wake up any dynamic body.
continue;
}
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
}
// println!("Selection: {}", instant::now() - t);
// let t = instant::now();
@@ -443,7 +538,9 @@ impl RigidBodySet {
// We already visited this body and its neighbors.
// Also, we don't propagate awake state through static bodies.
continue;
} else if self.stack.len() < island_marker {
}
if self.stack.len() < island_marker {
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
@@ -454,29 +551,16 @@ impl RigidBodySet {
island_marker = self.stack.len();
}
rb.wake_up();
rb.wake_up(false);
rb.active_island_id = self.active_islands.len() - 1;
rb.active_set_id = self.active_dynamic_set.len();
rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
rb.active_set_timestamp = self.active_set_timestamp;
self.active_dynamic_set.push(handle);
// Read all the contacts and push objects touching this one.
for collider_handle in &rb.colliders {
let collider = &colliders[*collider_handle];
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other =
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
let other_body = colliders[other].parent;
self.stack.push(other_body);
break;
}
}
}
}
// Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider.
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::other_handle((inter.0, inter.1), handle);

255
src/geometry/broad_phase.rs
View File

@@ -1,255 +0,0 @@
use crate::geometry::ColliderHandle;
use ncollide::bounding_volume::AABB;
#[cfg(feature = "simd-is-enabled")]
use {
crate::geometry::WAABB,
crate::math::{Point, SIMD_WIDTH},
crate::utils::WVec,
simba::simd::SimdBool as _,
};
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ColliderPair {
pub collider1: ColliderHandle,
pub collider2: ColliderHandle,
}
impl ColliderPair {
pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
ColliderPair {
collider1,
collider2,
}
}
pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
Self::new(collider1, collider2)
} else {
Self::new(collider2, collider1)
}
}
pub fn swap(self) -> Self {
Self::new(self.collider2, self.collider1)
}
pub fn zero() -> Self {
Self {
collider1: ColliderHandle::from_raw_parts(0, 0),
collider2: ColliderHandle::from_raw_parts(0, 0),
}
}
}
pub struct WAABBHierarchyIntersections {
curr_level_interferences: Vec<usize>,
next_level_interferences: Vec<usize>,
}
impl WAABBHierarchyIntersections {
pub fn new() -> Self {
Self {
curr_level_interferences: Vec::new(),
next_level_interferences: Vec::new(),
}
}
pub fn computed_interferences(&self) -> &[usize] {
&self.curr_level_interferences[..]
}
pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec<usize> {
&mut self.curr_level_interferences
}
}
#[cfg(feature = "simd-is-enabled")]
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct WAABBHierarchy {
levels: Vec<Vec<WAABB>>,
}
#[cfg(feature = "simd-is-enabled")]
impl WAABBHierarchy {
pub fn new(aabbs: &[AABB<f32>]) -> Self {
let mut waabbs: Vec<_> = aabbs
.chunks_exact(SIMD_WIDTH)
.map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH]))
.collect();
if aabbs.len() % SIMD_WIDTH != 0 {
let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH;
let last_i = aabbs.len() - 1;
let last_waabb =
WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]);
waabbs.push(last_waabb);
}
let mut levels = vec![waabbs];
loop {
let last_level = levels.last().unwrap();
let mut next_level = Vec::new();
for chunk in last_level.chunks_exact(SIMD_WIDTH) {
let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]);
let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]);
next_level.push(WAABB::new(mins, maxs));
}
// Deal with the last non-exact chunk.
if last_level.len() % SIMD_WIDTH != 0 {
let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH;
let last_id = last_level.len() - 1;
let mins = array![|ii| last_level[(first_id + ii).min(last_id)]
.mins
.horizontal_inf(); SIMD_WIDTH];
let maxs = array![|ii| last_level[(first_id + ii).min(last_id)]
.maxs
.horizontal_sup(); SIMD_WIDTH];
let mins = Point::from(mins);
let maxs = Point::from(maxs);
next_level.push(WAABB::new(mins, maxs));
}
if next_level.len() == 1 {
levels.push(next_level);
break;
}
levels.push(next_level);
}
Self { levels }
}
pub fn compute_interferences_with(
&self,
aabb: AABB<f32>,
workspace: &mut WAABBHierarchyIntersections,
) {
let waabb1 = WAABB::splat(aabb);
workspace.next_level_interferences.clear();
workspace.curr_level_interferences.clear();
workspace.curr_level_interferences.push(0);
for level in self.levels.iter().rev() {
for i in &workspace.curr_level_interferences {
// This `if let` handle the case when `*i` is out of bounds because
// the initial number of aabbs was not a power of SIMD_WIDTH.
if let Some(waabb2) = level.get(*i) {
// NOTE: using `intersect.bitmask()` and performing bit comparisons
// is much more efficient than testing if each intersect.extract(i) is true.
let intersect = waabb1.intersects_lanewise(waabb2);
let bitmask = intersect.bitmask();
for j in 0..SIMD_WIDTH {
if (bitmask & (1 << j)) != 0 {
workspace.next_level_interferences.push(i * SIMD_WIDTH + j)
}
}
}
}
std::mem::swap(
&mut workspace.curr_level_interferences,
&mut workspace.next_level_interferences,
);
workspace.next_level_interferences.clear();
}
}
}
#[cfg(not(feature = "simd-is-enabled"))]
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct WAABBHierarchy {
levels: Vec<Vec<AABB<f32>>>,
}
#[cfg(not(feature = "simd-is-enabled"))]
impl WAABBHierarchy {
const GROUP_SIZE: usize = 4;
pub fn new(aabbs: &[AABB<f32>]) -> Self {
use ncollide::bounding_volume::BoundingVolume;
let mut levels = vec![aabbs.to_vec()];
loop {
let last_level = levels.last().unwrap();
let mut next_level = Vec::new();
for chunk in last_level.chunks(Self::GROUP_SIZE) {
let mut merged = chunk[0];
for aabb in &chunk[1..] {
merged.merge(aabb)
}
next_level.push(merged);
}
if next_level.len() == 1 {
levels.push(next_level);
break;
}
levels.push(next_level);
}
Self { levels }
}
pub fn compute_interferences_with(
&self,
aabb1: AABB<f32>,
workspace: &mut WAABBHierarchyIntersections,
) {
use ncollide::bounding_volume::BoundingVolume;
workspace.next_level_interferences.clear();
workspace.curr_level_interferences.clear();
workspace.curr_level_interferences.push(0);
for level in self.levels[1..].iter().rev() {
for i in &workspace.curr_level_interferences {
for j in 0..Self::GROUP_SIZE {
if let Some(aabb2) = level.get(*i + j) {
if aabb1.intersects(aabb2) {
workspace
.next_level_interferences
.push((i + j) * Self::GROUP_SIZE)
}
}
}
}
std::mem::swap(
&mut workspace.curr_level_interferences,
&mut workspace.next_level_interferences,
);
workspace.next_level_interferences.clear();
}
// Last level.
for i in &workspace.curr_level_interferences {
for j in 0..Self::GROUP_SIZE {
if let Some(aabb2) = self.levels[0].get(*i + j) {
if aabb1.intersects(aabb2) {
workspace.next_level_interferences.push(i + j)
}
}
}
}
std::mem::swap(
&mut workspace.curr_level_interferences,
&mut workspace.next_level_interferences,
);
workspace.next_level_interferences.clear();
}
}

134
src/geometry/broad_phase_multi_sap.rs
View File

@@ -1,5 +1,6 @@
use crate::data::pubsub::Subscription;
use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet};
use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
use crate::math::{Point, Vector, DIM};
#[cfg(feature = "enhanced-determinism")]
use crate::utils::FxHashMap32 as HashMap;
@@ -15,6 +16,41 @@ const NEXT_FREE_SENTINEL: u32 = u32::MAX;
const SENTINEL_VALUE: f32 = f32::MAX;
const CELL_WIDTH: f32 = 20.0;
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ColliderPair {
pub collider1: ColliderHandle,
pub collider2: ColliderHandle,
}
impl ColliderPair {
pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
ColliderPair {
collider1,
collider2,
}
}
pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
Self::new(collider1, collider2)
} else {
Self::new(collider2, collider1)
}
}
pub fn swap(self) -> Self {
Self::new(self.collider2, self.collider1)
}
pub fn zero() -> Self {
Self {
collider1: ColliderHandle::from_raw_parts(0, 0),
collider2: ColliderHandle::from_raw_parts(0, 0),
}
}
}
pub enum BroadPhasePairEvent {
AddPair(ColliderPair),
DeletePair(ColliderPair),
@@ -392,6 +428,7 @@ impl SAPRegion {
pub struct BroadPhase {
proxies: Proxies,
regions: HashMap<Point<i32>, SAPRegion>,
removed_colliders: Option<Subscription<RemovedCollider>>,
deleted_any: bool,
// We could think serializing this workspace is useless.
// It turns out is is important to serialize at least its capacity
@@ -480,6 +517,7 @@ impl BroadPhase {
/// Create a new empty broad-phase.
pub fn new() -> Self {
BroadPhase {
removed_colliders: None,
proxies: Proxies::new(),
regions: HashMap::default(),
reporting: HashMap::default(),
@@ -487,46 +525,60 @@ impl BroadPhase {
}
}
pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) {
for collider in handles.iter().filter_map(|h| colliders.get(*h)) {
if collider.proxy_index == crate::INVALID_USIZE {
// This collider has not been added to the broad-phase yet.
continue;
/// Maintain the broad-phase internal state by taking collider removal into account.
pub fn maintain(&mut self, colliders: &mut ColliderSet) {
// Ensure we already subscribed.
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
}
let mut cursor = self.removed_colliders.take().unwrap();
for collider in colliders.removed_colliders.read(&cursor) {
self.remove_collider(collider.proxy_index);
}
colliders.removed_colliders.ack(&mut cursor);
self.removed_colliders = Some(cursor);
}
fn remove_collider<'a>(&mut self, proxy_index: usize) {
if proxy_index == crate::INVALID_USIZE {
// This collider has not been added to the broad-phase yet.
return;
}
let proxy = &mut self.proxies[proxy_index];
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
// Discretize the AABB to find the regions that need to be invalidated.
let start = point_key(proxy.aabb.mins);
let end = point_key(proxy.aabb.maxs);
#[cfg(feature = "dim2")]
for i in start.x..=end.x {
for j in start.y..=end.y {
if let Some(region) = self.regions.get_mut(&Point::new(i, j)) {
region.predelete_proxy(proxy_index);
self.deleted_any = true;
}
}
}
let proxy = &mut self.proxies[collider.proxy_index];
// Push the proxy to infinity, but not beyond the sentinels.
proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
// Discretize the AABB to find the regions that need to be invalidated.
let start = point_key(proxy.aabb.mins);
let end = point_key(proxy.aabb.maxs);
#[cfg(feature = "dim2")]
for i in start.x..=end.x {
for j in start.y..=end.y {
if let Some(region) = self.regions.get_mut(&Point::new(i, j)) {
region.predelete_proxy(collider.proxy_index);
#[cfg(feature = "dim3")]
for i in start.x..=end.x {
for j in start.y..=end.y {
for k in start.z..=end.z {
if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) {
region.predelete_proxy(proxy_index);
self.deleted_any = true;
}
}
}
#[cfg(feature = "dim3")]
for i in start.x..=end.x {
for j in start.y..=end.y {
for k in start.z..=end.z {
if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) {
region.predelete_proxy(collider.proxy_index);
self.deleted_any = true;
}
}
}
}
self.proxies.remove(collider.proxy_index);
}
self.proxies.remove(proxy_index);
}
pub(crate) fn update_aabbs(
@@ -664,16 +716,13 @@ impl BroadPhase {
mod test {
use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
use crate::pipeline::PhysicsPipeline;
#[test]
fn test_add_update_remove() {
let mut broad_phase = BroadPhase::new();
let mut narrow_phase = NarrowPhase::new();
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut joints = JointSet::new();
let mut pipeline = PhysicsPipeline::new();
let rb = RigidBodyBuilder::new_dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
@@ -682,15 +731,8 @@ mod test {
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
pipeline.remove_rigid_body(
hrb,
&mut broad_phase,
&mut narrow_phase,
&mut bodies,
&mut colliders,
&mut joints,
);
bodies.remove(hrb, &mut colliders, &mut joints);
broad_phase.maintain(&mut colliders);
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
// Create another body.

52
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,49 @@ 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(_) | Shape::Trimesh(_) => {
// 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)
}
#[cfg(feature = "dim3")]
Shape::Trimesh(trimesh) => {
trimesh.toi_and_normal_with_ray(position, ray, max_toi, true)
}
Shape::HeightField(heightfield) => {
heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
}
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -353,6 +397,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);

47
src/geometry/collider_set.rs
View File

@@ -1,14 +1,25 @@
use crate::data::arena::Arena;
use crate::data::pubsub::PubSub;
use crate::dynamics::{RigidBodyHandle, RigidBodySet};
use crate::geometry::Collider;
use crate::geometry::{Collider, ColliderGraphIndex};
use std::ops::{Index, IndexMut};
/// The unique identifier of a collider added to a collider set.
pub type ColliderHandle = crate::data::arena::Index;
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(crate) struct RemovedCollider {
pub handle: ColliderHandle,
pub(crate) contact_graph_index: ColliderGraphIndex,
pub(crate) proximity_graph_index: ColliderGraphIndex,
pub(crate) proxy_index: usize,
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A set of colliders that can be handled by a physics `World`.
pub struct ColliderSet {
pub(crate) removed_colliders: PubSub<RemovedCollider>,
pub(crate) colliders: Arena<Collider>,
}
@@ -16,6 +27,7 @@ impl ColliderSet {
/// Create a new empty set of colliders.
pub fn new() -> Self {
ColliderSet {
removed_colliders: PubSub::new(),
colliders: Arena::new(),
}
}
@@ -26,7 +38,7 @@ impl ColliderSet {
}
/// Iterate through all the colliders on this set.
pub fn iter(&self) -> impl Iterator<Item = (ColliderHandle, &Collider)> {
pub fn iter(&self) -> impl ExactSizeIterator<Item = (ColliderHandle, &Collider)> {
self.colliders.iter()
}
@@ -60,8 +72,35 @@ impl ColliderSet {
handle
}
pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option<Collider> {
self.colliders.remove(handle)
/// Remove a collider from this set and update its parent accordingly.
pub fn remove(
&mut self,
handle: ColliderHandle,
bodies: &mut RigidBodySet,
) -> Option<Collider> {
let collider = self.colliders.remove(handle)?;
/*
* Delete the collider from its parent body.
*/
if let Some(parent) = bodies.get_mut_internal(collider.parent) {
parent.remove_collider_internal(handle, &collider);
bodies.wake_up(collider.parent, true);
}
/*
* Publish removal.
*/
let message = RemovedCollider {
handle,
contact_graph_index: collider.contact_graph_index,
proximity_graph_index: collider.proximity_graph_index,
proxy_index: collider.proxy_index,
};
self.removed_colliders.publish(message);
Some(collider)
}
/// Gets the collider with the given handle without a known generation.

8
src/geometry/contact_generator/heightfield_shape_contact_generator.rs
View File

@@ -71,10 +71,10 @@ fn do_generate_contacts(
} else {
manifold.subshape_index_pair.1
};
println!(
"Restoring for {} [chosen with {:?}]",
subshape_id, manifold.subshape_index_pair
);
// println!(
// "Restoring for {} [chosen with {:?}]",
// subshape_id, manifold.subshape_index_pair
// );
// Use dummy shapes for the dispatch.
#[cfg(feature = "dim2")]

13
src/geometry/contact_generator/trimesh_shape_contact_generator.rs
View File

@@ -1,11 +1,11 @@
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext,
};
use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyIntersections};
use crate::geometry::{Collider, ContactManifold, Shape, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
pub struct TrimeshShapeContactGeneratorWorkspace {
interferences: WAABBHierarchyIntersections,
interferences: Vec<usize>,
local_aabb2: AABB<f32>,
old_interferences: Vec<usize>,
old_manifolds: Vec<ContactManifold>,
@@ -14,7 +14,7 @@ pub struct TrimeshShapeContactGeneratorWorkspace {
impl TrimeshShapeContactGeneratorWorkspace {
pub fn new() -> Self {
Self {
interferences: WAABBHierarchyIntersections::new(),
interferences: Vec::new(),
local_aabb2: AABB::new_invalid(),
old_interferences: Vec::new(),
old_manifolds: Vec::new(),
@@ -74,7 +74,7 @@ fn do_generate_contacts(
let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value?
std::mem::swap(
&mut workspace.old_interferences,
workspace.interferences.computed_interferences_mut(),
&mut workspace.interferences,
);
std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds);
ctxt.pair.manifolds.clear();
@@ -108,16 +108,17 @@ fn do_generate_contacts(
// workspace.old_manifolds.len()
// );
workspace.interferences.clear();
trimesh1
.waabbs()
.compute_interferences_with(local_aabb2, &mut workspace.interferences);
.intersect_aabb(&local_aabb2, &mut workspace.interferences);
workspace.local_aabb2 = local_aabb2;
}
/*
* Dispatch to the specific solver by keeping the previous manifold if we already had one.
*/
let new_interferences = workspace.interferences.computed_interferences();
let new_interferences = &workspace.interferences;
let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
let mut old_manifolds_it = workspace.old_manifolds.drain(..);

15
src/geometry/mod.rs
View File

@@ -36,11 +36,15 @@ 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;
pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections};
pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent;
pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
pub(crate) use self::collider_set::RemovedCollider;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::contact::WContact;
#[cfg(feature = "dim2")]
@@ -48,12 +52,11 @@ 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::waabb::{WRay, WAABB};
pub(crate) use self::wquadtree::WQuadtree;
//pub(crate) use self::z_order::z_cmp_floats;
mod ball;
mod broad_phase;
mod broad_phase_multi_sap;
mod capsule;
mod collider;
@@ -75,6 +78,6 @@ mod proximity_detector;
pub(crate) mod sat;
pub(crate) mod triangle;
mod trimesh;
#[cfg(feature = "simd-is-enabled")]
mod waabb;
mod wquadtree;
//mod z_order;

123
src/geometry/narrow_phase.rs
View File

@@ -14,13 +14,16 @@ use crate::geometry::proximity_detector::{
// proximity_detector::ProximityDetectionContextSimd, WBall,
//};
use crate::geometry::{
BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair,
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent,
ProximityPair, RemovedCollider,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
//#[cfg(feature = "simd-is-enabled")]
//use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::data::pubsub::Subscription;
use crate::ncollide::query::Proximity;
use crate::pipeline::EventHandler;
use std::collections::HashMap;
//use simba::simd::SimdValue;
/// The narrow-phase responsible for computing precise contact information between colliders.
@@ -28,6 +31,7 @@ use crate::pipeline::EventHandler;
pub struct NarrowPhase {
contact_graph: InteractionGraph<ContactPair>,
proximity_graph: InteractionGraph<ProximityPair>,
removed_colliders: Option<Subscription<RemovedCollider>>,
// ball_ball: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
// shape_shape: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
// ball_ball_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>,
@@ -42,6 +46,7 @@ impl NarrowPhase {
Self {
contact_graph: InteractionGraph::new(),
proximity_graph: InteractionGraph::new(),
removed_colliders: None,
// ball_ball: Vec::new(),
// shape_shape: Vec::new(),
// ball_ball_prox: Vec::new(),
@@ -73,45 +78,84 @@ impl NarrowPhase {
// &mut self.contact_graph.interactions
// }
pub(crate) fn remove_colliders(
/// Maintain the narrow-phase internal state by taking collider removal into account.
pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) {
// Ensure we already subscribed.
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
}
let mut cursor = self.removed_colliders.take().unwrap();
// TODO: avoid these hash-maps.
// They are necessary to handle the swap-remove done internally
// by the contact/proximity graphs when a node is removed.
let mut prox_id_remap = HashMap::new();
let mut contact_id_remap = HashMap::new();
for i in 0.. {
if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
let proximity_graph_id = prox_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(collider.proximity_graph_index);
let contact_graph_id = contact_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(collider.contact_graph_index);
self.remove_collider(
proximity_graph_id,
contact_graph_id,
colliders,
bodies,
&mut prox_id_remap,
&mut contact_id_remap,
);
} else {
break;
}
}
colliders.removed_colliders.ack(&mut cursor);
self.removed_colliders = Some(cursor);
}
pub(crate) fn remove_collider<'a>(
&mut self,
handles: &[ColliderHandle],
proximity_graph_id: ColliderGraphIndex,
contact_graph_id: ColliderGraphIndex,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
) {
for handle in handles {
if let Some(collider) = colliders.get(*handle) {
let proximity_graph_id = collider.proximity_graph_index;
let contact_graph_id = collider.contact_graph_index;
// Wake up every body in contact with the deleted collider.
for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
if let Some(parent) = colliders.get(a).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
// Wake up every body in contact with the deleted collider.
for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
if let Some(parent) = colliders.get(a).map(|c| c.parent) {
bodies.wake_up(parent)
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent)
}
}
// We have to manage the fact that one other collider will
// have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) {
if let Some(replacement) = colliders.get_mut(replacement) {
replacement.proximity_graph_index = proximity_graph_id;
} else {
prox_id_remap.insert(replacement, proximity_graph_id);
}
}
// We have to manage the fact that one other collider will
// have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self
.proximity_graph
.remove_node(proximity_graph_id)
.and_then(|h| colliders.get_mut(h))
{
replacement.proximity_graph_index = proximity_graph_id;
}
if let Some(replacement) = self
.contact_graph
.remove_node(contact_graph_id)
.and_then(|h| colliders.get_mut(h))
{
replacement.contact_graph_index = contact_graph_id;
}
if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
if let Some(replacement) = colliders.get_mut(replacement) {
replacement.contact_graph_index = contact_graph_id;
} else {
contact_id_remap.insert(replacement, contact_graph_id);
}
}
}
@@ -119,6 +163,7 @@ impl NarrowPhase {
pub(crate) fn register_pairs(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
broad_phase_events: &[BroadPhasePairEvent],
events: &dyn EventHandler,
) {
@@ -218,9 +263,13 @@ impl NarrowPhase {
.contact_graph
.remove_edge(co1.contact_graph_index, co2.contact_graph_index);
// Emit a contact stopped event if we had a proximity before removing the edge.
// Emit a contact stopped event if we had a contact before removing the edge.
// Also wake up the dynamic bodies that were in contact.
if let Some(ctct) = contact_pair {
if ctct.has_any_active_contact() {
bodies.wake_up(co1.parent, true);
bodies.wake_up(co2.parent, true);
events.handle_contact_event(ContactEvent::Stopped(
pair.collider1,
pair.collider2,
@@ -250,8 +299,7 @@ impl NarrowPhase {
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
{
if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) {
// No need to update this contact because nothing moved.
return;
}
@@ -359,7 +407,8 @@ impl NarrowPhase {
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()))
|| (!rb1.is_dynamic() && !rb2.is_dynamic())
{
// No need to update this contact because nothing moved.
return;

13
src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs
View File

@@ -1,11 +1,11 @@
use crate::geometry::proximity_detector::{
PrimitiveProximityDetectionContext, ProximityDetectionContext,
};
use crate::geometry::{Collider, Proximity, Shape, Trimesh, WAABBHierarchyIntersections};
use crate::geometry::{Collider, Proximity, Shape, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
pub struct TrimeshShapeProximityDetectorWorkspace {
interferences: WAABBHierarchyIntersections,
interferences: Vec<usize>,
local_aabb2: AABB<f32>,
old_interferences: Vec<usize>,
}
@@ -13,7 +13,7 @@ pub struct TrimeshShapeProximityDetectorWorkspace {
impl TrimeshShapeProximityDetectorWorkspace {
pub fn new() -> Self {
Self {
interferences: WAABBHierarchyIntersections::new(),
interferences: Vec::new(),
local_aabb2: AABB::new_invalid(),
old_interferences: Vec::new(),
}
@@ -67,19 +67,20 @@ fn do_detect_proximity(
let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value?
std::mem::swap(
&mut workspace.old_interferences,
&mut workspace.interferences.computed_interferences_mut(),
&mut workspace.interferences,
);
workspace.interferences.clear();
trimesh1
.waabbs()
.compute_interferences_with(local_aabb2, &mut workspace.interferences);
.intersect_aabb(&local_aabb2, &mut workspace.interferences);
workspace.local_aabb2 = local_aabb2;
}
/*
* Dispatch to the specific solver by keeping the previous manifold if we already had one.
*/
let new_interferences = workspace.interferences.computed_interferences();
let new_interferences = &workspace.interferences;
let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
let mut best_proximity = Proximity::Disjoint;

109
src/geometry/trimesh.rs
View File

@@ -1,13 +1,19 @@
use crate::geometry::{Triangle, WAABBHierarchy};
use crate::geometry::{Triangle, WQuadtree};
use crate::math::{Isometry, Point};
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
#[cfg(feature = "dim3")]
use {
crate::geometry::{Ray, RayIntersection},
ncollide::query::RayCast,
};
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A triangle mesh.
pub struct Trimesh {
waabb_tree: WAABBHierarchy,
wquadtree: WQuadtree<usize>,
aabb: AABB<f32>,
vertices: Vec<Point<f32>>,
indices: Vec<Point3<u32>>,
@@ -25,41 +31,24 @@ impl Trimesh {
"A triangle mesh must contain at least one triangle."
);
// z-sort the indices.
// indices.sort_unstable_by(|idx, jdx| {
// let ti = Triangle::new(
// vertices[idx[0] as usize],
// vertices[idx[1] as usize],
// vertices[idx[2] as usize],
// );
// let tj = Triangle::new(
// vertices[jdx[0] as usize],
// vertices[jdx[1] as usize],
// vertices[jdx[2] as usize],
// );
// let center_i = (ti.a.coords + ti.b.coords + ti.c.coords) / 3.0;
// let center_j = (tj.a.coords + tj.b.coords + tj.c.coords) / 3.0;
// crate::geometry::z_cmp_floats(center_i.as_slice(), center_j.as_slice())
// .unwrap_or(std::cmp::Ordering::Equal)
// });
let aabb = AABB::from_points(&vertices);
let data = indices.iter().enumerate().map(|(i, idx)| {
let aabb = Triangle::new(
vertices[idx[0] as usize],
vertices[idx[1] as usize],
vertices[idx[2] as usize],
)
.local_bounding_volume();
(i, aabb)
});
let aabbs: Vec<_> = indices
.iter()
.map(|idx| {
Triangle::new(
vertices[idx[0] as usize],
vertices[idx[1] as usize],
vertices[idx[2] as usize],
)
.local_bounding_volume()
})
.collect();
let waabb_tree = WAABBHierarchy::new(&aabbs);
let mut wquadtree = WQuadtree::new();
// NOTE: we apply no dilation factor because we won't
// update this tree dynamically.
wquadtree.clear_and_rebuild(data, 0.0);
Self {
waabb_tree,
wquadtree,
aabb,
vertices,
indices,
@@ -71,8 +60,8 @@ impl Trimesh {
self.aabb.transform_by(pos)
}
pub(crate) fn waabbs(&self) -> &WAABBHierarchy {
&self.waabb_tree
pub(crate) fn waabbs(&self) -> &WQuadtree<usize> {
&self.wquadtree
}
/// The number of triangles forming this mesh.
@@ -120,3 +109,53 @@ impl Trimesh {
}
}
}
#[cfg(feature = "dim3")]
impl RayCast<f32> for Trimesh {
fn toi_and_normal_with_ray(
&self,
m: &Isometry<f32>,
ray: &Ray,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection> {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
let ls_ray = ray.inverse_transform_by(m);
self.wquadtree
.cast_ray(&ls_ray, max_toi, &mut intersections);
let mut best: Option<RayIntersection> = None;
for inter in intersections {
let tri = self.triangle(inter);
if let Some(inter) = tri.toi_and_normal_with_ray(m, ray, max_toi, solid) {
if let Some(curr) = &mut best {
if curr.toi > inter.toi {
*curr = inter;
}
} else {
best = Some(inter);
}
}
}
best
}
fn intersects_ray(&self, m: &Isometry<f32>, ray: &Ray, max_toi: f32) -> bool {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
let ls_ray = ray.inverse_transform_by(m);
self.wquadtree
.cast_ray(&ls_ray, max_toi, &mut intersections);
for inter in intersections {
let tri = self.triangle(inter);
if tri.intersects_ray(m, ray, max_toi) {
return true;
}
}
false
}
}

108
src/geometry/waabb.rs
View File

@@ -1,8 +1,27 @@
#[cfg(feature = "serde-serialize")]
use crate::math::DIM;
use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH};
use crate::geometry::Ray;
use crate::math::{Point, Vector, DIM, SIMD_WIDTH};
use crate::utils;
use ncollide::bounding_volume::AABB;
use simba::simd::{SimdPartialOrd, SimdValue};
use num::{One, Zero};
use {
crate::math::{SimdBool, SimdFloat},
simba::simd::{SimdPartialOrd, SimdValue},
};
#[derive(Debug, Copy, Clone)]
pub(crate) struct WRay {
pub origin: Point<SimdFloat>,
pub dir: Vector<SimdFloat>,
}
impl WRay {
pub fn splat(ray: Ray) -> Self {
Self {
origin: Point::splat(ray.origin),
dir: Vector::splat(ray.dir),
}
}
}
#[derive(Debug, Copy, Clone)]
pub(crate) struct WAABB {
@@ -28,6 +47,7 @@ impl serde::Serialize for WAABB {
.coords
.map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
);
let mut waabb = serializer.serialize_struct("WAABB", 2)?;
waabb.serialize_field("mins", &mins)?;
waabb.serialize_field("maxs", &maxs)?;
@@ -73,8 +93,8 @@ impl<'de> serde::Deserialize<'de> for WAABB {
}
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 {
@@ -84,8 +104,73 @@ impl WAABB {
}
}
pub fn dilate_by_factor(&mut self, factor: SimdFloat) {
let dilation = (self.maxs - self.mins) * factor;
self.mins -= dilation;
self.maxs += dilation;
}
pub fn replace(&mut self, i: usize, aabb: AABB<f32>) {
self.mins.replace(i, aabb.mins);
self.maxs.replace(i, aabb.maxs);
}
pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool {
let _0 = SimdFloat::zero();
let _1 = SimdFloat::one();
let _infinity = SimdFloat::splat(f32::MAX);
let mut hit = SimdBool::splat(true);
let mut tmin = SimdFloat::zero();
let mut tmax = max_toi;
// TODO: could this be optimized more considering we really just need a boolean answer?
for i in 0usize..DIM {
let is_not_zero = ray.dir[i].simd_ne(_0);
let is_zero_test =
ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i]);
let is_not_zero_test = {
let denom = _1 / ray.dir[i];
let mut inter_with_near_plane =
((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -_infinity);
let mut inter_with_far_plane =
((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, _infinity);
let gt = inter_with_near_plane.simd_gt(inter_with_far_plane);
utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane);
tmin = tmin.simd_max(inter_with_near_plane);
tmax = tmax.simd_min(inter_with_far_plane);
tmin.simd_le(tmax)
};
hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test);
}
hit
}
#[cfg(feature = "dim2")]
pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool {
pub fn contains(&self, other: &WAABB) -> SimdBool {
self.mins.x.simd_le(other.mins.x)
& self.mins.y.simd_le(other.mins.y)
& self.maxs.x.simd_ge(other.maxs.x)
& self.maxs.y.simd_ge(other.maxs.y)
}
#[cfg(feature = "dim3")]
pub fn contains(&self, other: &WAABB) -> SimdBool {
self.mins.x.simd_le(other.mins.x)
& self.mins.y.simd_le(other.mins.y)
& self.mins.z.simd_le(other.mins.z)
& self.maxs.x.simd_ge(other.maxs.x)
& self.maxs.y.simd_ge(other.maxs.y)
& self.maxs.z.simd_ge(other.maxs.z)
}
#[cfg(feature = "dim2")]
pub fn intersects(&self, other: &WAABB) -> SimdBool {
self.mins.x.simd_le(other.maxs.x)
& other.mins.x.simd_le(self.maxs.x)
& self.mins.y.simd_le(other.maxs.y)
@@ -93,7 +178,7 @@ impl WAABB {
}
#[cfg(feature = "dim3")]
pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool {
pub fn intersects(&self, other: &WAABB) -> SimdBool {
self.mins.x.simd_le(other.maxs.x)
& other.mins.x.simd_le(self.maxs.x)
& self.mins.y.simd_le(other.maxs.y)
@@ -101,6 +186,13 @@ impl WAABB {
& self.mins.z.simd_le(other.maxs.z)
& other.mins.z.simd_le(self.maxs.z)
}
pub fn to_merged_aabb(&self) -> AABB<f32> {
AABB::new(
self.mins.coords.map(|e| e.simd_horizontal_min()).into(),
self.maxs.coords.map(|e| e.simd_horizontal_max()).into(),
)
}
}
impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {

560
src/geometry/wquadtree.rs Normal file
View File

@@ -0,0 +1,560 @@
use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB};
use crate::geometry::{WRay, WAABB};
use crate::math::Point;
#[cfg(feature = "dim3")]
use crate::math::Vector;
use crate::simd::{SimdFloat, SIMD_WIDTH};
use ncollide::bounding_volume::BoundingVolume;
use simba::simd::{SimdBool, SimdValue};
use std::collections::VecDeque;
use std::ops::Range;
pub trait IndexedData: Copy {
fn default() -> Self;
fn index(&self) -> usize;
}
impl IndexedData for usize {
fn default() -> Self {
u32::MAX as usize
}
fn index(&self) -> usize {
*self
}
}
impl IndexedData for ColliderHandle {
fn default() -> Self {
ColliderSet::invalid_handle()
}
fn index(&self) -> usize {
self.into_raw_parts().0
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct NodeIndex {
index: u32, // Index of the addressed node in the `nodes` array.
lane: u8, // SIMD lane of the addressed node.
}
impl NodeIndex {
fn new(index: u32, lane: u8) -> Self {
Self { index, lane }
}
fn invalid() -> Self {
Self {
index: u32::MAX,
lane: 0,
}
}
}
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct WQuadtreeNode {
waabb: WAABB,
// Index of the nodes of the 4 nodes represented by self.
// If this is a leaf, it contains the proxy ids instead.
children: [u32; 4],
parent: NodeIndex,
leaf: bool, // TODO: pack this with the NodexIndex.lane?
dirty: bool, // TODO: move this to a separate bitvec?
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct WQuadtreeProxy<T> {
node: NodeIndex,
data: T, // The collider data. TODO: only set the collider generation here?
}
impl<T: IndexedData> WQuadtreeProxy<T> {
fn invalid() -> Self {
Self {
node: NodeIndex::invalid(),
data: T::default(),
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug)]
pub struct WQuadtree<T> {
nodes: Vec<WQuadtreeNode>,
dirty_nodes: VecDeque<u32>,
proxies: Vec<WQuadtreeProxy<T>>,
}
// FIXME: this should be generic too.
impl WQuadtree<ColliderHandle> {
pub fn pre_update(&mut self, data: ColliderHandle) {
let id = data.into_raw_parts().0;
let node_id = self.proxies[id].node.index;
let node = &mut self.nodes[node_id as usize];
if !node.dirty {
node.dirty = true;
self.dirty_nodes.push_back(node_id);
}
}
pub fn update(&mut self, colliders: &ColliderSet, dilation_factor: f32) {
// Loop on the dirty leaves.
let dilation_factor = SimdFloat::splat(dilation_factor);
while let Some(id) = self.dirty_nodes.pop_front() {
// NOTE: this will data the case where we reach the root of the tree.
if let Some(node) = self.nodes.get(id as usize) {
// Compute the new WAABB.
let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH];
for (child_id, new_aabb) in node.children.iter().zip(new_aabbs.iter_mut()) {
if node.leaf {
// We are in a leaf: compute the colliders' AABBs.
if let Some(proxy) = self.proxies.get(*child_id as usize) {
let collider = &colliders[proxy.data];
*new_aabb = collider.compute_aabb();
}
} else {
// We are in an internal node: compute the children's AABBs.
if let Some(node) = self.nodes.get(*child_id as usize) {
*new_aabb = node.waabb.to_merged_aabb();
}
}
}
let node = &mut self.nodes[id as usize];
let new_waabb = WAABB::from(new_aabbs);
if !node.waabb.contains(&new_waabb).all() {
node.waabb = new_waabb;
node.waabb.dilate_by_factor(dilation_factor);
self.dirty_nodes.push_back(node.parent.index);
}
node.dirty = false;
}
}
}
}
impl<T: IndexedData> WQuadtree<T> {
pub fn new() -> Self {
WQuadtree {
nodes: Vec::new(),
dirty_nodes: VecDeque::new(),
proxies: Vec::new(),
}
}
pub fn clear_and_rebuild(
&mut self,
data: impl ExactSizeIterator<Item = (T, AABB)>,
dilation_factor: f32,
) {
self.nodes.clear();
self.proxies.clear();
// Create proxies.
let mut indices = Vec::with_capacity(data.len());
let mut aabbs = vec![AABB::new_invalid(); data.len()];
self.proxies = vec![WQuadtreeProxy::invalid(); data.len()];
for (data, aabb) in data {
let index = data.index();
if index >= self.proxies.len() {
self.proxies.resize(index + 1, WQuadtreeProxy::invalid());
aabbs.resize(index + 1, AABB::new_invalid());
}
self.proxies[index].data = data;
aabbs[index] = aabb;
indices.push(index);
}
// Build the tree recursively.
let root_node = WQuadtreeNode {
waabb: WAABB::new_invalid(),
children: [1, u32::MAX, u32::MAX, u32::MAX],
parent: NodeIndex::invalid(),
leaf: false,
dirty: false,
};
self.nodes.push(root_node);
let root_id = NodeIndex::new(0, 0);
let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id, dilation_factor);
self.nodes[0].waabb = WAABB::from([
aabb,
AABB::new_invalid(),
AABB::new_invalid(),
AABB::new_invalid(),
]);
}
fn do_recurse_build(
&mut self,
indices: &mut [usize],
aabbs: &[AABB],
parent: NodeIndex,
dilation_factor: f32,
) -> (u32, AABB) {
if indices.len() <= 4 {
// Leaf case.
let my_id = self.nodes.len();
let mut my_aabb = AABB::new_invalid();
let mut leaf_aabbs = [AABB::new_invalid(); 4];
let mut proxy_ids = [u32::MAX; 4];
for (k, id) in indices.iter().enumerate() {
my_aabb.merge(&aabbs[*id]);
leaf_aabbs[k] = aabbs[*id];
proxy_ids[k] = *id as u32;
self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8);
}
let mut node = WQuadtreeNode {
waabb: WAABB::from(leaf_aabbs),
children: proxy_ids,
parent,
leaf: true,
dirty: false,
};
node.waabb
.dilate_by_factor(SimdFloat::splat(dilation_factor));
self.nodes.push(node);
return (my_id as u32, my_aabb);
}
// Compute the center and variance along each dimension.
// In 3D we compute the variance to not-subdivide the dimension with lowest variance.
// Therefore variance computation is not needed in 2D because we only have 2 dimension
// to split in the first place.
let mut center = Point::origin();
#[cfg(feature = "dim3")]
let mut variance = Vector::zeros();
let denom = 1.0 / (indices.len() as f32);
for i in &*indices {
let coords = aabbs[*i].center().coords;
center += coords * denom;
#[cfg(feature = "dim3")]
{
variance += coords.component_mul(&coords) * denom;
}
}
#[cfg(feature = "dim3")]
{
variance = variance - center.coords.component_mul(&center.coords);
}
// Find the axis with minimum variance. This is the axis along
// which we are **not** subdividing our set.
#[allow(unused_mut)] // Does not need to be mutable in 2D.
let mut subdiv_dims = [0, 1];
#[cfg(feature = "dim3")]
{
let min = variance.imin();
subdiv_dims[0] = (min + 1) % 3;
subdiv_dims[1] = (min + 2) % 3;
}
// Split the set along the two subdiv_dims dimensions.
// TODO: should we split wrt. the median instead of the average?
// TODO: we should ensure each subslice contains at least 4 elements each (or less if
// indices has less than 16 elements in the first place.
let (left, right) = split_indices_wrt_dim(indices, &aabbs, &center, subdiv_dims[0]);
let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, &center, subdiv_dims[1]);
let (right_bottom, right_top) =
split_indices_wrt_dim(right, &aabbs, &center, subdiv_dims[1]);
// println!(
// "Recursing on children: {}, {}, {}, {}",
// left_bottom.len(),
// left_top.len(),
// right_bottom.len(),
// right_top.len()
// );
let node = WQuadtreeNode {
waabb: WAABB::new_invalid(),
children: [0; 4], // Will be set after the recursive call
parent,
leaf: false,
dirty: false,
};
let id = self.nodes.len() as u32;
self.nodes.push(node);
// Recurse!
let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0), dilation_factor);
let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1), dilation_factor);
let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2), dilation_factor);
let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3), dilation_factor);
// Now we know the indices of the grand-nodes.
self.nodes[id as usize].children = [a.0, b.0, c.0, d.0];
self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]);
self.nodes[id as usize]
.waabb
.dilate_by_factor(SimdFloat::splat(dilation_factor));
// TODO: will this chain of .merged be properly optimized?
let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1);
(id, my_aabb)
}
// FIXME: implement a visitor pattern to merge intersect_aabb
// and intersect_ray into a single method.
pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec<T>) {
if self.nodes.is_empty() {
return;
}
// Special case for the root.
let mut stack = vec![0u32];
let waabb = WAABB::splat(*aabb);
while let Some(inode) = stack.pop() {
let node = self.nodes[inode as usize];
let intersections = node.waabb.intersects(&waabb);
let bitmask = intersections.bitmask();
for ii in 0..SIMD_WIDTH {
if (bitmask & (1 << ii)) != 0 {
if node.leaf {
// We found a leaf!
// Unfortunately, invalid AABBs return a intersection as well.
if let Some(proxy) = self.proxies.get(node.children[ii] as usize) {
out.push(proxy.data);
}
} else {
// Internal node, visit the child.
// Unfortunately, we have this check because invalid AABBs
// return a intersection as well.
if node.children[ii] as usize <= self.nodes.len() {
stack.push(node.children[ii]);
}
}
}
}
}
}
pub fn cast_ray(&self, ray: &Ray, max_toi: f32, out: &mut Vec<T>) {
if self.nodes.is_empty() {
return;
}
// Special case for the root.
let mut stack = vec![0u32];
let wray = WRay::splat(*ray);
let wmax_toi = SimdFloat::splat(max_toi);
while let Some(inode) = stack.pop() {
let node = self.nodes[inode as usize];
let hits = node.waabb.intersects_ray(&wray, wmax_toi);
let bitmask = hits.bitmask();
for ii in 0..SIMD_WIDTH {
if (bitmask & (1 << ii)) != 0 {
if node.leaf {
// We found a leaf!
// Unfortunately, invalid AABBs return a hit as well.
if let Some(proxy) = self.proxies.get(node.children[ii] as usize) {
out.push(proxy.data);
}
} else {
// Internal node, visit the child.
// Un fortunately, we have this check because invalid AABBs
// return a hit as well.
if node.children[ii] as usize <= self.nodes.len() {
stack.push(node.children[ii]);
}
}
}
}
}
}
}
#[allow(dead_code)]
struct WQuadtreeIncrementalBuilderStep {
range: Range<usize>,
parent: NodeIndex,
}
#[allow(dead_code)]
struct WQuadtreeIncrementalBuilder<T> {
quadtree: WQuadtree<T>,
to_insert: Vec<WQuadtreeIncrementalBuilderStep>,
aabbs: Vec<AABB>,
indices: Vec<usize>,
}
#[allow(dead_code)]
impl<T: IndexedData> WQuadtreeIncrementalBuilder<T> {
pub fn new() -> Self {
Self {
quadtree: WQuadtree::new(),
to_insert: Vec::new(),
aabbs: Vec::new(),
indices: Vec::new(),
}
}
pub fn update_single_depth(&mut self) {
if let Some(to_insert) = self.to_insert.pop() {
let indices = &mut self.indices[to_insert.range];
// Leaf case.
if indices.len() <= 4 {
let id = self.quadtree.nodes.len();
let mut aabb = AABB::new_invalid();
let mut leaf_aabbs = [AABB::new_invalid(); 4];
let mut proxy_ids = [u32::MAX; 4];
for (k, id) in indices.iter().enumerate() {
aabb.merge(&self.aabbs[*id]);
leaf_aabbs[k] = self.aabbs[*id];
proxy_ids[k] = *id as u32;
}
let node = WQuadtreeNode {
waabb: WAABB::from(leaf_aabbs),
children: proxy_ids,
parent: to_insert.parent,
leaf: true,
dirty: false,
};
self.quadtree.nodes[to_insert.parent.index as usize].children
[to_insert.parent.lane as usize] = id as u32;
self.quadtree.nodes[to_insert.parent.index as usize]
.waabb
.replace(to_insert.parent.lane as usize, aabb);
self.quadtree.nodes.push(node);
return;
}
// Compute the center and variance along each dimension.
// In 3D we compute the variance to not-subdivide the dimension with lowest variance.
// Therefore variance computation is not needed in 2D because we only have 2 dimension
// to split in the first place.
let mut center = Point::origin();
#[cfg(feature = "dim3")]
let mut variance = Vector::zeros();
let denom = 1.0 / (indices.len() as f32);
let mut aabb = AABB::new_invalid();
for i in &*indices {
let coords = self.aabbs[*i].center().coords;
aabb.merge(&self.aabbs[*i]);
center += coords * denom;
#[cfg(feature = "dim3")]
{
variance += coords.component_mul(&coords) * denom;
}
}
#[cfg(feature = "dim3")]
{
variance = variance - center.coords.component_mul(&center.coords);
}
// Find the axis with minimum variance. This is the axis along
// which we are **not** subdividing our set.
#[allow(unused_mut)] // Does not need to be mutable in 2D.
let mut subdiv_dims = [0, 1];
#[cfg(feature = "dim3")]
{
let min = variance.imin();
subdiv_dims[0] = (min + 1) % 3;
subdiv_dims[1] = (min + 2) % 3;
}
// Split the set along the two subdiv_dims dimensions.
// TODO: should we split wrt. the median instead of the average?
// TODO: we should ensure each subslice contains at least 4 elements each (or less if
// indices has less than 16 elements in the first place.
let (left, right) =
split_indices_wrt_dim(indices, &self.aabbs, &center, subdiv_dims[0]);
let (left_bottom, left_top) =
split_indices_wrt_dim(left, &self.aabbs, &center, subdiv_dims[1]);
let (right_bottom, right_top) =
split_indices_wrt_dim(right, &self.aabbs, &center, subdiv_dims[1]);
let node = WQuadtreeNode {
waabb: WAABB::new_invalid(),
children: [0; 4], // Will be set after the recursive call
parent: to_insert.parent,
leaf: false,
dirty: false,
};
let id = self.quadtree.nodes.len() as u32;
self.quadtree.nodes.push(node);
// Recurse!
let a = left_bottom.len();
let b = a + left_top.len();
let c = b + right_bottom.len();
let d = c + right_top.len();
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: 0..a,
parent: NodeIndex::new(id, 0),
});
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: a..b,
parent: NodeIndex::new(id, 1),
});
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: b..c,
parent: NodeIndex::new(id, 2),
});
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: c..d,
parent: NodeIndex::new(id, 3),
});
self.quadtree.nodes[to_insert.parent.index as usize].children
[to_insert.parent.lane as usize] = id as u32;
self.quadtree.nodes[to_insert.parent.index as usize]
.waabb
.replace(to_insert.parent.lane as usize, aabb);
}
}
}
fn split_indices_wrt_dim<'a>(
indices: &'a mut [usize],
aabbs: &[AABB],
split_point: &Point<f32>,
dim: usize,
) -> (&'a mut [usize], &'a mut [usize]) {
let mut icurr = 0;
let mut ilast = indices.len() - 1;
// The loop condition we can just do 0..indices.len()
// instead of the test icurr < ilast because we know
// we will iterate exactly once per index.
for _ in 0..indices.len() {
let i = indices[icurr];
let center = aabbs[i].center();
if center[dim] > split_point[dim] {
indices.swap(icurr, ilast);
ilast -= 1;
} else {
icurr += 1;
}
}
indices.split_at_mut(icurr)
}

24
src/lib.rs
View File

@@ -44,7 +44,6 @@ macro_rules! enable_flush_to_zero(
}
);
#[cfg(feature = "simd-is-enabled")]
macro_rules! array(
($callback: expr; SIMD_WIDTH) => {
{
@@ -139,7 +138,6 @@ pub mod utils;
#[cfg(feature = "dim2")]
/// Math primitives used throughout Rapier.
pub mod math {
#[cfg(feature = "simd-is-enabled")]
pub use super::simd::*;
use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2};
@@ -182,7 +180,6 @@ pub mod math {
#[cfg(feature = "dim3")]
/// Math primitives used throughout Rapier.
pub mod math {
#[cfg(feature = "simd-is-enabled")]
pub use super::simd::*;
use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3};
@@ -220,6 +217,19 @@ pub mod math {
pub type SdpMatrix<N> = crate::utils::SdpMatrix3<N>;
}
#[cfg(not(feature = "simd-is-enabled"))]
mod simd {
use simba::simd::{AutoBoolx4, AutoF32x4};
/// The number of lanes of a SIMD number.
pub const SIMD_WIDTH: usize = 4;
/// SIMD_WIDTH - 1
pub const SIMD_LAST_INDEX: usize = 3;
/// A SIMD float with SIMD_WIDTH lanes.
pub type SimdFloat = AutoF32x4;
/// A SIMD bool with SIMD_WIDTH lanes.
pub type SimdBool = AutoBoolx4;
}
#[cfg(feature = "simd-is-enabled")]
mod simd {
#[allow(unused_imports)]
@@ -233,16 +243,16 @@ mod simd {
/// SIMD_WIDTH - 1
pub const SIMD_LAST_INDEX: usize = 3;
#[cfg(not(feature = "simd-nightly"))]
/// A SIMD float with SIMD_WIDTH lanes.
/// A SIMD float with SIMD_WIDTH lanes.
pub type SimdFloat = WideF32x4;
#[cfg(not(feature = "simd-nightly"))]
/// A SIMD bool with SIMD_WIDTH lanes.
/// A SIMD bool with SIMD_WIDTH lanes.
pub type SimdBool = WideBoolF32x4;
#[cfg(feature = "simd-nightly")]
/// A SIMD float with SIMD_WIDTH lanes.
/// A SIMD float with SIMD_WIDTH lanes.
pub type SimdFloat = f32x4;
#[cfg(feature = "simd-nightly")]
/// A bool float with SIMD_WIDTH lanes.
/// A bool float with SIMD_WIDTH lanes.
pub type SimdBool = m32x4;
// pub const SIMD_WIDTH: usize = 8;

28
src/pipeline/collision_pipeline.rs
View File

@@ -1,6 +1,6 @@
//! Physics pipeline structures.
use crate::dynamics::{JointSet, RigidBody, RigidBodyHandle, RigidBodySet};
use crate::dynamics::{JointSet, RigidBodySet};
use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
use crate::pipeline::EventHandler;
@@ -50,7 +50,7 @@ impl CollisionPipeline {
self.broad_phase_events.clear();
broad_phase.find_pairs(&mut self.broad_phase_events);
narrow_phase.register_pairs(colliders, &self.broad_phase_events, events);
narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events);
narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events);
@@ -84,28 +84,4 @@ impl CollisionPipeline {
bodies.modified_inactive_set.clear();
}
/// Remove a rigid-body and all its associated data.
pub fn remove_rigid_body(
&mut self,
handle: RigidBodyHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
) -> Option<RigidBody> {
// Remove the body.
let body = bodies.remove_internal(handle)?;
// Remove this rigid-body from the broad-phase and narrow-phase.
broad_phase.remove_colliders(&body.colliders, colliders);
narrow_phase.remove_colliders(&body.colliders, colliders, bodies);
// Remove all colliders attached to this body.
for collider in &body.colliders {
colliders.remove_internal(*collider);
}
Some(body)
}
}

2
src/pipeline/mod.rs
View File

@@ -3,7 +3,9 @@
pub use collision_pipeline::CollisionPipeline;
pub use event_handler::{ChannelEventCollector, EventHandler};
pub use physics_pipeline::PhysicsPipeline;
pub use query_pipeline::QueryPipeline;
mod collision_pipeline;
mod event_handler;
mod physics_pipeline;
mod query_pipeline;

176
src/pipeline/physics_pipeline.rs
View File

@@ -3,12 +3,11 @@
use crate::counters::Counters;
#[cfg(not(feature = "parallel"))]
use crate::dynamics::IslandSolver;
use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandle, RigidBodySet};
use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet,
ContactManifoldIndex, NarrowPhase,
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
};
use crate::math::Vector;
use crate::pipeline::EventHandler;
@@ -71,10 +70,20 @@ impl PhysicsPipeline {
joints: &mut JointSet,
events: &dyn EventHandler,
) {
// println!("Step");
self.counters.step_started();
broad_phase.maintain(colliders);
narrow_phase.maintain(colliders, bodies);
bodies.maintain_active_set();
// Update kinematic bodies velocities.
// TODO: what is the best place for this? It should at least be
// located before the island computation because we test the velocity
// there to determine if this kinematic body should wake-up dynamic
// bodies it is touching.
bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
});
self.counters.stages.collision_detection_time.start();
self.counters.cd.broad_phase_time.start();
self.broadphase_collider_pairs.clear();
@@ -91,7 +100,7 @@ impl PhysicsPipeline {
broad_phase.find_pairs(&mut self.broad_phase_events);
// println!("Find pairs time: {}", instant::now() - t);
narrow_phase.register_pairs(colliders, &self.broad_phase_events, events);
narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
self.counters.cd.broad_phase_time.pause();
// println!("Num contact pairs: {}", pairs.len());
@@ -122,11 +131,6 @@ impl PhysicsPipeline {
);
self.counters.stages.island_construction_time.pause();
// Update kinematic bodies velocities.
bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
});
if self.manifold_indices.len() < bodies.num_islands() {
self.manifold_indices
.resize(bodies.num_islands(), Vec::new());
@@ -245,63 +249,46 @@ impl PhysicsPipeline {
bodies.modified_inactive_set.clear();
self.counters.step_completed();
}
/// Remove a collider and all its associated data.
pub fn remove_collider(
&mut self,
handle: ColliderHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
) -> Option<Collider> {
broad_phase.remove_colliders(&[handle], colliders);
narrow_phase.remove_colliders(&[handle], colliders, bodies);
let collider = colliders.remove_internal(handle)?;
if let Some(parent) = bodies.get_mut_internal(collider.parent) {
parent.remove_collider_internal(handle, &collider);
bodies.wake_up(collider.parent);
}
Some(collider)
}
/// Remove a rigid-body and all its associated data.
pub fn remove_rigid_body(
&mut self,
handle: RigidBodyHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
) -> Option<RigidBody> {
// Remove the body.
let body = bodies.remove_internal(handle)?;
// Remove this rigid-body from the broad-phase and narrow-phase.
broad_phase.remove_colliders(&body.colliders, colliders);
narrow_phase.remove_colliders(&body.colliders, colliders, bodies);
// Remove all joints attached to this body.
joints.remove_rigid_body(body.joint_graph_index, bodies);
// Remove all colliders attached to this body.
for collider in &body.colliders {
colliders.remove_internal(*collider);
}
Some(body)
}
}
#[cfg(test)]
mod test {
use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use crate::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet};
use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
use crate::math::Vector;
use crate::pipeline::PhysicsPipeline;
#[test]
fn kinematic_and_static_contact_crash() {
let mut colliders = ColliderSet::new();
let mut joints = JointSet::new();
let mut pipeline = PhysicsPipeline::new();
let mut bf = BroadPhase::new();
let mut nf = NarrowPhase::new();
let mut bodies = RigidBodySet::new();
let rb = RigidBodyBuilder::new_static().build();
let h1 = bodies.insert(rb.clone());
let co = ColliderBuilder::ball(10.0).build();
colliders.insert(co.clone(), h1, &mut bodies);
// The same but with a kinematic body.
let rb = RigidBodyBuilder::new_kinematic().build();
let h2 = bodies.insert(rb.clone());
colliders.insert(co, h2, &mut bodies);
pipeline.step(
&Vector::zeros(),
&IntegrationParameters::default(),
&mut bf,
&mut nf,
&mut bodies,
&mut colliders,
&mut joints,
&(),
);
}
#[test]
fn rigid_body_removal_before_step() {
let mut colliders = ColliderSet::new();
@@ -310,42 +297,65 @@ mod test {
let mut bf = BroadPhase::new();
let mut nf = NarrowPhase::new();
let mut set = RigidBodySet::new();
let rb = RigidBodyBuilder::new_dynamic().build();
let mut bodies = RigidBodySet::new();
// Check that removing the body right after inserting it works.
let h1 = set.insert(rb.clone());
pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints);
// We add two dynamic bodies, one kinematic body and one static body before removing
// them. This include a non-regression test where deleting a kimenatic body crashes.
let rb = RigidBodyBuilder::new_dynamic().build();
let h1 = bodies.insert(rb.clone());
let h2 = bodies.insert(rb.clone());
// The same but with a kinematic body.
let rb = RigidBodyBuilder::new_kinematic().build();
let h3 = bodies.insert(rb.clone());
// The same but with a static body.
let rb = RigidBodyBuilder::new_static().build();
let h4 = bodies.insert(rb.clone());
let to_delete = [h1, h2, h3, h4];
for h in &to_delete {
bodies.remove(*h, &mut colliders, &mut joints);
}
pipeline.step(
&Vector::zeros(),
&IntegrationParameters::default(),
&mut bf,
&mut nf,
&mut bodies,
&mut colliders,
&mut joints,
&(),
);
}
#[test]
fn rigid_body_removal_snapshot_handle_determinism() {
let mut colliders = ColliderSet::new();
let mut joints = JointSet::new();
let mut pipeline = PhysicsPipeline::new();
let mut bf = BroadPhase::new();
let mut nf = NarrowPhase::new();
let mut set = RigidBodySet::new();
let mut bodies = RigidBodySet::new();
let rb = RigidBodyBuilder::new_dynamic().build();
let h1 = set.insert(rb.clone());
let h2 = set.insert(rb.clone());
let h3 = set.insert(rb.clone());
let h1 = bodies.insert(rb.clone());
let h2 = bodies.insert(rb.clone());
let h3 = bodies.insert(rb.clone());
pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints);
pipeline.remove_rigid_body(h3, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints);
pipeline.remove_rigid_body(h2, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints);
bodies.remove(h1, &mut colliders, &mut joints);
bodies.remove(h3, &mut colliders, &mut joints);
bodies.remove(h2, &mut colliders, &mut joints);
let ser_set = bincode::serialize(&set).unwrap();
let mut set2: RigidBodySet = bincode::deserialize(&ser_set).unwrap();
let ser_bodies = bincode::serialize(&bodies).unwrap();
let mut bodies2: RigidBodySet = bincode::deserialize(&ser_bodies).unwrap();
let h1a = set.insert(rb.clone());
let h2a = set.insert(rb.clone());
let h3a = set.insert(rb.clone());
let h1a = bodies.insert(rb.clone());
let h2a = bodies.insert(rb.clone());
let h3a = bodies.insert(rb.clone());
let h1b = set2.insert(rb.clone());
let h2b = set2.insert(rb.clone());
let h3b = set2.insert(rb.clone());
let h1b = bodies2.insert(rb.clone());
let h2b = bodies2.insert(rb.clone());
let h3b = bodies2.insert(rb.clone());
assert_eq!(h1a, h1b);
assert_eq!(h2a, h2b);

113
src/pipeline/query_pipeline.rs Normal file
View File

@@ -0,0 +1,113 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree};
/// A pipeline for performing queries on all the colliders of a scene.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct QueryPipeline {
quadtree: WQuadtree<ColliderHandle>,
tree_built: bool,
dilation_factor: f32,
}
impl Default for QueryPipeline {
fn default() -> Self {
Self::new()
}
}
impl QueryPipeline {
/// Initializes an empty query pipeline.
pub fn new() -> Self {
Self {
quadtree: WQuadtree::new(),
tree_built: false,
dilation_factor: 0.01,
}
}
/// Update the acceleration structure on the query pipeline.
pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
if !self.tree_built {
let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
// FIXME: uncomment this once we handle insertion/removals properly.
// self.tree_built = true;
return;
}
for (_, body) in bodies
.iter_active_dynamic()
.chain(bodies.iter_active_kinematic())
{
for handle in &body.colliders {
self.quadtree.pre_update(*handle)
}
}
self.quadtree.update(colliders, self.dilation_factor);
}
/// Find the closest intersection between a ray and a set of collider.
///
/// # 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<'a>(
&self,
colliders: &'a ColliderSet,
ray: &Ray,
max_toi: f32,
) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
// TODO: avoid allocation?
let mut inter = Vec::new();
self.quadtree.cast_ray(ray, max_toi, &mut inter);
let mut best = f32::MAX;
let mut result = None;
for handle in inter {
let collider = &colliders[handle];
if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
if inter.toi < best {
best = inter.toi;
result = Some((handle, collider, inter));
}
}
}
result
}
/// Find the all intersections between a ray and a set of collider and passes them to a callback.
///
/// # 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.
/// - `callback`: function executed on each collider for which a ray intersection has been found.
/// There is no guarantees on the order the results will be yielded. If this callback returns `false`,
/// this method will exit early, ignory any further raycast.
pub fn interferences_with_ray<'a>(
&self,
colliders: &'a ColliderSet,
ray: &Ray,
max_toi: f32,
mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
) {
// TODO: avoid allocation?
let mut inter = Vec::new();
self.quadtree.cast_ray(ray, max_toi, &mut inter);
for handle in inter {
let collider = &colliders[handle];
if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
if !callback(handle, collider, inter) {
return;
}
}
}
}
}

30
src/utils.rs
View File

@@ -5,13 +5,15 @@ use crate::dynamics::RigidBodyHandle;
use indexmap::IndexMap as HashMap;
use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3};
use num::Zero;
#[cfg(feature = "simd-is-enabled")]
use simba::simd::SimdValue;
#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
use std::collections::HashMap;
use std::ops::{Add, Mul};
#[cfg(feature = "simd-is-enabled")]
use {crate::simd::SimdFloat, na::SimdPartialOrd, num::One};
use {
crate::simd::{SimdBool, SimdFloat},
na::SimdPartialOrd,
num::One,
};
// pub(crate) const SIN_10_DEGREES: f32 = 0.17364817766;
// pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
@@ -31,6 +33,16 @@ pub(crate) fn inv(val: f32) -> f32 {
}
}
/// Conditionally swaps each lanes of `a` with those of `b`.
///
/// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then
/// `a.extract(i)` is swapped with `b.extract(i)`.
pub fn simd_swap(do_swap: SimdBool, a: &mut SimdFloat, b: &mut SimdFloat) {
let _a = *a;
*a = b.select(do_swap, *a);
*b = _a.select(do_swap, *b);
}
/// Trait to copy the sign of each component of one scalar/vector/matrix to another.
pub trait WSign<Rhs>: Sized {
// See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652
@@ -77,7 +89,6 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WSign<SimdFloat> for SimdFloat {
fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
self.simd_copysign(to)
@@ -102,7 +113,6 @@ impl WComponent for f32 {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WComponent for SimdFloat {
type Element = f32;
@@ -319,7 +329,6 @@ impl WDot<f32> for f32 {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WCrossMatrix for Vector3<SimdFloat> {
type CrossMat = Matrix3<SimdFloat>;
@@ -334,7 +343,6 @@ impl WCrossMatrix for Vector3<SimdFloat> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WCrossMatrix for Vector2<SimdFloat> {
type CrossMat = Vector2<SimdFloat>;
@@ -344,7 +352,6 @@ impl WCrossMatrix for Vector2<SimdFloat> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WCross<Vector3<SimdFloat>> for Vector3<SimdFloat> {
type Result = Vector3<SimdFloat>;
@@ -353,7 +360,6 @@ impl WCross<Vector3<SimdFloat>> for Vector3<SimdFloat> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WCross<Vector2<SimdFloat>> for SimdFloat {
type Result = Vector2<SimdFloat>;
@@ -362,7 +368,6 @@ impl WCross<Vector2<SimdFloat>> for SimdFloat {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WCross<Vector2<SimdFloat>> for Vector2<SimdFloat> {
type Result = SimdFloat;
@@ -373,7 +378,6 @@ impl WCross<Vector2<SimdFloat>> for Vector2<SimdFloat> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WDot<Vector3<SimdFloat>> for Vector3<SimdFloat> {
type Result = SimdFloat;
@@ -382,7 +386,6 @@ impl WDot<Vector3<SimdFloat>> for Vector3<SimdFloat> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WDot<Vector2<SimdFloat>> for Vector2<SimdFloat> {
type Result = SimdFloat;
@@ -391,7 +394,6 @@ impl WDot<Vector2<SimdFloat>> for Vector2<SimdFloat> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WDot<SimdFloat> for SimdFloat {
type Result = SimdFloat;
@@ -445,7 +447,6 @@ impl WAngularInertia<f32> for f32 {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WAngularInertia<SimdFloat> for SimdFloat {
type AngVector = SimdFloat;
type LinVector = Vector2<SimdFloat>;
@@ -860,7 +861,6 @@ impl WAngularInertia<f32> for SdpMatrix3<f32> {
}
}
#[cfg(feature = "simd-is-enabled")]
impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
type AngVector = Vector3<SimdFloat>;
type LinVector = Vector3<SimdFloat>;