三. 向量计算(为SolidWorks写扩展方法)

一. SolidWorks中的向量计算和变换

• SolidWorks 提供了一个IMathUtility 来提供几何计算和矩阵变换.但在有些情况下不能满足计算需求或者计算起来比较繁琐.
  此处使用 C# 的扩展方法扩展了一部分向量计算, 也重新定义了向量对象以便进行更方便有效的操作.

二. 使用方法

• 将文章后面的代码配置如下

文件架构

• 引用自己类的命名空间边可以使用扩展的api

MathUtility 扩展方法:

MathUtility 扩展方法

转换为自定义向量类将可以使用更多的计算方法:

转换为自定义向量类将可以使用更多的计算方法

image.png

三. 配置方法- 自己创建向量类和向量计算方法

2.1 添加一个通用配置类来配置一些计算常量的 IMathUtility的访问

    public class MathUtil
    {
        public static MathUtility swMathUtility { get; set; }

        public const double Epsilon = 2.2204460492503131e-016;

        /// 

        /// 每弧度代表的角度
        /// 
        public const double Rad2Deg = (180.0 / System.Math.PI);
        public const double ZeroTolerance = 1e-08;

        public static double Clamp(double f, double low, double high)
        {
            return (f < low) ? low : (f > high) ? high : f;
        }
    }

2.2 添加一个Vector2 的平面向量类,具体实现如下

public class Vector2
    {
        public double X { get; set; }

        public double Y { get; set; }

        public Vector2(double x,double y)
        {
            X = x;
            Y = y;
        }

        /// 

        /// 角度转换为弧度
        /// 
        /// 
        /// 
        public static Vector2 FromAngleRad(double angle)
        {
            return new Vector2(System.Math.Cos(angle), System.Math.Sin(angle));
        }

        /// 

        /// 长度平方值
        /// 
        public double LengthSquared
        {
            get { return X * X + Y * Y; }
        }

        /// 

        /// 长度
        /// 
        public double Length
        {
            get { return (double)System.Math.Sqrt(LengthSquared); }
        }

        /// 

        /// 单位化
        /// 
        /// 
        /// 
        public double Normalize(double epsilon = MathUtil.Epsilon)
        {
            double length = Length;
            if (length > epsilon) {
                double invLength = 1.0 / length;
                X *= invLength;
                Y *= invLength;
            } else {
                length = 0;
                X = Y = 0;
            }
            return length;
        }

        public bool IsFinite
        {
            get { double f = X + Y; return double.IsNaN(f) == false && double.IsInfinity(f) == false; }
        }

        /// 

        /// 点乘
        /// 
        /// 
        /// 
        public double Dot(Vector2 v2)
        {
            return X * v2.X + Y * v2.Y;
        }

        /// 

        /// 叉乘
        /// 
        /// 
        /// 
        public double Cross(Vector2 v2)
        {
            return X * v2.Y - Y * v2.X;
        }
        /// 

        /// 求与另外一个向量角度--单位度deg
        /// 
        /// 
        /// 
        public double AngleD(Vector2 v2)
        {
            double fDot = MathUtil.Clamp(Dot(v2), -1, 1);
            return System.Math.Acos(fDot) * MathUtil.Rad2Deg;
        }
        /// 

        /// 两个向量角度--单位度deg
        /// 
        /// 
        /// 
        /// 
        public static double AngleD(Vector2 v1, Vector2 v2)
        {
            return v1.AngleD(v2);
        }
        /// 

        /// 求与另外一个向量角度--单位弧度Rad
        /// 
        /// 
        /// 
        public double AngleR(Vector2 v2)
        {
            double fDot = MathUtil.Clamp(Dot(v2), -1, 1);
            return System.Math.Acos(fDot);
        }
        /// 

        /// 两个向量角度--单位弧度Rad
        /// 
        /// 
        /// 
        /// 
        public static double AngleR(Vector2 v1, Vector2 v2)
        {
            return v1.AngleR(v2);
        }

        /// 

        /// 圆整
        /// 
        /// 
        public void Round(int nDecimals)
        {
            X = System.Math.Round(X, nDecimals);
            Y = System.Math.Round(Y, nDecimals);
        }

        public double DistanceSquared(Vector2 v2)
        {
            double dx = v2.X - X, dy = v2.Y - Y;
            return dx * dx + dy * dy;
        }
        public double Distance(Vector2 v2)
        {
            double dx = v2.X - X, dy = v2.Y - Y;
            return System.Math.Sqrt(dx * dx + dy * dy);
        }

        public void Set(Vector2 o)
        {
            X = o.X; Y = o.Y;
        }
        public void Set(double fX, double fY)
        {
            X= fX; Y = fY;
        }
        public void Add(Vector2 o)
        {
           X+= o.X; Y += o.Y;
        }
        public void Subtract(Vector2 o)
        {
            X -= o.X; Y -= o.Y;
        }
    }

2.3 添加三维向量计算类Vector

