use std::collections::HashMap;
use na::{Isometry2, Vector2};
use ncollide::shape::{self, Shape};
use nphysics::force_generator::DefaultForceGeneratorSet;
use nphysics::joint::DefaultJointConstraintSet;
use nphysics::material::BasicMaterial;
use nphysics::object::{
Collider, ColliderAnchor, DefaultBodyHandle, DefaultBodySet, DefaultColliderSet, RigidBody,
};
use nphysics::world::DefaultMechanicalWorld;
use std::f32;
use wrapped2d::b2;
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>,
nphysics2box2d: HashMap<DefaultBodyHandle, b2::BodyHandle>,
}
impl Box2dWorld {
pub fn from_nphysics(
mechanical_world: &DefaultMechanicalWorld<f32>,
bodies: &DefaultBodySet<f32>,
colliders: &DefaultColliderSet<f32>,
_joint_constraints: &DefaultJointConstraintSet<f32>,
_force_generators: &DefaultForceGeneratorSet<f32>,
) -> Self {
let world = b2::World::new(&na_vec_to_b2_vec(&mechanical_world.gravity));
let mut res = Box2dWorld {
world,
nphysics2box2d: HashMap::new(),
};
res.insert_bodies(bodies, colliders);
res
}
fn insert_bodies(&mut self, bodies: &DefaultBodySet<f32>, colliders: &DefaultColliderSet<f32>) {
for (handle, body) in bodies.iter() {
let part = body.part(0).unwrap();
let pos = part.position();
let vel = part.velocity();
let body_type = if body.is_static() {
b2::BodyType::Static
} else {
b2::BodyType::Dynamic
};
let linear_damping;
let angular_damping;
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(&pos.translation.vector),
angle: pos.rotation.angle(),
linear_velocity: na_vec_to_b2_vec(&vel.linear),
angular_velocity: vel.angular,
linear_damping,
angular_damping,
..b2::BodyDef::new()
};
let b2_handle = self.world.create_body(&def);
self.nphysics2box2d.insert(handle, b2_handle);
}
for (_, collider) in colliders.iter() {
match collider.anchor() {
ColliderAnchor::OnBodyPart { body_part, .. } => {
if let Some(b2_handle) = self.nphysics2box2d.get(&body_part.0) {
let mut b2_body = self.world.body_mut(*b2_handle);
if collider.is_ccd_enabled() {
b2_body.set_bullet(true);
}
Self::create_fixtures(
collider,
collider.shape(),
&collider.position_wrt_body(),
&mut *b2_body,
);
}
}
ColliderAnchor::OnDeformableBody { .. } => {
println!("Deformable bodies are not supported by Box2D.")
}
}
}
}
fn create_fixtures(
collider: &Collider<f32, DefaultBodyHandle>,
shape: &dyn Shape<f32>,
dpos: &Isometry2<f32>,
body: &mut b2::MetaBody<NoUserData>,
) {
let center = na_vec_to_b2_vec(&dpos.translation.vector);
let mut fixture_def = b2::FixtureDef::new();
fixture_def.restitution = 0.0;
fixture_def.friction = 0.5;
fixture_def.density = collider.density();
fixture_def.is_sensor = collider.is_sensor();
fixture_def.filter = b2::Filter::new();
if let Some(material) = collider.material().downcast_ref::<BasicMaterial<f32>>() {
fixture_def.restitution = material.restitution;
fixture_def.friction = material.friction;
}
if let Some(_s) = shape.as_shape::<shape::Plane<f32>>() {
} else if let Some(s) = shape.as_shape::<shape::Ball<f32>>() {
let mut b2_shape = b2::CircleShape::new();
b2_shape.set_radius(s.radius());
b2_shape.set_position(center);
body.create_fixture(&b2_shape, &mut fixture_def);
} else if let Some(s) = shape.as_shape::<shape::Cuboid<f32>>() {
let half_extents = s.half_extents();
let b2_shape = b2::PolygonShape::new_oriented_box(
half_extents.x,
half_extents.y,
¢er,
dpos.rotation.angle(),
);
body.create_fixture(&b2_shape, &mut fixture_def);
} else if let Some(_s) = shape.as_shape::<shape::Capsule<f32>>() {
} else if let Some(cp) = shape.as_shape::<shape::Compound<f32>>() {
for (shift, shape) in cp.shapes() {
Self::create_fixtures(collider, &**shape, &(dpos * shift), body)
}
} else if let Some(ch) = shape.as_shape::<shape::ConvexPolygon<f32>>() {
let points: Vec<_> = ch
.points()
.iter()
.map(|p| dpos * 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(_s) = shape.as_shape::<shape::Polyline<f32>>() {
} else if let Some(_s) = shape.as_shape::<shape::HeightField<f32>>() {
}
}
pub fn step(&mut self, mechanical_world: &mut DefaultMechanicalWorld<f32>) {
self.world
.set_continuous_physics(mechanical_world.integration_parameters.max_ccd_substeps != 0);
mechanical_world.counters.step_started();
self.world.step(
mechanical_world.integration_parameters.dt(),
mechanical_world
.integration_parameters
.max_velocity_iterations as i32,
mechanical_world
.integration_parameters
.max_position_iterations as i32,
);
mechanical_world.counters.step_completed();
}
pub fn sync(&self, _bodies: &mut DefaultBodySet<f32>, colliders: &mut DefaultColliderSet<f32>) {
for (_, collider) in colliders.iter_mut() {
match collider.anchor() {
ColliderAnchor::OnBodyPart {
body_part,
position_wrt_body_part,
} => {
if let Some(pb2_handle) = self.nphysics2box2d.get(&body_part.0) {
let body = self.world.body(*pb2_handle);
let pos = b2_transform_to_na_isometry(body.transform());
let new_pos = pos * position_wrt_body_part;
collider.set_position(new_pos)
}
}
ColliderAnchor::OnDeformableBody { .. } => {}
}
}
}
}