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);
}