//================= Copyright kotofyt, All rights reserved ==================// // // Purpose: Wrapper for rapier physics // //===========================================================================// #![allow(nonstandard_style)] #![allow(unused)] macro_rules! V_malloc { ($t:ty, $count:expr) => { malloc(size_of::<$t>() * $count) as *mut $t }; } use std::{default, ops::Index, ptr::{self, null, null_mut}, slice::from_raw_parts, sync::Arc}; use parry3d::{glamx::vec3, shape::{Shape, ShapeType, SharedShape}}; use parry3d::{glamx::{Pose3A}, query::ShapeCastOptions}; use rapier3d::{geometry::Ball, na::{UnitQuaternion, Vector4, coordinates::XYZ}}; use rapier3d::prelude::*; use libc::{malloc, free}; #[repr(C)] #[derive(Default, Debug)] pub struct Vector { x: f32, y: f32, z: f32, } #[repr(C)] #[derive(Default, Debug)] pub struct Quat { x: f32, y: f32, z: f32, w: f32, } #[repr(C)] #[derive(Clone, Copy)] pub struct BallShape_t { m_fRadius: f32, } #[repr(C)] #[derive(Clone, Copy)] #[derive(Debug)] pub struct CuboidShape_t { m_fExtentX: f32, m_fExtentY: f32, m_fExtentZ: f32, } #[repr(C)] #[derive(Clone, Copy)] #[derive(Debug)] pub struct TriangleMeshShape_t { m_pfPositions: *const f32, m_nPositionCount: u32, m_puIndicies: *const f32, m_nIndiciesCount: u32, } #[derive(Clone)] #[derive(Debug)] pub struct RapierShape_t { m_sharedShape: SharedShape, } #[repr(C)] #[derive(Debug)] pub enum EPhysicsBodyType { k_EPhysics_Static, k_EPhysics_Dynamic, k_EPhysics_KinematicPositionBased, k_EPhysics_KinematicVelocityBased, } #[repr(C)] #[derive(Debug, Default)] pub struct CastResult_t { m_bIsHit: bool, m_hCollider: *const RapierCollider_t, m_vCollisionPoint: Vector, m_fTime: f32, m_fDistance: f32, } #[derive(Clone)] #[derive(Debug)] pub struct RapierCollider_t { m_collider: Collider, } #[derive(Clone)] pub struct RapierWorld_t { m_colliders: ColliderSet, m_rigidBodies: RigidBodySet, m_islandManager: IslandManager, m_broadPhase: DefaultBroadPhase, m_narrowPhase: NarrowPhase, m_impulseJointSet: ImpulseJointSet, m_multibodyJointSet: MultibodyJointSet, m_ccdSolver: CCDSolver, } #[derive(Clone)] pub struct RapierPhysicsBody_t { m_body: RigidBody, m_pCollider: *mut RapierCollider_t, m_pWorld: *mut RapierWorld_t, m_hRigidBodyHandle: RigidBodyHandle, m_hColliderHandle: ColliderHandle, } #[derive(Clone, Copy)] pub struct RapierPhysics_t { } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsBody_SetPosition( this: *mut RapierPhysicsBody_t, fX: f32, fY: f32, fZ: f32 ) { let world: &mut RapierWorld_t = &mut *(*this).m_pWorld; world.m_rigidBodies[(*this).m_hRigidBodyHandle] .set_translation(vec3(fX, fY, fZ), true); } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsBody_SetRotation( this: *mut RapierPhysicsBody_t, fX: f32, fY: f32, fZ: f32, fW: f32 ) { let world: &mut RapierWorld_t = &mut *(*this).m_pWorld; world.m_rigidBodies[(*this).m_hRigidBodyHandle] .set_rotation(parry3d::glamx::Quat::from_xyzw(fX, fY, fZ, fW), true); } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsBody_GetPosition( this: *mut RapierPhysicsBody_t ) -> Vector { let world: &mut RapierWorld_t = &mut *(*this).m_pWorld; let mut position = world.m_rigidBodies[(*this).m_hRigidBodyHandle] .translation().to_array(); return Vector { x: position[0], y: position[1], z: position[2]} } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsBody_GetRotation( this: *mut RapierPhysicsBody_t ) -> Quat { let world: &mut RapierWorld_t = &mut *(*this).m_pWorld; let rotationVector = world.m_rigidBodies[(*this).m_hRigidBodyHandle] .rotation().to_array(); Quat{ x: rotationVector[0], y: rotationVector[1], z: rotationVector[2], w: rotationVector[3]} } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsBody_SetType( this: *mut RapierPhysicsBody_t, eType: EPhysicsBodyType ) { let world: &mut RapierWorld_t = &mut *(*this).m_pWorld; let eRapierBodyType: RigidBodyType; match eType { EPhysicsBodyType::k_EPhysics_Static => eRapierBodyType = RigidBodyType::Fixed, EPhysicsBodyType::k_EPhysics_Dynamic => eRapierBodyType = RigidBodyType::Dynamic, EPhysicsBodyType::k_EPhysics_KinematicPositionBased => eRapierBodyType = RigidBodyType::KinematicPositionBased, EPhysicsBodyType::k_EPhysics_KinematicVelocityBased => eRapierBodyType = RigidBodyType::KinematicVelocityBased, } world.m_rigidBodies[(*this).m_hRigidBodyHandle] .set_body_type(eRapierBodyType, true); } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsWorld_Frame( this: *mut RapierWorld_t, fDelta: f32 ) { let vGravity = vec3(0.0, -9.8, 0.0); let mut integrationParameters = IntegrationParameters::default(); integrationParameters.dt = fDelta; let mut physicsPipeline = PhysicsPipeline::new(); let physicsHooks = (); let eventHandler = (); physicsPipeline.step( vGravity, &integrationParameters, &mut (*this).m_islandManager, &mut (*this).m_broadPhase, &mut (*this).m_narrowPhase, &mut (*this).m_rigidBodies, &mut (*this).m_colliders, &mut (*this).m_impulseJointSet, &mut (*this).m_multibodyJointSet, &mut (*this).m_ccdSolver, &physicsHooks, &eventHandler, ); } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsWorld_CreateRigidBody( this: *mut RapierWorld_t, pCollider: *mut RapierCollider_t, eType: EPhysicsBodyType ) -> *mut RapierPhysicsBody_t { let eRapierBodyType: RigidBodyType; match eType { EPhysicsBodyType::k_EPhysics_Static => eRapierBodyType = RigidBodyType::Fixed, EPhysicsBodyType::k_EPhysics_Dynamic => eRapierBodyType = RigidBodyType::Dynamic, EPhysicsBodyType::k_EPhysics_KinematicPositionBased => eRapierBodyType = RigidBodyType::KinematicPositionBased, EPhysicsBodyType::k_EPhysics_KinematicVelocityBased => eRapierBodyType = RigidBodyType::KinematicVelocityBased, } let pBody = V_malloc!(RapierPhysicsBody_t, 1); std::ptr::write(&mut (*pBody).m_body, RigidBodyBuilder::new(eRapierBodyType).build()); (*pBody).m_pCollider = pCollider; let hRigidBodyHandle = (*this).m_rigidBodies.insert((*pBody).m_body.clone()); let hColliderHandle = (*this).m_colliders.insert_with_parent( (*pCollider).m_collider.clone(), hRigidBodyHandle, &mut (*this).m_rigidBodies); (*pBody).m_hRigidBodyHandle = hRigidBodyHandle; (*pBody).m_hColliderHandle = hColliderHandle; (*pBody).m_pWorld = this; pBody } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsWorld_RayCast( this: *mut RapierWorld_t, vBegin: Vector, vEnd: Vector ) -> CastResult_t { let mut cast = CastResult_t::default(); let vDir = Vector{ x: vEnd.x-vBegin.x, y:vEnd.y-vBegin.y, z:vEnd.z-vBegin.z}; let fMaxDistance = f32::sqrt(vDir.x*vDir.x+vDir.y*vDir.y+vDir.z*vDir.z); let vNormalizedDir = Vector{ x: vDir.x/fMaxDistance, y:vDir.y/fMaxDistance, z:vDir.z/fMaxDistance}; let ray = Ray::new( Vec3 { x: vBegin.x, y: vBegin.y, z: vBegin.z }, Vec3 { x: vNormalizedDir.x, y: vNormalizedDir.y, z: vNormalizedDir.z }); let queryPipeline = (*this).m_broadPhase.as_query_pipeline( (*this).m_narrowPhase.query_dispatcher(), &(*this).m_rigidBodies, &(*this).m_colliders, QueryFilter::default(), ); if let Some((handle, intersection)) = queryPipeline.cast_ray_and_get_normal(&ray, fMaxDistance, true) { cast.m_bIsHit = true; cast.m_fDistance = intersection.time_of_impact; cast.m_fTime = intersection.time_of_impact/fMaxDistance; cast.m_vCollisionPoint = Vector{ x: vBegin.x + vNormalizedDir.x * intersection.time_of_impact, y: vBegin.y + vNormalizedDir.y * intersection.time_of_impact, z: vBegin.z + vNormalizedDir.z * intersection.time_of_impact, }; } cast } #[no_mangle] pub unsafe extern "C" fn CRapierPhysicsWorld_ShapeCast( this: *mut RapierWorld_t, pShape: *mut RapierShape_t, vOrientation: Quat, vBegin: Vector, vEnd: Vector ) -> CastResult_t { let mut cast = CastResult_t::default(); let vDir = Vector{ x: vEnd.x-vBegin.x, y:vEnd.y-vBegin.y, z:vEnd.z-vBegin.z}; let fMaxDistance = f32::sqrt(vDir.x*vDir.x+vDir.y*vDir.y+vDir.z*vDir.z); let vNormalizedDir = Vector{ x: vDir.x/fMaxDistance, y:vDir.y/fMaxDistance, z:vDir.z/fMaxDistance}; let queryPipeline = (*this).m_broadPhase.as_query_pipeline( (*this).m_narrowPhase.query_dispatcher(), &(*this).m_rigidBodies, &(*this).m_colliders, QueryFilter::default(), ); let vRustDir = Vector{ x: vNormalizedDir.x, y: vNormalizedDir.y, z: vNormalizedDir.z }; let mut castOptions = ShapeCastOptions::default(); castOptions.stop_at_penetration = true; castOptions.max_time_of_impact = fMaxDistance; let shapeStats = Pose3::from_parts(Vec3::from_array([vBegin.x, vBegin.y, vBegin.z]),glamx::Quat::from_xyzw(vOrientation.x, vOrientation.y, vOrientation.z, vOrientation.w)); let shape: &dyn Shape; match (*pShape).m_sharedShape.as_typed_shape() { TypedShape::Ball(s) => { shape = s; } TypedShape::Cuboid(s) => { shape = s; } TypedShape::TriMesh(s) => { shape = s; } default => { return cast; } } if let Some((handle, intersection)) = queryPipeline.cast_shape( &shapeStats, Vec3 { x: vNormalizedDir.x, y: vNormalizedDir.y, z: vNormalizedDir.z }, shape, castOptions) { cast.m_bIsHit = true; cast.m_fDistance = intersection.time_of_impact; cast.m_fTime = intersection.time_of_impact/fMaxDistance; cast.m_vCollisionPoint = Vector{ x: vBegin.x + vNormalizedDir.x * intersection.time_of_impact, y: vBegin.y + vNormalizedDir.y * intersection.time_of_impact, z: vBegin.z + vNormalizedDir.z * intersection.time_of_impact, }; } cast } #[no_mangle] pub unsafe extern "C" fn CRapierPhysics_New() -> *mut RapierPhysics_t { let physics = RapierPhysics_t {}; let pPhysics = V_malloc!(RapierPhysics_t, 1); *pPhysics = physics.clone(); pPhysics } #[no_mangle] pub unsafe extern "C" fn CRapierPhysics_CreateBall( this: *mut RapierPhysics_t, ball: BallShape_t ) -> *mut RapierShape_t { let pShapeMemory: *mut RapierShape_t = V_malloc!(RapierShape_t, 1); std::ptr::write(&mut (*pShapeMemory).m_sharedShape, SharedShape::new(Ball::new(ball.m_fRadius))); pShapeMemory } #[no_mangle] pub unsafe extern "C" fn CRapierPhysics_CreateCube( this: *mut RapierPhysics_t, cuboid: CuboidShape_t ) -> *mut RapierShape_t { let pShapeMemory: *mut RapierShape_t = V_malloc!(RapierShape_t, 1); std::ptr::write(&mut (*pShapeMemory).m_sharedShape, SharedShape::new( Cuboid::new(vec3(cuboid.m_fExtentX, cuboid.m_fExtentY, cuboid.m_fExtentZ))) ); pShapeMemory } #[no_mangle] pub unsafe extern "C" fn CRapierPhysics_CreateTriangleMesh( this: *mut RapierPhysics_t, triangle: TriangleMeshShape_t ) -> *mut RapierShape_t { let pShapeMemory: *mut RapierShape_t = V_malloc!(RapierShape_t, 1); let positions = Vec::from_raw_parts(triangle.m_pfPositions as *mut Vec3, (triangle.m_nPositionCount/3) as usize, (triangle.m_nPositionCount/3) as usize); let mut indices: Vec<[u32; 3]>; if ( triangle.m_nIndiciesCount == 0 ) { if ( triangle.m_nPositionCount % 3 != 0 ) { return null_mut(); } indices = vec![[0,0,0]; (triangle.m_nPositionCount/9) as usize]; for i in 0..indices.len() { let u = i as u32; indices[i][0] = u*3; indices[i][1] = u*3+1; indices[i][2] = u*3+2; } } else { if ( triangle.m_nIndiciesCount % 3 != 0 ) { return null_mut(); } indices = Vec::from_raw_parts(triangle.m_puIndicies as *mut [u32; 3], (triangle.m_nIndiciesCount / 3) as usize, (triangle.m_nIndiciesCount / 3) as usize); } let mesh = TriMesh::new(positions, indices); match mesh { Ok(m) => { std::ptr::write(&mut (*pShapeMemory).m_sharedShape, SharedShape::new(m)); } default => { return null_mut(); } } pShapeMemory } #[no_mangle] pub unsafe extern "C" fn CRapierPhysics_CreateCollider( this: *mut RapierPhysics_t, pShape: *mut RapierShape_t ) -> *mut RapierCollider_t { let shape: &SharedShape = &(*pShape).m_sharedShape; let pCollider = V_malloc!(RapierCollider_t, 1); std::ptr::write(&mut (*pCollider).m_collider, ColliderBuilder::new(shape.clone()).build()); pCollider } #[no_mangle] pub unsafe extern "C" fn CRapierPhysics_CreateWorld( this: *mut RapierPhysics_t ) -> *mut RapierWorld_t { let pWorld: *mut RapierWorld_t = V_malloc!(RapierWorld_t, 1); let world: &mut RapierWorld_t = &mut *pWorld; std::ptr::write(&mut (*pWorld).m_colliders, ColliderSet::new()); std::ptr::write(&mut (*pWorld).m_rigidBodies, RigidBodySet::new()); std::ptr::write(&mut (*pWorld).m_islandManager, IslandManager::new()); std::ptr::write(&mut (*pWorld).m_broadPhase, DefaultBroadPhase::new()); std::ptr::write(&mut (*pWorld).m_narrowPhase, NarrowPhase::new()); std::ptr::write(&mut (*pWorld).m_impulseJointSet, ImpulseJointSet::new()); std::ptr::write(&mut (*pWorld).m_multibodyJointSet, MultibodyJointSet::new()); std::ptr::write(&mut (*pWorld).m_ccdSolver, CCDSolver::new()); pWorld }