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

Introduction

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

Prototype

public double distanceSq(Vector<Euclidean2D> p) 

Source Link

Usage

From source file:edu.unc.cs.gamma.rvo.KdTree.java

/**
 * Recursive method for querying the visibility between two points within a
 * specified radius./*w ww .j a  v a  2s .  c  om*/
 *
 * @param q1     The first point between which visibility is to be tested.
 * @param q2     The second point between which visibility is to be tested.
 * @param radius The radius within which visibility is to be tested.
 * @param node   The current obstacle k-D node.
 * @return True if q1 and q2 are mutually visible within the radius; false
 * otherwise.
 */
private static boolean queryVisibilityRecursive(Vector2D q1, Vector2D q2, double radius,
        ObstacleTreeNode node) {
    if (node == null) {
        return true;
    }

    final Obstacle obstacle1 = node.obstacle;
    final Obstacle obstacle2 = obstacle1.next;

    final double q1LeftOfI = RVOMath.leftOf(obstacle1.point, obstacle2.point, q1);
    final double q2LeftOfI = RVOMath.leftOf(obstacle1.point, obstacle2.point, q2);
    final double invLengthI = 1.0 / obstacle2.point.distanceSq(obstacle1.point);

    if (q1LeftOfI >= 0.0 && q2LeftOfI >= 0.0) {
        return queryVisibilityRecursive(q1, q2, radius, node.left)
                && (q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius
                        && q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius
                        || queryVisibilityRecursive(q1, q2, radius, node.right));
    }

    if (q1LeftOfI <= 0.0 && q2LeftOfI <= 0.0) {
        return queryVisibilityRecursive(q1, q2, radius, node.right)
                && (q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius
                        && q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius
                        || queryVisibilityRecursive(q1, q2, radius, node.left));
    }

    if (q1LeftOfI >= 0.0 && q2LeftOfI <= 0.0) {
        // One can see through obstacle from left to right.
        return queryVisibilityRecursive(q1, q2, radius, node.left)
                && queryVisibilityRecursive(q1, q2, radius, node.right);
    }

    final double point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point);
    final double point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point);
    final double invLengthQ = 1.0 / q2.distanceSq(q1);

    return point1LeftOfQ * point2LeftOfQ >= 0.0 && point1LeftOfQ * point1LeftOfQ * invLengthQ > radius * radius
            && point2LeftOfQ * point2LeftOfQ * invLengthQ > radius * radius
            && queryVisibilityRecursive(q1, q2, radius, node.left)
            && queryVisibilityRecursive(q1, q2, radius, node.right);
}

---