/*

Constants managed by this file:
===============================

Input constants:
----------------

Lengths of parts of the body:

  leg_length
  leg_half_length
  upper_leg_length
  lower_leg_length
  hip_width
  back_length
  shoulder_width
  arm_length
  arm_half_length
  upper_arm_length
  lower_arm_length

Radii of joints of the body:

  default_rad		- See section defaults
  ankle_rad
  knee_rad
  hip_rad
  shoulder_rad
  elbow_rad
  wrist_rad

Angles:

  killed_leg_ang	- How far (in degrees) the legs are spread
			  when sitting killed
  killed_back_ang	- How far the torso is bent backwards when killed
  killed_nod_ang
  losing_back_ang	- How far the torso is bent forwards when losing
  losing_min_nod_ang
  losing_max_nod_ang
  losing_shake_ang
  walking_back_ang	- How far the torso is bent forwards when walking
  walking_arm_ang	- How far the arms swing when walking

Other:

  winning_jump_height
  step_length
  left_eye_pos		- Position of the place where hand and eye meet when
			  weeping (relative to neck_pos for an upright head).
  left_eye_normal
  leg_sep		- The space between the legs
  arm_sep		- The space between the arms and the legs


Output constants:
-----------------

Positions:

  left_ankle_pos
  right_ankle_pos
  left_knee_pos
  right_knee_pos
  hip_pos
  left_hip_pos
  right_hip_pos
  neck_pos
  left_shoulder_pos
  right_shoulder_pos
  left_elbow_pos
  right_elbow_pos
  left_wrist_pos
  right_wrist_pos

Transforms:

  left_lower_leg_trans
  right_lower_leg_trans
  left_upper_leg_trans
  right_upper_leg_trans
  back_trans
  head_trans
  left_lower_arm_trans
  right_lower_arm_trans
  left_upper_arm_trans
  right_upper_arm_trans



The idea of this file:
======================

This file should aid in placing parts of the body.
The idea is that for, say, the upper left leg, the following can be used:

  cone {
    left_knee_pos  knee_rad
    left_hip_pos  hip_rad
  }

or

  cone {
    -upper_leg_length*y  knee_rad
    0  hip_rad
    transform left_upper_leg_trans
  }



Invokation of this file's functionality:
========================================

The input is done by declaring the input constants outside of this file.
A call of Erase_humanoid_input_constants empties previous values
(so that defaults are available again).
A call of Define_humanoid_output_constants starts the calculation.



Defaults of the input constants:
================================

Constants that are not listed here do not have default values.

  leg_length, upper_leg_length, lower_leg_length:
	If leg_half_length is given, upper_leg_length and lower_leg_length
        default to it. If otherwise only two of the other three are given,
        the third defaults such that
	  leg_length = upper_leg_length+lower_leg_length.
	If only leg_length is given, the default is that
	  upper_leg_length=lower_leg_length.
	If all three are given or otherwise defaulted and
	  leg_length < upper_leg_length+lower_leg_length
        then the leg will never be fully stretched.

  hip_width:
	2*max(ankle_rad, knee_rad, hip_rad)+leg_sep

  shoulder_width:
	hip_width +
	2*max(ankle_rad, knee_rad, hip_rad) +
        2*arm_sep +
	2*max(wrist_rad, elbow_rad, shoulder_rad)

  arm_length, upper_arm_length, lower_arm_length:
	similar to legs.

  ankle_rad, knee_rad, hip_rad, shoulder_rad, elbow_rad, wrist_rad:
	default_rad
	If all of the above are given, default_rad is not needed.

  killed_leg_ang, killed_back_ang, killed_nod_ang,
  losing_back_ang, losing_min_nod_ang, losing_shake_ang,
  walking_back_ang, walking_arm_ang:
	0

  losing_max_nod_ang:
	losing_min_nod_ang

  winning_jump_height, leg_sep, arm_sep:
	0

  step_length:
	2

  left_eye_normal:
	-z


*/


#ifndef(humanoid_frames_INCLUDED)
#declare humanoid_frames_INCLUDED = 1;



