// Last edited on 2011-08-16 21:55:04 by stolfilocal
// Slave walking.


#declare slave_walking_hip_B_max  = +60;      // Max hip angle during walking.
#declare slave_walking_hip_B_min  =  -5;      // Min hip angle during walking.

#declare slave_walking_shoulder_B_max  = +20; // Max shoulder angle during walking.
#declare slave_walking_shoulder_B_min  = -15; // Min shoulder angle during walking.

#declare slave_walking_elbow_B  = 25;         // Elbow angle during walking.

#declare slave_walking_wphase_down_R = 0.25;  // {wphase} when the right foot lands.
#declare slave_walking_wphase_down_L = 0.75;  // {wphase} when the left foot lands.

// Common parameters:
  // {walk}   0 = not walking, 1 = walking with broad strides.
  // {wphase} phase in walking cycle (period 1).
  // {which} which leg or arm (+1 left, -1 right).

#macro slave_walking_arm_params(walk,which,wphase,arm_A)
  // Stores in {arm_A[i]} the parameters of a slave's arm during walking motion.

  // Compute the equivalent phase {wp} for the left arm:
  #if (which > 0)
    #local wp = mod(wphase, 1.0);
  #else
    #local wp = mod(wphase + 0.5, 1.0);
  #end

  #local sho_B_max = slave_walking_shoulder_B_max;
  #local sho_B_min = slave_walking_shoulder_B_min;
  
  #local sho_B_amp = walk * (sho_B_max - sho_B_min)/2;
  #local sho_B_mid = (sho_B_max + sho_B_min)/2;
  
  #local sho_B = sho_B_mid + sho_B_amp*cos(2*pi*wp);
  
  #local elb_B = walk * slave_walking_elbow_B;
  
  #declare arm_A[0] = 0;       // Shoulder A (axial upper arm twist).
  #declare arm_A[1] = sho_B;   // Shoulder B (forward swing).        
  #declare arm_A[2] = 5;       // Shoulder C (opening swing).        
  #declare arm_A[3] = 0;       // Elbow A (axial forearm twist).     
  #declare arm_A[4] = elb_B;   // Elbow B (forward bend).            
  #declare arm_A[5] = 0;       // Wrist A (inward bend).             

#end

#macro slave_walking_knee_critical_A(hip_LB,hip_RB,kne_RA)
  // Computes the angle at the left knee when the right heel touches down.
  // {hip_LB} angle at left hip (negative = back).
  // {hip_RB} angle at right hip (positive = forward).
  // {kne_RA} angle at right knee (positive = backward).
  
  #local M = slave_tigh_len;
  #local N = slave_shin_len;
  // Height of right leg, ankle artic to hip artic:
  #local hleg = M*cos(radians(hip_RB)) + N*cos(radians(hip_RB - kne_RA));
  // Height of left thigh, knee artic to hip artic:
  #local htig = M*cos(radians(hip_LB)); // height of left tigh.
  // Cosine of angle at left knee to achieve same height:
  #local kne_Lcos = (hleg - htig)/N;
  // Angle at left knee to achieve same height:
  #local kne_LA = degrees(acos(kne_Lcos));
  kne_LA
#end

