Class v Avoid va

Класс v_Avoid_va

/**
 * 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));
	}
}