83 lines
4.7 KiB
GDScript
83 lines
4.7 KiB
GDScript
|
|
extends RefCounted
|
||
|
|
var root: Node3D
|
||
|
|
var variation=0.0
|
||
|
|
var model
|
||
|
|
var feet=[]
|
||
|
|
var previous=Vector3.ZERO
|
||
|
|
var phase=.1
|
||
|
|
var clock=0.0
|
||
|
|
var weight=0.0
|
||
|
|
var arm_pose=[Vector3.ZERO,Vector3.ZERO]
|
||
|
|
var body_rotation=Vector3.ZERO
|
||
|
|
var body_height=0.0
|
||
|
|
var last_direction=Vector3.ZERO
|
||
|
|
const CYCLE_DISTANCE=.48
|
||
|
|
func setup(record):
|
||
|
|
model=record.model;root=record.root;variation=record.phase;previous=root.global_position
|
||
|
|
feet.clear()
|
||
|
|
phase=.1;weight=0;body_height=0;body_rotation=Vector3.ZERO;last_direction=Vector3.ZERO
|
||
|
|
for side in ["L","R"]:
|
||
|
|
var leg=model.get_node("Leg"+side);var knee=leg.get_node("Knee");var foot=knee.get_node("Foot")
|
||
|
|
feet.append({"leg":leg,"knee":knee,"foot":foot,"side":side,"anchor":foot.global_position,"target":foot.global_position,"start":foot.global_position,"landing":foot.global_position,"swing":false,"yaw":root.rotation.y,"last_phase":.1 if side=="L" else .6})
|
||
|
|
func reset():
|
||
|
|
previous=root.global_position;weight=0;phase=.1;last_direction=Vector3.ZERO
|
||
|
|
model.position=Vector3.ZERO;body_height=0;body_rotation=Vector3.ZERO;model.rotation=Vector3.ZERO
|
||
|
|
for f in feet:f.leg.rotation=Vector3.ZERO;f.knee.rotation=Vector3.ZERO;f.foot.rotation=Vector3.ZERO
|
||
|
|
for f in feet:
|
||
|
|
f.anchor=f.foot.global_position;f.anchor.y=root.global_position.y+.06;f.target=f.anchor;f.swing=false;f.yaw=root.rotation.y
|
||
|
|
func solve(f):
|
||
|
|
var hip=f.leg.global_position;var l1=f.knee.position.length();var l2=f.foot.position.length()
|
||
|
|
var delta=f.target-hip;var distance=clampf(delta.length(),absf(l1-l2)+.001,l1+l2-.001)
|
||
|
|
var direction=delta.normalized();var forward=root.global_basis.z.normalized()
|
||
|
|
var bend=(forward-direction*forward.dot(direction)).normalized()
|
||
|
|
var along=(l1*l1-l2*l2+distance*distance)/(2*distance)
|
||
|
|
var elbow=hip+direction*along+bend*sqrt(maxf(0,l1*l1-along*along))
|
||
|
|
f.leg.quaternion=Quaternion(f.knee.position.normalized(),(f.leg.get_parent().to_local(elbow)-f.leg.position).normalized())
|
||
|
|
f.knee.quaternion=Quaternion(f.foot.position.normalized(),(f.leg.to_local(f.target)-f.knee.position).normalized())
|
||
|
|
f.foot.global_rotation=Vector3(0,f.yaw,0)
|
||
|
|
func update(dt,acting=false):
|
||
|
|
dt=minf(dt,.05);clock+=dt
|
||
|
|
var delta=root.global_position-previous;delta.y=0;previous=root.global_position
|
||
|
|
if delta.length()>.20:reset();delta=Vector3.ZERO
|
||
|
|
var distance=delta.length();var moving=distance>.00005
|
||
|
|
weight=move_toward(weight,1.0 if moving else 0.0,dt*5)
|
||
|
|
if moving:phase=fposmod(phase+distance/CYCLE_DISTANCE,1)
|
||
|
|
var forward=delta.normalized() if moving else root.global_basis.z.normalized()
|
||
|
|
if moving and last_direction.length_squared()>.5 and forward.dot(last_direction)<.75:
|
||
|
|
phase=.30 if not feet[0].swing else .80
|
||
|
|
if moving:last_direction=forward
|
||
|
|
for i in range(feet.size()):
|
||
|
|
var f=feet[i];var p=fposmod(phase+i*.5,1);var ground=root.global_position.y+.06
|
||
|
|
if moving:
|
||
|
|
if p>=.56:
|
||
|
|
if not f.swing:f.start=f.target;f.swing=true
|
||
|
|
f.landing=root.global_position+root.global_basis.x*(-.135 if f.side=="L" else .135)+forward*.08;f.landing.y=ground
|
||
|
|
var t=clampf((p-.56)/.44,0,1);f.target=f.start.lerp(f.landing,smoothstep(0,1,t));f.target.y=ground+sin(t*PI)*.065
|
||
|
|
else:
|
||
|
|
if f.swing:f.anchor=f.landing;f.anchor.y=ground;f.swing=false;f.yaw=root.rotation.y
|
||
|
|
f.target=f.anchor
|
||
|
|
else:
|
||
|
|
f.target.y=move_toward(f.target.y,ground,dt*.35)
|
||
|
|
if absf(f.target.y-ground)<.001:f.anchor=f.target;f.swing=false
|
||
|
|
f.last_phase=p
|
||
|
|
var desired_rotation=model.rotation if acting else Vector3(sin(phase*TAU)*.018*weight,0,sin(phase*TAU)*.025*weight)
|
||
|
|
body_rotation=body_rotation.lerp(desired_rotation,1-exp(-dt*9));model.rotation=body_rotation
|
||
|
|
var desired_height=model.position.y if acting and root.get_meta("activity","")=="hum" else -.025+sin(phase*TAU*2)*.007*weight+sin(clock*2)*.003*(1-weight)
|
||
|
|
body_height=lerpf(body_height,desired_height,1-exp(-dt*9));model.position.y=body_height
|
||
|
|
var drop=0.0
|
||
|
|
for f in feet:
|
||
|
|
var hip=f.leg.global_position;var reach=(f.knee.position.length()+f.foot.position.length())*.985
|
||
|
|
var horizontal=Vector2(hip.x-f.target.x,hip.z-f.target.z).length_squared()
|
||
|
|
drop=maxf(drop,hip.y-f.target.y-sqrt(maxf(.001,reach*reach-horizontal)))
|
||
|
|
model.position.y-=clampf(drop,0,.16)
|
||
|
|
for f in feet:solve(f)
|
||
|
|
for i in range(2):
|
||
|
|
var arm=model.get_node("ArmL" if i==0 else "ArmR")
|
||
|
|
var desired=arm.rotation if acting else Vector3(sin(phase*TAU+i*PI)*.22*weight,0,(-.06 if i==0 else .06)*(1-weight))
|
||
|
|
arm_pose[i]=arm_pose[i].lerp(desired,1-exp(-dt*8));arm.rotation=arm_pose[i]
|
||
|
|
arm.get_node("Elbow").rotation.x=-.05-sin(phase*TAU+i*PI)*.045*weight
|
||
|
|
model.get_node("Head").rotation=Vector3(sin(clock*2)*.012, sin(clock*.8+variation)*.025, -sin(phase*TAU)*.018*weight)
|
||
|
|
var blink=absf(sin(clock*.68+variation))
|
||
|
|
for side in ["L","R"]:model.get_node("Head/Eye"+side).scale.y=clampf(blink*24,.10,1)
|
||
|
|
model.get_node("Tail").rotation.y=sin(clock*2.4+variation)*(.12+.08*weight)
|