#macro slave_walking_leg_params(walk,which,wphase,leg_A)
  // Stores in {leg_A[i]} the parameters of a slave's leg during walking motion.
  
  #local hip_B_max = slave_walking_hip_B_max*walk; // Max angle at hip.
  #local hip_B_min = slave_walking_hip_B_min*walk; // Min angle at hip.
  
  // Compute the equivalent phase {wp} for the left leg:
  #if (which > 0)
    #local wp = mod(wphase, 1.0);
  #else
    #local wp = mod(wphase + 0.5, 1.0);
  #end
  
  // Critical phases and angles in walking cycle:
  #local wp_0 = 0.00;    // Standing on left heel.
  #local hip_LB_0 = +5;   
  #local kne_LA_0 = +10;  
  #local ank_LA_0 = hip_LB_0 - kne_LA_0; 
  
  #local hip_RB_0 = +15;   
  #local kne_RA_0 = +25;  
  #local ank_RA_0 = hip_RB_0 - kne_RA_0 + 5; 
  
  #local wp_1 = slave_walking_wphase_down_R;  // Right heel touches down.
  #local hip_RB_1 = hip_B_max;   
  #local kne_RA_1 = hip_RB_1 - 5;  
  #local ank_RA_1 = hip_RB_1 - kne_RA_1; 
  
  #local hip_LB_1 = hip_B_min;   
  #local kne_LA_1 = slave_walking_knee_critical_A(hip_LB_1,hip_RB_1,kne_RA_1);  
  #local ank_LA_1 = hip_LB_1 - kne_LA_1; 
  
  #local wp_2 = 0.50;  // Standing on right heel.
  #local hip_LB_2 = hip_RB_0;   
  #local kne_LA_2 = kne_RA_0;  
  #local ank_LA_2 = ank_RA_0; 
  
  #local hip_RB_2 = hip_LB_0;   
  #local kne_RA_2 = kne_LA_0;  
  #local ank_RA_2 = ank_LA_0; 

  #local wp_3 = slave_walking_wphase_down_L;  // Left heel touches down.
  #local hip_LB_3 = hip_RB_1;   
  #local kne_LA_3 = kne_RA_1;  
  #local ank_LA_3 = ank_RA_1; 
  
  #local hip_RB_3 = hip_LB_1;   
  #local kne_RA_3 = kne_LA_1;  
  #local ank_RA_3 = ank_LA_1; 

  #local wp_4 = 1.00;  // Standing on left heel again.
  
  #if ((wp >= wp_0) & (wp <= wp_1))
    #local hip_B = interp2(wp_0,hip_LB_0, wp_1,hip_LB_1, wp);
    #local kne_A = interp2(wp_0,kne_LA_0, wp_1,kne_LA_1, wp); 
    #local ank_A = interp2(wp_0,ank_LA_0, wp_1,ank_LA_1, wp); 
  #end

  #if ((wp >= wp_1) & (wp <= wp_2))
    #local hip_B = interp2(wp_1,hip_LB_1, wp_2,hip_LB_2, wp);
    #local kne_A = interp2(wp_1,kne_LA_1, wp_2,kne_LA_2, wp); 
    #local ank_A = interp2(wp_1,ank_LA_1, wp_2,ank_LA_2, wp); 
  #end

  #if ((wp >= wp_2) & (wp <= wp_3))
    #local hip_B = interp2(wp_2,hip_LB_2, wp_3,hip_LB_3, wp);
    #local kne_A = interp2(wp_2,kne_LA_2, wp_3,kne_LA_3, wp); 
    #local ank_A = interp2(wp_2,ank_LA_2, wp_3,ank_LA_3, wp); 
  #end

  #if ((wp >= wp_3) & (wp <= wp_4))
    #local hip_B = interp2(wp_3,hip_LB_3, wp_4,hip_LB_0, wp);
    #local kne_A = interp2(wp_3,kne_LA_3, wp_4,kne_LA_0, wp); 
    #local ank_A = interp2(wp_3,ank_LA_3, wp_4,ank_LA_0, wp); 
  #end
  
  #local hip_A = 0; // Axial thigh twist.
  #local ank_B = 0; // Foot roll angle.
  
  // Store the parameters:
  #declare leg_A[0] =     0;  // Hip A (axial thigh twist).
  #declare leg_A[1] = hip_B;  // Hip B (forward swing).        
  #declare leg_A[2] =     0;  // Hip C (opening swing).        
  #declare leg_A[3] = kne_A;  // Knee A (backward bend).     
  #declare leg_A[4] = ank_A;  // Ankle A (downward pitch).            
  #declare leg_A[5] =     0;  // Ankle B (inward roll).             

