Esempio n. 1
0
        //<summary>
        //判断三点共线(第三点到直线的距离作为精度【相关技术要求】)
        //</summary>
        //<returns>true:共线</returns>false:不共线
        public static bool CKYThreeColline(DataType.StaubliRobotData.St_PointRx x_pPoint1, DataType.StaubliRobotData.St_PointRx x_pPoint2, DataType.StaubliRobotData.St_PointRx x_pPoint3, double x_nDistancePrecision, ref double x_nActualError)
        {
            bool l_bResult = false;

            DataType.BasicDataType.vector l_V1, l_V2, l_V3;
            l_V1 = Point2Vector(x_pPoint1);
            l_V2 = Point2Vector(x_pPoint2);
            l_V3 = Point2Vector(x_pPoint3);
            double l_nDistance1 = BasicMathTool.VectorDistance(l_V1, l_V2);
            double l_nDistance2 = BasicMathTool.VectorDistance(l_V1, l_V3);

            if (l_nDistance1 <= l_nDistance2)
            {
                double l_nDiastance = PointToLineDistance(l_V1, l_V2, l_V3);
                if (l_nDiastance <= x_nDistancePrecision)
                {
                    l_bResult = true; x_nActualError = l_nDiastance;
                }
                else
                {
                    l_bResult = false;
                }
            }
            else
            {
                l_bResult = false;
            }
            return(l_bResult);
        }
Esempio n. 2
0
        /// <summary>
        ///  判断点是否在圆上(CKY标准,点到圆弧最近距离)
        /// </summary>
        /// <returns></returns>
        public static bool CKYIsOnCircle(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3, DataType.BasicDataType.vector x_vVector4, double x_nLinePrecision, double x_nCirclePrecision, ref double x_nActualError)
        {
            bool l_bResult = false;

            DataType.BasicDataType.vector           l_vCenterPoint;
            DataType.BasicDataType.PlaneCoefficient l_pCoefficient;
            double l_nRadius;

            l_bResult = CreatPlane(x_vVector1, x_vVector2, x_vVector3, x_nLinePrecision, out l_pCoefficient);
            if (l_bResult == true)
            {
                l_bResult = IsOnPlane(x_vVector4, l_pCoefficient, x_nCirclePrecision);
                if (l_bResult == true)
                {
                    l_bResult = CreatCircle(x_vVector1, x_vVector2, x_vVector3, out l_vCenterPoint, out l_nRadius);
                    double l_nDistance             = BasicMathTool.VectorDistance(l_vCenterPoint, x_vVector4);
                    double l_nPointToPlaneDistance = PointToPlaneDistance(x_vVector4, l_pCoefficient);
                    double l_nActualError          = Math.Sqrt(Math.Pow(l_nDistance - l_nRadius, 2) + Math.Pow(l_nPointToPlaneDistance, 2));
                    if (l_nActualError <= x_nCirclePrecision)
                    //if (Math.Abs(l_nDistance - l_nRadius) <= x_nPrecision)
                    {
                        l_bResult      = true;
                        x_nActualError = l_nActualError;
                    }
                    else
                    {
                        l_bResult = false;
                    }
                }
            }
            return(l_bResult);
        }
Esempio n. 3
0
        /// <summary>
        ///  判断点是否在圆上
        /// </summary>
        /// <returns></returns>
        public static bool IsOnCircle(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3, DataType.BasicDataType.vector x_vVector4, double x_nLinePrecision, double x_nPrecision)
        {
            bool l_bResult = false;

            DataType.BasicDataType.vector           l_vCenterPoint;
            DataType.BasicDataType.PlaneCoefficient l_pCoefficient;
            double l_nRadius;

            l_bResult = CreatPlane(x_vVector1, x_vVector2, x_vVector3, x_nLinePrecision, out l_pCoefficient);
            if (l_bResult == true)
            {
                l_bResult = IsOnPlane(x_vVector4, l_pCoefficient, x_nPrecision);
                if (l_bResult == true)
                {
                    l_bResult = CreatCircle(x_vVector1, x_vVector2, x_vVector3, out l_vCenterPoint, out l_nRadius);
                    double l_nDistance = BasicMathTool.VectorDistance(l_vCenterPoint, x_vVector4);
                    if (Math.Abs(l_nDistance - l_nRadius) <= x_nPrecision)
                    {
                        l_bResult = true;
                    }
                    else
                    {
                        l_bResult = false;
                    }
                }
            }
            return(l_bResult);
        }
