251 lines
9.3 KiB
Rust
251 lines
9.3 KiB
Rust
use std::collections::HashMap;
|
|
|
|
use na::{Isometry2, Vector2};
|
|
use rapier::counters::Counters;
|
|
use rapier::dynamics::{ImpulseJointSet, IntegrationParameters, RigidBodyHandle, RigidBodySet};
|
|
use rapier::geometry::{Collider, ColliderSet};
|
|
use std::f32;
|
|
|
|
use wrapped2d::b2;
|
|
// use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef};
|
|
use wrapped2d::user_data::NoUserData;
|
|
|
|
fn na_vec_to_b2_vec(v: Vector2<f32>) -> b2::Vec2 {
|
|
b2::Vec2 { x: v.x, y: v.y }
|
|
}
|
|
|
|
fn b2_vec_to_na_vec(v: b2::Vec2) -> Vector2<f32> {
|
|
Vector2::new(v.x, v.y)
|
|
}
|
|
|
|
fn b2_transform_to_na_isometry(v: b2::Transform) -> Isometry2<f32> {
|
|
Isometry2::new(b2_vec_to_na_vec(v.pos), v.rot.angle())
|
|
}
|
|
|
|
pub struct Box2dWorld {
|
|
world: b2::World<NoUserData>,
|
|
rapier2box2d: HashMap<RigidBodyHandle, b2::BodyHandle>,
|
|
}
|
|
|
|
impl Box2dWorld {
|
|
#[profiling::function]
|
|
pub fn from_rapier(
|
|
gravity: Vector2<f32>,
|
|
bodies: &RigidBodySet,
|
|
colliders: &ColliderSet,
|
|
impulse_joints: &ImpulseJointSet,
|
|
) -> Self {
|
|
let mut world = b2::World::new(&na_vec_to_b2_vec(gravity));
|
|
world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled()));
|
|
|
|
let mut res = Box2dWorld {
|
|
world,
|
|
rapier2box2d: HashMap::new(),
|
|
};
|
|
|
|
res.insert_bodies(bodies);
|
|
res.insert_colliders(colliders);
|
|
res.insert_joints(impulse_joints);
|
|
res
|
|
}
|
|
|
|
fn insert_bodies(&mut self, bodies: &RigidBodySet) {
|
|
for (handle, body) in bodies.iter() {
|
|
let body_type = if !body.is_dynamic() {
|
|
b2::BodyType::Static
|
|
} else {
|
|
b2::BodyType::Dynamic
|
|
};
|
|
|
|
let linear_damping = 0.0;
|
|
let angular_damping = 0.0;
|
|
|
|
// if let Some(rb) = body.downcast_ref::<RigidBody<f32>>() {
|
|
// linear_damping = rb.linear_damping();
|
|
// angular_damping = rb.angular_damping();
|
|
// } else {
|
|
// linear_damping = 0.0;
|
|
// angular_damping = 0.0;
|
|
// }
|
|
|
|
let def = b2::BodyDef {
|
|
body_type,
|
|
position: na_vec_to_b2_vec(body.position().translation.vector),
|
|
angle: body.position().rotation.angle(),
|
|
linear_velocity: na_vec_to_b2_vec(*body.linvel()),
|
|
angular_velocity: body.angvel(),
|
|
linear_damping,
|
|
angular_damping,
|
|
bullet: body.is_ccd_enabled(),
|
|
..b2::BodyDef::new()
|
|
};
|
|
let b2_handle = self.world.create_body(&def);
|
|
self.rapier2box2d.insert(handle, b2_handle);
|
|
}
|
|
}
|
|
|
|
fn insert_colliders(&mut self, colliders: &ColliderSet) {
|
|
for (_, collider) in colliders.iter() {
|
|
if let Some(parent) = collider.parent() {
|
|
let b2_body_handle = self.rapier2box2d[&parent];
|
|
let mut b2_body = self.world.body_mut(b2_body_handle);
|
|
Self::create_fixture(&collider, &mut *b2_body);
|
|
}
|
|
}
|
|
}
|
|
|
|
fn insert_joints(&mut self, _impulse_joints: &ImpulseJointSet) {
|
|
/*
|
|
for joint in impulse_joints.iter() {
|
|
let body_a = self.rapier2box2d[&joint.1.body1];
|
|
let body_b = self.rapier2box2d[&joint.1.body2];
|
|
|
|
match &joint.1.params {
|
|
JointParams::BallJoint(params) => {
|
|
let def = RevoluteJointDef {
|
|
body_a,
|
|
body_b,
|
|
collide_connected: true,
|
|
local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords),
|
|
local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords),
|
|
reference_angle: 0.0,
|
|
enable_limit: false,
|
|
lower_angle: 0.0,
|
|
upper_angle: 0.0,
|
|
enable_motor: false,
|
|
motor_speed: 0.0,
|
|
max_motor_torque: 0.0,
|
|
};
|
|
|
|
self.world.create_joint(&def);
|
|
}
|
|
JointParams::FixedJoint(params) => {
|
|
let def = WeldJointDef {
|
|
body_a,
|
|
body_b,
|
|
collide_connected: true,
|
|
local_anchor_a: na_vec_to_b2_vec(params.local_frame1.translation.vector),
|
|
local_anchor_b: na_vec_to_b2_vec(params.local_frame2.translation.vector),
|
|
reference_angle: 0.0,
|
|
frequency: 0.0,
|
|
damping_ratio: 0.0,
|
|
};
|
|
|
|
self.world.create_joint(&def);
|
|
}
|
|
JointParams::PrismaticJoint(params) => {
|
|
let def = PrismaticJointDef {
|
|
body_a,
|
|
body_b,
|
|
collide_connected: true,
|
|
local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords),
|
|
local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords),
|
|
local_axis_a: na_vec_to_b2_vec(params.local_axis1().into_inner()),
|
|
reference_angle: 0.0,
|
|
enable_limit: params.limits_enabled,
|
|
lower_translation: params.limits[0],
|
|
upper_translation: params.limits[1],
|
|
enable_motor: false,
|
|
max_motor_force: 0.0,
|
|
motor_speed: 0.0,
|
|
};
|
|
|
|
self.world.create_joint(&def);
|
|
}
|
|
}
|
|
}
|
|
|
|
*/
|
|
}
|
|
|
|
fn create_fixture(collider: &Collider, body: &mut b2::MetaBody<NoUserData>) {
|
|
let center = na_vec_to_b2_vec(
|
|
collider
|
|
.position_wrt_parent()
|
|
.copied()
|
|
.unwrap_or(na::one())
|
|
.translation
|
|
.vector,
|
|
);
|
|
let mut fixture_def = b2::FixtureDef::new();
|
|
|
|
fixture_def.restitution = collider.material().restitution;
|
|
fixture_def.friction = collider.material().friction;
|
|
fixture_def.density = collider.density();
|
|
fixture_def.is_sensor = collider.is_sensor();
|
|
fixture_def.filter = b2::Filter::new();
|
|
|
|
let shape = collider.shape();
|
|
|
|
if let Some(b) = shape.as_ball() {
|
|
let mut b2_shape = b2::CircleShape::new();
|
|
b2_shape.set_radius(b.radius);
|
|
b2_shape.set_position(center);
|
|
body.create_fixture(&b2_shape, &mut fixture_def);
|
|
} else if let Some(p) = shape.as_convex_polygon() {
|
|
let vertices: Vec<_> = p
|
|
.points()
|
|
.iter()
|
|
.map(|p| na_vec_to_b2_vec(p.coords))
|
|
.collect();
|
|
let b2_shape = b2::PolygonShape::new_with(&vertices);
|
|
body.create_fixture(&b2_shape, &mut fixture_def);
|
|
} else if let Some(c) = shape.as_cuboid() {
|
|
let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
|
|
body.create_fixture(&b2_shape, &mut fixture_def);
|
|
// } else if let Some(polygon) = shape.as_polygon() {
|
|
// let points: Vec<_> = poly
|
|
// .vertices()
|
|
// .iter()
|
|
// .map(|p| collider.position_wrt_parent() * p)
|
|
// .map(|p| na_vec_to_b2_vec(p.coords))
|
|
// .collect();
|
|
// let b2_shape = b2::PolygonShape::new_with(&points);
|
|
// body.create_fixture(&b2_shape, &mut fixture_def);
|
|
} else if let Some(heightfield) = shape.as_heightfield() {
|
|
let mut segments = heightfield.segments();
|
|
let seg1 = segments.next().unwrap();
|
|
let mut vertices = vec![
|
|
na_vec_to_b2_vec(seg1.a.coords),
|
|
na_vec_to_b2_vec(seg1.b.coords),
|
|
];
|
|
|
|
// TODO: this will not handle holes properly.
|
|
segments.for_each(|seg| {
|
|
vertices.push(na_vec_to_b2_vec(seg.b.coords));
|
|
});
|
|
|
|
let b2_shape = b2::ChainShape::new_chain(&vertices);
|
|
body.create_fixture(&b2_shape, &mut fixture_def);
|
|
} else {
|
|
eprintln!("Creating a shape unknown to the Box2d backend.");
|
|
}
|
|
}
|
|
|
|
#[profiling::function]
|
|
pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) {
|
|
counters.step_started();
|
|
self.world
|
|
.step(params.dt, params.num_solver_iterations.get() as i32, 1);
|
|
counters.step_completed();
|
|
}
|
|
|
|
#[profiling::function]
|
|
pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) {
|
|
for (handle, body) in bodies.iter_mut() {
|
|
if let Some(pb2_handle) = self.rapier2box2d.get(&handle) {
|
|
let b2_body = self.world.body(*pb2_handle);
|
|
let pos = b2_transform_to_na_isometry(b2_body.transform().clone());
|
|
body.set_position(pos, false);
|
|
|
|
for coll_handle in body.colliders() {
|
|
let collider = &mut colliders[*coll_handle];
|
|
collider.set_position(
|
|
pos * collider.position_wrt_parent().copied().unwrap_or(na::one()),
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|