Clothoid.java

  1. package org.opentrafficsim.core.geometry;

  2. import org.djunits.unit.AngleUnit;
  3. import org.djunits.unit.LengthUnit;
  4. import org.djunits.unit.LinearDensityUnit;
  5. import org.djunits.value.vdouble.scalar.Direction;
  6. import org.djunits.value.vdouble.scalar.Length;
  7. import org.djunits.value.vdouble.scalar.LinearDensity;

  8. /**
  9.  * Generate an OTSLine3D for a clothoid. <br>
  10.  * Derived from odrSpiral.c by M. Dupuis @ VIRES GmbH <br>
  11.  *
  12.  * <pre>
  13.  *        Licensed under the Apache License, Version 2.0 (the "License");
  14.  *        you may not use this file except in compliance with the License.
  15.  *        You may obtain a copy of the License at
  16.  *
  17.  *            http://www.apache.org/licenses/LICENSE-2.0
  18.  *
  19.  *        Unless required by applicable law or agreed to in writing, software
  20.  *        distributed under the License is distributed on an "AS IS" BASIS,
  21.  *        WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  22.  *        See the License for the specific language governing permissions and
  23.  *        limitations under the License.
  24.  * </pre>
  25.  * <p>
  26.  * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  27.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  28.  * <p>
  29.  * @version $Revision$, $LastChangedDate$, by $Author$, initial version Nov 2, 2015 <br>
  30.  * @author M. Dupuis @ VIRES GmbH
  31.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  32.  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  33.  */
  34. public final class Clothoid
  35. {
  36.     /** Utility class. */
  37.     private Clothoid()
  38.     {
  39.         // do not instantiate
  40.     }

  41.     /*- ===================================================
  42.      *  file:       odrSpiral.c
  43.      * ---------------------------------------------------
  44.      *  purpose:    free method for computing spirals
  45.      *              in OpenDRIVE applications
  46.      * ---------------------------------------------------
  47.      *  using methods of CEPHES library
  48.      * ---------------------------------------------------
  49.      *  first edit: 09.03.2010 by M. Dupuis @ VIRES GmbH
  50.      *  last mod.:  09.03.2010 by M. Dupuis @ VIRES GmbH
  51.      * ===================================================
  52.         Copyright 2010 VIRES Simulationstechnologie GmbH

  53.         Licensed under the Apache License, Version 2.0 (the "License");
  54.         you may not use this file except in compliance with the License.
  55.         You may obtain a copy of the License at

  56.             http://www.apache.org/licenses/LICENSE-2.0

  57.         Unless required by applicable law or agreed to in writing, software
  58.         distributed under the License is distributed on an "AS IS" BASIS,
  59.         WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  60.         See the License for the specific language governing permissions and
  61.         limitations under the License.
  62.        
  63.        
  64.         NOTE:
  65.         The methods have been realized using the CEPHES library

  66.             http://www.netlib.org/cephes/

  67.         and do neither constitute the only nor the exclusive way of implementing
  68.         spirals for OpenDRIVE applications. Their sole purpose is to facilitate
  69.         the interpretation of OpenDRIVE spiral data.
  70.      */

  71.     //@formatter:off
  72.     /** S(x) for small x numerator. */
  73.     static final double[] SN = {
  74.         -2.99181919401019853726E3,
  75.         7.08840045257738576863E5,
  76.         -6.29741486205862506537E7,
  77.         2.54890880573376359104E9,
  78.         -4.42979518059697779103E10,
  79.         3.18016297876567817986E11
  80.     };

  81.     /** S(x) for small x denominator. */
  82.     static final double[] SD = {
  83.         2.81376268889994315696E2,
  84.         4.55847810806532581675E4,
  85.         5.17343888770096400730E6,
  86.         4.19320245898111231129E8,
  87.         2.24411795645340920940E10,
  88.         6.07366389490084639049E11
  89.     };

  90.     /** C(x) for small x numerator. */
  91.     static final double[] CN = {
  92.         -4.98843114573573548651E-8,
  93.         9.50428062829859605134E-6,
  94.         -6.45191435683965050962E-4,
  95.         1.88843319396703850064E-2,
  96.         -2.05525900955013891793E-1,
  97.         9.99999999999999998822E-1
  98.     };

  99.     /** C(x) for small x denominator. */
  100.     static final double[] CD = {
  101.         3.99982968972495980367E-12,
  102.         9.15439215774657478799E-10,
  103.         1.25001862479598821474E-7,
  104.         1.22262789024179030997E-5,
  105.         8.68029542941784300606E-4,
  106.         4.12142090722199792936E-2,
  107.         1.00000000000000000118E0
  108.     };

  109.     /** Auxiliary function f(x) numerator. */
  110.     static final double[] FN = {
  111.         4.21543555043677546506E-1,
  112.         1.43407919780758885261E-1,
  113.         1.15220955073585758835E-2,
  114.         3.45017939782574027900E-4,
  115.         4.63613749287867322088E-6,
  116.         3.05568983790257605827E-8,
  117.         1.02304514164907233465E-10,
  118.         1.72010743268161828879E-13,
  119.         1.34283276233062758925E-16,
  120.         3.76329711269987889006E-20
  121.     };

  122.     /** Auxiliary function f(x) denominator. */
  123.     static final double[] FD = {
  124.         7.51586398353378947175E-1,
  125.         1.16888925859191382142E-1,
  126.         6.44051526508858611005E-3,
  127.         1.55934409164153020873E-4,
  128.         1.84627567348930545870E-6,
  129.         1.12699224763999035261E-8,
  130.         3.60140029589371370404E-11,
  131.         5.88754533621578410010E-14,
  132.         4.52001434074129701496E-17,
  133.         1.25443237090011264384E-20
  134.     };

  135.     /** Auxiliary function g(x) numerator. */
  136.     static final double[] GN = {
  137.         5.04442073643383265887E-1,
  138.         1.97102833525523411709E-1,
  139.         1.87648584092575249293E-2,
  140.         6.84079380915393090172E-4,
  141.         1.15138826111884280931E-5,
  142.         9.82852443688422223854E-8,
  143.         4.45344415861750144738E-10,
  144.         1.08268041139020870318E-12,
  145.         1.37555460633261799868E-15,
  146.         8.36354435630677421531E-19,
  147.         1.86958710162783235106E-22
  148.     };

  149.     /** Auxiliary function g(x) denominator. */
  150.     static final double[] GD = {
  151.         1.47495759925128324529E0,
  152.         3.37748989120019970451E-1,
  153.         2.53603741420338795122E-2,
  154.         8.14679107184306179049E-4,
  155.         1.27545075667729118702E-5,
  156.         1.04314589657571990585E-7,
  157.         4.60680728146520428211E-10,
  158.         1.10273215066240270757E-12,
  159.         1.38796531259578871258E-15,
  160.         8.39158816283118707363E-19,
  161.         1.86958710162783236342E-22
  162.     };
  163.     //@formatter:on

  164.     /**
  165.      * Compute a polynomial in x.
  166.      * @param x double; x
  167.      * @param coef double[]; coefficients
  168.      * @return polynomial in x
  169.      */
  170.     private static double polevl(final double x, final double[] coef)
  171.     {
  172.         double result = coef[0];
  173.         for (int i = 0; i < coef.length; i++)
  174.         {
  175.             result = result * x + coef[i];
  176.         }
  177.         return result;
  178.     }

  179.     /**
  180.      * Compute a polynomial in x.
  181.      * @param x double; x
  182.      * @param coef double[]; coefficients
  183.      * @return polynomial in x
  184.      */
  185.     private static double p1evl(final double x, final double[] coef)
  186.     {
  187.         double result = x + coef[0];
  188.         for (int i = 0; i < coef.length; i++)
  189.         {
  190.             result = result * x + coef[i];
  191.         }
  192.         return result;
  193.     }

  194.     /**
  195.      * Approximate the Fresnel function.
  196.      * @param xxa double; the xxa parameter
  197.      * @return double[]; array with two double values c and s
  198.      */
  199.     private static double[] fresnel(final double xxa)
  200.     {
  201.         final double x = Math.abs(xxa);
  202.         final double x2 = x * x;
  203.         double cc, ss;

  204.         if (x2 < 2.5625)
  205.         {
  206.             final double t = x2 * x2;
  207.             ss = x * x2 * polevl(t, SN) / p1evl(t, SD);
  208.             cc = x * polevl(t, CN) / polevl(t, CD);
  209.         }
  210.         else if (x > 36974.0)
  211.         {
  212.             cc = 0.5;
  213.             ss = 0.5;
  214.         }
  215.         else
  216.         {
  217.             double t = Math.PI * x2;
  218.             final double u = 1.0 / (t * t);
  219.             t = 1.0 / t;
  220.             final double f = 1.0 - u * polevl(u, FN) / p1evl(u, FD);
  221.             final double g = t * polevl(u, GN) / p1evl(u, GD);

  222.             t = Math.PI * 0.5 * x2;
  223.             final double c = Math.cos(t);
  224.             final double s = Math.sin(t);
  225.             t = Math.PI * x;
  226.             cc = 0.5 + (f * s - g * c) / t;
  227.             ss = 0.5 - (f * c + g * s) / t;
  228.         }

  229.         if (xxa < 0.0)
  230.         {
  231.             cc = -cc;
  232.             ss = -ss;
  233.         }
  234.         return new double[]{cc, ss};
  235.     }

  236.     /**
  237.      * Approximate one point of the "standard" spiral (curvature at start is 0).
  238.      * @param s run-length along spiral
  239.      * @param cDot first derivative of curvature [1/m2]
  240.      * @param initialCurvature double; curvature at start
  241.      * @return double[]; array of three double values containing x, y, and tangent direction
  242.      */
  243.     private static double[] odrSpiral(final double s, final double cDot, final double initialCurvature)
  244.     {
  245.         double a = Math.sqrt(Math.PI / Math.abs(cDot));

  246.         double[] xy = fresnel(initialCurvature + s / a);
  247.         return new double[]{xy[0] * a, xy[1] * a * Math.signum(cDot), s * s * cDot * 0.5};
  248.     }

  249.     /**
  250.      * Approximate a clothoid that starts in the x-direction.
  251.      * @param initialCurvature double; curvature at start
  252.      * @param curvatureDerivative double; rate of curvature change along the clothoid
  253.      * @param length double; total length of the clothoid
  254.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  255.      * @return OTSLine3D; the line; the z-component of each point is set to 0
  256.      * @throws OTSGeometryException if the number of segments is too low
  257.      */
  258.     private static OTSLine3D clothoid(final double initialCurvature, final double curvatureDerivative,
  259.         final double length, final int numSegments) throws OTSGeometryException
  260.     {
  261.         OTSPoint3D[] points = new OTSPoint3D[numSegments + 1];
  262.         double[] offset = odrSpiral(initialCurvature / curvatureDerivative, curvatureDerivative, initialCurvature);
  263.         double sinRot = Math.sin(offset[2]);
  264.         double cosRot = Math.cos(offset[2]);
  265.         for (int i = 0; i <= numSegments; i++)
  266.         {
  267.             double[] xyd =
  268.                 odrSpiral(i * length / numSegments + initialCurvature / curvatureDerivative, curvatureDerivative,
  269.                     initialCurvature);
  270.             double dx = xyd[0] - offset[0];
  271.             double dy = xyd[1] - offset[1];
  272.             points[i] = new OTSPoint3D(dx * cosRot + dy * sinRot, dy * cosRot - dx * sinRot, 0);
  273.         }
  274.         return new OTSLine3D(points);
  275.     }

  276.     /**
  277.      * Approximate a clothoid that starts at a given point in the given direction and curvature. Elevation is linearly
  278.      * interpolated over the length of the clothoid.
  279.      * @param x1 double; x-coordinate of the start point
  280.      * @param y1 double; y-coordinate of the start point
  281.      * @param startElevation double; z-component at start of the curve
  282.      * @param startDirection double; rotation in radians at the start of the curve
  283.      * @param startCurvature double; curvature at the start of the clothoid
  284.      * @param endCurvature double; curvature at the end of the clothoid
  285.      * @param length double; length of the clothoid
  286.      * @param endElevation double; z-component at end of the curve
  287.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  288.      * @return OTSLine3D; the clothoid
  289.      * @throws OTSGeometryException if the number of segments is too low
  290.      */
  291.     @SuppressWarnings("checkstyle:parameternumber")
  292.     private static OTSLine3D clothoid(final double x1, final double y1, final double startElevation,
  293.         final double startDirection, final double startCurvature, final double endCurvature, final double length,
  294.         final double endElevation, final int numSegments) throws OTSGeometryException
  295.     {
  296.         OTSLine3D result = clothoid(startCurvature, (endCurvature - startCurvature) / length, length, numSegments);
  297.         double sinRot = Math.sin(startDirection);
  298.         double cosRot = Math.cos(startDirection);
  299.         OTSPoint3D[] list = new OTSPoint3D[result.size()];
  300.         double elevationPerStep = (endElevation - startElevation) / (result.size() - 1);
  301.         for (int i = 0; i < result.size(); i++)
  302.         {
  303.             try
  304.             {
  305.                 OTSPoint3D p = result.get(i);
  306.                 list[i] =
  307.                     new OTSPoint3D(x1 + cosRot * p.x + sinRot * p.y, y1 + cosRot * p.y - sinRot * p.x, startElevation
  308.                         + i * elevationPerStep);
  309.             }
  310.             catch (OTSGeometryException ge)
  311.             {
  312.                 // cannot happen
  313.                 System.err.println("CANNOT HAPPEN; if you see this; let us know what you did.");
  314.             }
  315.         }
  316.         return new OTSLine3D(list);
  317.     }

  318.     /**
  319.      * Approximate a clothoid with curvature 0 at start.
  320.      * @param start OTSPoint3D; starting point of the clothoid
  321.      * @param startDirection Direction; start direction of the clothoid
  322.      * @param endCurvature double; curvature at the end of the clothoid [1/m]
  323.      * @param length Length; length of the clothoid
  324.      * @param endElevation Length; elevation at end of the clothoid
  325.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  326.      * @return OTSLine3D; the clothoid
  327.      * @throws OTSGeometryException if the number of segments is too low
  328.      */
  329.     public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection, final double endCurvature,
  330.         final Length length, final Length endElevation, final int numSegments) throws OTSGeometryException
  331.     {
  332.         return clothoid(start.x, start.y, start.z, startDirection.si, 0, endCurvature, length.si, endElevation.si,
  333.             numSegments);
  334.     }

  335.     /**
  336.      * Approximate a clothoid.
  337.      * @param start OTSPoint3D; starting point of the clothoid
  338.      * @param startDirection Direction; start direction of the clothoid
  339.      * @param startCurvature double; curvature at the start of the clothoid [1/m]
  340.      * @param endCurvature double; curvature at the end of the clothoid [1/m]
  341.      * @param length Length; length of the clothoid
  342.      * @param endElevation Length; elevation at end of the clothoid
  343.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  344.      * @return OTSLine3D; the clothoid
  345.      * @throws OTSGeometryException if the number of segments is too low
  346.      */
  347.     public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection,
  348.         final double startCurvature, final double endCurvature, final Length length, final Length endElevation,
  349.         final int numSegments) throws OTSGeometryException
  350.     {
  351.         return clothoid(start.x, start.y, start.x, startDirection.si, startCurvature, endCurvature, length.si,
  352.             endElevation.si, numSegments);
  353.     }

  354.     /**
  355.      * Approximate a clothoid with curvature 0 at start.
  356.      * @param start OTSPoint3D; starting point of the clothoid
  357.      * @param startDirection Direction; start direction of the clothoid
  358.      * @param endCurvature LinearDensity; curvature at the end of the clothoid
  359.      * @param length Length; length of the clothoid
  360.      * @param endElevation Length; elevation at end of the clothoid
  361.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  362.      * @return OTSLine3D; the clothoid
  363.      * @throws OTSGeometryException if the number of segments is too low
  364.      */
  365.     public static OTSLine3D
  366.         clothoid(final OTSPoint3D start, final Direction startDirection, final LinearDensity endCurvature,
  367.             final Length length, final Length endElevation, final int numSegments) throws OTSGeometryException
  368.     {
  369.         return clothoid(start, startDirection, 0, endCurvature.si, length, endElevation, numSegments);
  370.     }

  371.     /**
  372.      * Approximate a clothoid.
  373.      * @param start OTSPoint3D; starting point of the clothoid
  374.      * @param startDirection Direction; start direction of the clothoid
  375.      * @param startCurvature double; curvature at the start of the clothoid [1/m]
  376.      * @param endCurvature double; curvature at the end of the clothoid [1/m]
  377.      * @param length Length; length of the clothoid
  378.      * @param endElevation Length; elevation at end of the clothoid
  379.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  380.      * @return OTSLine3D; the clothoid
  381.      * @throws OTSGeometryException if the number of segments is too low
  382.      */
  383.     public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection,
  384.         final LinearDensity startCurvature, final LinearDensity endCurvature, final Length length,
  385.         final Length endElevation, final int numSegments) throws OTSGeometryException
  386.     {
  387.         return clothoid(start, startDirection, startCurvature.si, endCurvature.si, length, endElevation, numSegments);
  388.     }

  389.     /**
  390.      * Demonstrate / test the clothoid methods.
  391.      * @param args String[]; the command line arguments (not used)
  392.      * @throws OTSGeometryException if the number of segments is too low
  393.      */
  394.     public static void main(final String[] args) throws OTSGeometryException
  395.     {
  396.         OTSLine3D line;
  397.         // line = clothoid(104.1485, 89.037488, 0, 0, 0, -0.04841457, 0, 3.2, 100);
  398.         // System.out.println(line.toPlotterFormat());
  399.         // line = clothoid(10, 10, 5, Math.PI / 8, 0 * -0.03, 0.04, 100, 15, 100);
  400.         line =
  401.             clothoid(new OTSPoint3D(10, 10, 5), new Direction(Math.PI / 8, AngleUnit.RADIAN), new LinearDensity(
  402.                 0 * -0.03, LinearDensityUnit.PER_METER), new LinearDensity(0.04, LinearDensityUnit.PER_METER),
  403.                 new Length(100, LengthUnit.METER), new Length(15, LengthUnit.METER), 100);
  404.         System.out.println(OTSGeometryUtil.printCoordinates("#", line, "\n"));
  405.     }

  406. }