First public release of Rapier.
This commit is contained in:
Sébastien Crozet
12
.github/FUNDING.yml
vendored
Normal file
12
.github/FUNDING.yml
vendored
Normal file
@@ -0,0 +1,12 @@
|
||||
# These are supported funding model platforms
|
||||
|
||||
github: [ "dimforge" ] # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2]
|
||||
patreon: # Replace with a single Patreon username
|
||||
open_collective: # Replace with a single Open Collective username
|
||||
ko_fi: # Replace with a single Ko-fi username
|
||||
tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel
|
||||
community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry
|
||||
liberapay: # Replace with a single Liberapay username
|
||||
issuehunt: # Replace with a single IssueHunt username
|
||||
otechie: # Replace with a single Otechie username
|
||||
custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2']
|
||||
8
.gitignore
vendored
Normal file
8
.gitignore
vendored
Normal file
@@ -0,0 +1,8 @@
|
||||
**/*.rs.bk
|
||||
Cargo.lock
|
||||
node_modules
|
||||
target
|
||||
.idea
|
||||
.DS_Store
|
||||
package-lock.json
|
||||
**/*.csv
|
||||
12
Cargo.toml
Normal file
12
Cargo.toml
Normal file
@@ -0,0 +1,12 @@
|
||||
[workspace]
|
||||
members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "examples2d/wasm",
|
||||
"build/rapier3d", "build/rapier_testbed3d", "examples3d", "examples3d/wasm",]
|
||||
|
||||
[patch.crates-io]
|
||||
#wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
|
||||
|
||||
[profile.release]
|
||||
debug = false
|
||||
codegen-units = 1
|
||||
#opt-level = 1
|
||||
#lto = true
|
||||
201
LICENSE
Normal file
201
LICENSE
Normal file
@@ -0,0 +1,201 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright 2020 Sébastien Crozet
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
30
README.md
Normal file
30
README.md
Normal file
@@ -0,0 +1,30 @@
|
||||
<p align="center">
|
||||
<img src="https://www.rapier.rs/img/rapier_logo_color_textpath.svg" alt="crates.io">
|
||||
</p>
|
||||
<p align="center">
|
||||
<a href="https://discord.gg/vt9DJSW">
|
||||
<img src="https://img.shields.io/discord/507548572338880513.svg?logo=discord&colorB=7289DA">
|
||||
</a>
|
||||
<a href="https://travis-ci.org/dimforge/rapier">
|
||||
<img src="https://travis-ci.org/dimforge/rapier.svg?branch=master" alt="Build status">
|
||||
</a>
|
||||
<a href="https://crates.io/crates/rapier">
|
||||
<img src="https://meritbadge.herokuapp.com/rapier?style=flat-square" alt="crates.io">
|
||||
</a>
|
||||
<a href="https://opensource.org/licenses/Apache-2.0">
|
||||
<img src="https://img.shields.io/badge/License-Apache%202.0-blue.svg">
|
||||
</a>
|
||||
</p>
|
||||
<p align = "center">
|
||||
<strong>
|
||||
<a href="https://rapier.rs">Website</a> | <a href="https://rapier.rs/docs/">Documentation</a>
|
||||
</p>
|
||||
|
||||
-----
|
||||
|
||||
<p align = "center">
|
||||
<b>2D and 3D physics engines</b>
|
||||
<i>for the Rust programming language.</i>
|
||||
</p>
|
||||
|
||||
-----
|
||||
53
build/rapier2d/Cargo.toml
Normal file
53
build/rapier2d/Cargo.toml
Normal file
@@ -0,0 +1,53 @@
|
||||
# Name idea: bident for 2D and trident for 3D
|
||||
[package]
|
||||
name = "rapier2d"
|
||||
version = "0.1.0"
|
||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||
description = "2-dimensional physics engine in Rust."
|
||||
documentation = "http://rapier.rs/rustdoc/rapier2d/index.html"
|
||||
homepage = "http://rapier.rs"
|
||||
repository = "https://github.com/rustsim/rapier"
|
||||
readme = "README.md"
|
||||
keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
|
||||
license = "Apache-2.0"
|
||||
edition = "2018"
|
||||
|
||||
[features]
|
||||
default = [ "dim2" ]
|
||||
dim2 = [ ]
|
||||
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 = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
|
||||
enhanced-determinism = [ "simba/libm_force", "indexmap" ]
|
||||
|
||||
[lib]
|
||||
name = "rapier2d"
|
||||
path = "../../src/lib.rs"
|
||||
required-features = [ "dim2" ]
|
||||
|
||||
|
||||
[dependencies]
|
||||
vec_map = "0.8"
|
||||
instant = { version = "0.1", features = [ "now" ]}
|
||||
num-traits = "0.2"
|
||||
nalgebra = "0.22"
|
||||
ncollide2d = "0.24"
|
||||
simba = "0.2"
|
||||
approx = "0.3"
|
||||
rayon = { version = "1", optional = true }
|
||||
crossbeam = "0.7"
|
||||
generational-arena = "0.2"
|
||||
arrayvec = "0.5"
|
||||
bit-vec = "0.6"
|
||||
rustc-hash = "1"
|
||||
serde = { version = "1", features = [ "derive" ], optional = true }
|
||||
indexmap = { version = "1", features = [ "serde-1" ], optional = true }
|
||||
|
||||
[dev-dependencies]
|
||||
bincode = "1"
|
||||
serde = { version = "1", features = [ "derive" ] }
|
||||
53
build/rapier3d/Cargo.toml
Normal file
53
build/rapier3d/Cargo.toml
Normal file
@@ -0,0 +1,53 @@
|
||||
# Name idea: bident for 2D and trident for 3D
|
||||
[package]
|
||||
name = "rapier3d"
|
||||
version = "0.1.0"
|
||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||
description = "3-dimensional physics engine in Rust."
|
||||
documentation = "http://rapier.rs/rustdoc/rapier2d/index.html"
|
||||
homepage = "http://rapier.rs"
|
||||
repository = "https://github.com/rustsim/rapier"
|
||||
readme = "README.md"
|
||||
keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
|
||||
license = "Apache-2.0"
|
||||
edition = "2018"
|
||||
|
||||
[features]
|
||||
default = [ "dim3" ]
|
||||
dim3 = [ ]
|
||||
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 = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
|
||||
enhanced-determinism = [ "simba/libm_force", "indexmap" ]
|
||||
|
||||
[lib]
|
||||
name = "rapier3d"
|
||||
path = "../../src/lib.rs"
|
||||
required-features = [ "dim3" ]
|
||||
|
||||
|
||||
[dependencies]
|
||||
vec_map = "0.8"
|
||||
instant = { version = "0.1", features = [ "now" ]}
|
||||
num-traits = "0.2"
|
||||
nalgebra = "0.22"
|
||||
ncollide3d = "0.24"
|
||||
simba = "0.2"
|
||||
approx = "0.3"
|
||||
rayon = { version = "1", optional = true }
|
||||
crossbeam = "0.7"
|
||||
generational-arena = "0.2"
|
||||
arrayvec = "0.5"
|
||||
bit-vec = "0.6"
|
||||
rustc-hash = "1"
|
||||
serde = { version = "1", features = [ "derive" ], optional = true }
|
||||
indexmap = { version = "1", features = [ "serde-1" ], optional = true }
|
||||
|
||||
[dev-dependencies]
|
||||
bincode = "1"
|
||||
serde = { version = "1", features = [ "derive" ] }
|
||||
45
build/rapier_testbed2d/Cargo.toml
Normal file
45
build/rapier_testbed2d/Cargo.toml
Normal file
@@ -0,0 +1,45 @@
|
||||
[package]
|
||||
name = "rapier_testbed2d"
|
||||
version = "0.1.0"
|
||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||
description = "Testbed for the 2-dimensional physics engine in Rust."
|
||||
homepage = "http://rapier.org"
|
||||
repository = "https://github.com/rustsim/rapier"
|
||||
keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
|
||||
license = "Apache-2.0"
|
||||
edition = "2018"
|
||||
|
||||
|
||||
[lib]
|
||||
name = "rapier_testbed2d"
|
||||
path = "../../src_testbed/lib.rs"
|
||||
required-features = [ "dim2" ]
|
||||
|
||||
[features]
|
||||
default = [ "dim2" ]
|
||||
dim2 = [ ]
|
||||
parallel = [ "rapier2d/parallel", "num_cpus" ]
|
||||
other-backends = [ "wrapped2d", "nphysics2d" ]
|
||||
|
||||
|
||||
[dependencies]
|
||||
nalgebra = "0.22"
|
||||
kiss3d = { version = "0.25", 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.24"
|
||||
nphysics2d = { version = "0.17", optional = true }
|
||||
crossbeam = "0.7"
|
||||
bincode = "1"
|
||||
flexbuffers = "0.1"
|
||||
md5 = "0.7"
|
||||
|
||||
|
||||
[dependencies.rapier2d]
|
||||
path = "../rapier2d"
|
||||
version = "0.1"
|
||||
features = [ "serde-serialize" ]
|
||||
47
build/rapier_testbed3d/Cargo.toml
Normal file
47
build/rapier_testbed3d/Cargo.toml
Normal file
@@ -0,0 +1,47 @@
|
||||
[package]
|
||||
name = "rapier_testbed3d"
|
||||
version = "0.1.0"
|
||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||
description = "Testbed for the 3-dimensional physics engine in Rust."
|
||||
homepage = "http://rapier.org"
|
||||
repository = "https://github.com/rustsim/rapier"
|
||||
keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ]
|
||||
license = "Apache-2.0"
|
||||
edition = "2018"
|
||||
|
||||
|
||||
[lib]
|
||||
name = "rapier_testbed3d"
|
||||
path = "../../src_testbed/lib.rs"
|
||||
required-features = [ "dim3" ]
|
||||
|
||||
[features]
|
||||
default = [ "dim3" ]
|
||||
dim3 = [ ]
|
||||
parallel = [ "rapier3d/parallel", "num_cpus" ]
|
||||
other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
|
||||
|
||||
[dependencies]
|
||||
nalgebra = "0.22"
|
||||
kiss3d = { version = "0.25", features = [ "conrod" ] }
|
||||
rand = "0.7"
|
||||
rand_pcg = "0.2"
|
||||
instant = { version = "0.1", features = [ "web-sys", "now" ]}
|
||||
bitflags = "1"
|
||||
glam = { version = "0.8", optional = true }
|
||||
num_cpus = { version = "1", optional = true }
|
||||
ncollide3d = "0.24"
|
||||
nphysics3d = { version = "0.17", optional = true }
|
||||
physx = { version = "0.6", optional = true }
|
||||
physx-sys = { version = "0.4", optional = true }
|
||||
crossbeam = "0.7"
|
||||
bincode = "1"
|
||||
flexbuffers = "0.1"
|
||||
serde_cbor = "0.11"
|
||||
md5 = "0.7"
|
||||
serde = { version = "1", features = [ "derive" ] }
|
||||
|
||||
[dependencies.rapier3d]
|
||||
path = "../rapier3d"
|
||||
version = "0.1"
|
||||
features = [ "serde-serialize" ]
|
||||
27
examples2d/Cargo.toml
Normal file
27
examples2d/Cargo.toml
Normal file
@@ -0,0 +1,27 @@
|
||||
[package]
|
||||
name = "nphysics-examples-2d"
|
||||
version = "0.1.0"
|
||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||
edition = "2018"
|
||||
|
||||
[features]
|
||||
parallel = [ "rapier2d/parallel", "rapier_testbed2d/parallel" ]
|
||||
simd-stable = [ "rapier2d/simd-stable" ]
|
||||
simd-nightly = [ "rapier2d/simd-nightly" ]
|
||||
other-backends = [ "rapier_testbed2d/other-backends" ]
|
||||
enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
|
||||
|
||||
[dependencies]
|
||||
rand = "0.7"
|
||||
Inflector = "0.11"
|
||||
nalgebra = "0.22"
|
||||
|
||||
[dependencies.rapier_testbed2d]
|
||||
path = "../build/rapier_testbed2d"
|
||||
|
||||
[dependencies.rapier2d]
|
||||
path = "../build/rapier2d"
|
||||
|
||||
[[bin]]
|
||||
name = "all_examples2"
|
||||
path = "./all_examples2.rs"
|
||||
90
examples2d/all_examples2.rs
Normal file
90
examples2d/all_examples2.rs
Normal file
@@ -0,0 +1,90 @@
|
||||
#![allow(dead_code)]
|
||||
|
||||
extern crate nalgebra as na;
|
||||
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
use wasm_bindgen::prelude::*;
|
||||
|
||||
use inflector::Inflector;
|
||||
|
||||
use rapier_testbed2d::Testbed;
|
||||
use std::cmp::Ordering;
|
||||
|
||||
mod balls2;
|
||||
mod boxes2;
|
||||
mod capsules2;
|
||||
mod debug_box_ball2;
|
||||
mod heightfield2;
|
||||
mod joints2;
|
||||
mod kinematic2;
|
||||
mod pyramid2;
|
||||
mod sensor2;
|
||||
mod stress_joint_ball2;
|
||||
mod stress_joint_fixed2;
|
||||
mod stress_joint_prismatic2;
|
||||
|
||||
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()?;
|
||||
// Some(hash[1..].to_string())
|
||||
}
|
||||
|
||||
#[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![
|
||||
("Balls", balls2::init_world),
|
||||
("Boxes", boxes2::init_world),
|
||||
("Capsules", capsules2::init_world),
|
||||
("Heightfield", heightfield2::init_world),
|
||||
("Joints", joints2::init_world),
|
||||
("Kinematic", kinematic2::init_world),
|
||||
("Pyramid", pyramid2::init_world),
|
||||
("Sensor", sensor2::init_world),
|
||||
("(Debug) box ball", debug_box_ball2::init_world),
|
||||
("(Stress test) joint ball", stress_joint_ball2::init_world),
|
||||
("(Stress test) joint fixed", stress_joint_fixed2::init_world),
|
||||
(
|
||||
"(Stress test) joint prismatic",
|
||||
stress_joint_prismatic2::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 = Testbed::from_builders(i, builders);
|
||||
|
||||
testbed.run()
|
||||
}
|
||||
68
examples2d/balls2.rs
Normal file
68
examples2d/balls2.rs
Normal file
@@ -0,0 +1,68 @@
|
||||
use na::Point2;
|
||||
use rapier2d::dynamics::{BodyStatus, 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 = 25.0;
|
||||
|
||||
/*
|
||||
let ground_shape = ShapeHandle::new(Cuboid::new(Vector2::new(ground_size, 1.0)));
|
||||
|
||||
let ground_handle = bodies.insert(Ground::new());
|
||||
let co = ColliderDesc::new(ground_shape)
|
||||
.translation(-Vector2::y())
|
||||
.build(BodyPartHandle(ground_handle, 0));
|
||||
colliders.insert(co);
|
||||
*/
|
||||
|
||||
/*
|
||||
* Create the balls
|
||||
*/
|
||||
let num = 50;
|
||||
let rad = 1.0;
|
||||
|
||||
let shiftx = rad * 2.5;
|
||||
let shifty = rad * 2.0;
|
||||
let centerx = shiftx * (num as f32) / 2.0;
|
||||
let centery = shifty / 2.0;
|
||||
|
||||
for i in 0..num {
|
||||
for j in 0usize..num * 5 {
|
||||
let x = i as f32 * shiftx - centerx;
|
||||
let y = j as f32 * shifty + centery;
|
||||
|
||||
let status = if j == 0 {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new(status).translation(x, y).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(0.0, 2.5), 5.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
73
examples2d/boxes2.rs
Normal file
73
examples2d/boxes2.rs
Normal file
@@ -0,0 +1,73 @@
|
||||
use na::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 = 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 * 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 cubes
|
||||
*/
|
||||
let num = 26;
|
||||
let rad = 0.5;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let centerx = shift * (num as f32) / 2.0;
|
||||
let centery = shift / 2.0;
|
||||
|
||||
for i in 0..num {
|
||||
for j in 0usize..num * 5 {
|
||||
let x = i as f32 * shift - centerx;
|
||||
let y = j as f32 * shift + centery + 2.0;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).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()
|
||||
}
|
||||
76
examples2d/capsules2.rs
Normal file
76
examples2d/capsules2.rs
Normal file
@@ -0,0 +1,76 @@
|
||||
use na::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 = 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 * 4.0)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size * 4.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 * 4.0)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the cubes
|
||||
*/
|
||||
let num = 26;
|
||||
let rad = 0.5;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let shifty = rad * 5.0;
|
||||
let centerx = shift * (num as f32) / 2.0;
|
||||
let centery = shift / 2.0;
|
||||
|
||||
for i in 0..num {
|
||||
for j in 0usize..num * 5 {
|
||||
let x = i as f32 * shift - centerx;
|
||||
let y = j as f32 * shifty + centery + 3.0;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::capsule_y(rad * 1.5, rad)
|
||||
.density(1.0)
|
||||
.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()
|
||||
}
|
||||
44
examples2d/debug_box_ball2.rs
Normal file
44
examples2d/debug_box_ball2.rs
Normal file
@@ -0,0 +1,44 @@
|
||||
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 rad = 1.0;
|
||||
let rigid_body = RigidBodyBuilder::new_static()
|
||||
.translation(0.0, -rad)
|
||||
.rotation(std::f32::consts::PI / 4.0)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
// Build the dynamic box rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.translation(0.0, 3.0 * rad)
|
||||
.can_sleep(false)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
// testbed.look_at(Point2::new(10.0, 10.0, 10.0), Point2::origin());
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
72
examples2d/heightfield2.rs
Normal file
72
examples2d/heightfield2.rs
Normal file
@@ -0,0 +1,72 @@
|
||||
use na::{DVector, Point2, Vector2};
|
||||
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 = Vector2::new(50.0, 1.0);
|
||||
let nsubdivs = 2000;
|
||||
|
||||
let heights = DVector::from_fn(nsubdivs + 1, |i, _| {
|
||||
if i == 0 || i == nsubdivs {
|
||||
80.0
|
||||
} else {
|
||||
(i as f32 * ground_size.x / (nsubdivs as f32)).cos() * 2.0
|
||||
}
|
||||
});
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static().build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::heightfield(heights, ground_size).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the cubes
|
||||
*/
|
||||
let num = 26;
|
||||
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 * 5 {
|
||||
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).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
} else {
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).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![("Heightfield", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
75
examples2d/joints2.rs
Normal file
75
examples2d/joints2.rs
Normal file
@@ -0,0 +1,75 @@
|
||||
use na::Point2;
|
||||
use rapier2d::dynamics::{BallJoint, BodyStatus, 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 mut joints = JointSet::new();
|
||||
|
||||
/*
|
||||
* Create the balls
|
||||
*/
|
||||
// Build the rigid body.
|
||||
// NOTE: a smaller radius (e.g. 0.1) breaks Box2D so
|
||||
// in order to be able to compare rapier with Box2D,
|
||||
// we set it to 0.4.
|
||||
let rad = 0.4;
|
||||
let numi = 100; // Num vertical nodes.
|
||||
let numk = 100; // Num horizontal nodes.
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for k in 0..numk {
|
||||
for i in 0..numi {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
let status = if i == 0 && (k % 4 == 0 || k == numk - 1) {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(fk * shift, -fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, child_handle, &mut bodies);
|
||||
|
||||
// Vertical joint.
|
||||
if i > 0 {
|
||||
let parent_handle = *body_handles.last().unwrap();
|
||||
let joint = BallJoint::new(Point2::origin(), Point2::new(0.0, shift));
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - numi;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = BallJoint::new(Point2::origin(), Point2::new(-shift, 0.0));
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(numk as f32 * rad, numi as f32 * -rad), 5.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
93
examples2d/kinematic2.rs
Normal file
93
examples2d/kinematic2.rs
Normal file
@@ -0,0 +1,93 @@
|
||||
use na::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 = 10.0;
|
||||
let ground_height = 0.1;
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static()
|
||||
.translation(0.0, -ground_height)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size, ground_height).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the boxes
|
||||
*/
|
||||
let num = 6;
|
||||
let rad = 0.2;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let centerx = shift * num as f32 / 2.0;
|
||||
let centery = shift / 2.0 + 3.04;
|
||||
|
||||
for i in 0usize..num {
|
||||
for j in 0usize..num * 50 {
|
||||
let x = i as f32 * shift - centerx;
|
||||
let y = j as f32 * shift + centery;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Setup a kinematic rigid body.
|
||||
*/
|
||||
let platform_body = RigidBodyBuilder::new_kinematic()
|
||||
.translation(-10.0 * rad, 1.5 + 0.8)
|
||||
.build();
|
||||
let platform_handle = bodies.insert(platform_body);
|
||||
let collider = ColliderBuilder::cuboid(rad * 10.0, rad)
|
||||
.density(1.0)
|
||||
.build();
|
||||
colliders.insert(collider, platform_handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Setup a callback to control the platform.
|
||||
*/
|
||||
testbed.add_callback(move |bodies, _, _, _, time| {
|
||||
let mut platform = bodies.get_mut(platform_handle).unwrap();
|
||||
let mut next_pos = platform.position;
|
||||
|
||||
let dt = 0.016;
|
||||
next_pos.translation.vector.y += (time * 5.0).sin() * dt;
|
||||
next_pos.translation.vector.x += time.sin() * 5.0 * dt;
|
||||
|
||||
if next_pos.translation.vector.x >= rad * 10.0 {
|
||||
next_pos.translation.vector.x -= dt;
|
||||
}
|
||||
if next_pos.translation.vector.x <= -rad * 10.0 {
|
||||
next_pos.translation.vector.x += dt;
|
||||
}
|
||||
|
||||
platform.set_next_kinematic_position(next_pos);
|
||||
});
|
||||
|
||||
/*
|
||||
* Run the simulation.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(0.0, 1.0), 40.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Kinematic body", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
60
examples2d/pyramid2.rs
Normal file
60
examples2d/pyramid2.rs
Normal file
@@ -0,0 +1,60 @@
|
||||
use na::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 = 100.0;
|
||||
let ground_thickness = 1.0;
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static().build();
|
||||
let ground_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build();
|
||||
colliders.insert(collider, ground_handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the cubes
|
||||
*/
|
||||
let num = 100;
|
||||
let rad = 0.5;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let centerx = shift * (num as f32) / 2.0;
|
||||
let centery = shift / 2.0 + ground_thickness + rad * 1.5;
|
||||
|
||||
for i in 0usize..num {
|
||||
for j in i..num {
|
||||
let fj = j as f32;
|
||||
let fi = i as f32;
|
||||
let x = (fi * shift / 2.0) + (fj - fi) * shift - centerx;
|
||||
let y = fi * shift + centery;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(0.0, 2.5), 5.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
101
examples2d/sensor2.rs
Normal file
101
examples2d/sensor2.rs
Normal file
@@ -0,0 +1,101 @@
|
||||
use na::{Point2, Point3};
|
||||
use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
|
||||
use rapier2d::geometry::{ColliderBuilder, ColliderSet, Proximity};
|
||||
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 = 200.1;
|
||||
let ground_height = 0.1;
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static()
|
||||
.translation(0.0, -ground_height)
|
||||
.build();
|
||||
let ground_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size, ground_height).build();
|
||||
colliders.insert(collider, ground_handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create some boxes.
|
||||
*/
|
||||
let num = 10;
|
||||
let rad = 0.2;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let centerx = shift * num as f32 / 2.0;
|
||||
|
||||
for i in 0usize..num {
|
||||
let x = i as f32 * shift - centerx;
|
||||
let y = 3.0;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
testbed.set_body_color(handle, Point3::new(0.5, 0.5, 1.0));
|
||||
}
|
||||
|
||||
/*
|
||||
* Create a cube that will have a ball-shaped sensor attached.
|
||||
*/
|
||||
|
||||
// Rigid body so that the sensor can move.
|
||||
let sensor = RigidBodyBuilder::new_dynamic()
|
||||
.translation(0.0, 10.0)
|
||||
.build();
|
||||
let sensor_handle = bodies.insert(sensor);
|
||||
|
||||
// Solid cube attached to the sensor which
|
||||
// other colliders can touch.
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, sensor_handle, &mut bodies);
|
||||
|
||||
// We create a collider desc without density because we don't
|
||||
// want it to contribute to the rigid body mass.
|
||||
let sensor_collider = ColliderBuilder::ball(rad * 5.0).sensor(true).build();
|
||||
colliders.insert(sensor_collider, sensor_handle, &mut bodies);
|
||||
|
||||
testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0));
|
||||
|
||||
// Callback that will be executed on the main loop to handle proximities.
|
||||
testbed.add_callback(move |_, colliders, events, graphics, _| {
|
||||
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),
|
||||
};
|
||||
|
||||
let parent_handle1 = colliders.get(prox.collider1).unwrap().parent();
|
||||
let parent_handle2 = colliders.get(prox.collider2).unwrap().parent();
|
||||
|
||||
if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
|
||||
graphics.set_body_color(parent_handle1, color);
|
||||
}
|
||||
if parent_handle2 != ground_handle && parent_handle2 != sensor_handle {
|
||||
graphics.set_body_color(parent_handle2, color);
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(0.0, 1.0), 100.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Sensor", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
72
examples2d/stress_joint_ball2.rs
Normal file
72
examples2d/stress_joint_ball2.rs
Normal file
@@ -0,0 +1,72 @@
|
||||
use na::Point2;
|
||||
use rapier2d::dynamics::{BallJoint, BodyStatus, 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 mut joints = JointSet::new();
|
||||
|
||||
/*
|
||||
* Create the balls
|
||||
*/
|
||||
// Build the rigid body.
|
||||
let rad = 0.4;
|
||||
let numi = 100; // Num vertical nodes.
|
||||
let numk = 100; // Num horizontal nodes.
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for k in 0..numk {
|
||||
for i in 0..numi {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
let status = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(fk * shift, -fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, child_handle, &mut bodies);
|
||||
|
||||
// Vertical joint.
|
||||
if i > 0 {
|
||||
let parent_handle = *body_handles.last().unwrap();
|
||||
let joint = BallJoint::new(Point2::origin(), Point2::new(0.0, shift));
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - numi;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = BallJoint::new(Point2::origin(), Point2::new(-shift, 0.0));
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(numk as f32 * rad, numi as f32 * -rad), 5.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
85
examples2d/stress_joint_fixed2.rs
Normal file
85
examples2d/stress_joint_fixed2.rs
Normal file
@@ -0,0 +1,85 @@
|
||||
use na::{Isometry2, Point2};
|
||||
use rapier2d::dynamics::{BodyStatus, FixedJoint, 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 mut joints = JointSet::new();
|
||||
|
||||
/*
|
||||
* Create the balls
|
||||
*/
|
||||
// Build the rigid body.
|
||||
let rad = 0.4;
|
||||
let num = 30; // Num vertical nodes.
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for xx in 0..4 {
|
||||
let x = xx as f32 * shift * (num as f32 + 2.0);
|
||||
|
||||
for yy in 0..4 {
|
||||
let y = yy as f32 * shift * (num as f32 + 4.0);
|
||||
|
||||
for k in 0..num {
|
||||
for i in 0..num {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
let status = if k == 0 {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(x + fk * shift, y - fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, child_handle, &mut bodies);
|
||||
|
||||
// Vertical joint.
|
||||
if i > 0 {
|
||||
let parent_handle = *body_handles.last().unwrap();
|
||||
let joint = FixedJoint::new(
|
||||
Isometry2::identity(),
|
||||
Isometry2::translation(0.0, shift),
|
||||
);
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - num;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = FixedJoint::new(
|
||||
Isometry2::identity(),
|
||||
Isometry2::translation(-shift, 0.0),
|
||||
);
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(50.0, 50.0), 5.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
69
examples2d/stress_joint_prismatic2.rs
Normal file
69
examples2d/stress_joint_prismatic2.rs
Normal file
@@ -0,0 +1,69 @@
|
||||
use na::{Point2, Unit, Vector2};
|
||||
use rapier2d::dynamics::{JointSet, PrismaticJoint, 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 mut joints = JointSet::new();
|
||||
|
||||
/*
|
||||
* Create the balls
|
||||
*/
|
||||
// Build the rigid body.
|
||||
let rad = 0.4;
|
||||
let num = 10;
|
||||
let shift = 1.0;
|
||||
|
||||
for l in 0..25 {
|
||||
let y = l as f32 * shift * (num as f32 + 2.0) * 2.0;
|
||||
|
||||
for j in 0..50 {
|
||||
let x = j as f32 * shift * 4.0;
|
||||
|
||||
let ground = RigidBodyBuilder::new_static().translation(x, y).build();
|
||||
let mut curr_parent = bodies.insert(ground);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).build();
|
||||
colliders.insert(collider, curr_parent, &mut bodies);
|
||||
|
||||
for i in 0..num {
|
||||
let y = y - (i + 1) as f32 * shift;
|
||||
let density = 1.0;
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
|
||||
let curr_child = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad).density(density).build();
|
||||
colliders.insert(collider, curr_child, &mut bodies);
|
||||
|
||||
let axis = if i % 2 == 0 {
|
||||
Unit::new_normalize(Vector2::new(1.0, 1.0))
|
||||
} else {
|
||||
Unit::new_normalize(Vector2::new(-1.0, 1.0))
|
||||
};
|
||||
|
||||
let mut prism =
|
||||
PrismaticJoint::new(Point2::origin(), axis, Point2::new(0.0, shift), axis);
|
||||
prism.limits_enabled = true;
|
||||
prism.limits[0] = -1.5;
|
||||
prism.limits[1] = 1.5;
|
||||
joints.insert(&mut bodies, curr_parent, curr_child, prism);
|
||||
|
||||
curr_parent = curr_child;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point2::new(80.0, 80.0), 15.0);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
27
examples3d/Cargo.toml
Normal file
27
examples3d/Cargo.toml
Normal file
@@ -0,0 +1,27 @@
|
||||
[package]
|
||||
name = "nphysics-examples-3d"
|
||||
version = "0.1.0"
|
||||
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
|
||||
edition = "2018"
|
||||
|
||||
[features]
|
||||
parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ]
|
||||
simd-stable = [ "rapier3d/simd-stable" ]
|
||||
simd-nightly = [ "rapier3d/simd-nightly" ]
|
||||
other-backends = [ "rapier_testbed3d/other-backends" ]
|
||||
enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
|
||||
|
||||
[dependencies]
|
||||
rand = "0.7"
|
||||
Inflector = "0.11"
|
||||
nalgebra = "0.22"
|
||||
|
||||
[dependencies.rapier_testbed3d]
|
||||
path = "../build/rapier_testbed3d"
|
||||
|
||||
[dependencies.rapier3d]
|
||||
path = "../build/rapier3d"
|
||||
|
||||
[[bin]]
|
||||
name = "all_examples3"
|
||||
path = "./all_examples3.rs"
|
||||
110
examples3d/all_examples3.rs
Normal file
110
examples3d/all_examples3.rs
Normal file
@@ -0,0 +1,110 @@
|
||||
#![allow(dead_code)]
|
||||
|
||||
extern crate nalgebra as na;
|
||||
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
use wasm_bindgen::prelude::*;
|
||||
|
||||
use inflector::Inflector;
|
||||
|
||||
use rapier_testbed3d::Testbed;
|
||||
use std::cmp::Ordering;
|
||||
|
||||
mod balls3;
|
||||
mod boxes3;
|
||||
mod capsules3;
|
||||
mod debug_boxes3;
|
||||
mod debug_triangle3;
|
||||
mod domino3;
|
||||
mod heightfield3;
|
||||
mod joints3;
|
||||
mod kinematic3;
|
||||
mod pyramid3;
|
||||
mod sensor3;
|
||||
mod stacks3;
|
||||
mod stress_joint_ball3;
|
||||
mod stress_joint_fixed3;
|
||||
mod stress_joint_prismatic3;
|
||||
mod stress_joint_revolute3;
|
||||
mod stress_keva3;
|
||||
mod trimesh3;
|
||||
|
||||
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![
|
||||
("Balls", balls3::init_world),
|
||||
("Boxes", boxes3::init_world),
|
||||
("Capsules", capsules3::init_world),
|
||||
("Domino", domino3::init_world),
|
||||
("Heightfield", heightfield3::init_world),
|
||||
("Joints", joints3::init_world),
|
||||
("Kinematic", kinematic3::init_world),
|
||||
("Stacks", stacks3::init_world),
|
||||
("Pyramid", pyramid3::init_world),
|
||||
("Sensor", sensor3::init_world),
|
||||
("Trimesh", trimesh3::init_world),
|
||||
("(Debug) boxes", debug_boxes3::init_world),
|
||||
("(Debug) triangle", debug_triangle3::init_world),
|
||||
("(Stress test) joint ball", stress_joint_ball3::init_world),
|
||||
("(Stress test) joint fixed", stress_joint_fixed3::init_world),
|
||||
(
|
||||
"(Stress test) joint revolute",
|
||||
stress_joint_revolute3::init_world,
|
||||
),
|
||||
(
|
||||
"(Stress test) joint prismatic",
|
||||
stress_joint_prismatic3::init_world,
|
||||
),
|
||||
("(Stress test) keva tower", stress_keva3::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 = Testbed::from_builders(i, builders);
|
||||
|
||||
testbed.run()
|
||||
}
|
||||
29
examples3d/balls3.rs
Normal file
29
examples3d/balls3.rs
Normal file
@@ -0,0 +1,29 @@
|
||||
use na::Point3;
|
||||
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();
|
||||
|
||||
let rb = RigidBodyBuilder::new_dynamic().build();
|
||||
let co = ColliderBuilder::cuboid(0.5, 0.5, 0.5).build();
|
||||
let h = bodies.insert(rb);
|
||||
colliders.insert(co, h, &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 main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
68
examples3d/boxes3.rs
Normal file
68
examples3d/boxes3.rs
Normal file
@@ -0,0 +1,68 @@
|
||||
use na::Point3;
|
||||
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 rad = 1.0;
|
||||
|
||||
let shift = rad * 2.0 + rad;
|
||||
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) * (rad * 2.0 + rad) * 0.5;
|
||||
|
||||
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;
|
||||
|
||||
// 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(rad, rad, rad).density(1.0).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()
|
||||
}
|
||||
69
examples3d/capsules3.rs
Normal file
69
examples3d/capsules3.rs
Normal file
@@ -0,0 +1,69 @@
|
||||
use na::Point3;
|
||||
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 rad = 1.0;
|
||||
|
||||
let shift = rad * 2.0 + rad;
|
||||
let shifty = rad * 4.0;
|
||||
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) * (rad * 2.0 + rad) * 0.5;
|
||||
|
||||
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 * shifty + centery + 3.0;
|
||||
let z = k as f32 * shift - centerz + offset;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::capsule_y(rad, rad).density(1.0).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()
|
||||
}
|
||||
47
examples3d/debug_boxes3.rs
Normal file
47
examples3d/debug_boxes3.rs
Normal file
@@ -0,0 +1,47 @@
|
||||
use na::{Point3, Vector3};
|
||||
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 = 100.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);
|
||||
|
||||
// Build the dynamic box rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.translation(1.1, 0.0, 0.0)
|
||||
.rotation(Vector3::new(0.8, 0.2, 0.1))
|
||||
.can_sleep(false)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
48
examples3d/debug_triangle3.rs
Normal file
48
examples3d/debug_triangle3.rs
Normal file
@@ -0,0 +1,48 @@
|
||||
use na::Point3;
|
||||
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();
|
||||
|
||||
// Triangle ground.
|
||||
let vtx = [
|
||||
Point3::new(-10.0, 0.0, -10.0),
|
||||
Point3::new(10.0, 0.0, -10.0),
|
||||
Point3::new(0.0, 0.0, 10.0),
|
||||
];
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static()
|
||||
.translation(0.0, 0.0, 0.0)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
// Dynamic box rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.translation(1.1, 0.01, 0.0)
|
||||
// .rotation(Vector3::new(0.8, 0.2, 0.1))
|
||||
.can_sleep(false)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
87
examples3d/domino3.rs
Normal file
87
examples3d/domino3.rs
Normal file
@@ -0,0 +1,87 @@
|
||||
use na::{Point3, Translation3, UnitQuaternion, Vector3};
|
||||
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 = 4000;
|
||||
let width = 1.0;
|
||||
let thickness = 0.1;
|
||||
|
||||
let colors = [Point3::new(0.7, 0.5, 0.9), Point3::new(0.6, 1.0, 0.6)];
|
||||
|
||||
let mut curr_angle = 0.0;
|
||||
let mut curr_rad = 10.0;
|
||||
let mut prev_angle;
|
||||
let mut skip = 0;
|
||||
for i in 0..num {
|
||||
let perimeter = 2.0 * std::f32::consts::PI * curr_rad;
|
||||
let spacing = thickness * 4.0;
|
||||
prev_angle = curr_angle;
|
||||
curr_angle += 2.0 * std::f32::consts::PI * spacing / perimeter;
|
||||
let (x, z) = curr_angle.sin_cos();
|
||||
|
||||
// Build the rigid body.
|
||||
let two_pi = 2.0 * std::f32::consts::PI;
|
||||
let nudged = curr_angle % two_pi < prev_angle % two_pi;
|
||||
let tilt = if nudged || i == num - 1 { 0.2 } else { 0.0 };
|
||||
|
||||
if skip == 0 {
|
||||
let rot = UnitQuaternion::new(Vector3::y() * curr_angle);
|
||||
let tilt = UnitQuaternion::new(rot * Vector3::z() * tilt);
|
||||
let position =
|
||||
Translation3::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad)
|
||||
* tilt
|
||||
* rot;
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().position(position).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width)
|
||||
.density(1.0)
|
||||
.build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
testbed.set_body_color(handle, colors[i % 2]);
|
||||
} else {
|
||||
skip -= 1;
|
||||
}
|
||||
|
||||
if nudged {
|
||||
skip = 5;
|
||||
}
|
||||
|
||||
curr_rad += 1.5 / perimeter;
|
||||
}
|
||||
|
||||
/*
|
||||
* 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()
|
||||
}
|
||||
78
examples3d/heightfield3.rs
Normal file
78
examples3d/heightfield3.rs
Normal file
@@ -0,0 +1,78 @@
|
||||
use na::{DMatrix, Point3, Vector3};
|
||||
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 = Vector3::new(200.0, 1.0, 200.0);
|
||||
let nsubdivs = 20;
|
||||
|
||||
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);
|
||||
x.sin() + z.cos()
|
||||
}
|
||||
});
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static().build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::heightfield(heights, ground_size).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the cubes
|
||||
*/
|
||||
let num = 8;
|
||||
let rad = 1.0;
|
||||
|
||||
let shift = rad * 2.0 + rad;
|
||||
let centerx = shift * (num / 2) as f32;
|
||||
let centery = shift / 2.0;
|
||||
let centerz = shift * (num / 2) as f32;
|
||||
|
||||
for j in 0usize..47 {
|
||||
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;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
|
||||
if j % 2 == 0 {
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
} else {
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).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 main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
272
examples3d/joints3.rs
Normal file
272
examples3d/joints3.rs
Normal file
@@ -0,0 +1,272 @@
|
||||
use na::{Isometry3, Point3, Unit, Vector3};
|
||||
use rapier3d::dynamics::{
|
||||
BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
|
||||
RigidBodySet,
|
||||
};
|
||||
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
|
||||
use rapier_testbed3d::Testbed;
|
||||
|
||||
fn create_prismatic_joints(
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
joints: &mut JointSet,
|
||||
origin: Point3<f32>,
|
||||
num: usize,
|
||||
) {
|
||||
let rad = 0.4;
|
||||
let shift = 1.0;
|
||||
|
||||
let ground = RigidBodyBuilder::new_static()
|
||||
.translation(origin.x, origin.y, origin.z)
|
||||
.build();
|
||||
let mut curr_parent = bodies.insert(ground);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
|
||||
colliders.insert(collider, curr_parent, bodies);
|
||||
|
||||
for i in 0..num {
|
||||
let z = origin.z + (i + 1) as f32 * shift;
|
||||
let density = 1.0;
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.translation(origin.x, origin.y, z)
|
||||
.build();
|
||||
let curr_child = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad)
|
||||
.density(density)
|
||||
.build();
|
||||
colliders.insert(collider, curr_child, bodies);
|
||||
|
||||
let axis = if i % 2 == 0 {
|
||||
Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
|
||||
} else {
|
||||
Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
|
||||
};
|
||||
|
||||
let z = Vector3::z();
|
||||
let mut prism = PrismaticJoint::new(
|
||||
Point3::origin(),
|
||||
axis,
|
||||
z,
|
||||
Point3::new(0.0, 0.0, -shift),
|
||||
axis,
|
||||
z,
|
||||
);
|
||||
prism.limits_enabled = true;
|
||||
prism.limits[0] = -2.0;
|
||||
prism.limits[1] = 2.0;
|
||||
joints.insert(bodies, curr_parent, curr_child, prism);
|
||||
|
||||
curr_parent = curr_child;
|
||||
}
|
||||
}
|
||||
|
||||
fn create_revolute_joints(
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
joints: &mut JointSet,
|
||||
origin: Point3<f32>,
|
||||
num: usize,
|
||||
) {
|
||||
let rad = 0.4;
|
||||
let shift = 2.0;
|
||||
|
||||
let ground = RigidBodyBuilder::new_static()
|
||||
.translation(origin.x, origin.y, 0.0)
|
||||
.build();
|
||||
let mut curr_parent = bodies.insert(ground);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
|
||||
colliders.insert(collider, curr_parent, bodies);
|
||||
|
||||
for i in 0..num {
|
||||
// Create four bodies.
|
||||
let z = origin.z + i as f32 * shift * 2.0 + shift;
|
||||
let positions = [
|
||||
Isometry3::translation(origin.x, origin.y, z),
|
||||
Isometry3::translation(origin.x + shift, origin.y, z),
|
||||
Isometry3::translation(origin.x + shift, origin.y, z + shift),
|
||||
Isometry3::translation(origin.x, origin.y, z + shift),
|
||||
];
|
||||
|
||||
let mut handles = [curr_parent; 4];
|
||||
for k in 0..4 {
|
||||
let density = 1.0;
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.position(positions[k])
|
||||
.build();
|
||||
handles[k] = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad)
|
||||
.density(density)
|
||||
.build();
|
||||
colliders.insert(collider, handles[k], bodies);
|
||||
}
|
||||
|
||||
// Setup four joints.
|
||||
let o = Point3::origin();
|
||||
let x = Vector3::x_axis();
|
||||
let z = Vector3::z_axis();
|
||||
|
||||
let revs = [
|
||||
RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
|
||||
RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x),
|
||||
RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
|
||||
RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x),
|
||||
];
|
||||
|
||||
joints.insert(bodies, curr_parent, handles[0], revs[0]);
|
||||
joints.insert(bodies, handles[0], handles[1], revs[1]);
|
||||
joints.insert(bodies, handles[1], handles[2], revs[2]);
|
||||
joints.insert(bodies, handles[2], handles[3], revs[3]);
|
||||
|
||||
curr_parent = handles[3];
|
||||
}
|
||||
}
|
||||
|
||||
fn create_fixed_joints(
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
joints: &mut JointSet,
|
||||
origin: Point3<f32>,
|
||||
num: usize,
|
||||
) {
|
||||
let rad = 0.4;
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for k in 0..num {
|
||||
for i in 0..num {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
// NOTE: the num - 2 test is to avoid two consecutive
|
||||
// fixed bodies. Because physx will crash if we add
|
||||
// a joint between these.
|
||||
let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(origin.x + fk * shift, origin.y, origin.z + fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, child_handle, bodies);
|
||||
|
||||
// Vertical joint.
|
||||
if i > 0 {
|
||||
let parent_handle = *body_handles.last().unwrap();
|
||||
let joint = FixedJoint::new(
|
||||
Isometry3::identity(),
|
||||
Isometry3::translation(0.0, 0.0, -shift),
|
||||
);
|
||||
joints.insert(bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - num;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = FixedJoint::new(
|
||||
Isometry3::identity(),
|
||||
Isometry3::translation(-shift, 0.0, 0.0),
|
||||
);
|
||||
joints.insert(bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn create_ball_joints(
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
joints: &mut JointSet,
|
||||
num: usize,
|
||||
) {
|
||||
let rad = 0.4;
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for k in 0..num {
|
||||
for i in 0..num {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(fk * shift, 0.0, fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).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));
|
||||
joints.insert(bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - num;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = BallJoint::new(Point3::origin(), Point3::new(-shift, 0.0, 0.0));
|
||||
joints.insert(bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn init_world(testbed: &mut Testbed) {
|
||||
/*
|
||||
* World
|
||||
*/
|
||||
let mut bodies = RigidBodySet::new();
|
||||
let mut colliders = ColliderSet::new();
|
||||
let mut joints = JointSet::new();
|
||||
|
||||
create_prismatic_joints(
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
&mut joints,
|
||||
Point3::new(20.0, 10.0, 0.0),
|
||||
5,
|
||||
);
|
||||
create_revolute_joints(
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
&mut joints,
|
||||
Point3::new(20.0, 0.0, 0.0),
|
||||
3,
|
||||
);
|
||||
create_fixed_joints(
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
&mut joints,
|
||||
Point3::new(0.0, 10.0, 0.0),
|
||||
5,
|
||||
);
|
||||
create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point3::new(15.0, 5.0, 42.0), Point3::new(13.0, 1.0, 1.0));
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
97
examples3d/kinematic3.rs
Normal file
97
examples3d/kinematic3.rs
Normal file
@@ -0,0 +1,97 @@
|
||||
use na::Point3;
|
||||
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 = 10.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 boxes
|
||||
*/
|
||||
let num = 6;
|
||||
let rad = 0.2;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let centerx = shift * num as f32 / 2.0;
|
||||
let centery = shift / 2.0 + 3.04;
|
||||
let centerz = shift * num as f32 / 2.0;
|
||||
|
||||
for i in 0usize..num {
|
||||
for j in 0usize..num {
|
||||
for k in 0usize..num {
|
||||
let x = i as f32 * shift - centerx;
|
||||
let y = j as f32 * shift + centery;
|
||||
let z = k as f32 * shift - centerz;
|
||||
|
||||
// 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(rad, rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Setup a kinematic rigid body.
|
||||
*/
|
||||
let platform_body = RigidBodyBuilder::new_kinematic()
|
||||
.translation(0.0, 1.5 + 0.8, -10.0 * rad)
|
||||
.build();
|
||||
let platform_handle = bodies.insert(platform_body);
|
||||
let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0)
|
||||
.density(1.0)
|
||||
.build();
|
||||
colliders.insert(collider, platform_handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Setup a callback to control the platform.
|
||||
*/
|
||||
testbed.add_callback(move |bodies, _, _, _, time| {
|
||||
let mut platform = bodies.get_mut(platform_handle).unwrap();
|
||||
let mut next_pos = platform.position;
|
||||
|
||||
let dt = 0.016;
|
||||
next_pos.translation.vector.y += (time * 5.0).sin() * dt;
|
||||
next_pos.translation.vector.z += time.sin() * 5.0 * dt;
|
||||
|
||||
if next_pos.translation.vector.z >= rad * 10.0 {
|
||||
next_pos.translation.vector.z -= dt;
|
||||
}
|
||||
if next_pos.translation.vector.z <= -rad * 10.0 {
|
||||
next_pos.translation.vector.z += dt;
|
||||
}
|
||||
|
||||
platform.set_next_kinematic_position(next_pos);
|
||||
});
|
||||
|
||||
/*
|
||||
* Run the simulation.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point3::new(-10.0, 5.0, -10.0), Point3::origin());
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Kinematic body", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
85
examples3d/pyramid3.rs
Normal file
85
examples3d/pyramid3.rs
Normal file
@@ -0,0 +1,85 @@
|
||||
use na::{Point3, Vector3};
|
||||
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
|
||||
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
|
||||
use rapier_testbed3d::Testbed;
|
||||
|
||||
fn create_pyramid(
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
offset: Vector3<f32>,
|
||||
stack_height: usize,
|
||||
half_extents: Vector3<f32>,
|
||||
) {
|
||||
let shift = half_extents * 2.5;
|
||||
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 rigid_body_handle = bodies.insert(rigid_body);
|
||||
|
||||
let collider =
|
||||
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z)
|
||||
.density(1.0)
|
||||
.build();
|
||||
colliders.insert(collider, rigid_body_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 = 50.0;
|
||||
let ground_height = 0.1;
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static()
|
||||
.translation(0.0, -ground_height, 0.0)
|
||||
.build();
|
||||
let ground_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
|
||||
colliders.insert(collider, ground_handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the cubes
|
||||
*/
|
||||
let cube_size = 1.0;
|
||||
let hext = Vector3::repeat(cube_size);
|
||||
let bottomy = cube_size;
|
||||
create_pyramid(
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
Vector3::new(0.0, bottomy, 0.0),
|
||||
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()
|
||||
}
|
||||
105
examples3d/sensor3.rs
Normal file
105
examples3d/sensor3.rs
Normal file
@@ -0,0 +1,105 @@
|
||||
use na::Point3;
|
||||
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
|
||||
use rapier3d::geometry::{ColliderBuilder, ColliderSet, Proximity};
|
||||
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 ground_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
|
||||
colliders.insert(collider, ground_handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create some boxes.
|
||||
*/
|
||||
let num = 10;
|
||||
let rad = 0.2;
|
||||
|
||||
let shift = rad * 2.0;
|
||||
let centerx = shift * num as f32 / 2.0;
|
||||
let centerz = shift * num as f32 / 2.0;
|
||||
|
||||
for i in 0usize..num {
|
||||
for k in 0usize..num {
|
||||
let x = i as f32 * shift - centerx;
|
||||
let y = 3.0;
|
||||
let z = k as f32 * shift - centerz;
|
||||
|
||||
// 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(rad, rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
testbed.set_body_color(handle, Point3::new(0.5, 0.5, 1.0));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Create a cube that will have a ball-shaped sensor attached.
|
||||
*/
|
||||
|
||||
// Rigid body so that the sensor can move.
|
||||
let sensor = RigidBodyBuilder::new_dynamic()
|
||||
.translation(0.0, 10.0, 0.0)
|
||||
.build();
|
||||
let sensor_handle = bodies.insert(sensor);
|
||||
|
||||
// Solid cube attached to the sensor which
|
||||
// other colliders can touch.
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, sensor_handle, &mut bodies);
|
||||
|
||||
// We create a collider desc without density because we don't
|
||||
// want it to contribute to the rigid body mass.
|
||||
let sensor_collider = ColliderBuilder::ball(rad * 5.0).sensor(true).build();
|
||||
colliders.insert(sensor_collider, sensor_handle, &mut bodies);
|
||||
|
||||
testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0));
|
||||
|
||||
// Callback that will be executed on the main loop to handle proximities.
|
||||
testbed.add_callback(move |_, colliders, events, graphics, _| {
|
||||
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),
|
||||
};
|
||||
|
||||
let parent_handle1 = colliders.get(prox.collider1).unwrap().parent();
|
||||
let parent_handle2 = colliders.get(prox.collider2).unwrap().parent();
|
||||
|
||||
if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
|
||||
graphics.set_body_color(parent_handle1, color);
|
||||
}
|
||||
if parent_handle2 != ground_handle && parent_handle2 != sensor_handle {
|
||||
graphics.set_body_color(parent_handle2, color);
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(Point3::new(-6.0, 4.0, -6.0), Point3::new(0.0, 1.0, 0.0));
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Sensor", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
195
examples3d/stacks3.rs
Normal file
195
examples3d/stacks3.rs
Normal file
@@ -0,0 +1,195 @@
|
||||
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)
|
||||
.density(1.0)
|
||||
.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)
|
||||
.density(1.0)
|
||||
.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)
|
||||
.density(1.0)
|
||||
.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(-110.0, bottomy, 0.0),
|
||||
12,
|
||||
hext,
|
||||
);
|
||||
create_pyramid(
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
Vector3::new(-80.0, bottomy, 0.0),
|
||||
12,
|
||||
hext,
|
||||
);
|
||||
create_pyramid(
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
Vector3::new(-50.0, bottomy, 0.0),
|
||||
12,
|
||||
hext,
|
||||
);
|
||||
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()
|
||||
}
|
||||
70
examples3d/stress_joint_ball3.rs
Normal file
70
examples3d/stress_joint_ball3.rs
Normal file
@@ -0,0 +1,70 @@
|
||||
use na::Point3;
|
||||
use rapier3d::dynamics::{BallJoint, BodyStatus, 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 mut joints = JointSet::new();
|
||||
|
||||
let rad = 0.4;
|
||||
let num = 100;
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for k in 0..num {
|
||||
for i in 0..num {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(fk * shift, 0.0, fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, child_handle, &mut 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));
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - num;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = BallJoint::new(Point3::origin(), Point3::new(-shift, 0.0, 0.0));
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(
|
||||
Point3::new(-110.0, -46.0, 170.0),
|
||||
Point3::new(54.0, -38.0, 29.0),
|
||||
);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
92
examples3d/stress_joint_fixed3.rs
Normal file
92
examples3d/stress_joint_fixed3.rs
Normal file
@@ -0,0 +1,92 @@
|
||||
use na::{Isometry3, Point3};
|
||||
use rapier3d::dynamics::{BodyStatus, FixedJoint, 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 mut joints = JointSet::new();
|
||||
|
||||
let rad = 0.4;
|
||||
let num = 5;
|
||||
let shift = 1.0;
|
||||
|
||||
let mut body_handles = Vec::new();
|
||||
|
||||
for m in 0..10 {
|
||||
let z = m as f32 * shift * (num as f32 + 2.0);
|
||||
|
||||
for l in 0..10 {
|
||||
let y = l as f32 * shift * 3.0;
|
||||
|
||||
for j in 0..5 {
|
||||
let x = j as f32 * shift * (num as f32) * 2.0;
|
||||
|
||||
for k in 0..num {
|
||||
for i in 0..num {
|
||||
let fk = k as f32;
|
||||
let fi = i as f32;
|
||||
|
||||
// NOTE: the num - 2 test is to avoid two consecutive
|
||||
// fixed bodies. Because physx will crash if we add
|
||||
// a joint between these.
|
||||
|
||||
let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
|
||||
BodyStatus::Static
|
||||
} else {
|
||||
BodyStatus::Dynamic
|
||||
};
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new(status)
|
||||
.translation(x + fk * shift, y, z + fi * shift)
|
||||
.build();
|
||||
let child_handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).build();
|
||||
colliders.insert(collider, child_handle, &mut bodies);
|
||||
|
||||
// Vertical joint.
|
||||
if i > 0 {
|
||||
let parent_handle = *body_handles.last().unwrap();
|
||||
let joint = FixedJoint::new(
|
||||
Isometry3::identity(),
|
||||
Isometry3::translation(0.0, 0.0, -shift),
|
||||
);
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
// Horizontal joint.
|
||||
if k > 0 {
|
||||
let parent_index = body_handles.len() - num;
|
||||
let parent_handle = body_handles[parent_index];
|
||||
let joint = FixedJoint::new(
|
||||
Isometry3::identity(),
|
||||
Isometry3::translation(-shift, 0.0, 0.0),
|
||||
);
|
||||
joints.insert(&mut bodies, parent_handle, child_handle, joint);
|
||||
}
|
||||
|
||||
body_handles.push(child_handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(
|
||||
Point3::new(-38.0, 14.0, 108.0),
|
||||
Point3::new(46.0, 12.0, 23.0),
|
||||
);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
81
examples3d/stress_joint_prismatic3.rs
Normal file
81
examples3d/stress_joint_prismatic3.rs
Normal file
@@ -0,0 +1,81 @@
|
||||
use na::{Point3, Unit, Vector3};
|
||||
use rapier3d::dynamics::{JointSet, PrismaticJoint, 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 mut joints = JointSet::new();
|
||||
|
||||
let rad = 0.4;
|
||||
let num = 5;
|
||||
let shift = 1.0;
|
||||
|
||||
for m in 0..8 {
|
||||
let z = m as f32 * shift * (num as f32 + 2.0);
|
||||
|
||||
for l in 0..8 {
|
||||
let y = l as f32 * shift * (num as f32) * 2.0;
|
||||
|
||||
for j in 0..50 {
|
||||
let x = j as f32 * shift * 4.0;
|
||||
|
||||
let ground = RigidBodyBuilder::new_static().translation(x, y, z).build();
|
||||
let mut curr_parent = bodies.insert(ground);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
|
||||
colliders.insert(collider, curr_parent, &mut bodies);
|
||||
|
||||
for i in 0..num {
|
||||
let z = z + (i + 1) as f32 * shift;
|
||||
let density = 1.0;
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
|
||||
let curr_child = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad)
|
||||
.density(density)
|
||||
.build();
|
||||
colliders.insert(collider, curr_child, &mut bodies);
|
||||
|
||||
let axis = if i % 2 == 0 {
|
||||
Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
|
||||
} else {
|
||||
Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
|
||||
};
|
||||
|
||||
let z = Vector3::z();
|
||||
let mut prism = PrismaticJoint::new(
|
||||
Point3::origin(),
|
||||
axis,
|
||||
z,
|
||||
Point3::new(0.0, 0.0, -shift),
|
||||
axis,
|
||||
z,
|
||||
);
|
||||
prism.limits_enabled = true;
|
||||
prism.limits[0] = -2.0;
|
||||
prism.limits[1] = 2.0;
|
||||
joints.insert(&mut bodies, curr_parent, curr_child, prism);
|
||||
|
||||
curr_parent = curr_child;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(
|
||||
Point3::new(262.0, 63.0, 124.0),
|
||||
Point3::new(101.0, 4.0, -3.0),
|
||||
);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
89
examples3d/stress_joint_revolute3.rs
Normal file
89
examples3d/stress_joint_revolute3.rs
Normal file
@@ -0,0 +1,89 @@
|
||||
use na::{Isometry3, Point3, Vector3};
|
||||
use rapier3d::dynamics::{JointSet, RevoluteJoint, 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 mut joints = JointSet::new();
|
||||
|
||||
let rad = 0.4;
|
||||
let num = 10;
|
||||
let shift = 2.0;
|
||||
|
||||
for l in 0..4 {
|
||||
let y = l as f32 * shift * (num as f32) * 3.0;
|
||||
|
||||
for j in 0..50 {
|
||||
let x = j as f32 * shift * 4.0;
|
||||
|
||||
let ground = RigidBodyBuilder::new_static()
|
||||
.translation(x, y, 0.0)
|
||||
.build();
|
||||
let mut curr_parent = bodies.insert(ground);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
|
||||
colliders.insert(collider, curr_parent, &mut bodies);
|
||||
|
||||
for i in 0..num {
|
||||
// Create four bodies.
|
||||
let z = i as f32 * shift * 2.0 + shift;
|
||||
let positions = [
|
||||
Isometry3::translation(x, y, z),
|
||||
Isometry3::translation(x + shift, y, z),
|
||||
Isometry3::translation(x + shift, y, z + shift),
|
||||
Isometry3::translation(x, y, z + shift),
|
||||
];
|
||||
|
||||
let mut handles = [curr_parent; 4];
|
||||
for k in 0..4 {
|
||||
let density = 1.0;
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.position(positions[k])
|
||||
.build();
|
||||
handles[k] = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad)
|
||||
.density(density)
|
||||
.build();
|
||||
colliders.insert(collider, handles[k], &mut bodies);
|
||||
}
|
||||
|
||||
// Setup four joints.
|
||||
let o = Point3::origin();
|
||||
let x = Vector3::x_axis();
|
||||
let z = Vector3::z_axis();
|
||||
|
||||
let revs = [
|
||||
RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
|
||||
RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x),
|
||||
RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
|
||||
RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x),
|
||||
];
|
||||
|
||||
joints.insert(&mut bodies, curr_parent, handles[0], revs[0]);
|
||||
joints.insert(&mut bodies, handles[0], handles[1], revs[1]);
|
||||
joints.insert(&mut bodies, handles[1], handles[2], revs[2]);
|
||||
joints.insert(&mut bodies, handles[2], handles[3], revs[3]);
|
||||
|
||||
curr_parent = handles[3];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Set up the testbed.
|
||||
*/
|
||||
testbed.set_world(bodies, colliders, joints);
|
||||
testbed.look_at(
|
||||
Point3::new(478.0, 83.0, 228.0),
|
||||
Point3::new(134.0, 83.0, -116.0),
|
||||
);
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
148
examples3d/stress_keva3.rs
Normal file
148
examples3d/stress_keva3.rs
Normal file
@@ -0,0 +1,148 @@
|
||||
use na::{Point3, Vector3};
|
||||
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
|
||||
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
|
||||
use rapier_testbed3d::Testbed;
|
||||
|
||||
pub fn build_block(
|
||||
testbed: &mut Testbed,
|
||||
bodies: &mut RigidBodySet,
|
||||
colliders: &mut ColliderSet,
|
||||
half_extents: Vector3<f32>,
|
||||
shift: Vector3<f32>,
|
||||
mut numx: usize,
|
||||
numy: usize,
|
||||
mut numz: usize,
|
||||
) {
|
||||
let dimensions = [half_extents.xyz(), half_extents.zyx()];
|
||||
let block_width = 2.0 * half_extents.z * numx as f32;
|
||||
let block_height = 2.0 * half_extents.y * numy as f32;
|
||||
let spacing = (half_extents.z * numx as f32 - half_extents.x) / (numz as f32 - 1.0);
|
||||
let mut color0 = Point3::new(0.7, 0.5, 0.9);
|
||||
let mut color1 = Point3::new(0.6, 1.0, 0.6);
|
||||
|
||||
for i in 0..numy {
|
||||
std::mem::swap(&mut numx, &mut numz);
|
||||
let dim = dimensions[i % 2];
|
||||
let y = dim.y * i as f32 * 2.0;
|
||||
|
||||
for j in 0..numx {
|
||||
let x = if i % 2 == 0 {
|
||||
spacing * j as f32 * 2.0
|
||||
} else {
|
||||
dim.x * j as f32 * 2.0
|
||||
};
|
||||
|
||||
for k in 0..numz {
|
||||
let z = if i % 2 == 0 {
|
||||
dim.z * k as f32 * 2.0
|
||||
} else {
|
||||
spacing * k as f32 * 2.0
|
||||
};
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.translation(
|
||||
x + dim.x + shift.x,
|
||||
y + dim.y + shift.y,
|
||||
z + dim.z + shift.z,
|
||||
)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z)
|
||||
.density(1.0)
|
||||
.build();
|
||||
colliders.insert(collider, handle, bodies);
|
||||
|
||||
testbed.set_body_color(handle, color0);
|
||||
std::mem::swap(&mut color0, &mut color1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Close the top.
|
||||
let dim = half_extents.zxy();
|
||||
|
||||
for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize {
|
||||
for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic()
|
||||
.translation(
|
||||
i as f32 * dim.x * 2.0 + dim.x + shift.x,
|
||||
dim.y + shift.y + block_height,
|
||||
j as f32 * dim.z * 2.0 + dim.z + shift.z,
|
||||
)
|
||||
.build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z)
|
||||
.density(1.0)
|
||||
.build();
|
||||
colliders.insert(collider, handle, bodies);
|
||||
testbed.set_body_color(handle, color0);
|
||||
std::mem::swap(&mut color0, &mut color1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 cubes
|
||||
*/
|
||||
let half_extents = Vector3::new(0.02, 0.1, 0.4) / 2.0 * 10.0;
|
||||
let mut block_height = 0.0;
|
||||
// These should only be set to odd values otherwise
|
||||
// the blocks won't align in the nicest way.
|
||||
let numy = [0, 9, 13, 17, 21, 41];
|
||||
let mut num_blocks_built = 0;
|
||||
|
||||
for i in (1..=5).rev() {
|
||||
let numx = i;
|
||||
let numy = numy[i];
|
||||
let numz = numx * 3 + 1;
|
||||
let block_width = numx as f32 * half_extents.z * 2.0;
|
||||
build_block(
|
||||
testbed,
|
||||
&mut bodies,
|
||||
&mut colliders,
|
||||
half_extents,
|
||||
Vector3::new(-block_width / 2.0, block_height, -block_width / 2.0),
|
||||
numx,
|
||||
numy,
|
||||
numz,
|
||||
);
|
||||
block_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0;
|
||||
num_blocks_built += numx * numy * numz;
|
||||
}
|
||||
|
||||
println!("Num keva blocks: {}", num_blocks_built);
|
||||
|
||||
/*
|
||||
* 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()
|
||||
}
|
||||
88
examples3d/trimesh3.rs
Normal file
88
examples3d/trimesh3.rs
Normal file
@@ -0,0 +1,88 @@
|
||||
use na::Point3;
|
||||
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.0f32;
|
||||
let ground_height = 1.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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
let rigid_body = RigidBodyBuilder::new_static().build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
let collider = ColliderBuilder::trimesh(vertices, indices).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
|
||||
/*
|
||||
* Create the cubes
|
||||
*/
|
||||
let num = 8;
|
||||
let rad = 1.0;
|
||||
|
||||
let shift = rad * 2.0 + rad;
|
||||
let centerx = shift * (num / 2) as f32;
|
||||
let centery = shift / 2.0;
|
||||
let centerz = shift * (num / 2) as f32;
|
||||
|
||||
for j in 0usize..47 {
|
||||
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;
|
||||
|
||||
// Build the rigid body.
|
||||
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
|
||||
let handle = bodies.insert(rigid_body);
|
||||
|
||||
if j % 2 == 0 {
|
||||
let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
|
||||
colliders.insert(collider, handle, &mut bodies);
|
||||
} else {
|
||||
let collider = ColliderBuilder::ball(rad).density(1.0).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 main() {
|
||||
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
|
||||
testbed.run()
|
||||
}
|
||||
49
src/counters/ccd_counters.rs
Normal file
49
src/counters/ccd_counters.rs
Normal file
@@ -0,0 +1,49 @@
|
||||
use crate::counters::Timer;
|
||||
use std::fmt::{Display, Formatter, Result};
|
||||
|
||||
/// Performance counters related to continuous collision detection (CCD).
|
||||
#[derive(Default, Clone, Copy)]
|
||||
pub struct CCDCounters {
|
||||
/// The number of substeps actually performed by the CCD resolution.
|
||||
pub num_substeps: usize,
|
||||
/// The total time spent for TOI computation in the CCD resolution.
|
||||
pub toi_computation_time: Timer,
|
||||
/// The total time spent for force computation and integration in the CCD resolution.
|
||||
pub solver_time: Timer,
|
||||
/// The total time spent by the broad-phase in the CCD resolution.
|
||||
pub broad_phase_time: Timer,
|
||||
/// The total time spent by the narrow-phase in the CCD resolution.
|
||||
pub narrow_phase_time: Timer,
|
||||
}
|
||||
|
||||
impl CCDCounters {
|
||||
/// Creates a new counter initialized to zero.
|
||||
pub fn new() -> Self {
|
||||
CCDCounters {
|
||||
num_substeps: 0,
|
||||
toi_computation_time: Timer::new(),
|
||||
solver_time: Timer::new(),
|
||||
broad_phase_time: Timer::new(),
|
||||
narrow_phase_time: Timer::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Resets this counter to 0.
|
||||
pub fn reset(&mut self) {
|
||||
self.num_substeps = 0;
|
||||
self.toi_computation_time.reset();
|
||||
self.solver_time.reset();
|
||||
self.broad_phase_time.reset();
|
||||
self.narrow_phase_time.reset();
|
||||
}
|
||||
}
|
||||
|
||||
impl Display for CCDCounters {
|
||||
fn fmt(&self, f: &mut Formatter) -> Result {
|
||||
writeln!(f, "Number of substeps: {}", self.num_substeps)?;
|
||||
writeln!(f, "TOI computation time: {}", self.toi_computation_time)?;
|
||||
writeln!(f, "Constraints solver time: {}", self.solver_time)?;
|
||||
writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
|
||||
writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
|
||||
}
|
||||
}
|
||||
32
src/counters/collision_detection_counters.rs
Normal file
32
src/counters/collision_detection_counters.rs
Normal file
@@ -0,0 +1,32 @@
|
||||
use crate::counters::Timer;
|
||||
use std::fmt::{Display, Formatter, Result};
|
||||
|
||||
/// Performance counters related to collision detection.
|
||||
#[derive(Default, Clone, Copy)]
|
||||
pub struct CollisionDetectionCounters {
|
||||
/// Number of contact pairs detected.
|
||||
pub ncontact_pairs: usize,
|
||||
/// Time spent for the broad-phase of the collision detection.
|
||||
pub broad_phase_time: Timer,
|
||||
/// Time spent for the narrow-phase of the collision detection.
|
||||
pub narrow_phase_time: Timer,
|
||||
}
|
||||
|
||||
impl CollisionDetectionCounters {
|
||||
/// Creates a new counter initialized to zero.
|
||||
pub fn new() -> Self {
|
||||
CollisionDetectionCounters {
|
||||
ncontact_pairs: 0,
|
||||
broad_phase_time: Timer::new(),
|
||||
narrow_phase_time: Timer::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Display for CollisionDetectionCounters {
|
||||
fn fmt(&self, f: &mut Formatter) -> Result {
|
||||
writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?;
|
||||
writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
|
||||
writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
|
||||
}
|
||||
}
|
||||
225
src/counters/mod.rs
Normal file
225
src/counters/mod.rs
Normal file
@@ -0,0 +1,225 @@
|
||||
//! Counters for benchmarking various parts of the physics engine.
|
||||
|
||||
use std::fmt::{Display, Formatter, Result};
|
||||
|
||||
pub use self::ccd_counters::CCDCounters;
|
||||
pub use self::collision_detection_counters::CollisionDetectionCounters;
|
||||
pub use self::solver_counters::SolverCounters;
|
||||
pub use self::stages_counters::StagesCounters;
|
||||
pub use self::timer::Timer;
|
||||
|
||||
mod ccd_counters;
|
||||
mod collision_detection_counters;
|
||||
mod solver_counters;
|
||||
mod stages_counters;
|
||||
mod timer;
|
||||
|
||||
/// Aggregation of all the performances counters tracked by nphysics.
|
||||
#[derive(Clone, Copy)]
|
||||
pub struct Counters {
|
||||
/// Whether thi counter is enabled or not.
|
||||
pub enabled: bool,
|
||||
/// Timer for a whole timestep.
|
||||
pub step_time: Timer,
|
||||
/// Timer used for debugging.
|
||||
pub custom: Timer,
|
||||
/// Counters of every satge of one time step.
|
||||
pub stages: StagesCounters,
|
||||
/// Counters of the collision-detection stage.
|
||||
pub cd: CollisionDetectionCounters,
|
||||
/// Counters of the constraints resolution and force computation stage.
|
||||
pub solver: SolverCounters,
|
||||
/// Counters for the CCD resolution stage.
|
||||
pub ccd: CCDCounters,
|
||||
}
|
||||
|
||||
impl Counters {
|
||||
/// Create a new set of counters initialized to wero.
|
||||
pub fn new(enabled: bool) -> Self {
|
||||
Counters {
|
||||
enabled,
|
||||
step_time: Timer::new(),
|
||||
custom: Timer::new(),
|
||||
stages: StagesCounters::new(),
|
||||
cd: CollisionDetectionCounters::new(),
|
||||
solver: SolverCounters::new(),
|
||||
ccd: CCDCounters::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Enable all the counters.
|
||||
pub fn enable(&mut self) {
|
||||
self.enabled = true;
|
||||
}
|
||||
|
||||
/// Return `true` if the counters are enabled.
|
||||
pub fn enabled(&self) -> bool {
|
||||
self.enabled
|
||||
}
|
||||
|
||||
/// Disable all the counters.
|
||||
pub fn disable(&mut self) {
|
||||
self.enabled = false;
|
||||
}
|
||||
|
||||
/// Notify that the time-step has started.
|
||||
pub fn step_started(&mut self) {
|
||||
if self.enabled {
|
||||
self.step_time.start();
|
||||
}
|
||||
}
|
||||
|
||||
/// Notfy that the time-step has finished.
|
||||
pub fn step_completed(&mut self) {
|
||||
if self.enabled {
|
||||
self.step_time.pause();
|
||||
}
|
||||
}
|
||||
|
||||
/// Total time spent for one of the physics engine.
|
||||
pub fn step_time(&self) -> f64 {
|
||||
self.step_time.time()
|
||||
}
|
||||
|
||||
/// Notify that the custom operation has started.
|
||||
pub fn custom_started(&mut self) {
|
||||
if self.enabled {
|
||||
self.custom.start();
|
||||
}
|
||||
}
|
||||
|
||||
/// Notfy that the custom operation has finished.
|
||||
pub fn custom_completed(&mut self) {
|
||||
if self.enabled {
|
||||
self.custom.pause();
|
||||
}
|
||||
}
|
||||
|
||||
/// Total time of a custom event.
|
||||
pub fn custom_time(&self) -> f64 {
|
||||
self.custom.time()
|
||||
}
|
||||
|
||||
/// Set the number of constraints generated.
|
||||
pub fn set_nconstraints(&mut self, n: usize) {
|
||||
self.solver.nconstraints = n;
|
||||
}
|
||||
|
||||
/// Set the number of contacts generated.
|
||||
pub fn set_ncontacts(&mut self, n: usize) {
|
||||
self.solver.ncontacts = n;
|
||||
}
|
||||
|
||||
/// Set the number of contact pairs generated.
|
||||
pub fn set_ncontact_pairs(&mut self, n: usize) {
|
||||
self.cd.ncontact_pairs = n;
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! measure_method {
|
||||
($started:ident, $stopped:ident, $time:ident, $info:ident. $timer:ident) => {
|
||||
impl Counters {
|
||||
/// Start this timer.
|
||||
pub fn $started(&mut self) {
|
||||
if self.enabled {
|
||||
self.$info.$timer.start()
|
||||
}
|
||||
}
|
||||
|
||||
/// Stop this timer.
|
||||
pub fn $stopped(&mut self) {
|
||||
if self.enabled {
|
||||
self.$info.$timer.pause()
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets the time elapsed for this timer.
|
||||
pub fn $time(&self) -> f64 {
|
||||
if self.enabled {
|
||||
self.$info.$timer.time()
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
measure_method!(
|
||||
update_started,
|
||||
update_completed,
|
||||
update_time,
|
||||
stages.update_time
|
||||
);
|
||||
measure_method!(
|
||||
collision_detection_started,
|
||||
collision_detection_completed,
|
||||
collision_detection_time,
|
||||
stages.collision_detection_time
|
||||
);
|
||||
measure_method!(
|
||||
island_construction_started,
|
||||
island_construction_completed,
|
||||
island_construction_time,
|
||||
stages.island_construction_time
|
||||
);
|
||||
measure_method!(
|
||||
solver_started,
|
||||
solver_completed,
|
||||
solver_time,
|
||||
stages.solver_time
|
||||
);
|
||||
measure_method!(ccd_started, ccd_completed, ccd_time, stages.ccd_time);
|
||||
|
||||
measure_method!(
|
||||
assembly_started,
|
||||
assembly_completed,
|
||||
assembly_time,
|
||||
solver.velocity_assembly_time
|
||||
);
|
||||
measure_method!(
|
||||
velocity_resolution_started,
|
||||
velocity_resolution_completed,
|
||||
velocity_resolution_time,
|
||||
solver.velocity_resolution_time
|
||||
);
|
||||
measure_method!(
|
||||
velocity_update_started,
|
||||
velocity_update_completed,
|
||||
velocity_update_time,
|
||||
solver.velocity_update_time
|
||||
);
|
||||
measure_method!(
|
||||
position_resolution_started,
|
||||
position_resolution_completed,
|
||||
position_resolution_time,
|
||||
solver.position_resolution_time
|
||||
);
|
||||
measure_method!(
|
||||
broad_phase_started,
|
||||
broad_phase_completed,
|
||||
broad_phase_time,
|
||||
cd.broad_phase_time
|
||||
);
|
||||
measure_method!(
|
||||
narrow_phase_started,
|
||||
narrow_phase_completed,
|
||||
narrow_phase_time,
|
||||
cd.narrow_phase_time
|
||||
);
|
||||
|
||||
impl Display for Counters {
|
||||
fn fmt(&self, f: &mut Formatter) -> Result {
|
||||
writeln!(f, "Total timestep time: {}", self.step_time)?;
|
||||
self.stages.fmt(f)?;
|
||||
self.cd.fmt(f)?;
|
||||
self.solver.fmt(f)?;
|
||||
writeln!(f, "Custom timer: {}", self.custom)
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for Counters {
|
||||
fn default() -> Self {
|
||||
Self::new(false)
|
||||
}
|
||||
}
|
||||
67
src/counters/solver_counters.rs
Normal file
67
src/counters/solver_counters.rs
Normal file
@@ -0,0 +1,67 @@
|
||||
use crate::counters::Timer;
|
||||
use std::fmt::{Display, Formatter, Result};
|
||||
|
||||
/// Performance counters related to constraints resolution.
|
||||
#[derive(Default, Clone, Copy)]
|
||||
pub struct SolverCounters {
|
||||
/// Number of constraints generated.
|
||||
pub nconstraints: usize,
|
||||
/// Number of contacts found.
|
||||
pub ncontacts: usize,
|
||||
/// Time spent for the resolution of the constraints (force computation).
|
||||
pub velocity_resolution_time: Timer,
|
||||
/// Time spent for the assembly of all the velocity constraints.
|
||||
pub velocity_assembly_time: Timer,
|
||||
/// Time spent for the update of the velocity of the bodies.
|
||||
pub velocity_update_time: Timer,
|
||||
/// Time spent for the assembly of all the position constraints.
|
||||
pub position_assembly_time: Timer,
|
||||
/// Time spent for the update of the position of the bodies.
|
||||
pub position_resolution_time: Timer,
|
||||
}
|
||||
|
||||
impl SolverCounters {
|
||||
/// Creates a new counter initialized to zero.
|
||||
pub fn new() -> Self {
|
||||
SolverCounters {
|
||||
nconstraints: 0,
|
||||
ncontacts: 0,
|
||||
velocity_assembly_time: Timer::new(),
|
||||
velocity_resolution_time: Timer::new(),
|
||||
velocity_update_time: Timer::new(),
|
||||
position_assembly_time: Timer::new(),
|
||||
position_resolution_time: Timer::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Reset all the counters to zero.
|
||||
pub fn reset(&mut self) {
|
||||
self.nconstraints = 0;
|
||||
self.ncontacts = 0;
|
||||
self.velocity_resolution_time.reset();
|
||||
self.velocity_assembly_time.reset();
|
||||
self.velocity_update_time.reset();
|
||||
self.position_assembly_time.reset();
|
||||
self.position_resolution_time.reset();
|
||||
}
|
||||
}
|
||||
|
||||
impl Display for SolverCounters {
|
||||
fn fmt(&self, f: &mut Formatter) -> Result {
|
||||
writeln!(f, "Number of contacts: {}", self.ncontacts)?;
|
||||
writeln!(f, "Number of constraints: {}", self.nconstraints)?;
|
||||
writeln!(f, "Velocity assembly time: {}", self.velocity_assembly_time)?;
|
||||
writeln!(
|
||||
f,
|
||||
"Velocity resolution time: {}",
|
||||
self.velocity_resolution_time
|
||||
)?;
|
||||
writeln!(f, "Velocity update time: {}", self.velocity_update_time)?;
|
||||
writeln!(f, "Position assembly time: {}", self.position_assembly_time)?;
|
||||
writeln!(
|
||||
f,
|
||||
"Position resolution time: {}",
|
||||
self.position_resolution_time
|
||||
)
|
||||
}
|
||||
}
|
||||
48
src/counters/stages_counters.rs
Normal file
48
src/counters/stages_counters.rs
Normal file
@@ -0,0 +1,48 @@
|
||||
use crate::counters::Timer;
|
||||
use std::fmt::{Display, Formatter, Result};
|
||||
|
||||
/// Performance counters related to each stage of the time step.
|
||||
#[derive(Default, Clone, Copy)]
|
||||
pub struct StagesCounters {
|
||||
/// Time spent for updating the kinematic and dynamics of every body.
|
||||
pub update_time: Timer,
|
||||
/// Total time spent for the collision detection (including both broad- and narrow- phases).
|
||||
pub collision_detection_time: Timer,
|
||||
/// Time spent for the computation of collision island and body activation/deactivation (sleeping).
|
||||
pub island_construction_time: Timer,
|
||||
/// Total time spent for the constraints resolution and position update.t
|
||||
pub solver_time: Timer,
|
||||
/// Total time spent for CCD and CCD resolution.
|
||||
pub ccd_time: Timer,
|
||||
}
|
||||
|
||||
impl StagesCounters {
|
||||
/// Create a new counter intialized to zero.
|
||||
pub fn new() -> Self {
|
||||
StagesCounters {
|
||||
update_time: Timer::new(),
|
||||
collision_detection_time: Timer::new(),
|
||||
island_construction_time: Timer::new(),
|
||||
solver_time: Timer::new(),
|
||||
ccd_time: Timer::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Display for StagesCounters {
|
||||
fn fmt(&self, f: &mut Formatter) -> Result {
|
||||
writeln!(f, "Update time: {}", self.update_time)?;
|
||||
writeln!(
|
||||
f,
|
||||
"Collision detection time: {}",
|
||||
self.collision_detection_time
|
||||
)?;
|
||||
writeln!(
|
||||
f,
|
||||
"Island construction time: {}",
|
||||
self.island_construction_time
|
||||
)?;
|
||||
writeln!(f, "Solver time: {}", self.solver_time)?;
|
||||
writeln!(f, "CCD time: {}", self.ccd_time)
|
||||
}
|
||||
}
|
||||
53
src/counters/timer.rs
Normal file
53
src/counters/timer.rs
Normal file
@@ -0,0 +1,53 @@
|
||||
use std::fmt::{Display, Error, Formatter};
|
||||
|
||||
/// A timer.
|
||||
#[derive(Copy, Clone, Debug, Default)]
|
||||
pub struct Timer {
|
||||
time: f64,
|
||||
start: Option<f64>,
|
||||
}
|
||||
|
||||
impl Timer {
|
||||
/// Creates a new timer initialized to zero and not started.
|
||||
pub fn new() -> Self {
|
||||
Timer {
|
||||
time: 0.0,
|
||||
start: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Resets the timer to 0.
|
||||
pub fn reset(&mut self) {
|
||||
self.time = 0.0
|
||||
}
|
||||
|
||||
/// Start the timer.
|
||||
pub fn start(&mut self) {
|
||||
self.time = 0.0;
|
||||
self.start = Some(instant::now());
|
||||
}
|
||||
|
||||
/// Pause the timer.
|
||||
pub fn pause(&mut self) {
|
||||
if let Some(start) = self.start {
|
||||
self.time += instant::now() - start;
|
||||
}
|
||||
self.start = None;
|
||||
}
|
||||
|
||||
/// Resume the timer.
|
||||
pub fn resume(&mut self) {
|
||||
self.start = Some(instant::now());
|
||||
}
|
||||
|
||||
/// The measured time between the last `.start()` and `.pause()` calls.
|
||||
pub fn time(&self) -> f64 {
|
||||
self.time
|
||||
}
|
||||
}
|
||||
|
||||
impl Display for Timer {
|
||||
fn fmt(&self, f: &mut Formatter) -> Result<(), Error> {
|
||||
write!(f, "{}s", self.time)
|
||||
}
|
||||
}
|
||||
1159
src/data/arena.rs
Normal file
1159
src/data/arena.rs
Normal file
File diff suppressed because it is too large
Load Diff
830
src/data/graph.rs
Normal file
830
src/data/graph.rs
Normal file
@@ -0,0 +1,830 @@
|
||||
// This is basically a stripped down version of petgraph's UnGraph.
|
||||
// - It is not generic wrt. the index type (we always use u32).
|
||||
// - It preserves associated edge iteration order after Serialization/Deserialization.
|
||||
// - It is always undirected.
|
||||
//! A stripped-down version of petgraph's UnGraph.
|
||||
|
||||
use std::cmp::max;
|
||||
use std::ops::{Index, IndexMut};
|
||||
|
||||
/// Node identifier.
|
||||
#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct NodeIndex(u32);
|
||||
|
||||
impl NodeIndex {
|
||||
#[inline]
|
||||
pub fn new(x: u32) -> Self {
|
||||
NodeIndex(x)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn index(self) -> usize {
|
||||
self.0 as usize
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn end() -> Self {
|
||||
NodeIndex(crate::INVALID_U32)
|
||||
}
|
||||
|
||||
fn _into_edge(self) -> EdgeIndex {
|
||||
EdgeIndex(self.0)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<u32> for NodeIndex {
|
||||
fn from(ix: u32) -> Self {
|
||||
NodeIndex(ix)
|
||||
}
|
||||
}
|
||||
|
||||
/// Edge identifier.
|
||||
#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct EdgeIndex(u32);
|
||||
|
||||
impl EdgeIndex {
|
||||
#[inline]
|
||||
pub fn new(x: u32) -> Self {
|
||||
EdgeIndex(x)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn index(self) -> usize {
|
||||
self.0 as usize
|
||||
}
|
||||
|
||||
/// An invalid `EdgeIndex` used to denote absence of an edge, for example
|
||||
/// to end an adjacency list.
|
||||
#[inline]
|
||||
pub fn end() -> Self {
|
||||
EdgeIndex(crate::INVALID_U32)
|
||||
}
|
||||
|
||||
fn _into_node(self) -> NodeIndex {
|
||||
NodeIndex(self.0)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<u32> for EdgeIndex {
|
||||
fn from(ix: u32) -> Self {
|
||||
EdgeIndex(ix)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub enum Direction {
|
||||
Outgoing = 0,
|
||||
Incoming = 1,
|
||||
}
|
||||
|
||||
impl Direction {
|
||||
fn opposite(self) -> Direction {
|
||||
match self {
|
||||
Direction::Outgoing => Direction::Incoming,
|
||||
Direction::Incoming => Direction::Outgoing,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const DIRECTIONS: [Direction; 2] = [Direction::Outgoing, Direction::Incoming];
|
||||
|
||||
/// The graph's node type.
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct Node<N> {
|
||||
/// Associated node data.
|
||||
pub weight: N,
|
||||
/// Next edge in outgoing and incoming edge lists.
|
||||
next: [EdgeIndex; 2],
|
||||
}
|
||||
|
||||
/// The graph's edge type.
|
||||
#[derive(Debug, Copy, Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct Edge<E> {
|
||||
/// Associated edge data.
|
||||
pub weight: E,
|
||||
/// Next edge in outgoing and incoming edge lists.
|
||||
next: [EdgeIndex; 2],
|
||||
/// Start and End node index
|
||||
node: [NodeIndex; 2],
|
||||
}
|
||||
|
||||
impl<E> Edge<E> {
|
||||
/// Return the source node index.
|
||||
pub fn source(&self) -> NodeIndex {
|
||||
self.node[0]
|
||||
}
|
||||
|
||||
/// Return the target node index.
|
||||
pub fn target(&self) -> NodeIndex {
|
||||
self.node[1]
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct Graph<N, E> {
|
||||
pub(crate) nodes: Vec<Node<N>>,
|
||||
pub(crate) edges: Vec<Edge<E>>,
|
||||
}
|
||||
|
||||
enum Pair<T> {
|
||||
Both(T, T),
|
||||
One(T),
|
||||
None,
|
||||
}
|
||||
|
||||
/// Get mutable references at index `a` and `b`.
|
||||
fn index_twice<T>(arr: &mut [T], a: usize, b: usize) -> Pair<&mut T> {
|
||||
if max(a, b) >= arr.len() {
|
||||
Pair::None
|
||||
} else if a == b {
|
||||
Pair::One(&mut arr[max(a, b)])
|
||||
} else {
|
||||
// safe because a, b are in bounds and distinct
|
||||
unsafe {
|
||||
let ar = &mut *(arr.get_unchecked_mut(a) as *mut _);
|
||||
let br = &mut *(arr.get_unchecked_mut(b) as *mut _);
|
||||
Pair::Both(ar, br)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<N, E> Graph<N, E> {
|
||||
/// Create a new `Graph` with estimated capacity.
|
||||
pub fn with_capacity(nodes: usize, edges: usize) -> Self {
|
||||
Graph {
|
||||
nodes: Vec::with_capacity(nodes),
|
||||
edges: Vec::with_capacity(edges),
|
||||
}
|
||||
}
|
||||
|
||||
/// Add a node (also called vertex) with associated data `weight` to the graph.
|
||||
///
|
||||
/// Computes in **O(1)** time.
|
||||
///
|
||||
/// Return the index of the new node.
|
||||
///
|
||||
/// **Panics** if the Graph is at the maximum number of nodes for its index
|
||||
/// type (N/A if usize).
|
||||
pub fn add_node(&mut self, weight: N) -> NodeIndex {
|
||||
let node = Node {
|
||||
weight,
|
||||
next: [EdgeIndex::end(), EdgeIndex::end()],
|
||||
};
|
||||
assert!(self.nodes.len() != crate::INVALID_USIZE);
|
||||
let node_idx = NodeIndex::new(self.nodes.len() as u32);
|
||||
self.nodes.push(node);
|
||||
node_idx
|
||||
}
|
||||
|
||||
/// Access the weight for node `a`.
|
||||
///
|
||||
/// Also available with indexing syntax: `&graph[a]`.
|
||||
pub fn node_weight(&self, a: NodeIndex) -> Option<&N> {
|
||||
self.nodes.get(a.index()).map(|n| &n.weight)
|
||||
}
|
||||
|
||||
/// Access the weight for edge `a`.
|
||||
///
|
||||
/// Also available with indexing syntax: `&graph[a]`.
|
||||
pub fn edge_weight(&self, a: EdgeIndex) -> Option<&E> {
|
||||
self.edges.get(a.index()).map(|e| &e.weight)
|
||||
}
|
||||
|
||||
/// Access the weight for edge `a` mutably.
|
||||
///
|
||||
/// Also available with indexing syntax: `&mut graph[a]`.
|
||||
pub fn edge_weight_mut(&mut self, a: EdgeIndex) -> Option<&mut E> {
|
||||
self.edges.get_mut(a.index()).map(|e| &mut e.weight)
|
||||
}
|
||||
|
||||
/// Add an edge from `a` to `b` to the graph, with its associated
|
||||
/// data `weight`.
|
||||
///
|
||||
/// Return the index of the new edge.
|
||||
///
|
||||
/// Computes in **O(1)** time.
|
||||
///
|
||||
/// **Panics** if any of the nodes don't exist.<br>
|
||||
/// **Panics** if the Graph is at the maximum number of edges for its index
|
||||
/// type (N/A if usize).
|
||||
///
|
||||
/// **Note:** `Graph` allows adding parallel (“duplicate”) edges. If you want
|
||||
/// to avoid this, use [`.update_edge(a, b, weight)`](#method.update_edge) instead.
|
||||
pub fn add_edge(&mut self, a: NodeIndex, b: NodeIndex, weight: E) -> EdgeIndex {
|
||||
assert!(self.edges.len() != crate::INVALID_USIZE);
|
||||
let edge_idx = EdgeIndex::new(self.edges.len() as u32);
|
||||
let mut edge = Edge {
|
||||
weight,
|
||||
node: [a, b],
|
||||
next: [EdgeIndex::end(); 2],
|
||||
};
|
||||
match index_twice(&mut self.nodes, a.index(), b.index()) {
|
||||
Pair::None => panic!("Graph::add_edge: node indices out of bounds"),
|
||||
Pair::One(an) => {
|
||||
edge.next = an.next;
|
||||
an.next[0] = edge_idx;
|
||||
an.next[1] = edge_idx;
|
||||
}
|
||||
Pair::Both(an, bn) => {
|
||||
// a and b are different indices
|
||||
edge.next = [an.next[0], bn.next[1]];
|
||||
an.next[0] = edge_idx;
|
||||
bn.next[1] = edge_idx;
|
||||
}
|
||||
}
|
||||
self.edges.push(edge);
|
||||
edge_idx
|
||||
}
|
||||
|
||||
/// Access the source and target nodes for `e`.
|
||||
pub fn edge_endpoints(&self, e: EdgeIndex) -> Option<(NodeIndex, NodeIndex)> {
|
||||
self.edges
|
||||
.get(e.index())
|
||||
.map(|ed| (ed.source(), ed.target()))
|
||||
}
|
||||
|
||||
/// Remove `a` from the graph if it exists, and return its weight.
|
||||
/// If it doesn't exist in the graph, return `None`.
|
||||
///
|
||||
/// Apart from `a`, this invalidates the last node index in the graph
|
||||
/// (that node will adopt the removed node index). Edge indices are
|
||||
/// invalidated as they would be following the removal of each edge
|
||||
/// with an endpoint in `a`.
|
||||
///
|
||||
/// Computes in **O(e')** time, where **e'** is the number of affected
|
||||
/// edges, including *n* calls to `.remove_edge()` where *n* is the number
|
||||
/// of edges with an endpoint in `a`, and including the edges with an
|
||||
/// endpoint in the displaced node.
|
||||
pub fn remove_node(&mut self, a: NodeIndex) -> Option<N> {
|
||||
self.nodes.get(a.index())?;
|
||||
for d in &DIRECTIONS {
|
||||
let k = *d as usize;
|
||||
|
||||
// Remove all edges from and to this node.
|
||||
loop {
|
||||
let next = self.nodes[a.index()].next[k];
|
||||
if next == EdgeIndex::end() {
|
||||
break;
|
||||
}
|
||||
let ret = self.remove_edge(next);
|
||||
debug_assert!(ret.is_some());
|
||||
let _ = ret;
|
||||
}
|
||||
}
|
||||
|
||||
// Use swap_remove -- only the swapped-in node is going to change
|
||||
// NodeIndex, so we only have to walk its edges and update them.
|
||||
|
||||
let node = self.nodes.swap_remove(a.index());
|
||||
|
||||
// Find the edge lists of the node that had to relocate.
|
||||
// It may be that no node had to relocate, then we are done already.
|
||||
let swap_edges = match self.nodes.get(a.index()) {
|
||||
None => return Some(node.weight),
|
||||
Some(ed) => ed.next,
|
||||
};
|
||||
|
||||
// The swapped element's old index
|
||||
let old_index = NodeIndex::new(self.nodes.len() as u32);
|
||||
let new_index = a;
|
||||
|
||||
// Adjust the starts of the out edges, and ends of the in edges.
|
||||
for &d in &DIRECTIONS {
|
||||
let k = d as usize;
|
||||
let mut edges = edges_walker_mut(&mut self.edges, swap_edges[k], d);
|
||||
while let Some(curedge) = edges.next_edge() {
|
||||
debug_assert!(curedge.node[k] == old_index);
|
||||
curedge.node[k] = new_index;
|
||||
}
|
||||
}
|
||||
Some(node.weight)
|
||||
}
|
||||
|
||||
/// For edge `e` with endpoints `edge_node`, replace links to it,
|
||||
/// with links to `edge_next`.
|
||||
fn change_edge_links(
|
||||
&mut self,
|
||||
edge_node: [NodeIndex; 2],
|
||||
e: EdgeIndex,
|
||||
edge_next: [EdgeIndex; 2],
|
||||
) {
|
||||
for &d in &DIRECTIONS {
|
||||
let k = d as usize;
|
||||
let node = match self.nodes.get_mut(edge_node[k].index()) {
|
||||
Some(r) => r,
|
||||
None => {
|
||||
debug_assert!(
|
||||
false,
|
||||
"Edge's endpoint dir={:?} index={:?} not found",
|
||||
d, edge_node[k]
|
||||
);
|
||||
return;
|
||||
}
|
||||
};
|
||||
let fst = node.next[k];
|
||||
if fst == e {
|
||||
//println!("Updating first edge 0 for node {}, set to {}", edge_node[0], edge_next[0]);
|
||||
node.next[k] = edge_next[k];
|
||||
} else {
|
||||
let mut edges = edges_walker_mut(&mut self.edges, fst, d);
|
||||
while let Some(curedge) = edges.next_edge() {
|
||||
if curedge.next[k] == e {
|
||||
curedge.next[k] = edge_next[k];
|
||||
break; // the edge can only be present once in the list.
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Remove an edge and return its edge weight, or `None` if it didn't exist.
|
||||
///
|
||||
/// Apart from `e`, this invalidates the last edge index in the graph
|
||||
/// (that edge will adopt the removed edge index).
|
||||
///
|
||||
/// Computes in **O(e')** time, where **e'** is the size of four particular edge lists, for
|
||||
/// the vertices of `e` and the vertices of another affected edge.
|
||||
pub fn remove_edge(&mut self, e: EdgeIndex) -> Option<E> {
|
||||
// every edge is part of two lists,
|
||||
// outgoing and incoming edges.
|
||||
// Remove it from both
|
||||
let (edge_node, edge_next) = match self.edges.get(e.index()) {
|
||||
None => return None,
|
||||
Some(x) => (x.node, x.next),
|
||||
};
|
||||
// Remove the edge from its in and out lists by replacing it with
|
||||
// a link to the next in the list.
|
||||
self.change_edge_links(edge_node, e, edge_next);
|
||||
self.remove_edge_adjust_indices(e)
|
||||
}
|
||||
|
||||
fn remove_edge_adjust_indices(&mut self, e: EdgeIndex) -> Option<E> {
|
||||
// swap_remove the edge -- only the removed edge
|
||||
// and the edge swapped into place are affected and need updating
|
||||
// indices.
|
||||
let edge = self.edges.swap_remove(e.index());
|
||||
let swap = match self.edges.get(e.index()) {
|
||||
// no elment needed to be swapped.
|
||||
None => return Some(edge.weight),
|
||||
Some(ed) => ed.node,
|
||||
};
|
||||
let swapped_e = EdgeIndex::new(self.edges.len() as u32);
|
||||
|
||||
// Update the edge lists by replacing links to the old index by references to the new
|
||||
// edge index.
|
||||
self.change_edge_links(swap, swapped_e, [e, e]);
|
||||
Some(edge.weight)
|
||||
}
|
||||
|
||||
/// Return an iterator of all edges of `a`.
|
||||
///
|
||||
/// - `Directed`: Outgoing edges from `a`.
|
||||
/// - `Undirected`: All edges connected to `a`.
|
||||
///
|
||||
/// Produces an empty iterator if the node doesn't exist.<br>
|
||||
/// Iterator element type is `EdgeReference<E, Ix>`.
|
||||
pub fn edges(&self, a: NodeIndex) -> Edges<E> {
|
||||
self.edges_directed(a, Direction::Outgoing)
|
||||
}
|
||||
|
||||
/// Return an iterator of all edges of `a`, in the specified direction.
|
||||
///
|
||||
/// - `Directed`, `Outgoing`: All edges from `a`.
|
||||
/// - `Directed`, `Incoming`: All edges to `a`.
|
||||
/// - `Undirected`, `Outgoing`: All edges connected to `a`, with `a` being the source of each
|
||||
/// edge.
|
||||
/// - `Undirected`, `Incoming`: All edges connected to `a`, with `a` being the target of each
|
||||
/// edge.
|
||||
///
|
||||
/// Produces an empty iterator if the node `a` doesn't exist.<br>
|
||||
/// Iterator element type is `EdgeReference<E, Ix>`.
|
||||
pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges<E> {
|
||||
Edges {
|
||||
skip_start: a,
|
||||
edges: &self.edges,
|
||||
direction: dir,
|
||||
next: match self.nodes.get(a.index()) {
|
||||
None => [EdgeIndex::end(), EdgeIndex::end()],
|
||||
Some(n) => n.next,
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
/// Return an iterator over all the edges connecting `a` and `b`.
|
||||
///
|
||||
/// - `Directed`: Outgoing edges from `a`.
|
||||
/// - `Undirected`: All edges connected to `a`.
|
||||
///
|
||||
/// Iterator element type is `EdgeReference<E, Ix>`.
|
||||
pub fn edges_connecting(&self, a: NodeIndex, b: NodeIndex) -> EdgesConnecting<E, Ty, Ix> {
|
||||
EdgesConnecting {
|
||||
target_node: b,
|
||||
edges: self.edges_directed(a, Direction::Outgoing),
|
||||
ty: PhantomData,
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
/// Lookup an edge from `a` to `b`.
|
||||
///
|
||||
/// Computes in **O(e')** time, where **e'** is the number of edges
|
||||
/// connected to `a` (and `b`, if the graph edges are undirected).
|
||||
pub fn find_edge(&self, a: NodeIndex, b: NodeIndex) -> Option<EdgeIndex> {
|
||||
self.find_edge_undirected(a, b).map(|(ix, _)| ix)
|
||||
}
|
||||
|
||||
/// Lookup an edge between `a` and `b`, in either direction.
|
||||
///
|
||||
/// If the graph is undirected, then this is equivalent to `.find_edge()`.
|
||||
///
|
||||
/// Return the edge index and its directionality, with `Outgoing` meaning
|
||||
/// from `a` to `b` and `Incoming` the reverse,
|
||||
/// or `None` if the edge does not exist.
|
||||
pub fn find_edge_undirected(
|
||||
&self,
|
||||
a: NodeIndex,
|
||||
b: NodeIndex,
|
||||
) -> Option<(EdgeIndex, Direction)> {
|
||||
match self.nodes.get(a.index()) {
|
||||
None => None,
|
||||
Some(node) => self.find_edge_undirected_from_node(node, b),
|
||||
}
|
||||
}
|
||||
|
||||
fn find_edge_undirected_from_node(
|
||||
&self,
|
||||
node: &Node<N>,
|
||||
b: NodeIndex,
|
||||
) -> Option<(EdgeIndex, Direction)> {
|
||||
for &d in &DIRECTIONS {
|
||||
let k = d as usize;
|
||||
let mut edix = node.next[k];
|
||||
while let Some(edge) = self.edges.get(edix.index()) {
|
||||
if edge.node[1 - k] == b {
|
||||
return Some((edix, d));
|
||||
}
|
||||
edix = edge.next[k];
|
||||
}
|
||||
}
|
||||
None
|
||||
}
|
||||
|
||||
/// Access the internal node array.
|
||||
pub fn raw_nodes(&self) -> &[Node<N>] {
|
||||
&self.nodes
|
||||
}
|
||||
|
||||
/// Access the internal edge array.
|
||||
pub fn raw_edges(&self) -> &[Edge<E>] {
|
||||
&self.edges
|
||||
}
|
||||
|
||||
/// Accessor for data structure internals: the first edge in the given direction.
|
||||
pub fn first_edge(&self, a: NodeIndex, dir: Direction) -> Option<EdgeIndex> {
|
||||
match self.nodes.get(a.index()) {
|
||||
None => None,
|
||||
Some(node) => {
|
||||
let edix = node.next[dir as usize];
|
||||
if edix == EdgeIndex::end() {
|
||||
None
|
||||
} else {
|
||||
Some(edix)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Accessor for data structure internals: the next edge for the given direction.
|
||||
pub fn next_edge(&self, e: EdgeIndex, dir: Direction) -> Option<EdgeIndex> {
|
||||
match self.edges.get(e.index()) {
|
||||
None => None,
|
||||
Some(node) => {
|
||||
let edix = node.next[dir as usize];
|
||||
if edix == EdgeIndex::end() {
|
||||
None
|
||||
} else {
|
||||
Some(edix)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// An iterator over either the nodes without edges to them or from them.
|
||||
pub struct Externals<'a, N: 'a> {
|
||||
iter: std::iter::Enumerate<std::slice::Iter<'a, Node<N>>>,
|
||||
dir: Direction,
|
||||
}
|
||||
|
||||
impl<'a, N: 'a> Iterator for Externals<'a, N> {
|
||||
type Item = NodeIndex;
|
||||
fn next(&mut self) -> Option<NodeIndex> {
|
||||
let k = self.dir as usize;
|
||||
loop {
|
||||
match self.iter.next() {
|
||||
None => return None,
|
||||
Some((index, node)) => {
|
||||
if node.next[k] == EdgeIndex::end() && node.next[1 - k] == EdgeIndex::end() {
|
||||
return Some(NodeIndex::new(index as u32));
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Iterator over the neighbors of a node.
|
||||
///
|
||||
/// Iterator element type is `NodeIndex`.
|
||||
///
|
||||
/// Created with [`.neighbors()`][1], [`.neighbors_directed()`][2] or
|
||||
/// [`.neighbors_undirected()`][3].
|
||||
///
|
||||
/// [1]: struct.Graph.html#method.neighbors
|
||||
/// [2]: struct.Graph.html#method.neighbors_directed
|
||||
/// [3]: struct.Graph.html#method.neighbors_undirected
|
||||
pub struct Neighbors<'a, E: 'a> {
|
||||
/// starting node to skip over
|
||||
skip_start: NodeIndex,
|
||||
edges: &'a [Edge<E>],
|
||||
next: [EdgeIndex; 2],
|
||||
}
|
||||
|
||||
impl<'a, E> Iterator for Neighbors<'a, E> {
|
||||
type Item = NodeIndex;
|
||||
|
||||
fn next(&mut self) -> Option<NodeIndex> {
|
||||
// First any outgoing edges
|
||||
match self.edges.get(self.next[0].index()) {
|
||||
None => {}
|
||||
Some(edge) => {
|
||||
self.next[0] = edge.next[0];
|
||||
return Some(edge.node[1]);
|
||||
}
|
||||
}
|
||||
// Then incoming edges
|
||||
// For an "undirected" iterator (traverse both incoming
|
||||
// and outgoing edge lists), make sure we don't double
|
||||
// count selfloops by skipping them in the incoming list.
|
||||
while let Some(edge) = self.edges.get(self.next[1].index()) {
|
||||
self.next[1] = edge.next[1];
|
||||
if edge.node[0] != self.skip_start {
|
||||
return Some(edge.node[0]);
|
||||
}
|
||||
}
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
struct EdgesWalkerMut<'a, E: 'a> {
|
||||
edges: &'a mut [Edge<E>],
|
||||
next: EdgeIndex,
|
||||
dir: Direction,
|
||||
}
|
||||
|
||||
fn edges_walker_mut<E>(
|
||||
edges: &mut [Edge<E>],
|
||||
next: EdgeIndex,
|
||||
dir: Direction,
|
||||
) -> EdgesWalkerMut<E> {
|
||||
EdgesWalkerMut { edges, next, dir }
|
||||
}
|
||||
|
||||
impl<'a, E> EdgesWalkerMut<'a, E> {
|
||||
fn next_edge(&mut self) -> Option<&mut Edge<E>> {
|
||||
self.next().map(|t| t.1)
|
||||
}
|
||||
|
||||
fn next(&mut self) -> Option<(EdgeIndex, &mut Edge<E>)> {
|
||||
let this_index = self.next;
|
||||
let k = self.dir as usize;
|
||||
match self.edges.get_mut(self.next.index()) {
|
||||
None => None,
|
||||
Some(edge) => {
|
||||
self.next = edge.next[k];
|
||||
Some((this_index, edge))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Iterator over the edges of from or to a node
|
||||
pub struct Edges<'a, E: 'a> {
|
||||
/// starting node to skip over
|
||||
skip_start: NodeIndex,
|
||||
edges: &'a [Edge<E>],
|
||||
|
||||
/// Next edge to visit.
|
||||
next: [EdgeIndex; 2],
|
||||
|
||||
/// For directed graphs: the direction to iterate in
|
||||
/// For undirected graphs: the direction of edges
|
||||
direction: Direction,
|
||||
}
|
||||
|
||||
impl<'a, E> Iterator for Edges<'a, E> {
|
||||
type Item = EdgeReference<'a, E>;
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
// type direction | iterate over reverse
|
||||
// |
|
||||
// Directed Outgoing | outgoing no
|
||||
// Directed Incoming | incoming no
|
||||
// Undirected Outgoing | both incoming
|
||||
// Undirected Incoming | both outgoing
|
||||
|
||||
// For iterate_over, "both" is represented as None.
|
||||
// For reverse, "no" is represented as None.
|
||||
let (iterate_over, reverse) = (None, Some(self.direction.opposite()));
|
||||
|
||||
if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing {
|
||||
let i = self.next[0].index();
|
||||
if let Some(Edge { node, weight, next }) = self.edges.get(i) {
|
||||
self.next[0] = next[0];
|
||||
return Some(EdgeReference {
|
||||
index: EdgeIndex(i as u32),
|
||||
node: if reverse == Some(Direction::Outgoing) {
|
||||
swap_pair(*node)
|
||||
} else {
|
||||
*node
|
||||
},
|
||||
weight,
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
if iterate_over.unwrap_or(Direction::Incoming) == Direction::Incoming {
|
||||
while let Some(Edge { node, weight, next }) = self.edges.get(self.next[1].index()) {
|
||||
let edge_index = self.next[1];
|
||||
self.next[1] = next[1];
|
||||
// In any of the "both" situations, self-loops would be iterated over twice.
|
||||
// Skip them here.
|
||||
if iterate_over.is_none() && node[0] == self.skip_start {
|
||||
continue;
|
||||
}
|
||||
|
||||
return Some(EdgeReference {
|
||||
index: edge_index,
|
||||
node: if reverse == Some(Direction::Incoming) {
|
||||
swap_pair(*node)
|
||||
} else {
|
||||
*node
|
||||
},
|
||||
weight,
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
|
||||
x.swap(0, 1);
|
||||
x
|
||||
}
|
||||
|
||||
impl<'a, E> Clone for Edges<'a, E> {
|
||||
fn clone(&self) -> Self {
|
||||
Edges {
|
||||
skip_start: self.skip_start,
|
||||
edges: self.edges,
|
||||
next: self.next,
|
||||
direction: self.direction,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Index the `Graph` by `NodeIndex` to access node weights.
|
||||
///
|
||||
/// **Panics** if the node doesn't exist.
|
||||
impl<N, E> Index<NodeIndex> for Graph<N, E> {
|
||||
type Output = N;
|
||||
fn index(&self, index: NodeIndex) -> &N {
|
||||
&self.nodes[index.index()].weight
|
||||
}
|
||||
}
|
||||
|
||||
/// Index the `Graph` by `NodeIndex` to access node weights.
|
||||
///
|
||||
/// **Panics** if the node doesn't exist.
|
||||
impl<N, E> IndexMut<NodeIndex> for Graph<N, E> {
|
||||
fn index_mut(&mut self, index: NodeIndex) -> &mut N {
|
||||
&mut self.nodes[index.index()].weight
|
||||
}
|
||||
}
|
||||
|
||||
/// Index the `Graph` by `EdgeIndex` to access edge weights.
|
||||
///
|
||||
/// **Panics** if the edge doesn't exist.
|
||||
impl<N, E> Index<EdgeIndex> for Graph<N, E> {
|
||||
type Output = E;
|
||||
fn index(&self, index: EdgeIndex) -> &E {
|
||||
&self.edges[index.index()].weight
|
||||
}
|
||||
}
|
||||
|
||||
/// Index the `Graph` by `EdgeIndex` to access edge weights.
|
||||
///
|
||||
/// **Panics** if the edge doesn't exist.
|
||||
impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> {
|
||||
fn index_mut(&mut self, index: EdgeIndex) -> &mut E {
|
||||
&mut self.edges[index.index()].weight
|
||||
}
|
||||
}
|
||||
|
||||
/// A “walker” object that can be used to step through the edge list of a node.
|
||||
///
|
||||
/// Created with [`.detach()`](struct.Neighbors.html#method.detach).
|
||||
///
|
||||
/// The walker does not borrow from the graph, so it lets you step through
|
||||
/// neighbors or incident edges while also mutating graph weights, as
|
||||
/// in the following example:
|
||||
pub struct WalkNeighbors {
|
||||
skip_start: NodeIndex,
|
||||
next: [EdgeIndex; 2],
|
||||
}
|
||||
|
||||
impl Clone for WalkNeighbors {
|
||||
fn clone(&self) -> Self {
|
||||
WalkNeighbors {
|
||||
skip_start: self.skip_start,
|
||||
next: self.next,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Reference to a `Graph` edge.
|
||||
#[derive(Debug)]
|
||||
pub struct EdgeReference<'a, E: 'a> {
|
||||
index: EdgeIndex,
|
||||
node: [NodeIndex; 2],
|
||||
weight: &'a E,
|
||||
}
|
||||
|
||||
impl<'a, E: 'a> EdgeReference<'a, E> {
|
||||
#[inline]
|
||||
pub fn id(&self) -> EdgeIndex {
|
||||
self.index
|
||||
}
|
||||
|
||||
#[inline]
|
||||
pub fn weight(&self) -> &'a E {
|
||||
self.weight
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, E> Clone for EdgeReference<'a, E> {
|
||||
fn clone(&self) -> Self {
|
||||
*self
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, E> Copy for EdgeReference<'a, E> {}
|
||||
|
||||
impl<'a, E> PartialEq for EdgeReference<'a, E>
|
||||
where
|
||||
E: PartialEq,
|
||||
{
|
||||
fn eq(&self, rhs: &Self) -> bool {
|
||||
self.index == rhs.index && self.weight == rhs.weight
|
||||
}
|
||||
}
|
||||
|
||||
/// Iterator over all nodes of a graph.
|
||||
pub struct NodeReferences<'a, N: 'a> {
|
||||
iter: std::iter::Enumerate<std::slice::Iter<'a, Node<N>>>,
|
||||
}
|
||||
|
||||
impl<'a, N> Iterator for NodeReferences<'a, N> {
|
||||
type Item = (NodeIndex, &'a N);
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
self.iter
|
||||
.next()
|
||||
.map(|(i, node)| (NodeIndex::new(i as u32), &node.weight))
|
||||
}
|
||||
|
||||
fn size_hint(&self) -> (usize, Option<usize>) {
|
||||
self.iter.size_hint()
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, N> DoubleEndedIterator for NodeReferences<'a, N> {
|
||||
fn next_back(&mut self) -> Option<Self::Item> {
|
||||
self.iter
|
||||
.next_back()
|
||||
.map(|(i, node)| (NodeIndex::new(i as u32), &node.weight))
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, N> ExactSizeIterator for NodeReferences<'a, N> {}
|
||||
4
src/data/mod.rs
Normal file
4
src/data/mod.rs
Normal file
@@ -0,0 +1,4 @@
|
||||
//! Data structures modified with guaranteed deterministic behavior after deserialization.
|
||||
|
||||
pub mod arena;
|
||||
pub(crate) mod graph;
|
||||
207
src/dynamics/integration_parameters.rs
Normal file
207
src/dynamics/integration_parameters.rs
Normal file
@@ -0,0 +1,207 @@
|
||||
/// Parameters for a time-step of the physics engine.
|
||||
#[derive(Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct IntegrationParameters {
|
||||
/// The timestep (default: `1.0 / 60.0`)
|
||||
dt: f32,
|
||||
/// The inverse of `dt`.
|
||||
inv_dt: f32,
|
||||
// /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
|
||||
// ///
|
||||
// /// This parameter is ignored if rapier is not compiled with is `parallel` feature.
|
||||
// /// Refer to rayon's documentation regarding how to configure the number of threads with either
|
||||
// /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`.
|
||||
// /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower
|
||||
// /// simulation than setting `multithreading_enabled` to `false`.
|
||||
// pub multithreading_enabled: bool,
|
||||
/// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`).
|
||||
/// This allows the user to take action during a timestep, in-between two CCD events.
|
||||
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,
|
||||
/// 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,
|
||||
/// 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,
|
||||
/// 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,
|
||||
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
|
||||
pub allowed_linear_error: f32,
|
||||
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
|
||||
pub prediction_distance: f32,
|
||||
/// Amount of angular drift of joint limits the engine wont
|
||||
/// attempt to correct (default: `0.001rad`).
|
||||
pub allowed_angular_error: f32,
|
||||
/// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
|
||||
pub max_linear_correction: f32,
|
||||
/// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
|
||||
pub max_angular_correction: f32,
|
||||
/// 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,
|
||||
/// 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`).
|
||||
pub max_position_iterations: usize,
|
||||
/// Minimum number of dynamic bodies in each active island (default: `128`).
|
||||
pub min_island_size: usize,
|
||||
/// Maximum number of iterations performed by the position-based constraints solver for CCD steps (default: `10`).
|
||||
///
|
||||
/// This should be sufficiently high so all penetration get resolved. For example, if CCD cause your
|
||||
/// objects to stutter, that may be because the number of CCD position iterations is too low, causing
|
||||
/// them to remain stuck in a penetration configuration for a few frames.
|
||||
///
|
||||
/// The highest this number, the highest its computational cost.
|
||||
pub max_ccd_position_iterations: usize,
|
||||
/// Maximum number of substeps performed by the solver (default: `1`).
|
||||
pub max_ccd_substeps: usize,
|
||||
/// Controls the number of Proximity::Intersecting events generated by a trigger during CCD resolution (default: `false`).
|
||||
///
|
||||
/// If false, triggers will only generate one Proximity::Intersecting event during a step, even
|
||||
/// if another colliders repeatedly enters and leaves the triggers during multiple CCD substeps.
|
||||
///
|
||||
/// If true, triggers will generate as many Proximity::Intersecting and Proximity::Disjoint/Proximity::WithinMargin
|
||||
/// events as the number of times a collider repeatedly enters and leaves the triggers during multiple CCD substeps.
|
||||
/// This is more computationally intensive.
|
||||
pub multiple_ccd_substep_sensor_events_enabled: bool,
|
||||
/// Whether penetration are taken into account in CCD resolution (default: `false`).
|
||||
///
|
||||
/// If this is set to `false` two penetrating colliders will not be considered to have any time of impact
|
||||
/// while they are penetrating. This may end up allowing some tunelling, but will avoid stuttering effect
|
||||
/// when the constraints solver fails to completely separate two colliders after a CCD contact.
|
||||
///
|
||||
/// If this is set to `true`, two penetrating colliders will be considered to have a time of impact
|
||||
/// equal to 0 until the constraints solver manages to separate them. This will prevent tunnelling
|
||||
/// almost completely, but may introduce stuttering effects when the constraints solver fails to completely
|
||||
/// separate two colliders after a CCD contact.
|
||||
// FIXME: this is a very binary way of handling penetration.
|
||||
// We should provide a more flexible solution by letting the user choose some
|
||||
// minimal amount of movement applied to an object that get stuck.
|
||||
pub ccd_on_penetration_enabled: bool,
|
||||
}
|
||||
|
||||
impl IntegrationParameters {
|
||||
/// Creates a set of integration parameters with the given values.
|
||||
pub fn new(
|
||||
dt: f32,
|
||||
// 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,
|
||||
max_velocity_iterations: usize,
|
||||
max_position_iterations: usize,
|
||||
max_ccd_position_iterations: usize,
|
||||
max_ccd_substeps: usize,
|
||||
return_after_ccd_substep: bool,
|
||||
multiple_ccd_substep_sensor_events_enabled: bool,
|
||||
ccd_on_penetration_enabled: bool,
|
||||
) -> Self {
|
||||
IntegrationParameters {
|
||||
dt,
|
||||
inv_dt: if dt == 0.0 { 0.0 } else { 1.0 / dt },
|
||||
// multithreading_enabled,
|
||||
erp,
|
||||
joint_erp,
|
||||
warmstart_coeff,
|
||||
restitution_velocity_threshold,
|
||||
allowed_linear_error,
|
||||
allowed_angular_error,
|
||||
max_linear_correction,
|
||||
max_angular_correction,
|
||||
prediction_distance,
|
||||
max_stabilization_multiplier,
|
||||
max_velocity_iterations,
|
||||
max_position_iterations,
|
||||
// FIXME: 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
|
||||
// tons of islands, reducing SIMD parallelism opportunities.
|
||||
min_island_size: 128,
|
||||
max_ccd_position_iterations,
|
||||
max_ccd_substeps,
|
||||
return_after_ccd_substep,
|
||||
multiple_ccd_substep_sensor_events_enabled,
|
||||
ccd_on_penetration_enabled,
|
||||
}
|
||||
}
|
||||
|
||||
/// The current time-stepping length.
|
||||
#[inline(always)]
|
||||
pub fn dt(&self) -> f32 {
|
||||
self.dt
|
||||
}
|
||||
|
||||
/// The inverse of the time-stepping length.
|
||||
///
|
||||
/// This is zero if `self.dt` is zero.
|
||||
#[inline(always)]
|
||||
pub fn inv_dt(&self) -> f32 {
|
||||
self.inv_dt
|
||||
}
|
||||
|
||||
/// Sets the time-stepping length.
|
||||
///
|
||||
/// This automatically recompute `self.inv_dt`.
|
||||
#[inline]
|
||||
pub fn set_dt(&mut self, dt: f32) {
|
||||
assert!(dt >= 0.0, "The time-stepping length cannot be negative.");
|
||||
self.dt = dt;
|
||||
if dt == 0.0 {
|
||||
self.inv_dt = 0.0
|
||||
} else {
|
||||
self.inv_dt = 1.0 / dt
|
||||
}
|
||||
}
|
||||
|
||||
/// Sets the inverse time-stepping length (i.e. the frequency).
|
||||
///
|
||||
/// This automatically recompute `self.dt`.
|
||||
#[inline]
|
||||
pub fn set_inv_dt(&mut self, inv_dt: f32) {
|
||||
self.inv_dt = inv_dt;
|
||||
if inv_dt == 0.0 {
|
||||
self.dt = 0.0
|
||||
} else {
|
||||
self.dt = 1.0 / inv_dt
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for IntegrationParameters {
|
||||
fn default() -> Self {
|
||||
Self::new(
|
||||
1.0 / 60.0,
|
||||
// true,
|
||||
0.2,
|
||||
0.2,
|
||||
1.0,
|
||||
1.0,
|
||||
0.005,
|
||||
0.001,
|
||||
0.2,
|
||||
0.2,
|
||||
0.002,
|
||||
0.2,
|
||||
4,
|
||||
1,
|
||||
10,
|
||||
1,
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
)
|
||||
}
|
||||
}
|
||||
34
src/dynamics/joint/ball_joint.rs
Normal file
34
src/dynamics/joint/ball_joint.rs
Normal file
@@ -0,0 +1,34 @@
|
||||
use crate::math::{Point, 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>,
|
||||
/// Where the ball joint is attached on the first body, expressed in the first body local frame.
|
||||
pub local_anchor2: Point<f32>,
|
||||
/// 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>,
|
||||
}
|
||||
|
||||
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 {
|
||||
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>,
|
||||
) -> Self {
|
||||
Self {
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
impulse,
|
||||
}
|
||||
}
|
||||
}
|
||||
33
src/dynamics/joint/fixed_joint.rs
Normal file
33
src/dynamics/joint/fixed_joint.rs
Normal file
@@ -0,0 +1,33 @@
|
||||
use crate::math::{Isometry, SpacialVector};
|
||||
|
||||
#[derive(Copy, Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// A joint that prevents all relative movement between two bodies.
|
||||
///
|
||||
/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
|
||||
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>,
|
||||
/// 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>,
|
||||
/// 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>,
|
||||
}
|
||||
|
||||
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 {
|
||||
Self {
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
impulse: SpacialVector::zeros(),
|
||||
}
|
||||
}
|
||||
}
|
||||
112
src/dynamics/joint/joint.rs
Normal file
112
src/dynamics/joint/joint.rs
Normal file
@@ -0,0 +1,112 @@
|
||||
#[cfg(feature = "dim3")]
|
||||
use crate::dynamics::RevoluteJoint;
|
||||
use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
|
||||
|
||||
#[derive(Copy, Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// An enum grouping all possible types of joints.
|
||||
pub enum JointParams {
|
||||
/// A Ball joint that removes all relative linear degrees of freedom between the affected bodies.
|
||||
BallJoint(BallJoint),
|
||||
/// A fixed joint that removes all relative degrees of freedom between the affected bodies.
|
||||
FixedJoint(FixedJoint),
|
||||
/// A prismatic joint that removes all degrees of degrees of freedom between the affected
|
||||
/// bodies except for the translation along one axis.
|
||||
PrismaticJoint(PrismaticJoint),
|
||||
#[cfg(feature = "dim3")]
|
||||
/// A revolute joint that removes all degrees of degrees of freedom between the affected
|
||||
/// bodies except for the translation along one axis.
|
||||
RevoluteJoint(RevoluteJoint),
|
||||
}
|
||||
|
||||
impl JointParams {
|
||||
/// An integer identifier for each type of joint.
|
||||
pub fn type_id(&self) -> usize {
|
||||
match self {
|
||||
JointParams::BallJoint(_) => 0,
|
||||
JointParams::FixedJoint(_) => 1,
|
||||
JointParams::PrismaticJoint(_) => 2,
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => 3,
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets a reference to the underlying ball joint, if `self` is one.
|
||||
pub fn as_ball_joint(&self) -> Option<&BallJoint> {
|
||||
if let JointParams::BallJoint(j) = self {
|
||||
Some(j)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets a reference to the underlying fixed joint, if `self` is one.
|
||||
pub fn as_fixed_joint(&self) -> Option<&FixedJoint> {
|
||||
if let JointParams::FixedJoint(j) = self {
|
||||
Some(j)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets a reference to the underlying prismatic joint, if `self` is one.
|
||||
pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
|
||||
if let JointParams::PrismaticJoint(j) = self {
|
||||
Some(j)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets a reference to the underlying revolute joint, if `self` is one.
|
||||
#[cfg(feature = "dim3")]
|
||||
pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> {
|
||||
if let JointParams::RevoluteJoint(j) = self {
|
||||
Some(j)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<BallJoint> for JointParams {
|
||||
fn from(j: BallJoint) -> Self {
|
||||
JointParams::BallJoint(j)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<FixedJoint> for JointParams {
|
||||
fn from(j: FixedJoint) -> Self {
|
||||
JointParams::FixedJoint(j)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
impl From<RevoluteJoint> for JointParams {
|
||||
fn from(j: RevoluteJoint) -> Self {
|
||||
JointParams::RevoluteJoint(j)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PrismaticJoint> for JointParams {
|
||||
fn from(j: PrismaticJoint) -> Self {
|
||||
JointParams::PrismaticJoint(j)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// A joint attached to two bodies.
|
||||
pub struct Joint {
|
||||
/// Handle to the first body attached to this joint.
|
||||
pub body1: RigidBodyHandle,
|
||||
/// Handle to the second body attached to this joint.
|
||||
pub body2: RigidBodyHandle,
|
||||
// A joint needs to know its handle to simplify its removal.
|
||||
pub(crate) handle: JointHandle,
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(crate) constraint_index: usize,
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(crate) position_constraint_index: usize,
|
||||
/// The joint geometric parameters and impulse.
|
||||
pub params: JointParams,
|
||||
}
|
||||
218
src/dynamics/joint/joint_set.rs
Normal file
218
src/dynamics/joint/joint_set.rs
Normal file
@@ -0,0 +1,218 @@
|
||||
use super::Joint;
|
||||
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
|
||||
|
||||
use crate::data::arena::{Arena, Index};
|
||||
use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
|
||||
|
||||
/// The unique identifier of a joint added to the joint set.
|
||||
pub type JointHandle = Index;
|
||||
pub(crate) type JointIndex = usize;
|
||||
pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
|
||||
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// 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>,
|
||||
}
|
||||
|
||||
impl JointSet {
|
||||
/// Creates a new empty set of joints.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
joint_ids: Arena::new(),
|
||||
joint_graph: InteractionGraph::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// 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> {
|
||||
&self.joint_graph
|
||||
}
|
||||
|
||||
/// Is the given joint handle valid?
|
||||
pub fn contains(&self, handle: JointHandle) -> bool {
|
||||
self.joint_ids.contains(handle)
|
||||
}
|
||||
|
||||
/// Gets the joint with the given handle.
|
||||
pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
|
||||
let id = self.joint_ids.get(handle)?;
|
||||
self.joint_graph.graph.edge_weight(*id)
|
||||
}
|
||||
|
||||
/// Gets the joint with the given handle without a known generation.
|
||||
///
|
||||
/// This is useful when you know you want the joint at position `i` but
|
||||
/// don't know what is its current generation number. Generation numbers are
|
||||
/// used to protect from the ABA problem because the joint position `i`
|
||||
/// are recycled between two insertion and a removal.
|
||||
///
|
||||
/// 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<(&Joint, JointHandle)> {
|
||||
let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
|
||||
Some((self.joint_graph.graph.edge_weight(*id)?, handle))
|
||||
}
|
||||
|
||||
/// Iterates through all the joint on this set.
|
||||
pub fn iter(&self) -> impl Iterator<Item = &Joint> {
|
||||
self.joint_graph.graph.edges.iter().map(|e| &e.weight)
|
||||
}
|
||||
|
||||
/// Iterates mutably through all the joint on this set.
|
||||
pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Joint> {
|
||||
self.joint_graph
|
||||
.graph
|
||||
.edges
|
||||
.iter_mut()
|
||||
.map(|e| &mut e.weight)
|
||||
}
|
||||
|
||||
// /// The set of joints as an array.
|
||||
// pub(crate) fn joints(&self) -> &[JointGraphEdge] {
|
||||
// // self.joint_graph
|
||||
// // .graph
|
||||
// // .edges
|
||||
// // .iter_mut()
|
||||
// // .map(|e| &mut e.weight)
|
||||
// }
|
||||
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
|
||||
&mut self.joint_graph.graph.edges[..]
|
||||
}
|
||||
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec<JointGraphEdge> {
|
||||
&mut self.joint_graph.graph.edges
|
||||
}
|
||||
|
||||
/// Inserts a new joint into this set and retrieve its handle.
|
||||
pub fn insert<J>(
|
||||
&mut self,
|
||||
bodies: &mut RigidBodySet,
|
||||
body1: RigidBodyHandle,
|
||||
body2: RigidBodyHandle,
|
||||
joint_params: J,
|
||||
) -> JointHandle
|
||||
where
|
||||
J: Into<JointParams>,
|
||||
{
|
||||
let handle = self.joint_ids.insert(0.into());
|
||||
let joint = Joint {
|
||||
body1,
|
||||
body2,
|
||||
handle,
|
||||
#[cfg(feature = "parallel")]
|
||||
constraint_index: 0,
|
||||
#[cfg(feature = "parallel")]
|
||||
position_constraint_index: 0,
|
||||
params: joint_params.into(),
|
||||
};
|
||||
|
||||
let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
|
||||
let (rb1, rb2) = (
|
||||
rb1.expect("Attempt to attach a joint to a non-existing body."),
|
||||
rb2.expect("Attempt to attach a joint to a non-existing body."),
|
||||
);
|
||||
|
||||
// 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) {
|
||||
rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
|
||||
}
|
||||
|
||||
if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) {
|
||||
rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
|
||||
}
|
||||
|
||||
let id = self
|
||||
.joint_graph
|
||||
.add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
|
||||
|
||||
self.joint_ids[handle] = id;
|
||||
handle
|
||||
}
|
||||
|
||||
/// Retrieve all the joints happening between two active bodies.
|
||||
// NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
|
||||
pub(crate) fn select_active_interactions(
|
||||
&self,
|
||||
bodies: &RigidBodySet,
|
||||
out: &mut Vec<Vec<JointIndex>>,
|
||||
) {
|
||||
for out_island in &mut out[..bodies.num_islands()] {
|
||||
out_island.clear();
|
||||
}
|
||||
|
||||
// FIXME: don't iterate through all the interactions.
|
||||
for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
|
||||
let joint = &edge.weight;
|
||||
let rb1 = &bodies[joint.body1];
|
||||
let rb2 = &bodies[joint.body2];
|
||||
|
||||
if (rb1.is_dynamic() || rb2.is_dynamic())
|
||||
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
|
||||
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
|
||||
{
|
||||
let island_index = if !rb1.is_dynamic() {
|
||||
rb2.active_island_id
|
||||
} else {
|
||||
rb1.active_island_id
|
||||
};
|
||||
|
||||
out[island_index].push(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn remove_rigid_body(
|
||||
&mut self,
|
||||
deleted_id: RigidBodyGraphIndex,
|
||||
bodies: &mut RigidBodySet,
|
||||
) {
|
||||
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.
|
||||
// Delete the node.
|
||||
let to_delete: Vec<_> = self
|
||||
.joint_graph
|
||||
.interactions_with(deleted_id)
|
||||
.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();
|
||||
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;
|
||||
}
|
||||
|
||||
// Wake up the attached bodies.
|
||||
bodies.wake_up(h1);
|
||||
bodies.wake_up(h2);
|
||||
}
|
||||
|
||||
if let Some(other) = self.joint_graph.remove_node(deleted_id) {
|
||||
// One rigid-body joint graph index may have been invalidated
|
||||
// so we need to update it.
|
||||
if let Some(replacement) = bodies.get_mut_internal(other) {
|
||||
replacement.joint_graph_index = deleted_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
16
src/dynamics/joint/mod.rs
Normal file
16
src/dynamics/joint/mod.rs
Normal file
@@ -0,0 +1,16 @@
|
||||
pub use self::ball_joint::BallJoint;
|
||||
pub use self::fixed_joint::FixedJoint;
|
||||
pub use self::joint::{Joint, JointParams};
|
||||
pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
|
||||
pub use self::joint_set::{JointHandle, JointSet};
|
||||
pub use self::prismatic_joint::PrismaticJoint;
|
||||
#[cfg(feature = "dim3")]
|
||||
pub use self::revolute_joint::RevoluteJoint;
|
||||
|
||||
mod ball_joint;
|
||||
mod fixed_joint;
|
||||
mod joint;
|
||||
mod joint_set;
|
||||
mod prismatic_joint;
|
||||
#[cfg(feature = "dim3")]
|
||||
mod revolute_joint;
|
||||
193
src/dynamics/joint/prismatic_joint.rs
Normal file
193
src/dynamics/joint/prismatic_joint.rs
Normal file
@@ -0,0 +1,193 @@
|
||||
use crate::math::{Isometry, Point, Vector, DIM};
|
||||
use crate::utils::WBasis;
|
||||
use na::Unit;
|
||||
#[cfg(feature = "dim2")]
|
||||
use na::Vector2;
|
||||
#[cfg(feature = "dim3")]
|
||||
use na::Vector5;
|
||||
|
||||
#[derive(Copy, Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// 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>,
|
||||
/// 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],
|
||||
/// 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>,
|
||||
/// 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>,
|
||||
/// 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],
|
||||
/// 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 motor_enabled: bool,
|
||||
// pub target_motor_vel: f32,
|
||||
// pub max_motor_impulse: f32,
|
||||
// pub motor_impulse: f32,
|
||||
}
|
||||
|
||||
impl PrismaticJoint {
|
||||
/// Creates a new prismatic joint with the given point of applications and axis, all expressed
|
||||
/// 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>>,
|
||||
) -> Self {
|
||||
Self {
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
local_axis1,
|
||||
local_axis2,
|
||||
basis1: local_axis1.orthonormal_basis(),
|
||||
basis2: local_axis2.orthonormal_basis(),
|
||||
impulse: na::zero(),
|
||||
limits_enabled: false,
|
||||
limits: [-f32::MAX, f32::MAX],
|
||||
limits_impulse: 0.0,
|
||||
// motor_enabled: false,
|
||||
// target_motor_vel: 0.0,
|
||||
// max_motor_impulse: f32::MAX,
|
||||
// motor_impulse: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Creates a new prismatic joint with the given point of applications and axis, all expressed
|
||||
/// in the local-space of the affected bodies.
|
||||
///
|
||||
/// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal
|
||||
/// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically
|
||||
/// 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>,
|
||||
) -> Self {
|
||||
let basis1 = if let Some(local_bitangent1) =
|
||||
Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
|
||||
{
|
||||
[
|
||||
local_bitangent1.into_inner(),
|
||||
local_bitangent1.cross(&local_axis1),
|
||||
]
|
||||
} else {
|
||||
local_axis1.orthonormal_basis()
|
||||
};
|
||||
|
||||
let basis2 = if let Some(local_bitangent2) =
|
||||
Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3)
|
||||
{
|
||||
[
|
||||
local_bitangent2.into_inner(),
|
||||
local_bitangent2.cross(&local_axis2),
|
||||
]
|
||||
} else {
|
||||
local_axis2.orthonormal_basis()
|
||||
};
|
||||
|
||||
Self {
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
local_axis1,
|
||||
local_axis2,
|
||||
basis1,
|
||||
basis2,
|
||||
impulse: na::zero(),
|
||||
limits_enabled: false,
|
||||
limits: [-f32::MAX, f32::MAX],
|
||||
limits_impulse: 0.0,
|
||||
// motor_enabled: false,
|
||||
// target_motor_vel: 0.0,
|
||||
// max_motor_impulse: f32::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>> {
|
||||
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>> {
|
||||
self.local_axis2
|
||||
}
|
||||
|
||||
// FIXME: precompute this?
|
||||
#[cfg(feature = "dim2")]
|
||||
pub(crate) fn local_frame1(&self) -> Isometry<f32> {
|
||||
use na::{Matrix2, Rotation2, UnitComplex};
|
||||
|
||||
let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
|
||||
let rotmat = Rotation2::from_matrix_unchecked(mat);
|
||||
let rotation = UnitComplex::from_rotation_matrix(&rotmat);
|
||||
let translation = self.local_anchor1.coords.into();
|
||||
Isometry::from_parts(translation, rotation)
|
||||
}
|
||||
|
||||
// FIXME: precompute this?
|
||||
#[cfg(feature = "dim2")]
|
||||
pub(crate) fn local_frame2(&self) -> Isometry<f32> {
|
||||
use na::{Matrix2, Rotation2, UnitComplex};
|
||||
|
||||
let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
|
||||
let rotmat = Rotation2::from_matrix_unchecked(mat);
|
||||
let rotation = UnitComplex::from_rotation_matrix(&rotmat);
|
||||
let translation = self.local_anchor2.coords.into();
|
||||
Isometry::from_parts(translation, rotation)
|
||||
}
|
||||
|
||||
// FIXME: precompute this?
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(crate) fn local_frame1(&self) -> Isometry<f32> {
|
||||
use na::{Matrix3, Rotation3, UnitQuaternion};
|
||||
|
||||
let mat = Matrix3::from_columns(&[
|
||||
self.local_axis1.into_inner(),
|
||||
self.basis1[0],
|
||||
self.basis1[1],
|
||||
]);
|
||||
let rotmat = Rotation3::from_matrix_unchecked(mat);
|
||||
let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
|
||||
let translation = self.local_anchor1.coords.into();
|
||||
Isometry::from_parts(translation, rotation)
|
||||
}
|
||||
|
||||
// FIXME: precompute this?
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(crate) fn local_frame2(&self) -> Isometry<f32> {
|
||||
use na::{Matrix3, Rotation3, UnitQuaternion};
|
||||
|
||||
let mat = Matrix3::from_columns(&[
|
||||
self.local_axis2.into_inner(),
|
||||
self.basis2[0],
|
||||
self.basis2[1],
|
||||
]);
|
||||
let rotmat = Rotation3::from_matrix_unchecked(mat);
|
||||
let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
|
||||
let translation = self.local_anchor2.coords.into();
|
||||
Isometry::from_parts(translation, rotation)
|
||||
}
|
||||
}
|
||||
46
src/dynamics/joint/revolute_joint.rs
Normal file
46
src/dynamics/joint/revolute_joint.rs
Normal file
@@ -0,0 +1,46 @@
|
||||
use crate::math::{Point, Vector};
|
||||
use crate::utils::WBasis;
|
||||
use na::{Unit, Vector5};
|
||||
|
||||
#[derive(Copy, Clone)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// 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>,
|
||||
/// 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>,
|
||||
/// The rotation axis of this revolute joint expressed in the local space of the first attached body.
|
||||
pub local_axis1: Unit<Vector<f32>>,
|
||||
/// The rotation axis of this revolute joint expressed in the local space of the second attached body.
|
||||
pub local_axis2: Unit<Vector<f32>>,
|
||||
/// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
|
||||
pub basis1: [Vector<f32>; 2],
|
||||
/// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
|
||||
pub basis2: [Vector<f32>; 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>,
|
||||
}
|
||||
|
||||
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>>,
|
||||
) -> Self {
|
||||
Self {
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
local_axis1,
|
||||
local_axis2,
|
||||
basis1: local_axis1.orthonormal_basis(),
|
||||
basis2: local_axis2.orthonormal_basis(),
|
||||
impulse: na::zero(),
|
||||
}
|
||||
}
|
||||
}
|
||||
206
src/dynamics/mass_properties.rs
Normal file
206
src/dynamics/mass_properties.rs
Normal file
@@ -0,0 +1,206 @@
|
||||
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
|
||||
use crate::utils;
|
||||
use num::Zero;
|
||||
use std::ops::{Add, AddAssign};
|
||||
#[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 {
|
||||
#[cfg(feature = "dim2")]
|
||||
pub(crate) 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,
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(crate) 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())
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(crate) 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 angular inertia tensor of the rigid body from its principal inertia values and axii.
|
||||
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 {
|
||||
if self.inv_mass != 0.0 {
|
||||
let mass = 1.0 / self.inv_mass;
|
||||
let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt);
|
||||
i + shift.norm_squared() * mass
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> Matrix3<f32> {
|
||||
if self.inv_mass != 0.0 {
|
||||
let mass = 1.0 / self.inv_mass;
|
||||
let matrix = self.reconstruct_inertia_matrix();
|
||||
let diag = shift.norm_squared();
|
||||
let diagm = Matrix3::from_diagonal_element(diag);
|
||||
matrix + (diagm + shift * shift.transpose()) * mass
|
||||
} else {
|
||||
Matrix3::zeros()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 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(&eigen.eigenvectors);
|
||||
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
|
||||
}
|
||||
}
|
||||
30
src/dynamics/mass_properties_ball.rs
Normal file
30
src/dynamics/mass_properties_ball.rs
Normal file
@@ -0,0 +1,30 @@
|
||||
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)
|
||||
}
|
||||
}
|
||||
60
src/dynamics/mass_properties_capsule.rs
Normal file
60
src/dynamics/mass_properties_capsule.rs
Normal file
@@ -0,0 +1,60 @@
|
||||
use crate::dynamics::MassProperties;
|
||||
#[cfg(feature = "dim3")]
|
||||
use crate::geometry::Capsule;
|
||||
use crate::math::{Point, PrincipalAngularInertia, Vector};
|
||||
|
||||
impl MassProperties {
|
||||
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)
|
||||
}
|
||||
}
|
||||
|
||||
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,
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
33
src/dynamics/mass_properties_cuboid.rs
Normal file
33
src/dynamics/mass_properties_cuboid.rs
Normal file
@@ -0,0 +1,33 @@
|
||||
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)
|
||||
}
|
||||
}
|
||||
144
src/dynamics/mass_properties_polygon.rs
Normal file
144
src/dynamics/mass_properties_polygon.rs
Normal file
@@ -0,0 +1,144 @@
|
||||
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)
|
||||
}
|
||||
30
src/dynamics/mod.rs
Normal file
30
src/dynamics/mod.rs
Normal file
@@ -0,0 +1,30 @@
|
||||
//! Structures related to dynamics: bodies, joints, etc.
|
||||
|
||||
pub use self::integration_parameters::IntegrationParameters;
|
||||
pub(crate) use self::joint::JointIndex;
|
||||
#[cfg(feature = "dim3")]
|
||||
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, RigidBodyMut, RigidBodySet};
|
||||
// #[cfg(not(feature = "parallel"))]
|
||||
pub(crate) use self::joint::JointGraphEdge;
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
pub(crate) use self::solver::IslandSolver;
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(crate) use self::solver::ParallelIslandSolver;
|
||||
|
||||
mod integration_parameters;
|
||||
mod joint;
|
||||
mod mass_properties;
|
||||
mod mass_properties_ball;
|
||||
mod mass_properties_capsule;
|
||||
mod mass_properties_cuboid;
|
||||
#[cfg(feature = "dim2")]
|
||||
mod mass_properties_polygon;
|
||||
mod rigid_body;
|
||||
mod rigid_body_set;
|
||||
mod solver;
|
||||
441
src/dynamics/rigid_body.rs
Normal file
441
src/dynamics/rigid_body.rs
Normal file
@@ -0,0 +1,441 @@
|
||||
use crate::dynamics::MassProperties;
|
||||
use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
|
||||
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
|
||||
use crate::utils::{WCross, WDot};
|
||||
use num::Zero;
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// The status of a body, governing the way it is affected by external forces.
|
||||
pub enum BodyStatus {
|
||||
/// A `BodyStatus::Dynamic` body can be affected by all external forces.
|
||||
Dynamic,
|
||||
/// A `BodyStatus::Static` body cannot be affected by external forces.
|
||||
Static,
|
||||
/// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled
|
||||
/// by the user at the position level while keeping realistic one-way interaction with dynamic bodies.
|
||||
///
|
||||
/// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body
|
||||
/// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be
|
||||
/// modified by the user and is independent from any contact or joint it is involved in.
|
||||
Kinematic,
|
||||
// Semikinematic, // A kinematic that performs automatic CCD with the static environment toi avoid traversing it?
|
||||
// Disabled,
|
||||
}
|
||||
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// A rigid body.
|
||||
///
|
||||
/// To create a new rigid-body, use the `RigidBodyBuilder` structure.
|
||||
#[derive(Debug)]
|
||||
pub struct RigidBody {
|
||||
/// The world-space position of the rigid-body.
|
||||
pub position: Isometry<f32>,
|
||||
pub(crate) predicted_position: Isometry<f32>,
|
||||
/// The local mass properties of the rigid-body.
|
||||
pub 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>,
|
||||
/// The linear velocity of the rigid-body.
|
||||
pub linvel: Vector<f32>,
|
||||
/// The angular velocity of the rigid-body.
|
||||
pub angvel: AngVector<f32>,
|
||||
pub(crate) linacc: Vector<f32>,
|
||||
pub(crate) angacc: AngVector<f32>,
|
||||
pub(crate) colliders: Vec<ColliderHandle>,
|
||||
/// Whether or not this rigid-body is sleeping.
|
||||
pub activation: ActivationStatus,
|
||||
pub(crate) joint_graph_index: RigidBodyGraphIndex,
|
||||
pub(crate) active_island_id: usize,
|
||||
pub(crate) active_set_id: usize,
|
||||
pub(crate) active_set_offset: usize,
|
||||
pub(crate) active_set_timestamp: u32,
|
||||
/// The status of the body, governing how it is affected by external forces.
|
||||
pub body_status: BodyStatus,
|
||||
}
|
||||
|
||||
impl Clone for RigidBody {
|
||||
fn clone(&self) -> Self {
|
||||
Self {
|
||||
colliders: Vec::new(),
|
||||
joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32),
|
||||
active_island_id: crate::INVALID_USIZE,
|
||||
active_set_id: crate::INVALID_USIZE,
|
||||
active_set_offset: crate::INVALID_USIZE,
|
||||
active_set_timestamp: crate::INVALID_U32,
|
||||
..*self
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl RigidBody {
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
position: Isometry::identity(),
|
||||
predicted_position: Isometry::identity(),
|
||||
mass_properties: MassProperties::zero(),
|
||||
world_com: Point::origin(),
|
||||
world_inv_inertia_sqrt: AngularInertia::zero(),
|
||||
linvel: Vector::zeros(),
|
||||
angvel: na::zero(),
|
||||
linacc: Vector::zeros(),
|
||||
angacc: na::zero(),
|
||||
colliders: Vec::new(),
|
||||
activation: ActivationStatus::new_active(),
|
||||
joint_graph_index: InteractionGraph::<()>::invalid_graph_index(),
|
||||
active_island_id: 0,
|
||||
active_set_id: 0,
|
||||
active_set_offset: 0,
|
||||
active_set_timestamp: 0,
|
||||
body_status: BodyStatus::Dynamic,
|
||||
}
|
||||
}
|
||||
|
||||
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.
|
||||
self.linacc = na::zero();
|
||||
self.angacc = na::zero();
|
||||
}
|
||||
}
|
||||
|
||||
/// The handles of colliders attached to this rigid body.
|
||||
pub fn colliders(&self) -> &[ColliderHandle] {
|
||||
&self.colliders[..]
|
||||
}
|
||||
|
||||
/// Is this rigid body dynamic?
|
||||
///
|
||||
/// A dynamic body can move freely and is affected by forces.
|
||||
pub fn is_dynamic(&self) -> bool {
|
||||
self.body_status == BodyStatus::Dynamic
|
||||
}
|
||||
|
||||
/// Is this rigid body kinematic?
|
||||
///
|
||||
/// A kinematic body can move freely but is not affected by forces.
|
||||
pub fn is_kinematic(&self) -> bool {
|
||||
self.body_status == BodyStatus::Kinematic
|
||||
}
|
||||
|
||||
/// Is this rigid body static?
|
||||
///
|
||||
/// A static body cannot move and is not affected by forces.
|
||||
pub fn is_static(&self) -> bool {
|
||||
self.body_status == BodyStatus::Static
|
||||
}
|
||||
|
||||
/// The mass of this rigid body.
|
||||
///
|
||||
/// Returns zero if this rigid body has an infinite mass.
|
||||
pub fn mass(&self) -> f32 {
|
||||
crate::utils::inv(self.mass_properties.inv_mass)
|
||||
}
|
||||
|
||||
/// Put this rigid body to sleep.
|
||||
///
|
||||
/// A sleeping body no longer moves and is no longer simulated by the physics engine unless
|
||||
/// it is waken up. It can be woken manually with `self.wake_up` or automatically due to
|
||||
/// external forces like contacts.
|
||||
pub fn sleep(&mut self) {
|
||||
self.activation.energy = 0.0;
|
||||
self.activation.sleeping = true;
|
||||
self.linvel = na::zero();
|
||||
self.angvel = na::zero();
|
||||
}
|
||||
|
||||
/// Wakes up this rigid body if it is sleeping.
|
||||
pub fn wake_up(&mut self) {
|
||||
self.activation.sleeping = false;
|
||||
|
||||
if self.activation.energy == 0.0 && self.is_dynamic() {
|
||||
self.activation.energy = self.activation.threshold.abs() * 2.0;
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn update_energy(&mut self) {
|
||||
let mix_factor = 0.01;
|
||||
let new_energy = (1.0 - mix_factor) * self.activation.energy
|
||||
+ mix_factor * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel));
|
||||
self.activation.energy = new_energy.min(self.activation.threshold.abs() * 4.0);
|
||||
}
|
||||
|
||||
/// Is this rigid body sleeping?
|
||||
pub fn is_sleeping(&self) -> bool {
|
||||
self.activation.sleeping
|
||||
}
|
||||
|
||||
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
|
||||
let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
|
||||
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) {
|
||||
self.position = self.integrate_velocity(dt) * self.position;
|
||||
}
|
||||
|
||||
/// Sets the position of this rigid body.
|
||||
pub fn set_position(&mut self, pos: Isometry<f32>) {
|
||||
self.position = pos;
|
||||
|
||||
// TODO: update the predicted position for dynamic bodies too?
|
||||
if self.is_static() {
|
||||
self.predicted_position = pos;
|
||||
}
|
||||
}
|
||||
|
||||
/// 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>) {
|
||||
if self.is_kinematic() {
|
||||
self.predicted_position = pos;
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) {
|
||||
let dpos = self.predicted_position * self.position.inverse();
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
self.angvel = dpos.rotation.angle() * inv_dt;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
self.angvel = dpos.rotation.scaled_axis() * inv_dt;
|
||||
}
|
||||
self.linvel = dpos.translation.vector * inv_dt;
|
||||
}
|
||||
|
||||
pub(crate) fn update_predicted_position(&mut self, dt: f32) {
|
||||
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
|
||||
.mass_properties
|
||||
.world_inv_inertia_sqrt(&self.position.rotation);
|
||||
}
|
||||
|
||||
/*
|
||||
* 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>) {
|
||||
if self.body_status == BodyStatus::Dynamic {
|
||||
self.linacc += force * self.mass_properties.inv_mass;
|
||||
}
|
||||
}
|
||||
|
||||
/// Applies an impulse at the center-of-mass of this rigid-body.
|
||||
pub fn apply_impulse(&mut self, impulse: Vector<f32>) {
|
||||
if self.body_status == BodyStatus::Dynamic {
|
||||
self.linvel += impulse * self.mass_properties.inv_mass;
|
||||
}
|
||||
}
|
||||
|
||||
/// Applies a torque at the center-of-mass of this rigid-body.
|
||||
#[cfg(feature = "dim2")]
|
||||
pub fn apply_torque(&mut self, torque: f32) {
|
||||
if self.body_status == BodyStatus::Dynamic {
|
||||
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
|
||||
}
|
||||
}
|
||||
|
||||
/// Applies a torque at the center-of-mass of this rigid-body.
|
||||
#[cfg(feature = "dim3")]
|
||||
pub fn apply_torque(&mut self, torque: Vector<f32>) {
|
||||
if self.body_status == BodyStatus::Dynamic {
|
||||
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
|
||||
}
|
||||
}
|
||||
|
||||
/// 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) {
|
||||
if self.body_status == BodyStatus::Dynamic {
|
||||
self.angvel +=
|
||||
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
|
||||
}
|
||||
}
|
||||
|
||||
/// 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>) {
|
||||
if self.body_status == BodyStatus::Dynamic {
|
||||
self.angvel +=
|
||||
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
|
||||
}
|
||||
}
|
||||
|
||||
/// 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>) {
|
||||
let torque = (point - self.world_com).gcross(force);
|
||||
self.apply_force(force);
|
||||
self.apply_torque(torque);
|
||||
}
|
||||
|
||||
/// 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>) {
|
||||
let torque_impulse = (point - self.world_com).gcross(impulse);
|
||||
self.apply_impulse(impulse);
|
||||
self.apply_torque_impulse(torque_impulse);
|
||||
}
|
||||
}
|
||||
|
||||
/// A builder for rigid-bodies.
|
||||
pub struct RigidBodyBuilder {
|
||||
position: Isometry<f32>,
|
||||
linvel: Vector<f32>,
|
||||
angvel: AngVector<f32>,
|
||||
body_status: BodyStatus,
|
||||
can_sleep: bool,
|
||||
}
|
||||
|
||||
impl RigidBodyBuilder {
|
||||
/// Initialize a new builder for a rigid body which is either static, dynamic, or kinematic.
|
||||
pub fn new(body_status: BodyStatus) -> Self {
|
||||
Self {
|
||||
position: Isometry::identity(),
|
||||
linvel: Vector::zeros(),
|
||||
angvel: na::zero(),
|
||||
body_status,
|
||||
can_sleep: true,
|
||||
}
|
||||
}
|
||||
|
||||
/// Initializes the builder of a new static rigid body.
|
||||
pub fn new_static() -> Self {
|
||||
Self::new(BodyStatus::Static)
|
||||
}
|
||||
|
||||
/// Initializes the builder of a new kinematic rigid body.
|
||||
pub fn new_kinematic() -> Self {
|
||||
Self::new(BodyStatus::Kinematic)
|
||||
}
|
||||
|
||||
/// Initializes the builder of a new dynamic rigid body.
|
||||
pub fn new_dynamic() -> Self {
|
||||
Self::new(BodyStatus::Dynamic)
|
||||
}
|
||||
|
||||
/// Sets the initial translation of the rigid-body to be created.
|
||||
#[cfg(feature = "dim2")]
|
||||
pub fn translation(mut self, x: f32, y: f32) -> Self {
|
||||
self.position.translation.x = x;
|
||||
self.position.translation.y = y;
|
||||
self
|
||||
}
|
||||
|
||||
/// 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 {
|
||||
self.position.translation.x = x;
|
||||
self.position.translation.y = y;
|
||||
self.position.translation.z = z;
|
||||
self
|
||||
}
|
||||
|
||||
/// Sets the initial orientation of the rigid-body to be created.
|
||||
pub fn rotation(mut self, angle: AngVector<f32>) -> 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 {
|
||||
self.position = pos;
|
||||
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 {
|
||||
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 {
|
||||
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 {
|
||||
self.angvel = angvel;
|
||||
self
|
||||
}
|
||||
|
||||
/// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
|
||||
pub fn can_sleep(mut self, can_sleep: bool) -> Self {
|
||||
self.can_sleep = can_sleep;
|
||||
self
|
||||
}
|
||||
|
||||
/// Build a new rigid-body with the parameters configured with this builder.
|
||||
pub fn build(&self) -> RigidBody {
|
||||
let mut rb = RigidBody::new();
|
||||
rb.predicted_position = self.position; // FIXME: compute the correct value?
|
||||
rb.set_position(self.position);
|
||||
rb.linvel = self.linvel;
|
||||
rb.angvel = self.angvel;
|
||||
rb.body_status = self.body_status;
|
||||
|
||||
if !self.can_sleep {
|
||||
rb.activation.threshold = -1.0;
|
||||
}
|
||||
|
||||
rb
|
||||
}
|
||||
}
|
||||
|
||||
/// The activation status of a body.
|
||||
///
|
||||
/// This controls whether a body is sleeping or not.
|
||||
/// If the threshold is negative, the body never sleeps.
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
#[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,
|
||||
/// The current pseudo-kinetic energy of the body.
|
||||
pub energy: f32,
|
||||
/// 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 {
|
||||
0.01
|
||||
}
|
||||
|
||||
/// Create a new activation status initialised with the default activation threshold and is active.
|
||||
pub fn new_active() -> Self {
|
||||
ActivationStatus {
|
||||
threshold: Self::default_threshold(),
|
||||
energy: Self::default_threshold() * 4.0,
|
||||
sleeping: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a new activation status initialised with the default activation threshold and is inactive.
|
||||
pub fn new_inactive() -> Self {
|
||||
ActivationStatus {
|
||||
threshold: Self::default_threshold(),
|
||||
energy: 0.0,
|
||||
sleeping: true,
|
||||
}
|
||||
}
|
||||
|
||||
/// Returns `true` if the body is not asleep.
|
||||
#[inline]
|
||||
pub fn is_active(&self) -> bool {
|
||||
self.energy != 0.0
|
||||
}
|
||||
}
|
||||
518
src/dynamics/rigid_body_set.rs
Normal file
518
src/dynamics/rigid_body_set.rs
Normal file
@@ -0,0 +1,518 @@
|
||||
#[cfg(feature = "parallel")]
|
||||
use rayon::prelude::*;
|
||||
|
||||
use crate::data::arena::Arena;
|
||||
use crate::dynamics::{Joint, RigidBody};
|
||||
use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
|
||||
use crossbeam::channel::{Receiver, Sender};
|
||||
use std::ops::{Deref, DerefMut, Index, IndexMut};
|
||||
|
||||
/// A mutable reference to a rigid-body.
|
||||
pub struct RigidBodyMut<'a> {
|
||||
rb: &'a mut RigidBody,
|
||||
was_sleeping: bool,
|
||||
handle: RigidBodyHandle,
|
||||
sender: &'a Sender<RigidBodyHandle>,
|
||||
}
|
||||
|
||||
impl<'a> RigidBodyMut<'a> {
|
||||
fn new(
|
||||
handle: RigidBodyHandle,
|
||||
rb: &'a mut RigidBody,
|
||||
sender: &'a Sender<RigidBodyHandle>,
|
||||
) -> Self {
|
||||
Self {
|
||||
was_sleeping: rb.is_sleeping(),
|
||||
handle,
|
||||
sender,
|
||||
rb,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> Deref for RigidBodyMut<'a> {
|
||||
type Target = RigidBody;
|
||||
fn deref(&self) -> &RigidBody {
|
||||
&*self.rb
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> DerefMut for RigidBodyMut<'a> {
|
||||
fn deref_mut(&mut self) -> &mut RigidBody {
|
||||
self.rb
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> Drop for RigidBodyMut<'a> {
|
||||
fn drop(&mut self) {
|
||||
if self.was_sleeping && !self.rb.is_sleeping() {
|
||||
self.sender.send(self.handle).unwrap();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// The unique handle of a rigid body added to a `RigidBodySet`.
|
||||
pub type RigidBodyHandle = crate::data::arena::Index;
|
||||
|
||||
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
/// A pair of rigid body handles.
|
||||
pub struct BodyPair {
|
||||
/// The first rigid body handle.
|
||||
pub body1: RigidBodyHandle,
|
||||
/// The second rigid body handle.
|
||||
pub body2: RigidBodyHandle,
|
||||
}
|
||||
|
||||
impl BodyPair {
|
||||
pub(crate) 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))]
|
||||
/// A set of rigid bodies that can be handled by a physics pipeline.
|
||||
pub struct RigidBodySet {
|
||||
// NOTE: the pub(crate) are needed by the broad phase
|
||||
// to avoid borrowing issues. It is also needed for
|
||||
// parallelism because the `Receiver` breaks the Sync impl.
|
||||
// Could we avoid this?
|
||||
pub(crate) bodies: Arena<RigidBody>,
|
||||
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
|
||||
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
|
||||
// Set of inactive bodies which have been modified.
|
||||
// This typically include static bodies which have been modified.
|
||||
pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
|
||||
pub(crate) active_islands: Vec<usize>,
|
||||
active_set_timestamp: u32,
|
||||
#[cfg_attr(feature = "serde-serialize", serde(skip))]
|
||||
can_sleep: Vec<RigidBodyHandle>, // Workspace.
|
||||
#[cfg_attr(feature = "serde-serialize", serde(skip))]
|
||||
stack: Vec<RigidBodyHandle>, // Workspace.
|
||||
#[cfg_attr(
|
||||
feature = "serde-serialize",
|
||||
serde(skip, default = "crossbeam::channel::unbounded")
|
||||
)]
|
||||
activation_channel: (Sender<RigidBodyHandle>, Receiver<RigidBodyHandle>),
|
||||
}
|
||||
|
||||
impl RigidBodySet {
|
||||
/// Create a new empty set of rigid bodies.
|
||||
pub fn new() -> Self {
|
||||
RigidBodySet {
|
||||
bodies: Arena::new(),
|
||||
active_dynamic_set: Vec::new(),
|
||||
active_kinematic_set: Vec::new(),
|
||||
modified_inactive_set: Vec::new(),
|
||||
active_islands: Vec::new(),
|
||||
active_set_timestamp: 0,
|
||||
can_sleep: Vec::new(),
|
||||
stack: Vec::new(),
|
||||
activation_channel: crossbeam::channel::unbounded(),
|
||||
}
|
||||
}
|
||||
|
||||
/// 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()
|
||||
}
|
||||
|
||||
pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
|
||||
let mut rb = &mut self.bodies[handle];
|
||||
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
|
||||
rb.active_set_id = self.active_dynamic_set.len();
|
||||
self.active_dynamic_set.push(handle);
|
||||
}
|
||||
}
|
||||
|
||||
/// Is the given body handle valid?
|
||||
pub fn contains(&self, handle: RigidBodyHandle) -> bool {
|
||||
self.bodies.contains(handle)
|
||||
}
|
||||
|
||||
/// Insert a rigid body into this set and retrieve its handle.
|
||||
pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
|
||||
let handle = self.bodies.insert(rb);
|
||||
let rb = &mut self.bodies[handle];
|
||||
rb.active_set_id = self.active_dynamic_set.len();
|
||||
|
||||
if !rb.is_sleeping() && rb.is_dynamic() {
|
||||
self.active_dynamic_set.push(handle);
|
||||
}
|
||||
|
||||
if rb.is_kinematic() {
|
||||
self.active_kinematic_set.push(handle);
|
||||
}
|
||||
|
||||
if !rb.is_dynamic() {
|
||||
self.modified_inactive_set.push(handle);
|
||||
}
|
||||
|
||||
handle
|
||||
}
|
||||
|
||||
pub(crate) fn num_islands(&self) -> usize {
|
||||
self.active_islands.len() - 1
|
||||
}
|
||||
|
||||
pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
|
||||
let rb = self.bodies.remove(handle)?;
|
||||
// Remove this body from the active dynamic set.
|
||||
if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
|
||||
self.active_dynamic_set.swap_remove(rb.active_set_id);
|
||||
|
||||
if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) {
|
||||
self.bodies[*replacement].active_set_id = rb.active_set_id;
|
||||
}
|
||||
}
|
||||
|
||||
Some(rb)
|
||||
}
|
||||
|
||||
/// Forces the specified rigid-body to wake up if it is dynamic.
|
||||
pub fn wake_up(&mut self, handle: RigidBodyHandle) {
|
||||
if let Some(rb) = self.bodies.get_mut(handle) {
|
||||
if rb.is_dynamic() {
|
||||
rb.wake_up();
|
||||
|
||||
if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
|
||||
rb.active_set_id = self.active_dynamic_set.len();
|
||||
self.active_dynamic_set.push(handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Gets the rigid-body with the given handle without a known generation.
|
||||
///
|
||||
/// This is useful when you know you want the rigid-body at position `i` but
|
||||
/// don't know what is its current generation number. Generation numbers are
|
||||
/// used to protect from the ABA problem because the rigid-body position `i`
|
||||
/// are recycled between two insertion and a removal.
|
||||
///
|
||||
/// 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)
|
||||
}
|
||||
|
||||
/// Gets a mutable reference to the rigid-body with the given handle without a known generation.
|
||||
///
|
||||
/// This is useful when you know you want the rigid-body at position `i` but
|
||||
/// don't know what is its current generation number. Generation numbers are
|
||||
/// used to protect from the ABA problem because the rigid-body position `i`
|
||||
/// are recycled between two insertion and a removal.
|
||||
///
|
||||
/// Using this is discouraged in favor of `self.get_mut(handle)` which does not
|
||||
/// suffer form the ABA problem.
|
||||
pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
|
||||
self.bodies.get_unknown_gen_mut(i)
|
||||
}
|
||||
|
||||
/// Gets the rigid-body with the given handle.
|
||||
pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
|
||||
self.bodies.get(handle)
|
||||
}
|
||||
|
||||
/// Gets a mutable reference to the rigid-body with the given handle.
|
||||
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<RigidBodyMut> {
|
||||
let sender = &self.activation_channel.0;
|
||||
self.bodies
|
||||
.get_mut(handle)
|
||||
.map(|rb| RigidBodyMut::new(handle, rb, sender))
|
||||
}
|
||||
|
||||
pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
|
||||
self.bodies.get_mut(handle)
|
||||
}
|
||||
|
||||
pub(crate) fn get2_mut_internal(
|
||||
&mut self,
|
||||
h1: RigidBodyHandle,
|
||||
h2: RigidBodyHandle,
|
||||
) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
|
||||
self.bodies.get2_mut(h1, h2)
|
||||
}
|
||||
|
||||
/// Iterates through all the rigid-bodies on this set.
|
||||
pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
|
||||
self.bodies.iter()
|
||||
}
|
||||
|
||||
/// Iterates mutably through all the rigid-bodies on this set.
|
||||
pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyMut)> {
|
||||
let sender = &self.activation_channel.0;
|
||||
self.bodies
|
||||
.iter_mut()
|
||||
.map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
|
||||
}
|
||||
|
||||
/// Iter through all the active dynamic rigid-bodies on this set.
|
||||
pub fn iter_active_dynamic<'a>(
|
||||
&'a self,
|
||||
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
|
||||
let bodies: &'a _ = &self.bodies;
|
||||
self.active_dynamic_set
|
||||
.iter()
|
||||
.filter_map(move |h| Some((*h, bodies.get(*h)?)))
|
||||
}
|
||||
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
pub(crate) fn iter_active_island<'a>(
|
||||
&'a self,
|
||||
island_id: usize,
|
||||
) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
|
||||
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
|
||||
let bodies: &'a _ = &self.bodies;
|
||||
self.active_dynamic_set[island_range]
|
||||
.iter()
|
||||
.filter_map(move |h| Some((*h, bodies.get(*h)?)))
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pub(crate) fn foreach_active_body_mut_internal(
|
||||
&mut self,
|
||||
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
|
||||
) {
|
||||
for handle in &self.active_dynamic_set {
|
||||
if let Some(rb) = self.bodies.get_mut(*handle) {
|
||||
f(*handle, rb)
|
||||
}
|
||||
}
|
||||
|
||||
for handle in &self.active_kinematic_set {
|
||||
if let Some(rb) = self.bodies.get_mut(*handle) {
|
||||
f(*handle, rb)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pub(crate) fn foreach_active_dynamic_body_mut_internal(
|
||||
&mut self,
|
||||
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
|
||||
) {
|
||||
for handle in &self.active_dynamic_set {
|
||||
if let Some(rb) = self.bodies.get_mut(*handle) {
|
||||
f(*handle, rb)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pub(crate) fn foreach_active_kinematic_body_mut_internal(
|
||||
&mut self,
|
||||
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
|
||||
) {
|
||||
for handle in &self.active_kinematic_set {
|
||||
if let Some(rb) = self.bodies.get_mut(*handle) {
|
||||
f(*handle, rb)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
pub(crate) fn foreach_active_island_body_mut_internal(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
|
||||
) {
|
||||
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) {
|
||||
f(*handle, rb)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "parallel")]
|
||||
#[inline(always)]
|
||||
#[allow(dead_code)]
|
||||
pub(crate) fn foreach_active_island_body_mut_internal_parallel(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
|
||||
) {
|
||||
use std::sync::atomic::Ordering;
|
||||
|
||||
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
|
||||
let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
|
||||
self.active_dynamic_set[island_range]
|
||||
.par_iter()
|
||||
.for_each_init(
|
||||
|| bodies.load(Ordering::Relaxed),
|
||||
|bodies, handle| {
|
||||
let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
|
||||
if let Some(rb) = bodies.get_mut(*handle) {
|
||||
f(*handle, rb)
|
||||
}
|
||||
},
|
||||
);
|
||||
}
|
||||
|
||||
// pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] {
|
||||
// &self.active_dynamic_set
|
||||
// }
|
||||
|
||||
pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
|
||||
self.active_islands[island_id]..self.active_islands[island_id + 1]
|
||||
}
|
||||
|
||||
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
|
||||
&self.active_dynamic_set[self.active_island_range(island_id)]
|
||||
}
|
||||
|
||||
pub(crate) fn maintain_active_set(&mut self) {
|
||||
for handle in self.activation_channel.1.try_iter() {
|
||||
if let Some(rb) = self.bodies.get_mut(handle) {
|
||||
// Push the body to the active set if it is not
|
||||
// sleeping and if it is not already inside of the active set.
|
||||
if !rb.is_sleeping() // May happen if the body was put to sleep manually.
|
||||
&& rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
|
||||
&& self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
|
||||
{
|
||||
rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
|
||||
self.active_dynamic_set.push(handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn update_active_set_with_contacts(
|
||||
&mut self,
|
||||
colliders: &ColliderSet,
|
||||
contact_graph: &InteractionGraph<ContactPair>,
|
||||
joint_graph: &InteractionGraph<Joint>,
|
||||
min_island_size: usize,
|
||||
) {
|
||||
assert!(
|
||||
min_island_size > 0,
|
||||
"The minimum island size must be at least 1."
|
||||
);
|
||||
|
||||
// Update the energy of every rigid body and
|
||||
// keep only those that may not sleep.
|
||||
// let t = instant::now();
|
||||
self.active_set_timestamp += 1;
|
||||
self.stack.clear();
|
||||
self.can_sleep.clear();
|
||||
|
||||
// NOTE: the `.rev()` is here so that two successive timesteps preserve
|
||||
// the order of the bodies in the `active_dynamic_set` vec. This reversal
|
||||
// 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];
|
||||
rb.update_energy();
|
||||
if rb.activation.energy <= rb.activation.threshold {
|
||||
// Mark them as sleeping for now. This will
|
||||
// be set to false during the graph traversal
|
||||
// if it should not be put to sleep.
|
||||
rb.activation.sleeping = true;
|
||||
self.can_sleep.push(h);
|
||||
} else {
|
||||
self.stack.push(h);
|
||||
}
|
||||
}
|
||||
|
||||
// println!("Selection: {}", instant::now() - t);
|
||||
|
||||
// let t = instant::now();
|
||||
// Propagation of awake state and awake island computation through the
|
||||
// traversal of the interaction graph.
|
||||
self.active_islands.clear();
|
||||
self.active_islands.push(0);
|
||||
|
||||
// The max avoid underflow when the stack is empty.
|
||||
let mut island_marker = self.stack.len().max(1) - 1;
|
||||
|
||||
while let Some(handle) = self.stack.pop() {
|
||||
let rb = &mut self.bodies[handle];
|
||||
|
||||
if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
|
||||
// We already visited this body and its neighbors.
|
||||
// Also, we don't propagate awake state through static bodies.
|
||||
continue;
|
||||
} else if self.stack.len() < island_marker {
|
||||
if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
|
||||
>= min_island_size
|
||||
{
|
||||
// We are starting a new island.
|
||||
self.active_islands.push(self.active_dynamic_set.len());
|
||||
}
|
||||
|
||||
island_marker = self.stack.len();
|
||||
}
|
||||
|
||||
rb.wake_up();
|
||||
rb.active_island_id = self.active_islands.len() - 1;
|
||||
rb.active_set_id = self.active_dynamic_set.len();
|
||||
rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
|
||||
rb.active_set_timestamp = self.active_set_timestamp;
|
||||
self.active_dynamic_set.push(handle);
|
||||
|
||||
// Read all the contacts and push objects touching this one.
|
||||
for collider_handle in &rb.colliders {
|
||||
let collider = &colliders[*collider_handle];
|
||||
|
||||
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
|
||||
for manifold in &inter.2.manifolds {
|
||||
if manifold.num_active_contacts() > 0 {
|
||||
let other =
|
||||
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
|
||||
let other_body = colliders[other].parent;
|
||||
self.stack.push(other_body);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
|
||||
let other = crate::utils::other_handle((inter.0, inter.1), handle);
|
||||
self.stack.push(other);
|
||||
}
|
||||
}
|
||||
|
||||
self.active_islands.push(self.active_dynamic_set.len());
|
||||
// println!(
|
||||
// "Extraction: {}, num islands: {}",
|
||||
// instant::now() - t,
|
||||
// self.active_islands.len() - 1
|
||||
// );
|
||||
|
||||
// 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];
|
||||
if b.activation.sleeping {
|
||||
b.sleep();
|
||||
}
|
||||
}
|
||||
// println!("Activation: {}", instant::now() - t);
|
||||
}
|
||||
}
|
||||
|
||||
impl Index<RigidBodyHandle> for RigidBodySet {
|
||||
type Output = RigidBody;
|
||||
|
||||
fn index(&self, index: RigidBodyHandle) -> &RigidBody {
|
||||
&self.bodies[index]
|
||||
}
|
||||
}
|
||||
|
||||
impl IndexMut<RigidBodyHandle> for RigidBodySet {
|
||||
fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
|
||||
&mut self.bodies[index]
|
||||
}
|
||||
}
|
||||
70
src/dynamics/solver/categorization.rs
Normal file
70
src/dynamics/solver/categorization.rs
Normal file
@@ -0,0 +1,70 @@
|
||||
use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
|
||||
|
||||
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(
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
out_ground: &mut Vec<ContactManifoldIndex>,
|
||||
out_not_ground: &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() {
|
||||
out_ground.push(*manifold_i)
|
||||
} else {
|
||||
out_not_ground.push(*manifold_i)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn categorize_joints(
|
||||
bodies: &RigidBodySet,
|
||||
joints: &[JointGraphEdge],
|
||||
joint_indices: &[JointIndex],
|
||||
ground_joints: &mut Vec<JointIndex>,
|
||||
nonground_joints: &mut Vec<JointIndex>,
|
||||
) {
|
||||
for joint_i in joint_indices {
|
||||
let joint = &joints[*joint_i].weight;
|
||||
let rb1 = &bodies[joint.body1];
|
||||
let rb2 = &bodies[joint.body2];
|
||||
|
||||
if !rb1.is_dynamic() || !rb2.is_dynamic() {
|
||||
ground_joints.push(*joint_i);
|
||||
} else {
|
||||
nonground_joints.push(*joint_i);
|
||||
}
|
||||
}
|
||||
}
|
||||
18
src/dynamics/solver/delta_vel.rs
Normal file
18
src/dynamics/solver/delta_vel.rs
Normal file
@@ -0,0 +1,18 @@
|
||||
use crate::math::{AngVector, Vector};
|
||||
use na::{Scalar, SimdRealField};
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
//#[repr(align(64))]
|
||||
pub(crate) struct DeltaVel<N: Scalar> {
|
||||
pub linear: Vector<N>,
|
||||
pub angular: AngVector<N>,
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> DeltaVel<N> {
|
||||
pub fn zero() -> Self {
|
||||
Self {
|
||||
linear: na::zero(),
|
||||
angular: na::zero(),
|
||||
}
|
||||
}
|
||||
}
|
||||
434
src/dynamics/solver/interaction_groups.rs
Normal file
434
src/dynamics/solver/interaction_groups.rs
Normal file
@@ -0,0 +1,434 @@
|
||||
use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use {
|
||||
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
|
||||
vec_map::VecMap,
|
||||
};
|
||||
|
||||
pub(crate) trait PairInteraction {
|
||||
fn body_pair(&self) -> BodyPair;
|
||||
}
|
||||
|
||||
impl<'a> PairInteraction for &'a mut ContactManifold {
|
||||
fn body_pair(&self) -> BodyPair {
|
||||
self.body_pair
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> PairInteraction for JointGraphEdge {
|
||||
fn body_pair(&self) -> BodyPair {
|
||||
BodyPair::new(self.weight.body1, self.weight.body2)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(crate) struct ParallelInteractionGroups {
|
||||
bodies_color: Vec<u128>, // Workspace.
|
||||
interaction_indices: Vec<usize>, // Workspace.
|
||||
interaction_colors: Vec<usize>, // Workspace.
|
||||
sorted_interactions: Vec<usize>,
|
||||
groups: Vec<usize>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "parallel")]
|
||||
impl ParallelInteractionGroups {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
bodies_color: Vec::new(),
|
||||
interaction_indices: Vec::new(),
|
||||
interaction_colors: Vec::new(),
|
||||
sorted_interactions: Vec::new(),
|
||||
groups: Vec::new(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn group(&self, i: usize) -> &[usize] {
|
||||
let range = self.groups[i]..self.groups[i + 1];
|
||||
&self.sorted_interactions[range]
|
||||
}
|
||||
pub fn num_groups(&self) -> usize {
|
||||
self.groups.len() - 1
|
||||
}
|
||||
|
||||
pub fn group_interactions<Interaction: PairInteraction>(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
interactions: &[Interaction],
|
||||
interaction_indices: &[usize],
|
||||
) {
|
||||
let num_island_bodies = bodies.active_island(island_id).len();
|
||||
self.bodies_color.clear();
|
||||
self.interaction_indices.clear();
|
||||
self.groups.clear();
|
||||
self.sorted_interactions.clear();
|
||||
self.interaction_colors.clear();
|
||||
|
||||
let mut color_len = [0; 128];
|
||||
self.bodies_color.resize(num_island_bodies, 0u128);
|
||||
self.interaction_indices
|
||||
.extend_from_slice(interaction_indices);
|
||||
self.interaction_colors.resize(interaction_indices.len(), 0);
|
||||
let bcolors = &mut self.bodies_color;
|
||||
|
||||
for (interaction_id, color) in self
|
||||
.interaction_indices
|
||||
.iter()
|
||||
.zip(self.interaction_colors.iter_mut())
|
||||
{
|
||||
let body_pair = interactions[*interaction_id].body_pair();
|
||||
let rb1 = &bodies[body_pair.body1];
|
||||
let rb2 = &bodies[body_pair.body2];
|
||||
|
||||
match (rb1.is_static(), rb2.is_static()) {
|
||||
(false, false) => {
|
||||
let color_mask =
|
||||
bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset];
|
||||
*color = (!color_mask).trailing_zeros() as usize;
|
||||
color_len[*color] += 1;
|
||||
bcolors[rb1.active_set_offset] |= 1 << *color;
|
||||
bcolors[rb2.active_set_offset] |= 1 << *color;
|
||||
}
|
||||
(true, false) => {
|
||||
let color_mask = bcolors[rb2.active_set_offset];
|
||||
*color = (!color_mask).trailing_zeros() as usize;
|
||||
color_len[*color] += 1;
|
||||
bcolors[rb2.active_set_offset] |= 1 << *color;
|
||||
}
|
||||
(false, true) => {
|
||||
let color_mask = bcolors[rb1.active_set_offset];
|
||||
*color = (!color_mask).trailing_zeros() as usize;
|
||||
color_len[*color] += 1;
|
||||
bcolors[rb1.active_set_offset] |= 1 << *color;
|
||||
}
|
||||
(true, true) => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
let mut sort_offsets = [0; 128];
|
||||
let mut last_offset = 0;
|
||||
|
||||
for i in 0..128 {
|
||||
if color_len[i] == 0 {
|
||||
break;
|
||||
}
|
||||
|
||||
self.groups.push(last_offset);
|
||||
sort_offsets[i] = last_offset;
|
||||
last_offset += color_len[i];
|
||||
}
|
||||
|
||||
self.sorted_interactions
|
||||
.resize(interaction_indices.len(), 0);
|
||||
|
||||
for (interaction_id, color) in interaction_indices
|
||||
.iter()
|
||||
.zip(self.interaction_colors.iter())
|
||||
{
|
||||
self.sorted_interactions[sort_offsets[*color]] = *interaction_id;
|
||||
sort_offsets[*color] += 1;
|
||||
}
|
||||
|
||||
self.groups.push(self.sorted_interactions.len());
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct InteractionGroups {
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
buckets: VecMap<([usize; SIMD_WIDTH], usize)>,
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
body_masks: Vec<u128>,
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub grouped_interactions: Vec<usize>,
|
||||
pub nongrouped_interactions: Vec<usize>,
|
||||
}
|
||||
|
||||
impl InteractionGroups {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
buckets: VecMap::new(),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
body_masks: Vec::new(),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
grouped_interactions: Vec::new(),
|
||||
nongrouped_interactions: Vec::new(),
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: there is a lot of duplicated code with group_manifolds here.
|
||||
// But we don't refactor just now because we may end up with distinct
|
||||
// grouping strategies in the future.
|
||||
#[cfg(not(feature = "simd-is-enabled"))]
|
||||
pub fn group_joints(
|
||||
&mut self,
|
||||
_island_id: usize,
|
||||
_bodies: &RigidBodySet,
|
||||
_interactions: &[JointGraphEdge],
|
||||
interaction_indices: &[JointIndex],
|
||||
) {
|
||||
self.nongrouped_interactions
|
||||
.extend_from_slice(interaction_indices);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn group_joints(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
interactions: &[JointGraphEdge],
|
||||
interaction_indices: &[JointIndex],
|
||||
) {
|
||||
// NOTE: in 3D we have up to 10 different joint types.
|
||||
// In 2D we only have 5 joint types.
|
||||
#[cfg(feature = "dim3")]
|
||||
const NUM_JOINT_TYPES: usize = 10;
|
||||
#[cfg(feature = "dim2")]
|
||||
const NUM_JOINT_TYPES: usize = 5;
|
||||
|
||||
// The j-th bit of joint_type_conflicts[i] indicates that the
|
||||
// j-th bucket contains a joint with a type different than `i`.
|
||||
let mut joint_type_conflicts = [0u128; NUM_JOINT_TYPES];
|
||||
|
||||
// Note: each bit of a body mask indicates what bucket already contains
|
||||
// a constraints involving this body.
|
||||
// FIXME: currently, this is a bit overconservative because when a bucket
|
||||
// is full, we don't clear the corresponding body mask bit. This may result
|
||||
// in less grouped constraints.
|
||||
self.body_masks
|
||||
.resize(bodies.active_island(island_id).len(), 0u128);
|
||||
|
||||
// NOTE: each bit of the occupied mask indicates what bucket already
|
||||
// contains at least one constraint.
|
||||
let mut occupied_mask = 0u128;
|
||||
|
||||
for interaction_i in interaction_indices {
|
||||
let interaction = &interactions[*interaction_i].weight;
|
||||
let body1 = &bodies[interaction.body1];
|
||||
let body2 = &bodies[interaction.body2];
|
||||
let is_static1 = !body1.is_dynamic();
|
||||
let is_static2 = !body2.is_dynamic();
|
||||
|
||||
if is_static1 && is_static2 {
|
||||
continue;
|
||||
}
|
||||
|
||||
let ijoint = interaction.params.type_id();
|
||||
let i1 = body1.active_set_offset;
|
||||
let i2 = body2.active_set_offset;
|
||||
let conflicts =
|
||||
self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
|
||||
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
|
||||
let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
|
||||
|
||||
let target_index = if conflictfree_occupied_targets != 0 {
|
||||
// Try to fill partial WContacts first.
|
||||
conflictfree_occupied_targets.trailing_zeros()
|
||||
} else {
|
||||
conflictfree_targets.trailing_zeros()
|
||||
};
|
||||
|
||||
if target_index == 128 {
|
||||
// The interaction conflicts with every bucket we can manage.
|
||||
// So push it in an nongrouped interaction list that won't be combined with
|
||||
// any other interactions.
|
||||
self.nongrouped_interactions.push(*interaction_i);
|
||||
continue;
|
||||
}
|
||||
|
||||
let target_mask_bit = 1 << target_index;
|
||||
|
||||
let bucket = self
|
||||
.buckets
|
||||
.entry(target_index as usize)
|
||||
.or_insert_with(|| ([0; SIMD_WIDTH], 0));
|
||||
|
||||
if bucket.1 == SIMD_LAST_INDEX {
|
||||
// We completed our group.
|
||||
(bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
|
||||
self.grouped_interactions.extend_from_slice(&bucket.0);
|
||||
bucket.1 = 0;
|
||||
occupied_mask &= !target_mask_bit;
|
||||
|
||||
for k in 0..NUM_JOINT_TYPES {
|
||||
joint_type_conflicts[k] &= !target_mask_bit;
|
||||
}
|
||||
} else {
|
||||
(bucket.0)[bucket.1] = *interaction_i;
|
||||
bucket.1 += 1;
|
||||
occupied_mask |= target_mask_bit;
|
||||
|
||||
for k in 0..ijoint {
|
||||
joint_type_conflicts[k] |= target_mask_bit;
|
||||
}
|
||||
for k in ijoint + 1..NUM_JOINT_TYPES {
|
||||
joint_type_conflicts[k] |= target_mask_bit;
|
||||
}
|
||||
}
|
||||
|
||||
// NOTE: static bodies don't transmit forces. Therefore they don't
|
||||
// imply any interaction conflicts.
|
||||
if !is_static1 {
|
||||
self.body_masks[i1] |= target_mask_bit;
|
||||
}
|
||||
|
||||
if !is_static2 {
|
||||
self.body_masks[i2] |= target_mask_bit;
|
||||
}
|
||||
}
|
||||
|
||||
self.nongrouped_interactions.extend(
|
||||
self.buckets
|
||||
.values()
|
||||
.flat_map(|e| e.0.iter().take(e.1).copied()),
|
||||
);
|
||||
self.buckets.clear();
|
||||
self.body_masks.iter_mut().for_each(|e| *e = 0);
|
||||
|
||||
assert!(
|
||||
self.grouped_interactions.len() % SIMD_WIDTH == 0,
|
||||
"Invalid SIMD contact grouping."
|
||||
);
|
||||
|
||||
// println!(
|
||||
// "Num grouped interactions: {}, nongrouped: {}",
|
||||
// self.grouped_interactions.len(),
|
||||
// self.nongrouped_interactions.len()
|
||||
// );
|
||||
}
|
||||
|
||||
pub fn clear_groups(&mut self) {
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
self.grouped_interactions.clear();
|
||||
self.nongrouped_interactions.clear();
|
||||
}
|
||||
|
||||
#[cfg(not(feature = "simd-is-enabled"))]
|
||||
pub fn group_manifolds(
|
||||
&mut self,
|
||||
_island_id: usize,
|
||||
_bodies: &RigidBodySet,
|
||||
_interactions: &[&mut ContactManifold],
|
||||
interaction_indices: &[ContactManifoldIndex],
|
||||
) {
|
||||
self.nongrouped_interactions
|
||||
.extend_from_slice(interaction_indices);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn group_manifolds(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
interactions: &[&mut ContactManifold],
|
||||
interaction_indices: &[ContactManifoldIndex],
|
||||
) {
|
||||
// Note: each bit of a body mask indicates what bucket already contains
|
||||
// a constraints involving this body.
|
||||
// FIXME: currently, this is a bit overconservative because when a bucket
|
||||
// is full, we don't clear the corresponding body mask bit. This may result
|
||||
// in less grouped contacts.
|
||||
// NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
|
||||
self.body_masks
|
||||
.resize(bodies.active_island(island_id).len(), 0u128);
|
||||
|
||||
// NOTE: each bit of the occupied mask indicates what bucket already
|
||||
// contains at least one constraint.
|
||||
let mut occupied_mask = 0u128;
|
||||
let max_interaction_points = interaction_indices
|
||||
.iter()
|
||||
.map(|i| interactions[*i].num_active_contacts())
|
||||
.max()
|
||||
.unwrap_or(1);
|
||||
|
||||
// FIXME: find a way to reduce the number of iteration.
|
||||
// There must be a way to iterate just once on every interaction indices
|
||||
// instead of MAX_MANIFOLD_POINTS times.
|
||||
for k in 1..=max_interaction_points {
|
||||
for interaction_i in interaction_indices {
|
||||
let interaction = &interactions[*interaction_i];
|
||||
|
||||
// FIXME: how could we avoid iterating
|
||||
// on each interaction at every iteration on k?
|
||||
if interaction.num_active_contacts() != k {
|
||||
continue;
|
||||
}
|
||||
|
||||
let body1 = &bodies[interaction.body_pair.body1];
|
||||
let body2 = &bodies[interaction.body_pair.body2];
|
||||
let is_static1 = !body1.is_dynamic();
|
||||
let is_static2 = !body2.is_dynamic();
|
||||
|
||||
// FIXME: don't generate interactions between static bodies in the first place.
|
||||
if is_static1 && is_static2 {
|
||||
continue;
|
||||
}
|
||||
|
||||
let i1 = body1.active_set_offset;
|
||||
let i2 = body2.active_set_offset;
|
||||
let conflicts = self.body_masks[i1] | self.body_masks[i2];
|
||||
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
|
||||
let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
|
||||
|
||||
let target_index = if conflictfree_occupied_targets != 0 {
|
||||
// Try to fill partial WContacts first.
|
||||
conflictfree_occupied_targets.trailing_zeros()
|
||||
} else {
|
||||
conflictfree_targets.trailing_zeros()
|
||||
};
|
||||
|
||||
if target_index == 128 {
|
||||
// The interaction conflicts with every bucket we can manage.
|
||||
// So push it in an nongrouped interaction list that won't be combined with
|
||||
// any other interactions.
|
||||
self.nongrouped_interactions.push(*interaction_i);
|
||||
continue;
|
||||
}
|
||||
|
||||
let target_mask_bit = 1 << target_index;
|
||||
|
||||
let bucket = self
|
||||
.buckets
|
||||
.entry(target_index as usize)
|
||||
.or_insert_with(|| ([0; SIMD_WIDTH], 0));
|
||||
|
||||
if bucket.1 == SIMD_LAST_INDEX {
|
||||
// We completed our group.
|
||||
(bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
|
||||
self.grouped_interactions.extend_from_slice(&bucket.0);
|
||||
bucket.1 = 0;
|
||||
occupied_mask = occupied_mask & (!target_mask_bit);
|
||||
} else {
|
||||
(bucket.0)[bucket.1] = *interaction_i;
|
||||
bucket.1 += 1;
|
||||
occupied_mask = occupied_mask | target_mask_bit;
|
||||
}
|
||||
|
||||
// NOTE: static bodies don't transmit forces. Therefore they don't
|
||||
// imply any interaction conflicts.
|
||||
if !is_static1 {
|
||||
self.body_masks[i1] |= target_mask_bit;
|
||||
}
|
||||
|
||||
if !is_static2 {
|
||||
self.body_masks[i2] |= target_mask_bit;
|
||||
}
|
||||
}
|
||||
|
||||
self.nongrouped_interactions.extend(
|
||||
self.buckets
|
||||
.values()
|
||||
.flat_map(|e| e.0.iter().take(e.1).copied()),
|
||||
);
|
||||
self.buckets.clear();
|
||||
self.body_masks.iter_mut().for_each(|e| *e = 0);
|
||||
occupied_mask = 0u128;
|
||||
}
|
||||
|
||||
assert!(
|
||||
self.grouped_interactions.len() % SIMD_WIDTH == 0,
|
||||
"Invalid SIMD contact grouping."
|
||||
);
|
||||
}
|
||||
}
|
||||
73
src/dynamics/solver/island_solver.rs
Normal file
73
src/dynamics/solver/island_solver.rs
Normal file
@@ -0,0 +1,73 @@
|
||||
use super::{PositionSolver, VelocitySolver};
|
||||
use crate::counters::Counters;
|
||||
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
|
||||
pub struct IslandSolver {
|
||||
velocity_solver: VelocitySolver,
|
||||
position_solver: PositionSolver,
|
||||
}
|
||||
|
||||
impl IslandSolver {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
velocity_solver: VelocitySolver::new(),
|
||||
position_solver: PositionSolver::new(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_island(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
counters: &mut Counters,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &mut RigidBodySet,
|
||||
manifolds: &mut [&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
joints: &mut [JointGraphEdge],
|
||||
joint_indices: &[JointIndex],
|
||||
) {
|
||||
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,
|
||||
);
|
||||
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(
|
||||
island_id,
|
||||
params,
|
||||
bodies,
|
||||
manifolds,
|
||||
&manifold_indices,
|
||||
joints,
|
||||
&joint_indices,
|
||||
);
|
||||
counters.solver.position_assembly_time.pause();
|
||||
}
|
||||
|
||||
counters.solver.velocity_update_time.resume();
|
||||
bodies
|
||||
.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt()));
|
||||
counters.solver.velocity_update_time.pause();
|
||||
|
||||
if manifold_indices.len() != 0 || joint_indices.len() != 0 {
|
||||
counters.solver.position_resolution_time.resume();
|
||||
self.position_solver
|
||||
.solve_constraints(island_id, params, bodies);
|
||||
counters.solver.position_resolution_time.pause();
|
||||
}
|
||||
}
|
||||
}
|
||||
165
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
Normal file
165
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
Normal file
@@ -0,0 +1,165 @@
|
||||
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
|
||||
#[cfg(feature = "dim2")]
|
||||
use crate::math::SdpMatrix;
|
||||
use crate::math::{AngularInertia, Isometry, Point, Rotation};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallPositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
|
||||
local_com1: Point<f32>,
|
||||
local_com2: Point<f32>,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
|
||||
ii1: AngularInertia<f32>,
|
||||
ii2: AngularInertia<f32>,
|
||||
|
||||
local_anchor1: Point<f32>,
|
||||
local_anchor2: Point<f32>,
|
||||
}
|
||||
|
||||
impl BallPositionConstraint {
|
||||
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
|
||||
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(),
|
||||
local_anchor1: cparams.local_anchor1,
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
position1: rb1.active_set_offset,
|
||||
position2: rb2.active_set_offset,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position1 = positions[self.position1 as usize];
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
let anchor1 = position1 * self.local_anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
|
||||
let com1 = position1 * self.local_com1;
|
||||
let com2 = position2 * self.local_com2;
|
||||
|
||||
let err = anchor1 - anchor2;
|
||||
|
||||
let centered_anchor1 = anchor1 - com1;
|
||||
let centered_anchor2 = anchor2 - com2;
|
||||
|
||||
let cmat1 = centered_anchor1.gcross_matrix();
|
||||
let cmat2 = centered_anchor2.gcross_matrix();
|
||||
|
||||
// NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
|
||||
// because it is anti-symmetric.
|
||||
#[cfg(feature = "dim3")]
|
||||
let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
|
||||
+ self.ii2.quadform(&cmat2).add_diagonal(self.im2);
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way. It is also
|
||||
// faster because in 2D lhs will be symmetric.
|
||||
#[cfg(feature = "dim2")]
|
||||
let lhs = {
|
||||
let m11 =
|
||||
self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
|
||||
let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
|
||||
let m22 =
|
||||
self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
|
||||
SdpMatrix::new(m11, m12, m22)
|
||||
};
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
let impulse = inv_lhs * -(err * params.joint_erp);
|
||||
|
||||
position1.translation.vector += self.im1 * impulse;
|
||||
position2.translation.vector -= self.im2 * impulse;
|
||||
|
||||
let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse));
|
||||
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
|
||||
|
||||
position1.rotation = Rotation::new(angle1) * position1.rotation;
|
||||
position2.rotation = Rotation::new(angle2) * position2.rotation;
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallPositionGroundConstraint {
|
||||
position2: usize,
|
||||
anchor1: Point<f32>,
|
||||
im2: f32,
|
||||
ii2: AngularInertia<f32>,
|
||||
local_anchor2: Point<f32>,
|
||||
local_com2: Point<f32>,
|
||||
}
|
||||
|
||||
impl BallPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &BallJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
if flipped {
|
||||
// Note the only thing that is flipped here
|
||||
// are the local_anchors. The rb1 and rb2 have
|
||||
// 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(),
|
||||
local_anchor2: cparams.local_anchor1,
|
||||
position2: rb2.active_set_offset,
|
||||
local_com2: rb2.mass_properties.local_com,
|
||||
}
|
||||
} else {
|
||||
Self {
|
||||
anchor1: rb1.predicted_position * cparams.local_anchor1,
|
||||
im2: rb2.mass_properties.inv_mass,
|
||||
ii2: rb2.world_inv_inertia_sqrt.squared(),
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
position2: rb2.active_set_offset,
|
||||
local_com2: rb2.mass_properties.local_com,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let com2 = position2 * self.local_com2;
|
||||
|
||||
let err = self.anchor1 - anchor2;
|
||||
let centered_anchor2 = anchor2 - com2;
|
||||
let cmat2 = centered_anchor2.gcross_matrix();
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let lhs = {
|
||||
let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2;
|
||||
let m12 = cmat2.x * cmat2.y * self.ii2;
|
||||
let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2;
|
||||
SdpMatrix::new(m11, m12, m22)
|
||||
};
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
let impulse = inv_lhs * -(err * params.joint_erp);
|
||||
position2.translation.vector -= self.im2 * impulse;
|
||||
|
||||
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
|
||||
position2.rotation = Rotation::new(angle2) * position2.rotation;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,199 @@
|
||||
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::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use simba::simd::SimdValue;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WBallPositionConstraint {
|
||||
position1: [usize; SIMD_WIDTH],
|
||||
position2: [usize; SIMD_WIDTH],
|
||||
|
||||
local_com1: Point<SimdFloat>,
|
||||
local_com2: Point<SimdFloat>,
|
||||
|
||||
im1: SimdFloat,
|
||||
im2: SimdFloat,
|
||||
|
||||
ii1: AngularInertia<SimdFloat>,
|
||||
ii2: AngularInertia<SimdFloat>,
|
||||
|
||||
local_anchor1: Point<SimdFloat>,
|
||||
local_anchor2: Point<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WBallPositionConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
) -> 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],
|
||||
)
|
||||
.squared();
|
||||
let ii2 = AngularInertia::<SimdFloat>::from(
|
||||
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
|
||||
)
|
||||
.squared();
|
||||
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
|
||||
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
|
||||
let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
|
||||
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
Self {
|
||||
local_com1,
|
||||
local_com2,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
local_anchor1,
|
||||
local_anchor2,
|
||||
position1,
|
||||
position2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
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]);
|
||||
|
||||
let anchor1 = position1 * self.local_anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
|
||||
let com1 = position1 * self.local_com1;
|
||||
let com2 = position2 * self.local_com2;
|
||||
|
||||
let err = anchor1 - anchor2;
|
||||
|
||||
let centered_anchor1 = anchor1 - com1;
|
||||
let centered_anchor2 = anchor2 - com2;
|
||||
|
||||
let cmat1 = centered_anchor1.gcross_matrix();
|
||||
let cmat2 = centered_anchor2.gcross_matrix();
|
||||
|
||||
// NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
|
||||
// because it is anti-symmetric.
|
||||
#[cfg(feature = "dim3")]
|
||||
let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
|
||||
+ self.ii2.quadform(&cmat2).add_diagonal(self.im2);
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
let lhs = {
|
||||
let m11 =
|
||||
self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
|
||||
let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
|
||||
let m22 =
|
||||
self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
|
||||
SdpMatrix::new(m11, m12, m22)
|
||||
};
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
|
||||
|
||||
position1.translation.vector += impulse * self.im1;
|
||||
position2.translation.vector -= impulse * self.im2;
|
||||
|
||||
let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse));
|
||||
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
|
||||
|
||||
position1.rotation = Rotation::new(angle1) * position1.rotation;
|
||||
position2.rotation = Rotation::new(angle2) * position2.rotation;
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.position1[ii]] = position1.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.position2[ii]] = position2.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[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>,
|
||||
}
|
||||
|
||||
impl WBallPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> Self {
|
||||
let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
|
||||
let anchor1 = position1
|
||||
* Point::from(array![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor2
|
||||
} 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],
|
||||
)
|
||||
.squared();
|
||||
let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
|
||||
cparams[ii].local_anchor1
|
||||
} else {
|
||||
cparams[ii].local_anchor2
|
||||
}; SIMD_WIDTH]);
|
||||
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
|
||||
|
||||
Self {
|
||||
anchor1,
|
||||
im2,
|
||||
ii2,
|
||||
local_anchor2,
|
||||
position2,
|
||||
local_com2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
|
||||
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let com2 = position2 * self.local_com2;
|
||||
|
||||
let err = self.anchor1 - anchor2;
|
||||
let centered_anchor2 = anchor2 - com2;
|
||||
let cmat2 = centered_anchor2.gcross_matrix();
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let lhs = {
|
||||
let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2;
|
||||
let m12 = cmat2.x * cmat2.y * self.ii2;
|
||||
let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2;
|
||||
SdpMatrix::new(m11, m12, m22)
|
||||
};
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
|
||||
position2.translation.vector -= impulse * self.im2;
|
||||
|
||||
let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
|
||||
position2.rotation = Rotation::new(angle2) * position2.rotation;
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.position2[ii]] = position2.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
238
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
Normal file
238
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
Normal file
@@ -0,0 +1,238 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
|
||||
};
|
||||
use crate::math::{SdpMatrix, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct BallVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
rhs: Vector<f32>,
|
||||
pub(crate) impulse: Vector<f32>,
|
||||
|
||||
gcross1: Vector<f32>,
|
||||
gcross2: Vector<f32>,
|
||||
|
||||
inv_lhs: SdpMatrix<f32>,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
}
|
||||
|
||||
impl BallVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &BallJoint,
|
||||
) -> Self {
|
||||
let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com;
|
||||
let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com;
|
||||
|
||||
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 rhs = -(vel1 - vel2);
|
||||
let lhs;
|
||||
|
||||
let cmat1 = anchor1.gcross_matrix();
|
||||
let cmat2 = anchor2.gcross_matrix();
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
lhs = rb2
|
||||
.world_inv_inertia_sqrt
|
||||
.squared()
|
||||
.quadform(&cmat2)
|
||||
.add_diagonal(im2)
|
||||
+ rb1
|
||||
.world_inv_inertia_sqrt
|
||||
.squared()
|
||||
.quadform(&cmat1)
|
||||
.add_diagonal(im1);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// 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 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 {
|
||||
joint_id,
|
||||
mj_lambda1: rb1.active_set_offset,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im1,
|
||||
im2,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
gcross1,
|
||||
gcross2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
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_lambda2.linear -= self.im2 * self.impulse;
|
||||
mj_lambda2.angular -= self.gcross2.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>]) {
|
||||
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 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_lambda2.linear -= self.im2 * impulse;
|
||||
mj_lambda2.angular -= self.gcross2.gcross(impulse);
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::BallJoint(ball) = &mut joint.params {
|
||||
ball.impulse = self.impulse
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
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,
|
||||
}
|
||||
|
||||
impl BallVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &BallJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let (anchor1, anchor2) = if flipped {
|
||||
(
|
||||
rb1.position * cparams.local_anchor2 - rb1.world_com,
|
||||
rb2.position * cparams.local_anchor1 - rb2.world_com,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
rb1.position * cparams.local_anchor1 - rb1.world_com,
|
||||
rb2.position * cparams.local_anchor2 - rb2.world_com,
|
||||
)
|
||||
};
|
||||
|
||||
let im2 = rb2.mass_properties.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
|
||||
.squared()
|
||||
.quadform(&cmat2)
|
||||
.add_diagonal(im2);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ii2 = rb2.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;
|
||||
lhs = SdpMatrix::new(m11, m12, m22)
|
||||
}
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
|
||||
BallVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im2,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
gcross2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
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_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
|
||||
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_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::BallJoint(ball) = &mut joint.params {
|
||||
ball.impulse = self.impulse
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,329 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use simba::simd::SimdValue;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WBallVelocityConstraint {
|
||||
mj_lambda1: [usize; SIMD_WIDTH],
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
rhs: Vector<SimdFloat>,
|
||||
pub(crate) impulse: Vector<SimdFloat>,
|
||||
|
||||
gcross1: Vector<SimdFloat>,
|
||||
gcross2: Vector<SimdFloat>,
|
||||
|
||||
inv_lhs: SdpMatrix<SimdFloat>,
|
||||
|
||||
im1: SimdFloat,
|
||||
im2: SimdFloat,
|
||||
}
|
||||
|
||||
impl WBallVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
) -> 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 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
|
||||
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
|
||||
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
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 rhs = -(vel1 - vel2);
|
||||
let lhs;
|
||||
|
||||
let cmat1 = anchor1.gcross_matrix();
|
||||
let cmat2 = anchor2.gcross_matrix();
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2)
|
||||
+ ii1_sqrt.squared().quadform(&cmat1).add_diagonal(im1);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ii1 = ii1_sqrt.squared();
|
||||
let ii2 = ii2_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 = ii1_sqrt.transform_lin_vector(anchor1);
|
||||
let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
|
||||
WBallVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
im2,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
gcross1,
|
||||
gcross2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
mj_lambda1.linear += self.impulse * self.im1;
|
||||
mj_lambda1.angular += self.gcross1.gcross(self.impulse);
|
||||
mj_lambda2.linear -= self.impulse * self.im2;
|
||||
mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
|
||||
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
|
||||
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_lambda2.linear -= impulse * self.im2;
|
||||
mj_lambda2.angular -= self.gcross2.gcross(impulse);
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::BallJoint(ball) = &mut joint.params {
|
||||
ball.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
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,
|
||||
}
|
||||
|
||||
impl WBallVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&BallJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> 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 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],
|
||||
);
|
||||
|
||||
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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let local_anchor2 = Point::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
|
||||
);
|
||||
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
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 rhs = vel2 - vel1;
|
||||
let lhs;
|
||||
|
||||
let cmat2 = anchor2.gcross_matrix();
|
||||
let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let ii2 = ii2_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;
|
||||
lhs = SdpMatrix::new(m11, m12, m22)
|
||||
}
|
||||
|
||||
let inv_lhs = lhs.inverse_unchecked();
|
||||
|
||||
WBallVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
gcross2,
|
||||
rhs,
|
||||
inv_lhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
mj_lambda2.linear -= self.impulse * self.im2;
|
||||
mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
|
||||
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);
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::BallJoint(ball) = &mut joint.params {
|
||||
ball.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,139 @@
|
||||
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
|
||||
use crate::math::{AngularInertia, Isometry, Point, 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>,
|
||||
|
||||
lin_inv_lhs: f32,
|
||||
ang_inv_lhs: AngularInertia<f32>,
|
||||
}
|
||||
|
||||
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 lin_inv_lhs = 1.0 / (im1 + im2);
|
||||
let ang_inv_lhs = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
local_anchor1: cparams.local_anchor1,
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
position1: rb1.active_set_offset,
|
||||
position2: rb2.active_set_offset,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
local_com1: rb1.mass_properties.local_com,
|
||||
local_com2: rb2.mass_properties.local_com,
|
||||
lin_inv_lhs,
|
||||
ang_inv_lhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position1 = positions[self.position1 as usize];
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
// Angular correction.
|
||||
let anchor1 = position1 * self.local_anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let ang_err = anchor2.rotation * anchor1.rotation.inverse();
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(ang_err.scaled_axis() * params.joint_erp);
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(ang_err.angle() * params.joint_erp);
|
||||
position1.rotation =
|
||||
Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
|
||||
position2.rotation =
|
||||
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
|
||||
|
||||
// Linear correction.
|
||||
let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
|
||||
let err = anchor2 - anchor1;
|
||||
let impulse = err * (self.lin_inv_lhs * params.joint_erp);
|
||||
position1.translation.vector += self.im1 * impulse;
|
||||
position2.translation.vector -= self.im2 * impulse;
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
#[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,
|
||||
}
|
||||
|
||||
impl FixedPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &FixedJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let anchor1;
|
||||
let local_anchor2;
|
||||
|
||||
if flipped {
|
||||
anchor1 = rb1.predicted_position * cparams.local_anchor2;
|
||||
local_anchor2 = cparams.local_anchor1;
|
||||
} else {
|
||||
anchor1 = rb1.predicted_position * cparams.local_anchor1;
|
||||
local_anchor2 = cparams.local_anchor2;
|
||||
};
|
||||
|
||||
Self {
|
||||
anchor1,
|
||||
local_anchor2,
|
||||
position2: rb2.active_set_offset,
|
||||
im2: rb2.mass_properties.inv_mass,
|
||||
ii2: rb2.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>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
// Angular correction.
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let ang_err = anchor2.rotation * self.anchor1.rotation.inverse();
|
||||
position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
|
||||
|
||||
// Linear correction.
|
||||
let anchor1 = Point::from(self.anchor1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
|
||||
let err = anchor2 - anchor1;
|
||||
// NOTE: no need to divide by im2 just to multiply right after.
|
||||
let impulse = err * params.joint_erp;
|
||||
position2.translation.vector -= impulse;
|
||||
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,370 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
|
||||
};
|
||||
use crate::math::{AngularInertia, Dim, SpacialVector, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
#[cfg(feature = "dim2")]
|
||||
use na::{Matrix3, Vector3};
|
||||
#[cfg(feature = "dim3")]
|
||||
use na::{Matrix6, Vector6, U3};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct FixedVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
impulse: SpacialVector<f32>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<f32>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<f32>,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
|
||||
ii1: AngularInertia<f32>,
|
||||
ii2: AngularInertia<f32>,
|
||||
|
||||
ii1_sqrt: AngularInertia<f32>,
|
||||
ii2_sqrt: AngularInertia<f32>,
|
||||
|
||||
r1: Vector<f32>,
|
||||
r2: Vector<f32>,
|
||||
}
|
||||
|
||||
impl FixedVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &FixedJoint,
|
||||
) -> 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 r1 = anchor1.translation.vector - rb1.world_com.coords;
|
||||
let r2 = anchor2.translation.vector - rb2.world_com.coords;
|
||||
let rmat1 = r1.gcross_matrix();
|
||||
let rmat2 = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let lhs00 =
|
||||
ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
|
||||
// Note that Cholesky only reads the lower-triangular part of the matrix
|
||||
// so we don't need to fill lhs01.
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
|
||||
let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
|
||||
let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
|
||||
let m13 = rmat1.x * ii1 + rmat2.x * ii2;
|
||||
let m23 = rmat1.y * ii1 + rmat2.y * ii2;
|
||||
let m33 = ii1 + ii2;
|
||||
lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
|
||||
}
|
||||
|
||||
// NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.try_inverse().expect("Singular system.");
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
|
||||
|
||||
let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
|
||||
let ang_dvel = -rb1.angvel + rb2.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
);
|
||||
|
||||
FixedVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: rb1.active_set_offset,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
ii1_sqrt: rb1.world_inv_inertia_sqrt,
|
||||
ii2_sqrt: rb2.world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
inv_lhs,
|
||||
r1,
|
||||
r2,
|
||||
rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_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>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
|
||||
+ mj_lambda2.linear
|
||||
+ ang_vel2.gcross(self.r2);
|
||||
let dangvel = -ang_vel1 + ang_vel2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::FixedJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct FixedVelocityGroundConstraint {
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
impulse: SpacialVector<f32>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<f32>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<f32>,
|
||||
|
||||
im2: f32,
|
||||
ii2: AngularInertia<f32>,
|
||||
ii2_sqrt: AngularInertia<f32>,
|
||||
r2: Vector<f32>,
|
||||
}
|
||||
|
||||
impl FixedVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &FixedJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let (anchor1, anchor2) = if flipped {
|
||||
(
|
||||
rb1.position * cparams.local_anchor2,
|
||||
rb2.position * cparams.local_anchor1,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
rb1.position * cparams.local_anchor1,
|
||||
rb2.position * cparams.local_anchor2,
|
||||
)
|
||||
};
|
||||
|
||||
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 r2 = anchor2.translation.vector - rb2.world_com.coords;
|
||||
let rmat2 = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let lhs10 = ii2.transform_matrix(&rmat2);
|
||||
let lhs11 = ii2.into_matrix();
|
||||
|
||||
// Note that Cholesky only reads the lower-triangular part of the matrix
|
||||
// so we don't need to fill lhs01.
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let m11 = im2 + rmat2.x * rmat2.x * ii2;
|
||||
let m12 = rmat2.x * rmat2.y * ii2;
|
||||
let m22 = im2 + rmat2.y * rmat2.y * ii2;
|
||||
let m13 = rmat2.x * ii2;
|
||||
let m23 = rmat2.y * ii2;
|
||||
let m33 = ii2;
|
||||
lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.try_inverse().expect("Singular system.");
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
|
||||
|
||||
let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
|
||||
let ang_dvel = rb2.angvel - rb1.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
);
|
||||
|
||||
FixedVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im2,
|
||||
ii2,
|
||||
ii2_sqrt: rb2.world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
inv_lhs,
|
||||
r2,
|
||||
rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let dangvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::FixedJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,472 @@
|
||||
use simba::simd::SimdValue;
|
||||
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, 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},
|
||||
};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WFixedVelocityConstraint {
|
||||
mj_lambda1: [usize; SIMD_WIDTH],
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
impulse: SpacialVector<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix3<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<SimdFloat>,
|
||||
|
||||
im1: SimdFloat,
|
||||
im2: SimdFloat,
|
||||
|
||||
ii1: AngularInertia<SimdFloat>,
|
||||
ii2: AngularInertia<SimdFloat>,
|
||||
|
||||
ii1_sqrt: AngularInertia<SimdFloat>,
|
||||
ii2_sqrt: AngularInertia<SimdFloat>,
|
||||
|
||||
r1: Vector<SimdFloat>,
|
||||
r2: Vector<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WFixedVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&FixedJoint; SIMD_WIDTH],
|
||||
) -> 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 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
|
||||
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
|
||||
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let ii1 = ii1_sqrt.squared();
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - world_com1.coords;
|
||||
let r2 = anchor2.translation.vector - world_com2.coords;
|
||||
let rmat1: CrossMatrix<_> = r1.gcross_matrix();
|
||||
let rmat2: CrossMatrix<_> = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let lhs00 =
|
||||
ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
|
||||
// Note that Cholesky only reads the lower-triangular part of the matrix
|
||||
// so we don't need to fill lhs01.
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
|
||||
let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
|
||||
let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
|
||||
let m13 = rmat1.x * ii1 + rmat2.x * ii2;
|
||||
let m23 = rmat1.y * ii1 + rmat2.y * ii2;
|
||||
let m33 = ii1 + ii2;
|
||||
lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
|
||||
}
|
||||
|
||||
// NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix?
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
|
||||
let ang_dvel = -angvel1 + angvel2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
);
|
||||
|
||||
WFixedVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
ii1_sqrt,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
inv_lhs,
|
||||
r1,
|
||||
r2,
|
||||
rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
|
||||
+ mj_lambda2.linear
|
||||
+ ang_vel2.gcross(self.r2);
|
||||
let dangvel = -ang_vel1 + ang_vel2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::FixedJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WFixedVelocityGroundConstraint {
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
impulse: SpacialVector<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector6<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix3<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector3<SimdFloat>,
|
||||
|
||||
im2: SimdFloat,
|
||||
ii2: AngularInertia<SimdFloat>,
|
||||
ii2_sqrt: AngularInertia<SimdFloat>,
|
||||
r2: Vector<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WFixedVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&FixedJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let local_anchor1 = Isometry::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_anchor2 = Isometry::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
|
||||
);
|
||||
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1.translation.vector - world_com1.coords;
|
||||
let r2 = anchor2.translation.vector - world_com2.coords;
|
||||
let rmat2: CrossMatrix<_> = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
|
||||
let lhs10 = ii2.transform_matrix(&rmat2);
|
||||
let lhs11 = ii2.into_matrix();
|
||||
|
||||
lhs = Matrix6::zeros();
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
// In 2D we just unroll the computation because
|
||||
// it's just easier that way.
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let m11 = im2 + rmat2.x * rmat2.x * ii2;
|
||||
let m12 = rmat2.x * rmat2.y * ii2;
|
||||
let m22 = im2 + rmat2.y * rmat2.y * ii2;
|
||||
let m13 = rmat2.x * ii2;
|
||||
let m23 = rmat2.y * ii2;
|
||||
let m33 = ii2;
|
||||
lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
|
||||
let ang_dvel = angvel2 - angvel1;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
|
||||
);
|
||||
|
||||
WFixedVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
inv_lhs,
|
||||
r2,
|
||||
rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let dangvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector6::new(
|
||||
dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
|
||||
) + self.rhs;
|
||||
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse[2];
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::FixedJoint(fixed) = &mut joint.params {
|
||||
fixed.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
340
src/dynamics/solver/joint_constraint/joint_constraint.rs
Normal file
340
src/dynamics/solver/joint_constraint/joint_constraint.rs
Normal file
@@ -0,0 +1,340 @@
|
||||
use super::{
|
||||
BallVelocityConstraint, BallVelocityGroundConstraint, FixedVelocityConstraint,
|
||||
FixedVelocityGroundConstraint, PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{
|
||||
WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
|
||||
WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
|
||||
WPrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
|
||||
};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::math::SIMD_WIDTH;
|
||||
|
||||
pub(crate) enum AnyJointVelocityConstraint {
|
||||
BallConstraint(BallVelocityConstraint),
|
||||
BallGroundConstraint(BallVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallConstraint(WBallVelocityConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallGroundConstraint(WBallVelocityGroundConstraint),
|
||||
FixedConstraint(FixedVelocityConstraint),
|
||||
FixedGroundConstraint(FixedVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WFixedConstraint(WFixedVelocityConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WFixedGroundConstraint(WFixedVelocityGroundConstraint),
|
||||
PrismaticConstraint(PrismaticVelocityConstraint),
|
||||
PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WPrismaticConstraint(WPrismaticVelocityConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WPrismaticGroundConstraint(WPrismaticVelocityGroundConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteConstraint(RevoluteVelocityConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteGroundConstraint(RevoluteVelocityGroundConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WRevoluteConstraint(WRevoluteVelocityConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WRevoluteGroundConstraint(WRevoluteVelocityGroundConstraint),
|
||||
#[allow(dead_code)] // The Empty variant is only used with parallel code.
|
||||
Empty,
|
||||
}
|
||||
|
||||
impl AnyJointVelocityConstraint {
|
||||
#[cfg(feature = "parallel")]
|
||||
pub fn num_active_constraints(_: &Joint) -> usize {
|
||||
1
|
||||
}
|
||||
|
||||
pub fn from_joint(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
joint: &Joint,
|
||||
bodies: &RigidBodySet,
|
||||
) -> Self {
|
||||
let rb1 = &bodies[joint.body1];
|
||||
let rb2 = &bodies[joint.body2];
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
|
||||
BallVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedConstraint(
|
||||
FixedVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
|
||||
PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
|
||||
RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn from_wide_joint(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
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];
|
||||
AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints,
|
||||
))
|
||||
}
|
||||
JointParams::FixedJoint(_) => {
|
||||
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints,
|
||||
))
|
||||
}
|
||||
JointParams::PrismaticJoint(_) => {
|
||||
let joints =
|
||||
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(
|
||||
WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => {
|
||||
let joints =
|
||||
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(
|
||||
WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_joint_ground(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
joint: &Joint,
|
||||
bodies: &RigidBodySet,
|
||||
) -> Self {
|
||||
let mut rb1 = &bodies[joint.body1];
|
||||
let mut rb2 = &bodies[joint.body2];
|
||||
let flipped = !rb2.is_dynamic();
|
||||
|
||||
if flipped {
|
||||
std::mem::swap(&mut rb1, &mut rb2);
|
||||
}
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
|
||||
BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
|
||||
FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
|
||||
),
|
||||
JointParams::PrismaticJoint(p) => {
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(
|
||||
PrismaticVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rb1, rb2, p, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
|
||||
RevoluteVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rb1, rb2, p, flipped,
|
||||
),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn from_wide_joint_ground(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
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];
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
if !rbs2[ii].is_dynamic() {
|
||||
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
|
||||
flipped[ii] = true;
|
||||
}
|
||||
}
|
||||
|
||||
match &joints[0].params {
|
||||
JointParams::BallJoint(_) => {
|
||||
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(
|
||||
WBallVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
JointParams::FixedJoint(_) => {
|
||||
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(
|
||||
WFixedVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
JointParams::PrismaticJoint(_) => {
|
||||
let joints =
|
||||
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(
|
||||
WPrismaticVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => {
|
||||
let joints =
|
||||
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(
|
||||
WRevoluteVelocityGroundConstraint::from_params(
|
||||
params, joint_id, rbs1, rbs2, joints, flipped,
|
||||
),
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
match self {
|
||||
AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
|
||||
AnyJointVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
match self {
|
||||
AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.solve(mj_lambdas),
|
||||
AnyJointVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
match self {
|
||||
AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all),
|
||||
|
||||
AnyJointVelocityConstraint::BallGroundConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallConstraint(c) => c.writeback_impulses(joints_all),
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WBallGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
AnyJointVelocityConstraint::FixedConstraint(c) => c.writeback_impulses(joints_all),
|
||||
AnyJointVelocityConstraint::FixedGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
|
||||
AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "dim3")]
|
||||
AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.writeback_impulses(joints_all),
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => {
|
||||
c.writeback_impulses(joints_all)
|
||||
}
|
||||
AnyJointVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,169 @@
|
||||
use super::{
|
||||
BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
|
||||
FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
|
||||
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
|
||||
use crate::math::Isometry;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::math::SIMD_WIDTH;
|
||||
|
||||
pub(crate) enum AnyJointPositionConstraint {
|
||||
BallJoint(BallPositionConstraint),
|
||||
BallGroundConstraint(BallPositionGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallJoint(WBallPositionConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
WBallGroundConstraint(WBallPositionGroundConstraint),
|
||||
FixedJoint(FixedPositionConstraint),
|
||||
FixedGroundConstraint(FixedPositionGroundConstraint),
|
||||
PrismaticJoint(PrismaticPositionConstraint),
|
||||
PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteJoint(RevolutePositionConstraint),
|
||||
#[cfg(feature = "dim3")]
|
||||
RevoluteGroundConstraint(RevolutePositionGroundConstraint),
|
||||
#[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];
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
|
||||
BallPositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
|
||||
FixedPositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
|
||||
PrismaticPositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteJoint(
|
||||
RevolutePositionConstraint::from_params(rb1, rb2, p),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option<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),
|
||||
))
|
||||
}
|
||||
JointParams::FixedJoint(_) => None,
|
||||
JointParams::PrismaticJoint(_) => None,
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => None,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
|
||||
let mut rb1 = &bodies[joint.body1];
|
||||
let mut rb2 = &bodies[joint.body2];
|
||||
let flipped = !rb2.is_dynamic();
|
||||
|
||||
if flipped {
|
||||
std::mem::swap(&mut rb1, &mut rb2);
|
||||
}
|
||||
|
||||
match &joint.params {
|
||||
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
|
||||
BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
),
|
||||
JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
|
||||
FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
),
|
||||
JointParams::PrismaticJoint(p) => {
|
||||
AnyJointPositionConstraint::PrismaticGroundConstraint(
|
||||
PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
)
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteGroundConstraint(
|
||||
RevolutePositionGroundConstraint::from_params(rb1, rb2, p, flipped),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub fn from_wide_joint_ground(
|
||||
joints: [&Joint; SIMD_WIDTH],
|
||||
bodies: &RigidBodySet,
|
||||
) -> Option<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];
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
if !rbs2[ii].is_dynamic() {
|
||||
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
|
||||
flipped[ii] = true;
|
||||
}
|
||||
}
|
||||
|
||||
match &joints[0].params {
|
||||
JointParams::BallJoint(_) => {
|
||||
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
|
||||
Some(AnyJointPositionConstraint::WBallGroundConstraint(
|
||||
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
|
||||
))
|
||||
}
|
||||
JointParams::FixedJoint(_) => None,
|
||||
JointParams::PrismaticJoint(_) => None,
|
||||
#[cfg(feature = "dim3")]
|
||||
JointParams::RevoluteJoint(_) => None,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
match self {
|
||||
AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WBallJoint(c) => c.solve(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
|
||||
AnyJointPositionConstraint::PrismaticGroundConstraint(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),
|
||||
AnyJointPositionConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
65
src/dynamics/solver/joint_constraint/mod.rs
Normal file
65
src/dynamics/solver/joint_constraint/mod.rs
Normal file
@@ -0,0 +1,65 @@
|
||||
pub(self) use ball_position_constraint::{BallPositionConstraint, BallPositionGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use ball_position_constraint_wide::{
|
||||
WBallPositionConstraint, WBallPositionGroundConstraint,
|
||||
};
|
||||
pub(self) use ball_velocity_constraint::{BallVelocityConstraint, BallVelocityGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use ball_velocity_constraint_wide::{
|
||||
WBallVelocityConstraint, WBallVelocityGroundConstraint,
|
||||
};
|
||||
pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
|
||||
pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use fixed_velocity_constraint_wide::{
|
||||
WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
|
||||
};
|
||||
pub(crate) use joint_constraint::AnyJointVelocityConstraint;
|
||||
pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
|
||||
pub(self) use prismatic_position_constraint::{
|
||||
PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
|
||||
};
|
||||
pub(self) use prismatic_velocity_constraint::{
|
||||
PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use prismatic_velocity_constraint_wide::{
|
||||
WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(self) use revolute_position_constraint::{
|
||||
RevolutePositionConstraint, RevolutePositionGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
pub(self) use revolute_velocity_constraint::{
|
||||
RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use revolute_velocity_constraint_wide::{
|
||||
WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
|
||||
};
|
||||
|
||||
mod ball_position_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod ball_position_constraint_wide;
|
||||
mod ball_velocity_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod ball_velocity_constraint_wide;
|
||||
mod fixed_position_constraint;
|
||||
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;
|
||||
mod prismatic_velocity_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod prismatic_velocity_constraint_wide;
|
||||
#[cfg(feature = "dim3")]
|
||||
mod revolute_position_constraint;
|
||||
#[cfg(feature = "dim3")]
|
||||
mod revolute_velocity_constraint;
|
||||
#[cfg(feature = "dim3")]
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod revolute_velocity_constraint_wide;
|
||||
@@ -0,0 +1,170 @@
|
||||
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
|
||||
use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
|
||||
use crate::utils::WAngularInertia;
|
||||
use na::Unit;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct PrismaticPositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
|
||||
ii1: AngularInertia<f32>,
|
||||
ii2: AngularInertia<f32>,
|
||||
|
||||
lin_inv_lhs: f32,
|
||||
ang_inv_lhs: AngularInertia<f32>,
|
||||
|
||||
limits: [f32; 2],
|
||||
|
||||
local_frame1: Isometry<f32>,
|
||||
local_frame2: Isometry<f32>,
|
||||
|
||||
local_axis1: Unit<Vector<f32>>,
|
||||
local_axis2: Unit<Vector<f32>>,
|
||||
}
|
||||
|
||||
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 lin_inv_lhs = 1.0 / (im1 + im2);
|
||||
let ang_inv_lhs = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
lin_inv_lhs,
|
||||
ang_inv_lhs,
|
||||
local_frame1: cparams.local_frame1(),
|
||||
local_frame2: cparams.local_frame2(),
|
||||
local_axis1: cparams.local_axis1,
|
||||
local_axis2: cparams.local_axis2,
|
||||
position1: rb1.active_set_offset,
|
||||
position2: rb2.active_set_offset,
|
||||
limits: cparams.limits,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position1 = positions[self.position1 as usize];
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
// Angular correction.
|
||||
let frame1 = position1 * self.local_frame1;
|
||||
let frame2 = position2 * self.local_frame2;
|
||||
let ang_err = frame2.rotation * frame1.rotation.inverse();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(ang_err.angle() * params.joint_erp);
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self
|
||||
.ang_inv_lhs
|
||||
.transform_vector(ang_err.scaled_axis() * params.joint_erp);
|
||||
position1.rotation =
|
||||
Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
|
||||
position2.rotation =
|
||||
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
|
||||
|
||||
// Linear correction.
|
||||
let anchor1 = position1 * Point::from(self.local_frame1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
|
||||
let axis1 = position1 * self.local_axis1;
|
||||
let dpos = anchor2 - anchor1;
|
||||
let limit_err = dpos.dot(&axis1);
|
||||
let mut err = dpos - *axis1 * limit_err;
|
||||
|
||||
if limit_err < self.limits[0] {
|
||||
err += *axis1 * (limit_err - self.limits[0]);
|
||||
} else if limit_err > self.limits[1] {
|
||||
err += *axis1 * (limit_err - self.limits[1]);
|
||||
}
|
||||
|
||||
let impulse = err * (self.lin_inv_lhs * params.joint_erp);
|
||||
position1.translation.vector += self.im1 * impulse;
|
||||
position2.translation.vector -= self.im2 * impulse;
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
#[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],
|
||||
}
|
||||
|
||||
impl PrismaticPositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &PrismaticJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let frame1;
|
||||
let local_frame2;
|
||||
let axis1;
|
||||
let local_axis2;
|
||||
|
||||
if flipped {
|
||||
frame1 = rb1.predicted_position * cparams.local_frame2();
|
||||
local_frame2 = cparams.local_frame1();
|
||||
axis1 = rb1.predicted_position * cparams.local_axis2;
|
||||
local_axis2 = cparams.local_axis1;
|
||||
} else {
|
||||
frame1 = rb1.predicted_position * cparams.local_frame1();
|
||||
local_frame2 = cparams.local_frame2();
|
||||
axis1 = rb1.predicted_position * cparams.local_axis1;
|
||||
local_axis2 = cparams.local_axis2;
|
||||
};
|
||||
|
||||
Self {
|
||||
frame1,
|
||||
local_frame2,
|
||||
axis1,
|
||||
local_axis2,
|
||||
position2: rb2.active_set_offset,
|
||||
limits: cparams.limits,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
// Angular correction.
|
||||
let frame2 = position2 * self.local_frame2;
|
||||
let ang_err = frame2.rotation * self.frame1.rotation.inverse();
|
||||
position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
|
||||
|
||||
// Linear correction.
|
||||
let anchor1 = Point::from(self.frame1.translation.vector);
|
||||
let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
|
||||
let dpos = anchor2 - anchor1;
|
||||
let limit_err = dpos.dot(&self.axis1);
|
||||
let mut err = dpos - *self.axis1 * limit_err;
|
||||
|
||||
if limit_err < self.limits[0] {
|
||||
err += *self.axis1 * (limit_err - self.limits[0]);
|
||||
} else if limit_err > self.limits[1] {
|
||||
err += *self.axis1 * (limit_err - self.limits[1]);
|
||||
}
|
||||
|
||||
// NOTE: no need to divide by im2 just to multiply right after.
|
||||
let impulse = err * params.joint_erp;
|
||||
position2.translation.vector -= impulse;
|
||||
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,558 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
|
||||
};
|
||||
use crate::math::{AngularInertia, 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},
|
||||
};
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
type LinImpulseDim = na::U1;
|
||||
#[cfg(feature = "dim3")]
|
||||
type LinImpulseDim = na::U2;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct PrismaticVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r1: Vector<f32>,
|
||||
r2: Vector<f32>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<f32>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<f32>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<f32>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<f32>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<f32>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<f32>,
|
||||
|
||||
limits_impulse: f32,
|
||||
limits_forcedirs: Option<(Vector<f32>, Vector<f32>)>,
|
||||
limits_rhs: f32,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<f32>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<f32>,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
|
||||
ii1_sqrt: AngularInertia<f32>,
|
||||
ii2_sqrt: AngularInertia<f32>,
|
||||
}
|
||||
|
||||
impl PrismaticVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &PrismaticJoint,
|
||||
) -> Self {
|
||||
// Linear part.
|
||||
let anchor1 = rb1.position * cparams.local_anchor1;
|
||||
let anchor2 = rb2.position * cparams.local_anchor2;
|
||||
let axis1 = rb1.position * cparams.local_axis1;
|
||||
let axis2 = rb2.position * cparams.local_axis2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let basis1 = rb1.position * cparams.basis1[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
rb1.position * cparams.basis1[0],
|
||||
rb1.position * cparams.basis1[1],
|
||||
]);
|
||||
|
||||
// #[cfg(feature = "dim2")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// #[cfg(feature = "dim3")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = r21 * basis1;
|
||||
// NOTE: we use basis2 := basis1 for now is that allows
|
||||
// simplifications of the computation without introducing
|
||||
// much instabilities.
|
||||
|
||||
let im1 = rb1.mass_properties.inv_mass;
|
||||
let ii1 = rb1.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 r2 = anchor2 - rb2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r1_mat_b1 = r1_mat * basis1;
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
|
||||
+ ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
lhs.fixed_slice_mut::<U2, U2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b1r1 = basis1.dot(&r1_mat);
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii1 + ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
|
||||
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let ang_rhs = rb2.angvel - rb1.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_rhs.x, ang_rhs);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
|
||||
|
||||
// Setup limit constraint.
|
||||
let mut limits_forcedirs = None;
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_impulse = 0.0;
|
||||
|
||||
if cparams.limits_enabled {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// FIXME: we should allow both limits to be active at
|
||||
// the same time, and allow predictive constraint activation.
|
||||
if dist < cparams.limits[0] {
|
||||
limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
|
||||
limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
|
||||
limits_impulse = cparams.limits_impulse;
|
||||
} else if dist > cparams.limits[1] {
|
||||
limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
|
||||
limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
|
||||
limits_impulse = cparams.limits_impulse;
|
||||
}
|
||||
}
|
||||
|
||||
PrismaticVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: rb1.active_set_offset,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im1,
|
||||
ii1_sqrt: rb1.world_inv_inertia_sqrt,
|
||||
im2,
|
||||
ii2_sqrt: rb2.world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
limits_impulse: limits_impulse * params.warmstart_coeff,
|
||||
limits_forcedirs,
|
||||
limits_rhs,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
|
||||
mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_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>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
/*
|
||||
* Joint consraint.
|
||||
*/
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
|
||||
let ang_dvel = ang_vel2 - ang_vel1;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
/*
|
||||
* Joint limits.
|
||||
*/
|
||||
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
|
||||
// FIXME: the transformation by ii2_sqrt could be avoided by
|
||||
// reusing some computations above.
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
revolute.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct PrismaticVelocityGroundConstraint {
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r2: Vector<f32>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<f32>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<f32>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<f32>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<f32>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<f32>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<f32>,
|
||||
|
||||
limits_impulse: f32,
|
||||
limits_rhs: f32,
|
||||
|
||||
axis2: Vector<f32>,
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<f32>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<f32>,
|
||||
limits_forcedir2: Option<Vector<f32>>,
|
||||
|
||||
im2: f32,
|
||||
ii2_sqrt: AngularInertia<f32>,
|
||||
}
|
||||
|
||||
impl PrismaticVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &PrismaticJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let anchor2;
|
||||
let anchor1;
|
||||
let axis2;
|
||||
let axis1;
|
||||
let basis1;
|
||||
|
||||
if flipped {
|
||||
anchor2 = rb2.position * cparams.local_anchor1;
|
||||
anchor1 = rb1.position * cparams.local_anchor2;
|
||||
axis2 = rb2.position * cparams.local_axis1;
|
||||
axis1 = rb1.position * cparams.local_axis2;
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
basis1 = rb1.position * cparams.basis2[0];
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
rb1.position * cparams.basis2[0],
|
||||
rb1.position * cparams.basis2[1],
|
||||
]);
|
||||
}
|
||||
} else {
|
||||
anchor2 = rb2.position * cparams.local_anchor2;
|
||||
anchor1 = rb1.position * cparams.local_anchor1;
|
||||
axis2 = rb2.position * cparams.local_axis2;
|
||||
axis1 = rb1.position * cparams.local_axis1;
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
basis1 = rb1.position * cparams.basis1[0];
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
rb1.position * cparams.basis1[0],
|
||||
rb1.position * cparams.basis1[1],
|
||||
]);
|
||||
}
|
||||
};
|
||||
|
||||
// #[cfg(feature = "dim2")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// #[cfg(feature = "dim3")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = r21 * basis1;
|
||||
// NOTE: we use basis2 := basis1 for now is that allows
|
||||
// simplifications of the computation without introducing
|
||||
// much instabilities.
|
||||
|
||||
let im2 = rb2.mass_properties.inv_mass;
|
||||
let ii2 = rb2.world_inv_inertia_sqrt.squared();
|
||||
let r1 = anchor1 - rb1.world_com;
|
||||
let r2 = anchor2 - rb2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii2 * r2_mat_b1;
|
||||
let lhs11 = ii2.into_matrix();
|
||||
lhs.fixed_slice_mut::<U2, U2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im2 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
|
||||
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let ang_rhs = rb2.angvel - rb1.angvel;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_rhs.x, ang_rhs);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
|
||||
|
||||
// Setup limit constraint.
|
||||
let mut limits_forcedir2 = None;
|
||||
let mut limits_rhs = 0.0;
|
||||
let mut limits_impulse = 0.0;
|
||||
|
||||
if cparams.limits_enabled {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// FIXME: we should allow both limits to be active at
|
||||
// the same time.
|
||||
// FIXME: allow predictive constraint activation.
|
||||
if dist < cparams.limits[0] {
|
||||
limits_forcedir2 = Some(axis2.into_inner());
|
||||
limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
|
||||
limits_impulse = cparams.limits_impulse;
|
||||
} else if dist > cparams.limits[1] {
|
||||
limits_forcedir2 = Some(-axis2.into_inner());
|
||||
limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
|
||||
limits_impulse = cparams.limits_impulse;
|
||||
}
|
||||
}
|
||||
|
||||
PrismaticVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im2,
|
||||
ii2_sqrt: rb2.world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
limits_impulse: limits_impulse * params.warmstart_coeff,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
axis2: axis2.into_inner(),
|
||||
limits_forcedir2,
|
||||
limits_rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
if let Some(limits_forcedir2) = self.limits_forcedir2 {
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
/*
|
||||
* Joint consraint.
|
||||
*/
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&lin_vel2);
|
||||
let ang_dvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
/*
|
||||
* Joint limits.
|
||||
*/
|
||||
if let Some(limits_forcedir2) = self.limits_forcedir2 {
|
||||
// FIXME: the transformation by ii2_sqrt could be avoided by
|
||||
// reusing some computations above.
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0);
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
revolute.limits_impulse = self.limits_impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,687 @@
|
||||
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
|
||||
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
|
||||
};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, Isometry, Point, SimdBool, SimdFloat, 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},
|
||||
};
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
type LinImpulseDim = na::U1;
|
||||
#[cfg(feature = "dim3")]
|
||||
type LinImpulseDim = na::U2;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WPrismaticVelocityConstraint {
|
||||
mj_lambda1: [usize; SIMD_WIDTH],
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r1: Vector<SimdFloat>,
|
||||
r2: Vector<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<SimdFloat>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<SimdFloat>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<SimdFloat>,
|
||||
|
||||
limits_impulse: SimdFloat,
|
||||
limits_forcedirs: Option<(Vector<SimdFloat>, Vector<SimdFloat>)>,
|
||||
limits_rhs: SimdFloat,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<SimdFloat>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<SimdFloat>,
|
||||
|
||||
im1: SimdFloat,
|
||||
im2: SimdFloat,
|
||||
|
||||
ii1_sqrt: AngularInertia<SimdFloat>,
|
||||
ii2_sqrt: AngularInertia<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WPrismaticVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&PrismaticJoint; SIMD_WIDTH],
|
||||
) -> 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 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
|
||||
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
|
||||
let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
|
||||
let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
|
||||
#[cfg(feature = "dim3")]
|
||||
let local_basis1 = [
|
||||
Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
|
||||
Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
|
||||
];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let axis1 = position1 * local_axis1;
|
||||
let axis2 = position2 * local_axis2;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let basis1 = position1 * local_basis1[0];
|
||||
#[cfg(feature = "dim3")]
|
||||
let basis1 =
|
||||
Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
|
||||
|
||||
// #[cfg(feature = "dim2")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// #[cfg(feature = "dim3")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = r21 * basis1;
|
||||
// NOTE: we use basis2 := basis1 for now is that allows
|
||||
// simplifications of the computation without introducing
|
||||
// much instabilities.
|
||||
|
||||
let ii1 = ii1_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r1_mat = r1.gcross_matrix();
|
||||
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r1_mat_b1 = r1_mat * basis1;
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
|
||||
+ ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
|
||||
let lhs11 = (ii1 + ii2).into_matrix();
|
||||
lhs.fixed_slice_mut::<U2, U2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b1r1 = basis1.dot(&r1_mat);
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii1 + ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
|
||||
let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let ang_rhs = angvel2 - angvel1;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_rhs.x, ang_rhs);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
|
||||
|
||||
// Setup limit constraint.
|
||||
let mut limits_forcedirs = None;
|
||||
let mut limits_rhs = na::zero();
|
||||
let mut limits_impulse = na::zero();
|
||||
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
|
||||
|
||||
if limits_enabled.any() {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// 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_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 sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
|
||||
|
||||
if sign != _0 {
|
||||
limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
|
||||
limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
|
||||
limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
|
||||
}
|
||||
}
|
||||
|
||||
WPrismaticVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
ii1_sqrt,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
limits_forcedirs,
|
||||
limits_rhs,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
|
||||
mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
/*
|
||||
* Joint consraint.
|
||||
*/
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
|
||||
let ang_dvel = ang_vel2 - ang_vel1;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
/*
|
||||
* Joint limits.
|
||||
*/
|
||||
if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
|
||||
// FIXME: the transformation by ii2_sqrt could be avoided by
|
||||
// reusing some computations above.
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse =
|
||||
(self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero());
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::PrismaticJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii);
|
||||
rev.limits_impulse = self.limits_impulse.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WPrismaticVelocityGroundConstraint {
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r2: Vector<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
inv_lhs: Matrix2<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
rhs: Vector2<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
impulse: Vector2<SimdFloat>,
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
inv_lhs: Matrix5<SimdFloat>,
|
||||
#[cfg(feature = "dim3")]
|
||||
rhs: Vector5<SimdFloat>,
|
||||
#[cfg(feature = "dim3")]
|
||||
impulse: Vector5<SimdFloat>,
|
||||
|
||||
limits_impulse: SimdFloat,
|
||||
limits_rhs: SimdFloat,
|
||||
|
||||
axis2: Vector<SimdFloat>,
|
||||
#[cfg(feature = "dim2")]
|
||||
basis1: Vector2<SimdFloat>,
|
||||
#[cfg(feature = "dim3")]
|
||||
basis1: Matrix3x2<SimdFloat>,
|
||||
limits_forcedir2: Option<Vector<SimdFloat>>,
|
||||
|
||||
im2: SimdFloat,
|
||||
ii2_sqrt: AngularInertia<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WPrismaticVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&PrismaticJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
let local_anchor1 = Point::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_anchor2 = Point::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_axis1 = Vector::from(
|
||||
array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_axis2 = Vector::from(
|
||||
array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
|
||||
);
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let basis1 = position1
|
||||
* Vector::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
|
||||
);
|
||||
#[cfg(feature = "dim3")]
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
position1
|
||||
* Vector::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
|
||||
),
|
||||
position1
|
||||
* Vector::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
|
||||
),
|
||||
]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let axis1 = position1 * local_axis1;
|
||||
let axis2 = position2 * local_axis2;
|
||||
|
||||
// #[cfg(feature = "dim2")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// #[cfg(feature = "dim3")]
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = r21 * basis1;
|
||||
// NOTE: we use basis2 := basis1 for now is that allows
|
||||
// simplifications of the computation without introducing
|
||||
// much instabilities.
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
#[allow(unused_mut)] // For 2D.
|
||||
let mut lhs;
|
||||
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
let r2_mat_b1 = r2_mat * basis1;
|
||||
|
||||
lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
|
||||
let lhs10 = ii2 * r2_mat_b1;
|
||||
let lhs11 = ii2.into_matrix();
|
||||
lhs.fixed_slice_mut::<U2, U2>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
|
||||
}
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
let b2r2 = basis1.dot(&r2_mat);
|
||||
let m11 = im2 + b2r2 * ii2 * b2r2;
|
||||
let m12 = basis1.dot(&r2_mat) * ii2;
|
||||
let m22 = ii2;
|
||||
lhs = SdpMatrix2::new(m11, m12, m22);
|
||||
}
|
||||
|
||||
let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
|
||||
let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
|
||||
|
||||
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
|
||||
// for which a textbook inverse is still efficient.
|
||||
#[cfg(feature = "dim2")]
|
||||
let inv_lhs = lhs.inverse_unchecked().into_matrix();
|
||||
#[cfg(feature = "dim3")]
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
|
||||
let ang_rhs = angvel2 - angvel1;
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_rhs.x, ang_rhs);
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
|
||||
|
||||
// Setup limit constraint.
|
||||
let mut limits_forcedir2 = None;
|
||||
let mut limits_rhs = na::zero();
|
||||
let mut limits_impulse = na::zero();
|
||||
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
|
||||
|
||||
if limits_enabled.any() {
|
||||
let danchor = anchor2 - anchor1;
|
||||
let dist = danchor.dot(&axis1);
|
||||
|
||||
// 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 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 sign = _1.select(use_min, (-_1).select(use_max, _0));
|
||||
|
||||
if sign != _0 {
|
||||
limits_forcedir2 = Some(axis2 * sign);
|
||||
limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign;
|
||||
limits_impulse = lim_impulse.select(use_min | use_max, _0);
|
||||
}
|
||||
}
|
||||
|
||||
WPrismaticVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
axis2,
|
||||
limits_forcedir2,
|
||||
limits_rhs,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = self.impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
if let Some(limits_forcedir2) = self.limits_forcedir2 {
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
/*
|
||||
* Joint consraint.
|
||||
*/
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let lin_dvel = self.basis1.tr_mul(&lin_vel2);
|
||||
let ang_dvel = ang_vel2;
|
||||
#[cfg(feature = "dim2")]
|
||||
let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
|
||||
#[cfg(feature = "dim3")]
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
|
||||
#[cfg(feature = "dim2")]
|
||||
let ang_impulse = impulse.y;
|
||||
#[cfg(feature = "dim3")]
|
||||
let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
/*
|
||||
* Joint limits.
|
||||
*/
|
||||
if let Some(limits_forcedir2) = self.limits_forcedir2 {
|
||||
// FIXME: the transformation by ii2_sqrt could be avoided by
|
||||
// reusing some computations above.
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
|
||||
let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
|
||||
+ self.limits_rhs;
|
||||
let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero());
|
||||
let dimpulse = new_impulse - self.limits_impulse;
|
||||
self.limits_impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::PrismaticJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii);
|
||||
rev.limits_impulse = self.limits_impulse.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
|
||||
use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
|
||||
use crate::utils::WAngularInertia;
|
||||
use na::Unit;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevolutePositionConstraint {
|
||||
position1: usize,
|
||||
position2: usize,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
|
||||
ii1: AngularInertia<f32>,
|
||||
ii2: AngularInertia<f32>,
|
||||
|
||||
lin_inv_lhs: f32,
|
||||
ang_inv_lhs: AngularInertia<f32>,
|
||||
|
||||
local_anchor1: Point<f32>,
|
||||
local_anchor2: Point<f32>,
|
||||
|
||||
local_axis1: Unit<Vector<f32>>,
|
||||
local_axis2: Unit<Vector<f32>>,
|
||||
}
|
||||
|
||||
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 lin_inv_lhs = 1.0 / (im1 + im2);
|
||||
let ang_inv_lhs = (ii1 + ii2).inverse();
|
||||
|
||||
Self {
|
||||
im1,
|
||||
im2,
|
||||
ii1,
|
||||
ii2,
|
||||
lin_inv_lhs,
|
||||
ang_inv_lhs,
|
||||
local_anchor1: cparams.local_anchor1,
|
||||
local_anchor2: cparams.local_anchor2,
|
||||
local_axis1: cparams.local_axis1,
|
||||
local_axis2: cparams.local_axis2,
|
||||
position1: rb1.active_set_offset,
|
||||
position2: rb2.active_set_offset,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position1 = positions[self.position1 as usize];
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
let axis1 = position1 * self.local_axis1;
|
||||
let axis2 = position2 * self.local_axis2;
|
||||
let delta_rot =
|
||||
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or(Rotation::identity());
|
||||
let ang_error = delta_rot.scaled_axis() * params.joint_erp;
|
||||
let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error);
|
||||
|
||||
position1.rotation =
|
||||
Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
|
||||
position2.rotation =
|
||||
Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
|
||||
|
||||
let anchor1 = position1 * self.local_anchor1;
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
|
||||
let delta_tra = anchor2 - anchor1;
|
||||
let lin_error = delta_tra * params.joint_erp;
|
||||
let lin_impulse = self.lin_inv_lhs * lin_error;
|
||||
|
||||
position1.translation.vector += self.im1 * lin_impulse;
|
||||
position2.translation.vector -= self.im2 * lin_impulse;
|
||||
|
||||
positions[self.position1 as usize] = position1;
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevolutePositionGroundConstraint {
|
||||
position2: usize,
|
||||
anchor1: Point<f32>,
|
||||
local_anchor2: Point<f32>,
|
||||
axis1: Unit<Vector<f32>>,
|
||||
local_axis2: Unit<Vector<f32>>,
|
||||
}
|
||||
|
||||
impl RevolutePositionGroundConstraint {
|
||||
pub fn from_params(
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &RevoluteJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let anchor1;
|
||||
let local_anchor2;
|
||||
let axis1;
|
||||
let local_axis2;
|
||||
|
||||
if flipped {
|
||||
anchor1 = rb1.predicted_position * cparams.local_anchor2;
|
||||
local_anchor2 = cparams.local_anchor1;
|
||||
axis1 = rb1.predicted_position * cparams.local_axis2;
|
||||
local_axis2 = cparams.local_axis1;
|
||||
} else {
|
||||
anchor1 = rb1.predicted_position * cparams.local_anchor1;
|
||||
local_anchor2 = cparams.local_anchor2;
|
||||
axis1 = rb1.predicted_position * cparams.local_axis1;
|
||||
local_axis2 = cparams.local_axis2;
|
||||
};
|
||||
|
||||
Self {
|
||||
anchor1,
|
||||
local_anchor2,
|
||||
axis1,
|
||||
local_axis2,
|
||||
position2: rb2.active_set_offset,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
let mut position2 = positions[self.position2 as usize];
|
||||
|
||||
let axis2 = position2 * self.local_axis2;
|
||||
|
||||
let delta_rot =
|
||||
Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp)
|
||||
.unwrap_or(Rotation::identity());
|
||||
position2.rotation = delta_rot * position2.rotation;
|
||||
|
||||
let anchor2 = position2 * self.local_anchor2;
|
||||
let delta_tra = anchor2 - self.anchor1;
|
||||
let lin_error = delta_tra * params.joint_erp;
|
||||
position2.translation.vector -= lin_error;
|
||||
|
||||
positions[self.position2 as usize] = position2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,294 @@
|
||||
use crate::dynamics::solver::DeltaVel;
|
||||
use crate::dynamics::{
|
||||
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
|
||||
};
|
||||
use crate::math::{AngularInertia, Vector};
|
||||
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevoluteVelocityConstraint {
|
||||
mj_lambda1: usize,
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r1: Vector<f32>,
|
||||
r2: Vector<f32>,
|
||||
|
||||
inv_lhs: Matrix5<f32>,
|
||||
rhs: Vector5<f32>,
|
||||
impulse: Vector5<f32>,
|
||||
|
||||
basis1: Matrix3x2<f32>,
|
||||
|
||||
im1: f32,
|
||||
im2: f32,
|
||||
|
||||
ii1_sqrt: AngularInertia<f32>,
|
||||
ii2_sqrt: AngularInertia<f32>,
|
||||
}
|
||||
|
||||
impl RevoluteVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &RevoluteJoint,
|
||||
) -> Self {
|
||||
// Linear part.
|
||||
let anchor1 = rb1.position * cparams.local_anchor1;
|
||||
let anchor2 = rb2.position * cparams.local_anchor2;
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
rb1.position * cparams.basis1[0],
|
||||
rb1.position * cparams.basis1[1],
|
||||
]);
|
||||
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// 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 ii1 = rb1.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 r2 = anchor2 - rb2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 =
|
||||
ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
|
||||
let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
|
||||
let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
|
||||
|
||||
// Note that cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
|
||||
let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
|
||||
|
||||
RevoluteVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1: rb1.active_set_offset,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im1,
|
||||
ii1_sqrt: rb1.world_inv_inertia_sqrt,
|
||||
basis1,
|
||||
im2,
|
||||
ii2_sqrt: rb2.world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_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>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
|
||||
- mj_lambda1.linear
|
||||
- ang_vel1.gcross(self.r1);
|
||||
let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += self.im1 * lin_impulse;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct RevoluteVelocityGroundConstraint {
|
||||
mj_lambda2: usize,
|
||||
|
||||
joint_id: JointIndex,
|
||||
|
||||
r2: Vector<f32>,
|
||||
|
||||
inv_lhs: Matrix5<f32>,
|
||||
rhs: Vector5<f32>,
|
||||
impulse: Vector5<f32>,
|
||||
|
||||
basis1: Matrix3x2<f32>,
|
||||
|
||||
im2: f32,
|
||||
|
||||
ii2_sqrt: AngularInertia<f32>,
|
||||
}
|
||||
|
||||
impl RevoluteVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: JointIndex,
|
||||
rb1: &RigidBody,
|
||||
rb2: &RigidBody,
|
||||
cparams: &RevoluteJoint,
|
||||
flipped: bool,
|
||||
) -> Self {
|
||||
let anchor2;
|
||||
let anchor1;
|
||||
let basis1;
|
||||
|
||||
if flipped {
|
||||
anchor1 = rb1.position * cparams.local_anchor2;
|
||||
anchor2 = rb2.position * cparams.local_anchor1;
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
rb1.position * cparams.basis2[0],
|
||||
rb1.position * cparams.basis2[1],
|
||||
]);
|
||||
} else {
|
||||
anchor1 = rb1.position * cparams.local_anchor1;
|
||||
anchor2 = rb2.position * cparams.local_anchor2;
|
||||
basis1 = Matrix3x2::from_columns(&[
|
||||
rb1.position * cparams.basis1[0],
|
||||
rb1.position * cparams.basis1[1],
|
||||
]);
|
||||
};
|
||||
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .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 r1 = anchor1 - rb1.world_com;
|
||||
let r2 = anchor2 - rb2.world_com;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
|
||||
let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
|
||||
let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
|
||||
|
||||
// Note that cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
|
||||
let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
|
||||
|
||||
RevoluteVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2: rb2.active_set_offset,
|
||||
im2,
|
||||
ii2_sqrt: rb2.world_inv_inertia_sqrt,
|
||||
impulse: cparams.impulse * params.warmstart_coeff,
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let ang_dvel = self.basis1.tr_mul(&ang_vel2);
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= self.im2 * lin_impulse;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
let joint = &mut joints_all[self.joint_id].weight;
|
||||
if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
|
||||
revolute.impulse = self.impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,397 @@
|
||||
use simba::simd::SimdValue;
|
||||
|
||||
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::utils::{WAngularInertia, WCross, WCrossMatrix};
|
||||
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WRevoluteVelocityConstraint {
|
||||
mj_lambda1: [usize; SIMD_WIDTH],
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r1: Vector<SimdFloat>,
|
||||
r2: Vector<SimdFloat>,
|
||||
|
||||
inv_lhs: Matrix5<SimdFloat>,
|
||||
rhs: Vector5<SimdFloat>,
|
||||
impulse: Vector5<SimdFloat>,
|
||||
|
||||
basis1: Matrix3x2<SimdFloat>,
|
||||
|
||||
im1: SimdFloat,
|
||||
im2: SimdFloat,
|
||||
|
||||
ii1_sqrt: AngularInertia<SimdFloat>,
|
||||
ii2_sqrt: AngularInertia<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WRevoluteVelocityConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&RevoluteJoint; SIMD_WIDTH],
|
||||
) -> 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 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
|
||||
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
|
||||
let local_basis1 = [
|
||||
Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
|
||||
Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
|
||||
];
|
||||
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
let basis1 =
|
||||
Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
|
||||
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// 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 ii1 = ii1_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r1_mat = r1.gcross_matrix();
|
||||
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 =
|
||||
ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
|
||||
let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
|
||||
let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
|
||||
|
||||
// Note that cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
|
||||
let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
|
||||
|
||||
WRevoluteVelocityConstraint {
|
||||
joint_id,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
im1,
|
||||
ii1_sqrt,
|
||||
basis1,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r1,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
|
||||
- mj_lambda1.linear
|
||||
- ang_vel1.gcross(self.r1);
|
||||
let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda1.linear += lin_impulse * self.im1;
|
||||
mj_lambda1.angular += self
|
||||
.ii1_sqrt
|
||||
.transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::RevoluteJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub(crate) struct WRevoluteVelocityGroundConstraint {
|
||||
mj_lambda2: [usize; SIMD_WIDTH],
|
||||
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
|
||||
r2: Vector<SimdFloat>,
|
||||
|
||||
inv_lhs: Matrix5<SimdFloat>,
|
||||
rhs: Vector5<SimdFloat>,
|
||||
impulse: Vector5<SimdFloat>,
|
||||
|
||||
basis1: Matrix3x2<SimdFloat>,
|
||||
|
||||
im2: SimdFloat,
|
||||
|
||||
ii2_sqrt: AngularInertia<SimdFloat>,
|
||||
}
|
||||
|
||||
impl WRevoluteVelocityGroundConstraint {
|
||||
pub fn from_params(
|
||||
params: &IntegrationParameters,
|
||||
joint_id: [JointIndex; SIMD_WIDTH],
|
||||
rbs1: [&RigidBody; SIMD_WIDTH],
|
||||
rbs2: [&RigidBody; SIMD_WIDTH],
|
||||
cparams: [&RevoluteJoint; SIMD_WIDTH],
|
||||
flipped: [bool; SIMD_WIDTH],
|
||||
) -> 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 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 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 mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
|
||||
|
||||
let local_anchor1 = Point::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_anchor2 = Point::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
|
||||
);
|
||||
let basis1 = Matrix3x2::from_columns(&[
|
||||
position1
|
||||
* Vector::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
|
||||
),
|
||||
position1
|
||||
* Vector::from(
|
||||
array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
|
||||
),
|
||||
]);
|
||||
|
||||
let anchor1 = position1 * local_anchor1;
|
||||
let anchor2 = position2 * local_anchor2;
|
||||
|
||||
// let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
|
||||
// .unwrap_or(Rotation::identity())
|
||||
// .to_rotation_matrix()
|
||||
// .into_inner();
|
||||
// let basis2 = /*r21 * */ basis1;
|
||||
let ii2 = ii2_sqrt.squared();
|
||||
let r1 = anchor1 - world_com1;
|
||||
let r2 = anchor2 - world_com2;
|
||||
let r2_mat = r2.gcross_matrix();
|
||||
|
||||
let mut lhs = Matrix5::zeros();
|
||||
let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
|
||||
let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
|
||||
let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
|
||||
|
||||
// Note that cholesky won't read the upper-right part
|
||||
// of lhs so we don't have to fill it.
|
||||
lhs.fixed_slice_mut::<U3, U3>(0, 0)
|
||||
.copy_from(&lhs00.into_matrix());
|
||||
lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
|
||||
lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
|
||||
|
||||
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
|
||||
|
||||
let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
|
||||
let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
|
||||
let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
|
||||
|
||||
WRevoluteVelocityGroundConstraint {
|
||||
joint_id,
|
||||
mj_lambda2,
|
||||
im2,
|
||||
ii2_sqrt,
|
||||
impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
|
||||
basis1,
|
||||
inv_lhs,
|
||||
rhs,
|
||||
r2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
|
||||
let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
|
||||
let ang_dvel = self.basis1.tr_mul(&ang_vel2);
|
||||
let rhs =
|
||||
Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
|
||||
let impulse = self.inv_lhs * rhs;
|
||||
self.impulse += impulse;
|
||||
let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
|
||||
let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
|
||||
|
||||
mj_lambda2.linear -= lin_impulse * self.im2;
|
||||
mj_lambda2.angular -= self
|
||||
.ii2_sqrt
|
||||
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
// FIXME: duplicated code with the non-ground constraint.
|
||||
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let joint = &mut joints_all[self.joint_id[ii]].weight;
|
||||
if let JointParams::RevoluteJoint(rev) = &mut joint.params {
|
||||
rev.impulse = self.impulse.extract(ii)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
56
src/dynamics/solver/mod.rs
Normal file
56
src/dynamics/solver/mod.rs
Normal file
@@ -0,0 +1,56 @@
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
pub(crate) use self::island_solver::IslandSolver;
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
|
||||
#[cfg(feature = "parallel")]
|
||||
pub(self) use self::parallel_position_solver::ParallelPositionSolver;
|
||||
#[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::velocity_solver::VelocitySolver;
|
||||
pub(self) use delta_vel::DeltaVel;
|
||||
pub(self) use interaction_groups::*;
|
||||
pub(self) use joint_constraint::*;
|
||||
pub(self) use position_constraint::*;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use position_constraint_wide::*;
|
||||
pub(self) use position_ground_constraint::*;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use position_ground_constraint_wide::*;
|
||||
pub(self) use velocity_constraint::*;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use velocity_constraint_wide::*;
|
||||
pub(self) use velocity_ground_constraint::*;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
pub(self) use velocity_ground_constraint_wide::*;
|
||||
|
||||
mod categorization;
|
||||
mod delta_vel;
|
||||
mod interaction_groups;
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
mod island_solver;
|
||||
mod joint_constraint;
|
||||
#[cfg(feature = "parallel")]
|
||||
mod parallel_island_solver;
|
||||
#[cfg(feature = "parallel")]
|
||||
mod parallel_position_solver;
|
||||
#[cfg(feature = "parallel")]
|
||||
mod parallel_velocity_solver;
|
||||
mod position_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod position_constraint_wide;
|
||||
mod position_ground_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod position_ground_constraint_wide;
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
mod position_solver;
|
||||
mod velocity_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod velocity_constraint_wide;
|
||||
mod velocity_ground_constraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
mod velocity_ground_constraint_wide;
|
||||
#[cfg(not(feature = "parallel"))]
|
||||
mod velocity_solver;
|
||||
259
src/dynamics/solver/parallel_island_solver.rs
Normal file
259
src/dynamics/solver/parallel_island_solver.rs
Normal file
@@ -0,0 +1,259 @@
|
||||
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
|
||||
use crate::dynamics::solver::ParallelPositionSolver;
|
||||
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
use crate::math::Isometry;
|
||||
use crate::utils::WAngularInertia;
|
||||
use rayon::Scope;
|
||||
use std::sync::atomic::{AtomicUsize, Ordering};
|
||||
|
||||
#[macro_export]
|
||||
#[doc(hidden)]
|
||||
macro_rules! concurrent_loop {
|
||||
(let batch_size = $batch_size: expr;
|
||||
for $elt: ident in $array: ident[$index_stream:expr,$index_count:expr] $f: expr) => {
|
||||
let max_index = $array.len();
|
||||
|
||||
if max_index > 0 {
|
||||
loop {
|
||||
let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
|
||||
if start_index > max_index {
|
||||
break;
|
||||
}
|
||||
|
||||
let end_index = (start_index + $batch_size).min(max_index);
|
||||
for $elt in &$array[start_index..end_index] {
|
||||
$f
|
||||
}
|
||||
|
||||
$index_count.fetch_add(end_index - start_index, Ordering::SeqCst);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
(let batch_size = $batch_size: expr;
|
||||
for $elt: ident in $array: ident[$index_stream:expr] $f: expr) => {
|
||||
let max_index = $array.len();
|
||||
|
||||
if max_index > 0 {
|
||||
loop {
|
||||
let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
|
||||
if start_index > max_index {
|
||||
break;
|
||||
}
|
||||
|
||||
let end_index = (start_index + $batch_size).min(max_index);
|
||||
for $elt in &$array[start_index..end_index] {
|
||||
$f
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
pub(crate) struct ThreadContext {
|
||||
pub batch_size: usize,
|
||||
// Velocity solver.
|
||||
pub constraint_initialization_index: AtomicUsize,
|
||||
pub num_initialized_constraints: AtomicUsize,
|
||||
pub joint_constraint_initialization_index: AtomicUsize,
|
||||
pub num_initialized_joint_constraints: AtomicUsize,
|
||||
pub warmstart_contact_index: AtomicUsize,
|
||||
pub num_warmstarted_contacts: AtomicUsize,
|
||||
pub warmstart_joint_index: AtomicUsize,
|
||||
pub num_warmstarted_joints: AtomicUsize,
|
||||
pub solve_interaction_index: AtomicUsize,
|
||||
pub num_solved_interactions: AtomicUsize,
|
||||
pub impulse_writeback_index: AtomicUsize,
|
||||
pub joint_writeback_index: AtomicUsize,
|
||||
pub body_integration_index: AtomicUsize,
|
||||
pub num_integrated_bodies: AtomicUsize,
|
||||
// Position solver.
|
||||
pub position_constraint_initialization_index: AtomicUsize,
|
||||
pub num_initialized_position_constraints: AtomicUsize,
|
||||
pub position_joint_constraint_initialization_index: AtomicUsize,
|
||||
pub num_initialized_position_joint_constraints: AtomicUsize,
|
||||
pub solve_position_interaction_index: AtomicUsize,
|
||||
pub num_solved_position_interactions: AtomicUsize,
|
||||
pub position_writeback_index: AtomicUsize,
|
||||
}
|
||||
|
||||
impl ThreadContext {
|
||||
pub fn new(batch_size: usize) -> Self {
|
||||
ThreadContext {
|
||||
batch_size, // TODO perhaps there is some optimal value we can compute depending on the island size?
|
||||
constraint_initialization_index: AtomicUsize::new(0),
|
||||
num_initialized_constraints: AtomicUsize::new(0),
|
||||
joint_constraint_initialization_index: AtomicUsize::new(0),
|
||||
num_initialized_joint_constraints: AtomicUsize::new(0),
|
||||
num_warmstarted_contacts: AtomicUsize::new(0),
|
||||
warmstart_contact_index: AtomicUsize::new(0),
|
||||
num_warmstarted_joints: AtomicUsize::new(0),
|
||||
warmstart_joint_index: AtomicUsize::new(0),
|
||||
solve_interaction_index: AtomicUsize::new(0),
|
||||
num_solved_interactions: AtomicUsize::new(0),
|
||||
impulse_writeback_index: AtomicUsize::new(0),
|
||||
joint_writeback_index: AtomicUsize::new(0),
|
||||
body_integration_index: AtomicUsize::new(0),
|
||||
num_integrated_bodies: AtomicUsize::new(0),
|
||||
position_constraint_initialization_index: AtomicUsize::new(0),
|
||||
num_initialized_position_constraints: AtomicUsize::new(0),
|
||||
position_joint_constraint_initialization_index: AtomicUsize::new(0),
|
||||
num_initialized_position_joint_constraints: AtomicUsize::new(0),
|
||||
solve_position_interaction_index: AtomicUsize::new(0),
|
||||
num_solved_position_interactions: AtomicUsize::new(0),
|
||||
position_writeback_index: AtomicUsize::new(0),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn lock_until_ge(val: &AtomicUsize, target: usize) {
|
||||
if target > 0 {
|
||||
// let backoff = crossbeam::utils::Backoff::new();
|
||||
std::sync::atomic::fence(Ordering::SeqCst);
|
||||
while val.load(Ordering::Relaxed) < target {
|
||||
// backoff.spin();
|
||||
// std::thread::yield_now();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub struct ParallelIslandSolver {
|
||||
mj_lambdas: Vec<DeltaVel<f32>>,
|
||||
positions: Vec<Isometry<f32>>,
|
||||
parallel_groups: ParallelInteractionGroups,
|
||||
parallel_joint_groups: ParallelInteractionGroups,
|
||||
parallel_velocity_solver: ParallelVelocitySolver,
|
||||
parallel_position_solver: ParallelPositionSolver,
|
||||
thread: ThreadContext,
|
||||
}
|
||||
|
||||
impl ParallelIslandSolver {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
mj_lambdas: Vec::new(),
|
||||
positions: Vec::new(),
|
||||
parallel_groups: ParallelInteractionGroups::new(),
|
||||
parallel_joint_groups: ParallelInteractionGroups::new(),
|
||||
parallel_velocity_solver: ParallelVelocitySolver::new(),
|
||||
parallel_position_solver: ParallelPositionSolver::new(),
|
||||
thread: ThreadContext::new(8),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_island<'s>(
|
||||
&'s mut self,
|
||||
scope: &Scope<'s>,
|
||||
island_id: usize,
|
||||
params: &'s IntegrationParameters,
|
||||
bodies: &'s mut RigidBodySet,
|
||||
manifolds: &'s mut Vec<&'s mut ContactManifold>,
|
||||
manifold_indices: &'s [ContactManifoldIndex],
|
||||
joints: &'s mut Vec<JointGraphEdge>,
|
||||
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?
|
||||
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(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds,
|
||||
&self.parallel_groups,
|
||||
joints,
|
||||
&self.parallel_joint_groups,
|
||||
);
|
||||
self.parallel_position_solver.init_constraint_groups(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds,
|
||||
&self.parallel_groups,
|
||||
joints,
|
||||
&self.parallel_joint_groups,
|
||||
);
|
||||
|
||||
self.mj_lambdas.clear();
|
||||
self.mj_lambdas
|
||||
.resize(bodies.active_island(island_id).len(), DeltaVel::zero());
|
||||
self.positions.clear();
|
||||
self.positions
|
||||
.resize(bodies.active_island(island_id).len(), Isometry::identity());
|
||||
|
||||
for _ in 0..num_task_per_island {
|
||||
// We use AtomicPtr because it is Send+Sync while *mut is not.
|
||||
// See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818
|
||||
let thread = &self.thread;
|
||||
let mj_lambdas = std::sync::atomic::AtomicPtr::new(&mut self.mj_lambdas as *mut _);
|
||||
let positions = std::sync::atomic::AtomicPtr::new(&mut self.positions as *mut _);
|
||||
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 _);
|
||||
|
||||
scope.spawn(move |_| {
|
||||
// Transmute *mut -> &mut
|
||||
let mj_lambdas: &mut Vec<DeltaVel<f32>> =
|
||||
unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
|
||||
let positions: &mut Vec<Isometry<f32>> =
|
||||
unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
|
||||
let bodies: &mut RigidBodySet =
|
||||
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
|
||||
let manifolds: &mut Vec<&mut ContactManifold> =
|
||||
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_position_solver: &mut ParallelPositionSolver = unsafe {
|
||||
std::mem::transmute(parallel_position_solver.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);
|
||||
|
||||
// Write results back to rigid bodies and integrate velocities.
|
||||
let island_range = bodies.active_island_range(island_id);
|
||||
let active_bodies = &bodies.active_dynamic_set[island_range];
|
||||
let bodies = &mut bodies.bodies;
|
||||
|
||||
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 dvel = mj_lambdas[rb.active_set_offset];
|
||||
rb.linvel += dvel.linear;
|
||||
rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
|
||||
rb.integrate(params.dt());
|
||||
positions[rb.active_set_offset] = rb.position;
|
||||
}
|
||||
}
|
||||
|
||||
// We need to way for every body to be integrated because it also
|
||||
// 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);
|
||||
|
||||
// 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];
|
||||
rb.set_position(positions[rb.active_set_offset]);
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
}
|
||||
582
src/dynamics/solver/parallel_position_solver.rs
Normal file
582
src/dynamics/solver/parallel_position_solver.rs
Normal file
@@ -0,0 +1,582 @@
|
||||
use super::ParallelInteractionGroups;
|
||||
use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext};
|
||||
use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
|
||||
use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint};
|
||||
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
|
||||
use crate::geometry::ContactManifold;
|
||||
use crate::math::Isometry;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::{
|
||||
dynamics::solver::{WPositionConstraint, WPositionGroundConstraint},
|
||||
simd::SIMD_WIDTH,
|
||||
};
|
||||
use std::sync::atomic::Ordering;
|
||||
|
||||
pub(crate) enum PositionConstraintDesc {
|
||||
NongroundNongrouped(usize),
|
||||
GroundNongrouped(usize),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
NongroundGrouped([usize; SIMD_WIDTH]),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroundGrouped([usize; SIMD_WIDTH]),
|
||||
}
|
||||
|
||||
pub(crate) struct ParallelPositionSolverContactPart {
|
||||
pub point_point: Vec<usize>,
|
||||
pub plane_point: Vec<usize>,
|
||||
pub ground_point_point: Vec<usize>,
|
||||
pub ground_plane_point: Vec<usize>,
|
||||
pub interaction_groups: InteractionGroups,
|
||||
pub ground_interaction_groups: InteractionGroups,
|
||||
pub constraints: Vec<AnyPositionConstraint>,
|
||||
pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
|
||||
pub parallel_desc_groups: Vec<usize>,
|
||||
}
|
||||
|
||||
pub(crate) struct ParallelPositionSolverJointPart {
|
||||
pub not_ground_interactions: Vec<usize>,
|
||||
pub ground_interactions: Vec<usize>,
|
||||
pub interaction_groups: InteractionGroups,
|
||||
pub ground_interaction_groups: InteractionGroups,
|
||||
pub constraints: Vec<AnyJointPositionConstraint>,
|
||||
pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
|
||||
pub parallel_desc_groups: Vec<usize>,
|
||||
}
|
||||
|
||||
impl ParallelPositionSolverContactPart {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
point_point: Vec::new(),
|
||||
plane_point: Vec::new(),
|
||||
ground_point_point: Vec::new(),
|
||||
ground_plane_point: Vec::new(),
|
||||
interaction_groups: InteractionGroups::new(),
|
||||
ground_interaction_groups: InteractionGroups::new(),
|
||||
constraints: Vec::new(),
|
||||
constraint_descs: Vec::new(),
|
||||
parallel_desc_groups: Vec::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
impl ParallelPositionSolverJointPart {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
not_ground_interactions: Vec::new(),
|
||||
ground_interactions: Vec::new(),
|
||||
interaction_groups: InteractionGroups::new(),
|
||||
ground_interaction_groups: InteractionGroups::new(),
|
||||
constraints: Vec::new(),
|
||||
constraint_descs: Vec::new(),
|
||||
parallel_desc_groups: Vec::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ParallelPositionSolverJointPart {
|
||||
pub fn init_constraints_groups(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
joints: &mut [JointGraphEdge],
|
||||
joint_groups: &ParallelInteractionGroups,
|
||||
) {
|
||||
let mut total_num_constraints = 0;
|
||||
let num_groups = joint_groups.num_groups();
|
||||
|
||||
self.interaction_groups.clear_groups();
|
||||
self.ground_interaction_groups.clear_groups();
|
||||
self.parallel_desc_groups.clear();
|
||||
self.constraint_descs.clear();
|
||||
self.parallel_desc_groups.push(0);
|
||||
|
||||
for i in 0..num_groups {
|
||||
let group = joint_groups.group(i);
|
||||
|
||||
self.not_ground_interactions.clear();
|
||||
self.ground_interactions.clear();
|
||||
categorize_joints(
|
||||
bodies,
|
||||
joints,
|
||||
group,
|
||||
&mut self.ground_interactions,
|
||||
&mut self.not_ground_interactions,
|
||||
);
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
let start_grouped = self.interaction_groups.grouped_interactions.len();
|
||||
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
|
||||
let start_nongrouped_ground =
|
||||
self.ground_interaction_groups.nongrouped_interactions.len();
|
||||
|
||||
self.interaction_groups.group_joints(
|
||||
island_id,
|
||||
bodies,
|
||||
joints,
|
||||
&self.not_ground_interactions,
|
||||
);
|
||||
self.ground_interaction_groups.group_joints(
|
||||
island_id,
|
||||
bodies,
|
||||
joints,
|
||||
&self.ground_interactions,
|
||||
);
|
||||
|
||||
// Compute constraint indices.
|
||||
for interaction_i in
|
||||
&self.interaction_groups.nongrouped_interactions[start_nongrouped..]
|
||||
{
|
||||
let joint = &mut joints[*interaction_i].weight;
|
||||
joint.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::NongroundNongrouped(*interaction_i),
|
||||
));
|
||||
total_num_constraints +=
|
||||
AnyJointPositionConstraint::num_active_constraints(joint, false);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
for interaction_i in
|
||||
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
|
||||
{
|
||||
let joint = &mut joints[interaction_i[0]].weight;
|
||||
joint.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::NongroundGrouped(
|
||||
array![|ii| interaction_i[ii]; SIMD_WIDTH],
|
||||
),
|
||||
));
|
||||
total_num_constraints +=
|
||||
AnyJointPositionConstraint::num_active_constraints(joint, true);
|
||||
}
|
||||
|
||||
for interaction_i in
|
||||
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
|
||||
{
|
||||
let joint = &mut joints[*interaction_i].weight;
|
||||
joint.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::GroundNongrouped(*interaction_i),
|
||||
));
|
||||
total_num_constraints +=
|
||||
AnyJointPositionConstraint::num_active_constraints(joint, false);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
for interaction_i in self.ground_interaction_groups.grouped_interactions
|
||||
[start_grouped_ground..]
|
||||
.chunks(SIMD_WIDTH)
|
||||
{
|
||||
let joint = &mut joints[interaction_i[0]].weight;
|
||||
joint.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::GroundGrouped(
|
||||
array![|ii| interaction_i[ii]; SIMD_WIDTH],
|
||||
),
|
||||
));
|
||||
total_num_constraints +=
|
||||
AnyJointPositionConstraint::num_active_constraints(joint, true);
|
||||
}
|
||||
|
||||
self.parallel_desc_groups.push(self.constraint_descs.len());
|
||||
}
|
||||
|
||||
// Resize the constraints set.
|
||||
self.constraints.clear();
|
||||
self.constraints
|
||||
.resize_with(total_num_constraints, || AnyJointPositionConstraint::Empty)
|
||||
}
|
||||
|
||||
fn fill_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
bodies: &RigidBodySet,
|
||||
joints_all: &[JointGraphEdge],
|
||||
) {
|
||||
let descs = &self.constraint_descs;
|
||||
|
||||
crate::concurrent_loop! {
|
||||
let batch_size = thread.batch_size;
|
||||
for desc in descs[thread.position_joint_constraint_initialization_index, thread.num_initialized_position_joint_constraints] {
|
||||
match &desc.1 {
|
||||
PositionConstraintDesc::NongroundNongrouped(joint_id) => {
|
||||
let joint = &joints_all[*joint_id].weight;
|
||||
let constraint = AnyJointPositionConstraint::from_joint(
|
||||
joint,
|
||||
bodies,
|
||||
);
|
||||
self.constraints[joint.position_constraint_index] = constraint;
|
||||
}
|
||||
PositionConstraintDesc::GroundNongrouped(joint_id) => {
|
||||
let joint = &joints_all[*joint_id].weight;
|
||||
let constraint = AnyJointPositionConstraint::from_joint_ground(
|
||||
joint,
|
||||
bodies,
|
||||
);
|
||||
self.constraints[joint.position_constraint_index] = constraint;
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
PositionConstraintDesc::NongroundGrouped(joint_id) => {
|
||||
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
|
||||
if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint(
|
||||
joints, bodies,
|
||||
) {
|
||||
self.constraints[joints[0].position_constraint_index] = constraint
|
||||
} else {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let constraint = AnyJointPositionConstraint::from_joint(joints[ii], bodies);
|
||||
self.constraints[joints[0].position_constraint_index + ii] = constraint;
|
||||
}
|
||||
}
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
PositionConstraintDesc::GroundGrouped(joint_id) => {
|
||||
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
|
||||
if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint_ground(
|
||||
joints, bodies,
|
||||
) {
|
||||
self.constraints[joints[0].position_constraint_index] = constraint
|
||||
} else {
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let constraint = AnyJointPositionConstraint::from_joint_ground(joints[ii], bodies);
|
||||
self.constraints[joints[0].position_constraint_index + ii] = constraint;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ParallelPositionSolverContactPart {
|
||||
pub fn init_constraints_groups(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &mut [&mut ContactManifold],
|
||||
manifold_groups: &ParallelInteractionGroups,
|
||||
) {
|
||||
let mut total_num_constraints = 0;
|
||||
let num_groups = manifold_groups.num_groups();
|
||||
|
||||
self.interaction_groups.clear_groups();
|
||||
self.ground_interaction_groups.clear_groups();
|
||||
self.parallel_desc_groups.clear();
|
||||
self.constraint_descs.clear();
|
||||
self.parallel_desc_groups.push(0);
|
||||
|
||||
for i in 0..num_groups {
|
||||
let group = manifold_groups.group(i);
|
||||
|
||||
self.ground_point_point.clear();
|
||||
self.ground_plane_point.clear();
|
||||
self.point_point.clear();
|
||||
self.plane_point.clear();
|
||||
categorize_position_contacts(
|
||||
bodies,
|
||||
manifolds,
|
||||
group,
|
||||
&mut self.ground_point_point,
|
||||
&mut self.ground_plane_point,
|
||||
&mut self.point_point,
|
||||
&mut self.plane_point,
|
||||
);
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
let start_grouped = self.interaction_groups.grouped_interactions.len();
|
||||
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
|
||||
let start_nongrouped_ground =
|
||||
self.ground_interaction_groups.nongrouped_interactions.len();
|
||||
|
||||
self.interaction_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds,
|
||||
&self.point_point,
|
||||
);
|
||||
self.interaction_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds,
|
||||
&self.plane_point,
|
||||
);
|
||||
self.ground_interaction_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds,
|
||||
&self.ground_point_point,
|
||||
);
|
||||
self.ground_interaction_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds,
|
||||
&self.ground_plane_point,
|
||||
);
|
||||
|
||||
// Compute constraint indices.
|
||||
for interaction_i in
|
||||
&self.interaction_groups.nongrouped_interactions[start_nongrouped..]
|
||||
{
|
||||
let manifold = &mut *manifolds[*interaction_i];
|
||||
manifold.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::NongroundNongrouped(*interaction_i),
|
||||
));
|
||||
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
for interaction_i in
|
||||
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
|
||||
{
|
||||
let manifold = &mut *manifolds[interaction_i[0]];
|
||||
manifold.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::NongroundGrouped(
|
||||
array![|ii| interaction_i[ii]; SIMD_WIDTH],
|
||||
),
|
||||
));
|
||||
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
|
||||
}
|
||||
|
||||
for interaction_i in
|
||||
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
|
||||
{
|
||||
let manifold = &mut *manifolds[*interaction_i];
|
||||
manifold.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::GroundNongrouped(*interaction_i),
|
||||
));
|
||||
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
for interaction_i in self.ground_interaction_groups.grouped_interactions
|
||||
[start_grouped_ground..]
|
||||
.chunks(SIMD_WIDTH)
|
||||
{
|
||||
let manifold = &mut *manifolds[interaction_i[0]];
|
||||
manifold.position_constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
PositionConstraintDesc::GroundGrouped(
|
||||
array![|ii| interaction_i[ii]; SIMD_WIDTH],
|
||||
),
|
||||
));
|
||||
total_num_constraints += PositionConstraint::num_active_constraints(manifold);
|
||||
}
|
||||
|
||||
self.parallel_desc_groups.push(self.constraint_descs.len());
|
||||
}
|
||||
|
||||
// Resize the constraints set.
|
||||
self.constraints.clear();
|
||||
self.constraints
|
||||
.resize_with(total_num_constraints, || AnyPositionConstraint::Empty)
|
||||
}
|
||||
|
||||
fn fill_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
) {
|
||||
let descs = &self.constraint_descs;
|
||||
|
||||
crate::concurrent_loop! {
|
||||
let batch_size = thread.batch_size;
|
||||
for desc in descs[thread.position_constraint_initialization_index, thread.num_initialized_position_constraints] {
|
||||
match &desc.1 {
|
||||
PositionConstraintDesc::NongroundNongrouped(manifold_id) => {
|
||||
let manifold = &*manifolds_all[*manifold_id];
|
||||
PositionConstraint::generate(
|
||||
params,
|
||||
manifold,
|
||||
bodies,
|
||||
&mut self.constraints,
|
||||
false,
|
||||
);
|
||||
}
|
||||
PositionConstraintDesc::GroundNongrouped(manifold_id) => {
|
||||
let manifold = &*manifolds_all[*manifold_id];
|
||||
PositionGroundConstraint::generate(
|
||||
params,
|
||||
manifold,
|
||||
bodies,
|
||||
&mut self.constraints,
|
||||
false,
|
||||
);
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
PositionConstraintDesc::NongroundGrouped(manifold_id) => {
|
||||
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
|
||||
WPositionConstraint::generate(
|
||||
params,
|
||||
manifolds,
|
||||
bodies,
|
||||
&mut self.constraints,
|
||||
false,
|
||||
);
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
PositionConstraintDesc::GroundGrouped(manifold_id) => {
|
||||
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
|
||||
WPositionGroundConstraint::generate(
|
||||
params,
|
||||
manifolds,
|
||||
bodies,
|
||||
&mut self.constraints,
|
||||
false,
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct ParallelPositionSolver {
|
||||
part: ParallelPositionSolverContactPart,
|
||||
joint_part: ParallelPositionSolverJointPart,
|
||||
}
|
||||
|
||||
impl ParallelPositionSolver {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
part: ParallelPositionSolverContactPart::new(),
|
||||
joint_part: ParallelPositionSolverJointPart::new(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn init_constraint_groups(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &mut [&mut ContactManifold],
|
||||
manifold_groups: &ParallelInteractionGroups,
|
||||
joints: &mut [JointGraphEdge],
|
||||
joint_groups: &ParallelInteractionGroups,
|
||||
) {
|
||||
self.part
|
||||
.init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
|
||||
self.joint_part
|
||||
.init_constraints_groups(island_id, bodies, joints, joint_groups);
|
||||
}
|
||||
|
||||
pub fn fill_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &[&mut ContactManifold],
|
||||
joints: &[JointGraphEdge],
|
||||
) {
|
||||
self.part
|
||||
.fill_constraints(thread, params, bodies, manifolds);
|
||||
self.joint_part.fill_constraints(thread, bodies, joints);
|
||||
ThreadContext::lock_until_ge(
|
||||
&thread.num_initialized_position_constraints,
|
||||
self.part.constraint_descs.len(),
|
||||
);
|
||||
ThreadContext::lock_until_ge(
|
||||
&thread.num_initialized_position_joint_constraints,
|
||||
self.joint_part.constraint_descs.len(),
|
||||
);
|
||||
}
|
||||
|
||||
pub fn solve_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
if self.part.constraint_descs.len() == 0 {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Solve constraints.
|
||||
*/
|
||||
{
|
||||
// Each thread will concurrently grab thread.batch_size constraint desc to
|
||||
// solve. If the batch size is large enough for to cross the boundary of
|
||||
// a palallel_desc_group, we have to wait util the current group is finished
|
||||
// before starting the next one.
|
||||
let mut start_index = thread
|
||||
.solve_position_interaction_index
|
||||
.fetch_add(thread.batch_size, Ordering::SeqCst);
|
||||
let mut batch_size = thread.batch_size;
|
||||
let contact_descs = &self.part.constraint_descs[..];
|
||||
let joint_descs = &self.joint_part.constraint_descs[..];
|
||||
let mut target_num_desc = 0;
|
||||
let mut shift = 0;
|
||||
|
||||
for _ in 0..params.max_position_iterations {
|
||||
macro_rules! solve {
|
||||
($part: expr) => {
|
||||
// Joint groups.
|
||||
for group in $part.parallel_desc_groups.windows(2) {
|
||||
let num_descs_in_group = group[1] - group[0];
|
||||
target_num_desc += num_descs_in_group;
|
||||
|
||||
while start_index < group[1] {
|
||||
let end_index = (start_index + batch_size).min(group[1]);
|
||||
|
||||
let constraints = if end_index == $part.constraint_descs.len() {
|
||||
&mut $part.constraints[$part.constraint_descs[start_index].0..]
|
||||
} else {
|
||||
&mut $part.constraints[$part.constraint_descs[start_index].0
|
||||
..$part.constraint_descs[end_index].0]
|
||||
};
|
||||
|
||||
for constraint in constraints {
|
||||
constraint.solve(params, positions);
|
||||
}
|
||||
|
||||
let num_solved = end_index - start_index;
|
||||
batch_size -= num_solved;
|
||||
|
||||
thread
|
||||
.num_solved_position_interactions
|
||||
.fetch_add(num_solved, Ordering::SeqCst);
|
||||
|
||||
if batch_size == 0 {
|
||||
start_index = thread
|
||||
.solve_position_interaction_index
|
||||
.fetch_add(thread.batch_size, Ordering::SeqCst);
|
||||
start_index -= shift;
|
||||
batch_size = thread.batch_size;
|
||||
} else {
|
||||
start_index += num_solved;
|
||||
}
|
||||
}
|
||||
ThreadContext::lock_until_ge(
|
||||
&thread.num_solved_position_interactions,
|
||||
target_num_desc,
|
||||
);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
solve!(self.joint_part);
|
||||
shift += joint_descs.len();
|
||||
start_index -= joint_descs.len();
|
||||
solve!(self.part);
|
||||
shift += contact_descs.len();
|
||||
start_index -= contact_descs.len();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
485
src/dynamics/solver/parallel_velocity_solver.rs
Normal file
485
src/dynamics/solver/parallel_velocity_solver.rs
Normal file
@@ -0,0 +1,485 @@
|
||||
use super::ParallelInteractionGroups;
|
||||
use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
|
||||
use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts};
|
||||
use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
|
||||
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
|
||||
use crate::geometry::ContactManifold;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::{
|
||||
dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
|
||||
simd::SIMD_WIDTH,
|
||||
};
|
||||
use std::sync::atomic::Ordering;
|
||||
|
||||
pub(crate) enum VelocityConstraintDesc {
|
||||
NongroundNongrouped(usize),
|
||||
GroundNongrouped(usize),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
NongroundGrouped([usize; SIMD_WIDTH]),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroundGrouped([usize; SIMD_WIDTH]),
|
||||
}
|
||||
|
||||
pub(crate) struct ParallelVelocitySolverPart<Constraint> {
|
||||
pub not_ground_interactions: Vec<usize>,
|
||||
pub ground_interactions: Vec<usize>,
|
||||
pub interaction_groups: InteractionGroups,
|
||||
pub ground_interaction_groups: InteractionGroups,
|
||||
pub constraints: Vec<Constraint>,
|
||||
pub constraint_descs: Vec<(usize, VelocityConstraintDesc)>,
|
||||
pub parallel_desc_groups: Vec<usize>,
|
||||
}
|
||||
|
||||
impl<Constraint> ParallelVelocitySolverPart<Constraint> {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
not_ground_interactions: Vec::new(),
|
||||
ground_interactions: Vec::new(),
|
||||
interaction_groups: InteractionGroups::new(),
|
||||
ground_interaction_groups: InteractionGroups::new(),
|
||||
constraints: Vec::new(),
|
||||
constraint_descs: Vec::new(),
|
||||
parallel_desc_groups: Vec::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! impl_init_constraints_group {
|
||||
($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
|
||||
impl ParallelVelocitySolverPart<$Constraint> {
|
||||
pub fn init_constraints_groups(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
interactions: &mut [$Interaction],
|
||||
interaction_groups: &ParallelInteractionGroups,
|
||||
) {
|
||||
let mut total_num_constraints = 0;
|
||||
let num_groups = interaction_groups.num_groups();
|
||||
|
||||
self.interaction_groups.clear_groups();
|
||||
self.ground_interaction_groups.clear_groups();
|
||||
self.parallel_desc_groups.clear();
|
||||
self.constraint_descs.clear();
|
||||
self.parallel_desc_groups.push(0);
|
||||
|
||||
for i in 0..num_groups {
|
||||
let group = interaction_groups.group(i);
|
||||
|
||||
self.not_ground_interactions.clear();
|
||||
self.ground_interactions.clear();
|
||||
$categorize(
|
||||
bodies,
|
||||
interactions,
|
||||
group,
|
||||
&mut self.ground_interactions,
|
||||
&mut self.not_ground_interactions,
|
||||
);
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
let start_grouped = self.interaction_groups.grouped_interactions.len();
|
||||
let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
|
||||
let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
|
||||
|
||||
self.interaction_groups.$group(
|
||||
island_id,
|
||||
bodies,
|
||||
interactions,
|
||||
&self.not_ground_interactions,
|
||||
);
|
||||
self.ground_interaction_groups.$group(
|
||||
island_id,
|
||||
bodies,
|
||||
interactions,
|
||||
&self.ground_interactions,
|
||||
);
|
||||
|
||||
// Compute constraint indices.
|
||||
for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] {
|
||||
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
|
||||
interaction.constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
VelocityConstraintDesc::NongroundNongrouped(*interaction_i),
|
||||
));
|
||||
total_num_constraints += $num_active_constraints(interaction);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
for interaction_i in
|
||||
self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
|
||||
{
|
||||
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
|
||||
interaction.constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
VelocityConstraintDesc::NongroundGrouped(
|
||||
array![|ii| interaction_i[ii]; SIMD_WIDTH],
|
||||
),
|
||||
));
|
||||
total_num_constraints += $num_active_constraints(interaction);
|
||||
}
|
||||
|
||||
for interaction_i in
|
||||
&self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
|
||||
{
|
||||
let interaction = &mut interactions[*interaction_i]$(.$weight)*;
|
||||
interaction.constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
VelocityConstraintDesc::GroundNongrouped(*interaction_i),
|
||||
));
|
||||
total_num_constraints += $num_active_constraints(interaction);
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
for interaction_i in self.ground_interaction_groups.grouped_interactions
|
||||
[start_grouped_ground..]
|
||||
.chunks(SIMD_WIDTH)
|
||||
{
|
||||
let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
|
||||
interaction.constraint_index = total_num_constraints;
|
||||
self.constraint_descs.push((
|
||||
total_num_constraints,
|
||||
VelocityConstraintDesc::GroundGrouped(
|
||||
array![|ii| interaction_i[ii]; SIMD_WIDTH],
|
||||
),
|
||||
));
|
||||
total_num_constraints += $num_active_constraints(interaction);
|
||||
}
|
||||
|
||||
self.parallel_desc_groups.push(self.constraint_descs.len());
|
||||
}
|
||||
|
||||
// Resize the constraints set.
|
||||
self.constraints.clear();
|
||||
self.constraints
|
||||
.resize_with(total_num_constraints, || $empty_constraint)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl_init_constraints_group!(
|
||||
AnyVelocityConstraint,
|
||||
&mut ContactManifold,
|
||||
categorize_velocity_contacts,
|
||||
group_manifolds,
|
||||
VelocityConstraint::num_active_constraints,
|
||||
AnyVelocityConstraint::Empty
|
||||
);
|
||||
|
||||
impl_init_constraints_group!(
|
||||
AnyJointVelocityConstraint,
|
||||
JointGraphEdge,
|
||||
categorize_joints,
|
||||
group_joints,
|
||||
AnyJointVelocityConstraint::num_active_constraints,
|
||||
AnyJointVelocityConstraint::Empty,
|
||||
weight
|
||||
);
|
||||
|
||||
impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
|
||||
fn fill_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
) {
|
||||
let descs = &self.constraint_descs;
|
||||
|
||||
crate::concurrent_loop! {
|
||||
let batch_size = thread.batch_size;
|
||||
for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
|
||||
match &desc.1 {
|
||||
VelocityConstraintDesc::NongroundNongrouped(manifold_id) => {
|
||||
let manifold = &*manifolds_all[*manifold_id];
|
||||
VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
|
||||
}
|
||||
VelocityConstraintDesc::GroundNongrouped(manifold_id) => {
|
||||
let manifold = &*manifolds_all[*manifold_id];
|
||||
VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
VelocityConstraintDesc::NongroundGrouped(manifold_id) => {
|
||||
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
|
||||
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
VelocityConstraintDesc::GroundGrouped(manifold_id) => {
|
||||
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
|
||||
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
|
||||
fn fill_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
joints_all: &[JointGraphEdge],
|
||||
) {
|
||||
let descs = &self.constraint_descs;
|
||||
|
||||
crate::concurrent_loop! {
|
||||
let batch_size = thread.batch_size;
|
||||
for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
|
||||
match &desc.1 {
|
||||
VelocityConstraintDesc::NongroundNongrouped(joint_id) => {
|
||||
let joint = &joints_all[*joint_id].weight;
|
||||
let constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
|
||||
self.constraints[joint.constraint_index] = constraint;
|
||||
}
|
||||
VelocityConstraintDesc::GroundNongrouped(joint_id) => {
|
||||
let joint = &joints_all[*joint_id].weight;
|
||||
let constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
|
||||
self.constraints[joint.constraint_index] = constraint;
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
VelocityConstraintDesc::NongroundGrouped(joint_id) => {
|
||||
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
|
||||
let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
|
||||
self.constraints[joints[0].constraint_index] = constraint;
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
VelocityConstraintDesc::GroundGrouped(joint_id) => {
|
||||
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
|
||||
let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
|
||||
self.constraints[joints[0].constraint_index] = constraint;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct ParallelVelocitySolver {
|
||||
part: ParallelVelocitySolverPart<AnyVelocityConstraint>,
|
||||
joint_part: ParallelVelocitySolverPart<AnyJointVelocityConstraint>,
|
||||
}
|
||||
|
||||
impl ParallelVelocitySolver {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
part: ParallelVelocitySolverPart::new(),
|
||||
joint_part: ParallelVelocitySolverPart::new(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn init_constraint_groups(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &mut [&mut ContactManifold],
|
||||
manifold_groups: &ParallelInteractionGroups,
|
||||
joints: &mut [JointGraphEdge],
|
||||
joint_groups: &ParallelInteractionGroups,
|
||||
) {
|
||||
self.part
|
||||
.init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
|
||||
self.joint_part
|
||||
.init_constraints_groups(island_id, bodies, joints, joint_groups);
|
||||
}
|
||||
|
||||
pub fn fill_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &[&mut ContactManifold],
|
||||
joints: &[JointGraphEdge],
|
||||
) {
|
||||
self.part
|
||||
.fill_constraints(thread, params, bodies, manifolds);
|
||||
self.joint_part
|
||||
.fill_constraints(thread, params, bodies, joints);
|
||||
ThreadContext::lock_until_ge(
|
||||
&thread.num_initialized_constraints,
|
||||
self.part.constraint_descs.len(),
|
||||
);
|
||||
ThreadContext::lock_until_ge(
|
||||
&thread.num_initialized_joint_constraints,
|
||||
self.joint_part.constraint_descs.len(),
|
||||
);
|
||||
}
|
||||
|
||||
pub fn solve_constraints(
|
||||
&mut self,
|
||||
thread: &ThreadContext,
|
||||
params: &IntegrationParameters,
|
||||
manifolds_all: &mut [&mut ContactManifold],
|
||||
joints_all: &mut [JointGraphEdge],
|
||||
mj_lambdas: &mut [DeltaVel<f32>],
|
||||
) {
|
||||
if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Warmstart constraints.
|
||||
*/
|
||||
{
|
||||
// Each thread will concurrently grab thread.batch_size constraint desc to
|
||||
// solve. If the batch size is large enough for to cross the boundary of
|
||||
// a parallel_desc_group, we have to wait util the current group is finished
|
||||
// before starting the next one.
|
||||
let mut target_num_desc = 0;
|
||||
let mut start_index = thread
|
||||
.warmstart_contact_index
|
||||
.fetch_add(thread.batch_size, Ordering::SeqCst);
|
||||
let mut batch_size = thread.batch_size;
|
||||
let mut shift = 0;
|
||||
|
||||
macro_rules! warmstart(
|
||||
($part: expr) => {
|
||||
for group in $part.parallel_desc_groups.windows(2) {
|
||||
let num_descs_in_group = group[1] - group[0];
|
||||
target_num_desc += num_descs_in_group;
|
||||
|
||||
while start_index < group[1] {
|
||||
let end_index = (start_index + batch_size).min(group[1]);
|
||||
|
||||
let constraints = if end_index == $part.constraint_descs.len() {
|
||||
&mut $part.constraints[$part.constraint_descs[start_index].0..]
|
||||
} else {
|
||||
&mut $part.constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
|
||||
};
|
||||
|
||||
for constraint in constraints {
|
||||
constraint.warmstart(mj_lambdas);
|
||||
}
|
||||
|
||||
let num_solved = end_index - start_index;
|
||||
batch_size -= num_solved;
|
||||
|
||||
thread
|
||||
.num_warmstarted_contacts
|
||||
.fetch_add(num_solved, Ordering::SeqCst);
|
||||
|
||||
if batch_size == 0 {
|
||||
start_index = thread
|
||||
.warmstart_contact_index
|
||||
.fetch_add(thread.batch_size, Ordering::SeqCst);
|
||||
start_index -= shift;
|
||||
batch_size = thread.batch_size;
|
||||
} else {
|
||||
start_index += num_solved;
|
||||
}
|
||||
}
|
||||
|
||||
ThreadContext::lock_until_ge(&thread.num_warmstarted_contacts, target_num_desc);
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
warmstart!(self.joint_part);
|
||||
shift = self.joint_part.constraint_descs.len();
|
||||
start_index -= shift;
|
||||
warmstart!(self.part);
|
||||
}
|
||||
|
||||
/*
|
||||
* Solve constraints.
|
||||
*/
|
||||
{
|
||||
// Each thread will concurrently grab thread.batch_size constraint desc to
|
||||
// solve. If the batch size is large enough for to cross the boundary of
|
||||
// a parallel_desc_group, we have to wait util the current group is finished
|
||||
// before starting the next one.
|
||||
let mut start_index = thread
|
||||
.solve_interaction_index
|
||||
.fetch_add(thread.batch_size, Ordering::SeqCst);
|
||||
let mut batch_size = thread.batch_size;
|
||||
let contact_descs = &self.part.constraint_descs[..];
|
||||
let joint_descs = &self.joint_part.constraint_descs[..];
|
||||
let mut target_num_desc = 0;
|
||||
let mut shift = 0;
|
||||
|
||||
for _ in 0..params.max_velocity_iterations {
|
||||
macro_rules! solve {
|
||||
($part: expr) => {
|
||||
// Joint groups.
|
||||
for group in $part.parallel_desc_groups.windows(2) {
|
||||
let num_descs_in_group = group[1] - group[0];
|
||||
|
||||
target_num_desc += num_descs_in_group;
|
||||
|
||||
while start_index < group[1] {
|
||||
let end_index = (start_index + batch_size).min(group[1]);
|
||||
|
||||
let constraints = if end_index == $part.constraint_descs.len() {
|
||||
&mut $part.constraints[$part.constraint_descs[start_index].0..]
|
||||
} else {
|
||||
&mut $part.constraints[$part.constraint_descs[start_index].0
|
||||
..$part.constraint_descs[end_index].0]
|
||||
};
|
||||
|
||||
// println!(
|
||||
// "Solving a constraint {:?}.",
|
||||
// rayon::current_thread_index()
|
||||
// );
|
||||
for constraint in constraints {
|
||||
constraint.solve(mj_lambdas);
|
||||
}
|
||||
|
||||
let num_solved = end_index - start_index;
|
||||
batch_size -= num_solved;
|
||||
|
||||
thread
|
||||
.num_solved_interactions
|
||||
.fetch_add(num_solved, Ordering::SeqCst);
|
||||
|
||||
if batch_size == 0 {
|
||||
start_index = thread
|
||||
.solve_interaction_index
|
||||
.fetch_add(thread.batch_size, Ordering::SeqCst);
|
||||
start_index -= shift;
|
||||
batch_size = thread.batch_size;
|
||||
} else {
|
||||
start_index += num_solved;
|
||||
}
|
||||
}
|
||||
ThreadContext::lock_until_ge(
|
||||
&thread.num_solved_interactions,
|
||||
target_num_desc,
|
||||
);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
solve!(self.joint_part);
|
||||
shift += joint_descs.len();
|
||||
start_index -= joint_descs.len();
|
||||
solve!(self.part);
|
||||
shift += contact_descs.len();
|
||||
start_index -= contact_descs.len();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Writeback impulses.
|
||||
*/
|
||||
let joint_constraints = &self.joint_part.constraints;
|
||||
let contact_constraints = &self.part.constraints;
|
||||
crate::concurrent_loop! {
|
||||
let batch_size = thread.batch_size;
|
||||
for constraint in joint_constraints[thread.joint_writeback_index] {
|
||||
constraint.writeback_impulses(joints_all);
|
||||
}
|
||||
}
|
||||
crate::concurrent_loop! {
|
||||
let batch_size = thread.batch_size;
|
||||
for constraint in contact_constraints[thread.impulse_writeback_index] {
|
||||
constraint.writeback_impulses(manifolds_all);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
246
src/dynamics/solver/position_constraint.rs
Normal file
246
src/dynamics/solver/position_constraint.rs
Normal file
@@ -0,0 +1,246 @@
|
||||
use crate::dynamics::solver::PositionGroundConstraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, KinematicsCategory};
|
||||
use crate::math::{
|
||||
AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WDot};
|
||||
|
||||
pub(crate) enum AnyPositionConstraint {
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroupedPointPointGround(WPositionGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroupedPlanePointGround(WPositionGroundConstraint),
|
||||
NongroupedPointPointGround(PositionGroundConstraint),
|
||||
NongroupedPlanePointGround(PositionGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroupedPointPoint(WPositionConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroupedPlanePoint(WPositionConstraint),
|
||||
NongroupedPointPoint(PositionConstraint),
|
||||
NongroupedPlanePoint(PositionConstraint),
|
||||
#[allow(dead_code)] // The Empty variant is only used with parallel code.
|
||||
Empty,
|
||||
}
|
||||
|
||||
impl AnyPositionConstraint {
|
||||
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
|
||||
match self {
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyPositionConstraint::GroupedPointPointGround(c) => {
|
||||
c.solve_point_point(params, positions)
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyPositionConstraint::GroupedPlanePointGround(c) => {
|
||||
c.solve_plane_point(params, positions)
|
||||
}
|
||||
AnyPositionConstraint::NongroupedPointPointGround(c) => {
|
||||
c.solve_point_point(params, positions)
|
||||
}
|
||||
AnyPositionConstraint::NongroupedPlanePointGround(c) => {
|
||||
c.solve_plane_point(params, positions)
|
||||
}
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyPositionConstraint::GroupedPointPoint(c) => c.solve_point_point(params, positions),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyPositionConstraint::GroupedPlanePoint(c) => c.solve_plane_point(params, positions),
|
||||
AnyPositionConstraint::NongroupedPointPoint(c) => {
|
||||
c.solve_point_point(params, positions)
|
||||
}
|
||||
AnyPositionConstraint::NongroupedPlanePoint(c) => {
|
||||
c.solve_plane_point(params, positions)
|
||||
}
|
||||
AnyPositionConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct PositionConstraint {
|
||||
pub rb1: usize,
|
||||
pub rb2: usize,
|
||||
// NOTE: the points are relative to the center of masses.
|
||||
pub local_p1: [Point<f32>; MAX_MANIFOLD_POINTS],
|
||||
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
|
||||
pub local_n1: Vector<f32>,
|
||||
pub num_contacts: u8,
|
||||
pub radius: f32,
|
||||
pub im1: f32,
|
||||
pub im2: f32,
|
||||
pub ii1: AngularInertia<f32>,
|
||||
pub ii2: AngularInertia<f32>,
|
||||
pub erp: f32,
|
||||
pub max_linear_correction: f32,
|
||||
}
|
||||
|
||||
impl PositionConstraint {
|
||||
#[cfg(feature = "parallel")]
|
||||
pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
|
||||
let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
|
||||
manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
|
||||
}
|
||||
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifold: &ContactManifold,
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyPositionConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let rb1 = &bodies[manifold.body_pair.body1];
|
||||
let rb2 = &bodies[manifold.body_pair.body2];
|
||||
let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
|
||||
let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
|
||||
let radius =
|
||||
manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
|
||||
|
||||
for (l, manifold_points) in manifold
|
||||
.active_contacts()
|
||||
.chunks(MAX_MANIFOLD_POINTS)
|
||||
.enumerate()
|
||||
{
|
||||
let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
|
||||
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
|
||||
|
||||
for l in 0..manifold_points.len() {
|
||||
local_p1[l] = manifold_points[l].local_p1 + shift1;
|
||||
local_p2[l] = manifold_points[l].local_p2 + shift2;
|
||||
}
|
||||
|
||||
let constraint = PositionConstraint {
|
||||
rb1: rb1.active_set_offset,
|
||||
rb2: rb2.active_set_offset,
|
||||
local_p1,
|
||||
local_p2,
|
||||
local_n1: manifold.local_n1,
|
||||
radius,
|
||||
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(),
|
||||
num_contacts: manifold_points.len() as u8,
|
||||
erp: params.erp,
|
||||
max_linear_correction: params.max_linear_correction,
|
||||
};
|
||||
|
||||
if push {
|
||||
if manifold.kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints.push(AnyPositionConstraint::NongroupedPointPoint(constraint));
|
||||
} else {
|
||||
out_constraints.push(AnyPositionConstraint::NongroupedPlanePoint(constraint));
|
||||
}
|
||||
} else {
|
||||
if manifold.kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints[manifold.constraint_index + l] =
|
||||
AnyPositionConstraint::NongroupedPointPoint(constraint);
|
||||
} else {
|
||||
out_constraints[manifold.constraint_index + l] =
|
||||
AnyPositionConstraint::NongroupedPlanePoint(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_point_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos1 = positions[self.rb1];
|
||||
let mut pos2 = positions[self.rb2];
|
||||
let allowed_err = params.allowed_linear_error;
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let p1 = pos1 * self.local_p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
let dpos = p2 - p1;
|
||||
|
||||
let sqdist = dpos.norm_squared();
|
||||
|
||||
// NOTE: only works for the point-point case.
|
||||
if sqdist < target_dist * target_dist {
|
||||
let dist = sqdist.sqrt();
|
||||
let n = dpos / dist;
|
||||
let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
|
||||
let dp1 = p1.coords - pos1.translation.vector;
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross1 = dp1.gcross(n);
|
||||
let gcross2 = -dp2.gcross(n);
|
||||
let ii_gcross1 = self.ii1.transform_vector(gcross1);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r =
|
||||
self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
let tra1 = Translation::from(n * (impulse * self.im1));
|
||||
let tra2 = Translation::from(n * (-impulse * self.im2));
|
||||
let rot1 = Rotation::new(ii_gcross1 * impulse);
|
||||
let rot2 = Rotation::new(ii_gcross2 * impulse);
|
||||
|
||||
pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation);
|
||||
pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.rb1] = pos1;
|
||||
positions[self.rb2] = pos2;
|
||||
}
|
||||
|
||||
pub fn solve_plane_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos1 = positions[self.rb1];
|
||||
let mut pos2 = positions[self.rb2];
|
||||
let allowed_err = params.allowed_linear_error;
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let n1 = pos1 * self.local_n1;
|
||||
let p1 = pos1 * self.local_p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
let dpos = p2 - p1;
|
||||
let dist = dpos.dot(&n1);
|
||||
|
||||
if dist < target_dist {
|
||||
let p1 = p2 - n1 * dist;
|
||||
let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
|
||||
let dp1 = p1.coords - pos1.translation.vector;
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross1 = dp1.gcross(n1);
|
||||
let gcross2 = -dp2.gcross(n1);
|
||||
let ii_gcross1 = self.ii1.transform_vector(gcross1);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r =
|
||||
self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
let tra1 = Translation::from(n1 * (impulse * self.im1));
|
||||
let tra2 = Translation::from(n1 * (-impulse * self.im2));
|
||||
let rot1 = Rotation::new(ii_gcross1 * impulse);
|
||||
let rot2 = Rotation::new(ii_gcross2 * impulse);
|
||||
|
||||
pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation);
|
||||
pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.rb1] = pos1;
|
||||
positions[self.rb2] = pos2;
|
||||
}
|
||||
}
|
||||
217
src/dynamics/solver/position_constraint_wide.rs
Normal file
217
src/dynamics/solver/position_constraint_wide.rs
Normal file
@@ -0,0 +1,217 @@
|
||||
use super::AnyPositionConstraint;
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, KinematicsCategory};
|
||||
use crate::math::{
|
||||
AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
|
||||
SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WDot};
|
||||
|
||||
use num::Zero;
|
||||
use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
|
||||
|
||||
pub(crate) struct WPositionConstraint {
|
||||
pub rb1: [usize; SIMD_WIDTH],
|
||||
pub rb2: [usize; SIMD_WIDTH],
|
||||
// NOTE: the points are relative to the center of masses.
|
||||
pub local_p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
|
||||
pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
|
||||
pub local_n1: Vector<SimdFloat>,
|
||||
pub radius: SimdFloat,
|
||||
pub im1: SimdFloat,
|
||||
pub im2: SimdFloat,
|
||||
pub ii1: AngularInertia<SimdFloat>,
|
||||
pub ii2: AngularInertia<SimdFloat>,
|
||||
pub erp: SimdFloat,
|
||||
pub max_linear_correction: SimdFloat,
|
||||
pub num_contacts: u8,
|
||||
}
|
||||
|
||||
impl WPositionConstraint {
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifolds: [&ContactManifold; SIMD_WIDTH],
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyPositionConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
|
||||
let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
|
||||
|
||||
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
|
||||
let sqrt_ii1: AngularInertia<SimdFloat> =
|
||||
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
|
||||
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
|
||||
let sqrt_ii2: AngularInertia<SimdFloat> =
|
||||
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
|
||||
|
||||
let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
|
||||
let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
|
||||
|
||||
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
|
||||
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
|
||||
|
||||
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
|
||||
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
|
||||
|
||||
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
|
||||
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
|
||||
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
|
||||
|
||||
let mut constraint = WPositionConstraint {
|
||||
rb1,
|
||||
rb2,
|
||||
local_p1: [Point::origin(); MAX_MANIFOLD_POINTS],
|
||||
local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
|
||||
local_n1,
|
||||
radius,
|
||||
im1,
|
||||
im2,
|
||||
ii1: sqrt_ii1.squared(),
|
||||
ii2: sqrt_ii2.squared(),
|
||||
erp: SimdFloat::splat(params.erp),
|
||||
max_linear_correction: SimdFloat::splat(params.max_linear_correction),
|
||||
num_contacts: num_points as u8,
|
||||
};
|
||||
|
||||
let shift1 = local_n1 * -radius1;
|
||||
let shift2 = local_n2 * -radius2;
|
||||
|
||||
for i in 0..num_points {
|
||||
let local_p1 =
|
||||
Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]);
|
||||
let local_p2 =
|
||||
Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]);
|
||||
|
||||
constraint.local_p1[i] = local_p1 + shift1;
|
||||
constraint.local_p2[i] = local_p2 + shift2;
|
||||
}
|
||||
|
||||
if push {
|
||||
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints.push(AnyPositionConstraint::GroupedPointPoint(constraint));
|
||||
} else {
|
||||
out_constraints.push(AnyPositionConstraint::GroupedPlanePoint(constraint));
|
||||
}
|
||||
} else {
|
||||
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
|
||||
AnyPositionConstraint::GroupedPointPoint(constraint);
|
||||
} else {
|
||||
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
|
||||
AnyPositionConstraint::GroupedPlanePoint(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_point_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
|
||||
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
|
||||
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let p1 = pos1 * self.local_p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
|
||||
let dpos = p2 - p1;
|
||||
let sqdist = dpos.norm_squared();
|
||||
|
||||
if sqdist.simd_lt(target_dist * target_dist).any() {
|
||||
let dist = sqdist.simd_sqrt();
|
||||
let n = dpos / dist;
|
||||
let err = ((dist - target_dist) * self.erp)
|
||||
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
|
||||
let dp1 = p1.coords - pos1.translation.vector;
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross1 = dp1.gcross(n);
|
||||
let gcross2 = -dp2.gcross(n);
|
||||
let ii_gcross1 = self.ii1.transform_vector(gcross1);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r =
|
||||
self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
pos1.translation = Translation::from(n * (impulse * self.im1)) * pos1.translation;
|
||||
pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
|
||||
pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation;
|
||||
pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
|
||||
}
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.rb1[ii]] = pos1.extract(ii);
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.rb2[ii]] = pos2.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_plane_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
|
||||
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
|
||||
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let n1 = pos1 * self.local_n1;
|
||||
let p1 = pos1 * self.local_p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
let dpos = p2 - p1;
|
||||
let dist = dpos.dot(&n1);
|
||||
|
||||
// NOTE: this condition does not seem to be useful perfomancewise?
|
||||
if dist.simd_lt(target_dist).any() {
|
||||
// NOTE: only works for the point-point case.
|
||||
let p1 = p2 - n1 * dist;
|
||||
let err = ((dist - target_dist) * self.erp)
|
||||
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
|
||||
let dp1 = p1.coords - pos1.translation.vector;
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross1 = dp1.gcross(n1);
|
||||
let gcross2 = -dp2.gcross(n1);
|
||||
let ii_gcross1 = self.ii1.transform_vector(gcross1);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r =
|
||||
self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
pos1.translation = Translation::from(n1 * (impulse * self.im1)) * pos1.translation;
|
||||
pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
|
||||
pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
|
||||
pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
|
||||
}
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.rb1[ii]] = pos1.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.rb2[ii]] = pos2.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
189
src/dynamics/solver/position_ground_constraint.rs
Normal file
189
src/dynamics/solver/position_ground_constraint.rs
Normal file
@@ -0,0 +1,189 @@
|
||||
use super::AnyPositionConstraint;
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, KinematicsCategory};
|
||||
use crate::math::{
|
||||
AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WDot};
|
||||
|
||||
pub(crate) struct PositionGroundConstraint {
|
||||
pub rb2: usize,
|
||||
// NOTE: the points are relative to the center of masses.
|
||||
pub p1: [Point<f32>; MAX_MANIFOLD_POINTS],
|
||||
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
|
||||
pub n1: Vector<f32>,
|
||||
pub num_contacts: u8,
|
||||
pub radius: f32,
|
||||
pub im2: f32,
|
||||
pub ii2: AngularInertia<f32>,
|
||||
pub erp: f32,
|
||||
pub max_linear_correction: f32,
|
||||
}
|
||||
|
||||
impl PositionGroundConstraint {
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifold: &ContactManifold,
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyPositionConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let mut rb1 = &bodies[manifold.body_pair.body1];
|
||||
let mut rb2 = &bodies[manifold.body_pair.body2];
|
||||
let flip = !rb2.is_dynamic();
|
||||
|
||||
let local_n1;
|
||||
let local_n2;
|
||||
|
||||
if flip {
|
||||
std::mem::swap(&mut rb1, &mut rb2);
|
||||
local_n1 = manifold.local_n2;
|
||||
local_n2 = manifold.local_n1;
|
||||
} else {
|
||||
local_n1 = manifold.local_n1;
|
||||
local_n2 = manifold.local_n2;
|
||||
};
|
||||
|
||||
let shift1 = local_n1 * -manifold.kinematics.radius1;
|
||||
let shift2 = local_n2 * -manifold.kinematics.radius2;
|
||||
let radius =
|
||||
manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */;
|
||||
|
||||
for (l, manifold_points) in manifold
|
||||
.active_contacts()
|
||||
.chunks(MAX_MANIFOLD_POINTS)
|
||||
.enumerate()
|
||||
{
|
||||
let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
|
||||
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
|
||||
|
||||
if flip {
|
||||
// Don't forget that we already swapped rb1 and rb2 above.
|
||||
// So if we flip, only manifold_points[k].{local_p1,local_p2} have to
|
||||
// be swapped.
|
||||
for k in 0..manifold_points.len() {
|
||||
p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1);
|
||||
local_p2[k] = manifold_points[k].local_p1 + shift2;
|
||||
}
|
||||
} else {
|
||||
for k in 0..manifold_points.len() {
|
||||
p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1);
|
||||
local_p2[k] = manifold_points[k].local_p2 + shift2;
|
||||
}
|
||||
}
|
||||
|
||||
let constraint = PositionGroundConstraint {
|
||||
rb2: rb2.active_set_offset,
|
||||
p1,
|
||||
local_p2,
|
||||
n1: rb1.predicted_position * local_n1,
|
||||
radius,
|
||||
im2: rb2.mass_properties.inv_mass,
|
||||
ii2: rb2.world_inv_inertia_sqrt.squared(),
|
||||
num_contacts: manifold_points.len() as u8,
|
||||
erp: params.erp,
|
||||
max_linear_correction: params.max_linear_correction,
|
||||
};
|
||||
|
||||
if push {
|
||||
if manifold.kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints.push(AnyPositionConstraint::NongroupedPointPointGround(
|
||||
constraint,
|
||||
));
|
||||
} else {
|
||||
out_constraints.push(AnyPositionConstraint::NongroupedPlanePointGround(
|
||||
constraint,
|
||||
));
|
||||
}
|
||||
} else {
|
||||
if manifold.kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints[manifold.constraint_index + l] =
|
||||
AnyPositionConstraint::NongroupedPointPointGround(constraint);
|
||||
} else {
|
||||
out_constraints[manifold.constraint_index + l] =
|
||||
AnyPositionConstraint::NongroupedPlanePointGround(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
pub fn solve_point_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos2 = positions[self.rb2];
|
||||
let allowed_err = params.allowed_linear_error;
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let p1 = self.p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
let dpos = p2 - p1;
|
||||
|
||||
let sqdist = dpos.norm_squared();
|
||||
|
||||
// NOTE: only works for the point-point case.
|
||||
if sqdist < target_dist * target_dist {
|
||||
let dist = sqdist.sqrt();
|
||||
let n = dpos / dist;
|
||||
let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross2 = -dp2.gcross(n);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
let tra2 = Translation::from(n * (-impulse * self.im2));
|
||||
let rot2 = Rotation::new(ii_gcross2 * impulse);
|
||||
pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.rb2] = pos2;
|
||||
}
|
||||
|
||||
pub fn solve_plane_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos2 = positions[self.rb2];
|
||||
let allowed_err = params.allowed_linear_error;
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let n1 = self.n1;
|
||||
let p1 = self.p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
let dpos = p2 - p1;
|
||||
let dist = dpos.dot(&n1);
|
||||
|
||||
if dist < target_dist {
|
||||
let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross2 = -dp2.gcross(n1);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
let tra2 = Translation::from(n1 * (-impulse * self.im2));
|
||||
let rot2 = Rotation::new(ii_gcross2 * impulse);
|
||||
pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
|
||||
}
|
||||
}
|
||||
|
||||
positions[self.rb2] = pos2;
|
||||
}
|
||||
}
|
||||
199
src/dynamics/solver/position_ground_constraint_wide.rs
Normal file
199
src/dynamics/solver/position_ground_constraint_wide.rs
Normal file
@@ -0,0 +1,199 @@
|
||||
use super::AnyPositionConstraint;
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, KinematicsCategory};
|
||||
use crate::math::{
|
||||
AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
|
||||
SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WCross, WDot};
|
||||
|
||||
use num::Zero;
|
||||
use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
|
||||
|
||||
pub(crate) struct WPositionGroundConstraint {
|
||||
pub rb2: [usize; SIMD_WIDTH],
|
||||
// NOTE: the points are relative to the center of masses.
|
||||
pub p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
|
||||
pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
|
||||
pub n1: Vector<SimdFloat>,
|
||||
pub radius: SimdFloat,
|
||||
pub im2: SimdFloat,
|
||||
pub ii2: AngularInertia<SimdFloat>,
|
||||
pub erp: SimdFloat,
|
||||
pub max_linear_correction: SimdFloat,
|
||||
pub num_contacts: u8,
|
||||
}
|
||||
|
||||
impl WPositionGroundConstraint {
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifolds: [&ContactManifold; SIMD_WIDTH],
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyPositionConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
|
||||
let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
|
||||
let mut flipped = [false; SIMD_WIDTH];
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
if !rbs2[ii].is_dynamic() {
|
||||
flipped[ii] = true;
|
||||
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
|
||||
let sqrt_ii2: AngularInertia<SimdFloat> =
|
||||
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
|
||||
|
||||
let local_n1 = Vector::from(
|
||||
array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_n2 = Vector::from(
|
||||
array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
|
||||
);
|
||||
|
||||
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
|
||||
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
|
||||
|
||||
let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
|
||||
|
||||
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
|
||||
|
||||
let n1 = position1 * local_n1;
|
||||
|
||||
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
|
||||
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
|
||||
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
|
||||
|
||||
let mut constraint = WPositionGroundConstraint {
|
||||
rb2,
|
||||
p1: [Point::origin(); MAX_MANIFOLD_POINTS],
|
||||
local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
|
||||
n1,
|
||||
radius,
|
||||
im2,
|
||||
ii2: sqrt_ii2.squared(),
|
||||
erp: SimdFloat::splat(params.erp),
|
||||
max_linear_correction: SimdFloat::splat(params.max_linear_correction),
|
||||
num_contacts: num_points as u8,
|
||||
};
|
||||
|
||||
for i in 0..num_points {
|
||||
let local_p1 = Point::from(
|
||||
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH],
|
||||
);
|
||||
let local_p2 = Point::from(
|
||||
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH],
|
||||
);
|
||||
|
||||
constraint.p1[i] = position1 * local_p1 - n1 * radius1;
|
||||
constraint.local_p2[i] = local_p2 - local_n2 * radius2;
|
||||
}
|
||||
|
||||
if push {
|
||||
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints
|
||||
.push(AnyPositionConstraint::GroupedPointPointGround(constraint));
|
||||
} else {
|
||||
out_constraints
|
||||
.push(AnyPositionConstraint::GroupedPlanePointGround(constraint));
|
||||
}
|
||||
} else {
|
||||
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
|
||||
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
|
||||
AnyPositionConstraint::GroupedPointPointGround(constraint);
|
||||
} else {
|
||||
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
|
||||
AnyPositionConstraint::GroupedPlanePointGround(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
pub fn solve_point_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
|
||||
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let p1 = self.p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
|
||||
let dpos = p2 - p1;
|
||||
let sqdist = dpos.norm_squared();
|
||||
|
||||
if sqdist.simd_lt(target_dist * target_dist).any() {
|
||||
let dist = sqdist.simd_sqrt();
|
||||
let n = dpos / dist;
|
||||
let err = ((dist - target_dist) * self.erp)
|
||||
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
let gcross2 = -dp2.gcross(n);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation;
|
||||
pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
|
||||
}
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.rb2[ii]] = pos2.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve_plane_point(
|
||||
&self,
|
||||
params: &IntegrationParameters,
|
||||
positions: &mut [Isometry<f32>],
|
||||
) {
|
||||
// FIXME: can we avoid most of the multiplications by pos1/pos2?
|
||||
// Compute jacobians.
|
||||
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
|
||||
let allowed_err = SimdFloat::splat(params.allowed_linear_error);
|
||||
let target_dist = self.radius - allowed_err;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let n1 = self.n1;
|
||||
let p1 = self.p1[k];
|
||||
let p2 = pos2 * self.local_p2[k];
|
||||
let dpos = p2 - p1;
|
||||
let dist = dpos.dot(&n1);
|
||||
|
||||
// NOTE: this condition does not seem to be useful perfomancewise?
|
||||
if dist.simd_lt(target_dist).any() {
|
||||
let err = ((dist - target_dist) * self.erp)
|
||||
.simd_clamp(-self.max_linear_correction, SimdFloat::zero());
|
||||
let dp2 = p2.coords - pos2.translation.vector;
|
||||
|
||||
let gcross2 = -dp2.gcross(n1);
|
||||
let ii_gcross2 = self.ii2.transform_vector(gcross2);
|
||||
|
||||
// Compute impulse.
|
||||
let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
|
||||
let impulse = err / inv_r;
|
||||
|
||||
// Apply impulse.
|
||||
pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
|
||||
pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
|
||||
}
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
positions[self.rb2[ii]] = pos2.extract(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
451
src/dynamics/solver/position_solver.rs
Normal file
451
src/dynamics/solver/position_solver.rs
Normal file
@@ -0,0 +1,451 @@
|
||||
use super::{
|
||||
AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint,
|
||||
};
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use super::{WPositionConstraint, WPositionGroundConstraint};
|
||||
use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
|
||||
use crate::dynamics::{
|
||||
solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
|
||||
};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
use crate::math::Isometry;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::math::SIMD_WIDTH;
|
||||
|
||||
pub(crate) struct PositionSolverJointPart {
|
||||
pub nonground_joints: Vec<JointIndex>,
|
||||
pub ground_joints: Vec<JointIndex>,
|
||||
pub nonground_joint_groups: InteractionGroups,
|
||||
pub ground_joint_groups: InteractionGroups,
|
||||
pub constraints: Vec<AnyJointPositionConstraint>,
|
||||
}
|
||||
|
||||
impl PositionSolverJointPart {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
nonground_joints: Vec::new(),
|
||||
ground_joints: Vec::new(),
|
||||
nonground_joint_groups: InteractionGroups::new(),
|
||||
ground_joint_groups: InteractionGroups::new(),
|
||||
constraints: Vec::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct PositionSolverPart {
|
||||
pub point_point_manifolds: Vec<ContactManifoldIndex>,
|
||||
pub plane_point_manifolds: Vec<ContactManifoldIndex>,
|
||||
pub point_point_ground_manifolds: Vec<ContactManifoldIndex>,
|
||||
pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>,
|
||||
pub point_point_groups: InteractionGroups,
|
||||
pub plane_point_groups: InteractionGroups,
|
||||
pub point_point_ground_groups: InteractionGroups,
|
||||
pub plane_point_ground_groups: InteractionGroups,
|
||||
pub constraints: Vec<AnyPositionConstraint>,
|
||||
}
|
||||
|
||||
impl PositionSolverPart {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
point_point_manifolds: Vec::new(),
|
||||
plane_point_manifolds: Vec::new(),
|
||||
point_point_ground_manifolds: Vec::new(),
|
||||
plane_point_ground_manifolds: Vec::new(),
|
||||
point_point_groups: InteractionGroups::new(),
|
||||
plane_point_groups: InteractionGroups::new(),
|
||||
point_point_ground_groups: InteractionGroups::new(),
|
||||
plane_point_ground_groups: InteractionGroups::new(),
|
||||
constraints: Vec::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct PositionSolver {
|
||||
positions: Vec<Isometry<f32>>,
|
||||
part: PositionSolverPart,
|
||||
joint_part: PositionSolverJointPart,
|
||||
}
|
||||
|
||||
impl PositionSolver {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
positions: Vec::new(),
|
||||
part: PositionSolverPart::new(),
|
||||
joint_part: PositionSolverJointPart::new(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn init_constraints(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
joints: &[JointGraphEdge],
|
||||
joint_constraint_indices: &[JointIndex],
|
||||
) {
|
||||
self.part
|
||||
.init_constraints(island_id, params, bodies, manifolds, manifold_indices);
|
||||
self.joint_part.init_constraints(
|
||||
island_id,
|
||||
params,
|
||||
bodies,
|
||||
joints,
|
||||
joint_constraint_indices,
|
||||
);
|
||||
}
|
||||
|
||||
pub fn solve_constraints(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &mut RigidBodySet,
|
||||
) {
|
||||
self.positions.clear();
|
||||
self.positions.extend(
|
||||
bodies
|
||||
.iter_active_island(island_id)
|
||||
.map(|(_, b)| b.position),
|
||||
);
|
||||
|
||||
for _ in 0..params.max_position_iterations {
|
||||
for constraint in &self.joint_part.constraints {
|
||||
constraint.solve(params, &mut self.positions)
|
||||
}
|
||||
|
||||
for constraint in &self.part.constraints {
|
||||
constraint.solve(params, &mut self.positions)
|
||||
}
|
||||
}
|
||||
|
||||
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
|
||||
rb.set_position(self.positions[rb.active_set_offset])
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionSolverPart {
|
||||
pub fn init_constraints(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
) {
|
||||
self.point_point_ground_manifolds.clear();
|
||||
self.plane_point_ground_manifolds.clear();
|
||||
self.point_point_manifolds.clear();
|
||||
self.plane_point_manifolds.clear();
|
||||
categorize_position_contacts(
|
||||
bodies,
|
||||
manifolds_all,
|
||||
manifold_indices,
|
||||
&mut self.point_point_ground_manifolds,
|
||||
&mut self.plane_point_ground_manifolds,
|
||||
&mut self.point_point_manifolds,
|
||||
&mut self.plane_point_manifolds,
|
||||
);
|
||||
|
||||
self.point_point_groups.clear_groups();
|
||||
self.point_point_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.point_point_manifolds,
|
||||
);
|
||||
self.plane_point_groups.clear_groups();
|
||||
self.plane_point_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.plane_point_manifolds,
|
||||
);
|
||||
|
||||
self.point_point_ground_groups.clear_groups();
|
||||
self.point_point_ground_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.point_point_ground_manifolds,
|
||||
);
|
||||
self.plane_point_ground_groups.clear_groups();
|
||||
self.plane_point_ground_groups.group_manifolds(
|
||||
island_id,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.plane_point_ground_manifolds,
|
||||
);
|
||||
|
||||
self.constraints.clear();
|
||||
|
||||
/*
|
||||
* Init non-ground contact constraints.
|
||||
*/
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
{
|
||||
compute_grouped_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.point_point_groups.grouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
compute_grouped_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.plane_point_groups.grouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
}
|
||||
compute_nongrouped_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.point_point_groups.nongrouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
compute_nongrouped_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.plane_point_groups.nongrouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
|
||||
/*
|
||||
* Init ground contact constraints.
|
||||
*/
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
{
|
||||
compute_grouped_ground_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.point_point_ground_groups.grouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
compute_grouped_ground_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.plane_point_ground_groups.grouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
}
|
||||
compute_nongrouped_ground_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.point_point_ground_groups.nongrouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
compute_nongrouped_ground_constraints(
|
||||
params,
|
||||
bodies,
|
||||
manifolds_all,
|
||||
&self.plane_point_ground_groups.nongrouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionSolverJointPart {
|
||||
pub fn init_constraints(
|
||||
&mut self,
|
||||
island_id: usize,
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
joints: &[JointGraphEdge],
|
||||
joint_constraint_indices: &[JointIndex],
|
||||
) {
|
||||
self.ground_joints.clear();
|
||||
self.nonground_joints.clear();
|
||||
categorize_joints(
|
||||
bodies,
|
||||
joints,
|
||||
joint_constraint_indices,
|
||||
&mut self.ground_joints,
|
||||
&mut self.nonground_joints,
|
||||
);
|
||||
|
||||
self.nonground_joint_groups.clear_groups();
|
||||
self.nonground_joint_groups
|
||||
.group_joints(island_id, bodies, joints, &self.nonground_joints);
|
||||
|
||||
self.ground_joint_groups.clear_groups();
|
||||
self.ground_joint_groups
|
||||
.group_joints(island_id, bodies, joints, &self.ground_joints);
|
||||
|
||||
/*
|
||||
* Init ground joint constraints.
|
||||
*/
|
||||
self.constraints.clear();
|
||||
compute_nongrouped_joint_ground_constraints(
|
||||
params,
|
||||
bodies,
|
||||
joints,
|
||||
&self.ground_joint_groups.nongrouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
{
|
||||
compute_grouped_joint_ground_constraints(
|
||||
params,
|
||||
bodies,
|
||||
joints,
|
||||
&self.ground_joint_groups.grouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
* Init non-ground joint constraints.
|
||||
*/
|
||||
compute_nongrouped_joint_constraints(
|
||||
params,
|
||||
bodies,
|
||||
joints,
|
||||
&self.nonground_joint_groups.nongrouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
{
|
||||
compute_grouped_joint_constraints(
|
||||
params,
|
||||
bodies,
|
||||
joints,
|
||||
&self.nonground_joint_groups.grouped_interactions,
|
||||
&mut self.constraints,
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn compute_nongrouped_constraints(
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
output: &mut Vec<AnyPositionConstraint>,
|
||||
) {
|
||||
for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
|
||||
PositionConstraint::generate(params, manifold, bodies, output, true)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
fn compute_grouped_constraints(
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
output: &mut Vec<AnyPositionConstraint>,
|
||||
) {
|
||||
for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
|
||||
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
|
||||
WPositionConstraint::generate(params, manifolds, bodies, output, true)
|
||||
}
|
||||
}
|
||||
|
||||
fn compute_nongrouped_ground_constraints(
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
output: &mut Vec<AnyPositionConstraint>,
|
||||
) {
|
||||
for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
|
||||
PositionGroundConstraint::generate(params, manifold, bodies, output, true)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
fn compute_grouped_ground_constraints(
|
||||
params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
manifolds_all: &[&mut ContactManifold],
|
||||
manifold_indices: &[ContactManifoldIndex],
|
||||
output: &mut Vec<AnyPositionConstraint>,
|
||||
) {
|
||||
for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
|
||||
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
|
||||
WPositionGroundConstraint::generate(params, manifolds, bodies, output, true);
|
||||
}
|
||||
}
|
||||
|
||||
fn compute_nongrouped_joint_ground_constraints(
|
||||
_params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
joints_all: &[JointGraphEdge],
|
||||
joint_indices: &[JointIndex],
|
||||
output: &mut Vec<AnyJointPositionConstraint>,
|
||||
) {
|
||||
for joint_i in joint_indices {
|
||||
let joint = &joints_all[*joint_i].weight;
|
||||
let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
|
||||
output.push(pos_constraint);
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
fn compute_grouped_joint_ground_constraints(
|
||||
_params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
joints_all: &[JointGraphEdge],
|
||||
joint_indices: &[JointIndex],
|
||||
output: &mut Vec<AnyJointPositionConstraint>,
|
||||
) {
|
||||
for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
|
||||
let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
|
||||
if let Some(pos_constraint) =
|
||||
AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
|
||||
{
|
||||
output.push(pos_constraint);
|
||||
} else {
|
||||
for joint in joints.iter() {
|
||||
output.push(AnyJointPositionConstraint::from_joint_ground(
|
||||
*joint, bodies,
|
||||
))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn compute_nongrouped_joint_constraints(
|
||||
_params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
joints_all: &[JointGraphEdge],
|
||||
joint_indices: &[JointIndex],
|
||||
output: &mut Vec<AnyJointPositionConstraint>,
|
||||
) {
|
||||
for joint_i in joint_indices {
|
||||
let joint = &joints_all[*joint_i];
|
||||
let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies);
|
||||
output.push(pos_constraint);
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
fn compute_grouped_joint_constraints(
|
||||
_params: &IntegrationParameters,
|
||||
bodies: &RigidBodySet,
|
||||
joints_all: &[JointGraphEdge],
|
||||
joint_indices: &[JointIndex],
|
||||
output: &mut Vec<AnyJointPositionConstraint>,
|
||||
) {
|
||||
for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
|
||||
let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
|
||||
if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) {
|
||||
output.push(pos_constraint);
|
||||
} else {
|
||||
for joint in joints.iter() {
|
||||
output.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
401
src/dynamics/solver/velocity_constraint.rs
Normal file
401
src/dynamics/solver/velocity_constraint.rs
Normal file
@@ -0,0 +1,401 @@
|
||||
use super::DeltaVel;
|
||||
use crate::dynamics::solver::VelocityGroundConstraint;
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
|
||||
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
|
||||
use simba::simd::SimdPartialOrd;
|
||||
|
||||
//#[repr(align(64))]
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) enum AnyVelocityConstraint {
|
||||
NongroupedGround(VelocityGroundConstraint),
|
||||
Nongrouped(VelocityConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
GroupedGround(WVelocityGroundConstraint),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
Grouped(WVelocityConstraint),
|
||||
#[allow(dead_code)] // The Empty variant is only used with parallel code.
|
||||
Empty,
|
||||
}
|
||||
|
||||
impl AnyVelocityConstraint {
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> {
|
||||
if let AnyVelocityConstraint::Nongrouped(c) = self {
|
||||
Some(c)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> {
|
||||
if let AnyVelocityConstraint::NongroupedGround(c) = self {
|
||||
Some(c)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
match self {
|
||||
AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas),
|
||||
AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas),
|
||||
AnyVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
match self {
|
||||
AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas),
|
||||
AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas),
|
||||
AnyVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) {
|
||||
match self {
|
||||
AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all),
|
||||
AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
|
||||
#[cfg(feature = "simd-is-enabled")]
|
||||
AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
|
||||
AnyVelocityConstraint::Empty => unreachable!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct VelocityConstraintElementPart {
|
||||
pub gcross1: AngVector<f32>,
|
||||
pub gcross2: AngVector<f32>,
|
||||
pub rhs: f32,
|
||||
pub impulse: f32,
|
||||
pub r: f32,
|
||||
}
|
||||
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
impl VelocityConstraintElementPart {
|
||||
fn zero() -> Self {
|
||||
Self {
|
||||
gcross1: na::zero(),
|
||||
gcross2: na::zero(),
|
||||
rhs: 0.0,
|
||||
impulse: 0.0,
|
||||
r: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct VelocityConstraintElement {
|
||||
pub normal_part: VelocityConstraintElementPart,
|
||||
pub tangent_part: [VelocityConstraintElementPart; DIM - 1],
|
||||
}
|
||||
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
impl VelocityConstraintElement {
|
||||
pub fn zero() -> Self {
|
||||
Self {
|
||||
normal_part: VelocityConstraintElementPart::zero(),
|
||||
tangent_part: [VelocityConstraintElementPart::zero(); DIM - 1],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct VelocityConstraint {
|
||||
pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
|
||||
pub im1: f32,
|
||||
pub im2: f32,
|
||||
pub limit: f32,
|
||||
pub mj_lambda1: usize,
|
||||
pub mj_lambda2: usize,
|
||||
pub manifold_id: ContactManifoldIndex,
|
||||
pub manifold_contact_id: usize,
|
||||
pub num_contacts: u8,
|
||||
pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
|
||||
}
|
||||
|
||||
impl VelocityConstraint {
|
||||
#[cfg(feature = "parallel")]
|
||||
pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
|
||||
let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
|
||||
manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
|
||||
}
|
||||
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifold_id: ContactManifoldIndex,
|
||||
manifold: &ContactManifold,
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyVelocityConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let rb1 = &bodies[manifold.body_pair.body1];
|
||||
let rb2 = &bodies[manifold.body_pair.body2];
|
||||
let mj_lambda1 = rb1.active_set_offset;
|
||||
let mj_lambda2 = rb2.active_set_offset;
|
||||
let force_dir1 = rb1.position * (-manifold.local_n1);
|
||||
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
|
||||
|
||||
for (l, manifold_points) in manifold
|
||||
.active_contacts()
|
||||
.chunks(MAX_MANIFOLD_POINTS)
|
||||
.enumerate()
|
||||
{
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
let mut constraint = VelocityConstraint {
|
||||
dir1: force_dir1,
|
||||
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
|
||||
im1: rb1.mass_properties.inv_mass,
|
||||
im2: rb2.mass_properties.inv_mass,
|
||||
limit: manifold.friction,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
manifold_id,
|
||||
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
|
||||
num_contacts: manifold_points.len() as u8,
|
||||
};
|
||||
|
||||
// TODO: this is a WIP optimization for WASM platforms.
|
||||
// For some reasons, the compiler does not inline the `Vec::push` method
|
||||
// in this method. This generates two memset and one memcpy which are both very
|
||||
// expansive.
|
||||
// This would likely be solved by some kind of "placement-push" (like emplace in C++).
|
||||
// In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
|
||||
// avoid spurious copying.
|
||||
// Is this optimization beneficial when targeting non-WASM platforms?
|
||||
//
|
||||
// NOTE: joints have the same problem, but it is not easy to refactor the code that way
|
||||
// for the moment.
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
let constraint = if push {
|
||||
let new_len = out_constraints.len() + 1;
|
||||
unsafe {
|
||||
out_constraints.resize_with(new_len, || {
|
||||
AnyVelocityConstraint::Nongrouped(
|
||||
std::mem::MaybeUninit::uninit().assume_init(),
|
||||
)
|
||||
});
|
||||
}
|
||||
out_constraints
|
||||
.last_mut()
|
||||
.unwrap()
|
||||
.as_nongrouped_mut()
|
||||
.unwrap()
|
||||
} else {
|
||||
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
|
||||
};
|
||||
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
{
|
||||
constraint.dir1 = force_dir1;
|
||||
constraint.im1 = rb1.mass_properties.inv_mass;
|
||||
constraint.im2 = rb2.mass_properties.inv_mass;
|
||||
constraint.limit = manifold.friction;
|
||||
constraint.mj_lambda1 = mj_lambda1;
|
||||
constraint.mj_lambda2 = mj_lambda2;
|
||||
constraint.manifold_id = manifold_id;
|
||||
constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
|
||||
constraint.num_contacts = manifold_points.len() as u8;
|
||||
}
|
||||
|
||||
for k in 0..manifold_points.len() {
|
||||
let manifold_point = &manifold_points[k];
|
||||
let dp1 = (rb1.position * manifold_point.local_p1).coords
|
||||
- rb1.position.translation.vector;
|
||||
let dp2 = (rb2.position * manifold_point.local_p2).coords
|
||||
- rb2.position.translation.vector;
|
||||
|
||||
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
|
||||
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
|
||||
|
||||
// Normal part.
|
||||
{
|
||||
let gcross1 = rb1
|
||||
.world_inv_inertia_sqrt
|
||||
.transform_vector(dp1.gcross(force_dir1));
|
||||
let gcross2 = rb2
|
||||
.world_inv_inertia_sqrt
|
||||
.transform_vector(dp2.gcross(-force_dir1));
|
||||
|
||||
let r = 1.0
|
||||
/ (rb1.mass_properties.inv_mass
|
||||
+ rb2.mass_properties.inv_mass
|
||||
+ gcross1.gdot(gcross1)
|
||||
+ gcross2.gdot(gcross2));
|
||||
|
||||
let rhs = (vel1 - vel2).dot(&force_dir1)
|
||||
+ manifold_point.dist.max(0.0) * params.inv_dt();
|
||||
|
||||
let impulse = manifold_points[k].impulse * warmstart_coeff;
|
||||
|
||||
constraint.elements[k].normal_part = VelocityConstraintElementPart {
|
||||
gcross1,
|
||||
gcross2,
|
||||
rhs,
|
||||
impulse,
|
||||
r,
|
||||
};
|
||||
}
|
||||
|
||||
// Tangent parts.
|
||||
{
|
||||
let tangents1 = force_dir1.orthonormal_basis();
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let gcross1 = rb1
|
||||
.world_inv_inertia_sqrt
|
||||
.transform_vector(dp1.gcross(tangents1[j]));
|
||||
let gcross2 = rb2
|
||||
.world_inv_inertia_sqrt
|
||||
.transform_vector(dp2.gcross(-tangents1[j]));
|
||||
let r = 1.0
|
||||
/ (rb1.mass_properties.inv_mass
|
||||
+ rb2.mass_properties.inv_mass
|
||||
+ gcross1.gdot(gcross1)
|
||||
+ gcross2.gdot(gcross2));
|
||||
let rhs = (vel1 - vel2).dot(&tangents1[j]);
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
|
||||
|
||||
constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
|
||||
gcross1,
|
||||
gcross2,
|
||||
rhs,
|
||||
impulse,
|
||||
r,
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
if push {
|
||||
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
|
||||
} else {
|
||||
out_constraints[manifold.constraint_index + l] =
|
||||
AnyVelocityConstraint::Nongrouped(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel::zero();
|
||||
let mut mj_lambda2 = DeltaVel::zero();
|
||||
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let elt = &self.elements[i].normal_part;
|
||||
mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
|
||||
mj_lambda1.angular += elt.gcross1 * elt.impulse;
|
||||
|
||||
mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
|
||||
mj_lambda2.angular += elt.gcross2 * elt.impulse;
|
||||
|
||||
// FIXME: move this out of the for loop?
|
||||
let tangents1 = self.dir1.orthonormal_basis();
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let elt = &self.elements[i].tangent_part[j];
|
||||
mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
|
||||
mj_lambda1.angular += elt.gcross1 * elt.impulse;
|
||||
|
||||
mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
|
||||
mj_lambda2.angular += elt.gcross2 * elt.impulse;
|
||||
}
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize].linear += mj_lambda1.linear;
|
||||
mj_lambdas[self.mj_lambda1 as usize].angular += mj_lambda1.angular;
|
||||
mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear;
|
||||
mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
// Solve friction.
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let tangents1 = self.dir1.orthonormal_basis();
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let normal_elt = &self.elements[i].normal_part;
|
||||
let elt = &mut self.elements[i].tangent_part[j];
|
||||
let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
|
||||
+ elt.gcross1.gdot(mj_lambda1.angular)
|
||||
- tangents1[j].dot(&mj_lambda2.linear)
|
||||
+ elt.gcross2.gdot(mj_lambda2.angular)
|
||||
+ elt.rhs;
|
||||
let limit = self.limit * normal_elt.impulse;
|
||||
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
|
||||
let dlambda = new_impulse - elt.impulse;
|
||||
elt.impulse = new_impulse;
|
||||
|
||||
mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda);
|
||||
mj_lambda1.angular += elt.gcross1 * dlambda;
|
||||
|
||||
mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
|
||||
mj_lambda2.angular += elt.gcross2 * dlambda;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve penetration.
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let elt = &mut self.elements[i].normal_part;
|
||||
let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)
|
||||
- self.dir1.dot(&mj_lambda2.linear)
|
||||
+ elt.gcross2.gdot(mj_lambda2.angular)
|
||||
+ elt.rhs;
|
||||
let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
|
||||
let dlambda = new_impulse - elt.impulse;
|
||||
elt.impulse = new_impulse;
|
||||
|
||||
mj_lambda1.linear += self.dir1 * (self.im1 * dlambda);
|
||||
mj_lambda1.angular += elt.gcross1 * dlambda;
|
||||
|
||||
mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
|
||||
mj_lambda2.angular += elt.gcross2 * dlambda;
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
|
||||
let manifold = &mut manifolds_all[self.manifold_id];
|
||||
let k_base = self.manifold_contact_id;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let active_contacts = manifold.active_contacts_mut();
|
||||
active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
active_contacts[k_base + k].tangent_impulse =
|
||||
self.elements[k].tangent_part[0].impulse;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
active_contacts[k_base + k].tangent_impulse = [
|
||||
self.elements[k].tangent_part[0].impulse,
|
||||
self.elements[k].tangent_part[1].impulse,
|
||||
];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
344
src/dynamics/solver/velocity_constraint_wide.rs
Normal file
344
src/dynamics/solver/velocity_constraint_wide.rs
Normal file
@@ -0,0 +1,344 @@
|
||||
use super::{AnyVelocityConstraint, DeltaVel};
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
use crate::math::{
|
||||
AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
|
||||
SIMD_WIDTH,
|
||||
};
|
||||
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
|
||||
use num::Zero;
|
||||
use simba::simd::{SimdPartialOrd, SimdValue};
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct WVelocityConstraintElementPart {
|
||||
pub gcross1: AngVector<SimdFloat>,
|
||||
pub gcross2: AngVector<SimdFloat>,
|
||||
pub rhs: SimdFloat,
|
||||
pub impulse: SimdFloat,
|
||||
pub r: SimdFloat,
|
||||
}
|
||||
|
||||
impl WVelocityConstraintElementPart {
|
||||
pub fn zero() -> Self {
|
||||
Self {
|
||||
gcross1: AngVector::zero(),
|
||||
gcross2: AngVector::zero(),
|
||||
rhs: SimdFloat::zero(),
|
||||
impulse: SimdFloat::zero(),
|
||||
r: SimdFloat::zero(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct WVelocityConstraintElement {
|
||||
pub normal_part: WVelocityConstraintElementPart,
|
||||
pub tangent_parts: [WVelocityConstraintElementPart; DIM - 1],
|
||||
}
|
||||
|
||||
impl WVelocityConstraintElement {
|
||||
pub fn zero() -> Self {
|
||||
Self {
|
||||
normal_part: WVelocityConstraintElementPart::zero(),
|
||||
tangent_parts: [WVelocityConstraintElementPart::zero(); DIM - 1],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct WVelocityConstraint {
|
||||
pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
|
||||
pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS],
|
||||
pub num_contacts: u8,
|
||||
pub im1: SimdFloat,
|
||||
pub im2: SimdFloat,
|
||||
pub limit: SimdFloat,
|
||||
pub mj_lambda1: [usize; SIMD_WIDTH],
|
||||
pub mj_lambda2: [usize; SIMD_WIDTH],
|
||||
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
|
||||
pub manifold_contact_id: usize,
|
||||
}
|
||||
|
||||
impl WVelocityConstraint {
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
|
||||
manifolds: [&ContactManifold; SIMD_WIDTH],
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyVelocityConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let inv_dt = SimdFloat::splat(params.inv_dt());
|
||||
let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
|
||||
let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
|
||||
|
||||
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
|
||||
let ii1: AngularInertia<SimdFloat> =
|
||||
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
|
||||
|
||||
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
|
||||
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
|
||||
|
||||
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
|
||||
|
||||
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
|
||||
let ii2: AngularInertia<SimdFloat> =
|
||||
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
|
||||
|
||||
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
|
||||
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
|
||||
|
||||
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
|
||||
|
||||
let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
|
||||
|
||||
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
|
||||
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
|
||||
|
||||
let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
|
||||
let warmstart_multiplier =
|
||||
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
|
||||
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
|
||||
|
||||
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
|
||||
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
|
||||
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
|
||||
|
||||
let mut constraint = WVelocityConstraint {
|
||||
dir1: force_dir1,
|
||||
elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
|
||||
im1,
|
||||
im2,
|
||||
limit: friction,
|
||||
mj_lambda1,
|
||||
mj_lambda2,
|
||||
manifold_id,
|
||||
manifold_contact_id: l,
|
||||
num_contacts: num_points as u8,
|
||||
};
|
||||
|
||||
for k in 0..num_points {
|
||||
// FIXME: can we avoid the multiplications by position1/position2 here?
|
||||
// By working as much as possible in local-space.
|
||||
let p1 = position1
|
||||
* Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]);
|
||||
let p2 = position2
|
||||
* Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
|
||||
|
||||
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
|
||||
|
||||
let impulse =
|
||||
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
|
||||
|
||||
let dp1 = p1.coords - position1.translation.vector;
|
||||
let dp2 = p2.coords - position2.translation.vector;
|
||||
|
||||
let vel1 = linvel1 + angvel1.gcross(dp1);
|
||||
let vel2 = linvel2 + angvel2.gcross(dp2);
|
||||
|
||||
// Normal part.
|
||||
{
|
||||
let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
|
||||
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
|
||||
|
||||
let r = SimdFloat::splat(1.0)
|
||||
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
|
||||
let rhs =
|
||||
(vel1 - vel2).dot(&force_dir1) + dist.simd_max(SimdFloat::zero()) * inv_dt;
|
||||
|
||||
constraint.elements[k].normal_part = WVelocityConstraintElementPart {
|
||||
gcross1,
|
||||
gcross2,
|
||||
rhs,
|
||||
impulse: impulse * warmstart_coeff,
|
||||
r,
|
||||
};
|
||||
}
|
||||
|
||||
// tangent parts.
|
||||
let tangents1 = force_dir1.orthonormal_basis();
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = SimdFloat::from(
|
||||
array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
|
||||
);
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = SimdFloat::from(
|
||||
array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
|
||||
);
|
||||
|
||||
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
|
||||
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
|
||||
let r = SimdFloat::splat(1.0)
|
||||
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
|
||||
let rhs = (vel1 - vel2).dot(&tangents1[j]);
|
||||
|
||||
constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart {
|
||||
gcross1,
|
||||
gcross2,
|
||||
rhs,
|
||||
impulse: impulse * warmstart_coeff,
|
||||
r,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
if push {
|
||||
out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
|
||||
} else {
|
||||
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
|
||||
AnyVelocityConstraint::Grouped(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let elt = &self.elements[i].normal_part;
|
||||
mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
|
||||
mj_lambda1.angular += elt.gcross1 * elt.impulse;
|
||||
|
||||
mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
|
||||
mj_lambda2.angular += elt.gcross2 * elt.impulse;
|
||||
|
||||
// FIXME: move this out of the for loop?
|
||||
let tangents1 = self.dir1.orthonormal_basis();
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let elt = &self.elements[i].tangent_parts[j];
|
||||
mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
|
||||
mj_lambda1.angular += elt.gcross1 * elt.impulse;
|
||||
|
||||
mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
|
||||
mj_lambda2.angular += elt.gcross2 * elt.impulse;
|
||||
}
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda1 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
let mut mj_lambda2 = DeltaVel {
|
||||
linear: Vector::from(
|
||||
array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
|
||||
),
|
||||
angular: AngVector::from(
|
||||
array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
|
||||
),
|
||||
};
|
||||
|
||||
// Solve friction first.
|
||||
for i in 0..self.num_contacts as usize {
|
||||
// FIXME: move this out of the for loop?
|
||||
let tangents1 = self.dir1.orthonormal_basis();
|
||||
let normal_elt = &self.elements[i].normal_part;
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let elt = &mut self.elements[i].tangent_parts[j];
|
||||
let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
|
||||
+ elt.gcross1.gdot(mj_lambda1.angular)
|
||||
- tangents1[j].dot(&mj_lambda2.linear)
|
||||
+ elt.gcross2.gdot(mj_lambda2.angular)
|
||||
+ elt.rhs;
|
||||
let limit = self.limit * normal_elt.impulse;
|
||||
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
|
||||
let dlambda = new_impulse - elt.impulse;
|
||||
elt.impulse = new_impulse;
|
||||
|
||||
mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda);
|
||||
mj_lambda1.angular += elt.gcross1 * dlambda;
|
||||
mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
|
||||
mj_lambda2.angular += elt.gcross2 * dlambda;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve non-penetration after friction.
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let elt = &mut self.elements[i].normal_part;
|
||||
let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)
|
||||
- self.dir1.dot(&mj_lambda2.linear)
|
||||
+ elt.gcross2.gdot(mj_lambda2.angular)
|
||||
+ elt.rhs;
|
||||
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
|
||||
let dlambda = new_impulse - elt.impulse;
|
||||
elt.impulse = new_impulse;
|
||||
|
||||
mj_lambda1.linear += self.dir1 * (self.im1 * dlambda);
|
||||
mj_lambda1.angular += elt.gcross1 * dlambda;
|
||||
mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
|
||||
mj_lambda2.angular += elt.gcross2 * dlambda;
|
||||
}
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
|
||||
}
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
|
||||
mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
|
||||
let tangent_impulses: [_; SIMD_WIDTH] =
|
||||
self.elements[k].tangent_parts[0].impulse.into();
|
||||
#[cfg(feature = "dim3")]
|
||||
let bitangent_impulses: [_; SIMD_WIDTH] =
|
||||
self.elements[k].tangent_parts[1].impulse.into();
|
||||
|
||||
for ii in 0..SIMD_WIDTH {
|
||||
let manifold = &mut manifolds_all[self.manifold_id[ii]];
|
||||
let k_base = self.manifold_contact_id;
|
||||
let active_contacts = manifold.active_contacts_mut();
|
||||
active_contacts[k_base + k].impulse = impulses[ii];
|
||||
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
active_contacts[k_base + k].tangent_impulse =
|
||||
[tangent_impulses[ii], bitangent_impulses[ii]];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
300
src/dynamics/solver/velocity_ground_constraint.rs
Normal file
300
src/dynamics/solver/velocity_ground_constraint.rs
Normal file
@@ -0,0 +1,300 @@
|
||||
use super::{AnyVelocityConstraint, DeltaVel};
|
||||
use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
|
||||
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
|
||||
|
||||
use crate::dynamics::{IntegrationParameters, RigidBodySet};
|
||||
use crate::geometry::{ContactManifold, ContactManifoldIndex};
|
||||
use simba::simd::SimdPartialOrd;
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct VelocityGroundConstraintElementPart {
|
||||
pub gcross2: AngVector<f32>,
|
||||
pub rhs: f32,
|
||||
pub impulse: f32,
|
||||
pub r: f32,
|
||||
}
|
||||
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
impl VelocityGroundConstraintElementPart {
|
||||
fn zero() -> Self {
|
||||
Self {
|
||||
gcross2: na::zero(),
|
||||
rhs: 0.0,
|
||||
impulse: 0.0,
|
||||
r: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct VelocityGroundConstraintElement {
|
||||
pub normal_part: VelocityGroundConstraintElementPart,
|
||||
pub tangent_part: [VelocityGroundConstraintElementPart; DIM - 1],
|
||||
}
|
||||
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
impl VelocityGroundConstraintElement {
|
||||
pub fn zero() -> Self {
|
||||
Self {
|
||||
normal_part: VelocityGroundConstraintElementPart::zero(),
|
||||
tangent_part: [VelocityGroundConstraintElementPart::zero(); DIM - 1],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
pub(crate) struct VelocityGroundConstraint {
|
||||
pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
|
||||
pub im2: f32,
|
||||
pub limit: f32,
|
||||
pub mj_lambda2: usize,
|
||||
pub manifold_id: ContactManifoldIndex,
|
||||
pub manifold_contact_id: usize,
|
||||
pub num_contacts: u8,
|
||||
pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
|
||||
}
|
||||
|
||||
impl VelocityGroundConstraint {
|
||||
pub fn generate(
|
||||
params: &IntegrationParameters,
|
||||
manifold_id: ContactManifoldIndex,
|
||||
manifold: &ContactManifold,
|
||||
bodies: &RigidBodySet,
|
||||
out_constraints: &mut Vec<AnyVelocityConstraint>,
|
||||
push: bool,
|
||||
) {
|
||||
let mut rb1 = &bodies[manifold.body_pair.body1];
|
||||
let mut rb2 = &bodies[manifold.body_pair.body2];
|
||||
let flipped = !rb2.is_dynamic();
|
||||
|
||||
if flipped {
|
||||
std::mem::swap(&mut rb1, &mut rb2);
|
||||
}
|
||||
|
||||
let mj_lambda2 = rb2.active_set_offset;
|
||||
let force_dir1 = if flipped {
|
||||
// NOTE: we already swapped rb1 and rb2
|
||||
// so we multiply by rb1.position.
|
||||
rb1.position * (-manifold.local_n2)
|
||||
} else {
|
||||
rb1.position * (-manifold.local_n1)
|
||||
};
|
||||
|
||||
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
|
||||
|
||||
for (l, manifold_points) in manifold
|
||||
.active_contacts()
|
||||
.chunks(MAX_MANIFOLD_POINTS)
|
||||
.enumerate()
|
||||
{
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
let mut constraint = VelocityGroundConstraint {
|
||||
dir1: force_dir1,
|
||||
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
|
||||
im2: rb2.mass_properties.inv_mass,
|
||||
limit: manifold.friction,
|
||||
mj_lambda2,
|
||||
manifold_id,
|
||||
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
|
||||
num_contacts: manifold_points.len() as u8,
|
||||
};
|
||||
|
||||
// TODO: this is a WIP optimization for WASM platforms.
|
||||
// For some reasons, the compiler does not inline the `Vec::push` method
|
||||
// in this method. This generates two memset and one memcpy which are both very
|
||||
// expansive.
|
||||
// This would likely be solved by some kind of "placement-push" (like emplace in C++).
|
||||
// In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
|
||||
// avoid spurious copying.
|
||||
// Is this optimization beneficial when targeting non-WASM platforms?
|
||||
//
|
||||
// NOTE: joints have the same problem, but it is not easy to refactor the code that way
|
||||
// for the moment.
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
let constraint = if push {
|
||||
let new_len = out_constraints.len() + 1;
|
||||
unsafe {
|
||||
out_constraints.resize_with(new_len, || {
|
||||
AnyVelocityConstraint::NongroupedGround(
|
||||
std::mem::MaybeUninit::uninit().assume_init(),
|
||||
)
|
||||
});
|
||||
}
|
||||
out_constraints
|
||||
.last_mut()
|
||||
.unwrap()
|
||||
.as_nongrouped_ground_mut()
|
||||
.unwrap()
|
||||
} else {
|
||||
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
|
||||
};
|
||||
|
||||
#[cfg(target_arch = "wasm32")]
|
||||
{
|
||||
constraint.dir1 = force_dir1;
|
||||
constraint.im2 = rb2.mass_properties.inv_mass;
|
||||
constraint.limit = manifold.friction;
|
||||
constraint.mj_lambda2 = mj_lambda2;
|
||||
constraint.manifold_id = manifold_id;
|
||||
constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
|
||||
constraint.num_contacts = manifold_points.len() as u8;
|
||||
}
|
||||
|
||||
for k in 0..manifold_points.len() {
|
||||
let manifold_point = &manifold_points[k];
|
||||
let (p1, p2) = if flipped {
|
||||
// NOTE: we already swapped rb1 and rb2
|
||||
// so we multiply by rb2.position.
|
||||
(
|
||||
rb1.position * manifold_point.local_p2,
|
||||
rb2.position * manifold_point.local_p1,
|
||||
)
|
||||
} else {
|
||||
(
|
||||
rb1.position * manifold_point.local_p1,
|
||||
rb2.position * manifold_point.local_p2,
|
||||
)
|
||||
};
|
||||
let dp2 = p2.coords - rb2.position.translation.vector;
|
||||
let dp1 = p1.coords - rb1.position.translation.vector;
|
||||
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
|
||||
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
|
||||
|
||||
// Normal part.
|
||||
{
|
||||
let gcross2 = rb2
|
||||
.world_inv_inertia_sqrt
|
||||
.transform_vector(dp2.gcross(-force_dir1));
|
||||
|
||||
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
|
||||
let rhs = -vel2.dot(&force_dir1)
|
||||
+ vel1.dot(&force_dir1)
|
||||
+ manifold_point.dist.max(0.0) * params.inv_dt();
|
||||
let impulse = manifold_points[k].impulse * warmstart_coeff;
|
||||
|
||||
constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
|
||||
gcross2,
|
||||
rhs,
|
||||
impulse,
|
||||
r,
|
||||
};
|
||||
}
|
||||
|
||||
// Tangent parts.
|
||||
{
|
||||
let tangents1 = force_dir1.orthonormal_basis();
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let gcross2 = rb2
|
||||
.world_inv_inertia_sqrt
|
||||
.transform_vector(dp2.gcross(-tangents1[j]));
|
||||
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
|
||||
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
|
||||
#[cfg(feature = "dim2")]
|
||||
let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
|
||||
#[cfg(feature = "dim3")]
|
||||
let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
|
||||
|
||||
constraint.elements[k].tangent_part[j] =
|
||||
VelocityGroundConstraintElementPart {
|
||||
gcross2,
|
||||
rhs,
|
||||
impulse,
|
||||
r,
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(not(target_arch = "wasm32"))]
|
||||
if push {
|
||||
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
|
||||
} else {
|
||||
out_constraints[manifold.constraint_index + l] =
|
||||
AnyVelocityConstraint::NongroupedGround(constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = DeltaVel::zero();
|
||||
let tangents1 = self.dir1.orthonormal_basis();
|
||||
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let elt = &self.elements[i].normal_part;
|
||||
mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
|
||||
mj_lambda2.angular += elt.gcross2 * elt.impulse;
|
||||
|
||||
for j in 0..DIM - 1 {
|
||||
let elt = &self.elements[i].tangent_part[j];
|
||||
mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
|
||||
mj_lambda2.angular += elt.gcross2 * elt.impulse;
|
||||
}
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear;
|
||||
mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
|
||||
}
|
||||
|
||||
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
|
||||
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
|
||||
|
||||
// Solve friction.
|
||||
let tangents1 = self.dir1.orthonormal_basis();
|
||||
|
||||
for i in 0..self.num_contacts as usize {
|
||||
for j in 0..DIM - 1 {
|
||||
let normal_elt = &self.elements[i].normal_part;
|
||||
let elt = &mut self.elements[i].tangent_part[j];
|
||||
let dimpulse = -tangents1[j].dot(&mj_lambda2.linear)
|
||||
+ elt.gcross2.gdot(mj_lambda2.angular)
|
||||
+ elt.rhs;
|
||||
let limit = self.limit * normal_elt.impulse;
|
||||
let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
|
||||
let dlambda = new_impulse - elt.impulse;
|
||||
elt.impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
|
||||
mj_lambda2.angular += elt.gcross2 * dlambda;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve penetration.
|
||||
for i in 0..self.num_contacts as usize {
|
||||
let elt = &mut self.elements[i].normal_part;
|
||||
let dimpulse =
|
||||
-self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
|
||||
let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
|
||||
let dlambda = new_impulse - elt.impulse;
|
||||
elt.impulse = new_impulse;
|
||||
|
||||
mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
|
||||
mj_lambda2.angular += elt.gcross2 * dlambda;
|
||||
}
|
||||
|
||||
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
|
||||
}
|
||||
|
||||
// FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
|
||||
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
|
||||
let manifold = &mut manifolds_all[self.manifold_id];
|
||||
let k_base = self.manifold_contact_id;
|
||||
|
||||
for k in 0..self.num_contacts as usize {
|
||||
let active_contacts = manifold.active_contacts_mut();
|
||||
active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
|
||||
#[cfg(feature = "dim2")]
|
||||
{
|
||||
active_contacts[k_base + k].tangent_impulse =
|
||||
self.elements[k].tangent_part[0].impulse;
|
||||
}
|
||||
#[cfg(feature = "dim3")]
|
||||
{
|
||||
active_contacts[k_base + k].tangent_impulse = [
|
||||
self.elements[k].tangent_part[0].impulse,
|
||||
self.elements[k].tangent_part[1].impulse,
|
||||
];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user