Esempio n. 4
0
        /// <summary>
        /// 空间点到直线距离 test success
        /// </summary>
        /// <param name="x_vVector1"></param>
        /// <param name="x_vVector2"></param>
        /// <param name="x_vVector3"></param>
        /// <returns></returns>
        public static double PointToLineDistance(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3)
        {
            double l_nDistance = 0;

            //d=l_x/l_y
            DataType.BasicDataType.vector l_MA  = BasicMathTool.SubVector(x_vVector1, x_vVector2);
            DataType.BasicDataType.vector l_MB  = BasicMathTool.SubVector(x_vVector1, x_vVector3);
            DataType.BasicDataType.vector l_MAB = BasicMathTool.Vect3CrossPord(l_MB, l_MA);
            double l_x = BasicMathTool.VectorNorm(l_MAB);
            double l_y = BasicMathTool.VectorNorm(l_MA);

            l_nDistance = (double)l_x / l_y;
            return(l_nDistance);
        }
Esempio n. 5
0
        /// <summary>
        /// 判断三点共线
        /// </summary>
        /// <returns>true:共线</returns>false:不共线
        public static bool ThreeColline(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3, double x_nPrecision)
        {
            bool l_bOk = false;

            DataType.BasicDataType.vector l_V1, l_V2;
            l_V1 = BasicMathTool.SubVector(x_vVector1, x_vVector2);
            l_V2 = BasicMathTool.SubVector(x_vVector2, x_vVector3);
            if (Math.Abs(l_V2.x * l_V1.y - l_V1.x * l_V2.y) <= x_nPrecision & Math.Abs(l_V2.x * l_V1.z - l_V2.z * l_V1.x) <= x_nPrecision)
            {
                l_bOk = true;
            }
            else
            {
                l_bOk = false;
            }
            return(l_bOk);
        }
