/*
* Returns true if a point is "behind" a line. Behind is the side of the line which (movementDir) faces.
*/
bool isBehindLine(const PointXY & pointToCheck, const SurfaceT & line, const PointXY & movementDir) {
// Check to which side of the exit we are crossing
const SurfaceT xAxis = SurfaceT(PointXY(0, 0), PointXY(100, 0));
double lineTiltAngle = line.getAngleDiffTo(xAxis);
double cT = cos(deg2rad(lineTiltAngle));
double sT = sin(deg2rad(lineTiltAngle));
PointXY transMoveDir = PointXY(cT * movementDir.getX() - sT * movementDir.getY(),
sT * movementDir.getX() + cT * movementDir.getY());
bool crossingInPositiveDir = transMoveDir.getY() > 0;
PointXY transPoint = pointToCheck - line.getMiddle();
transPoint = PointXY(cT * transPoint.getX() - sT * transPoint.getY(),
sT * transPoint.getX() + cT * transPoint.getY());
return (crossingInPositiveDir && transPoint.getY() > 0)
|| (!crossingInPositiveDir && transPoint.getY() <= 0);
}
/* 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;
}