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);
}
}
/*
* Returns the smallest distance between two surfaces (limited to their endpoints).
*/
double distBetweenSurfs(const SurfaceT & s1, const SurfaceT & s2) {
// Distance s1.P1 from SurfaceT s2
PointXY test = nearestPointOnSurf(s2, s1.getP1(), true);
double closestDistSq = test.distFromSq(s1.getP1());
// Distance s1.P2 from SurfaceT s2
test = nearestPointOnSurf(s2, s1.getP2(), true);
double testDistSq = test.distFromSq(s1.getP2());
if (testDistSq < closestDistSq) {
closestDistSq = testDistSq;
}
// Distance SurfaceT s1 from s2.P1
test = nearestPointOnSurf(s1, s2.getP1(), true);
testDistSq = test.distFromSq(s2.getP1());
if (testDistSq < closestDistSq) {
closestDistSq = testDistSq;
}
// Distance SurfaceT s1 from s2.P2
test = nearestPointOnSurf(s1, s2.getP2(), true);
testDistSq = test.distFromSq(s2.getP2());
if (testDistSq < closestDistSq) {
closestDistSq = testDistSq;
}
return sqrt(closestDistSq);
}
/* 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;
}