Files

108 lines
6.2 KiB
GDScript

extends RefCounted
var root: Node3D
var variation=0.0
var model
var feet=[]
var skin=preload("res://imp_skin.gd").new()
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()
skin=preload("res://imp_skin.gd").new();skin.setup(model)
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({"width":leg.position.x+knee.position.x+foot.position.x,"forward":leg.position.z+knee.position.z+foot.position.z,"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
skin.update()
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()*float(model.get_meta("knee_bend",1.0))
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 pose_work_hands():
var activity=root.get_meta("activity","")
if activity not in ["clean","mop","draw"]:return
var t=clock
for side in ["L","R"]:
if side=="L" and activity!="mop":continue
var arm=model.get_node("Arm"+side);var elbow=arm.get_node("Elbow");var hand=elbow.get_node("Hand")
var target=Vector3(.12,.35,.57) if side=="R" else Vector3(-.12,.54,.47)
if activity=="mop":target.z+=sin(t*3)*.10
elif activity=="clean":target=Vector3(.30+cos(t*6)*.04,.54+sin(t*6)*.035,.54)
else:target=Vector3(.26+cos(t*7)*.045,.84+sin(t*7)*.045,.53)
target=root.to_global(target)
var shoulder=arm.global_position;var l1=elbow.position.length();var l2=hand.position.length()
var delta=target-shoulder;var distance=clampf(delta.length(),absf(l1-l2)+.001,l1+l2-.001)
var direction=delta.normalized();target=shoulder+direction*distance
var outward=root.global_basis.x*(-1 if side=="L" else 1)
var bend=(outward-direction*outward.dot(direction)).normalized()
var along=(l1*l1-l2*l2+distance*distance)/(2*distance)
var joint=shoulder+direction*along+bend*sqrt(maxf(0,l1*l1-along*along))
arm.quaternion=Quaternion(elbow.position.normalized(),(arm.get_parent().to_local(joint)-arm.position).normalized())
elbow.quaternion=Quaternion(hand.position.normalized(),(arm.to_local(target)-elbow.position).normalized())
hand.global_rotation=root.global_rotation
arm_pose[0 if side=="L" else 1]=arm.rotation
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*f.width+forward*(f.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)
if acting:pose_work_hands()
skin.update()