Fix order of results from shape result (#120)

- Fix #105

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)

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)

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]
 

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))

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())

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")

src/rapier_wrapper/query.rs

@@ -215,6 +215,14 @@ impl PhysicsEngine {
         shape_vel2: Vector<Real>,
         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,17 +243,19 @@ 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();
                         result.normal2 = hit.normal2.into_inner();
                         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;
                 }