PositionVector
GeomHelper::makeRing(const double radius1, const double radius2, const Position& center, unsigned int nPoints) {
if (nPoints < 3) {
WRITE_ERROR("GeomHelper::makeRing() requires nPoints>=3");
}
if (radius1 >= radius2) {
WRITE_ERROR("GeomHelper::makeRing() requires radius2>radius1");
}
PositionVector ring;
ring.push_back({radius1, 0});
ring.push_back({radius2, 0});
for (unsigned int i = 1; i < nPoints; ++i) {
const double a = 2.0 * M_PI * (double)i / (double) nPoints;
ring.push_back({radius2 * cos(a), radius2 * sin(a)});
}
ring.push_back({radius2, 0});
ring.push_back({radius1, 0});
for (unsigned int i = 1; i < nPoints; ++i) {
const double a = -2.0 * M_PI * (double)i / (double) nPoints;
ring.push_back({radius1 * cos(a), radius1 * sin(a)});
}
ring.push_back({radius1, 0});
ring.add(center);
return ring;
}
PositionVector
GeomHelper::makeCircle(const double radius, const Position& center, unsigned int nPoints) {
if (nPoints < 3) {
WRITE_ERROR("GeomHelper::makeCircle() requires nPoints>=3");
}
PositionVector circle;
circle.push_back({radius, 0});
for (unsigned int i = 1; i < nPoints; ++i) {
const double a = 2.0 * M_PI * (double)i / (double) nPoints;
circle.push_back({radius * cos(a), radius * sin(a)});
}
circle.push_back({radius, 0});
circle.add(center);
return circle;
}
bool
NWWriter_OpenDrive::writeGeomSmooth(const PositionVector& shape, double speed, OutputDevice& device, OutputDevice& elevationDevice, double straightThresh, double& length) {
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << "writeGeomSmooth\n n=" << shape.size() << " shape=" << toString(shape) << "\n";
}
#endif
bool ok = true;
const double longThresh = speed; // 16.0; // make user-configurable (should match the sampling rate of the source data)
const double curveCutout = longThresh / 2; // 8.0; // make user-configurable (related to the maximum turning rate)
// the length of the segment that is added for cutting a corner can be bounded by 2*curveCutout (prevent the segment to be classified as 'long')
assert(longThresh >= 2 * curveCutout);
assert(shape.size() > 2);
// add intermediate points wherever there is a strong angular change between long segments
// assume the geometry is simplified so as not to contain consecutive colinear points
PositionVector shape2 = shape;
double maxAngleDiff = 0;
double offset = 0;
for (int j = 1; j < (int)shape.size() - 1; ++j) {
//const double hdg = shape.angleAt2D(j);
const Position& p0 = shape[j - 1];
const Position& p1 = shape[j];
const Position& p2 = shape[j + 1];
const double dAngle = fabs(GeomHelper::angleDiff(p0.angleTo2D(p1), p1.angleTo2D(p2)));
const double length1 = p0.distanceTo2D(p1);
const double length2 = p1.distanceTo2D(p2);
maxAngleDiff = MAX2(maxAngleDiff, dAngle);
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << " j=" << j << " dAngle=" << RAD2DEG(dAngle) << " length1=" << length1 << " length2=" << length2 << "\n";
}
#endif
if (dAngle > straightThresh
&& (length1 > longThresh || j == 1)
&& (length2 > longThresh || j == (int)shape.size() - 2)) {
shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 - MIN2(length1 - POSITION_EPS, curveCutout)));
shape2.insertAtClosest(shape.positionAtOffset2D(offset + length1 + MIN2(length2 - POSITION_EPS, curveCutout)));
shape2.removeClosest(p1);
}
offset += length1;
}
const int numPoints = (int)shape2.size();
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << " n=" << numPoints << " shape2=" << toString(shape2) << "\n";
}
#endif
if (maxAngleDiff < straightThresh) {
length = writeGeomLines(shape2, device, elevationDevice, 0);
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << " special case: all lines. maxAngleDiff=" << maxAngleDiff << "\n";
}
#endif
return ok;
}
// write the long segments as lines, short segments as curves
offset = 0;
for (int j = 0; j < numPoints - 1; ++j) {
const Position& p0 = shape2[j];
const Position& p1 = shape2[j + 1];
PositionVector line;
line.push_back(p0);
line.push_back(p1);
const double lineLength = line.length2D();
if (lineLength >= longThresh) {
offset = writeGeomLines(line, device, elevationDevice, offset);
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << " writeLine=" << toString(line) << "\n";
}
#endif
} else {
// find control points
PositionVector begShape;
PositionVector endShape;
if (j == 0 || j == numPoints - 2) {
// keep the angle of the first/last segment but end at the front of the shape
begShape = line;
begShape.add(p0 - begShape.back());
} else if (j == 1 || p0.distanceTo2D(shape2[j - 1]) > longThresh) {
// use the previous segment if it is long or the first one
begShape.push_back(shape2[j - 1]);
begShape.push_back(p0);
} else {
// end at p0 with mean angle of the previous and current segment
begShape.push_back(shape2[j - 1]);
begShape.push_back(p1);
begShape.add(p0 - begShape.back());
}
if (j == 0 || j == numPoints - 2) {
// keep the angle of the first/last segment but start at the end of the shape
endShape = line;
endShape.add(p1 - endShape.front());
} else if (j == numPoints - 3 || p1.distanceTo2D(shape2[j + 2]) > longThresh) {
// use the next segment if it is long or the final one
endShape.push_back(p1);
endShape.push_back(shape2[j + 2]);
} else {
// start at p1 with mean angle of the current and next segment
endShape.push_back(p0);
endShape.push_back(shape2[j + 2]);
endShape.add(p1 - endShape.front());
}
const double extrapolateLength = MIN2((double)25, lineLength / 4);
PositionVector init = NBNode::bezierControlPoints(begShape, endShape, false, extrapolateLength, extrapolateLength, ok, 0, straightThresh);
if (init.size() == 0) {
// could not compute control points, write line
offset = writeGeomLines(line, device, elevationDevice, offset);
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << " writeLine lineLength=" << lineLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
}
#endif
} else {
// write bezier
const double curveLength = bezier(init, 12).length2D();
offset = writeGeomPP3(device, elevationDevice, init, curveLength, offset);
#ifdef DEBUG_SMOOTH_GEOM
if (DEBUGCOND) {
std::cout << " writeCurve lineLength=" << lineLength << " curveLength=" << curveLength << " begShape" << j << "=" << toString(begShape) << " endShape" << j << "=" << toString(endShape) << " init" << j << "=" << toString(init) << "\n";
}
#endif
}
}
}
length = offset;
return ok;
}
double
NWWriter_OpenDrive::writeGeomPP3(
OutputDevice& device,
OutputDevice& elevationDevice,
PositionVector init,
double length,
double offset) {
assert(init.size() == 3 || init.size() == 4);
// avoid division by 0
length = MAX2(POSITION_EPS, length);
const Position p = init.front();
const double hdg = init.angleAt2D(0);
// backup elevation values
const PositionVector initZ = init;
// translate to u,v coordinates
init.add(-p.x(), -p.y(), -p.z());
init.rotate2D(-hdg);
// parametric coefficients
double aU, bU, cU, dU;
double aV, bV, cV, dV;
double aZ, bZ, cZ, dZ;
// unfactor the Bernstein polynomials of degree 2 (or 3) and collect the coefficients
if (init.size() == 3) {
//f(x, a, b ,c) = a + (2*b - 2*a)*x + (a - 2*b + c)*x*x
aU = init[0].x();
bU = 2 * init[1].x() - 2 * init[0].x();
cU = init[0].x() - 2 * init[1].x() + init[2].x();
dU = 0;
aV = init[0].y();
bV = 2 * init[1].y() - 2 * init[0].y();
cV = init[0].y() - 2 * init[1].y() + init[2].y();
dV = 0;
// elevation is not parameteric on [0:1] but on [0:length]
aZ = initZ[0].z();
bZ = (2 * initZ[1].z() - 2 * initZ[0].z()) / length;
cZ = (initZ[0].z() - 2 * initZ[1].z() + initZ[2].z()) / (length * length);
dZ = 0;
} else {
// f(x, a, b, c, d) = a + (x*((3*b) - (3*a))) + ((x*x)*((3*a) + (3*c) - (6*b))) + ((x*x*x)*((3*b) - (3*c) - a + d))
aU = init[0].x();
bU = 3 * init[1].x() - 3 * init[0].x();
cU = 3 * init[0].x() - 6 * init[1].x() + 3 * init[2].x();
dU = -init[0].x() + 3 * init[1].x() - 3 * init[2].x() + init[3].x();
aV = init[0].y();
bV = 3 * init[1].y() - 3 * init[0].y();
cV = 3 * init[0].y() - 6 * init[1].y() + 3 * init[2].y();
dV = -init[0].y() + 3 * init[1].y() - 3 * init[2].y() + init[3].y();
// elevation is not parameteric on [0:1] but on [0:length]
aZ = initZ[0].z();
bZ = (3 * initZ[1].z() - 3 * initZ[0].z()) / length;
cZ = (3 * initZ[0].z() - 6 * initZ[1].z() + 3 * initZ[2].z()) / (length * length);
dZ = (-initZ[0].z() + 3 * initZ[1].z() - 3 * initZ[2].z() + initZ[3].z()) / (length * length * length);
}
device.openTag("geometry");
device.writeAttr("s", offset);
device.writeAttr("x", p.x());
device.writeAttr("y", p.y());
device.writeAttr("hdg", hdg);
device.writeAttr("length", length);
device.openTag("paramPoly3");
device.writeAttr("aU", aU);
device.writeAttr("bU", bU);
device.writeAttr("cU", cU);
device.writeAttr("dU", dU);
device.writeAttr("aV", aV);
device.writeAttr("bV", bV);
device.writeAttr("cV", cV);
device.writeAttr("dV", dV);
device.closeTag();
device.closeTag();
// write elevation
elevationDevice.openTag("elevation");
elevationDevice.writeAttr("s", offset);
elevationDevice.writeAttr("a", aZ);
elevationDevice.writeAttr("b", bZ);
elevationDevice.writeAttr("c", cZ);
elevationDevice.writeAttr("d", dZ);
elevationDevice.closeTag();
return offset + length;
}
