/**
* Fit a circle to three given points on the circumference. The method draws lines (chords)
* between two pairs of the points, constructs their perpendicular bisectors, and finds the
* intersection point of those two lines. This is the centre of the circle as radii passing
* through the centre are perpendicular bisectors of chords in a circle. The radius is the
* distance between the centre and any one of the initial three points.
*/
private QDCircle threePointCircleFit(List <QDPoint> pts)
{
QDLine l1 = new QDLine(pts[0], pts[1]);
QDLine l2 = new QDLine(pts[1], pts[2]);
QDInfiniteLine i1 = new QDInfiniteLine(l1.getMidpoint(), l1.getPerpAngleD());
QDInfiniteLine i2 = new QDInfiniteLine(l2.getMidpoint(), l2.getPerpAngleD());
QDPoint centre = i1.intersect(i2);
float radius = QDUtils.QDUtils.getPtToPtDist(centre, pts[0]);
return(new QDCircle(centre, radius));
}
// Add a point to the segment list and check for a corner in the segment
// Returns true if a corner was detected, false otherwise
public bool addPoint(QDPoint pt)
{
// Special case for the first point
if (pts.Count == 0)
{
start = pt;
pts.Add(pt);
sampledMarkerPt = pt;
return(false);
}
// Get the pt to pt gradient
float angle = QDUtils.QDUtils.getPtToPtAngleD(pts.Last(), pt);
// Store summary statistics
if (angle > angle_max)
{
angle_max = angle;
}
if (angle < angle_min)
{
angle_min = angle;
}
angle_sum += angle;
// Only stores the angles between points sufficiently far apart. This is the most
// noise robust, and useful for getting the general form of the input shape
if (QDUtils.QDUtils.getPtToPtDist(sampledMarkerPt, pt) > SUBSAMP_PT_DIST)
{
sampledPtAnglesD.Add(QDUtils.QDUtils.getPtToPtAngleD(sampledMarkerPt, pt));
sampledMarkerPt = pt;
}
// Add values to vectors
anglesD.Add(angle);
pts.Add(pt);
// Step 1: track back to get testPt
float TEST_RADIUS = 50.0f;
float CNR_RADIUS = 10.0f;
QDPoint testPt = null, cnrCandidatePt = null, currPt = pt;
int midIdx = -1, testIdx = -1, cnrCandidateIdx = -1;
midIdx = backtrackByDist(pts, pts.Count - 2, TEST_RADIUS / 2.0f);
if (midIdx < 0)
{
return(false);
}
testIdx = backtrackByDist(pts, midIdx, TEST_RADIUS / 2.0f);
if (testIdx < 0)
{
return(false);
}
testPt = pts[testIdx];
// Step 2: construct line from testPt to currPt
QDInfiniteLine testLine = new QDInfiniteLine(testPt, currPt);
// Step 3: Find perp dist from each pt between test and curr to the line
float maxDist = 0.0f;
int maxIdx = -1;
for (int i = testIdx + 1; i < pts.Count - 1; i++)
{
float thisDist = testLine.getDistToPoint(pts[i]);
if (thisDist > maxDist)
{
maxDist = thisDist;
maxIdx = i;
}
}
// Step 4: confirm cnrCandidatePt
if (maxIdx < 0)
{
return(false);
}
cnrCandidatePt = pts[maxIdx];
int cnrIdx = maxIdx;
if (!(QDUtils.QDUtils.getPtToPtDist(cnrCandidatePt, currPt) > CNR_RADIUS))
{
return(false);
}
// Step 5: construct lines
float oldAngleD = new QDLine(testPt, cnrCandidatePt).angleD;
float newAngleD = new QDLine(cnrCandidatePt, currPt).angleD;
// Step 6: test angle between lines
if (Math.Abs(QDUtils.QDUtils.angleDiffMinorD(oldAngleD, newAngleD)) > CNR_ANGLE_THRESH)
{
// Step 7: test for continuous curvature
int strTestIdx = backtrackByDist(pts, testIdx, CNR_RADIUS);
if (strTestIdx < 0)
{
return(false);
}
int strTest2Idx = backtrackByDist(pts, strTestIdx, CNR_RADIUS);
if (strTest2Idx < 0)
{
return(false);
}
float seg1 = new QDLine(pts[strTest2Idx], pts[strTestIdx]).angleD;
float seg2 = new QDLine(pts[strTestIdx], pts[testIdx]).angleD;
if (Math.Abs(QDUtils.QDUtils.angleDiffMinorD(seg1, seg2)) < 30.0f)
{
cornerTerminated = true;
}
else
{
cornerTerminated = false;
}
}
if (cornerTerminated)
{
newSeg = new QDInputPointSet(CORNER_WINDOW, CNR_ANGLE_THRESH);
newSeg.addPoint(cnrCandidatePt); // Add the corner point
int lastIdx = pts.Count - 1;
for (int i = 0; i < lastIdx - cnrIdx; i++) // For how many points there are after the corner
{
newSeg.addPoint(this.pts[cnrIdx + 1]); // Copy points after the corner into new seg
this.pts.RemoveAt(cnrIdx + 1); // Delete points after corner from current set
this.anglesD.Remove(cnrIdx); // There is one less angle than pts since first two pts make 1 angle
}
// Remove subsampled angles as well up to before the corner
int deleteSubSamp = (int)Math.Ceiling(CNR_RADIUS / SUBSAMP_PT_DIST);
for (int i = 0; i < deleteSubSamp; i++)
{
sampledPtAnglesD.Remove(sampledPtAnglesD.Count - deleteSubSamp + i);
}
return(true);
}
return(false);
}
public List <QDShape> getExtrinsicConstraints(QDInputPointSet ptSet)
{
List <QDShape> modifiedShapes = new List <QDShape>();
if (ptSet.shapeType == QDShapeTypes.LINE)
{
QDLine line = (QDLine)ptSet.fittedShape;
List <QDShapeDBPoint> nearStartPts = shapeDB.getShapesNearPoint(line.start, SEARCH_RADIUS);
foreach (QDShapeDBPoint pt in nearStartPts)
{
if (pt.type == QDPointTypes.LINE_START || pt.type == QDPointTypes.LINE_FINISH)
{
QDLine nearLine = (QDLine)pt.shape;
if (!modifiedShapes.Contains(nearLine))
{
modifiedShapes.Add(nearLine);
}
QDInfiniteLine thisInf = new QDInfiniteLine(line.start, line.finish);
QDInfiniteLine nearInf = new QDInfiniteLine(nearLine.start, nearLine.finish);
// Check for nearly parallel lines
if (Math.Abs(QDUtils.QDUtils.angleDiffMinorD(line.angleD, nearLine.angleD)) > 10f)
{
QDPoint intersectPt = thisInf.intersect(nearInf);
if (QDUtils.QDUtils.getPtToPtDist(line.start, intersectPt) < SEARCH_RADIUS * 1.25)
{
line.start = intersectPt;
if (pt.type == QDPointTypes.LINE_START)
{
nearLine.start = intersectPt;
}
else
{
nearLine.finish = intersectPt;
}
}
}
}
}
/**
* GOTTA BE A BETTER WAY OF DOING THIS RATHER THAN DOING IT ONCE FOR START AND ONCE FOR FINISH
*/
List <QDShapeDBPoint> nearFinishPts = shapeDB.getShapesNearPoint(line.finish, SEARCH_RADIUS);
foreach (QDShapeDBPoint pt in nearFinishPts)
{
if (pt.type == QDPointTypes.LINE_START || pt.type == QDPointTypes.LINE_FINISH)
{
QDLine nearLine = (QDLine)pt.shape;
if (!modifiedShapes.Contains(nearLine))
{
modifiedShapes.Add(nearLine);
}
QDInfiniteLine thisInf = new QDInfiniteLine(line.start, line.finish);
QDInfiniteLine nearInf = new QDInfiniteLine(nearLine.start, nearLine.finish);
// Check for nearly parallel lines
if (Math.Abs(QDUtils.QDUtils.angleDiffMinorD(line.angleD, nearLine.angleD)) > 10f)
{
QDPoint intersectPt = thisInf.intersect(nearInf);
if (QDUtils.QDUtils.getPtToPtDist(line.finish, intersectPt) < SEARCH_RADIUS * 1.25)
{
line.finish = intersectPt;
if (pt.type == QDPointTypes.LINE_START)
{
nearLine.start = intersectPt;
}
else
{
nearLine.finish = intersectPt;
}
}
}
}
}
}
return(modifiedShapes);
}
