Merge pull request #79 from dimforge/split_geom

Move most of the geometric code to another crate.

.github/workflows/rapier-ci-build.yml

@@ -18,6 +18,8 @@ jobs:
       run: cargo fmt -- --check
   build-native:
     runs-on: ubuntu-latest
+    env:
+      RUSTFLAGS: -D warnings
     steps:
     - uses: actions/checkout@v2
     - run: sudo apt-get install -y cmake libxcb-composite0-dev
@@ -49,6 +51,8 @@ jobs:
       run: cargo check -j 1 --verbose -p rapier-examples-3d;
   build-wasm:
     runs-on: ubuntu-latest
+    env:
+      RUSTFLAGS: -D warnings
     steps:
       - uses: actions/checkout@v2
       - run: rustup target add wasm32-unknown-unknown

CHANGELOG.md

@@ -1,3 +1,63 @@
+## v0.5.0
+In this release we are dropping `ncollide` and use our new crate [`parry`](https://parry.rs)
+instead! This comes with a lot of new features, as well as two new crates: `rapier2d-f64` and
+`rapier3d-f64` for physics simulation with 64-bits floats.
+
+### Added
+- Added a `RAPIER.version()` function at the root of the package to retrieve the version of Rapier
+  as a string.
+
+Several geometric queries have been added to the `QueryPipeline`:
+- `QueryPipeline::intersections_with_ray`: get all colliders intersecting a ray.
+- `QueryPipeline::intersection_with_shape`: get one collider intersecting a shape.
+- `QueryPipeline::project_point`: get the projection of a point on the closest collider.
+- `QueryPipeline::intersections_with_point`: get all the colliders containing a point.
+- `QueryPipeline::cast_shape`: get the first collider intersecting a shape moving linearly
+  (aka. sweep test).
+- `QueryPipeline::intersections_with_shape`: get all the colliders intersecting a shape.
+
+Several new shape types are now supported:
+- `RoundCuboid`, `Segment`, `Triangle`, `RoundTriangle`, `Polyline`, `ConvexPolygon` (2D only),
+  `RoundConvexPolygon` (2D only), `ConvexPolyhedron` (3D only), `RoundConvexPolyhedron` (3D only),
+  `RoundCone` (3D only).
+
+It is possible to build `ColliderDesc` using these new shapes:
+- `ColliderBuilder::round_cuboid`, `ColliderBuilder::segment`, `ColliderBuilder::triangle`, `ColliderBuilder::round_triangle`,
+  `ColliderBuilder::convex_hull`, `ColliderBuilder::round_convex_hull`, `ColliderBuilder::polyline`,
+  `ColliderBuilder::convex_polyline` (2D only), `ColliderBuilder::round_convex_polyline` (2D only),
+  `ColliderBuilder::convex_mesh` (3D only),`ColliderBuilder::round_convex_mesh` (3D only), `ColliderBuilder::round_cone` (3D only).
+
+It is possible to specify different rules for combining friction and restitution coefficients
+of the two colliders involved in a contact with:
+- `ColliderDesc::friction_combine_rule`, and `ColliderDesc::restitution_combine_rule`.
+
+Various RigidBody-related getter and setters have been added:
+- `RigidBodyBuilder::gravity_scale`, `RigidBody::gravity_scale`, `RigidBody::set_gravity_scale` to get/set the scale
+  factor applied to the gravity affecting a rigid-body. Setting this to 0.0 will make the rigid-body ignore gravity.
+- `RigidBody::set_linear_damping` and `RigidBody::set_angular_damping` to set the linear and angular damping of
+  the rigid-body.
+- `RigidBodyBuilder::restrict_rotations` to prevent rotations along specific coordinate axes. This replaces the three
+  boolean arguments previously passed to `.set_principal_angular_inertia`.
+  
+### Breaking changes
+Breaking changes related to contacts:
+- The way contacts are represented changed. Refer to the documentation of `parry::query::ContactManifold`, `parry::query::TrackedContact`
+  and `rapier::geometry::ContactManifoldData` and `rapier::geometry::ContactData` for details.
+
+Breaking changes related to rigid-bodies:
+- The `RigidBodyDesc.setMass` takes only one argument now. Use `RigidBodyDesc.lockTranslations` to lock the translational
+  motion of the rigid-body.
+- The `RigidBodyDesc.setPrincipalAngularInertia` no longer have boolean parameters to lock rotations.
+  Use `RigidBodyDesc.lockRotations` or `RigidBodyDesc.restrictRotations` to lock the rotational motion of the rigid-body.
+
+Breaking changes related to colliders:
+- The `Polygon` shape no longer exists. For a 2D convex polygon, use a `ConvexPolygon` instead.
+- All occurrences of `Trimesh` have been replaced by `TriMesh` (note the change in case).
+
+Breaking changes related to events:
+- Rename all occurrences of `Proximity` to `Intersection`.
+- The `Proximity` enum has been removed, it's replaced by a boolean.
+
 ## v0.4.2
 - Fix a bug in angular inertia tensor computation that could cause rotations not to
   work properly.

Cargo.toml

@@ -1,15 +1,31 @@
 [workspace]
-members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmarks2d",
-            "build/rapier3d", "build/rapier_testbed3d", "examples3d", "benchmarks3d" ]
+members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "examples2d", "benchmarks2d",
+            "build/rapier3d", "build/rapier3d-f64", "build/rapier_testbed3d", "examples3d", "benchmarks3d" ]
 
 [patch.crates-io]
 #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
+
 #simba = { path = "../simba" }
 #ncollide2d = { path = "../ncollide/build/ncollide2d" }
 #ncollide3d = { path = "../ncollide/build/ncollide3d" }
 #nphysics2d = { path = "../nphysics/build/nphysics2d" }
 #nphysics3d = { path = "../nphysics/build/nphysics3d" }
 #kiss3d = { path = "../kiss3d" }
+#parry2d = { path = "../parry/build/parry2d" }
+#parry3d = { path = "../parry/build/parry3d" }
+#parry2d-f64 = { path = "../parry/build/parry2d-f64" }
+#parry3d-f64 = { path = "../parry/build/parry3d-f64" }
+#nalgebra = { path = "../nalgebra" }
+
+#kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
+#parry2d = { git = "https://github.com/sebcrozet/parry" }
+#parry3d = { git = "https://github.com/sebcrozet/parry" }
+#parry2d-f64 = { git = "https://github.com/sebcrozet/parry" }
+#parry3d-f64 = { git = "https://github.com/sebcrozet/parry" }
+#ncollide2d = { git = "https://github.com/dimforge/ncollide" }
+#ncollide3d = { git = "https://github.com/dimforge/ncollide" }
+#nphysics2d = { git = "https://github.com/dimforge/nphysics" }
+#nphysics3d = { git = "https://github.com/dimforge/nphysics" }
 
 [profile.release]
 #debug = true

benchmarks2d/Cargo.toml

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.23"
+nalgebra   = "0.24"
 
 [dependencies.rapier_testbed2d]
 path = "../build/rapier_testbed2d"

benchmarks2d/all_benchmarks2.rs

@@ -13,6 +13,7 @@ use std::cmp::Ordering;
 mod balls2;
 mod boxes2;
 mod capsules2;
+mod convex_polygons2;
 mod heightfield2;
 mod joint_ball2;
 mod joint_fixed2;
