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

Outsource the Shape trait, wquadtree, and shape types.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-14 15:51:43 +01:00
parent 9bf1321f8f
commit cc6d1b9730
47 changed files with 444 additions and 3363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/data/arena.rs
View File

@@ -3,6 +3,7 @@
//! See https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs.
//! This has been modified to have a fully deterministic deserialization (including for the order of
//! Index attribution after a deserialization of the arena.
use buckler::partitioning::IndexedData;
use std::cmp;
use std::iter::{self, Extend, FromIterator, FusedIterator};
use std::mem;
@@ -51,6 +52,16 @@ pub struct Index {
generation: u64,
}
impl IndexedData for Index {
fn default() -> Self {
Self::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
}
fn index(&self) -> usize {
self.into_raw_parts().0
}
}
impl Index {
/// Create a new `Index` from its raw parts.
///

442
src/dynamics/mass_properties.rs
View File

@@ -1,442 +0,0 @@
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
use crate::utils;
use num::Zero;
use std::ops::{Add, AddAssign, Sub, SubAssign};
#[cfg(feature = "dim3")]
use {na::Matrix3, std::ops::MulAssign};
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// The local mass properties of a rigid-body.
pub struct MassProperties {
/// The center of mass of a rigid-body expressed in its local-space.
pub local_com: Point<f32>,
/// The inverse of the mass of a rigid-body.
///
/// If this is zero, the rigid-body is assumed to have infinite mass.
pub inv_mass: f32,
/// The inverse of the principal angular inertia of the rigid-body.
///
/// Components set to zero are assumed to be infinite along the corresponding principal axis.
pub inv_principal_inertia_sqrt: AngVector<f32>,
#[cfg(feature = "dim3")]
/// The principal vectors of the local angular inertia tensor of the rigid-body.
pub principal_inertia_local_frame: Rotation<f32>,
}
impl MassProperties {
/// Initializes the mass properties with the given center-of-mass, mass, and angular inertia.
///
/// The center-of-mass is specified in the local-space of the rigid-body.
#[cfg(feature = "dim2")]
pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
let inv_mass = utils::inv(mass);
let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt());
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
}
}
/// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia.
///
/// The center-of-mass is specified in the local-space of the rigid-body.
/// The principal angular inertia are the angular inertia along the coordinate axes in the local-space
/// of the rigid-body.
#[cfg(feature = "dim3")]
pub fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self {
Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity())
}
/// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia.
///
/// The center-of-mass is specified in the local-space of the rigid-body.
/// The principal angular inertia are the angular inertia along the coordinate axes defined by
/// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body.
#[cfg(feature = "dim3")]
pub fn with_principal_inertia_frame(
local_com: Point<f32>,
mass: f32,
principal_inertia: AngVector<f32>,
principal_inertia_local_frame: Rotation<f32>,
) -> Self {
let inv_mass = utils::inv(mass);
let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
principal_inertia_local_frame,
}
}
/// The world-space center of mass of the rigid-body.
pub fn world_com(&self, pos: &Isometry<f32>) -> Point<f32> {
pos * self.local_com
}
#[cfg(feature = "dim2")]
/// The world-space inverse angular inertia tensor of the rigid-body.
pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation<f32>) -> AngularInertia<f32> {
self.inv_principal_inertia_sqrt
}
#[cfg(feature = "dim3")]
/// The world-space inverse angular inertia tensor of the rigid-body.
pub fn world_inv_inertia_sqrt(&self, rot: &Rotation<f32>) -> AngularInertia<f32> {
if !self.inv_principal_inertia_sqrt.is_zero() {
let mut lhs = (rot * self.principal_inertia_local_frame)
.to_rotation_matrix()
.into_inner();
let rhs = lhs.transpose();
lhs.column_mut(0)
.mul_assign(self.inv_principal_inertia_sqrt.x);
lhs.column_mut(1)
.mul_assign(self.inv_principal_inertia_sqrt.y);
lhs.column_mut(2)
.mul_assign(self.inv_principal_inertia_sqrt.z);
let inertia = lhs * rhs;
AngularInertia::from_sdp_matrix(inertia)
} else {
AngularInertia::zero()
}
}
#[cfg(feature = "dim3")]
/// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
self.principal_inertia_local_frame.to_rotation_matrix()
* Matrix3::from_diagonal(&inv_principal_inertia)
* self
.principal_inertia_local_frame
.inverse()
.to_rotation_matrix()
}
#[cfg(feature = "dim3")]
/// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
self.principal_inertia_local_frame.to_rotation_matrix()
* Matrix3::from_diagonal(&principal_inertia)
* self
.principal_inertia_local_frame
.inverse()
.to_rotation_matrix()
}
#[cfg(feature = "dim2")]
pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> f32 {
let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt);
if self.inv_mass != 0.0 {
let mass = 1.0 / self.inv_mass;
i + shift.norm_squared() * mass
} else {
i
}
}
#[cfg(feature = "dim3")]
pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> Matrix3<f32> {
let matrix = self.reconstruct_inertia_matrix();
if self.inv_mass != 0.0 {
let mass = 1.0 / self.inv_mass;
let diag = shift.norm_squared();
let diagm = Matrix3::from_diagonal_element(diag);
matrix + (diagm + shift * shift.transpose()) * mass
} else {
matrix
}
}
/// Transform each element of the mass properties.
pub fn transform_by(&self, m: &Isometry<f32>) -> Self {
// NOTE: we don't apply the parallel axis theorem here
// because the center of mass is also transformed.
Self {
local_com: m * self.local_com,
inv_mass: self.inv_mass,
inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt,
#[cfg(feature = "dim3")]
principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame,
}
}
}
impl Zero for MassProperties {
fn zero() -> Self {
Self {
inv_mass: 0.0,
inv_principal_inertia_sqrt: na::zero(),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
local_com: Point::origin(),
}
}
fn is_zero(&self) -> bool {
*self == Self::zero()
}
}
impl Sub<MassProperties> for MassProperties {
type Output = Self;
#[cfg(feature = "dim2")]
fn sub(self, other: MassProperties) -> Self {
if self.is_zero() || other.is_zero() {
return self;
}
let m1 = utils::inv(self.inv_mass);
let m2 = utils::inv(other.inv_mass);
let inv_mass = utils::inv(m1 - m2);
let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 - i2;
// NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt());
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
}
}
#[cfg(feature = "dim3")]
fn sub(self, other: MassProperties) -> Self {
if self.is_zero() || other.is_zero() {
return self;
}
let m1 = utils::inv(self.inv_mass);
let m2 = utils::inv(other.inv_mass);
let inv_mass = utils::inv(m1 - m2);
let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 - i2;
let eigen = inertia.symmetric_eigen();
let principal_inertia_local_frame =
Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one());
let principal_inertia = eigen.eigenvalues;
// NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt()));
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
principal_inertia_local_frame,
}
}
}
impl SubAssign<MassProperties> for MassProperties {
fn sub_assign(&mut self, rhs: MassProperties) {
*self = *self - rhs
}
}
impl Add<MassProperties> for MassProperties {
type Output = Self;
#[cfg(feature = "dim2")]
fn add(self, other: MassProperties) -> Self {
if self.is_zero() {
return other;
} else if other.is_zero() {
return self;
}
let m1 = utils::inv(self.inv_mass);
let m2 = utils::inv(other.inv_mass);
let inv_mass = utils::inv(m1 + m2);
let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass;
let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 + i2;
let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt());
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
}
}
#[cfg(feature = "dim3")]
fn add(self, other: MassProperties) -> Self {
if self.is_zero() {
return other;
} else if other.is_zero() {
return self;
}
let m1 = utils::inv(self.inv_mass);
let m2 = utils::inv(other.inv_mass);
let inv_mass = utils::inv(m1 + m2);
let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass;
let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 + i2;
let eigen = inertia.symmetric_eigen();
let principal_inertia_local_frame =
Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one());
let principal_inertia = eigen.eigenvalues;
let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
principal_inertia_local_frame,
}
}
}
impl AddAssign<MassProperties> for MassProperties {
fn add_assign(&mut self, rhs: MassProperties) {
*self = *self + rhs
}
}
impl approx::AbsDiffEq for MassProperties {
type Epsilon = f32;
fn default_epsilon() -> Self::Epsilon {
f32::default_epsilon()
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon);
#[cfg(feature = "dim3")]
let inertia_is_ok = self
.reconstruct_inverse_inertia_matrix()
.abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon);
inertia_is_ok
&& self.local_com.abs_diff_eq(&other.local_com, epsilon)
&& self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
&& self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
}
}
impl approx::RelativeEq for MassProperties {
fn default_max_relative() -> Self::Epsilon {
f32::default_max_relative()
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
&other.inv_principal_inertia_sqrt,
epsilon,
max_relative,
);
#[cfg(feature = "dim3")]
let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
&other.reconstruct_inverse_inertia_matrix(),
epsilon,
max_relative,
);
inertia_is_ok
&& self
.local_com
.relative_eq(&other.local_com, epsilon, max_relative)
&& self
.inv_mass
.relative_eq(&other.inv_mass, epsilon, max_relative)
}
}
#[cfg(test)]
mod test {
use super::MassProperties;
use crate::geometry::ColliderBuilder;
use crate::math::{Point, Rotation, Vector};
use approx::assert_relative_eq;
use num::Zero;
#[test]
fn mass_properties_add_partial_zero() {
let m1 = MassProperties {
local_com: Point::origin(),
inv_mass: 2.0,
inv_principal_inertia_sqrt: na::zero(),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
};
let m2 = MassProperties {
local_com: Point::origin(),
inv_mass: 0.0,
#[cfg(feature = "dim2")]
inv_principal_inertia_sqrt: 1.0,
#[cfg(feature = "dim3")]
inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
};
let result = MassProperties {
local_com: Point::origin(),
inv_mass: 2.0,
#[cfg(feature = "dim2")]
inv_principal_inertia_sqrt: 1.0,
#[cfg(feature = "dim3")]
inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0),
#[cfg(feature = "dim3")]
principal_inertia_local_frame: Rotation::identity(),
};
assert_eq!(m1 + m2, result);
assert_eq!(m2 + m1, result);
}
#[test]
fn mass_properties_add_sub() {
// Check that addition and subtraction of mass properties behave as expected.
let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build();
let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build();
let c3 = ColliderBuilder::ball(5.0).build();
let m1 = c1.mass_properties();
let m2 = c2.mass_properties();
let m3 = c3.mass_properties();
let m1m2m3 = m1 + m2 + m3;
assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6);
assert_relative_eq!(
m1m2m3 - m1 - m2 - m3,
MassProperties::zero(),
epsilon = 1.0e-6
);
}
}

30
src/dynamics/mass_properties_ball.rs
View File

@@ -1,30 +0,0 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::math::Vector;
use crate::math::{Point, PrincipalAngularInertia};
impl MassProperties {
pub(crate) fn ball_volume_unit_angular_inertia(
radius: f32,
) -> (f32, PrincipalAngularInertia<f32>) {
#[cfg(feature = "dim2")]
{
let volume = std::f32::consts::PI * radius * radius;
let i = radius * radius / 2.0;
(volume, i)
}
#[cfg(feature = "dim3")]
{
let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0;
let i = radius * radius * 2.0 / 5.0;
(volume, Vector::repeat(i))
}
}
pub(crate) fn from_ball(density: f32, radius: f32) -> Self {
let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius);
let mass = vol * density;
Self::new(Point::origin(), mass, unit_i * mass)
}
}

39
src/dynamics/mass_properties_capsule.rs
View File

@@ -1,39 +0,0 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::geometry::Capsule;
use crate::math::Point;
impl MassProperties {
pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
let half_height = (b - a).norm() / 2.0;
let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius);
let cap_vol = cyl_vol + ball_vol;
let cap_mass = cap_vol * density;
let mut cap_unit_i = cyl_unit_i + ball_unit_i;
let local_com = na::center(&a, &b);
#[cfg(feature = "dim2")]
{
let h = half_height * 2.0;
let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
cap_unit_i += extra;
Self::new(local_com, cap_mass, cap_unit_i * cap_mass)
}
#[cfg(feature = "dim3")]
{
let h = half_height * 2.0;
let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
cap_unit_i.x += extra;
cap_unit_i.z += extra;
let local_frame = Capsule::new(a, b, radius).rotation_wrt_y();
Self::with_principal_inertia_frame(
local_com,
cap_mass,
cap_unit_i * cap_mass,
local_frame,
)
}
}
}

29
src/dynamics/mass_properties_cone.rs
View File

@@ -1,29 +0,0 @@
use crate::dynamics::MassProperties;
use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
impl MassProperties {
pub(crate) fn cone_y_volume_unit_inertia(
half_height: f32,
radius: f32,
) -> (f32, PrincipalAngularInertia<f32>) {
let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
let sq_radius = radius * radius;
let sq_height = half_height * half_height * 4.0;
let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
let principal = sq_radius * 3.0 / 10.0;
(volume, Vector::new(off_principal, principal, off_principal))
}
pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
let cyl_mass = cyl_vol * density;
Self::with_principal_inertia_frame(
Point::new(0.0, -half_height / 2.0, 0.0),
cyl_mass,
cyl_unit_i * cyl_mass,
Rotation::identity(),
)
}
}

