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 void Flip()
{
clsPoint3d p3 = default(clsPoint3d);
p3 = P1.Copy();
P1 = P2.Copy();
P2 = p3.Copy();
}
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 clsLine3d(clsPoint3d pt1, clsPoint3d pt2)
{
myP1 = pt1.Copy();
myP2 = pt2.Copy();
}
public PGPointComparerFromPoint(clsPoint3d p1)
{
myPt = p1.Copy();
}
public clsMarkerPoint Copy(bool includeConfirmedFlag = false, bool avoidHistory = false)
{
clsMarkerPoint myCopy = new clsMarkerPoint();
int i;
myCopy.MarkerID = myMarkerID;
myCopy.SeenFromMarkerID = mySeenFromMarkerID;
myCopy.ActualMarkerID = myActualMarkerID;
myCopy.Origin = myOrigin;
myCopy.XAxisPoint = myEndXAxis;
myCopy.YAxisPoint = myEndYAxis;
myCopy.Point = myPoint?.Copy();
myCopy.VX = VX;
myCopy.VY = VY;
myCopy.VZ = VZ;
if (includeConfirmedFlag)
{
myCopy.SetConfirmedFlag(_confirmed);
}
foreach (int myID in mySeenFromMarkerIDs)
{
myCopy.SeenFromMarkerIDs.Add(myID);
}
foreach (clsPoint3d p1 in myCameraPoints)
{
myCopy.CameraPoints.Add(p1.Copy());
}
foreach (clsPoint3d p1 in mySeenFromCameraPoints)
{
myCopy.SeenFromCameraPoints.Add(p1.Copy());
}
foreach (clsPoint3d p1 in myPts1)
{
myCopy.OriginPoints.Add(p1.Copy());
}
foreach (clsPoint3d p1 in myPts2)
{
myCopy.EndXAxisPoints.Add(p1.Copy());
}
foreach (clsPoint3d p1 in myPts3)
{
myCopy.EndYAxisPoints.Add(p1.Copy());
}
for (i = 0; i <= 15; i++)
{
myCopy.ModelViewMatrix[i] = myModelViewMatrix[i];
}
for (i = 0; i < GyroData.Count; i++)
{
myCopy.GyroData.Add(GyroData[i]?.Copy());
}
for (i = 0; i < LastGyroData.Count; i++)
{
myCopy.LastGyroData.Add(LastGyroData[i]?.Copy());
}
for (i = 0; i < AccelData.Count; i++)
{
myCopy.AccelData.Add(AccelData[i]?.Copy());
}
for (i = 0; i < LastAccelData.Count; i++)
{
myCopy.LastAccelData.Add(LastAccelData[i]?.Copy());
}
if (myVerticalVect != null)
{
myCopy.VerticalVect = myVerticalVect.Copy();
}
myCopy.BulkheadHeight = BulkheadHeight;
if (!avoidHistory)
{
foreach (clsMarkerPoint myHistoricPoint in myHistory)
{
myCopy.History.Add(myHistoricPoint.Copy());
}
}
return(myCopy);
}