Improved surface normal alignment

This commit is contained in:
2024-02-20 23:58:44 +01:00
parent 1e5ab718bf
commit 4e632608e1
4 changed files with 55 additions and 37 deletions

View File

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