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;
}
