import javax.vecmath.Point2f; //导入依赖的package包/类
public static int contains(final DoublePolygon[] triangles, final Point2f pt)
{
	int ct = -1;
	
	for (int k = 0; k < triangles.length; k++)
	{
		// Here, we can actually give out all the local variables except the BGR, 
		// the BGR is not shape based and will be given by function VO_LoadTexturesFromShapes()
		if( triangles[k].contains(pt.x, pt.y) )
		{
			ct = k;
			break;
		}
	}
	
	return ct;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/** Constructor to create a VO_Triangle2D object with three coordinate vertexes in vector format */
   public VO_Triangle2D(final Point2f[] iVertexes)
   {
   							super(iVertexes);
   							if(this.m_MatShape[0].length != 3)
							{
								System.out.println( "This is not a triangle!");
								System.out.println( "Please ensure the shape has 3 columns");
								System.exit(1);
							}
							if(this.m_MatShape.length != 2)
							{
								System.out.println( "This triangle is not in 2D!");
								System.exit(1);
							}
							this.m_dD 			= this.Calc_dD ();
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public static int					IsPointInTriangles(final Point2f pt, final VO_Triangle2DStructure[] triangles)
{
    int NbOfTriangles = triangles.length;
    DoublePolygon[] dps = new DoublePolygon[NbOfTriangles];
    int npoints = 3;
    double[] xpoints = new double[npoints];
    double[] ypoints = new double[npoints];
    
    for(int i = 0; i < NbOfTriangles; i++)
    {
    	xpoints[0] = triangles[i].GetA2DPoint(0).x;
    	xpoints[1] = triangles[i].GetA2DPoint(1).x;
    	xpoints[2] = triangles[i].GetA2DPoint(2).x;
    	ypoints[0] = triangles[i].GetA2DPoint(0).y;
    	ypoints[1] = triangles[i].GetA2DPoint(1).y;
    	ypoints[2] = triangles[i].GetA2DPoint(2).y;
    	dps[i] = new DoublePolygon(xpoints, ypoints, 3);
    }

	return (com.visionopen.jiapei.utils.Common.contains(dps, pt));
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public ImageData GetCurrentShapeImageData()
{
	currentShape = new VO_Shape(m_CurrentAlignedShape);
	currentShape.Scale(m_appearanceModel.m_fAverageShapeSize);
	float[] sm = com.visionopen.jiapei.utils.Common.Neg(currentShape.Min());
	currentShape.Translate(sm);
	Rectangle rect = currentShape.GetShapeBoundRect();
	shapeImageData = new ImageData(rect.width, rect.height, templateImageData.depth, palette);
	
	Point2f pt = new Point2f();
	int NbOfPoints = currentShape.GetNbOfPoints();
	for(int i = 0; i < NbOfPoints; i++)
   	{
   		pt = currentShape.GetA2DPoint(i);
   		shapeImageData.setPixel((int)pt.x, (int)pt.y, 0xFFFFFF);
   	}
	
	return shapeImageData;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
    * Sets this sound's distance gain attenuation - where gain scale factor
    * is applied to sound based on distance listener is from sound source.
    * @param distance attenuation pairs of (distance,gain-scale-factor)
    */  
   void setDistanceGain(Point2f[] attenuation) { 
       // if attenuation array null set both attenuation components to null
       if (attenuation == null) {
           this.attenuationDistance = null;
           this.attenuationGain = null;
    // QUESTION: is this needed so that dispatch***() doesn't
    // fail with null?
           return; 
       }

       int attenuationLength = attenuation.length;
       this.attenuationDistance = new float[attenuationLength];
       this.attenuationGain = new float[attenuationLength];
       for (int i = 0; i < attenuationLength; i++) {
          this.attenuationDistance[i] = attenuation[i].x;
          this.attenuationGain[i] = attenuation[i].y;
       }
       dispatchAttribChange(DISTANCE_GAIN_DIRTY_BIT, attenuation);
if (source != null && source.isLive()) {
    notifySceneGraphChanged(false);
}
   }
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
 * Retieves sound's distance attenuation
 * Put the contents of the two separate distance and gain arrays into
 * an array of Point2f.
 * @param attenuation containing distance attenuation pairs
 */ 
void getDistanceGain(Point2f[] attenuation) {
    // write into arrays passed in, don't do a new
    if (attenuation == null)
        return;
    if (this.attenuationDistance == null ||
        this.attenuationGain == null)
        return;
    int attenuationLength = attenuation.length;
    // attenuationDistance and Gain array lengths should be the same
    int distanceLength = this.attenuationDistance.length;
    if (distanceLength > attenuationLength)
        distanceLength = attenuationLength;
    for (int i=0; i< distanceLength; i++) {
        attenuation[i].x = attenuationDistance[i];
        attenuation[i].y = attenuationGain[i];
    }
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
    * Set Distance Filter (based on distances and frequency cutoff)
    * @param attenuation array of pairs defining distance frequency cutoff
    */
   synchronized void setDistanceFilter(Point2f[] attenuation) {
       if (attenuation == null) {
           this.filterType = NO_FILTERING;
           return;
       }
       int attenuationLength = attenuation.length;
       if (attenuationLength == 0) {
           this.filterType = NO_FILTERING;
           return;
       }
       this.filterType = LOW_PASS;
       // Reallocate every time unless size of new array equal old array
       if ( distance == null ||
           (distance != null && (distance.length != attenuationLength) ) ) {
           this.distance = new float[attenuationLength];
           this.frequencyCutoff = new float[attenuationLength];
       }
       for (int i = 0; i< attenuationLength; i++) {
           this.distance[i] = attenuation[i].x;
           this.frequencyCutoff[i] = attenuation[i].y;
       }
this.aaDirty = true;
notifyUsers();
   }
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
 * Retrieve Distance Filter (distances and frequency cutoff)
 * @return attenaution pairs of distance and frequency cutoff filter
 */
void getDistanceFilter(Point2f[] attenuation) {
    // Write into existing param array already allocated
    if (attenuation == null)  
        return;
    if (this.distance == null || this.frequencyCutoff == null)
        return;
    // The two filter attenuation arrays length should be the same
    // We can assume that distance and filter lengths are the same
    // and are non-zero.
    int distanceLength = this.distance.length;
    // check that attenuation array large enough to contain
    // auralAttribute arrays
    if (distanceLength > attenuation.length)
        distanceLength = attenuation.length;
    for (int i=0; i< distanceLength; i++) {
        attenuation[i].x = this.distance[i];
        if (filterType == NO_FILTERING)
            attenuation[i].y = Sound.NO_FILTER;
        else if (filterType == LOW_PASS)
            attenuation[i].y = this.frequencyCutoff[i];
        if (debugFlag)
            debugPrint("AAR: getDistF: " + attenuation[i].x + ", " +
              attenuation[i].y);
    }
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**  
 * Retieves sound's back distance attenuation
 * Put the contents of the two separate distance and gain arrays into
 * an array of Point2f.
 * @param attenuation containing distance attenuation pairs
 */  
void getBackDistanceGain(Point2f[] attenuation) {
    // Write into arrays passed in, don't do a new
    if (attenuation == null)
        return;
    if (this.backAttenuationDistance == null ||
        this.backAttenuationGain == null)
        return;
    // These two array length should be the same
    // can assume lengths are non-zero
    int distanceLength = this.backAttenuationDistance.length;
    int attenuationLength = attenuation.length;
    if (distanceLength < attenuationLength)
        distanceLength = attenuationLength;
    for (int i=0; i< distanceLength; i++) {
        attenuation[i].x = this.backAttenuationDistance[i];
        attenuation[i].y = this.backAttenuationGain[i];
    }
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
 * Constructs and initializes a new AuralAttributes object using specified
 * parameters including an array of Point2f for the distanceFilter.
 * @param gain amplitude scale factor
 * @param rolloff atmospheric (changing speed of sound) scale factor
 * @param reflectionCoefficient reflective/absorptive factor applied to reflections
 * @param reverbDelay delay time before start of reverberation
 * @param reverbOrder limit to number of reflections added to reverb signal
 * @param distanceFilter frequency cutoff
 * @param frequencyScaleFactor applied to change of pitch
 * @param velocityScaleFactor applied to velocity of sound in relation to listener
 */  
public AuralAttributes(float     	gain,
                  float      	rolloff,
                  float      	reflectionCoefficient,
                  float      	reverbDelay,
                  int	      	reverbOrder,
                  Point2f[]         distanceFilter,
                  float      	frequencyScaleFactor,
                  float      	velocityScaleFactor) {
    // set default read capabilities
    setDefaultReadCapabilities(readCapabilities);

    ((AuralAttributesRetained)this.retained).setAttributeGain(gain);
    ((AuralAttributesRetained)this.retained).setRolloff(rolloff);
    ((AuralAttributesRetained)this.retained).setReflectionCoefficient(
                  reflectionCoefficient);
    ((AuralAttributesRetained)this.retained).setReverbDelay(reverbDelay);
    ((AuralAttributesRetained)this.retained).setReverbOrder(reverbOrder);
    ((AuralAttributesRetained)this.retained).setDistanceFilter(
                  distanceFilter);
    ((AuralAttributesRetained)this.retained).setFrequencyScaleFactor(
                  frequencyScaleFactor);
    ((AuralAttributesRetained)this.retained).setVelocityScaleFactor(
                  velocityScaleFactor);
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
 * Construct a PointSound object accepting Point3f as input for the position
 * and accepting an array of Point2f for the distance attenuation values 
 * where each pair in the array contains a distance and a gain scale factor.
 * @param soundData sound data associated with this sound source node
 * @param initialGain amplitude scale factor applied to sound source
 * @param loopCount number of times loop is looped
 * @param release flag denoting playing sound data to end
 * @param continuous denotes that sound silently plays when disabled
 * @param enable sound switched on/off
 * @param region scheduling bounds
 * @param priority playback ranking value
 * @param position 3D location of source
 * @param distanceGain array of (distance,gain) pairs controling attenuation
 */  
public PointSound(MediaContainer soundData,
                  float initialGain,
                  int loopCount,
                  boolean release,
                  boolean continuous,
                  boolean enable,
                  Bounds  region,
                  float   priority,
                  Point3f position, 
                  Point2f[] distanceGain) {

    super(soundData, initialGain, loopCount, release, continuous, 
               enable, region, priority );

    // set default read capabilities
    setDefaultReadCapabilities(readCapabilities);

    ((PointSoundRetained)this.retained).setPosition(position);
    ((PointSoundRetained)this.retained).setDistanceGain(distanceGain);
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public boolean deserializeCommand(String data) {
	
	startScaleFactor = new Point2f();
	endScaledFactor = new Point2f();
	
	String[] tokens = 
			data.split(TestDemultiplexerConstants.SERIAL_SEPARATOR);
	
	if (tokens.length != NUM_SERIAL_TOKENS) {
		return false;
	} else if (!tokens[0].equals(SERIALIZED_KEY)) {
		return false;
	}
	
	this.startScaleFactor.set(
			Float.parseFloat(tokens[1]), 
			Float.parseFloat(tokens[2]));
	
	this.endScaledFactor.set(
			Float.parseFloat(tokens[3]), 
			Float.parseFloat(tokens[4]));
	
	this.ms = Long.parseLong(tokens[5]);
	
	return true;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public SomaticContext(SomaticContext state) {
	frames = new EnumMap<Frames, FrameState>(Frames.class);
	for (Frames frame : state.frames.keySet())
		frames.put(frame, state.frames.get(frame).clone());
	robot = state.robot;
	com = new Vector3f(state.com);
	bodyPosture = new Matrix3f(state.bodyPosture);
	bodyHeight = state.bodyHeight;
	confidence = state.confidence;
	time = state.time;
	leftOnGround = state.leftOnGround;
	rightOnGround = state.rightOnGround;
	leftPressure = state.leftPressure;
	rightPressure = state.rightPressure;
	leftCOP = new Point2f(state.leftCOP);
	rightCOP = new Point2f(state.rightCOP);
	cop = new Point2f(state.cop);
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/**
 * ボールをトラッキングします.
 *
 * makemotion_headが発行されればtrueを返し，ボールが見つからない場合はfalseを返します.
 *
 * @return
 */
private boolean trackBall() {
	VisualObject vo = context.getBall().getVision();
	if (vo.confidence > BALLCONF_THRESHOLD) {
		Point2f angle = vo.angle;
		float dx = -angle.getX();
		float dy = -angle.getY();

		float kpTh = 0.25f;
		float kpGain = 0.25f;
		if (Math.abs(dx) > kpTh)
			dx -= Math.copySign(kpTh * kpGain, dx);
		else
			dx *= kpGain;

		if (Math.abs(dy) > kpTh)
			dy -= Math.copySign(kpTh * kpGain, dy);
		else
			dy *= kpGain;
		context.makemotion_head_rel(dx, dy, 200);
		return true;
	}
	return false;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
private boolean trackGoal(VisualObject goal) {

		if (goal.confidence > GOALCONF_THRESHOLD) {
			Point2f angle = goal.angle;
			float dx = -angle.getX();
			float dy = -angle.getY();

			float kpTh = 0.25f;
			float kpGain = 0.4f;
			if (Math.abs(dx) > kpTh)
				dx -= Math.copySign(kpTh * kpGain, dx);
			else
				dx *= kpGain;

			if (Math.abs(dy) > kpTh)
				dy -= Math.copySign(kpTh * kpGain, dy);
			else
				dy *= kpGain;
			context.makemotion_head_rel(dx, dy, 200);
			return true;
		}
		return false;
	}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public void testAngle2carthesian() {
	float angleX = (float) Math.toRadians(45);
	float angleY = (float) Math.toRadians(45);
	float d = 100.0f;

	Vector3f v = new Vector3f();
	Point2f p = new Point2f(angleX, angleY);
	System.out.println(p);

	Coordinates.angle2carthesian(d, p, v);
	// assertEquals(d, v.length(), 0.001);

	System.out.println(v);
	Coordinates.carthesian2angle(v, p);
	// assertEquals(new Vector3f(d, angleX, angleY), v);
	System.out.println(p);

	Vector3f polar = new Vector3f();
	Coordinates.carthesian2polar(v, polar);
	System.out.println(polar);

}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
/** Default constructor to create a VO_WarpingPoint object */
public VO_WarpingPoint()
{
    this.m_CVPosition								= new Point2f(0.0f, 0.0f);
    this.m_iTriangleIndex							= 0;
    this.m_iPointIndex								= 0;
    this.m_VOTriangle2DStructure					= null;
    this.m_Gradients								= null;
    this.m_Jacobian_One								= null;
    this.m_JacobianMatrix4ShapeModel				= null;
    this.m_JacobianMatrix4GlobalShapeNorm			= null;
    this.m_SteepestDescentImages4ShapeModel			= null;
    this.m_SteepestDescentImages4GlobalShapeNorm	= null;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public Point2f GetLeftTop()
{
	Point2f res = new Point2f();
	res.x = MinX();
	res.y = MinY(); 
	return res;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public Point2f GetRightBottom()
{
	Point2f res = new Point2f();
	res.x = MaxX();
	res.y = MaxY();
	return res;
}
 

import javax.vecmath.Point2f; //导入依赖的package包/类
public Point2f GetLeftBottom()
{
	Point2f res = new Point2f();
	res.x = MinX();
	res.y = MaxY(); 
	return res;
}