@Override
public void removeElement(Editable p) {
super.removeElement(p);
TrackSegment seg = (TrackSegment) p;
seg.setWrapper(null);
}
@FireExtended
public void mergeAllSegments() {
final Collection<Editable> segs = getData();
TrackSegment first = null;
for (final Iterator<Editable> iterator = segs.iterator(); iterator.hasNext(); ) {
final TrackSegment segment = (TrackSegment) iterator.next();
if (first == null) {
// aaah, now, if this is a TMA segment we've got to replace it with
// a normal track segment. You can't join new track sections onto the
// end
// of a tma segment
if (segment instanceof CoreTMASegment) {
final CoreTMASegment tma = (CoreTMASegment) segment;
first = new TrackSegment(tma);
} else {
// cool, just go ahead
first = segment;
}
} else {
first.append((Layer) segment);
}
}
// ditch the segments
this.removeAllElements();
// and put the first one back in
this.addSegment(first);
// and fire some kind of update...
}
/** utility method to reveal all positions in a track */
@FireReformatted
public void revealAllPositions() {
final Enumeration<Editable> theEnum = elements();
while (theEnum.hasMoreElements()) {
final TrackSegment seg = (TrackSegment) theEnum.nextElement();
final Enumeration<Editable> ele = seg.elements();
while (ele.hasMoreElements()) {
final Editable editable = ele.nextElement();
final FixWrapper fix = (FixWrapper) editable;
fix.setVisible(true);
}
}
}
@Override
public void setWrapper(final TrackWrapper wrapper) {
// is it different?
if (wrapper == _myTrack) return;
// store the value
super.setWrapper(wrapper);
// update our segments
final Collection<Editable> items = getData();
for (final Iterator<Editable> iterator = items.iterator(); iterator.hasNext(); ) {
final TrackSegment seg = (TrackSegment) iterator.next();
seg.setWrapper(_myTrack);
}
}
public void addSegment(final TrackSegment segment) {
segment.setWrapper(_myTrack);
if (this.size() == 1) {
// aah, currently, it's name's probably wrong sort out it's date
final TrackSegment first = (TrackSegment) getData().iterator().next();
first.sortOutDate(null);
}
super.add(segment);
// if we've just got the one, set it's name to positions
if (this.size() == 1) {
final TrackSegment first = (TrackSegment) getData().iterator().next();
first.setName("Positions");
}
}
// Counter to how many TrackPoint has the Track
public int countTrackPoints() {
// Object to count the total
int trackPointTotal = 0;
// Object to count the total of TrackPoints from each TrackSegment
int segmentTrackPoints = 0;
// Creating a loop for each TrackSegment in the Track
for (TrackSegment trackSegment : this.getSegments()) {
// Getting the total by using the function size
segmentTrackPoints = trackSegment.getTrackPoints().size();
// Summing the size
trackPointTotal = trackPointTotal + segmentTrackPoints;
}
return trackPointTotal;
}
private boolean withinRightCorner() {
return (segment != null && !segment.isUnknown() && segment.isRight()) || situation.isRight();
}
private PlanElement2011 plan(
PlanStackData planData,
SensorData data,
TrackModel trackModel,
OpponentObserver observer,
boolean planning) {
int index = planData.currentSegment();
TrackSegment current = trackModel.getSegment(index);
TrackSegment next = trackModel.getSegment(trackModel.incrementIndex(index));
TrackSegment prev = trackModel.getSegment(trackModel.decrementIndex(index));
double end = planData.end();
double start = planData.start();
if (Plan.TEXT_DEBUG && planning) {
plan.println("");
plan.println("------------ PlanStraight------------");
plan.println(
"Start/End: "
+ Utils.dTS(start)
+ ", "
+ Utils.dTS(end)
+ " ["
+ Utils.dTS(end - start)
+ "m]"
+ " - ");
planData.print();
plan.println("");
}
// plan like the last element?
boolean planLikeLast = current.contains(data.getDistanceFromStartLine());
if (planLikeLast) {
start = data.getDistanceRaced();
if (Plan.TEXT_DEBUG && planning) {
plan.println("This is the last segment to plan for...");
}
}
// check if we can combine the planning of this segment and the previous
if (!planLikeLast && prev.isStraight()) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Previous is also straight, checking further...");
}
if (start - prev.getLength() <= data.getDistanceRaced()) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Prev is the last segment to plan for");
}
start = data.getDistanceRaced();
planLikeLast = true;
} else {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Prev is also a middle segment");
}
start -= prev.getLength();
int prevIndex = trackModel.decrementIndex(index);
int prevPrevIndex = trackModel.decrementIndex(prevIndex);
if (Plan.TEXT_DEBUG && planning) {
plan.println("Switching prev from " + prevIndex + " to " + prevPrevIndex);
}
prev = trackModel.getSegment(prevPrevIndex);
}
if (Plan.TEXT_DEBUG && planning) {
plan.println(
"Moving start to " + Utils.dTS(start) + ", new length " + Utils.dTS(end - start) + "m");
}
planData.popSegment();
}
double brakeDistance = 0.0;
if (planData.approachSpeed != Plan2013.MAX_SPEED) {
if (planData.first()) {
brakeDistance =
plan.calcBrakingZoneStraight(data.getSpeed(), end - start, planData.approachSpeed);
} else {
brakeDistance =
plan.calcBrakingZoneStraight(planData.speed(), end - start, planData.approachSpeed);
}
}
if (Plan.TEXT_DEBUG && planning) {
plan.println("Brake distance: " + Utils.dTS(brakeDistance) + "m");
}
Point2D targetPosition = OpponentObserver.NO_RECOMMENDED_POINT;
if (observer.otherCars()
&& observer.getRecommendedPosition() != OpponentObserver.NO_RECOMMENDED_POINT) {
Point2D recommendation = new Point2D.Double();
recommendation.setLocation(observer.getRecommendedPosition());
if (start <= recommendation.getY() && recommendation.getY() < end) {
targetPosition = new Point2D.Double();
targetPosition.setLocation(recommendation);
}
}
// track positions
ArrayList<Interpolator> positions = new ArrayList<>();
double currStart = start;
double remainingLength = end - start;
double currPosition = plan.getAnchorPoint(prev, current);
if (planLikeLast) {
currPosition = data.getTrackPosition();
}
if (Plan.TEXT_DEBUG && planning) {
plan.println(Utils.dTS(remainingLength) + "m remain...");
}
if (targetPosition != OpponentObserver.NO_RECOMMENDED_POINT) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("I need to take care of other cars, point is " + targetPosition.toString());
plan.println("Right now, i'm planning with currStart: " + Utils.dTS(currStart));
}
if (targetPosition.getY() < currStart) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Point is before currStart, moving it to +1m");
}
targetPosition.setLocation(targetPosition.getX(), targetPosition.getY() + 1.0);
}
if (Plan.TEXT_DEBUG && planning) {
plan.println("Checking, if there is enough room...");
}
double lengthNeeded = targetPosition.getY() - currStart;
if (Plan.TEXT_DEBUG && planning) {
plan.println(
"I need at least "
+ Utils.dTS(lengthNeeded)
+ "m, "
+ Utils.dTS(remainingLength)
+ "m remain");
}
if (remainingLength < lengthNeeded) {
targetPosition = OpponentObserver.NO_RECOMMENDED_POINT;
if (Plan.TEXT_DEBUG && planning) {
plan.println("Cannot overtake, not enough room");
}
}
}
if (targetPosition != OpponentObserver.NO_RECOMMENDED_POINT) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Trying to overtake");
}
// switch Position 1
double[] xP = new double[3];
double[] yP = new double[3];
xP[0] = currStart;
xP[2] = targetPosition.getY();
xP[1] = (xP[0] + xP[2]) / 2.0;
yP[0] = currPosition;
yP[2] = targetPosition.getX();
yP[1] = (yP[0] + yP[2]) / 2.0;
if (Plan.TEXT_DEBUG && planning) {
plan.println("Switch Position 1:");
for (int k = 0; k < xP.length; ++k) {
plan.println(xP[k] + " , " + yP[k]);
}
}
CubicSpline spline = new CubicSpline(xP, yP);
spline.setDerivLimits(0.0, 0.0);
positions.add(new FlanaganCubicWrapper(spline));
currStart = xP[2];
remainingLength = end - currStart;
currPosition = targetPosition.getX();
// overtaking
if (Plan.TEXT_DEBUG && planning) {
plan.println("Overtaking line");
}
xP = new double[3];
xP[0] = currStart;
xP[2] = end - 150.0;
if (Plan.TEXT_DEBUG && planning) {
plan.println(
"Overtaking line: "
+ Utils.dTS(currStart)
+ " "
+ Utils.dTS(end - 150.0)
+ " "
+ targetPosition.getX());
}
positions.add(new ConstantValue(xP[0], xP[2], targetPosition.getX()));
currStart = xP[2];
remainingLength = end - currStart;
currPosition = targetPosition.getX();
}
// simply drive towards the target position for the next corner
if (Plan.TEXT_DEBUG && planning) {
plan.println("Planning towards the next corner...");
}
double[] xP = new double[3];
double[] yP = new double[3];
xP[0] = currStart;
xP[2] = end;
xP[1] = (xP[0] + xP[2]) / 2.0;
yP[0] = currPosition;
double absCurrPosition =
SensorData.calcAbsoluteTrackPosition(currPosition, trackModel.getWidth());
double possibleDelta = Plan2013.calcPossibleSwitchDelta(data, end - currStart);
double anchor = plan.getAnchorPoint(current, next);
double absDesiredPosition = SensorData.calcAbsoluteTrackPosition(anchor, trackModel.getWidth());
if (Plan.TEXT_DEBUG && planning) {
plan.println("absCurrPosition: " + Utils.dTS(absCurrPosition));
plan.println("PossibleDelta: " + Utils.dTS(possibleDelta));
plan.println("absDesiredPosition: " + Utils.dTS(absDesiredPosition));
}
if (Math.abs(absDesiredPosition - absCurrPosition) <= possibleDelta) {
if (Plan.TEXT_DEBUG && planning) {
plan.println("Positioning ok");
}
yP[2] = plan.getAnchorPoint(current, next);
} else {
// move to the right
if (anchor < 0) {
yP[2] =
SensorData.calcRelativeTrackPosition(
absCurrPosition + possibleDelta, trackModel.getWidth());
} else {
// move to the left
yP[2] =
SensorData.calcRelativeTrackPosition(
absCurrPosition - possibleDelta, trackModel.getWidth());
}
}
yP[1] = (yP[0] + yP[2]) / 2.0;
if (Plan.TEXT_DEBUG && planning) {
plan.println("Position towards the next corner:");
for (int k = 0; k < xP.length; ++k) {
plan.println(xP[k] + " , " + yP[k]);
}
}
CubicSpline spline = new CubicSpline(xP, yP);
spline.setDerivLimits(0.0, 0.0);
positions.add(new FlanaganCubicWrapper(spline));
// target speed
double[] xS;
double[] yS;
if (brakeDistance >= end - start || brakeDistance == 0.0) {
xS = new double[3];
yS = new double[3];
xS[0] = start;
xS[2] = end;
xS[1] = (xS[0] + xS[2]) / 2.0;
if (brakeDistance == 0.0) {
yS[0] = Plan2013.MAX_SPEED;
yS[1] = Plan2013.MAX_SPEED;
yS[2] = Plan2013.MAX_SPEED;
planData.approachSpeed = Plan2013.MAX_SPEED;
} else {
yS[0] = planData.approachSpeed;
yS[1] = planData.approachSpeed;
yS[2] = planData.approachSpeed;
planData.approachSpeed =
plan.calcApproachSpeedStraight(planData.approachSpeed, end - start);
}
} else {
xS = new double[4];
yS = new double[4];
xS[0] = start;
xS[1] = end - brakeDistance;
xS[2] = end - (brakeDistance * 0.99);
xS[3] = end;
yS[0] = Plan2013.MAX_SPEED;
yS[1] = Plan2013.MAX_SPEED;
yS[2] = planData.approachSpeed;
yS[3] = planData.approachSpeed;
planData.approachSpeed = Plan2013.MAX_SPEED;
}
if (Plan.TEXT_DEBUG && planning) {
plan.println("Speed:");
for (int i = 0; i < xS.length; ++i) {
plan.println(xS[i] + " , " + yS[i]);
}
}
LinearInterpolator speed;
try {
speed = new LinearInterpolator(xS, yS);
} catch (RuntimeException e) {
System.out.println("*****************EXCEPTION**************");
System.out.println(
"Start/End: "
+ Utils.dTS(start)
+ ", "
+ Utils.dTS(end)
+ " ["
+ Utils.dTS(end - start)
+ "m]"
+ " - ");
System.out.println("Segment: " + current.toString());
System.out.println("Start: " + start);
System.out.println("End: " + end);
System.out.println("BrakeDistance: " + brakeDistance);
System.out.println("Data:");
try {
java.io.OutputStreamWriter osw = new java.io.OutputStreamWriter(System.out);
SensorData.writeHeader(osw);
osw.append('\n');
data.write(osw);
osw.flush();
} catch (Exception schwupp) {
}
System.out.println("");
System.out.println("Speed:");
for (int i = 0; i < xS.length; ++i) {
System.out.println(xS[i] + " , " + yS[i]);
}
System.out.println("Complete Model:");
trackModel.print();
throw e;
}
PlanElement2011 element = new PlanElement2011(xS[0], xS[xS.length - 1], "Accelerate");
for (Interpolator cs : positions) {
element.attachPosition(cs);
}
element.setSpeed(speed);
return element;
}
protected Term assembleTrackSegment(
long frameNumber1,
long x1,
long y1,
long frameNumber2,
long x2,
long y2,
TrackSegment trackSegment) {
Term prologFrame1 = new PrologInteger(frameNumber1);
Term prologX1 = new PrologInteger(x1);
Term prologY1 = new PrologInteger(y1);
Term prologFrame2 = new PrologInteger(frameNumber2);
Term prologX2 = new PrologInteger(x2);
Term prologY2 = new PrologInteger(y2);
Term list3 = formTrackOfBlob(trackSegment);
Term prologMeanBlobArea = new PrologReal(trackSegment.meanBlobArea);
Term prologMeanForegroundArea = new PrologReal(trackSegment.meanForegroundArea);
Term prologMeanCharacteristicLength = new PrologReal(trackSegment.meanCharacteristicLength);
Term prologMeanSquaredCharacteristicLength =
new PrologReal(trackSegment.meanSquaredCharacteristicLength);
Term prologMeanStandardizedArea = new PrologReal(trackSegment.meanStandardizedArea);
Term prologMeanContourLength = new PrologReal(trackSegment.meanContourLength);
Term prologWR2Mean = new PrologReal(trackSegment.getWindowedR2Mean());
Term prologWR2StandardDeviation = new PrologReal(trackSegment.getWindowedR2StandardDeviation());
Term prologWR2Skewness = new PrologReal(trackSegment.getWindowedR2Skewness());
Term prologWR2Kurtosis = new PrologReal(trackSegment.getWindowedR2Kurtosis());
Term prologWR2Cardinality = new PrologInteger(trackSegment.getWindowedR2Cardinality());
Term prologMeanVelocity = new PrologReal(trackSegment.meanVelocity);
Term prologTrackSegment =
new PrologSet(
-SymbolCodes.symbolCode_E_frame1,
prologFrame1,
new PrologSet(
-SymbolCodes.symbolCode_E_x1,
prologX1,
new PrologSet(
-SymbolCodes.symbolCode_E_y1,
prologY1,
new PrologSet(
-SymbolCodes.symbolCode_E_frame2,
prologFrame2,
new PrologSet(
-SymbolCodes.symbolCode_E_x2,
prologX2,
new PrologSet(
-SymbolCodes.symbolCode_E_y2,
prologY2,
new PrologSet(
-SymbolCodes.symbolCode_E_coordinates,
list3,
new PrologSet(
-SymbolCodes.symbolCode_E_mean_blob_area,
prologMeanBlobArea,
new PrologSet(
-SymbolCodes.symbolCode_E_mean_foreground_area,
prologMeanForegroundArea,
new PrologSet(
-SymbolCodes
.symbolCode_E_mean_characteristic_length,
prologMeanCharacteristicLength,
new PrologSet(
-SymbolCodes
.symbolCode_E_mean_squared_characteristic_length,
prologMeanSquaredCharacteristicLength,
new PrologSet(
-SymbolCodes
.symbolCode_E_mean_standardized_area,
prologMeanStandardizedArea,
new PrologSet(
-SymbolCodes
.symbolCode_E_mean_contour_length,
prologMeanContourLength,
new PrologSet(
-SymbolCodes.symbolCode_E_wr2_mean,
prologWR2Mean,
new PrologSet(
-SymbolCodes
.symbolCode_E_wr2_standard_deviation,
prologWR2StandardDeviation,
new PrologSet(
-SymbolCodes
.symbolCode_E_wr2_skewness,
prologWR2Skewness,
new PrologSet(
-SymbolCodes
.symbolCode_E_wr2_kurtosis,
prologWR2Kurtosis,
new PrologSet(
-SymbolCodes
.symbolCode_E_wr2_cardinality,
prologWR2Cardinality,
new PrologSet(
-SymbolCodes
.symbolCode_E_mean_velocity,
prologMeanVelocity,
PrologEmptySet
.instance)))))))))))))))))));
return prologTrackSegment;
}
protected Term formTrackOfBlob(TrackSegment segment) {
long beginningTime = segment.beginningTime;
int[][] rectangles = segment.rectangles;
double[] windowedR2ReferentValues = segment.getWindowedR2ReferentValues();
Term list3 = PrologEmptyList.instance;
for (int n = rectangles.length - 1; n >= 0; n--) {
int[] currentRectangle = rectangles[n];
long currentForegroundArea = segment.foregroundAreaValues[n];
double currentCharacteristicLength = segment.characteristicLengthValues[n];
long currentContourLength = segment.contourLengthValues[n];
double currentR2 = windowedR2ReferentValues[n];
double currentVelocity = segment.velocityValues[n];
Term prologFrameN = new PrologInteger(beginningTime + currentRectangle[0]);
Term prologXn =
new PrologInteger(StrictMath.round((currentRectangle[1] + currentRectangle[2]) / 2.0));
Term prologYn =
new PrologInteger(StrictMath.round((currentRectangle[3] + currentRectangle[4]) / 2.0));
Term prologWidthN = new PrologInteger(currentRectangle[2] - currentRectangle[1]);
Term prologHeightN = new PrologInteger(currentRectangle[4] - currentRectangle[3]);
Term prologForegroundAreaN = new PrologInteger(currentForegroundArea);
Term prologCharacteristicLengthN = new PrologReal(currentCharacteristicLength);
Term prologContourLengthN = new PrologInteger(currentContourLength);
Term prologR2N = new PrologReal(currentR2);
Term prologVelocityN = new PrologReal(currentVelocity);
Term prologRectangle =
new PrologSet(
-SymbolCodes.symbolCode_E_frame,
prologFrameN,
new PrologSet(
-SymbolCodes.symbolCode_E_x,
prologXn,
new PrologSet(
-SymbolCodes.symbolCode_E_y,
prologYn,
new PrologSet(
-SymbolCodes.symbolCode_E_width,
prologWidthN,
new PrologSet(
-SymbolCodes.symbolCode_E_height,
prologHeightN,
new PrologSet(
-SymbolCodes.symbolCode_E_foreground_area,
prologForegroundAreaN,
new PrologSet(
-SymbolCodes.symbolCode_E_characteristic_length,
prologCharacteristicLengthN,
new PrologSet(
-SymbolCodes.symbolCode_E_contour_length,
prologContourLengthN,
new PrologSet(
-SymbolCodes.symbolCode_E_r2,
prologR2N,
new PrologSet(
-SymbolCodes.symbolCode_E_velocity,
prologVelocityN,
PrologEmptySet.instance))))))))));
list3 = new PrologList(prologRectangle, list3);
}
;
return list3;
}