#declare atanf=function(b,a)				// b=sin, a=cos
  {(select((b),
    select((a),
      select((a)-(b),
        atan2(-(b),-(a))-pi,
        -pi/2-atan2(-(a),-(b))),
      select((a)+(b),
        atan2((a),-(b))-pi/2,
        -atan2(-(b),(a)))),
    select((a),
      select((a)+(b),
        pi-atan2((b),-(a)),
        pi/2+atan2(-(a),(b))),
      select((a)-(b),
        pi/2-atan2((a),(b)),
        atan2((b),(a)))
      )))}



#macro Erase_humanoid_input_constants()
  #ifdef (leg_length) #undef leg_length #end
  #ifdef (leg_half_length) #undef leg_half_length #end
  #ifdef (upper_leg_length) #undef upper_leg_length #end
  #ifdef (lower_leg_length) #undef lower_leg_length #end
  #ifdef (hip_width) #undef hip_width #end
  #ifdef (back_length) #undef back_length #end
  #ifdef (shoulder_width) #undef shoulder_width #end
  #ifdef (arm_length) #undef arm_length #end
  #ifdef (arm_half_length) #undef arm_half_length #end
  #ifdef (upper_arm_length) #undef upper_arm_length #end
  #ifdef (lower_arm_length) #undef lower_arm_length #end
  #ifdef (leg_sep) #undef leg_sep #end
  #ifdef (arm_sep) #undef arm_sep #end
  #ifdef (default_rad) #undef default_rad #end
  #ifdef (ankle_rad) #undef ankle_rad #end
  #ifdef (knee_rad) #undef knee_rad #end
  #ifdef (hip_rad) #undef hip_rad #end
  #ifdef (shoulder_rad) #undef shoulder_rad #end
  #ifdef (elbow_rad) #undef elbow_rad #end
  #ifdef (wrist_rad) #undef wrist_rad #end
  #ifdef (killed_leg_ang) #undef killed_leg_ang #end
  #ifdef (killed_back_ang) #undef killed_back_ang #end
  #ifdef (killed_nod_ang) #undef killed_nod_ang #end
  #ifdef (losing_back_ang) #undef losing_back_ang #end
  #ifdef (losing_min_nod_ang) #undef losing_min_nod_ang #end
  #ifdef (losing_max_nod_ang) #undef losing_max_nod_ang #end
  #ifdef (losing_shake_ang) #undef losing_shake_ang #end
  #ifdef (walking_back_ang) #undef walking_back_ang #end
  #ifdef (walking_arm_ang) #undef walking_arm_ang #end
  #ifdef (winning_jump_height) #undef winning_jump_height #end
  #ifdef (step_length) #undef step_length #end
  #ifdef (left_eye_pos) #undef left_eye_pos #end
  #ifdef (left_eye_normal) #undef left_eye_normal #end
#end



