// Last edited on 2020-11-27 18:48:25 by jstolfi
// Motion of a grasping tentacle

#macro tentacle_grasping_key_bezier_point_positions(NQ,TQ, ip, A0,Z3, mP, rgen)
  // Returns array of {NQ}  elements with positions or velocities of 
  // the Bezier control point of a grasping tentacle in each key frame.
  // The last element is equal to the first one.
  
  // {TQ} = Array of {NQ} elements with the clock values of the key frames.
  // {ip} = number of the Bezier point, 0 to 9.
  // {A0} = coords of base of tentacle (assumed fixed).
  // {Z3} = mean coords of tip of tentacle when not grasping.
  // {mP} = max control point perturbation from straight line {A0:Z3}.
  // {rgen} = random number generator.
  
  // Orthogonal frame {uu,vv,ww} for line {A0:Z3}:
  #local L = vlength(Z3 - A0);
  #local uu = (Z3 - A0)/L; // Drection from {A0} to {Z3}.
  #if ((abs(uu.x) > abs(uu.y)) &  (abs(uu.x) > abs(uu.z)))
    #local vv = < -uu.y, +uu.x, 0 >;
  #elseif ((abs(uu.y) > abs(uu.z)) &  (abs(uu.y) > abs(uu.x)))
    #local vv = < 0, -uu.z, +uu.y >;
  #else
    #local vv = < +uu.z, 0, -uu.x >;
  #end
  #local vv = vv/vlength(vv);
  #local ww = < uu.y*vv.z - uu.z*vv.y, uu.z*vv.x - uu.x*vv.z, uu.x*vv.y - uu.y*vv.x >;

  #local Lc = 2.0*L;  // Distance from {A0} to center of grasped object.
  #local Rc = 0.3*L;  // Nominal radius of grasped object.
  #local Lf = 0.5*L;  // Distance from {A0} to center of object after pulling it in.
  
  // Bezier control points 0..9 for tentacle at halfway, max, and final elongation:
  #local PH = tentacle_grasping_bezier_point_positions_halfway(A0,Lc,Rc,Lf,uu,vv,ww)
  #local PM = tentacle_grasping_bezier_point_positions_max(A0,Lc,Rc,Lf,uu,vv,ww)
  #local PF = tentacle_grasping_bezier_point_positions_final(A0,Lc,Rc,Lf,uu,vv,ww)

  #local PR = tentacle_random_key_bezier_point_positions(NQ, ip, A0,Z3, mP, rgen)  // Random positions for point {ip}.

  #local P = array[NQ]
  #local kq = 0;
  #while (kq < NQ-1)
    #local tt = TQ[kq];  // Clock value of key frame.
    #local r = tentacle_grasping_mixing_coeffs(tt) 
    #local P[kq] = r[0]*PH[ip] + r[1]*PM[ip] + r[2]*PF[ip] + r[3]*PR[kq];
    #local kq = kq + 1;
  #end
  #local P[NQ-1] = P[0];
  P
#end

#macro tentacle_grasping_mixing_coeffs(tt)
  // Returns an array of 4 elements with the mixing weights for
  // the three "key" grasping positions (halfway, max, final)
  // and of the random motion, corresponding to the clock value {tt}.
  
  #local rR = 0.0; // Factor for random motion.
  #local rH = 0.0; // Factor for halfway grasping pose.
  #local rM = 0.0; // Factor for max extent grasping pose.
  #local rF = 0.0; // Factor for final grasping pose.
  
  #if ((tt < 0.10) | (tt > 0.90))
    #local rR = 1.0;  
  #elseif ((tt >= 0.10) & (tt <= 0.30))
    #local rR = 0.5*(1 + cos(5*pi*(tt - 0.10))); 
  #elseif ((tt >= 0.70) & (tt <= 0.90))
    #local rR = 0.5*(1 - cos(5*pi*(tt - 0.70)));  
  #end
  #if ((tt > 0.10) & (tt < 0.50))
    #local rH = 0.5*(1 - cos(5*pi*(tt - 0.10)));  
  #end
  #if ((tt > 0.30) & (tt < 0.70))
    #local rM = 0.5*(1 - cos(5*pi*(tt - 0.30)));  
  #end
  #if ((tt > 0.50) & (tt < 0.90))
    #local rF = 0.5*(1 - cos(5*pi*(tt - 0.50)));
  #else
    #local rF = 0.0;
  #end
  // These coefficients should add to 1 so no normalization is necessary.
  
  #local r = array[4] 
  #local r[0] = rH;
  #local r[1] = rM;
  #local r[2] = rF;
  #local r[3] = rR;
  r
