/// <summary>
/// Determines whether a point lies on the plane
/// </summary>
/// <param name="pointToCheck"></param>
/// <returns></returns>
public bool IsInPlane(CartesianPoint pointToCheck)
{
double dotProductOfPlane = this.Point.DotProduct(Normal);
double dotProductOfPointToCheck = pointToCheck.DotProduct(Normal);
double delta = dotProductOfPlane - dotProductOfPointToCheck;
double tolerance = 0.001;
return (delta * delta) < tolerance;
}
public void Can_be_constructed()
{
GeometricVector planeNormal = new GeometricVector(0, 0, 1);
CartesianPoint pointOnPlane = new CartesianPoint(0, 0, 0);
Plane SUT = new Plane(planeNormal, pointOnPlane);
Assert.AreEqual(planeNormal, SUT.Normal);
Assert.AreEqual(pointOnPlane, SUT.Point);
}
public UnboundedLine(GeometricVector vectorOfLine, CartesianPoint pointOnLine)
{
Guard.AgainstNullArgument(vectorOfLine, "vectorOfLine");
Guard.AgainstNullArgument(pointOnLine, "pointOnLine");
this.Vector = vectorOfLine;
this.PointOnLine = pointOnLine;
}
public static double AreaQuadrilateral(CartesianPoint point0, CartesianPoint point1, CartesianPoint point2, CartesianPoint point3)
{
GeometricVector diagonal1 = point2.Subtract(point0);
GeometricVector diagonal2 = point3.Subtract(point1);
double crossProduct = (diagonal1[DegreeOfFreedom.X] * diagonal2[DegreeOfFreedom.Y]) - (diagonal1[DegreeOfFreedom.Y] * diagonal2[DegreeOfFreedom.X]);
return 0.5 * crossProduct;
}
/// <summary>
///
/// </summary>
/// <param name="point0"></param>
/// <param name="point1"></param>
/// <param name="point2"></param>
/// <returns></returns>
public static double AreaTriangle(CartesianPoint point0, CartesianPoint point1, CartesianPoint point2)
{
GeometricVector side01 = point1.Subtract(point0);
GeometricVector side02 = point2.Subtract(point0);
GeometricVector crossProduct = side01.CrossProduct(side02);
double quadArea = crossProduct.Norm(2);
return quadArea * 0.5;
}
public void Can_determine_if_equal()
{
CartesianPoint SUT = new CartesianPoint(3, 4, 5);
CartesianPoint equal = new CartesianPoint(3, 4, 5);
CartesianPoint unequal = new CartesianPoint(4, 5, 6);
Assert.IsTrue(SUT.Equals(equal));
Assert.IsFalse(SUT.Equals(unequal));
}
public void Can_add_a_vector()
{
CartesianPoint SUT = new CartesianPoint(3, 4, 5);
GeometricVector other = new GeometricVector(5, 4, -3);
CartesianPoint result = SUT.Add(other);
Assert.AreEqual(8, result.X);
Assert.AreEqual(8, result.Y);
Assert.AreEqual(2, result.Z);
}
public void Can_calculate_vector_to_another_point()
{
CartesianPoint SUT = new CartesianPoint(3, 4, 5);
CartesianPoint other = new CartesianPoint(5, 4, 3);
GeometricVector result = SUT.VectorTo(other);
Assert.AreEqual(2, result.X);
Assert.AreEqual(0, result.Y);
Assert.AreEqual(-2, result.Z);
}
public void Can_subtract_another_point()
{
CartesianPoint SUT = new CartesianPoint(3, 4, 5);
CartesianPoint other = new CartesianPoint(5, 4, 3);
GeometricVector result = SUT.Subtract(other);
Assert.AreEqual(-2, result.X);
Assert.AreEqual(0, result.Y);
Assert.AreEqual(2, result.Z);
}
public void Can_be_constructed()
{
CartesianPoint SUT = new CartesianPoint(3, 4, 5);
Assert.AreEqual(3, SUT.X);
Assert.AreEqual(4, SUT.Y);
Assert.AreEqual(5, SUT.Z);
Assert.AreEqual(3, SUT[DegreeOfFreedom.X]);
Assert.AreEqual(4, SUT[DegreeOfFreedom.Y]);
Assert.AreEqual(5, SUT[DegreeOfFreedom.Z]);
}
public void Can_determine_if_point_is_in_plane()
{
GeometricVector planeNormal = new GeometricVector(0, 0, 1);
CartesianPoint pointOnPlane = new CartesianPoint(0, 0, 0);
Plane SUT = new Plane(planeNormal, pointOnPlane);
CartesianPoint pointInPlane = new CartesianPoint(1, 1, 0);
CartesianPoint pointOutOfPlane = new CartesianPoint(0, 0, 1);
Assert.IsTrue(SUT.IsInPlane(pointInPlane));
Assert.IsFalse(SUT.IsInPlane(pointOutOfPlane));
}
/// <summary>
/// Calculates the perpendicular line from this line to the given point
/// </summary>
/// <param name="pointNotOnLine"></param>
/// <returns></returns>
public BoundedLine PerpendicularLineTo(CartesianPoint pointNotOnLine)
{
GeometricVector betweenPoints = pointNotOnLine.Subtract(this.PointOnLine);
GeometricVector normalizedLineVector = this.Vector.Normalize(2);
double projectionDistanceOfEndPointAlongLine = betweenPoints.DotProduct(normalizedLineVector);
GeometricVector vectorAlongLine = normalizedLineVector.Multiply(projectionDistanceOfEndPointAlongLine);
CartesianPoint endPointOfPerpendicularLine = this.PointOnLine.Add(vectorAlongLine);
GeometricVector result = pointNotOnLine.Subtract(endPointOfPerpendicularLine);
return new BoundedLine(endPointOfPerpendicularLine, result);
}
public void Can_be_created()
{
GeometricVector vectorOfLine = new GeometricVector(1, 2, 3);
CartesianPoint pointOnLine = new CartesianPoint(3, 4, 5);
UnboundedLine SUT = new UnboundedLine(vectorOfLine, pointOnLine);
Assert.AreEqual(vectorOfLine, SUT.Vector);
Assert.AreEqual(pointOnLine, SUT.PointOnLine);
Assert.AreEqual(vectorOfLine.X, SUT.X);
Assert.AreEqual(vectorOfLine.Y, SUT.Y);
Assert.AreEqual(vectorOfLine.Z, SUT.Z);
}
public void Can_determine_if_point_is_on_line()
{
GeometricVector vectorOfLine = new GeometricVector(2, 2, 2);
CartesianPoint pointOnLine = new CartesianPoint(3, 4, 5);
UnboundedLine SUT = new UnboundedLine(vectorOfLine, pointOnLine);
CartesianPoint aPointOnTheLine = new CartesianPoint(4, 5, 6);
CartesianPoint aPointNotOnTheLine = new CartesianPoint(3, 5, 7);
CartesianPoint aPointOnTheLineInNegativeDirectionFromDefinedPointOnLine = new CartesianPoint(2, 3, 4);
Assert.IsTrue(SUT.IsOnLine(aPointOnTheLine));
Assert.IsFalse(SUT.IsOnLine(aPointNotOnTheLine));
Assert.IsTrue(SUT.IsOnLine(aPointOnTheLineInNegativeDirectionFromDefinedPointOnLine));
}
public virtual bool IsOnLine(CartesianPoint pointToCheck)
{
GeometricVector normalizedVector = Vector.Normalize(2);
GeometricVector vectorToPointToCheck = this.PointOnLine.VectorTo(pointToCheck);
vectorToPointToCheck = vectorToPointToCheck.Normalize(2);
if (normalizedVector.Equals(vectorToPointToCheck))
{
return true;
}
GeometricVector inversedNormalizedVector = normalizedVector.Negate();
return inversedNormalizedVector.Equals(vectorToPointToCheck);
}
public void Can_calculate_perpendicular_line_to_a_point()
{
GeometricVector vectorOfLine = new GeometricVector(2, 2, 0);
CartesianPoint pointOnLine = new CartesianPoint(3, 4, 5);
UnboundedLine SUT = new UnboundedLine(vectorOfLine, pointOnLine);
CartesianPoint perpendicularPoint = new CartesianPoint(3, 6, 5);
BoundedLine result = SUT.PerpendicularLineTo(perpendicularPoint);
Assert.AreEqual(-1, result.X);
Assert.AreEqual(1, result.Y);
Assert.AreEqual(0, result.Z);
Assert.AreEqual(4, result.Start.X);
Assert.AreEqual(5, result.Start.Y);
Assert.AreEqual(5, result.Start.Z);
}
public override bool IsOnLine(CartesianPoint pointToCheck)
{
GeometricVector normalizedVector = this.Vector.Normalize(2);
GeometricVector vectorToPointToCheck = this.Start.VectorTo(pointToCheck);
if (vectorToPointToCheck.X == 0 && vectorToPointToCheck.Y == 0 && vectorToPointToCheck.Z == 0)
{
// the pointToCheck is at the start of the bounded line
return true;
}
GeometricVector normalizedVectorToPointToCheck = vectorToPointToCheck.Normalize(2);
if (!normalizedVector.Equals(normalizedVectorToPointToCheck))
{
return false;
}
double norm = this.Vector.Norm(2);
double normToPointToCheck = vectorToPointToCheck.Norm(2);
return normToPointToCheck <= norm;
}
private double LocalYOf(CartesianPoint point)
{
return(-sina * point.X + cosa * point.Y + originLocalY);
}
public double SignedDistanceOf(CartesianPoint point)
{
return(LocalYOf(point)); // This suffices since there is no scaling involved in the transformation.
}
public CartesianPoint TransformGlobalToLocalPoint(CartesianPoint point)
{
return(new CartesianPoint(cosa * point.X + sina * point.Y + originLocalX,
-sina * point.X + cosa * point.Y + originLocalY));
}
// The normal vector for the positive region.
public Vector2 NormalVectorThrough(CartesianPoint point)
{
return(normalVector.Copy());
}
//Perhaps I can override the intersects method and project the other segment onto the local system!
public LineSegment2D.SegmentSegmentPosition IntersectionWith(LineSegment2D segment,
out CartesianPoint intersectionPoint)
{
return(segment.IntersectionWith(new LineSegment2D(Start, End), out intersectionPoint));
}
/// <summary>
/// Initializes the state to the supplied observation.
/// </summary>
/// <param name="z"></param>
public void Initialize(Position3D z)
{
SquareMatrix3D Hi = SquareMatrix3D.Invert(this.H);
this.z = z.ToCartesianPoint(_ellipsoid);
//s.x = inv(s.H)*s.z;
this.x = Hi * this.z;
//s.P = inv(s.H)*s.R*inv(s.H');
this.P = Hi * this.R * SquareMatrix3D.Invert(SquareMatrix3D.Transpose(this.H));
_lastObservation = DateTime.Now;
_delay = TimeSpan.Zero;
}
public Plane(GeometricVector planeNormal, CartesianPoint pointOnPlane)
{
this.Normal = planeNormal;
this.Point = pointOnPlane;
}
/// <summary>
/// Инициализация всех дочерних сущности для заданной сущности. Используется рекурсивно для каждого вложенного параметра.
/// </summary>
/// <param name="itemParam">Родительский параметр, для которого необходимо инициализировать параметры</param>
/// <param name="entitiesDictionary">Словарь всех сущностей, полученных из STEP файла. Ключем должен быть номер сущности.</param>
/// <param name="parentNode">Родительский узел дерева TreeView. Инициализация происходит параллельно.</param>
/// <returns>Метод возвращает проинициализированную сущность до самого нижнего уровня.</returns>
private IEntityModel parseItemParam(EntityParam itemParam, Dictionary <int, BaseEntity> entitiesDictionary, TreeNode parentNode)
{
if (mCallback != null)
{
currentProgress += progressStep;
mCallback.extractionStep(currentProgress < 99 ? currentProgress : 99);
}
//Инициализируемый параметр
IEntityModel newParam = null;
//Инициализируемый дочерний узел дерева
TreeNode childNode = null;
//Номер сущности, хранящийся в параметре
int itemNumber;
if (itemParam.value.StartsWith("#") && int.TryParse(itemParam.value.Substring(1), out itemNumber))
{
//Сущность из словаря, получаемая по номеру
BaseEntity nextEntity;
if (entitiesDictionary.TryGetValue(itemNumber, out nextEntity))
{
//Подбор необходимого алгоритма инициализации по имени сущности из STEP файла
switch (nextEntity.Name)
{
//Комментарии для одного алгоритма, далее по аналогии.
case Axis2Placement3D.NAME:
//Создание объекта для соответствующего имени сущности
Axis2Placement3D tempParamA2P3D = new Axis2Placement3D();
//Создания нового узла дерева (узлы деревьев формируются параллельно инициализации сущностей)
childNode = new TreeNode(tempParamA2P3D.StepName);
//Присвоение имени (title) объекту сущности из параметров, полученных парсером из STEP файла
tempParamA2P3D.Name = nextEntity.ParsedParams[0][0].value;
//Добавление соотвествующего узла дерева
childNode.Nodes.Add("Label: " + tempParamA2P3D.Name);
//Рекурсивный вызов метода для инициализации остальных внутренних параметров
tempParamA2P3D.Location = (CartesianPoint)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamA2P3D.Axis = (Direction)parseItemParam(nextEntity.ParsedParams[2][0], entitiesDictionary, childNode);
tempParamA2P3D.RefDirection = (Direction)parseItemParam(nextEntity.ParsedParams[3][0], entitiesDictionary, childNode);
//Присвоение абстрактной переменной текущей сущности
newParam = tempParamA2P3D;
break;
case ManifoldSolidBrep.NAME:
ManifoldSolidBrep tempParamMSB = new ManifoldSolidBrep();
childNode = new TreeNode(tempParamMSB.StepName);
tempParamMSB.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamMSB.Name);
tempParamMSB.Outer = (ClosedShell)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
newParam = tempParamMSB;
break;
case ClosedShell.NAME:
ClosedShell tempParamCS = new ClosedShell();
childNode = new TreeNode(tempParamCS.StepName);
tempParamCS.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamCS.Name);
tempParamCS.CfsFaces = new List <Face>(nextEntity.ParsedParams[1].Count);
TreeNode childParamNode = new TreeNode("Cfs faces");
childNode.Nodes.Add(childParamNode);
foreach (var itemParamCS in nextEntity.ParsedParams[1])
{
IEntityModel param = parseItemParam(itemParamCS, entitiesDictionary, childParamNode);
tempParamCS.CfsFaces.Add((Face)param);
}
newParam = tempParamCS;
break;
case AdvancedFace.NAME:
AdvancedFace tempParamAF = new AdvancedFace();
childNode = new TreeNode(tempParamAF.StepName);
tempParamAF.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamAF.Name);
tempParamAF.Bounds = new List <FaceBound>(nextEntity.ParsedParams[1].Count);
TreeNode childParamNodeAF = new TreeNode("Bounds");
childNode.Nodes.Add(childParamNodeAF);
foreach (var itemParamAFB in nextEntity.ParsedParams[1])
{
IEntityModel param = parseItemParam(itemParamAFB, entitiesDictionary, childParamNodeAF);
tempParamAF.Bounds.Add((FaceBound)param);
}
tempParamAF.FaceGeometry = (Surface)parseItemParam(nextEntity.ParsedParams[2][0], entitiesDictionary, childNode);
tempParamAF.SameSense = nextEntity.ParsedParams[3][0].value;
childNode.Nodes.Add("Same sense: " + tempParamAF.SameSense);
newParam = tempParamAF;
break;
case FaceBound.NAME:
FaceBound tempParamFB = new FaceBound();
childNode = new TreeNode(tempParamFB.StepName);
tempParamFB.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamFB.Name);
tempParamFB.Bound = (Loop)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamFB.Orientation = nextEntity.ParsedParams[2][0].value;
childNode.Nodes.Add("Orientation: " + tempParamFB.Orientation);
newParam = tempParamFB;
break;
case FaceOuterBound.NAME:
FaceOuterBound tempParamFOB = new FaceOuterBound();
childNode = new TreeNode(tempParamFOB.StepName);
tempParamFOB.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamFOB.Name);
tempParamFOB.Bound = (Loop)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamFOB.Orientation = nextEntity.ParsedParams[2][0].value;
childNode.Nodes.Add("Orientation: " + tempParamFOB.Orientation);
newParam = tempParamFOB;
break;
case ConicalSurface.NAME:
ConicalSurface tempParamCoSu = new ConicalSurface();
childNode = new TreeNode(tempParamCoSu.StepName);
tempParamCoSu.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamCoSu.Name);
tempParamCoSu.Position = (Axis2Placement3D)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamCoSu.Radius = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[2][0].value);
childNode.Nodes.Add("Radius: " + tempParamCoSu.Radius);
tempParamCoSu.SemiAngle = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[3][0].value);
childNode.Nodes.Add("Semi angle: " + tempParamCoSu.SemiAngle);
newParam = tempParamCoSu;
break;
case CylindricalSurface.NAME:
CylindricalSurface tempParamCySu = new CylindricalSurface();
childNode = new TreeNode(tempParamCySu.StepName);
tempParamCySu.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamCySu.Name);
tempParamCySu.Position = (Axis2Placement3D)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamCySu.Radius = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[2][0].value);
childNode.Nodes.Add("Radius: " + tempParamCySu.Radius);
newParam = tempParamCySu;
break;
case EdgeLoop.NAME:
EdgeLoop tempParamEL = new EdgeLoop();
childNode = new TreeNode(tempParamEL.StepName);
tempParamEL.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamEL.Name);
tempParamEL.EdgeList = new List <OrientedEdge>(nextEntity.ParsedParams[1].Count);
TreeNode childParamNodeEL = new TreeNode("Edge list");
childNode.Nodes.Add(childParamNodeEL);
foreach (var itemParamEL in nextEntity.ParsedParams[1])
{
IEntityModel param = parseItemParam(itemParamEL, entitiesDictionary, childParamNodeEL);
tempParamEL.EdgeList.Add((OrientedEdge)param);
}
newParam = tempParamEL;
break;
case OrientedEdge.NAME:
OrientedEdge tempParamOE = new OrientedEdge();
childNode = new TreeNode(tempParamOE.StepName);
tempParamOE.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamOE.Name);
tempParamOE.EdgeStart = (Vertex)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamOE.EdgeEnd = (Vertex)parseItemParam(nextEntity.ParsedParams[2][0], entitiesDictionary, childNode);
tempParamOE.EdgeElement = (Edge)parseItemParam(nextEntity.ParsedParams[3][0], entitiesDictionary, childNode);
tempParamOE.Orientation = nextEntity.ParsedParams[4][0].value;
childNode.Nodes.Add("Orientation: " + tempParamOE.Orientation);
newParam = tempParamOE;
break;
case Edge.NAME:
Edge tempParamE = new Edge();
childNode = new TreeNode(tempParamE.StepName);
tempParamE.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamE.Name);
tempParamE.EdgeStart = (Vertex)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamE.EdgeEnd = (Vertex)parseItemParam(nextEntity.ParsedParams[2][0], entitiesDictionary, childNode);
newParam = tempParamE;
break;
case VertexPoint.NAME:
VertexPoint tempParamVP = new VertexPoint();
childNode = new TreeNode(tempParamVP.StepName);
tempParamVP.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamVP.Name);
tempParamVP.VertexGeometry = (CartesianPoint)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
newParam = tempParamVP;
break;
case CartesianPoint.NAME:
CartesianPoint tempParamCP = new CartesianPoint();
childNode = new TreeNode(tempParamCP.StepName);
tempParamCP.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamCP.Name);
tempParamCP.X = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[1][0].value);
childNode.Nodes.Add("X: " + tempParamCP.X);
tempParamCP.Y = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[1][1].value);
childNode.Nodes.Add("Y: " + tempParamCP.Y);
tempParamCP.Z = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[1][2].value);
childNode.Nodes.Add("Z: " + tempParamCP.Z);
newParam = tempParamCP;
break;
case EdgeCurve.NAME:
EdgeCurve tempParamEC = new EdgeCurve();
childNode = new TreeNode(tempParamEC.StepName);
tempParamEC.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamEC.Name);
tempParamEC.EdgeStart = (Vertex)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamEC.EdgeEnd = (Vertex)parseItemParam(nextEntity.ParsedParams[2][0], entitiesDictionary, childNode);
tempParamEC.EdgeGeometry = (Curve)parseItemParam(nextEntity.ParsedParams[3][0], entitiesDictionary, childNode);
tempParamEC.SameSense = nextEntity.ParsedParams[4][0].value;
childNode.Nodes.Add("Same sense: " + tempParamEC.SameSense);
newParam = tempParamEC;
break;
case Circle.NAME:
Circle tempParamCi = new Circle();
childNode = new TreeNode(tempParamCi.StepName);
tempParamCi.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamCi.Name);
tempParamCi.Position = (Axis2Placement3D)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamCi.Radius = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[2][0].value);
childNode.Nodes.Add("Radius: " + tempParamCi.Radius);
newParam = tempParamCi;
break;
case Direction.NAME:
Direction tempParamDi = new Direction();
childNode = new TreeNode(tempParamDi.StepName);
tempParamDi.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamDi.Name);
TreeNode childParamNodeDi = new TreeNode("Direction ratios");
childNode.Nodes.Add(childParamNodeDi);
tempParamDi.DirectionRatios = new List <double>(3);
tempParamDi.DirectionRatios.Add((double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[1][0].value));
tempParamDi.DirectionRatios.Add((double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[1][1].value));
tempParamDi.DirectionRatios.Add((double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[1][2].value));
childParamNodeDi.Nodes.Add(tempParamDi.DirectionRatios[0].ToString());
childParamNodeDi.Nodes.Add(tempParamDi.DirectionRatios[1].ToString());
childParamNodeDi.Nodes.Add(tempParamDi.DirectionRatios[2].ToString());
newParam = tempParamDi;
break;
case Line.NAME:
Line tempParamL = new Line();
childNode = new TreeNode(tempParamL.StepName);
tempParamL.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamL.Name);
tempParamL.Pnt = (CartesianPoint)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamL.Dir = (Vector)parseItemParam(nextEntity.ParsedParams[2][0], entitiesDictionary, childNode);
newParam = tempParamL;
break;
case Vector.NAME:
Vector tempParamV = new Vector();
childNode = new TreeNode(tempParamV.StepName);
tempParamV.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamV.Name);
tempParamV.Orientation = (Direction)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamV.Magnitude = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[2][0].value);
childNode.Nodes.Add("Magnitude: " + tempParamV.Magnitude);
newParam = tempParamV;
break;
case Plane.NAME:
Plane tempParamPl = new Plane();
childNode = new TreeNode(tempParamPl.StepName);
tempParamPl.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamPl.Name);
tempParamPl.Position = (Axis2Placement3D)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
newParam = tempParamPl;
break;
case ToroidalSurface.NAME:
ToroidalSurface tempParamTorSu = new ToroidalSurface();
childNode = new TreeNode(tempParamTorSu.StepName);
tempParamTorSu.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add("Label: " + tempParamTorSu.Name);
tempParamTorSu.Position = (Axis2Placement3D)parseItemParam(nextEntity.ParsedParams[1][0], entitiesDictionary, childNode);
tempParamTorSu.MajorRadius = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[2][0].value);
childNode.Nodes.Add("MajorRadius: " + tempParamTorSu.MajorRadius);
tempParamTorSu.MinorRadius = (double)doubleConverter.ConvertFromInvariantString(nextEntity.ParsedParams[3][0].value);
childNode.Nodes.Add("MinorRadius: " + tempParamTorSu.MinorRadius);
newParam = tempParamTorSu;
break;
case BoundedSurface.NAME:
BoundedSurface tempParamBoundSu = new BoundedSurface();
childNode = new TreeNode(tempParamBoundSu.StepName);
//tempParamBoundSu.Name = nextEntity.ParsedParams[0][0].value;
childNode.Nodes.Add(/*"Label: " + tempParamBoundSu.Name + */ "[Not implemented]");
newParam = tempParamBoundSu;
break;
default:
//Ошибка генерируется в том случае, если сущность не определена ни одним из алгоритмов => нужно создать новый класс сущности и прописать соответствующей ей алгоритм инициализации.
throw new InvalidDataException("Not found entity: " + nextEntity.Name);
}
//Добавление в родительский узел TreeView нового дочернего узла, проинициализированного в алгоритмах выше.
parentNode.Nodes.Add(childNode);
}
}
return(newParam);
}
/// <summary>
/// Initializes the state to the supplied observation.
/// </summary>
/// <param name="z">The z.</param>
public void Initialize(Position3D z)
{
SquareMatrix3D hi = SquareMatrix3D.Invert(_h);
_z = z.ToCartesianPoint(_ellipsoid);
//s.x = inv(s.H)*s.z;
_x = hi * _z;
//s.P = inv(s.H)*s.R*inv(s.H');
_p = hi * _r * SquareMatrix3D.Invert(SquareMatrix3D.Transpose(_h));
_lastObservation = DateTime.Now;
_delay = TimeSpan.Zero;
}
public void SetUp()
{
vectorOfLine = new GeometricVector(2, 3, 4);
startPoint = new CartesianPoint(3, 4, 5);
SUT = new BoundedLine(startPoint, vectorOfLine);
}
private double LocalXOf(CartesianPoint point)
{
return(cosa * point.X + sina * point.Y + originLocalX);
}
public CartesianPoint(CartesianPoint p ):base(){AddNext();this.Coordinates=new List1to3_LengthMeasure((LengthMeasure)p.x,(LengthMeasure)p.y,(LengthMeasure)p.z);this.EndOfLineComment=p.EndOfLineComment;} // issue: need for distinction 2D/3D here
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid,
CartesianPoint u, CartesianPoint x, CartesianPoint z,
SquareMatrix3D A, SquareMatrix3D B, SquareMatrix3D H,
SquareMatrix3D P, SquareMatrix3D Q, SquareMatrix3D R)
{
this._deviceError = deviceError.IsEmpty ? DilutionOfPrecision.CurrentAverageDevicePrecision.Value : deviceError.Value;
this._horizontalDOP = horizontalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : horizontalDOP.Value;
this._verticalDOP = verticalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : verticalDOP.Value;
this._ellipsoid = ellipsoid == null ? Ellipsoid.Default : ellipsoid;
double hCovariance = this._deviceError * this._horizontalDOP;
double vCovariance = this._deviceError * this._verticalDOP;
this.u = u.IsInvalid ? CartesianPoint.Empty : u;
this.x = x;
this.z = z.IsInvalid ? CartesianPoint.Empty : z;
this.A = A == null ? new SquareMatrix3D() : A;
this.B = B == null ? new SquareMatrix3D() : B;
this.H = H == null ? new SquareMatrix3D() : H;
this.P = P == null ? new SquareMatrix3D() : P;
this.Q = Q == null ? SquareMatrix3D.Default(0) : Q;
this.R = R == null ? new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance) : R;
this._interval = 0;
this._errorState = Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2));
this._delay = TimeSpan.MaxValue;
this._lastObservation = DateTime.MinValue;
if (!gpsPosition.IsEmpty)
Initialize(gpsPosition);
}
public Axis2Placement3D Clone(CartesianPoint p,string EndOfLineComment=null) {return new Axis2Placement3D(Location:p,Axis:this.Axis,RefDirection:this.RefDirection,EndOfLineComment:EndOfLineComment);}
public BoundedLine(CartesianPoint startOfLine, GeometricVector vectorOfLineFromStartToEnd)
: base(vectorOfLineFromStartToEnd, startOfLine)
{
// empty
}
public void Can_determine_if_point_is_on_the_line()
{
vectorOfLine = new GeometricVector(2, 2, 2);
startPoint = new CartesianPoint(3, 4, 5);
SUT = new BoundedLine(startPoint, vectorOfLine);
CartesianPoint pointOnTheLine = new CartesianPoint(4, 5, 6);
CartesianPoint pointAtEndOfLine = new CartesianPoint(5, 6, 7);
CartesianPoint pointAtStartOfLine = new CartesianPoint(3, 4, 5);
CartesianPoint pointBeyondEndOfLine = new CartesianPoint(6, 7, 8);
CartesianPoint pointBeyondStartOfLine = new CartesianPoint(2, 3, 4);
Assert.IsTrue(SUT.IsOnLine(pointOnTheLine));
Assert.IsTrue(SUT.IsOnLine(pointAtEndOfLine));
Assert.IsTrue(SUT.IsOnLine(pointAtStartOfLine));
Assert.IsFalse(SUT.IsOnLine(pointBeyondEndOfLine));
Assert.IsFalse(SUT.IsOnLine(pointBeyondStartOfLine));
}
/// <summary>
/// Initializes a new instance of the <see cref="KalmanSystemState"/> struct.
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <param name="u">The u.</param>
/// <param name="x">The x.</param>
/// <param name="z">The z.</param>
/// <param name="a">A.</param>
/// <param name="b">The b.</param>
/// <param name="h">The h.</param>
/// <param name="p">The p.</param>
/// <param name="q">The q.</param>
/// <param name="r">The r.</param>
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid,
CartesianPoint u, CartesianPoint x, CartesianPoint z,
SquareMatrix3D a, SquareMatrix3D b, SquareMatrix3D h,
SquareMatrix3D p, SquareMatrix3D q, SquareMatrix3D r)
{
_deviceError = deviceError.IsEmpty ? DilutionOfPrecision.CurrentAverageDevicePrecision.Value : deviceError.Value;
_horizontalDOP = horizontalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : horizontalDOP.Value;
_verticalDOP = verticalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : verticalDOP.Value;
_ellipsoid = ellipsoid ?? Ellipsoid.Default;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
_u = u.IsInvalid ? CartesianPoint.Empty : u;
_x = x;
_z = z.IsInvalid ? CartesianPoint.Empty : z;
_a = a ?? new SquareMatrix3D();
_b = b ?? new SquareMatrix3D();
_h = h ?? new SquareMatrix3D();
_p = p ?? new SquareMatrix3D();
_q = q ?? SquareMatrix3D.Default(0);
_r = r ?? new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
_interval = 0;
_errorState = Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2));
_delay = TimeSpan.MaxValue;
_lastObservation = DateTime.MinValue;
if (!gpsPosition.IsEmpty)
Initialize(gpsPosition);
}
public BoundedLine(CartesianPoint startOfLine, CartesianPoint endOfLine)
: base(endOfLine.Subtract(startOfLine), startOfLine)
{
// empty
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <param name="z">The z.</param>
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (_x.IsInvalid)
{
Initialize(z);
return;
}
// More insanity
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Covariance values are invalid. Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_deviceError = deviceError.Value;
_horizontalDOP = horizontalDOP.Value;
_verticalDOP = verticalDOP.Value;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
// Setup the observation covariance (measurement error)
_r = new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
#region Process Noise Estimation
// Get the translation of the last correction
CartesianPoint subX = _x.ToPosition3D(_ellipsoid)
.TranslateTo(bearing, speed.ToDistance(_delay), _ellipsoid)
.ToCartesianPoint();
// Get the vector of the translation and the last observation
//CartesianPoint w = (subX - this.z);
CartesianPoint w =
new CartesianPoint(
Distance.FromMeters(subX.X.Value - _z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - _z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - _z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
_q = new SquareMatrix3D(
Math.Abs(w.X.Value), 0, 0,
0, Math.Abs(w.Y.Value), 0,
0, 0, Math.Abs(w.Z.Value));
#endregion Process Noise Estimation
// Update the observation state
_z = z.ToCartesianPoint(_ellipsoid);
#region State vector prediction and covariance
//s.x = s.A*s.x + s.B*s.u;
//this.x = this.A * this.x + this.B * this.u;
CartesianPoint ax = _a.TransformVector(_x);
CartesianPoint bu = _b.TransformVector(_u);
_x =
new CartesianPoint(
Distance.FromMeters(ax.X.Value + bu.X.Value),
Distance.FromMeters(ax.Y.Value + bu.Y.Value),
Distance.FromMeters(ax.Z.Value + bu.Z.Value));
//s.P = s.A * s.P * s.A' + s.Q;
_p = _a * _p * SquareMatrix3D.Transpose(_a) + _q;
#endregion State vector prediction and covariance
#region Kalman gain factor
//K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D ht = SquareMatrix3D.Transpose(_h);
SquareMatrix3D k = _p * ht * SquareMatrix3D.Invert(_h * _p * ht + _r);
#endregion Kalman gain factor
#region Observational correction
//s.x = s.x + K*(s.z-s.H*s.x);
//this.x = this.x + K * (this.z - this.H * this.x);
CartesianPoint hx = _h.TransformVector(_x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(_z.X.Value - hx.X.Value),
Distance.FromMeters(_z.Y.Value - hx.Y.Value),
Distance.FromMeters(_z.Z.Value - hx.Z.Value));
CartesianPoint kzHx = k.TransformVector(zHx);
_x =
new CartesianPoint(
Distance.FromMeters(_x.X.Value + kzHx.X.Value),
Distance.FromMeters(_x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(_x.Z.Value + kzHx.Z.Value));
//s.P = s.P - K*s.H*s.P;
_p = _p - k * _h * _p;
#endregion Observational correction
// Bump the state count
_interval++;
// Calculate the average error for the system stste.
_errorState = (_errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
_delay = now.Subtract(_lastObservation);
_lastObservation = now;
}
public bool IsInsideBoundary(CartesianPoint point)
{
return(boundary.IsInside(point));
}