#macro Apply_humanoid_defaults()

  #ifndef(ankle_rad)
    #declare ankle_rad = default_rad;
  #end
  #ifndef(knee_rad)
    #declare knee_rad = default_rad;
  #end
  #ifndef(hip_rad)
    #declare hip_rad = default_rad;
  #end
  #ifndef(shoulder_rad)
    #declare shoulder_rad = default_rad;
  #end
  #ifndef(elbow_rad)
    #declare elbow_rad = default_rad;
  #end
  #ifndef(wrist_rad)
    #declare wrist_rad = default_rad;
  #end

  #ifndef(lower_leg_length)
    #ifdef(leg_half_length)
      #declare lower_leg_length = leg_half_length;
    #else
      #ifndef(upper_leg_length)
        #declare upper_leg_length = leg_length/2;
      #end
      #declare lower_leg_length = leg_length-upper_leg_length;
    #end
  #end
  #ifndef(upper_leg_length)
    #ifdef(leg_half_length)
      #declare upper_leg_length = leg_half_length;
    #else
      #declare upper_leg_length = leg_length-lower_leg_length;
    #end
  #end
  #ifndef(leg_length)
    #declare leg_length = upper_leg_length+lower_leg_length;
  #end

  #ifndef(leg_sep)
    #declare leg_sep = 0;
  #end
  #ifndef(arm_sep)
    #declare arm_sep = 0;
  #end
  #ifndef(hip_width)
    #declare hip_width = 2*max(ankle_rad,knee_rad,hip_rad)+leg_sep;
  #end
  #ifndef(shoulder_width)
    #declare shoulder_width = hip_width +
      2*max(ankle_rad,knee_rad,hip_rad) +
      2*arm_sep +
      2*max(wrist_rad,elbow_rad,shoulder_rad);
  #end

  #ifndef(lower_arm_length)
    #ifdef(arm_half_length)
      #declare lower_arm_length = arm_half_length;
    #else
      #ifndef(upper_arm_length)
        #declare upper_arm_length = arm_length/2;
      #end
      #declare lower_arm_length = arm_length-upper_arm_length;
    #end
  #end
  #ifndef(upper_arm_length)
    #ifdef(arm_half_length)
      #declare upper_arm_length = arm_half_length;
    #else
      #declare upper_arm_length = arm_length-lower_arm_length;
    #end
  #end
  #ifndef(arm_length)
    #declare arm_length = upper_arm_length+lower_arm_length;
  #end

  #ifndef(killed_leg_ang)
    #declare killed_leg_ang = 0;
  #end
  #ifndef(killed_back_ang)
    #declare killed_back_ang = 0;
  #end
  #ifndef(killed_nod_ang)
    #declare killed_nod_ang = 0;
  #end

  #ifndef(losing_back_ang)
    #declare losing_back_ang = 0;
  #end
  #ifndef(losing_min_nod_ang)
    #declare losing_min_nod_ang = 0;
  #end
  #ifndef(losing_max_nod_ang)
    #declare losing_max_nod_ang = losing_min_nod_ang;
  #end
  #ifndef(losing_shake_ang)
    #declare losing_shake_ang = 0;
  #end

  #ifndef(winning_jump_height)
    #declare winning_jump_height = 0;
  #end

  #ifndef(walking_back_ang)
    #declare walking_back_ang = 0;
  #end
  #ifndef(walking_arm_ang)
    #declare walking_arm_ang = 0;
  #end

  #ifndef(step_length)
    #declare step_length = 2;
  #end

  #ifndef(left_eye_normal)
    #declare left_eye_normal = -z;
  #end

#end



