Cybernetics Wiki
Advertisement

Класс v_Avoid_va[]

  1 /**
  2  * This node (motor schema) generates a vector away from the
  3  * items detected by its embedded perceptual schema.
  4  * Magnitude varies from 0 to 1.
  5  * (c)1997, 1998 Tucker Balch
  6  */
  7 
  8 package EDU.gatech.cc.is.clay;
  9 
 10 import java.lang.*;
 11 import EDU.gatech.cc.is.util.Vec2;
 12 import EDU.gatech.cc.is.util.Units;
 13 
 14 public class v_Avoid_va extends NodeVec2
 15 {
 16 	public static final boolean DEBUG = Node.DEBUG;
 17 	private NodeVec2Array	embedded1;
 18 	private double		sphere = 1.0;
 19 	private double	        safety = 0.0;
 20 
 21 	/**
 22 	Instantiate a v_Avoid_va schema.
 23 	@param soe double, the sphere of influence beyond which the hazards
 24 		are not considered.
 25 	@param s double, the safety zone, inside of which a maximum repulsion
 26 		from the object is generated.
 27 	@param im1 NodeVec2Array, the embedded node that generates a list
 28 		of items to avoid.
 29 	*/
 30 	public v_Avoid_va(double soe, double s, NodeVec2Array im1)
 31 	{
 32 		if (DEBUG) System.out.println("v_Avoid_va: instantiated.");
 33 		embedded1 = im1;
 34 		if ((soe < s) || (soe<0) || (s<0))
 35 		{
 36 			System.out.println("v_Avoid_va: illegal parameters");
 37 			return;
 38 		}
 39 		sphere = soe;
 40 		safety = s;
 41 	}
 42 
 43 
 44 	Vec2	last_val = new Vec2();
 45 	long	lasttime = 0;
 46 	/**
 47 	Return a Vec2 representing the direction to go away from
 48 	the detected hazards.
 49 	@param timestamp long, only get new information if timestamp > than last call
 50                 or timestamp == -1.
 51 	@return the movement vector.
 52 	*/
 53 	public Vec2 Value(long timestamp)
 54 	{
 55 		double	tempmag;
 56 		double	max_mag=0;
 57 
 58 		if ((timestamp > lasttime)||(timestamp == -1))
 59 		{
 60 			/*--- reset the timestamp ---*/
 61 			if (timestamp > 0) lasttime = timestamp;
 62 
 63 			/*--- reset output ---*/
 64 			last_val.setr(0);
 65 
 66 			/*--- get the list of obstacles ---*/
 67 			Vec2[] obs = embedded1.Value(timestamp);
 68 
 69 			/*--- consider each obstacle ---*/
 70 			for(int i = 0; i<obs.length; i++)
 71 			{
 72 				/*--- too close ? ---*/
 73 				if (obs[i].r < safety)
 74 				{
 75 					tempmag = -1*Units.HUGE;
 76 				}
 77 				/*--- controlled zone ---*/
 78 				else if (obs[i].r < sphere)
 79 					tempmag = 
 80 						-1*(sphere - obs[i].r)/
 81 						(sphere - safety);
 82 				/*--- outside sphere ---*/
 83 				else tempmag = 0;
 84 				
 85 				/*--- set the repulsive vector ---*/
 86 				obs[i].setr(tempmag);
 87 
 88 				/*--- check if maximum value ---*/
 89 				if(Math.abs(tempmag)>max_mag) 
 90 					max_mag = Math.abs(tempmag);
 91 
 92 				/*--- add it to the sum ---*/
 93 				if (DEBUG) System.out.println(obs[i]);
 94 				last_val.add(obs[i]);
 95 			}
 96 			if (last_val.r>1.0)
 97 				last_val.setr(max_mag);
 98 			if (DEBUG) System.out.println("v_Avoid_va.Value: "+
 99 				obs.length+" obstacles "+
100 				"output "+
101 				last_val);
102 		}
103 		return (new Vec2(last_val.x, last_val.y));
104 	}
105 }
Advertisement