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

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;