#macro Define_humanoid_output_constants()
  Apply_humanoid_defaults()

  #if (2*leg_length<step_length)
    #warning "legs are too short\n"
  #end

  #local walking_hip_height =
    ankle_rad+sqrt(pow(leg_length,2)-pow(step_length/2,2));
  #local standing_hip_height = ankle_rad+leg_length;
  #local leg_ang = 0;

  #if (playerWalking)

    #local playerWalkingTime_=playerWalkingTime+1/8;
    #if (playerWalkingTime_>1)
      #local playerWalkingTime_=playerWalkingTime_-1;
    #end

    #local aux_ang_1 = radians(walking_back_ang);
    #local aux_ang_2 = radians(walking_arm_ang/2*cos(playerWalkingTime_*2*pi));

    #if (playerWalkingTime_<=0.5)
      #declare left_ankle_pos =
        <hip_width/2,ankle_rad,(playerWalkingTime_-0.25)*2*step_length>;
      #declare right_ankle_pos =
        <-hip_width/2,
          ankle_rad+sin(playerWalkingTime_*2*pi),
          (0.25-playerWalkingTime_)*2*step_length>;
    #else
      #declare left_ankle_pos =
        <hip_width/2,
          ankle_rad+sin((playerWalkingTime_-0.5)*2*pi),
          (0.75-playerWalkingTime_)*2*step_length>;
      #declare right_ankle_pos =
        <-hip_width/2,ankle_rad,(playerWalkingTime_-0.75)*2*step_length>;
    #end
    #declare hip_pos = <0,walking_hip_height,0>;
    #declare left_hip_pos = <hip_width/2,walking_hip_height,0>;
    #declare right_hip_pos = <-hip_width/2,walking_hip_height,0>;
    #declare neck_pos = hip_pos+<0,cos(aux_ang_1),-sin(aux_ang_1)>*back_length;
    #declare left_shoulder_pos = neck_pos+shoulder_width/2*x;
    #declare right_shoulder_pos = neck_pos-shoulder_width/2*x;
    #declare nod_ang = 0;
    #declare shake_ang = 0;
    #declare left_wrist_pos = left_shoulder_pos+
      <0,-cos(aux_ang_2),sin(aux_ang_2)>*arm_length;
    #declare right_wrist_pos = right_shoulder_pos+
      <0,-cos(aux_ang_2),-sin(aux_ang_2)>*arm_length;

  #else#if (playerStanding)

    #declare left_ankle_pos = <hip_width/2,ankle_rad,0>;
    #declare right_ankle_pos = <-hip_width/2,ankle_rad,0>;
    #declare hip_pos = <0,standing_hip_height,0>;
    #declare left_hip_pos = <hip_width/2,standing_hip_height,0>;
    #declare right_hip_pos = <-hip_width/2,standing_hip_height,0>;
    #declare neck_pos = hip_pos+back_length*y;
    #declare left_shoulder_pos = neck_pos+shoulder_width/2*x;
    #declare right_shoulder_pos = neck_pos-shoulder_width/2*x;
    #declare nod_ang = 0;
    #declare shake_ang = 0;
    #declare left_wrist_pos = left_shoulder_pos-arm_length*y;
    #declare right_wrist_pos = right_shoulder_pos-arm_length*y;

  #else#if (playerKilled)

    #local leg_ang = killed_leg_ang/2;

    #local aux_len_1 = sqrt(pow(leg_length,2)-pow(hip_rad-ankle_rad,2));
    #local aux_ang_1 = radians(killed_leg_ang)/2;
    #local aux_ang_2 = radians(killed_back_ang);

    #declare left_ankle_pos =
      <hip_width/2+sin(aux_ang_1)*aux_len_1,
        ankle_rad,
        -cos(aux_ang_1)*aux_len_1/2>;
    #declare right_ankle_pos = left_ankle_pos*<-1,1,1>;
    #declare hip_pos = <0,hip_rad,cos(aux_ang_1)*aux_len_1/2>;
    #declare left_hip_pos = hip_pos+hip_width/2*x;
    #declare right_hip_pos = hip_pos-hip_width/2*x;
    #declare neck_pos = hip_pos+<0,cos(aux_ang_2),-sin(aux_ang_2)>*back_length;
    #declare left_shoulder_pos = neck_pos+shoulder_width/2*x;
    #declare right_shoulder_pos = neck_pos-shoulder_width/2*x;
    #declare nod_ang = killed_nod_ang;
    #declare shake_ang = 0;
    #declare left_wrist_pos = (left_shoulder_pos.y>arm_length+wrist_rad ?
      left_shoulder_pos-arm_length*y :
      <left_shoulder_pos.x
          +sqrt(pow(arm_length,2)-pow(left_shoulder_pos.y-wrist_rad,2)),
        wrist_rad,left_shoulder_pos.z>)
    #declare right_wrist_pos = (right_shoulder_pos.y>arm_length+wrist_rad ?
      right_shoulder_pos-arm_length*y :
      <right_shoulder_pos.x
          -sqrt(pow(arm_length,2)-pow(right_shoulder_pos.y-wrist_rad,2)),
        wrist_rad,right_shoulder_pos.z>)

  #else#if (playerLosing)

    #local aux_len_1 = ankle_rad+hip_rad;
    #local aux_ang_1 =
      asin((ankle_rad-knee_rad)/lower_leg_length)+
      acos((pow(lower_leg_length,2)+pow(aux_len_1,2)-pow(upper_leg_length,2))
        / (2*lower_leg_length*aux_len_1));
    #local aux_ang_2 = radians(losing_back_ang);

    #declare left_ankle_pos = <hip_width/2,ankle_rad,cos(aux_ang_1)*aux_len_1>;
    #declare right_ankle_pos = left_ankle_pos*<-1,1,1>;
    #declare hip_pos = <0,sin(aux_ang_1)*aux_len_1+ankle_rad,0>;
    #declare left_hip_pos = hip_pos+hip_width/2*x;
    #declare right_hip_pos = hip_pos-hip_width/2*x;
    #declare neck_pos = hip_pos+<0,cos(aux_ang_2),-sin(aux_ang_2)>*back_length;
    #declare left_shoulder_pos = neck_pos+shoulder_width/2*x;
    #declare right_shoulder_pos = neck_pos-shoulder_width/2*x;
    #declare nod_ang = (losing_max_nod_ang+losing_min_nod_ang +
      cos(pi*playerLosingTime)*(losing_max_nod_ang-losing_min_nod_ang))/2;
    #declare shake_ang = cos(pi*playerLosingTime)*losing_shake_ang/2;
    #declare left_wrist_pos = neck_pos+vrotate(vrotate(
      left_eye_pos+left_eye_normal*wrist_rad,
      shake_ang*y),
      -(losing_back_ang+nod_ang)*x);
    #declare right_wrist_pos = neck_pos+vrotate(vrotate(
      (left_eye_pos+left_eye_normal*wrist_rad)*<-1,1,1>,
      shake_ang*y),
      -(losing_back_ang+nod_ang)*x);

  #else // playerWinning

    #declare aux_len_1 = cos(pi*playerWinningTime)*winning_jump_height;

    #declare left_ankle_pos = <hip_width/2,ankle_rad+max(0,aux_len_1),0>;
    #declare right_ankle_pos = left_ankle_pos*<-1,1,1>;
    #declare hip_pos = <0,standing_hip_height+aux_len_1,0>;
    #declare left_hip_pos = hip_pos+hip_width/2*x;
    #declare right_hip_pos = hip_pos-hip_width/2*x;
    #declare neck_pos = hip_pos+back_length*y;
    #declare left_shoulder_pos = neck_pos+shoulder_width/2*x;
    #declare right_shoulder_pos = neck_pos-shoulder_width/2*x;
    #declare nod_ang = 0;
    #declare shake_ang = 0;
    #declare left_wrist_pos = left_shoulder_pos +
      (cos(pi*playerWinningTime)*(arm_length-wrist_rad-shoulder_rad)+
        arm_length+wrist_rad+shoulder_rad)/2*y;
    #declare right_wrist_pos = right_shoulder_pos +
      (cos(pi*playerWinningTime)*(arm_length-wrist_rad-shoulder_rad)+
        arm_length+wrist_rad+shoulder_rad)/2*y;

  #end#end#end#end

  #declare left_leg_length = vlength(left_hip_pos-left_ankle_pos);
  #declare right_leg_length = vlength(right_hip_pos-right_ankle_pos);
