PointXY nearestPointOnSurf(const SurfaceT & surf, const PointXY & other, bool limitToEndpoints) {
    /* Find point P so that 	P = P1 + u(P2-P1)					(P is on the line P1,P2)
     * Also, 					(P3-P) dot (P2-P1) = 0.				(P3,P is orthogonal to P1,P2)
     * Substituted:				(P3-P1 - u(P2-P1)) dot (P2-P1) = 0
     * Solve for u!
     */

    double x1 = surf.getX1(), y1 = surf.getY1();
    double x2 = surf.getX2(), y2 = surf.getY2();
    double x3 = other.getX(), y3 = other.getY();

    double u = ((x3 - x1) * (x2 - x1) + (y3 - y1) * (y2 - y1))
            / (surf.getLength() * surf.getLength()); // Note: abs(P2-P1) == length

    // Now: p is on the infinite line described by p1 and p2. But is it on the surface segment?
    if (limitToEndpoints && (u < 0.0 || u > 1.0)) {
        // p is not within the surface bounds. Return the nearest end point instead!
        if (surf.getP1().distFromSq(other) < surf.getP2().distFromSq(other))
            return surf.getP1();
        else
            return surf.getP2();
    } else {
        // Substitute to find p
        double xP = x1 + u * (x2 - x1);
        double yP = y1 + u * (y2 - y1);
        return PointXY(xP, yP);
    }
}
/* Extends the surface (tobeextended) to have the length of (extensionReference).
 * Keeps the ID of tobeextended, but changes occlusion information.
 * If extendFromPoint1 is true, then P1 will be fixed and P2 be changed.
 */
const SurfaceT extendSurfaceT(const SurfaceT & toBeExtended,
        const SurfaceT & extensionReference,
        bool extendFromPoint1) {
    if (!toBeExtended.isValid()) {
        cerr << "WARNING: Cannot extend surface: No surface to extend!" << endl;
        return SurfaceT::INVALID;
    }

    double oldLength = toBeExtended.getLength();
    double newLength = extensionReference.getLength();
    PointXY newP1, newP2;

    // Extend the surface along its direction vector
    if (extendFromPoint1) {
        double dirX = (toBeExtended.getX2() - toBeExtended.getX1()) / oldLength;
        double dirY = (toBeExtended.getY2() - toBeExtended.getY1()) / oldLength;
        newP1 = toBeExtended.getP1();
        newP2 = PointXY(newP1.getX() + dirX * newLength,
                newP1.getY() + dirY * newLength);
    } else {
        double dirX = (toBeExtended.getX1() - toBeExtended.getX2()) / oldLength;
        double dirY = (toBeExtended.getY1() - toBeExtended.getY2()) / oldLength;
        newP2 = toBeExtended.getP2();
        newP1 = PointXY(newP2.getX() + dirX * newLength,
                newP2.getY() + dirY * newLength);
    }

    // Change the points and keep the ID
    SurfaceT extendedSurf(newP1, newP2, toBeExtended.getId());

    // If the (longer) surface has both occluding edges, then the extended one will have too.
    if (extensionReference.isP1Occluding() && extensionReference.isP2Occluding()) {
        extendedSurf.setP1Occluding(true);
        extendedSurf.setP2Occluding(true);
    } else {
        // Otherwise keep old occlusion info (usually one occluding edge out of two)
        extendedSurf.setP1Occluding(toBeExtended.isP1Occluding());
        extendedSurf.setP2Occluding(toBeExtended.isP2Occluding());
    }
    extendedSurf.setBoundarySurf(toBeExtended.isBoundarySurf());
    return extendedSurf;
}
/* Gets the angle and distance between newSurf and referenceNew, and applies them to referenceOld.
 * The result is a surface in the old coordinate space.
 * Used for updating the MFIS with new surfaces.
 */
