pub mod rigidbody;
use ncollide3d::world::CollisionWorld;
use ncollide3d::pipeline::{
CollisionObjectSlabHandle, GeometricQueryType, CollisionObject, CollisionObjects, CollisionGroups
};
use ncollide3d::shape::*;
use ncollide3d::procedural;
use ncollide3d::query::{ContactManifold, RayIntersection};
use ncollide3d::narrow_phase::ContactAlgorithm;
#[cfg(feature="blender")]
use rinblender;
use rinecs::{self, Component, EntitiesCreationExt, EntitiesExt, Entity, Has, HasOr, Read, Write};
use rin_math::{
Isometry3, Pnt2, Pnt3, Mat4, Rect, Vec3, vec4, one, pnt3, Translation, ToVec, ToPnt, RealField,
Swizzles3
};
#[cfg(feature="blender")]
use rin_math::{Vec4};
use rin_graphics::{
self, CameraExt, Vertex, Mesh, vertex3d, Mesh3D, IndexT, Node,
node::DynamicTransformation
};
use rin_util::{Error, Result};
use std::marker::PhantomData;
use smallvec::SmallVec;
use std::cmp::Ordering;
use std::fmt::{self, Debug};
#[cfg(feature="blender")]
use crate::skinning::SkeletonRef;
#[cfg(feature="blender")]
use crate::geometry::AnimatedGeometry;
use crate::transformation::Bone;
#[cfg(feature="blender")]
use serde::{Serialize, Deserialize};
use crate::DeferredScene;
use itertools::Itertools;
pub use ncollide3d::query::Ray;
pub struct Physics;
impl crate::Bundle for Physics{
type Parameters = ();
fn parameters(&self) -> Option<&Self::Parameters> {
None
}
fn name(&self) -> &str{
""
}
fn setup(self, _world: &mut DeferredScene){
}
}
#[derive(Component, Clone)]
#[debug_as_string]
pub struct Shape(pub ShapeHandle<f32>);
impl Shape {
pub fn new<S: ncollide3d::shape::Shape<f32>>(shape: S) -> Shape {
Shape( ShapeHandle::new(shape) )
}
}
impl Debug for Shape{
fn fmt(&self, fmt: &mut fmt::Formatter) -> fmt::Result{
fmt.debug_tuple("mutiny::physics::Shape")
.finish()
}
}
#[derive(Component, Clone, Copy, Debug)]
#[debug_as_string]
pub struct Offset(pub Isometry3);
#[derive(Component, Clone, Copy, Debug)]
#[debug_as_string]
pub struct CollisionHandle(pub CollisionObjectSlabHandle);
#[cfg(feature="gl")]
pub mod gpu{
use rin_graphics as graphics;
use rin_gl as gl;
use rinecs::Component;
#[derive(Component, Debug)]
#[debug_as_string]
pub struct DebugGeometry(pub gl::SimpleVao<graphics::Vertex3D>);
}
pub trait MouseGroup{
fn mouse_group() -> Self;
}
pub struct Collisions<Group: Into<usize>, UserData = rinecs::Entity>{
pub world: CollisionWorld<f32, UserData>,
pub contacts_query: GeometricQueryType<f32>,
_marker: PhantomData<Group>
}
unsafe impl<Group: Into<usize>> Send for Collisions<Group>{}
pub fn ray_from_screen(screen_pos: &Pnt2, camera_pos: &Pnt3, viewport: &Rect<i32>, proj_view: &Mat4) -> Ray<f32>{
let x = 2.0f32 * (screen_pos.x - viewport.pos.x as f32) / viewport.width as f32 - 1.0f32;
let y = 1.0f32 - 2.0f32 * (screen_pos.y - viewport.pos.y as f32) / viewport.height as f32;
let inv_proj_view = proj_view.try_inverse().unwrap();
let p0 = inv_proj_view * vec4(x, y, -1., 1.);
let p1 = inv_proj_view * vec4(x, y, 1., 1.);
let ray_dir = p1.xyz() / p1.w - p0.xyz() / p0.w;
Ray::new(*camera_pos, ray_dir)
}
pub fn ray_from_screen_reversed_z(screen_pos: &Pnt2, camera_pos: &Pnt3, viewport: &Rect<i32>, proj_view: &Mat4) -> Ray<f32>{
let x = 2.0f32 * (screen_pos.x - viewport.pos.x as f32) / viewport.width as f32 - 1.0f32;
let y = 1.0f32 - 2.0f32 * (screen_pos.y - viewport.pos.y as f32) / viewport.height as f32;
let inv_proj_view = proj_view.try_inverse().unwrap();
let p0 = inv_proj_view * vec4(x, y, 1., 1.);
let p1 = inv_proj_view * vec4(x, y, - 1., 1.);
let ray_dir = p1.xyz() / p1.w - p0.xyz() / p0.w;
Ray::new(*camera_pos, ray_dir)
}
pub fn ray_from_screen_camera(screen_pos: &Pnt2, viewport: &Rect<i32>, camera: &dyn CameraExt) -> Ray<f32>{
if camera.reversed_z(){
ray_from_screen_reversed_z(screen_pos, &camera.position(), viewport, &camera.projection_view())
}else{
ray_from_screen(screen_pos, &camera.position(), viewport, &camera.projection_view())
}
}
pub fn ray_shape_intersection<T, S>(ray: &Ray<T>, m: &Isometry3<T>, shape: &S, max_toi: T) -> Option<Pnt3<T>>
where
T: RealField,
S: ncollide3d::query::RayCast<T>
{
shape.toi_with_ray(&one(), &ray, max_toi, true).map(|toi| ray.origin + ray.dir * toi)
}
impl<Group: Into<usize> + Copy, UserData> Collisions<Group, UserData>{
pub fn new() -> Collisions<Group, UserData>{
let world = CollisionWorld::new(0.02);
let contacts_query = GeometricQueryType::Contacts(0.0, 0.0);
Collisions{
world,
contacts_query,
_marker: PhantomData
}
}
pub fn add(&mut self,
group: Group,
whitelist: &[Group],
blacklist: &[Group],
data: UserData,
shape: ShapeHandle<f32>,
iso: Isometry3) -> (CollisionObjectSlabHandle, &mut CollisionObject<f32, UserData>)
{
let mut groups = CollisionGroups::new();
groups.set_membership(&[group.into()]);
for &group in whitelist {
groups.modify_whitelist(group.into(), true)
}
for &group in blacklist {
groups.modify_blacklist(group.into(), true)
}
self.world.add(iso, shape, groups, self.contacts_query, data)
}
pub fn remove(&mut self, handles: &[CollisionObjectSlabHandle]) {
self.world.remove(handles)
}
pub fn update(&mut self){
self.world.update();
}
pub fn contacts(&self) -> impl Iterator<Item =
(CollisionObjectSlabHandle, CollisionObjectSlabHandle, &ContactAlgorithm<f32>, &ContactManifold<f32>)>
{
self.world.contact_pairs(true)
}
pub fn set_position(&mut self, handle: CollisionObjectSlabHandle, iso: Isometry3){
self.world.get_mut(handle).unwrap().set_position(iso);
}
pub fn objects(&self) -> CollisionObjects<f32, UserData>{
self.world.collision_objects()
}
pub fn object(&self, handle: CollisionObjectSlabHandle) -> Option<&CollisionObject<f32, UserData>>{
self.world.collision_object(handle)
}
pub fn object_mut(&mut self, handle: CollisionObjectSlabHandle) -> Option<&mut CollisionObject<f32, UserData>>{
self.world.get_mut(handle)
}
pub fn ray_intersection(
&self,
ray_group: Group,
whitelist: &[Group],
blacklist: &[Group],
ray: &Ray<f32>,
max_toi: f32) -> Option<(CollisionObjectSlabHandle, &CollisionObject<f32, UserData>, RayIntersection<f32>)>
{
let mut collision_groups = CollisionGroups::new();
collision_groups.set_membership(&[ray_group.into()]);
for &group in whitelist {
collision_groups.modify_whitelist(group.into(), true);
}
for &group in blacklist {
collision_groups.modify_blacklist(group.into(), true);
}
let ray_collisions = self.world.interferences_with_ray(&ray, max_toi, &collision_groups);
ray_collisions.min_by_key(|c| NonNan::new(c.2.toi).unwrap())
}
pub fn ray_intersections(
&self,
ray_group: Group,
whitelist: &[Group],
blacklist: &[Group],
ray: &Ray<f32>,
max_toi: f32) -> impl Iterator<Item = (CollisionObjectSlabHandle, &CollisionObject<f32, UserData>, RayIntersection<f32>)>
{
let mut collision_groups = CollisionGroups::new();
collision_groups.set_membership(&[ray_group.into()]);
for &group in whitelist {
collision_groups.modify_whitelist(group.into(), true);
}
for &group in blacklist {
collision_groups.modify_blacklist(group.into(), true);
}
let ray_collisions = self.world.interferences_with_ray(&ray, max_toi, &collision_groups);
ray_collisions.sorted_by_key(|c| NonNan::new(c.2.toi).unwrap())
}
}
impl<Group: Into<usize> + Copy + MouseGroup, UserData> Collisions<Group, UserData>{
pub fn mouse_select(
&self,
whitelist: &[Group],
blacklist: &[Group],
mouse: &Pnt2,
max_toi: f32,
viewport: &Rect<i32>,
camera: &dyn CameraExt) -> Option<&UserData>
{
self.mouse_intersection(whitelist, blacklist, mouse, max_toi, viewport, camera)
.map(|(_, _, obj, _intersection)| obj.data())
}
pub fn mouse_select_all(
&self,
whitelist: &[Group],
blacklist: &[Group],
mouse: &Pnt2,
max_toi: f32,
viewport: &Rect<i32>,
camera: &dyn CameraExt) -> impl Iterator<Item = &UserData>
{
self.mouse_intersections(whitelist, blacklist, mouse, max_toi, viewport, camera)
.map(|(_, _, obj, _intersection)| obj.data())
}
pub fn mouse_intersections(
&self,
whitelist: &[Group],
blacklist: &[Group],
mouse: &Pnt2,
max_toi: f32,
viewport: &Rect<i32>,
camera: &dyn CameraExt) -> impl Iterator<Item = (Ray<f32>, CollisionObjectSlabHandle, &CollisionObject<f32, UserData>, RayIntersection<f32>)>
{
let ray = ray_from_screen_camera(mouse, viewport, camera);
self.ray_intersections(Group::mouse_group(), whitelist, blacklist, &ray, max_toi)
.map(move |(handle, obj, intersection)| (ray, handle, obj, intersection))
}
pub fn mouse_intersection(
&self,
whitelist: &[Group],
blacklist: &[Group],
mouse: &Pnt2,
max_toi: f32,
viewport: &Rect<i32>,
camera: &dyn CameraExt) -> Option<(Ray<f32>, CollisionObjectSlabHandle, &CollisionObject<f32, UserData>, RayIntersection<f32>)>
{
let ray = ray_from_screen_camera(mouse, viewport, camera);
self.ray_intersection(Group::mouse_group(), whitelist, blacklist, &ray, max_toi)
.map(|(handle, obj, intersection)| (ray, handle, obj, intersection))
}
}
impl<Group: Into<usize> + Copy, UserData: From<rinecs::Entity>> Collisions<Group, UserData>{
pub fn add_entity<'a, C: EntitiesCreationExt<'a>>(&mut self,
group: Group,
whitelist: &[Group],
blacklist: &[Group],
world: &mut C,
entity: Entity,
shape: ShapeHandle<f32>,
iso: Isometry3) -> CollisionObjectSlabHandle
{
let (handle, object) = self.add(group, whitelist, blacklist, entity.into(), shape, iso);
world.add_component_to(&entity, CollisionHandle(handle));
handle
}
pub fn remove_entity<'a, C: EntitiesExt<'a>>(&mut self, entity: &Entity, entities: &C){
let handle = entities.component_for::<CollisionHandle>(entity).unwrap();
log::debug!("Removing handle {:?} for entity {}", *handle, entity.guid());
self.remove(&[handle.0]);
}
pub fn change_shape<'a, E: EntitiesExt<'a>, I: Into<Option<Isometry3>>>(
&mut self,
entity: &Entity,
shape: ShapeHandle<f32>,
iso: I,
entities: &E) -> Result<()>
{
let mut handle = entities.component_for_mut::<CollisionHandle>(entity)
.ok_or_else(|| Error::new("Can't change collisions shape, couldn't find entity"))?;
let current_obj = self.world.collision_object(handle.0)
.ok_or_else(|| Error::new("Couldn't find entity handle in collisions world"))?;
let iso = iso.into().unwrap_or(*current_obj.position());
let groups = *current_obj.collision_groups();
self.world.remove(&[handle.0]);
let (slab_handle, new_object) = self.world.add(
iso,
shape,
groups,
self.contacts_query,
(*entity).into());
handle.0 = slab_handle;
Ok(())
}
#[cfg(feature="blender")]
fn add_character<C: CreationContext, V: Vertex<Position = Vec4> + Serialize + Deserialize<'static> + 'static>(&mut self,
group: Group,
whitelist: &[Group],
blacklist: &[Group],
world: &mut C,
character_entity: &Entity,
character_models: &[Entity]) -> CollisionObjectSlabHandle
{
let (mins, maxs) = {
let character_models = world.iter_for_entities::<(
Read<Node>,
Read<AnimatedGeometry<V>>,
Read<SkeletonRef>,
), _>(character_models.iter().cloned());
let mut vertices = character_models.flat_map(|(trafo, geom, skeleton_ref)|{
let skeleton_trafo = world
.component_for::<Node>(skeleton_ref)
.unwrap();
let skeleton_scale = skeleton_trafo.scale();
let inv_scale = vec3!(1./skeleton_scale.x, 1./skeleton_scale.y, 1./skeleton_scale.z);
let scale = trafo.global_scale().component_mul(&inv_scale);
geom.geom.iter().map(move |v| v.position().xyz().component_mul(&scale).to_pnt())
});
let first = vertices.next().unwrap();
vertices.fold((first, first), |(mins, maxs), v|{
let mins = pnt3( mins.x.min(v.x), mins.y.min(v.y), mins.z.min(v.z) );
let maxs = pnt3( maxs.x.max(v.x), maxs.y.max(v.y), maxs.z.max(v.z) );
(mins, maxs)
})
};
let bounding = AABB::new(mins, maxs);
let cuboid = Cuboid::new(bounding.half_extents());
let offset = Isometry3::new(bounding.center().to_vec(), zero());
let trimesh = procedural_to_rinmesh(&cuboid.to_trimesh(()), &offset, &vec3!(1.));
let shape = ShapeHandle::new(cuboid);
let trafo = world
.component_for::<Node>(&character_entity)
.unwrap()
.clone();
let iso = to_iso(&trafo);
let handle = self.add_entity(group, whitelist, blacklist, world, *character_entity,
shape.clone(), iso * offset);
world.add_component_to(character_entity, Shape(shape));
world.add_component_to(character_entity, Isometry(iso));
world.add_component_to(character_entity, Offset(offset));
handle
}
pub fn update_dynamic_entities<'a, E: EntitiesExt<'a>>(&mut self, entities: &E){
let trafos = entities.changed_iter_for::<(
Read<Node>,
HasOr<(DynamicTransformation, Bone)>,
Entity,
Read<CollisionHandle>,
Read<Offset>,
)>();
for (trafo, _, e, handle, offset) in trafos{
let iso = to_iso(&trafo);
self.world.get_mut(handle.0).unwrap().set_position( iso * offset.0);
}
self.world.update();
}
pub fn update_all_entities<'a, E: EntitiesExt<'a>>(&mut self, entities: &E){
let trafos = entities.changed_iter_for::<(
Read<Node>,
Entity,
Read<CollisionHandle>,
Read<Offset>,
)>();
for (trafo, e, handle, offset) in trafos{
let iso = to_iso(&trafo);
self.world.get_mut(handle.0).unwrap().set_position(iso * offset.0);
}
self.world.update();
}
}
#[cfg(feature="blender")]
fn blend_mesh_indices(blend_mesh: &rinblender::Mesh, offset: usize) -> Vec<Pnt3<usize>>{
if blend_mesh.mvert.is_empty() || blend_mesh.mloop.is_empty(){
return vec![];
}
let loops = &blend_mesh.mloop;
blend_mesh.mpoly.iter().flat_map(|poly|{
let loopstart = poly.loopstart as usize;
let totloop = poly.totloop;
if totloop == 4{
let v0 = loops[loopstart].v as usize + offset;
let v1 = loops[loopstart+1].v as usize + offset;
let v2 = loops[loopstart+2].v as usize + offset;
let v3 = loops[loopstart+3].v as usize + offset;
vec![Pnt3::new(v0,v1,v2), Pnt3::new(v2,v3,v0)]
}else if totloop == 3{
let v0 = loops[loopstart].v as usize + offset;
let v1 = loops[loopstart+1].v as usize + offset;
let v2 = loops[loopstart+2].v as usize + offset;
vec![Pnt3::new(v0, v1, v2)]
}else if totloop > 4{
let v0 = loops[loopstart];
loops[loopstart + 1 .. loopstart + totloop as usize]
.windows(2)
.map(|v1v2| pnt3!(v0.v as usize + offset, v1v2[0].v as usize + offset, v1v2[1].v as usize + offset))
.collect()
}else{
vec![]
}
}).collect()
}
pub fn rinmesh_to_trimesh<V: Vertex>(mesh: &Mesh<V>, scale: Vec3) -> TriMesh<f32>
where <V as Vertex>::Position: Swizzles3<f32, Swizzle3=Vec3>
{
let vertices = mesh.iter()
.map(|v| v.position().xyz().component_mul(&scale).to_pnt())
.collect();
let indices = if mesh.indices().is_empty(){
mesh.vertices().chunks(3).enumerate()
.map(|(idx, _)| pnt3(idx*3, idx*3+1, idx*3+2))
.collect::<Vec<_>>()
}else{
mesh.indices().chunks(3)
.map(|idx| pnt3(idx[0] as usize, idx[1] as usize, idx[2] as usize))
.collect()
};
TriMesh::new(
vertices,
indices,
None,
)
}
#[inline]
fn transform(p: Pnt3, offset: &Isometry3, scale: &Vec3) -> Pnt3{
(offset * p).to_vec().component_mul(scale).to_pnt()
}
pub fn trimesh_to_rinmesh(trimesh: &TriMesh<f32>, scale: Vec3) -> Mesh3D{
let vertices = trimesh.points().iter()
.map(|p| vertex3d(p.to_vec().component_mul(&scale)) );
let indices = trimesh.faces().iter()
.flat_map(|p| SmallVec::from([p.indices.x as IndexT, p.indices.y as IndexT, p.indices.z as IndexT]))
.collect();
Mesh::from_vertices_indices(vertices.collect(), indices)
}
pub fn to_iso(n: &Node) -> Isometry3{
Isometry3::from_parts(Translation::from(n.global_position().to_vec()), n.global_orientation())
}
pub fn procedural_to_rinmesh(trimesh: &procedural::TriMesh<f32>, offset: &Isometry3, scale: &Vec3) -> Mesh3D{
let vertices = trimesh.coords.iter().map(|p| p.to_vec())
.map(|p| {
let pos = transform(p.to_pnt(), offset, scale);
vertex3d(pos.to_vec())
});
let indices = match trimesh.indices {
procedural::IndexBuffer::Unified(ref indices) => indices.iter()
.flat_map(|p| SmallVec::from([p.x as IndexT, p.y as IndexT, p.z as IndexT]))
.collect(),
procedural::IndexBuffer::Split(ref indices) => {
indices.iter()
.flat_map(|p|{
SmallVec::from([p.x.x as IndexT, p.y.x as IndexT, p.z.x as IndexT])
})
.collect()
}
};
Mesh::from_vertices_indices(vertices.collect(), indices)
}
#[derive(PartialEq,PartialOrd)]
pub struct NonNan(pub f32);
impl NonNan {
pub fn new(val: f32) -> Option<NonNan> {
if val.is_nan() {
None
} else {
Some(NonNan(val))
}
}
}
impl Eq for NonNan {}
impl Ord for NonNan {
fn cmp(&self, other: &NonNan) -> Ordering {
self.partial_cmp(other).unwrap()
}
}