/*
   ...ang_1: angle between upper leg and line ankle-hip.
   ...ang_2: angle at knee.
   ...ang_3: angle between upper leg and -y.
*/
  #local left_leg_aux_ang_1 = degrees(acos(
    (pow(left_leg_length,2)+pow(upper_leg_length,2)-pow(lower_leg_length,2))
      / (2*left_leg_length*upper_leg_length)));
  #local left_leg_aux_ang_2 = degrees(acos(
    (pow(upper_leg_length,2)+pow(lower_leg_length,2)-pow(left_leg_length,2))
      / (2*upper_leg_length*lower_leg_length)));
  #local left_leg_aux_ang_3 = degrees(atanf(
    left_hip_pos.z-left_ankle_pos.z,
    left_hip_pos.y-left_ankle_pos.y));
  #local right_leg_aux_ang_1 = degrees(acos(
    (pow(right_leg_length,2)+pow(upper_leg_length,2)-pow(lower_leg_length,2))
      / (2*right_leg_length*upper_leg_length)));
  #local right_leg_aux_ang_2 = degrees(acos(
    (pow(upper_leg_length,2)+pow(lower_leg_length,2)-pow(right_leg_length,2))
      / (2*upper_leg_length*lower_leg_length)));
  #local right_leg_aux_ang_3 = degrees(atanf(
    right_hip_pos.z-right_ankle_pos.z,
    right_hip_pos.y-right_ankle_pos.y));

  #declare left_knee_pos = left_hip_pos +
    vrotate(vrotate(-upper_leg_length*y,
      (left_leg_aux_ang_1+left_leg_aux_ang_3)*x),
      -leg_ang*y);
  #declare right_knee_pos = right_hip_pos +
    vrotate(vrotate(-upper_leg_length*y,
      (right_leg_aux_ang_1+right_leg_aux_ang_3)*x),
      leg_ang*y);
  #declare left_lower_leg_trans = transform {
      rotate (left_leg_aux_ang_1+left_leg_aux_ang_2+left_leg_aux_ang_3-180)*x
      rotate -leg_ang*y
      translate left_knee_pos
    }
  #declare left_upper_leg_trans = transform {
      rotate (left_leg_aux_ang_1+left_leg_aux_ang_3)*x
      rotate -leg_ang*y
      translate left_hip_pos
    }
  #declare right_lower_leg_trans = transform {
      rotate
        (right_leg_aux_ang_1+right_leg_aux_ang_2+right_leg_aux_ang_3-180)*x
      rotate leg_ang*y
      translate right_knee_pos
    }
  #declare right_upper_leg_trans = transform {
      rotate (right_leg_aux_ang_1+right_leg_aux_ang_3)*x
      rotate leg_ang*y
      translate right_hip_pos
    }

  #local back_ang = degrees(atanf(hip_pos.z-neck_pos.z,neck_pos.y-hip_pos.y));

  #declare back_trans = transform {
    rotate -back_ang*x
    translate neck_pos
  }

  #declare head_trans = transform {
    rotate shake_ang*y
    rotate -(back_ang+nod_ang)*x
    translate neck_pos
  }

  #local left_arm_vec = left_wrist_pos-left_shoulder_pos;
  #local left_arm_length = vlength(left_arm_vec);
  #local right_arm_vec = right_wrist_pos-right_shoulder_pos;
  #local right_arm_length = vlength(right_arm_vec);