SurfaceT distanceAngleTransform(const SurfaceT & newSurf,
        const SurfaceT & referenceNew,
        const SurfaceT & referenceOld,
        SurfaceTMatch::MatchingSurfaceTEndpoints referencePoints) {
    // Decide which point to use as reference, and whether the surfaces are facing in the same direction
    PointXY refPointNew;
    PointXY refPointOld;
    bool facingOppositeDirection = false;
    switch (referencePoints) {
        case SurfaceTMatch::OLD1_NEW1_AND_OLD2_NEW2:
        case SurfaceTMatch::OLD1_NEW1_ONLY:
            refPointNew = referenceNew.getP1();
            refPointOld = referenceOld.getP1();
            break;

        case SurfaceTMatch::OLD2_NEW2_ONLY:
            refPointNew = referenceNew.getP2();
            refPointOld = referenceOld.getP2();
            break;

        case SurfaceTMatch::OLD1_NEW2_AND_OLD2_NEW1:
        case SurfaceTMatch::OLD1_NEW2_ONLY:
            refPointNew = referenceNew.getP2();
            refPointOld = referenceOld.getP1();
            facingOppositeDirection = true;
            break;

        case SurfaceTMatch::OLD2_NEW1_ONLY:
            refPointNew = referenceNew.getP1();
            refPointOld = referenceOld.getP2();
            facingOppositeDirection = true;
            break;

        default:
            cerr << "WARNING: Reference surface endpoints don't match!";
            return SurfaceT::INVALID;
    }

    /* For each new surface, get the distance and angle relative to the reference surface (for both points).
     * The distance is relative to P1 of the reference,
     * the angle is relative to the direction (p1->p2) of the reference.
     */
    double distanceP1 = newSurf.getP1().distFrom(refPointNew);
    double distanceP2 = newSurf.getP2().distFrom(refPointNew);

    double angleP1 = referenceNew.getAngleDiffTo(SurfaceT(refPointNew, newSurf.getP1()));
    double angleP2 = referenceNew.getAngleDiffTo(SurfaceT(refPointNew, newSurf.getP2()));

    // Construct new points, using angle and distance on the reference in the MFIS

    // Get the (normalized) direction of the MFIS reference surface
    double refDirLength = referenceOld.getLength();
    double refDirX = (referenceOld.getX2() - referenceOld.getX1()) / refDirLength;
    double refDirY = (referenceOld.getY2() - referenceOld.getY1()) / refDirLength;

    // If the reference surfaces are facing opposite each other, invert the direction
    if (facingOppositeDirection) {
        refDirX *= -1;
        refDirY *= -1;
    }

    // Rotate the direction by the angle
    double dirXP1 = cos(deg2rad(angleP1)) * refDirX - sin(deg2rad(angleP1)) * refDirY;
    double dirYP1 = sin(deg2rad(angleP1)) * refDirX + cos(deg2rad(angleP1)) * refDirY;

    double dirXP2 = cos(deg2rad(angleP2)) * refDirX - sin(deg2rad(angleP2)) * refDirY;
    double dirYP2 = sin(deg2rad(angleP2)) * refDirX + cos(deg2rad(angleP2)) * refDirY;

    // Move the distance along the new direction
    double xP1MFIS = refPointOld.getX() + distanceP1 * dirXP1;
    double yP1MFIS = refPointOld.getY() + distanceP1 * dirYP1;

    double xP2MFIS = refPointOld.getX() + distanceP2 * dirXP2;
    double yP2MFIS = refPointOld.getY() + distanceP2 * dirYP2;

    // Construct a new surface for the MFIS and copy the ID
    SurfaceT transformedSurfaceT(PointXY(xP1MFIS, yP1MFIS),
            PointXY(xP2MFIS, yP2MFIS),
            newSurf.getId());

    // Copy the occlusion information
    transformedSurfaceT.setP1Occluding(newSurf.isP1Occluding());
    transformedSurfaceT.setP2Occluding(newSurf.isP2Occluding());
    transformedSurfaceT.setBoundarySurf(newSurf.isBoundarySurf());

    return transformedSurfaceT;
}