#end

#macro slave_walking(walk,wphase,blink,gape,gaze_e,gaze_a,tx)
  // The walking slave.
  // {tx}     skin texture.
  
  // Slave is facing towards {+x} with mid-hips at origin.
  
  #local neck_dz = slave_neck_dz;
  
  #local arm_LA = array[6];
  #local arm_RA = array[6];
  
  slave_walking_arm_params(walk,+1,wphase,arm_LA)
  slave_walking_arm_params(walk,-1,wphase,arm_RA)
  
  #local leg_LA = array[6];
  #local leg_RA = array[6];
  
  slave_walking_leg_params(walk,+1,wphase,leg_LA)
  slave_walking_leg_params(walk,-1,wphase,leg_RA)
  
  union{
    object{ slave_head(blink,gape,gaze_e,gaze_a,tx) translate neck_dz*z }
    union{
      object{ slave_torso() }
      object{ slave_placed_arm(0,arm_LA,+1) }
      object{ slave_placed_arm(0,arm_RA,-1) }
      object{ slave_placed_leg(0,leg_LA,+1) }
      object{ slave_placed_leg(0,leg_RA,-1) }
      texture{ tx }
    }
  }
#end

#macro slave_walking_support_rel_pos(walk,wphase)
  // Position of bottom of heel that is resting on ground, rel to mid-hip.
  // Point is on sole of foot, below ankle artic.
  
  #local wp_1 = slave_walking_wphase_down_R;  // Right heel touches down.
  #local wp_3 = slave_walking_wphase_down_L;  // Left heel touches down.
  
  #local wp = mod(wphase,1);
  #if ((wp >= wp_1) & (wp < wp_3))
    // Right heel is on ground.
    #local which = -1;
  #else
    // Left heel is on ground.
    #local which = +1;
  #end
  
  #local leg_A = array[6];
  slave_walking_leg_params(walk,which,wphase,leg_A)
  
  #local heel_bot_obj = slave_placed_leg(true,leg_A,which)
  #local heel_bot_pos = (max_extent(heel_bot_obj) + min_extent(heel_bot_obj))/2; 
  heel_bot_pos
#end 

#macro slave_walking_stride_len(walk)
  // Length of one stride by the slave.
  // Defined as the forwards X distance between heels at moment when both are on ground.
  
  // Critical phases in walking cycle:
  #local wp_1 = slave_walking_wphase_down_R;  // Right heel touches down.
  
  #local heel_bot_B = slave_walking_support_rel_pos(walk,wp_1 - 0.0001); // Left heel, behind.
  #local heel_bot_F = slave_walking_support_rel_pos(walk,wp_1 + 0.0001); // Right heel, forward.
  (heel_bot_F - heel_bot_B).x
#end

#macro slave_walking_multiple_feet_positions(walk,N,fp_ini,fp_1,fp_3)
  // Computes the foot positions for {slave_walking_multiple}.
  // {fp_ini} (OUT) must be var; foot position at beginning of walk.
  // {fp_1}   (OUT) must be array [N]; positions at or just after phase {wp_1} in each cycle.
  // {fp_3}   (OUT) must be array [N]; positions at or just after phase {wp_3} in each cycle.
  
  // Critical phases in walking cycle:
  #local wp_1 = slave_walking_wphase_down_R;  // Right heel touches down.
  #local wp_3 = slave_walking_wphase_down_L;  // Left heel touches down.
  
  // Compute critical relative positions {rp_ini,rp_1,rp_3} of contact heel:
  #local rp_ini = slave_walking_support_rel_pos(walk,0.00);           // Contact foot at beginning of walk.
  #local rp_fw_1   = slave_walking_support_rel_pos(walk,wp_1+0.0001); // Forward foot at {wp_1} .
  #local rp_bw_1   = slave_walking_support_rel_pos(walk,wp_1-0.0001); // Backward foot at {wp_1}.
  #local rp_fw_3   = slave_walking_support_rel_pos(walk,wp_3+0.0001); // Forward foot at {wp_3} .
  #local rp_bw_3   = slave_walking_support_rel_pos(walk,wp_3-0.0001); // Backward foot at {wp_3}.
  
  // Compute the desired position {fp_ini} of contact heel at beginning of walk:
  #declare fp_ini = < rp_ini.x, rp_ini.y, 0 >;
  
  // Compute the desired positions of contact foot at critical times of each cycle:
  #local i = 0;
  #local fp_cur = fp_ini;
  #while (i < N)
    #local dp = rp_fw_1 - rp_bw_1;
    #local fp_cur = fp_cur + < dp.x, dp.y, 0 >;
    #declare fp_1[i] = fp_cur;

    #local dp = rp_fw_3 - rp_bw_3;
    #local fp_cur = fp_cur + < dp.x, dp.y, 0 >;
    #declare fp_3[i] = fp_cur;
    #local i = i + 1;
  #end
