Clothoid.java

  1. package org.opentrafficsim.core.geometry;

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

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

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

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

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

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

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

  75.         and do neither constitute the only nor the exclusive way of implementing 
  76.         spirals for OpenDRIVE applications. Their sole purpose is to facilitate 
  77.         the interpretation of OpenDRIVE spiral data.
  78.      */

  80.     //@formatter:off
  81.     /** S(x) for small x numerator. */
  82.     static final double[] SN = {
  83.         -2.99181919401019853726E3,
  84.         7.08840045257738576863E5,
  85.         -6.29741486205862506537E7,
  86.         2.54890880573376359104E9,
  87.         -4.42979518059697779103E10,
  88.         3.18016297876567817986E11 
  89.     };

  91.     /** S(x) for small x denominator. */
  92.     static final double[] SD = {
  93.         2.81376268889994315696E2, 
  94.         4.55847810806532581675E4, 
  95.         5.17343888770096400730E6, 
  96.         4.19320245898111231129E8,
  97.         2.24411795645340920940E10, 
  98.         6.07366389490084639049E11
  99.     };

  101.     /** C(x) for small x numerator. */
  102.     static final double[] CN = {
  103.         -4.98843114573573548651E-8, 
  104.         9.50428062829859605134E-6, 
  105.         -6.45191435683965050962E-4,
  106.         1.88843319396703850064E-2, 
  107.         -2.05525900955013891793E-1, 
  108.         9.99999999999999998822E-1
  109.     };

  111.     /** C(x) for small x denominator. */
  112.     static final double[] CD = {
  113.         3.99982968972495980367E-12, 
  114.         9.15439215774657478799E-10, 
  115.         1.25001862479598821474E-7,
  116.         1.22262789024179030997E-5, 
  117.         8.68029542941784300606E-4, 
  118.         4.12142090722199792936E-2, 
  119.         1.00000000000000000118E0 
  120.     };

  122.     /** Auxiliary function f(x) numerator. */
  123.     static final double[] FN = {
  124.         4.21543555043677546506E-1, 
  125.         1.43407919780758885261E-1, 
  126.         1.15220955073585758835E-2,
  127.         3.45017939782574027900E-4, 
  128.         4.63613749287867322088E-6, 
  129.         3.05568983790257605827E-8, 
  130.         1.02304514164907233465E-10,
  131.         1.72010743268161828879E-13, 
  132.         1.34283276233062758925E-16, 
  133.         3.76329711269987889006E-20
  134.     };

  136.     /** Auxiliary function f(x) denominator. */
  137.     static final double[] FD = {
  138.         7.51586398353378947175E-1, 
  139.         1.16888925859191382142E-1, 
  140.         6.44051526508858611005E-3, 
  141.         1.55934409164153020873E-4,
  142.         1.84627567348930545870E-6, 
  143.         1.12699224763999035261E-8, 
  144.         3.60140029589371370404E-11, 
  145.         5.88754533621578410010E-14,
  146.         4.52001434074129701496E-17, 
  147.         1.25443237090011264384E-20 
  148.     };

  150.     /** Auxiliary function g(x) numerator. */
  151.     static final double[] GN = {
  152.         5.04442073643383265887E-1, 
  153.         1.97102833525523411709E-1, 
  154.         1.87648584092575249293E-2,
  155.         6.84079380915393090172E-4, 
  156.         1.15138826111884280931E-5, 
  157.         9.82852443688422223854E-8, 
  158.         4.45344415861750144738E-10,
  159.         1.08268041139020870318E-12, 
  160.         1.37555460633261799868E-15, 
  161.         8.36354435630677421531E-19, 
  162.         1.86958710162783235106E-22
  163.     };

  165.     /** Auxiliary function g(x) denominator. */
  166.     static final double[] GD = {
  167.         1.47495759925128324529E0, 
  168.         3.37748989120019970451E-1, 
  169.         2.53603741420338795122E-2, 
  170.         8.14679107184306179049E-4,
  171.         1.27545075667729118702E-5, 
  172.         1.04314589657571990585E-7, 
  173.         4.60680728146520428211E-10, 
  174.         1.10273215066240270757E-12,
  175.         1.38796531259578871258E-15, 
  176.         8.39158816283118707363E-19, 
  177.         1.86958710162783236342E-22
  178.     };
  179.     //@formatter:on

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

  197.     /**
  198.      * Compute a polynomial in x.
  199.      * @param x double; x
  200.      * @param coef double[]; coefficients
  201.      * @return polynomial in x
  202.      */
  203.     private static double p1evl(final double x, final double[] coef)
  204.     {
  205.         double result = x + coef[0];
  206.         for (int i = 0; i < coef.length; i++)
  207.         {
  208.             result = result * x + coef[i];
  209.         }
  210.         return result;
  211.     }

  213.     /**
  214.      * Approximate the Fresnel function.
  215.      * @param xxa double; the xxa parameter
  216.      * @return double[]; array with two double values c and s
  217.      */
  218.     private static double[] fresnel(final double xxa)
  219.     {
  220.         final double x = Math.abs(xxa);
  221.         final double x2 = x * x;
  222.         double cc, ss;

  224.         if (x2 < 2.5625)
  225.         {
  226.             final double t = x2 * x2;
  227.             ss = x * x2 * polevl(t, SN) / p1evl(t, SD);
  228.             cc = x * polevl(t, CN) / polevl(t, CD);
  229.         }
  230.         else if (x > 36974.0)
  231.         {
  232.             cc = 0.5;
  233.             ss = 0.5;
  234.         }
  235.         else
  236.         {
  237.             double t = Math.PI * x2;
  238.             final double u = 1.0 / (t * t);
  239.             t = 1.0 / t;
  240.             final double f = 1.0 - u * polevl(u, FN) / p1evl(u, FD);
  241.             final double g = t * polevl(u, GN) / p1evl(u, GD);

  243.             t = Math.PI * 0.5 * x2;
  244.             final double c = Math.cos(t);
  245.             final double s = Math.sin(t);
  246.             t = Math.PI * x;
  247.             cc = 0.5 + (f * s - g * c) / t;
  248.             ss = 0.5 - (f * c + g * s) / t;
  249.         }

  251.         if (xxa < 0.0)
  252.         {
  253.             cc = -cc;
  254.             ss = -ss;
  255.         }
  256.         return new double[]{cc, ss};
  257.     }

  259.     /**
  260.      * Approximate one point of the "standard" spiral (curvature at start is 0).
  261.      * @param s run-length along spiral
  262.      * @param cDot first derivative of curvature [1/m2]
  263.      * @param initialCurvature double; curvature at start
  264.      * @return double[]; array of three double values containing x, y, and tangent direction
  265.      */
  266.     private static double[] odrSpiral(final double s, final double cDot, final double initialCurvature)
  267.     {
  268.         double a = Math.sqrt(Math.PI / Math.abs(cDot));

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

  274.     /**
  275.      * Approximate a clothoid that starts in the x-direction.
  276.      * @param initialCurvature double; curvature at start
  277.      * @param curvatureDerivative double; rate of curvature change along the clothoid
  278.      * @param length double; total length of the clothoid
  279.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  280.      * @return OTSLine3D; the line; the z-component of each point is set to 0
  281.      * @throws OTSGeometryException if the number of segments is too low
  282.      */
  283.     private static OTSLine3D clothoid(final double initialCurvature, final double curvatureDerivative,
  284.         final double length, final int numSegments) throws OTSGeometryException
  285.     {
  286.         OTSPoint3D[] points = new OTSPoint3D[numSegments + 1];
  287.         double[] offset = odrSpiral(initialCurvature / curvatureDerivative, curvatureDerivative, initialCurvature);
  288.         double sinRot = Math.sin(offset[2]);
  289.         double cosRot = Math.cos(offset[2]);
  290.         for (int i = 0; i <= numSegments; i++)
  291.         {
  292.             double[] xyd =
  293.                 odrSpiral(i * length / numSegments + initialCurvature / curvatureDerivative, curvatureDerivative,
  294.                     initialCurvature);
  295.             double dx = xyd[0] - offset[0];
  296.             double dy = xyd[1] - offset[1];
  297.             points[i] = new OTSPoint3D(dx * cosRot + dy * sinRot, dy * cosRot - dx * sinRot, 0);
  298.         }
  299.         return new OTSLine3D(points);
  300.     }

  302.     /**
  303.      * Approximate a clothoid that starts at a given point in the given direction and curvature. Elevation is linearly
  304.      * interpolated over the length of the clothoid.
  305.      * @param x1 double; x-coordinate of the start point
  306.      * @param y1 double; y-coordinate of the start point
  307.      * @param startElevation double; z-component at start of the curve
  308.      * @param startDirection double; rotation in radians at the start of the curve
  309.      * @param startCurvature double; curvature at the start of the clothoid
  310.      * @param endCurvature double; curvature at the end of the clothoid
  311.      * @param length double; length of the clothoid
  312.      * @param endElevation double; z-component at end of the curve
  313.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  314.      * @return OTSLine3D; the clothoid
  315.      * @throws OTSGeometryException if the number of segments is too low
  316.      */
  317.     @SuppressWarnings("checkstyle:parameternumber")
  318.     private static OTSLine3D clothoid(final double x1, final double y1, final double startElevation,
  319.         final double startDirection, final double startCurvature, final double endCurvature, final double length,
  320.         final double endElevation, final int numSegments) throws OTSGeometryException
  321.     {
  322.         OTSLine3D result = clothoid(startCurvature, (endCurvature - startCurvature) / length, length, numSegments);
  323.         double sinRot = Math.sin(startDirection);
  324.         double cosRot = Math.cos(startDirection);
  325.         OTSPoint3D[] list = new OTSPoint3D[result.size()];
  326.         double elevationPerStep = (endElevation - startElevation) / (result.size() - 1);
  327.         for (int i = 0; i < result.size(); i++)
  328.         {
  329.             try
  330.             {
  331.                 OTSPoint3D p = result.get(i);
  332.                 list[i] =
  333.                     new OTSPoint3D(x1 + cosRot * p.x + sinRot * p.y, y1 + cosRot * p.y - sinRot * p.x, startElevation
  334.                         + i * elevationPerStep);
  335.             }
  336.             catch (OTSGeometryException ge)
  337.             {
  338.                 // cannot happen
  339.                 System.err.println("CANNOT HAPPEN; if you see this; let us know what you did.");
  340.             }
  341.         }
  342.         return new OTSLine3D(list);
  343.     }

  345.     /**
  346.      * Approximate a clothoid with curvature 0 at start.
  347.      * @param start OTSPoint3D; starting point of the clothoid
  348.      * @param startDirection Direction; start direction of the clothoid
  349.      * @param endCurvature double; curvature at the end of the clothoid [1/m]
  350.      * @param length Length; length of the clothoid
  351.      * @param endElevation Length; elevation at end of the clothoid
  352.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  353.      * @return OTSLine3D; the clothoid
  354.      * @throws OTSGeometryException if the number of segments is too low
  355.      */
  356.     public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection, final double endCurvature,
  357.         final Length length, final Length endElevation, final int numSegments) throws OTSGeometryException
  358.     {
  359.         return clothoid(start.x, start.y, start.z, startDirection.si, 0, endCurvature, length.si, endElevation.si,
  360.             numSegments);
  361.     }

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

  383.     /**
  384.      * Approximate a clothoid with curvature 0 at start.
  385.      * @param start OTSPoint3D; starting point of the clothoid
  386.      * @param startDirection Direction; start direction of the clothoid
  387.      * @param endCurvature LinearDensity; curvature at the end of the clothoid
  388.      * @param length Length; length of the clothoid
  389.      * @param endElevation Length; elevation at end of the clothoid
  390.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  391.      * @return OTSLine3D; the clothoid
  392.      * @throws OTSGeometryException if the number of segments is too low
  393.      */
  394.     public static OTSLine3D
  395.         clothoid(final OTSPoint3D start, final Direction startDirection, final LinearDensity endCurvature,
  396.             final Length length, final Length endElevation, final int numSegments) throws OTSGeometryException
  397.     {
  398.         return clothoid(start, startDirection, 0, endCurvature.si, length, endElevation, numSegments);
  399.     }

  401.     /**
  402.      * Approximate a clothoid.
  403.      * @param start OTSPoint3D; starting point of the clothoid
  404.      * @param startDirection Direction; start direction of the clothoid
  405.      * @param startCurvature double; curvature at the start of the clothoid [1/m]
  406.      * @param endCurvature double; curvature at the end of the clothoid [1/m]
  407.      * @param length Length; length of the clothoid
  408.      * @param endElevation Length; elevation at end of the clothoid
  409.      * @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
  410.      * @return OTSLine3D; the clothoid
  411.      * @throws OTSGeometryException if the number of segments is too low
  412.      */
  413.     public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection,
  414.         final LinearDensity startCurvature, final LinearDensity endCurvature, final Length length,
  415.         final Length endElevation, final int numSegments) throws OTSGeometryException
  416.     {
  417.         return clothoid(start, startDirection, startCurvature.si, endCurvature.si, length, endElevation, numSegments);
  418.     }

  420.     /**
  421.      * Demonstrate / test the clothoid methods.
  422.      * @param args String[]; the command line arguments (not used)
  423.      * @throws OTSGeometryException if the number of segments is too low
  424.      */
  425.     public static void main(final String[] args) throws OTSGeometryException
  426.     {
  427.         OTSLine3D line;
  428.         // line = clothoid(104.1485, 89.037488, 0, 0, 0, -0.04841457, 0, 3.2, 100);
  429.         // System.out.println(line.toPlotterFormat());
  430.         // line = clothoid(10, 10, 5, Math.PI / 8, 0 * -0.03, 0.04, 100, 15, 100);
  431.         line =
  432.             clothoid(new OTSPoint3D(10, 10, 5), new Direction(Math.PI / 8, AngleUnit.RADIAN), new LinearDensity(
  433.                 0 * -0.03, LinearDensityUnit.PER_METER), new LinearDensity(0.04, LinearDensityUnit.PER_METER),
  434.                 new Length(100, LengthUnit.METER), new Length(15, LengthUnit.METER), 100);
  435.         System.out.println(OTSGeometryUtil.printCoordinates("#", line, "\n"));
  436.     }

  438. }
Created with JaCoCo 0.7.5.201505241946