33
src/dynamics/mass_properties_cuboid.rs
View File

@@ -1,33 +0,0 @@
use crate::dynamics::MassProperties;
use crate::math::{Point, PrincipalAngularInertia, Vector};
impl MassProperties {
pub(crate) fn cuboid_volume_unit_inertia(
half_extents: Vector<f32>,
) -> (f32, PrincipalAngularInertia<f32>) {
#[cfg(feature = "dim2")]
{
let volume = half_extents.x * half_extents.y * 4.0;
let ix = (half_extents.x * half_extents.x) / 3.0;
let iy = (half_extents.y * half_extents.y) / 3.0;
(volume, ix + iy)
}
#[cfg(feature = "dim3")]
{
let volume = half_extents.x * half_extents.y * half_extents.z * 8.0;
let ix = (half_extents.x * half_extents.x) / 3.0;
let iy = (half_extents.y * half_extents.y) / 3.0;
let iz = (half_extents.z * half_extents.z) / 3.0;
(volume, Vector::new(iy + iz, ix + iz, ix + iy))
}
}
pub(crate) fn from_cuboid(density: f32, half_extents: Vector<f32>) -> Self {
let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents);
let mass = vol * density;
Self::new(Point::origin(), mass, unit_i * mass)
}
}

40
src/dynamics/mass_properties_cylinder.rs
View File

@@ -1,40 +0,0 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::math::{Point, Rotation};
use crate::math::{PrincipalAngularInertia, Vector};
impl MassProperties {
pub(crate) fn cylinder_y_volume_unit_inertia(
half_height: f32,
radius: f32,
) -> (f32, PrincipalAngularInertia<f32>) {
#[cfg(feature = "dim2")]
{
Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
}
#[cfg(feature = "dim3")]
{
let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
let sq_radius = radius * radius;
let sq_height = half_height * half_height * 4.0;
let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
(volume, inertia)
}
}
#[cfg(feature = "dim3")]
pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self {
let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
let cyl_mass = cyl_vol * density;
Self::with_principal_inertia_frame(
Point::origin(),
cyl_mass,
cyl_unit_i * cyl_mass,
Rotation::identity(),
)
}
}

146
src/dynamics/mass_properties_polygon.rs
View File

@@ -1,146 +0,0 @@
#![allow(dead_code)] // TODO: remove this
use crate::dynamics::MassProperties;
use crate::math::Point;
impl MassProperties {
pub(crate) fn from_polygon(density: f32, vertices: &[Point<f32>]) -> MassProperties {
let (area, com) = convex_polygon_area_and_center_of_mass(vertices);
if area == 0.0 {
return MassProperties::new(com, 0.0, 0.0);
}
let mut itot = 0.0;
let factor = 1.0 / 6.0;
let mut iterpeek = vertices.iter().peekable();
let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or
while let Some(elem) = iterpeek.next() {
let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement));
// algorithm adapted from Box2D
let e1 = *elem - com;
let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com;
let ex1 = e1[0];
let ey1 = e1[1];
let ex2 = e2[0];
let ey2 = e2[1];
let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
let ipart = factor * (intx2 + inty2);
itot += ipart * area;
}
Self::new(com, area * density, itot * density)
}
}
fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point<f32>]) -> (f32, Point<f32>) {
let geometric_center = convex_polygon
.iter()
.fold(Point::origin(), |e1, e2| e1 + e2.coords)
/ convex_polygon.len() as f32;
let mut res = Point::origin();
let mut areasum = 0.0;
let mut iterpeek = convex_polygon.iter().peekable();
let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or.
while let Some(elem) = iterpeek.next() {
let (a, b, c) = (
elem,
iterpeek.peek().unwrap_or(&firstelement),
&geometric_center,
);
let area = triangle_area(a, b, c);
let center = (a.coords + b.coords + c.coords) / 3.0;
res += center * area;
areasum += area;
}
if areasum == 0.0 {
(areasum, geometric_center)
} else {
(areasum, res / areasum)
}
}
pub fn triangle_area(pa: &Point<f32>, pb: &Point<f32>, pc: &Point<f32>) -> f32 {
// Kahan's formula.
let a = na::distance(pa, pb);
let b = na::distance(pb, pc);
let c = na::distance(pc, pa);
let (c, b, a) = sort3(&a, &b, &c);
let a = *a;
let b = *b;
let c = *c;
let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c));
sqr.sqrt() * 0.25
}
/// Sorts a set of three values in increasing order.
#[inline]
pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) {
let a_b = *a > *b;
let a_c = *a > *c;
let b_c = *b > *c;
let sa;
let sb;
let sc;
// Sort the three values.
// FIXME: move this to the utilities?
if a_b {
// a > b
if a_c {
// a > c
sc = a;
if b_c {
// b > c
sa = c;
sb = b;
} else {
// b <= c
sa = b;
sb = c;
}
} else {
// a <= c
sa = b;
sb = a;
sc = c;
}
} else {
// a < b
if !a_c {
// a <= c
sa = a;
if b_c {
// b > c
sb = c;
sc = b;
} else {
sb = b;
sc = c;
}
} else {
// a > c
sa = c;
sb = a;
sc = b;
}
}
(sa, sb, sc)
}

11
src/dynamics/mod.rs
View File

@@ -7,9 +7,9 @@ pub use self::joint::RevoluteJoint;
pub use self::joint::{
BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint,
};
pub use self::mass_properties::MassProperties;
pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
pub use buckler::shape::MassProperties;
// #[cfg(not(feature = "parallel"))]
pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::rigid_body::RigidBodyChanges;
@@ -20,15 +20,6 @@ pub(crate) use self::solver::ParallelIslandSolver;
mod integration_parameters;
mod joint;
mod mass_properties;
mod mass_properties_ball;
mod mass_properties_capsule;
#[cfg(feature = "dim3")]
mod mass_properties_cone;
mod mass_properties_cuboid;
mod mass_properties_cylinder;
#[cfg(feature = "dim2")]
mod mass_properties_polygon;
mod rigid_body;
mod rigid_body_set;
mod solver;

