private void ComputeLong(EdgePlacerSystem.AnnotatedNode first, EdgePlacerSystem.AnnotatedNode last, LongPlanData data) { var lenPerCount = data.Length / data.Count; _vertices.Clear(); _vertices.Add(first); var time = 0f; var prev = data.Curve.Sample(0); var lengthElapsed = 0d; const float timeStep = .001f; for (var i = 1; i < data.Count; i++) { while (lengthElapsed < lenPerCount * i) { time += timeStep; var curr = data.Curve.Sample(time); lengthElapsed += Vector3D.Distance(prev, curr); prev = curr; } _vertices.Add(CreateVertex(prev)); } _vertices.Add(last); }
private MatrixD ComputeVertexMatrix(EdgePlacerSystem.AnnotatedNode vert, int index) { if (vert.Existing != null) { return(vert.Existing.Matrix); } var prevPos = (index - 1) >= 0 ? (EdgePlacerSystem.AnnotatedNode?)_vertices[index - 1] : null; var nextPos = (index + 1) < _vertices.Count ? (EdgePlacerSystem.AnnotatedNode?)_vertices[index + 1] : null; var tan = Vector3D.Zero; if (prevPos.HasValue) { var t = (vert.Position - prevPos.Value.Position).SafeNormalized(); tan += tan.Dot(t) < 0 ? -t : t; } if (nextPos.HasValue) { var t2 = (vert.Position - nextPos.Value.Position).SafeNormalized(); tan += tan.Dot(t2) < 0 ? -t2 : t2; } if (prevPos.HasValue != nextPos.HasValue) { // try Quadratic bez with control point equidistance from both nodes. if (prevPos?.Existing != null) { var pp = prevPos.Value.Existing; tan = CurveExtensions.ExpandToCubic(pp.Position, pp.Position + pp.Tangent, vert.Position, RailConstants.LongBezControlLimit) - vert.Position; } else if (nextPos?.Existing != null) { var pp = nextPos.Value.Existing; tan = CurveExtensions.ExpandToCubic(pp.Position, pp.Position + pp.Tangent, vert.Position, RailConstants.LongBezControlLimit) - vert.Position; } } if (!tan.IsValid() || tan.LengthSquared() < 1e-3f) { tan = Vector3D.Cross(vert.Up, vert.Up.Shifted()); } tan.SafeNormalized(); return(MatrixD.CreateWorld(vert.Position, tan, vert.Up)); }
public LongPlanData(EdgePlacerBehavior epa, EdgePlacerSystem.AnnotatedNode first, EdgePlacerSystem.AnnotatedNode last) { epa._vertices.Clear(); epa._vertices.Add(first); epa._vertices.Add(last); M1 = epa.ComputeVertexMatrix(first, 0); M2 = epa.ComputeVertexMatrix(last, 1); Curve = PrepareSphericalBez(M1, M2); var length = Curve.LengthAuto(epa.PlacedDefinition.Distance.Min / 8); Length = length; var minCount = (int)Math.Ceiling(length / epa.PlacedDefinition.Distance.Max); var maxCount = (int)Math.Floor(length / epa.PlacedDefinition.Distance.Min); var idealCount = (int)Math.Round(length / epa.PlacedDefinition.PreferredDistance); Count = MathHelper.Clamp(idealCount, minCount, maxCount); }