public clsPoint3d ClosestPointToLine(clsLine3d l2)
{
clsPoint3d v1;
clsPoint3d v2;
clsPoint3d M;
double m2;
clsPoint3d R;
clsPoint3d P21;
double t1;
clsPoint3d Q1;
//Find the actual point on this line where the perpendicular distance hits.
v1 = new clsPoint3d(DX(), DY(), DZ());
v1.Normalise();
v2 = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ());
v2.Normalise();
M = v2.Cross(v1);
m2 = M.Dot(M);
if (m2 < mdlGeometry.myTol)
{
return(l2.P1);
}
//Parallel
P21 = new clsPoint3d(l2.X1 - X1, l2.Y1 - Y1, l2.Z1 - Z1);
R = P21.Cross(M);
R.Scale(1 / m2);
t1 = R.Dot(v2);
Q1 = P1 + t1 * v1;
return(Q1);
}
public clsPGPoint(clsPoint3d myPt, int anID)
{
X = myPt.X;
Y = myPt.Y;
Z = myPt.Z;
myID = anID;
}
public void SetEndPoint()
{
if (myActualMarkerID < myGFMarkerID)
{
if (myActualMarkerID >= 0 && myActualMarkerID < 50)
{
myPoint = myOrigin + VX * 160 - VY * 45;
}
else if (myActualMarkerID >= 50 && myActualMarkerID < 100)
{
myPoint = myOrigin - VX * 160 - VY * 45;
}
}
else if (myActualMarkerID >= myGFMarkerID && myActualMarkerID <= myRightBulkheadMarkerID) //GF, Step & Bulkhead Markers
{
myPoint = myOrigin.Copy();
}
else if (myActualMarkerID >= myDoorHingeRightMarkerID && myActualMarkerID <= myDoorFrameLeftMarkerID) //Door Markers
{
myPoint = myOrigin + VY * 65.0;
}
else if (myActualMarkerID >= myObstruct1MarkerID && myActualMarkerID <= myObstruct4MarkerID) //Obstruction Markers
{
myPoint = myOrigin + VY * 65.0;
}
else if (myActualMarkerID >= myWall1MarkerID && myActualMarkerID <= myWall4MarkerID) //Wall Markers
{
myPoint = myOrigin + VY * 65.0;
}
else
{
myPoint = myOrigin.Copy();
}
}
public bool IsOnShortLine(clsPoint3d pt1, double aTol = 0, bool excludeEnds = false)
{
clsLine3d l1 = default(clsLine3d);
clsLine3d l2 = default(clsLine3d);
double d = 0;
if (aTol == 0)
{
aTol = mdlGeometry.myTol;
}
if (Abs(DistanceToPoint(pt1)) > aTol)
{
return(false);
}
l1 = new clsLine3d(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);
}
public void Flip()
{
clsPoint3d p3 = default(clsPoint3d);
p3 = P1.Copy();
P1 = P2.Copy();
P2 = p3.Copy();
}
public void Reverse()
{
clsPoint3d pt1 = default(clsPoint3d);
pt1 = myP1;
myP1 = myP2;
myP2 = pt1;
}
public double DistanceToLine(clsLine3d l2)
{
//Perpendicular distance between 3d lines. Limited to the line segments.
double myNormalDist;
clsPoint3d v1;
clsPoint3d v2;
clsPoint3d M;
double m2;
clsPoint3d R;
clsPoint3d P21;
double t1;
double t2;
clsPoint3d Q1;
clsPoint3d Q2;
//Find the actual point on this line where the perpendicular distance hits. If it is off the line, then find the minimum distance between the end points
v1 = new clsPoint3d(DX(), DY(), DZ());
v1.Normalise();
v2 = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ());
v2.Normalise();
P21 = new clsPoint3d(l2.X1 - X1, l2.X1 - X1, l2.X1 - X1);
M = v2.Cross(v1);
m2 = M.Dot(M);
if (m2 < mdlGeometry.myTol)
{
return(DistanceToPoint(l2.P1));
}
//Parallel
myNormalDist = Abs(P21.Dot(M)) / Sqrt(m2);
//Perpendicular distance
R = P21.Cross(M);
R.Scale(1 / m2);
t1 = R.Dot(v2);
Q1 = P1 + t1 * v1;
if (t1 < 0)
{
Q1 = P1;
}
if (t1 > Length)
{
Q1 = P2;
}
t2 = R.Dot(v1);
Q2 = l2.P1 + t2 * v2;
if (t2 < 0)
{
Q2 = l2.P1;
}
if (t2 > l2.Length)
{
Q2 = l2.P2;
}
return(Q1.Dist(Q2));
}
public void RotateAboutX(double a)
{
clsPoint3d p1 = default(clsPoint3d);
p1 = new clsPoint3d(y, z, 0);
p1.Rotate(a);
y = p1.x;
z = p1.y;
}
public void RotateAboutZ(double a)
{
clsPoint3d p1 = default(clsPoint3d);
p1 = new clsPoint3d(x, y, 0);
p1.Rotate(a);
x = p1.x;
y = p1.y;
}
public clsPoint3d Normalised()
{
//Returns a copy of the normalised point
clsPoint3d p1 = default(clsPoint3d);
p1 = Copy();
p1.Normalise();
return(p1);
}
public clsLine3d Copy()
{
clsPoint3d pt1 = new clsPoint3d();
clsPoint3d pt2 = new clsPoint3d();
pt1 = P1.Copy();
pt2 = P2.Copy();
return(new clsLine3d(pt1, pt2));
}
public void RotateAboutY(double a)
{
clsPoint3d p1 = default(clsPoint3d);
p1 = new clsPoint3d(-x, z, 0);
p1.Rotate(a);
x = -p1.x;
z = p1.y;
}
public clsMarkerPoint()
{
GyroData = new List <clsPoint3d>();
LastGyroData = new List <clsPoint3d>();
AccelData = new List <clsPoint3d>();
LastAccelData = new List <clsPoint3d>();
VX = new clsPoint3d(1, 0, 0);
VY = new clsPoint3d(0, 1, 0);
VZ = new clsPoint3d(0, 0, 1);
}
public double Lambda(clsPoint3d pt1)
{
clsLine3d l1 = default(clsLine3d);
clsLine3d l2 = default(clsLine3d);
l1 = new clsLine3d(P1, pt1);
l2 = Copy();
l2.Normalise();
return(l1.Dot(l2) / Length);
}
public bool IsOnLine(clsPoint3d aPt)
{
clsLine3d l1 = default(clsLine3d);
l1 = new clsLine3d(P2, aPt);
if (mdlGeometry.IsSameDbl(Cross(l1).Length, 0))
{
return(true);
}
return(false);
}
public clsPoint3d Cross(clsPoint3d p1)
{
double x1 = 0;
double y1 = 0;
double z1 = 0;
x1 = myY * p1.z - myZ * p1.y;
y1 = -(myX * p1.z - myZ * p1.x);
z1 = myX * p1.y - myY * p1.x;
return(new clsPoint3d(x1, y1, z1));
}
public bool IsSameTol(clsPoint3d p1, double aTol = 0)
{
if ((object)p1 == null)
{
return(false);
}
if (aTol == 0)
{
aTol = mdlGeometry.myTol;
}
return(Dist(p1) < aTol);
}
public static clsPoint3d ProjectPoint(clsPoint3d p1, clsLine3d l1)
{
double d1 = 0;
clsLine3d l2 = new clsLine3d();
clsLine3d l3 = new clsLine3d();
l2 = l1.Copy();
l2.Normalise();
l3 = new clsLine3d(l1.P1, p1);
d1 = l2.Dot(l3);
l2.Length = d1;
return(l2.P2);
}
public double DistanceToPoint(clsPoint3d aPt)
{
double d1 = 0;
double d2 = 0;
clsLine3d l2 = default(clsLine3d);
clsLine3d l3 = default(clsLine3d);
l2 = Copy();
l2.Normalise();
l3 = new clsLine3d(P1, aPt);
d1 = l2.Dot(l3);
d2 = l3.Length;
return(Sqrt(Pow(d2, 2) - Pow(d1, 2)));
}
public static double Dist3d(clsPoint3d p1, clsLine3d l1)
{
double d1 = 0;
double d2 = 0;
clsLine3d l2 = new clsLine3d();
clsLine3d l3 = new clsLine3d();
l2 = l1.Copy();
l2.Normalise();
l3 = new clsLine3d(l1.P1, p1);
d1 = l2.Dot(l3);
d2 = l3.Length;
return(Sqrt(Pow(d2, 2) - Pow(d1, 2)));
}
public void SetEndPointBasedOnZVectors()
{
int n = 0;
if (myPts1.Count == 0)
{
return;
}
//Let's try without this for a while:
//CompactMarkers()
myOrigin = ProcessPointListPair(myPts1, mySeenFromCameraPoints, ref n);
if (myOrigin == null || myOrigin.Length < myTol || n == 0)
{
goto quitOut;
}
VX = AverageAxis(myPts1, myPts2);
if (VX == null || VX.Length < myTol)
{
goto quitOut;
}
VX.Normalise();
myEndXAxis = myOrigin + VX;
VY = AverageAxis(myPts1, myPts3);
if (VY == null || VY.Length < myTol)
{
goto quitOut;
}
VY.Normalise();
myEndYAxis = myOrigin + VY;
SetZNormal();
myEndXAxis = myOrigin + VX;
myEndYAxis = myOrigin + VY;
SetEndPoint();
return;
quitOut:
myOrigin = new clsPoint3d();
myEndXAxis = new clsPoint3d();
myEndYAxis = new clsPoint3d();
myPoint = new clsPoint3d();
}
public double MaxAnglePerpendicular(ref clsPoint3d maxAV1, ref clsPoint3d maxAV2)
{
double a;
double maxA;
clsPoint3d py, pz;
clsPoint3d p1 = null, p2 = null, p3 = null, p4 = null;
if (MaxAngle(ref p3, ref p4) < myTol)
{
return(0);
}
pz = new clsPoint3d(0, 0, 1.0);
py = pz.Cross(p3);
if (IsSameDbl(py.Length, 0))
{
py = pz.Cross(p4);
}
if (IsSameDbl(py.Length, 0))
{
return(0);
}
py.Normalise();
maxA = 0;
for (int j = 0; j <= myCameraPoints.Count - 2; j++)
{
for (int k = j + 1; k <= myCameraPoints.Count - 1; k++)
{
p1 = myCameraPoints[j];
p1 = py * py.Dot(p1) + pz * pz.Dot(p1);
p1.Normalise();
p2 = myCameraPoints[k];
p2 = py * py.Dot(p2) + pz * pz.Dot(p2);
p2.Normalise();
a = Acos(p1.Dot(p2));
if (Abs(a) > maxA)
{
maxA = Abs(a);
maxAV1 = p1.Copy();
maxAV2 = p2.Copy();
}
}
}
return(maxA);
}
public static double DistancePointLine3D(clsPoint3d p1, clsLine3d l1)
{
//Distance between 3D point and truncated 3D line
clsPoint3d p3d = new clsPoint3d();
double l = 0;
p3d = ProjectPoint(p1, l1);
l = l1.Lambda(p3d);
if (l < 0)
{
return(p1.Dist(l1.P1));
}
if (l > 1)
{
return(p1.Dist(l1.P2));
}
return(p1.Dist(p3d));
}
public bool OKToConfirm(ref string myErrString, ref clsPoint3d maxAV1, ref clsPoint3d maxAV2, ref clsPoint3d maxAV3, ref clsPoint3d maxAV4, ref bool myAngleOK, ref bool myAngle2OK, ref double a1, ref double a2)
{
myErrString = "";
myAngleOK = false;
myAngle2OK = false;
//-DEVELOPMENT CHANGE
a1 = MaxDistance(ref maxAV1, ref maxAV2);
if (a1 < 500)
{
myErrString = "Distance = " + Round(a1) + " < 500mm";
}
else
{
myAngleOK = true;
}
//a1 = MaxAngle(ref maxAV1, ref maxAV2);
//if (a1 < 20 * PI / 180) {
// myErrString = "Angle Range = " + Round(a1 * 180 / PI, 1) + " < 20°";
//} else {
// myAngleOK = true;
//}
a2 = MaxAnglePerpendicular(ref maxAV3, ref maxAV4);
a2 = 25 * PI / 180;
if (a2 < 20 * PI / 180)
{
if (myErrString == "")
{
myErrString = "Perpendicular Angle Range = " + Round(a2 * 180 / PI, 1) + " < 20°";
}
return(false);
}
else
{
myAngle2OK = true;
}
//Make sure that if we are using bundle adjustment, then this marker has been recorded by GTSAM
//if (myAngleOK && myAngle2OK && Stannah_API.AppPreferences.GTSAMBundleAdjustment && !HasMarkerBeenSeenByGTSAM(MarkerID)) return false;
return(myAngleOK && myAngle2OK);
}
public double DistanceToShortLine(clsPoint3d pt1)
{
clsPoint3d pt2 = default(clsPoint3d);
pt2 = mdlGeometry.ProjectPoint(pt1, this);
if (Lambda(pt2) < 0)
{
pt2 = P1;
}
if (Lambda(pt2) > 1)
{
pt2 = P2;
}
if (mdlGeometry.IsSameDbl(Length, 0))
{
pt2 = P1;
}
return(pt1.Dist(pt2));
}
public static clsPoint3d ProjectPointOntoPlaneAlongZ(clsPoint p, clsLine3d l1)
{
clsLine3d v1 = new clsLine3d();
double u = 0;
double r = 0;
double z = 0;
clsPoint3d p1 = new clsPoint3d();
v1 = new clsLine3d(new clsPoint3d(0, 0, 0), new clsPoint3d(0, 0, 1));
u = v1.Dot(l1);
//This is the length of the normal vector, measured in the vertical direction
p1 = new clsPoint3d(p.X - l1.X1, p.Y - l1.Y1, 0 - l1.Z1);
r = p1.Dot(l1.DP());
//r is the distance of our point to the nearest point on the plane
//r = v1.Dot(myV)
z = r / u;
//z is the vertical distance of our point to the plane. It is sign sensitive - positive is above the plane.
return(new clsPoint3d(p.X, p.Y, -z));
}
public double MaxDistance(ref clsPoint3d maxAV1, ref clsPoint3d maxAV2)
{
double d;
double maxD;
clsPoint3d p1, p2;
clsPoint3d px, py, pz;
maxD = 0;
pz = new clsPoint3d(0, 0, 1.0);
for (int j = 0; j <= myCameraPoints.Count - 2; j++)
{
p1 = myCameraPoints[j];
for (int k = j + 1; k <= myCameraPoints.Count - 1; k++)
{
p2 = myCameraPoints[k];
py = new clsPoint3d((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2, (p1.Z + p2.Z) / 2).Point2D().Point3d(0);
if (IsSameDbl(py.Length, 0))
{
continue;
}
py.Normalise();
px = pz.Cross(py);
px.Normalise();
p1 = px * p1.Dot(px) + pz * p1.Dot(pz);
p1.Normalise();
p2 = px * p2.Dot(px) + pz * p2.Dot(pz);
p2.Normalise();
d = myCameraPoints[j].Dist(myCameraPoints[k]);
if (Abs(d) > maxD)
{
maxD = Abs(d);
maxAV1 = p1.Copy();
maxAV2 = p2.Copy();
}
}
}
return(maxD);
}
public bool Overlaps(clsLine3d aLine, double aTol = 0)
{
clsLine3d l1 = default(clsLine3d);
clsPoint3d p3 = default(clsPoint3d);
clsPoint3d p4 = default(clsPoint3d);
if (aTol == 0)
{
aTol = mdlGeometry.myTol;
}
l1 = aLine.Copy();
if ((!IsOnLine(l1.P1)) | (!IsOnLine(l1.P2)))
{
return(false);
}
if (Dot(l1) < 0)
{
l1.Reverse();
}
p3 = l1.P1;
p4 = l1.P2;
if (P1 == p3)
{
return(true);
}
if (P1 == p4 | P2 == p3)
{
return(false);
}
if (IsOnShortLine(p4))
{
return(true);
}
if (l1.IsOnShortLine(P1, aTol) | l1.IsOnShortLine(P2, aTol))
{
return(true);
}
return(false);
}
public void RotateAboutLine(clsLine3d l1, double theta)
{
clsPoint3d p1 = default(clsPoint3d);
clsPoint3d p2 = default(clsPoint3d);
clsPoint3d p2a = default(clsPoint3d);
clsPoint3d p2b = default(clsPoint3d);
clsPoint3d p3 = default(clsPoint3d);
clsLine3d l2 = default(clsLine3d);
if (l1.IsOnLine(this))
{
return;
}
//Setup a coordinate system with X running along l1 and the origin at l1.p2
l2 = new clsLine3d(l1.P2.Copy(), Copy());
p1 = new clsPoint3d(l1.DX(), l1.DY(), l1.DZ());
p1.Length = 1;
p2 = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ());
p2a = new clsPoint3d(l2.DX(), l2.DY(), l2.DZ());
p2.Length = 1;
p3 = p1.Cross(p2);
p3.Length = 1;
p2 = p1.Cross(p3);
p2.Length = 1;
if (p2.Dot(p2a) < 0)
{
p2.Scale(-1);
}
p2b = new clsPoint3d(p1.Dot(p2a), p2.Dot(p2a), p3.Dot(p2a));
p2b.RotateAboutX(theta);
myX = l1.X2 + p2b.x * p1.x + p2b.y * p2.x + p2b.z * p3.x;
myY = l1.Y2 + p2b.x * p1.y + p2b.y * p2.y + p2b.z * p3.y;
myZ = l1.Z2 + p2b.x * p1.z + p2b.y * p2.z + p2b.z * p3.z;
}
private static clsPoint ModelToImageSpace2(Matrix4 projection, Matrix4 modelview, int[] vp, ARParam arParams, clsPoint3d p1)
{
Vector4 vec;
vec.X = (float)p1.X;
vec.Y = (float)p1.Y;
vec.Z = (float)p1.Z;
vec.W = 1.0f;
Vector4.Transform(ref vec, ref modelview, out vec);
Vector4.Transform(ref vec, ref projection, out vec);
if (vec.W > float.Epsilon || vec.W < -float.Epsilon)
{
vec.X /= vec.W;
vec.Y /= vec.W;
vec.Z /= vec.W;
}
var p3d = new clsPoint3d(vp[0] + (1.0f + vec.X) * vp[2] / 2.0f, vp[3] - (vp[1] + (1.0f + vec.Y) * vp[3] / 2.0f), (1.0f + vec.Z) / 2.0f);
arParamIdeal2Observ(arParams.dist_factor, p3d.X, p3d.Y, out double ox, out double oy, arParams.dist_function_version);
return(new clsPoint(ox, oy));
}