/*
   ...ang_1: angle between upper arm and line wrist-shoulder.
   ...ang_2: angle at elbow.
   ...ang_3: angle between line wrist-shoulder and plane y/z.
   ...ang_4: angle between -y and line wrist-shoulder.
*/
  #local left_arm_aux_ang_1 = degrees(acos(
    (pow(left_arm_length,2)+pow(upper_arm_length,2)-pow(lower_arm_length,2))
      / (2*left_arm_length*upper_arm_length)));
  #local left_arm_aux_ang_2 = degrees(acos(
    (pow(upper_arm_length,2)+pow(lower_arm_length,2)-pow(left_arm_length,2))
      / (2*upper_arm_length*lower_arm_length)));
  #local left_arm_aux_ang_3 = -degrees(atanf(
    left_arm_vec.x,
    vlength(left_arm_vec*<0,1,1>)));
  #local left_arm_aux_ang_4 = degrees(atanf(
    -left_arm_vec.z,
    -left_arm_vec.y));
  #local right_arm_aux_ang_1 = degrees(acos(
    (pow(right_arm_length,2)+pow(upper_arm_length,2)-pow(lower_arm_length,2))
      / (2*right_arm_length*upper_arm_length)));
  #local right_arm_aux_ang_2 = degrees(acos(
    (pow(upper_arm_length,2)+pow(lower_arm_length,2)-pow(right_arm_length,2))
      / (2*upper_arm_length*lower_arm_length)));
  #local right_arm_aux_ang_3 = degrees(atanf(
    right_arm_vec.x,
    vlength(right_arm_vec*<0,1,1>)));
  #local right_arm_aux_ang_4 = degrees(atanf(
    -right_arm_vec.z,
    -right_arm_vec.y));

  #declare left_elbow_pos = left_shoulder_pos + vrotate(vrotate(vrotate(
    -upper_arm_length*y,
    -left_arm_aux_ang_1*x),
    -left_arm_aux_ang_3*z),
    left_arm_aux_ang_4*x);
  #declare right_elbow_pos = right_shoulder_pos + vrotate(vrotate(vrotate(
    -upper_arm_length*y,
    -right_arm_aux_ang_1*x),
    right_arm_aux_ang_3*z),
    right_arm_aux_ang_4*x);
  #declare left_upper_arm_trans = transform {
      rotate -left_arm_aux_ang_1*x
      rotate -left_arm_aux_ang_3*z
      rotate left_arm_aux_ang_4*x
      translate left_shoulder_pos
    }
  #declare left_lower_arm_trans = transform {
      rotate (180-left_arm_aux_ang_2-left_arm_aux_ang_1)*x
      rotate -left_arm_aux_ang_3*z
      rotate left_arm_aux_ang_4*x
      translate left_elbow_pos
    }
  #declare right_upper_arm_trans = transform {
      rotate -right_arm_aux_ang_1*x
      rotate right_arm_aux_ang_3*z
      rotate right_arm_aux_ang_4*x
      translate right_shoulder_pos
    }
  #declare right_lower_arm_trans = transform {
      rotate (180-right_arm_aux_ang_2-right_arm_aux_ang_1)*x
      rotate right_arm_aux_ang_3*z
      rotate right_arm_aux_ang_4*x
      translate right_elbow_pos
    }

#end



#end