#end

#macro slave_walking_multiple(walk,N,wphase,blink,gape,gaze_e,gaze_a,tx)
  // The walking slave for {N} walk cycles as {wphase} ranges from 0 to {N}.
  // {tx}     skin texture.
  // Slave starts with mid-hips on Z axis, lower foot at {z=0}.
  
  // Critical phases in walking cycle:
  #local wp_1 = slave_walking_wphase_down_R;  // Right heel touches down.
  #local wp_3 = slave_walking_wphase_down_L;  // Left heel touches down.
  
  // Compute the desired positions of contact foot at critical times of each cycle:
  #local fp_ini = 0;       // Initial posititon (to be redefined).
  #local fp_1 = array[NC]; // Position at and just after phase {wp_1}
  #local fp_3 = array[NC]; // Position at and just after phase {wp_3}
  slave_walking_multiple_feet_positions(walk,N,fp_ini,fp_1,fp_3)
  
  // Normalize current phase {wp} to range between 0 and {N}:
  #local wp = min(N, max(0, wphase));
  
  // Get current support point {rp_cur} position rel mid-hips:
  #local rp_cur = slave_walking_support_rel_pos(walk,wp);
  
  // Decide point {fp_cur} where that point should be:
  #if (wp < wp_1)
    #local fp_cur = fp_ini;
  #else
    #local i_cur = int(wp - wp_1);  // Cyle index, counting from phase {wp_1}
    #local wpf_cur = (wp - wp_1) - i_cur;    // Phase within that cycle.
    #if (wpf_cur < 0.5)
      #local fp_cur = fp_1[i_cur];
    #else
      #local fp_cur = fp_3[i_cur];
    #end
  #end
  object{ slave_walking(walk,wp,blink,gape,gaze_e,gaze_a,tx) translate (fp_cur-rp_cur) }
  
#end


#macro slave_walking_multiple_footprints(walk,N,ht)  
  // Footprints/pedestals for {}.
  
  //  {N} number of full cycles in walk.
  //  {ht} height of pedestals.

  // Compute the desired positions of contact foot at critical times of each cycle:
  #local fp_ini = 0;       // Initial posititon (to be redefined).
  #local fp_1 = array[N]; // Position at and just after phase {wp_1}
  #local fp_3 = array[N]; // Position at and just after phase {wp_3}
  slave_walking_multiple_feet_positions(walk,N,fp_ini,fp_1,fp_3)
  
  // Footprint markers:
  #local prt = cylinder{ -1*z, 0*z, 1 scale < 100, 100, ht > texture{ tx_hole } }
  #local footprints = 
    union{
      object{ prt translate fp_ini }
      #local i = 0;
      #while(i < N)
        object{ prt translate fp_1[i] }
        object{ prt translate fp_3[i] }
        #local i = i + 1;
      #end
    }
  object{ footprints }
#end