Skip to content

Commit

Permalink
finally implement unsafe fraction for all. (#279)
Browse files Browse the repository at this point in the history 
- Fixes #193
  • Loading branch information
@Ughuuu
Ughuuu authored Oct 13, 2024
1 parent db38058 commit 3488587
Show file tree
Hide file tree
Showing 2 changed files with 131 additions and 60 deletions.
146 changes: 106 additions & 40 deletions src/rapier_wrapper/query.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,11 @@ pub struct PointHitInfo {
pub collider: ColliderHandle,
pub user_data: UserData,
}
#[derive(Default)]
#[derive(Default, Debug)]
pub struct ShapeCastResult {
pub collided: bool,
pub toi: Real,
pub toi_unsafe: Real,
pub pixel_witness1: Vector<Real>,
pub pixel_witness2: Vector<Real>,
pub normal1: Vector<Real>,
Expand All @@ -51,6 +52,7 @@ impl ShapeCastResult {
ShapeCastResult {
collided: false,
toi: 1.0,
toi_unsafe: 1.0,
collider: ColliderHandle::invalid(),
pixel_witness1: zero(),
pixel_witness2: zero(),
Expand Down Expand Up @@ -285,12 +287,14 @@ impl PhysicsEngine {
world_handle: WorldHandle,
shape_vel: Vector<Real>,
shape_info: ShapeInfo,
margin: Real,
collide_with_body: bool,
collide_with_area: bool,
handle_excluded_info: &QueryExcludedInfo,
physics_collision_objects: &PhysicsCollisionObjects,
physics_ids: &PhysicsIds,
space: &RapierSpace,
needs_exact: bool,
) -> ShapeCastResult {
let mut result = ShapeCastResult::new();
if let Some(raw_shared_shape) = self.get_shape(shape_info.handle) {
Expand All @@ -314,49 +318,111 @@ impl PhysicsEngine {
)
};
filter.predicate = Some(&predicate);
let shape_cast_options = ShapeCastOptions {
max_time_of_impact: 1.0,
stop_at_penetration: true,
compute_impact_geometry_on_penetration: true,
..Default::default()
};
if let Some((collider_handle, hit)) =
physics_world.physics_objects.query_pipeline.cast_shape(
&physics_world.physics_objects.rigid_body_set,
&physics_world.physics_objects.collider_set,
&shape_transform,
&shape_vel,
shared_shape.as_ref(),
shape_cast_options,
filter,
)
{
if hit.status == ShapeCastStatus::Failed
|| hit.status == ShapeCastStatus::OutOfIterations
let velocity_size = shape_vel.magnitude();
if velocity_size < DEFAULT_EPSILON {
if let Some(collider_handle) = physics_world
.physics_objects
.query_pipeline
.intersection_with_shape(
&physics_world.physics_objects.rigid_body_set,
&physics_world.physics_objects.collider_set,
&shape_transform,
shared_shape.as_ref(),
filter,
)
{
godot_warn!("shape casting status warn: {:?}", hit.status);
result.collided = true;
result.toi = 0.0;
result.collider = collider_handle;
result.user_data = physics_world.get_collider_user_data(collider_handle);
if let Some(collider) = physics_world
.physics_objects
.collider_set
.get(collider_handle)
{
let pos12 = shape_transform.inv_mul(collider.position());
if let Ok(contact) = physics_world
.physics_objects
.query_pipeline
.query_dispatcher()
.contact(&pos12, shared_shape.as_ref(), collider.shape(), margin)
&& let Some(contact) = contact
{
result.normal1 = contact.normal1.into_inner();
result.normal2 = contact.normal2.into_inner();
result.pixel_witness1 = contact.point1.coords;
result.pixel_witness2 =
contact.point2.coords + collider.position().translation.vector;
//witness2 += collider.position().translation.vector;
} else {
godot_error!("contact error");
}
} else {
godot_error!("collider not found");
}
}
result.collided = true;
result.toi = hit.time_of_impact;
result.normal1 = hit.normal1.into_inner();
result.normal2 = hit.normal2.into_inner();
result.collider = collider_handle;
result.user_data = physics_world.get_collider_user_data(collider_handle);
// first is in world space
let witness1 = hit.witness1;
// second is translated by collider transform
let mut witness2 = hit.witness2;
if let Some(collider) = physics_world
.physics_objects
.collider_set
.get(collider_handle)
} else {
let shape_cast_options = ShapeCastOptions {
max_time_of_impact: 1.0,
stop_at_penetration: true,
compute_impact_geometry_on_penetration: true,
target_distance: margin,
};
if let Some((collider_handle, hit)) =
physics_world.physics_objects.query_pipeline.cast_shape(
&physics_world.physics_objects.rigid_body_set,
&physics_world.physics_objects.collider_set,
&shape_transform,
&shape_vel,
shared_shape.as_ref(),
shape_cast_options,
filter,
)
{
witness2 += collider.position().translation.vector;
} else {
godot_error!("collider not found");
if hit.status == ShapeCastStatus::Failed
|| hit.status == ShapeCastStatus::OutOfIterations
{
godot_warn!("shape casting status warn: {:?}", hit.status);
}
result.collided = true;
result.toi = hit.time_of_impact;
result.toi_unsafe = hit.time_of_impact;
result.normal1 = hit.normal1.into_inner();
result.normal2 = hit.normal2.into_inner();
result.collider = collider_handle;
result.user_data = physics_world.get_collider_user_data(collider_handle);
// first is in world space
let witness1 = hit.witness1;
// second is translated by collider transform
let mut witness2 = hit.witness2;
if let Some(collider) = physics_world
.physics_objects
.collider_set
.get(collider_handle)
{
result.pixel_witness1 = witness1.coords;
result.pixel_witness2 = witness2.coords;
// the time of impact isn't exact. Compute unsafe time of impact.
if needs_exact {
let mut hit_transform = shape_transform;
hit_transform.translation.vector += shape_vel * hit.time_of_impact;
let pos12 = hit_transform.inv_mul(collider.position());
// They are separated.
if let Ok(distance) = physics_world
.physics_objects
.query_pipeline
.query_dispatcher()
.distance(&pos12, shared_shape.as_ref(), collider.shape())
&& distance > DEFAULT_EPSILON
{
result.toi_unsafe += distance / velocity_size;
}
}
witness2 += collider.position().translation.vector;
} else {
godot_error!("collider not found");
}
}
result.pixel_witness1 = witness1.coords;
result.pixel_witness2 = witness2.coords;
}
}
}
Expand Down
45 changes: 25 additions & 20 deletions src/spaces/rapier_direct_space_state_impl.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ impl RapierDirectSpaceStateImpl {
shape_rid: Rid,
transform: Transform,
motion: Vector,
_margin: f32,
margin: f32,
collision_mask: u32,
collide_with_bodies: bool,
collide_with_areas: bool,
Expand Down Expand Up @@ -213,12 +213,14 @@ impl RapierDirectSpaceStateImpl {
space.get_state().get_id(),
vector_to_rapier(motion),
shape_info,
margin,
collide_with_bodies,
collide_with_areas,
&query_excluded_info,
&physics_data.collision_objects,
&physics_data.ids,
space,
false,
);
if !result.collided {
break;
Expand Down Expand Up @@ -257,7 +259,7 @@ impl RapierDirectSpaceStateImpl {
shape_rid: Rid,
transform: Transform,
motion: Vector,
_margin: f32,
margin: f32,
collision_mask: u32,
collide_with_bodies: bool,
collide_with_areas: bool,
Expand All @@ -277,25 +279,24 @@ impl RapierDirectSpaceStateImpl {
query_collision_layer_mask: collision_mask,
..Default::default()
};
let hit = physics_data
.physics_engine
.shape_casting(
space.get_state().get_id(),
rapier_motion,
shape_info,
collide_with_bodies,
collide_with_areas,
&query_excluded_info,
&physics_data.collision_objects,
&physics_data.ids,
space,
)
.toi;
let result = physics_data.physics_engine.shape_casting(
space.get_state().get_id(),
rapier_motion,
shape_info,
margin,
collide_with_bodies,
collide_with_areas,
&query_excluded_info,
&physics_data.collision_objects,
&physics_data.ids,
space,
true,
);
// TODO compute actual safe and unsafe
let closest_safe = closest_safe as *mut real;
*closest_safe = hit;
*closest_safe = result.toi;
let closest_unsafe = closest_unsafe as *mut real;
*closest_unsafe = hit;
*closest_unsafe = result.toi_unsafe;
true
}

Expand All @@ -305,7 +306,7 @@ impl RapierDirectSpaceStateImpl {
shape_rid: Rid,
transform: Transform,
motion: Vector,
_margin: f32,
margin: f32,
collision_mask: u32,
collide_with_bodies: bool,
collide_with_areas: bool,
Expand Down Expand Up @@ -338,12 +339,14 @@ impl RapierDirectSpaceStateImpl {
space.get_state().get_id(),
vector_to_rapier(motion),
shape_info,
margin,
collide_with_bodies,
collide_with_areas,
&query_excluded_info,
&physics_data.collision_objects,
&physics_data.ids,
space,
false,
);
if !result.collided {
break;
Expand All @@ -368,7 +371,7 @@ impl RapierDirectSpaceStateImpl {
shape_rid: Rid,
transform: Transform,
motion: Vector,
_margin: f32,
margin: f32,
collision_mask: u32,
collide_with_bodies: bool,
collide_with_areas: bool,
Expand All @@ -391,12 +394,14 @@ impl RapierDirectSpaceStateImpl {
space.get_state().get_id(),
rapier_motion,
shape_info,
margin,
collide_with_bodies,
collide_with_areas,
&query_excluded_info,
&physics_data.collision_objects,
&physics_data.ids,
space,
false,
);
if !result.collided {
return false;
Expand Down

0 comments on commit 3488587

Please sign in to comment.