public static Curve InterpolatePointsToCurve
(this IModeler modeler
, List <Vector3> points
, double chordTolerance
, bool simplify = true
, bool closedCurve = false)
{
points = CurveExtensions.FilterOutShortLines(points, 1e-5).ToList();
if (closedCurve)
{
points.Add(points.First());
}
var lines = points
.Buffer(2, 1)
.Where(b => b.Count == 2)
.Select(ps => modeler.CreateTrimmedLine(ps[0], ps[1]))
.Cast <ICurve>()
.ToArray();
var curve = modeler.MergeCurves(lines);
if (simplify)
{
curve = curve.SimplifyBCurve(chordTolerance);
}
return(curve);
}
private static CubicSphericalCurve PrepareSphericalBez(MatrixD m1, MatrixD m2)
{
CurveExtensions.AlignFwd(ref m1, ref m2);
return(new CubicSphericalCurve(
MyGamePruningStructureSandbox.GetClosestPlanet(m1.Translation)?.PositionComp.WorldVolume.Center ??
Vector3D.Zero, m1, m2));
}
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));
}
private void Simulate()
{
var root = Entity;
while (root != null && root.Physics == null)
{
root = root.Parent;
}
if (root?.Physics == null || root.Physics.IsStatic)
{
if (!RailConstants.Debug.DrawBogiePhysics)
{
return;
}
var drawPivot = Entity.GetPosition() + 4 * Entity.PositionComp.WorldMatrix.Up;
var colorTarget = new Vector4(1, 1, 0, 1);
MySimpleObjectDraw.DrawLine(Entity.GetPosition(), drawPivot, DebugMtl, ref colorTarget,
.01f);
return;
}
// Clean good controllers
{
_goodControllers.Clear();
FindAttachedAnimControllers(Entity, _goodControllers);
_removedControllers.Clear();
foreach (var k in _activeControllers)
{
if (!_goodControllers.Contains(k))
{
_removedControllers.Add(k);
}
}
foreach (var k in _removedControllers)
{
_activeControllers.Remove(k);
}
_removedControllers.Clear();
var sync = false;
foreach (var k in _goodControllers)
{
if (_activeControllers.Add(k))
{
sync = true;
}
}
if (sync)
{
AddScheduledCallback(ControllerSync, 30);
}
}
var physics = root.Physics;
var pivotWorld = Entity.PositionComp.WorldMatrix.Translation;
var best = double.MaxValue;
var bestTangent = Vector3.Zero;
Edge bestEdge = null;
RailSegmentComponent bestEdgeSegment;
RailSegmentDefinition.RailSegmentCaps?bestEdgeCaps = null;
float bestTime = 0;
using (var e = Graph.Edges.SortedByDistance(pivotWorld))
while (e.MoveNext())
{
if (Math.Sqrt(e.Current.DistanceSquared) - TotalBias > best)
{
break;
}
var edge = (Edge)e.Current.UserData;
if (edge.Curve == null)
{
continue;
}
var edgeSegment = edge.Owner.Entity.Components.Get <RailSegmentComponent>();
var edgeCaps = edgeSegment?.Definition.CapabilitiesFor(edge.Owner.Entity.GetBuildRatio());
if (edgeSegment == null || !edgeCaps.HasValue)
{
continue; // no capabilities at this stage
}
float t0 = 0, t1 = 1;
CurveExtensions.NearestPoint(edge.Curve, pivotWorld, 16, ref t0, ref t1);
var p0 = edge.Curve.Sample(t0);
var p1 = edge.Curve.Sample(t1);
var dir = p1 - p0;
var factor = (float)MathHelper.Clamp(dir.Dot(pivotWorld - p0) / dir.LengthSquared(), 0, 1);
var t = t0 + (t1 - t0) * factor;
var pos = edge.Curve.Sample(t);
var dist = Vector3D.Distance(pos, pivotWorld);
if (dist - TotalBias > best)
{
continue;
}
var tangent = (Vector3)edge.Curve.SampleDerivative(t);
tangent.Normalize();
const float switchingEpsilon = 0.25f;
var switched = false;
if (t < switchingEpsilon)
{
switched = edge.From.IsSwitchedTo(edge.To);
}
else if (t >= 1 - switchingEpsilon)
{
switched = edge.To.IsSwitchedTo(edge.From);
}
if (switched)
{
dist -= SwitchingDistanceBias + AlignmentTangentBias;
}
else
{
dist -= AlignmentTangentBias * Math.Abs(Entity.PositionComp.WorldMatrix.Forward.Dot(tangent));
}
if (RailConstants.Debug.DrawBogieEdges)
{
edge.Draw(0, 1, switched ? new Vector4(0, 1, 0, 1) : new Vector4(1, 0, 1, 1), 2);
}
// ReSharper disable once InvertIf
if (dist < best)
{
best = dist;
bestTangent = tangent;
bestEdge = edge;
bestEdgeSegment = edgeSegment;
bestEdgeCaps = edgeCaps;
bestTime = t;
}
}
var selfVelocity = physics.GetVelocityAtPoint(pivotWorld);
SetAnimVar(SpeedZVar, selfVelocity.Dot((Vector3)Entity.PositionComp.WorldMatrix.Forward));
if (bestEdge == null)
{
if (!RailConstants.Debug.DrawBogiePhysics)
{
return;
}
var drawPivot = pivotWorld + 4 * Entity.PositionComp.WorldMatrix.Up;
var colorTarget = new Vector4(1, 0, 0, 1);
MySimpleObjectDraw.DrawLine(pivotWorld, drawPivot, DebugMtl, ref colorTarget,
.01f);
return;
}
if (best > Definition.DetachDistance)
{
if (!RailConstants.Debug.DrawBogiePhysics)
{
return;
}
var drawPivot = pivotWorld + 4 * Entity.PositionComp.WorldMatrix.Up;
var colorTarget = new Vector4(1, 0, 1, 1);
MySimpleObjectDraw.DrawLine(pivotWorld, drawPivot, DebugMtl, ref colorTarget,
.01f);
return;
}
var up = (Vector3)Vector3D.Lerp(bestEdge.From.Up, bestEdge.To.Up, bestTime);
// Not aligned vertically, abort
if (Entity.PositionComp.WorldMatrix.Up.Dot(up) < 0.5)
{
return;
}
if (Entity.PositionComp.WorldMatrix.Forward.Dot(bestTangent) < 0)
{
bestTangent = -bestTangent;
}
var curvePosition = bestEdge.Curve.Sample(bestTime) + up * Definition.VerticalOffset;
var normal = Vector3.Cross(bestTangent, up);
normal.Normalize();
up = Vector3.Cross(normal, bestTangent);
up.Normalize();
var impulse = Vector3.Zero;
var allowDeactivation = true;
var effectiveMass = root.Physics.Mass;
var inertiaTensor = root.Physics.InertiaTensor;
var qCurrent = Quaternion.CreateFromRotationMatrix(Entity.PositionComp.WorldMatrix);
var qDesired = Quaternion.CreateFromRotationMatrix(Matrix.CreateWorld(Vector3.Zero, bestTangent, up));
var qConj = Quaternion.Multiply(Quaternion.Conjugate(qCurrent), qDesired);
var localAngularDesired = 2 * qConj.W * new Vector3(qConj.X, qConj.Y, qConj.Z);
if (localAngularDesired.LengthSquared() > .01f)
{
allowDeactivation = false;
}
var desiredAngular = Vector3.Transform(localAngularDesired, qCurrent) * 2;
var rotApply = desiredAngular;
var angularImpulse = Vector3.TransformNormal(desiredAngular - 0.25f * physics.AngularVelocity, inertiaTensor) *
Definition.OrientationConvergenceFactor /
MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
var com = physics.GetCenterOfMassWorld();
// a) spring joint along normal to get dot(normal, (pivot*matrix - position)) == 0
var err = (Vector3)(curvePosition - pivotWorld);
// preemptive up force to counteract gravity.
var gravityHere = MyGravityProviderSystem.CalculateTotalGravityInPoint(pivotWorld);
impulse += Vector3.Dot(gravityHere, up) * up * physics.Mass * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
impulse += SolveImpulse(err, normal, physics.LinearVelocity, effectiveMass);
// b) half spring joint along up to get dot(up, (pivot*matrix - position)) >= 0
impulse += SolveImpulse(err, up, physics.LinearVelocity, effectiveMass, 1); // only up force
if (err.LengthSquared() > .01f)
{
allowDeactivation = false;
}
var braking = false;
// Hack until I fix EquinoxCore
if (Definition.MaxVelocity > 0 && (_powerObserver.IsPowered || Definition.NeedsPower == PowerObserver.RequiredPowerEnum.None))
{
var cvel = physics.LinearVelocity.Dot(bestTangent);
var velocityMod = 0f;
// get nearest character
var totalControl = 0f;
var controllers = 0;
var sprinters = 0;
foreach (var player in _activeControllers)
{
var component = player?.Get <MyCharacterMovementComponent>();
if (component == null)
{
continue;
}
var control = Vector3.TransformNormal(component.MoveIndicator, player.WorldMatrix).Dot(bestTangent);
totalControl += control;
controllers++;
var speed = new MyMovementSpeed {
Forward = 1f
};
var speedMod = component.ApplyMovementSpeedEffects(speed);
velocityMod = Math.Max(velocityMod, speedMod.Forward);
if (!component.WantsSprint)
{
continue;
}
var stats = component.Container?.Get <MyEntityStatComponent>();
stats?.AddEffect(SprintingEffect);
sprinters++;
}
if (controllers > 0)
{
braking |= Math.Sign(cvel) != Math.Sign(totalControl) && Math.Abs(cvel) > 0.01 &&
Math.Abs(totalControl) > 0.01;
SetAnimVar(BrakingVar, braking ? 1 : 0);
_powerFactor = MathHelper.Clamp(_powerFactor * (1 - PowerSmooth) + totalControl * PowerSmooth, -1, 1);
if (Math.Abs(totalControl) < .01f)
{
_powerFactor = 0;
}
var velocityLimit = Definition.MaxVelocity * velocityMod;
velocityLimit = Math.Min(velocityLimit, bestEdgeCaps.Value.MaxSpeed);
var forceFactorBase = MathHelper.Clamp(1 - Math.Abs(cvel) / velocityLimit, 0, 1);
var forceFactorControl = Math.Abs(_powerFactor);
if (sprinters > 0)
{
forceFactorControl *= 5 * ((float)sprinters) / controllers;
}
forceFactorControl *= .9f + controllers * .1f;
var dir = Math.Sign(_powerFactor);
if (dir != Math.Sign(cvel))
{
forceFactorBase = 1;
}
var force = Definition.MaxForce * forceFactorControl * forceFactorBase * dir * bestTangent *
MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
physics.AddForce(MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE, force, com,
Vector3.Zero);
}
}
Vector3 frictiveImpulse;
{
var frictiveNormalForce = Math.Max(0, impulse.Dot(up) / MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS);
var frictiveCoefficient = Math.Max(Definition.CoefficientOfFriction, braking ? Definition.BrakingCoefficientOfFriction : 0);
if (physics.LinearVelocity.LengthSquared() > .01f)
{
frictiveCoefficient *= 0.75f;
}
var frictiveForce = frictiveCoefficient * (bestEdgeCaps?.Friction ?? 1) * frictiveNormalForce;
// clamp frictive impulse to at-max stopping.
var tangentMomentumAfterUpdate =
(physics.Mass * (physics.LinearVelocity + gravityHere * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS) + impulse).Dot(bestTangent);
var frictiveFloatImpulse = frictiveForce * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
if (frictiveFloatImpulse > Math.Abs(tangentMomentumAfterUpdate))
{
// Stationary, allow deactivation
frictiveImpulse = -tangentMomentumAfterUpdate * bestTangent;
}
else
{
frictiveImpulse = -Math.Sign(tangentMomentumAfterUpdate) * frictiveFloatImpulse * bestTangent;
allowDeactivation = false;
}
impulse += frictiveImpulse;
}
if (!MyAPIGateway.Utilities.IsDedicated && RailConstants.Debug.DrawBogiePhysics)
{
var drawPivot = pivotWorld + 4 * up;
var colorTarget = Vector4.One;
MySimpleObjectDraw.DrawLine(pivotWorld, curvePosition, DebugMtl, ref colorTarget,
.01f);
var colorMarker = new Vector4(0, 1, 0, 1);
MySimpleObjectDraw.DrawLine(drawPivot, drawPivot + frictiveImpulse * 10, DebugMtl, ref colorMarker,
.01f);
var colorImpulse = new Vector4(1, 0, 0, 1);
MySimpleObjectDraw.DrawLine(drawPivot, drawPivot + impulse, DebugMtl, ref colorImpulse,
.01f);
var colorAngImpulse = new Vector4(0, 0, 1, 1);
MySimpleObjectDraw.DrawLine(drawPivot, drawPivot + rotApply, DebugMtl, ref colorAngImpulse,
.01f);
}
if (allowDeactivation)
{
// physics.Sleep();
// if (!physics.IsActive)
// return;
}
physics.AddForce(MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE, impulse, com, Vector3.Zero);
physics.AddForce(MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE, Vector3.Zero, pivotWorld, angularImpulse);
}
private static CubicCurve PrepareNormalBez(MatrixD m1, MatrixD m2)
{
CurveExtensions.AlignFwd(ref m1, ref m2);
return(new CubicCurve(m1, m2));
}
private bool TryFindEdge(out FindEdgeResult result)
{
result = default;
var prevEdgeReal = _prevEdge?.GetEdge(_graph);
var position = Entity.PositionComp.WorldMatrix.Translation;
result.Score = Definition.DetachDistance + TotalBias;
using (var e = _graph.Edges.SortedByDistance(position))
while (e.MoveNext())
{
if (Math.Sqrt(e.Current.DistanceSquared) - TotalBias > result.Score)
{
break;
}
var edge = (Edge)e.Current.UserData;
if (edge.Curve == null)
{
continue;
}
var edgeSegment = RailSegmentFor(edge);
var edgeCaps = edgeSegment?.Definition.CapabilitiesFor(edge.Owner.Entity.GetBuildRatio());
if (edgeSegment == null || !edgeCaps.HasValue)
{
continue; // no capabilities at this stage
}
if (!CanUseEdge(prevEdgeReal, edge))
{
continue;
}
float t0 = 0, t1 = 1;
CurveExtensions.NearestPoint(edge.Curve, position, 16, ref t0, ref t1);
var p0 = edge.Curve.Sample(t0);
var p1 = edge.Curve.Sample(t1);
var dir = p1 - p0;
var factor = (float)MathHelper.Clamp(dir.Dot(position - p0) / dir.LengthSquared(), 0, 1);
var t = t0 + (t1 - t0) * factor;
var pos = edge.Curve.Sample(t);
var score = Vector3D.Distance(pos, position);
if (score - TotalBias > result.Score)
{
continue;
}
var tangent = (Vector3)edge.Curve.SampleDerivative(t);
tangent.Normalize();
const float switchingEpsilon = 0.25f;
var switched = false;
if (t < switchingEpsilon)
{
switched = edge.From.IsSwitchedTo(edge.To);
}
else if (t >= 1 - switchingEpsilon)
{
switched = edge.To.IsSwitchedTo(edge.From);
}
var forwardDotTangent = Entity.PositionComp.WorldMatrix.Forward.Dot(tangent);
if (forwardDotTangent < 0)
{
forwardDotTangent = -forwardDotTangent;
tangent = -tangent;
}
if (switched)
{
score -= SwitchingDistanceBias + AlignmentTangentBias;
}
else
{
score -= AlignmentTangentBias * forwardDotTangent;
}
if (RailConstants.Debug.DrawBogieEdges)
{
edge.Draw(0, 1, switched ? new Vector4(0, 1, 0, 1) : new Vector4(1, 0, 1, 1), 2);
}
// ReSharper disable once InvertIf
if (score < result.Score)
{
result.Score = score;
result.Tangent = tangent;
result.Edge = edge;
result.SegmentCaps = edgeCaps.Value;
result.EdgeFactor = t;
}
}
if (result.Edge == null || result.Score > Definition.DetachDistance)
{
return(false);
}
result.Up = (Vector3)Vector3D.Lerp(result.Edge.From.Up, result.Edge.To.Up, result.EdgeFactor);
// Not aligned vertically, abort
if (Entity.PositionComp.WorldMatrix.Up.Dot(result.Up) < 0.5)
{
return(false);
}
result.Position = result.Edge.Curve.Sample(result.EdgeFactor) + result.Up * Definition.VerticalOffset;
result.Normal = Vector3.Cross(result.Tangent, result.Up);
result.Normal.Normalize();
result.Up = Vector3.Cross(result.Normal, result.Tangent);
result.Up.Normalize();
return(true);
}