From 07eb6971d30f05517329318ddb0b87fadb70ed38 Mon Sep 17 00:00:00 2001
From: Dragos Daian <daian.dragos@yahoo.com>
Date: Sat, 28 Oct 2023 22:24:02 +0200
Subject: [PATCH] upd

---
 src/rapier2d-wrapper/src/physics_hooks.rs |  7 +++++--
 src/servers/rapier_body_utils_2d.cpp      | 23 +++++++++++++++++++++++
 src/spaces/rapier_space_2d.cpp            | 11 +++++++----
 3 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/src/rapier2d-wrapper/src/physics_hooks.rs b/src/rapier2d-wrapper/src/physics_hooks.rs
index 10ef0319..3634dff7 100644
--- a/src/rapier2d-wrapper/src/physics_hooks.rs
+++ b/src/rapier2d-wrapper/src/physics_hooks.rs
@@ -78,9 +78,12 @@ impl<'a> PhysicsHooks for PhysicsHooksCollisionFilter<'a> {
 
             let collider_1 = &context.colliders[context.collider1];
             let collider_2 = &context.colliders[context.collider2];
-            let body1 = &context.bodies[context.rigid_body1.unwrap()];
+			let rigidbody1_handle = context.rigid_body1.unwrap();
+            let body1 = context.bodies.get(rigidbody1_handle);
+			assert!(body1.is_some());
+			let body1 = body1.unwrap();
+			//body1.apply_impulse(vector![10.0, 0.0], true);
             let body2 = &context.bodies[context.rigid_body2.unwrap()];
-
 			let mut filter_info = CollisionFilterInfo::new();
 			filter_info.user_data1 = UserData::new(collider_1.user_data);
 			filter_info.user_data2 = UserData::new(collider_2.user_data);
diff --git a/src/servers/rapier_body_utils_2d.cpp b/src/servers/rapier_body_utils_2d.cpp
index 5cbbfdc4..089c7f1e 100644
--- a/src/servers/rapier_body_utils_2d.cpp
+++ b/src/servers/rapier_body_utils_2d.cpp
@@ -58,6 +58,29 @@ bool RapierBodyUtils2D::body_motion_recover(
 				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::ContactResult contact = rapier2d::shapes_contact(p_space.get_handle(), body_shape_info, col_shape_info, p_margin);
 
 				if (!contact.collided) {
diff --git a/src/spaces/rapier_space_2d.cpp b/src/spaces/rapier_space_2d.cpp
index de39c205..0eceed21 100644
--- a/src/spaces/rapier_space_2d.cpp
+++ b/src/spaces/rapier_space_2d.cpp
@@ -142,11 +142,14 @@ rapier2d::OneWayDirection RapierSpace2D::collision_modify_contacts_callback(rapi
 		if (collision_object_1->get_type() == RapierCollisionObject2D::TYPE_BODY && collision_object_2->get_type() == RapierCollisionObject2D::TYPE_BODY) {
 			RapierBody2D *body1 = static_cast<RapierBody2D *>(collision_object_1);
 			RapierBody2D *body2 = static_cast<RapierBody2D *>(collision_object_2);
-			//if (body1->is_static()) {
-			// TODO figure out when to set this.
-			//body2->set_linear_velocity(body2->get_linear_velocity() + body1->get_static_linear_velocity());
+			if (!body1->get_static_linear_velocity().is_zero_approx()) {
+				//body2->apply_central_impulse(body1->get_static_linear_velocity());
+				//body2->apply_central_impulse(Vector2(100,0));
+			}
+			if (!body2->get_static_linear_velocity().is_zero_approx()) {
+				//body1->apply_central_impulse(body2->get_static_linear_velocity());
+			}
 			//body2->set_angular_velocity(body2->get_angular_velocity() + body1->get_static_angular_velocity());
-			//}
 			//if (body2->is_static()) {
 			// TODO figure out when to set this.
 			//body1->set_linear_velocity(body1->get_linear_velocity() + body2->get_static_linear_velocity());