Example usage for org.apache.commons.math3.geometry.euclidean.twod Vector2D distance

Introduction

In this page you can find the example usage for org.apache.commons.math3.geometry.euclidean.twod Vector2D distance.

Prototype

public static double distance(Vector2D p1, Vector2D p2) 

Source Link

Document

Compute the distance between two vectors according to the L₂ norm.

Usage

From source file:edu.snu.leader.discrete.simulator.GautraisConflictDecisionCalculator.java

private double calculateConflict(Decision decision) {
    Agent agent = decision.getAgent();//from   w w w .j  a  va2s.  co  m
    Agent leader = decision.getLeader();
    double Ci = 0.1;

    // calculate the leader's next location
    Vector2D leaderNextLocation = leader.getCurrentDestination().add(leader.getCurrentVelocity());
    // calculate the sides of a triangle
    // calculate side from agent's preferred destination to leader's next
    double A = Vector2D.distance(agent.getPreferredDestination().getVector(), leaderNextLocation);
    // calculate side from agent's preferred destination to leader's current
    double B = Vector2D.distance(agent.getPreferredDestination().getVector(), leader.getCurrentLocation());
    // calculate side from leader's current to leader's next
    double C = Vector2D.distance(leader.getCurrentLocation(), leaderNextLocation);

    // check if the leader is in the agent's preferred destination
    if (leader.getCurrentLocation()
            .distance1(agent.getPreferredDestination().getVector()) < _destinationSizeRadius) {
        Ci = 0.1;
    }
    // check if the leader is not moving
    else if (leader.getCurrentVelocity().equals(Vector2D.ZERO)) {
        Ci = .9;
    } else {
        double angle = 0.0;

        if (A <= 0 || B <= 0 || C <= 0) {
            // if a side is 0 then there is no triangle it is a line
            // if segment B is longer than C then the degree should be 180
            if (B > C) {
                angle = 180;
            }
            // if the segment B is shorter than C then the degree should be
            // 0
            else {
                angle = 0.0;
            }
        }
        // have three sides so use law of cosines
        else {
            // calculate angle between leader's current position and agent's
            // preferred destination by law of cosines
            double lawOfCosines = (Math.pow(A, 2) - Math.pow(B, 2) - Math.pow(C, 2)) / (-2 * B * C);
            // because of rounding error there can be lawOfCosines values
            // that are oh so slightly larger or smaller than 1 or -1
            // this augments them to their correct values
            if (lawOfCosines < -1) {
                lawOfCosines = -1;
            } else if (lawOfCosines > 1) {
                lawOfCosines = 1;
            }
            angle = Math.acos(lawOfCosines);
        }

        // if angle is greater than 180 than it becomes 360 - angle
        if (angle > 180) {
            angle = 360 - angle;
        }
        // make it into degrees
        angle = angle * 180 / Math.PI;
        // calculate conflict
        Ci = angle / 180;
    }

    // prevent K value from becoming 0
    if (Ci < .1) {
        Ci = .1;
    } else if (Ci > .9) {
        Ci = .9;
    }

    // set the conflict for the decision
    decision.setConflict(Ci);
    // return the conflict value for whatever needs to use it
    return Ci;
}

---