@@ -55,6 +56,7 @@ pub fn main() {
         ("Balls", balls2::init_world),
         ("Boxes", boxes2::init_world),
         ("Capsules", capsules2::init_world),
+        ("Convex polygons", convex_polygons2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Pyramid", pyramid2::init_world),
         ("(Stress test) joint ball", joint_ball2::init_world),

benchmarks2d/capsules2.rs

@@ -41,6 +41,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * Create the cubes
      */
     let num = 26;
+    let numy = num * 5;
     let rad = 0.5;
 
     let shift = rad * 2.0;
@@ -49,7 +50,7 @@ pub fn init_world(testbed: &mut Testbed) {
     let centery = shift / 2.0;
 
     for i in 0..num {
-        for j in 0usize..num * 5 {
+        for j in 0usize..numy {
             let x = i as f32 * shift - centerx;
             let y = j as f32 * shifty + centery + 3.0;
 

benchmarks2d/convex_polygons2.rs

@@ -0,0 +1,86 @@
+use na::Point2;
+use rand::distributions::{Distribution, Standard};
+use rand::{rngs::StdRng, SeedableRng};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 30.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(-ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the convex polygons
+     */
+    let num = 26;
+    let scale = 2.0;
+    let border_rad = 0.0;
+
+    let shift = border_rad * 2.0 + scale;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+
+    let mut rng = StdRng::seed_from_u64(0);
+    let distribution = Standard;
+
+    for i in 0..num {
+        for j in 0usize..num * 5 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift * 2.0 + centery + 2.0;
+
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+
+            let mut points = Vec::new();
+
+            for _ in 0..10 {
+                let pt: Point2<f32> = distribution.sample(&mut rng);
+                points.push(pt * scale);
+            }
+
+            let collider = ColliderBuilder::convex_hull(&points).unwrap().build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 50.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

benchmarks3d/Cargo.toml

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.23"
+nalgebra   = "0.24"
 
 [dependencies.rapier_testbed3d]
 path = "../build/rapier_testbed3d"
@@ -25,7 +25,3 @@ path = "../build/rapier3d"
 [[bin]]
 name = "all_benchmarks3"
 path = "all_benchmarks3.rs"
-
-[[bin]]
-name = "harness_capsules3"
-path = "harness_capsules3.rs"

benchmarks3d/all_benchmarks3.rs

@@ -14,6 +14,7 @@ mod balls3;
 mod boxes3;
 mod capsules3;
 mod compound3;
+mod convex_polyhedron3;
 mod heightfield3;
 mod joint_ball3;
 mod joint_fixed3;
@@ -52,6 +53,7 @@ pub fn main() {
         ("Boxes", boxes3::init_world),
         ("Capsules", capsules3::init_world),
         ("Compound", compound3::init_world),
+        ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),

benchmarks3d/convex_polyhedron3.rs

@@ -0,0 +1,83 @@
+use na::Point3;
+use rand::distributions::{Distribution, Standard};
+use rand::{rngs::StdRng, SeedableRng};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let scale = 2.0;
+    let rad = 1.0;
+    let border_rad = 0.1;
+
+    let shift = border_rad * 2.0 + scale;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * shift * 0.5;
+
+    let mut rng = StdRng::seed_from_u64(0);
+    let distribution = Standard;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                let mut points = Vec::new();
+                for _ in 0..10 {
+                    let pt: Point3<f32> = distribution.sample(&mut rng);
+                    points.push(pt * scale);
+                }
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::round_convex_hull(&points, border_rad)
+                    .unwrap()
+                    .build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

benchmarks3d/heightfield3.rs

@@ -1,4 +1,4 @@
-use na::{DMatrix, Point3, Vector3};
+use na::{ComplexField, DMatrix, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -23,7 +23,11 @@ pub fn init_world(testbed: &mut Testbed) {
         } else {
             let x = i as f32 * ground_size.x / (nsubdivs as f32);
             let z = j as f32 * ground_size.z / (nsubdivs as f32);
-            x.sin() + z.cos()
+
+            // NOTE: make sure we use the sin/cos from simba to ensure
+            // cross-platform determinism of the example when the
+            // enhanced_determinism feature is enabled.
+            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
         }
     });
 

benchmarks3d/trimesh3.rs

@@ -1,6 +1,6 @@
-use na::Point3;
+use na::{ComplexField, DMatrix, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -14,28 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Ground
      */
-    let ground_size = 200.0f32;
-    let ground_height = 1.0;
+    let ground_size = Vector3::new(200.0, 1.0, 200.0);
     let nsubdivs = 20;
 
-    let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
-    let indices = quad
-        .flat_indices()
-        .chunks(3)
-        .map(|is| Point3::new(is[0], is[2], is[1]))
-        .collect();
-    let mut vertices = quad.coords;
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
+            10.0
+        } else {
+            let x = i as f32 * ground_size.x / (nsubdivs as f32);
+            let z = j as f32 * ground_size.z / (nsubdivs as f32);
 
-    // ncollide generates a quad with `z` as the normal.
-    // so we switch z and y here and set a random altitude at each point.
-    for p in vertices.iter_mut() {
-        p.z = p.y;
-        p.y = (p.x.sin() + p.z.cos()) * ground_height;
-
-        if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
-            p.y = 10.0;
+            // NOTE: make sure we use the sin/cos from simba to ensure
+            // cross-platform determinism of the example when the
+            // enhanced_determinism feature is enabled.
+            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
         }
-    }
+    });
+
+    // Here we will build our trimesh from the mesh representation of an
+    // heightfield.
+    let heightfield = HeightField::new(heights, ground_size);
+    let (vertices, indices) = heightfield.to_trimesh();
 
     let rigid_body = RigidBodyBuilder::new_static().build();
     let handle = bodies.insert(rigid_body);

build/rapier2d-f64/Cargo.toml

@@ -0,0 +1,61 @@
+[package]
+name    = "rapier2d-f64"
+version = "0.4.2"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "2-dimensional physics engine in Rust."
+documentation = "http://docs.rs/rapier2d"
+homepage = "http://rapier.rs"
+repository = "https://github.com/dimforge/rapier"
+readme = "README.md"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+license = "Apache-2.0"
+edition = "2018"
+
+[badges]
+maintenance = { status = "actively-developed" }
+
+[features]
+default = [ "dim2", "f64" ]
+dim2    = [ ]
+f64     = [ ]
+parallel = [ "rayon" ]
+simd-stable = [ "simba/wide", "simd-is-enabled" ]
+simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
+# Do not enable this feature directly. It is automatically
+# enabled with the "simd-stable" or "simd-nightly" feature.
+simd-is-enabled = [ ]
+wasm-bindgen = [ "instant/wasm-bindgen" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "parry2d-f64/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
+enhanced-determinism = [ "simba/libm_force", "parry2d-f64/enhanced-determinism", "indexmap" ]
+
+[lib]
+name = "rapier2d_f64"
+path = "../../src/lib.rs"
+required-features = [ "dim2", "f64" ]
+
+
+[dependencies]
+vec_map = "0.8"
+instant = { version = "0.1", features = [ "now" ]}
+num-traits = "0.2"
+nalgebra = "0.24"
+parry2d-f64 = "0.1"
+simba = "0.3"
+approx = "0.4"
+rayon = { version = "1", optional = true }
+crossbeam = "0.8"
+generational-arena = "0.2"
+arrayvec = "0.5"
+bit-vec = "0.6"
+rustc-hash = "1"
+serde = { version = "1", features = [ "derive" ], optional = true }
+erased-serde = { version = "0.3", optional = true }
+indexmap = { version = "1", features = [ "serde-1" ], optional = true }
+downcast-rs = "1.2"
+num-derive = "0.3"
+bitflags = "1"
+
+[dev-dependencies]
+bincode = "1"
+serde = { version = "1", features = [ "derive" ] }

build/rapier2d/Cargo.toml

@@ -7,13 +7,18 @@ documentation = "http://docs.rs/rapier2d"
 homepage = "http://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
 keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
 license = "Apache-2.0"
 edition = "2018"
 
+[badges]
+maintenance = { status = "actively-developed" }
+
 [features]
-default = [ "dim2" ]
+default = [ "dim2", "f32" ]
 dim2    = [ ]
+f32     = [ ]
 parallel = [ "rayon" ]
 simd-stable = [ "simba/wide", "simd-is-enabled" ]
 simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
@@ -21,21 +26,21 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
 # enabled with the "simd-stable" or "simd-nightly" feature.
 simd-is-enabled = [ ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
-enhanced-determinism = [ "simba/libm_force", "indexmap" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "parry2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
+enhanced-determinism = [ "simba/libm_force", "parry2d/enhanced-determinism", "indexmap" ]
 
 [lib]
 name = "rapier2d"
 path = "../../src/lib.rs"
-required-features = [ "dim2" ]
+required-features = [ "dim2", "f32" ]
 
 
 [dependencies]
 vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.23"
-ncollide2d = "0.26"
+nalgebra = "0.24"
+parry2d = "0.1"
 simba = "0.3"
 approx = "0.4"
 rayon = { version = "1", optional = true }

build/rapier3d-f64/Cargo.toml

@@ -0,0 +1,60 @@
+[package]
+name    = "rapier3d-f64"
+version = "0.4.2"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "3-dimensional physics engine in Rust."
+documentation = "http://docs.rs/rapier3d"
+homepage = "http://rapier.rs"
+repository = "https://github.com/dimforge/rapier"
+readme = "README.md"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
+license = "Apache-2.0"
+edition = "2018"
+
+[badges]
+maintenance = { status = "actively-developed" }
+
+[features]
+default = [ "dim3", "f64" ]
+dim3    = [ ]
+f64     = [ ]
+parallel = [ "rayon" ]
+simd-stable = [ "parry3d-f64/simd-stable", "simba/wide", "simd-is-enabled" ]
+simd-nightly = [ "parry3d-f64/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
+# Do not enable this feature directly. It is automatically
+# enabled with the "simd-stable" or "simd-nightly" feature.
+simd-is-enabled = [ ]
+wasm-bindgen = [ "instant/wasm-bindgen" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "parry3d-f64/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
+enhanced-determinism = [ "simba/libm_force", "parry3d-f64/enhanced-determinism" ]
+
+[lib]
+name = "rapier3d_f64"
+path = "../../src/lib.rs"
+required-features = [ "dim3", "f64" ]
+
+
+[dependencies]
+vec_map = "0.8"
+instant = { version = "0.1", features = [ "now" ]}
+num-traits = "0.2"
+nalgebra = "0.24"
+parry3d-f64 = "0.1"
+simba = "0.3"
+approx = "0.4"
+rayon = { version = "1", optional = true }
+crossbeam = "0.8"
+generational-arena = "0.2"
+arrayvec = "0.5"
+bit-vec = "0.6"
+rustc-hash = "1"
+serde = { version = "1", features = [ "derive" ], optional = true }
+erased-serde = { version = "0.3", optional = true }
+downcast-rs = "1.2"
+num-derive = "0.3"
+bitflags = "1"
+
+[dev-dependencies]
+bincode = "1"
+serde = { version = "1", features = [ "derive" ] }

build/rapier3d/Cargo.toml

@@ -7,35 +7,40 @@ documentation = "http://docs.rs/rapier3d"
 homepage = "http://rapier.rs"
 repository = "https://github.com/dimforge/rapier"
 readme = "README.md"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
 keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
 license = "Apache-2.0"
 edition = "2018"
 
+[badges]
+maintenance = { status = "actively-developed" }
+
 [features]
-default = [ "dim3" ]
+default = [ "dim3", "f32" ]
 dim3    = [ ]
+f32     = [ ]
 parallel = [ "rayon" ]
-simd-stable = [ "simba/wide", "simd-is-enabled" ]
-simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
+simd-stable = [ "parry3d/simd-stable", "simba/wide", "simd-is-enabled" ]
+simd-nightly = [ "parry3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ]
 # Do not enable this feature directly. It is automatically
 # enabled with the "simd-stable" or "simd-nightly" feature.
 simd-is-enabled = [ ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
-enhanced-determinism = [ "simba/libm_force", "indexmap" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "parry3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
+enhanced-determinism = [ "simba/libm_force", "parry3d/enhanced-determinism" ]
 
 [lib]
 name = "rapier3d"
 path = "../../src/lib.rs"
-required-features = [ "dim3" ]
+required-features = [ "dim3", "f32" ]
 
 
 [dependencies]
 vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.23"
-ncollide3d = "0.26"
+nalgebra = "0.24"
+parry3d = "0.1"
 simba = "0.3"
 approx = "0.4"
 rayon = { version = "1", optional = true }
@@ -46,7 +51,6 @@ bit-vec = "0.6"
 rustc-hash = "1"
 serde = { version = "1", features = [ "derive" ], optional = true }
 erased-serde = { version = "0.3", optional = true }
-indexmap = { version = "1", features = [ "serde-1" ], optional = true }
 downcast-rs = "1.2"
 num-derive = "0.3"
 bitflags = "1"

build/rapier_testbed2d/Cargo.toml

@@ -2,13 +2,16 @@
 name    = "rapier_testbed2d"
 version = "0.4.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-description = "Testbed for the 2-dimensional physics engine in Rust."
+description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
 keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
 license = "Apache-2.0"
 edition = "2018"
 
+[badges]
+maintenance = { status = "actively-developed" }
 
 [lib]
 name = "rapier_testbed2d"
@@ -23,16 +26,17 @@ other-backends = [ "wrapped2d", "nphysics2d" ]
 
 
 [dependencies]
-nalgebra   = "0.23"
-kiss3d     = { version = "0.28", features = [ "conrod" ] }
+nalgebra   = "0.24"
+kiss3d     = { version = "0.29", features = [ "conrod" ] }
 rand       = "0.7"
 rand_pcg   = "0.2"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 num_cpus   = { version = "1", optional = true }
 wrapped2d  = { version = "0.4", optional = true }
-ncollide2d = "0.26"
-nphysics2d = { version = "0.18", optional = true }
+parry2d = "0.1"
+ncollide2d = "0.27"
+nphysics2d = { version = "0.19", optional = true }
 crossbeam = "0.8"
 bincode = "1"
 Inflector  = "0.11"

build/rapier_testbed3d/Cargo.toml

@@ -2,13 +2,16 @@
 name    = "rapier_testbed3d"
 version = "0.4.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
-description = "Testbed for the 3-dimensional physics engine in Rust."
+description = "Testbed for the Rapier3-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
 repository = "https://github.com/dimforge/rapier"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
 keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
 license = "Apache-2.0"
 edition = "2018"
 
+[badges]
+maintenance = { status = "actively-developed" }
 
 [lib]
 name = "rapier_testbed3d"
@@ -22,17 +25,18 @@ parallel = [ "rapier3d/parallel", "num_cpus" ]
 other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
 
 [dependencies]
-nalgebra   = "0.23"
-kiss3d     = { version = "0.28", features = [ "conrod" ] }
+nalgebra   = "0.24"
+kiss3d     = { version = "0.29", features = [ "conrod" ] }
 rand       = "0.7"
 rand_pcg   = "0.2"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
-glam       = { version = "0.10", optional = true }
+glam       = { version = "0.11", optional = true }
 num_cpus   = { version = "1", optional = true }
-ncollide3d = "0.26"
-nphysics3d = { version = "0.18", optional = true }
-physx      = { version = "0.8", optional = true }
+parry3d = "0.1"
+ncollide3d = "0.27"
+nphysics3d = { version = "0.19", optional = true }
+physx      = { version = "0.10", optional = true }
 physx-sys = { version = "0.4", optional = true }
 crossbeam = "0.8"
 bincode = "1"

examples2d/Cargo.toml

@@ -14,7 +14,9 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.23"
+nalgebra   = "0.24"
+lyon       = "0.17"
+usvg       = "0.13"
 
 [dependencies.rapier_testbed2d]
 path = "../build/rapier_testbed2d"

examples2d/all_examples2.rs

@@ -12,15 +12,18 @@ use std::cmp::Ordering;
 
 mod add_remove2;
 mod collision_groups2;
+mod convex_polygons2;
 mod damping2;
 mod debug_box_ball2;
 mod heightfield2;
 mod joints2;
 mod locked_rotations2;
 mod platform2;
+mod polyline2;
 mod pyramid2;
 mod restitution2;
 mod sensor2;
+mod trimesh2;
 
 fn demo_name_from_command_line() -> Option<String> {
     let mut args = std::env::args();
@@ -57,14 +60,17 @@ pub fn main() {
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Add remove", add_remove2::init_world),
         ("Collision groups", collision_groups2::init_world),
+        ("Convex polygons", convex_polygons2::init_world),
         ("Damping", damping2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),
         ("Platform", platform2::init_world),
+        ("Polyline", polyline2::init_world),
         ("Pyramid", pyramid2::init_world),
         ("Restitution", restitution2::init_world),
         ("Sensor", sensor2::init_world),
+        ("Trimesh", trimesh2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
     ];
 

examples2d/convex_polygons2.rs

@@ -0,0 +1,86 @@
+use na::Point2;
+use rand::distributions::{Distribution, Standard};
+use rand::{rngs::StdRng, SeedableRng};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 30.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(-ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the convex polygons
+     */
+    let num = 14;
+    let scale = 4.0;
+    let border_rad = 0.0;
+
+    let shift = border_rad * 2.0 + scale;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+
+    let mut rng = StdRng::seed_from_u64(0);
+    let distribution = Standard;
+
+    for i in 0..num {
+        for j in 0usize..num * 4 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift * 2.0 + centery + 2.0;
+
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+
+            let mut points = Vec::new();
+
+            for _ in 0..10 {
+                let pt: Point2<f32> = distribution.sample(&mut rng);
+                points.push(pt * scale);
+            }
+
+            let collider = ColliderBuilder::convex_hull(&points).unwrap().build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 50.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}

examples2d/polyline2.rs

@@ -0,0 +1,74 @@
+use na::{ComplexField, Point2};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let nsubdivs = 2000;
+    let step_size = ground_size / (nsubdivs as f32);
+    let mut points = Vec::new();
+
+    points.push(Point2::new(-ground_size / 2.0, 40.0));
+    for i in 1..nsubdivs - 1 {
+        let x = -ground_size / 2.0 + i as f32 * step_size;
+        let y = ComplexField::cos(i as f32 * step_size) * 2.0;
+        points.push(Point2::new(x, y));
+    }
+    points.push(Point2::new(ground_size / 2.0, 40.0));
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::polyline(points, None).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 20;
+    let rad = 0.5;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift + centery + 3.0;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+
+            if j % 2 == 0 {
+                let collider = ColliderBuilder::cuboid(rad, rad).build();
+                colliders.insert(collider, handle, &mut bodies);
+            } else {
+                let collider = ColliderBuilder::ball(rad).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 0.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Heightfield", init_world)]);
+    testbed.run()
+}

examples2d/sensor2.rs

@@ -1,6 +1,6 @@
 use na::{Point2, Point3};
 use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier2d::geometry::{ColliderBuilder, ColliderSet, Proximity};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -70,10 +70,11 @@ pub fn init_world(testbed: &mut Testbed) {
 
     // Callback that will be executed on the main loop to handle proximities.
     testbed.add_callback(move |_, mut graphics, physics, events, _| {
-        while let Ok(prox) = events.proximity_events.try_recv() {
-            let color = match prox.new_status {
-                Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
-                Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
+        while let Ok(prox) = events.intersection_events.try_recv() {
+            let color = if prox.intersecting {
+                Point3::new(1.0, 1.0, 0.0)
+            } else {
+                Point3::new(0.5, 0.5, 1.0)
             };
 
             let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent();

examples2d/trimesh2.rs

@@ -0,0 +1,263 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+use lyon::math::Point;
+use lyon::path::PathEvent;
+use lyon::tessellation::geometry_builder::*;
+use lyon::tessellation::{self, FillOptions, FillTessellator};
+use usvg::prelude::*;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 25.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(ground_size, ground_size)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(-ground_size, ground_size)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the trimeshes from a tesselated SVG.
+     */
+    let mut fill_tess = FillTessellator::new();
+    let opt = usvg::Options::default();
+    let rtree = usvg::Tree::from_str(RAPIER_SVG_STR, &opt).unwrap();
+    let mut ith = 0;
+
+    for node in rtree.root().descendants() {
+        if let usvg::NodeKind::Path(ref p) = *node.borrow() {
+            let transform = node.transform();
+            if p.fill.is_some() {
+                let path = PathConvIter {
+                    iter: p.data.iter(),
+                    first: Point::new(0.0, 0.0),
+                    prev: Point::new(0.0, 0.0),
+                    deferred: None,
+                    needs_end: false,
+                };
+
+                let mut mesh: VertexBuffers<_, u32> = VertexBuffers::new();
+                fill_tess
+                    .tessellate(
+                        path,
+                        &FillOptions::tolerance(0.01),
+                        &mut BuffersBuilder::new(&mut mesh, VertexCtor { prim_id: 0 }),
+                    )
+                    .expect("Tesselation failed.");
+
+                let angle = transform.get_rotate() as f32;
+
+                let (sx, sy) = (
+                    transform.get_scale().0 as f32 * 0.2,
+                    transform.get_scale().1 as f32 * 0.2,
+                );
+
+                let indices: Vec<_> = mesh.indices.chunks(3).map(|v| [v[0], v[1], v[2]]).collect();
+                let vertices: Vec<_> = mesh
+                    .vertices
+                    .iter()
+                    .map(|v| Point2::new(v.position[0] * sx, v.position[1] * -sy))
+                    .collect();
+
+                for k in 0..5 {
+                    let collider =
+                        ColliderBuilder::trimesh(vertices.clone(), indices.clone()).build();
+                    let rigid_body = RigidBodyBuilder::new_dynamic()
+                        .translation(ith as f32 * 8.0 - 20.0, 20.0 + k as f32 * 11.0)
+                        .rotation(angle)
+                        .build();
+                    let handle = bodies.insert(rigid_body);
+                    colliders.insert(collider, handle, &mut bodies);
+                }
+
+                ith += 1;
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 20.0), 17.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}
+
+const RAPIER_SVG_STR: &'static str = r#"
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg width="100%" height="100%" viewBox="0 0 527 131" version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" xml:space="preserve" xmlns:serif="http://www.serif.com/" style="fill-rule:evenodd;clip-rule:evenodd;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:1.5;">
+    <g transform="matrix(1,0,0,1,1,-673)">
+        <g transform="matrix(1,0,0,1,-5.31448,644.547)">
+            <g transform="matrix(1.34947,0,0,1.34947,-1559.19,-910.299)">
+                <g transform="matrix(50,0,0,50,1277.28,785.746)">
+                    <path d="M0.021,-0.074L0.074,-0.08C0.093,-0.082 0.106,-0.086 0.113,-0.092C0.119,-0.097 0.122,-0.111 0.122,-0.132L0.122,-0.597C0.122,-0.607 0.122,-0.616 0.121,-0.623C0.12,-0.63 0.117,-0.636 0.114,-0.641C0.111,-0.646 0.105,-0.65 0.098,-0.653C0.09,-0.656 0.08,-0.659 0.067,-0.661L0.025,-0.668L0.024,-0.685C0.04,-0.685 0.053,-0.685 0.063,-0.685C0.072,-0.684 0.081,-0.685 0.088,-0.686L0.222,-0.693C0.238,-0.695 0.255,-0.696 0.272,-0.696C0.349,-0.696 0.407,-0.685 0.446,-0.662C0.485,-0.639 0.504,-0.6 0.504,-0.543C0.504,-0.504 0.494,-0.473 0.474,-0.45C0.454,-0.427 0.424,-0.406 0.383,-0.387L0.493,-0.194C0.508,-0.167 0.522,-0.147 0.533,-0.132C0.544,-0.117 0.556,-0.106 0.567,-0.099C0.578,-0.092 0.589,-0.087 0.602,-0.085C0.614,-0.082 0.629,-0.08 0.647,-0.077L0.647,-0.059C0.633,-0.06 0.618,-0.06 0.601,-0.06L0.498,-0.06C0.483,-0.06 0.472,-0.06 0.465,-0.059L0.313,-0.341C0.302,-0.362 0.286,-0.372 0.263,-0.372L0.21,-0.372L0.21,-0.138C0.21,-0.122 0.213,-0.11 0.22,-0.103C0.226,-0.096 0.237,-0.09 0.253,-0.086L0.302,-0.074L0.302,-0.056C0.254,-0.059 0.205,-0.06 0.156,-0.06C0.106,-0.06 0.061,-0.059 0.021,-0.056L0.021,-0.074ZM0.21,-0.4C0.343,-0.395 0.41,-0.438 0.41,-0.527C0.41,-0.569 0.396,-0.603 0.367,-0.628C0.337,-0.653 0.297,-0.665 0.245,-0.665C0.24,-0.665 0.234,-0.665 0.228,-0.665C0.222,-0.664 0.216,-0.664 0.21,-0.663L0.21,-0.4Z" style="fill:rgb(235,237,240);fill-rule:nonzero;"/>
+                </g>
+                <g transform="matrix(50,0,0,50,1309.08,785.746)">
+                    <path d="M0.201,-0.049C0.174,-0.049 0.15,-0.054 0.129,-0.064C0.108,-0.073 0.091,-0.086 0.078,-0.103C0.065,-0.12 0.055,-0.139 0.048,-0.162C0.041,-0.185 0.038,-0.209 0.038,-0.235C0.038,-0.27 0.044,-0.302 0.056,-0.329C0.068,-0.356 0.084,-0.378 0.103,-0.396C0.122,-0.414 0.144,-0.428 0.169,-0.437C0.193,-0.446 0.218,-0.451 0.243,-0.451C0.256,-0.451 0.269,-0.45 0.28,-0.447C0.291,-0.444 0.301,-0.441 0.31,-0.438C0.32,-0.435 0.33,-0.431 0.339,-0.426L0.395,-0.455L0.404,-0.452C0.404,-0.4 0.404,-0.348 0.405,-0.297C0.405,-0.245 0.405,-0.193 0.405,-0.141C0.405,-0.114 0.416,-0.102 0.439,-0.103C0.444,-0.103 0.449,-0.104 0.452,-0.106C0.455,-0.107 0.459,-0.109 0.462,-0.111C0.465,-0.114 0.468,-0.116 0.471,-0.118L0.481,-0.112C0.476,-0.1 0.47,-0.089 0.463,-0.08C0.456,-0.072 0.448,-0.065 0.438,-0.059C0.428,-0.052 0.416,-0.049 0.403,-0.049C0.38,-0.049 0.364,-0.056 0.355,-0.071C0.346,-0.085 0.341,-0.105 0.341,-0.13L0.341,-0.16C0.337,-0.146 0.331,-0.132 0.324,-0.119C0.316,-0.106 0.306,-0.094 0.295,-0.084C0.284,-0.073 0.27,-0.065 0.255,-0.059C0.24,-0.052 0.222,-0.049 0.201,-0.049ZM0.341,-0.27C0.341,-0.293 0.339,-0.313 0.336,-0.332C0.332,-0.351 0.326,-0.367 0.317,-0.38C0.308,-0.393 0.297,-0.403 0.284,-0.41C0.27,-0.417 0.253,-0.42 0.232,-0.42C0.218,-0.42 0.203,-0.417 0.188,-0.412C0.172,-0.406 0.158,-0.397 0.145,-0.384C0.132,-0.371 0.122,-0.354 0.114,-0.334C0.105,-0.313 0.101,-0.289 0.101,-0.26C0.101,-0.238 0.104,-0.217 0.11,-0.197C0.115,-0.176 0.123,-0.158 0.134,-0.143C0.145,-0.128 0.158,-0.115 0.173,-0.107C0.188,-0.097 0.206,-0.093 0.226,-0.093C0.242,-0.093 0.257,-0.097 0.271,-0.105C0.284,-0.113 0.296,-0.124 0.306,-0.139C0.316,-0.153 0.324,-0.17 0.331,-0.189C0.337,-0.208 0.34,-0.229 0.341,-0.252L0.341,-0.27Z" style="fill:rgb(235,237,240);fill-rule:nonzero;"/>
+                </g>
+                <g transform="matrix(50,0,0,50,1334.28,785.746)">
+                    <path d="M0.156,-0.398C0.217,-0.443 0.276,-0.463 0.332,-0.459C0.379,-0.456 0.416,-0.437 0.445,-0.402C0.473,-0.367 0.487,-0.32 0.487,-0.262C0.487,-0.232 0.482,-0.204 0.471,-0.177C0.46,-0.15 0.445,-0.127 0.426,-0.108C0.407,-0.089 0.384,-0.073 0.358,-0.062C0.331,-0.051 0.303,-0.045 0.272,-0.045C0.235,-0.045 0.197,-0.055 0.156,-0.074L0.156,0.078C0.156,0.087 0.159,0.093 0.165,0.099C0.17,0.103 0.181,0.107 0.198,0.11L0.258,0.12L0.258,0.137C0.249,0.136 0.238,0.135 0.227,0.135C0.215,0.134 0.201,0.134 0.186,0.133L0.145,0.133C0.122,0.133 0.1,0.133 0.079,0.134C0.058,0.135 0.04,0.136 0.023,0.137L0.023,0.12L0.065,0.112C0.075,0.11 0.082,0.107 0.085,0.103C0.088,0.098 0.089,0.09 0.089,0.077L0.089,-0.364C0.089,-0.389 0.085,-0.401 0.076,-0.402L0.027,-0.41L0.03,-0.424C0.047,-0.431 0.061,-0.437 0.074,-0.443C0.086,-0.449 0.095,-0.454 0.102,-0.458C0.109,-0.463 0.117,-0.468 0.124,-0.475C0.131,-0.482 0.136,-0.489 0.141,-0.496L0.156,-0.496L0.156,-0.398ZM0.156,-0.367L0.156,-0.152C0.156,-0.128 0.167,-0.108 0.188,-0.093C0.209,-0.077 0.236,-0.069 0.27,-0.069C0.291,-0.069 0.311,-0.073 0.328,-0.082C0.345,-0.09 0.36,-0.101 0.373,-0.117C0.386,-0.132 0.395,-0.149 0.402,-0.17C0.409,-0.191 0.412,-0.213 0.412,-0.238C0.412,-0.266 0.408,-0.291 0.401,-0.314C0.394,-0.336 0.383,-0.355 0.37,-0.37C0.356,-0.385 0.34,-0.397 0.321,-0.404C0.302,-0.411 0.282,-0.413 0.259,-0.41C0.226,-0.406 0.192,-0.392 0.156,-0.367Z" style="fill:rgb(235,237,240);fill-rule:nonzero;"/>
+                </g>
+                <g transform="matrix(50,0,0,50,1360.53,785.746)">
+                    <path d="M0.246,-0.056C0.235,-0.057 0.221,-0.058 0.203,-0.059C0.184,-0.06 0.162,-0.06 0.135,-0.06C0.108,-0.06 0.086,-0.06 0.068,-0.059C0.049,-0.058 0.035,-0.057 0.024,-0.056L0.024,-0.073L0.078,-0.084C0.099,-0.088 0.109,-0.1 0.109,-0.12L0.109,-0.319C0.109,-0.335 0.105,-0.347 0.097,-0.354C0.089,-0.361 0.073,-0.368 0.049,-0.373L0.049,-0.388C0.072,-0.394 0.094,-0.403 0.113,-0.415C0.132,-0.427 0.147,-0.442 0.158,-0.46L0.175,-0.46L0.175,-0.117C0.175,-0.107 0.177,-0.1 0.18,-0.095C0.183,-0.09 0.188,-0.087 0.197,-0.085L0.246,-0.073L0.246,-0.056ZM0.137,-0.667C0.15,-0.667 0.161,-0.663 0.17,-0.654C0.179,-0.645 0.184,-0.634 0.184,-0.621C0.184,-0.61 0.179,-0.6 0.17,-0.591C0.16,-0.582 0.148,-0.578 0.135,-0.578C0.123,-0.578 0.113,-0.582 0.105,-0.591C0.096,-0.599 0.092,-0.609 0.092,-0.621C0.092,-0.634 0.096,-0.645 0.106,-0.654C0.115,-0.663 0.125,-0.667 0.137,-0.667Z" style="fill:rgb(235,237,240);fill-rule:nonzero;"/>
+                </g>
+                <g transform="matrix(50,0,0,50,1374.03,785.746)">
+                    <path d="M0.386,-0.309L0.111,-0.309C0.109,-0.296 0.108,-0.283 0.108,-0.271C0.108,-0.22 0.122,-0.178 0.149,-0.147C0.176,-0.115 0.214,-0.099 0.262,-0.099C0.285,-0.099 0.307,-0.103 0.326,-0.113C0.345,-0.122 0.366,-0.137 0.389,-0.158L0.393,-0.135C0.37,-0.105 0.344,-0.082 0.315,-0.067C0.285,-0.052 0.25,-0.044 0.211,-0.044C0.186,-0.044 0.164,-0.049 0.143,-0.06C0.122,-0.07 0.103,-0.084 0.088,-0.103C0.072,-0.122 0.06,-0.143 0.051,-0.169C0.042,-0.194 0.038,-0.221 0.038,-0.25C0.038,-0.279 0.043,-0.307 0.053,-0.333C0.062,-0.358 0.076,-0.38 0.093,-0.399C0.11,-0.418 0.131,-0.432 0.155,-0.443C0.179,-0.454 0.205,-0.459 0.233,-0.459C0.255,-0.459 0.275,-0.455 0.294,-0.448C0.312,-0.441 0.328,-0.431 0.341,-0.418C0.354,-0.405 0.365,-0.389 0.373,-0.37C0.38,-0.351 0.385,-0.331 0.386,-0.309ZM0.116,-0.332L0.261,-0.332C0.271,-0.332 0.28,-0.332 0.287,-0.333C0.294,-0.333 0.3,-0.334 0.305,-0.337C0.31,-0.339 0.313,-0.342 0.314,-0.347C0.315,-0.352 0.314,-0.358 0.312,-0.367C0.308,-0.384 0.3,-0.4 0.288,-0.414C0.275,-0.428 0.256,-0.435 0.231,-0.435C0.201,-0.435 0.176,-0.426 0.156,-0.408C0.136,-0.389 0.123,-0.364 0.116,-0.332Z" style="fill:rgb(235,237,240);fill-rule:nonzero;"/>
+                </g>
+                <g transform="matrix(50,0,0,50,1395.58,785.746)">
+                    <path d="M0.024,-0.056L0.024,-0.072L0.072,-0.081C0.085,-0.084 0.094,-0.089 0.098,-0.096C0.101,-0.103 0.103,-0.115 0.103,-0.132L0.103,-0.324C0.103,-0.337 0.1,-0.347 0.095,-0.353C0.089,-0.359 0.076,-0.364 0.057,-0.368L0.031,-0.374L0.033,-0.389C0.059,-0.398 0.081,-0.409 0.1,-0.422C0.119,-0.435 0.135,-0.455 0.149,-0.482L0.167,-0.482C0.168,-0.464 0.169,-0.445 0.17,-0.424C0.171,-0.403 0.171,-0.381 0.171,-0.358C0.178,-0.37 0.185,-0.382 0.194,-0.393C0.202,-0.404 0.212,-0.415 0.224,-0.426C0.247,-0.448 0.267,-0.459 0.285,-0.459C0.302,-0.459 0.315,-0.454 0.324,-0.445C0.333,-0.436 0.338,-0.424 0.338,-0.409C0.338,-0.396 0.334,-0.385 0.327,-0.378C0.319,-0.37 0.309,-0.366 0.298,-0.366C0.287,-0.366 0.275,-0.369 0.263,-0.376C0.251,-0.383 0.241,-0.386 0.232,-0.386C0.225,-0.386 0.219,-0.383 0.212,-0.376C0.205,-0.369 0.198,-0.361 0.192,-0.352C0.186,-0.343 0.181,-0.333 0.177,-0.323C0.173,-0.312 0.171,-0.304 0.171,-0.297L0.171,-0.13C0.171,-0.113 0.174,-0.101 0.18,-0.096C0.185,-0.09 0.197,-0.086 0.216,-0.083L0.288,-0.072L0.288,-0.056C0.271,-0.057 0.251,-0.058 0.23,-0.059C0.208,-0.06 0.185,-0.06 0.16,-0.06L0.146,-0.06C0.123,-0.06 0.101,-0.06 0.081,-0.059C0.06,-0.058 0.041,-0.057 0.024,-0.056Z" style="fill:rgb(235,237,240);fill-rule:nonzero;"/>
+                </g>
+            </g>
+        </g>
+    </g>
+</svg>
+"#;
+
+pub struct PathConvIter<'a> {
+    iter: std::slice::Iter<'a, usvg::PathSegment>,
+    prev: Point,
+    first: Point,
+    needs_end: bool,
+    deferred: Option<PathEvent>,
+}
+
+impl<'l> Iterator for PathConvIter<'l> {
+    type Item = PathEvent;
+    fn next(&mut self) -> Option<PathEvent> {
+        if self.deferred.is_some() {
+            return self.deferred.take();
+        }
+
+        let next = self.iter.next();
+        match next {
+            Some(usvg::PathSegment::MoveTo { x, y }) => {
+                if self.needs_end {
+                    let last = self.prev;
+                    let first = self.first;
+                    self.needs_end = false;
+                    self.prev = Point::new(*x as f32, *y as f32);
+                    self.deferred = Some(PathEvent::Begin { at: self.prev });
+                    self.first = self.prev;
+                    Some(PathEvent::End {
+                        last,
+                        first,
+                        close: false,
+                    })
+                } else {
+                    self.first = Point::new(*x as f32, *y as f32);
+                    Some(PathEvent::Begin { at: self.first })
+                }
+            }
+            Some(usvg::PathSegment::LineTo { x, y }) => {
+                self.needs_end = true;
+                let from = self.prev;
+                self.prev = Point::new(*x as f32, *y as f32);
+                Some(PathEvent::Line {
+                    from,
+                    to: self.prev,
+                })
+            }
+            Some(usvg::PathSegment::CurveTo {
+                x1,
+                y1,
+                x2,
+                y2,
+                x,
+                y,
+            }) => {
+                self.needs_end = true;
+                let from = self.prev;
+                self.prev = Point::new(*x as f32, *y as f32);
+                Some(PathEvent::Cubic {
+                    from,
+                    ctrl1: Point::new(*x1 as f32, *y1 as f32),
+                    ctrl2: Point::new(*x2 as f32, *y2 as f32),
+                    to: self.prev,
+                })
+            }
+            Some(usvg::PathSegment::ClosePath) => {
+                self.needs_end = false;
+                self.prev = self.first;
+                Some(PathEvent::End {
+                    last: self.prev,
+                    first: self.first,
+                    close: true,
+                })
+            }
+            None => {
+                if self.needs_end {
+                    self.needs_end = false;
+                    let last = self.prev;
+                    let first = self.first;
+                    Some(PathEvent::End {
+                        last,
+                        first,
+                        close: false,
+                    })
+                } else {
+                    None
+                }
+            }
+        }
+    }
+}
+
+pub struct VertexCtor {
+    pub prim_id: u32,
+}
+
+impl FillVertexConstructor<GpuVertex> for VertexCtor {
+    fn new_vertex(&mut self, vertex: tessellation::FillVertex) -> GpuVertex {
+        GpuVertex {
+            position: vertex.position().to_array(),
+            prim_id: self.prim_id,
+        }
+    }
+}
+
+impl StrokeVertexConstructor<GpuVertex> for VertexCtor {
+    fn new_vertex(&mut self, vertex: tessellation::StrokeVertex) -> GpuVertex {
+        GpuVertex {
+            position: vertex.position().to_array(),
+            prim_id: self.prim_id,
+        }
+    }
+}
+
+#[repr(C)]
+#[derive(Copy, Clone)]
+pub struct GpuVertex {
+    pub position: [f32; 2],
+    pub prim_id: u32,
+}

examples3d/Cargo.toml

@@ -14,7 +14,8 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.23"
+nalgebra   = "0.24"
+kiss3d     = "0.29"
 
 [dependencies.rapier_testbed3d]
 path = "../build/rapier_testbed3d"
@@ -25,3 +26,7 @@ path = "../build/rapier3d"
 [[bin]]
 name = "all_examples3"
 path = "./all_examples3.rs"
+
+[[bin]]
+name = "harness_capsules3"
+path = "./harness_capsules3.rs"

examples3d/all_examples3.rs

@@ -12,6 +12,8 @@ use std::cmp::Ordering;
 
 mod collision_groups3;
 mod compound3;
+mod convex_decomposition3;
+mod convex_polyhedron3;
 mod damping3;
 mod debug_add_remove_collider3;
 mod debug_boxes3;
@@ -31,7 +33,6 @@ mod platform3;
 mod primitives3;
 mod restitution3;
 mod sensor3;
-mod stacks3;
 mod trimesh3;
 
 fn demo_name_from_command_line() -> Option<String> {
@@ -75,6 +76,8 @@ pub fn main() {
         ("Primitives", primitives3::init_world),
         ("Collision groups", collision_groups3::init_world),
         ("Compound", compound3::init_world),
+        ("Convex decomposition", convex_decomposition3::init_world),
+        ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Damping", damping3::init_world),
         ("Domino", domino3::init_world),
         ("Heightfield", heightfield3::init_world),
@@ -82,9 +85,8 @@ pub fn main() {
         ("Locked rotations", locked_rotations3::init_world),
         ("Platform", platform3::init_world),
         ("Restitution", restitution3::init_world),
-        ("Stacks", stacks3::init_world),
         ("Sensor", sensor3::init_world),
-        ("Trimesh", trimesh3::init_world),
+        ("TriMesh", trimesh3::init_world),
         ("Keva tower", keva3::init_world),
         (
             "(Debug) add/rm collider",

examples3d/compound3.rs

@@ -1,6 +1,6 @@
-use na::Point3;
+use na::{Isometry3, Point3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, SharedShape};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -28,6 +28,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * Create the cubes
      */
     let num = 8;
+    let numy = 15;
     let rad = 0.2;
 
     let shift = rad * 4.0 + rad;
@@ -37,7 +38,7 @@ pub fn init_world(testbed: &mut Testbed) {
 
     let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
 
-    for j in 0usize..15 {
+    for j in 0usize..numy {
         for i in 0..num {
             for k in 0usize..num {
                 let x = i as f32 * shift * 5.0 - centerx + offset;
@@ -47,16 +48,39 @@ pub fn init_world(testbed: &mut Testbed) {
                 // Build the rigid body.
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
-                let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
-                let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
-                    .translation(rad * 10.0, rad * 10.0, 0.0)
-                    .build();
-                let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
-                    .translation(-rad * 10.0, rad * 10.0, 0.0)
-                    .build();
-                colliders.insert(collider1, handle, &mut bodies);
-                colliders.insert(collider2, handle, &mut bodies);
-                colliders.insert(collider3, handle, &mut bodies);
+
+                // First option: attach several colliders to a single rigid-body.
+                if j < numy / 2 {
+                    let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
+                    let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                        .translation(rad * 10.0, rad * 10.0, 0.0)
+                        .build();
+                    let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                        .translation(-rad * 10.0, rad * 10.0, 0.0)
+                        .build();
+                    colliders.insert(collider1, handle, &mut bodies);
+                    colliders.insert(collider2, handle, &mut bodies);
+                    colliders.insert(collider3, handle, &mut bodies);
+                } else {
+                    // Second option: create a compound shape and attach it to a single collider.
+                    let shapes = vec![
+                        (
+                            Isometry3::identity(),
+                            SharedShape::cuboid(rad * 10.0, rad, rad),
+                        ),
+                        (
+                            Isometry3::translation(rad * 10.0, rad * 10.0, 0.0),
+                            SharedShape::cuboid(rad, rad * 10.0, rad),
+                        ),
+                        (
+                            Isometry3::translation(-rad * 10.0, rad * 10.0, 0.0),
+                            SharedShape::cuboid(rad, rad * 10.0, rad),
+                        ),
+                    ];
+
+                    let collider = ColliderBuilder::compound(shapes).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                }
             }
         }
 

examples3d/convex_decomposition3.rs

@@ -0,0 +1,133 @@
+use kiss3d::loader::obj;
+use na::{Point3, Translation3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, SharedShape};
+use rapier3d::parry::bounding_volume::{self, BoundingVolume};
+use rapier_testbed3d::Testbed;
+use std::path::Path;
+
+/*
+ * NOTE: The `r` macro is only here to convert from f64 to the `N` scalar type.
+ * This simplifies experimentation with various scalar types (f32, fixed-point numbers, etc.)
+ */
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 50.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the convex decompositions.
+     */
+    let geoms = models();
+    let ngeoms = geoms.len();
+    let width = (ngeoms as f32).sqrt() as usize;
+    let num_duplications = 4;
+    let shift = 5.0f32;
+
+    for (igeom, obj_path) in geoms.into_iter().enumerate() {
+        let deltas = na::one();
+        let mtl_path = Path::new("");
+
+        let mut shapes = Vec::new();
+        println!("Parsing and decomposing: {}", obj_path);
+        let obj = obj::parse_file(&Path::new(&obj_path), &mtl_path, "");
+
+        if let Ok(model) = obj {
+            let meshes: Vec<_> = model
+                .into_iter()
+                .map(|mesh| mesh.1.to_trimesh().unwrap())
+                .collect();
+
+            // Compute the size of the model, to scale it and have similar size for everything.
+            let mut aabb =
+                bounding_volume::details::point_cloud_aabb(&deltas, &meshes[0].coords[..]);
+
+            for mesh in meshes[1..].iter() {
+                aabb.merge(&bounding_volume::details::point_cloud_aabb(
+                    &deltas,
+                    &mesh.coords[..],
+                ));
+            }
+
+            let center = aabb.center().coords;
+            let diag = (aabb.maxs - aabb.mins).norm();
+
+            for mut trimesh in meshes.into_iter() {
+                trimesh.translate_by(&Translation3::from(-center));
+                trimesh.scale_by_scalar(6.0 / diag);
+
+                let vertices = trimesh.coords;
+                let indices: Vec<_> = trimesh
+                    .indices
+                    .unwrap_unified()
+                    .into_iter()
+                    .map(|idx| [idx.x, idx.y, idx.z])
+                    .collect();
+
+                let decomposed_shape = SharedShape::convex_decomposition(&vertices, &indices);
+                shapes.push(decomposed_shape);
+            }
+
+            // let compound = SharedShape::compound(compound_parts);
+
+            for k in 1..num_duplications + 1 {
+                let x = (igeom % width) as f32 * shift;
+                let y = (igeom / width) as f32 * shift + 4.0;
+                let z = k as f32 * shift;
+
+                let body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(body);
+
+                for shape in &shapes {
+                    let collider = ColliderBuilder::new(shape.clone()).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn models() -> Vec<String> {
+    vec![
+        "media/models/camel_decimated.obj".to_string(),
+        "media/models/chair.obj".to_string(),
+        "media/models/cup_decimated.obj".to_string(),
+        "media/models/dilo_decimated.obj".to_string(),
+        "media/models/feline_decimated.obj".to_string(),
+        "media/models/genus3_decimated.obj".to_string(),
+        "media/models/hand2_decimated.obj".to_string(),
+        "media/models/hand_decimated.obj".to_string(),
+        "media/models/hornbug.obj".to_string(),
+        "media/models/octopus_decimated.obj".to_string(),
+        "media/models/rabbit_decimated.obj".to_string(),
+        "media/models/rust_logo.obj".to_string(),
+        "media/models/rust_logo_simplified.obj".to_string(),
+        "media/models/screwdriver_decimated.obj".to_string(),
+        "media/models/table.obj".to_string(),
+        "media/models/tstTorusModel.obj".to_string(),
+        // "media/models/tstTorusModel2.obj".to_string(),
+        // "media/models/tstTorusModel3.obj".to_string(),
+    ]
+}

examples3d/convex_polyhedron3.rs

@@ -0,0 +1,78 @@
+use na::Point3;
+use rand::distributions::{Distribution, Standard};
+use rand::{rngs::StdRng, SeedableRng};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 40.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the polyhedra
+     */
+    let num = 5;
+    let scale = 2.0;
+    let border_rad = 0.1;
+
+    let shift = border_rad * 2.0 + scale;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut rng = StdRng::seed_from_u64(0);
+    let distribution = Standard;
+
+    for j in 0usize..25 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz;
+
+                let mut points = Vec::new();
+                for _ in 0..10 {
+                    let pt: Point3<f32> = distribution.sample(&mut rng);
+                    points.push(pt * scale);
+                }
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::round_convex_hull(&points, border_rad)
+                    .unwrap()
+                    .build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(30.0, 30.0, 30.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/debug_trimesh3.rs

@@ -24,18 +24,18 @@ pub fn init_world(testbed: &mut Testbed) {
         Point3::new(-width, -width, width),
     ];
     let idx = vec![
-        Point3::new(0, 1, 2),
-        Point3::new(0, 2, 3),
-        Point3::new(4, 5, 6),
-        Point3::new(4, 6, 7),
-        Point3::new(0, 4, 7),
-        Point3::new(0, 7, 3),
-        Point3::new(1, 5, 6),
-        Point3::new(1, 6, 2),
-        Point3::new(3, 2, 7),
-        Point3::new(2, 6, 7),
-        Point3::new(0, 1, 5),
-        Point3::new(0, 5, 4),
+        [0, 1, 2],
+        [0, 2, 3],
+        [4, 5, 6],
+        [4, 6, 7],
+        [0, 4, 7],
+        [0, 7, 3],
+        [1, 5, 6],
+        [1, 6, 2],
+        [3, 2, 7],
+        [2, 6, 7],
+        [0, 1, 5],
+        [0, 5, 4],
     ];
 
     // Dynamic box rigid body.

examples3d/heightfield3.rs

@@ -1,6 +1,6 @@
-use na::{ComplexField, DMatrix, Point3, Vector3};
+use na::{ComplexField, DMatrix, Isometry3, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, SharedShape};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -27,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) {
             // NOTE: make sure we use the sin/cos from simba to ensure
             // cross-platform determinism of the example when the
             // enhanced_determinism feature is enabled.
-            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
+            ComplexField::sin(x) + ComplexField::cos(z)
         }
     });
 
@@ -58,14 +58,32 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                let collider = match j % 5 {
+                let collider = match j % 6 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
                     2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
                     3 => ColliderBuilder::cone(rad, rad).build(),
-                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
+                    4 => ColliderBuilder::capsule_y(rad, rad).build(),
+                    _ => {
+                        let shapes = vec![
+                            (
+                                Isometry3::identity(),
+                                SharedShape::cuboid(rad, rad / 2.0, rad / 2.0),
+                            ),
+                            (
+                                Isometry3::translation(rad, 0.0, 0.0),
+                                SharedShape::cuboid(rad / 2.0, rad, rad / 2.0),
+                            ),
+                            (
+                                Isometry3::translation(-rad, 0.0, 0.0),
+                                SharedShape::cuboid(rad / 2.0, rad, rad / 2.0),
+                            ),
+                        ];
+
+                        ColliderBuilder::compound(shapes).build()
+                    }
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

examples3d/joints3.rs

@@ -196,16 +196,16 @@ fn create_ball_joints(
             };
 
             let rigid_body = RigidBodyBuilder::new(status)
-                .translation(fk * shift, 0.0, fi * shift)
+                .translation(fk * shift, 0.0, fi * shift * 2.0)
                 .build();
             let child_handle = bodies.insert(rigid_body);
-            let collider = ColliderBuilder::ball(rad).build();
+            let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
             colliders.insert(collider, child_handle, bodies);
 
             // Vertical joint.
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
-                let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift));
+                let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift * 2.0));
                 joints.insert(bodies, parent_handle, child_handle, joint);
             }
 

examples3d/locked_rotations3.rs

@@ -33,7 +33,7 @@ pub fn init_world(testbed: &mut Testbed) {
     let rigid_body = RigidBodyBuilder::new_dynamic()
         .translation(0.0, 3.0, 0.0)
         .lock_translations()
-        .principal_angular_inertia(Vector3::zeros(), Vector3::new(true, false, false))
+        .restrict_rotations(true, false, false)
         .build();
     let handle = bodies.insert(rigid_body);
     let collider = ColliderBuilder::cuboid(0.2, 0.6, 2.0).build();
@@ -50,6 +50,8 @@ pub fn init_world(testbed: &mut Testbed) {
     let handle = bodies.insert(rigid_body);
     let collider = ColliderBuilder::capsule_y(0.6, 0.4).build();
     colliders.insert(collider, handle, &mut bodies);
+    let collider = ColliderBuilder::capsule_x(0.6, 0.4).build();
+    colliders.insert(collider, handle, &mut bodies);
 
     /*
      * Set up the testbed.

examples3d/media/models/chair.obj

@@ -0,0 +1,760 @@
+# 857
+# 512
+v -0.203961 0.754510 0.211019
+v -0.199603 1.785490 -0.001498
+v -0.199163 0.754510 -0.001498
+v -0.199830 1.524170 0.214066
+v -0.214032 1.655100 0.216193
+v -0.214032 0.738577 0.216193
+v -0.209234 0.738577 -0.001498
+v 0.056176 0.210912 -0.001498
+v -0.011230 0.210913 0.215059
+v -0.012112 0.210912 -0.001498
+v 0.057057 0.210913 0.214728
+v -0.011230 0.275571 0.215059
+v -0.012112 0.275571 -0.001498
+v 0.057057 0.275571 0.214728
+v 0.056176 0.275571 -0.001498
+v -0.146260 0.206221 0.217801
+v 0.182828 0.206221 0.216341
+v 0.182828 0.280263 0.216341
+v -0.146260 0.280263 0.217801
+v 0.181545 0.206221 0.286352
+v -0.147543 0.206221 0.287812
+v 0.181545 0.280263 0.286352
+v -0.147543 0.280263 0.287812
+v -0.208590 -0.001787 0.279676
+v -0.214032 0.243242 0.216341
+v -0.207557 -0.001787 0.223301
+v -0.215315 0.243242 0.286352
+v -0.152985 -0.001787 0.224477
+v -0.154018 -0.001787 0.280852
+v 0.188270 -0.001787 0.279676
+v 0.189303 -0.001787 0.223301
+v 0.250600 0.243242 0.217801
+v 0.243875 -0.001787 0.224477
+v 0.249317 0.243242 0.287812
+v 0.242842 -0.001787 0.280852
+v -0.214032 0.377162 0.216341
+v -0.146260 0.377162 0.217801
+v 0.182828 0.377162 0.216341
+v 0.250600 0.377162 0.217801
+v -0.128461 1.675360 -0.001498
+v -0.146260 1.508020 0.217653
+v -0.132386 1.472780 0.176497
+v -0.141462 1.768750 -0.001498
+v -0.132386 0.781150 0.176497
+v -0.141462 0.738577 -0.001498
+v -0.128461 0.781150 -0.001498
+v -0.146260 0.738577 0.217653
+v -0.160112 1.524170 0.214922
+v -0.159884 1.785490 -0.001498
+v -0.174244 1.836230 0.258962
+v -0.187331 1.836230 0.244894
+v -0.187584 1.836230 0.258675
+v -0.173991 1.836230 0.245182
+v -0.160446 1.785010 0.231735
+v -0.161202 1.785010 0.272981
+v -0.200373 1.785010 0.230875
+v -0.201129 1.785010 0.272121
+v -0.137050 1.756640 0.235571
+v -0.195826 1.756640 0.206731
+v -0.224525 1.756640 0.268286
+v -0.165749 1.756640 0.297125
+v -0.137050 1.715860 0.235571
+v -0.164105 1.756640 0.207415
+v -0.164105 1.715860 0.207415
+v -0.195826 1.715860 0.206731
+v -0.223891 1.756640 0.233700
+v -0.223891 1.715860 0.233700
+v -0.165749 1.715860 0.297125
+v -0.137684 1.756640 0.270157
+v -0.137684 1.715860 0.270157
+v -0.224525 1.715860 0.268286
+v -0.197470 1.756640 0.296441
+v -0.197470 1.715860 0.296441
+v -0.161841 1.691610 0.233120
+v -0.162545 1.691610 0.271537
+v -0.199030 1.691610 0.232319
+v -0.199734 1.691610 0.270736
+v -0.161841 1.668880 0.233120
+v -0.162545 1.668880 0.271537
+v -0.199030 1.668880 0.232319
+v -0.199734 1.668880 0.270736
+v -0.146260 1.655100 0.217653
+v -0.147543 1.655100 0.287664
+v -0.215315 1.655100 0.286203
+v -0.159884 1.900270 -0.001498
+v -0.160112 1.638960 0.214922
+v -0.199830 1.638960 0.214066
+v -0.199603 1.900270 -0.001498
+v -0.146260 0.683386 0.217653
+v -0.141462 0.682417 -0.001498
+v -0.209234 0.682417 -0.001498
+v -0.214032 0.683393 0.216193
+v -0.147543 0.738577 0.287664
+v -0.215315 0.710897 0.286203
+v -0.213681 0.377162 -0.001498
+v -0.214032 0.456870 0.216341
+v -0.213680 0.456870 -0.001498
+v -0.231718 0.471319 0.172684
+v -0.231718 0.471319 -0.001498
+v 0.261861 0.726253 0.301147
+v -0.134999 0.726253 0.301147
+v 0.245288 0.738577 0.287664
+v -0.133232 0.726253 0.204720
+v 0.246571 0.738577 0.217653
+v 0.263628 0.726253 0.204720
+v -0.133232 0.690088 0.204720
+v -0.147543 0.683217 0.287664
+v -0.134999 0.690088 0.301147
+v 0.263628 0.690088 0.204720
+v 0.261861 0.690088 0.301147
+v 0.249317 0.683217 0.287664
+v 0.250600 0.683217 0.217653
+v -0.147543 0.609148 0.288033
+v 0.181545 0.609148 0.286573
+v 0.182828 0.608985 0.216563
+v -0.146260 0.608985 0.218023
+v -0.146260 0.456870 0.217801
+v 0.182828 0.456870 0.216341
+v 0.250600 0.456870 0.217801
+v 0.247841 0.456870 -0.001498
+v 0.247847 0.377162 -0.001498
+v 0.262209 0.471319 0.172684
+v 0.262209 0.471319 -0.001498
+v -0.231718 0.520293 -0.001498
+v -0.231718 0.520293 0.172684
+v -0.223747 0.491776 0.204447
+v -0.163649 0.564918 -0.001498
+v -0.163649 0.564918 0.172684
+v 0.199481 0.564918 -0.001498
+v 0.199481 0.564918 0.172684
+v 0.262209 0.520293 -0.001498
+v 0.262209 0.520293 0.172684
+v -0.163649 0.522718 0.208096
+v 0.199480 0.522718 0.208096
+v 0.261171 0.491776 0.208368
+v 0.182828 0.377162 -0.001498
+v -0.147984 0.377162 -0.001498
+v -0.203961 0.754510 -0.214016
+v -0.199830 1.524170 -0.217063
+v -0.214032 1.655100 -0.219189
+v -0.214032 0.738577 -0.219189
+v -0.011230 0.210913 -0.218055
+v 0.057057 0.210913 -0.217724
+v -0.011230 0.275571 -0.218055
+v 0.057057 0.275571 -0.217724
+v -0.146260 0.206221 -0.220798
+v 0.182828 0.206221 -0.219337
+v 0.182828 0.280263 -0.219337
+v -0.146260 0.280263 -0.220798
+v -0.147543 0.206221 -0.290808
+v 0.181545 0.206221 -0.289348
+v -0.147543 0.280263 -0.290808
+v 0.181545 0.280263 -0.289348
+v -0.208590 -0.001787 -0.282672
+v -0.207557 -0.001787 -0.226297
+v -0.214032 0.243242 -0.219337
+v -0.215315 0.243242 -0.289348
+v -0.152985 -0.001787 -0.227473
+v -0.154018 -0.001787 -0.283848
+v 0.188270 -0.001787 -0.282672
+v 0.189303 -0.001787 -0.226297
+v 0.243875 -0.001787 -0.227473
+v 0.250600 0.243242 -0.220798
+v 0.242842 -0.001787 -0.283848
+v 0.249317 0.243242 -0.290808
+v -0.214032 0.377162 -0.219337
+v -0.146260 0.377162 -0.220798
+v 0.182828 0.377162 -0.219337
+v 0.250600 0.377162 -0.220798
+v -0.132386 1.472780 -0.179493
+v -0.146260 1.508020 -0.220649
+v -0.132386 0.781150 -0.179493
+v -0.146260 0.738577 -0.220649
+v -0.160112 1.524170 -0.217918
+v -0.174244 1.836230 -0.261959
+v -0.187584 1.836230 -0.261671
+v -0.187331 1.836230 -0.247891
+v -0.173991 1.836230 -0.248178
+v -0.160446 1.785010 -0.234732
+v -0.161202 1.785010 -0.275978
+v -0.200373 1.785010 -0.233871
+v -0.201129 1.785010 -0.275117
+v -0.137050 1.756640 -0.238567
+v -0.195826 1.756640 -0.209728
+v -0.224525 1.756640 -0.271282
+v -0.165749 1.756640 -0.300121
+v -0.164105 1.756640 -0.210411
+v -0.137050 1.715860 -0.238567
+v -0.164105 1.715860 -0.210411
+v -0.223891 1.756640 -0.236696
+v -0.195826 1.715860 -0.209728
+v -0.223891 1.715860 -0.236696
+v -0.137684 1.756640 -0.273153
+v -0.165749 1.715860 -0.300121
+v -0.137684 1.715860 -0.273153
+v -0.197470 1.756640 -0.299438
+v -0.224525 1.715860 -0.271282
+v -0.197470 1.715860 -0.299438
+v -0.161841 1.691610 -0.236117
+v -0.162545 1.691610 -0.274534
+v -0.199030 1.691610 -0.235315
+v -0.199734 1.691610 -0.273733
+v -0.161841 1.668880 -0.236117
+v -0.162545 1.668880 -0.274534
+v -0.199030 1.668880 -0.235315
+v -0.199734 1.668880 -0.273733
+v -0.146260 1.655100 -0.220649
+v -0.147543 1.655100 -0.290660
+v -0.215315 1.655100 -0.289200
+v -0.160112 1.638960 -0.217918
+v -0.199830 1.638960 -0.217063
+v -0.146260 0.683386 -0.220649
+v -0.214032 0.683393 -0.219189
+v -0.147543 0.738577 -0.290660
+v -0.215315 0.710897 -0.289200
+v -0.214032 0.456870 -0.219337
+v -0.231718 0.471319 -0.175680
+v 0.261861 0.726253 -0.304144
+v -0.134999 0.726253 -0.304144
+v 0.245288 0.738577 -0.290660
+v -0.133232 0.726253 -0.207717
+v 0.263628 0.726253 -0.207717
+v 0.246571 0.738577 -0.220649
+v -0.133232 0.690088 -0.207717
+v -0.134999 0.690088 -0.304144
+v -0.147543 0.683217 -0.290660
+v 0.263628 0.690088 -0.207717
+v 0.261861 0.690088 -0.304144
+v 0.249317 0.683217 -0.290660
+v 0.250600 0.683217 -0.220649
+v 0.181545 0.609149 -0.289570
+v -0.147543 0.609149 -0.291030
+v 0.182828 0.608985 -0.219559
+v -0.146260 0.608985 -0.221019
+v -0.146260 0.456870 -0.220798
+v 0.182828 0.456870 -0.219337
+v 0.250600 0.456870 -0.220798
+v 0.262209 0.471319 -0.175680
+v -0.231718 0.520293 -0.175680
+v -0.223747 0.491776 -0.207443
+v -0.163649 0.564918 -0.175680
+v 0.199481 0.564918 -0.175680
+v 0.262209 0.520293 -0.175680
+v -0.163649 0.522718 -0.211092
+v 0.199480 0.522718 -0.211092
+v 0.261171 0.491776 -0.211365
+f 1 2 3
+f 2 1 4
+f 5 4 6
+f 1 6 4
+f 6 1 7
+f 3 7 1
+f 8 9 10
+f 9 8 11
+f 10 12 13
+f 12 10 9
+f 13 14 15
+f 14 13 12
+f 15 11 8
+f 11 15 14
+f 11 16 9
+f 16 11 17
+f 12 18 14
+f 18 12 19
+f 9 19 12
+f 19 9 16
+f 14 17 11
+f 17 14 18
+f 16 20 21
+f 20 16 17
+f 21 22 23
+f 22 21 20
+f 23 18 19
+f 18 23 22
+f 24 25 26
+f 25 24 27
+f 26 16 28
+f 16 26 25
+f 28 21 29
+f 21 28 16
+f 29 27 24
+f 27 29 21
+f 30 17 31
+f 17 30 20
+f 31 32 33
+f 32 31 17
+f 33 34 35
+f 34 33 32
+f 35 20 30
+f 20 35 34
+f 25 19 16
+f 27 21 23
+f 32 17 18
+f 34 22 20
+f 27 36 25
+f 23 19 37
+f 22 38 18
+f 34 32 39
+f 18 39 32
+f 39 18 38
+f 25 37 19
+f 37 25 36
+f 40 41 42
+f 41 40 43
+f 44 45 46
+f 45 44 47
+f 42 47 44
+f 47 42 41
+f 48 43 49
+f 43 48 41
+f 46 40 44
+f 42 44 40
+f 50 51 52
+f 51 50 53
+f 54 50 55
+f 50 54 53
+f 56 53 54
+f 53 56 51
+f 57 51 56
+f 51 57 52
+f 55 52 57
+f 52 55 50
+f 55 58 54
+f 54 59 56
+f 56 60 57
+f 57 61 55
+f 58 62 63
+f 64 63 62
+f 59 65 66
+f 67 66 65
+f 61 68 69
+f 70 69 68
+f 60 71 72
+f 73 72 71
+f 74 70 75
+f 76 64 74
+f 77 67 76
+f 75 73 77
+f 58 55 69
+f 59 54 63
+f 60 56 66
+f 61 57 72
+f 70 74 62
+f 64 76 65
+f 67 77 71
+f 73 75 68
+f 55 61 69
+f 58 63 54
+f 60 72 57
+f 59 66 56
+f 68 75 70
+f 74 64 62
+f 77 73 71
+f 76 67 65
+f 69 62 58
+f 62 69 70
+f 63 65 59
+f 65 63 64
+f 66 71 60
+f 71 66 67
+f 72 68 61
+f 68 72 73
+f 75 78 74
+f 78 75 79
+f 74 80 76
+f 80 74 78
+f 76 81 77
+f 81 76 80
+f 77 79 75
+f 79 77 81
+f 78 5 80
+f 5 78 82
+f 79 82 78
+f 82 79 83
+f 81 83 79
+f 83 81 84
+f 80 84 81
+f 84 80 5
+f 85 48 49
+f 48 85 86
+f 2 87 88
+f 87 2 4
+f 88 86 85
+f 86 88 87
+f 86 41 48
+f 41 86 82
+f 87 82 86
+f 82 87 5
+f 4 5 87
+f 83 41 82
+f 45 89 90
+f 89 45 47
+f 91 6 7
+f 6 91 92
+f 90 92 91
+f 92 90 89
+f 83 47 41
+f 47 83 93
+f 84 93 83
+f 93 84 94
+f 5 94 84
+f 94 5 6
+f 95 96 97
+f 96 95 36
+f 97 98 99
+f 98 97 96
+f 100 93 101
+f 93 100 102
+f 103 104 105
+f 104 103 47
+f 105 102 100
+f 102 105 104
+f 104 93 102
+f 93 104 47
+f 106 47 103
+f 47 106 89
+f 101 107 108
+f 107 101 93
+f 94 107 93
+f 94 6 92
+f 100 109 105
+f 109 100 110
+f 110 107 111
+f 107 110 108
+f 112 106 109
+f 106 112 89
+f 111 113 114
+f 113 111 107
+f 115 89 112
+f 89 115 116
+f 114 116 115
+f 116 114 113
+f 105 106 103
+f 106 105 109
+f 101 110 100
+f 110 101 108
+f 94 113 107
+f 92 89 116
+f 27 92 96
+f 92 27 94
+f 117 113 23
+f 113 117 116
+f 23 94 27
+f 94 23 113
+f 96 116 117
+f 116 96 92
+f 117 23 37
+f 36 27 96
+f 28 24 26
+f 24 28 29
+f 112 110 111
+f 110 112 109
+f 22 115 118
+f 115 22 114
+f 119 111 34
+f 111 119 112
+f 34 114 22
+f 114 34 111
+f 118 112 119
+f 112 118 115
+f 119 34 39
+f 38 22 118
+f 33 30 31
+f 30 33 35
+f 39 120 119
+f 120 39 121
+f 117 38 118
+f 38 117 37
+f 120 122 119
+f 122 120 123
+f 124 98 125
+f 98 124 99
+f 126 98 96
+f 98 126 125
+f 127 125 128
+f 125 127 124
+f 129 128 130
+f 128 129 127
+f 131 130 132
+f 130 131 129
+f 133 125 126
+f 125 133 128
+f 134 128 133
+f 128 134 130
+f 135 130 134
+f 130 135 132
+f 123 132 122
+f 132 123 131
+f 119 132 135
+f 132 119 122
+f 121 38 136
+f 38 121 39
+f 137 38 37
+f 38 137 136
+f 137 36 95
+f 36 137 37
+f 118 135 134
+f 135 118 119
+f 117 134 133
+f 134 117 118
+f 96 133 126
+f 133 96 117
+f 138 3 2
+f 2 139 138
+f 140 141 139
+f 138 139 141
+f 141 7 138
+f 3 138 7
+f 8 10 142
+f 142 143 8
+f 10 13 144
+f 144 142 10
+f 13 15 145
+f 145 144 13
+f 15 8 143
+f 143 145 15
+f 143 142 146
+f 146 147 143
+f 144 145 148
+f 148 149 144
+f 142 144 149
+f 149 146 142
+f 145 143 147
+f 147 148 145
+f 146 150 151
+f 151 147 146
+f 150 152 153
+f 153 151 150
+f 152 149 148
+f 148 153 152
+f 154 155 156
+f 156 157 154
+f 155 158 146
+f 146 156 155
+f 158 159 150
+f 150 146 158
+f 159 154 157
+f 157 150 159
+f 160 161 147
+f 147 151 160
+f 161 162 163
+f 163 147 161
+f 162 164 165
+f 165 163 162
+f 164 160 151
+f 151 165 164
+f 156 146 149
+f 157 152 150
+f 163 148 147
+f 165 151 153
+f 157 156 166
+f 152 167 149
+f 153 148 168
+f 165 169 163
+f 148 163 169
+f 169 168 148
+f 156 149 167
+f 167 166 156
+f 40 170 171
+f 171 43 40
+f 172 46 45
+f 45 173 172
+f 170 172 173
+f 173 171 170
+f 174 49 43
+f 43 171 174
+f 46 172 40
+f 170 40 172
+f 175 176 177
+f 177 178 175
+f 179 180 175
+f 175 178 179
+f 181 179 178
+f 178 177 181
+f 182 181 177
+f 177 176 182
+f 180 182 176
+f 176 175 180
+f 180 179 183
+f 179 181 184
+f 181 182 185
+f 182 180 186
+f 183 187 188
+f 189 188 187
+f 184 190 191
+f 192 191 190
+f 186 193 194
+f 195 194 193
+f 185 196 197
+f 198 197 196
+f 199 200 195
+f 201 199 189
+f 202 201 192
+f 200 202 198
+f 183 193 180
+f 184 187 179
+f 185 190 181
+f 186 196 182
+f 195 188 199
+f 189 191 201
+f 192 197 202
+f 198 194 200
+f 180 193 186
+f 183 179 187
+f 185 182 196
+f 184 181 190
+f 194 195 200
+f 199 188 189
+f 202 197 198
+f 201 191 192
+f 193 183 188
+f 188 195 193
+f 187 184 191
+f 191 189 187
+f 190 185 197
+f 197 192 190
+f 196 186 194
+f 194 198 196
+f 200 199 203
+f 203 204 200
+f 199 201 205
+f 205 203 199
+f 201 202 206
+f 206 205 201
+f 202 200 204
+f 204 206 202
+f 203 205 140
+f 140 207 203
+f 204 203 207
+f 207 208 204
+f 206 204 208
+f 208 209 206
+f 205 206 209
+f 209 140 205
+f 85 49 174
+f 174 210 85
+f 2 88 211
+f 211 139 2
+f 88 85 210
+f 210 211 88
+f 210 174 171
+f 171 207 210
+f 211 210 207
+f 207 140 211
+f 139 211 140
+f 208 207 171
+f 45 90 212
+f 212 173 45
+f 91 7 141
+f 141 213 91
+f 90 91 213
+f 213 212 90
+f 208 171 173
+f 173 214 208
+f 209 208 214
+f 214 215 209
+f 140 209 215
+f 215 141 140
+f 95 97 216
+f 216 166 95
+f 97 99 217
+f 217 216 97
+f 218 219 214
+f 214 220 218
+f 221 222 223
+f 223 173 221
+f 222 218 220
+f 220 223 222
+f 223 220 214
+f 214 173 223
+f 224 221 173
+f 173 212 224
+f 219 225 226
+f 226 214 219
+f 215 214 226
+f 215 213 141
+f 218 222 227
+f 227 228 218
+f 228 229 226
+f 226 225 228
+f 230 227 224
+f 224 212 230
+f 229 231 232
+f 232 226 229
+f 233 230 212
+f 212 234 233
+f 231 233 234
+f 234 232 231
+f 222 221 224
+f 224 227 222
+f 219 218 228
+f 228 225 219
+f 215 226 232
+f 213 234 212
+f 157 216 213
+f 213 215 157
+f 235 152 232
+f 232 234 235
+f 152 157 215
+f 215 232 152
+f 216 235 234
+f 234 213 216
+f 235 167 152
+f 166 216 157
+f 158 155 154
+f 154 159 158
+f 230 229 228
+f 228 227 230
+f 153 236 233
+f 233 231 153
+f 237 165 229
+f 229 230 237
+f 165 153 231
+f 231 229 165
+f 236 237 230
+f 230 233 236
+f 237 169 165
+f 168 236 153
+f 162 161 160
+f 160 164 162
+f 169 237 120
+f 120 121 169
+f 235 236 168
+f 168 167 235
+f 120 237 238
+f 238 123 120
+f 124 239 217
+f 217 99 124
+f 240 216 217
+f 217 239 240
+f 127 241 239
+f 239 124 127
+f 129 242 241
+f 241 127 129
+f 131 243 242
+f 242 129 131
+f 244 240 239
+f 239 241 244
+f 245 244 241
+f 241 242 245
+f 246 245 242
+f 242 243 246
+f 123 238 243
+f 243 131 123
+f 237 246 243
+f 243 238 237
+f 121 136 168
+f 168 169 121
+f 137 167 168
+f 168 136 137
+f 137 95 166
+f 166 167 137
+f 236 245 246
+f 246 237 236
+f 235 244 245
+f 245 236 235
+f 216 240 244
+f 244 235 216

examples3d/media/models/table.obj

@@ -0,0 +1,142 @@
+# 48
+# 92
+v -4.000000 0.875000 2.000000
+v -4.000000 1.125000 2.000000
+v 4.000000 1.125000 2.000000
+v 4.000000 0.875000 2.000000
+v -4.000000 0.875000 -2.000000
+v -4.000000 1.125000 -2.000000
+v 4.000000 1.125000 -2.000000
+v 4.000000 0.875000 -2.000000
+v -3.760000 0.875000 2.000000
+v -3.440000 0.875000 2.000000
+v 3.440000 0.875000 2.000000
+v 3.760000 0.875000 2.000000
+v -3.760000 0.875000 -2.000000
+v -3.440000 0.875000 -2.000000
+v 3.440000 0.875000 -2.000000
+v 3.760000 0.875000 -2.000000
+v -3.760000 0.875000 1.760000
+v -3.760000 0.875000 1.440000
+v -3.760000 0.875000 -1.440000
+v -3.760000 0.875000 -1.760000
+v -3.440000 0.875000 1.760000
+v -3.440000 0.875000 1.440000
+v -3.440000 0.875000 -1.440000
+v -3.440000 0.875000 -1.760000
+v 3.440000 0.875000 1.760000
+v 3.440000 0.875000 1.440000
+v 3.440000 0.875000 -1.440000
+v 3.440000 0.875000 -1.760000
+v 3.760000 0.875000 1.760000
+v 3.760000 0.875000 1.440000
+v 3.760000 0.875000 -1.440000
+v 3.760000 0.875000 -1.760000
+v 3.440000 -2.125000 -1.760000
+v 3.760000 -2.125000 -1.760000
+v 3.760000 -2.125000 -1.440000
+v 3.440000 -2.125000 -1.440000
+v 3.440000 -2.125000 1.440000
+v 3.760000 -2.125000 1.440000
+v 3.760000 -2.125000 1.760000
+v 3.440000 -2.125000 1.760000
+v -3.760000 -2.125000 -1.760000
+v -3.440000 -2.125000 -1.760000
+v -3.440000 -2.125000 -1.440000
+v -3.760000 -2.125000 -1.440000
+v -3.760000 -2.125000 1.440000
+v -3.440000 -2.125000 1.440000
+v -3.440000 -2.125000 1.760000
+v -3.760000 -2.125000 1.760000
+f 1 6 5
+f 1 9 2
+f 1 17 9
+f 2 6 1
+f 2 10 3
+f 3 6 2
+f 3 8 7
+f 3 12 4
+f 4 8 3
+f 4 31 8
+f 5 19 1
+f 6 13 5
+f 7 6 3
+f 7 15 6
+f 8 16 7
+f 8 32 16
+f 9 17 10
+f 10 2 9
+f 10 17 21
+f 10 21 11
+f 11 3 10
+f 11 21 25
+f 11 25 12
+f 12 3 11
+f 12 25 29
+f 12 29 4
+f 13 6 14
+f 13 20 5
+f 13 24 20
+f 14 6 15
+f 14 24 13
+f 14 28 24
+f 15 7 16
+f 15 28 14
+f 15 32 28
+f 16 32 15
+f 17 1 18
+f 17 47 21
+f 18 1 19
+f 18 45 17
+f 19 5 20
+f 19 22 18
+f 19 43 23
+f 20 41 19
+f 21 47 22
+f 22 19 23
+f 22 25 21
+f 22 45 18
+f 23 26 22
+f 23 43 24
+f 24 27 23
+f 24 41 20
+f 25 22 26
+f 25 39 29
+f 26 23 27
+f 26 37 25
+f 27 24 28
+f 27 30 26
+f 27 35 31
+f 28 33 27
+f 29 39 30
+f 30 4 29
+f 30 27 31
+f 30 37 26
+f 31 4 30
+f 31 35 32
+f 32 8 31
+f 32 33 28
+f 33 32 34
+f 34 32 35
+f 34 35 33
+f 35 27 36
+f 36 27 33
+f 36 33 35
+f 37 30 38
+f 38 30 39
+f 38 39 37
+f 39 25 40
+f 40 25 37
+f 40 37 39
+f 41 24 42
+f 42 24 43
+f 42 43 41
+f 43 19 44
+f 44 19 41
+f 44 41 43
+f 45 22 46
+f 46 22 47
+f 46 47 45
+f 47 17 48
+f 48 17 45
+f 48 45 47

examples3d/media/models/tstTorusModel.obj

@@ -0,0 +1,866 @@
+# 288
+# 576
+v -5.347100 14.901700 -54.360001
+v 4.640000 20.457001 -51.556599
+v 5.025300 14.901700 -52.994400
+v -5.347100 20.457001 -52.871498
+v 3.587500 24.523701 -47.628502
+v -5.347100 24.523701 -48.804699
+v 2.149700 26.012300 -42.262501
+v -5.347100 26.012300 -43.249401
+v 0.711800 24.523701 -36.896500
+v -5.347100 24.523701 -37.694099
+v -0.340700 20.457001 -32.968300
+v -5.347100 20.457001 -33.627399
+v -0.726000 14.901700 -31.530500
+v -5.347100 14.901700 -32.138901
+v -0.340700 9.346400 -32.968300
+v -5.347100 9.346400 -33.627399
+v 0.711800 5.279700 -36.896500
+v -5.347100 5.279700 -37.694099
+v 2.149700 3.791200 -42.262501
+v -5.347100 3.791200 -43.249401
+v 3.587500 5.279700 -47.628399
+v -5.347100 5.279700 -48.804699
+v 4.640000 9.346400 -51.556599
+v -5.347100 9.346400 -52.871498
+v 13.946500 20.457001 -47.701801
+v 14.690800 14.901700 -48.990898
+v 11.913100 24.523701 -44.179901
+v 9.135500 26.012300 -39.368801
+v 6.357800 24.523701 -34.557800
+v 4.324500 20.457001 -31.035900
+v 3.580200 14.901700 -29.746799
+v 4.324500 9.346400 -31.035900
+v 6.357800 5.279700 -34.557800
+v 9.135500 3.791200 -39.368801
+v 11.913100 5.279700 -44.179901
+v 13.946500 9.346400 -47.701801
+v 21.938200 20.457001 -41.569500
+v 22.990700 14.901700 -42.622101
+v 19.062500 24.523701 -38.693901
+v 15.134400 26.012300 -34.765701
+v 11.206200 24.523701 -30.837601
+v 8.330600 20.457001 -27.961901
+v 7.278000 14.901700 -26.909401
+v 8.330600 9.346400 -27.961901
+v 11.206200 5.279700 -30.837601
+v 15.134400 3.791200 -34.765701
+v 19.062500 5.279700 -38.693901
+v 21.938200 9.346400 -41.569500
+v 28.070400 20.457001 -33.577900
+v 29.359501 14.901700 -34.322201
+v 24.548500 24.523701 -31.544500
+v 19.737400 26.012300 -28.766899
+v 14.926400 24.523701 -25.989201
+v 11.404500 20.457001 -23.955900
+v 10.115400 14.901700 -23.211599
+v 11.404500 9.346400 -23.955900
+v 14.926400 5.279700 -25.989201
+v 19.737400 3.791200 -28.766899
+v 24.548500 5.279700 -31.544500
+v 28.070400 9.346400 -33.577900
+v 31.925200 20.457001 -24.271400
+v 33.362999 14.901700 -24.656700
+v 27.997101 24.523701 -23.218901
+v 22.631100 26.012300 -21.781099
+v 17.265100 24.523701 -20.343201
+v 13.336900 20.457001 -19.290701
+v 11.899100 14.901700 -18.905399
+v 13.336900 9.346400 -19.290701
+v 17.265100 5.279700 -20.343201
+v 22.631100 3.791200 -21.781000
+v 27.997101 5.279700 -23.218901
+v 31.925200 9.346400 -24.271400
+v 33.240101 20.457001 -14.284300
+v 34.728600 14.901700 -14.284300
+v 29.173300 24.523701 -14.284300
+v 23.618000 26.012300 -14.284300
+v 18.062799 24.523701 -14.284300
+v 13.996000 20.457001 -14.284300
+v 12.507500 14.901700 -14.284300
+v 13.996000 9.346400 -14.284300
+v 18.062799 5.279700 -14.284300
+v 23.618000 3.791200 -14.284300
+v 29.173300 5.279700 -14.284300
+v 33.240101 9.346400 -14.284300
+v 31.925200 20.457001 -4.297200
+v 33.362999 14.901700 -3.912000
+v 27.997101 24.523701 -5.349800
+v 22.631100 26.012300 -6.787600
+v 17.265100 24.523701 -8.225400
+v 13.336900 20.457001 -9.278000
+v 11.899100 14.901700 -9.663200
+v 13.336900 9.346400 -9.278000
+v 17.265100 5.279700 -8.225400
+v 22.631100 3.791200 -6.787600
+v 27.997101 5.279700 -5.349800
+v 31.925200 9.346400 -4.297300
+v 28.070400 20.457001 5.009200
+v 29.359501 14.901700 5.753500
+v 24.548500 24.523701 2.975900
+v 19.737400 26.012300 0.198200
+v 14.926400 24.523701 -2.579400
+v 11.404500 20.457001 -4.612800
+v 10.115400 14.901700 -5.357100
+v 11.404500 9.346400 -4.612800
+v 14.926400 5.279700 -2.579400
+v 19.737400 3.791200 0.198200
+v 24.548500 5.279700 2.975900
+v 28.070400 9.346400 5.009200
+v 21.938200 20.457001 13.000900
+v 22.990700 14.901700 14.053400
+v 19.062500 24.523701 10.125300
+v 15.134400 26.012300 6.197100
+v 11.206200 24.523701 2.268900
+v 8.330600 20.457001 -0.606700
+v 7.278000 14.901700 -1.659300
+v 8.330600 9.346400 -0.606700
+v 11.206200 5.279700 2.268900
+v 15.134400 3.791200 6.197100
+v 19.062500 5.279700 10.125300
+v 21.938200 9.346400 13.000900
+v 13.946500 20.457001 19.133101
+v 14.690800 14.901700 20.422199
+v 11.913100 24.523701 15.611200
+v 9.135500 26.012300 10.800200
+v 6.357800 24.523701 5.989200
+v 4.324500 20.457001 2.467300
+v 3.580200 14.901700 1.178200
+v 4.324500 9.346400 2.467300
+v 6.357800 5.279700 5.989200
+v 9.135500 3.791200 10.800200
+v 11.913100 5.279700 15.611200
+v 13.946500 9.346400 19.133101
+v 4.640000 20.457001 22.988001
+v 5.025300 14.901700 24.425800
+v 3.587500 24.523701 19.059799
+v 2.149700 26.012300 13.693800
+v 0.711800 24.523701 8.327800
+v -0.340700 20.457001 4.399600
+v -0.726000 14.901700 2.961800
+v -0.340700 9.346400 4.399600
+v 0.711800 5.279700 8.327800
+v 2.149700 3.791200 13.693800
+v 3.587500 5.279700 19.059799
+v 4.640000 9.346400 22.988001
+v -5.347100 20.457001 24.302799
+v -5.347100 14.901700 25.791300
+v -5.347100 24.523701 20.236000
+v -5.347100 26.012300 14.680800
+v -5.347100 24.523701 9.125500
+v -5.347100 20.457001 5.058700
+v -5.347100 14.901700 3.570200
+v -5.347100 9.346400 5.058700
+v -5.347100 5.279700 9.125500
+v -5.347100 3.791200 14.680800
+v -5.347100 5.279700 20.236000
+v -5.347100 9.346400 24.302799
+v -15.334100 20.457001 22.988001
+v -15.719400 14.901700 24.425800
+v -14.281600 24.523701 19.059799
+v -12.843800 26.012300 13.693800
+v -11.406000 24.523701 8.327800
+v -10.353400 20.457001 4.399600
+v -9.968200 14.901700 2.961800
+v -10.353400 9.346400 4.399600
+v -11.406000 5.279700 8.327800
+v -12.843800 3.791200 13.693800
+v -14.281600 5.279700 19.059799
+v -15.334100 9.346400 22.988001
+v -24.640600 20.457001 19.133101
+v -25.384899 14.901700 20.422199
+v -22.607201 24.523701 15.611200
+v -19.829599 26.012300 10.800200
+v -17.052000 24.523701 5.989200
+v -15.018600 20.457001 2.467300
+v -14.274300 14.901700 1.178200
+v -15.018600 9.346400 2.467300
+v -17.052000 5.279700 5.989200
+v -19.829599 3.791200 10.800200
+v -22.607201 5.279700 15.611200
+v -24.640600 9.346400 19.133101
+v -32.632301 20.457001 13.000900
+v -33.684799 14.901700 14.053400
+v -29.756599 24.523701 10.125300
+v -25.828501 26.012300 6.197100
+v -21.900299 24.523701 2.268900
+v -19.024700 20.457001 -0.606700
+v -17.972099 14.901700 -1.659300
+v -19.024700 9.346400 -0.606700
+v -21.900299 5.279700 2.268900
+v -25.828501 3.791200 6.197100
+v -29.756599 5.279700 10.125300
+v -32.632301 9.346400 13.000900
+v -38.764500 20.457001 5.009200
+v -40.053600 14.901700 5.753500
+v -35.242599 24.523701 2.975900
+v -30.431601 26.012300 0.198200
+v -25.620600 24.523701 -2.579400
+v -22.098600 20.457001 -4.612800
+v -20.809500 14.901700 -5.357100
+v -22.098600 9.346400 -4.612800
+v -25.620600 5.279700 -2.579400
+v -30.431601 3.791200 0.198200
+v -35.242599 5.279700 2.975900
+v -38.764500 9.346400 5.009200
+v -42.619400 20.457001 -4.297200
+v -44.057201 14.901700 -3.912000
+v -38.691200 24.523701 -5.349800
+v -33.325199 26.012300 -6.787600
+v -27.959200 24.523701 -8.225400
+v -24.031000 20.457001 -9.278000
+v -22.593201 14.901700 -9.663200
+v -24.031000 9.346400 -9.278000
+v -27.959200 5.279700 -8.225400
+v -33.325199 3.791200 -6.787600
+v -38.691200 5.279700 -5.349800
+v -42.619400 9.346400 -4.297200
+v -43.934200 20.457001 -14.284300
+v -45.422699 14.901700 -14.284300
+v -39.867401 24.523701 -14.284300
+v -34.312199 26.012300 -14.284300
+v -28.756901 24.523701 -14.284300
+v -24.690100 20.457001 -14.284300
+v -23.201599 14.901700 -14.284300
+v -24.690100 9.346400 -14.284300
+v -28.756901 5.279700 -14.284300
+v -34.312199 3.791200 -14.284300
+v -39.867401 5.279700 -14.284300
+v -43.934200 9.346400 -14.284300
+v -42.619400 20.457001 -24.271400
+v -44.057201 14.901700 -24.656700
+v -38.691200 24.523701 -23.218800
+v -33.325199 26.012300 -21.781000
+v -27.959200 24.523701 -20.343201
+v -24.031000 20.457001 -19.290701
+v -22.593201 14.901700 -18.905399
+v -24.031000 9.346400 -19.290701
+v -27.959200 5.279700 -20.343201
+v -33.325199 3.791200 -21.781000
+v -38.691200 5.279700 -23.218800
+v -42.619400 9.346400 -24.271400
+v -38.764500 20.457001 -33.577900
+v -40.053600 14.901700 -34.322102
+v -35.242599 24.523701 -31.544500
+v -30.431601 26.012300 -28.766899
+v -25.620600 24.523701 -25.989201
+v -22.098700 20.457001 -23.955900
+v -20.809500 14.901700 -23.211599
+v -22.098700 9.346400 -23.955900
+v -25.620600 5.279700 -25.989201
+v -30.431601 3.791200 -28.766899
+v -35.242599 5.279700 -31.544500
+v -38.764500 9.346400 -33.577900
+v -32.632301 20.457001 -41.569500
+v -33.684799 14.901700 -42.622101
+v -29.756701 24.523701 -38.693901
+v -25.828501 26.012300 -34.765701
+v -21.900299 24.523701 -30.837601
+v -19.024700 20.457001 -27.961901
+v -17.972099 14.901700 -26.909401
+v -19.024700 9.346400 -27.961901
+v -21.900299 5.279700 -30.837601
+v -25.828501 3.791200 -34.765701
+v -29.756701 5.279700 -38.693901
+v -32.632301 9.346400 -41.569500
+v -24.640600 20.457001 -47.701801
+v -25.384899 14.901700 -48.990898
+v -22.607300 24.523701 -44.179798
+v -19.829599 26.012300 -39.368801
+v -17.052000 24.523701 -34.557800
+v -15.018600 20.457001 -31.035900
+v -14.274300 14.901700 -29.746799
+v -15.018600 9.346400 -31.035900
+v -17.052000 5.279700 -34.557800
+v -19.829599 3.791200 -39.368801
+v -22.607300 5.279700 -44.179798
+v -24.640600 9.346400 -47.701801
+v -15.334200 20.457001 -51.556599
+v -15.719400 14.901700 -52.994400
+v -14.281600 24.523701 -47.628502
+v -12.843800 26.012300 -42.262501
+v -11.406000 24.523701 -36.896500
+v -10.353400 20.457001 -32.968300
+v -9.968200 14.901700 -31.530500
+v -10.353400 9.346400 -32.968300
+v -11.406000 5.279700 -36.896500
+v -12.843800 3.791200 -42.262501
+v -14.281600 5.279700 -47.628399
+v -15.334200 9.346400 -51.556599
+f 1 2 3
+f 1 4 2
+f 4 5 2
+f 4 6 5
+f 6 7 5
+f 6 8 7
+f 8 9 7
+f 8 10 9
+f 10 11 9
+f 10 12 11
+f 12 13 11
+f 12 14 13
+f 14 15 13
+f 14 16 15
+f 16 17 15
+f 16 18 17
+f 18 19 17
+f 18 20 19
+f 20 21 19
+f 20 22 21
+f 22 23 21
+f 22 24 23
+f 24 3 23
+f 24 1 3
+f 3 25 26
+f 3 2 25
+f 2 27 25
+f 2 5 27
+f 5 28 27
+f 5 7 28
+f 7 29 28
+f 7 9 29
+f 9 30 29
+f 9 11 30
+f 11 31 30
+f 11 13 31
+f 13 32 31
+f 13 15 32
+f 15 33 32
+f 15 17 33
+f 17 34 33
+f 17 19 34
+f 19 35 34
+f 19 21 35
+f 21 36 35
+f 21 23 36
+f 23 26 36
+f 23 3 26
+f 26 37 38
+f 26 25 37
+f 25 39 37
+f 25 27 39
+f 27 40 39
+f 27 28 40
+f 28 41 40
+f 28 29 41
+f 29 42 41
+f 29 30 42
+f 30 43 42
+f 30 31 43
+f 31 44 43
+f 31 32 44
+f 32 45 44
+f 32 33 45
+f 33 46 45
+f 33 34 46
+f 34 47 46
+f 34 35 47
+f 35 48 47
+f 35 36 48
+f 36 38 48
+f 36 26 38
+f 38 49 50
+f 38 37 49
+f 37 51 49
+f 37 39 51
+f 39 52 51
+f 39 40 52
+f 40 53 52
+f 40 41 53
+f 41 54 53
+f 41 42 54
+f 42 55 54
+f 42 43 55
+f 43 56 55
+f 43 44 56
+f 44 57 56
+f 44 45 57
+f 45 58 57
+f 45 46 58
+f 46 59 58
+f 46 47 59
+f 47 60 59
+f 47 48 60
+f 48 50 60
+f 48 38 50
+f 50 61 62
+f 50 49 61
+f 49 63 61
+f 49 51 63
+f 51 64 63
+f 51 52 64
+f 52 65 64
+f 52 53 65
+f 53 66 65
+f 53 54 66
+f 54 67 66
+f 54 55 67
+f 55 68 67
+f 55 56 68
+f 56 69 68
+f 56 57 69
+f 57 70 69
+f 57 58 70
+f 58 71 70
+f 58 59 71
+f 59 72 71
+f 59 60 72
+f 60 62 72
+f 60 50 62
+f 62 73 74
+f 62 61 73
+f 61 75 73
+f 61 63 75
+f 63 76 75
+f 63 64 76
+f 64 77 76
+f 64 65 77
+f 65 78 77
+f 65 66 78
+f 66 79 78
+f 66 67 79
+f 67 80 79
+f 67 68 80
+f 68 81 80
+f 68 69 81
+f 69 82 81
+f 69 70 82
+f 70 83 82
+f 70 71 83
+f 71 84 83
+f 71 72 84
+f 72 74 84
+f 72 62 74
+f 74 85 86
+f 74 73 85
+f 73 87 85
+f 73 75 87
+f 75 88 87
+f 75 76 88
+f 76 89 88
+f 76 77 89
+f 77 90 89
+f 77 78 90
+f 78 91 90
+f 78 79 91
+f 79 92 91
+f 79 80 92
+f 80 93 92
+f 80 81 93
+f 81 94 93
+f 81 82 94
+f 82 95 94
+f 82 83 95
+f 83 96 95
+f 83 84 96
+f 84 86 96
+f 84 74 86
+f 86 97 98
+f 86 85 97
+f 85 99 97
+f 85 87 99
+f 87 100 99
+f 87 88 100
+f 88 101 100
+f 88 89 101
+f 89 102 101
+f 89 90 102
+f 90 103 102
+f 90 91 103
+f 91 104 103
+f 91 92 104
+f 92 105 104
+f 92 93 105
+f 93 106 105
+f 93 94 106
+f 94 107 106
+f 94 95 107
+f 95 108 107
+f 95 96 108
+f 96 98 108
+f 96 86 98
+f 98 109 110
+f 98 97 109
+f 97 111 109
+f 97 99 111
+f 99 112 111
+f 99 100 112
+f 100 113 112
+f 100 101 113
+f 101 114 113
+f 101 102 114
+f 102 115 114
+f 102 103 115
+f 103 116 115
+f 103 104 116
+f 104 117 116
+f 104 105 117
+f 105 118 117
+f 105 106 118
+f 106 119 118
+f 106 107 119
+f 107 120 119
+f 107 108 120
+f 108 110 120
+f 108 98 110
+f 110 121 122
+f 110 109 121
+f 109 123 121
+f 109 111 123
+f 111 124 123
+f 111 112 124
+f 112 125 124
+f 112 113 125
+f 113 126 125
+f 113 114 126
+f 114 127 126
+f 114 115 127
+f 115 128 127
+f 115 116 128
+f 116 129 128
+f 116 117 129
+f 117 130 129
+f 117 118 130
+f 118 131 130
+f 118 119 131
+f 119 132 131
+f 119 120 132
+f 120 122 132
+f 120 110 122
+f 122 133 134
+f 122 121 133
+f 121 135 133
+f 121 123 135
+f 123 136 135
+f 123 124 136
+f 124 137 136
+f 124 125 137
+f 125 138 137
+f 125 126 138
+f 126 139 138
+f 126 127 139
+f 127 140 139
+f 127 128 140
+f 128 141 140
+f 128 129 141
+f 129 142 141
+f 129 130 142
+f 130 143 142
+f 130 131 143
+f 131 144 143
+f 131 132 144
+f 132 134 144
+f 132 122 134
+f 134 145 146
+f 134 133 145
+f 133 147 145
+f 133 135 147
+f 135 148 147
+f 135 136 148
+f 136 149 148
+f 136 137 149
+f 137 150 149
+f 137 138 150
+f 138 151 150
+f 138 139 151
+f 139 152 151
+f 139 140 152
+f 140 153 152
+f 140 141 153
+f 141 154 153
+f 141 142 154
+f 142 155 154
+f 142 143 155
+f 143 156 155
+f 143 144 156
+f 144 146 156
+f 144 134 146
+f 146 157 158
+f 146 145 157
+f 145 159 157
+f 145 147 159
+f 147 160 159
+f 147 148 160
+f 148 161 160
+f 148 149 161
+f 149 162 161
+f 149 150 162
+f 150 163 162
+f 150 151 163
+f 151 164 163
+f 151 152 164
+f 152 165 164
+f 152 153 165
+f 153 166 165
+f 153 154 166
+f 154 167 166
+f 154 155 167
+f 155 168 167
+f 155 156 168
+f 156 158 168
+f 156 146 158
+f 158 169 170
+f 158 157 169
+f 157 171 169
+f 157 159 171
+f 159 172 171
+f 159 160 172
+f 160 173 172
+f 160 161 173
+f 161 174 173
+f 161 162 174
+f 162 175 174
+f 162 163 175
+f 163 176 175
+f 163 164 176
+f 164 177 176
+f 164 165 177
+f 165 178 177
+f 165 166 178
+f 166 179 178
+f 166 167 179
+f 167 180 179
+f 167 168 180
+f 168 170 180
+f 168 158 170
+f 170 181 182
+f 170 169 181
+f 169 183 181
+f 169 171 183
+f 171 184 183
+f 171 172 184
+f 172 185 184
+f 172 173 185
+f 173 186 185
+f 173 174 186
+f 174 187 186
+f 174 175 187
+f 175 188 187
+f 175 176 188
+f 176 189 188
+f 176 177 189
+f 177 190 189
+f 177 178 190
+f 178 191 190
+f 178 179 191
+f 179 192 191
+f 179 180 192
+f 180 182 192
+f 180 170 182
+f 182 193 194
+f 182 181 193
+f 181 195 193
+f 181 183 195
+f 183 196 195
+f 183 184 196
+f 184 197 196
+f 184 185 197
+f 185 198 197
+f 185 186 198
+f 186 199 198
+f 186 187 199
+f 187 200 199
+f 187 188 200
+f 188 201 200
+f 188 189 201
+f 189 202 201
+f 189 190 202
+f 190 203 202
+f 190 191 203
+f 191 204 203
+f 191 192 204
+f 192 194 204
+f 192 182 194
+f 194 205 206
+f 194 193 205
+f 193 207 205
+f 193 195 207
+f 195 208 207
+f 195 196 208
+f 196 209 208
+f 196 197 209
+f 197 210 209
+f 197 198 210
+f 198 211 210
+f 198 199 211
+f 199 212 211
+f 199 200 212
+f 200 213 212
+f 200 201 213
+f 201 214 213
+f 201 202 214
+f 202 215 214
+f 202 203 215
+f 203 216 215
+f 203 204 216
+f 204 206 216
+f 204 194 206
+f 206 217 218
+f 206 205 217
+f 205 219 217
+f 205 207 219
+f 207 220 219
+f 207 208 220
+f 208 221 220
+f 208 209 221
+f 209 222 221
+f 209 210 222
+f 210 223 222
+f 210 211 223
+f 211 224 223
+f 211 212 224
+f 212 225 224
+f 212 213 225
+f 213 226 225
+f 213 214 226
+f 214 227 226
+f 214 215 227
+f 215 228 227
+f 215 216 228
+f 216 218 228
+f 216 206 218
+f 218 229 230
+f 218 217 229
+f 217 231 229
+f 217 219 231
+f 219 232 231
+f 219 220 232
+f 220 233 232
+f 220 221 233
+f 221 234 233
+f 221 222 234
+f 222 235 234
+f 222 223 235
+f 223 236 235
+f 223 224 236
+f 224 237 236
+f 224 225 237
+f 225 238 237
+f 225 226 238
+f 226 239 238
+f 226 227 239
+f 227 240 239
+f 227 228 240
+f 228 230 240
+f 228 218 230
+f 230 241 242
+f 230 229 241
+f 229 243 241
+f 229 231 243
+f 231 244 243
+f 231 232 244
+f 232 245 244
+f 232 233 245
+f 233 246 245
+f 233 234 246
+f 234 247 246
+f 234 235 247
+f 235 248 247
+f 235 236 248
+f 236 249 248
+f 236 237 249
+f 237 250 249
+f 237 238 250
+f 238 251 250
+f 238 239 251
+f 239 252 251
+f 239 240 252
+f 240 242 252
+f 240 230 242
+f 242 253 254
+f 242 241 253
+f 241 255 253
+f 241 243 255
+f 243 256 255
+f 243 244 256
+f 244 257 256
+f 244 245 257
+f 245 258 257
+f 245 246 258
+f 246 259 258
+f 246 247 259
+f 247 260 259
+f 247 248 260
+f 248 261 260
+f 248 249 261
+f 249 262 261
+f 249 250 262
+f 250 263 262
+f 250 251 263
+f 251 264 263
+f 251 252 264
+f 252 254 264
+f 252 242 254
+f 254 265 266
+f 254 253 265
+f 253 267 265
+f 253 255 267
+f 255 268 267
+f 255 256 268
+f 256 269 268
+f 256 257 269
+f 257 270 269
+f 257 258 270
+f 258 271 270
+f 258 259 271
+f 259 272 271
+f 259 260 272
+f 260 273 272
+f 260 261 273
+f 261 274 273
+f 261 262 274
+f 262 275 274
+f 262 263 275
+f 263 276 275
+f 263 264 276
+f 264 266 276
+f 264 254 266
+f 266 277 278
+f 266 265 277
+f 265 279 277
+f 265 267 279
+f 267 280 279
+f 267 268 280
+f 268 281 280
+f 268 269 281
+f 269 282 281
+f 269 270 282
+f 270 283 282
+f 270 271 283
+f 271 284 283
+f 271 272 284
+f 272 285 284
+f 272 273 285
+f 273 286 285
+f 273 274 286
+f 274 287 286
+f 274 275 287
+f 275 288 287
+f 275 276 288
+f 276 278 288
+f 276 266 278
+f 278 4 1
+f 278 277 4
+f 277 6 4
+f 277 279 6
+f 279 8 6
+f 279 280 8
+f 280 10 8
+f 280 281 10
+f 281 12 10
+f 281 282 12
+f 282 14 12
+f 282 283 14
+f 283 16 14
+f 283 284 16
+f 284 18 16
+f 284 285 18
+f 285 20 18
+f 285 286 20
+f 286 22 20
+f 286 287 22
+f 287 24 22
+f 287 288 24
+f 288 1 24
+f 288 278 1

examples3d/media/models/tstTorusModel2.obj

@@ -0,0 +1,866 @@
+# 288
+# 576
+v 0.095100 47.078098 0.000000
+v 11.781200 43.899101 6.122500
+v 12.205800 45.483700 0.000000
+v 0.095100 45.437599 6.122500
+v 10.621200 39.569801 10.604500
+v 0.095100 40.955601 10.604500
+v 9.036600 33.655899 12.245100
+v 0.095100 34.833099 12.245100
+v 7.451900 27.742001 10.604500
+v 0.095100 28.710600 10.604500
+v 6.291900 23.412701 6.122500
+v 0.095100 24.228600 6.122500
+v 5.867300 21.828100 0.000000
+v 0.095100 22.587999 0.000000
+v 6.291900 23.412701 -6.122500
+v 0.095100 24.228600 -6.122500
+v 7.451900 27.742001 -10.604500
+v 0.095100 28.710600 -10.604500
+v 9.036600 33.655899 -12.245100
+v 0.095100 34.833099 -12.245100
+v 10.621200 39.569801 -10.604500
+v 0.095100 40.955601 -10.604500
+v 11.781200 43.899101 -6.122500
+v 0.095100 45.437599 -6.122500
+v 22.670900 39.388500 6.122500
+v 23.491199 40.809200 0.000000
+v 20.429899 35.506901 10.604500
+v 17.368700 30.204700 12.245100
+v 14.307400 24.902399 10.604500
+v 12.066400 21.020901 6.122500
+v 11.246100 19.600100 0.000000
+v 12.066400 21.020901 -6.122500
+v 14.307400 24.902399 -10.604500
+v 17.368700 30.204700 -12.245100
+v 20.429899 35.506901 -10.604500
+v 22.670900 39.388500 -6.122500
+v 32.022099 32.213001 6.122500
+v 33.182098 33.373100 0.000000
+v 28.852900 29.043800 10.604500
+v 24.523600 24.714500 12.245100
+v 20.194300 20.385201 10.604500
+v 17.025101 17.216000 6.122500
+v 15.865000 16.055901 0.000000
+v 17.025101 17.216000 -6.122500
+v 20.194300 20.385201 -10.604500
+v 24.523600 24.714500 -12.245100
+v 28.852900 29.043800 -10.604500
+v 32.022099 32.213001 -6.122500
+v 39.197601 22.861799 6.122500
+v 40.618301 23.682100 0.000000
+v 35.316002 20.620800 10.604500
+v 30.013800 17.559601 12.245100
+v 24.711500 14.498300 10.604500
+v 20.830000 12.257300 6.122500
+v 19.409201 11.437000 0.000000
+v 20.830000 12.257300 -6.122500
+v 24.711500 14.498300 -10.604500
+v 30.013800 17.559601 -12.245100
+v 35.316002 20.620800 -10.604500
+v 39.197498 22.861799 -6.122500
+v 43.708199 11.972100 6.122500
+v 45.292801 12.396700 0.000000
+v 39.378899 10.812100 10.604500
+v 33.465000 9.227500 12.245100
+v 27.551100 7.642800 10.604500
+v 23.221800 6.482800 6.122500
+v 21.637199 6.058200 0.000000
+v 23.221800 6.482800 -6.122500
+v 27.551100 7.642800 -10.604500
+v 33.465000 9.227500 -12.245100
+v 39.378899 10.812100 -10.604500
+v 43.708199 11.972100 -6.122500
+v 45.246700 0.286000 6.122500
+v 46.887199 0.286000 0.000000
+v 40.764702 0.286000 10.604500
+v 34.642200 0.286000 12.245100
+v 28.519699 0.286000 10.604500
+v 24.037701 0.286000 6.122500
+v 22.397100 0.286000 0.000000
+v 24.037701 0.286000 -6.122500
+v 28.519699 0.286000 -10.604500
+v 34.642200 0.286000 -12.245100
+v 40.764702 0.286000 -10.604500
+v 45.246700 0.286000 -6.122500
+v 43.708199 -11.400100 6.122500
+v 45.292801 -11.824700 0.000000
+v 39.378899 -10.240000 10.604500
+v 33.465000 -8.655400 12.245100
+v 27.551100 -7.070800 10.604500
+v 23.221800 -5.910800 6.122500
+v 21.637199 -5.486200 0.000000
+v 23.221800 -5.910800 -6.122500
+v 27.551100 -7.070800 -10.604500
+v 33.465000 -8.655400 -12.245100
+v 39.378899 -10.240000 -10.604500
+v 43.708199 -11.400100 -6.122500
+v 39.197601 -22.289801 6.122500
+v 40.618301 -23.110001 0.000000
+v 35.316002 -20.048800 10.604500
+v 30.013800 -16.987499 12.245100
+v 24.711500 -13.926200 10.604500
+v 20.830000 -11.685200 6.122500
+v 19.409201 -10.865000 0.000000
+v 20.830000 -11.685200 -6.122500
+v 24.711500 -13.926200 -10.604500
+v 30.013800 -16.987499 -12.245100
+v 35.316002 -20.048800 -10.604500
+v 39.197498 -22.289801 -6.122500
+v 32.022099 -31.641001 6.122500
+v 33.182098 -32.800999 0.000000
+v 28.852900 -28.471701 10.604500
+v 24.523600 -24.142401 12.245100
+v 20.194300 -19.813200 10.604500
+v 17.025101 -16.643900 6.122500
+v 15.865000 -15.483900 0.000000
+v 17.025101 -16.643900 -6.122500
+v 20.194300 -19.813200 -10.604500
+v 24.523600 -24.142401 -12.245100
+v 28.852900 -28.471701 -10.604500
+v 32.022099 -31.641001 -6.122500
+v 22.670900 -38.816399 6.122500
+v 23.491199 -40.237099 0.000000
+v 20.429899 -34.934898 10.604500
+v 17.368700 -29.632601 12.245100
+v 14.307400 -24.330299 10.604500
+v 12.066400 -20.448799 6.122500
+v 11.246100 -19.028099 0.000000
+v 12.066400 -20.448799 -6.122500
+v 14.307400 -24.330299 -10.604500
+v 17.368700 -29.632601 -12.245100
+v 20.429899 -34.934898 -10.604500
+v 22.670900 -38.816399 -6.122500
+v 11.781200 -43.327099 6.122500
+v 12.205800 -44.911701 0.000000
+v 10.621200 -38.997799 10.604500
+v 9.036600 -33.083900 12.245100
+v 7.451900 -27.170000 10.604500
+v 6.291900 -22.840700 6.122500
+v 5.867300 -21.256100 0.000000
+v 6.291900 -22.840700 -6.122500
+v 7.451900 -27.170000 -10.604500
+v 9.036600 -33.083900 -12.245100
+v 10.621200 -38.997799 -10.604500
+v 11.781200 -43.327099 -6.122500
+v 0.095100 -44.865601 6.122500
+v 0.095100 -46.506100 0.000000
+v 0.095100 -40.383598 10.604500
+v 0.095100 -34.261002 12.245100
+v 0.095100 -28.138500 10.604500
+v 0.095100 -23.656500 6.122500
+v 0.095100 -22.016001 0.000000
+v 0.095100 -23.656500 -6.122500
+v 0.095100 -28.138500 -10.604500
+v 0.095100 -34.261002 -12.245100
+v 0.095100 -40.383598 -10.604500
+v 0.095100 -44.865601 -6.122500
+v -11.591000 -43.327099 6.122500
+v -12.015600 -44.911701 0.000000
+v -10.430900 -38.997799 10.604500
+v -8.846300 -33.083900 12.245100
+v -7.261700 -27.170000 10.604500
+v -6.101700 -22.840700 6.122500
+v -5.677100 -21.256100 0.000000
+v -6.101700 -22.840700 -6.122500
+v -7.261700 -27.170000 -10.604500
+v -8.846300 -33.083900 -12.245100
+v -10.430900 -38.997799 -10.604500
+v -11.591000 -43.327099 -6.122500
+v -22.480700 -38.816399 6.122500
+v -23.300900 -40.237099 0.000000
+v -20.239700 -34.934898 10.604500
+v -17.178400 -29.632601 12.245100
+v -14.117100 -24.330299 10.604500
+v -11.876100 -20.448799 6.122500
+v -11.055900 -19.028099 0.000000
+v -11.876100 -20.448799 -6.122500
+v -14.117100 -24.330299 -10.604500
+v -17.178400 -29.632601 -12.245100
+v -20.239700 -34.934898 -10.604500
+v -22.480700 -38.816399 -6.122500
+v -31.831900 -31.641001 6.122500
+v -32.991901 -32.800999 0.000000
+v -28.662600 -28.471701 10.604500
+v -24.333300 -24.142401 12.245100
+v -20.004101 -19.813200 10.604500
+v -16.834801 -16.643900 6.122500
+v -15.674800 -15.483900 0.000000
+v -16.834801 -16.643900 -6.122500
+v -20.004101 -19.813200 -10.604500
+v -24.333300 -24.142401 -12.245100
+v -28.662600 -28.471701 -10.604500
+v -31.831900 -31.641001 -6.122500
+v -39.007301 -22.289801 6.122500
+v -40.428001 -23.110001 0.000000
+v -35.125801 -20.048800 10.604500
+v -29.823500 -16.987499 12.245100
+v -24.521200 -13.926200 10.604500
+v -20.639700 -11.685200 6.122500
+v -19.219000 -10.865000 0.000000
+v -20.639700 -11.685200 -6.122500
+v -24.521200 -13.926200 -10.604500
+v -29.823500 -16.987499 -12.245100
+v -35.125801 -20.048800 -10.604500
+v -39.007301 -22.289801 -6.122500
+v -43.518002 -11.400100 6.122500
+v -45.102600 -11.824700 0.000000
+v -39.188702 -10.240100 10.604500
+v -33.274799 -8.655400 12.245100
+v -27.360901 -7.070800 10.604500
+v -23.031601 -5.910800 6.122500
+v -21.447001 -5.486200 0.000000
+v -23.031601 -5.910800 -6.122500
+v -27.360901 -7.070800 -10.604500
+v -33.274799 -8.655400 -12.245100
+v -39.188702 -10.240100 -10.604500
+v -43.518002 -11.400100 -6.122500
+v -45.056499 0.286000 6.122500
+v -46.696999 0.286000 0.000000
+v -40.574501 0.286000 10.604500
+v -34.451900 0.286000 12.245100
+v -28.329399 0.286000 10.604500
+v -23.847401 0.286000 6.122500
+v -22.206900 0.286000 0.000000
+v -23.847401 0.286000 -6.122500
+v -28.329399 0.286000 -10.604500
+v -34.451900 0.286000 -12.245100
+v -40.574501 0.286000 -10.604500
+v -45.056499 0.286000 -6.122500
+v -43.518002 11.972100 6.122500
+v -45.102600 12.396700 0.000000
+v -39.188702 10.812100 10.604500
+v -33.274799 9.227500 12.245100
+v -27.360901 7.642800 10.604500
+v -23.031601 6.482800 6.122500
+v -21.447001 6.058200 0.000000
+v -23.031601 6.482800 -6.122500
+v -27.360901 7.642800 -10.604500
+v -33.274799 9.227400 -12.245100
+v -39.188702 10.812100 -10.604500
+v -43.518002 11.972100 -6.122500
+v -39.007301 22.861799 6.122500
+v -40.428101 23.682100 0.000000
+v -35.125801 20.620800 10.604500
+v -29.823500 17.559500 12.245100
+v -24.521299 14.498300 10.604500
+v -20.639700 12.257300 6.122500
+v -19.219000 11.437000 0.000000
+v -20.639700 12.257300 -6.122500
+v -24.521299 14.498300 -10.604500
+v -29.823500 17.559500 -12.245100
+v -35.125801 20.620800 -10.604500
+v -39.007301 22.861799 -6.122500
+v -31.831900 32.213001 6.122500
+v -32.991901 33.373001 0.000000
+v -28.662600 29.043800 10.604500
+v -24.333401 24.714500 12.245100
+v -20.004101 20.385201 10.604500
+v -16.834801 17.215900 6.122500
+v -15.674800 16.055901 0.000000
+v -16.834801 17.215900 -6.122500
+v -20.004101 20.385201 -10.604500
+v -24.333401 24.714500 -12.245100
+v -28.662600 29.043800 -10.604500
+v -31.831900 32.213001 -6.122500
+v -22.480700 39.388401 6.122500
+v -23.301001 40.809200 0.000000
+v -20.239700 35.506901 10.604500
+v -17.178400 30.204700 12.245100
+v -14.117200 24.902399 10.604500
+v -11.876200 21.020901 6.122500
+v -11.055900 19.600100 0.000000
+v -11.876200 21.020901 -6.122500
+v -14.117200 24.902399 -10.604500
+v -17.178400 30.204700 -12.245100
+v -20.239700 35.506901 -10.604500
+v -22.480700 39.388401 -6.122500
+v -11.591000 43.899101 6.122500
+v -12.015600 45.483700 0.000000
+v -10.431000 39.569801 10.604500
+v -8.846300 33.655899 12.245100
+v -7.261700 27.742001 10.604500
+v -6.101700 23.412701 6.122500
+v -5.677100 21.828100 0.000000
+v -6.101700 23.412701 -6.122500
+v -7.261700 27.742001 -10.604500
+v -8.846300 33.655899 -12.245100
+v -10.431000 39.569801 -10.604500
+v -11.591000 43.899101 -6.122500
+f 1 2 3
+f 1 4 2
+f 4 5 2
+f 4 6 5
+f 6 7 5
+f 6 8 7
+f 8 9 7
+f 8 10 9
+f 10 11 9
+f 10 12 11
+f 12 13 11
+f 12 14 13
+f 14 15 13
+f 14 16 15
+f 16 17 15
+f 16 18 17
+f 18 19 17
+f 18 20 19
+f 20 21 19
+f 20 22 21
+f 22 23 21
+f 22 24 23
+f 24 3 23
+f 24 1 3
+f 3 25 26
+f 3 2 25
+f 2 27 25
+f 2 5 27
+f 5 28 27
+f 5 7 28
+f 7 29 28
+f 7 9 29
+f 9 30 29
+f 9 11 30
+f 11 31 30
+f 11 13 31
+f 13 32 31
+f 13 15 32
+f 15 33 32
+f 15 17 33
+f 17 34 33
+f 17 19 34
+f 19 35 34
+f 19 21 35
+f 21 36 35
+f 21 23 36
+f 23 26 36
+f 23 3 26
+f 26 37 38
+f 26 25 37
+f 25 39 37
+f 25 27 39
+f 27 40 39
+f 27 28 40
+f 28 41 40
+f 28 29 41
+f 29 42 41
+f 29 30 42
+f 30 43 42
+f 30 31 43
+f 31 44 43
+f 31 32 44
+f 32 45 44
+f 32 33 45
+f 33 46 45
+f 33 34 46
+f 34 47 46
+f 34 35 47
+f 35 48 47
+f 35 36 48
+f 36 38 48
+f 36 26 38
+f 38 49 50
+f 38 37 49
+f 37 51 49
+f 37 39 51
+f 39 52 51
+f 39 40 52
+f 40 53 52
+f 40 41 53
+f 41 54 53
+f 41 42 54
+f 42 55 54
+f 42 43 55
+f 43 56 55
+f 43 44 56
+f 44 57 56
+f 44 45 57
+f 45 58 57
+f 45 46 58
+f 46 59 58
+f 46 47 59
+f 47 60 59
+f 47 48 60
+f 48 50 60
+f 48 38 50
+f 50 61 62
+f 50 49 61
+f 49 63 61
+f 49 51 63
+f 51 64 63
+f 51 52 64
+f 52 65 64
+f 52 53 65
+f 53 66 65
+f 53 54 66
+f 54 67 66
+f 54 55 67
+f 55 68 67
+f 55 56 68
+f 56 69 68
+f 56 57 69
+f 57 70 69
+f 57 58 70
+f 58 71 70
+f 58 59 71
+f 59 72 71
+f 59 60 72
+f 60 62 72
+f 60 50 62
+f 62 73 74
+f 62 61 73
+f 61 75 73
+f 61 63 75
+f 63 76 75
+f 63 64 76
+f 64 77 76
+f 64 65 77
+f 65 78 77
+f 65 66 78
+f 66 79 78
+f 66 67 79
+f 67 80 79
+f 67 68 80
+f 68 81 80
+f 68 69 81
+f 69 82 81
+f 69 70 82
+f 70 83 82
+f 70 71 83
+f 71 84 83
+f 71 72 84
+f 72 74 84
+f 72 62 74
+f 74 85 86
+f 74 73 85
+f 73 87 85
+f 73 75 87
+f 75 88 87
+f 75 76 88
+f 76 89 88
+f 76 77 89
+f 77 90 89
+f 77 78 90
+f 78 91 90
+f 78 79 91
+f 79 92 91
+f 79 80 92
+f 80 93 92
+f 80 81 93
+f 81 94 93
+f 81 82 94
+f 82 95 94
+f 82 83 95
+f 83 96 95
+f 83 84 96
+f 84 86 96
+f 84 74 86
+f 86 97 98
+f 86 85 97
+f 85 99 97
+f 85 87 99
+f 87 100 99
+f 87 88 100
+f 88 101 100
+f 88 89 101
+f 89 102 101
+f 89 90 102
+f 90 103 102
+f 90 91 103
+f 91 104 103
+f 91 92 104
+f 92 105 104
+f 92 93 105
+f 93 106 105
+f 93 94 106
+f 94 107 106
+f 94 95 107
+f 95 108 107
+f 95 96 108
+f 96 98 108
+f 96 86 98
+f 98 109 110
+f 98 97 109
+f 97 111 109
+f 97 99 111
+f 99 112 111
+f 99 100 112
+f 100 113 112
+f 100 101 113
+f 101 114 113
+f 101 102 114
+f 102 115 114
+f 102 103 115
+f 103 116 115
+f 103 104 116
+f 104 117 116
+f 104 105 117
+f 105 118 117
+f 105 106 118
+f 106 119 118
+f 106 107 119
+f 107 120 119
+f 107 108 120
+f 108 110 120
+f 108 98 110
+f 110 121 122
+f 110 109 121
+f 109 123 121
+f 109 111 123
+f 111 124 123
+f 111 112 124
+f 112 125 124
+f 112 113 125
+f 113 126 125
+f 113 114 126
+f 114 127 126
+f 114 115 127
+f 115 128 127
+f 115 116 128
+f 116 129 128
+f 116 117 129
+f 117 130 129
+f 117 118 130
+f 118 131 130
+f 118 119 131
+f 119 132 131
+f 119 120 132
+f 120 122 132
+f 120 110 122
+f 122 133 134
+f 122 121 133
+f 121 135 133
+f 121 123 135
+f 123 136 135
+f 123 124 136
+f 124 137 136
+f 124 125 137
+f 125 138 137
+f 125 126 138
+f 126 139 138
+f 126 127 139
+f 127 140 139
+f 127 128 140
+f 128 141 140
+f 128 129 141
+f 129 142 141
+f 129 130 142
+f 130 143 142
+f 130 131 143
+f 131 144 143
+f 131 132 144
+f 132 134 144
+f 132 122 134
+f 134 145 146
+f 134 133 145
+f 133 147 145
+f 133 135 147
+f 135 148 147
+f 135 136 148
+f 136 149 148
+f 136 137 149
+f 137 150 149
+f 137 138 150
+f 138 151 150
+f 138 139 151
+f 139 152 151
+f 139 140 152
+f 140 153 152
+f 140 141 153
+f 141 154 153
+f 141 142 154
+f 142 155 154
+f 142 143 155
+f 143 156 155
+f 143 144 156
+f 144 146 156
+f 144 134 146
+f 146 157 158
+f 146 145 157
+f 145 159 157
+f 145 147 159
+f 147 160 159
+f 147 148 160
+f 148 161 160
+f 148 149 161
+f 149 162 161
+f 149 150 162
+f 150 163 162
+f 150 151 163
+f 151 164 163
+f 151 152 164
+f 152 165 164
+f 152 153 165
+f 153 166 165
+f 153 154 166
+f 154 167 166
+f 154 155 167
+f 155 168 167
+f 155 156 168
+f 156 158 168
+f 156 146 158
+f 158 169 170
+f 158 157 169
+f 157 171 169
+f 157 159 171
+f 159 172 171
+f 159 160 172
+f 160 173 172
+f 160 161 173
+f 161 174 173
+f 161 162 174
+f 162 175 174
+f 162 163 175
+f 163 176 175
+f 163 164 176
+f 164 177 176
+f 164 165 177
+f 165 178 177
+f 165 166 178
+f 166 179 178
+f 166 167 179
+f 167 180 179
+f 167 168 180
+f 168 170 180
+f 168 158 170
+f 170 181 182
+f 170 169 181
+f 169 183 181
+f 169 171 183
+f 171 184 183
+f 171 172 184
+f 172 185 184
+f 172 173 185
+f 173 186 185
+f 173 174 186
+f 174 187 186
+f 174 175 187
+f 175 188 187
+f 175 176 188
+f 176 189 188
+f 176 177 189
+f 177 190 189
+f 177 178 190
+f 178 191 190
+f 178 179 191
+f 179 192 191
+f 179 180 192
+f 180 182 192
+f 180 170 182
+f 182 193 194
+f 182 181 193
+f 181 195 193
+f 181 183 195
+f 183 196 195
+f 183 184 196
+f 184 197 196
+f 184 185 197
+f 185 198 197
+f 185 186 198
+f 186 199 198
+f 186 187 199
+f 187 200 199
+f 187 188 200
+f 188 201 200
+f 188 189 201
+f 189 202 201
+f 189 190 202
+f 190 203 202
+f 190 191 203
+f 191 204 203
+f 191 192 204
+f 192 194 204
+f 192 182 194
+f 194 205 206
+f 194 193 205
+f 193 207 205
+f 193 195 207
+f 195 208 207
+f 195 196 208
+f 196 209 208
+f 196 197 209
+f 197 210 209
+f 197 198 210
+f 198 211 210
+f 198 199 211
+f 199 212 211
+f 199 200 212
+f 200 213 212
+f 200 201 213
+f 201 214 213
+f 201 202 214
+f 202 215 214
+f 202 203 215
+f 203 216 215
+f 203 204 216
+f 204 206 216
+f 204 194 206
+f 206 217 218
+f 206 205 217
+f 205 219 217
+f 205 207 219
+f 207 220 219
+f 207 208 220
+f 208 221 220
+f 208 209 221
+f 209 222 221
+f 209 210 222
+f 210 223 222
+f 210 211 223
+f 211 224 223
+f 211 212 224
+f 212 225 224
+f 212 213 225
+f 213 226 225
+f 213 214 226
+f 214 227 226
+f 214 215 227
+f 215 228 227
+f 215 216 228
+f 216 218 228
+f 216 206 218
+f 218 229 230
+f 218 217 229
+f 217 231 229
+f 217 219 231
+f 219 232 231
+f 219 220 232
+f 220 233 232
+f 220 221 233
+f 221 234 233
+f 221 222 234
+f 222 235 234
+f 222 223 235
+f 223 236 235
+f 223 224 236
+f 224 237 236
+f 224 225 237
+f 225 238 237
+f 225 226 238
+f 226 239 238
+f 226 227 239
+f 227 240 239
+f 227 228 240
+f 228 230 240
+f 228 218 230
+f 230 241 242
+f 230 229 241
+f 229 243 241
+f 229 231 243
+f 231 244 243
+f 231 232 244
+f 232 245 244
+f 232 233 245
+f 233 246 245
+f 233 234 246
+f 234 247 246
+f 234 235 247
+f 235 248 247
+f 235 236 248
+f 236 249 248
+f 236 237 249
+f 237 250 249
+f 237 238 250
+f 238 251 250
+f 238 239 251
+f 239 252 251
+f 239 240 252
+f 240 242 252
+f 240 230 242
+f 242 253 254
+f 242 241 253
+f 241 255 253
+f 241 243 255
+f 243 256 255
+f 243 244 256
+f 244 257 256
+f 244 245 257
+f 245 258 257
+f 245 246 258
+f 246 259 258
+f 246 247 259
+f 247 260 259
+f 247 248 260
+f 248 261 260
+f 248 249 261
+f 249 262 261
+f 249 250 262
+f 250 263 262
+f 250 251 263
+f 251 264 263
+f 251 252 264
+f 252 254 264
+f 252 242 254
+f 254 265 266
+f 254 253 265
+f 253 267 265
+f 253 255 267
+f 255 268 267
+f 255 256 268
+f 256 269 268
+f 256 257 269
+f 257 270 269
+f 257 258 270
+f 258 271 270
+f 258 259 271
+f 259 272 271
+f 259 260 272
+f 260 273 272
+f 260 261 273
+f 261 274 273
+f 261 262 274
+f 262 275 274
+f 262 263 275
+f 263 276 275
+f 263 264 276
+f 264 266 276
+f 264 254 266
+f 266 277 278
+f 266 265 277
+f 265 279 277
+f 265 267 279
+f 267 280 279
+f 267 268 280
+f 268 281 280
+f 268 269 281
+f 269 282 281
+f 269 270 282
+f 270 283 282
+f 270 271 283
+f 271 284 283
+f 271 272 284
+f 272 285 284
+f 272 273 285
+f 273 286 285
+f 273 274 286
+f 274 287 286
+f 274 275 287
+f 275 288 287
+f 275 276 288
+f 276 278 288
+f 276 266 278
+f 278 4 1
+f 278 277 4
+f 277 6 4
+f 277 279 6
+f 279 8 6
+f 279 280 8
+f 280 10 8
+f 280 281 10
+f 281 12 10
+f 281 282 12
+f 282 14 12
+f 282 283 14
+f 283 16 14
+f 283 284 16
+f 284 18 16
+f 284 285 18
+f 285 20 18
+f 285 286 20
+f 286 22 20
+f 286 287 22
+f 287 24 22
+f 287 288 24
+f 288 1 24
+f 288 278 1

examples3d/media/models/tstTorusModel3.obj

@@ -0,0 +1,866 @@
+# 288
+# 576
+v -0.666200 40.158199 0.000000
+v 9.406100 37.876801 3.565500
+v 9.653400 38.799599 0.000000
+v -0.666200 39.202900 3.565500
+v 8.730500 35.355598 6.175700
+v -0.666200 36.592701 6.175700
+v 7.807700 31.911600 7.131100
+v -0.666200 33.027199 7.131100
+v 6.884900 28.467501 6.175700
+v -0.666200 29.461599 6.175700
+v 6.209300 25.946301 3.565500
+v -0.666200 26.851500 3.565500
+v 5.962100 25.023500 0.000000
+v -0.666200 25.896099 0.000000
+v 6.209300 25.946301 -3.565500
+v -0.666200 26.851500 -3.565500
+v 6.884900 28.467501 -6.175700
+v -0.666200 29.461599 -6.175700
+v 7.807700 31.911600 -7.131100
+v -0.666200 33.027199 -7.131100
+v 8.730500 35.355598 -6.175700
+v -0.666200 36.592701 -6.175700
+v 9.406100 37.876801 -3.565500
+v -0.666200 39.202900 -3.565500
+v 18.791901 33.989101 3.565500
+v 19.269600 34.816502 0.000000
+v 17.486900 31.728600 6.175700
+v 15.704100 28.640800 7.131100
+v 13.921300 25.552900 6.175700
+v 12.616300 23.292500 3.565500
+v 12.138600 22.465099 0.000000
+v 12.616300 23.292500 -3.565500
+v 13.921300 25.552900 -6.175700
+v 15.704100 28.640800 -7.131100
+v 17.486900 31.728600 -6.175700
+v 18.791901 33.989101 -3.565500
+v 26.851801 27.804600 3.565500
+v 27.527300 28.480101 0.000000
+v 25.006100 25.958900 6.175700
+v 22.484900 23.437700 7.131100
+v 19.963699 20.916500 6.175700
+v 18.118000 19.070801 3.565500
+v 17.442499 18.395300 0.000000
+v 18.118000 19.070801 -3.565500
+v 19.963699 20.916500 -6.175700
+v 22.484900 23.437700 -7.131100
+v 25.006100 25.958900 -6.175700
+v 26.851801 27.804600 -3.565500
+v 33.036301 19.744699 3.565500
+v 33.863701 20.222401 0.000000
+v 30.775801 18.439699 6.175700
+v 27.688000 16.656900 7.131100
+v 24.600100 14.874100 6.175700
+v 22.339701 13.569100 3.565500
+v 21.512300 13.091400 0.000000
+v 22.339701 13.569100 -3.565500
+v 24.600100 14.874100 -6.175700
+v 27.688000 16.656900 -7.131100
+v 30.775801 18.439699 -6.175700
+v 33.036301 19.744699 -3.565500
+v 36.924000 10.358900 3.565500
+v 37.846901 10.606200 0.000000
+v 34.402802 9.683300 6.175700
+v 30.958799 8.760500 7.131100
+v 27.514700 7.837700 6.175700
+v 24.993500 7.162100 3.565500
+v 24.070700 6.914800 0.000000
+v 24.993500 7.162100 -3.565500
+v 27.514700 7.837700 -6.175700
+v 30.958799 8.760500 -7.131100
+v 34.402802 9.683300 -6.175700
+v 36.924000 10.358900 -3.565500
+v 38.250099 0.286600 3.565500
+v 39.205399 0.286600 0.000000
+v 35.639900 0.286600 6.175700
+v 32.074402 0.286600 7.131100
+v 28.508801 0.286600 6.175700
+v 25.898701 0.286600 3.565500
+v 24.943300 0.286600 0.000000
+v 25.898701 0.286600 -3.565500
+v 28.508801 0.286600 -6.175700
+v 32.074402 0.286600 -7.131100
+v 35.639900 0.286600 -6.175700
+v 38.250099 0.286600 -3.565500
+v 36.924000 -9.785600 3.565500
+v 37.846901 -10.032900 0.000000
+v 34.402802 -9.110100 6.175700
+v 30.958799 -8.187300 7.131100
+v 27.514700 -7.264400 6.175700
+v 24.993500 -6.588900 3.565500
+v 24.070700 -6.341600 0.000000
+v 24.993500 -6.588900 -3.565500
+v 27.514700 -7.264400 -6.175700
+v 30.958799 -8.187300 -7.131100
+v 34.402802 -9.110100 -6.175700
+v 36.924000 -9.785600 -3.565500
+v 33.036301 -19.171499 3.565500
+v 33.863701 -19.649200 0.000000
+v 30.775801 -17.866400 6.175700
+v 27.688000 -16.083700 7.131100
+v 24.600100 -14.300900 6.175700
+v 22.339701 -12.995800 3.565500
+v 21.512300 -12.518100 0.000000
+v 22.339701 -12.995800 -3.565500
+v 24.600100 -14.300900 -6.175700
+v 27.688000 -16.083700 -7.131100
+v 30.775801 -17.866400 -6.175700
+v 33.036301 -19.171499 -3.565500
+v 26.851801 -27.231300 3.565500
+v 27.527300 -27.906900 0.000000
+v 25.006100 -25.385700 6.175700
+v 22.484900 -22.864401 7.131100
+v 19.963699 -20.343201 6.175700
+v 18.118000 -18.497601 3.565500
+v 17.442499 -17.822001 0.000000
+v 18.118000 -18.497601 -3.565500
+v 19.963699 -20.343201 -6.175700
+v 22.484900 -22.864401 -7.131100
+v 25.006100 -25.385700 -6.175700
+v 26.851801 -27.231300 -3.565500
+v 18.791901 -33.415798 3.565500
+v 19.269600 -34.243198 0.000000
+v 17.486900 -31.155399 6.175700
+v 15.704100 -28.067499 7.131100
+v 13.921300 -24.979700 6.175700
+v 12.616300 -22.719200 3.565500
+v 12.138600 -21.891800 0.000000
+v 12.616300 -22.719200 -3.565500
+v 13.921300 -24.979700 -6.175700
+v 15.704100 -28.067499 -7.131100
+v 17.486900 -31.155399 -6.175700
+v 18.791901 -33.415798 -3.565500
+v 9.406100 -37.303600 3.565500
+v 9.653400 -38.226398 0.000000
+v 8.730500 -34.782398 6.175700
+v 7.807700 -31.338301 7.131100
+v 6.884900 -27.894300 6.175700
+v 6.209300 -25.373100 3.565500
+v 5.962100 -24.450199 0.000000
+v 6.209300 -25.373100 -3.565500
+v 6.884900 -27.894300 -6.175700
+v 7.807700 -31.338301 -7.131100
+v 8.730500 -34.782398 -6.175700
+v 9.406100 -37.303600 -3.565500
+v -0.666200 -38.629601 3.565500
+v -0.666200 -39.584999 0.000000
+v -0.666200 -36.019501 6.175700
+v -0.666200 -32.453899 7.131100
+v -0.666200 -28.888399 6.175700
+v -0.666200 -26.278200 3.565500
+v -0.666200 -25.322901 0.000000
+v -0.666200 -26.278200 -3.565500
+v -0.666200 -28.888399 -6.175700
+v -0.666200 -32.453899 -7.131100
+v -0.666200 -36.019501 -6.175700
+v -0.666200 -38.629601 -3.565500
+v -10.738400 -37.303600 3.565500
+v -10.985700 -38.226398 0.000000
+v -10.062900 -34.782398 6.175700
+v -9.140000 -31.338301 7.131100
+v -8.217200 -27.894300 6.175700
+v -7.541700 -25.373100 3.565500
+v -7.294400 -24.450199 0.000000
+v -7.541700 -25.373100 -3.565500
+v -8.217200 -27.894300 -6.175700
+v -9.140000 -31.338301 -7.131100
+v -10.062900 -34.782398 -6.175700
+v -10.738400 -37.303600 -3.565500
+v -20.124300 -33.415798 3.565500
+v -20.601999 -34.243198 0.000000
+v -18.819201 -31.155399 6.175700
+v -17.036400 -28.067499 7.131100
+v -15.253700 -24.979700 6.175700
+v -13.948600 -22.719200 3.565500
+v -13.470900 -21.891800 0.000000
+v -13.948600 -22.719200 -3.565500
+v -15.253700 -24.979700 -6.175700
+v -17.036400 -28.067499 -7.131100
+v -18.819201 -31.155399 -6.175700
+v -20.124300 -33.415798 -3.565500
+v -28.184099 -27.231300 3.565500
+v -28.859699 -27.906900 0.000000
+v -26.338400 -25.385700 6.175700
+v -23.817200 -22.864401 7.131100
+v -21.296000 -20.343201 6.175700
+v -19.450399 -18.497601 3.565500
+v -18.774799 -17.822001 0.000000
+v -19.450399 -18.497601 -3.565500
+v -21.296000 -20.343201 -6.175700
+v -23.817200 -22.864401 -7.131100
+v -26.338400 -25.385700 -6.175700
+v -28.184099 -27.231300 -3.565500
+v -34.368599 -19.171499 3.565500
+v -35.195999 -19.649200 0.000000
+v -32.108200 -17.866400 6.175700
+v -29.020300 -16.083700 7.131100
+v -25.932501 -14.300900 6.175700
+v -23.672001 -12.995800 3.565500
+v -22.844601 -12.518100 0.000000
+v -23.672001 -12.995800 -3.565500
+v -25.932501 -14.300900 -6.175700
+v -29.020300 -16.083700 -7.131100
+v -32.108200 -17.866400 -6.175700
+v -34.368599 -19.171499 -3.565500
+v -38.256401 -9.785700 3.565500
+v -39.179199 -10.032900 0.000000
+v -35.735100 -9.110100 6.175700
+v -32.291100 -8.187300 7.131100
+v -28.847099 -7.264400 6.175700
+v -26.325899 -6.588900 3.565500
+v -25.403000 -6.341600 0.000000
+v -26.325800 -6.588900 -3.565500
+v -28.847099 -7.264400 -6.175700
+v -32.291100 -8.187300 -7.131100
+v -35.735100 -9.110100 -6.175700
+v -38.256401 -9.785700 -3.565500
+v -39.582401 0.286600 3.565500
+v -40.537800 0.286600 0.000000
+v -36.972198 0.286600 6.175700
+v -33.406700 0.286600 7.131100
+v -29.841200 0.286600 6.175700
+v -27.231001 0.286600 3.565500
+v -26.275600 0.286600 0.000000
+v -27.231001 0.286600 -3.565500
+v -29.841200 0.286600 -6.175700
+v -33.406700 0.286600 -7.131100
+v -36.972198 0.286600 -6.175700
+v -39.582401 0.286600 -3.565500
+v -38.256401 10.358900 3.565500
+v -39.179199 10.606100 0.000000
+v -35.735199 9.683300 6.175700
+v -32.291100 8.760500 7.131100
+v -28.847099 7.837700 6.175700
+v -26.325899 7.162100 3.565500
+v -25.403000 6.914800 0.000000
+v -26.325899 7.162100 -3.565500
+v -28.847099 7.837700 -6.175700
+v -32.291100 8.760500 -7.131100
+v -35.735199 9.683300 -6.175700
+v -38.256401 10.358900 -3.565500
+v -34.368599 19.744699 3.565500
+v -35.195999 20.222401 0.000000
+v -32.108200 18.439600 6.175700
+v -29.020300 16.656900 7.131100
+v -25.932501 14.874100 6.175700
+v -23.672001 13.569000 3.565500
+v -22.844601 13.091300 0.000000
+v -23.672001 13.569000 -3.565500
+v -25.932501 14.874100 -6.175700
+v -29.020300 16.656900 -7.131100
+v -32.108200 18.439600 -6.175700
+v -34.368599 19.744699 -3.565500
+v -28.184099 27.804501 3.565500
+v -28.859699 28.480101 0.000000
+v -26.338499 25.958900 6.175700
+v -23.817200 23.437700 7.131100
+v -21.296000 20.916500 6.175700
+v -19.450399 19.070801 3.565500
+v -18.774799 18.395201 0.000000
+v -19.450399 19.070801 -3.565500
+v -21.296000 20.916500 -6.175700
+v -23.817200 23.437700 -7.131100
+v -26.338499 25.958900 -6.175700
+v -28.184099 27.804501 -3.565500
+v -20.124300 33.989101 3.565500
+v -20.601999 34.816399 0.000000
+v -18.819201 31.728600 6.175700
+v -17.036501 28.640800 7.131100
+v -15.253700 25.552900 6.175700
+v -13.948600 23.292500 3.565500
+v -13.470900 22.465099 0.000000
+v -13.948600 23.292500 -3.565500
+v -15.253700 25.552900 -6.175700
+v -17.036400 28.640800 -7.131100
+v -18.819201 31.728600 -6.175700
+v -20.124300 33.989101 -3.565500
+v -10.738400 37.876801 3.565500
+v -10.985700 38.799599 0.000000
+v -10.062900 35.355598 6.175700
+v -9.140100 31.911600 7.131100
+v -8.217200 28.467501 6.175700
+v -7.541700 25.946301 3.565500
+v -7.294400 25.023500 0.000000
+v -7.541700 25.946301 -3.565500
+v -8.217200 28.467501 -6.175700
+v -9.140100 31.911600 -7.131100
+v -10.062900 35.355598 -6.175700
+v -10.738400 37.876801 -3.565500
+f 1 2 3
+f 1 4 2
+f 4 5 2
+f 4 6 5
+f 6 7 5
+f 6 8 7
+f 8 9 7
+f 8 10 9
+f 10 11 9
+f 10 12 11
+f 12 13 11
+f 12 14 13
+f 14 15 13
+f 14 16 15
+f 16 17 15
+f 16 18 17
+f 18 19 17
+f 18 20 19
+f 20 21 19
+f 20 22 21
+f 22 23 21
+f 22 24 23
+f 24 3 23
+f 24 1 3
+f 3 25 26
+f 3 2 25
+f 2 27 25
+f 2 5 27
+f 5 28 27
+f 5 7 28
+f 7 29 28
+f 7 9 29
+f 9 30 29
+f 9 11 30
+f 11 31 30
+f 11 13 31
+f 13 32 31
+f 13 15 32
+f 15 33 32
+f 15 17 33
+f 17 34 33
+f 17 19 34
+f 19 35 34
+f 19 21 35
+f 21 36 35
+f 21 23 36
+f 23 26 36
+f 23 3 26
+f 26 37 38
+f 26 25 37
+f 25 39 37
+f 25 27 39
+f 27 40 39
+f 27 28 40
+f 28 41 40
+f 28 29 41
+f 29 42 41
+f 29 30 42
+f 30 43 42
+f 30 31 43
+f 31 44 43
+f 31 32 44
+f 32 45 44
+f 32 33 45
+f 33 46 45
+f 33 34 46
+f 34 47 46
+f 34 35 47
+f 35 48 47
+f 35 36 48
+f 36 38 48
+f 36 26 38
+f 38 49 50
+f 38 37 49
+f 37 51 49
+f 37 39 51
+f 39 52 51
+f 39 40 52
+f 40 53 52
+f 40 41 53
+f 41 54 53
+f 41 42 54
+f 42 55 54
+f 42 43 55
+f 43 56 55
+f 43 44 56
+f 44 57 56
+f 44 45 57
+f 45 58 57
+f 45 46 58
+f 46 59 58
+f 46 47 59
+f 47 60 59
+f 47 48 60
+f 48 50 60
+f 48 38 50
+f 50 61 62
+f 50 49 61
+f 49 63 61
+f 49 51 63
+f 51 64 63
+f 51 52 64
+f 52 65 64
+f 52 53 65
+f 53 66 65
+f 53 54 66
+f 54 67 66
+f 54 55 67
+f 55 68 67
+f 55 56 68
+f 56 69 68
+f 56 57 69
+f 57 70 69
+f 57 58 70
+f 58 71 70
+f 58 59 71
+f 59 72 71
+f 59 60 72
+f 60 62 72
+f 60 50 62
+f 62 73 74
+f 62 61 73
+f 61 75 73
+f 61 63 75
+f 63 76 75
+f 63 64 76
+f 64 77 76
+f 64 65 77
+f 65 78 77
+f 65 66 78
+f 66 79 78
+f 66 67 79
+f 67 80 79
+f 67 68 80
+f 68 81 80
+f 68 69 81
+f 69 82 81
+f 69 70 82
+f 70 83 82
+f 70 71 83
+f 71 84 83
+f 71 72 84
+f 72 74 84
+f 72 62 74
+f 74 85 86
+f 74 73 85
+f 73 87 85
+f 73 75 87
+f 75 88 87
+f 75 76 88
+f 76 89 88
+f 76 77 89
+f 77 90 89
+f 77 78 90
+f 78 91 90
+f 78 79 91
+f 79 92 91
+f 79 80 92
+f 80 93 92
+f 80 81 93
+f 81 94 93
+f 81 82 94
+f 82 95 94
+f 82 83 95
+f 83 96 95
+f 83 84 96
+f 84 86 96
+f 84 74 86
+f 86 97 98
+f 86 85 97
+f 85 99 97
+f 85 87 99
+f 87 100 99
+f 87 88 100
+f 88 101 100
+f 88 89 101
+f 89 102 101
+f 89 90 102
+f 90 103 102
+f 90 91 103
+f 91 104 103
+f 91 92 104
+f 92 105 104
+f 92 93 105
+f 93 106 105
+f 93 94 106
+f 94 107 106
+f 94 95 107
+f 95 108 107
+f 95 96 108
+f 96 98 108
+f 96 86 98
+f 98 109 110
+f 98 97 109
+f 97 111 109
+f 97 99 111
+f 99 112 111
+f 99 100 112
+f 100 113 112
+f 100 101 113
+f 101 114 113
+f 101 102 114
+f 102 115 114
+f 102 103 115
+f 103 116 115
+f 103 104 116
+f 104 117 116
+f 104 105 117
+f 105 118 117
+f 105 106 118
+f 106 119 118
+f 106 107 119
+f 107 120 119
+f 107 108 120
+f 108 110 120
+f 108 98 110
+f 110 121 122
+f 110 109 121
+f 109 123 121
+f 109 111 123
+f 111 124 123
+f 111 112 124
+f 112 125 124
+f 112 113 125
+f 113 126 125
+f 113 114 126
+f 114 127 126
+f 114 115 127
+f 115 128 127
+f 115 116 128
+f 116 129 128
+f 116 117 129
+f 117 130 129
+f 117 118 130
+f 118 131 130
+f 118 119 131
+f 119 132 131
+f 119 120 132
+f 120 122 132
+f 120 110 122
+f 122 133 134
+f 122 121 133
+f 121 135 133
+f 121 123 135
+f 123 136 135
+f 123 124 136
+f 124 137 136
+f 124 125 137
+f 125 138 137
+f 125 126 138
+f 126 139 138
+f 126 127 139
+f 127 140 139
+f 127 128 140
+f 128 141 140
+f 128 129 141
+f 129 142 141
+f 129 130 142
+f 130 143 142
+f 130 131 143
+f 131 144 143
+f 131 132 144
+f 132 134 144
+f 132 122 134
+f 134 145 146
+f 134 133 145
+f 133 147 145
+f 133 135 147
+f 135 148 147
+f 135 136 148
+f 136 149 148
+f 136 137 149
+f 137 150 149
+f 137 138 150
+f 138 151 150
+f 138 139 151
+f 139 152 151
+f 139 140 152
+f 140 153 152
+f 140 141 153
+f 141 154 153
+f 141 142 154
+f 142 155 154
+f 142 143 155
+f 143 156 155
+f 143 144 156
+f 144 146 156
+f 144 134 146
+f 146 157 158
+f 146 145 157
+f 145 159 157
+f 145 147 159
+f 147 160 159
+f 147 148 160
+f 148 161 160
+f 148 149 161
+f 149 162 161
+f 149 150 162
+f 150 163 162
+f 150 151 163
+f 151 164 163
+f 151 152 164
+f 152 165 164
+f 152 153 165
+f 153 166 165
+f 153 154 166
+f 154 167 166
+f 154 155 167
+f 155 168 167
+f 155 156 168
+f 156 158 168
+f 156 146 158
+f 158 169 170
+f 158 157 169
+f 157 171 169
+f 157 159 171
+f 159 172 171
+f 159 160 172
+f 160 173 172
+f 160 161 173
+f 161 174 173
+f 161 162 174
+f 162 175 174
+f 162 163 175
+f 163 176 175
+f 163 164 176
+f 164 177 176
+f 164 165 177
+f 165 178 177
+f 165 166 178
+f 166 179 178
+f 166 167 179
+f 167 180 179
+f 167 168 180
+f 168 170 180
+f 168 158 170
+f 170 181 182
+f 170 169 181
+f 169 183 181
+f 169 171 183
+f 171 184 183
+f 171 172 184
+f 172 185 184
+f 172 173 185
+f 173 186 185
+f 173 174 186
+f 174 187 186
+f 174 175 187
+f 175 188 187
+f 175 176 188
+f 176 189 188
+f 176 177 189
+f 177 190 189
+f 177 178 190
+f 178 191 190
+f 178 179 191
+f 179 192 191
+f 179 180 192
+f 180 182 192
+f 180 170 182
+f 182 193 194
+f 182 181 193
+f 181 195 193
+f 181 183 195
+f 183 196 195
+f 183 184 196
+f 184 197 196
+f 184 185 197
+f 185 198 197
+f 185 186 198
+f 186 199 198
+f 186 187 199
+f 187 200 199
+f 187 188 200
+f 188 201 200
+f 188 189 201
+f 189 202 201
+f 189 190 202
+f 190 203 202
+f 190 191 203
+f 191 204 203
+f 191 192 204
+f 192 194 204
+f 192 182 194
+f 194 205 206
+f 194 193 205
+f 193 207 205
+f 193 195 207
+f 195 208 207
+f 195 196 208
+f 196 209 208
+f 196 197 209
+f 197 210 209
+f 197 198 210
+f 198 211 210
+f 198 199 211
+f 199 212 211
+f 199 200 212
+f 200 213 212
+f 200 201 213
+f 201 214 213
+f 201 202 214
+f 202 215 214
+f 202 203 215
+f 203 216 215
+f 203 204 216
+f 204 206 216
+f 204 194 206
+f 206 217 218
+f 206 205 217
+f 205 219 217
+f 205 207 219
+f 207 220 219
+f 207 208 220
+f 208 221 220
+f 208 209 221
+f 209 222 221
+f 209 210 222
+f 210 223 222
+f 210 211 223
+f 211 224 223
+f 211 212 224
+f 212 225 224
+f 212 213 225
+f 213 226 225
+f 213 214 226
+f 214 227 226
+f 214 215 227
+f 215 228 227
+f 215 216 228
+f 216 218 228
+f 216 206 218
+f 218 229 230
+f 218 217 229
+f 217 231 229
+f 217 219 231
+f 219 232 231
+f 219 220 232
+f 220 233 232
+f 220 221 233
+f 221 234 233
+f 221 222 234
+f 222 235 234
+f 222 223 235
+f 223 236 235
+f 223 224 236
+f 224 237 236
+f 224 225 237
+f 225 238 237
+f 225 226 238
+f 226 239 238
+f 226 227 239
+f 227 240 239
+f 227 228 240
+f 228 230 240
+f 228 218 230
+f 230 241 242
+f 230 229 241
+f 229 243 241
+f 229 231 243
+f 231 244 243
+f 231 232 244
+f 232 245 244
+f 232 233 245
+f 233 246 245
+f 233 234 246
+f 234 247 246
+f 234 235 247
+f 235 248 247
+f 235 236 248
+f 236 249 248
+f 236 237 249
+f 237 250 249
+f 237 238 250
+f 238 251 250
+f 238 239 251
+f 239 252 251
+f 239 240 252
+f 240 242 252
+f 240 230 242
+f 242 253 254
+f 242 241 253
+f 241 255 253
+f 241 243 255
+f 243 256 255
+f 243 244 256
+f 244 257 256
+f 244 245 257
+f 245 258 257
+f 245 246 258
+f 246 259 258
+f 246 247 259
+f 247 260 259
+f 247 248 260
+f 248 261 260
+f 248 249 261
+f 249 262 261
+f 249 250 262
+f 250 263 262
+f 250 251 263
+f 251 264 263
+f 251 252 264
+f 252 254 264
+f 252 242 254
+f 254 265 266
+f 254 253 265
+f 253 267 265
+f 253 255 267
+f 255 268 267
+f 255 256 268
+f 256 269 268
+f 256 257 269
+f 257 270 269
+f 257 258 270
+f 258 271 270
+f 258 259 271
+f 259 272 271
+f 259 260 272
+f 260 273 272
+f 260 261 273
+f 261 274 273
+f 261 262 274
+f 262 275 274
+f 262 263 275
+f 263 276 275
+f 263 264 276
+f 264 266 276
+f 264 254 266
+f 266 277 278
+f 266 265 277
+f 265 279 277
+f 265 267 279
+f 267 280 279
+f 267 268 280
+f 268 281 280
+f 268 269 281
+f 269 282 281
+f 269 270 282
+f 270 283 282
+f 270 271 283
+f 271 284 283
+f 271 272 284
+f 272 285 284
+f 272 273 285
+f 273 286 285
+f 273 274 286
+f 274 287 286
+f 274 275 287
+f 275 288 287
+f 275 276 288
+f 276 278 288
+f 276 266 278
+f 278 4 1
+f 278 277 4
+f 277 6 4
+f 277 279 6
+f 279 8 6
+f 279 280 8
+f 280 10 8
+f 280 281 10
+f 281 12 10
+f 281 282 12
+f 282 14 12
+f 282 283 14
+f 283 16 14
+f 283 284 16
+f 284 18 16
+f 284 285 18
+f 285 20 18
+f 285 286 20
+f 286 22 20
+f 286 287 22
+f 287 24 22
+f 287 288 24
+f 288 1 24
+f 288 278 1

examples3d/sensor3.rs

@@ -1,6 +1,6 @@
 use na::Point3;
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet, Proximity};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -74,10 +74,11 @@ pub fn init_world(testbed: &mut Testbed) {
 
     // Callback that will be executed on the main loop to handle proximities.
     testbed.add_callback(move |_, mut graphics, physics, events, _| {
-        while let Ok(prox) = events.proximity_events.try_recv() {
-            let color = match prox.new_status {
-                Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
-                Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
+        while let Ok(prox) = events.intersection_events.try_recv() {
+            let color = if prox.intersecting {
+                Point3::new(1.0, 1.0, 0.0)
+            } else {
+                Point3::new(0.5, 0.5, 1.0)
             };
 
             let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent();

examples3d/stacks3.rs

@@ -1,171 +0,0 @@
-use na::{Point3, Translation3, UnitQuaternion, Vector3};
-use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
-use rapier_testbed3d::Testbed;
-
-fn create_tower_circle(
-    bodies: &mut RigidBodySet,
-    colliders: &mut ColliderSet,
-    offset: Vector3<f32>,
-    stack_height: usize,
-    nsubdivs: usize,
-    half_extents: Vector3<f32>,
-) {
-    let ang_step = std::f32::consts::PI * 2.0 / nsubdivs as f32;
-    let radius = 1.3 * nsubdivs as f32 * half_extents.x / std::f32::consts::PI;
-
-    let shift = half_extents * 2.0;
-    for i in 0usize..stack_height {
-        for j in 0..nsubdivs {
-            let fj = j as f32;
-            let fi = i as f32;
-            let y = fi * shift.y;
-            let pos = Translation3::from(offset)
-                * UnitQuaternion::new(Vector3::y() * (fi / 2.0 + fj) * ang_step)
-                * Translation3::new(0.0, y, radius);
-
-            // Build the rigid body.
-            let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).build();
-            let handle = bodies.insert(rigid_body);
-            let collider =
-                ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
-            colliders.insert(collider, handle, bodies);
-        }
-    }
-}
-
-fn create_wall(
-    bodies: &mut RigidBodySet,
-    colliders: &mut ColliderSet,
-    offset: Vector3<f32>,
-    stack_height: usize,
-    half_extents: Vector3<f32>,
-) {
-    let shift = half_extents * 2.0;
-    for i in 0usize..stack_height {
-        for j in i..stack_height {
-            let fj = j as f32;
-            let fi = i as f32;
-            let x = offset.x;
-            let y = fi * shift.y + offset.y;
-            let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
-                - stack_height as f32 * half_extents.z;
-
-            // Build the rigid body.
-            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
-            let handle = bodies.insert(rigid_body);
-            let collider =
-                ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
-            colliders.insert(collider, handle, bodies);
-        }
-    }
-}
-
-fn create_pyramid(
-    bodies: &mut RigidBodySet,
-    colliders: &mut ColliderSet,
-    offset: Vector3<f32>,
-    stack_height: usize,
-    half_extents: Vector3<f32>,
-) {
-    let shift = half_extents * 2.0;
-
-    for i in 0usize..stack_height {
-        for j in i..stack_height {
-            for k in i..stack_height {
-                let fi = i as f32;
-                let fj = j as f32;
-                let fk = k as f32;
-                let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x
-                    - stack_height as f32 * half_extents.x;
-                let y = fi * shift.y + offset.y;
-                let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
-                    - stack_height as f32 * half_extents.z;
-
-                // Build the rigid body.
-                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
-                let handle = bodies.insert(rigid_body);
-                let collider =
-                    ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
-                colliders.insert(collider, handle, bodies);
-            }
-        }
-    }
-}
-
-pub fn init_world(testbed: &mut Testbed) {
-    /*
-     * World
-     */
-    let mut bodies = RigidBodySet::new();
-    let mut colliders = ColliderSet::new();
-    let joints = JointSet::new();
-
-    /*
-     * Ground
-     */
-    let ground_size = 200.0;
-    let ground_height = 0.1;
-
-    let rigid_body = RigidBodyBuilder::new_static()
-        .translation(0.0, -ground_height, 0.0)
-        .build();
-    let handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
-    colliders.insert(collider, handle, &mut bodies);
-
-    /*
-     * Create the cubes
-     */
-    let cube_size = 1.0;
-    let hext = Vector3::repeat(cube_size);
-    let bottomy = cube_size * 50.0;
-
-    create_pyramid(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(-20.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_wall(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(-2.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_wall(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(4.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_wall(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(10.0, bottomy, 0.0),
-        12,
-        hext,
-    );
-    create_tower_circle(
-        &mut bodies,
-        &mut colliders,
-        Vector3::new(25.0, bottomy, 0.0),
-        8,
-        24,
-        hext,
-    );
-
-    /*
-     * Set up the testbed.
-     */
-    testbed.set_world(bodies, colliders, joints);
-    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
-}
-
-fn main() {
-    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
-    testbed.run()
-}

examples3d/trimesh3.rs

@@ -1,6 +1,6 @@
-use na::{ComplexField, Point3};
+use na::{ComplexField, DMatrix, Isometry3, Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField, SharedShape};
 use rapier_testbed3d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -14,31 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Ground
      */
-    let ground_size = 100.0f32;
-    let ground_height = 1.0;
+    let ground_size = Vector3::new(100.0, 1.0, 100.0);
     let nsubdivs = 20;
 
-    let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
-    let indices = quad
-        .flat_indices()
-        .chunks(3)
-        .map(|is| Point3::new(is[0], is[2], is[1]))
-        .collect();
-    let mut vertices = quad.coords;
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
+            10.0
+        } else {
+            let x = i as f32 * ground_size.x / (nsubdivs as f32);
+            let z = j as f32 * ground_size.z / (nsubdivs as f32);
 
-    // ncollide generates a quad with `z` as the normal.
-    // so we switch z and y here and set a random altitude at each point.
-    for p in vertices.iter_mut() {
-        p.z = p.y;
-        // NOTE: make sure we use the sin/cos from simba to ensure
-        // cross-platform determinism of the example when the
-        // enhanced_determinism feature is enabled.
-        p.y = (<f32 as ComplexField>::sin(p.x) + <f32 as ComplexField>::cos(p.z)) * ground_height;
-
-        if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
-            p.y = 10.0;
+            // NOTE: make sure we use the sin/cos from simba to ensure
+            // cross-platform determinism of the example when the
+            // enhanced_determinism feature is enabled.
+            <f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
         }
-    }
+    });
+
+    // Here we will build our trimesh from the mesh representation of an
+    // heightfield.
+    let heightfield = HeightField::new(heights, ground_size);
+    let (vertices, indices) = heightfield.to_trimesh();
 
     let rigid_body = RigidBodyBuilder::new_static().build();
     let handle = bodies.insert(rigid_body);
@@ -67,14 +63,32 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                let collider = match j % 5 {
+                let collider = match j % 6 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
                     2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
                     3 => ColliderBuilder::cone(rad, rad).build(),
-                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
+                    4 => ColliderBuilder::capsule_y(rad, rad).build(),
+                    _ => {
+                        let shapes = vec![
+                            (
+                                Isometry3::identity(),
+                                SharedShape::cuboid(rad, rad / 2.0, rad / 2.0),
+                            ),
+                            (
+                                Isometry3::translation(rad, 0.0, 0.0),
+                                SharedShape::cuboid(rad / 2.0, rad, rad / 2.0),
+                            ),
+                            (
+                                Isometry3::translation(-rad, 0.0, 0.0),
+                                SharedShape::cuboid(rad / 2.0, rad, rad / 2.0),
+                            ),
+                        ];
+
+                        ColliderBuilder::compound(shapes).build()
+                    }
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

src/data/arena.rs

@@ -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 parry::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.
     ///

src/data/hashmap.rs

@@ -1,137 +0,0 @@
-//! A hash-map that behaves deterministically when the
-//! `enhanced-determinism` feature is enabled.
-
-#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))]
-use indexmap::IndexMap as StdHashMap;
-#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
-use std::collections::HashMap as StdHashMap;
-
-/// Serializes only the capacity of a hash-map instead of its actual content.
-#[cfg(feature = "serde-serialize")]
-pub fn serialize_hashmap_capacity<S: serde::Serializer, K, V, H: std::hash::BuildHasher>(
-    map: &StdHashMap<K, V, H>,
-    s: S,
-) -> Result<S::Ok, S::Error> {
-    s.serialize_u64(map.capacity() as u64)
-}
-
-/// Creates a new hash-map with its capacity deserialized from `d`.
-#[cfg(feature = "serde-serialize")]
-pub fn deserialize_hashmap_capacity<
-    'de,
-    D: serde::Deserializer<'de>,
-    K,
-    V,
-    H: std::hash::BuildHasher + Default,
->(
-    d: D,
-) -> Result<StdHashMap<K, V, H>, D::Error> {
-    struct CapacityVisitor;
-    impl<'de> serde::de::Visitor<'de> for CapacityVisitor {
-        type Value = u64;
-
-        fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
-            write!(formatter, "an integer between 0 and 2^64")
-        }
-
-        fn visit_u64<E: serde::de::Error>(self, val: u64) -> Result<Self::Value, E> {
-            Ok(val)
-        }
-    }
-
-    let capacity = d.deserialize_u64(CapacityVisitor)? as usize;
-    Ok(StdHashMap::with_capacity_and_hasher(
-        capacity,
-        Default::default(),
-    ))
-}
-
-/*
- * FxHasher taken from rustc_hash, except that it does not depend on the pointer size.
- */
-#[cfg(feature = "enhanced-determinism")]
-pub type FxHashMap32<K, V> = indexmap::IndexMap<K, V, std::hash::BuildHasherDefault<FxHasher32>>;
-#[cfg(feature = "enhanced-determinism")]
-pub use {self::FxHashMap32 as HashMap, indexmap::map::Entry};
-#[cfg(not(feature = "enhanced-determinism"))]
-pub use {rustc_hash::FxHashMap as HashMap, std::collections::hash_map::Entry};
-
-const K: u32 = 0x9e3779b9;
-
-// Same as FxHasher, but with the guarantee that the internal hash is
-// an u32 instead of something that depends on the platform.
-pub struct FxHasher32 {
-    hash: u32,
-}
-
-impl Default for FxHasher32 {
-    #[inline]
-    fn default() -> FxHasher32 {
-        FxHasher32 { hash: 0 }
-    }
-}
-
-impl FxHasher32 {
-    #[inline]
-    fn add_to_hash(&mut self, i: u32) {
-        use std::ops::BitXor;
-        self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K);
-    }
-}
-
-impl std::hash::Hasher for FxHasher32 {
-    #[inline]
-    fn write(&mut self, mut bytes: &[u8]) {
-        use std::convert::TryInto;
-        let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap());
-        let mut hash = FxHasher32 { hash: self.hash };
-        assert!(std::mem::size_of::<u32>() <= 8);
-        while bytes.len() >= std::mem::size_of::<u32>() {
-            hash.add_to_hash(read_u32(bytes) as u32);
-            bytes = &bytes[std::mem::size_of::<u32>()..];
-        }
-        if (std::mem::size_of::<u32>() > 4) && (bytes.len() >= 4) {
-            hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32);
-            bytes = &bytes[4..];
-        }
-        if (std::mem::size_of::<u32>() > 2) && bytes.len() >= 2 {
-            hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32);
-            bytes = &bytes[2..];
-        }
-        if (std::mem::size_of::<u32>() > 1) && bytes.len() >= 1 {
-            hash.add_to_hash(bytes[0] as u32);
-        }
-        self.hash = hash.hash;
-    }
-
-    #[inline]
-    fn write_u8(&mut self, i: u8) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn write_u16(&mut self, i: u16) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn write_u32(&mut self, i: u32) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn write_u64(&mut self, i: u64) {
-        self.add_to_hash(i as u32);
-        self.add_to_hash((i >> 32) as u32);
-    }
-
-    #[inline]
-    fn write_usize(&mut self, i: usize) {
-        self.add_to_hash(i as u32);
-    }
-
-    #[inline]
-    fn finish(&self) -> u64 {
-        self.hash as u64
-    }
-}

src/data/maybe_serializable_data.rs

@@ -1,17 +0,0 @@
-use downcast_rs::{impl_downcast, DowncastSync};
-#[cfg(feature = "serde-serialize")]
-use erased_serde::Serialize;
-
-/// Piece of data that may be serializable.
-pub trait MaybeSerializableData: DowncastSync {
-    /// Convert this shape as a serializable entity.
-    #[cfg(feature = "serde-serialize")]
-    fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
-        None
-    }
-
-    /// Clones `self`.
-    fn clone_dyn(&self) -> Box<dyn MaybeSerializableData>;
-}
-
-impl_downcast!(sync MaybeSerializableData);

src/data/mod.rs

@@ -1,11 +1,9 @@
 //! Data structures modified with guaranteed deterministic behavior after deserialization.
 
 pub use self::coarena::Coarena;
-pub use self::maybe_serializable_data::MaybeSerializableData;
+pub use parry::utils::MaybeSerializableData;
 
 pub mod arena;
 mod coarena;
 pub(crate) mod graph;
-pub(crate) mod hashmap;
-mod maybe_serializable_data;
 pub mod pubsub;

src/dynamics/coefficient_combine_rule.rs

@@ -0,0 +1,34 @@
+use crate::math::Real;
+
+/// Rules used to combine two coefficients.
+///
+/// This is used to determine the effective restitution and
+/// friction coefficients for a contact between two colliders.
+/// Each collider has its combination rule of type
+/// `CoefficientCombineRule`. And the rule
+/// actually used is given by `max(first_combine_rule as usize, second_combine_rule as usize)`.
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum CoefficientCombineRule {
+    /// The two coefficients are averaged.
+    Average = 0,
+    /// The smallest coefficient is chosen.
+    Min,
+    /// The two coefficients are multiplied.
+    Multiply,
+    /// The greatest coefficient is chosen.
+    Max,
+}
+
+impl CoefficientCombineRule {
+    pub(crate) fn combine(coeff1: Real, coeff2: Real, rule_value1: u8, rule_value2: u8) -> Real {
+        let effective_rule = rule_value1.max(rule_value2);
+
+        match effective_rule {
+            0 => (coeff1 + coeff1) / 2.0,
+            1 => coeff1.min(coeff2),
+            2 => coeff1 * coeff2,
+            _ => coeff1.max(coeff2),
+        }
+    }
+}

src/dynamics/integration_parameters.rs

@@ -1,9 +1,11 @@
+use crate::math::Real;
+
 /// Parameters for a time-step of the physics engine.
 #[derive(Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct IntegrationParameters {
     /// The timestep length (default: `1.0 / 60.0`)
-    pub dt: f32,
+    pub dt: Real,
 
     //    /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
     //    ///
@@ -18,31 +20,31 @@ pub struct IntegrationParameters {
     pub return_after_ccd_substep: bool,
     /// The Error Reduction Parameter in `[0, 1]` is the proportion of
     /// the positional error to be corrected at each time step (default: `0.2`).
-    pub erp: f32,
+    pub erp: Real,
     /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
     /// the positional error to be corrected at each time step (default: `0.2`).
-    pub joint_erp: f32,
+    pub joint_erp: Real,
     /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
     /// when they are re-used to initialize the solver (default `1.0`).
-    pub warmstart_coeff: f32,
+    pub warmstart_coeff: Real,
     /// Contacts at points where the involved bodies have a relative
     /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`).
-    pub restitution_velocity_threshold: f32,
+    pub restitution_velocity_threshold: Real,
     /// Amount of penetration the engine wont attempt to correct (default: `0.005m`).
-    pub allowed_linear_error: f32,
+    pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
-    pub prediction_distance: f32,
+    pub prediction_distance: Real,
     /// Amount of angular drift of joint limits the engine wont
     /// attempt to correct (default: `0.001rad`).
-    pub allowed_angular_error: f32,
+    pub allowed_angular_error: Real,
     /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
-    pub max_linear_correction: f32,
+    pub max_linear_correction: Real,
     /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
-    pub max_angular_correction: f32,
+    pub max_angular_correction: Real,
     /// Maximum nonlinear SOR-prox scaling parameter when the constraint
     /// correction direction is close to the kernel of the involved multibody's
     /// jacobian (default: `0.2`).
-    pub max_stabilization_multiplier: f32,
+    pub max_stabilization_multiplier: Real,
     /// Maximum number of iterations performed by the velocity constraints solver (default: `4`).
     pub max_velocity_iterations: usize,
     /// Maximum number of iterations performed by the position-based constraints solver (default: `1`).
@@ -88,18 +90,18 @@ impl IntegrationParameters {
     /// Creates a set of integration parameters with the given values.
     #[deprecated = "Use `IntegrationParameters { dt: 60.0, ..Default::default() }` instead"]
     pub fn new(
-        dt: f32,
+        dt: Real,
         //        multithreading_enabled: bool,
-        erp: f32,
-        joint_erp: f32,
-        warmstart_coeff: f32,
-        restitution_velocity_threshold: f32,
-        allowed_linear_error: f32,
-        allowed_angular_error: f32,
-        max_linear_correction: f32,
-        max_angular_correction: f32,
-        prediction_distance: f32,
-        max_stabilization_multiplier: f32,
+        erp: Real,
+        joint_erp: Real,
+        warmstart_coeff: Real,
+        restitution_velocity_threshold: Real,
+        allowed_linear_error: Real,
+        allowed_angular_error: Real,
+        max_linear_correction: Real,
+        max_angular_correction: Real,
+        prediction_distance: Real,
+        max_stabilization_multiplier: Real,
         max_velocity_iterations: usize,
         max_position_iterations: usize,
         max_ccd_position_iterations: usize,
@@ -140,7 +142,7 @@ impl IntegrationParameters {
     /// The current time-stepping length.
     #[inline(always)]
     #[deprecated = "You can just read the `IntegrationParams::dt` value directly"]
-    pub fn dt(&self) -> f32 {
+    pub fn dt(&self) -> Real {
         self.dt
     }
 
@@ -148,7 +150,7 @@ impl IntegrationParameters {
     ///
     /// This is zero if `self.dt` is zero.
     #[inline(always)]
-    pub fn inv_dt(&self) -> f32 {
+    pub fn inv_dt(&self) -> Real {
         if self.dt == 0.0 {
             0.0
         } else {
@@ -159,7 +161,7 @@ impl IntegrationParameters {
     /// Sets the time-stepping length.
     #[inline]
     #[deprecated = "You can just set the `IntegrationParams::dt` value directly"]
-    pub fn set_dt(&mut self, dt: f32) {
+    pub fn set_dt(&mut self, dt: Real) {
         assert!(dt >= 0.0, "The time-stepping length cannot be negative.");
         self.dt = dt;
     }
@@ -168,7 +170,7 @@ impl IntegrationParameters {
     ///
     /// This automatically recompute `self.dt`.
     #[inline]
-    pub fn set_inv_dt(&mut self, inv_dt: f32) {
+    pub fn set_inv_dt(&mut self, inv_dt: Real) {
         if inv_dt == 0.0 {
             self.dt = 0.0
         } else {

src/dynamics/joint/ball_joint.rs

@@ -1,29 +1,29 @@
-use crate::math::{Point, Vector};
+use crate::math::{Point, Real, Vector};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A joint that removes all relative linear motion between a pair of points on two bodies.
 pub struct BallJoint {
     /// Where the ball joint is attached on the first body, expressed in the first body local frame.
-    pub local_anchor1: Point<f32>,
+    pub local_anchor1: Point<Real>,
     /// Where the ball joint is attached on the first body, expressed in the first body local frame.
-    pub local_anchor2: Point<f32>,
+    pub local_anchor2: Point<Real>,
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector<f32>,
+    pub impulse: Vector<Real>,
 }
 
 impl BallJoint {
     /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
-    pub fn new(local_anchor1: Point<f32>, local_anchor2: Point<f32>) -> Self {
+    pub fn new(local_anchor1: Point<Real>, local_anchor2: Point<Real>) -> Self {
         Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
     }
 
     pub(crate) fn with_impulse(
-        local_anchor1: Point<f32>,
-        local_anchor2: Point<f32>,
-        impulse: Vector<f32>,
+        local_anchor1: Point<Real>,
+        local_anchor2: Point<Real>,
+        impulse: Vector<Real>,
     ) -> Self {
         Self {
             local_anchor1,

src/dynamics/joint/fixed_joint.rs

@@ -1,4 +1,4 @@
-use crate::math::{Isometry, SpacialVector};
+use crate::math::{Isometry, Real, SpacialVector};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -8,22 +8,22 @@ use crate::math::{Isometry, SpacialVector};
 pub struct FixedJoint {
     /// The frame of reference for the first body affected by this joint, expressed in the local frame
     /// of the first body.
-    pub local_anchor1: Isometry<f32>,
+    pub local_anchor1: Isometry<Real>,
     /// The frame of reference for the second body affected by this joint, expressed in the local frame
     /// of the first body.
-    pub local_anchor2: Isometry<f32>,
+    pub local_anchor2: Isometry<Real>,
     /// The impulse applied to the first body affected by this joint.
     ///
     /// The impulse applied to the second body affected by this joint is given by `-impulse`.
     /// This combines both linear and angular impulses:
     /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
     /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
-    pub impulse: SpacialVector<f32>,
+    pub impulse: SpacialVector<Real>,
 }
 
 impl FixedJoint {
     /// Creates a new fixed joint from the frames of reference of both bodies.
-    pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self {
+    pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
         Self {
             local_anchor1,
             local_anchor2,

src/dynamics/joint/joint_set.rs

@@ -1,11 +1,36 @@
 use super::Joint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
-use crate::data::arena::{Arena, Index};
+use crate::data::arena::Arena;
 use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
 
 /// The unique identifier of a joint added to the joint set.
-pub type JointHandle = Index;
+/// The unique identifier of a collider added to a collider set.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct JointHandle(pub(crate) crate::data::arena::Index);
+
+impl JointHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (usize, u64) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid joint handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_USIZE,
+            crate::INVALID_U64,
+        ))
+    }
+}
+
 pub(crate) type JointIndex = usize;
 pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
 
@@ -13,7 +38,7 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
 /// A set of joints that can be handled by a physics `World`.
 pub struct JointSet {
     joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
-    joint_graph: InteractionGraph<Joint>,
+    joint_graph: InteractionGraph<RigidBodyHandle, Joint>,
 }
 
 impl JointSet {
@@ -25,29 +50,24 @@ impl JointSet {
         }
     }
 
-    /// An always-invalid joint handle.
-    pub fn invalid_handle() -> JointHandle {
-        JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
-    }
-
     /// The number of joints on this set.
     pub fn len(&self) -> usize {
         self.joint_graph.graph.edges.len()
     }
 
     /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
-    pub fn joint_graph(&self) -> &InteractionGraph<Joint> {
+    pub fn joint_graph(&self) -> &InteractionGraph<RigidBodyHandle, Joint> {
         &self.joint_graph
     }
 
     /// Is the given joint handle valid?
     pub fn contains(&self, handle: JointHandle) -> bool {
-        self.joint_ids.contains(handle)
+        self.joint_ids.contains(handle.0)
     }
 
     /// Gets the joint with the given handle.
     pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
-        let id = self.joint_ids.get(handle)?;
+        let id = self.joint_ids.get(handle.0)?;
         self.joint_graph.graph.edge_weight(*id)
     }
 
@@ -62,7 +82,10 @@ impl JointSet {
     /// suffer form the ABA problem.
     pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> {
         let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
-        Some((self.joint_graph.graph.edge_weight(*id)?, handle))
+        Some((
+            self.joint_graph.graph.edge_weight(*id)?,
+            JointHandle(handle),
+        ))
     }
 
     /// Iterates through all the joint on this set.
@@ -117,7 +140,7 @@ impl JointSet {
         let joint = Joint {
             body1,
             body2,
-            handle,
+            handle: JointHandle(handle),
             #[cfg(feature = "parallel")]
             constraint_index: 0,
             #[cfg(feature = "parallel")]
@@ -133,11 +156,13 @@ impl JointSet {
 
         // NOTE: the body won't have a graph index if it does not
         // have any joint attached.
-        if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) {
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index)
+        {
             rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
         }
 
-        if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) {
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index)
+        {
             rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
         }
 
@@ -146,7 +171,7 @@ impl JointSet {
             .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
 
         self.joint_ids[handle] = id;
-        handle
+        JointHandle(handle)
     }
 
     /// Retrieve all the joints happening between two active bodies.
@@ -191,7 +216,7 @@ impl JointSet {
         bodies: &mut RigidBodySet,
         wake_up: bool,
     ) -> Option<Joint> {
-        let id = self.joint_ids.remove(handle)?;
+        let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
         if wake_up {
@@ -207,7 +232,7 @@ impl JointSet {
         let removed_joint = self.joint_graph.graph.remove_edge(id);
 
         if let Some(edge) = self.joint_graph.graph.edge_weight(id) {
-            self.joint_ids[edge.handle] = id;
+            self.joint_ids[edge.handle.0] = id;
         }
 
         removed_joint
@@ -218,7 +243,7 @@ impl JointSet {
         deleted_id: RigidBodyGraphIndex,
         bodies: &mut RigidBodySet,
     ) {
-        if InteractionGraph::<()>::is_graph_index_valid(deleted_id) {
+        if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) {
             // We have to delete each joint one by one in order to:
             // - Wake-up the attached bodies.
             // - Update our Handle -> graph edge mapping.
@@ -229,12 +254,12 @@ impl JointSet {
                 .map(|e| (e.0, e.1, e.2.handle))
                 .collect();
             for (h1, h2, to_delete_handle) in to_delete {
-                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap();
+                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle.0).unwrap();
                 self.joint_graph.graph.remove_edge(to_delete_edge_id);
 
                 // Update the id of the edge which took the place of the deleted one.
                 if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) {
-                    self.joint_ids[j.handle] = to_delete_edge_id;
+                    self.joint_ids[j.handle.0] = to_delete_edge_id;
                 }
 
                 // Wake up the attached bodies.

src/dynamics/joint/prismatic_joint.rs

@@ -1,4 +1,4 @@
-use crate::math::{Isometry, Point, Vector, DIM};
+use crate::math::{Isometry, Point, Real, Vector, DIM};
 use crate::utils::WBasis;
 use na::Unit;
 #[cfg(feature = "dim2")]
@@ -11,35 +11,35 @@ use na::Vector5;
 /// A joint that removes all relative motion between two bodies, except for the translations along one axis.
 pub struct PrismaticJoint {
     /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body.
-    pub local_anchor1: Point<f32>,
+    pub local_anchor1: Point<Real>,
     /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
-    pub local_anchor2: Point<f32>,
-    pub(crate) local_axis1: Unit<Vector<f32>>,
-    pub(crate) local_axis2: Unit<Vector<f32>>,
-    pub(crate) basis1: [Vector<f32>; DIM - 1],
-    pub(crate) basis2: [Vector<f32>; DIM - 1],
+    pub local_anchor2: Point<Real>,
+    pub(crate) local_axis1: Unit<Vector<Real>>,
+    pub(crate) local_axis2: Unit<Vector<Real>>,
+    pub(crate) basis1: [Vector<Real>; DIM - 1],
+    pub(crate) basis2: [Vector<Real>; DIM - 1],
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     #[cfg(feature = "dim3")]
-    pub impulse: Vector5<f32>,
+    pub impulse: Vector5<Real>,
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
     #[cfg(feature = "dim2")]
-    pub impulse: Vector2<f32>,
+    pub impulse: Vector2<Real>,
     /// Whether or not this joint should enforce translational limits along its axis.
     pub limits_enabled: bool,
     /// The min an max relative position of the attached bodies along this joint's axis.
-    pub limits: [f32; 2],
+    pub limits: [Real; 2],
     /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
-    pub limits_impulse: f32,
+    pub limits_impulse: Real,
     // pub motor_enabled: bool,
-    // pub target_motor_vel: f32,
-    // pub max_motor_impulse: f32,
-    // pub motor_impulse: f32,
+    // pub target_motor_vel: Real,
+    // pub max_motor_impulse: Real,
+    // pub motor_impulse: Real,
 }
 
 impl PrismaticJoint {
@@ -47,10 +47,10 @@ impl PrismaticJoint {
     /// in the local-space of the affected bodies.
     #[cfg(feature = "dim2")]
     pub fn new(
-        local_anchor1: Point<f32>,
-        local_axis1: Unit<Vector<f32>>,
-        local_anchor2: Point<f32>,
-        local_axis2: Unit<Vector<f32>>,
+        local_anchor1: Point<Real>,
+        local_axis1: Unit<Vector<Real>>,
+        local_anchor2: Point<Real>,
+        local_axis2: Unit<Vector<Real>>,
     ) -> Self {
         Self {
             local_anchor1,
@@ -61,11 +61,11 @@ impl PrismaticJoint {
             basis2: local_axis2.orthonormal_basis(),
             impulse: na::zero(),
             limits_enabled: false,
-            limits: [-f32::MAX, f32::MAX],
+            limits: [-Real::MAX, Real::MAX],
             limits_impulse: 0.0,
             // motor_enabled: false,
             // target_motor_vel: 0.0,
-            // max_motor_impulse: f32::MAX,
+            // max_motor_impulse: Real::MAX,
             // motor_impulse: 0.0,
         }
     }
@@ -78,12 +78,12 @@ impl PrismaticJoint {
     /// computed arbitrarily.
     #[cfg(feature = "dim3")]
     pub fn new(
-        local_anchor1: Point<f32>,
-        local_axis1: Unit<Vector<f32>>,
-        local_tangent1: Vector<f32>,
-        local_anchor2: Point<f32>,
-        local_axis2: Unit<Vector<f32>>,
-        local_tangent2: Vector<f32>,
+        local_anchor1: Point<Real>,
+        local_axis1: Unit<Vector<Real>>,
+        local_tangent1: Vector<Real>,
+        local_anchor2: Point<Real>,
+        local_axis2: Unit<Vector<Real>>,
+        local_tangent2: Vector<Real>,
     ) -> Self {
         let basis1 = if let Some(local_bitangent1) =
             Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
@@ -116,28 +116,28 @@ impl PrismaticJoint {
             basis2,
             impulse: na::zero(),
             limits_enabled: false,
-            limits: [-f32::MAX, f32::MAX],
+            limits: [-Real::MAX, Real::MAX],
             limits_impulse: 0.0,
             // motor_enabled: false,
             // target_motor_vel: 0.0,
-            // max_motor_impulse: f32::MAX,
+            // max_motor_impulse: Real::MAX,
             // motor_impulse: 0.0,
         }
     }
 
     /// The local axis of this joint, expressed in the local-space of the first attached body.
-    pub fn local_axis1(&self) -> Unit<Vector<f32>> {
+    pub fn local_axis1(&self) -> Unit<Vector<Real>> {
         self.local_axis1
     }
 
     /// The local axis of this joint, expressed in the local-space of the second attached body.
-    pub fn local_axis2(&self) -> Unit<Vector<f32>> {
+    pub fn local_axis2(&self) -> Unit<Vector<Real>> {
         self.local_axis2
     }
 
     // FIXME: precompute this?
     #[cfg(feature = "dim2")]
-    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame1(&self) -> Isometry<Real> {
         use na::{Matrix2, Rotation2, UnitComplex};
 
         let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
@@ -149,7 +149,7 @@ impl PrismaticJoint {
 
     // FIXME: precompute this?
     #[cfg(feature = "dim2")]
-    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame2(&self) -> Isometry<Real> {
         use na::{Matrix2, Rotation2, UnitComplex};
 
         let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
@@ -161,7 +161,7 @@ impl PrismaticJoint {
 
     // FIXME: precompute this?
     #[cfg(feature = "dim3")]
-    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame1(&self) -> Isometry<Real> {
         use na::{Matrix3, Rotation3, UnitQuaternion};
 
         let mat = Matrix3::from_columns(&[
@@ -177,7 +177,7 @@ impl PrismaticJoint {
 
     // FIXME: precompute this?
     #[cfg(feature = "dim3")]
-    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+    pub(crate) fn local_frame2(&self) -> Isometry<Real> {
         use na::{Matrix3, Rotation3, UnitQuaternion};
 
         let mat = Matrix3::from_columns(&[

src/dynamics/joint/revolute_joint.rs

@@ -1,4 +1,4 @@
-use crate::math::{Point, Vector};
+use crate::math::{Point, Real, Vector};
 use crate::utils::WBasis;
 use na::{Unit, Vector5};
 
@@ -7,31 +7,31 @@ use na::{Unit, Vector5};
 /// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
 pub struct RevoluteJoint {
     /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
-    pub local_anchor1: Point<f32>,
+    pub local_anchor1: Point<Real>,
     /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
-    pub local_anchor2: Point<f32>,
+    pub local_anchor2: Point<Real>,
     /// The rotation axis of this revolute joint expressed in the local space of the first attached body.
-    pub local_axis1: Unit<Vector<f32>>,
+    pub local_axis1: Unit<Vector<Real>>,
     /// The rotation axis of this revolute joint expressed in the local space of the second attached body.
-    pub local_axis2: Unit<Vector<f32>>,
+    pub local_axis2: Unit<Vector<Real>>,
     /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
-    pub basis1: [Vector<f32>; 2],
+    pub basis1: [Vector<Real>; 2],
     /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
-    pub basis2: [Vector<f32>; 2],
+    pub basis2: [Vector<Real>; 2],
     /// The impulse applied by this joint on the first body.
     ///
     /// The impulse applied to the second body is given by `-impulse`.
-    pub impulse: Vector5<f32>,
+    pub impulse: Vector5<Real>,
 }
 
 impl RevoluteJoint {
     /// Creates a new revolute joint with the given point of applications and axis, all expressed
     /// in the local-space of the affected bodies.
     pub fn new(
-        local_anchor1: Point<f32>,
-        local_axis1: Unit<Vector<f32>>,
-        local_anchor2: Point<f32>,
-        local_axis2: Unit<Vector<f32>>,
+        local_anchor1: Point<Real>,
+        local_axis1: Unit<Vector<Real>>,
+        local_anchor2: Point<Real>,
+        local_axis2: Unit<Vector<Real>>,
     ) -> Self {
         Self {
             local_anchor1,

src/dynamics/mass_properties.rs

@@ -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
-        );
-    }
-}

src/dynamics/mass_properties_ball.rs

@@ -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)
-    }
-}

src/dynamics/mass_properties_capsule.rs

@@ -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,
-            )
-        }
-    }
-}

src/dynamics/mass_properties_cone.rs

@@ -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(),
-        )
-    }
-}

src/dynamics/mass_properties_cuboid.rs

@@ -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)
-    }
-}

src/dynamics/mass_properties_cylinder.rs

@@ -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(),
-        )
-    }
-}

src/dynamics/mass_properties_polygon.rs

@@ -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)
-}

src/dynamics/mod.rs

@@ -7,10 +7,11 @@ 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 parry::mass_properties::MassProperties;
 // #[cfg(not(feature = "parallel"))]
+pub use self::coefficient_combine_rule::CoefficientCombineRule;
 pub(crate) use self::joint::JointGraphEdge;
 pub(crate) use self::rigid_body::RigidBodyChanges;
 #[cfg(not(feature = "parallel"))]
@@ -18,17 +19,9 @@ pub(crate) use self::solver::IslandSolver;
 #[cfg(feature = "parallel")]
 pub(crate) use self::solver::ParallelIslandSolver;
 
+mod coefficient_combine_rule;
 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;

src/dynamics/rigid_body.rs

@@ -2,7 +2,9 @@ use crate::dynamics::MassProperties;
 use crate::geometry::{
     Collider, ColliderHandle, ColliderSet, InteractionGraph, RigidBodyGraphIndex,
 };
-use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
+};
 use crate::utils::{self, WCross, WDot};
 use num::Zero;
 
@@ -29,10 +31,10 @@ bitflags::bitflags! {
     #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
     /// Flags affecting the behavior of the constraints solver for a given contact manifold.
     pub(crate) struct RigidBodyFlags: u8 {
-        const IGNORE_COLLIDER_MASS              = 1 << 0;
-        const IGNORE_COLLIDER_ANGULAR_INERTIA_X = 1 << 1;
-        const IGNORE_COLLIDER_ANGULAR_INERTIA_Y = 1 << 2;
-        const IGNORE_COLLIDER_ANGULAR_INERTIA_Z = 1 << 3;
+        const TRANSLATION_LOCKED = 1 << 0;
+        const ROTATION_LOCKED_X = 1 << 1;
+        const ROTATION_LOCKED_Y = 1 << 2;
+        const ROTATION_LOCKED_Z = 1 << 3;
     }
 }
 
@@ -54,25 +56,29 @@ bitflags::bitflags! {
 #[derive(Debug, Clone)]
 pub struct RigidBody {
     /// The world-space position of the rigid-body.
-    pub(crate) position: Isometry<f32>,
-    pub(crate) predicted_position: Isometry<f32>,
+    pub(crate) position: Isometry<Real>,
+    pub(crate) predicted_position: Isometry<Real>,
     /// The local mass properties of the rigid-body.
     pub(crate) mass_properties: MassProperties,
     /// The world-space center of mass of the rigid-body.
-    pub world_com: Point<f32>,
-    /// The square-root of the inverse angular inertia tensor of the rigid-body.
-    pub world_inv_inertia_sqrt: AngularInertia<f32>,
+    pub world_com: Point<Real>,
+    /// The inverse mass taking into account translation locking.
+    pub effective_inv_mass: Real,
+    /// The square-root of the world-space inverse angular inertia tensor of the rigid-body,
+    /// taking into account rotation locking.
+    pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
     /// The linear velocity of the rigid-body.
-    pub(crate) linvel: Vector<f32>,
+    pub(crate) linvel: Vector<Real>,
     /// The angular velocity of the rigid-body.
-    pub(crate) angvel: AngVector<f32>,
+    pub(crate) angvel: AngVector<Real>,
     /// Damping factor for gradually slowing down the translational motion of the rigid-body.
-    pub linear_damping: f32,
+    pub linear_damping: Real,
     /// Damping factor for gradually slowing down the angular motion of the rigid-body.
-    pub angular_damping: f32,
-    pub(crate) linacc: Vector<f32>,
-    pub(crate) angacc: AngVector<f32>,
+    pub angular_damping: Real,
+    pub(crate) linacc: Vector<Real>,
+    pub(crate) angacc: AngVector<Real>,
     pub(crate) colliders: Vec<ColliderHandle>,
+    pub(crate) gravity_scale: Real,
     /// Whether or not this rigid-body is sleeping.
     pub activation: ActivationStatus,
     pub(crate) joint_graph_index: RigidBodyGraphIndex,
@@ -95,16 +101,18 @@ impl RigidBody {
             predicted_position: Isometry::identity(),
             mass_properties: MassProperties::zero(),
             world_com: Point::origin(),
-            world_inv_inertia_sqrt: AngularInertia::zero(),
+            effective_inv_mass: 0.0,
+            effective_world_inv_inertia_sqrt: AngularInertia::zero(),
             linvel: Vector::zeros(),
             angvel: na::zero(),
             linacc: Vector::zeros(),
             angacc: na::zero(),
+            gravity_scale: 1.0,
             linear_damping: 0.0,
             angular_damping: 0.0,
             colliders: Vec::new(),
             activation: ActivationStatus::new_active(),
-            joint_graph_index: InteractionGraph::<()>::invalid_graph_index(),
+            joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
             active_island_id: 0,
             active_set_id: 0,
             active_set_offset: 0,
@@ -118,22 +126,21 @@ impl RigidBody {
 
     pub(crate) fn reset_internal_references(&mut self) {
         self.colliders = Vec::new();
-        self.joint_graph_index = InteractionGraph::<()>::invalid_graph_index();
+        self.joint_graph_index = InteractionGraph::<(), ()>::invalid_graph_index();
         self.active_island_id = 0;
         self.active_set_id = 0;
         self.active_set_offset = 0;
         self.active_set_timestamp = 0;
     }
 
-    pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector<f32>) {
-        if self.mass_properties.inv_mass != 0.0 {
-            self.linvel += (gravity + self.linacc) * dt;
-            self.angvel += self.angacc * dt;
-
-            // Reset the accelerations.
+    pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector<Real>) {
+        if self.effective_inv_mass != 0.0 {
+            self.linvel += (gravity * self.gravity_scale + self.linacc) * dt;
             self.linacc = na::zero();
-            self.angacc = na::zero();
         }
+
+        self.angvel += self.angacc * dt;
+        self.angacc = na::zero();
     }
 
     /// The mass properties of this rigid-body.
@@ -184,7 +191,7 @@ impl RigidBody {
     /// The mass of this rigid body.
     ///
     /// Returns zero if this rigid body has an infinite mass.
-    pub fn mass(&self) -> f32 {
+    pub fn mass(&self) -> Real {
         utils::inv(self.mass_properties.inv_mass)
     }
 
@@ -193,10 +200,25 @@ impl RigidBody {
     /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position`
     /// method and is used for estimating the kinematic body velocity at the next timestep.
     /// For non-kinematic bodies, this value is currently unspecified.
-    pub fn predicted_position(&self) -> &Isometry<f32> {
+    pub fn predicted_position(&self) -> &Isometry<Real> {
         &self.predicted_position
     }
 
+    /// The scale factor applied to the gravity affecting this rigid-body.
+    pub fn gravity_scale(&self) -> Real {
+        self.gravity_scale
+    }
+
+    /// Sets the gravity scale facter for this rigid-body.
+    pub fn set_gravity_scale(&mut self, scale: Real, wake_up: bool) {
+        if wake_up && self.activation.sleeping {
+            self.changes.insert(RigidBodyChanges::SLEEP);
+            self.activation.sleeping = false;
+        }
+
+        self.gravity_scale = scale;
+    }
+
     /// Adds a collider to this rigid-body.
     pub(crate) fn add_collider(&mut self, handle: ColliderHandle, coll: &Collider) {
         self.changes.set(
@@ -208,40 +230,10 @@ impl RigidBody {
             .mass_properties()
             .transform_by(coll.position_wrt_parent());
         self.colliders.push(handle);
-        self.mass_properties += Self::filter_collider_mass_props(mass_properties, self.flags);
+        self.mass_properties += mass_properties;
         self.update_world_mass_properties();
     }
 
-    fn filter_collider_mass_props(
-        mut props: MassProperties,
-        flags: RigidBodyFlags,
-    ) -> MassProperties {
-        if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_MASS) {
-            props.inv_mass = 0.0;
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
-                props.inv_principal_inertia_sqrt = 0.0;
-            }
-        }
-        #[cfg(feature = "dim3")]
-        {
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X) {
-                props.inv_principal_inertia_sqrt.x = 0.0;
-            }
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y) {
-                props.inv_principal_inertia_sqrt.y = 0.0;
-            }
-            if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
-                props.inv_principal_inertia_sqrt.z = 0.0;
-            }
-        }
-
-        props
-    }
-
     pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
         for handle in &self.colliders {
             let collider = &mut colliders[*handle];
@@ -258,7 +250,7 @@ impl RigidBody {
             let mass_properties = coll
                 .mass_properties()
                 .transform_by(coll.position_wrt_parent());
-            self.mass_properties -= Self::filter_collider_mass_props(mass_properties, self.flags);
+            self.mass_properties -= mass_properties;
             self.update_world_mass_properties();
         }
     }
@@ -311,13 +303,13 @@ impl RigidBody {
         !self.linvel.is_zero() || !self.angvel.is_zero()
     }
 
-    fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
+    fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
         let com = &self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
 
-    pub(crate) fn integrate(&mut self, dt: f32) {
+    pub(crate) fn integrate(&mut self, dt: Real) {
         // TODO: do we want to apply damping before or after the velocity integration?
         self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
         self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
@@ -326,19 +318,19 @@ impl RigidBody {
     }
 
     /// The linear velocity of this rigid-body.
-    pub fn linvel(&self) -> &Vector<f32> {
+    pub fn linvel(&self) -> &Vector<Real> {
         &self.linvel
     }
 
     /// The angular velocity of this rigid-body.
     #[cfg(feature = "dim2")]
-    pub fn angvel(&self) -> f32 {
+    pub fn angvel(&self) -> Real {
         self.angvel
     }
 
     /// The angular velocity of this rigid-body.
     #[cfg(feature = "dim3")]
-    pub fn angvel(&self) -> &Vector<f32> {
+    pub fn angvel(&self) -> &Vector<Real> {
         &self.angvel
     }
 
@@ -346,7 +338,7 @@ impl RigidBody {
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
-    pub fn set_linvel(&mut self, linvel: Vector<f32>, wake_up: bool) {
+    pub fn set_linvel(&mut self, linvel: Vector<Real>, wake_up: bool) {
         self.linvel = linvel;
 
         if self.is_dynamic() && wake_up {
@@ -359,7 +351,7 @@ impl RigidBody {
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
     #[cfg(feature = "dim2")]
-    pub fn set_angvel(&mut self, angvel: f32, wake_up: bool) {
+    pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) {
         self.angvel = angvel;
 
         if self.is_dynamic() && wake_up {
@@ -372,7 +364,7 @@ impl RigidBody {
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
     #[cfg(feature = "dim3")]
-    pub fn set_angvel(&mut self, angvel: Vector<f32>, wake_up: bool) {
+    pub fn set_angvel(&mut self, angvel: Vector<Real>, wake_up: bool) {
         self.angvel = angvel;
 
         if self.is_dynamic() && wake_up {
@@ -381,7 +373,7 @@ impl RigidBody {
     }
 
     /// The world-space position of this rigid-body.
-    pub fn position(&self) -> &Isometry<f32> {
+    pub fn position(&self) -> &Isometry<Real> {
         &self.position
     }
 
@@ -394,7 +386,7 @@ impl RigidBody {
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
     /// put to sleep because it did not move for a while.
-    pub fn set_position(&mut self, pos: Isometry<f32>, wake_up: bool) {
+    pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
         self.changes.insert(RigidBodyChanges::POSITION);
         self.set_position_internal(pos);
 
@@ -404,7 +396,7 @@ impl RigidBody {
         }
     }
 
-    pub(crate) fn set_position_internal(&mut self, pos: Isometry<f32>) {
+    pub(crate) fn set_position_internal(&mut self, pos: Isometry<Real>) {
         self.position = pos;
 
         // TODO: update the predicted position for dynamic bodies too?
@@ -414,13 +406,13 @@ impl RigidBody {
     }
 
     /// If this rigid body is kinematic, sets its future position after the next timestep integration.
-    pub fn set_next_kinematic_position(&mut self, pos: Isometry<f32>) {
+    pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
         if self.is_kinematic() {
             self.predicted_position = pos;
         }
     }
 
-    pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) {
+    pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) {
         let dpos = self.predicted_position * self.position.inverse();
         #[cfg(feature = "dim2")]
         {
@@ -433,24 +425,56 @@ impl RigidBody {
         self.linvel = dpos.translation.vector * inv_dt;
     }
 
-    pub(crate) fn update_predicted_position(&mut self, dt: f32) {
+    pub(crate) fn update_predicted_position(&mut self, dt: Real) {
         self.predicted_position = self.integrate_velocity(dt) * self.position;
     }
 
     pub(crate) fn update_world_mass_properties(&mut self) {
         self.world_com = self.mass_properties.world_com(&self.position);
-        self.world_inv_inertia_sqrt = self
+        self.effective_inv_mass = self.mass_properties.inv_mass;
+        self.effective_world_inv_inertia_sqrt = self
             .mass_properties
             .world_inv_inertia_sqrt(&self.position.rotation);
+
+        // Take into account translation/rotation locking.
+        if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) {
+            self.effective_inv_mass = 0.0;
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
+                self.effective_world_inv_inertia_sqrt = 0.0;
+            }
+        }
+        #[cfg(feature = "dim3")]
+        {
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) {
+                self.effective_world_inv_inertia_sqrt.m11 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+            }
+
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) {
+                self.effective_world_inv_inertia_sqrt.m22 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+            }
+            if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
+                self.effective_world_inv_inertia_sqrt.m33 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+            }
+        }
     }
 
     /*
      * Application of forces/impulses.
      */
     /// Applies a force at the center-of-mass of this rigid-body.
-    pub fn apply_force(&mut self, force: Vector<f32>, wake_up: bool) {
+    pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.linacc += force * self.mass_properties.inv_mass;
+            self.linacc += force * self.effective_inv_mass;
 
             if wake_up {
                 self.wake_up(true);
@@ -459,9 +483,9 @@ impl RigidBody {
     }
 
     /// Applies an impulse at the center-of-mass of this rigid-body.
-    pub fn apply_impulse(&mut self, impulse: Vector<f32>, wake_up: bool) {
+    pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.linvel += impulse * self.mass_properties.inv_mass;
+            self.linvel += impulse * self.effective_inv_mass;
 
             if wake_up {
                 self.wake_up(true);
@@ -471,9 +495,10 @@ impl RigidBody {
 
     /// Applies a torque at the center-of-mass of this rigid-body.
     #[cfg(feature = "dim2")]
-    pub fn apply_torque(&mut self, torque: f32, wake_up: bool) {
+    pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+            self.angacc += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque);
 
             if wake_up {
                 self.wake_up(true);
@@ -483,9 +508,10 @@ impl RigidBody {
 
     /// Applies a torque at the center-of-mass of this rigid-body.
     #[cfg(feature = "dim3")]
-    pub fn apply_torque(&mut self, torque: Vector<f32>, wake_up: bool) {
+    pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+            self.angacc += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque);
 
             if wake_up {
                 self.wake_up(true);
@@ -495,10 +521,10 @@ impl RigidBody {
 
     /// Applies an impulsive torque at the center-of-mass of this rigid-body.
     #[cfg(feature = "dim2")]
-    pub fn apply_torque_impulse(&mut self, torque_impulse: f32, wake_up: bool) {
+    pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angvel +=
-                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+            self.angvel += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque_impulse);
 
             if wake_up {
                 self.wake_up(true);
@@ -508,10 +534,10 @@ impl RigidBody {
 
     /// Applies an impulsive torque at the center-of-mass of this rigid-body.
     #[cfg(feature = "dim3")]
-    pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<f32>, wake_up: bool) {
+    pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
         if self.body_status == BodyStatus::Dynamic {
-            self.angvel +=
-                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+            self.angvel += self.effective_world_inv_inertia_sqrt
+                * (self.effective_world_inv_inertia_sqrt * torque_impulse);
 
             if wake_up {
                 self.wake_up(true);
@@ -520,7 +546,7 @@ impl RigidBody {
     }
 
     /// Applies a force at the given world-space point of this rigid-body.
-    pub fn apply_force_at_point(&mut self, force: Vector<f32>, point: Point<f32>, wake_up: bool) {
+    pub fn apply_force_at_point(&mut self, force: Vector<Real>, point: Point<Real>, wake_up: bool) {
         let torque = (point - self.world_com).gcross(force);
         self.apply_force(force, wake_up);
         self.apply_torque(torque, wake_up);
@@ -529,8 +555,8 @@ impl RigidBody {
     /// Applies an impulse at the given world-space point of this rigid-body.
     pub fn apply_impulse_at_point(
         &mut self,
-        impulse: Vector<f32>,
-        point: Point<f32>,
+        impulse: Vector<Real>,
+        point: Point<Real>,
         wake_up: bool,
     ) {
         let torque_impulse = (point - self.world_com).gcross(impulse);
@@ -539,7 +565,7 @@ impl RigidBody {
     }
 
     /// The velocity of the given world-space point on this rigid-body.
-    pub fn velocity_at_point(&self, point: &Point<f32>) -> Vector<f32> {
+    pub fn velocity_at_point(&self, point: &Point<Real>) -> Vector<Real> {
         let dpt = point - self.world_com;
         self.linvel + self.angvel.gcross(dpt)
     }
@@ -547,11 +573,12 @@ impl RigidBody {
 
 /// A builder for rigid-bodies.
 pub struct RigidBodyBuilder {
-    position: Isometry<f32>,
-    linvel: Vector<f32>,
-    angvel: AngVector<f32>,
-    linear_damping: f32,
-    angular_damping: f32,
+    position: Isometry<Real>,
+    linvel: Vector<Real>,
+    angvel: AngVector<Real>,
+    gravity_scale: Real,
+    linear_damping: Real,
+    angular_damping: Real,
     body_status: BodyStatus,
     flags: RigidBodyFlags,
     mass_properties: MassProperties,
@@ -567,6 +594,7 @@ impl RigidBodyBuilder {
             position: Isometry::identity(),
             linvel: Vector::zeros(),
             angvel: na::zero(),
+            gravity_scale: 1.0,
             linear_damping: 0.0,
             angular_damping: 0.0,
             body_status,
@@ -593,9 +621,15 @@ impl RigidBodyBuilder {
         Self::new(BodyStatus::Dynamic)
     }
 
+    /// Sets the scale applied to the gravity force affecting the rigid-body to be created.
+    pub fn gravity_scale(mut self, x: Real) -> Self {
+        self.gravity_scale = x;
+        self
+    }
+
     /// Sets the initial translation of the rigid-body to be created.
     #[cfg(feature = "dim2")]
-    pub fn translation(mut self, x: f32, y: f32) -> Self {
+    pub fn translation(mut self, x: Real, y: Real) -> Self {
         self.position.translation.x = x;
         self.position.translation.y = y;
         self
@@ -603,7 +637,7 @@ impl RigidBodyBuilder {
 
     /// Sets the initial translation of the rigid-body to be created.
     #[cfg(feature = "dim3")]
-    pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
+    pub fn translation(mut self, x: Real, y: Real, z: Real) -> Self {
         self.position.translation.x = x;
         self.position.translation.y = y;
         self.position.translation.z = z;
@@ -611,13 +645,13 @@ impl RigidBodyBuilder {
     }
 
     /// Sets the initial orientation of the rigid-body to be created.
-    pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
+    pub fn rotation(mut self, angle: AngVector<Real>) -> Self {
         self.position.rotation = Rotation::new(angle);
         self
     }
 
     /// Sets the initial position (translation and orientation) of the rigid-body to be created.
-    pub fn position(mut self, pos: Isometry<f32>) -> Self {
+    pub fn position(mut self, pos: Isometry<Real>) -> Self {
         self.position = pos;
         self
     }
@@ -644,76 +678,53 @@ impl RigidBodyBuilder {
     }
 
     /// Prevents this rigid-body from translating because of forces.
-    ///
-    /// This is equivalent to `self.mass(0.0, false)`. See the
-    /// documentation of [`RigidBodyBuilder::mass`] for more details.
-    pub fn lock_translations(self) -> Self {
-        self.mass(0.0, false)
+    pub fn lock_translations(mut self) -> Self {
+        self.flags.set(RigidBodyFlags::TRANSLATION_LOCKED, true);
+        self
     }
 
     /// Prevents this rigid-body from rotating because of forces.
-    ///
-    /// This is equivalent to `self.principal_inertia(0.0, false)` (in 2D) or
-    /// `self.principal_inertia(Vector3::zeros(), Vector3::repeat(false))` (in 3D).
-    ///
-    /// See the documentation of [`RigidBodyBuilder::principal_inertia`] for more details.
-    pub fn lock_rotations(self) -> Self {
-        #[cfg(feature = "dim2")]
-        return self.principal_angular_inertia(0.0, false);
-        #[cfg(feature = "dim3")]
-        return self.principal_angular_inertia(Vector::zeros(), Vector::repeat(false));
+    pub fn lock_rotations(mut self) -> Self {
+        self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true);
+        self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true);
+        self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true);
+        self
+    }
+
+    /// Only allow rotations of this rigid-body around specific coordinate axes.
+    #[cfg(feature = "dim3")]
+    pub fn restrict_rotations(
+        mut self,
+        allow_rotations_x: bool,
+        allow_rotations_y: bool,
+        allow_rotations_z: bool,
+    ) -> Self {
+        self.flags
+            .set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x);
+        self.flags
+            .set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y);
+        self.flags
+            .set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z);
+        self
     }
 
     /// Sets the mass of the rigid-body being built.
-    ///
-    /// In order to lock the translations of this rigid-body (by
-    /// making them kinematic), call `.mass(0.0, false)`.
-    ///
-    /// If `colliders_contribution_enabled` is `false`, then the mass specified here
-    /// will be the final mass of the rigid-body created by this builder.
-    /// If `colliders_contribution_enabled` is `true`, then the final mass of the rigid-body
-    /// will depends on the initial mass set by this method to which is added
-    /// the contributions of all the colliders with non-zero density attached to
-    /// this rigid-body.
-    pub fn mass(mut self, mass: f32, colliders_contribution_enabled: bool) -> Self {
+    pub fn mass(mut self, mass: Real) -> Self {
         self.mass_properties.inv_mass = utils::inv(mass);
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_MASS,
-            !colliders_contribution_enabled,
-        );
         self
     }
     /// Sets the angular inertia of this rigid-body.
-    ///
-    /// In order to lock the rotations of this rigid-body (by
-    /// making them kinematic), call `.principal_inertia(0.0, false)`.
-    ///
-    /// If `colliders_contribution_enabled` is `false`, then the principal inertia specified here
-    /// will be the final principal inertia of the rigid-body created by this builder.
-    /// If `colliders_contribution_enabled` is `true`, then the final principal of the rigid-body
-    /// will depend on the initial principal inertia set by this method to which is added
-    /// the contributions of all the colliders with non-zero density attached to this rigid-body.
     #[cfg(feature = "dim2")]
-    pub fn principal_angular_inertia(
-        mut self,
-        inertia: f32,
-        colliders_contribution_enabled: bool,
-    ) -> Self {
+    pub fn principal_angular_inertia(mut self, inertia: Real) -> Self {
         self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia);
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X
-                | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y
-                | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
-            !colliders_contribution_enabled,
-        );
         self
     }
 
     /// Use `self.principal_angular_inertia` instead.
     #[cfg(feature = "dim2")]
     #[deprecated(note = "renamed to `principal_angular_inertia`.")]
-    pub fn principal_inertia(self, inertia: f32, colliders_contribution_enabled: bool) -> Self {
-        self.principal_angular_inertia(inertia, colliders_contribution_enabled)
+    pub fn principal_inertia(self, inertia: Real) -> Self {
+        self.principal_angular_inertia(inertia)
     }
 
     /// Sets the principal angular inertia of this rigid-body.
@@ -729,43 +740,23 @@ impl RigidBodyBuilder {
     /// to which is added the contributions of all the colliders with non-zero density
     /// attached to this rigid-body.
     #[cfg(feature = "dim3")]
-    pub fn principal_angular_inertia(
-        mut self,
-        inertia: AngVector<f32>,
-        colliders_contribution_enabled: AngVector<bool>,
-    ) -> Self {
+    pub fn principal_angular_inertia(mut self, inertia: AngVector<Real>) -> Self {
         self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv);
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X,
-            !colliders_contribution_enabled.x,
-        );
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y,
-            !colliders_contribution_enabled.y,
-        );
-        self.flags.set(
-            RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
-            !colliders_contribution_enabled.z,
-        );
         self
     }
 
     /// Use `self.principal_angular_inertia` instead.
     #[cfg(feature = "dim3")]
     #[deprecated(note = "renamed to `principal_angular_inertia`.")]
-    pub fn principal_inertia(
-        self,
-        inertia: AngVector<f32>,
-        colliders_contribution_enabled: AngVector<bool>,
-    ) -> Self {
-        self.principal_angular_inertia(inertia, colliders_contribution_enabled)
+    pub fn principal_inertia(self, inertia: AngVector<Real>) -> Self {
+        self.principal_angular_inertia(inertia)
     }
 
     /// Sets the damping factor for the linear part of the rigid-body motion.
     ///
     /// The higher the linear damping factor is, the more quickly the rigid-body
     /// will slow-down its translational movement.
-    pub fn linear_damping(mut self, factor: f32) -> Self {
+    pub fn linear_damping(mut self, factor: Real) -> Self {
         self.linear_damping = factor;
         self
     }
@@ -774,27 +765,27 @@ impl RigidBodyBuilder {
     ///
     /// The higher the angular damping factor is, the more quickly the rigid-body
     /// will slow-down its rotational movement.
-    pub fn angular_damping(mut self, factor: f32) -> Self {
+    pub fn angular_damping(mut self, factor: Real) -> Self {
         self.angular_damping = factor;
         self
     }
 
     /// Sets the initial linear velocity of the rigid-body to be created.
     #[cfg(feature = "dim2")]
-    pub fn linvel(mut self, x: f32, y: f32) -> Self {
+    pub fn linvel(mut self, x: Real, y: Real) -> Self {
         self.linvel = Vector::new(x, y);
         self
     }
 
     /// Sets the initial linear velocity of the rigid-body to be created.
     #[cfg(feature = "dim3")]
-    pub fn linvel(mut self, x: f32, y: f32, z: f32) -> Self {
+    pub fn linvel(mut self, x: Real, y: Real, z: Real) -> Self {
         self.linvel = Vector::new(x, y, z);
         self
     }
 
     /// Sets the initial angular velocity of the rigid-body to be created.
-    pub fn angvel(mut self, angvel: AngVector<f32>) -> Self {
+    pub fn angvel(mut self, angvel: AngVector<Real>) -> Self {
         self.angvel = angvel;
         self
     }
@@ -823,6 +814,7 @@ impl RigidBodyBuilder {
         rb.mass_properties = self.mass_properties;
         rb.linear_damping = self.linear_damping;
         rb.angular_damping = self.angular_damping;
+        rb.gravity_scale = self.gravity_scale;
         rb.flags = self.flags;
 
         if self.can_sleep && self.sleeping {
@@ -845,16 +837,16 @@ impl RigidBodyBuilder {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct ActivationStatus {
     /// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
-    pub threshold: f32,
+    pub threshold: Real,
     /// The current pseudo-kinetic energy of the body.
-    pub energy: f32,
+    pub energy: Real,
     /// Is this body already sleeping?
     pub sleeping: bool,
 }
 
 impl ActivationStatus {
     /// The default amount of energy bellow which a body can be put to sleep by nphysics.
-    pub fn default_threshold() -> f32 {
+    pub fn default_threshold() -> Real {
         0.01
     }
 

src/dynamics/rigid_body_set.rs

@@ -3,11 +3,45 @@ use rayon::prelude::*;
 
 use crate::data::arena::Arena;
 use crate::dynamics::{Joint, JointSet, RigidBody, RigidBodyChanges};
-use crate::geometry::{ColliderHandle, ColliderSet, InteractionGraph, NarrowPhase};
+use crate::geometry::{ColliderSet, InteractionGraph, NarrowPhase};
+use parry::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
 
 /// The unique handle of a rigid body added to a `RigidBodySet`.
-pub type RigidBodyHandle = crate::data::arena::Index;
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct RigidBodyHandle(pub(crate) crate::data::arena::Index);
+
+impl RigidBodyHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (usize, u64) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid rigid-body handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_USIZE,
+            crate::INVALID_U64,
+        ))
+    }
+}
+
+impl IndexedData for RigidBodyHandle {
+    fn default() -> Self {
+        Self(IndexedData::default())
+    }
+
+    fn index(&self) -> usize {
+        self.0.index()
+    }
+}
 
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -20,13 +54,10 @@ pub struct BodyPair {
 }
 
 impl BodyPair {
-    pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
+    /// Builds a new pair of rigid-body handles.
+    pub fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
         BodyPair { body1, body2 }
     }
-
-    pub(crate) fn swap(self) -> Self {
-        Self::new(self.body2, self.body1)
-    }
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -70,11 +101,6 @@ impl RigidBodySet {
         }
     }
 
-    /// An always-invalid rigid-body handle.
-    pub fn invalid_handle() -> RigidBodyHandle {
-        RigidBodyHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
-    }
-
     /// The number of rigid bodies on this set.
     pub fn len(&self) -> usize {
         self.bodies.len()
@@ -82,7 +108,7 @@ impl RigidBodySet {
 
     /// Is the given body handle valid?
     pub fn contains(&self, handle: RigidBodyHandle) -> bool {
-        self.bodies.contains(handle)
+        self.bodies.contains(handle.0)
     }
 
     /// Insert a rigid body into this set and retrieve its handle.
@@ -92,10 +118,10 @@ impl RigidBodySet {
         rb.reset_internal_references();
         rb.changes.set(RigidBodyChanges::all(), true);
 
-        let handle = self.bodies.insert(rb);
+        let handle = RigidBodyHandle(self.bodies.insert(rb));
         self.modified_bodies.push(handle);
 
-        let rb = &mut self.bodies[handle];
+        let rb = &mut self.bodies[handle.0];
 
         if rb.is_kinematic() {
             rb.active_set_id = self.active_kinematic_set.len();
@@ -112,7 +138,7 @@ impl RigidBodySet {
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
     ) -> Option<RigidBody> {
-        let rb = self.bodies.remove(handle)?;
+        let rb = self.bodies.remove(handle.0)?;
         /*
          * Update active sets.
          */
@@ -123,7 +149,7 @@ impl RigidBodySet {
                 active_set.swap_remove(rb.active_set_id);
 
                 if let Some(replacement) = active_set.get(rb.active_set_id) {
-                    self.bodies[*replacement].active_set_id = rb.active_set_id;
+                    self.bodies[replacement.0].active_set_id = rb.active_set_id;
                 }
             }
         }
@@ -152,7 +178,7 @@ impl RigidBodySet {
     /// If `strong` is `true` then it is assured that the rigid-body will
     /// remain awake during multiple subsequent timesteps.
     pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
-        if let Some(rb) = self.bodies.get_mut(handle) {
+        if let Some(rb) = self.bodies.get_mut(handle.0) {
             // TODO: what about kinematic bodies?
             if rb.is_dynamic() {
                 rb.wake_up(strong);
@@ -175,7 +201,9 @@ impl RigidBodySet {
     /// Using this is discouraged in favor of `self.get(handle)` which does not
     /// suffer form the ABA problem.
     pub fn get_unknown_gen(&self, i: usize) -> Option<(&RigidBody, RigidBodyHandle)> {
-        self.bodies.get_unknown_gen(i)
+        self.bodies
+            .get_unknown_gen(i)
+            .map(|(b, h)| (b, RigidBodyHandle(h)))
     }
 
     /// Gets a mutable reference to the rigid-body with the given handle without a known generation.
@@ -191,19 +219,19 @@ impl RigidBodySet {
         let result = self.bodies.get_unknown_gen_mut(i)?;
         if !self.modified_all_bodies && !result.0.changes.contains(RigidBodyChanges::MODIFIED) {
             result.0.changes = RigidBodyChanges::MODIFIED;
-            self.modified_bodies.push(result.1);
+            self.modified_bodies.push(RigidBodyHandle(result.1));
         }
-        Some(result)
+        Some((result.0, RigidBodyHandle(result.1)))
     }
 
     /// Gets the rigid-body with the given handle.
     pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
-        self.bodies.get(handle)
+        self.bodies.get(handle.0)
     }
 
     /// Gets a mutable reference to the rigid-body with the given handle.
     pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
-        let result = self.bodies.get_mut(handle)?;
+        let result = self.bodies.get_mut(handle.0)?;
         if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
             result.changes = RigidBodyChanges::MODIFIED;
             self.modified_bodies.push(handle);
@@ -212,7 +240,7 @@ impl RigidBodySet {
     }
 
     pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
-        self.bodies.get_mut(handle)
+        self.bodies.get_mut(handle.0)
     }
 
     pub(crate) fn get2_mut_internal(
@@ -220,19 +248,19 @@ impl RigidBodySet {
         h1: RigidBodyHandle,
         h2: RigidBodyHandle,
     ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
-        self.bodies.get2_mut(h1, h2)
+        self.bodies.get2_mut(h1.0, h2.0)
     }
 
     /// Iterates through all the rigid-bodies on this set.
     pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
-        self.bodies.iter()
+        self.bodies.iter().map(|(h, b)| (RigidBodyHandle(h), b))
     }
 
     /// Iterates mutably through all the rigid-bodies on this set.
     pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, &mut RigidBody)> {
         self.modified_bodies.clear();
         self.modified_all_bodies = true;
-        self.bodies.iter_mut()
+        self.bodies.iter_mut().map(|(h, b)| (RigidBodyHandle(h), b))
     }
 
     /// Iter through all the active kinematic rigid-bodies on this set.
@@ -242,7 +270,7 @@ impl RigidBodySet {
         let bodies: &'a _ = &self.bodies;
         self.active_kinematic_set
             .iter()
-            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
     }
 
     /// Iter through all the active dynamic rigid-bodies on this set.
@@ -252,7 +280,7 @@ impl RigidBodySet {
         let bodies: &'a _ = &self.bodies;
         self.active_dynamic_set
             .iter()
-            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
     }
 
     #[cfg(not(feature = "parallel"))]
@@ -264,7 +292,7 @@ impl RigidBodySet {
         let bodies: &'a _ = &self.bodies;
         self.active_dynamic_set[island_range]
             .iter()
-            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+            .filter_map(move |h| Some((*h, bodies.get(h.0)?)))
     }
 
     #[inline(always)]
@@ -273,13 +301,13 @@ impl RigidBodySet {
         mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
     ) {
         for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(*handle) {
+            if let Some(rb) = self.bodies.get_mut(handle.0) {
                 f(*handle, rb)
             }
         }
 
         for handle in &self.active_kinematic_set {
-            if let Some(rb) = self.bodies.get_mut(*handle) {
+            if let Some(rb) = self.bodies.get_mut(handle.0) {
                 f(*handle, rb)
             }
         }
@@ -291,7 +319,7 @@ impl RigidBodySet {
         mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
     ) {
         for handle in &self.active_dynamic_set {
-            if let Some(rb) = self.bodies.get_mut(*handle) {
+            if let Some(rb) = self.bodies.get_mut(handle.0) {
                 f(*handle, rb)
             }
         }
@@ -303,7 +331,7 @@ impl RigidBodySet {
         mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
     ) {
         for handle in &self.active_kinematic_set {
-            if let Some(rb) = self.bodies.get_mut(*handle) {
+            if let Some(rb) = self.bodies.get_mut(handle.0) {
                 f(*handle, rb)
             }
         }
@@ -318,7 +346,7 @@ impl RigidBodySet {
     ) {
         let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
         for handle in &self.active_dynamic_set[island_range] {
-            if let Some(rb) = self.bodies.get_mut(*handle) {
+            if let Some(rb) = self.bodies.get_mut(handle.0) {
                 f(*handle, rb)
             }
         }
@@ -342,7 +370,7 @@ impl RigidBodySet {
                 || bodies.load(Ordering::Relaxed),
                 |bodies, handle| {
                     let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
-                    if let Some(rb) = bodies.get_mut(*handle) {
+                    if let Some(rb) = bodies.get_mut(handle.0) {
                         f(*handle, rb)
                     }
                 },
@@ -405,7 +433,7 @@ impl RigidBodySet {
             for (handle, rb) in self.bodies.iter_mut() {
                 Self::maintain_one(
                     colliders,
-                    handle,
+                    RigidBodyHandle(handle),
                     rb,
                     &mut self.modified_inactive_set,
                     &mut self.active_kinematic_set,
@@ -414,9 +442,10 @@ impl RigidBodySet {
             }
 
             self.modified_bodies.clear();
+            self.modified_all_bodies = false;
         } else {
             for handle in self.modified_bodies.drain(..) {
-                if let Some(rb) = self.bodies.get_mut(handle) {
+                if let Some(rb) = self.bodies.get_mut(handle.0) {
                     Self::maintain_one(
                         colliders,
                         handle,
@@ -434,7 +463,7 @@ impl RigidBodySet {
         &mut self,
         colliders: &ColliderSet,
         narrow_phase: &NarrowPhase,
-        joint_graph: &InteractionGraph<Joint>,
+        joint_graph: &InteractionGraph<RigidBodyHandle, Joint>,
         min_island_size: usize,
     ) {
         assert!(
@@ -454,7 +483,7 @@ impl RigidBodySet {
         // does not seem to affect performances nor stability. However it makes
         // debugging slightly nicer so we keep this rev.
         for h in self.active_dynamic_set.drain(..).rev() {
-            let rb = &mut self.bodies[h];
+            let rb = &mut self.bodies[h.0];
             rb.update_energy();
             if rb.activation.energy <= rb.activation.threshold {
                 // Mark them as sleeping for now. This will
@@ -469,18 +498,18 @@ impl RigidBodySet {
 
         // Read all the contacts and push objects touching touching this rigid-body.
         #[inline(always)]
-        fn push_contacting_colliders(
+        fn push_contacting_bodies(
             rb: &RigidBody,
             colliders: &ColliderSet,
             narrow_phase: &NarrowPhase,
-            stack: &mut Vec<ColliderHandle>,
+            stack: &mut Vec<RigidBodyHandle>,
         ) {
             for collider_handle in &rb.colliders {
                 if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
                     for inter in contacts {
                         for manifold in &inter.2.manifolds {
-                            if manifold.num_active_contacts() > 0 {
-                                let other = crate::utils::other_handle(
+                            if !manifold.data.solver_contacts.is_empty() {
+                                let other = crate::utils::select_other(
                                     (inter.0, inter.1),
                                     *collider_handle,
                                 );
@@ -497,7 +526,7 @@ impl RigidBodySet {
         // Now iterate on all active kinematic bodies and push all the bodies
         // touching them to the stack so they can be woken up.
         for h in self.active_kinematic_set.iter() {
-            let rb = &self.bodies[*h];
+            let rb = &self.bodies[h.0];
 
             if !rb.is_moving() {
                 // If the kinematic body does not move, it does not have
@@ -505,7 +534,7 @@ impl RigidBodySet {
                 continue;
             }
 
-            push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack);
+            push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
         }
 
         //        println!("Selection: {}", instant::now() - t);
@@ -520,7 +549,7 @@ impl RigidBodySet {
         let mut island_marker = self.stack.len().max(1) - 1;
 
         while let Some(handle) = self.stack.pop() {
-            let rb = &mut self.bodies[handle];
+            let rb = &mut self.bodies[handle.0];
 
             if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
                 // We already visited this body and its neighbors.
@@ -548,10 +577,10 @@ impl RigidBodySet {
 
             // Transmit the active state to all the rigid-bodies with colliders
             // in contact or joined with this collider.
-            push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack);
+            push_contacting_bodies(rb, colliders, narrow_phase, &mut self.stack);
 
             for inter in joint_graph.interactions_with(rb.joint_graph_index) {
-                let other = crate::utils::other_handle((inter.0, inter.1), handle);
+                let other = crate::utils::select_other((inter.0, inter.1), handle);
                 self.stack.push(other);
             }
         }
@@ -566,7 +595,7 @@ impl RigidBodySet {
         // Actually put to sleep bodies which have not been detected as awake.
         //        let t = instant::now();
         for h in &self.can_sleep {
-            let b = &mut self.bodies[*h];
+            let b = &mut self.bodies[h.0];
             if b.activation.sleeping {
                 b.sleep();
             }
@@ -579,12 +608,12 @@ impl Index<RigidBodyHandle> for RigidBodySet {
     type Output = RigidBody;
 
     fn index(&self, index: RigidBodyHandle) -> &RigidBody {
-        &self.bodies[index]
+        &self.bodies[index.0]
     }
 }
 
 impl IndexMut<RigidBodyHandle> for RigidBodySet {
     fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
-        &mut self.bodies[index]
+        &mut self.bodies[index.0]
     }
 }

src/dynamics/solver/categorization.rs

@@ -1,35 +1,7 @@
 use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
-use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
-pub(crate) fn categorize_position_contacts(
-    bodies: &RigidBodySet,
-    manifolds: &[&mut ContactManifold],
-    manifold_indices: &[ContactManifoldIndex],
-    out_point_point_ground: &mut Vec<ContactManifoldIndex>,
-    out_plane_point_ground: &mut Vec<ContactManifoldIndex>,
-    out_point_point: &mut Vec<ContactManifoldIndex>,
-    out_plane_point: &mut Vec<ContactManifoldIndex>,
-) {
-    for manifold_i in manifold_indices {
-        let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
-
-        if !rb1.is_dynamic() || !rb2.is_dynamic() {
-            match manifold.kinematics.category {
-                KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
-                KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
-            }
-        } else {
-            match manifold.kinematics.category {
-                KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
-                KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
-            }
-        }
-    }
-}
-
-pub(crate) fn categorize_velocity_contacts(
+pub(crate) fn categorize_contacts(
     bodies: &RigidBodySet,
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
@@ -38,8 +10,8 @@ pub(crate) fn categorize_velocity_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.body_pair.body1];
-        let rb2 = &bodies[manifold.body_pair.body2];
+        let rb1 = &bodies[manifold.data.body_pair.body1];
+        let rb2 = &bodies[manifold.data.body_pair.body2];
 
         if !rb1.is_dynamic() || !rb2.is_dynamic() {
             out_ground.push(*manifold_i)

src/dynamics/solver/delta_vel.rs

@@ -3,7 +3,7 @@ use na::{Scalar, SimdRealField};
 
 #[derive(Copy, Clone, Debug)]
 //#[repr(align(64))]
-pub(crate) struct DeltaVel<N: Scalar> {
+pub(crate) struct DeltaVel<N: Scalar + Copy> {
     pub linear: Vector<N>,
     pub angular: AngVector<N>,
 }

src/dynamics/solver/interaction_groups.rs

@@ -12,7 +12,7 @@ pub(crate) trait PairInteraction {
 
 impl<'a> PairInteraction for &'a mut ContactManifold {
     fn body_pair(&self) -> BodyPair {
-        self.body_pair
+        self.data.body_pair
     }
 }
 
@@ -338,7 +338,7 @@ impl InteractionGroups {
         let mut occupied_mask = 0u128;
         let max_interaction_points = interaction_indices
             .iter()
-            .map(|i| interactions[*i].num_active_contacts())
+            .map(|i| interactions[*i].data.num_active_contacts())
             .max()
             .unwrap_or(1);
 
@@ -351,12 +351,12 @@ impl InteractionGroups {
 
                 // FIXME: how could we avoid iterating
                 // on each interaction at every iteration on k?
-                if interaction.num_active_contacts() != k {
+                if interaction.data.num_active_contacts() != k {
                     continue;
                 }
 
-                let body1 = &bodies[interaction.body_pair.body1];
-                let body2 = &bodies[interaction.body_pair.body2];
+                let body1 = &bodies[interaction.data.body_pair.body1];
+                let body2 = &bodies[interaction.data.body_pair.body2];
                 let is_static1 = !body1.is_dynamic();
                 let is_static2 = !body2.is_dynamic();
 

src/dynamics/solver/island_solver.rs

@@ -1,9 +1,15 @@
 use super::{PositionSolver, VelocitySolver};
 use crate::counters::Counters;
+use crate::dynamics::solver::{
+    AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
+    AnyVelocityConstraint, SolverConstraints,
+};
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub struct IslandSolver {
+    contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
+    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
     velocity_solver: VelocitySolver,
     position_solver: PositionSolver,
 }
@@ -11,6 +17,8 @@ pub struct IslandSolver {
 impl IslandSolver {
     pub fn new() -> Self {
         Self {
+            contact_constraints: SolverConstraints::new(),
+            joint_constraints: SolverConstraints::new(),
             velocity_solver: VelocitySolver::new(),
             position_solver: PositionSolver::new(),
         }
@@ -29,33 +37,23 @@ impl IslandSolver {
     ) {
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.velocity_assembly_time.resume();
-            self.velocity_solver.init_constraints(
-                island_id,
-                params,
-                bodies,
-                manifolds,
-                &manifold_indices,
-                joints,
-                &joint_indices,
-            );
+            self.contact_constraints
+                .init(island_id, params, bodies, manifolds, manifold_indices);
+            self.joint_constraints
+                .init(island_id, params, bodies, joints, joint_indices);
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
-            self.velocity_solver
-                .solve_constraints(island_id, params, bodies, manifolds, joints);
-            counters.solver.velocity_resolution_time.pause();
-
-            counters.solver.position_assembly_time.resume();
-            self.position_solver.init_constraints(
+            self.velocity_solver.solve(
                 island_id,
                 params,
                 bodies,
                 manifolds,
-                &manifold_indices,
                 joints,
-                &joint_indices,
+                &mut self.contact_constraints.velocity_constraints,
+                &mut self.joint_constraints.velocity_constraints,
             );
-            counters.solver.position_assembly_time.pause();
+            counters.solver.velocity_resolution_time.pause();
         }
 
         counters.solver.velocity_update_time.resume();
@@ -64,8 +62,13 @@ impl IslandSolver {
 
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.position_resolution_time.resume();
-            self.position_solver
-                .solve_constraints(island_id, params, bodies);
+            self.position_solver.solve(
+                island_id,
+                params,
+                bodies,
+                &self.contact_constraints.position_constraints,
+                &self.joint_constraints.position_constraints,
+            );
             counters.solver.position_resolution_time.pause();
         }
     }

src/dynamics/solver/joint_constraint/ball_position_constraint.rs

@@ -1,7 +1,7 @@
 use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
 #[cfg(feature = "dim2")]
 use crate::math::SdpMatrix;
-use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint {
     position1: usize,
     position2: usize,
 
-    local_com1: Point<f32>,
-    local_com2: Point<f32>,
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    local_anchor1: Point<f32>,
-    local_anchor2: Point<f32>,
+    local_anchor1: Point<Real>,
+    local_anchor2: Point<Real>,
 }
 
 impl BallPositionConstraint {
@@ -27,10 +27,10 @@ impl BallPositionConstraint {
         Self {
             local_com1: rb1.mass_properties.local_com,
             local_com2: rb2.mass_properties.local_com,
-            im1: rb1.mass_properties.inv_mass,
-            im2: rb2.mass_properties.inv_mass,
-            ii1: rb1.world_inv_inertia_sqrt.squared(),
-            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            im1: rb1.effective_inv_mass,
+            im2: rb2.effective_inv_mass,
+            ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
             position1: rb1.active_set_offset,
@@ -38,7 +38,7 @@ impl BallPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -95,11 +95,11 @@ impl BallPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct BallPositionGroundConstraint {
     position2: usize,
-    anchor1: Point<f32>,
-    im2: f32,
-    ii2: AngularInertia<f32>,
-    local_anchor2: Point<f32>,
-    local_com2: Point<f32>,
+    anchor1: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    local_anchor2: Point<Real>,
+    local_com2: Point<Real>,
 }
 
 impl BallPositionGroundConstraint {
@@ -115,8 +115,8 @@ impl BallPositionGroundConstraint {
             // already been flipped by the caller.
             Self {
                 anchor1: rb1.predicted_position * cparams.local_anchor2,
-                im2: rb2.mass_properties.inv_mass,
-                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                im2: rb2.effective_inv_mass,
+                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor1,
                 position2: rb2.active_set_offset,
                 local_com2: rb2.mass_properties.local_com,
@@ -124,8 +124,8 @@ impl BallPositionGroundConstraint {
         } else {
             Self {
                 anchor1: rb1.predicted_position * cparams.local_anchor1,
-                im2: rb2.mass_properties.inv_mass,
-                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                im2: rb2.effective_inv_mass,
+                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 local_anchor2: cparams.local_anchor2,
                 position2: rb2.active_set_offset,
                 local_com2: rb2.mass_properties.local_com,
@@ -133,7 +133,7 @@ impl BallPositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
         let anchor2 = position2 * self.local_anchor2;

src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs

@@ -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, Real, 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,14 +31,14 @@ 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(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1 = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
-        let ii2 = AngularInertia::<SimdFloat>::from(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let ii2 = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
@@ -60,7 +60,7 @@ impl WBallPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
         let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
 
@@ -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,9 +141,9 @@ 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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2 = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         )
         .squared();
         let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
@@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
 
         let anchor2 = position2 * self.local_anchor2;
@@ -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));

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{SdpMatrix, Vector};
+use crate::math::{AngularInertia, Real, SdpMatrix, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -12,16 +12,19 @@ pub(crate) struct BallVelocityConstraint {
 
     joint_id: JointIndex,
 
-    rhs: Vector<f32>,
-    pub(crate) impulse: Vector<f32>,
+    rhs: Vector<Real>,
+    pub(crate) impulse: Vector<Real>,
 
-    gcross1: Vector<f32>,
-    gcross2: Vector<f32>,
+    r1: Vector<Real>,
+    r2: Vector<Real>,
 
-    inv_lhs: SdpMatrix<f32>,
+    inv_lhs: SdpMatrix<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
+
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl BallVelocityConstraint {
@@ -37,8 +40,8 @@ impl BallVelocityConstraint {
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
 
         let rhs = -(vel1 - vel2);
         let lhs;
@@ -49,12 +52,12 @@ impl BallVelocityConstraint {
         #[cfg(feature = "dim3")]
         {
             lhs = rb2
-                .world_inv_inertia_sqrt
+                .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
                 .add_diagonal(im2)
                 + rb1
-                    .world_inv_inertia_sqrt
+                    .effective_world_inv_inertia_sqrt
                     .squared()
                     .quadform(&cmat1)
                     .add_diagonal(im1);
@@ -64,17 +67,14 @@ impl BallVelocityConstraint {
         // it's just easier that way.
         #[cfg(feature = "dim2")]
         {
-            let ii1 = rb1.world_inv_inertia_sqrt.squared();
-            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
             let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
             lhs = SdpMatrix::new(m11, m12, m22)
         }
 
-        let gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1);
-        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
-
         let inv_lhs = lhs.inverse_unchecked();
 
         BallVelocityConstraint {
@@ -84,42 +84,46 @@ impl BallVelocityConstraint {
             im1,
             im2,
             impulse: cparams.impulse * params.warmstart_coeff,
-            gcross1,
-            gcross2,
+            r1: anchor1,
+            r2: anchor2,
             rhs,
             inv_lhs,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         mj_lambda1.linear += self.im1 * self.impulse;
-        mj_lambda1.angular += self.gcross1.gcross(self.impulse);
+        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
-        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
-        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
+        let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let dvel = -vel1 + vel2 + self.rhs;
 
         let impulse = self.inv_lhs * dvel;
         self.impulse += impulse;
 
         mj_lambda1.linear += self.im1 * impulse;
-        mj_lambda1.angular += self.gcross1.gcross(impulse);
+        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
 
         mj_lambda2.linear -= self.im2 * impulse;
-        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -137,11 +141,12 @@ impl BallVelocityConstraint {
 pub(crate) struct BallVelocityGroundConstraint {
     mj_lambda2: usize,
     joint_id: JointIndex,
-    rhs: Vector<f32>,
-    impulse: Vector<f32>,
-    gcross2: Vector<f32>,
-    inv_lhs: SdpMatrix<f32>,
-    im2: f32,
+    rhs: Vector<Real>,
+    impulse: Vector<Real>,
+    r2: Vector<Real>,
+    inv_lhs: SdpMatrix<Real>,
+    im2: Real,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl BallVelocityGroundConstraint {
@@ -165,20 +170,19 @@ impl BallVelocityGroundConstraint {
             )
         };
 
-        let im2 = rb2.mass_properties.inv_mass;
+        let im2 = rb2.effective_inv_mass;
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
         let rhs = vel2 - vel1;
 
         let cmat2 = anchor2.gcross_matrix();
-        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
 
         let lhs;
 
         #[cfg(feature = "dim3")]
         {
             lhs = rb2
-                .world_inv_inertia_sqrt
+                .effective_world_inv_inertia_sqrt
                 .squared()
                 .quadform(&cmat2)
                 .add_diagonal(im2);
@@ -186,7 +190,7 @@ impl BallVelocityGroundConstraint {
 
         #[cfg(feature = "dim2")]
         {
-            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
             let m11 = im2 + cmat2.x * cmat2.x * ii2;
             let m12 = cmat2.x * cmat2.y * ii2;
             let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -200,30 +204,32 @@ impl BallVelocityGroundConstraint {
             mj_lambda2: rb2.active_set_offset,
             im2,
             impulse: cparams.impulse * params.warmstart_coeff,
-            gcross2,
+            r2: anchor2,
             rhs,
             inv_lhs,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
-        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
         let dvel = vel2 + self.rhs;
 
         let impulse = self.inv_lhs * dvel;
         self.impulse += impulse;
 
         mj_lambda2.linear -= self.im2 * impulse;
-        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }

src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs

@@ -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, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use simba::simd::SimdValue;
@@ -15,16 +15,19 @@ 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>,
+    r1: Vector<SimdReal>,
+    r2: Vector<SimdReal>,
 
-    inv_lhs: SdpMatrix<SimdFloat>,
+    inv_lhs: SdpMatrix<SimdReal>,
 
-    im1: SimdFloat,
-    im2: SimdFloat,
+    im1: SimdReal,
+    im2: SimdReal,
+
+    ii1_sqrt: AngularInertia<SimdReal>,
+    ii2_sqrt: AngularInertia<SimdReal>,
 }
 
 impl WBallVelocityConstraint {
@@ -37,21 +40,21 @@ 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(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
@@ -62,8 +65,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;
 
@@ -88,9 +91,6 @@ impl WBallVelocityConstraint {
             lhs = SdpMatrix::new(m11, m12, m22)
         }
 
-        let gcross1 = ii1_sqrt.transform_lin_vector(anchor1);
-        let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
-
         let inv_lhs = lhs.inverse_unchecked();
 
         WBallVelocityConstraint {
@@ -99,15 +99,17 @@ impl WBallVelocityConstraint {
             mj_lambda2,
             im1,
             im2,
-            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
-            gcross1,
-            gcross2,
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            r1: anchor1,
+            r2: anchor2,
             rhs,
             inv_lhs,
+            ii1_sqrt,
+            ii2_sqrt,
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -126,9 +128,9 @@ impl WBallVelocityConstraint {
         };
 
         mj_lambda1.linear += self.impulse * self.im1;
-        mj_lambda1.angular += self.gcross1.gcross(self.impulse);
+        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
         mj_lambda2.linear -= self.impulse * self.im2;
-        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -140,8 +142,8 @@ impl WBallVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
-        let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        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 +151,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],
             ),
@@ -158,18 +160,20 @@ impl WBallVelocityConstraint {
             ),
         };
 
-        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
-        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
+        let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let dvel = -vel1 + vel2 + self.rhs;
 
         let impulse = self.inv_lhs * dvel;
         self.impulse += impulse;
 
         mj_lambda1.linear += impulse * self.im1;
-        mj_lambda1.angular += self.gcross1.gcross(impulse);
+        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
 
         mj_lambda2.linear -= impulse * self.im2;
-        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -195,11 +199,12 @@ 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>,
+    r2: Vector<SimdReal>,
+    inv_lhs: SdpMatrix<SimdReal>,
+    im2: SimdReal,
+    ii2_sqrt: AngularInertia<SimdReal>,
 }
 
 impl WBallVelocityGroundConstraint {
@@ -213,7 +218,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,11 +226,11 @@ 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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
@@ -237,13 +242,12 @@ 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;
 
         let cmat2 = anchor2.gcross_matrix();
-        let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
 
         #[cfg(feature = "dim3")]
         {
@@ -267,14 +271,15 @@ impl WBallVelocityGroundConstraint {
             joint_id,
             mj_lambda2,
             im2,
-            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
-            gcross2,
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            r2: anchor2,
             rhs,
             inv_lhs,
+            ii2_sqrt,
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -285,7 +290,7 @@ impl WBallVelocityGroundConstraint {
         };
 
         mj_lambda2.linear -= self.impulse * self.im2;
-        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -293,8 +298,8 @@ impl WBallVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
-        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
             ),
@@ -303,14 +308,15 @@ impl WBallVelocityGroundConstraint {
             ),
         };
 
-        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
         let dvel = vel2 + self.rhs;
 
         let impulse = self.inv_lhs * dvel;
         self.impulse += impulse;
 
         mj_lambda2.linear -= impulse * self.im2;
-        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);

src/dynamics/solver/joint_constraint/fixed_position_constraint.rs

@@ -1,30 +1,30 @@
 use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
-use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
 use crate::utils::WAngularInertia;
 
 #[derive(Debug)]
 pub(crate) struct FixedPositionConstraint {
     position1: usize,
     position2: usize,
-    local_anchor1: Isometry<f32>,
-    local_anchor2: Isometry<f32>,
-    local_com1: Point<f32>,
-    local_com2: Point<f32>,
-    im1: f32,
-    im2: f32,
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    local_anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
+    im1: Real,
+    im2: Real,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    lin_inv_lhs: f32,
-    ang_inv_lhs: AngularInertia<f32>,
+    lin_inv_lhs: Real,
+    ang_inv_lhs: AngularInertia<Real>,
 }
 
 impl FixedPositionConstraint {
     pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
         let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
@@ -44,7 +44,7 @@ impl FixedPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -81,12 +81,12 @@ impl FixedPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct FixedPositionGroundConstraint {
     position2: usize,
-    anchor1: Isometry<f32>,
-    local_anchor2: Isometry<f32>,
-    local_com2: Point<f32>,
-    im2: f32,
-    ii2: AngularInertia<f32>,
-    impulse: f32,
+    anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com2: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    impulse: Real,
 }
 
 impl FixedPositionGroundConstraint {
@@ -111,14 +111,14 @@ impl FixedPositionGroundConstraint {
             anchor1,
             local_anchor2,
             position2: rb2.active_set_offset,
-            im2: rb2.mass_properties.inv_mass,
-            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            im2: rb2.effective_inv_mass,
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
             local_com2: rb2.mass_properties.local_com,
             impulse: 0.0,
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
         // Angular correction.

src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs

@@ -0,0 +1,51 @@
+use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
+use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WFixedPositionConstraint {
+    constraints: [FixedPositionConstraint; SIMD_WIDTH],
+}
+
+impl WFixedPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&FixedJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WFixedPositionGroundConstraint {
+    constraints: [FixedPositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WFixedPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&FixedJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{AngularInertia, Dim, SpacialVector, Vector};
+use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim2")]
 use na::{Matrix3, Vector3};
@@ -16,29 +16,29 @@ pub(crate) struct FixedVelocityConstraint {
 
     joint_id: JointIndex,
 
-    impulse: SpacialVector<f32>,
+    impulse: SpacialVector<Real>,
 
     #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim3")]
-    rhs: Vector6<f32>,
+    rhs: Vector6<Real>,
 
     #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim2")]
-    rhs: Vector3<f32>,
+    rhs: Vector3<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    ii1_sqrt: AngularInertia<f32>,
-    ii2_sqrt: AngularInertia<f32>,
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
 
-    r1: Vector<f32>,
-    r2: Vector<f32>,
+    r1: Vector<Real>,
+    r2: Vector<Real>,
 }
 
 impl FixedVelocityConstraint {
@@ -51,10 +51,10 @@ impl FixedVelocityConstraint {
     ) -> Self {
         let anchor1 = rb1.position * cparams.local_anchor1;
         let anchor2 = rb2.position * cparams.local_anchor2;
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1.translation.vector - rb1.world_com.coords;
         let r2 = anchor2.translation.vector - rb2.world_com.coords;
         let rmat1 = r1.gcross_matrix();
@@ -118,8 +118,8 @@ impl FixedVelocityConstraint {
             im2,
             ii1,
             ii2,
-            ii1_sqrt: rb1.world_inv_inertia_sqrt,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             r1,
@@ -128,7 +128,7 @@ impl FixedVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -152,7 +152,7 @@ impl FixedVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -207,22 +207,22 @@ pub(crate) struct FixedVelocityGroundConstraint {
 
     joint_id: JointIndex,
 
-    impulse: SpacialVector<f32>,
+    impulse: SpacialVector<Real>,
 
     #[cfg(feature = "dim3")]
-    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim3")]
-    rhs: Vector6<f32>,
+    rhs: Vector6<Real>,
 
     #[cfg(feature = "dim2")]
-    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
     #[cfg(feature = "dim2")]
-    rhs: Vector3<f32>,
+    rhs: Vector3<Real>,
 
-    im2: f32,
-    ii2: AngularInertia<f32>,
-    ii2_sqrt: AngularInertia<f32>,
-    r2: Vector<f32>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+    r2: Vector<Real>,
 }
 
 impl FixedVelocityGroundConstraint {
@@ -248,8 +248,8 @@ impl FixedVelocityGroundConstraint {
 
         let r1 = anchor1.translation.vector - rb1.world_com.coords;
 
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r2 = anchor2.translation.vector - rb2.world_com.coords;
         let rmat2 = r2.gcross_matrix();
 
@@ -304,7 +304,7 @@ impl FixedVelocityGroundConstraint {
             mj_lambda2: rb2.active_set_offset,
             im2,
             ii2,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             r2,
@@ -312,7 +312,7 @@ impl FixedVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -329,7 +329,7 @@ impl FixedVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -5,16 +5,16 @@ use crate::dynamics::{
     FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
 use crate::math::{
-    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector,
-    SIMD_WIDTH,
+    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
+    Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix6, Vector6, U3};
 #[cfg(feature = "dim2")]
 use {
-    crate::utils::SdpMatrix3,
     na::{Matrix3, Vector3},
+    parry::utils::SdpMatrix3,
 };
 
 #[derive(Debug)]
@@ -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,21 +59,21 @@ 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(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_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,
@@ -158,7 +158,7 @@ impl WFixedVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -202,8 +202,8 @@ impl WFixedVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
-        let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        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,16 +308,16 @@ 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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
@@ -386,14 +386,14 @@ impl WFixedVelocityGroundConstraint {
             im2,
             ii2,
             ii2_sqrt,
-            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
             inv_lhs,
             r2,
             rhs,
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -420,8 +420,8 @@ impl WFixedVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
-        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
             ),

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -17,6 +17,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
 };
+use crate::math::Real;
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
 
@@ -220,7 +221,7 @@ impl AnyJointVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         match self {
             AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
@@ -254,7 +255,7 @@ impl AnyJointVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         match self {
             AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -4,12 +4,19 @@ use super::{
 };
 #[cfg(feature = "dim3")]
 use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
+#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
+
 #[cfg(feature = "simd-is-enabled")]
-use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
+use super::{
+    WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
+    WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
+    WPrismaticPositionGroundConstraint,
+};
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
-use crate::math::Isometry;
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
+use crate::math::{Isometry, Real};
 
 pub(crate) enum AnyJointPositionConstraint {
     BallJoint(BallPositionConstraint),
@@ -20,35 +27,29 @@ pub(crate) enum AnyJointPositionConstraint {
     WBallGroundConstraint(WBallPositionGroundConstraint),
     FixedJoint(FixedPositionConstraint),
     FixedGroundConstraint(FixedPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WFixedJoint(WFixedPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WFixedGroundConstraint(WFixedPositionGroundConstraint),
     PrismaticJoint(PrismaticPositionConstraint),
     PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WPrismaticJoint(WPrismaticPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WPrismaticGroundConstraint(WPrismaticPositionGroundConstraint),
     #[cfg(feature = "dim3")]
     RevoluteJoint(RevolutePositionConstraint),
     #[cfg(feature = "dim3")]
     RevoluteGroundConstraint(RevolutePositionGroundConstraint),
+    #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+    WRevoluteJoint(WRevolutePositionConstraint),
+    #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+    WRevoluteGroundConstraint(WRevolutePositionGroundConstraint),
     #[allow(dead_code)] // The Empty variant is only used with parallel code.
     Empty,
 }
 
 impl AnyJointPositionConstraint {
-    #[cfg(feature = "parallel")]
-    pub fn num_active_constraints(joint: &Joint, grouped: bool) -> usize {
-        #[cfg(feature = "simd-is-enabled")]
-        if !grouped {
-            1
-        } else {
-            match &joint.params {
-                JointParams::BallJoint(_) => 1,
-                _ => SIMD_WIDTH, // For joints that don't support SIMD position constraints yet.
-            }
-        }
-
-        #[cfg(not(feature = "simd-is-enabled"))]
-        {
-            1
-        }
-    }
-
     pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
         let rb1 = &bodies[joint.body1];
         let rb2 = &bodies[joint.body2];
@@ -71,21 +72,38 @@ impl AnyJointPositionConstraint {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option<Self> {
+    pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
         let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
         let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
 
         match &joints[0].params {
             JointParams::BallJoint(_) => {
                 let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
-                Some(AnyJointPositionConstraint::WBallJoint(
-                    WBallPositionConstraint::from_params(rbs1, rbs2, joints),
+                AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
+                    rbs1, rbs2, joints,
                 ))
             }
-            JointParams::FixedJoint(_) => None,
-            JointParams::PrismaticJoint(_) => None,
+            JointParams::FixedJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
+                    rbs1, rbs2, joints,
+                ))
+            }
+            JointParams::PrismaticJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WPrismaticJoint(
+                    WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
+                )
+            }
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => None,
+            JointParams::RevoluteJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WRevoluteJoint(
+                    WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
+                )
+            }
         }
     }
 
@@ -118,10 +136,7 @@ impl AnyJointPositionConstraint {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn from_wide_joint_ground(
-        joints: [&Joint; SIMD_WIDTH],
-        bodies: &RigidBodySet,
-    ) -> Option<Self> {
+    pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
         let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
         let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
         let mut flipped = [false; SIMD_WIDTH];
@@ -136,18 +151,35 @@ impl AnyJointPositionConstraint {
         match &joints[0].params {
             JointParams::BallJoint(_) => {
                 let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
-                Some(AnyJointPositionConstraint::WBallGroundConstraint(
+                AnyJointPositionConstraint::WBallGroundConstraint(
                     WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
-                ))
+                )
+            }
+            JointParams::FixedJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WFixedGroundConstraint(
+                    WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                )
+            }
+            JointParams::PrismaticJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WPrismaticGroundConstraint(
+                    WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                )
             }
-            JointParams::FixedJoint(_) => None,
-            JointParams::PrismaticJoint(_) => None,
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => None,
+            JointParams::RevoluteJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WRevoluteGroundConstraint(
+                    WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                )
+            }
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         match self {
             AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),
@@ -157,12 +189,24 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WPrismaticJoint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WPrismaticGroundConstraint(c) => c.solve(params, positions),
             #[cfg(feature = "dim3")]
             AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions),
             #[cfg(feature = "dim3")]
             AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+            AnyJointPositionConstraint::WRevoluteJoint(c) => c.solve(params, positions),
+            #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+            AnyJointPositionConstraint::WRevoluteGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::Empty => unreachable!(),
         }
     }

src/dynamics/solver/joint_constraint/mod.rs

@@ -9,6 +9,10 @@ pub(self) use ball_velocity_constraint_wide::{
     WBallVelocityConstraint, WBallVelocityGroundConstraint,
 };
 pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use fixed_position_constraint_wide::{
+    WFixedPositionConstraint, WFixedPositionGroundConstraint,
+};
 pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 pub(self) use fixed_velocity_constraint_wide::{
@@ -19,6 +23,10 @@ pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
 pub(self) use prismatic_position_constraint::{
     PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
 };
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use prismatic_position_constraint_wide::{
+    WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
+};
 pub(self) use prismatic_velocity_constraint::{
     PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
 };
@@ -30,12 +38,15 @@ pub(self) use prismatic_velocity_constraint_wide::{
 pub(self) use revolute_position_constraint::{
     RevolutePositionConstraint, RevolutePositionGroundConstraint,
 };
+#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+pub(self) use revolute_position_constraint_wide::{
+    WRevolutePositionConstraint, WRevolutePositionGroundConstraint,
+};
 #[cfg(feature = "dim3")]
 pub(self) use revolute_velocity_constraint::{
     RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
 };
-#[cfg(feature = "dim3")]
-#[cfg(feature = "simd-is-enabled")]
+#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
 pub(self) use revolute_velocity_constraint_wide::{
     WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
 };
@@ -47,19 +58,24 @@ mod ball_velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod ball_velocity_constraint_wide;
 mod fixed_position_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod fixed_position_constraint_wide;
 mod fixed_velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod fixed_velocity_constraint_wide;
 mod joint_constraint;
 mod joint_position_constraint;
 mod prismatic_position_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod prismatic_position_constraint_wide;
 mod prismatic_velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod prismatic_velocity_constraint_wide;
 #[cfg(feature = "dim3")]
 mod revolute_position_constraint;
+#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
+mod revolute_position_constraint_wide;
 #[cfg(feature = "dim3")]
 mod revolute_velocity_constraint;
-#[cfg(feature = "dim3")]
-#[cfg(feature = "simd-is-enabled")]
+#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
 mod revolute_velocity_constraint_wide;

src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs

@@ -1,5 +1,5 @@
 use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
-use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
 use crate::utils::WAngularInertia;
 use na::Unit;
 
@@ -8,30 +8,30 @@ pub(crate) struct PrismaticPositionConstraint {
     position1: usize,
     position2: usize,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    lin_inv_lhs: f32,
-    ang_inv_lhs: AngularInertia<f32>,
+    lin_inv_lhs: Real,
+    ang_inv_lhs: AngularInertia<Real>,
 
-    limits: [f32; 2],
+    limits: [Real; 2],
 
-    local_frame1: Isometry<f32>,
-    local_frame2: Isometry<f32>,
+    local_frame1: Isometry<Real>,
+    local_frame2: Isometry<Real>,
 
-    local_axis1: Unit<Vector<f32>>,
-    local_axis2: Unit<Vector<f32>>,
+    local_axis1: Unit<Vector<Real>>,
+    local_axis2: Unit<Vector<Real>>,
 }
 
 impl PrismaticPositionConstraint {
     pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
         let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
@@ -52,7 +52,7 @@ impl PrismaticPositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -99,11 +99,11 @@ impl PrismaticPositionConstraint {
 #[derive(Debug)]
 pub(crate) struct PrismaticPositionGroundConstraint {
     position2: usize,
-    frame1: Isometry<f32>,
-    local_frame2: Isometry<f32>,
-    axis1: Unit<Vector<f32>>,
-    local_axis2: Unit<Vector<f32>>,
-    limits: [f32; 2],
+    frame1: Isometry<Real>,
+    local_frame2: Isometry<Real>,
+    axis1: Unit<Vector<Real>>,
+    local_axis2: Unit<Vector<Real>>,
+    limits: [Real; 2],
 }
 
 impl PrismaticPositionGroundConstraint {
@@ -140,7 +140,7 @@ impl PrismaticPositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
         // Angular correction.

src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs

@@ -0,0 +1,51 @@
+use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WPrismaticPositionConstraint {
+    constraints: [PrismaticPositionConstraint; SIMD_WIDTH],
+}
+
+impl WPrismaticPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&PrismaticJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WPrismaticPositionGroundConstraint {
+    constraints: [PrismaticPositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WPrismaticPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&PrismaticJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -2,14 +2,14 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
 };
-use crate::math::{AngularInertia, Vector};
+use crate::math::{AngularInertia, Real, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
 use {
-    crate::utils::SdpMatrix2,
     na::{Matrix2, Vector2},
+    parry::utils::SdpMatrix2,
 };
 
 #[cfg(feature = "dim2")]
@@ -24,37 +24,37 @@ pub(crate) struct PrismaticVelocityConstraint {
 
     joint_id: JointIndex,
 
-    r1: Vector<f32>,
-    r2: Vector<f32>,
+    r1: Vector<Real>,
+    r2: Vector<Real>,
 
     #[cfg(feature = "dim3")]
-    inv_lhs: Matrix5<f32>,
+    inv_lhs: Matrix5<Real>,
     #[cfg(feature = "dim3")]
-    rhs: Vector5<f32>,
+    rhs: Vector5<Real>,
     #[cfg(feature = "dim3")]
-    impulse: Vector5<f32>,
+    impulse: Vector5<Real>,
 
     #[cfg(feature = "dim2")]
-    inv_lhs: Matrix2<f32>,
+    inv_lhs: Matrix2<Real>,
     #[cfg(feature = "dim2")]
-    rhs: Vector2<f32>,
+    rhs: Vector2<Real>,
     #[cfg(feature = "dim2")]
-    impulse: Vector2<f32>,
+    impulse: Vector2<Real>,
 
-    limits_impulse: f32,
-    limits_forcedirs: Option<(Vector<f32>, Vector<f32>)>,
-    limits_rhs: f32,
+    limits_impulse: Real,
+    limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
+    limits_rhs: Real,
 
     #[cfg(feature = "dim2")]
-    basis1: Vector2<f32>,
+    basis1: Vector2<Real>,
     #[cfg(feature = "dim3")]
-    basis1: Matrix3x2<f32>,
+    basis1: Matrix3x2<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1_sqrt: AngularInertia<f32>,
-    ii2_sqrt: AngularInertia<f32>,
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl PrismaticVelocityConstraint {
@@ -92,13 +92,13 @@ impl PrismaticVelocityConstraint {
         // simplifications of the computation without introducing
         // much instabilities.
 
-        let im1 = rb1.mass_properties.inv_mass;
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
         let r1_mat = r1.gcross_matrix();
 
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r2 = anchor2 - rb2.world_com;
         let r2_mat = r2.gcross_matrix();
 
@@ -176,9 +176,9 @@ impl PrismaticVelocityConstraint {
             mj_lambda1: rb1.active_set_offset,
             mj_lambda2: rb2.active_set_offset,
             im1,
-            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             im2,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             limits_impulse: limits_impulse * params.warmstart_coeff,
             limits_forcedirs,
@@ -191,7 +191,7 @@ impl PrismaticVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -220,7 +220,7 @@ impl PrismaticVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -295,34 +295,34 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
 
     joint_id: JointIndex,
 
-    r2: Vector<f32>,
+    r2: Vector<Real>,
 
     #[cfg(feature = "dim2")]
-    inv_lhs: Matrix2<f32>,
+    inv_lhs: Matrix2<Real>,
     #[cfg(feature = "dim2")]
-    rhs: Vector2<f32>,
+    rhs: Vector2<Real>,
     #[cfg(feature = "dim2")]
-    impulse: Vector2<f32>,
+    impulse: Vector2<Real>,
 
     #[cfg(feature = "dim3")]
-    inv_lhs: Matrix5<f32>,
+    inv_lhs: Matrix5<Real>,
     #[cfg(feature = "dim3")]
-    rhs: Vector5<f32>,
+    rhs: Vector5<Real>,
     #[cfg(feature = "dim3")]
-    impulse: Vector5<f32>,
+    impulse: Vector5<Real>,
 
-    limits_impulse: f32,
-    limits_rhs: f32,
+    limits_impulse: Real,
+    limits_rhs: Real,
 
-    axis2: Vector<f32>,
+    axis2: Vector<Real>,
     #[cfg(feature = "dim2")]
-    basis1: Vector2<f32>,
+    basis1: Vector2<Real>,
     #[cfg(feature = "dim3")]
-    basis1: Matrix3x2<f32>,
-    limits_forcedir2: Option<Vector<f32>>,
+    basis1: Matrix3x2<Real>,
+    limits_forcedir2: Option<Vector<Real>>,
 
-    im2: f32,
-    ii2_sqrt: AngularInertia<f32>,
+    im2: Real,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl PrismaticVelocityGroundConstraint {
@@ -388,8 +388,8 @@ impl PrismaticVelocityGroundConstraint {
         // simplifications of the computation without introducing
         // much instabilities.
 
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
         let r2 = anchor2 - rb2.world_com;
         let r2_mat = r2.gcross_matrix();
@@ -465,7 +465,7 @@ impl PrismaticVelocityGroundConstraint {
             joint_id,
             mj_lambda2: rb2.active_set_offset,
             im2,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             limits_impulse: limits_impulse * params.warmstart_coeff,
             basis1,
@@ -478,7 +478,7 @@ impl PrismaticVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
@@ -499,7 +499,7 @@ impl PrismaticVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         /*

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -5,15 +5,15 @@ 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, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
 };
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
 use {
-    crate::utils::SdpMatrix2,
     na::{Matrix2, Vector2},
+    parry::utils::SdpMatrix2,
 };
 
 #[cfg(feature = "dim2")]
@@ -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,21 +71,21 @@ 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(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_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,
@@ -236,7 +236,7 @@ impl WPrismaticVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -285,7 +285,7 @@ impl WPrismaticVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -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,16 +424,16 @@ 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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_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,
@@ -585,7 +585,7 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -616,7 +616,7 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -1,5 +1,5 @@
 use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
-use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
 use crate::utils::WAngularInertia;
 use na::Unit;
 
@@ -8,28 +8,28 @@ pub(crate) struct RevolutePositionConstraint {
     position1: usize,
     position2: usize,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1: AngularInertia<f32>,
-    ii2: AngularInertia<f32>,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
 
-    lin_inv_lhs: f32,
-    ang_inv_lhs: AngularInertia<f32>,
+    lin_inv_lhs: Real,
+    ang_inv_lhs: AngularInertia<Real>,
 
-    local_anchor1: Point<f32>,
-    local_anchor2: Point<f32>,
+    local_anchor1: Point<Real>,
+    local_anchor2: Point<Real>,
 
-    local_axis1: Unit<Vector<f32>>,
-    local_axis2: Unit<Vector<f32>>,
+    local_axis1: Unit<Vector<Real>>,
+    local_axis2: Unit<Vector<Real>>,
 }
 
 impl RevolutePositionConstraint {
     pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
         let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
@@ -49,7 +49,7 @@ impl RevolutePositionConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
@@ -83,10 +83,10 @@ impl RevolutePositionConstraint {
 #[derive(Debug)]
 pub(crate) struct RevolutePositionGroundConstraint {
     position2: usize,
-    anchor1: Point<f32>,
-    local_anchor2: Point<f32>,
-    axis1: Unit<Vector<f32>>,
-    local_axis2: Unit<Vector<f32>>,
+    anchor1: Point<Real>,
+    local_anchor2: Point<Real>,
+    axis1: Unit<Vector<Real>>,
+    local_axis2: Unit<Vector<Real>>,
 }
 
 impl RevolutePositionGroundConstraint {
@@ -122,7 +122,7 @@ impl RevolutePositionGroundConstraint {
         }
     }
 
-    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
         let axis2 = position2 * self.local_axis2;

src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs

@@ -0,0 +1,51 @@
+use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WRevolutePositionConstraint {
+    constraints: [RevolutePositionConstraint; SIMD_WIDTH],
+}
+
+impl WRevolutePositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&RevoluteJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WRevolutePositionGroundConstraint {
+    constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WRevolutePositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
-use crate::math::{AngularInertia, Vector};
+use crate::math::{AngularInertia, Real, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 
@@ -13,20 +13,20 @@ pub(crate) struct RevoluteVelocityConstraint {
 
     joint_id: JointIndex,
 
-    r1: Vector<f32>,
-    r2: Vector<f32>,
+    r1: Vector<Real>,
+    r2: Vector<Real>,
 
-    inv_lhs: Matrix5<f32>,
-    rhs: Vector5<f32>,
-    impulse: Vector5<f32>,
+    inv_lhs: Matrix5<Real>,
+    rhs: Vector5<Real>,
+    impulse: Vector5<Real>,
 
-    basis1: Matrix3x2<f32>,
+    basis1: Matrix3x2<Real>,
 
-    im1: f32,
-    im2: f32,
+    im1: Real,
+    im2: Real,
 
-    ii1_sqrt: AngularInertia<f32>,
-    ii2_sqrt: AngularInertia<f32>,
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl RevoluteVelocityConstraint {
@@ -52,14 +52,14 @@ impl RevoluteVelocityConstraint {
         //        let basis2 = r21 * basis1;
         // NOTE: to simplify, we use basis2 = basis1.
         // Though we may want to test if that does not introduce any instability.
-        let im1 = rb1.mass_properties.inv_mass;
-        let im2 = rb2.mass_properties.inv_mass;
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
 
-        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
         let r1_mat = r1.gcross_matrix();
 
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r2 = anchor2 - rb2.world_com;
         let r2_mat = r2.gcross_matrix();
 
@@ -87,10 +87,10 @@ impl RevoluteVelocityConstraint {
             mj_lambda1: rb1.active_set_offset,
             mj_lambda2: rb2.active_set_offset,
             im1,
-            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             basis1,
             im2,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             inv_lhs,
             rhs,
@@ -99,7 +99,7 @@ impl RevoluteVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -120,7 +120,7 @@ impl RevoluteVelocityConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
@@ -165,17 +165,17 @@ pub(crate) struct RevoluteVelocityGroundConstraint {
 
     joint_id: JointIndex,
 
-    r2: Vector<f32>,
+    r2: Vector<Real>,
 
-    inv_lhs: Matrix5<f32>,
-    rhs: Vector5<f32>,
-    impulse: Vector5<f32>,
+    inv_lhs: Matrix5<Real>,
+    rhs: Vector5<Real>,
+    impulse: Vector5<Real>,
 
-    basis1: Matrix3x2<f32>,
+    basis1: Matrix3x2<Real>,
 
-    im2: f32,
+    im2: Real,
 
-    ii2_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<Real>,
 }
 
 impl RevoluteVelocityGroundConstraint {
@@ -212,8 +212,8 @@ impl RevoluteVelocityGroundConstraint {
         //            .to_rotation_matrix()
         //            .into_inner();
         //        let basis2 = /*r21 * */ basis1;
-        let im2 = rb2.mass_properties.inv_mass;
-        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
         let r2 = anchor2 - rb2.world_com;
         let r2_mat = r2.gcross_matrix();
@@ -240,7 +240,7 @@ impl RevoluteVelocityGroundConstraint {
             joint_id,
             mj_lambda2: rb2.active_set_offset,
             im2,
-            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse: cparams.impulse * params.warmstart_coeff,
             basis1,
             inv_lhs,
@@ -249,7 +249,7 @@ impl RevoluteVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
@@ -263,7 +263,7 @@ impl RevoluteVelocityGroundConstraint {
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -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, Real, 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,21 +41,21 @@ 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(
-            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_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,
@@ -123,7 +123,7 @@ impl WRevoluteVelocityConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -164,7 +164,7 @@ impl WRevoluteVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda1 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
@@ -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,16 +255,16 @@ 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(
-            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
         let impulse = Vector5::from(array![|ii| cparams[ii].impulse; 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,
@@ -330,7 +330,7 @@ impl WRevoluteVelocityGroundConstraint {
         }
     }
 
-    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
@@ -354,7 +354,7 @@ impl WRevoluteVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = DeltaVel {
             linear: Vector::from(
                 array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],

src/dynamics/solver/mod.rs

@@ -5,10 +5,14 @@ pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContex
 #[cfg(feature = "parallel")]
 pub(self) use self::parallel_position_solver::ParallelPositionSolver;
 #[cfg(feature = "parallel")]
+pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints;
+#[cfg(feature = "parallel")]
 pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
 #[cfg(not(feature = "parallel"))]
 pub(self) use self::position_solver::PositionSolver;
 #[cfg(not(feature = "parallel"))]
+pub(self) use self::solver_constraints::SolverConstraints;
+#[cfg(not(feature = "parallel"))]
 pub(self) use self::velocity_solver::VelocitySolver;
 pub(self) use delta_vel::DeltaVel;
 pub(self) use interaction_groups::*;
@@ -37,6 +41,8 @@ mod parallel_island_solver;
 #[cfg(feature = "parallel")]
 mod parallel_position_solver;
 #[cfg(feature = "parallel")]
+mod parallel_solver_constraints;
+#[cfg(feature = "parallel")]
 mod parallel_velocity_solver;
 mod position_constraint;
 #[cfg(feature = "simd-is-enabled")]
@@ -46,6 +52,8 @@ mod position_ground_constraint;
 mod position_ground_constraint_wide;
 #[cfg(not(feature = "parallel"))]
 mod position_solver;
+#[cfg(not(feature = "parallel"))]
+mod solver_constraints;
 mod velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod velocity_constraint_wide;

src/dynamics/solver/parallel_island_solver.rs

@@ -1,8 +1,11 @@
 use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
-use crate::dynamics::solver::ParallelPositionSolver;
+use crate::dynamics::solver::{
+    AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
+    AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
+};
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
-use crate::math::Isometry;
+use crate::math::{Isometry, Real};
 use crate::utils::WAngularInertia;
 use rayon::Scope;
 use std::sync::atomic::{AtomicUsize, Ordering};
@@ -119,12 +122,14 @@ impl ThreadContext {
 }
 
 pub struct ParallelIslandSolver {
-    mj_lambdas: Vec<DeltaVel<f32>>,
-    positions: Vec<Isometry<f32>>,
+    mj_lambdas: Vec<DeltaVel<Real>>,
+    positions: Vec<Isometry<Real>>,
     parallel_groups: ParallelInteractionGroups,
     parallel_joint_groups: ParallelInteractionGroups,
-    parallel_velocity_solver: ParallelVelocitySolver,
-    parallel_position_solver: ParallelPositionSolver,
+    parallel_contact_constraints:
+        ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
+    parallel_joint_constraints:
+        ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
     thread: ThreadContext,
 }
 
@@ -135,8 +140,8 @@ impl ParallelIslandSolver {
             positions: Vec::new(),
             parallel_groups: ParallelInteractionGroups::new(),
             parallel_joint_groups: ParallelInteractionGroups::new(),
-            parallel_velocity_solver: ParallelVelocitySolver::new(),
-            parallel_position_solver: ParallelPositionSolver::new(),
+            parallel_contact_constraints: ParallelSolverConstraints::new(),
+            parallel_joint_constraints: ParallelSolverConstraints::new(),
             thread: ThreadContext::new(8),
         }
     }
@@ -153,25 +158,21 @@ impl ParallelIslandSolver {
         joint_indices: &[JointIndex],
     ) {
         let num_threads = rayon::current_num_threads();
-        let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
+        let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
         self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
         self.parallel_groups
             .group_interactions(island_id, bodies, manifolds, manifold_indices);
         self.parallel_joint_groups
             .group_interactions(island_id, bodies, joints, joint_indices);
-        self.parallel_velocity_solver.init_constraint_groups(
+        self.parallel_contact_constraints.init_constraint_groups(
             island_id,
             bodies,
             manifolds,
             &self.parallel_groups,
-            joints,
-            &self.parallel_joint_groups,
         );
-        self.parallel_position_solver.init_constraint_groups(
+        self.parallel_joint_constraints.init_constraint_groups(
             island_id,
             bodies,
-            manifolds,
-            &self.parallel_groups,
             joints,
             &self.parallel_joint_groups,
         );
@@ -192,16 +193,16 @@ impl ParallelIslandSolver {
             let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
             let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _);
             let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _);
-            let parallel_velocity_solver =
-                std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _);
-            let parallel_position_solver =
-                std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _);
+            let parallel_contact_constraints =
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _);
+            let parallel_joint_constraints =
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_joint_constraints as *mut _);
 
             scope.spawn(move |_| {
                 // Transmute *mut -> &mut
-                let mj_lambdas: &mut Vec<DeltaVel<f32>> =
+                let mj_lambdas: &mut Vec<DeltaVel<Real>> =
                     unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
-                let positions: &mut Vec<Isometry<f32>> =
+                let positions: &mut Vec<Isometry<Real>> =
                     unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
                 let bodies: &mut RigidBodySet =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
@@ -209,18 +210,34 @@ impl ParallelIslandSolver {
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
                 let joints: &mut Vec<JointGraphEdge> =
                     unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) };
-                let parallel_velocity_solver: &mut ParallelVelocitySolver = unsafe {
-                    std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed))
+                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
+                    std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
                 };
-                let parallel_position_solver: &mut ParallelPositionSolver = unsafe {
-                    std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed))
+                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> = unsafe {
+                    std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
                 };
 
                 enable_flush_to_zero!(); // Ensure this is enabled on each thread.
-                parallel_velocity_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
-                parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
-                parallel_velocity_solver
-                    .solve_constraints(&thread, params, manifolds, joints, mj_lambdas);
+                parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
+                parallel_joint_constraints.fill_constraints(&thread, params, bodies, joints);
+                ThreadContext::lock_until_ge(
+                    &thread.num_initialized_constraints,
+                    parallel_contact_constraints.constraint_descs.len(),
+                );
+                ThreadContext::lock_until_ge(
+                    &thread.num_initialized_joint_constraints,
+                    parallel_joint_constraints.constraint_descs.len(),
+                );
+
+                ParallelVelocitySolver::solve(
+                        &thread,
+                        params,
+                        manifolds,
+                        joints,
+                        mj_lambdas,
+                        parallel_contact_constraints,
+                        parallel_joint_constraints
+                );
 
                 // Write results back to rigid bodies and integrate velocities.
                 let island_range = bodies.active_island_range(island_id);
@@ -230,10 +247,10 @@ impl ParallelIslandSolver {
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
                     for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let rb = &mut bodies[*handle];
+                        let rb = &mut bodies[handle.0];
                         let dvel = mj_lambdas[rb.active_set_offset];
                         rb.linvel += dvel.linear;
-                        rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
+                        rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);
                         rb.integrate(params.dt);
                         positions[rb.active_set_offset] = rb.position;
                     }
@@ -243,13 +260,19 @@ impl ParallelIslandSolver {
                 // initialized `positions` to the updated values.
                 ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len());
 
-                parallel_position_solver.solve_constraints(&thread, params, positions);
+                ParallelPositionSolver::solve(
+                    &thread,
+                    params,
+                    positions,
+                    parallel_contact_constraints,
+                    parallel_joint_constraints
+                );
 
                 // Write results back to rigid bodies.
                 concurrent_loop! {
                     let batch_size = thread.batch_size;
                     for handle in active_bodies[thread.position_writeback_index] {
-                        let rb = &mut bodies[*handle];
+                        let rb = &mut bodies[handle.0];
                         rb.set_position_internal(positions[rb.active_set_offset]);
                     }
                 }