com.badlogic.gdx.ai.steer.utils.paths.LinePath Maven / Gradle / Ivy
/*******************************************************************************
* Copyright 2014 See AUTHORS file.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
******************************************************************************/
package com.badlogic.gdx.ai.steer.utils.paths;
import com.badlogic.gdx.ai.steer.utils.Path;
import com.badlogic.gdx.ai.steer.utils.paths.LinePath.LinePathParam;
import com.badlogic.gdx.math.MathUtils;
import com.badlogic.gdx.math.Vector;
import com.badlogic.gdx.utils.Array;
/** A {@code LinePath} is a path for path following behaviors that is made up of a series of waypoints. Each waypoint is connected
* to the successor with a {@link Segment}.
*
* @param Type of vector, either 2D or 3D, implementing the {@link Vector} interface
*
* @author davebaol
* @author Daniel Holderbaum */
public class LinePath> implements Path {
private Array> segments;
private boolean isOpen;
private float pathLength;
private T nearestPointOnCurrentSegment;
private T nearestPointOnPath;
private T tmpB;
private T tmpC;
/** Creates a closed {@code LinePath} for the specified {@code waypoints}.
* @param waypoints the points making up the path
* @throws IllegalArgumentException if {@code waypoints} is {@code null} or has less than two (2) waypoints. */
public LinePath (Array waypoints) {
this(waypoints, false);
}
/** Creates a {@code LinePath} for the specified {@code waypoints}.
* @param waypoints the points making up the path
* @param isOpen a flag indicating whether the path is open or not
* @throws IllegalArgumentException if {@code waypoints} is {@code null} or has less than two (2) waypoints. */
public LinePath (Array waypoints, boolean isOpen) {
this.isOpen = isOpen;
createPath(waypoints);
nearestPointOnCurrentSegment = waypoints.first().cpy();
nearestPointOnPath = waypoints.first().cpy();
tmpB = waypoints.first().cpy();
tmpC = waypoints.first().cpy();
}
@Override
public boolean isOpen () {
return isOpen;
}
@Override
public float getLength () {
return pathLength;
}
@Override
public T getStartPoint () {
return segments.first().begin;
}
@Override
public T getEndPoint () {
return segments.peek().end;
}
/** 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 LinePathParam createParam () {
return new LinePathParam();
}
// We pass the last parameter value to the path in order to calculate the current
// parameter value. This is essential to avoid nasty problems when lines are close together.
// We should limit the algorithm to only considering areas of the path close to the previous
// parameter value. The character is unlikely to have moved far, after all.
// This technique, assuming the new value is close to the old one, is called coherence, and it is a
// feature of many geometric algorithms.
// TODO: Currently coherence is not implemented.
@Override
public float calculateDistance (T agentCurrPos, LinePathParam parameter) {
// Find the nearest segment
float smallestDistance2 = Float.POSITIVE_INFINITY;
Segment nearestSegment = null;
for (int i = 0; i < segments.size; i++) {
Segment segment = segments.get(i);
float distance2 = calculatePointSegmentSquareDistance(nearestPointOnCurrentSegment, segment.begin, segment.end,
agentCurrPos);
// first point
if (distance2 < smallestDistance2) {
nearestPointOnPath.set(nearestPointOnCurrentSegment);
smallestDistance2 = distance2;
nearestSegment = segment;
parameter.segmentIndex = i;
}
}
// Distance from path start
float lengthOnPath = nearestSegment.cumulativeLength - nearestPointOnPath.dst(nearestSegment.end);
parameter.setDistance(lengthOnPath);
return lengthOnPath;
}
@Override
public void calculateTargetPosition (T out, LinePathParam param, float targetDistance) {
if (isOpen) {
// Open path support
if (targetDistance < 0) {
// Clamp target distance to the min
targetDistance = 0;
} else if (targetDistance > pathLength) {
// Clamp target distance to the max
targetDistance = pathLength;
}
} else {
// Closed path support
if (targetDistance < 0) {
// Backwards
targetDistance = pathLength + (targetDistance % pathLength);
} else if (targetDistance > pathLength) {
// Forward
targetDistance = targetDistance % pathLength;
}
}
// Walk through lines to see on which line we are
Segment desiredSegment = null;
for (int i = 0; i < segments.size; i++) {
Segment segment = segments.get(i);
if (segment.cumulativeLength >= targetDistance) {
desiredSegment = segment;
break;
}
}
// begin-------targetPos-------end
float distance = desiredSegment.cumulativeLength - targetDistance;
out.set(desiredSegment.begin).sub(desiredSegment.end).scl(distance / desiredSegment.length).add(desiredSegment.end);
}
/** Sets up this {@link Path} using the given way points.
* @param waypoints The way points of this path.
* @throws IllegalArgumentException if {@code waypoints} is {@code null} or empty. */
public void createPath (Array waypoints) {
if (waypoints == null || waypoints.size < 2)
throw new IllegalArgumentException("waypoints cannot be null and must contain at least two (2) waypoints");
segments = new Array>(waypoints.size);
pathLength = 0;
T curr = waypoints.first();
T prev = null;
for (int i = 1; i <= waypoints.size; i++) {
prev = curr;
if (i < waypoints.size)
curr = waypoints.get(i);
else if (isOpen)
break; // keep the path open
else
curr = waypoints.first(); // close the path
Segment segment = new Segment(prev, curr);
pathLength += segment.length;
segment.cumulativeLength = pathLength;
segments.add(segment);
}
}
public Array> getSegments () {
return segments;
}
/** A {@code LinePathParam} contains the status of a {@link LinePath}.
*
* @author davebaol */
public static class LinePathParam implements Path.PathParam {
int segmentIndex;
float distance;
@Override
public float getDistance () {
return distance;
}
@Override
public void setDistance (float distance) {
this.distance = distance;
}
/** Returns the index of the current segment along the path */
public int getSegmentIndex () {
return segmentIndex;
}
}
/** A {@code Segment} connects two consecutive waypoints of a {@link LinePath}.
*
* @param Type of vector, either 2D or 3D, implementing the {@link Vector} interface
*
* @author davebaol */
public static class Segment> {
T begin;
T end;
float length;
float cumulativeLength;
/** Creates a {@code Segment} for the 2 given points.
* @param begin
* @param end */
Segment (T begin, T end) {
this.begin = begin;
this.end = end;
this.length = begin.dst(end);
}
/** Returns the start point of this segment. */
public T getBegin () {
return begin;
}
/** Returns the end point of this segment. */
public T getEnd () {
return end;
}
/** Returns the length of this segment. */
public float getLength () {
return length;
}
/** Returns the cumulative length from the first waypoint of the {@link LinePath} this segment belongs to. */
public float getCumulativeLength () {
return cumulativeLength;
}
}
}
© 2015 - 2024 Weber Informatics LLC | Privacy Policy