Fix character body one way direction (#24)

- Fixes #10 - Update README.md
appsinacup · Nov 1, 2023 · 8311e78 · 8311e78
1 parent 2d9a7dd
commit 8311e78
Show file tree

Hide file tree

Showing 9 changed files with 134 additions and 143 deletions.
diff --git a/README.md b/README.md
@@ -9,7 +9,7 @@
     <a href="https://github.com/dimforge/rapier/releases/tag/v0.17.2" alt="Rapier2D Version">
         <img src="https://img.shields.io/badge/Rapier2D-v0.17.2-%23478cbf?logoColor=white" /></a>
     <a href="https://github.com/godotengine/godot-cpp" alt="Godot Version">
-        <img src="https://img.shields.io/badge/Godot-v4.1-%23478cbf?logo=godot-engine&logoColor=white" /></a>
+        <img src="https://img.shields.io/badge/Godot-v4.2-%23478cbf?logo=godot-engine&logoColor=white" /></a>
     <a href="https://github.com/appsinacup/godot-rapier2d/graphs/contributors" alt="Contributors">
         <img src="https://img.shields.io/github/contributors/appsinacup/godot-rapier2d" /></a>
     <a href="https://github.com/appsinacup/godot-rapier2d/pulse" alt="Activity">
@@ -34,19 +34,20 @@ A 2d [rapier](https://github.com/dimforge/rapier) physics server for [Godot Engi
 
 # Limitations
 
-- One way direction for CharacterBody2D missing.
 - Static Body Constant Speed for CharacterBody2D and RigidBody2D missing.
 - Spring Joint missing.
 - Shape skewing missing.
-- Shape Cast Margin isn't supported.
+- Shape Cast Margin is missing.
+- Warmstart missing (affects boxes stackability)
 
 # Supported Platforms
 
 - Windows (x86_64, x86_32)
 - macOS (x86-64 + arm64 Universal)
 - Linux (x86_64)
 - Android (x86_64, arm64)
-- iOS (arm64) without signing
+- iOS (arm64)
+- Web (wasm32)
 
 # Installation
 
@@ -64,11 +65,9 @@ Video Tutorial:
 
 # Features
 
-- Single and double float precision build
-- SIMD (Single instruction, multiple data) build
-- Cross-platform determinism (assuming the rest of your code is also deterministic)
-
-[More on different rapier features](https://rapier.rs/docs/user_guides/rust/getting_started)
+- Single and Double float precision build.
+- SIMD (Single instruction, multiple data) build.
+- Cross-platform determinism build.
 
 # Comparison
 

diff --git a/src/rapier2d-wrapper/includes/rapier2d_wrapper.h b/src/rapier2d-wrapper/includes/rapier2d_wrapper.h
@@ -178,6 +178,9 @@ using ContactPointCallback = bool (*)(Handle world_handle,
 struct OneWayDirection {
 	bool body1;
 	bool body2;
+	Real body1_margin;
+	Real body2_margin;
+	Real last_timestep;
 };
 
 using CollisionModifyContactsCallback = OneWayDirection (*)(Handle world_handle,

diff --git a/src/rapier2d-wrapper/src/physics_hooks.rs b/src/rapier2d-wrapper/src/physics_hooks.rs
@@ -8,6 +8,9 @@ use crate::user_data::*;
 pub struct OneWayDirection {
     pub body1 : bool,
     pub body2 : bool,
+    pub body1_margin : Real,
+    pub body2_margin : Real,
+	pub last_timestep: Real,
 }
 
 pub type CollisionFilterCallback = Option<extern "C" fn(world_handle : Handle, filter_info : &CollisionFilterInfo) -> bool>;
@@ -89,10 +92,19 @@ impl<'a> PhysicsHooks for PhysicsHooksCollisionFilter<'a> {
             let allowed_local_n2 = collider_2.position().rotation * Vector::y();
 			let one_way_direction = callback(self.world_handle, &filter_info);
             let mut contact_is_pass_through = false;
+			let mut dist: Real = 0.0;
+			if let Some(contact) = context.manifold.find_deepest_contact() {
+				dist = contact.dist;
+			}
+
             if one_way_direction.body1 {
-                contact_is_pass_through = body2.linvel().normalize().dot(&allowed_local_n1) <= DEFAULT_EPSILON * 10.0;
+				let motion_len = body2.linvel().magnitude();
+				let max_allowed = motion_len * Real::max(body2.linvel().normalize().dot(&allowed_local_n1), 0.0) + one_way_direction.body1_margin;
+                contact_is_pass_through = body2.linvel().dot(&allowed_local_n1) <= DEFAULT_EPSILON * 10.0 || dist < -max_allowed;
             } else if one_way_direction.body2 {
-                contact_is_pass_through = body1.linvel().normalize().dot(&allowed_local_n2) <= DEFAULT_EPSILON * 10.0;
+				let motion_len = body1.linvel().magnitude();
+				let max_allowed = motion_len * Real::max(body1.linvel().normalize().dot(&allowed_local_n2), 0.0) + one_way_direction.body2_margin;
+                contact_is_pass_through = body1.linvel().dot(&allowed_local_n2) <= DEFAULT_EPSILON * 10.0 || dist < -max_allowed;
             }
             if contact_is_pass_through {
                 context.solver_contacts.clear();

diff --git a/src/servers/rapier_body_utils_2d.cpp b/src/servers/rapier_body_utils_2d.cpp
@@ -1,18 +1,47 @@
 #include "rapier_body_utils_2d.h"
 #include "../bodies/rapier_body_2d.h"
+#include "../shapes/rapier_separation_ray_shape_2d.h"
 #include "../shapes/rapier_shape_2d.h"
 #include "../spaces/rapier_space_2d.h"
 
 #define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
 #define BODY_MOTION_RECOVER_ATTEMPTS 4
 #define BODY_MOTION_RECOVER_RATIO 0.4
 
+bool should_skip_collision_one_dir(rapier2d::ContactResult contact, RapierShape2D *body_shape, RapierBody2D *collision_body, int shape_index, const Transform2D &col_shape_transform, real_t p_margin, real_t last_step, Vector2 p_motion) {
+	real_t dist = contact.distance;
+	if (!contact.within_margin && body_shape->allows_one_way_collision() && collision_body->is_shape_set_as_one_way_collision(shape_index)) {
+		real_t valid_depth = 10e20;
+		Vector2 valid_dir = col_shape_transform.columns[1].normalized();
+
+		real_t owc_margin = collision_body->get_shape_one_way_collision_margin(shape_index);
+		valid_depth = MAX(owc_margin, p_margin);
+
+		if (collision_body->get_type() == RapierCollisionObject2D::TYPE_BODY) {
+			const RapierBody2D *b = collision_body;
+			if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
+				//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the
+				//given direction
+				Vector2 lv = b->get_linear_velocity();
+				//compute displacement from linear velocity
+				Vector2 motion = lv * last_step;
+				real_t motion_len = motion.length();
+				motion.normalize();
+				valid_depth += motion_len * MAX(motion.dot(valid_dir), 0.0);
+			}
+		}
+		Vector2 motion = p_motion;
+		real_t motion_len = motion.length();
+		valid_depth += motion_len * MAX(motion.normalized().dot(valid_dir), 0.0);
+		if ((dist < -valid_depth) || (p_motion.normalized().dot(valid_dir) < CMP_EPSILON * 10.0)) {
+			return true;
+		}
+	}
+	return false;
+}
+
 bool RapierBodyUtils2D::body_motion_recover(
-		const RapierSpace2D &p_space,
-		RapierBody2D &p_body,
-		Transform2D &p_transform,
-		real_t p_margin,
-		Vector2 &p_recover_motion) {
+		const RapierSpace2D &p_space, RapierBody2D &p_body, Transform2D &p_transform, const Vector2 &p_motion, real_t p_margin, Vector2 &p_recover_motion) {
 	int shape_count = p_body.get_shape_count();
 	ERR_FAIL_COND_V(shape_count < 1, false);
 	real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
@@ -27,7 +56,8 @@ bool RapierBodyUtils2D::body_motion_recover(
 		Rect2 margin_aabb = p_transform.xform(body_aabb);
 		margin_aabb = margin_aabb.grow(p_margin);
 
-		int result_count = p_space.rapier_intersect_aabb(margin_aabb, p_body.get_collision_mask(), true, false, results, 32, &result_count, p_body.get_rid());
+		int result_count = p_space.rapier_intersect_aabb(
+				margin_aabb, p_body.get_collision_mask(), true, false, results, 32, &result_count, p_body.get_rid());
 		// Optimization
 		if (result_count == 0) {
 			break;
@@ -42,7 +72,8 @@ bool RapierBodyUtils2D::body_motion_recover(
 
 			RapierShape2D *body_shape = p_body.get_shape(body_shape_idx);
 			Transform2D const &body_shape_transform = p_transform * p_body.get_shape_transform(body_shape_idx);
-			rapier2d::ShapeInfo body_shape_info = rapier2d::shape_info_from_body_shape(body_shape->get_rapier_shape(), body_shape_transform);
+			rapier2d::ShapeInfo body_shape_info =
+					rapier2d::shape_info_from_body_shape(body_shape->get_rapier_shape(), body_shape_transform);
 
 			for (int result_idx = 0; result_idx < result_count; ++result_idx) {
 				rapier2d::PointHitInfo &result = results[result_idx];
@@ -56,41 +87,22 @@ bool RapierBodyUtils2D::body_motion_recover(
 				RapierShape2D *col_shape = collision_body->get_shape(shape_index);
 
 				Transform2D const &col_shape_transform = collision_body->get_transform() * collision_body->get_shape_transform(shape_index);
-				rapier2d::ShapeInfo col_shape_info = rapier2d::shape_info_from_body_shape(col_shape->get_rapier_shape(), col_shape_transform);
-
-				/*
-				if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
-					cbk.valid_dir = col_obj_shape_xform.columns[1].normalized();
-
-					real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
-					cbk.valid_depth = MAX(owc_margin, margin); //user specified, but never less than actual margin or it won't work
-					cbk.invalid_by_dir = 0;
-
-					if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
-						const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
-						if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) {
-							//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
-							Vector2 lv = b->get_linear_velocity();
-							//compute displacement from linear velocity
-							Vector2 motion = lv * last_step;
-							real_t motion_len = motion.length();
-							motion.normalize();
-							cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
-						}
-					}
-				}*/
+				rapier2d::ShapeInfo col_shape_info =
+						rapier2d::shape_info_from_body_shape(col_shape->get_rapier_shape(), col_shape_transform);
 
 				rapier2d::ContactResult contact = rapier2d::shapes_contact(p_space.get_handle(), body_shape_info, col_shape_info, p_margin);
 
 				if (!contact.collided) {
 					continue;
 				}
-
-				recovered = true;
-
+				if (should_skip_collision_one_dir(contact, body_shape, collision_body, shape_index, col_shape_transform, p_margin, p_space.get_last_step(), p_motion)) {
+					continue;
+				}
 				Vector2 a(contact.point1.x, contact.point1.y);
 				Vector2 b(contact.point2.x, contact.point2.y);
 
+				recovered = true;
+
 				// Compute plane on b towards a.
 				Vector2 n = Vector2(contact.normal1.x, contact.normal1.y);
 				// Move it outside as to fit the margin
@@ -118,15 +130,9 @@ bool RapierBodyUtils2D::body_motion_recover(
 	return recovered;
 }
 
-void RapierBodyUtils2D::cast_motion(
-		const RapierSpace2D &p_space,
-		RapierBody2D &p_body,
-		const Transform2D &p_transform,
-		const Vector2 &p_motion,
-		real_t p_margin,
-		real_t &p_closest_safe,
-		real_t &p_closest_unsafe,
-		int &p_best_body_shape) {
+void RapierBodyUtils2D::cast_motion(const RapierSpace2D &p_space, RapierBody2D &p_body, const Transform2D &p_transform,
+		const Vector2 &p_motion, bool p_collide_separation_ray, real_t contact_max_allowed_penetration, real_t p_margin,
+		real_t &p_closest_safe, real_t &p_closest_unsafe, int &p_best_body_shape) {
 	Rect2 body_aabb = p_body.get_aabb();
 	Rect2 margin_aabb = p_transform.xform(body_aabb);
 
@@ -136,7 +142,8 @@ void RapierBodyUtils2D::cast_motion(
 	motion_aabb = motion_aabb.merge(margin_aabb);
 
 	rapier2d::PointHitInfo results[32];
-	int result_count = p_space.rapier_intersect_aabb(motion_aabb, p_body.get_collision_mask(), true, false, results, 32, &result_count, p_body.get_rid());
+	int result_count = p_space.rapier_intersect_aabb(
+			motion_aabb, p_body.get_collision_mask(), true, false, results, 32, &result_count, p_body.get_rid());
 
 	if (result_count == 0) {
 		return;
@@ -151,6 +158,16 @@ void RapierBodyUtils2D::cast_motion(
 		Transform2D const &body_shape_transform = p_transform * p_body.get_shape_transform(body_shape_idx);
 		rapier2d::ShapeInfo body_shape_info = rapier2d::shape_info_from_body_shape(body_shape->get_rapier_shape(), body_shape_transform);
 
+		// Colliding separation rays allows to properly snap to the ground,
+		// otherwise it's not needed in regular motion.
+		if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) {
+			// When slide on slope is on, separation ray shape acts like a
+			// regular shape.
+			if (!static_cast<RapierSeparationRayShape2D *>(body_shape)->get_slide_on_slope()) {
+				continue;
+			}
+		}
+
 		bool stuck = false;
 		real_t best_safe = 1.0;
 		real_t best_unsafe = 1.0;
@@ -172,6 +189,12 @@ void RapierBodyUtils2D::cast_motion(
 			body_shape_info.position = { (body_shape_transform.get_origin()).x, (body_shape_transform.get_origin()).y };
 			rapier2d::ContactResult step_contact = rapier2d::shapes_contact(p_space.get_handle(), body_shape_info, col_shape_info, 0.0);
 			if (step_contact.collided && !step_contact.within_margin) {
+				if (body_shape->allows_one_way_collision() && collision_body->is_shape_set_as_one_way_collision(shape_index)) {
+					Vector2 direction = col_shape_transform.columns[1].normalized();
+					if (p_motion.normalized().dot(direction) < 0) {
+						continue;
+					}
+				}
 				p_closest_safe = 0;
 				p_closest_unsafe = 0;
 				p_best_body_shape = body_shape_idx; //sadly it's the best
@@ -186,16 +209,18 @@ void RapierBodyUtils2D::cast_motion(
 			for (int k = 0; k < 8; k++) {
 				real_t fraction = low + (hi - low) * fraction_coeff;
 
-				body_shape_info.position = rapier2d::Vector{ (body_shape_transform.get_origin() + p_motion * fraction).x, (body_shape_transform.get_origin() + p_motion * fraction).y };
+				body_shape_info.position = rapier2d::Vector{ (body_shape_transform.get_origin() + p_motion * fraction).x,
+					(body_shape_transform.get_origin() + p_motion * fraction).y };
 				rapier2d::ContactResult step_contact = rapier2d::shapes_contact(p_space.get_handle(), body_shape_info, col_shape_info, 0.0);
 				if (step_contact.collided && !step_contact.within_margin) {
 					hi = fraction;
 					if ((k == 0) || (low > 0.0)) { // Did it not collide before?
 						// When alternating or first iteration, use dichotomy.
 						fraction_coeff = 0.5;
 					} else {
-						// When colliding again, converge faster towards low fraction
-						// for more accurate results with long motions that collide near the start.
+						// When colliding again, converge faster towards low
+						// fraction for more accurate results with long motions
+						// that collide near the start.
 						fraction_coeff = 0.25;
 					}
 				} else {
@@ -204,13 +229,20 @@ void RapierBodyUtils2D::cast_motion(
 						// When alternating or first iteration, use dichotomy.
 						fraction_coeff = 0.5;
 					} else {
-						// When not colliding again, converge faster towards high fraction
-						// for more accurate results with long motions that collide near the end.
+						// When not colliding again, converge faster towards
+						// high fraction for more accurate results with long
+						// motions that collide near the end.
 						fraction_coeff = 0.75;
 					}
 				}
 			}
-
+			body_shape_info.position =
+					rapier2d::Vector{ (body_shape_transform.get_origin() + p_motion * (hi + contact_max_allowed_penetration)).x,
+						(body_shape_transform.get_origin() + p_motion * (hi + contact_max_allowed_penetration)).y };
+			rapier2d::ContactResult contact = rapier2d::shapes_contact(p_space.get_handle(), body_shape_info, col_shape_info, 0.0);
+			if (should_skip_collision_one_dir(contact, body_shape, collision_body, shape_index, col_shape_transform, p_margin, p_space.get_last_step(), p_motion)) {
+				continue;
+			}
 			if (low < best_safe) {
 				best_safe = low;
 				best_unsafe = hi;
@@ -228,14 +260,8 @@ void RapierBodyUtils2D::cast_motion(
 	}
 }
 
-bool RapierBodyUtils2D::body_motion_collide(
-		const RapierSpace2D &p_space,
-		RapierBody2D &p_body,
-		const Transform2D &p_transform,
-		const Vector2 &p_motion,
-		int p_best_body_shape,
-		real_t p_margin,
-		PhysicsServer2DExtensionMotionResult *p_result) {
+bool RapierBodyUtils2D::body_motion_collide(const RapierSpace2D &p_space, RapierBody2D &p_body, const Transform2D &p_transform,
+		const Vector2 &p_motion, int p_best_body_shape, real_t p_margin, PhysicsServer2DExtensionMotionResult *p_result) {
 	int shape_count = p_body.get_shape_count();
 	ERR_FAIL_COND_V(shape_count < 1, false);
 	Rect2 body_aabb = p_body.get_aabb();
@@ -248,7 +274,8 @@ bool RapierBodyUtils2D::body_motion_collide(
 	motion_aabb = motion_aabb.merge(margin_aabb);
 
 	rapier2d::PointHitInfo results[32];
-	int result_count = p_space.rapier_intersect_aabb(motion_aabb, p_body.get_collision_mask(), true, false, results, 32, &result_count, p_body.get_rid());
+	int result_count = p_space.rapier_intersect_aabb(
+			motion_aabb, p_body.get_collision_mask(), true, false, results, 32, &result_count, p_body.get_rid());
 	// Optimization
 	if (result_count == 0) {
 		return false;
@@ -285,10 +312,18 @@ bool RapierBodyUtils2D::body_motion_collide(
 			RapierShape2D *col_shape = collision_body->get_shape(shape_index);
 			Transform2D const &col_shape_transform = collision_body->get_transform() * collision_body->get_shape_transform(shape_index);
 			rapier2d::ShapeInfo col_shape_info = rapier2d::shape_info_from_body_shape(col_shape->get_rapier_shape(), col_shape_transform);
+
 			rapier2d::ContactResult contact = rapier2d::shapes_contact(p_space.get_handle(), body_shape_info, col_shape_info, p_margin);
 			if (!contact.collided) {
 				continue;
 			}
+
+			Vector2 a(contact.point1.x, contact.point1.y);
+			Vector2 b(contact.point2.x, contact.point2.y);
+
+			if (should_skip_collision_one_dir(contact, body_shape, collision_body, shape_index, col_shape_transform, p_margin, p_space.get_last_step(), p_motion)) {
+				continue;
+			}
 			if (contact.distance < min_distance) {
 				min_distance = contact.distance;
 				best_collision_body = collision_body;

diff --git a/src/servers/rapier_body_utils_2d.h b/src/servers/rapier_body_utils_2d.h
@@ -14,30 +14,14 @@ class RapierShape2D;
 class RapierBodyUtils2D {
 public:
 	static bool body_motion_recover(
-			const RapierSpace2D &p_space,
-			RapierBody2D &p_body,
-			Transform2D &p_transform,
-			real_t p_margin,
-			Vector2 &p_recover_motion);
+			const RapierSpace2D &p_space, RapierBody2D &p_body, Transform2D &p_transform, const Vector2 &p_motion, real_t p_margin, Vector2 &p_recover_motion);
 
-	static void cast_motion(
-			const RapierSpace2D &p_space,
-			RapierBody2D &p_body,
-			const Transform2D &p_transform,
-			const Vector2 &p_motion,
-			real_t p_margin,
-			real_t &p_closest_safe,
-			real_t &p_closest_unsafe,
-			int &p_best_body_shape);
+	static void cast_motion(const RapierSpace2D &p_space, RapierBody2D &p_body, const Transform2D &p_transform, const Vector2 &p_motion,
+			bool p_collide_separation_ray, real_t contact_max_allowed_penetration, real_t p_margin, real_t &p_closest_safe,
+			real_t &p_closest_unsafe, int &p_best_body_shape);
 
-	static bool body_motion_collide(
-			const RapierSpace2D &p_space,
-			RapierBody2D &p_body,
-			const Transform2D &p_transform,
-			const Vector2 &p_motion,
-			int p_best_body_shape,
-			real_t p_margin,
-			PhysicsServer2DExtensionMotionResult *p_result);
+	static bool body_motion_collide(const RapierSpace2D &p_space, RapierBody2D &p_body, const Transform2D &p_transform,
+			const Vector2 &p_motion, int p_best_body_shape, real_t p_margin, PhysicsServer2DExtensionMotionResult *p_result);
 };
 
 #endif // RAPIER_BODY_UTILS_2D_H