• 具体实现如下

 public class Vector3
    {

        private double x;
        private double y;
        private double z;

        public double X { get => x; set => x = value; }
        public double Y { get => y; set => y = value; }
        public double Z { get => z; set => z = value; }
        public static Vector3 Zero { get; internal set; }
        public static Vector3 UnitZ { get; internal set; }

        public Vector3(double[] arrayData)
        {
            if (arrayData.Length >= 3)
            {
                this.X = arrayData[0];
                this.Y = arrayData[1];
                this.Z = arrayData[2];
            }
        }

        public Vector3(double v1, double v2, double v3)
        {
            this.X = v1;
            this.Y = v2;
            this.Z = v3;
        }

        public Vector2 xy
        {
            get { return new Vector2(x, y); }
            set { x = value.X; y = value.Y; }
        }
        public Vector2 xz
        {
            get { return new Vector2(x, z); }
            set { x = value.X; z = value.Y; }
        }
        public Vector2 yz
        {
            get { return new Vector2(y, z); }
            set { y = value.X; z = value.Y; }
        }

        public double LengthSquared
        {
            get { return x * x + y * y + z * z; }
        }
        public double Length
        {
            get { return System.Math.Sqrt(LengthSquared); }
        }


        public double[] ToDoubles()
        {
            return new double[] { X, Y, Z };
        }

        public MathPoint ToSwMathPoint(MathUtility math = null)
        {
            if (math == null)
            {
                math = MathUtil.swMathUtility;
            }
            if (math == null)
            {
                throw new NullReferenceException("MathUtility未将对象引用到对象的实例");
            }
            return math.CreatePoint(ToDoubles());
        }

        /// 

        /// 求三个坐标的绝对值长度和
        /// 
        public double LengthL1
        {
            get { return System.Math.Abs(x) + System.Math.Abs(y) + System.Math.Abs(z); }
        }

        public double Max
        {
            get { return System.Math.Max(x, Math.Max(y, z)); }
        }
        public double Min
        {
            get { return Math.Min(x, Math.Min(y, z)); }
        }

        public double MaxAbs
        {
            get { return Math.Max(Math.Abs(x), Math.Max(Math.Abs(y), Math.Abs(z))); }
        }
        public double MinAbs
        {
            get { return Math.Min(Math.Abs(x), Math.Min(Math.Abs(y), Math.Abs(z))); }
        }

        public Vector3 Abs
        {
            get { return new Vector3(Math.Abs(x), Math.Abs(y), Math.Abs(z)); }
        }

        public double Normalize(double epsilon = MathUtil.Epsilon)
        {
            double length = Length;
            if (length > epsilon)
            {
                double invLength = 1.0 / length;
                x *= invLength;
                y *= invLength;
                z *= invLength;
            }
            else
            {
                length = 0;
                x = y = z = 0;
            }
            return length;
        }

        public Vector3 Unit(double epsilon = MathUtil.Epsilon)
        {
           
                double length = Length;
                if (length > MathUtil.Epsilon)
                {
                    double invLength = 1.0 / length;
                    return new Vector3(x * invLength, y * invLength, z * invLength);
                }
                else
                    return Vector3.Zero;
            
        }

        public bool IsNormalized
        {
            get { return Math.Abs((x * x + y * y + z * z) - 1) < MathUtil.ZeroTolerance; }
        }

        public bool IsFinite
        {
            get { double f = x + y + z; return double.IsNaN(f) == false && double.IsInfinity(f) == false; }
        }

        public void Round(int nDecimals)
        {
            x = Math.Round(x, nDecimals);
            y = Math.Round(y, nDecimals);
            z = Math.Round(z, nDecimals);
        }

        public double Dot(Vector3 v2)
        {
            return x * v2.x + y * v2.y + z * v2.z;
        }
        public double Dot(ref Vector3 v2)
        {
            return x * v2.x + y * v2.y + z * v2.z;
        }
        public static double Dot(Vector3 v1, Vector3 v2)
        {
            return v1.Dot(ref v2);
        }

        public Vector3 Cross(Vector3 v2)
        {
            return new Vector3(
                y * v2.z - z * v2.y,
                z * v2.x - x * v2.z,
                x * v2.y - y * v2.x);
        }
        public Vector3 Cross(ref Vector3 v2)
        {
            return new Vector3(
                y * v2.z - z * v2.y,
                z * v2.x - x * v2.z,
                x * v2.y - y * v2.x);
        }

        public static Vector3 Cross(Vector3 v1, Vector3 v2)
        {
            return v1.Cross(ref v2);
        }

        /// 

        /// 叉乘后单位化
        /// 
        /// 
        /// 
        public Vector3 UnitCross(ref Vector3 v2)
        {
            Vector3 n = new Vector3(
                y * v2.z - z * v2.y,
                z * v2.x - x * v2.z,
                x * v2.y - y * v2.x);
            n.Normalize();
            return n;
        }

        /// 

        /// 叉乘后单位化
        /// 
        /// 
        /// 
        public Vector3 UnitCross(Vector3 v2)
        {
            return UnitCross(ref v2);
        }

        public double AngleD(Vector3 v2)
        {
            double fDot = MathUtil.Clamp(Dot(v2), -1, 1);
            return Math.Acos(fDot) * MathUtil.Rad2Deg;
        }
        public static double AngleD(Vector3 v1, Vector3 v2)
        {
            return v1.AngleD(v2);
        }
        public double AngleR(Vector3 v2)
        {
            double fDot = MathUtil.Clamp(Dot(v2), -1, 1);
            return Math.Acos(fDot);
        }
        public static double AngleR(Vector3 v1, Vector3 v2)
        {
            return v1.AngleR(v2);
        }

        public double DistanceSquared(Vector3 v2)
        {
            double dx = v2.x - x, dy = v2.y - y, dz = v2.z - z;
            return dx * dx + dy * dy + dz * dz;
        }
        public double DistanceSquared(ref Vector3 v2)
        {
            double dx = v2.x - x, dy = v2.y - y, dz = v2.z - z;
            return dx * dx + dy * dy + dz * dz;
        }

        public double Distance(Vector3 v2)
        {
            double dx = v2.x - x, dy = v2.y - y, dz = v2.z - z;
            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }
        public double Distance(ref Vector3 v2)
        {
            double dx = v2.x - x, dy = v2.y - y, dz = v2.z - z;
            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        public void Set(Vector3 o)
        {
            x = o.x; y = o.y; z = o.z;
        }
        public void Set(double fX, double fY, double fZ)
        {
            x = fX; y = fY; z = fZ;
        }

        public Vector3 Add(Vector3 vector3)
        {
            return new Vector3(x + vector3.x, y + vector3.y, z + vector3.z);
        }

        public Vector3 Scale(double length)
        {
            return new Vector3(x * length, y * length, z * length);
        }
    }

四. 配置SolidWorks 计算类的扩展方法

4.1 扩展 MathUtility

public static class MathUtilityExtension
    {
        public static MathPoint PointEx(this MathUtility math, double[] value)
        {
            return math.CreatePoint(value);
        }

        public static MathVector ZAxis(this MathUtility math)
        {
            return math.CreateVector(new double[] { 0, 0, 1 });
        }

        public static MathVector XAxis(this MathUtility math)
        {
            return math.CreateVector(new double[] { 1, 0, 0 });
        }

        public static MathVector YAxis(this MathUtility math)
        {
            return math.CreateVector(new double[] { 0, 1, 0 });
        }
    }

4.2 扩展 MathPoint

• 具体实现如下

public static class MathPointExtension
    {
        /// 

        /// 转换到Vector3
        /// 
        /// 
        /// 
        public static Vector3 ToVector3(this MathPoint mathPoint)
        {
            return new Vector3((double[])mathPoint.ArrayData);
        }

        /// 

        /// 计算在某轴上的投影
        /// 
        /// 
        /// 
        /// 
        /// 
        public static MathPoint Project(this IMathPoint point, IMathPoint origin, IMathVector axis)
        {
            var a = (IMathVector)point.Subtract(origin);
            var t = a.Project(axis);
            var v = (MathVector)axis.Scale(t);
            return (MathPoint)origin.AddVector(v);
        }

        public static MathVector SubtractTs(this IMathPoint a, IMathPoint b)
        {
            return (MathVector)a.Subtract(b);
        }

        public static MathVector SubtractTs(this IMathVector a, IMathVector b)
        {
            return (MathVector)a.Subtract(b);
        }

        /// 

        /// 此点在 XY Plane 中的角度
        /// 
        /// 
        /// 
        public static double Angle2D(this IMathPoint p)
        {
            var pData = p.ArrayData.CastArray();
            var angle = System.Math.Atan2(pData[1], pData[0]);
            return angle < 0.0 ? angle + 2 * System.Math.PI : angle;
        }
    }

4.3 扩展 MathVector

 public static class MathVectorExtension
    {

        /// 

        /// a X b => 投影到 b
        /// gives the multiplier for b which would be the projection of a on b
        /// 
        /// 
        /// 
        /// 
        public static double Project(this IMathVector a, IMathVector b)
        {
            return a.Dot(b) / (b.Dot(b));
        }


        public static Vector3 ToVector3(this MathVector mathvector)
        {
            return new Vector3((double[])mathvector.ArrayData);
        }

        public static double LengthOfProjectionXY(this double[] vector)
        {
            return System.Math.Sqrt(vector.Take(2).Sum(c => System.Math.Pow(c, 2)));
        }

        /// 

        /// 两个向量之间的角度
        /// 
        /// 
        /// 
        /// 
        public static double AngleBetweenVectors(this IMathVector v0, IMathVector v1)
        {
            if (((double[])v0.ArrayData).Length == ((double[])v1.ArrayData).Length)
            {
                var sign = System.Math.Sign(((IMathVector)(v0.Cross(v1))).ArrayData.CastArray()[2]);
                var ret = System.Math.Acos(v0.Dot(v1) / (v0.GetLength() * v1.GetLength()));
                return sign * ret;
            }
            throw new Exception("Vectors must have the same dimension!");
        }
    }