Clothoid.java
package org.opentrafficsim.core.geometry;
import org.djunits.unit.AngleUnit;
import org.djunits.unit.LengthUnit;
import org.djunits.unit.LinearDensityUnit;
import org.djunits.value.vdouble.scalar.Direction;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.LinearDensity;
/**
* Generate an OTSLine3D for a clothoid. <br>
* Derived from odrSpiral.c by M. Dupuis @ VIRES GmbH <br>
*
* <pre>
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
* </pre>
* <p>
* Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
* BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
* <p>
* @version $Revision$, $LastChangedDate$, by $Author$, initial version Nov 2, 2015 <br>
* @author M. Dupuis @ VIRES GmbH
* @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
* @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
*/
public final class Clothoid
{
/** Utility class. */
private Clothoid()
{
// do not instantiate
}
/*- ===================================================
* file: odrSpiral.c
* ---------------------------------------------------
* purpose: free method for computing spirals
* in OpenDRIVE applications
* ---------------------------------------------------
* using methods of CEPHES library
* ---------------------------------------------------
* first edit: 09.03.2010 by M. Dupuis @ VIRES GmbH
* last mod.: 09.03.2010 by M. Dupuis @ VIRES GmbH
* ===================================================
Copyright 2010 VIRES Simulationstechnologie GmbH
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
NOTE:
The methods have been realized using the CEPHES library
http://www.netlib.org/cephes/
and do neither constitute the only nor the exclusive way of implementing
spirals for OpenDRIVE applications. Their sole purpose is to facilitate
the interpretation of OpenDRIVE spiral data.
*/
//@formatter:off
/** S(x) for small x numerator. */
static final double[] SN = {
-2.99181919401019853726E3,
7.08840045257738576863E5,
-6.29741486205862506537E7,
2.54890880573376359104E9,
-4.42979518059697779103E10,
3.18016297876567817986E11
};
/** S(x) for small x denominator. */
static final double[] SD = {
2.81376268889994315696E2,
4.55847810806532581675E4,
5.17343888770096400730E6,
4.19320245898111231129E8,
2.24411795645340920940E10,
6.07366389490084639049E11
};
/** C(x) for small x numerator. */
static final double[] CN = {
-4.98843114573573548651E-8,
9.50428062829859605134E-6,
-6.45191435683965050962E-4,
1.88843319396703850064E-2,
-2.05525900955013891793E-1,
9.99999999999999998822E-1
};
/** C(x) for small x denominator. */
static final double[] CD = {
3.99982968972495980367E-12,
9.15439215774657478799E-10,
1.25001862479598821474E-7,
1.22262789024179030997E-5,
8.68029542941784300606E-4,
4.12142090722199792936E-2,
1.00000000000000000118E0
};
/** Auxiliary function f(x) numerator. */
static final double[] FN = {
4.21543555043677546506E-1,
1.43407919780758885261E-1,
1.15220955073585758835E-2,
3.45017939782574027900E-4,
4.63613749287867322088E-6,
3.05568983790257605827E-8,
1.02304514164907233465E-10,
1.72010743268161828879E-13,
1.34283276233062758925E-16,
3.76329711269987889006E-20
};
/** Auxiliary function f(x) denominator. */
static final double[] FD = {
7.51586398353378947175E-1,
1.16888925859191382142E-1,
6.44051526508858611005E-3,
1.55934409164153020873E-4,
1.84627567348930545870E-6,
1.12699224763999035261E-8,
3.60140029589371370404E-11,
5.88754533621578410010E-14,
4.52001434074129701496E-17,
1.25443237090011264384E-20
};
/** Auxiliary function g(x) numerator. */
static final double[] GN = {
5.04442073643383265887E-1,
1.97102833525523411709E-1,
1.87648584092575249293E-2,
6.84079380915393090172E-4,
1.15138826111884280931E-5,
9.82852443688422223854E-8,
4.45344415861750144738E-10,
1.08268041139020870318E-12,
1.37555460633261799868E-15,
8.36354435630677421531E-19,
1.86958710162783235106E-22
};
/** Auxiliary function g(x) denominator. */
static final double[] GD = {
1.47495759925128324529E0,
3.37748989120019970451E-1,
2.53603741420338795122E-2,
8.14679107184306179049E-4,
1.27545075667729118702E-5,
1.04314589657571990585E-7,
4.60680728146520428211E-10,
1.10273215066240270757E-12,
1.38796531259578871258E-15,
8.39158816283118707363E-19,
1.86958710162783236342E-22
};
//@formatter:on
/**
* Compute a polynomial in x.
* @param x double; x
* @param coef double[]; coefficients
* @return polynomial in x
*/
private static double polevl(final double x, final double[] coef)
{
double result = coef[0];
for (int i = 0; i < coef.length; i++)
{
result = result * x + coef[i];
}
return result;
}
/**
* Compute a polynomial in x.
* @param x double; x
* @param coef double[]; coefficients
* @return polynomial in x
*/
private static double p1evl(final double x, final double[] coef)
{
double result = x + coef[0];
for (int i = 0; i < coef.length; i++)
{
result = result * x + coef[i];
}
return result;
}
/**
* Approximate the Fresnel function.
* @param xxa double; the xxa parameter
* @return double[]; array with two double values c and s
*/
private static double[] fresnel(final double xxa)
{
final double x = Math.abs(xxa);
final double x2 = x * x;
double cc, ss;
if (x2 < 2.5625)
{
final double t = x2 * x2;
ss = x * x2 * polevl(t, SN) / p1evl(t, SD);
cc = x * polevl(t, CN) / polevl(t, CD);
}
else if (x > 36974.0)
{
cc = 0.5;
ss = 0.5;
}
else
{
double t = Math.PI * x2;
final double u = 1.0 / (t * t);
t = 1.0 / t;
final double f = 1.0 - u * polevl(u, FN) / p1evl(u, FD);
final double g = t * polevl(u, GN) / p1evl(u, GD);
t = Math.PI * 0.5 * x2;
final double c = Math.cos(t);
final double s = Math.sin(t);
t = Math.PI * x;
cc = 0.5 + (f * s - g * c) / t;
ss = 0.5 - (f * c + g * s) / t;
}
if (xxa < 0.0)
{
cc = -cc;
ss = -ss;
}
return new double[]{cc, ss};
}
/**
* Approximate one point of the "standard" spiral (curvature at start is 0).
* @param s run-length along spiral
* @param cDot first derivative of curvature [1/m2]
* @param initialCurvature double; curvature at start
* @return double[]; array of three double values containing x, y, and tangent direction
*/
private static double[] odrSpiral(final double s, final double cDot, final double initialCurvature)
{
double a = Math.sqrt(Math.PI / Math.abs(cDot));
double[] xy = fresnel(initialCurvature + s / a);
return new double[]{xy[0] * a, xy[1] * a * Math.signum(cDot), s * s * cDot * 0.5};
}
/**
* Approximate a clothoid that starts in the x-direction.
* @param initialCurvature double; curvature at start
* @param curvatureDerivative double; rate of curvature change along the clothoid
* @param length double; total length of the clothoid
* @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
* @return OTSLine3D; the line; the z-component of each point is set to 0
* @throws OTSGeometryException if the number of segments is too low
*/
private static OTSLine3D clothoid(final double initialCurvature, final double curvatureDerivative,
final double length, final int numSegments) throws OTSGeometryException
{
OTSPoint3D[] points = new OTSPoint3D[numSegments + 1];
double[] offset = odrSpiral(initialCurvature / curvatureDerivative, curvatureDerivative, initialCurvature);
double sinRot = Math.sin(offset[2]);
double cosRot = Math.cos(offset[2]);
for (int i = 0; i <= numSegments; i++)
{
double[] xyd =
odrSpiral(i * length / numSegments + initialCurvature / curvatureDerivative, curvatureDerivative,
initialCurvature);
double dx = xyd[0] - offset[0];
double dy = xyd[1] - offset[1];
points[i] = new OTSPoint3D(dx * cosRot + dy * sinRot, dy * cosRot - dx * sinRot, 0);
}
return new OTSLine3D(points);
}
/**
* Approximate a clothoid that starts at a given point in the given direction and curvature. Elevation is linearly
* interpolated over the length of the clothoid.
* @param x1 double; x-coordinate of the start point
* @param y1 double; y-coordinate of the start point
* @param startElevation double; z-component at start of the curve
* @param startDirection double; rotation in radians at the start of the curve
* @param startCurvature double; curvature at the start of the clothoid
* @param endCurvature double; curvature at the end of the clothoid
* @param length double; length of the clothoid
* @param endElevation double; z-component at end of the curve
* @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
* @return OTSLine3D; the clothoid
* @throws OTSGeometryException if the number of segments is too low
*/
@SuppressWarnings("checkstyle:parameternumber")
private static OTSLine3D clothoid(final double x1, final double y1, final double startElevation,
final double startDirection, final double startCurvature, final double endCurvature, final double length,
final double endElevation, final int numSegments) throws OTSGeometryException
{
OTSLine3D result = clothoid(startCurvature, (endCurvature - startCurvature) / length, length, numSegments);
double sinRot = Math.sin(startDirection);
double cosRot = Math.cos(startDirection);
OTSPoint3D[] list = new OTSPoint3D[result.size()];
double elevationPerStep = (endElevation - startElevation) / (result.size() - 1);
for (int i = 0; i < result.size(); i++)
{
try
{
OTSPoint3D p = result.get(i);
list[i] =
new OTSPoint3D(x1 + cosRot * p.x + sinRot * p.y, y1 + cosRot * p.y - sinRot * p.x, startElevation
+ i * elevationPerStep);
}
catch (OTSGeometryException ge)
{
// cannot happen
System.err.println("CANNOT HAPPEN; if you see this; let us know what you did.");
}
}
return new OTSLine3D(list);
}
/**
* Approximate a clothoid with curvature 0 at start.
* @param start OTSPoint3D; starting point of the clothoid
* @param startDirection Direction; start direction of the clothoid
* @param endCurvature double; curvature at the end of the clothoid [1/m]
* @param length Length; length of the clothoid
* @param endElevation Length; elevation at end of the clothoid
* @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
* @return OTSLine3D; the clothoid
* @throws OTSGeometryException if the number of segments is too low
*/
public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection, final double endCurvature,
final Length length, final Length endElevation, final int numSegments) throws OTSGeometryException
{
return clothoid(start.x, start.y, start.z, startDirection.si, 0, endCurvature, length.si, endElevation.si,
numSegments);
}
/**
* Approximate a clothoid.
* @param start OTSPoint3D; starting point of the clothoid
* @param startDirection Direction; start direction of the clothoid
* @param startCurvature double; curvature at the start of the clothoid [1/m]
* @param endCurvature double; curvature at the end of the clothoid [1/m]
* @param length Length; length of the clothoid
* @param endElevation Length; elevation at end of the clothoid
* @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
* @return OTSLine3D; the clothoid
* @throws OTSGeometryException if the number of segments is too low
*/
public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection,
final double startCurvature, final double endCurvature, final Length length, final Length endElevation,
final int numSegments) throws OTSGeometryException
{
return clothoid(start.x, start.y, start.x, startDirection.si, startCurvature, endCurvature, length.si,
endElevation.si, numSegments);
}
/**
* Approximate a clothoid with curvature 0 at start.
* @param start OTSPoint3D; starting point of the clothoid
* @param startDirection Direction; start direction of the clothoid
* @param endCurvature LinearDensity; curvature at the end of the clothoid
* @param length Length; length of the clothoid
* @param endElevation Length; elevation at end of the clothoid
* @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
* @return OTSLine3D; the clothoid
* @throws OTSGeometryException if the number of segments is too low
*/
public static OTSLine3D
clothoid(final OTSPoint3D start, final Direction startDirection, final LinearDensity endCurvature,
final Length length, final Length endElevation, final int numSegments) throws OTSGeometryException
{
return clothoid(start, startDirection, 0, endCurvature.si, length, endElevation, numSegments);
}
/**
* Approximate a clothoid.
* @param start OTSPoint3D; starting point of the clothoid
* @param startDirection Direction; start direction of the clothoid
* @param startCurvature double; curvature at the start of the clothoid [1/m]
* @param endCurvature double; curvature at the end of the clothoid [1/m]
* @param length Length; length of the clothoid
* @param endElevation Length; elevation at end of the clothoid
* @param numSegments int; number of segments used to approximate (the number of points is one higher than this)
* @return OTSLine3D; the clothoid
* @throws OTSGeometryException if the number of segments is too low
*/
public static OTSLine3D clothoid(final OTSPoint3D start, final Direction startDirection,
final LinearDensity startCurvature, final LinearDensity endCurvature, final Length length,
final Length endElevation, final int numSegments) throws OTSGeometryException
{
return clothoid(start, startDirection, startCurvature.si, endCurvature.si, length, endElevation, numSegments);
}
/**
* Demonstrate / test the clothoid methods.
* @param args String[]; the command line arguments (not used)
* @throws OTSGeometryException if the number of segments is too low
*/
public static void main(final String[] args) throws OTSGeometryException
{
OTSLine3D line;
// line = clothoid(104.1485, 89.037488, 0, 0, 0, -0.04841457, 0, 3.2, 100);
// System.out.println(line.toPlotterFormat());
// line = clothoid(10, 10, 5, Math.PI / 8, 0 * -0.03, 0.04, 100, 15, 100);
line =
clothoid(new OTSPoint3D(10, 10, 5), new Direction(Math.PI / 8, AngleUnit.RADIAN), new LinearDensity(
0 * -0.03, LinearDensityUnit.PER_METER), new LinearDensity(0.04, LinearDensityUnit.PER_METER),
new Length(100, LengthUnit.METER), new Length(15, LengthUnit.METER), 100);
System.out.println(OTSGeometryUtil.printCoordinates("#", line, "\n"));
}
}