ContinuousClothoid.java

  1. package org.opentrafficsim.core.geometry;

  2. import org.djunits.value.vdouble.scalar.Angle;
  3. import org.djunits.value.vdouble.scalar.Direction;
  4. import org.djutils.draw.line.PolyLine2d;
  5. import org.djutils.draw.point.OrientedPoint2d;
  6. import org.djutils.draw.point.Point2d;
  7. import org.djutils.exceptions.Throw;
  8. import org.djutils.exceptions.Try;

  9. /**
  10.  * Continuous definition of a clothoid. The following definitions are available:
  11.  * <ul>
  12.  * <li>A clothoid between two directed <i>points</i>.</li>
  13.  * <li>A clothoid originating from a directed point with start curvature, end curvature, and <i>length</i> specified.</li>
  14.  * <li>A clothoid originating from a directed point with start curvature, end curvature, and <i>A-value</i> specified.</li>
  15.  * </ul>
  16.  * This class is based on:
  17.  * <ul>
  18.  * <li>Dale Connor and Lilia Krivodonova (2014) "Interpolation of two-dimensional curves with Euler spirals", Journal of
  19.  * Computational and Applied Mathematics, Volume 261, 1 May 2014, pp. 320-332.</li>
  20.  * <li>D.J. Waltona and D.S. Meek (2009) "G<sup>1</sup> interpolation with a single Cornu spiral segment", Journal of
  21.  * Computational and Applied Mathematics, Volume 223, Issue 1, 1 January 2009, pp. 86-96.</li>
  22.  * </ul>
  23.  * <p>
  24.  * Copyright (c) 2023-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  25.  * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  26.  * </p>
  27.  * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
  28.  * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
  29.  * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
  30.  * @see <a href="https://www.sciencedirect.com/science/article/pii/S0377042713006286">Connor and Krivodonova (2014)</a>
  31.  * @see <a href="https://www.sciencedirect.com/science/article/pii/S0377042704000925">Waltona and Meek (2009)</a>
  32.  */
  33. public class ContinuousClothoid implements ContinuousLine
  34. {

  35.     /** Threshold to consider input to be a trivial straight or circle arc. The value is 1/10th of a degree. */
  36.     private static final double ANGLE_TOLERANCE = 2.0 * Math.PI / 3600.0;

  37.     /** Stopping tolerance for the Secant method to find optimal theta values. */
  38.     private static final double SECANT_TOLERANCE = 1e-8;

  39.     /** Start point with direction. */
  40.     private final OrientedPoint2d startPoint;

  41.     /** End point with direction. */
  42.     private final OrientedPoint2d endPoint;

  43.     /** Start curvature. */
  44.     private final double startCurvature;

  45.     /** End curvature. */
  46.     private final double endCurvature;

  47.     /** Length. */
  48.     private final double length;

  49.     /**
  50.      * A-value; for scaling the Fresnal integral. The regular clothoid A-parameter is obtained by dividing by
  51.      * {@code Math.sqrt(Math.PI)}.
  52.      */
  53.     private final double a;

  54.     /** Minimum alpha value of line to draw. */
  55.     private final double alphaMin;

  56.     /** Maximum alpha value of line to draw. */
  57.     private final double alphaMax;

  58.     /** Unit vector from the origin of the clothoid, towards the positive side. */
  59.     private final double[] t0;

  60.     /** Normal unit vector to t0. */
  61.     private final double[] n0;

  62.     /** Whether the line needs to be flipped. */
  63.     private final boolean opposite;

  64.     /** Whether the line is reflected. */
  65.     private final boolean reflected;

  66.     /** Simplification to straight when valid. */
  67.     private final ContinuousStraight straight;

  68.     /** Simplification to arc when valid. */
  69.     private final ContinuousArc arc;

  70.     /** Whether the shift was determined. */
  71.     private boolean shiftDetermined;

  72.     /** Shift in x-coordinate of start point. */
  73.     private double shiftX;

  74.     /** Shift in y-coordinate of start point. */
  75.     private double shiftY;

  76.     /** Additional shift in x-coordinate towards end point. */
  77.     private double dShiftX;

  78.     /** Additional shift in y-coordinate towards end point. */
  79.     private double dShiftY;

  80.     /**
  81.      * Create clothoid between two directed points. This constructor is based on the procedure in:<br>
  82.      * <br>
  83.      * Dale Connor and Lilia Krivodonova (2014) "Interpolation of two-dimensional curves with Euler spirals", Journal of
  84.      * Computational and Applied Mathematics, Volume 261, 1 May 2014, pp. 320-332.<br>
  85.      * <br>
  86.      * Which applies the theory proven in:<br>
  87.      * <br>
  88.      * D.J. Waltona and D.S. Meek (2009) "G<sup>1</sup> interpolation with a single Cornu spiral segment", Journal of
  89.      * Computational and Applied Mathematics, Volume 223, Issue 1, 1 January 2009, pp. 86-96.<br>
  90.      * <br>
  91.      * This procedure guarantees that the resulting line has the minimal angle rotation that is required to connect the points.
  92.      * If the points approximate a straight line or circle, with a tolerance of up 1/10th of a degree, those respective lines
  93.      * are created. The numerical approximation of the underlying Fresnal integral is different from the paper. See
  94.      * {@code Clothoid.fresnal()}.
  95.      * @param startPoint OrientedPoint2d; start point.
  96.      * @param endPoint OrientedPoint2d; end point.
  97.      * @see <a href="https://www.sciencedirect.com/science/article/pii/S0377042713006286">Connor and Krivodonova (2014)</a>
  98.      * @see <a href="https://www.sciencedirect.com/science/article/pii/S0377042704000925">Waltona and Meek (2009)</a>
  99.      */
  100.     public ContinuousClothoid(final OrientedPoint2d startPoint, final OrientedPoint2d endPoint)
  101.     {
  102.         Throw.whenNull(startPoint, "Start point may not be null.");
  103.         Throw.whenNull(endPoint, "End point may not be null.");
  104.         this.startPoint = startPoint;
  105.         this.endPoint = endPoint;

  106.         double dx = endPoint.x - startPoint.x;
  107.         double dy = endPoint.y - startPoint.y;
  108.         double d2 = Math.hypot(dx, dy); // length of straight line from start to end
  109.         double d = Math.atan2(dy, dx); // angle of line through start and end points

  110.         double phi1 = normalizeAngle(d - startPoint.dirZ);
  111.         double phi2 = normalizeAngle(endPoint.dirZ - d);
  112.         double phi1Abs = Math.abs(phi1);
  113.         double phi2Abs = Math.abs(phi2);

  114.         if (phi1Abs < ANGLE_TOLERANCE && phi2Abs < ANGLE_TOLERANCE)
  115.         {
  116.             // Straight
  117.             this.length = Math.hypot(endPoint.x - startPoint.x, endPoint.y - startPoint.y);
  118.             this.a = Double.POSITIVE_INFINITY;
  119.             this.startCurvature = 0.0;
  120.             this.endCurvature = 0.0;
  121.             this.straight = new ContinuousStraight(startPoint, this.length);
  122.             this.arc = null;
  123.             this.alphaMin = 0.0;
  124.             this.alphaMax = 0.0;
  125.             this.t0 = null;
  126.             this.n0 = null;
  127.             this.opposite = false;
  128.             this.reflected = false;
  129.             return;
  130.         }
  131.         else if (Math.abs(phi2 - phi1) < ANGLE_TOLERANCE)
  132.         {
  133.             // Arc
  134.             double r = .5 * d2 / Math.sin(phi1);
  135.             double cosStartDirection = Math.cos(startPoint.dirZ);
  136.             double sinStartDirection = Math.sin(startPoint.dirZ);
  137.             double ang = Math.PI / 2.0;
  138.             double cosAng = Math.cos(ang); // =0
  139.             double sinAng = Math.sin(ang); // =1
  140.             double x0 = startPoint.x - r * (cosStartDirection * cosAng + sinStartDirection * sinAng);
  141.             double y0 = startPoint.y - r * (cosStartDirection * -sinAng + sinStartDirection * cosAng);
  142.             double from = Math.atan2(startPoint.y - y0, startPoint.x - x0);
  143.             double to = Math.atan2(endPoint.y - y0, endPoint.x - x0);
  144.             if (r < 0 && to > from)
  145.             {
  146.                 to = to - 2.0 * Math.PI;
  147.             }
  148.             else if (r > 0 && to < from)
  149.             {
  150.                 to = to + 2.0 * Math.PI;
  151.             }
  152.             Angle angle = Angle.instantiateSI(Math.abs(to - from));
  153.             this.length = angle.si * Math.abs(r);
  154.             this.a = 0.0;
  155.             this.startCurvature = 1.0 / r;
  156.             this.endCurvature = 1.0 / r;
  157.             this.straight = null;
  158.             this.arc = new ContinuousArc(startPoint, Math.abs(r), r > 0.0, angle);
  159.             this.alphaMin = 0.0;
  160.             this.alphaMax = 0.0;
  161.             this.t0 = null;
  162.             this.n0 = null;
  163.             this.opposite = false;
  164.             this.reflected = false;
  165.             return;
  166.         }
  167.         this.straight = null;
  168.         this.arc = null;

  169.         // The algorithm assumes |phi2| to be larger than |phi1|. If this is not the case, the clothoid is created in the
  170.         // opposite direction.
  171.         if (phi2Abs < phi1Abs)
  172.         {
  173.             this.opposite = true;
  174.             double phi3 = phi1;
  175.             phi1 = -phi2;
  176.             phi2 = -phi3;
  177.             dx = -dx;
  178.             dy = -dy;
  179.         }
  180.         else
  181.         {
  182.             this.opposite = false;
  183.         }

  184.         // The algorithm assumes 0 < phi2 < pi. If this is not the case, the input and output are reflected on 'd'.
  185.         this.reflected = phi2 < 0 || phi2 > Math.PI;
  186.         if (this.reflected)
  187.         {
  188.             phi1 = -phi1;
  189.             phi2 = -phi2;
  190.         }

  191.         // h(phi1, phi2) guarantees for negative values along with 0 < phi1 < phi2 < pi, that a C-shaped clothoid exists.
  192.         double[] cs = Fresnel.fresnel(alphaToT(phi1 + phi2));
  193.         double h = cs[1] * Math.cos(phi1) - cs[0] * Math.sin(phi1);
  194.         boolean cShape = 0 < phi1 && phi1 < phi2 && phi2 < Math.PI && h < 0; // otherwise, S-shape
  195.         double theta = getTheta(phi1, phi2, cShape);
  196.         double aSign = cShape ? -1.0 : 1.0;
  197.         double thetaSign = -aSign;

  198.         double v1 = theta + phi1 + phi2;
  199.         double v2 = theta + phi1;
  200.         double[] cs0 = Fresnel.fresnel(alphaToT(theta));
  201.         double[] cs1 = Fresnel.fresnel(alphaToT(v1));
  202.         this.a = d2 / ((cs1[1] + aSign * cs0[1]) * Math.sin(v2) + (cs1[0] + aSign * cs0[0]) * Math.cos(v2));

  203.         dx /= d2; // normalized
  204.         dy /= d2;
  205.         if (this.reflected)
  206.         {
  207.             // reflect t0 and n0 on 'd' so that the created output clothoid is reflected back after input was reflected
  208.             this.t0 = new double[] {Math.cos(-v2) * dx + Math.sin(-v2) * dy, -Math.sin(-v2) * dx + Math.cos(-v2) * dy};
  209.             this.n0 = new double[] {-this.t0[1], this.t0[0]};
  210.         }
  211.         else
  212.         {
  213.             this.t0 = new double[] {Math.cos(v2) * dx + Math.sin(v2) * dy, -Math.sin(v2) * dx + Math.cos(v2) * dy};
  214.             this.n0 = new double[] {this.t0[1], -this.t0[0]};
  215.         }

  216.         this.alphaMin = thetaSign * theta;
  217.         this.alphaMax = v1; // alphaMax = theta + phi1 + phi2, which is v1
  218.         double sign = (this.reflected ? -1.0 : 1.0);
  219.         double curveMin = Math.PI * alphaToT(this.alphaMin) / this.a;
  220.         double curveMax = Math.PI * alphaToT(v1) / this.a;
  221.         this.startCurvature = sign * (this.opposite ? -curveMax : curveMin);
  222.         this.endCurvature = sign * (this.opposite ? -curveMin : curveMax);
  223.         this.length = this.a * (alphaToT(v1) - alphaToT(this.alphaMin));
  224.     }

  225.     /**
  226.      * Create clothoid from one point based on curvature and A-value.
  227.      * @param startPoint OrientedPoint2d; start point.
  228.      * @param a Length; A-value.
  229.      * @param startCurvature double; start curvature.
  230.      * @param endCurvature double; end curvature;
  231.      */
  232.     public ContinuousClothoid(final OrientedPoint2d startPoint, final double a, final double startCurvature,
  233.             final double endCurvature)
  234.     {
  235.         Throw.whenNull(startPoint, "Start point may not be null.");
  236.         Throw.when(a <= 0.0, IllegalArgumentException.class, "A value must be above 0.");
  237.         this.startPoint = startPoint;
  238.         // Scale 'a', due to parameter conversion between C(alpha)/S(alpha) and C(t)/S(t); t = sqrt(2*alpha/pi).
  239.         this.a = a * Math.sqrt(Math.PI);
  240.         this.length = a * a * Math.abs(endCurvature - startCurvature);
  241.         this.startCurvature = startCurvature;
  242.         this.endCurvature = endCurvature;

  243.         double l1 = a * a * startCurvature;
  244.         double l2 = a * a * endCurvature;
  245.         this.alphaMin = Math.abs(l1) * startCurvature / 2.0;
  246.         this.alphaMax = Math.abs(l2) * endCurvature / 2.0;

  247.         double ang = normalizeAngle(startPoint.dirZ) - Math.abs(this.alphaMin);
  248.         this.t0 = new double[] {Math.cos(ang), Math.sin(ang)};
  249.         this.n0 = new double[] {this.t0[1], -this.t0[0]};
  250.         Direction endDirection = Direction.instantiateSI(ang + Math.abs(this.alphaMax));
  251.         if (startCurvature > endCurvature)
  252.         {
  253.             // In these cases the algorithm works in the negative direction. We need to flip over the line through the start
  254.             // point that runs perpendicular to the start direction.
  255.             double m = Math.tan(startPoint.dirZ + Math.PI / 2.0);

  256.             // Linear algebra flipping, see: https://math.stackexchange.com/questions/525082/reflection-across-a-line
  257.             double onePlusMm = 1.0 + m * m;
  258.             double oneMinusMm = 1.0 - m * m;
  259.             double mmMinusOne = m * m - 1.0;
  260.             double twoM = 2.0 * m;
  261.             double t00 = this.t0[0];
  262.             double t01 = this.t0[1];
  263.             double n00 = this.n0[0];
  264.             double n01 = this.n0[1];
  265.             this.t0[0] = (oneMinusMm * t00 + 2 * m * t01) / onePlusMm;
  266.             this.t0[1] = (twoM * t00 + mmMinusOne * t01) / onePlusMm;
  267.             this.n0[0] = (oneMinusMm * n00 + 2 * m * n01) / onePlusMm;
  268.             this.n0[1] = (twoM * n00 + mmMinusOne * n01) / onePlusMm;

  269.             double ang2 = Math.atan2(this.t0[1], this.t0[0]);
  270.             endDirection = Direction.instantiateSI(ang2 - Math.abs(this.alphaMax) + Math.PI);
  271.         }
  272.         PolyLine2d line = flatten(new Flattener.NumSegments(1));
  273.         Point2d end = Try.assign(() -> line.get(line.size() - 1), "Line does not have an end point.");
  274.         this.endPoint = new OrientedPoint2d(end.x, end.y, endDirection.si);

  275.         // Fields not relevant for definition with curvatures
  276.         this.straight = null;
  277.         this.arc = null;
  278.         this.opposite = false;
  279.         this.reflected = false;
  280.     }

  281.     /**
  282.      * Create clothoid from one point based on curvature and length. This method calculates the A-value as
  283.      * <i>sqrt(L/|k2-k1|)</i>, where <i>L</i> is the length of the resulting clothoid, and <i>k2</i> and <i>k1</i> are the end
  284.      * and start curvature.
  285.      * @param startPoint OrientedPoint2d; start point.
  286.      * @param length double; Length of the resulting clothoid.
  287.      * @param startCurvature double; start curvature.
  288.      * @param endCurvature double; end curvature;
  289.      * @return ContinuousClothoid; clothoid based on curvature and length.
  290.      */
  291.     public static ContinuousClothoid withLength(final OrientedPoint2d startPoint, final double length,
  292.             final double startCurvature, final double endCurvature)
  293.     {
  294.         Throw.when(length <= 0.0, IllegalArgumentException.class, "Length must be above 0.");
  295.         double a = Math.sqrt(length / Math.abs(endCurvature - startCurvature));
  296.         return new ContinuousClothoid(startPoint, a, startCurvature, endCurvature);
  297.     }

  298.     /**
  299.      * Normalizes the angle to be in the range [-pi pi].
  300.      * @param angle double; angle.
  301.      * @return double; angle in the range [-pi pi].
  302.      */
  303.     private static double normalizeAngle(final double angle)
  304.     {
  305.         double out = angle;
  306.         while (out > Math.PI)
  307.         {
  308.             out -= 2 * Math.PI;
  309.         }
  310.         while (out < -Math.PI)
  311.         {
  312.             out += 2 * Math.PI;
  313.         }
  314.         return out;
  315.     }

  316.     /**
  317.      * Performs alpha to t variable change.
  318.      * @param alpha double; alpha value, must be positive.
  319.      * @return double; t value (length along the Fresnel integral, also known as x).
  320.      */
  321.     private static double alphaToT(final double alpha)
  322.     {
  323.         return alpha >= 0 ? Math.sqrt(alpha * 2.0 / Math.PI) : -Math.sqrt(-alpha * 2.0 / Math.PI);
  324.     }

  325.     /**
  326.      * Returns theta value given shape to use. If no such value is found, the other shape may be attempted.
  327.      * @param phi1 double; phi1.
  328.      * @param phi2 double; phi2.
  329.      * @param cShape boolean; C-shaped, or S-shaped otherwise.
  330.      * @return double; theta value; the number of radians that is moved on to a side of the full clothoid.
  331.      */
  332.     private static double getTheta(final double phi1, final double phi2, final boolean cShape)
  333.     {
  334.         double sign, phiMin, phiMax;
  335.         if (cShape)
  336.         {
  337.             double lambda = (1 - Math.cos(phi1)) / (1 - Math.cos(phi2));
  338.             phiMin = 0.0;
  339.             phiMax = (lambda * lambda * (phi1 + phi2)) / (1 - (lambda * lambda));
  340.             sign = -1.0;
  341.         }
  342.         else
  343.         {
  344.             phiMin = Math.max(0, -phi1);
  345.             phiMax = Math.PI / 2 - phi1;
  346.             sign = 1;
  347.         }

  348.         double fMin = fTheta(phiMin, phi1, phi2, sign);
  349.         double fMax = fTheta(phiMax, phi1, phi2, sign);
  350.         if (fMin * fMax > 0)
  351.         {
  352.             throw new RuntimeException("f(phiMin) and f(phiMax) have the same sign, we cant find f(theta) = 0 between them.");
  353.         }

  354.         // Find optimum using Secant method, see https://en.wikipedia.org/wiki/Secant_method
  355.         double x0 = phiMin;
  356.         double x1 = phiMax;
  357.         double x2 = 0;
  358.         for (int i = 0; i < 100; i++) // max 100 iterations, otherwise use latest x2 value
  359.         {
  360.             double f1 = fTheta(x1, phi1, phi2, sign);
  361.             x2 = x1 - f1 * (x1 - x0) / (f1 - fTheta(x0, phi1, phi2, sign));
  362.             x2 = Math.max(Math.min(x2, phiMax), phiMin); // this line is an essential addition to keep the algorithm at bay
  363.             x0 = x1;
  364.             x1 = x2;
  365.             if (Math.abs(x0 - x1) < SECANT_TOLERANCE || Math.abs(x0 / x1 - 1) < SECANT_TOLERANCE
  366.                     || Math.abs(f1) < SECANT_TOLERANCE)
  367.             {
  368.                 return x2;
  369.             }
  370.         }

  371.         return x2;
  372.     }

  373.     /**
  374.      * Function who's solution <i>f</i>(<i>theta</i>) = 0 for the given value of <i>phi1</i> and <i>phi2</i> gives the angle
  375.      * that solves fitting a C-shaped clothoid through two points. This assumes that <i>sign</i> = -1. If <i>sign</i> = 1, this
  376.      * changes to <i>g</i>(<i>theta</i>) = 0 being a solution for an S-shaped clothoid.
  377.      * @param theta double; angle defining the curvature of the resulting clothoid.
  378.      * @param phi1 double; angle between the line through both end points, and the direction of the first point.
  379.      * @param phi2 double; angle between the line through both end points, and the direction of the last point.
  380.      * @param sign double; 1 for C-shaped, -1 for S-shaped.
  381.      * @return double; <i>f</i>(<i>theta</i>) for <i>sign</i> = -1, or <i>g</i>(<i>theta</i>) for <i>sign</i> = 1.
  382.      */
  383.     private static double fTheta(final double theta, final double phi1, final double phi2, final double sign)
  384.     {
  385.         double thetaPhi1 = theta + phi1;
  386.         double[] cs0 = Fresnel.fresnel(alphaToT(theta));
  387.         double[] cs1 = Fresnel.fresnel(alphaToT(thetaPhi1 + phi2));
  388.         return (cs1[1] + sign * cs0[1]) * Math.cos(thetaPhi1) - (cs1[0] + sign * cs0[0]) * Math.sin(thetaPhi1);
  389.     }

  390.     /** {@inheritDoc} */
  391.     @Override
  392.     public OrientedPoint2d getStartPoint()
  393.     {
  394.         return this.startPoint;
  395.     }

  396.     /** {@inheritDoc} */
  397.     @Override
  398.     public OrientedPoint2d getEndPoint()
  399.     {
  400.         return this.endPoint;
  401.     }

  402.     /** {@inheritDoc} */
  403.     @Override
  404.     public double getStartCurvature()
  405.     {
  406.         return this.startCurvature;
  407.     }

  408.     /** {@inheritDoc} */
  409.     @Override
  410.     public double getEndCurvature()
  411.     {
  412.         return this.endCurvature;
  413.     }

  414.     /** {@inheritDoc} */
  415.     @Override
  416.     public double getStartRadius()
  417.     {
  418.         return 1.0 / this.startCurvature;
  419.     }

  420.     /** {@inheritDoc} */
  421.     @Override
  422.     public double getEndRadius()
  423.     {
  424.         return 1.0 / this.endCurvature;
  425.     }

  426.     /**
  427.      * Return A, the clothoid scaling parameter.
  428.      * @return double; a, the clothoid scaling parameter.
  429.      */
  430.     public double getA()
  431.     {
  432.         // Scale 'a', due to parameter conversion between C(alpha)/S(alpha) and C(t)/S(t); t = sqrt(2*alpha/pi).
  433.         // The value of 'this.a' is used when scaling the Fresnel integral, which is why this is stored.
  434.         return this.a / Math.sqrt(Math.PI);
  435.     }

  436.     /**
  437.      * Calculates shifts if these have not yet been calculated.
  438.      */
  439.     private void assureShift()
  440.     {
  441.         if (this.shiftDetermined)
  442.         {
  443.             return;
  444.         }

  445.         OrientedPoint2d p1 = this.opposite ? this.endPoint : this.startPoint;
  446.         OrientedPoint2d p2 = this.opposite ? this.startPoint : this.endPoint;

  447.         // Create first point to figure out the required overall shift
  448.         double[] csMin = Fresnel.fresnel(alphaToT(this.alphaMin));
  449.         double xMin = this.a * (csMin[0] * this.t0[0] - csMin[1] * this.n0[0]);
  450.         double yMin = this.a * (csMin[0] * this.t0[1] - csMin[1] * this.n0[1]);
  451.         this.shiftX = p1.x - xMin;
  452.         this.shiftY = p1.y - yMin;

  453.         // Due to numerical precision, we linearly scale over alpha such that the final point is exactly on p2
  454.         if (p2 != null)
  455.         {
  456.             double[] csMax = Fresnel.fresnel(alphaToT(this.alphaMax));
  457.             double xMax = this.a * (csMax[0] * this.t0[0] - csMax[1] * this.n0[0]);
  458.             double yMax = this.a * (csMax[0] * this.t0[1] - csMax[1] * this.n0[1]);
  459.             this.dShiftX = p2.x - (xMax + this.shiftX);
  460.             this.dShiftY = p2.y - (yMax + this.shiftY);
  461.         }
  462.         else
  463.         {
  464.             this.dShiftX = 0.0;
  465.             this.dShiftY = 0.0;
  466.         }

  467.         this.shiftDetermined = true;
  468.     }

  469.     /**
  470.      * Returns a point on the clothoid at a fraction of curvature along the clothoid.
  471.      * @param fraction double; fraction of curvature along the clothoid.
  472.      * @param offset double; offset relative to radius.
  473.      * @return Point2d; point on the clothoid at a fraction of curvature along the clothoid.
  474.      */
  475.     private Point2d getPoint(final double fraction, final double offset)
  476.     {
  477.         double f = this.opposite ? 1.0 - fraction : fraction;
  478.         double alpha = this.alphaMin + f * (this.alphaMax - this.alphaMin);
  479.         double[] cs = Fresnel.fresnel(alphaToT(alpha));
  480.         double x = this.shiftX + this.a * (cs[0] * this.t0[0] - cs[1] * this.n0[0]) + f * this.dShiftX;
  481.         double y = this.shiftY + this.a * (cs[0] * this.t0[1] - cs[1] * this.n0[1]) + f * this.dShiftY;
  482.         double d = getDirection(alpha) + Math.PI / 2;
  483.         return new Point2d(x + Math.cos(d) * offset, y + Math.sin(d) * offset);
  484.     }

  485.     /**
  486.      * Returns the direction at given alpha.
  487.      * @param alpha double; alpha.
  488.      * @return double; direction at given alpha.
  489.      */
  490.     private double getDirection(final double alpha)
  491.     {
  492.         double rot = Math.atan2(this.t0[1], this.t0[0]);
  493.         // abs because alpha = -3deg has the same direction as alpha = 3deg in an S-curve where alpha = 0 is the middle
  494.         rot += this.reflected ? -Math.abs(alpha) : Math.abs(alpha);
  495.         if (this.opposite)
  496.         {
  497.             rot += Math.PI;
  498.         }
  499.         return normalizeAngle(rot);
  500.     }

  501.     /** {@inheritDoc} */
  502.     @Override
  503.     public PolyLine2d flatten(final Flattener flattener)
  504.     {
  505.         Throw.whenNull(flattener, "Flattener may not be null.");
  506.         if (this.straight != null)
  507.         {
  508.             return this.straight.flatten(flattener);
  509.         }
  510.         if (this.arc != null)
  511.         {
  512.             return this.arc.flatten(flattener);
  513.         }
  514.         assureShift();
  515.         return flattener.flatten(new FlattableLine()
  516.         {
  517.             /** {@inheritDoc} */
  518.             @Override
  519.             public Point2d get(final double fraction)
  520.             {
  521.                 return getPoint(fraction, 0.0);
  522.             }

  523.             /** {@inheritDoc} */
  524.             @Override
  525.             public double getDirection(final double fraction)
  526.             {
  527.                 return ContinuousClothoid.this.getDirection(ContinuousClothoid.this.alphaMin
  528.                         + fraction * (ContinuousClothoid.this.alphaMax - ContinuousClothoid.this.alphaMin));
  529.             }
  530.         });
  531.     }

  532.     /** {@inheritDoc} */
  533.     @Override
  534.     public PolyLine2d flattenOffset(final FractionalLengthData offsets, final Flattener flattener)
  535.     {
  536.         Throw.whenNull(offsets, "Offsets may not be null.");
  537.         Throw.whenNull(flattener, "Flattener may not be null.");
  538.         if (this.straight != null)
  539.         {
  540.             return this.straight.flattenOffset(offsets, flattener);
  541.         }
  542.         if (this.arc != null)
  543.         {
  544.             return this.arc.flattenOffset(offsets, flattener);
  545.         }
  546.         assureShift();
  547.         return flattener.flatten(new FlattableLine()
  548.         {
  549.             /** {@inheritDoc} */
  550.             @Override
  551.             public Point2d get(final double fraction)
  552.             {
  553.                 return getPoint(fraction, offsets.get(fraction));
  554.             }

  555.             /** {@inheritDoc} */
  556.             @Override
  557.             public double getDirection(final double fraction)
  558.             {
  559.                 return ContinuousClothoid.this.getDirection(ContinuousClothoid.this.alphaMin
  560.                         + fraction * (ContinuousClothoid.this.alphaMax - ContinuousClothoid.this.alphaMin));
  561.             }
  562.         });
  563.     }

  564.     /** {@inheritDoc} */
  565.     @Override
  566.     public double getLength()
  567.     {
  568.         return this.length;
  569.     }

  570.     /**
  571.      * Returns whether the shape was applied as a Clothoid, an Arc, or as a Straight, depending on start and end position and
  572.      * direction.
  573.      * @return String; "Clothoid", "Arc" or "Straight".
  574.      */
  575.     public String getAppliedShape()
  576.     {
  577.         return this.straight == null ? (this.arc == null ? "Clothoid" : "Arc") : "Straight";
  578.     }

  579.     /** {@inheritDoc} */
  580.     @Override
  581.     public String toString()
  582.     {
  583.         return "ContinuousClothoid [startPoint=" + this.startPoint + ", endPoint=" + this.endPoint + ", startCurvature="
  584.                 + this.startCurvature + ", endCurvature=" + this.endCurvature + ", length=" + this.length + "]";
  585.     }

  586. }