Avoid Cone Chaser

#VRML V2.0 utf8

PROTO AvoidConeChaser [
		      field SFVec3f position 0 0 0
		      field SFVec3f goal 0 0 0
		      field SFFloat speed 1
		      field MFVec3f obstacles [ ]
		      field SFInt32 index 0
		      field SFVec3f direction 0 0 1
		      field SFVec3f scale 1 1 1
		      field SFFloat turnLimit 1   # radians per second
		      eventIn SFVec3f set_goal
		      eventIn MFVec3f obstacles_changed
		      eventOut MFFloat report_position
		     ]
{
   Group {
      children [
	 DEF MOVEME Transform {
	    translation IS position
	    children Transform {
	       translation 0 0 -.25
	       rotation 1 0 0  1.57 
	       scale IS scale
	       children Shape {
		  appearance Appearance { material Material {} }
		  geometry Cone {}
	       }
	    }
	 }

	 DEF HEART TimeSensor { loop TRUE }

	 # update direction and position when receive "heartbeats"
	 DEF SCRIPT Script {
	    eventIn SFTime beat
	    eventIn SFVec3f set_goal IS set_goal
	    eventIn MFVec3f obstacles_changed IS obstacles_changed
	    eventOut SFVec3f position_changed
	    eventOut SFRotation rotation_changed  # update own rotation
	    eventOut MFFloat report_position IS report_position
	    field SFInt32 index IS index
	    field SFVec3f direction IS direction
	    field SFVec3f position IS position
	    field SFVec3f goal IS goal
	    field SFFloat speed IS speed
	    field SFTime lastBeat 0
	    field MFVec3f obstacles IS obstacles
	    field SFFloat repulse 10
	    field SFFloat attract 2
	    field SFFloat power 2
	    field SFFloat rotation 0
	    field SFFloat turnLimit IS turnLimit  # max radians per second
	    field SFFloat changeAngle 0  # desired amount to rotate by
	    field SFFloat twopi 6.2832  # so don't keep recalculating
	    field SFBool first TRUE
	    url "vrmlscript:

// clamp val between min and max numbers
function clamp(val, min, max) {
   if(val<min) return min;
   else if(val > max) return max;
   else return val;
}

function repulseForce(obstacle) {
   v = position.subtract(obstacle);
   ods = repulse * 1/Math.pow(v.length(), power);
   return v.normalize().multiply(ods);
}

// Every so often, a script node's eventsProcessed function is called
// after a few events have been received.  Complicated calculations should
// go here.  In this case, calculating the new direction we should be
// travelling.
function eventsProcessed() {
   // OLD:   desiredDirection = goal.subtract(position).normalize();
   // NEW:
   // compute force of attraction towards goal ('Go G-Force!')
   gforce = goal.subtract(position).normalize().multiply(attract);

   // compute repulsive force away from obstacles
   rforce = new SFVec3f(0,0,0);
   for(i=0; i<obstacles.length; i++) {
      if(i!=index) {
	 r = repulseForce(obstacles[i]);
	 rforce = rforce.add(r);
      }
   }
   force = gforce.add(rforce);
   desiredDirection = force.normalize();

   // calc rotation from z to be going in desiredDirection
   a = new SFVec3f(0, 0, 1);
   b = desiredDirection;
   adb = a.dot(b);
   desiredRotation = Math.acos(a.dot(b));
   if(b.x < 0) desiredRotation = twopi - desiredRotation;

   // calc angle we need to change to reach desiredRotation
   desiredAngle = desiredRotation - rotation;
   if(desiredAngle > Math.PI) desiredAngle += -twopi;
   else if(desiredAngle < -Math.PI) desiredAngle += twopi;

   // clamp the angle to our turnLimit to slow turn
   changeAngle = clamp(desiredAngle, -turnLimit, turnLimit);
}

function calcDirection(t) {
   // update direction and rotation
   maxAngleThisBeat = turnLimit * t;
   angle = clamp(changeAngle, -maxAngleThisBeat, maxAngleThisBeat);
   rotation += angle;
   if (rotation < 0) rotation = rotation + twopi;
   if (rotation > twopi) rotation = rotation - twopi;
   r = new SFRotation(0, 1, 0, rotation);
   rotation_changed = r;
   x = Math.round(direction.x * 10) / 10;
   y = Math.round(direction.y * 10) / 10;
   z = Math.round(direction.z * 10) / 10;
   direction = r.multVec(new SFVec3f(0, 0, 1));
}

function set_goal(val) {
   goal = val;
}

function beat(val) {
   if(first) { // initialize lastBeat on first heartbeat
      first = FALSE;
   }
   else {
      timeElapsed = val - lastBeat;
      calcDirection(timeElapsed);
      position = position.add(direction.multiply(speed*timeElapsed));
      position_changed = position;
      report_position = new MFFloat(index, position.x, position.y, position.z);
   }
   lastBeat = val;
}

function obstacles_changed(val) {
   obstacles = val;
}

"
	 }
	]
   }

   ROUTE HEART.time TO SCRIPT.beat
   ROUTE SCRIPT.position_changed TO MOVEME.translation
   ROUTE SCRIPT.rotation_changed TO MOVEME.rotation
}