1 package org.opentrafficsim.core.geometry;
2
3 import java.util.ArrayList;
4 import java.util.List;
5 import java.util.Locale;
6 import java.util.stream.Collectors;
7 import java.util.stream.DoubleStream;
8
9 import org.djunits.value.vdouble.scalar.Angle;
10 import org.djutils.draw.line.PolyLine2d;
11 import org.djutils.draw.point.OrientedPoint2d;
12 import org.djutils.draw.point.Point2d;
13 import org.djutils.exceptions.Throw;
14
15
16
17
18
19
20
21
22
23
24
25 public final class OtsGeometryUtil
26 {
27
28 private OtsGeometryUtil()
29 {
30
31 }
32
33
34
35
36
37
38
39 public static String printCoordinate(final String prefix, final Point2d point)
40 {
41 return String.format(Locale.US, "%s %8.3f,%8.3f ", prefix, point.x, point.y);
42 }
43
44
45
46
47
48
49
50
51 public static String printCoordinates(final String prefix, final Point2d[] coordinates, final String separator)
52 {
53 return printCoordinates(prefix + "(" + coordinates.length + " pts)", coordinates, 0, coordinates.length, separator);
54 }
55
56
57
58
59
60
61
62
63 public static String printCoordinates(final String prefix, final OtsLine2d line, final String separator)
64 {
65 return printCoordinates(prefix + "(" + line.size() + " pts)", line.getPoints(), 0, line.size(), separator);
66 }
67
68
69
70
71
72
73
74
75
76
77 public static String printCoordinates(final String prefix, final Point2d[] points, final int fromIndex, final int toIndex,
78 final String separator)
79 {
80 StringBuilder result = new StringBuilder();
81 result.append(prefix);
82 String operator = "M";
83 for (int i = fromIndex; i < toIndex; i++)
84 {
85 result.append(separator);
86 result.append(printCoordinate(operator, points[i]));
87 operator = "L";
88 }
89 return result.toString();
90 }
91
92
93
94
95
96
97
98
99 public static int getNumSegmentsForRadius(final double maxSpatialError, final Angle angle, final double r)
100 {
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115 return (int) Math.ceil(angle.si / (2.0 * Math.acos(1.0 - maxSpatialError / r)));
116 }
117
118
119
120
121
122
123
124
125 public static OrientedPoint2d offsetPoint(final OrientedPoint2d point, final double offset)
126 {
127 return new OrientedPoint2d(point.x - Math.sin(point.dirZ) * offset, point.y + Math.cos(point.dirZ) * offset,
128 point.dirZ);
129 }
130
131
132
133
134
135
136
137
138
139
140
141
142 public static final PolyLine2d offsetLine(final PolyLine2d line, final double[] relativeFractions, final double[] offsets)
143 throws OtsGeometryException
144 {
145 Throw.whenNull(relativeFractions, "relativeFraction may not be null");
146 Throw.whenNull(offsets, "offsets may not be null");
147 Throw.when(relativeFractions.length < 2, OtsGeometryException.class, "size of relativeFractions must be >= 2");
148 Throw.when(relativeFractions.length != offsets.length, OtsGeometryException.class,
149 "size of relativeFractions must be equal to size of offsets");
150 Throw.when(relativeFractions[0] < 0, OtsGeometryException.class, "relativeFractions may not start before 0");
151 Throw.when(relativeFractions[relativeFractions.length - 1] > 1, OtsGeometryException.class,
152 "relativeFractions may not end beyond 1");
153 List<Double> fractionsList = DoubleStream.of(relativeFractions).boxed().collect(Collectors.toList());
154 List<Double> offsetsList = DoubleStream.of(offsets).boxed().collect(Collectors.toList());
155 if (relativeFractions[0] != 0)
156 {
157 fractionsList.add(0, 0.0);
158 offsetsList.add(0, 0.0);
159 }
160 if (relativeFractions[relativeFractions.length - 1] < 1.0)
161 {
162 fractionsList.add(1.0);
163 offsetsList.add(0.0);
164 }
165 PolyLine2d[] offsetLine = new PolyLine2d[fractionsList.size()];
166 for (int i = 0; i < fractionsList.size(); i++)
167 {
168 offsetLine[i] = line.offsetLine(offsetsList.get(i));
169 }
170 List<Point2d> out = new ArrayList<>();
171 Point2d prevCoordinate = null;
172 final double tooClose = 0.05;
173
174 for (int i = 0; i < offsetsList.size() - 1; i++)
175 {
176 Throw.when(fractionsList.get(i + 1) <= fractionsList.get(i), OtsGeometryException.class,
177 "fractions must be in ascending order");
178 PolyLine2d startGeometry = offsetLine[i].extractFractional(fractionsList.get(i), fractionsList.get(i + 1));
179 PolyLine2d endGeometry = offsetLine[i + 1].extractFractional(fractionsList.get(i), fractionsList.get(i + 1));
180 double firstLength = startGeometry.getLength();
181 double secondLength = endGeometry.getLength();
182 int firstIndex = 0;
183 int secondIndex = 0;
184 while (firstIndex < startGeometry.size() && secondIndex < endGeometry.size())
185 {
186 double firstRatio = firstIndex < startGeometry.size() ? startGeometry.lengthAtIndex(firstIndex) / firstLength
187 : Double.MAX_VALUE;
188 double secondRatio = secondIndex < endGeometry.size() ? endGeometry.lengthAtIndex(secondIndex) / secondLength
189 : Double.MAX_VALUE;
190 double ratio;
191 if (firstRatio < secondRatio)
192 {
193 ratio = firstRatio;
194 firstIndex++;
195 }
196 else
197 {
198 ratio = secondRatio;
199 secondIndex++;
200 }
201 Point2d firstCoordinate = startGeometry.getLocation(ratio * firstLength);
202 Point2d secondCoordinate = endGeometry.getLocation(ratio * secondLength);
203 Point2d resultCoordinate = new Point2d((1 - ratio) * firstCoordinate.x + ratio * secondCoordinate.x,
204 (1 - ratio) * firstCoordinate.y + ratio * secondCoordinate.y);
205 if (null == prevCoordinate || resultCoordinate.distance(prevCoordinate) > tooClose)
206 {
207 out.add(resultCoordinate);
208 prevCoordinate = resultCoordinate;
209 }
210 }
211 }
212 return new PolyLine2d(out.toArray(new Point2d[out.size()]));
213 }
214
215 }