/** @see java.lang.Object#toString() */
@Override
public String toString() {
return String.format(
"<%s %.2f %s %s %.4f %.4f %.4f>",
originPos.toString(),
originDir,
target.toString(),
targetLocal.toString(),
targetDist,
targetTurnAngle,
costToDest);
}
/**
* Create a new waypoint.
*
* @param originPos Starting robot coordinates in global coordinates.
* @param originDir Starting robot direction in degrees.
* @param target Target point in global coordinates.
* @param costToDest Cost of the path from here to the final destination.
* @param isEndpoint Whether the target is at the end of the path.
*/
public Waypoint(
Vector2D originPos,
double originDir,
Vector2D target,
double costToDest,
boolean isEndpoint) {
this.originPos = originPos;
this.originDir = originDir;
this.target = target;
this.costToDest = costToDest;
this.isEndpoint = isEndpoint;
targetLocal = Vector2D.subtract(target, originPos);
targetLocal = Vector2D.rotateVector(targetLocal, -originDir);
targetDist = targetLocal.getLength();
targetTurnAngle = targetLocal.getDirection();
}