#end

#macro tentacle_grasping_bezier_point_positions_halfway(A0,Lc,Rc,Lf,uu,vv,ww)
  // The 10 Bezier control points of the tentacle halfway through the
  // grasping motion.
  
  #local P = array[10];
  #local P[0] = A0;
  #local P[1] = A0 + (1/7.5)*Lc*uu;
  #local P[2] = A0 + (2/7.5)*Lc*uu - 0.20*Rc*vv;
  #local P[4] = A0 + (4/7.5)*Lc*uu - 0.80*Rc*vv;
  #local P[5] = A0 + (5/7.5)*Lc*uu - 1.10*Rc*vv;
  #local P[7] = A0 + (7/7.5)*Lc*uu - 1.70*Rc*vv;
  #local P[8] = A0 + (8/7.5)*Lc*uu - 1.70*Rc*vv;
  #local P[9] = A0 + (8.5/7.5)*Lc*uu - 1.40*Rc*vv;

  #local P[3] = (P[2]+ P[4])/2;
  #local P[6] = (P[5]+ P[7])/2;
  
  P
#end

#macro tentacle_grasping_bezier_point_positions_max(A0,Lc,Rc,Lf,uu,vv,ww)
  // The 10 Bezier control points of the tentacle at the max extension of the
  // grasping motion, closing around object.
  
  #local P = array[10];
  
  #local P[0] = A0;
  #local P[1] = A0 + (1/6)*Lc*uu;
  #local P[2] = A0 + (2/6)*Lc*uu - 0.33*Rc*vv;
  #local P[4] = A0 + (4/6)*Lc*uu - 0.67*Rc*vv;
  #local P[5] = A0 + (5/6)*Lc*uu - 1.00*Rc*vv;
  #local P[7] = A0 + (Lc + 1.50*Rc)*uu - 1.00*Rc*vv;
  #local P[8] = A0 + (Lc + 1.50*Rc)*uu + 1.00*Rc*vv;
  #local P[9] = A0 + Lc*uu + 1.00*Rc*vv;

  #local P[3] = (P[2]+ P[4])/2;
  #local P[6] = (P[5]+ P[7])/2;
  
  P
#end

#macro tentacle_grasping_bezier_point_positions_final(A0,Lc,Rc,Lf,uu,vv,ww)
  // The 10 Bezier control points of the tentacle at the end of the
  // grasping motion.
  
  #local P = array[10];
  #local P[0] = A0;
  #local P[1] = A0 + (1/6)*Lf*uu;
  #local P[2] = A0 + (2/6)*Lf*uu - 0.33*Rc*vv;
  #local P[4] = A0 + (4/6)*Lf*uu - 0.67*Rc*vv;
  #local P[5] = A0 + (5/6)*Lf*uu - 1.00*Rc*vv;
  #local P[7] = A0 + (Lf + 1.50*Rc)*uu - 1.00*Rc*vv;
  #local P[8] = A0 + (Lf + 1.25*Rc)*uu + 1.25*Rc*vv;
  #local P[9] = A0 + (Lf - 0.25*Rc)*uu + 0.75*Rc*vv;

  #local P[3] = (P[2]+ P[4])/2;
  #local P[6] = (P[5]+ P[7])/2;
  
  P
#end