44
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH};
use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdReal, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint {
position1: [usize; SIMD_WIDTH],
position2: [usize; SIMD_WIDTH],
local_com1: Point<SimdFloat>,
local_com2: Point<SimdFloat>,
local_com1: Point<SimdReal>,
local_com2: Point<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1: AngularInertia<SimdFloat>,
ii2: AngularInertia<SimdFloat>,
ii1: AngularInertia<SimdReal>,
ii2: AngularInertia<SimdReal>,
local_anchor1: Point<SimdFloat>,
local_anchor2: Point<SimdFloat>,
local_anchor1: Point<SimdReal>,
local_anchor2: Point<SimdReal>,
}
impl WBallPositionConstraint {
@@ -31,13 +31,13 @@ impl WBallPositionConstraint {
) -> Self {
let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
let ii2 = AngularInertia::<SimdFloat>::from(
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
@@ -97,7 +97,7 @@ impl WBallPositionConstraint {
};
let inv_lhs = lhs.inverse_unchecked();
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
position1.translation.vector += impulse * self.im1;
position2.translation.vector -= impulse * self.im2;
@@ -120,11 +120,11 @@ impl WBallPositionConstraint {
#[derive(Debug)]
pub(crate) struct WBallPositionGroundConstraint {
position2: [usize; SIMD_WIDTH],
anchor1: Point<SimdFloat>,
im2: SimdFloat,
ii2: AngularInertia<SimdFloat>,
local_anchor2: Point<SimdFloat>,
local_com2: Point<SimdFloat>,
anchor1: Point<SimdReal>,
im2: SimdReal,
ii2: AngularInertia<SimdReal>,
local_anchor2: Point<SimdReal>,
local_com2: Point<SimdReal>,
}
impl WBallPositionGroundConstraint {
@@ -141,8 +141,8 @@ impl WBallPositionGroundConstraint {
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
@@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint {
};
let inv_lhs = lhs.inverse_unchecked();
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp));
position2.translation.vector -= impulse * self.im2;
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));

64
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -3,7 +3,7 @@ use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use simba::simd::SimdValue;
@@ -15,16 +15,16 @@ pub(crate) struct WBallVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
rhs: Vector<SimdFloat>,
pub(crate) impulse: Vector<SimdFloat>,
rhs: Vector<SimdReal>,
pub(crate) impulse: Vector<SimdReal>,
gcross1: Vector<SimdFloat>,
gcross2: Vector<SimdFloat>,
gcross1: Vector<SimdReal>,
gcross2: Vector<SimdReal>,
inv_lhs: SdpMatrix<SimdFloat>,
inv_lhs: SdpMatrix<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
}
impl WBallVelocityConstraint {
@@ -37,20 +37,20 @@ impl WBallVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -62,8 +62,8 @@ impl WBallVelocityConstraint {
let anchor1 = position1 * local_anchor1 - world_com1;
let anchor2 = position2 * local_anchor2 - world_com2;
let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
let rhs = -(vel1 - vel2);
let lhs;
@@ -99,7 +99,7 @@ impl WBallVelocityConstraint {
mj_lambda2,
im1,
im2,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
gcross1,
gcross2,
rhs,
@@ -141,7 +141,7 @@ impl WBallVelocityConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
@@ -149,7 +149,7 @@ impl WBallVelocityConstraint {
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
};
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
@@ -195,11 +195,11 @@ impl WBallVelocityConstraint {
pub(crate) struct WBallVelocityGroundConstraint {
mj_lambda2: [usize; SIMD_WIDTH],
joint_id: [JointIndex; SIMD_WIDTH],
rhs: Vector<SimdFloat>,
pub(crate) impulse: Vector<SimdFloat>,
gcross2: Vector<SimdFloat>,
inv_lhs: SdpMatrix<SimdFloat>,
im2: SimdFloat,
rhs: Vector<SimdReal>,
pub(crate) impulse: Vector<SimdReal>,
gcross2: Vector<SimdReal>,
inv_lhs: SdpMatrix<SimdReal>,
im2: SimdReal,
}
impl WBallVelocityGroundConstraint {
@@ -213,7 +213,7 @@ impl WBallVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
@@ -221,10 +221,10 @@ impl WBallVelocityGroundConstraint {
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -237,8 +237,8 @@ impl WBallVelocityGroundConstraint {
let anchor1 = position1 * local_anchor1 - world_com1;
let anchor2 = position2 * local_anchor2 - world_com2;
let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1);
let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2);
let rhs = vel2 - vel1;
let lhs;
@@ -267,7 +267,7 @@ impl WBallVelocityGroundConstraint {
joint_id,
mj_lambda2,
im2,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
gcross2,
rhs,
inv_lhs,
@@ -294,7 +294,7 @@ impl WBallVelocityGroundConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),

76
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector,
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdReal, SpacialVector, Vector,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -24,29 +24,29 @@ pub(crate) struct WFixedVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
impulse: SpacialVector<SimdFloat>,
impulse: SpacialVector<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<SimdFloat>,
rhs: Vector6<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<SimdFloat>,
inv_lhs: Matrix3<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector3<SimdFloat>,
rhs: Vector3<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1: AngularInertia<SimdFloat>,
ii2: AngularInertia<SimdFloat>,
ii1: AngularInertia<SimdReal>,
ii2: AngularInertia<SimdReal>,
ii1_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
r1: Vector<SimdFloat>,
r2: Vector<SimdFloat>,
r1: Vector<SimdReal>,
r2: Vector<SimdReal>,
}
impl WFixedVelocityConstraint {
@@ -59,20 +59,20 @@ impl WFixedVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -150,7 +150,7 @@ impl WFixedVelocityConstraint {
ii2,
ii1_sqrt,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
inv_lhs,
r1,
r2,
@@ -203,7 +203,7 @@ impl WFixedVelocityConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
@@ -211,7 +211,7 @@ impl WFixedVelocityConstraint {
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
};
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
@@ -279,22 +279,22 @@ pub(crate) struct WFixedVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
impulse: SpacialVector<SimdFloat>,
impulse: SpacialVector<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
#[cfg(feature = "dim3")]
rhs: Vector6<SimdFloat>,
rhs: Vector6<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix3<SimdFloat>,
inv_lhs: Matrix3<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector3<SimdFloat>,
rhs: Vector3<SimdReal>,
im2: SimdFloat,
ii2: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
r2: Vector<SimdFloat>,
im2: SimdReal,
ii2: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
r2: Vector<SimdReal>,
}
impl WFixedVelocityGroundConstraint {
@@ -308,15 +308,15 @@ impl WFixedVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -386,7 +386,7 @@ impl WFixedVelocityGroundConstraint {
im2,
ii2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
inv_lhs,
r2,
rhs,
@@ -421,7 +421,7 @@ impl WFixedVelocityGroundConstraint {
}
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),

114
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, SimdBool, SimdFloat, Vector, SIMD_WIDTH,
AngVector, AngularInertia, Isometry, Point, SimdBool, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[cfg(feature = "dim3")]
@@ -28,37 +28,37 @@ pub(crate) struct WPrismaticVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r1: Vector<SimdFloat>,
r2: Vector<SimdFloat>,
r1: Vector<SimdReal>,
r2: Vector<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
#[cfg(feature = "dim3")]
rhs: Vector5<SimdFloat>,
rhs: Vector5<SimdReal>,
#[cfg(feature = "dim3")]
impulse: Vector5<SimdFloat>,
impulse: Vector5<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<SimdFloat>,
inv_lhs: Matrix2<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector2<SimdFloat>,
rhs: Vector2<SimdReal>,
#[cfg(feature = "dim2")]
impulse: Vector2<SimdFloat>,
impulse: Vector2<SimdReal>,
limits_impulse: SimdFloat,
limits_forcedirs: Option<(Vector<SimdFloat>, Vector<SimdFloat>)>,
limits_rhs: SimdFloat,
limits_impulse: SimdReal,
limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
limits_rhs: SimdReal,
#[cfg(feature = "dim2")]
basis1: Vector2<SimdFloat>,
basis1: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<SimdFloat>,
basis1: Matrix3x2<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WPrismaticVelocityConstraint {
@@ -71,20 +71,20 @@ impl WPrismaticVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -199,14 +199,14 @@ impl WPrismaticVelocityConstraint {
// FIXME: we should allow both limits to be active at
// the same time + allow predictive constraint activation.
let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
let _0: SimdFloat = na::zero();
let _1: SimdFloat = na::one();
let _0: SimdReal = na::zero();
let _1: SimdReal = na::one();
let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
if sign != _0 {
@@ -224,8 +224,8 @@ impl WPrismaticVelocityConstraint {
ii1_sqrt,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
limits_forcedirs,
limits_rhs,
basis1,
@@ -383,34 +383,34 @@ pub(crate) struct WPrismaticVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r2: Vector<SimdFloat>,
r2: Vector<SimdReal>,
#[cfg(feature = "dim2")]
inv_lhs: Matrix2<SimdFloat>,
inv_lhs: Matrix2<SimdReal>,
#[cfg(feature = "dim2")]
rhs: Vector2<SimdFloat>,
rhs: Vector2<SimdReal>,
#[cfg(feature = "dim2")]
impulse: Vector2<SimdFloat>,
impulse: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
inv_lhs: Matrix5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
#[cfg(feature = "dim3")]
rhs: Vector5<SimdFloat>,
rhs: Vector5<SimdReal>,
#[cfg(feature = "dim3")]
impulse: Vector5<SimdFloat>,
impulse: Vector5<SimdReal>,
limits_impulse: SimdFloat,
limits_rhs: SimdFloat,
limits_impulse: SimdReal,
limits_rhs: SimdReal,
axis2: Vector<SimdFloat>,
axis2: Vector<SimdReal>,
#[cfg(feature = "dim2")]
basis1: Vector2<SimdFloat>,
basis1: Vector2<SimdReal>,
#[cfg(feature = "dim3")]
basis1: Matrix3x2<SimdFloat>,
limits_forcedir2: Option<Vector<SimdFloat>>,
basis1: Matrix3x2<SimdReal>,
limits_forcedir2: Option<Vector<SimdReal>>,
im2: SimdFloat,
ii2_sqrt: AngularInertia<SimdFloat>,
im2: SimdReal,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WPrismaticVelocityGroundConstraint {
@@ -424,15 +424,15 @@ impl WPrismaticVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -551,14 +551,14 @@ impl WPrismaticVelocityGroundConstraint {
// FIXME: we should allow both limits to be active at
// the same time + allow predictive constraint activation.
let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let lim_impulse = SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
let use_min = dist.simd_lt(min_limit);
let use_max = dist.simd_gt(max_limit);
let _0: SimdFloat = na::zero();
let _1: SimdFloat = na::one();
let _0: SimdReal = na::zero();
let _1: SimdReal = na::one();
let sign = _1.select(use_min, (-_1).select(use_max, _0));
if sign != _0 {
@@ -573,8 +573,8 @@ impl WPrismaticVelocityGroundConstraint {
mj_lambda2,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
basis1,
inv_lhs,
rhs,

60
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
};
use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, SIMD_WIDTH};
use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, SIMD_WIDTH};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
@@ -15,20 +15,20 @@ pub(crate) struct WRevoluteVelocityConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r1: Vector<SimdFloat>,
r2: Vector<SimdFloat>,
r1: Vector<SimdReal>,
r2: Vector<SimdReal>,
inv_lhs: Matrix5<SimdFloat>,
rhs: Vector5<SimdFloat>,
impulse: Vector5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
rhs: Vector5<SimdReal>,
impulse: Vector5<SimdReal>,
basis1: Matrix3x2<SimdFloat>,
basis1: Matrix3x2<SimdReal>,
im1: SimdFloat,
im2: SimdFloat,
im1: SimdReal,
im2: SimdReal,
ii1_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdFloat>,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WRevoluteVelocityConstraint {
@@ -41,20 +41,20 @@ impl WRevoluteVelocityConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdFloat>::from(
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -115,7 +115,7 @@ impl WRevoluteVelocityConstraint {
basis1,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
inv_lhs,
rhs,
r1,
@@ -231,17 +231,17 @@ pub(crate) struct WRevoluteVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH],
r2: Vector<SimdFloat>,
r2: Vector<SimdReal>,
inv_lhs: Matrix5<SimdFloat>,
rhs: Vector5<SimdFloat>,
impulse: Vector5<SimdFloat>,
inv_lhs: Matrix5<SimdReal>,
rhs: Vector5<SimdReal>,
impulse: Vector5<SimdReal>,
basis1: Matrix3x2<SimdFloat>,
basis1: Matrix3x2<SimdReal>,
im2: SimdFloat,
im2: SimdReal,
ii2_sqrt: AngularInertia<SimdFloat>,
ii2_sqrt: AngularInertia<SimdReal>,
}
impl WRevoluteVelocityGroundConstraint {
@@ -255,15 +255,15 @@ impl WRevoluteVelocityGroundConstraint {
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdFloat>::from(
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -322,7 +322,7 @@ impl WRevoluteVelocityGroundConstraint {
mj_lambda2,
im2,
ii2_sqrt,
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
impulse: impulse * SimdReal::splat(params.warmstart_coeff),
basis1,
inv_lhs,
rhs,

48
src/dynamics/solver/position_constraint_wide.rs
View File

@@ -2,7 +2,7 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, KinematicsCategory};
use crate::math::{
AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -14,16 +14,16 @@ pub(crate) struct WPositionConstraint {
pub rb1: [usize; SIMD_WIDTH],
pub rb2: [usize; SIMD_WIDTH],
// NOTE: the points are relative to the center of masses.
pub local_p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<SimdFloat>,
pub radius: SimdFloat,
pub im1: SimdFloat,
pub im2: SimdFloat,
pub ii1: AngularInertia<SimdFloat>,
pub ii2: AngularInertia<SimdFloat>,
pub erp: SimdFloat,
pub max_linear_correction: SimdFloat,
pub local_p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im1: SimdReal,
pub im2: SimdReal,
pub ii1: AngularInertia<SimdReal>,
pub ii2: AngularInertia<SimdReal>,
pub erp: SimdReal,
pub max_linear_correction: SimdReal,
pub num_contacts: u8,
}
@@ -38,18 +38,18 @@ impl WPositionConstraint {
let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdFloat> =
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
@@ -57,7 +57,7 @@ impl WPositionConstraint {
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/;
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -74,8 +74,8 @@ impl WPositionConstraint {
im2,
ii1: sqrt_ii1.squared(),
ii2: sqrt_ii2.squared(),
erp: SimdFloat::splat(params.erp),
max_linear_correction: SimdFloat::splat(params.max_linear_correction),
erp: SimdReal::splat(params.erp),
max_linear_correction: SimdReal::splat(params.max_linear_correction),
num_contacts: num_points as u8,
};
@@ -119,7 +119,7 @@ impl WPositionConstraint {
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -133,7 +133,7 @@ impl WPositionConstraint {
let dist = sqdist.simd_sqrt();
let n = dpos / dist;
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp1 = p1.coords - pos1.translation.vector;
let dp2 = p2.coords - pos2.translation.vector;
@@ -173,7 +173,7 @@ impl WPositionConstraint {
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -188,7 +188,7 @@ impl WPositionConstraint {
// NOTE: only works for the point-point case.
let p1 = p2 - n1 * dist;
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp1 = p1.coords - pos1.translation.vector;
let dp2 = p2.coords - pos2.translation.vector;

40
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -2,7 +2,7 @@ use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, KinematicsCategory};
use crate::math::{
AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -13,14 +13,14 @@ use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
pub(crate) struct WPositionGroundConstraint {
pub rb2: [usize; SIMD_WIDTH],
// NOTE: the points are relative to the center of masses.
pub p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
pub n1: Vector<SimdFloat>,
pub radius: SimdFloat,
pub im2: SimdFloat,
pub ii2: AngularInertia<SimdFloat>,
pub erp: SimdFloat,
pub max_linear_correction: SimdFloat,
pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im2: SimdReal,
pub ii2: AngularInertia<SimdReal>,
pub erp: SimdReal,
pub max_linear_correction: SimdReal,
pub num_contacts: u8,
}
@@ -45,8 +45,8 @@ impl WPositionGroundConstraint {
}
}
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from(
@@ -63,15 +63,15 @@ impl WPositionGroundConstraint {
array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let coll_pos1 =
delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/;
let n1 = coll_pos1 * local_n1;
@@ -87,8 +87,8 @@ impl WPositionGroundConstraint {
radius,
im2,
ii2: sqrt_ii2.squared(),
erp: SimdFloat::splat(params.erp),
max_linear_correction: SimdFloat::splat(params.max_linear_correction),
erp: SimdReal::splat(params.erp),
max_linear_correction: SimdReal::splat(params.max_linear_correction),
num_contacts: num_points as u8,
};
@@ -131,7 +131,7 @@ impl WPositionGroundConstraint {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -145,7 +145,7 @@ impl WPositionGroundConstraint {
let dist = sqdist.simd_sqrt();
let n = dpos / dist;
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp2 = p2.coords - pos2.translation.vector;
let gcross2 = -dp2.gcross(n);
let ii_gcross2 = self.ii2.transform_vector(gcross2);
@@ -173,7 +173,7 @@ impl WPositionGroundConstraint {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
@@ -186,7 +186,7 @@ impl WPositionGroundConstraint {
// NOTE: this condition does not seem to be useful perfomancewise?
if dist.simd_lt(target_dist).any() {
let err = ((dist - target_dist) * self.erp)
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
.simd_clamp(-self.max_linear_correction, SimdReal::zero());
let dp2 = p2.coords - pos2.translation.vector;
let gcross2 = -dp2.gcross(n1);

67
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -11,11 +11,11 @@ use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityConstraintElementPart {
pub gcross1: AngVector<SimdFloat>,
pub gcross2: AngVector<SimdFloat>,
pub rhs: SimdFloat,
pub impulse: SimdFloat,
pub r: SimdFloat,
pub gcross1: AngVector<SimdReal>,
pub gcross2: AngVector<SimdReal>,
pub rhs: SimdReal,
pub impulse: SimdReal,
pub r: SimdReal,
}
impl WVelocityConstraintElementPart {
@@ -23,9 +23,9 @@ impl WVelocityConstraintElementPart {
Self {
gcross1: AngVector::zero(),
gcross2: AngVector::zero(),
rhs: SimdFloat::zero(),
impulse: SimdFloat::zero(),
r: SimdFloat::zero(),
rhs: SimdReal::zero(),
impulse: SimdReal::zero(),
r: SimdReal::zero(),
}
}
}
@@ -47,12 +47,12 @@ impl WVelocityConstraintElement {
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityConstraint {
pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im1: SimdFloat,
pub im2: SimdFloat,
pub limit: SimdFloat,
pub im1: SimdReal,
pub im2: SimdReal,
pub limit: SimdReal,
pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -68,29 +68,29 @@ impl WVelocityConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let inv_dt = SimdReal::splat(params.inv_dt());
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdFloat> =
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
@@ -103,14 +103,13 @@ impl WVelocityConstraint {
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -137,10 +136,10 @@ impl WVelocityConstraint {
let p2 = coll_pos2
* Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -153,13 +152,13 @@ impl WVelocityConstraint {
let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdFloat::splat(1.0)
let r = SimdReal::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
let mut rhs = (vel1 - vel2).dot(&force_dir1);
let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
let rhs_with_restitution = rhs + rhs * restitution;
rhs = rhs_with_restitution.select(use_restitution, rhs);
rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
constraint.elements[k].normal_part = WVelocityConstraintElementPart {
gcross1,
@@ -175,17 +174,17 @@ impl WVelocityConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdFloat::splat(1.0)
let r = SimdReal::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
@@ -309,7 +308,7 @@ impl WVelocityConstraint {
- self.dir1.dot(&mj_lambda2.linear)
+ elt.gcross2.gdot(mj_lambda2.angular)
+ elt.rhs;
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero());
let dlambda = new_impulse - elt.impulse;
elt.impulse = new_impulse;

59
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -2,7 +2,7 @@ use super::{AnyVelocityConstraint, DeltaVel};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
AngVector, AngularInertia, Isometry, Point, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -11,19 +11,19 @@ use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityGroundConstraintElementPart {
pub gcross2: AngVector<SimdFloat>,
pub rhs: SimdFloat,
pub impulse: SimdFloat,
pub r: SimdFloat,
pub gcross2: AngVector<SimdReal>,
pub rhs: SimdReal,
pub impulse: SimdReal,
pub r: SimdReal,
}
impl WVelocityGroundConstraintElementPart {
pub fn zero() -> Self {
Self {
gcross2: AngVector::zero(),
rhs: SimdFloat::zero(),
impulse: SimdFloat::zero(),
r: SimdFloat::zero(),
rhs: SimdReal::zero(),
impulse: SimdReal::zero(),
r: SimdReal::zero(),
}
}
}
@@ -45,11 +45,11 @@ impl WVelocityGroundConstraintElement {
#[derive(Copy, Clone, Debug)]
pub(crate) struct WVelocityGroundConstraint {
pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: SimdFloat,
pub limit: SimdFloat,
pub im2: SimdReal,
pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: usize,
@@ -64,7 +64,7 @@ impl WVelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let inv_dt = SimdReal::splat(params.inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
@@ -76,15 +76,15 @@ impl WVelocityGroundConstraint {
}
}
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdFloat> =
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
@@ -109,14 +109,13 @@ impl WVelocityGroundConstraint {
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -143,10 +142,10 @@ impl WVelocityGroundConstraint {
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
);
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -157,12 +156,12 @@ impl WVelocityGroundConstraint {
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let mut rhs = (vel1 - vel2).dot(&force_dir1);
let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
let rhs_with_restitution = rhs + rhs * restitution;
rhs = rhs_with_restitution.select(use_restitution, rhs);
rhs += dist.simd_max(SimdFloat::zero()) * inv_dt;
rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
gcross2,
@@ -177,16 +176,16 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
constraint.elements[k].tangent_parts[j] =
@@ -274,7 +273,7 @@ impl WVelocityGroundConstraint {
let elt = &mut self.elements[i].normal_part;
let dimpulse =
-self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero());
let dlambda = new_impulse - elt.impulse;
elt.impulse = new_impulse;

8
src/geometry/ball.rs
View File

@@ -1,16 +1,16 @@
#[cfg(feature = "simd-is-enabled")]
use crate::math::{Point, SimdFloat};
use crate::math::{Point, SimdReal};
#[cfg(feature = "simd-is-enabled")]
#[derive(Copy, Clone, Debug)]
pub(crate) struct WBall {
pub center: Point<SimdFloat>,
pub radius: SimdFloat,
pub center: Point<SimdReal>,
pub radius: SimdReal,
}
#[cfg(feature = "simd-is-enabled")]
impl WBall {
pub fn new(center: Point<SimdFloat>, radius: SimdFloat) -> Self {
pub fn new(center: Point<SimdReal>, radius: SimdReal) -> Self {
WBall { center, radius }
}
}

18
src/geometry/collider.rs
View File

@@ -1,12 +1,13 @@
use crate::buckler::shape::HalfSpace;
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
use crate::geometry::{
Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle,
Trimesh,
};
#[cfg(feature = "dim3")]
use crate::geometry::{Cone, Cylinder, RoundCylinder};
use crate::geometry::InteractionGroups;
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use buckler::bounding_volume::AABB;
use buckler::shape::{
Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle,
};
#[cfg(feature = "dim3")]
use buckler::shape::{Cone, Cylinder, RoundCylinder};
use na::Point3;
use std::ops::Deref;
use std::sync::Arc;
@@ -77,7 +78,7 @@ impl ColliderShape {
/// Initializes a triangle mesh shape defined by its vertex and index buffers.
pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
ColliderShape(Arc::new(Trimesh::new(vertices, indices)))
ColliderShape(Arc::new(TriMesh::new(vertices, indices)))
}
/// Initializes an heightfield shape defined by its set of height and a scale
@@ -165,8 +166,9 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
Some(ShapeType::Capsule) => deser::<A, Capsule>(&mut seq)?,
Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
Some(ShapeType::Segment) => deser::<A, Segment>(&mut seq)?,
Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
Some(ShapeType::TriMesh) => deser::<A, TriMesh>(&mut seq)?,
Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
Some(ShapeType::HalfSpace) => deser::<A, HalfSpace>(&mut seq)?,
#[cfg(feature = "dim3")]
Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
#[cfg(feature = "dim3")]

12
src/geometry/contact.rs
View File

@@ -6,7 +6,7 @@ use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManif
use crate::math::{Isometry, Point, Vector};
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SimdFloat, SIMD_WIDTH},
crate::math::{SimdReal, SIMD_WIDTH},
simba::simd::SimdValue,
};
@@ -22,11 +22,11 @@ bitflags::bitflags! {
#[cfg(feature = "simd-is-enabled")]
pub(crate) struct WContact {
pub local_p1: Point<SimdFloat>,
pub local_p2: Point<SimdFloat>,
pub local_n1: Vector<SimdFloat>,
pub local_n2: Vector<SimdFloat>,
pub dist: SimdFloat,
pub local_p1: Point<SimdReal>,
pub local_p2: Point<SimdReal>,
pub local_n1: Vector<SimdReal>,
pub local_n2: Vector<SimdReal>,
pub dist: SimdReal,
pub fid1: [u8; SIMD_WIDTH],
pub fid2: [u8; SIMD_WIDTH],
}

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

@@ -5,12 +5,12 @@ use crate::math::{Point, Vector};
use {
crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd,
crate::geometry::{WBall, WContact},
crate::math::{Isometry, SimdFloat, SIMD_WIDTH},
crate::math::{Isometry, SimdReal, SIMD_WIDTH},
simba::simd::SimdValue,
};
#[cfg(feature = "simd-is-enabled")]
fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdFloat>) -> WContact {
fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdReal>) -> WContact {
let dcenter = ball2.center - ball1.center;
let center_dist = dcenter.magnitude();
let normal = dcenter / center_dist;
@@ -30,9 +30,9 @@ fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdFlo
pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) {
let pos_ba = ctxt.positions2.inverse() * ctxt.positions1;
let radii_a =
SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
let radii_b =
SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
let wball_a = WBall::new(Point::origin(), radii_a);
let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b);

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

@@ -3,9 +3,9 @@ use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
ContactGenerator, ContactGeneratorWorkspace, ContactPhase,
HeightFieldShapeContactGeneratorWorkspace, PrimitiveContactGenerator,
TrimeshShapeContactGeneratorWorkspace,
TriMeshShapeContactGeneratorWorkspace,
};
use crate::geometry::ShapeType;
use buckler::shape::ShapeType;
/// Trait implemented by structures responsible for selecting a collision-detection algorithm
/// for a given pair of shapes.
@@ -114,13 +114,13 @@ impl ContactDispatcher for DefaultContactDispatcher {
shape2: ShapeType,
) -> (ContactPhase, Option<ContactGeneratorWorkspace>) {
match (shape1, shape2) {
(ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => (
(ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_trimesh_shape,
..ContactGenerator::default()
}),
Some(ContactGeneratorWorkspace::from(
TrimeshShapeContactGeneratorWorkspace::new(),
TriMeshShapeContactGeneratorWorkspace::new(),
)),
),
(ShapeType::HeightField, _) | (_, ShapeType::HeightField) => (

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

@@ -1,11 +1,11 @@
use crate::data::MaybeSerializableData;
use crate::geometry::{
Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
SolverFlags,
ShapeType, SolverFlags,
};
use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::math::{SimdReal, SIMD_WIDTH};
use crate::pipeline::EventHandler;
#[derive(Copy, Clone)]
@@ -158,8 +158,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> {
pub colliders2: [&'a Collider; SIMD_WIDTH],
pub shapes1: [&'a dyn Shape; SIMD_WIDTH],
pub shapes2: [&'a dyn Shape; SIMD_WIDTH],
pub positions1: &'a Isometry<SimdFloat>,
pub positions2: &'a Isometry<SimdFloat>,
pub positions1: &'a Isometry<SimdReal>,
pub positions2: &'a Isometry<SimdReal>,
pub manifolds: &'a mut [&'b mut ContactManifold],
pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>],
}

6
src/geometry/contact_generator/contact_generator_workspace.rs
View File

@@ -2,7 +2,7 @@ use crate::data::MaybeSerializableData;
#[cfg(feature = "dim3")]
use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
HeightFieldShapeContactGeneratorWorkspace, TrimeshShapeContactGeneratorWorkspace,
HeightFieldShapeContactGeneratorWorkspace, TriMeshShapeContactGeneratorWorkspace,
WorkspaceSerializationTag,
};
@@ -81,8 +81,8 @@ impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace {
Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => {
deser::<A, HeightFieldShapeContactGeneratorWorkspace>(&mut seq)?
}
Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => {
deser::<A, TrimeshShapeContactGeneratorWorkspace>(&mut seq)?
Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => {
deser::<A, TriMeshShapeContactGeneratorWorkspace>(&mut seq)?
}
#[cfg(feature = "dim3")]
Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => {

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

@@ -5,9 +5,10 @@ use crate::geometry::contact_generator::{
ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext,
PrimitiveContactGenerator,
};
use crate::geometry::{Collider, ContactManifold, ContactManifoldData};
#[cfg(feature = "dim2")]
use crate::geometry::Capsule;
use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape};
use buckler::shape::Capsule;
use buckler::shape::{HeightField, Shape};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
@@ -95,7 +96,7 @@ fn do_generate_contacts(
#[cfg(feature = "dim3")]
let sub_shape1 = *part1;
let sub_detector = match workspace.sub_detectors.entry(i) {
let sub_detector = match workspace.sub_detectors.entry(i as usize) {
Entry::Occupied(entry) => {
let sub_detector = entry.into_mut();
let manifold = workspace.old_manifolds[sub_detector.manifold_id].take();
@@ -119,7 +120,7 @@ fn do_generate_contacts(
collider2,
solver_flags,
);
manifolds.push(ContactManifold::with_data((i, 0), manifold_data));
manifolds.push(ContactManifold::with_data((i as usize, 0), manifold_data));
entry.insert(sub_detector)
}

2
src/geometry/contact_generator/mod.rs
View File

@@ -25,7 +25,7 @@ pub use self::pfm_pfm_contact_generator::{
// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
pub use self::contact_generator_workspace::ContactGeneratorWorkspace;
pub use self::trimesh_shape_contact_generator::{
generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
generate_contacts_trimesh_shape, TriMeshShapeContactGeneratorWorkspace,
};
pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag;

2
src/geometry/contact_generator/serializable_workspace_tag.rs
View File

@@ -2,7 +2,7 @@ use num_derive::FromPrimitive;
#[derive(Copy, Clone, Debug, FromPrimitive)]
pub(super) enum WorkspaceSerializationTag {
TrimeshShapeContactGeneratorWorkspace = 0,
TriMeshShapeContactGeneratorWorkspace = 0,
#[cfg(feature = "dim3")]
PfmPfmContactGeneratorWorkspace,
HeightfieldShapeContactGeneratorWorkspace,

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

@@ -3,21 +3,21 @@ use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext,
};
use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, Trimesh};
use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, TriMesh};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct TrimeshShapeContactGeneratorWorkspace {
interferences: Vec<usize>,
pub struct TriMeshShapeContactGeneratorWorkspace {
interferences: Vec<u32>,
local_aabb2: AABB,
old_interferences: Vec<usize>,
old_interferences: Vec<u32>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
}
impl TrimeshShapeContactGeneratorWorkspace {
impl TriMeshShapeContactGeneratorWorkspace {
pub fn new() -> Self {
Self {
interferences: Vec::new(),
@@ -40,7 +40,7 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) {
}
fn do_generate_contacts(
trimesh1: &Trimesh,
trimesh1: &TriMesh,
collider1: &Collider,
collider2: &Collider,
ctxt: &mut ContactGenerationContext,
@@ -52,14 +52,14 @@ fn do_generate_contacts(
ctxt.pair.pair
};
let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt
let workspace: &mut TriMeshShapeContactGeneratorWorkspace = ctxt
.pair
.generator_workspace
.as_mut()
.expect("The TrimeshShapeContactGeneratorWorkspace is missing.")
.expect("The TriMeshShapeContactGeneratorWorkspace is missing.")
.0
.downcast_mut()
.expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace.");
.expect("Invalid workspace type, expected a TriMeshShapeContactGeneratorWorkspace.");
/*
* Compute interferences.
@@ -97,9 +97,9 @@ fn do_generate_contacts(
.iter()
.map(|manifold| {
if manifold.data.pair.collider1 == ctxt_collider1 {
manifold.subshape_index_pair.0
manifold.subshape_index_pair.0 as u32
} else {
manifold.subshape_index_pair.1
manifold.subshape_index_pair.1 as u32
}
})
.collect();
@@ -118,7 +118,7 @@ fn do_generate_contacts(
workspace.interferences.clear();
trimesh1
.waabbs()
.quadtree()
.intersect_aabb(&local_aabb2, &mut workspace.interferences);
workspace.local_aabb2 = local_aabb2;
}
@@ -134,7 +134,7 @@ fn do_generate_contacts(
// TODO: don't redispatch at each frame (we should probably do the same as
// the heightfield).
for (i, triangle_id) in new_interferences.iter().enumerate() {
if *triangle_id >= trimesh1.num_triangles() {
if *triangle_id >= trimesh1.num_triangles() as u32 {
// Because of SIMD padding, the broad-phase may return tiangle indices greater
// than the max.
continue;
@@ -160,7 +160,7 @@ fn do_generate_contacts(
ctxt.solver_flags,
);
ContactManifold::with_data((*triangle_id, 0), data)
ContactManifold::with_data((*triangle_id as usize, 0), data)
} else {
// We already have a manifold for this triangle.
old_inter_it.next();
@@ -206,11 +206,11 @@ fn do_generate_contacts(
}
}
impl MaybeSerializableData for TrimeshShapeContactGeneratorWorkspace {
impl MaybeSerializableData for TriMeshShapeContactGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace as u32,
super::WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace as u32,
self,
))
}

14
src/geometry/mod.rs
View File

@@ -13,9 +13,6 @@ pub use self::narrow_phase::NarrowPhase;
pub use self::polygon::Polygon;
pub use self::proximity::ProximityPair;
pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher};
#[cfg(feature = "dim3")]
pub use self::round_cylinder::RoundCylinder;
pub use self::trimesh::Trimesh;
pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter};
pub use buckler::query::Proximity;
@@ -106,11 +103,10 @@ pub(crate) use self::collider_set::RemovedCollider;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::contact::WContact;
pub(crate) use self::narrow_phase::ContactManifoldIndex;
pub(crate) use self::waabb::{WRay, WAABB};
pub(crate) use self::wquadtree::WQuadtree;
pub(crate) use buckler::partitioning::WQuadtree;
//pub(crate) use self::z_order::z_cmp_floats;
pub use self::interaction_groups::InteractionGroups;
pub use self::shape::{Shape, ShapeType};
pub use buckler::shape::*;
mod ball;
mod broad_phase_multi_sap;
@@ -125,12 +121,6 @@ mod proximity;
mod proximity_detector;
pub(crate) mod sat;
pub(crate) mod triangle;
mod trimesh;
mod waabb;
mod wquadtree;
//mod z_order;
mod interaction_groups;
#[cfg(feature = "dim3")]
mod round_cylinder;
mod shape;
mod user_callbacks;

2
src/geometry/narrow_phase.rs
View File

@@ -20,7 +20,7 @@ use crate::geometry::{
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
//#[cfg(feature = "simd-is-enabled")]
//use crate::math::{SimdFloat, SIMD_WIDTH};
//use crate::math::{SimdReal, SIMD_WIDTH};
use crate::buckler::query::Proximity;
use crate::data::pubsub::Subscription;
use crate::data::Coarena;

8
src/geometry/proximity_detector/ball_ball_proximity_detector.rs
View File

@@ -5,12 +5,12 @@ use crate::math::Point;
#[cfg(feature = "simd-is-enabled")]
use {
crate::geometry::{proximity_detector::PrimitiveProximityDetectionContextSimd, WBall},
crate::math::{SimdFloat, SIMD_WIDTH},
crate::math::{SimdReal, SIMD_WIDTH},
simba::simd::SimdValue,
};
#[cfg(feature = "simd-is-enabled")]
fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdFloat {
fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdReal {
let dcenter = ball2.center - ball1.center;
let center_dist = dcenter.magnitude();
center_dist - ball1.radius - ball2.radius
@@ -22,9 +22,9 @@ pub fn detect_proximity_ball_ball_simd(
) -> [Proximity; SIMD_WIDTH] {
let pos_ba = ctxt.positions2.inverse() * ctxt.positions1;
let radii_a =
SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
let radii_b =
SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
let wball_a = WBall::new(Point::origin(), radii_a);
let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b);

2
src/geometry/proximity_detector/mod.rs
View File

@@ -15,7 +15,7 @@ pub use self::proximity_detector::{
};
pub use self::proximity_dispatcher::{DefaultProximityDispatcher, ProximityDispatcher};
pub use self::trimesh_shape_proximity_detector::{
detect_proximity_trimesh_shape, TrimeshShapeProximityDetectorWorkspace,
detect_proximity_trimesh_shape, TriMeshShapeProximityDetectorWorkspace,
};
mod ball_ball_proximity_detector;

6
src/geometry/proximity_detector/proximity_detector.rs
View File

@@ -3,7 +3,7 @@ use crate::geometry::{
};
use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::math::{SimdReal, SIMD_WIDTH};
use crate::pipeline::EventHandler;
use std::any::Any;
@@ -134,8 +134,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> {
pub colliders2: [&'a Collider; SIMD_WIDTH],
pub shapes1: [&'a dyn Shape; SIMD_WIDTH],
pub shapes2: [&'a dyn Shape; SIMD_WIDTH],
pub positions1: &'a Isometry<SimdFloat>,
pub positions2: &'a Isometry<SimdFloat>,
pub positions1: &'a Isometry<SimdReal>,
pub positions2: &'a Isometry<SimdReal>,
pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],
}

10
src/geometry/proximity_detector/proximity_dispatcher.rs
View File

@@ -1,6 +1,6 @@
use crate::geometry::proximity_detector::{
PrimitiveProximityDetector, ProximityDetector, ProximityPhase,
TrimeshShapeProximityDetectorWorkspace,
TriMeshShapeProximityDetectorWorkspace,
};
use crate::geometry::ShapeType;
use std::any::Any;
@@ -113,19 +113,19 @@ impl ProximityDispatcher for DefaultProximityDispatcher {
shape2: ShapeType,
) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>) {
match (shape1, shape2) {
(ShapeType::Trimesh, _) => (
(ShapeType::TriMesh, _) => (
ProximityPhase::NearPhase(ProximityDetector {
detect_proximity: super::detect_proximity_trimesh_shape,
..ProximityDetector::default()
}),
Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())),
Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())),
),
(_, ShapeType::Trimesh) => (
(_, ShapeType::TriMesh) => (
ProximityPhase::NearPhase(ProximityDetector {
detect_proximity: super::detect_proximity_trimesh_shape,
..ProximityDetector::default()
}),
Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())),
Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())),
),
_ => {
let (gen, workspace) = self.dispatch_primitives(shape1, shape2);

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

@@ -2,15 +2,15 @@ use crate::buckler::bounding_volume::{BoundingVolume, AABB};
use crate::geometry::proximity_detector::{
PrimitiveProximityDetectionContext, ProximityDetectionContext,
};
use crate::geometry::{Collider, Proximity, ShapeType, Trimesh};
use crate::geometry::{Collider, Proximity, ShapeType, TriMesh};
pub struct TrimeshShapeProximityDetectorWorkspace {
interferences: Vec<usize>,
pub struct TriMeshShapeProximityDetectorWorkspace {
interferences: Vec<u32>,
local_aabb2: AABB,
old_interferences: Vec<usize>,
old_interferences: Vec<u32>,
}
impl TrimeshShapeProximityDetectorWorkspace {
impl TriMeshShapeProximityDetectorWorkspace {
pub fn new() -> Self {
Self {
interferences: Vec::new(),
@@ -34,18 +34,18 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P
}
fn do_detect_proximity(
trimesh1: &Trimesh,
trimesh1: &TriMesh,
collider1: &Collider,
collider2: &Collider,
ctxt: &mut ProximityDetectionContext,
) -> Proximity {
let workspace: &mut TrimeshShapeProximityDetectorWorkspace = ctxt
let workspace: &mut TriMeshShapeProximityDetectorWorkspace = ctxt
.pair
.detector_workspace
.as_mut()
.expect("The TrimeshShapeProximityDetectorWorkspace is missing.")
.expect("The TriMeshShapeProximityDetectorWorkspace is missing.")
.downcast_mut()
.expect("Invalid workspace type, expected a TrimeshShapeProximityDetectorWorkspace.");
.expect("Invalid workspace type, expected a TriMeshShapeProximityDetectorWorkspace.");
/*
* Compute interferences.
@@ -72,7 +72,7 @@ fn do_detect_proximity(
workspace.interferences.clear();
trimesh1
.waabbs()
.quadtree()
.intersect_aabb(&local_aabb2, &mut workspace.interferences);
workspace.local_aabb2 = local_aabb2;
}
@@ -86,7 +86,7 @@ fn do_detect_proximity(
let shape_type2 = collider2.shape().shape_type();
for triangle_id in new_interferences.iter() {
if *triangle_id >= trimesh1.num_triangles() {
if *triangle_id >= trimesh1.num_triangles() as u32 {
// Because of SIMD padding, the broad-phase may return tiangle indices greater
// than the max.
continue;

92
src/geometry/round_cylinder.rs
View File

@@ -1,92 +0,0 @@
use crate::geometry::Cylinder;
use crate::math::{Isometry, Point, Vector};
use buckler::query::{
gjk::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
};
use buckler::shape::{FeatureId, SupportMap};
use na::Unit;
/// A rounded cylinder.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug)]
pub struct RoundCylinder {
/// The cylinder being rounded.
pub cylinder: Cylinder,
/// The rounding radius.
pub border_radius: f32,
}
impl RoundCylinder {
/// Create sa new cylinder where all its edges and vertices are rounded by a radius of `radius`.
///
/// This is done by applying a dilation of the given radius to the cylinder.
pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self {
Self {
cylinder: Cylinder::new(half_height, radius),
border_radius,
}
}
}
impl SupportMap for RoundCylinder {
fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
self.local_support_point_toward(&Unit::new_normalize(*dir))
}
fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius
}
fn support_point(&self, transform: &Isometry<f32>, dir: &Vector<f32>) -> Point<f32> {
let local_dir = transform.inverse_transform_vector(dir);
transform * self.local_support_point(&local_dir)
}
fn support_point_toward(
&self,
transform: &Isometry<f32>,
dir: &Unit<Vector<f32>>,
) -> Point<f32> {
let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir));
transform * self.local_support_point_toward(&local_dir)
}
}
impl RayCast for RoundCylinder {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection> {
buckler::query::details::local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
ray,
max_toi,
solid,
)
}
}
// TODO: if PointQuery had a `project_point_with_normal` method, we could just
// call this and adjust the projected point accordingly.
impl PointQuery for RoundCylinder {
#[inline]
fn project_local_point(&self, point: &Point<f32>, solid: bool) -> PointProjection {
buckler::query::details::local_point_projection_on_support_map(
self,
&mut VoronoiSimplex::new(),
point,
solid,
)
}
#[inline]
fn project_local_point_and_get_feature(
&self,
point: &Point<f32>,
) -> (PointProjection, FeatureId) {
(self.project_local_point(point, false), FeatureId::Unknown)
}
}

393
src/geometry/shape.rs
View File

@@ -1,393 +0,0 @@
use crate::dynamics::MassProperties;
use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh};
use crate::math::Isometry;
use buckler::bounding_volume::AABB;
use buckler::query::{PointQuery, RayCast};
use downcast_rs::{impl_downcast, DowncastSync};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
use num::Zero;
use num_derive::FromPrimitive;
#[cfg(feature = "dim3")]
use {
crate::geometry::{Cone, Cylinder, RoundCylinder},
buckler::bounding_volume::BoundingVolume,
buckler::shape::PolygonalFeatureMap,
};
#[derive(Copy, Clone, Debug, FromPrimitive)]
/// Enum representing the type of a shape.
pub enum ShapeType {
/// A ball shape.
Ball = 0,
/// A convex polygon shape.
Polygon,
/// A cuboid shape.
Cuboid,
/// A capsule shape.
Capsule,
/// A segment shape.
Segment,
/// A triangle shape.
Triangle,
/// A triangle mesh shape.
Trimesh,
/// A heightfield shape.
HeightField,
#[cfg(feature = "dim3")]
/// A cylindrical shape.
Cylinder,
#[cfg(feature = "dim3")]
/// A cylindrical shape.
Cone,
// /// A custom shape type.
// Custom(u8),
// /// A cuboid with rounded corners.
// RoundedCuboid,
// /// A triangle with rounded corners.
// RoundedTriangle,
// /// A triangle-mesh with rounded corners.
// RoundedTrimesh,
// /// An heightfield with rounded corners.
// RoundedHeightField,
/// A cylinder with rounded corners.
#[cfg(feature = "dim3")]
RoundCylinder,
// /// A cone with rounded corners.
// RoundedCone,
}
/// Trait implemented by shapes usable by Rapier.
pub trait Shape: RayCast + PointQuery + DowncastSync {
/// Convert this shape as a serializable entity.
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
None
}
// TODO: add a compute_local_aabb method?
/// Computes the AABB of this shape.
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB;
/// Compute the mass-properties of this shape given its uniform density.
fn mass_properties(&self, density: f32) -> MassProperties;
/// Gets the type tag of this shape.
fn shape_type(&self) -> ShapeType;
/// Converts this shape to a polygonal feature-map, if it is one.
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
None
}
// fn as_rounded(&self) -> Option<&Rounded<Box<AnyShape>>> {
// None
// }
}
impl_downcast!(sync Shape);
impl dyn Shape {
/// Converts this abstract shape to a ball, if it is one.
pub fn as_ball(&self) -> Option<&Ball> {
self.downcast_ref()
}
/// Converts this abstract shape to a cuboid, if it is one.
pub fn as_cuboid(&self) -> Option<&Cuboid> {
self.downcast_ref()
}
/// Converts this abstract shape to a capsule, if it is one.
pub fn as_capsule(&self) -> Option<&Capsule> {
self.downcast_ref()
}
/// Converts this abstract shape to a triangle, if it is one.
pub fn as_triangle(&self) -> Option<&Triangle> {
self.downcast_ref()
}
/// Converts this abstract shape to a triangle mesh, if it is one.
pub fn as_trimesh(&self) -> Option<&Trimesh> {
self.downcast_ref()
}
/// Converts this abstract shape to a heightfield, if it is one.
pub fn as_heightfield(&self) -> Option<&HeightField> {
self.downcast_ref()
}
/// Converts this abstract shape to a cylinder, if it is one.
#[cfg(feature = "dim3")]
pub fn as_cylinder(&self) -> Option<&Cylinder> {
self.downcast_ref()
}
/// Converts this abstract shape to a cone, if it is one.
#[cfg(feature = "dim3")]
pub fn as_cone(&self) -> Option<&Cone> {
self.downcast_ref()
}
/// Converts this abstract shape to a cone, if it is one.
#[cfg(feature = "dim3")]
pub fn as_round_cylinder(&self) -> Option<&RoundCylinder> {
self.downcast_ref()
}
}
impl Shape for Ball {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_ball(density, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Ball
}
}
// impl Shape for Polygon {
// #[cfg(feature = "serde-serialize")]
// fn as_serialize(&self) -> Option<&dyn Serialize> {
// Some(self as &dyn Serialize)
// }
//
// fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
// self.aabb(position)
// }
//
// fn mass_properties(&self, _density: f32) -> MassProperties {
// unimplemented!()
// }
//
// fn shape_type(&self) -> ShapeType {
// ShapeType::Polygon
// }
// }
impl Shape for Cuboid {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_cuboid(density, self.half_extents)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Cuboid
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((self as &dyn PolygonalFeatureMap, 0.0))
}
}
impl Shape for Capsule {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Capsule
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((&self.segment as &dyn PolygonalFeatureMap, self.radius))
}
}
impl Shape for Triangle {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::Triangle
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((self as &dyn PolygonalFeatureMap, 0.0))
}
}
impl Shape for Segment {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::Segment
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((self as &dyn PolygonalFeatureMap, 0.0))
}
}
impl Shape for Trimesh {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::Trimesh
}
}
impl Shape for HeightField {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::HeightField
}
}
#[cfg(feature = "dim3")]
impl Shape for Cylinder {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_cylinder(density, self.half_height, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Cylinder
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((self as &dyn PolygonalFeatureMap, 0.0))
}
}
#[cfg(feature = "dim3")]
impl Shape for Cone {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_cone(density, self.half_height, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Cone
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((self as &dyn PolygonalFeatureMap, 0.0))
}
}
#[cfg(feature = "dim3")]
impl Shape for RoundCylinder {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.cylinder
.compute_aabb(position)
.loosened(self.border_radius)
}
fn mass_properties(&self, density: f32) -> MassProperties {
// We ignore the margin here.
self.cylinder.mass_properties(density)
}
fn shape_type(&self) -> ShapeType {
ShapeType::RoundCylinder
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
Some((
&self.cylinder as &dyn PolygonalFeatureMap,
self.border_radius,
))
}
}

203
src/geometry/trimesh.rs
View File

@@ -1,203 +0,0 @@
use crate::geometry::{
Cuboid, HeightField, PointProjection, Ray, RayIntersection, Triangle, WQuadtree,
};
use crate::math::{Isometry, Point};
use buckler::bounding_volume::AABB;
use buckler::query::{PointQuery, RayCast};
use buckler::shape::FeatureId;
use na::Point3;
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A triangle mesh.
pub struct Trimesh {
wquadtree: WQuadtree<usize>,
aabb: AABB,
vertices: Vec<Point<f32>>,
indices: Vec<Point3<u32>>,
}
impl Trimesh {
/// Creates a new triangle mesh from a vertex buffer and an index buffer.
pub fn new(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
assert!(
vertices.len() > 1,
"A triangle mesh must contain at least one point."
);
assert!(
indices.len() > 1,
"A triangle mesh must contain at least one triangle."
);
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_aabb();
(i, aabb)
});
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 {
wquadtree,
aabb,
vertices,
indices,
}
}
/// Compute the axis-aligned bounding box of this triangle mesh.
pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
self.aabb.transform_by(pos)
}
pub(crate) fn waabbs(&self) -> &WQuadtree<usize> {
&self.wquadtree
}
/// The number of triangles forming this mesh.
pub fn num_triangles(&self) -> usize {
self.indices.len()
}
/// An iterator through all the triangles of this mesh.
pub fn triangles(&self) -> impl Iterator<Item = Triangle> + '_ {
self.indices.iter().map(move |ids| {
Triangle::new(
self.vertices[ids.x as usize],
self.vertices[ids.y as usize],
self.vertices[ids.z as usize],
)
})
}
/// Get the `i`-th triangle of this mesh.
pub fn triangle(&self, i: usize) -> Triangle {
let idx = self.indices[i];
Triangle::new(
self.vertices[idx.x as usize],
self.vertices[idx.y as usize],
self.vertices[idx.z as usize],
)
}
/// The vertex buffer of this mesh.
pub fn vertices(&self) -> &[Point<f32>] {
&self.vertices[..]
}
/// The index buffer of this mesh.
pub fn indices(&self) -> &[Point3<u32>] {
&self.indices
}
/// A flat view of the index buffer of this mesh.
pub fn flat_indices(&self) -> &[u32] {
unsafe {
let len = self.indices.len() * 3;
let data = self.indices.as_ptr() as *const u32;
std::slice::from_raw_parts(data, len)
}
}
}
impl PointQuery for Trimesh {
fn project_local_point(&self, _pt: &Point<f32>, _solid: bool) -> PointProjection {
// TODO
unimplemented!()
}
fn project_local_point_and_get_feature(
&self,
_pt: &Point<f32>,
) -> (PointProjection, FeatureId) {
// TODO
unimplemented!()
}
}
#[cfg(feature = "dim2")]
impl RayCast for Trimesh {
fn cast_local_ray_and_get_normal(
&self,
_ray: &Ray,
_max_toi: f32,
_solid: bool,
) -> Option<RayIntersection> {
// TODO
None
}
fn intersects_ray(&self, _m: &Isometry<f32>, _ray: &Ray, _max_toi: f32) -> bool {
// TODO
false
}
}
#[cfg(feature = "dim3")]
impl RayCast for Trimesh {
fn cast_local_ray_and_get_normal(
&self,
ray: &Ray,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection> {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
self.wquadtree.cast_ray(&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.cast_local_ray_and_get_normal(ray, max_toi, solid) {
if let Some(curr) = &mut best {
if curr.toi > inter.toi {
*curr = inter;
}
} else {
best = Some(inter);
}
}
}
best
}
fn intersects_local_ray(&self, ray: &Ray, max_toi: f32) -> bool {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
self.wquadtree.cast_ray(&ray, max_toi, &mut intersections);
for inter in intersections {
let tri = self.triangle(inter);
if tri.intersects_local_ray(ray, max_toi) {
return true;
}
}
false
}
}
#[cfg(feature = "dim3")]
impl From<HeightField> for Trimesh {
fn from(heightfield: HeightField) -> Self {
let (vtx, idx) = heightfield.to_trimesh();
Trimesh::new(vtx, idx)
}
}
#[cfg(feature = "dim3")]
impl From<Cuboid> for Trimesh {
fn from(cuboid: Cuboid) -> Self {
let (vtx, idx) = cuboid.to_trimesh();
Trimesh::new(vtx, idx)
}
}

217
src/geometry/waabb.rs
View File

@@ -1,217 +0,0 @@
use crate::geometry::Ray;
use crate::math::{Point, Vector, DIM, SIMD_WIDTH};
use crate::utils;
use buckler::bounding_volume::AABB;
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 {
pub mins: Point<SimdFloat>,
pub maxs: Point<SimdFloat>,
}
#[cfg(feature = "serde-serialize")]
impl serde::Serialize for WAABB {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
use serde::ser::SerializeStruct;
let mins: Point<[f32; SIMD_WIDTH]> = Point::from(
self.mins
.coords
.map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
);
let maxs: Point<[f32; SIMD_WIDTH]> = Point::from(
self.maxs
.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)?;
waabb.end()
}
}
#[cfg(feature = "serde-serialize")]
impl<'de> serde::Deserialize<'de> for WAABB {
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: serde::Deserializer<'de>,
{
struct Visitor {};
impl<'de> serde::de::Visitor<'de> for Visitor {
type Value = WAABB;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(
formatter,
"two arrays containing at least {} floats",
SIMD_WIDTH * DIM * 2
)
}
fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
let mins: Point<[f32; SIMD_WIDTH]> = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
let maxs: Point<[f32; SIMD_WIDTH]> = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(1, &self))?;
let mins = Point::from(mins.coords.map(|e| SimdFloat::from(e)));
let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e)));
Ok(WAABB { mins, maxs })
}
}
deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {})
}
}
impl WAABB {
pub fn new_invalid() -> Self {
Self::splat(AABB::new_invalid())
}
pub fn splat(aabb: AABB) -> Self {
Self {
mins: Point::splat(aabb.mins),
maxs: Point::splat(aabb.maxs),
}
}
pub fn dilate_by_factor(&mut self, factor: SimdFloat) {
// If some of the AABBs on this WAABB are invalid,
// don't, dilate them.
let is_valid = self.mins.x.simd_le(self.maxs.x);
let factor = factor.select(is_valid, SimdFloat::zero());
// NOTE: we multiply each by factor instead of doing
// (maxs - mins) * factor. That's to avoid overflows (and
// therefore NaNs if this WAABB contains some invalid
// AABBs initialised with f32::MAX
let dilation = self.maxs * factor - self.mins * factor;
self.mins -= dilation;
self.maxs += dilation;
}
pub fn replace(&mut self, i: usize, aabb: AABB) {
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 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)
& other.mins.y.simd_le(self.maxs.y)
}
#[cfg(feature = "dim3")]
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)
& other.mins.y.simd_le(self.maxs.y)
& self.mins.z.simd_le(other.maxs.z)
& other.mins.z.simd_le(self.maxs.z)
}
pub fn to_merged_aabb(&self) -> AABB {
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; SIMD_WIDTH]> for WAABB {
fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self {
let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH];
WAABB {
mins: Point::from(mins),
maxs: Point::from(maxs),
}
}
}

587
src/geometry/wquadtree.rs
View File

@@ -1,587 +0,0 @@
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 buckler::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();
// 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] {
ilast -= 1;
indices.swap(icurr, ilast);
} else {
icurr += 1;
}
}
if icurr == 0 || icurr == indices.len() {
// We don't want to return one empty set. But
// this can happen if all the coordinates along the
// given dimension are equal.
// In this is the case, we just split in the middle.
let half = indices.len() / 2;
indices.split_at_mut(half)
} else {
indices.split_at_mut(icurr)
}
}
#[cfg(test)]
mod test {
use crate::geometry::{WQuadtree, AABB};
use crate::math::{Point, Vector};
#[test]
fn multiple_identical_AABB_stack_overflow() {
// A stack overflow was caused during the construction of the
// WAABB tree with more than four AABB with the same center.
let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into());
for k in 0..20 {
let mut tree = WQuadtree::new();
tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0);
}
}
}

140
src/lib.rs
View File

@@ -53,14 +53,6 @@ macro_rules! array(
#[allow(dead_code)]
fn create_arr<T>(mut callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] {
[callback(0usize), callback(1usize), callback(2usize), callback(3usize)]
// [callback(0usize), callback(1usize), callback(2usize), callback(3usize),
// callback(4usize), callback(5usize), callback(6usize), callback(7usize)]
// [callback(0usize), callback(1usize), callback(2usize), callback(3usize),
// callback(4usize), callback(5usize), callback(6usize), callback(7usize),
// callback(8usize), callback(9usize), callback(10usize), callback(11usize),
// callback(12usize), callback(13usize), callback(14usize), callback(15usize)]
}
create_arr($callback)
@@ -136,134 +128,4 @@ pub mod dynamics;
pub mod geometry;
pub mod pipeline;
pub mod utils;
#[cfg(feature = "dim2")]
/// Math primitives used throughout Rapier.
pub mod math {
pub use super::simd::*;
use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2};
/// The dimension of the physics simulated by this crate.
pub const DIM: usize = 2;
/// The maximum number of point a contact manifold can hold.
pub const MAX_MANIFOLD_POINTS: usize = 2;
/// The dimension of the physics simulated by this crate, given as a type-level-integer.
pub type Dim = U2;
/// The maximum number of angular degrees of freedom of a rigid body given as a type-level-integer.
pub type AngDim = U1;
/// A 2D isometry, i.e., a rotation followed by a translation.
pub type Isometry<N> = Isometry2<N>;
/// A 2D vector.
pub type Vector<N> = Vector2<N>;
/// A scalar used for angular velocity.
///
/// This is called `AngVector` for coherence with the 3D version of this crate.
pub type AngVector<N> = N;
/// A 2D point.
pub type Point<N> = Point2<N>;
/// A 2D rotation expressed as an unit complex number.
pub type Rotation<N> = UnitComplex<N>;
/// A 2D translation.
pub type Translation<N> = Translation2<N>;
/// The angular inertia of a rigid body.
pub type AngularInertia<N> = N;
/// The principal angular inertia of a rigid body.
pub type PrincipalAngularInertia<N> = N;
/// A matrix that represent the cross product with a given vector.
pub type CrossMatrix<N> = Vector2<N>;
/// A 2x2 matrix.
pub type Matrix<N> = Matrix2<N>;
/// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body.
pub type SpacialVector<N> = Vector3<N>;
/// A 2D symmetric-definite-positive matrix.
pub type SdpMatrix<N> = crate::utils::SdpMatrix2<N>;
}
#[cfg(feature = "dim3")]
/// Math primitives used throughout Rapier.
pub mod math {
pub use super::simd::*;
use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3};
/// The dimension of the physics simulated by this crate.
pub const DIM: usize = 3;
/// The maximum number of point a contact manifold can hold.
pub const MAX_MANIFOLD_POINTS: usize = 4;
/// The dimension of the physics simulated by this crate, given as a type-level-integer.
pub type Dim = U3;
/// The maximum number of angular degrees of freedom of a rigid body given as a type-level-integer.
pub type AngDim = U3;
/// A 3D isometry, i.e., a rotation followed by a translation.
pub type Isometry<N> = Isometry3<N>;
/// A 3D vector.
pub type Vector<N> = Vector3<N>;
/// An axis-angle vector used for angular velocity.
pub type AngVector<N> = Vector3<N>;
/// A 3D point.
pub type Point<N> = Point3<N>;
/// A 3D rotation expressed as an unit quaternion.
pub type Rotation<N> = UnitQuaternion<N>;
/// A 3D translation.
pub type Translation<N> = Translation3<N>;
/// The angular inertia of a rigid body.
pub type AngularInertia<N> = crate::utils::SdpMatrix3<N>;
/// The principal angular inertia of a rigid body.
pub type PrincipalAngularInertia<N> = Vector3<N>;
/// A matrix that represent the cross product with a given vector.
pub type CrossMatrix<N> = Matrix3<N>;
/// A 3x3 matrix.
pub type Matrix<N> = Matrix3<N>;
/// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body.
pub type SpacialVector<N> = Vector6<N>;
/// A 3D symmetric-definite-positive matrix.
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)]
#[cfg(feature = "simd-nightly")]
use simba::simd::{f32x16, f32x4, f32x8, m32x16, m32x4, m32x8, u8x16, u8x4, u8x8};
#[cfg(feature = "simd-stable")]
use simba::simd::{WideBoolF32x4, WideF32x4};
/// The number of lanes of a SIMD number.
pub const SIMD_WIDTH: usize = 4;
/// SIMD_WIDTH - 1
pub const SIMD_LAST_INDEX: usize = 3;
#[cfg(not(feature = "simd-nightly"))]
/// A SIMD float with SIMD_WIDTH lanes.
pub type SimdFloat = WideF32x4;
#[cfg(not(feature = "simd-nightly"))]
/// A SIMD bool with SIMD_WIDTH lanes.
pub type SimdBool = WideBoolF32x4;
#[cfg(feature = "simd-nightly")]
/// A SIMD float with SIMD_WIDTH lanes.
pub type SimdFloat = f32x4;
#[cfg(feature = "simd-nightly")]
/// A bool float with SIMD_WIDTH lanes.
pub type SimdBool = m32x4;
// pub const SIMD_WIDTH: usize = 8;
// pub const SIMD_LAST_INDEX: usize = 7;
// pub type SimdFloat = f32x8;
// pub type SimdBool = m32x8;
// pub const SIMD_WIDTH: usize = 16;
// pub const SIMD_LAST_INDEX: usize = 15;
// pub type SimdFloat = f32x16;
// pub type SimdBool = m32x16;
}
pub use buckler::math;

5
src/pipeline/query_pipeline.rs
View File

@@ -47,7 +47,10 @@ impl QueryPipeline {
}
}
self.quadtree.update(colliders, self.dilation_factor);
self.quadtree.update(
|handle| colliders[*handle].compute_aabb(),
self.dilation_factor,
);
}
/// Find the closest intersection between a ray and a set of collider.

632
src/utils.rs
View File

@@ -7,7 +7,7 @@ use simba::simd::SimdValue;
use std::ops::{Add, Mul};
use {
crate::simd::{SimdBool, SimdFloat},
crate::math::{AngularInertia, SimdBool, SimdReal},
na::SimdPartialOrd,
num::One,
};
@@ -32,16 +32,6 @@ 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
@@ -88,8 +78,8 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
}
}
impl WSign<SimdFloat> for SimdFloat {
fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
impl WSign<SimdReal> for SimdReal {
fn copy_sign_to(self, to: SimdReal) -> SimdReal {
to.simd_copysign(self)
}
}
@@ -112,7 +102,7 @@ impl WComponent for f32 {
}
}
impl WComponent for SimdFloat {
impl WComponent for SimdReal {
type Element = f32;
fn min_component(self) -> Self::Element {
@@ -328,22 +318,22 @@ impl WDot<f32> for f32 {
}
}
impl WCrossMatrix for Vector3<SimdFloat> {
type CrossMat = Matrix3<SimdFloat>;
impl WCrossMatrix for Vector3<SimdReal> {
type CrossMat = Matrix3<SimdReal>;
#[inline]
#[rustfmt::skip]
fn gcross_matrix(self) -> Self::CrossMat {
Matrix3::new(
SimdFloat::zero(), -self.z, self.y,
self.z, SimdFloat::zero(), -self.x,
-self.y, self.x, SimdFloat::zero(),
SimdReal::zero(), -self.z, self.y,
self.z, SimdReal::zero(), -self.x,
-self.y, self.x, SimdReal::zero(),
)
}
}
impl WCrossMatrix for Vector2<SimdFloat> {
type CrossMat = Vector2<SimdFloat>;
impl WCrossMatrix for Vector2<SimdReal> {
type CrossMat = Vector2<SimdReal>;
#[inline]
fn gcross_matrix(self) -> Self::CrossMat {
@@ -351,24 +341,24 @@ impl WCrossMatrix for Vector2<SimdFloat> {
}
}
impl WCross<Vector3<SimdFloat>> for Vector3<SimdFloat> {
type Result = Vector3<SimdFloat>;
impl WCross<Vector3<SimdReal>> for Vector3<SimdReal> {
type Result = Vector3<SimdReal>;
fn gcross(&self, rhs: Self) -> Self::Result {
self.cross(&rhs)
}
}
impl WCross<Vector2<SimdFloat>> for SimdFloat {
type Result = Vector2<SimdFloat>;
impl WCross<Vector2<SimdReal>> for SimdReal {
type Result = Vector2<SimdReal>;
fn gcross(&self, rhs: Vector2<SimdFloat>) -> Self::Result {
fn gcross(&self, rhs: Vector2<SimdReal>) -> Self::Result {
Vector2::new(-rhs.y * *self, rhs.x * *self)
}
}
impl WCross<Vector2<SimdFloat>> for Vector2<SimdFloat> {
type Result = SimdFloat;
impl WCross<Vector2<SimdReal>> for Vector2<SimdReal> {
type Result = SimdReal;
fn gcross(&self, rhs: Self) -> Self::Result {
let yx = Vector2::new(rhs.y, rhs.x);
@@ -377,26 +367,26 @@ impl WCross<Vector2<SimdFloat>> for Vector2<SimdFloat> {
}
}
impl WDot<Vector3<SimdFloat>> for Vector3<SimdFloat> {
type Result = SimdFloat;
impl WDot<Vector3<SimdReal>> for Vector3<SimdReal> {
type Result = SimdReal;
fn gdot(&self, rhs: Vector3<SimdFloat>) -> Self::Result {
fn gdot(&self, rhs: Vector3<SimdReal>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
}
}
impl WDot<Vector2<SimdFloat>> for Vector2<SimdFloat> {
type Result = SimdFloat;
impl WDot<Vector2<SimdReal>> for Vector2<SimdReal> {
type Result = SimdReal;
fn gdot(&self, rhs: Vector2<SimdFloat>) -> Self::Result {
fn gdot(&self, rhs: Vector2<SimdReal>) -> Self::Result {
self.x * rhs.x + self.y * rhs.y
}
}
impl WDot<SimdFloat> for SimdFloat {
type Result = SimdFloat;
impl WDot<SimdReal> for SimdReal {
type Result = SimdReal;
fn gdot(&self, rhs: SimdFloat) -> Self::Result {
fn gdot(&self, rhs: SimdReal) -> Self::Result {
*self * rhs
}
}
@@ -446,26 +436,26 @@ impl WAngularInertia<f32> for f32 {
}
}
impl WAngularInertia<SimdFloat> for SimdFloat {
type AngVector = SimdFloat;
type LinVector = Vector2<SimdFloat>;
type AngMatrix = SimdFloat;
impl WAngularInertia<SimdReal> for SimdReal {
type AngVector = SimdReal;
type LinVector = Vector2<SimdReal>;
type AngMatrix = SimdReal;
fn inverse(&self) -> Self {
let zero = <SimdFloat>::zero();
let zero = <SimdReal>::zero();
let is_zero = self.simd_eq(zero);
(<SimdFloat>::one() / *self).select(is_zero, zero)
(<SimdReal>::one() / *self).select(is_zero, zero)
}
fn transform_lin_vector(&self, pt: Vector2<SimdFloat>) -> Vector2<SimdFloat> {
fn transform_lin_vector(&self, pt: Vector2<SimdReal>) -> Vector2<SimdReal> {
pt * *self
}
fn transform_vector(&self, pt: SimdFloat) -> SimdFloat {
fn transform_vector(&self, pt: SimdReal) -> SimdReal {
*self * pt
}
fn squared(&self) -> SimdFloat {
fn squared(&self) -> SimdReal {
*self * *self
}
@@ -478,325 +468,8 @@ impl WAngularInertia<SimdFloat> for SimdFloat {
}
}
/// A 2x2 symmetric-definite-positive matrix.
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SdpMatrix2<N> {
/// The component at the first row and first column of this matrix.
pub m11: N,
/// The component at the first row and second column of this matrix.
pub m12: N,
/// The component at the second row and second column of this matrix.
pub m22: N,
}
impl<N: SimdRealField> SdpMatrix2<N> {
/// A new SDP 2x2 matrix with the given components.
///
/// Because the matrix is symmetric, only the lower off-diagonal component is required.
pub fn new(m11: N, m12: N, m22: N) -> Self {
Self { m11, m12, m22 }
}
/// Build an `SdpMatrix2` structure from a plain matrix, assuming it is SDP.
///
/// No check is performed to ensure `mat` is actually SDP.
pub fn from_sdp_matrix(mat: na::Matrix2<N>) -> Self {
Self {
m11: mat.m11,
m12: mat.m12,
m22: mat.m22,
}
}
/// Create a new SDP matrix filled with zeros.
pub fn zero() -> Self {
Self {
m11: N::zero(),
m12: N::zero(),
m22: N::zero(),
}
}
/// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero.
pub fn diagonal(val: N) -> Self {
Self {
m11: val,
m12: N::zero(),
m22: val,
}
}
/// Adds `val` to the diagonal components of `self`.
pub fn add_diagonal(&mut self, elt: N) -> Self {
Self {
m11: self.m11 + elt,
m12: self.m12,
m22: self.m22 + elt,
}
}
/// Compute the inverse of this SDP matrix without performing any inversibility check.
pub fn inverse_unchecked(&self) -> Self {
let determinant = self.m11 * self.m22 - self.m12 * self.m12;
let m11 = self.m22 / determinant;
let m12 = -self.m12 / determinant;
let m22 = self.m11 / determinant;
Self { m11, m12, m22 }
}
/// Convert this SDP matrix to a regular matrix representation.
pub fn into_matrix(self) -> Matrix2<N> {
Matrix2::new(self.m11, self.m12, self.m12, self.m22)
}
}
impl<N: SimdRealField> Add<SdpMatrix2<N>> for SdpMatrix2<N> {
type Output = Self;
fn add(self, rhs: SdpMatrix2<N>) -> Self {
Self::new(self.m11 + rhs.m11, self.m12 + rhs.m12, self.m22 + rhs.m22)
}
}
impl<N: SimdRealField> Mul<Vector2<N>> for SdpMatrix2<N> {
type Output = Vector2<N>;
fn mul(self, rhs: Vector2<N>) -> Self::Output {
Vector2::new(
self.m11 * rhs.x + self.m12 * rhs.y,
self.m12 * rhs.x + self.m22 * rhs.y,
)
}
}
/// A 3x3 symmetric-definite-positive matrix.
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SdpMatrix3<N> {
/// The component at the first row and first column of this matrix.
pub m11: N,
/// The component at the first row and second column of this matrix.
pub m12: N,
/// The component at the first row and third column of this matrix.
pub m13: N,
/// The component at the second row and second column of this matrix.
pub m22: N,
/// The component at the second row and third column of this matrix.
pub m23: N,
/// The component at the third row and third column of this matrix.
pub m33: N,
}
impl<N: SimdRealField> SdpMatrix3<N> {
/// A new SDP 3x3 matrix with the given components.
///
/// Because the matrix is symmetric, only the lower off-diagonal components is required.
pub fn new(m11: N, m12: N, m13: N, m22: N, m23: N, m33: N) -> Self {
Self {
m11,
m12,
m13,
m22,
m23,
m33,
}
}
/// Build an `SdpMatrix3` structure from a plain matrix, assuming it is SDP.
///
/// No check is performed to ensure `mat` is actually SDP.
pub fn from_sdp_matrix(mat: na::Matrix3<N>) -> Self {
Self {
m11: mat.m11,
m12: mat.m12,
m13: mat.m13,
m22: mat.m22,
m23: mat.m23,
m33: mat.m33,
}
}
/// Create a new SDP matrix filled with zeros.
pub fn zero() -> Self {
Self {
m11: N::zero(),
m12: N::zero(),
m13: N::zero(),
m22: N::zero(),
m23: N::zero(),
m33: N::zero(),
}
}
/// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero.
pub fn diagonal(val: N) -> Self {
Self {
m11: val,
m12: N::zero(),
m13: N::zero(),
m22: val,
m23: N::zero(),
m33: val,
}
}
/// Are all components of this matrix equal to zero?
pub fn is_zero(&self) -> bool {
self.m11.is_zero()
&& self.m12.is_zero()
&& self.m13.is_zero()
&& self.m22.is_zero()
&& self.m23.is_zero()
&& self.m33.is_zero()
}
/// Compute the inverse of this SDP matrix without performing any inversibility check.
pub fn inverse_unchecked(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
let determinant =
self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
let inv_det = N::one() / determinant;
SdpMatrix3 {
m11: minor_m12_m23 * inv_det,
m12: -minor_m11_m23 * inv_det,
m13: minor_m11_m22 * inv_det,
m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det,
m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det,
m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det,
}
}
/// Compute the quadratic form `m.transpose() * self * m`.
pub fn quadform3x2(&self, m: &Matrix3x2<N>) -> SdpMatrix2<N> {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0;
let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1;
let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1;
SdpMatrix2 { m11, m12, m22 }
}
/// Compute the quadratic form `m.transpose() * self * m`.
pub fn quadform(&self, m: &Matrix3<N>) -> Self {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33;
let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33;
let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33;
let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0;
let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1;
let m13 = m.m11 * x2 + m.m21 * y2 + m.m31 * z2;
let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1;
let m23 = m.m12 * x2 + m.m22 * y2 + m.m32 * z2;
let m33 = m.m13 * x2 + m.m23 * y2 + m.m33 * z2;
Self {
m11,
m12,
m13,
m22,
m23,
m33,
}
}
/// Adds `elt` to the diagonal components of `self`.
pub fn add_diagonal(&self, elt: N) -> Self {
Self {
m11: self.m11 + elt,
m12: self.m12,
m13: self.m13,
m22: self.m22 + elt,
m23: self.m23,
m33: self.m33 + elt,
}
}
}
impl<N: Add<N>> Add<SdpMatrix3<N>> for SdpMatrix3<N> {
type Output = SdpMatrix3<N::Output>;
fn add(self, rhs: SdpMatrix3<N>) -> Self::Output {
SdpMatrix3 {
m11: self.m11 + rhs.m11,
m12: self.m12 + rhs.m12,
m13: self.m13 + rhs.m13,
m22: self.m22 + rhs.m22,
m23: self.m23 + rhs.m23,
m33: self.m33 + rhs.m33,
}
}
}
impl<N: SimdRealField> Mul<Vector3<N>> for SdpMatrix3<N> {
type Output = Vector3<N>;
fn mul(self, rhs: Vector3<N>) -> Self::Output {
let x = self.m11 * rhs.x + self.m12 * rhs.y + self.m13 * rhs.z;
let y = self.m12 * rhs.x + self.m22 * rhs.y + self.m23 * rhs.z;
let z = self.m13 * rhs.x + self.m23 * rhs.y + self.m33 * rhs.z;
Vector3::new(x, y, z)
}
}
impl<N: SimdRealField> Mul<Matrix3<N>> for SdpMatrix3<N> {
type Output = Matrix3<N>;
fn mul(self, rhs: Matrix3<N>) -> Self::Output {
let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31;
let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31;
let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31;
let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32;
let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32;
let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32;
let x2 = self.m11 * rhs.m13 + self.m12 * rhs.m23 + self.m13 * rhs.m33;
let y2 = self.m12 * rhs.m13 + self.m22 * rhs.m23 + self.m23 * rhs.m33;
let z2 = self.m13 * rhs.m13 + self.m23 * rhs.m23 + self.m33 * rhs.m33;
Matrix3::new(x0, x1, x2, y0, y1, y2, z0, z1, z2)
}
}
impl<N: SimdRealField> Mul<Matrix3x2<N>> for SdpMatrix3<N> {
type Output = Matrix3x2<N>;
fn mul(self, rhs: Matrix3x2<N>) -> Self::Output {
let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31;
let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31;
let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31;
let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32;
let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32;
let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32;
Matrix3x2::new(x0, x1, y0, y1, z0, z1)
}
}
impl WAngularInertia<f32> for SdpMatrix3<f32> {
#[cfg(feature = "dim3")]
impl WAngularInertia<f32> for AngularInertia<f32> {
type AngVector = Vector3<f32>;
type LinVector = Vector3<f32>;
type AngMatrix = Matrix3<f32>;
@@ -812,7 +485,7 @@ impl WAngularInertia<f32> for SdpMatrix3<f32> {
if determinant.is_zero() {
Self::zero()
} else {
SdpMatrix3 {
AngularInertia {
m11: minor_m12_m23 / determinant,
m12: -minor_m11_m23 / determinant,
m13: minor_m11_m22 / determinant,
@@ -824,7 +497,7 @@ impl WAngularInertia<f32> for SdpMatrix3<f32> {
}
fn squared(&self) -> Self {
SdpMatrix3 {
AngularInertia {
m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
@@ -860,10 +533,11 @@ impl WAngularInertia<f32> for SdpMatrix3<f32> {
}
}
impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
type AngVector = Vector3<SimdFloat>;
type LinVector = Vector3<SimdFloat>;
type AngMatrix = Matrix3<SimdFloat>;
#[cfg(feature = "dim3")]
impl WAngularInertia<SimdReal> for AngularInertia<SimdReal> {
type AngVector = Vector3<SimdReal>;
type LinVector = Vector3<SimdReal>;
type AngMatrix = Matrix3<SimdReal>;
fn inverse(&self) -> Self {
let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
@@ -873,11 +547,11 @@ impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
let determinant =
self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
let zero = <SimdFloat>::zero();
let zero = <SimdReal>::zero();
let is_zero = determinant.simd_eq(zero);
let inv_det = (<SimdFloat>::one() / determinant).select(is_zero, zero);
let inv_det = (<SimdReal>::one() / determinant).select(is_zero, zero);
SdpMatrix3 {
AngularInertia {
m11: minor_m12_m23 * inv_det,
m12: -minor_m11_m23 * inv_det,
m13: minor_m11_m22 * inv_det,
@@ -887,11 +561,11 @@ impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
}
}
fn transform_lin_vector(&self, v: Vector3<SimdFloat>) -> Vector3<SimdFloat> {
fn transform_lin_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
self.transform_vector(v)
}
fn transform_vector(&self, v: Vector3<SimdFloat>) -> Vector3<SimdFloat> {
fn transform_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
@@ -899,7 +573,7 @@ impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
}
fn squared(&self) -> Self {
SdpMatrix3 {
AngularInertia {
m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
@@ -910,7 +584,7 @@ impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
}
#[rustfmt::skip]
fn into_matrix(self) -> Matrix3<SimdFloat> {
fn into_matrix(self) -> Matrix3<SimdReal> {
Matrix3::new(
self.m11, self.m12, self.m13,
self.m12, self.m22, self.m23,
@@ -919,7 +593,7 @@ impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
}
#[rustfmt::skip]
fn transform_matrix(&self, m: &Matrix3<SimdFloat>) -> Matrix3<SimdFloat> {
fn transform_matrix(&self, m: &Matrix3<SimdReal>) -> Matrix3<SimdReal> {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
@@ -940,206 +614,6 @@ impl WAngularInertia<SimdFloat> for SdpMatrix3<SimdFloat> {
}
}
impl<T> From<[SdpMatrix3<f32>; 4]> for SdpMatrix3<T>
where
T: From<[f32; 4]>,
{
fn from(data: [SdpMatrix3<f32>; 4]) -> Self {
SdpMatrix3 {
m11: T::from([data[0].m11, data[1].m11, data[2].m11, data[3].m11]),
m12: T::from([data[0].m12, data[1].m12, data[2].m12, data[3].m12]),
m13: T::from([data[0].m13, data[1].m13, data[2].m13, data[3].m13]),
m22: T::from([data[0].m22, data[1].m22, data[2].m22, data[3].m22]),
m23: T::from([data[0].m23, data[1].m23, data[2].m23, data[3].m23]),
m33: T::from([data[0].m33, data[1].m33, data[2].m33, data[3].m33]),
}
}
}
#[cfg(feature = "simd-nightly")]
impl From<[SdpMatrix3<f32>; 8]> for SdpMatrix3<simba::simd::f32x8> {
fn from(data: [SdpMatrix3<f32>; 8]) -> Self {
SdpMatrix3 {
m11: simba::simd::f32x8::from([
data[0].m11,
data[1].m11,
data[2].m11,
data[3].m11,
data[4].m11,
data[5].m11,
data[6].m11,
data[7].m11,
]),
m12: simba::simd::f32x8::from([
data[0].m12,
data[1].m12,
data[2].m12,
data[3].m12,
data[4].m12,
data[5].m12,
data[6].m12,
data[7].m12,
]),
m13: simba::simd::f32x8::from([
data[0].m13,
data[1].m13,
data[2].m13,
data[3].m13,
data[4].m13,
data[5].m13,
data[6].m13,
data[7].m13,
]),
m22: simba::simd::f32x8::from([
data[0].m22,
data[1].m22,
data[2].m22,
data[3].m22,
data[4].m22,
data[5].m22,
data[6].m22,
data[7].m22,
]),
m23: simba::simd::f32x8::from([
data[0].m23,
data[1].m23,
data[2].m23,
data[3].m23,
data[4].m23,
data[5].m23,
data[6].m23,
data[7].m23,
]),
m33: simba::simd::f32x8::from([
data[0].m33,
data[1].m33,
data[2].m33,
data[3].m33,
data[4].m33,
data[5].m33,
data[6].m33,
data[7].m33,
]),
}
}
}
#[cfg(feature = "simd-nightly")]
impl From<[SdpMatrix3<f32>; 16]> for SdpMatrix3<simba::simd::f32x16> {
fn from(data: [SdpMatrix3<f32>; 16]) -> Self {
SdpMatrix3 {
m11: simba::simd::f32x16::from([
data[0].m11,
data[1].m11,
data[2].m11,
data[3].m11,
data[4].m11,
data[5].m11,
data[6].m11,
data[7].m11,
data[8].m11,
data[9].m11,
data[10].m11,
data[11].m11,
data[12].m11,
data[13].m11,
data[14].m11,
data[15].m11,
]),
m12: simba::simd::f32x16::from([
data[0].m12,
data[1].m12,
data[2].m12,
data[3].m12,
data[4].m12,
data[5].m12,
data[6].m12,
data[7].m12,
data[8].m12,
data[9].m12,
data[10].m12,
data[11].m12,
data[12].m12,
data[13].m12,
data[14].m12,
data[15].m12,
]),
m13: simba::simd::f32x16::from([
data[0].m13,
data[1].m13,
data[2].m13,
data[3].m13,
data[4].m13,
data[5].m13,
data[6].m13,
data[7].m13,
data[8].m13,
data[9].m13,
data[10].m13,
data[11].m13,
data[12].m13,
data[13].m13,
data[14].m13,
data[15].m13,
]),
m22: simba::simd::f32x16::from([
data[0].m22,
data[1].m22,
data[2].m22,
data[3].m22,
data[4].m22,
data[5].m22,
data[6].m22,
data[7].m22,
data[8].m22,
data[9].m22,
data[10].m22,
data[11].m22,
data[12].m22,
data[13].m22,
data[14].m22,
data[15].m22,
]),
m23: simba::simd::f32x16::from([
data[0].m23,
data[1].m23,
data[2].m23,
data[3].m23,
data[4].m23,
data[5].m23,
data[6].m23,
data[7].m23,
data[8].m23,
data[9].m23,
data[10].m23,
data[11].m23,
data[12].m23,
data[13].m23,
data[14].m23,
data[15].m23,
]),
m33: simba::simd::f32x16::from([
data[0].m33,
data[1].m33,
data[2].m33,
data[3].m33,
data[4].m33,
data[5].m33,
data[6].m33,
data[7].m33,
data[8].m33,
data[9].m33,
data[10].m33,
data[11].m33,
data[12].m33,
data[13].m33,
data[14].m33,
data[15].m33,
]),
}
}
}
// This is an RAII structure that enables flushing denormal numbers
// to zero, and automatically reseting previous flags once it is dropped.
#[derive(Clone, Debug, PartialEq, Eq)]