@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; }