#!/usr/bin/wish

# walking_men.tcl
# Bones Animation of walking men in a Canvas
#
# Copyright 2005: Andre Adrian, adrianandre aT compuserve dOt de
# Version: 21aug2005
#
# License: Use this software like you want. You are not allowed to
# delete the name of the original author (Andre Adrian).
# You are not allowed to monopolize this software or the algorithms
# as your intellectual property.
# There is no warrenty, no fitness for purpose and so on.

proc joint {x1 y1 x3 y3 r} {
  # calculate joint position = point of intersection of 2 circles
  # x1 y3: hip position or center of circle 1
  # x3 y3: ankle position or center of circle 2
  # r: thigh length or radius (same radius for both circles)
  # returns 2 values for joint position as list
  
  set xd [expr $x3-$x1]
  set yd [expr $y3-$y1]
  set d  [expr sqrt($xd*$xd + $yd*$yd)]
  if {$d > [expr 2*$r]} {
    # error: solution impossible, return something not to wrong
    set x2 [expr ($x1+$x3)/2]
    set y2 [expr ($y1+$y3)/2]
    return [list $x2 $y2]
  }
  
  # for rotation use right angled triangle with hypotenuse d
  # sin(th) = yd/d, cos(th) = xd/d
  # Rotate [r 0] by [xd/d yd/d]
  # [px py] is vector with length r on line between hip to ankle
  set px [expr $r*$xd/$d]
  set py [expr $r*$yd/$d]

  # for rotation use right angled triangle with hypotenuse r
  # sin(th) = sqrt(r^2 - (d/2)^2)/r, cos(th) = d/(2*r)
  # Rotate [px py] by [d/(2*r) sqrt(r^2 - (d/2)^2)/r]
  # [x2 y2] is vector length r pointing to point of intersection
  # of 2 circles with centers hip and ankle and radius of r each
  set d2 [expr $d/2]
  set a1 [expr $d2/$r]
  set a2 [expr sqrt($r*$r - $d2*$d2)/$r]
  set x2 [expr $x1 + $a1*$px - $a2*$py]
  set y2 [expr $y1 + $a2*$px + $a1*$py]
  return [list $x2 $y2]
}

proc bodydraw {tag r  x0 y0 dx dy} {
  # draw torso and head
  # tag: graphical objects tag
  # r: thigh length
  # x0 y0: hip joint position (common parameter)
  # dx dy: hip joint movement vector (common parameter)
  # returns command and parameters list for call again

  set x1 [expr $x0-5]   ;# shoulder
  set y1 [expr $y0-$r]
  set x2 $x1            ;# head
  set y2 [expr $y1-17]
  
  # erase old graphical object, draw new one
  .ca delete $tag
  .ca create oval [expr $x2-10] [expr $y2-10] \
    [expr $x2+5] [expr $y2+10] \
    -tags $tag -fill orange -outline orange
  .ca create line $x2 $y2 $x1 $y1 \
    -tags $tag -width 9 -capstyle round -fill orange  
  .ca create line $x1 $y1 $x0 $y0 \
    -tags $tag -width 11 -capstyle round -fill blue
    
  # parameters for call again
  return [list bodydraw $tag $r]
}

proc armdraw {tag h r i di imax dw r0  x0 y0 dx dy} {
  # animate and draw one arm
  # tag: graphical objects tag
  # h: distance shoulder to hand
  # r: forearm length
  # i: hand movement index
  # di: hand movement direction
  # imax: hand movement trigger reverse value
  # dw: arm waving multiplier
  # r0: y distance hip joint to shoulder
  # x0 y0: hip joint position (common parameter)
  # dx dy: hip joint movement vector (common parameter)
  # returns command and parameters list for call again
  
  set x0 [expr $x0-5]   ;# shoulder position
  set y0 [expr $y0-$r0]
  
  # Hand position relative to shoulder
  set w [expr $i*$dw]
  set h1 [expr $h]
  
  # Hand movement (quadratic function lift movement)
  set q [expr (abs(1.0*$i/$imax))]
  set h2 [expr $h1-5*$q*$q]  
  
  set xy [joint $w $h2 0 0 $r]
  set x1 [lindex $xy 0]   ;# separate the 2 return values
  set y1 [lindex $xy 1]
  
  set x1 [expr $x0+$x1]   ;# elbow position absolute
  set y1 [expr $y0+$y1]
  set x2 [expr $x0+$w]    ;# hand position absolute
  set y2 [expr $y0+$h2]

  # erase old graphical object, draw new one
  .ca delete $tag
  .ca create line $x0 $y0 $x1 $y1 -tags $tag \
    -width 8 -capstyle round -fill red
  .ca create line $x1 $y1 $x2 $y2 -tags $tag \
    -width 7 -capstyle round -fill blue
  .ca create oval [expr $x2-5] [expr $y2-5] \
    [expr $x2+5] [expr $y2+5] \
    -tags $tag -fill orange -outline orange
    
  # hand relative to shoulder Movement
  set i [expr $i+$di]
  if {abs($i) >= $imax} {
    set di [expr 0-$di]
  }
  
  return [list armdraw $tag $h $r $i $di $imax $dw $r0]
}

