use ncollide3d::shape::*;
use ncollide3d::bounding_volume::{ AABB, BoundingSphere, HasBoundingVolume };
use rin_math::{
Isometry3, Vec3, one, ToVec, ToPnt, vec3, Deg, zero, Swizzles3, Angle
};
use rin_graphics::{Vertex, Mesh,};
use super::{ Shape, Offset, rinmesh_to_trimesh };
use serde_derive::{Deserialize, Serialize};
#[derive(Clone,Debug,Copy, Serialize, Deserialize)]
#[repr(u16)]
pub enum RigidBodyType{
Active=0,
Passive,
}
#[derive(Clone,Debug,Copy, Serialize, Deserialize)]
#[repr(u16)]
pub enum RigidBodyShape{
Cuboid,
Sphere,
Capsule,
Cylinder,
Cone,
ConvexHull,
Mesh,
}
#[derive(Clone,Debug,Copy, Serialize, Deserialize)]
#[repr(C)]
pub struct RigidBody{
pub ty: RigidBodyType,
pub shape: RigidBodyShape,
pub friction: f32,
}
pub fn geometry_aabb<'a, I, V>(vertices: I) -> Option<AABB<f32>>
where
I: IntoIterator<Item = &'a V>,
V: Vertex + 'a,
<V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let mut vertices = vertices.into_iter();
vertices.next().map(|first| {
let mut mins = first.position().xyz();
let mut maxs = first.position().xyz();
for p in vertices.map(|v| v.position().xyz()){
mins.x = mins.x.min(p.x);
mins.y = mins.y.min(p.y);
mins.z = mins.z.min(p.z);
maxs.x = maxs.x.max(p.x);
maxs.y = maxs.y.max(p.y);
maxs.z = maxs.z.max(p.z);
}
AABB::new(mins.to_pnt(), maxs.to_pnt())
})
}
pub fn geometry_bounding_sphere<V: Vertex>(mesh: &Mesh<V>) -> Option<BoundingSphere<f32>>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
if mesh.is_empty() {
return None
}
let acc: Vec3 = mesh.vertices().iter()
.map(|v| v.position().xyz())
.sum();
let centroid = acc / mesh.vertices().len() as f32;
let first = mesh.vertices()[0].position().xyz();
let distance_sq = (first - centroid).norm_squared();
let (furthest, _) = mesh.iter()
.skip(1)
.fold((first, distance_sq), |(furthest, max_distance_sq), v| {
let v = v.position().xyz();
let distance_sq = (v - centroid).norm_squared();
if distance_sq > max_distance_sq {
(v, distance_sq)
}else{
(furthest, max_distance_sq)
}
});
let radius = (furthest - centroid).norm();
Some(BoundingSphere::new(centroid.to_pnt(), radius))
}
pub fn aabb_to_box_and_offset(aabb: &AABB<f32>) -> (Shape, Offset) {
let cuboid = Cuboid::new(aabb.half_extents());
let offset = Isometry3::new(aabb.center().to_vec(), zero());
(Shape::new(cuboid), Offset(offset))
}
pub fn geometry_box_and_offset<V: Vertex>(mesh: &Mesh<V>) -> Option<(Shape, Offset)>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let aabb = geometry_aabb(mesh.vertices())?;
Some(aabb_to_box_and_offset(&aabb))
}
pub fn aabb_to_capsule_and_offset(aabb: &AABB<f32>) -> (Shape, Offset) {
let radius = aabb.half_extents().x.max(aabb.half_extents().y);
let half_height = aabb.half_extents().z;
let center = aabb.center();
let capsule = Capsule::new(half_height, radius);
let offset = Isometry3::new(
center.to_vec(),
vec3(Deg(90.).to_rad().value(), 0.,0.)
);
(Shape::new(capsule), Offset(offset))
}
pub fn geometry_capsule_and_offset<V: Vertex>(mesh: &Mesh<V>) -> Option<(Shape, Offset)>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let aabb = geometry_aabb(mesh.vertices())?;
Some(aabb_to_capsule_and_offset(&aabb))
}
pub fn aabb_to_sphere_and_offset(aabb: &AABB<f32>) -> (Shape, Offset) {
let cuboid = Cuboid::new(aabb.half_extents());
let bounding_sphere: BoundingSphere<f32> = cuboid.local_bounding_volume();
let radius = bounding_sphere.radius();
let ball = Ball::new(radius);
let center = bounding_sphere.center();
let offset = Isometry3::new(
center.to_vec(),
vec3(Deg(90.).to_rad().value(), 0.,0.)
);
(Shape::new(ball), Offset(offset))
}
pub fn geometry_sphere_and_offset<V: Vertex>(mesh: &Mesh<V>) -> Option<(Shape, Offset)>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let bounding_sphere = geometry_bounding_sphere(mesh)?;
let radius = bounding_sphere.radius();
let ball = Ball::new(radius);
let center = bounding_sphere.center();
let offset = Isometry3::new(
center.to_vec(),
vec3(0., 0.,0.)
);
Some((Shape::new(ball), Offset(offset)))
}
pub fn geometry_trimesh_and_offset<V: Vertex>(mesh: &Mesh<V>) -> Option<(Shape, Offset)>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let trimesh = rinmesh_to_trimesh(mesh, vec3!(1.));
Some((Shape::new(trimesh), Offset(one())))
}
pub fn geometry_convexhull_and_offset<V: Vertex>(mesh: &Mesh<V>) -> Option<(Shape, Offset)>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let points = mesh.vertices()
.iter()
.map(|v| v.position().xzy().to_pnt())
.collect();
let indices = mesh.indices()
.iter()
.map(|i| *i as usize)
.collect::<Vec<_>>();
let convex_hull = ConvexHull::try_new(points, &indices)?;
Some((Shape::new(convex_hull), Offset(one())))
}
pub fn geometry_shape_and_offset<V: Vertex>(mesh: &Mesh<V>, shape: RigidBodyShape) -> Option<(Shape, Offset)>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
match shape {
RigidBodyShape::Capsule => geometry_capsule_and_offset(mesh),
RigidBodyShape::ConvexHull => geometry_convexhull_and_offset(mesh),
RigidBodyShape::Cuboid => geometry_box_and_offset(mesh),
RigidBodyShape::Mesh => geometry_trimesh_and_offset(mesh),
RigidBodyShape::Sphere => geometry_sphere_and_offset(mesh),
RigidBodyShape::Cone => unimplemented!("Cone shape not supported yet"),
RigidBodyShape::Cylinder => unimplemented!("Cylinder shape not supported yet"),
}
}