IK Spider Leg

#VRML V2.0 utf8

PROTO SpiderLeg [
		 field SFVec3f base 0 0 0  # hip position in space
		 field SFVec3f target 1 0 0  # where should foot go?
		 eventIn SFVec3f set_base
		 eventIn SFVec3f set_target
		]
{
   Group {
      children [
	 # shadow
	 DEF SHADOW Transform {
	    children Shape {
	       appearance Appearance {
		  material Material { emissiveColor 0 0 0 }
	       }
	       geometry IndexedLineSet {
		  coord DEF SHADCOORDS Coordinate { 
		     point [ 0 .01 0, 2 .01 0 ] }
		  coordIndex [ 0 1 ]
	       }
	    }
	 }
	 
	 # leg
	 DEF HIPY Transform {
	    translation IS base
	    set_translation IS set_base
	    children DEF HIPZ Transform {
	       children [
		  DEF LINE Shape {
		     appearance Appearance {
			material Material { emissiveColor .7 .3 .2 }
		     }
		     geometry IndexedLineSet {
			coord Coordinate { point [ 0 0 0, 1 0 0 ] }
			coordIndex [ 0 1 ]
		     }
		  }
		  DEF KNEE Transform {
		     translation 1 0 0
		     children USE LINE
		  }
		 ]
	    }
	 }

	 # do IK and calc shadow
	 DEF SCRIPT Script {
	    eventIn SFVec3f set_base IS set_base
	    eventIn SFVec3f set_target IS set_target
	    field SFVec3f base IS base
	    field SFVec3f target IS target
	    eventOut SFRotation hipYRotation
	    eventOut SFRotation hipZRotation
	    eventOut SFRotation kneeRotation
	    eventOut MFVec3f shadowPoints
	    eventOut SFVec3f shadowPosition
	    url "vrmlscript:

function calcJointAngles()
{
   // calc shoulder y rotation (angle to rotate 1 0 0 to point at target)
   a = new SFVec3f(1, 0, 0);
   b = new SFVec3f(target.x-base.x, 0, target.z-base.z);
   b = b.normalize();
   angle = Math.acos(a.dot(b));
   if(target.z > base.z) angle = 2*Math.PI - angle;
   hipYRotation = new SFRotation(0, 1, 0, angle);
   
   // calc shoulder z rotation (to get knee calfLength away from target)
   thighLength = 1;
   thighLength2 = Math.pow(thighLength,2);
   calfLength = 1;
   calfLength2 = Math.pow(calfLength,2);
   targDist = target.subtract(base).length();
   targDist2 = Math.pow(targDist,2);

   shadowPoints = new MFVec3f();
   shadowPoints[0] = new SFVec3f(0, 0.01, 0);

   legLength = thighLength + calfLength;
   xztarg = new SFVec3f(target.x-base.x, 0, target.z-base.z);

   if(targDist > legLength) {
      theta = 0;
      alpha = 0;
      shadowPoints[1] = new SFVec3f(legLength, 0.01, 0);
   } 
   else {
      a = new SFVec3f(1, 0, 0);
      b = new SFVec3f(xztarg.length(), -base.y, 0);
      b = b.normalize();
      angle = Math.acos(a.dot(b));
      theta = Math.acos((calfLength2 - thighLength2 - targDist2) / 
			(-2 * thighLength * targDist));
      theta -= angle;

      alpha = Math.PI - Math.acos((targDist2 - calfLength2 - thighLength2) /
				  (-2 * calfLength * thighLength));
      alpha = -alpha;
      shadowPoints[1] = new SFVec3f(xztarg.length(), .01, 0);
   }
   hipZRotation = new SFRotation(0, 0, 1, theta);
   kneeRotation = new SFRotation(0, 0, 1, alpha);
}

// reposition hip attachment
function set_base(val) { 
   base = val; 
   shadowPosition = new SFVec3f(base.x, 0, base.z);
   calcJointAngles();
}

// reposition foot
function set_target(val) {
   target = val;
   calcJointAngles();
}

function initialize() {
   calcJointAngles();
}

"
	 }
	]
   }
   ROUTE SCRIPT.hipYRotation TO HIPY.rotation
   ROUTE SCRIPT.hipYRotation TO SHADOW.rotation
   ROUTE SCRIPT.hipZRotation TO HIPZ.rotation
   ROUTE SCRIPT.kneeRotation TO KNEE.rotation
   ROUTE SCRIPT.shadowPoints TO SHADCOORDS.point
   ROUTE SCRIPT.shadowPosition TO SHADOW.translation
}