public static void main(String[] args) {
for (double absMeasure = 0.0; absMeasure < 100.0; absMeasure += 1.0) {
double angleD = 10.0;
System.out.print(Utils.dTS(angleD));
// double absMeasure = Math.abs(situation.getMeasure());
if (absMeasure >= 20.0 && absMeasure < 35.0) {
angleD *= 1.0 + ((absMeasure - 20.0) / (35.0 - 20.0));
} else if (absMeasure >= 35.0 && absMeasure < 60.0) {
angleD *= 2.0 + ((absMeasure - 35.0) / (60.0 - 35.0));
} else if (absMeasure >= 60.0) {
angleD *= 3.0;
}
System.out.println(" " + Utils.dTS(absMeasure) + " " + angleD);
}
}
@Override
public void execute(SensorData data, ModifiableAction action) {
// steering based on the "biggest sensor value" heuristic
double[] rawSensors = data.getRawTrackEdgeSensors();
int index = 0;
double longest = rawSensors[0];
for (int i = 1; i < rawSensors.length; ++i) {
if (rawSensors[i] > longest) {
index = i;
longest = rawSensors[i];
}
}
double angleD = -angles[index];
if (TEXT_DEBUG) {
System.out.println(
Utils.timeToExactString(data.getCurrentLapTime())
+ " @ "
+ data.getDistanceRacedS()
+ "- track: "
+ data.getTrackPositionS()
+ ", index: "
+ index
+ ", angleD: "
+ Utils.dTS(angleD));
}
double absMeasure = Math.abs(situation.getMeasure());
if (absMeasure >= 20.0 && absMeasure < 35.0) {
angleD *= 1.0 + ((absMeasure - 20.0) / (35.0 - 20.0));
} else if (absMeasure >= 35.0 && absMeasure < 60.0) {
angleD *= 2.0 + ((absMeasure - 35.0) / (60.0 - 35.0));
} else if (absMeasure >= 60.0) {
angleD *= 3.0;
}
if (TEXT_DEBUG) {
System.out.println("absMeasure: " + Utils.dTS(absMeasure) + ", angleD: " + Utils.dTS(angleD));
}
double trackPos = data.getTrackPosition();
double trackAngle = data.getAngleToTrackAxis();
if (situation.isCorner()) {
boolean mirror = situation.isRight();
if (mirror) {
trackPos *= -1.0;
trackAngle *= -1.0;
angleD *= -1.0;
index = 18 - index;
}
double absTrackPos = SensorData.calcAbsoluteTrackPosition(trackPos, trackWidth);
// inside
if (absTrackPos - (CAR_WIDTH * 0.5) < INSIDE_MARGIN && trackAngle < 0.0) {
double beta = (INSIDE_MARGIN - (absTrackPos - (CAR_WIDTH * 0.5))) / INSIDE_MARGIN;
if (TEXT_DEBUG) {
System.out.println("Inside");
}
// not so close -> open steering
if (beta < 0.5) {
// System.out.println("IN - Opening@"+Utils.dTS(data.getDistanceFromStartLine()));
if (TEXT_DEBUG) {
System.out.print("Beta: " + Utils.dTS(beta) + ", old: " + Utils.dTS(angleD));
}
double alpha = (beta - 0.5) * -(1 / 0.5);
angleD *= alpha;
if (TEXT_DEBUG) {
System.out.println(", alpha: " + Utils.dTS(alpha) + ", new: " + Utils.dTS(angleD));
}
} else {
// System.out.println("IN - Other@"+Utils.dTS(data.getDistanceFromStartLine()));
beta = Math.min(beta, 1.0);
// very close, steer in the other direction
if (TEXT_DEBUG) {
System.out.print("Beta: " + Utils.dTS(beta) + ", old: " + Utils.dTS(angleD));
}
double alpha = (beta - 0.5) * (1 / 0.5);
angleD = Math.toDegrees(trackAngle) * alpha;
// angleD = 2.0*alpha;
if (TEXT_DEBUG) {
System.out.println(", alpha: " + Utils.dTS(alpha) + ", new: " + Utils.dTS(angleD));
}
}
}
// outside
if (absTrackPos + (CAR_WIDTH * 0.5) > trackWidth - OUTSIDE_MARGIN) {
// System.out.println("OUT@"+Utils.dTS(data.getDistanceFromStartLine()));
if (TEXT_DEBUG) {
System.out.println("Outside");
}
// increase steering angle
double alpha = Math.min(absMeasure, OUTSIDE_DOUBLE) / OUTSIDE_DOUBLE;
double beta =
((absTrackPos + (CAR_WIDTH * 0.5)) - (trackWidth - OUTSIDE_MARGIN)) / OUTSIDE_MARGIN;
if (TEXT_DEBUG) {
System.out.print("Beta: " + Utils.dTS(beta) + ", old: " + Utils.dTS(angleD));
}
beta = Math.min(beta, 1.0);
angleD *= 1.0 + (alpha * beta);
if (TEXT_DEBUG) {
System.out.println(
", 1+alpha: " + Utils.dTS(1.0 + alpha) + ", new: " + Utils.dTS(angleD));
}
}
// if(Math.abs(Math.toDegrees(trackAngle)) < 10.0){
// System.out.println(absMeasure + ";" + trackPos + ";" + Math.toDegrees(trackAngle) + ";"
// + index + ";" + angleD);
// }
if (mirror) {
angleD *= -1.0;
}
}
angleD = Math.min(45.0, Math.max(-45.0, angleD));
action.setSteering(angleD / 45.0);
action.limitValues();
if (TEXT_DEBUG) {
System.out.println("Final value: " + action.getSteeringS());
}
}
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;
}