From a7dfd18dccbbd4fadf3cf0aa77ab7db1935eee18 Mon Sep 17 00:00:00 2001 From: Dragos Daian Date: Thu, 27 Jun 2024 17:41:24 +0200 Subject: [PATCH] Fix order of results from shape result (#120) - Fix https://github.com/appsinacup/godot-rapier-physics/issues/105 --- bin2d/Marker2D.gd | 38 ++++++ bin2d/main.tscn | 86 +++++++++++++ bin2d/project.godot | 2 - bin2d/ray_cast_2d.gd | 12 ++ bin2d/test.gd | 15 +-- bin2d/test.tscn | 114 ++++-------------- src/rapier_wrapper/query.rs | 39 ++++-- src/servers/mod.rs | 8 +- src/servers/rapier_physics_server_2d.rs | 10 ++ src/servers/rapier_physics_server_3d.rs | 10 ++ .../rapier_physics_server_factory_2d.rs | 14 --- .../rapier_physics_server_factory_3d.rs | 14 --- src/spaces/rapier_direct_space_state_impl.rs | 5 +- 13 files changed, 224 insertions(+), 143 deletions(-) create mode 100644 bin2d/Marker2D.gd create mode 100644 bin2d/main.tscn create mode 100644 bin2d/ray_cast_2d.gd delete mode 100644 src/servers/rapier_physics_server_factory_2d.rs delete mode 100644 src/servers/rapier_physics_server_factory_3d.rs diff --git a/bin2d/Marker2D.gd b/bin2d/Marker2D.gd new file mode 100644 index 00000000..1de0d503 --- /dev/null +++ b/bin2d/Marker2D.gd @@ -0,0 +1,38 @@ +class_name CollideShapeTest +extends Marker2D + +var point0: Vector2 +var point1: Vector2 +var state2d: PhysicsDirectSpaceState2D +var params := PhysicsShapeQueryParameters2D.new() + +var strings: Array[String] = ["circle", "rectangle", "capsule", "segment"] +var shapes: Array[Shape2D] = [CircleShape2D.new(), RectangleShape2D.new(), CapsuleShape2D.new(), SegmentShape2D.new()] +var idx := 0 + +func _ready() -> void: + state2d = get_world_2d().direct_space_state + + var _transform = Transform2D.IDENTITY + _transform.origin = global_position + + params.transform = transform + params.shape = shapes[idx] + +func _physics_process(_delta: float) -> void: + queue_redraw() + + if Input.is_action_just_pressed("ui_select"): + idx += 1 + if idx == len(shapes): idx = 0 + params.shape = shapes[idx] + + var results = state2d.collide_shape(params, 1) + if len(results) == 2: + point0 = results[0] + point1 = results[1] + +func _draw() -> void: + draw_string(ThemeDB.fallback_font, Vector2(-15, 100), strings[idx],) + draw_circle(point1 - global_position, 3, Color.GREEN) + draw_circle(point0 - global_position, 2, Color.RED) diff --git a/bin2d/main.tscn b/bin2d/main.tscn new file mode 100644 index 00000000..bf8b0055 --- /dev/null +++ b/bin2d/main.tscn @@ -0,0 +1,86 @@ +[gd_scene load_steps=5 format=3 uid="uid://c5db4vfhoj8mt"] + +[ext_resource type="Script" path="res://Marker2D.gd" id="1_s2y4y"] + +[sub_resource type="CircleShape2D" id="CircleShape2D_5bkp5"] +radius = 64.0 + +[sub_resource type="RectangleShape2D" id="RectangleShape2D_pbyr1"] +size = Vector2(64, 128) + +[sub_resource type="CapsuleShape2D" id="CapsuleShape2D_042x3"] +radius = 32.0 +height = 128.0 + +[node name="Main" type="Node2D"] + +[node name="StaticBody2D" type="StaticBody2D" parent="."] +position = Vector2(208, 328) +input_pickable = true +metadata/_edit_group_ = true + +[node name="CollisionShape2D" type="CollisionShape2D" parent="StaticBody2D"] +shape = SubResource("CircleShape2D_5bkp5") + +[node name="StaticBody2D2" type="StaticBody2D" parent="."] +position = Vector2(432, 328) +input_pickable = true +metadata/_edit_group_ = true + +[node name="CollisionShape2D" type="CollisionShape2D" parent="StaticBody2D2"] +shape = SubResource("RectangleShape2D_pbyr1") + +[node name="StaticBody2D3" type="StaticBody2D" parent="."] +position = Vector2(624, 328) +input_pickable = true +metadata/_edit_group_ = true + +[node name="CollisionShape2D" type="CollisionShape2D" parent="StaticBody2D3"] +shape = SubResource("CapsuleShape2D_042x3") + +[node name="StaticBody2D4" type="StaticBody2D" parent="."] +position = Vector2(808, 328) +input_pickable = true +metadata/_edit_group_ = true + +[node name="CollisionPolygon2D" type="CollisionPolygon2D" parent="StaticBody2D4"] +position = Vector2(0, -16) +polygon = PackedVector2Array(-32, -40, -8, -16, -32, 16, -8, 48, -32, 72, 32, 72, 8, 48, 32, 16, 8, -16, 32, -40) + +[node name="StaticBody2D5" type="StaticBody2D" parent="."] +position = Vector2(1000, 328) +input_pickable = true +metadata/_edit_group_ = true + +[node name="CollisionPolygon2D" type="CollisionPolygon2D" parent="StaticBody2D5"] +position = Vector2(0, -16) +polygon = PackedVector2Array(32, -48, -32, -48, -32, 80, 32, 80, 32, -48, 16, -32, 16, 64, -16, 64, -16, -32, 16, -32) + +[node name="Marker2D" type="Marker2D" parent="."] +position = Vector2(208, 312) +gizmo_extents = 64.0 +script = ExtResource("1_s2y4y") + +[node name="Marker2D2" type="Marker2D" parent="."] +position = Vector2(424, 312) +gizmo_extents = 64.0 +script = ExtResource("1_s2y4y") + +[node name="Marker2D3" type="Marker2D" parent="."] +position = Vector2(624, 312) +gizmo_extents = 64.0 +script = ExtResource("1_s2y4y") + +[node name="Marker2D4" type="Marker2D" parent="."] +position = Vector2(808, 312) +gizmo_extents = 64.0 +script = ExtResource("1_s2y4y") + +[node name="Marker2D5" type="Marker2D" parent="."] +position = Vector2(976, 320) +gizmo_extents = 64.0 +script = ExtResource("1_s2y4y") + +[node name="Camera2D" type="Camera2D" parent="."] +position = Vector2(592, 320) +zoom = Vector2(1.275, 1.275) diff --git a/bin2d/project.godot b/bin2d/project.godot index 2b374e9c..cb57e22c 100644 --- a/bin2d/project.godot +++ b/bin2d/project.godot @@ -11,9 +11,7 @@ config_version=5 [application] config/name="Godot Rapier 2D" -run/main_scene="res://test.tscn" config/features=PackedStringArray("4.3", "Forward Plus") -config/icon="res://logo_square.png" [display] diff --git a/bin2d/ray_cast_2d.gd b/bin2d/ray_cast_2d.gd new file mode 100644 index 00000000..929e754d --- /dev/null +++ b/bin2d/ray_cast_2d.gd @@ -0,0 +1,12 @@ +extends ShapeCast2D + + +# Called when the node enters the scene tree for the first time. +func _ready() -> void: + pass # Replace with function body. + + +# Called every frame. 'delta' is the elapsed time since the previous frame. +func _process(delta: float) -> void: + print(get_collision_point(0)) + print(get_collision_point(1)) diff --git a/bin2d/test.gd b/bin2d/test.gd index 41f484d5..886f8a05 100644 --- a/bin2d/test.gd +++ b/bin2d/test.gd @@ -1,14 +1,9 @@ extends Node2D -@export var body: RigidBody2D -var collider:= RapierColliderJSON.new() -func _on_timer_timeout(): - get_tree().quit() - - -func _process(delta: float) -> void: - return +func _ready() -> void: var space := get_viewport().find_world_2d().direct_space_state as RapierDirectSpaceState2D - var space_json := FileAccess.open("user://shapes.json", FileAccess.WRITE) - space_json.store_string(space.export_json()) + var space_json := FileAccess.open("user://space.json", FileAccess.WRITE) + var shapes_json := FileAccess.open("user://shapes.json", FileAccess.WRITE) + #space_json.store_string(space.export_json()) + #shapes_json.store_string(RapierPhysicsServer2D.shapes_export_json()) diff --git a/bin2d/test.tscn b/bin2d/test.tscn index 30cca92c..29ba447b 100644 --- a/bin2d/test.tscn +++ b/bin2d/test.tscn @@ -1,102 +1,40 @@ -[gd_scene load_steps=11 format=3 uid="uid://d4exls53gi0df"] +[gd_scene load_steps=6 format=3 uid="uid://cn4jscixu2bmf"] -[ext_resource type="Script" path="res://test.gd" id="1_25l3y"] -[ext_resource type="Script" path="res://collision_shape_2d.gd" id="2_phxx1"] -[ext_resource type="Script" path="res://area_2d.gd" id="3_65y5d"] -[ext_resource type="Script" path="res://rigid_body_2d_3.gd" id="4_tdasl"] +[ext_resource type="PackedScene" uid="uid://c7x3cn5xnyu5r" path="res://static.tscn" id="1_57sdo"] +[ext_resource type="Script" path="res://test.gd" id="3_cq71l"] +[ext_resource type="Script" path="res://ray_cast_2d.gd" id="4_jkjeo"] -[sub_resource type="WorldBoundaryShape2D" id="WorldBoundaryShape2D_j5l1g"] +[sub_resource type="CircleShape2D" id="CircleShape2D_6y2xx"] +radius = 1.0 -[sub_resource type="RectangleShape2D" id="RectangleShape2D_sg3i2"] +[sub_resource type="CircleShape2D" id="CircleShape2D_ykjo6"] -[sub_resource type="RectangleShape2D" id="RectangleShape2D_l7hpj"] -size = Vector2(10, 10) +[node name="SceneRayOnSwitch" type="Node2D"] -[sub_resource type="CircleShape2D" id="CircleShape2D_54ruq"] +[node name="StaticBody2D3" parent="." instance=ExtResource("1_57sdo")] +position = Vector2(840, 536) +rotation = -0.00976837 +scale = Vector2(1.38592, 14.6304) -[sub_resource type="CircleShape2D" id="CircleShape2D_qdw7a"] -radius = 139.004 +[node name="Node2D" type="Node2D" parent="."] +script = ExtResource("3_cq71l") -[sub_resource type="CircleShape2D" id="CircleShape2D_6ybn3"] +[node name="Node2D2" type="Node2D" parent="."] -[node name="Test" type="Node2D"] -script = ExtResource("1_25l3y") +[node name="RayCast2D" type="ShapeCast2D" parent="."] +shape = SubResource("CircleShape2D_6y2xx") +target_position = Vector2(195, 77) +script = ExtResource("4_jkjeo") -[node name="Camera2D" type="Camera2D" parent="."] -scale = Vector2(4, 4) -zoom = Vector2(2, 2) - -[node name="Timer" type="Timer" parent="."] -wait_time = 10.0 - -[node name="StaticBody2D" type="StaticBody2D" parent="."] -collision_layer = 3 -collision_mask = 3 - -[node name="CollisionShape2D" type="CollisionShape2D" parent="StaticBody2D"] -shape = SubResource("WorldBoundaryShape2D_j5l1g") - -[node name="StaticBody2D2" type="StaticBody2D" parent="."] -position = Vector2(-52, -141) - -[node name="PinJoint2D" type="PinJoint2D" parent="StaticBody2D2"] -position = Vector2(2, 1) -node_a = NodePath("..") -node_b = NodePath("../../RigidBody2D2") - -[node name="CollisionShape2D" type="CollisionShape2D" parent="StaticBody2D2"] -shape = SubResource("RectangleShape2D_sg3i2") - -[node name="RigidBody2D" type="RigidBody2D" parent="."] -freeze = true +[node name="RigidBody2D" type="StaticBody2D" parent="."] [node name="CollisionShape2D" type="CollisionShape2D" parent="RigidBody2D"] -position = Vector2(0, -6) -shape = SubResource("RectangleShape2D_l7hpj") -script = ExtResource("2_phxx1") +position = Vector2(65, 27) +shape = SubResource("CircleShape2D_ykjo6") -[node name="RigidBody2D2" type="RigidBody2D" parent="."] -position = Vector2(-142, -100) +[node name="RigidBody2D2" type="StaticBody2D" parent="."] +position = Vector2(84, 32) [node name="CollisionShape2D" type="CollisionShape2D" parent="RigidBody2D2"] -shape = SubResource("CircleShape2D_54ruq") - -[node name="RayCast2D" type="RayCast2D" parent="."] -position = Vector2(0, -25) -hit_from_inside = true - -[node name="Area2D" type="Area2D" parent="."] -collision_layer = 2 -collision_mask = 5 -script = ExtResource("3_65y5d") - -[node name="CollisionShape2D" type="CollisionShape2D" parent="Area2D"] -position = Vector2(210, -17) -shape = SubResource("CircleShape2D_qdw7a") - -[node name="RigidBody2D3" type="RigidBody2D" parent="."] -position = Vector2(208, -126) -collision_layer = 2 -collision_mask = 2 -script = ExtResource("4_tdasl") - -[node name="CollisionShape2D" type="CollisionShape2D" parent="RigidBody2D3"] -shape = SubResource("CircleShape2D_6ybn3") - -[node name="RigidBody2D4" type="RigidBody2D" parent="."] -position = Vector2(211, -67) -collision_layer = 2 - -[node name="CollisionShape2D" type="CollisionShape2D" parent="RigidBody2D4"] -shape = SubResource("CircleShape2D_6ybn3") - -[node name="RigidBody2D5" type="RigidBody2D" parent="."] -position = Vector2(211, -150) -collision_mask = 2 - -[node name="CollisionShape2D" type="CollisionShape2D" parent="RigidBody2D5"] -shape = SubResource("CircleShape2D_6ybn3") - -[connection signal="timeout" from="Timer" to="." method="_on_timer_timeout"] -[connection signal="body_entered" from="Area2D" to="Area2D" method="_on_body_entered"] -[connection signal="body_entered" from="RigidBody2D3" to="RigidBody2D3" method="_on_body_entered"] +position = Vector2(65, 27) +shape = SubResource("CircleShape2D_ykjo6") diff --git a/src/rapier_wrapper/query.rs b/src/rapier_wrapper/query.rs index a919c934..f01c6ddb 100644 --- a/src/rapier_wrapper/query.rs +++ b/src/rapier_wrapper/query.rs @@ -215,6 +215,14 @@ impl PhysicsEngine { shape_vel2: Vector, shape_info2: ShapeInfo, ) -> ShapeCastResult { + let mut shape_vel1 = shape_vel1; + if shape_vel1 == Vector::zeros() { + shape_vel1 = Vector::identity() * 1e-3; + } + let mut shape_vel2 = shape_vel2; + if shape_vel2 == Vector::zeros() { + shape_vel2 = Vector::identity() * 1e-3; + } let mut result = ShapeCastResult::new(); if let Some(raw_shared_shape1) = self.get_shape(shape_info1.handle) { let shared_shape1 = scale_shape(raw_shared_shape1, shape_info1); @@ -235,8 +243,9 @@ impl PhysicsEngine { shared_shape2.as_ref(), shape_cast_options, ); - if let Ok(hit) = toi_result { - if let Some(hit) = hit { + match toi_result { + Ok(None) => {} + Ok(Some(hit)) => { result.collided = true; result.toi = hit.time_of_impact; result.normal1 = hit.normal1.into_inner(); @@ -244,8 +253,9 @@ impl PhysicsEngine { result.pixel_witness1 = hit.witness1.coords; result.pixel_witness2 = hit.witness2.coords; } - // can we get a hit without a result? - godot_error!("hit without a result"); + Err(err) => { + godot_error!("toi error: {:?}", err); + } } } } @@ -263,6 +273,10 @@ impl PhysicsEngine { physics_collision_objects: &PhysicsCollisionObjects, space: &RapierSpace, ) -> ShapeCastResult { + let mut shape_vel = shape_vel; + if shape_vel == Vector::zeros() { + shape_vel = Vector::identity() * 1e-3; + } let mut result = ShapeCastResult::new(); if let Some(raw_shared_shape) = self.get_shape(shape_info.handle) { let shared_shape = scale_shape(raw_shared_shape, shape_info); @@ -285,9 +299,11 @@ impl PhysicsEngine { }; filter.predicate = Some(&predicate); let mut shape_cast_options = ShapeCastOptions::default(); + //shape_cast_options.max_time_of_impact = Real::MAX; shape_cast_options.max_time_of_impact = 1.0; shape_cast_options.compute_impact_geometry_on_penetration = true; shape_cast_options.stop_at_penetration = true; + shape_cast_options.target_distance = 0.0; if let Some((collider_handle, hit)) = physics_world.physics_objects.query_pipeline.cast_shape( &physics_world.physics_objects.rigid_body_set, @@ -305,10 +321,19 @@ impl PhysicsEngine { result.normal2 = hit.normal2.into_inner(); result.collider = collider_handle; result.user_data = physics_world.get_collider_user_data(collider_handle); - // first is world space + // first is in world space let witness1 = hit.witness1; - // second is local space - let witness2 = shape_transform.transform_point(&hit.witness2); + // second is translated by collider transform + let mut witness2 = hit.witness2; + if let Some(collider) = physics_world + .physics_objects + .collider_set + .get(collider_handle) + { + witness2 += collider.position().translation.vector; + } else { + godot_error!("collider not found"); + } result.pixel_witness1 = witness1.coords; result.pixel_witness2 = witness2.coords; } diff --git a/src/servers/mod.rs b/src/servers/mod.rs index b6b0cb7b..5fecdf9d 100644 --- a/src/servers/mod.rs +++ b/src/servers/mod.rs @@ -6,10 +6,6 @@ pub mod rapier_physics_server_2d; #[cfg(feature = "dim3")] pub mod rapier_physics_server_3d; pub mod rapier_physics_server_extra; -#[cfg(feature = "dim2")] -pub mod rapier_physics_server_factory_2d; -#[cfg(feature = "dim3")] -pub mod rapier_physics_server_factory_3d; pub mod rapier_physics_server_impl; pub mod rapier_project_settings; #[cfg(feature = "dim2")] @@ -21,7 +17,7 @@ pub fn register_server() { use godot::engine::PhysicsServer2DManager; let mut manager = PhysicsServer2DManager::singleton(); let factory = - crate::servers::rapier_physics_server_factory_2d::RapierPhysicsServerFactory2D::new_alloc(); + crate::servers::rapier_physics_server_2d::RapierPhysicsServerFactory2D::new_alloc(); manager.register_server("Rapier2D".into(), factory.callable("create_server")); } #[cfg(feature = "dim3")] @@ -29,7 +25,7 @@ pub fn register_server() { use godot::engine::PhysicsServer3DManager; let mut manager = PhysicsServer3DManager::singleton(); let factory = - crate::servers::rapier_physics_server_factory_3d::RapierPhysicsServerFactory3D::new_alloc(); + crate::servers::rapier_physics_server_3d::RapierPhysicsServerFactory3D::new_alloc(); manager.register_server("Rapier3D".into(), factory.callable("create_server")); } pub fn register_scene() { diff --git a/src/servers/rapier_physics_server_2d.rs b/src/servers/rapier_physics_server_2d.rs index 2d665f49..652b6f93 100644 --- a/src/servers/rapier_physics_server_2d.rs +++ b/src/servers/rapier_physics_server_2d.rs @@ -8,6 +8,16 @@ use godot::prelude::*; use super::rapier_physics_server_impl::RapierPhysicsServerImpl; use crate::types::*; +#[derive(GodotClass, Default)] +#[class(base=Object,init,tool)] +pub struct RapierPhysicsServerFactory2D; +#[godot_api] +impl RapierPhysicsServerFactory2D { + #[func] + fn create_server() -> Gd { + RapierPhysicsServer2D::new_alloc() + } +} #[derive(GodotClass)] #[class(base=PhysicsServer2DExtension, tool)] pub struct RapierPhysicsServer2D { diff --git a/src/servers/rapier_physics_server_3d.rs b/src/servers/rapier_physics_server_3d.rs index 65ef0347..16039570 100644 --- a/src/servers/rapier_physics_server_3d.rs +++ b/src/servers/rapier_physics_server_3d.rs @@ -6,6 +6,16 @@ use godot::prelude::*; use super::rapier_physics_server_impl::RapierPhysicsServerImpl; use crate::types::*; +#[derive(GodotClass, Default)] +#[class(base=Object,init,tool)] +pub struct RapierPhysicsServerFactory3D; +#[godot_api] +impl RapierPhysicsServerFactory3D { + #[func] + fn create_server() -> Gd { + RapierPhysicsServer3D::new_alloc() + } +} #[derive(GodotClass)] #[class(base=PhysicsServer3DExtension, tool)] pub struct RapierPhysicsServer3D { diff --git a/src/servers/rapier_physics_server_factory_2d.rs b/src/servers/rapier_physics_server_factory_2d.rs deleted file mode 100644 index 58963b64..00000000 --- a/src/servers/rapier_physics_server_factory_2d.rs +++ /dev/null @@ -1,14 +0,0 @@ -use godot::obj::NewAlloc; -use godot::prelude::*; - -use crate::servers::rapier_physics_server_2d::*; -#[derive(GodotClass, Default)] -#[class(base=Object,init,tool)] -pub struct RapierPhysicsServerFactory2D; -#[godot_api] -impl RapierPhysicsServerFactory2D { - #[func] - fn create_server() -> Gd { - RapierPhysicsServer2D::new_alloc() - } -} diff --git a/src/servers/rapier_physics_server_factory_3d.rs b/src/servers/rapier_physics_server_factory_3d.rs deleted file mode 100644 index 4e821cde..00000000 --- a/src/servers/rapier_physics_server_factory_3d.rs +++ /dev/null @@ -1,14 +0,0 @@ -use godot::obj::NewAlloc; -use godot::prelude::*; - -use crate::servers::rapier_physics_server_3d::*; -#[derive(GodotClass, Default)] -#[class(base=Object,init,tool)] -pub struct RapierPhysicsServerFactory3D; -#[godot_api] -impl RapierPhysicsServerFactory3D { - #[func] - fn create_server() -> Gd { - RapierPhysicsServer3D::new_alloc() - } -} diff --git a/src/spaces/rapier_direct_space_state_impl.rs b/src/spaces/rapier_direct_space_state_impl.rs index 983f1375..c25ced99 100644 --- a/src/spaces/rapier_direct_space_state_impl.rs +++ b/src/spaces/rapier_direct_space_state_impl.rs @@ -331,8 +331,8 @@ impl RapierDirectSpaceStateImpl { result.collider; query_excluded_info.query_exclude_size += 1; unsafe { - (*results_out.add(array_idx)) = vector_to_godot(result.pixel_witness1); - (*results_out.add(array_idx + 1)) = vector_to_godot(result.pixel_witness2); + (*results_out.add(array_idx)) = vector_to_godot(result.pixel_witness2); + (*results_out.add(array_idx + 1)) = vector_to_godot(result.pixel_witness1); } array_idx += 2; cpt += 1; @@ -397,6 +397,7 @@ impl RapierDirectSpaceStateImpl { r_info.normal = vector_to_godot(result.normal1); r_info.rid = rid; r_info.shape = shape_index as i32; + r_info.point = vector_to_godot(result.pixel_witness1); } true }