Improved surface normal alignment

client/gizmo/Gizmo3D.tscn

@@ -0,0 +1,34 @@
+[gd_scene load_steps=6 format=3 uid="uid://cwl3bqdt5x5b8"]
+
+[sub_resource type="BoxMesh" id="BoxMesh_u4ygv"]
+size = Vector3(0.05, 0.05, 0.05)
+
+[sub_resource type="BoxMesh" id="BoxMesh_qml7q"]
+size = Vector3(0.025, 0.025, 0.265)
+
+[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_a8qud"]
+albedo_color = Color(1, 0, 0, 1)
+
+[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_m0kih"]
+albedo_color = Color(0, 1, 0, 1)
+
+[sub_resource type="StandardMaterial3D" id="StandardMaterial3D_8tfkb"]
+albedo_color = Color(0, 0, 1, 1)
+
+[node name="Gizmo3D" type="MeshInstance3D"]
+mesh = SubResource("BoxMesh_u4ygv")
+
+[node name="X" type="MeshInstance3D" parent="."]
+transform = Transform3D(-4.37114e-08, 0, 1, 0, 1, 0, -1, 0, -4.37114e-08, 0.174581, 0, -7.63118e-09)
+mesh = SubResource("BoxMesh_qml7q")
+surface_material_override/0 = SubResource("StandardMaterial3D_a8qud")
+
+[node name="Y" type="MeshInstance3D" parent="."]
+transform = Transform3D(1, 0, 0, 0, -4.37114e-08, 1, 0, -1, -4.37114e-08, 0, 0.174581, -7.63118e-09)
+mesh = SubResource("BoxMesh_qml7q")
+surface_material_override/0 = SubResource("StandardMaterial3D_m0kih")
+
+[node name="Z" type="MeshInstance3D" parent="."]
+transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0.174581)
+mesh = SubResource("BoxMesh_qml7q")
+surface_material_override/0 = SubResource("StandardMaterial3D_8tfkb")

client/math/Math.gd

@@ -8,3 +8,8 @@ static func dampf(from: float, to: float, weight: float, smoothing: float = 0.75
 
 static func damp_angle(from: float, to: float, weight: float, smoothing: float = 0.75):
 	return lerp_angle(from, to, 1 - exp(-smoothing * weight))
+
+static func from_to_rotation(from: Vector3, to: Vector3) -> Quaternion:
+	var axis := from.cross(to).normalized()
+	var angle := from.angle_to(to)
+	return Quaternion(axis, angle)

client/player/Player.tscn

@@ -125,7 +125,6 @@ libraries = {
 skeleton = NodePath("../Model/Female/Armature/GeneralSkeleton")
 movement = NodePath("../Movement")
 footsteps = SubResource("AudioStreamRandomizer_4yj1k")
-step_threshold = 0.1
 
 [node name="Health" parent="." instance=ExtResource("2_np5ag")]
 max_value = 100.0

client/player/footsteps/FootstepsComponent.gd

@@ -31,17 +31,14 @@ func _ready():
 
 func _process(delta: float):
 	for foot in feet:
-		var global_pose := skeleton.get_bone_global_pose(foot.bone_id)
 		var old_position := foot.position
-		foot.position = skeleton.to_global(global_pose.origin)
+		var global_pose := skeleton.get_bone_global_pose(foot.bone_id)
+		var world_pose := skeleton.global_transform * global_pose
+		foot.position = world_pose.origin
 
 		if foot.floor_distance < ik_step_threshold:
-			var bone_to_global := skeleton.global_transform * global_pose
-			var normal_local := bone_to_global.basis.transposed() * foot.normal
-			var pose := skeleton.get_bone_pose(foot.bone_id)
-			var rot := get_foot_rotation(pose.basis, normal_local)
 			ik_time = minf(ik_time + ik_speed * delta, 1.0)
-			skeleton.set_bone_pose_rotation(foot.bone_id, pose.basis.slerp(rot, ik_time))
+			align_surface_normal(foot, world_pose)
 		else:
 			ik_time = 0
 
@@ -81,31 +78,14 @@ func play(audio_player: AudioStreamPlayer3D):
 	audio_player.stream = footsteps
 	audio_player.play()
 
-func get_foot_rotation(base: Basis, normal: Vector3) -> Basis:
-	var rot := align_with_y(base, normal)
-	return rot.rotated(Vector3.UP, PI)
+func align_surface_normal(foot: Foot, world_pose: Transform3D):
+	var pose := skeleton.get_bone_pose(foot.bone_id)
+	var local_normal := world_pose.basis.transposed() * foot.normal
+	var new_basis := get_foot_rotation(pose.basis, local_normal)
+	skeleton.set_bone_pose_rotation(foot.bone_id, pose.basis.slerp(new_basis, ik_time))
 
-func align_with_y(base: Basis, normal: Vector3) -> Basis:
-	base.x = -base.z.cross(normal)
-	base.y = normal
-	return base.orthonormalized()
-
-# func get_foot_rotation2(base: Basis, normal: Vector3) -> Basis:
-# 	normal.x = -normal.x
-# 	normal.z = -normal.z
-# 	return align_up(base, normal)
-
-# func align_up(base: Basis, normal: Vector3) -> Basis:
-# 	var result = Basis()
-# 	var node_scale = base.get_scale().abs()
-
-# 	result.x = normal.cross(base.z)
-# 	result.y = normal
-# 	result.z = base.x.cross(normal)
-
-# 	result = result.orthonormalized()
-# 	result.x *= node_scale.x
-# 	result.y *= node_scale.y
-# 	result.z *= node_scale.z
-
-# 	return result
+func get_foot_rotation(old: Basis, normal: Vector3) -> Basis:
+	var new := Basis(-old.x, normal, old.z)
+	new = new.orthonormalized()
+	new = new.rotated(Vector3.UP, PI)
+	return new