// HOW TO WALK ALONG A PATH
// 
// Technique developed by Rune S. Johansen

// Render as animation with clock going from 0 to 1 and
// with at least 100 frames.

// Find a vector perpendicular to V2 and in the plane of
// V1 and V2. In other words, the new vector is a version
// of V1 adjusted to be perpendicular to V2.
#macro VPerp_Adjust(V1, V2)
    vnormalize(vcross(vcross(V2, V1), V2))
#end

#macro Matrix_Trans(A, B, C, D)
    transform {
        matrix < A.x, A.y, A.z,
                 B.x, B.y, B.z,
                 C.x, C.y, C.z,
                 D.x, D.y, D.z>
    }
#end

// A quick scene
camera {location 10*y look_at 0 rotate -60*x angle 12 translate 0.2*y}
light_source {<1,2,-1>*1000, color 0.5 shadowless}
light_source {<1,2,-1>*1000, color 0.5}
plane {y, 0 pigment {checker rgb 1.1, rgb 0.9 scale 0.2}}

#declare MyTex =
texture {
   pigment {
      bumps scale 0.03
      color_map {
         [0.5, color <1.0,1.1,1.2>]
         [0.6, color <0.8,0.7,0.6>]
      }
   }
}

// A path is specified as a macro WalkPath(Time) returning a point
// for any given time value:
#macro WalkPath (Time)
   vrotate(Time*x,180*Time*y)
#end

// The current time value Time is specified,
// usually dependent on the clock.
#declare Time  = -1+2*clock;

// For any given Time value there will be a time value
// for the previous "touchdown" of the foot and one for
// the next "touchdown" of the foot. The previous one is
// found using a floor() function of the current Time value.
// Higher frequency can beachieved by multiplying the Time
// value by a constant and dividing the result with the same
// constant. The next time value is found by adding 1 to the
// time inside the floor() function:
#declare PrevTime = floor(Time*4  )/4;
#declare NextTime = floor(Time*4+1)/4;

// Three points on the specified WalkPath are found -
// one for the current Time value, one for the previous
// "touchdown" and one for the next "touchdown".
#declare CurPoint  = WalkPath(Time);
#declare PrevPoint = WalkPath(PrevTime);
#declare NextPoint = WalkPath(NextTime);

// For the three time values are also calculated forward vectors,
// that is, vectors pointing in the direction the path is going.
#declare CurForward  = vnormalize(WalkPath(Time    +0.001)-CurPoint );
#declare PrevForward = vnormalize(WalkPath(PrevTime+0.001)-PrevPoint);
#declare NextForward = vnormalize(WalkPath(NextTime+0.001)-NextPoint);

// Here is a tricky part. CycleTime calculates a value that
// can be from 0 to 1, which is a walk full cycle for the foot.
// If the current Time value is close to the time value of the
// previous "touchdown" then CycleTime is close to 0.
// If the current Time value is close to the time value of the
// next "touchdown" then CycleTime is close to 1.
#declare CycleTime = ((Time-PrevTime)/(NextTime-PrevTime));

// When the foot hit the ground it shouldn't leave instantly
// but wait on the ground for a little while before moving on.
// This is achieved with the FootMove value which is a variation
// of CycleTime.
// When CycleTime goes from 0.00 to 0.25, FootMove stays at 0.
// When CycleTime goes from 0.25 to 0.75, FootMove goes from 0 to 1.
// When CycleTime goes from 0.75 to 1.00, FootMove stays at 1.
#declare FootMove = min(1,max(0,CycleTime*2-0.5));

// Now calculate the current location of the foot by
// interpolating between the location of the last touchdown
// and the location of the next touchdown. The FootMove value
// controls the interpolation.
// A sin() function is added to make the foot move in a curve
// up and down instead of just in a straight line.
#declare FootPoint = (
   +PrevPoint*(1-FootMove)
   +NextPoint*(  FootMove)
   +sin(FootMove*pi)*0.1*y
);
// The current forward vector of the foot is found by interpolating
// between the forward vector at the last touchdown and at the next.
#declare FootForward = vnormalize(
   +PrevForward*(1-FootMove)
   +NextForward*(  FootMove)
);

// Given FootPoint and FootForward, calculate a transform for the foot.
#declare FootZ = FootForward;
#declare FootY = VPerp_Adjust(y,FootZ);
#declare FootX = vcross(FootY,FootZ);
#declare FootP = FootPoint;
#declare FootT = Matrix_Trans(FootX,FootY,FootZ,FootP);
// A simple representation of a foot.
cone {
   -0.1*z, 0.06, 0.1*z, 0.04 scale <1,0.5,1>
   texture {MyTex} transform {FootT}
}

