423 lines
13 KiB
Rust
423 lines
13 KiB
Rust
//================= 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
|
|
}
|
|
|