Avoid Box Chaser

#VRML V2.0 utf8

PROTO AvoidBoxChaser [
		      field SFVec3f position 0 0 0
		      field SFVec3f goal 0 0 0
		      field SFFloat speed 1
		      field MFVec3f obstacles [ ]
		      field SFInt32 index -1
		      eventIn SFVec3f set_goal
		      eventIn MFVec3f obstacles_changed
		      eventOut MFFloat report_position
		     ]
{
   Group {
      children [
	 DEF MOVEME Transform {
	    translation IS position
	    children Shape {
	       appearance Appearance { material Material {} }
	       geometry Box { size .5 .5 .5 }
	    }
	 }

	 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 MFFloat report_position IS report_position
	    field SFInt32 index IS index
	    field SFVec3f direction 0 0 1
	    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 SFBool first TRUE
	    field SFFloat repulse 4
	    field SFFloat attract 2
	    field SFFloat power 2.5
	    url "vrmlscript:

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

function calcDirection() {
   // compute force of attraction towards goal
   gforce = goal.subtract(position).normalize().multiply(attract);

   // computer 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);

   direction = force.normalize();
}

function set_goal(val) {
   goal = val;
   calcDirection();
}

function beat(val) {
   if(first) { // initialize lastBeat on first heartbeat
      lastBeat = val; 
      first = FALSE;
   }
   else {
      timeElapsed = val - lastBeat;
      calcDirection();
      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
}