Esempio n. 6
0
        //public static bool CKYIsOnCircle(DataType.StaubliRobotData.St_PointRx x_pPoint1, DataType.StaubliRobotData.St_PointRx x_pPoint2, DataType.StaubliRobotData.St_PointRx x_pPoint3, DataType.StaubliRobotData.St_PointRx x_pPoint4, double x_nLinePrecision, double x_nCirclePrecision,ref double x_nPreviousRadius,ref double x_nSubsequentRadius, ref double x_nActualError)
        //{
        //    bool l_bResult = false;
        //    DataType.BasicDataType.vector l_v1, l_v2, l_v3, l_v4;
        //    l_v1 = Point2Vector(x_pPoint1);
        //    l_v2 = Point2Vector(x_pPoint2);
        //    l_v3 = Point2Vector(x_pPoint3);
        //    l_v4 = Point2Vector(x_pPoint4);
        //    l_bResult = CKYIsOnCircle(l_v1, l_v2, l_v3, l_v4, x_nLinePrecision, x_nCirclePrecision,ref x_nPreviousRadius,ref x_nSubsequentRadius ,ref x_nActualError);
        //    return l_bResult;
        //}

        ///// <summary>
        /////  判断点是否在圆上(CKY标准,点到圆弧最近距离)
        ///// </summary>
        ///// <returns></returns>
        //public static bool CKYIsOnCircle(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3, DataType.BasicDataType.vector x_vVector4, double x_nLinePrecision, double x_nCirclePrecision,ref double x_nPreviousRadius,ref double x_nSubsequentRadius ,ref double x_nActualError)
        //{
        //    bool l_bResult = false;
        //    DataType.BasicDataType.vector l_vCenterPoint1,l_vCenterPoint2;
        //    DataType.BasicDataType.PlaneCoefficient l_pCoefficient;
        //    double l_nRadius1,l_nRadius2;
        //    l_bResult = CreatPlane(x_vVector1, x_vVector2, x_vVector3, x_nLinePrecision, out l_pCoefficient);
        //    if (l_bResult == true)
        //    {
        //        l_bResult = IsOnPlane(x_vVector4, l_pCoefficient, x_nCirclePrecision);
        //        if (l_bResult == true)
        //        {
        //            l_bResult = CreatCircle(x_vVector1, x_vVector2, x_vVector3, out l_vCenterPoint1, out l_nRadius1);
        //            l_bResult = CreatCircle(x_vVector1, x_vVector2, x_vVector4, out l_vCenterPoint2, out l_nRadius2);
        //            if (l_bResult == true)
        //            {
        //                //double l_nActualError = BasicMathTool.VectorDistance(l_vCenterPoint1, l_vCenterPoint2);
        //                double l_nActualError = Math.Abs(l_nRadius1 - l_nRadius2);
        //                if (l_nActualError <= x_nCirclePrecision)
        //                {
        //                    l_bResult = true;
        //                    x_nActualError = l_nActualError;
        //                    x_nPreviousRadius = l_nRadius1;
        //                    x_nSubsequentRadius = l_nRadius2;
        //                }
        //                else
        //                {
        //                    l_bResult = false;
        //                }
        //            }
        //            else
        //            {
        //                l_bResult = false;
        //            }
        //        }
        //    }
        //    return l_bResult;
        //}

        /// <summary>
        /// 三点建立平面 test success
        /// </summary>
        /// <param name="x_vVector1"></param>
        /// <param name="x_vVector2"></param>
        /// <param name="x_vVector3"></param>
        /// <param name="x_pPlaneCoefficient"></param>
        /// <returns>false 三点共线
        /// </returns>true 成功
        public static bool CreatPlane(DataType.BasicDataType.vector x_vVector1, DataType.BasicDataType.vector x_vVector2, DataType.BasicDataType.vector x_vVector3, double x_nLinePrecision, out DataType.BasicDataType.PlaneCoefficient x_pPlaneCoefficient)
        {
            x_pPlaneCoefficient.a = 0;
            x_pPlaneCoefficient.b = 0;
            x_pPlaneCoefficient.c = 0;
            x_pPlaneCoefficient.d = 0;
            bool l_bResule = false;

            //l_bResule = ThreeColline(x_vVector1, x_vVector2, x_vVector3,x_nLinePrecision);
            l_bResule = ThreeColline(x_vVector1, x_vVector2, x_vVector3, 0.0000001);
            if (l_bResule == false)
            {
                DataType.BasicDataType.vector l_vVector1 = BasicMathTool.SubVector(x_vVector1, x_vVector2);
                DataType.BasicDataType.vector l_vVector2 = BasicMathTool.SubVector(x_vVector1, x_vVector3);
                DataType.BasicDataType.vector l_vVector3 = BasicMathTool.Vect3CrossPord(l_vVector1, l_vVector2);
                x_pPlaneCoefficient.a = l_vVector3.x;
                x_pPlaneCoefficient.b = l_vVector3.y;
                x_pPlaneCoefficient.c = l_vVector3.z;
                x_pPlaneCoefficient.d = -x_pPlaneCoefficient.a * x_vVector1.x - x_pPlaneCoefficient.b * x_vVector1.y - x_pPlaneCoefficient.c * x_vVector1.z;
                l_bResule             = true;
            }
            return(l_bResule);
        }