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

Merge pull request #282 from dimforge/critical-damping

Browse Source 
Improve the CFM implementation
This commit is contained in:
Sébastien Crozet
2022-01-23 08:56:27 -08:00
committed by GitHub
parent ca635674fc b3b675d2de
commit 1608a1323e
43 changed files with 929 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@@ -6,4 +6,4 @@ target
.DS_Store
package-lock.json
**/*.csv
.vscode/
.history

326
.vscode/launch.json vendored Normal file
View File

@@ -0,0 +1,326 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in library 'rapier2d'",
"cargo": {
"args": [
"test",
"--no-run",
"--lib",
"--package=rapier2d"
],
"filter": {
"name": "rapier2d",
"kind": "lib"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in library 'rapier2d_f64'",
"cargo": {
"args": [
"test",
"--no-run",
"--lib",
"--package=rapier2d-f64"
],
"filter": {
"name": "rapier2d_f64",
"kind": "lib"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in library 'rapier_testbed2d'",
"cargo": {
"args": [
"test",
"--no-run",
"--lib",
"--package=rapier_testbed2d"
],
"filter": {
"name": "rapier_testbed2d",
"kind": "lib"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug executable 'all_examples2'",
"cargo": {
"args": [
"build",
"--bin=all_examples2",
"--package=rapier-examples-2d"
],
"filter": {
"name": "all_examples2",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in executable 'all_examples2'",
"cargo": {
"args": [
"test",
"--no-run",
"--bin=all_examples2",
"--package=rapier-examples-2d"
],
"filter": {
"name": "all_examples2",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug executable 'all_benchmarks2'",
"cargo": {
"args": [
"build",
"--bin=all_benchmarks2",
"--package=rapier-benchmarks-2d"
],
"filter": {
"name": "all_benchmarks2",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in executable 'all_benchmarks2'",
"cargo": {
"args": [
"test",
"--no-run",
"--bin=all_benchmarks2",
"--package=rapier-benchmarks-2d"
],
"filter": {
"name": "all_benchmarks2",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in library 'rapier3d'",
"cargo": {
"args": [
"test",
"--no-run",
"--lib",
"--package=rapier3d"
],
"filter": {
"name": "rapier3d",
"kind": "lib"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in library 'rapier3d_f64'",
"cargo": {
"args": [
"test",
"--no-run",
"--lib",
"--package=rapier3d-f64"
],
"filter": {
"name": "rapier3d_f64",
"kind": "lib"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in library 'rapier_testbed3d'",
"cargo": {
"args": [
"test",
"--no-run",
"--lib",
"--package=rapier_testbed3d"
],
"filter": {
"name": "rapier_testbed3d",
"kind": "lib"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug executable 'all_examples3'",
"cargo": {
"args": [
"build",
"--bin=all_examples3",
"--package=rapier-examples-3d"
],
"filter": {
"name": "all_examples3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in executable 'all_examples3'",
"cargo": {
"args": [
"test",
"--no-run",
"--bin=all_examples3",
"--package=rapier-examples-3d"
],
"filter": {
"name": "all_examples3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Run 'all_examples3'",
"cargo": {
"args": [
"run",
"--release",
"--bin=all_examples3",
"--package=rapier-examples-3d"
],
"filter": {
"name": "all_examples3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug executable 'harness_capsules3'",
"cargo": {
"args": [
"build",
"--bin=harness_capsules3",
"--package=rapier-examples-3d"
],
"filter": {
"name": "harness_capsules3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in executable 'harness_capsules3'",
"cargo": {
"args": [
"test",
"--no-run",
"--bin=harness_capsules3",
"--package=rapier-examples-3d"
],
"filter": {
"name": "harness_capsules3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug executable 'all_benchmarks3'",
"cargo": {
"args": [
"build",
"--bin=all_benchmarks3",
"--package=rapier-benchmarks-3d"
],
"filter": {
"name": "all_benchmarks3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
},
{
"type": "lldb",
"request": "launch",
"name": "Debug unit tests in executable 'all_benchmarks3'",
"cargo": {
"args": [
"test",
"--no-run",
"--bin=all_benchmarks3",
"--package=rapier-benchmarks-3d"
],
"filter": {
"name": "all_benchmarks3",
"kind": "bin"
}
},
"args": [],
"cwd": "${workspaceFolder}"
}
]
}

103
.vscode/tasks.json vendored Normal file
View File

@@ -0,0 +1,103 @@
{
// See https://go.microsoft.com/fwlink/?LinkId=733558
// for the documentation about the tasks.json format
"version": "2.0.0",
"tasks": [
{
"label": "run 3d (no-simd - release) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples3",
"--release",
"--features",
"other-backends",
"--",
"--pause"
],
"group": "build"
},
{
"label": "run 3d (simd - release) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples3",
"--release",
"--features",
"simd-stable,other-backends",
"--",
"--pause"
],
"group": "build"
},
{
"label": "run 3d (simd - parallel - release) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples3",
"--release",
"--features",
"simd-stable,other-backends,parallel",
"--",
"--pause"
],
"group": "build"
},
{
"label": "run 2d (no-simd - release) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples2",
"--release",
"--features",
"other-backends",
"--",
"--pause"
],
"group": "build"
},
{
"label": "run 2d (simd - release) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples2",
"--release",
"--features",
"simd-stable,other-backends",
"--",
"--pause"
],
"group": "build"
},
{
"label": "run 2d (simd - parallel - release) ",
"type": "shell",
"command": "cargo",
"args": [
"run",
"--bin",
"all_examples2",
"--release",
"--features",
"simd-stable,other-backends,parallel",
"--",
"--pause"
],
"group": "build"
}
]
}

4
Cargo.toml
View File

@@ -1,6 +1,6 @@
[workspace]
members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "examples2d", "benchmarks2d",
"crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "examples3d", "benchmarks3d" ]
members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
"crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d" ]
resolver = "2"
[patch.crates-io]

0
examples3d/media/models/camel_decimated.obj → assets/3d/camel_decimated.obj
View File

0
examples3d/media/models/chair.obj → assets/3d/chair.obj
View File

0
examples3d/media/models/cup_decimated.obj → assets/3d/cup_decimated.obj
View File

0
examples3d/media/models/dilo_decimated.obj → assets/3d/dilo_decimated.obj
View File

0
examples3d/media/models/feline_decimated.obj → assets/3d/feline_decimated.obj
View File

0
examples3d/media/models/genus3_decimated.obj → assets/3d/genus3_decimated.obj
View File

0
examples3d/media/models/hand2_decimated.obj → assets/3d/hand2_decimated.obj
View File

0
examples3d/media/models/hand_decimated.obj → assets/3d/hand_decimated.obj
View File

0
examples3d/media/models/hornbug.obj → assets/3d/hornbug.obj
View File

0
examples3d/media/models/octopus_decimated.obj → assets/3d/octopus_decimated.obj
View File

0
examples3d/media/models/rabbit_decimated.obj → assets/3d/rabbit_decimated.obj
View File

0
examples3d/media/models/rust_logo_simplified.obj → assets/3d/rust_logo_simplified.obj
View File

0
examples3d/media/models/screwdriver_decimated.obj → assets/3d/screwdriver_decimated.obj
View File

0
examples3d/media/models/table.obj → assets/3d/table.obj
View File

0
examples3d/media/models/tstTorusModel.obj → assets/3d/tstTorusModel.obj
View File

0
examples3d/media/models/tstTorusModel2.obj → assets/3d/tstTorusModel2.obj
View File

0
examples3d/media/models/tstTorusModel3.obj → assets/3d/tstTorusModel3.obj
View File

57
crates/rapier_testbed2d-f64/Cargo.toml Normal file
View File

@@ -0,0 +1,57 @@
[package]
name = "rapier_testbed2d-f64"
version = "0.12.0-alpha.1"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
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", "impulse_joints" ]
license = "Apache-2.0"
edition = "2021"
[badges]
maintenance = { status = "actively-developed" }
[lib]
name = "rapier_testbed2d"
path = "../../src_testbed/lib.rs"
required-features = [ "dim2" ]
[features]
default = [ "dim2" ]
dim2 = [ ]
parallel = [ "rapier/parallel", "num_cpus" ]
other-backends = [ "wrapped2d" ]
[dependencies]
nalgebra = { version = "0.30", features = [ "rand" ] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
crossbeam = "0.8"
bincode = "1"
Inflector = "0.11"
md5 = "0.7"
bevy_egui = "0.10"
bevy_ecs = "0.6"
# Dependencies for native only.
[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render", "x11"]}
# Dependencies for WASM only.
[target.'cfg(target_arch = "wasm32")'.dependencies]
bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
#bevy_webgl2 = "0.5"
[dependencies.rapier]
package = "rapier2d-f64"
path = "../rapier2d-f64"
version = "0.12.0-alpha.1"
features = [ "serde-serialize" ]

55
crates/rapier_testbed3d-f64/Cargo.toml Normal file
View File

@@ -0,0 +1,55 @@
[package]
name = "rapier_testbed3d-f64"
version = "0.12.0-alpha.1"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
description = "Testbed for the Rapier 3-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", "impulse_joints" ]
license = "Apache-2.0"
edition = "2021"
[badges]
maintenance = { status = "actively-developed" }
[lib]
name = "rapier_testbed3d"
path = "../../src_testbed/lib.rs"
required-features = [ "dim3" ]
[features]
default = [ "dim3" ]
dim3 = [ ]
parallel = [ "rapier/parallel", "num_cpus" ]
[dependencies]
nalgebra = { version = "0.30", features = [ "rand" ] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
crossbeam = "0.8"
bincode = "1"
md5 = "0.7"
Inflector = "0.11"
serde = { version = "1", features = [ "derive" ] }
bevy_egui = "0.10"
bevy_ecs = "0.6"
# Dependencies for native only.
[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render", "x11"]}
# Dependencies for WASM only.
[target.'cfg(target_arch = "wasm32")'.dependencies]
bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
#bevy_webgl2 = "0.5"
[dependencies.rapier]
package = "rapier3d-f64"
path = "../rapier3d-f64"
version = "0.12.0-alpha.1"
features = [ "serde-serialize" ]

36
examples3d-f64/Cargo.toml Normal file
View File

@@ -0,0 +1,36 @@
[package]
name = "rapier-examples-3d-f64"
version = "0.1.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
edition = "2021"
default-run = "all_examples3-f64"
[features]
parallel = [ "rapier3d-f64/parallel", "rapier_testbed3d-f64/parallel" ]
simd-stable = [ "rapier3d-f64/simd-stable" ]
simd-nightly = [ "rapier3d-f64/simd-nightly" ]
enhanced-determinism = [ "rapier3d-f64/enhanced-determinism" ]
[dependencies]
rand = "0.8"
getrandom = { version = "0.2", features = [ "js" ] }
Inflector = "0.11"
wasm-bindgen = "0.2"
obj-rs = { version = "0.6", default-features = false }
bincode = "1"
serde = "1"
[dependencies.rapier_testbed3d-f64]
path = "../crates/rapier_testbed3d-f64"
[dependencies.rapier3d-f64]
path = "../crates/rapier3d-f64"
[[bin]]
name = "all_examples3-f64"
path = "./all_examples3-f64.rs"
#[lib]
#crate-type = ["cdylib", "rlib"]
#path = "./all_examples3_wasm.rs"

67
examples3d-f64/all_examples3-f64.rs Normal file
View File

@@ -0,0 +1,67 @@
#![allow(dead_code)]
#[cfg(target_arch = "wasm32")]
use wasm_bindgen::prelude::*;
extern crate rapier3d_f64 as rapier3d;
use inflector::Inflector;
use rapier_testbed3d::{Testbed, TestbedApp};
use std::cmp::Ordering;
mod debug_serialized3;
fn demo_name_from_command_line() -> Option<String> {
let mut args = std::env::args();
while let Some(arg) = args.next() {
if &arg[..] == "--example" {
return args.next();
}
}
None
}
#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))]
fn demo_name_from_url() -> Option<String> {
None
// let window = stdweb::web::window();
// let hash = window.location()?.search().ok()?;
// if hash.len() > 0 {
// Some(hash[1..].to_string())
// } else {
// None
// }
}
#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))]
fn demo_name_from_url() -> Option<String> {
None
}
#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))]
pub fn main() {
let demo = demo_name_from_command_line()
.or_else(|| demo_name_from_url())
.unwrap_or(String::new())
.to_camel_case();
let mut builders: Vec<(_, fn(&mut Testbed))> =
vec![("(Debug) serialized", debug_serialized3::init_world)];
// Lexicographic sort, with stress tests moved at the end of the list.
builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) {
(true, true) | (false, false) => a.0.cmp(b.0),
(true, false) => Ordering::Greater,
(false, true) => Ordering::Less,
});
let i = builders
.iter()
.position(|builder| builder.0.to_camel_case().as_str() == demo.as_str())
.unwrap_or(0);
let testbed = TestbedApp::from_builders(i, builders);
testbed.run()
}

36
examples3d-f64/debug_serialized3.rs Normal file
View File

@@ -0,0 +1,36 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
#[derive(serde::Deserialize)]
struct State {
pub islands: IslandManager,
pub broad_phase: BroadPhase,
pub narrow_phase: NarrowPhase,
pub bodies: RigidBodySet,
pub colliders: ColliderSet,
pub impulse_joints: ImpulseJointSet,
pub multibody_joints: MultibodyJointSet,
pub ccd_solver: CCDSolver,
}
pub fn init_world(testbed: &mut Testbed) {
/*
* Set up the testbed.
*/
let bytes = std::fs::read("state.bin").unwrap();
let mut state: State = bincode::deserialize(&bytes).unwrap();
testbed.set_world(
state.bodies,
state.colliders,
state.impulse_joints,
state.multibody_joints,
);
testbed.harness_mut().physics.islands = state.islands;
testbed.harness_mut().physics.broad_phase = state.broad_phase;
testbed.harness_mut().physics.narrow_phase = state.narrow_phase;
testbed.harness_mut().physics.ccd_solver = state.ccd_solver;
testbed.set_graphics_shift(vector![-541.0, -6377257.0, -61.0]);
testbed.look_at(point![10.0, 10.0, 10.0], point![0.0, 0.0, 0.0]);
}

