Java中使用向量简单模拟人工势场虚拟力

public class APF {
    private List<RUAVs> ruAvsArrayList;
    private RUAVs ruaVs;
    private ArrayList<Integer> leader;
    double moveDistance = 20.0;//节点移动速度
    double influenceRadius = 200.0;//节点影响范围

    public APF(ArrayList<Integer> leader, List<RUAVs> ruAvsArrayList, RUAVs ruaVs) {
        this.leader = leader;
        this.ruAvsArrayList = ruAvsArrayList;
        this.ruaVs = ruaVs;
    }
    public Vector2D attractive() {
        int Ka = 10;//引力系数
//        double att = 0.5 * Ka * Math.pow(r,2);
//        return att;
        Vector2D att = new Vector2D(leader.get(0) - ruaVs.getPosition_index_x(),leader.get(1) - ruaVs.getPosition_index_y());
        att.normalize().multiply(Ka * 0.5 * Math.pow(att.getLength(),2));
        return att;
    }
    public Vector2D repulsion() {
        int Krep = 10;//斥力系数
        Vector2D rep = new Vector2D();
        for (int i = 0; i < ruAvsArrayList.size();i++) {
            Vector2D bijiao = new Vector2D(ruaVs.getPosition_index_x() - ruAvsArrayList.get(i).getPosition_index_x(),ruaVs.getPosition_index_y() - ruAvsArrayList.get(i).getPosition_index_y());
            if ((bijiao.getLength() <= influenceRadius) && (ruAvsArrayList.get(i) != ruaVs) && (ruAvsArrayList.get(i).getPosition_index_y() != ruaVs.getPosition_index_y())) {
                Vector2D temp = new Vector2D(ruaVs.getPosition_index_x() - ruAvsArrayList.get(i).getPosition_index_x(),ruaVs.getPosition_index_y() - ruAvsArrayList.get(i).getPosition_index_y());
                temp.normalize().multiply(Krep * (1 / temp.getLength() - 1 / influenceRadius) / Math.pow(temp.getLength(),2));
                rep = rep.add(temp);
            }
        }
        return rep;
    }
    //合力
    public Vector2D allForce() {
        Vector2D allForce = this.attractive().add(this.repulsion());
        return allForce;
    }
    //更新当前位置
    public void update() {
        Vector2D force = allForce();
//        ruaVs.setPosition_index_x((int)(ruaVs.getPosition_index_x() + moveDistance * force.getLength() * Math.sin(force.getAngle())));
//        ruaVs.setPosition_index_y((int)(ruaVs.getPosition_index_y() + moveDistance * force.getLength() * Math.cos(force.getAngle())));
        ruaVs.setPosition_index_x((int)(ruaVs.getPosition_index_x() + moveDistance * force.getX()));
        ruaVs.setPosition_index_y((int)(ruaVs.getPosition_index_y() + moveDistance * force.getY()));
    }
}

public class Vector2D {
	private double x;
	private double y;
	
	public Vector2D()
	{
		x = 0;
		y = 0;
	}
	
	public Vector2D(double _x, double _y)
	{
		x = _x;
		y = _y;
	}
	
	//获取弧度
	public double getRadian()
	{
		return Math.atan2(y, x);
	}
	
	//获取角度
	public double getAngle()
	{
		return getRadian() / Math.PI * 180;
	}
	
	public Vector2D clone()
	{
		return new Vector2D(x,y);
	}
	
	public double getLength()
	{
		return Math.sqrt(getLengthSQ());
	}
	
	public double getLengthSQ()
	{
		return x * x + y * y;
	}
	
	//向量置零
	public Vector2D Zero()
	{
		x = 0;
		y = 0;
		return this;
	}
	
	public boolean isZero()
	{
		return x == 0 && y == 0;
	}
	
	//向量的长度设置为我们期待的value
	public void setLength(double value) 
	{
		double _angle = getAngle();
		x = Math.cos(_angle) * value;
		y = Math.sin(_angle) * value;
	}
	
	//向量的标准化（方向不变，长度为1）
	public Vector2D normalize()
	{
		double length = getLength();
		x = x / length;
		y = y / length;
		return this;
	}
	//是否已经标准化
	public boolean isNormalized()
	{
		return getLength() == 1.0;
	}
	
	//向量的方向翻转
	public Vector2D reverse()
	{
		x = -x;
		y = -y;
		return this;
	}
	
	//2个向量的数量积(点积)
	public double dotProduct(Vector2D v)
	{
		return x * v.x + y * v.y;
	}
	
	//2个向量的向量积(叉积)
	public double crossProduct(Vector2D v)
	{
		return x * v.y - y * v.x;
	}

	//计算2个向量的夹角弧度
	//参考点积公式:v1 * v2 = cos<v1,v2> * |v1| *|v2|
	public static double radianBetween(Vector2D v1, Vector2D v2)
	{
		if(!v1.isNormalized()) v1 = v1.clone().normalize(); // |v1| = 1
		if(!v2.isNormalized()) v2 = v2.clone().normalize(); // |v2| = 1
		return Math.acos(v1.dotProduct(v2)); 
	}
	
	//弧度 = 角度乘以PI后再除以180、 推理可得弧度换算角度的公式
	//弧度转角度
	public static double radian2Angle(double radian)
	{
		return radian / Math.PI * 180;
	}
	//向量加
	public Vector2D add(Vector2D v)
	{
		return new Vector2D(x + v.x, y + v.y);
	}
	//向量减
	public Vector2D subtract(Vector2D v)
	{
		return new Vector2D(x - v.x, y - v.y);
	}
	//向量乘
	public Vector2D multiply(double value)
	{
		return new Vector2D(x * value, y * value);
	}
	//向量除
	public Vector2D divide(double value)
	{
		return new Vector2D(x / value, y / value);
	}

	public double getX() {
		return x;
	}
	public double getY() {
		return y;
	}
}