proc legdraw {tag h r i di imax dw  x0 y0 dx dy} {
  # animate and draw one Leg
  # tag: graphical objects tag
  # h: distance hip joint to foot
  # r: thigh length
  # i: foot movement index
  # di: foot movement direction
  # imax: foot movement trigger reverse value
  # dw: foot movement vector
  # x0 y0: hip joint position (common parameter)
  # dx dy: hip joint movement vector (common parameter)
  # returns command and parameters list for call again

  # Foot position relative to hip joint
  set w [expr $i*$dw]
  set h1 [expr $h-$w*$dy]
  
  # Foot movement (linear lift movement)
  if {$di > 0} {
    # Foot moving backwards = on floor
    set h2 $h1
  } else {
    # Foot moving forewards = lift
    # (linear function lift movement)
    # 1.0* to force float calculation
    set h2 [expr $h1-10*(1-abs(1.0*$i/$imax))]
  }
  
  set xy [joint 0 0 $w $h2 $r]
  set x1 [lindex $xy 0]   ;# separate the 2 return values
  set y1 [lindex $xy 1]
  
  set x1 [expr $x0+$x1]   ;# knee position absolute
  set y1 [expr $y0+$y1]
  set x2 [expr $x0+$w]    ;# ankle position absolute
  set y2 [expr $y0+$h2]

  if {$h2 >= $h1} {
    # Foot flat on floor
    set x3 [expr $x2-$r/3]  ;# toes position absolute
    set y3 $y2
  } else {
    # Ankle lifted. Toes position is normal vector of knee 
    # to ankle line with length r/3
    set x3 [expr $x2+($y1-$y2)/3]
    set y3 [expr $y2-($x1-$x2)/3]    
    if {$y3 > $y0+$h1} {
      # Toes are below floor.
      # Average Toes position to avoid short Foot length
      set x3 [expr ($x3 + $x2-$r/3)/2]
      set y3 [expr $y0+$h1]
    }
  }

  # erase old graphical object, draw new one
  .ca delete $tag
  .ca create line $x0 $y0 $x1 $y1 -tags $tag \
    -width 10 -capstyle round -fill blue
  .ca create line $x1 $y1 $x2 $y2 -tags $tag \
    -width 9 -capstyle round -fill green
  .ca create line $x2 $y2 $x3 $y3 -tags $tag \
    -width 9 -capstyle round -fill black
  
  # Ankle relative to hip joint Movement
  set i [expr $i+$di]
  if {abs($i) >= $imax} {
    set di [expr 0-$di]
  }
  
  return [list legdraw $tag $h $r $i $di $imax $dw]
}

proc doManList {dl x0 y0 dx dy} {
  # draw graphical objects for one man
  # dl: commands with parameters list
  # x0 y0: hip joint position (common parameter)
  # dx dy: hip joint movement vector (common parameter)
  # returns command and parameters list for call again

  foreach command $dl {
    lappend command $x0 $y0 $dx $dy
    lappend newdl [eval $command]
  }
  
  # hip joint Movement
  set x0 [expr $x0+$dx]
  set y0 [expr $y0+$dy]
  if {$x0 < 0} {
    set x0 [expr $x0+[.ca cget -width]]
  }
  if {$x0 > [.ca cget -width]} {
    set x0 0
  }
  if {$y0 < 65 || $y0 > [.ca cget -height]-65} {
    # change direction
    set dy [expr 0-$dy]
  }
  
  return [list doManList $newdl $x0 $y0 $dx $dy]
}  

proc manInit {i r dw v} {
  # create Man List for animation
  # i: Man identifier integer
  # r: thigh length
  # dw: arm waving multiplier
  # v: velocity of animation
  # returns command and parameters list for animation

  set h [expr 1.8*$r]
  set r1 [expr 0.67*$r] ;# forearm length
  set h1 [expr 1.8*$r1]
  set f [expr 20.0/$v]

  lappend dl [list armdraw  RArm$i $h1 $r1 0  1 $f $dw $r]
  lappend dl [list legdraw  RLeg$i $h $r   0  1 $f $v]
  lappend dl [list bodydraw Body$i $r]
  lappend dl [list legdraw  LLeg$i $h $r   0 -1 $f $v]
  lappend dl [list armdraw  LArm$i $h1 $r1 0 -1 $f $dw $r]

  return $dl
}

proc doDrawList {sl} {
  # draw all graphical objects (do it in one go to avoid flicker)
  # sl: commands with parameters list
  # returns nothing, but calls itself with after
  
  foreach command $sl {
    lappend newsl [eval $command]
  }
  
  # call again with new parameters
  after 40 [list doDrawList $newsl]
}

# create a canvas to draw the graphical objects on
canvas .ca -width 400 -height 200
pack .ca

# init Man 1 - slow speed, go up and down, little arm waving
lappend sl [list doManList [manInit 1 30 0.5 1] 30 100 -1 0.1]

# init Man 2 - medium speed, go straight, more arm waving
lappend sl [list doManList [manInit 2 28 0.7 1.5] 160 109 -1.5 0]

# init Man 3 - fast speed, go straight, much arm waving
lappend sl [list doManList [manInit 3 33 1.5 2] 300 100 -2 -0]

# start animation
doDrawList $sl