/** Returns the square distance of the nearest point on line segment {@code a-b}, from point {@code c}. Also, the {@code out} * vector is assigned to the nearest point. * @param out the output vector that contains the nearest point on return * @param a the start point of the line segment * @param b the end point of the line segment * @param c the point to calculate the distance from */ public float calculatePointSegmentSquareDistance (T out, T a, T b, T c) { out.set(a); tmpB.set(b); tmpC.set(c); T ab = tmpB.sub(a); float abLen2 = ab.len2(); if (abLen2 != 0) { float t = (tmpC.sub(a)).dot(ab) / abLen2; out.mulAdd(ab, MathUtils.clamp(t, 0, 1)); } return out.dst2(c); }
@Override public boolean reportNeighbor (Steerable<T> neighbor) { // Calculate the time to collision relativePosition.set(neighbor.getPosition()).sub(owner.getPosition()); relativeVelocity.set(neighbor.getLinearVelocity()).sub(owner.getLinearVelocity()); float relativeSpeed2 = relativeVelocity.len2(); // Collision can't happen when the agents have the same linear velocity. // Also, note that timeToCollision would be NaN due to the indeterminate form 0/0 and, // since any comparison involving NaN returns false, it would become the shortestTime, // so defeating the algorithm. if (relativeSpeed2 == 0) return false; float timeToCollision = -relativePosition.dot(relativeVelocity) / relativeSpeed2; // If timeToCollision is negative, i.e. the owner is already moving away from the the neighbor, // or it's not the most imminent collision then no action needs to be taken. if (timeToCollision <= 0 || timeToCollision >= shortestTime) return false; // Check if it is going to be a collision at all float distance = relativePosition.len(); float minSeparation = distance - (float)Math.sqrt(relativeSpeed2) * timeToCollision /* shortestTime */; if (minSeparation > owner.getBoundingRadius() + neighbor.getBoundingRadius()) return false; // Store most imminent collision data shortestTime = timeToCollision; firstNeighbor = neighbor; firstMinSeparation = minSeparation; firstDistance = distance; firstRelativePosition.set(relativePosition); firstRelativeVelocity.set(relativeVelocity); return true; }
if (ownerOrientation.dot(toAgent) > coneThreshold) { if (callback.reportNeighbor(currentAgent)) { currentAgent.setTagged(true);
/** Returns the square distance of the nearest point on line segment {@code a-b}, from point {@code c}. Also, the {@code out} * vector is assigned to the nearest point. * @param out the output vector that contains the nearest point on return * @param a the start point of the line segment * @param b the end point of the line segment * @param c the point to calculate the distance from */ public float calculatePointSegmentSquareDistance (T out, T a, T b, T c) { out.set(a); tmpB.set(b); tmpC.set(c); T ab = tmpB.sub(a); float abLen2 = ab.len2(); if (abLen2 != 0) { float t = (tmpC.sub(a)).dot(ab) / abLen2; out.mulAdd(ab, MathUtils.clamp(t, 0, 1)); } return out.dst2(c); }
@Override public boolean reportNeighbor (Steerable<T> neighbor) { // Calculate the time to collision relativePosition.set(neighbor.getPosition()).sub(owner.getPosition()); relativeVelocity.set(neighbor.getLinearVelocity()).sub(owner.getLinearVelocity()); float relativeSpeed2 = relativeVelocity.len2(); // Collision can't happen when the agents have the same linear velocity. // Also, note that timeToCollision would be NaN due to the indeterminate form 0/0 and, // since any comparison involving NaN returns false, it would become the shortestTime, // so defeating the algorithm. if (relativeSpeed2 == 0) return false; float timeToCollision = -relativePosition.dot(relativeVelocity) / relativeSpeed2; // If timeToCollision is negative, i.e. the owner is already moving away from the the neighbor, // or it's not the most imminent collision then no action needs to be taken. if (timeToCollision <= 0 || timeToCollision >= shortestTime) return false; // Check if it is going to be a collision at all float distance = relativePosition.len(); float minSeparation = distance - (float)Math.sqrt(relativeSpeed2) * timeToCollision /* shortestTime */; if (minSeparation > owner.getBoundingRadius() + neighbor.getBoundingRadius()) return false; // Store most imminent collision data shortestTime = timeToCollision; firstNeighbor = neighbor; firstMinSeparation = minSeparation; firstDistance = distance; firstRelativePosition.set(relativePosition); firstRelativeVelocity.set(relativeVelocity); return true; }
if (ownerOrientation.dot(toAgent) > coneThreshold) { if (callback.reportNeighbor(currentAgent)) { currentAgent.setTagged(true);