/**
* This node (motor schema) generates a vector away from the
* items detected by its embedded perceptual schema.
* Magnitude varies from 0 to 1.
* (c)1997, 1998 Tucker Balch
*/
package EDU.gatech.cc.is.clay;
import java.lang.*;
import EDU.gatech.cc.is.util.Vec2;
import EDU.gatech.cc.is.util.Units;
public class v_Avoid_va extends NodeVec2
{
public static final boolean DEBUG = Node.DEBUG;
private NodeVec2Array embedded1;
private double sphere = 1.0;
private double safety = 0.0;
/**
Instantiate a v_Avoid_va schema.
@param soe double, the sphere of influence beyond which the hazards
are not considered.
@param s double, the safety zone, inside of which a maximum repulsion
from the object is generated.
@param im1 NodeVec2Array, the embedded node that generates a list
of items to avoid.
*/
public v_Avoid_va(double soe, double s, NodeVec2Array im1)
{
if (DEBUG) System.out.println("v_Avoid_va: instantiated.");
embedded1 = im1;
if ((soe < s) || (soe<0) || (s<0))
{
System.out.println("v_Avoid_va: illegal parameters");
return;
}
sphere = soe;
safety = s;
}
Vec2 last_val = new Vec2();
long lasttime = 0;
/**
Return a Vec2 representing the direction to go away from
the detected hazards.
@param timestamp long, only get new information if timestamp > than last call
or timestamp == -1.
@return the movement vector.
*/
public Vec2 Value(long timestamp)
{
double tempmag;
double max_mag=0;
if ((timestamp > lasttime)||(timestamp == -1))
{
/*--- reset the timestamp ---*/
if (timestamp > 0) lasttime = timestamp;
/*--- reset output ---*/
last_val.setr(0);
/*--- get the list of obstacles ---*/
Vec2[] obs = embedded1.Value(timestamp);
/*--- consider each obstacle ---*/
for(int i = 0; i<obs.length; i++)
{
/*--- too close ? ---*/
if (obs[i].r < safety)
{
tempmag = -1*Units.HUGE;
}
/*--- controlled zone ---*/
else if (obs[i].r < sphere)
tempmag =
-1*(sphere - obs[i].r)/
(sphere - safety);
/*--- outside sphere ---*/
else tempmag = 0;
/*--- set the repulsive vector ---*/
obs[i].setr(tempmag);
/*--- check if maximum value ---*/
if(Math.abs(tempmag)>max_mag)
max_mag = Math.abs(tempmag);
/*--- add it to the sum ---*/
if (DEBUG) System.out.println(obs[i]);
last_val.add(obs[i]);
}
if (last_val.r>1.0)
last_val.setr(max_mag);
if (DEBUG) System.out.println("v_Avoid_va.Value: "+
obs.length+" obstacles "+
"output "+
last_val);
}
return (new Vec2(last_val.x, last_val.y));
}
}