// Given CurPoint and CurForward, calculate a transform for the "body".
#declare BodyZ = CurForward;
#declare BodyY = VPerp_Adjust(y,BodyZ);
#declare BodyX = vcross(BodyY,BodyZ);
#declare BodyP = CurPoint+0.5*y;
#declare BodyT = Matrix_Trans(BodyX,BodyY,BodyZ,BodyP);
// A simple representation of a body.
sphere {0, 0.06 texture {MyTex} transform {BodyT}}

// The following is just some inverse kinematics calculations
// taken directly from the "Goodies Page" on my website:
// http://rsj.mobilixnet.dk/3d/goodies/goodies.html

// Specify HipPoint and AnklePoint, and the entire leg will follow.
// 
// You also need to specify the length of the thigh and the shin as
// well as the direction in which the knee should be pointing.
// 
// Note that the KneeDirection should always be adjusted to fit your
// needs. If your are making an animation where the knee points in
// many different directions, the KneeDirection should be animated
// accordingly.

#declare HipPoint      = BodyP;
#declare AnklePoint    = FootPoint-0.1*FootForward;
#declare ThighLength   = 0.25;
#declare ShinLength    = 0.35;
#declare KneeDirection = (CurForward+FootForward)/2;

// Now specify the objects used for the thigh and the shin.

#declare ThighObject =
sphere {0, 1 scale <0.05,0.25/2,0.05> translate <0,-0.25/2,0> texture {MyTex}}

#declare ShinObject =
sphere {0, 1 scale <0.05,0.35/2,0.05> translate <0,-0.35/2,0> texture {MyTex}}

// Knee calculates the point of the knee given these input:
// pA: Point of the ankle.
// pH: Point of the hip.
// LT: Length of thigh.
// LS: Length of shin.
// vD: Direction in which the knee is pointing.
#macro Knee(pA,pH,LT,LS,vD) // by Tor Olav Kristensen
   #local vB=pA-pH;
   #local LB=vlength(vB);
   #if(LB>LT+LS) #error "Ankle and hip are too far apart.\n" #end
   #if(LB<abs(LT-LS)) #error "Ankle and hip are too close.\n" #end
   #local aa=(LB*LB+LT*LT-LS*LS)/2/LB;
   #local bb=sqrt(LT*LT-aa*aa);
   #local vF=vcross(vB,vcross(vD,vB));
   (pH+aa*vnormalize(vB)+bb*vnormalize(vF))
#end

// Perpendiculize will adjust the vector V1
// so that it is perpendicular to the vector V2.
// The input is V1 and V2, and the macro returns
// the adjusted version of V1.
#macro Perpendiculize (V1,V2) // by Rune S. Johansen
   vnormalize(vcross(vcross(V2,V1),V2))
#end

// Vectors2Matrix converts 4 vectors to a matrix transform.
// One vector for each of the 4 rows in a matrix.
#macro Vectors2Matrix (vA,vB,vC,vD) // by Rune S. Johansen
   matrix <vA.x,vA.y,vA.z,vB.x,vB.y,vB.z,vC.x,vC.y,vC.z,vD.x,vD.y,vD.z>
#end

// Now do the calculations to find the transformations used to place
// the thigh and shin.

#declare KneeVector = Perpendiculize(KneeDirection,HipPoint-AnklePoint);

#declare KneePoint = Knee(HipPoint,AnklePoint,ShinLength,ThighLength,KneeVector);

#declare ThighY = vnormalize(HipPoint-KneePoint);
#declare ThighZ = Perpendiculize(KneeVector,ThighY);
#declare ThighX = vcross(ThighY,ThighZ);
#declare ThighP = HipPoint;
#declare ThighTransform = transform{Vectors2Matrix(ThighX,ThighY,ThighZ,ThighP)}

#declare ShinY = vnormalize(KneePoint-AnklePoint);
#declare ShinZ = Perpendiculize(KneeVector,ShinY);
#declare ShinX = vcross(ShinY,ShinZ);
#declare ShinP = KneePoint;
#declare ShinTransform = transform{Vectors2Matrix(ShinX,ShinY,ShinZ,ShinP)}

// And last, place the thigh and shin objects in the scene...

object {ThighObject transform ThighTransform}
object {ShinObject transform ShinTransform}