Beispiel #1
0
 /// <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;
 }
Beispiel #2
0
 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);
 }
Beispiel #3
0
        public UnboundedLine(GeometricVector vectorOfLine, CartesianPoint pointOnLine)
        {
            Guard.AgainstNullArgument(vectorOfLine, "vectorOfLine");
            Guard.AgainstNullArgument(pointOnLine, "pointOnLine");

            this.Vector = vectorOfLine;
            this.PointOnLine = pointOnLine;
        }
Beispiel #4
0
        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;
        }
Beispiel #5
0
        /// <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;
        }
Beispiel #6
0
        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));
        }
Beispiel #7
0
        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);
        }
Beispiel #8
0
        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);
        }
Beispiel #9
0
        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);
        }
Beispiel #10
0
        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]);
        }
Beispiel #11
0
        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));
        }
Beispiel #12
0
        /// <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);
        }
Beispiel #13
0
        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);
        }
Beispiel #14
0
        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));
        }
Beispiel #15
0
        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);
        }
Beispiel #16
0
        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);
        }
Beispiel #17
0
        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;
        }
Beispiel #18
0
 private double LocalYOf(CartesianPoint point)
 {
     return(-sina * point.X + cosa * point.Y + originLocalY);
 }
Beispiel #19
0
 public double SignedDistanceOf(CartesianPoint point)
 {
     return(LocalYOf(point)); // This suffices since there is no scaling involved in the transformation.
 }
Beispiel #20
0
 public CartesianPoint TransformGlobalToLocalPoint(CartesianPoint point)
 {
     return(new CartesianPoint(cosa * point.X + sina * point.Y + originLocalX,
                               -sina * point.X + cosa * point.Y + originLocalY));
 }
Beispiel #21
0
 // The normal vector for the positive region.
 public Vector2 NormalVectorThrough(CartesianPoint point)
 {
     return(normalVector.Copy());
 }
Beispiel #22
0
 //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));
 }
Beispiel #23
0
        /// <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;
        }
Beispiel #24
0
 public Plane(GeometricVector planeNormal, CartesianPoint pointOnPlane)
 {
     this.Normal = planeNormal;
     this.Point = pointOnPlane;
 }
Beispiel #25
0
        /// <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);
        }
Beispiel #26
0
        /// <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;
        }
Beispiel #27
0
 public void SetUp()
 {
     vectorOfLine = new GeometricVector(2, 3, 4);
     startPoint = new CartesianPoint(3, 4, 5);
     SUT = new BoundedLine(startPoint, vectorOfLine);
 }
Beispiel #28
0
 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
Beispiel #30
0
        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);}
Beispiel #32
0
 public BoundedLine(CartesianPoint startOfLine, GeometricVector vectorOfLineFromStartToEnd)
     : base(vectorOfLineFromStartToEnd, startOfLine)
 {
     // empty
 }
Beispiel #33
0
        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));
        }
Beispiel #34
0
        /// <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);
        }
Beispiel #35
0
 public BoundedLine(CartesianPoint startOfLine, CartesianPoint endOfLine)
     : base(endOfLine.Subtract(startOfLine), startOfLine)
 {
     // empty
 }
Beispiel #36
0
        /// <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));
 }