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