36
examples3d/convex_decomposition3.rs
View File

@@ -110,23 +110,23 @@ pub fn init_world(testbed: &mut Testbed) {
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(),
"assets/3d/camel_decimated.obj".to_string(),
"assets/3d/chair.obj".to_string(),
"assets/3d/cup_decimated.obj".to_string(),
"assets/3d/dilo_decimated.obj".to_string(),
"assets/3d/feline_decimated.obj".to_string(),
"assets/3d/genus3_decimated.obj".to_string(),
"assets/3d/hand2_decimated.obj".to_string(),
"assets/3d/hand_decimated.obj".to_string(),
"assets/3d/hornbug.obj".to_string(),
"assets/3d/octopus_decimated.obj".to_string(),
"assets/3d/rabbit_decimated.obj".to_string(),
// "assets/3d/rust_logo.obj".to_string(),
"assets/3d/rust_logo_simplified.obj".to_string(),
"assets/3d/screwdriver_decimated.obj".to_string(),
"assets/3d/table.obj".to_string(),
"assets/3d/tstTorusModel.obj".to_string(),
// "assets/3d/tstTorusModel2.obj".to_string(),
// "assets/3d/tstTorusModel3.obj".to_string(),
]
}

56
src/dynamics/integration_parameters.rs
View File

