using System; namespace CaeGlobals { public class Vec3D { // Variables public double X; public double Y; public double Z; // Properties public double Len { get { return Math.Sqrt(X * X + Y * Y + Z * Z); } } public double Len2 { get { return X * X + Y * Y + Z * Z; } } public double[] Coor { get { return new double[] { X, Y, Z }; } set { X = value[0]; Y = value[1]; Z = value[2]; } } // Constructors public Vec3D() { X = 0; Y = 0; Z = 0; } public Vec3D(double x0, double y0, double z0) { X = x0; Y = y0; Z = z0; } public Vec3D(double[] xyz) { X = xyz[0]; Y = xyz[1]; Z = xyz[2]; } public Vec3D(Vec3D vec) { X = vec.X; Y = vec.Y; Z = vec.Z; } // Methods public double Normalize() { double n = Math.Sqrt(X * X + Y * Y + Z * Z); if (n > 0 && n != 1) { X /= n; Y /= n; Z /= n; } return n; } public void Abs() { if (X < 0) X = -X; if (Y < 0) Y = -Y; if (Z < 0) Z = -Z; } public Vec3D DeepCopy() { return new Vec3D(this); } public double[] CoorRounded(int numDigits) { return new double[] { Math.Round(X, numDigits), Math.Round(Y, numDigits), Math.Round(Z, numDigits) }; } #region STATIC UTILITIES // Copy public static void Copy(double[] vec1, ref double[] vec) { vec[0] = vec1[0]; vec[1] = vec1[1]; vec[2] = vec1[2]; } public static void CopyNormalize(double[] vec1, ref double[] vec) { vec[0] = vec1[0]; vec[1] = vec1[1]; vec[2] = vec1[2]; Vec3D.Normalize(ref vec); } // Normalize vector public static void Normalize(ref double[] vec) { double n = Math.Sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]); if (n > 0) { vec[0] = vec[0] / n; vec[1] = vec[1] / n; vec[2] = vec[2] / n; } } // Get normalized direction vector public static void GetNorDirVec(double[] p1, double[] p2, ref double[] vec) { vec[0] = p2[0] - p1[0]; vec[1] = p2[1] - p1[1]; vec[2] = p2[2] - p1[2]; Vec3D.Normalize(ref vec); } public static Vec3D GetNorDirVec(double[] p1, double[] p2) { Vec3D vector = new Vec3D(); vector.X = p2[0] - p1[0]; vector.Y = p2[1] - p1[1]; vector.Z = p2[2] - p1[2]; vector.Normalize(); return vector; } public static Vec3D GetDirVec(double[] p1, double[] p2) { Vec3D vector = new Vec3D(); vector.X = p2[0] - p1[0]; vector.Y = p2[1] - p1[1]; vector.Z = p2[2] - p1[2]; return vector; } // Add public static Vec3D operator +(Vec3D v1, Vec3D v2) { return new Vec3D(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); } // Subtract public static Vec3D operator -(Vec3D v1, Vec3D v2) { return new Vec3D(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); } // Multiply public static Vec3D operator *(double a, Vec3D v) { return new Vec3D(a * v.X, a * v.Y, a * v.Z); } public static Vec3D operator *(Vec3D v, double a) { return new Vec3D(a * v.X, a * v.Y, a * v.Z); } // Cross product public static void CrossProduct(Vec3D u, Vec3D v, ref Vec3D vec) { vec.X = u.Y * v.Z - v.Y * u.Z; vec.Y = v.X * u.Z - u.X * v.Z; vec.Z = u.X * v.Y - v.X * u.Y; } public static Vec3D CrossProduct(Vec3D u, Vec3D v) { Vec3D vector = new Vec3D(); vector.X = u.Y * v.Z - v.Y * u.Z; vector.Y = v.X * u.Z - u.X * v.Z; vector.Z = u.X * v.Y - v.X * u.Y; return vector; } public static void CrossProduct(double[] u, double[] v, ref double[] vec) { vec[0] = u[1] * v[2] - v[1] * u[2]; vec[1] = v[0] * u[2] - u[0] * v[2]; vec[2] = u[0] * v[1] - v[0] * u[1]; } // Dot product public static double DotProduct(Vec3D u, Vec3D v) { return (u.X * v.X + u.Y * v.Y + u.Z * v.Z); } // Angle public static double GetAngleAtP2Deg(Vec3D p1, Vec3D p2, Vec3D p3) { Vec3D v1 = p1 - p2; Vec3D v2 = p3 - p2; v1.Normalize(); v2.Normalize(); // double dot = Vec3D.DotProduct(v1, v2); if (dot > 1) dot = 1; else if (dot < -1) dot = -1; double angle = Math.Acos(dot); // Convert the angle from radians to degrees angle *= (180.0 / Math.PI); // return angle; } // Circle public static void GetCircle(Vec3D baseV1, Vec3D baseV2, Vec3D baseV3, out double r, out Vec3D center, out Vec3D axis) { Vec3D n12 = baseV1 - baseV2; Vec3D n21 = baseV2 - baseV1; Vec3D n23 = baseV2 - baseV3; Vec3D n32 = baseV3 - baseV2; Vec3D n13 = baseV1 - baseV3; Vec3D n31 = baseV3 - baseV1; Vec3D n12xn23 = CrossProduct(n12, n23); // r = (n12.Len * n23.Len * n31.Len) / (2 * n12xn23.Len); // double denominator = 2 * n12xn23.Len2; double alpha = n23.Len2 * DotProduct(n12, n13) / denominator; double beta = n13.Len2 * DotProduct(n21, n23) / denominator; double gama = n12.Len2 * DotProduct(n31, n32) / denominator; // center = alpha * baseV1 + beta * baseV2 + gama * baseV3; // axis = CrossProduct(n21, n32); axis.Normalize(); } public static void GetCircle(Vec3D baseV1, Vec3D baseV2, Vec3D baseV3, out double r, out double arcAngleDeg, out Vec3D center, out Vec3D axis) { Vec3D n12 = baseV1 - baseV2; Vec3D n21 = baseV2 - baseV1; Vec3D n23 = baseV2 - baseV3; Vec3D n32 = baseV3 - baseV2; Vec3D n13 = baseV1 - baseV3; Vec3D n31 = baseV3 - baseV1; Vec3D n12xn23 = Vec3D.CrossProduct(n12, n23); // r = (n12.Len * n23.Len * n31.Len) / (2 * n12xn23.Len); // double denominator = 2 * n12xn23.Len2; double alpha = n23.Len2 * DotProduct(n12, n13) / denominator; double beta = n13.Len2 * DotProduct(n21, n23) / denominator; double gama = n12.Len2 * DotProduct(n31, n32) / denominator; // center = alpha * baseV1 + beta * baseV2 + gama * baseV3; // axis = CrossProduct(n21, n32); axis.Normalize(); // Vec3D r1 = baseV1 - center; Vec3D r2 = baseV2 - center; Vec3D r3 = baseV3 - center; r1.Normalize(); r2.Normalize(); r3.Normalize(); arcAngleDeg = Math.Acos(DotProduct(r1, r2)) * 180 / Math.PI; arcAngleDeg += Math.Acos(DotProduct(r2, r3)) * 180 / Math.PI; } public static void GetCircleR(Vec3D baseV1, Vec3D baseV2, Vec3D baseV3, out double r) { Vec3D n12 = baseV1 - baseV2; Vec3D n23 = baseV2 - baseV3; Vec3D n31 = baseV3 - baseV1; Vec3D n12xn23 = CrossProduct(n12, n23); // r = (n12.Len * n23.Len * n31.Len) / (2 * n12xn23.Len); } #endregion } }