private static void GetCenterPointForDatum(clsPoint pt, double[,] model, ARParam arParams, int[] vp, Image <Gray, byte> grayImage, ref Emgu.CV.Util.VectorOfPointF centerPoints)
{
var cpt = ModelToImageSpace(arParams, model, pt);
var halfSquare = GetSquareForDatum(arParams, model, pt);
if (halfSquare < 8)
{
return;
}
if (cpt.x - halfSquare < 0 || cpt.x + halfSquare > vp[2] || cpt.y - halfSquare <0 || cpt.y + halfSquare> vp[3])
{
return;
}
var rect = new Rectangle((int)cpt.x - halfSquare, (int)cpt.y - halfSquare, 2 * halfSquare, 2 * halfSquare);
var region = new Mat(grayImage.Mat, rect);
var binaryRegion = region.Clone();
double otsuThreshold = CvInvoke.Threshold(region, binaryRegion, 0.0, 255.0, Emgu.CV.CvEnum.ThresholdType.Otsu);
int nonzero = CvInvoke.CountNonZero(binaryRegion);
var square = 4 * halfSquare * halfSquare;
if (nonzero > square * 0.2f && nonzero < square * 0.8f)
{
centerPoints.Push(new PointF[] { new PointF((float)cpt.X, (float)cpt.Y) });
}
}
//Moves the line
public void Move(clsPoint pt)
{
X1 = X1 + pt.x;
X2 = X2 + pt.x;
Y1 = Y1 + pt.y;
Y2 = Y2 + pt.y;
}
public bool IsOnShortLine(clsPoint pt1, double aTol = 0, bool excludeEnds = false)
{
clsLine l1 = default(clsLine);
clsLine l2 = default(clsLine);
double d = 0;
if (aTol == 0)
{
aTol = mdlGeometry.myTol;
}
if (Abs(pt1.Dist(this)) > aTol)
{
return(false);
}
l1 = new clsLine(P1, pt1);
l2 = Copy();
l2.Normalise();
d = l1.Dot(l2);
if (d < -aTol)
{
return(false);
}
if (d > Length + aTol)
{
return(false);
}
if (excludeEnds && (P1 == pt1 | P2 == pt1))
{
return(false);
}
return(true);
}
//Moves it by a fixed amount
public void Move(clsPoint pt1)
{
X1 = X1 + pt1.x;
Y1 = Y1 + pt1.y;
X2 = X2 + pt1.x;
Y2 = Y2 + pt1.y;
}
public bool PointLiesOnArc(clsPoint p1)
{
double a;
a = AngleFromPoint(p1);
if (myAngle1 <= myAngle2 & Direction == 1)
{
if (a >= myAngle1 - mdlGeometry.myTol & a <= myAngle2 + mdlGeometry.myTol)
{
return(true);
}
}
else if (myAngle1 > myAngle2 & Direction == 1)
{
if (a >= myAngle1 - mdlGeometry.myTol | a <= myAngle2 + mdlGeometry.myTol)
{
return(true);
}
}
else if (myAngle1 <= myAngle2 & Direction == -1)
{
if (a <= myAngle1 + mdlGeometry.myTol | a >= myAngle2 - mdlGeometry.myTol)
{
return(true);
}
}
else if (myAngle1 > myAngle2 & Direction == -1)
{
if (a <= myAngle1 + mdlGeometry.myTol & a >= myAngle2 - mdlGeometry.myTol)
{
return(true);
}
}
return(false);
}
public double DistanceToNearestSegment(clsPoint p1, ref int nSeg, ref int nPt, int avoidPt1 = -1, int avoidPt2 = -1, int includePt = -1)
{
int i;
int j;
clsLine l1;
double d;
double minD;
//Find the point nearest
nPt = GetNearestPointIndex(p1);
//Find the segment closest to the point to add (ignore riser segments)
nSeg = 0;
minD = 10000;
for (i = 0; i <= NumPoints; i++)
{
if ((i != avoidPt1 & i != avoidPt2) & (includePt == -1 || (i == includePt | i == includePt - 1 | (includePt == 0 & i == NumPoints))))
{
j = i + 1;
if (j > NumPoints)
{
j = 0;
}
l1 = new clsLine(Point(i), Point(j));
d = l1.DistanceToShortLine(p1);
if (d < minD)
{
minD = d;
nSeg = i;
}
}
}
return(minD);
}
public clsLine TangentToPointUp(clsPoint pt1)
{
//Goes on a transition up
double a;
double x;
clsLine l1 = new clsLine();
if (pt1.X < Centre.X + mdlGeometry.myTol)
{
return(null);
}
a = Atan((pt1.Y - Centre.Y) / Abs((pt1.X - Centre.X)));
x = Radius / pt1.Dist(Centre);
if (x >= 1)
{
return(null);
}
a = a - Acos(x);
l1.P1.X = Centre.X + Radius * Cos(a);
l1.P1.Y = Centre.Y + Radius * Sin(a);
l1.P2.X = pt1.X;
l1.P2.Y = pt1.Y;
return(l1);
}
public bool IsPointInside(clsPoint p1)
{
//Uses the "winding number" to check if a point lies inside or outside the sketch
int i;
double a;
double a1;
clsLine l1;
clsLine l2;
a = 0;
l1 = new clsLine(p1, Point(0));
for (i = 1; i <= myPoints.Count - 1; i++)
{
l2 = new clsLine(p1, Point(i));
a1 = mdlGeometry.Angle(l1, l2);
a = a + a1;
l1 = l2.Copy();
}
l2 = new clsLine(p1, Point(0));
a1 = mdlGeometry.Angle(l1, l2);
a = a + a1;
if (mdlGeometry.IsSameDbl(a, 0))
{
return(false);
}
return(true);
}
static int GetSquareForDatum(ARParam arParams, double[,] model, clsPoint pt)
{
var cpt = ModelToImageSpace(arParams, model, pt);
var pt1 = ModelToImageSpace(arParams, model, new clsPoint(pt.x - 8, pt.y - 8));
var pt2 = ModelToImageSpace(arParams, model, new clsPoint(pt.x + 8, pt.y - 8));
var pt3 = ModelToImageSpace(arParams, model, new clsPoint(pt.x + 8, pt.y + 8));
var pt4 = ModelToImageSpace(arParams, model, new clsPoint(pt.x - 8, pt.y + 8));
var l1 = new clsLine(pt1, pt2);
var l2 = new clsLine(pt2, pt3);
var l3 = new clsLine(pt3, pt4);
var l4 = new clsLine(pt4, pt1);
double d = 100;
var l = new clsLine(cpt, new clsPoint(cpt.x - 1, cpt.y - 1));
var p1 = l.Intersect(l1);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
p1 = l.Intersect(l2);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
p1 = l.Intersect(l3);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
p1 = l.Intersect(l4);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
l = new clsLine(cpt, new clsPoint(cpt.x + 1, cpt.y - 1));
p1 = l.Intersect(l1);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
p1 = l.Intersect(l2);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
p1 = l.Intersect(l3);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
p1 = l.Intersect(l4);
if (p1 != null && p1.Dist(cpt) < d)
{
d = p1.Dist(cpt);
}
return((int)(d / Sqrt(2.0) + 0.5));
}
public void Reverse()
{
clsPoint pt1 = default(clsPoint);
pt1 = myP1;
myP1 = myP2;
myP2 = pt1;
}
public clsArc(clsPoint aCentre, double aRadius, double anAngle1, double anAngle2, int aDirection)
{
Centre = aCentre.Copy();
Radius = aRadius;
Angle1 = anAngle1;
Angle2 = anAngle2;
Direction = aDirection;
}
public void InsertPoint(clsPoint aPoint, int anIndex)
{
if (anIndex == -1 | anIndex > myPoints.Count)
{
anIndex = myPoints.Count;
}
myPoints.Insert(anIndex, aPoint.Copy());
}
public bool IsPointInside2(clsPoint p1)
{
//Avoids the case when one of the points is exactly on one of the lines
if (IsPointTouching(p1))
{
return(false);
}
return(IsPointInside(p1));
}
public void RotateAboutPoint(clsPoint p1, double ang)
{
int i;
for (i = 0; i <= myPoints.Count - 1; i++)
{
myPoints[i].RotateAboutPoint(p1, ang);
}
}
public void Load(StreamReader sr)
{
Centre = new clsPoint();
Centre.Load(sr);
Radius = Convert.ToDouble(sr.ReadLine());
myAngle1 = Convert.ToDouble(sr.ReadLine());
myAngle2 = Convert.ToDouble(sr.ReadLine());
myDirection = Convert.ToInt32(sr.ReadLine());
}
public clsLine Copy()
{
clsPoint pt1 = new clsPoint();
clsPoint pt2 = new clsPoint();
pt1 = P1.Copy();
pt2 = P2.Copy();
return(new clsLine(pt1, pt2));
}
//Public Sub RotateAboutX(ByVal theta As Double)
// Dim r, theta2 As Double
// r = Sqrt(y * y + z * z)
// theta2 = mdlGeometry.Angle(y, z)
// y = r * Cos(theta + theta2)
// z = r * Sin(theta + theta2)
//End Sub
public void RotateAboutPoint(clsPoint p1, double theta)
{
double r = 0;
double theta2 = 0;
r = Sqrt((x - p1.x) * (x - p1.x) + (y - p1.y) * (y - p1.y));
theta2 = mdlGeometry.Angle(x - p1.x, y - p1.y);
x = p1.x + r * Cos(theta + theta2);
y = p1.y + r * Sin(theta + theta2);
}
public double Distance(clsPoint p1, clsPoint p2)
{
//Returns the distance along the circumference between 2 points, starting from p1 and travelling in CCW direction
double a1 = 0;
double a2 = 0;
a1 = AngleFromPoint(p1);
a2 = AngleFromPoint(p2);
return(Distance(a1, a2));
}
public static double Dist(clsPoint p1, clsLine l1)
{
//+ve for outside the wall, left handed; Also, +ve for above straight.
clsLine l2 = new clsLine();
clsLine l3 = new clsLine();
l2 = (clsLine)l1.Normal();
l3 = new clsLine(l1.P1, p1);
return(l2.Dot(l3));
}
public static bool IsOnLine(clsPoint p1, clsPoint p2, clsPoint p3)
{
if (Abs(Dist(p1, new clsLine(p2, p3))) > myTol)
{
return(false);
}
else
{
return(true);
}
}
private static double DistanceToEllipse(clsPoint pt, RotatedRect rect)
{
var c = rect.Center;
var a = rect.Angle * PI / 180f;
pt.Move(-c.X, -c.Y);
pt.Rotate(-a);
var pt2 = NearestPointOnEllipse(pt, rect.Size.Width / 2, rect.Size.Height / 2);
return(pt.Dist(pt2));
}
public bool IsSameTol(clsPoint p1, double aTol = 0)
{
if ((object)p1 == null)
{
return(false);
}
if (aTol == 0)
{
aTol = myTol;
}
return(Dist(p1) < aTol);
}
public clsLine TangentToHeight(double h)
{
clsPoint pt1;
clsPoint pt2;
pt2 = StartPoint();
if (StartAngle() == 0 | pt2.Y <= h)
{
return(null);
}
pt1 = new clsPoint(pt2.X - (pt2.Y - h) / Tan(StartAngle()), h);
return(new clsLine(pt1, pt2));
}
public static clsPoint ProjectPoint(clsPoint p1, clsLine l1)
{
double d = 0;
clsPoint pt1 = new clsPoint();
clsLine l2 = new clsLine();
d = Dist(p1, l1);
pt1 = p1.Copy();
l2 = (clsLine)l1.Normal();
l2.Normalise();
l2.Scale(-d);
pt1.Move(l2.P2.X, l2.P2.Y);
return(pt1);
}
public clsPoint Tangent(double a)
{
//Unit vector in the CCW direction
clsPoint aPt = default(clsPoint);
double x = 0;
double y = 0;
double d = 0;
aPt = Point(a);
x = -(aPt.y - Centre.y);
y = (aPt.x - Centre.x);
d = Sqrt(Pow(x, 2) + Pow(y, 2));
return(new clsPoint(x / d, y / d));
}
public clsLine TangentLine(double a)
{
//Unit vector in the CCW direction, starting from the Tangent point
clsPoint aPt = default(clsPoint);
double x = 0;
double y = 0;
double d = 0;
aPt = Point(a);
x = -(aPt.y - Centre.y);
y = (aPt.x - Centre.x);
d = Sqrt(Pow(x, 2) + Pow(y, 2));
return(new clsLine(aPt.x, aPt.y, aPt.x + x / d, aPt.y + y / d));
}
public int GetPoint(clsPoint p1)
{
//Gets the index of the 2d point
int i;
for (i = 0; i <= NumPoints; i++)
{
if (p1 == myPoints[i])
{
return(i);
}
}
return(-1);
}
public double Lambda(clsPoint pt1)
{
clsPoint p1a = default(clsPoint);
clsPoint p2a = default(clsPoint);
double d = 0;
d = Length;
if (mdlGeometry.IsSameDbl(d, 0))
{
return(0);
}
p1a = pt1 - P1;
p2a = Point.Normalised();
return(p1a.Dot(p2a) / d);
}
public void AddPoint(clsPoint aPoint, int i = -1)
{
if (aPoint == null)
{
return;
}
if (i == -1)
{
myPoints.Add(aPoint.Copy());
}
else
{
myPoints.Insert(i, aPoint.Copy());
}
}
public bool IsOnLine(clsPoint aPt, double aTol = 0)
{
if (aTol == 0)
{
aTol = mdlGeometry.myTol;
}
if (Abs(aPt.Dist(this)) > aTol)
{
return(false);
}
else
{
return(true);
}
}