@@ -29,13 +29,11 @@ pub struct IntegrationParameters {
/// A good non-zero value is around `0.2`.
/// (default `0.0`).
pub erp: Real,
/// 0-1: multiplier applied to each accumulated impulse during constraints resolution.
/// This is similar to the concept of CFN (Constraint Force Mixing) except that it is
/// a multiplicative factor instead of an additive factor.
/// Larger values lead to stiffer constraints (1.0 being completely stiff).
/// Smaller values lead to more compliant constraints.
pub delassus_inv_factor: Real,
/// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
/// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
/// before stabilization).
/// (default `0.25`).
pub damping_ratio: Real,
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
pub allowed_linear_error: Real,
@@ -89,10 +87,42 @@ impl IntegrationParameters {
}
}
/// Convenience: `erp / dt`
#[inline]
pub(crate) fn erp_inv_dt(&self) -> Real {
self.erp * self.inv_dt()
/// The ERP coefficient, multiplied by the inverse timestep length.
pub fn erp_inv_dt(&self) -> Real {
0.8 / self.dt
}
/// The CFM factor to be used in the constraints resolution.
pub fn cfm_factor(&self) -> Real {
// Compute CFM assuming a critically damped spring multiplied by the dampingratio.
let inv_erp_minus_one = 1.0 / self.erp - 1.0;
// let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
// let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * inv_erp_minus_one);
// let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
// NOTE: This simplies to cfm = cfm_coefff / projected_mass:
let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
/ ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio);
// Furthermore, we use this coefficient inside of the impulse resolution.
// Surprisingly, several simplifications happen there.
// Let `m` the projected mass of the constraint.
// Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)`
// We have:
// new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse)
// = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse)
// = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel
// = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff)
// = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel)
// So, setting cfm_factor = 1 / (1 + cfm_coeff).
// We obtain:
// new_impulse = cfm_factor * (old_impulse - m * delta_vel)
//
// The value returned by this function is this cfm_factor that can be used directly
// in the constraints solver.
1.0 / (1.0 + cfm_coeff)
}
}
@@ -103,14 +133,14 @@ impl Default for IntegrationParameters {
min_ccd_dt: 1.0 / 60.0 / 100.0,
velocity_solve_fraction: 1.0,
erp: 0.8,
delassus_inv_factor: 0.75,
damping_ratio: 0.25,
allowed_linear_error: 0.001, // 0.005
prediction_distance: 0.002,
max_velocity_iterations: 4,
max_velocity_friction_iterations: 8,
max_stabilization_iterations: 1,
interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
// FIXME: what is the optimal value for min_island_size?
// TODO: what is the optimal value for min_island_size?
// It should not be too big so that we don't end up with
// huge islands that don't fit in cache.
// However we don't want it to be too small and end up with

5
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -23,6 +23,7 @@ pub(crate) enum AnyGenericVelocityConstraint {
impl AnyGenericVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
@@ -31,6 +32,7 @@ impl AnyGenericVelocityConstraint {
) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
cfm_factor,
jacobians,
mj_lambdas,
generic_mj_lambdas,
@@ -38,6 +40,7 @@ impl AnyGenericVelocityConstraint {
solve_friction,
),
AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
cfm_factor,
jacobians,
generic_mj_lambdas,
solve_restitution,
@@ -379,6 +382,7 @@ impl GenericVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
@@ -400,6 +404,7 @@ impl GenericVelocityConstraint {
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityConstraintElement::generic_solve_group(
cfm_factor,
elements,
jacobians,
&self.velocity_constraint.dir1,

8
src/dynamics/solver/generic_velocity_constraint_element.rs
View File

@@ -243,6 +243,7 @@ impl VelocityConstraintNormalPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
cfm_factor: Real,
j_id: usize,
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
@@ -261,7 +262,7 @@ impl VelocityConstraintNormalPart<Real> {
+ mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+ self.rhs;
let new_impulse = (self.impulse - self.r * dvel).max(0.0);
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -291,6 +292,7 @@ impl VelocityConstraintNormalPart<Real> {
impl VelocityConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,
elements: &mut [Self],
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
@@ -318,8 +320,8 @@ impl VelocityConstraintElement<Real> {
for element in elements.iter_mut() {
element.normal_part.generic_solve(
nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, mj_lambda2,
mj_lambdas,
cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1,
mj_lambda2, mj_lambdas,
);
nrm_j_id += j_step;
}

2
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -210,6 +210,7 @@ impl GenericVelocityGroundConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
jacobians: &DVector<Real>,
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
@@ -220,6 +221,7 @@ impl GenericVelocityGroundConstraint {
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityGroundConstraintElement::generic_solve_group(
cfm_factor,
elements,
jacobians,
self.velocity_constraint.limit,

10
src/dynamics/solver/generic_velocity_ground_constraint_element.rs
View File

@@ -75,6 +75,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
cfm_factor: Real,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
@@ -86,7 +87,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs;
let new_impulse = (self.impulse - self.r * dvel).max(0.0);
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -101,6 +102,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
impl VelocityGroundConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,
elements: &mut [Self],
jacobians: &DVector<Real>,
limit: Real,
@@ -119,9 +121,9 @@ impl VelocityGroundConstraintElement<Real> {
let mut nrm_j_id = j_id;
for element in elements.iter_mut() {
element
.normal_part
.generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
element.normal_part.generic_solve(
cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas,
);
nrm_j_id += j_step;
}
}

9
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -54,6 +54,7 @@ impl ParallelVelocitySolver {
let joint_descs = &joint_constraints.constraint_descs[..];
let mut target_num_desc = 0;
let mut shift = 0;
let cfm_factor = params.cfm_factor();
for _ in 0..params.max_velocity_iterations {
macro_rules! solve {
@@ -116,7 +117,13 @@ impl ParallelVelocitySolver {
);
shift += joint_descs.len();
start_index -= joint_descs.len();
solve!(contact_constraints, &mut self.mj_lambdas, true, true);
solve!(
contact_constraints,
cfm_factor,
&mut self.mj_lambdas,
true,
true
);
shift += contact_descs.len();
start_index -= contact_descs.len();
}

20
src/dynamics/solver/velocity_constraint.rs
View File

@@ -55,23 +55,26 @@ impl AnyVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
match self {
AnyVelocityConstraint::NongroupedGround(c) => {
c.solve(mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
}
AnyVelocityConstraint::Nongrouped(c) => {
c.solve(mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::GroupedGround(c) => {
c.solve(mj_lambdas, solve_normal, solve_friction)
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
}
#[cfg(feature = "simd-is-enabled")]
AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas, solve_normal, solve_friction),
AnyVelocityConstraint::Grouped(c) => {
c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
}
AnyVelocityConstraint::Empty => unreachable!(),
}
}
@@ -236,7 +239,7 @@ impl VelocityConstraint {
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = params.delassus_inv_factor
let projected_mass = 1.0
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
@@ -251,14 +254,13 @@ impl VelocityConstraint {
let rhs_bias = /* is_resting
* */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).min(0.0);
constraint.elements[k].normal_part = VelocityConstraintNormalPart {
gcross1,
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
impulse: 0.0,
r,
impulse: na::zero(),
r: projected_mass,
};
}
@@ -310,6 +312,7 @@ impl VelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -318,6 +321,7 @@ impl VelocityConstraint {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
VelocityConstraintElement::solve_group(
cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]

6
src/dynamics/solver/velocity_constraint_element.rs
View File

@@ -131,6 +131,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
#[inline]
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
@@ -143,7 +144,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
- dir1.dot(&mj_lambda2.linear)
+ self.gcross2.gdot(mj_lambda2.angular)
+ self.rhs;
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -171,6 +172,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
@@ -191,7 +193,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
for element in elements.iter_mut() {
element
.normal_part
.solve(&dir1, im1, im2, mj_lambda1, mj_lambda2);
.solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2);
}
}

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

@@ -48,9 +48,8 @@ impl WVelocityConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
@@ -121,7 +120,6 @@ impl WVelocityConstraint {
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let tangent_velocity =
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
@@ -137,10 +135,11 @@ impl WVelocityConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let imsum = im1 + im2;
let r = delassus_inv_factor
let projected_mass = SimdReal::splat(1.0)
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -154,8 +153,8 @@ impl WVelocityConstraint {
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
impulse: na::zero(),
r,
impulse: SimdReal::splat(0.0),
r: projected_mass,
};
}
@@ -202,6 +201,7 @@ impl WVelocityConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -221,6 +221,7 @@ impl WVelocityConstraint {
};
VelocityConstraintElement::solve_group(
SimdReal::splat(cfm_factor),
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]

8
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -153,7 +153,7 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = params.delassus_inv_factor
let projected_mass = 1.0
/ (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2));
@@ -172,8 +172,8 @@ impl VelocityGroundConstraint {
gcross2,
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
impulse: 0.0,
r,
impulse: na::zero(),
r: projected_mass,
};
}
@@ -219,6 +219,7 @@ impl VelocityGroundConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -226,6 +227,7 @@ impl VelocityGroundConstraint {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
VelocityGroundConstraintElement::solve_group(
cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]

16
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -109,12 +109,17 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
}
#[inline]
pub fn solve(&mut self, dir1: &Vector<N>, im2: &Vector<N>, mj_lambda2: &mut DeltaVel<N>)
where
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im2: &Vector<N>,
mj_lambda2: &mut DeltaVel<N>,
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -139,6 +144,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
@@ -155,7 +161,9 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
// Solve penetration.
if solve_normal {
for element in elements.iter_mut() {
element.normal_part.solve(&dir1, im2, mj_lambda2);
element
.normal_part
.solve(cfm_factor, &dir1, im2, mj_lambda2);
}
}

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

@@ -43,9 +43,8 @@ impl WVelocityGroundConstraint {
{
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
@@ -143,8 +142,9 @@ impl WVelocityGroundConstraint {
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = delassus_inv_factor
let projected_mass = SimdReal::splat(1.0)
/ (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -158,7 +158,7 @@ impl WVelocityGroundConstraint {
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
impulse: na::zero(),
r,
r: projected_mass,
};
}
@@ -199,6 +199,7 @@ impl WVelocityGroundConstraint {
pub fn solve(
&mut self,
cfm_factor: Real,
mj_lambdas: &mut [DeltaVel<Real>],
solve_normal: bool,
solve_friction: bool,
@@ -211,6 +212,7 @@ impl WVelocityGroundConstraint {
};
VelocityGroundConstraintElement::solve_group(
SimdReal::splat(cfm_factor),
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]

164
src/dynamics/solver/velocity_solver.rs
View File

@@ -50,6 +50,7 @@ impl VelocitySolver {
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyDamping>,
{
let cfm_factor = params.cfm_factor();
self.mj_lambdas.clear();
self.mj_lambdas
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
@@ -93,18 +94,36 @@ impl VelocitySolver {
}
for constraint in &mut *contact_constraints {
constraint.solve(&mut self.mj_lambdas[..], true, solve_friction);
constraint.solve(cfm_factor, &mut self.mj_lambdas[..], true, false);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
true,
solve_friction,
false,
);
}
if solve_friction {
for constraint in &mut *contact_constraints {
constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
false,
true,
);
}
}
}
let remaining_friction_iterations =
@@ -118,11 +137,12 @@ impl VelocitySolver {
for _ in 0..remaining_friction_iterations {
for constraint in &mut *contact_constraints {
constraint.solve(&mut self.mj_lambdas[..], false, true);
constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
cfm_factor,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
@@ -147,10 +167,7 @@ impl VelocitySolver {
multibody.velocities += mj_lambdas;
multibody.integrate(params.dt);
multibody.forward_kinematics(bodies, false);
if params.max_stabilization_iterations > 0 {
multibody.velocities = prev_vels;
}
multibody.velocities = prev_vels;
}
} else {
let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
@@ -177,88 +194,85 @@ impl VelocitySolver {
new_poss.next_position =
new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
bodies.set_internal(handle.0, new_poss);
if params.max_stabilization_iterations == 0 {
bodies.set_internal(handle.0, new_vels);
}
}
}
if params.max_stabilization_iterations > 0 {
for joint in &mut *joint_constraints {
joint.remove_bias_from_rhs();
for joint in &mut *joint_constraints {
joint.remove_bias_from_rhs();
}
for constraint in &mut *contact_constraints {
constraint.remove_bias_from_rhs();
}
for constraint in &mut *generic_contact_constraints {
constraint.remove_bias_from_rhs();
}
for _ in 0..params.max_stabilization_iterations {
for constraint in &mut *joint_constraints {
constraint.solve(
generic_joint_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
);
}
for constraint in &mut *contact_constraints {
constraint.remove_bias_from_rhs();
constraint.solve(1.0, &mut self.mj_lambdas[..], true, false);
}
for constraint in &mut *generic_contact_constraints {
constraint.remove_bias_from_rhs();
constraint.solve(
1.0,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
true,
false,
);
}
for _ in 0..params.max_stabilization_iterations {
for constraint in &mut *joint_constraints {
constraint.solve(
generic_joint_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
);
}
for constraint in &mut *contact_constraints {
constraint.solve(&mut self.mj_lambdas[..], true, true);
}
for constraint in &mut *generic_contact_constraints {
constraint.solve(
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
true,
true,
);
}
for constraint in &mut *contact_constraints {
constraint.solve(1.0, &mut self.mj_lambdas[..], false, true);
}
// Update velocities.
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
for constraint in &mut *generic_contact_constraints {
constraint.solve(
1.0,
generic_contact_jacobians,
&mut self.mj_lambdas[..],
&mut self.generic_mj_lambdas,
false,
true,
);
}
}
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities += mj_lambdas;
}
} else {
let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
// Update velocities.
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
.get_multibody_mut_internal(link.multibody)
.unwrap();
let dvel = self.mj_lambdas[ids.active_set_offset];
let dangvel = mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
// let mut curr_vel_pseudo_energy = 0.0;
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
// curr_vel_pseudo_energy = vels.pseudo_kinetic_energy();
vels.linvel += dvel.linear;
vels.angvel += dangvel;
});
// let impulse_vel_pseudo_energy = RigidBodyVelocity {
// linvel: dvel.linear,
// angvel: dangvel,
// }
// .pseudo_kinetic_energy();
//
// bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
// activation.energy =
// impulse_vel_pseudo_energy.max(curr_vel_pseudo_energy / 5.0);
// });
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let mj_lambdas = self
.generic_mj_lambdas
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities += mj_lambdas;
}
} else {
let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[ids.active_set_offset];
let dangvel = mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
});
}
}

8
src_testbed/graphics.rs
View File

@@ -5,7 +5,7 @@ use na::{point, Point3};
use crate::objects::node::EntityWithGraphics;
use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
use rapier::math::{Isometry, Real};
use rapier::math::{Isometry, Real, Vector};
//use crate::objects::capsule::Capsule;
//#[cfg(feature = "dim3")]
//use crate::objects::mesh::Mesh;
@@ -30,6 +30,7 @@ pub struct GraphicsManager {
b2wireframe: HashMap<RigidBodyHandle, bool>,
ground_color: Point3<f32>,
prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
pub gfx_shift: Vector<Real>,
}
impl GraphicsManager {
@@ -42,6 +43,7 @@ impl GraphicsManager {
ground_color: point![0.5, 0.5, 0.5],
b2wireframe: HashMap::new(),
prefab_meshes: HashMap::new(),
gfx_shift: Vector::zeros(),
}
}
@@ -239,7 +241,7 @@ impl GraphicsManager {
new_nodes
.iter_mut()
.for_each(|n| n.update(colliders, components));
.for_each(|n| n.update(colliders, components, &self.gfx_shift));
// for node in new_nodes.iter_mut().filter_map(|n| n.scene_node_mut()) {
// if self.b2wireframe.get(&handle).cloned() == Some(true) {
@@ -368,7 +370,7 @@ impl GraphicsManager {
// }
// }
n.update(colliders, components);
n.update(colliders, components, &self.gfx_shift);
}
}
}

15
src_testbed/objects/node.rs
View File

@@ -10,7 +10,7 @@ use bevy::render::render_resource::PrimitiveTopology;
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
#[cfg(feature = "dim3")]
use rapier::geometry::{Cone, Cylinder};
use rapier::math::{Isometry, Real};
use rapier::math::{Isometry, Real, Vector};
use crate::graphics::BevyMaterial;
#[cfg(feature = "dim2")]
@@ -168,15 +168,20 @@ impl EntityWithGraphics {
self.base_color = color;
}
pub fn update(&mut self, colliders: &ColliderSet, components: &mut Query<(&mut Transform,)>) {
pub fn update(
&mut self,
colliders: &ColliderSet,
components: &mut Query<(&mut Transform,)>,
gfx_shift: &Vector<Real>,
) {
if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
let co_pos = co.position() * self.delta;
pos.translation.x = co_pos.translation.vector.x as f32;
pos.translation.y = co_pos.translation.vector.y as f32;
pos.translation.x = (co_pos.translation.vector.x + gfx_shift.x) as f32;
pos.translation.y = (co_pos.translation.vector.y + gfx_shift.y) as f32;
#[cfg(feature = "dim3")]
{
pos.translation.z = co_pos.translation.vector.z as f32;
pos.translation.z = (co_pos.translation.vector.z + gfx_shift.z) as f32;
pos.rotation = Quat::from_xyzw(
co_pos.rotation.i as f32,
co_pos.rotation.j as f32,

10
src_testbed/testbed.rs
View File

@@ -362,7 +362,7 @@ impl TestbedApp {
vsync: true,
..Default::default()
})
.insert_resource(ClearColor(Color::rgb(0.85, 0.85, 0.85)))
.insert_resource(ClearColor(Color::rgb(0.15, 0.15, 0.15)))
.insert_resource(Msaa { samples: 4 })
.insert_resource(WgpuOptions {
// Required for wireframes.
@@ -536,6 +536,14 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
}
}
pub fn set_graphics_shift(&mut self, shift: Vector<Real>) {
if !self.state.camera_locked {
if let Some(graphics) = &mut self.graphics {
graphics.graphics.gfx_shift = shift;
}
}
}
#[cfg(feature = "dim2")]
pub fn look_at(&mut self, at: Point2<f32>, zoom: f32) {
if !self.state.camera_locked {