@Override
public Point2D calculatePosition(Point2D pos, double speed) {
if (isBackWard) {}
PathIterator p = fCurve.getPathIterator(null);
FlatteningPathIterator f = new FlatteningPathIterator(p, reduceCarSpeed(speed));
while (!f.isDone()) {
float[] pts = new float[6];
switch (f.currentSegment(pts)) {
case PathIterator.SEG_MOVETO:
case PathIterator.SEG_LINETO:
Point2D point = new Point2D.Float(pts[0], pts[1]);
if (point.distance(pos) == 0) {
f.next();
if (!f.isDone()) {
pts = new float[6];
f.currentSegment(pts);
return new Point2D.Float(pts[0], pts[1]);
} else {
return new Point2D.Double(-1, -1);
}
}
}
f.next();
}
return new Point2D.Double(-1, -1);
}
/**
* determines whether the car is still on this lane
*
* @param fCar car to test with
* @param lane
* @return
*/
public boolean carOnLane(Car fCar, Lane lane) {
PathIterator p = fCurve.getPathIterator(null);
FlatteningPathIterator f = new FlatteningPathIterator(p, reduceCarSpeed(fCar.getSpeed()));
while (!f.isDone()) {
float[] pts = new float[6];
switch (f.currentSegment(pts)) {
case PathIterator.SEG_MOVETO:
case PathIterator.SEG_LINETO:
Point2D point = new Point2D.Float(pts[0], pts[1]);
if (point.distance(fCar.getPosition()) <= 1) {
return true;
}
}
f.next();
}
return false;
}
@Override
public double getLength() {
if (fLength == 0) {
PathIterator iter = fCurve.getPathIterator(null, 0.5);
double[] curSeg = new double[2];
iter.currentSegment(curSeg);
iter.next();
double x0 = curSeg[0];
double y0 = curSeg[1];
while (!iter.isDone()) {
iter.currentSegment(curSeg);
fLength +=
Math.sqrt((curSeg[0] - x0) * (curSeg[0] - x0) + (curSeg[1] - y0) * (curSeg[1] - y0));
x0 = curSeg[0];
y0 = curSeg[1];
iter.next();
}
}
return fLength;
}
@Override
public void relocate(Car fCar) {
PathIterator p = fCurve.getPathIterator(null);
FlatteningPathIterator f = new FlatteningPathIterator(p, reduceCarSpeed(fCar.getSpeed()));
Point2D nearestPoint = null;
double distance = 1000;
while (!f.isDone()) {
float[] pts = new float[6];
switch (f.currentSegment(pts)) {
case PathIterator.SEG_MOVETO:
case PathIterator.SEG_LINETO:
Point2D point = new Point2D.Float(pts[0], pts[1]);
double distance1 = point.distance(fCar.getPosition());
if (distance1 < distance) {
nearestPoint = point;
distance = distance1;
}
}
f.next();
}
if (nearestPoint != null) {
fCar.setPosition(nearestPoint);
}
}