orion/phy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

lots

Browse Source
This commit is contained in:
Orion Kindel
2025-01-25 11:53:54 -06:00
parent 34cc04ff1a
commit 6481857e8e
25 changed files with 1077 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Cargo.lock generated
View File

@@ -5128,6 +5128,7 @@ dependencies = [
"num-traits",
"num_cpus",
"ordered-float",
"parry3d",
"postcard",
"rand",
"rapier3d",
@@ -5141,6 +5142,7 @@ dependencies = [
"tinyvec",
"tobj",
"tritet",
"wide",
]
[[package]]
@@ -6686,9 +6688,9 @@ dependencies = [
[[package]]
name = "wide"
version = "0.7.31"
version = "0.7.32"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cc0ca27312d1e9218687a4e804cd4b7410bf54125d54d13e5170efbf09066d24"
checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22"
dependencies = [
"bytemuck",
"safe_arch",

2
Cargo.toml
View File

@@ -50,6 +50,8 @@ itertools = "0.14.0"
serde_cbor = "0.11.2"
postcard = { version = "1.1.1", features = ["use-std"] }
bevy_framepace = "0.18"
wide = "0.7.32"
parry3d = "0.18.0"
[[bin]]
name = "example"

BIN
prefab/Anaconda.stl
View File

Binary file not shown.

302
prefab/anaconda-bones.json Normal file
View File

@@ -0,0 +1,302 @@
{
"bone": {
"head": {
"x": 0.6070398688316345,
"y": 3.488184690475464,
"z": 0.19564415514469147
},
"tail": {
"x": 0.6236315965652466,
"y": 3.4085373878479004,
"z": 0.1993076056241989
}
},
"bone_009": {
"head": {
"x": 0.6236315965652466,
"y": 3.4085373878479004,
"z": 0.1993076056241989
},
"tail": {
"x": 0.6183035969734192,
"y": 3.316044569015503,
"z": 0.20030179619789124
}
},
"bone_007": {
"head": {
"x": 0.6183035969734192,
"y": 3.316044569015503,
"z": 0.20030179619789124
},
"tail": {
"x": 0.5486531853675842,
"y": 3.2271180152893066,
"z": 0.19193406403064728
}
},
"bone_008": {
"head": {
"x": 0.5486531853675842,
"y": 3.2271180152893066,
"z": 0.19193406403064728
},
"tail": {
"x": 0.4748899042606354,
"y": 3.1820590496063232,
"z": 0.19288358092308044
}
},
"bone_006": {
"head": {
"x": 0.4748899042606354,
"y": 3.1820590496063232,
"z": 0.19288358092308044
},
"tail": {
"x": 0.32671600580215454,
"y": 3.11191725730896,
"z": 0.19421303272247314
}
},
"bone_010": {
"head": {
"x": 0.32671600580215454,
"y": 3.11191725730896,
"z": 0.19421303272247314
},
"tail": {
"x": 0.19413162767887115,
"y": 2.997762441635132,
"z": 0.1879468560218811
}
},
"bone_004": {
"head": {
"x": 0.19413162767887115,
"y": 2.997762441635132,
"z": 0.1879468560218811
},
"tail": {
"x": 0.08068132400512695,
"y": 2.8404202461242676,
"z": 0.18485479056835175
}
},
"bone_011": {
"head": {
"x": 0.08068132400512695,
"y": 2.8404202461242676,
"z": 0.18485479056835175
},
"tail": {
"x": -0.014009974896907806,
"y": 2.670964241027832,
"z": 0.17771753668785095
}
},
"bone_005": {
"head": {
"x": -0.014009974896907806,
"y": 2.670964241027832,
"z": 0.17771753668785095
},
"tail": {
"x": -0.07677791267633438,
"y": 2.5024752616882324,
"z": 0.17266269028186798
}
},
"bone_012": {
"head": {
"x": -0.07677791267633438,
"y": 2.5024752616882324,
"z": 0.17266269028186798
},
"tail": {
"x": -0.11340910941362381,
"y": 2.3294928073883057,
"z": 0.17319327592849731
}
},
"bone_002": {
"head": {
"x": -0.11340910941362381,
"y": 2.3294928073883057,
"z": 0.17319327592849731
},
"tail": {
"x": -0.08609960973262787,
"y": 2.0730838775634766,
"z": 0.17111866176128387
}
},
"bone_015": {
"head": {
"x": -0.08609960973262787,
"y": 2.0730838775634766,
"z": 0.17111866176128387
},
"tail": {
"x": -0.05879011005163193,
"y": 1.816674828529358,
"z": 0.16904404759407043
}
},
"bone_014": {
"head": {
"x": -0.05879011005163193,
"y": 1.816674828529358,
"z": 0.16904404759407043
},
"tail": {
"x": -0.031480610370635986,
"y": 1.5602657794952393,
"z": 0.166969433426857
}
},
"bone_016": {
"head": {
"x": -0.031480610370635986,
"y": 1.5602657794952393,
"z": 0.166969433426857
},
"tail": {
"x": -0.004171114414930344,
"y": 1.3038568496704102,
"z": 0.16489481925964355
}
},
"bone_013": {
"head": {
"x": -0.004171114414930344,
"y": 1.3038568496704102,
"z": 0.16489481925964355
},
"tail": {
"x": 0.05561229586601257,
"y": 1.041410207748413,
"z": 0.16239383816719055
}
},
"bone_017": {
"head": {
"x": 0.05561229586601257,
"y": 1.041410207748413,
"z": 0.16239383816719055
},
"tail": {
"x": 0.15776894986629486,
"y": 0.6679806709289551,
"z": 0.14728130400180817
}
},
"bone_021": {
"head": {
"x": 0.15776894986629486,
"y": 0.6679806709289551,
"z": 0.14728130400180817
},
"tail": {
"x": 0.1563047468662262,
"y": 0.28363165259361267,
"z": 0.1468256264925003
}
},
"bone_001": {
"head": {
"x": 0.1563047468662262,
"y": 0.28363165259361267,
"z": 0.1468256264925003
},
"tail": {
"x": 0.14030331373214722,
"y": -0.11586377024650574,
"z": 0.16327451169490814
}
},
"bone_019": {
"head": {
"x": 0.14030331373214722,
"y": -0.11586377024650574,
"z": 0.16327451169490814
},
"tail": {
"x": 0.08491484820842743,
"y": -0.5309418439865112,
"z": 0.15609216690063477
}
},
"bone_018": {
"head": {
"x": 0.08491484820842743,
"y": -0.5309418439865112,
"z": 0.15609216690063477
},
"tail": {
"x": 0.02284509874880314,
"y": -0.9243378639221191,
"z": 0.14005453884601593
}
},
"bone_020": {
"head": {
"x": 0.02284509874880314,
"y": -0.9243378639221191,
"z": 0.14005453884601593
},
"tail": {
"x": -0.03272989019751549,
"y": -1.3230215311050415,
"z": 0.1516072154045105
}
},
"bone_003": {
"head": {
"x": -0.03272989019751549,
"y": -1.3230215311050415,
"z": 0.1516072154045105
},
"tail": {
"x": -0.0303995069116354,
"y": -1.721837043762207,
"z": 0.1625695526599884
}
},
"bone_023": {
"head": {
"x": -0.0303995069116354,
"y": -1.721837043762207,
"z": 0.1625695526599884
},
"tail": {
"x": -0.034437455236911774,
"y": -2.1189937591552734,
"z": 0.17180347442626953
}
},
"bone_022": {
"head": {
"x": -0.034437455236911774,
"y": -2.1189937591552734,
"z": 0.17180347442626953
},
"tail": {
"x": -0.05511781573295593,
"y": -2.520186185836792,
"z": 0.22017113864421844
}
},
"bone_024": {
"head": {
"x": -0.05511781573295593,
"y": -2.520186185836792,
"z": 0.22017113864421844
},
"tail": {
"x": -0.024445362389087677,
"y": -2.902860403060913,
"z": 0.22326646745204926
}
}
}

BIN
prefab/anaconda.stl Normal file
View File

Binary file not shown.

15
prefab/export_highlighted_armature.py
View File

@@ -2,14 +2,15 @@ import bpy
import json
from bpy import context
o = bpy.context.object
out = {}
vec3 = lambda vec: {"x": vec.x, "y": vec.y, "z": vec.z}
for b in o.pose.bones:
head = o.matrix_world @ b.matrix @ b.head
tail = o.matrix_world @ b.matrix @ b.tail
name = b.name.replace('.', '_').replace('-', '_').lower()
out[name] = {"head": vec3(head), "tail": vec3(tail)}
bone_head = lambda ctx, bone: vec3(ctx.matrix_world.translation + bone.head_local)
bone_tail = lambda ctx, bone: vec3(ctx.matrix_world.translation + bone.tail_local)
bone = lambda ctx, bone: {"head": bone_head(ctx, bone), "tail": bone_tail(ctx, bone)}
bone_name = lambda name: name.replace('.', '_').replace('-', '_').lower()
arm_context = bpy.context.object
arm_data = arm_context.data
out = {bone_name(b.name) : bone(arm_context, b) for b in arm_data.bones}
print('\n\n\n\n')
print(json.dumps(out))

0
prefab/female_base-bones.json → prefab/human-female-bones.json
View File

0
prefab/female_base_500f.stl → prefab/human-female.stl
View File

1
prefab/human-male-bones.json Normal file
View File

File diff suppressed because one or more lines are too long

BIN
prefab/human-male.stl Normal file
View File

Binary file not shown.

1
prefab/male_base-bones.json
View File

File diff suppressed because one or more lines are too long

BIN
prefab/male_base_500f.stl
View File

Binary file not shown.

126
src/body.rs
View File

@@ -1,4 +1,5 @@
use std::ops::DerefMut;
use std::sync::Arc;
use ::bevy::app::{Plugin, Update};
use ::bevy::asset::Assets;
@@ -13,6 +14,7 @@ use ::bevy::prelude::{Added,
Drag,
DragEnd,
Entity,
IntoSystemConfigs,
Mesh as BevyMesh,
Mesh3d,
Meshable,
@@ -32,7 +34,8 @@ use ::bevy::render::mesh::VertexAttributeValues;
use bevy::prelude as bevy;
use bevy_ecs::component::ComponentIdFor;
use bevy_pbr::MeshMaterial3d;
use nalgebra::{Isometry3, Unit, Vector3};
use nalgebra::{Isometry3, Translation3, Unit, Vector3};
use parry3d::shape::SharedShape;
use rapier3d::prelude::{Collider, TypedShape};
use crate::camera::OrbitCamera;
@@ -57,12 +60,10 @@ impl Plugin for BodyPlugin {
.add_systems(Update, Body::draggable_added)
.add_systems(Update, Body::collider_added)
.add_systems(Update, Body::mesh_added)
.add_systems(Update, Body::rigid_committed)
.add_systems(Update, Body::xpbd_committed)
.add_systems(Update, Body::sync_rigid_building)
.add_systems(Update, Body::sync_xpbd_building)
.add_systems(Update, Body::sync_rigid)
.add_systems(Update, Body::sync_xpbd);
.add_systems(Update,
(Body::rigid_committed, Body::sync_rigid_building, Body::sync_rigid).chain())
.add_systems(Update,
(Body::xpbd_committed, Body::sync_xpbd_building, Body::sync_xpbd).chain());
}
}
@@ -144,47 +145,19 @@ pub mod physics {
pub struct XPBDVertexIndex(pub Vec<rapier::RigidBodyHandle>);
impl XPBDVertexIndex {
pub fn new(bodies: &rapier::RigidBodySet,
xpbd: &xpbd::Set,
pub fn new(xpbd: &xpbd::Set,
handle: xpbd::Handle,
trimesh: &TriMesh)
-> Self {
let vert2rigidbody = |body: &xpbd::Body| {
let point2handle = body.handles
.iter()
.map(|b| {
let point = bodies.get(*b).unwrap().translation();
(*point, *b)
})
.collect::<Vec<(Vector3<f32>, rapier::RigidBodyHandle)>>();
trimesh.vertices
.iter()
.map(|point| {
let Some((dist, body)) =
point2handle.iter()
.fold(Option::<(f32, rapier::RigidBodyHandle)>::None,
|found, (pt, body)| {
let dist = (*pt - point).magnitude();
match found {
| Some((fdist, fbody)) if fdist < dist => {
Some((fdist, fbody))
},
| _ => Some((dist, *body)),
}
})
else {
unreachable!()
};
if dist >= 0.1 {
panic!();
}
body
})
.collect::<Vec<_>>()
};
Self(xpbd.get(handle, vert2rigidbody).unwrap())
let face2handle = xpbd.get(handle, |b| b.trimesh.iter().map(|xs| xs.map(|x| b.handles[x])).collect::<Vec<_>>()).unwrap();
let mut v2b: Vec<Option<rapier::RigidBodyHandle>> = trimesh.vertices.iter().map(|_| None).collect();
trimesh.faces.iter().enumerate().for_each(|(fix, face)| {
face.iter().enumerate().for_each(|(fvix, vix)| {
v2b[(*vix) as usize] = Some(face2handle[fix][fvix]);
});
});
let v2b = v2b.into_iter().map(|a| a.unwrap()).collect();
Self(v2b)
}
}
}
@@ -247,6 +220,8 @@ impl Body {
};
let body = phy.bodies.get_mut(*h).unwrap();
body.set_body_type(rapier3d::prelude::RigidBodyType::KinematicPositionBased, true);
let (cam_id, mut cam, _) = cam.single_mut();
cam.enabled = false;
@@ -272,13 +247,13 @@ impl Body {
let denom = plane_normal.dot(&ray_dir);
let t = plane_normal.dot(&(plane_origin - ray_origin)) / denom;
let t = ray_origin + t * ray_dir;
body.set_translation(t, true);
body.set_linvel(Vector3::zeros(), true);
body.set_next_kinematic_position(Isometry3 {translation: Translation3::from(t), ..Default::default()});
}
fn on_drag_end(t: Trigger<Pointer<DragEnd>>,
mut commands: Commands,
plane_id: ComponentIdFor<Dragging>,
mut rigid: Query<&physics::RigidBody>,
mut cam: Query<&mut OrbitCamera>,
mut phy: Query<&mut PhysicsState>) {
let mut phy = phy.single_mut();
@@ -287,6 +262,11 @@ impl Body {
}
phy.ghosting = false;
if let Ok(physics::RigidBody(h)) = rigid.get_mut(t.entity()) {
let body = phy.bodies.get_mut(*h).unwrap();
body.set_body_type(rapier3d::prelude::RigidBodyType::Dynamic, true);
};
commands.entity(t.entity()).remove_by_id(plane_id.get());
}
@@ -358,7 +338,9 @@ impl Body {
let mut phy = phy.single_mut();
let PhysicsState { bodies, colliders, .. } = phy.deref_mut();
for (id, t, builder, mut coll) in &mut q {
coll.0.shape_mut().scale_dyn(&t.0.vector, 10);
let shp = coll.0.shape().scale_dyn(&t.0.vector, 10).unwrap();
let shp = SharedShape(Arc::from(shp));
coll.0.set_shape(shp);
let body = bodies.insert(builder.0.clone());
colliders.insert_with_parent(coll.0.clone(), body, bodies);
commands.entity(id)
@@ -385,33 +367,37 @@ impl Body {
.. } = phy.deref_mut();
for (id, mut t, mut s, mut builder, mut tri, mut tet, mut bones) in &mut q {
let builder = &mut builder.0;
let scale = &mut s.0.vector;
let translation = builder.transform.translation.vector;
let rotation = builder.transform.rotation;
let bones = &mut bones.0;
let tet = &mut tet.0;
let tri = &mut tri.0;
let builder = &mut builder.0;
tet.scale(scale);
tet.translate(&builder.transform.translation.vector);
tet.rotate(&builder.transform.rotation);
tet.translate(&translation);
tet.rotate(&rotation);
tri.scale(scale);
tri.translate(&builder.transform.translation.vector);
tri.rotate_about(tet.centroid(), &builder.transform.rotation);
tri.translate(&translation);
tri.rotate_about(tet.centroid(), &rotation);
bones.scale(scale);
bones.translate(&builder.transform.translation.vector);
bones.rotate_about(&tet.centroid(), &builder.transform.rotation);
bones.translate(&translation);
bones.rotate_about(&tet.centroid(), &rotation);
builder.transform = Isometry3::identity();
let (bone_bodies_, extra_constraints) = bones.insert_for_xpbd(0.5,
builder.stiffness.unwrap()
* 5.0,
tet,
bodies,
colliders,
impulse_joints);
let (bone_bodies_, extra_constraints) = {
bones.insert_for_xpbd(builder.stiffness.unwrap() * 1.0,
tet,
bodies,
colliders,
impulse_joints)
};
for body in bone_bodies_.iter()
.flat_map(|(_, (a, b, c))| [a, b, c])
@@ -425,7 +411,10 @@ impl Body {
Draggable));
}
let xpbd_handle = core::mem::take(builder).mesh(tet.clone())
let xpbd_handle = core::mem::take(builder).distance_constraint_damping(0.10)
.tetrahedra_volume_constraint_damping(0.10)
.linear_damping(0.10)
.mesh(tri.clone(), tet.clone())
.extra_constraints(extra_constraints)
.build(xpbd, bodies, colliders)
.unwrap();
@@ -434,7 +423,7 @@ impl Body {
t.rotation = Quat::IDENTITY;
t.translation = Vec3::splat(0.0);
let verts = physics::XPBDVertexIndex::new(bodies, xpbd, xpbd_handle, tri);
let verts = physics::XPBDVertexIndex::new(xpbd, xpbd_handle, tri);
commands.entity(id)
.insert((physics::XPBD(xpbd_handle), verts))
.remove::<building::Commit>();
@@ -461,7 +450,8 @@ impl Body {
}
}
fn sync_rigid(phy: Query<&PhysicsState>, mut q: Query<(&physics::RigidBody, &mut Transform), With<Mesh3d>>) {
fn sync_rigid(phy: Query<&PhysicsState>,
mut q: Query<(&physics::RigidBody, &mut Transform), With<Mesh3d>>) {
let phy = phy.single();
for (body, mut transform) in &mut q {
let body = phy.bodies.get(body.0).unwrap();
@@ -475,9 +465,11 @@ impl Body {
fn sync_xpbd(mut meshes: ResMut<Assets<BevyMesh>>,
phy: Query<&PhysicsState>,
mut q: Query<(&physics::XPBDVertexIndex, &Mesh3d)>) {
mut q: Query<(&physics::XPBDVertexIndex, &Mesh3d, &mut Transform)>) {
let phy = phy.single();
for (bodies, mesh) in &mut q {
for (bodies, mesh, mut t) in &mut q {
*t = Transform::IDENTITY;
let mesh = meshes.get_mut(&mesh.0).unwrap();
let vertices = match mesh.attribute_mut(BevyMesh::ATTRIBUTE_POSITION).unwrap() {

27
src/debug_render.rs
View File

@@ -1,9 +1,11 @@
use bevy::app::{App, Plugin, Update};
use bevy::color::palettes::css::WHITE;
use bevy::color::Color;
use bevy::prelude::{Gizmos, Query, ResMut, Resource};
use bevy::prelude::{Gizmos, Query, ResMut, Resource, Transform};
use nalgebra::{SimdValue, Vector3};
use rapier3d::math::Point;
use rapier3d::prelude::{DebugRenderBackend,
use rapier3d::prelude::{Aabb,
DebugRenderBackend,
DebugRenderMode,
DebugRenderObject,
DebugRenderPipeline,
@@ -70,7 +72,7 @@ fn debug_render_scene(mut debug_render: ResMut<DebugRenderPipelineResource>,
&phy.multibody_joints,
&phy.narrow_phase);
render_xpbd_bodies(&mut backend, &phy.bodies, &phy.xpbd);
// render_xpbd_bodies(&mut backend, &phy.bodies, &phy.xpbd);
}
}
@@ -80,9 +82,11 @@ pub fn render_xpbd_bodies(backend: &mut BevyLinesRenderBackend,
struct Colors {
vol: [f32; 4],
dst: [f32; 4],
contact: [f32; 4],
}
let color = Colors { vol: [126.0, 0.9, 0.4, 0.2],
dst: [169.0, 0.9, 0.4, 0.8] };
dst: [169.0, 0.9, 0.4, 0.8],
contact: [0.0, 1.0, 1.0, 1.0] };
macro_rules! ixs2positions {
($body:expr, $a:expr) => {
@@ -101,6 +105,16 @@ pub fn render_xpbd_bodies(backend: &mut BevyLinesRenderBackend,
}
for (_, body) in xpbd.iter() {
let aabb = body.aabb();
let translation = aabb.center();
let hext = aabb.half_extents();
backend.gizmos
.cuboid(Transform { translation: <[f32; 3]>::from(translation).into(),
scale: [hext.x, hext.y, hext.z].map(|a| a * 2.0).into(),
..Default::default() },
WHITE);
body.constraints
.iter()
.flat_map(|c| match c {
@@ -129,6 +143,11 @@ pub fn render_xpbd_bodies(backend: &mut BevyLinesRenderBackend,
| xpbd::constraint::impl_::Constraint::TetrahedronVolume(single) => {
vec![(color.vol, ixs2positions!(body, single.idxs))]
},
| xpbd::constraint::impl_::Constraint::Contact(contact) => {
let b = bodies.get(body.handles[contact.idx]).unwrap();
let t = b.translation();
vec![(color.contact, vec![*t, contact.contact_point])]
},
})
.for_each(|(color, positions)| {
let center = positions.iter()

22
src/either.rs
View File

@@ -1,3 +1,5 @@
use core::fmt::Debug;
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub enum Either<A, B> {
Left(A),
@@ -11,6 +13,26 @@ impl<A> Either<A, A> {
}
impl<A, B> Either<A, B> {
pub fn unwrap_left(self) -> A
where B: Debug
{
match self {
| Self::Left(t) => t,
| Self::Right(e) => panic!("called `Either::unwrap_left()` on a `Right` value: {:?}",
&e),
}
}
pub fn unwrap_right(self) -> B
where A: Debug
{
match self {
| Self::Right(t) => t,
| Self::Left(e) => panic!("called `Either::unwrap_right()` on a `Left` value: {:?}",
&e),
}
}
pub fn as_ref(&self) -> Either<&A, &B> {
match self {
| Either::Left(a) => Either::Left(a),

4
src/main.rs
View File

@@ -119,6 +119,10 @@ pub fn map_array16<T, R>(a: [T; 16], f: impl Fn(T) -> R) -> [R; 16] {
#[macro_export]
macro_rules! map_array {
(3; $a:expr, $f:expr) => {{
let [a0, a1, a2] = $a;
[$f(a0), $f(a1), $f(a2)]
}};
(4; $a:expr, $f:expr) => {{
let [a0, a1, a2, a3] = $a;
[$f(a0), $f(a1), $f(a2), $f(a3)]

113
src/mesh.rs
View File

@@ -1,5 +1,6 @@
use fixed::types::{I24F8, I32F32};
use nalgebra::{Scale3, UnitQuaternion, Vector3};
use parry3d::query::sat::cuboid_segment_find_local_separating_edge_twoway;
use rapier3d::prelude::*;
use rustc_hash::{FxHashMap, FxHashSet};
use serde::{Deserialize, Serialize};
@@ -15,6 +16,33 @@ pub struct TriMesh {
}
impl TriMesh {
pub fn new(faces: Vec<[u32; 3]>, vertices: Vec<Vector3<f32>>) -> Self {
let faces = faces.into_iter()
.map(|ixs| ixs.map(|x| vertices[x as usize]))
.collect::<Vec<_>>();
let vertices = vertices.iter()
.copied()
.map(|a| a.map(I32F32::from_num))
.collect::<FxHashSet<_>>()
.into_iter()
.collect::<Vec<_>>();
let faces = faces.into_iter()
.map(|pts| {
pts.map(|a| {
vertices.iter()
.enumerate()
.find(|(_, a_)| **a_ == a.map(I32F32::from_num))
.unwrap()
.0 as u32
})
})
.collect::<Vec<_>>();
Self { faces,
vertices: vertices.into_iter()
.map(|a| a.map(I32F32::to_num))
.collect() }
}
pub fn scale(&mut self, scale: &Vector3<f32>) {
let scale = |pt: &mut Vector3<f32>| {
pt[0] *= scale[0];
@@ -120,19 +148,40 @@ impl Mesh {
let mut vertices: FxHashSet<Vector3<I32F32>> = Default::default();
let mut edges: FxHashSet<(Vector3<I32F32>, Vector3<I32F32>)> = Default::default();
tets.iter().for_each(|pts| {
let [(_, a), (_, b), (_, c), (_, d)] = pts;
[a,b,c,d].into_iter().map(|a| a.map(I32F32::from_num)).for_each(|a| {vertices.insert(a);});
[(a,b), (a,c),(a,d), (b,c), (b,d), (c,d)]
.into_iter()
.map(|(a, b)| (a.map(I32F32::from_num), b.map(I32F32::from_num)))
.map(|(a, b)| if a <= b {(a, b)} else {(b, a)})
.for_each(|(a, b)| {edges.insert((a, b));});
});
let tets = tets.into_iter()
.inspect(|pts| {
let [(_, a), (_, b), (_, c), (_, d)] = pts;
[a, b, c, d].into_iter()
.map(|a| a.map(I32F32::from_num))
.for_each(|a| {
vertices.insert(a);
});
[(a, b), (a, c), (a, d), (b, c), (b, d), (c, d)].into_iter()
.map(|(a, b)| {
(a.map(I32F32::from_num),
b.map(I32F32::from_num))
})
.map(|(a, b)| {
if a <= b {
(a, b)
} else {
(b, a)
}
})
.for_each(|(a, b)| {
edges.insert((a, b));
});
})
.map(|pts| pts.map(|(b, pt)| (b, pt.map(I32F32::from_num).map(I32F32::to_num))))
.collect();
Mesh { tets,
edges: edges.into_iter().map(|(a, b)| (a.map(I32F32::to_num), b.map(I32F32::to_num))).collect(),
vertices: vertices.into_iter().map(|a| a.map(I32F32::to_num)).collect() }
edges: edges.into_iter()
.map(|(a, b)| (a.map(I32F32::to_num), b.map(I32F32::to_num)))
.collect(),
vertices: vertices.into_iter()
.map(|a| a.map(I32F32::to_num))
.collect() }
}
pub fn from_stl(stl: &[u8],
@@ -480,7 +529,7 @@ impl Bones {
let axis = UnitVector::new_normalize(Vector3::y_axis().cross(&d.normalize()));
let rot_angle = Vector3::y_axis().dot(&d.normalize()).acos();
let rotation = UnitQuaternion::from_axis_angle(&axis, rot_angle);
let body = bodies.insert(RigidBodyBuilder::new(RigidBodyType::Dynamic).position(Isometry {translation: Translation::from(center), rotation}));
let body = bodies.insert(RigidBodyBuilder::new(RigidBodyType::Dynamic).linear_damping(0.5) .position(Isometry {translation: Translation::from(center), rotation}));
colliders.insert_with_parent(ColliderBuilder::round_cylinder(dist / 2.0, 0.01, 0.01).density(10.0), body, bodies);
body
}
@@ -488,7 +537,6 @@ impl Bones {
#[allow(clippy::type_complexity)]
pub fn insert_for_xpbd(
&self,
thresh: f32,
xpbd::constraint::Stiffness::YoungPoisson(young, poisson): xpbd::constraint::Stiffness<f32>,
mesh: &Mesh,
bodies: &mut RigidBodySet,
@@ -502,27 +550,34 @@ impl Bones {
let mut mesh_joints =
Vec::<(Vector3<f32>, xpbd::constraint::DistanceBuilder<false, true, true>)>::new();
for a in &bones {
for (p, b) in [(a.head, a.head_body),
(a.tail, a.tail_body),
(a.center, a.link_body)]
{
mesh.vertices
let bone_bodies = bones.iter().flat_map(|b| {
[
(b.head, b.head_body),
(b.tail, b.tail_body),
(b.center, b.link_body),
]
}).collect::<Vec<_>>();
let closest_bone_body = |p: Vector3<f32>| -> (Vector3<f32>, RigidBodyHandle) {
*bone_bodies.iter().min_by_key(|(p_, _)| {
I32F32::from_num((*p_ - p).magnitude())
})
.unwrap()
};
for v in mesh.vertices
.iter()
.copied()
.filter(|p_| (p - p_).magnitude() <= thresh)
.for_each(|p_| {
let dst = (p - p_).magnitude();
mesh_joints.push((p_,
.copied() {
let (p, body) = closest_bone_body(v);
let dst = (p - v).magnitude();
mesh_joints.push((v,
DistanceBuilder::new().priority(10)
.body_2(b)
.body_2(body)
.rest_length(dst)
.stiffness(young, poisson)));
});
}
dbg!(&mesh_joints);
}
for a in &bones {
impulse_joints.insert(a.head_body,
a.link_body,
SphericalJointBuilder::new().local_anchor2(vector![0.0,

80
src/prefab.rs
View File

@@ -1,6 +1,8 @@
use std::{collections::VecDeque, sync::LazyLock};
use std::collections::VecDeque;
use std::sync::LazyLock;
use ::bevy::{prelude::{Cuboid, Meshable}, render::mesh::{Indices, MeshVertexAttribute, VertexAttributeValues}};
use ::bevy::prelude::{Cuboid, Meshable};
use ::bevy::render::mesh::{Indices, MeshVertexAttribute, VertexAttributeValues};
use bevy::prelude as bevy;
use itertools::Itertools;
use nalgebra::{vector, Point, Vector3};
@@ -9,19 +11,27 @@ use rustc_hash::FxHashMap;
use crate::mesh::{self, Bones, Mesh};
pub const HUMAN_MALE_BONES_JSON: &str = include_str!("../prefab/male_base-bones.json");
pub const ANACONDA_BONES_JSON: &str = include_str!("../prefab/anaconda-bones.json");
pub static ANACONDA_BONES: LazyLock<Bones> =
LazyLock::new(|| serde_json::from_str(ANACONDA_BONES_JSON).unwrap());
pub const HUMAN_MALE_BONES_JSON: &str = include_str!("../prefab/human-male-bones.json");
pub static HUMAN_MALE_BONES: LazyLock<Bones> =
LazyLock::new(|| serde_json::from_str(HUMAN_MALE_BONES_JSON).unwrap());
pub const HUMAN_FEMALE_BONES_JSON: &str = include_str!("../prefab/female_base-bones.json");
pub const HUMAN_FEMALE_BONES_JSON: &str = include_str!("../prefab/human-female-bones.json");
pub static HUMAN_FEMALE_BONES: LazyLock<Bones> =
LazyLock::new(|| serde_json::from_str(HUMAN_FEMALE_BONES_JSON).unwrap());
pub const HUMAN_MALE_STL: &[u8] = include_bytes!("../prefab/male_base_500f.stl");
pub const ANACONDA_STL: &[u8] = include_bytes!("../prefab/anaconda.stl");
pub static ANACONDA_MESH: LazyLock<Mesh> =
LazyLock::new(|| Mesh::from_stl(ANACONDA_STL, Some(0.001), None, None));
pub const HUMAN_MALE_STL: &[u8] = include_bytes!("../prefab/human-male.stl");
pub static HUMAN_MALE_MESH: LazyLock<Mesh> =
LazyLock::new(|| Mesh::from_stl(HUMAN_MALE_STL, None, None, None));
pub const HUMAN_FEMALE_STL: &[u8] = include_bytes!("../prefab/female_base_500f.stl");
pub const HUMAN_FEMALE_STL: &[u8] = include_bytes!("../prefab/human-female.stl");
pub static HUMAN_FEMALE_MESH: LazyLock<Mesh> =
LazyLock::new(|| Mesh::from_stl(HUMAN_FEMALE_STL, None, None, None));
@@ -50,6 +60,7 @@ impl Prefab {
pub const fn all() -> &'static [Prefab] {
&[Self::HumanMale,
Self::HumanFemale,
Self::Snake,
Self::Cylinder,
Self::Cube]
}
@@ -69,6 +80,16 @@ impl Prefab {
pub fn trimesh(self) -> mesh::TriMesh {
let (faces, vertices) = match self {
| Prefab::Snake => {
let msh = mesh_loader::stl::from_slice(ANACONDA_STL).unwrap();
let vertices = msh.meshes[0].vertices
.iter()
.copied()
.map(|[x, y, z]| Vector3::new(x, y, z))
.collect::<Vec<_>>();
let faces = msh.meshes[0].faces.clone();
(faces, vertices)
},
| Prefab::HumanMale => {
let msh = mesh_loader::stl::from_slice(HUMAN_MALE_STL).unwrap();
let vertices = msh.meshes[0].vertices
@@ -90,7 +111,7 @@ impl Prefab {
(faces, points)
},
| Prefab::Cylinder => {
let (points, faces) = rapier::Cylinder::new(0.5, 0.5).to_trimesh(10);
let (points, faces) = rapier::Cylinder::new(0.5, 0.5).to_trimesh(20);
let points = points.into_iter().map(|a| a.coords).collect::<Vec<_>>();
(faces, points)
},
@@ -101,17 +122,16 @@ impl Prefab {
let (points, faces) = subdiv(a);
(faces, points)
},
| Prefab::Snake => todo!(),
};
mesh::TriMesh { faces, vertices }
mesh::TriMesh::new(faces, vertices)
}
pub fn mesh(self) -> (Mesh, Bones) {
match self {
| Prefab::HumanMale => (HUMAN_MALE_MESH.clone(), HUMAN_MALE_BONES.clone()),
| Prefab::HumanFemale => (HUMAN_FEMALE_MESH.clone(), HUMAN_FEMALE_BONES.clone()),
| Prefab::Snake => todo!(),
| Prefab::Snake => (ANACONDA_MESH.clone(), ANACONDA_BONES.clone()),
| Prefab::Cylinder => {
let tri = self.trimesh();
let mesh = Mesh::from_trimesh(&tri.vertices, &tri.faces, None);
@@ -147,11 +167,16 @@ fn bevy_mesh_tris(mesh: &bevy::Mesh) -> (Vec<Vector3<f32>>, Vec<[u32; 3]>) {
unreachable!();
};
let VertexAttributeValues::Float32x3(points) = mesh.attribute(bevy::Mesh::ATTRIBUTE_POSITION).unwrap() else {
let VertexAttributeValues::Float32x3(points) =
mesh.attribute(bevy::Mesh::ATTRIBUTE_POSITION).unwrap()
else {
unreachable!();
};
let points = points.clone().into_iter().map(Vector3::from).collect::<Vec<_>>();
let points = points.clone()
.into_iter()
.map(Vector3::from)
.collect::<Vec<_>>();
let mut faces_iter = faces.clone().into_iter();
let mut faces_chunked = vec![];
while let Some(a) = faces_iter.next_array::<3>() {
@@ -161,25 +186,24 @@ fn bevy_mesh_tris(mesh: &bevy::Mesh) -> (Vec<Vector3<f32>>, Vec<[u32; 3]>) {
(points, faces_chunked)
}
fn subdiv((points, faces): (Vec<Vector3<f32>>, Vec<[u32; 3]>)) -> (Vec<Vector3<f32>>, Vec<[u32; 3]>) {
fn subdiv((points, faces): (Vec<Vector3<f32>>, Vec<[u32; 3]>))
-> (Vec<Vector3<f32>>, Vec<[u32; 3]>) {
let mut new_points = points;
let mut new_faces = Vec::new();
for ixs in faces {
let [ax,bx,cx] = ixs;
let [a,b,c] = ixs.map(|x| new_points[x as usize]);
let ab = (a + b) / 2.0;
let bc = (b + c) / 2.0;
let ca = (a + c) / 2.0;
let len = new_points.len();
let [abx,bcx,cax] = [len, len + 1, len + 2].map(|n| n as u32);
new_points.extend([ab,bc,ca]);
new_faces.extend([
[ax,abx,cax],
[abx,bx,bcx],
[bcx,cx,cax],
[cax,abx,bcx],
].into_iter());
}
let [ax, bx, cx] = ixs;
let [a, b, c] = ixs.map(|x| new_points[x as usize]);
let ab = (a + b) / 2.0;
let bc = (b + c) / 2.0;
let ca = (a + c) / 2.0;
let len = new_points.len();
let [abx, bcx, acx] = [len, len + 1, len + 2].map(|n| n as u32);
new_points.extend([ab, bc, ca]);
new_faces.extend([[ax, abx, acx],
[bx, abx, bcx],
[cx, acx, bcx],
[abx, bcx, acx]].into_iter());
}
(new_points, new_faces)
}

157
src/xpbd/builder.rs
View File

@@ -5,13 +5,23 @@ use fixed::types::I32F32;
use graph::ConstraintId;
use itertools::Itertools;
use nalgebra::Isometry3;
use rapier3d::math::Translation;
use rapier3d::na::Vector3;
use rapier3d::prelude::{RigidBodyHandle, *};
use rapier3d::prelude::{ActiveHooks,
ColliderBuilder,
ColliderHandle,
ColliderSet,
RigidBodyBuilder,
RigidBodyHandle,
RigidBodySet,
RigidBodyType};
use rustc_hash::{FxHashMap, FxHashSet};
use super::constraint::{graph, Constraint, Stiffness};
use super::{constraint, step, Body, Handle, Set};
use crate::mesh::Mesh;
use crate::either::Either;
use crate::mesh::{Mesh, TriMesh};
use crate::xpbd::step::Step;
fn vec_fixed(a: Vector3<f32>) -> Vector3<I32F32> {
a.map(I32F32::from_num)
@@ -43,11 +53,13 @@ struct Inserted {
pub(self) extra_bodies: FxHashMap<RigidBodyHandle, Vec<ColliderHandle>>,
pub(self) volume_constraints: Vec<constraint::TetrahedronVolume>,
pub(self) distance_constraints: Vec<constraint::Distance>,
pub(self) faces: Vec<[RigidBodyHandle; 3]>,
}
impl Inserted {
#[allow(clippy::too_many_arguments)]
pub(self) fn insert(mesh: &Mesh,
trimesh: &TriMesh,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
extra_dst: Vec<(Vector3<f32>,
@@ -56,9 +68,12 @@ impl Inserted {
particle_mass: f32,
collider_radius: f32,
collider_skin: Option<f32>,
linear_damping: f32,
tet_damping: f32,
dst_damping: f32)
-> Self {
let TriMesh { faces, vertices } = trimesh;
let mut me = Self::default();
let mut extra_dst = extra_dst.into_iter().map(Some).collect();
me.spawn_points(mesh,
@@ -66,12 +81,34 @@ impl Inserted {
colliders,
particle_mass,
collider_radius,
collider_skin, &mut extra_dst);
collider_skin,
linear_damping,
&mut extra_dst);
me.faces = faces.into_iter()
.map(|a| {
a.map(|x| {
let v = vertices[x as usize].map(I32F32::from_num);
if let Some((body, _)) = me.point2body.get(&v) {
*body
} else {
me.point2body.iter().min_by_key(|a| {
I32F32::from_num((a.0 - v).map(I32F32::to_num::<f32>).magnitude())
})
.unwrap()
.1
.0
}
})
})
.collect();
me.tet_constraints(mesh, tet_damping, stiffness);
me.dst_constraints(bodies, mesh, dst_damping, stiffness);
me.dst_constraints(mesh, dst_damping, stiffness);
let dropped_extra_constraints = extra_dst.iter().filter(|a| a.is_some()).count();
if dropped_extra_constraints > 0 {
eprintln!("Dropped {} extra constraints because no matching points could be found!", dropped_extra_constraints);
eprintln!("Dropped {} extra constraints because no matching points could be found!",
dropped_extra_constraints);
}
me
}
@@ -83,12 +120,14 @@ impl Inserted {
particle_mass: f32,
collider_radius: f32,
collider_skin: Option<f32>,
linear_damping: f32,
extra_constraints: &mut Vec<Option<(Vector3<f32>,
constraint::DistanceBuilder<false,
true,
true>)>>) {
constraint::DistanceBuilder<false,
true,
true>)>>) {
for point in &mesh.vertices {
let body = RigidBodyBuilder::new(RigidBodyType::Dynamic).lock_rotations()
.linear_damping(linear_damping)
.can_sleep(false)
.translation(*point)
.build();
@@ -105,26 +144,24 @@ impl Inserted {
let coll = colliders.insert_with_parent(coll, body, bodies);
self.point2body.insert(vec_fixed(*point), (body, coll));
let x_dst_dist = extra_constraints.iter().filter_map(|a| a.as_ref()).map(|(p, _)| (*p - point).magnitude()).collect::<Vec<_>>();
dbg!(x_dst_dist);
let cs =
extra_constraints
.iter_mut().filter_map(|opt| {
if let Some((p, _)) = opt {
if (*p - point).magnitude() < 1e-2 {
return opt.take();
}
}
let cs = extra_constraints.iter_mut()
.filter_map(|opt| {
if let Some((p, _)) = opt {
if (*p - point).magnitude() < 1e-2 {
return opt.take();
}
}
None
}).collect::<Vec<_>>();
None
})
.collect::<Vec<_>>();
for (_, c) in cs.into_iter() {
let colls = bodies.get(c.get_body_2()).unwrap().colliders().to_vec();
self.extra_bodies.insert(c.get_body_2(), colls);
self.distance_constraints.push(c.body_1(body).build());
}
for (_, c) in cs.into_iter() {
let colls = bodies.get(c.get_body_2()).unwrap().colliders().to_vec();
self.extra_bodies.insert(c.get_body_2(), colls);
self.distance_constraints.push(c.body_1(body).build());
}
}
}
@@ -141,12 +178,7 @@ impl Inserted {
}
}
fn dst_constraints(&mut self,
bodies: &RigidBodySet,
mesh: &Mesh,
damping: f32,
stiffness: Stiffness<f32>,
) {
fn dst_constraints(&mut self, mesh: &Mesh, damping: f32, stiffness: Stiffness<f32>) {
for (p1, p2) in &mesh.edges {
let edge = [p1, p2];
let [b1, b2] = edge.map(|p| self.point2body.get(&vec_fixed(*p)).unwrap().0);
@@ -160,7 +192,6 @@ impl Inserted {
rest_length: dist };
self.distance_constraints.push(c);
}
}
}
@@ -169,6 +200,7 @@ impl Inserted {
#[derive(Clone, Debug)]
pub struct Builder {
pub transform: Isometry3<f32>,
pub trimesh: Option<TriMesh>,
pub mesh: Option<Mesh>,
pub collider_radius: Option<f32>,
pub collider_skin: Option<f32>,
@@ -176,6 +208,7 @@ pub struct Builder {
pub substeps: Option<usize>,
pub stiffness: Option<Stiffness<f32>>,
pub extra_constraints: Vec<(Vector3<f32>, constraint::DistanceBuilder<false, true, true>)>,
pub linear_damping: Option<f32>,
pub distance_constraint_damping: Option<f32>,
pub tetrahedra_volume_constraint_damping: Option<f32>,
}
@@ -184,11 +217,13 @@ impl Default for Builder {
fn default() -> Self {
Self { transform: Isometry3::identity(),
mesh: None,
trimesh: None,
collider_radius: None,
collider_skin: None,
mass: None,
substeps: None,
stiffness: None,
linear_damping: None,
distance_constraint_damping: None,
tetrahedra_volume_constraint_damping: None,
extra_constraints: Default::default() }
@@ -229,7 +264,8 @@ impl Builder {
}
/// Add a tetrahedral mesh (**required**)
pub fn mesh(mut self, mesh: Mesh) -> Self {
pub fn mesh(mut self, trimesh: TriMesh, mesh: Mesh) -> Self {
self.trimesh = Some(trimesh);
self.mesh = Some(mesh);
self
}
@@ -248,6 +284,12 @@ impl Builder {
self
}
/// Set particle linear damping
pub fn linear_damping(mut self, d: f32) -> Self {
self.linear_damping = Some(d);
self
}
/// Set default volume constraint damping (not linear damping ex. air friction)
pub fn tetrahedra_volume_constraint_damping(mut self, d: f32) -> Self {
self.tetrahedra_volume_constraint_damping = Some(d);
@@ -326,8 +368,6 @@ impl Builder {
});
}
dbg!("post-packed constraint count", packed.iter().map(|a| a.count()).fold(0, |sum, a| a + sum));
packed
}
@@ -339,17 +379,20 @@ impl Builder {
-> Result<Handle, Error> {
let Self { transform,
mesh,
trimesh,
collider_radius,
collider_skin,
mass,
substeps,
stiffness,
extra_constraints,
linear_damping,
tetrahedra_volume_constraint_damping,
distance_constraint_damping, } = self;
let substeps = substeps.unwrap_or(100);
let trimesh = trimesh.ok_or(Error::MeshRequired)?;
let mut mesh = mesh.ok_or(Error::MeshRequired)?;
mesh.rotate(&transform.rotation);
mesh.translate(&transform.translation.vector);
@@ -359,6 +402,7 @@ impl Builder {
let stiffness = stiffness.ok_or(Error::StiffnessRequired)?;
let Stiffness::YoungPoisson(young, poisson) = stiffness;
let collider_radius = collider_radius.unwrap_or(0.02);
let linear_damping = linear_damping.unwrap_or(0.0);
let tetrahedra_volume_constraint_damping = tetrahedra_volume_constraint_damping.unwrap_or(0.0);
let distance_constraint_damping = distance_constraint_damping.unwrap_or(0.0);
@@ -366,6 +410,7 @@ impl Builder {
let dts = dt / substeps as f32;
let inserted = Inserted::insert(&mesh,
&trimesh,
bodies,
colliders,
extra_constraints,
@@ -373,6 +418,7 @@ impl Builder {
particle_mass,
collider_radius,
collider_skin,
linear_damping,
tetrahedra_volume_constraint_damping,
distance_constraint_damping);
@@ -386,29 +432,48 @@ impl Builder {
type Handle2Ix = FxHashMap<RigidBodyHandle, usize>;
type Bodies = Vec<step::Body>;
let (handle2ix, bodies): (Handle2Ix, Bodies) = handles.iter()
.copied()
.enumerate()
.map(|(ix, handle)| {
let body = bodies.get(handle).unwrap();
let body = step::Body::new(dts, body);
((handle, ix), body)
})
.collect();
let immovable = inserted.extra_bodies
.keys()
.copied()
.collect::<FxHashSet<_>>();
let (handle2ix, bodies): (Handle2Ix, Bodies) =
handles.iter()
.copied()
.enumerate()
.map(|(ix, handle)| {
let body = bodies.get(handle).unwrap();
let body = step::Body::new(dts, immovable.contains(&handle), body);
((handle, ix), body)
})
.collect();
let constraints = Self::build_constraints(inserted.distance_constraints,
inserted.volume_constraints,
&handle2ix);
let faces = inserted.faces
.iter()
.map(|faces| {
faces.map(|v| {
handles.iter()
.enumerate()
.find_map(|(ix, a)| if *a == v { Some(ix) } else { None })
.unwrap()
})
})
.collect::<Vec<_>>();
let body = Body { dt,
trimesh: faces,
dt_substep: dts,
substeps,
young,
poisson,
self_collision,
handles,
immovable: inserted.extra_bodies.keys().copied().collect(),
step_buf: bodies,
immovable,
step: Step { dt: dts, bodies },
constraints };
Ok(xpbd.insert(body))

7
src/xpbd/constraint/graph.rs
View File

@@ -80,7 +80,9 @@ impl Graph {
/// divides the constraints into a vec of vecs of constraints, where the inner vecs'
/// constraints are guaranteed to be the same priority and share no vertices.
pub fn constraints_disj(&self, priorities: &[isize]) -> Vec<Vec<ConstraintId>> {
dbg!("pre-graph constraint count", self.constraints.len(), priorities.len());
dbg!("pre-graph constraint count",
self.constraints.len(),
priorities.len());
let vertexmask = self.vertices.keys().map(|VertexId(n)| *n).collect::<Mask>();
let vertexmask0 = vertexmask.clone().zeroed();
@@ -125,7 +127,8 @@ impl Graph {
}
let sets: Vec<Vec<_>> = matchings.into_iter().map(|(_, cs, _)| cs).collect();
dbg!("post-graph constraint count", sets.iter().fold(0, |sum, a| sum + a.len()));
dbg!("post-graph constraint count",
sets.iter().fold(0, |sum, a| sum + a.len()));
sets
}
}

62
src/xpbd/constraint/impl_.rs
View File

@@ -28,6 +28,7 @@ pub enum Constraint {
Distance(distance::Single),
PackedTetrahedronVolume(tetrahedron_volume::Packed),
TetrahedronVolume(tetrahedron_volume::Single),
Contact(contact::Contact),
}
impl Constraint {
@@ -96,6 +97,7 @@ impl super::Constraint for Constraint {
| Self::Distance(a) => a.apply(step),
| Self::PackedTetrahedronVolume(a) => a.apply(step),
| Self::TetrahedronVolume(a) => a.apply(step),
| Self::Contact(a) => a.apply(step),
}
}
@@ -105,6 +107,7 @@ impl super::Constraint for Constraint {
| Self::Distance(a) => a.priority(),
| Self::PackedTetrahedronVolume(a) => a.priority(),
| Self::TetrahedronVolume(a) => a.priority(),
| Self::Contact(a) => a.priority(),
}
}
@@ -114,6 +117,7 @@ impl super::Constraint for Constraint {
| Self::Distance(a) => a.body_idxs(),
| Self::PackedTetrahedronVolume(a) => a.body_idxs(),
| Self::TetrahedronVolume(a) => a.body_idxs(),
| Self::Contact(a) => a.body_idxs(),
}
}
@@ -123,6 +127,58 @@ impl super::Constraint for Constraint {
| Self::Distance(a) => a.init(handle2ix),
| Self::PackedTetrahedronVolume(a) => a.init(handle2ix),
| Self::TetrahedronVolume(a) => a.init(handle2ix),
| Self::Contact(a) => a.init(handle2ix),
}
}
}
/// XPBD <> XPBD collision
pub mod contact {
use nalgebra::{UnitVector3, Vector3};
use super::super::Constraint;
/// A contact between a body and a face on another body
#[derive(Debug, PartialEq, Clone, Copy, serde::Serialize, serde::Deserialize)]
pub struct Contact {
/// Index of particle in [`Step.bodies`]
pub idx: usize,
/// Contact normal
pub normal: UnitVector3<f32>,
/// Point nearest to this body on the surface of the other body's mesh
pub contact_point: Vector3<f32>,
/// Index of the other body being contacted in the [`crate::xpbd::Set`]
pub other: usize,
/// Index to the face this contact (may) have penetrated on the
/// other body's trimesh
pub face: usize,
}
impl Constraint for Contact {
fn apply(&mut self, step: &mut crate::xpbd::step::Step) {
let b = &mut step.bodies[self.idx];
let vmax = 10.0; // m/s
let signed_dist = (b.next.p - self.contact_point).dot(&self.normal.into_inner());
if signed_dist >= 0.0 {
return;
}
let depth = (b.next.p - self.contact_point).magnitude();
let depth = depth.min(vmax * step.dt);
let correction = self.normal.into_inner() * depth;
b.next.p += correction;
}
fn priority(&self) -> isize {
100
}
fn body_idxs(&self) -> Vec<usize> {
vec![]
}
fn init(&mut self, _: &rustc_hash::FxHashMap<rapier3d::prelude::RigidBodyHandle, usize>) {
()
}
}
}
@@ -274,7 +330,8 @@ pub mod distance {
bodies: &[step::Body<T>; 2],
deltas: [Vector3<T>; 2])
-> [Vector3<T>; 2]
where Vector3<T>: Mul<T, Output = Vector3<T>>
where Vector3<T>: Mul<T, Output = Vector3<T>>,
<T as SimdValue>::SimdBool: core::fmt::Debug + Copy
{
let dt = T::splat(dt);
let atilde = compliance / dt.simd_powi(2);
@@ -484,7 +541,8 @@ pub mod tetrahedron_volume {
volume: T::SimdRealField,
bodies: &[step::Body<T>; 4])
-> [Vector3<T>; 4]
where Vector3<T>: Mul<T, Output = Vector3<T>>
where Vector3<T>: Mul<T, Output = Vector3<T>>,
<T as SimdValue>::SimdBool: Copy + core::fmt::Debug
{
let constraint = volume - rest_volume;

2
src/xpbd/handle.rs
View File

@@ -1,3 +1,3 @@
/// Handle to an xpbd body in an xpbd body set.
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[derive(Debug, PartialEq, Clone, Copy, serde::Serialize, serde::Deserialize)]
pub struct Handle(pub usize);

175
src/xpbd/mod.rs
View File

@@ -1,9 +1,14 @@
use core::f32;
use core::fmt::Debug;
use std::collections::BTreeMap;
pub use builder::Builder;
use constraint::Constraint;
pub use handle::Handle;
use nalgebra::Vector3;
use rapier3d::parry::bounding_volume::Aabb;
use rapier3d::parry::partitioning::Qbvh;
use rapier3d::parry::query::details::RayCompositeShapeToiAndNormalBestFirstVisitor;
use rapier3d::prelude::{RigidBodyHandle, *};
use rustc_hash::FxHashSet;
pub use set::Set;
@@ -23,42 +28,43 @@ pub struct Body {
pub substeps: usize,
pub young: f32,
pub poisson: f32,
pub trimesh: Vec<[usize; 3]>,
pub self_collision: FxHashSet<ColliderHandle>,
pub handles: Vec<RigidBodyHandle>,
pub immovable: FxHashSet<RigidBodyHandle>,
pub step_buf: Vec<step::Body>,
pub step: Step,
pub constraints: Vec<constraint::impl_::Constraint>,
}
impl Body {
/// Do a single simulation step
pub fn step(&mut self, bodies: &mut RigidBodySet) {
for ix in 0..self.handles.len() {
self.step_buf[ix] = step::Body::new(self.dt_substep, bodies.get(self.handles[ix]).unwrap());
/// Do a single simulation substep
pub fn substep(&mut self) {
let Self { constraints, step, .. } = self;
for c in constraints.iter_mut() {
c.apply(step);
}
let mut step = Step { bodies: core::mem::take(&mut self.step_buf),
dt: self.dt_substep };
step.predict_next();
for _ in 0..self.substeps {
for c in self.constraints.iter_mut() {
c.apply(&mut step);
}
self.constraints = core::mem::take(&mut self.constraints).into_iter()
.filter(|a| {
!matches!(a,
constraint::impl_::Constraint::Contact(_))
})
.collect();
}
step.bodies.iter_mut().enumerate().for_each(|(ix, b)| {
if self.immovable.contains(&self.handles[ix]) {
*b = b.immovable();
} else {
*b = b.predict_next(false, self.dt_substep)
}
});
}
self.step_buf = step.bodies;
for (ix, p) in self.step_buf.iter().enumerate() {
/// Apply simulation results to rapier environment
pub fn apply(&mut self, bodies: &mut RigidBodySet) {
for (ix, p) in self.step.bodies.iter().enumerate() {
let handle = self.handles[ix];
let rb = bodies.get_mut(handle).unwrap();
if !rb.is_dynamic() {
return;
}
let pred = rb.predict_position_using_velocity_and_forces(self.dt);
let dx = p.next.p - pred.translation.vector;
if rb.is_dynamic() {
@@ -72,4 +78,127 @@ impl Body {
}
}
}
/// Sync step bodies with rapier world positions
pub fn pre_step(&mut self, bodies: &mut RigidBodySet) {
for ix in 0..self.handles.len() {
self.step
.init_body(ix, bodies.get(self.handles[ix]).unwrap());
}
}
pub fn generate_parry_trimesh(&self) -> parry3d::shape::TriMesh {
let vertices = self.step
.bodies
.iter()
.map(|b| Point::from(b.next.p))
.collect::<Vec<_>>();
let faces = self.trimesh
.iter()
.copied()
.map(|a| a.map(|a| a as u32))
.collect();
let mut trimesh = rapier3d::prelude::TriMesh::new(vertices, faces).unwrap();
trimesh.set_flags(TriMeshFlags::ORIENTED).unwrap();
trimesh
}
pub fn generate_contacts(&mut self,
thresh: f32,
ix_of_other: usize,
other: &parry3d::shape::TriMesh) {
if !other.aabb(&Isometry::identity()).intersects(&self.aabb()) {
return;
};
let points = self.step
.bodies
.iter()
.map(|b| b.next.p)
.collect::<Vec<_>>();
for (ix, pt) in points.iter().enumerate() {
let Some((proj, (face, _))) =
other.project_local_point_and_get_location_with_max_dist(&Point::from(*pt), true, thresh)
else {
continue;
};
if !proj.is_inside {
continue;
};
let Some(normal) = other.feature_normal(FeatureId::Face(face)) else {
unreachable!()
};
self.constraints.push(
constraint::impl_::Constraint::Contact(
constraint::impl_::contact::Contact {
idx: ix,
normal,
contact_point: proj.point.coords,
other: ix_of_other,
face: face as usize,
}
),
);
}
}
pub fn update_contacts(&mut self, bodies: &[Self]) {
self.constraints
.iter_mut()
.filter_map(|c| match c {
| constraint::impl_::Constraint::Contact(c) => Some(c),
| _ => None,
})
.for_each(|contact| {
let face = bodies[contact.other].trimesh[contact.face];
let [a, b, c] = face.map(|x| Point::from(bodies[contact.other].step.bodies[x].next.p));
let tri = parry3d::shape::Triangle::new(a, b, c);
let proj =
tri.project_local_point(&Point::from(self.step.bodies[contact.idx].next.p), false);
contact.normal = tri.normal().unwrap();
contact.contact_point = proj.point.coords;
});
}
pub fn aabb(&self) -> Aabb {
let (Some(mins), Some(maxs)) = self.step
.bodies
.iter()
.fold((None, None), |(mins, maxs), b| {
let p = b.next.p;
match (mins, maxs) {
| (None, None) => (Some(p), Some(p)),
| (Some(mut mins), Some(mut maxs)) => {
if mins.x > p.x {
mins.x = p.x;
}
if mins.y > p.y {
mins.y = p.y;
}
if mins.z > p.z {
mins.z = p.z;
}
if maxs.x < p.x {
maxs.x = p.x;
}
if maxs.y < p.y {
maxs.y = p.y;
}
if maxs.z < p.z {
maxs.z = p.z;
}
(Some(mins), Some(maxs))
},
| _ => unreachable!(),
}
})
else {
unreachable!()
};
Aabb { mins: Point::from(mins),
maxs: Point::from(maxs) }
}
}

96
src/xpbd/set.rs
View File

@@ -5,6 +5,7 @@ use std::ops::Deref;
use std::ptr::NonNull;
use std::sync::{Arc, Mutex, MutexGuard};
use bevy::winit::select_monitor;
use rapier3d::prelude::*;
use super::handle::Handle;
@@ -59,8 +60,74 @@ impl Set {
/// Do a single simulation step
pub fn step(&self, bodies: &mut RigidBodySet) {
let mut bs = self.bodies.lock().unwrap();
for b in bs.iter_mut() {
b.step(bodies);
if bs.len() == 0 {
return;
}
// SAFETY
// Each body needs a mutable reference to other bodies
// to do collision detection, index check prevents
// double mutable reference to the same element, and
// elements are not moved or removed within the vec
// from under either mutable reference.
let (bsa, bsb) = unsafe {
let ptr = (&mut bs) as *mut MutexGuard<Vec<Body>>;
(ptr.as_mut().unwrap(), ptr.as_mut().unwrap())
};
// WANT
// interleaved substeps of all bodies
//
// eg. substeps for [a,b,c] is [10,50,250] where a:b and b:c is 1:5
// it should look like:
//
// a: . .
// b: . . . . . .
// c: . . . . . . . . . . . . . . . . . . . . .
//
// [7, 70, 200] - substeps (frequency)
// 1400 - least common multiple
// [200, 20, 7] - substep delays
// then for n in 0..1400, for m in delays, if n % m == 0 do substep
let substeps = bsa.iter().map(|b| b.substeps).collect::<Vec<_>>();
let substep_lcm = substeps.iter()
.skip(1)
.fold(substeps[0], |b, a| (a * b) / gcd(*a, b));
let substep_interval = substeps.iter().map(|a| substep_lcm / a).collect::<Vec<_>>();
for b in bsa.iter_mut() {
b.pre_step(bodies);
}
// At the top of the step, regenerate trimeshes and perform collision detection
// within a threshold. For each potential collision, make contact constraints.
let trimesh = bs.iter()
.map(|b| b.generate_parry_trimesh())
.collect::<Vec<_>>();
let contact_constraint_thresh = 0.1;
for (ax, ab) in bsa.iter_mut().enumerate() {
for (bx, mesh) in trimesh.iter().enumerate() {
if ax == bx {
continue;
}
ab.generate_contacts(contact_constraint_thresh, bx, mesh);
}
}
for n in 0..substep_lcm {
for (ax, ab) in bsa.iter_mut().enumerate() {
// update contact constraints with update face
// positions
ab.update_contacts(&bsb);
if n == 0 || n % substep_interval[ax] == 0 {
ab.substep();
}
}
}
for b in bsa.iter_mut() {
b.apply(bodies);
}
}
@@ -135,3 +202,28 @@ impl<'a> Iterator for Iter<'a> {
iter.next().map(|(ix, a)| (Handle(ix), a))
}
}
// https://docs.rs/num-integer/0.1.46/src/num_integer/lib.rs.html#870
fn gcd(mut m: usize, mut n: usize) -> usize {
if m == 0 || n == 0 {
return m | n;
}
// find common factors of 2
let shift = (m | n).trailing_zeros();
// divide n and m by 2 until odd
m >>= m.trailing_zeros();
n >>= n.trailing_zeros();
while m != n {
if m > n {
m -= n;
m >>= m.trailing_zeros();
} else {
n -= m;
n >>= n.trailing_zeros();
}
}
m << shift
}

113
src/xpbd/step.rs
View File

@@ -1,21 +1,87 @@
use core::fmt::Debug;
use nalgebra::{Scalar, SimdComplexField, SimdValue};
use nalgebra::{ArrayStorage, Scalar, SimdComplexField, SimdValue, Vector3};
use rapier3d::prelude::RigidBody;
use simba::simd::WideF32x8;
use simba::simd::{WideBoolF32x8, WideF32x8};
use crate::rigidbody_hack;
#[derive(Debug)]
pub const fn wide_bool(b: [bool; 8]) -> WideBoolF32x8 {
pub const fn b2f(b: bool) -> f32 {
if b {
1.0
} else {
0.0
}
}
WideBoolF32x8(wide::f32x8::new(crate::map_array!(8; b, b2f)))
}
pub const fn pack_vector(f: [Vector3<f32>; 8]) -> Vector3<WideF32x8> {
pub const fn vector_x(f: Vector3<f32>) -> f32 {
f.data.0[0][0]
}
pub const fn vector_y(f: Vector3<f32>) -> f32 {
f.data.0[0][1]
}
pub const fn vector_z(f: Vector3<f32>) -> f32 {
f.data.0[0][2]
}
let x = crate::map_array!(8; f, vector_x);
let y = crate::map_array!(8; f, vector_y);
let z = crate::map_array!(8; f, vector_z);
Vector3::from_array_storage(ArrayStorage([[WideF32x8(wide::f32x8::new(x)),
WideF32x8(wide::f32x8::new(y)),
WideF32x8(wide::f32x8::new(z))]]))
}
#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct Step {
pub dt: f32,
pub bodies: Vec<Body>,
}
impl Step {
pub fn init_body(&mut self, ix: usize, body: &RigidBody) {
let p = *body.translation();
let v = *body.linvel();
let b = &mut self.bodies[ix];
let f = unsafe {
let body = core::mem::transmute::<&RigidBody, &rigidbody_hack::RigidBody>(body);
body.forces.force
};
let init = body::State { p, v, f };
b.init = init;
b.prev = init;
b.next = init;
*b = b.predict_next(true, self.dt)
}
pub fn predict_next(&mut self) {
let Self { bodies, .. } = self;
bodies.iter_mut().for_each(|b| {
if b.immovable {
*b = b.immovable();
} else {
*b = b.predict_next(false, self.dt);
}
});
}
}
#[derive(Clone, Copy, Debug, serde::Serialize, serde::Deserialize)]
pub struct Body<T = f32> {
#[serde(bound = "T: Copy + Scalar")]
pub struct Body<T: SimdValue = f32>
where T::SimdBool: Copy + Debug
{
#[serde(bound = "T: Copy + Scalar, <T as SimdValue>::SimdBool: serde::Serialize + for<'a> serde::Deserialize<'a>")]
pub mass: T,
pub immovable: <T as SimdValue>::SimdBool,
pub inv_mass: T,
pub linear_damping: T,
pub init: body::State<T>,
@@ -26,17 +92,20 @@ pub struct Body<T = f32> {
impl Body {
pub const ZERO_F32: Body<f32> = Body { mass: 0.0,
inv_mass: 0.0,
immovable: false,
linear_damping: 0.0,
init: body::State::ZERO_F32,
prev: body::State::ZERO_F32,
next: body::State::ZERO_F32 };
pub const ZERO_F32X8: Body<WideF32x8> = Body { mass: WideF32x8::ZERO,
inv_mass: WideF32x8::ZERO,
linear_damping: WideF32x8::ZERO,
init: body::State::ZERO_F32X8,
prev: body::State::ZERO_F32X8,
next: body::State::ZERO_F32X8 };
pub const ZERO_F32X8: Body<WideF32x8> =
Body { mass: WideF32x8::ZERO,
immovable: WideBoolF32x8(wide::f32x8::new([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])),
inv_mass: WideF32x8::ZERO,
linear_damping: WideF32x8::ZERO,
init: body::State::ZERO_F32X8,
prev: body::State::ZERO_F32X8,
next: body::State::ZERO_F32X8 };
}
impl Body<f32> {
@@ -52,7 +121,7 @@ impl Body<f32> {
self.next
}
pub fn new(dt_substep: f32, body: &RigidBody) -> Self {
pub fn new(dt_substep: f32, immovable: bool, body: &RigidBody) -> Self {
let p = *body.translation();
let v = *body.linvel();
let f = unsafe {
@@ -65,6 +134,7 @@ impl Body<f32> {
let init = body::State { p, v, f };
Self { mass,
immovable,
inv_mass,
init,
linear_damping: body.linear_damping(),
@@ -72,10 +142,11 @@ impl Body<f32> {
next: init }.predict_next(true, dt_substep)
}
pub fn pack(ps: &[&Self; 8]) -> Body<WideF32x8> {
let inv_mass: WideF32x8 = WideF32x8::from(crate::map_array!(8; ps, _.inv_mass));
pub const fn pack(ps: &[&Self; 8]) -> Body<WideF32x8> {
let inv_mass: WideF32x8 = WideF32x8(wide::f32x8::new(crate::map_array!(8; ps, _.inv_mass)));
Body { mass: WideF32x8::ZERO,
immovable: wide_bool(crate::map_array!(8; ps, _.immovable)),
inv_mass,
linear_damping: WideF32x8::ZERO,
init: body::State::ZERO_F32X8,
@@ -84,7 +155,9 @@ impl Body<f32> {
}
}
impl<T: Debug + Copy + SimdComplexField + SimdValue<Element = f32>> Body<T> {
impl<T: Debug + Copy + SimdComplexField + SimdValue<Element = f32>> Body<T>
where <T as SimdValue>::SimdBool: Copy + core::fmt::Debug
{
pub fn immovable(mut self) -> Self {
self.prev = self.init;
self.next = self.init;
@@ -123,10 +196,11 @@ impl<T: Debug + Copy + SimdComplexField + SimdValue<Element = f32>> Body<T> {
pub mod body {
use nalgebra::{ArrayStorage, Scalar};
use num_traits::Zero;
use rapier3d::na::Vector3;
use simba::simd::WideF32x8;
use super::pack_vector;
#[derive(Clone, Copy, Debug, serde::Serialize, serde::Deserialize)]
pub struct State<T = f32> {
/// Position
@@ -161,12 +235,11 @@ pub mod body {
self.p
}
pub fn pack(ss: [Self; 8]) -> State<WideF32x8> {
let p: Vector3<WideF32x8> = crate::map_array!(8; ss, _.p).into();
pub const fn pack(ss: [Self; 8]) -> State<WideF32x8> {
let p = pack_vector(crate::map_array!(8; ss, _.p));
State { p,
v: Vector3::zero(),
f: Vector3::zero() }
..Self::ZERO_F32X8 }
}
}
}