Vector3d point(double ang) { var maxr = Math.Max(MajorRadius, MinorRadius); var minr = Math.Min(MajorRadius, MinorRadius); MajorRadius = maxr; MinorRadius = minr; var mtr4 = Matrix4d.CreateFromAxisAngle(Normal, ang); var res = Vector4d.Transform(new Vector4d(norm), mtr4); var realAng = Vector3d.CalculateAngle(res.Xyz, RefDir); var rad = MajorRadius * MinorRadius / (Math.Sqrt(Math.Pow(MajorRadius * Math.Sin(realAng), 2) + Math.Pow(MinorRadius * Math.Cos(realAng), 2))); res *= rad; return(Location + res.Xyz); }
internal Vector3d CalcConjugateNormal() { var nm = tring1.GetPlane().Normal.Normalized(); var point0 = AuxPoint0; var point1 = AuxPoint1; var neg = -nm; var axis = (edge.End - edge.Start).Normalized(); PlaneSurface pp = new PlaneSurface() { Position = edge.Start, Normal = axis }; var prj0 = pp.ProjPoint(point0) - edge.Start; var prj1 = pp.ProjPoint(point1) - edge.Start; var crs1 = Vector3d.Cross(prj0, prj1) / prj0.Length / prj1.Length; var ang2 = Vector3d.CalculateAngle(prj0, prj1); if (crs1.Length > 1e-8) { axis = -crs1.Normalized(); } var mtr = Matrix4d.CreateFromAxisAngle(axis, ang2); var mtr2 = Matrix4d.CreateFromAxisAngle(axis, -ang2); var trans = Vector3d.Transform(neg, mtr2); var check = Vector3d.Transform(prj0.Normalized(), mtr); var check2 = Vector3d.Transform(prj1.Normalized(), mtr); if (!(Vector3d.Cross(tring2.GetPlane().Normal, trans).Length < 1e-8 || Vector3d.Cross(tring2.GetPlane().Normal, -trans).Length < 1e-8)) { DebugHelper.Error?.Invoke("inconsistent normal was calculated"); } normal1 = trans; return(trans); }
public static double Angle(this Vector3d a, Vector3d b) { return(Vector3d.CalculateAngle(a, b)); }
public static void DrawArrow( Action <DrawPen, Vector3d, Vector3d> DrawLine, DrawPen pen, Vector3d pt0, Vector3d pt1, ArrowTypes type, ArrowPos pos, double len, double width) { DrawLine(pen, pt0, pt1); Vector3d d = pt1 - pt0; double dl = d.Length; if (dl < 0.00001) { return; } Vector3d tmp = new Vector3d(dl, 0, 0); double angle = Vector3d.CalculateAngle(tmp, d); Vector3d normal = CadMath.CrossProduct(tmp, d); // 回転軸 if (normal.Length < 0.0001) { normal = new Vector3d(0, 0, 1); } else { normal = normal.UnitVector(); normal = CadMath.Normal(tmp, d); } CadQuaternion q = CadQuaternion.RotateQuaternion(normal, -angle); CadQuaternion r = q.Conjugate(); ArrowHead a; if (pos == ArrowPos.END || pos == ArrowPos.START_END) { a = ArrowHead.Create(type, ArrowPos.END, len, width); a.Rotate(q, r); a += pt1; DrawLine(pen, a.p0.vector, a.p1.vector); DrawLine(pen, a.p0.vector, a.p2.vector); DrawLine(pen, a.p0.vector, a.p3.vector); DrawLine(pen, a.p0.vector, a.p4.vector); } if (pos == ArrowPos.START || pos == ArrowPos.START_END) { a = ArrowHead.Create(type, ArrowPos.START, len, width); a.Rotate(q, r); a += pt0; DrawLine(pen, a.p0.vector, a.p1.vector); DrawLine(pen, a.p0.vector, a.p2.vector); DrawLine(pen, a.p0.vector, a.p3.vector); DrawLine(pen, a.p0.vector, a.p4.vector); } }