Cone Chaser

#VRML V2.0 utf8

PROTO ConeChaser [
	      field SFVec3f position 0 0 0
	      field SFVec3f goal 0 0 0
	      field SFVec3f direction 0 0 1
	      field SFVec3f scale 1 1 1
	      field SFFloat speed 1  # meters per second
	      field SFFloat turnLimit 1   # radians per second
	      eventIn SFVec3f set_goal
	     ]
{
   Group {
      children [
	 DEF MOVEME Transform {
	    translation IS position
	    children Transform {
	       translation 0 0 -.5
	       rotation 1 0 0  1.57 
	       scale IS scale
	       children Shape {
		  appearance Appearance { material Material {} }
		  geometry Cone {}
	       }
	    }
	 }

	 # heart sends "regular" events to body for state updates
	 DEF HEART TimeSensor { loop TRUE }

	 # Calculate and update direction and position 
         # when receive "heartbeats".  When new goal is received,
         # calculate a new desired direction, but don't actually change
         # rotation, direction, etc... let beat event do that.
         # Otherwise it will turn faster when the goal is moving.
	 DEF SCRIPT Script {
	    eventIn SFTime beat
	    eventIn SFVec3f set_goal IS set_goal
	    eventOut SFVec3f position_changed  # update own position
	    eventOut SFRotation rotation_changed  # update own rotation
	    field SFVec3f direction IS direction
	    field SFVec3f position IS position
	    field SFVec3f goal IS goal
	    field SFFloat speed IS speed  # meters per second 
	    field SFTime lastBeat 0
	    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;
}

// 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() {
   desiredDirection = goal.subtract(position).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 set_goal(val) {
   goal = val;
}

function beat(val) {
   // on first heartbeat, just store time
   if(lastBeat == 0) {
      lastBeat = val;
   }
   else {
      timeElapsed = val - lastBeat;

      position = position.add(direction.multiply(speed*timeElapsed));
      position_changed = position;
      
      // update direction and rotation
      maxAngleThisBeat = turnLimit * timeElapsed;
      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;
      direction = r.multVec(new SFVec3f(0, 0, 1));
      
      lastBeat = val;
   }
}

"
	 }
	]
   }

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