// Last edited on 2011-08-16 21:12:27 by stolfilocal




#macro slave_walking_leg(virt,walk,which,wphase)
  // Slave's leg during walking motion.
  // Leg is placed assuming mid-hips at origin.
  
  #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 ank_B = 0;
  
  #local hip_pos = slave_hip_pos;             // Position of left hip artic.
  object{ slave_left_leg(virt,kne_A,ank_A,ank_B) rotate -hip_B*y translate hip_pos scale <+1,which,+1> }
#end