public static JointParameters ComputeEdgeParameters(AnnotatedNode from, AnnotatedNode to)
{
var result = new JointParameters();
var dirNext = to.Position - from.Position;
result.Length = dirNext.Normalize();
var dotFrom = Math.Abs(Vector3.Normalize(from.Tangent).Dot((Vector3)dirNext));
var dotTo = Math.Abs(Vector3.Normalize(to.Tangent).Dot((Vector3)dirNext));
result.BendRadians = Math.Acos(Math.Min(dotFrom, dotTo));
var planet = MyGamePruningStructureSandbox.GetClosestPlanet(@from.Position);
// ReSharper disable once InvertIf
if (planet?.PositionComp != null)
{
var center = planet.PositionComp.WorldVolume.Center;
var elevationA = Vector3D.Distance(from.Position, center);
var elevationB = Vector3D.Distance(to.Position, center);
var deltaElevation = elevationB - elevationA;
var grade = deltaElevation / result.Length;
result.Grade = grade;
}
return(result);
}
/// <summary>
/// Defines the movement characteristics.
/// </summary>
private void DefineMovementCharacteristics()
{
StartTime = JointParameters.Times[0];
EndTime = JointParameters.Times.Last();
InitializeQueryVars();
for (var i = 0; i < NumAllJoints; i++)
{
var j = SimulationJoints[i];
if (j.TypeOfJoint == JointType.R)
{
continue;
}
j.OrigSlidePosition = JointParameters[0.0][oIOSJ[i], 6];
j.MinSlidePosition = JointParameters.Parameters.Min(jp => jp[oIOSJ[i], 6]);
j.MaxSlidePosition = JointParameters.Parameters.Max(jp => jp[oIOSJ[i], 6]);
}
if (LessThanFullRotation())
{
//if the crank input couldn't rotate all the way around,then this is easy...
CycleType = CycleTypes.LessThanFullCycle;
return;
}
// if the simulation did go all the way around then, we should be careful to ensure is connects correctly.
var cyclePeriodTime = Constants.FullCircle / Math.Abs(InputSpeed);
if (DoesMechanismRepeatOnCrankCycle(cyclePeriodTime))
{
StartTime = 0.0;
EndTime = cyclePeriodTime;
CycleType = CycleTypes.OneCycle;
while (JointParameters.Times.Last() >= EndTime)
{
var time = JointParameters.Times.Last();
var parameters = JointParameters.Parameters.Last();
JointParameters.RemoveAt(JointParameters.LastIndex);
time -= cyclePeriodTime;
JointParameters.AddNearBegin(time, parameters);
parameters = LinkParameters.Parameters.Last();
LinkParameters.RemoveAt(LinkParameters.LastIndex);
LinkParameters.AddNearEnd(time, parameters);
}
while (JointParameters.Times[0] < 0.0)
{
var time = JointParameters.Times[0];
var parameters = JointParameters.Parameters[0];
JointParameters.RemoveAt(0);
time += cyclePeriodTime;
JointParameters.AddNearEnd(time, parameters);
parameters = LinkParameters.Parameters[0];
LinkParameters.RemoveAt(0);
LinkParameters.AddNearEnd(time, parameters);
}
}
else
{
CycleType = CycleTypes.MoreThanOneCycle;
}
InitializeQueryVars();
}
/// <summary>
/// Simulates the specified joints.
/// </summary>
/// <param name="joints">The joints.</param>
/// <param name="links">The links.</param>
/// <param name="Forward">The forward.</param>
private void Simulate(List <Joint> joints, List <Link> links, Boolean Forward)
{
var timeStep = (Forward == (InputSpeed > 0)) ? FixedTimeStep : -FixedTimeStep;
var startingPosChange = (Forward == (InputSpeed > 0))
? Constants.DefaultStepSize
: -Constants.DefaultStepSize;
if (InputJoint.TypeOfJoint == JointType.P)
{
startingPosChange *= AverageLength;
}
var maxLengthError = MaxSmoothingError * AverageLength;
var currentTime = 0.0;
Boolean validPosition;
var posFinder = new PositionFinder(joints, links, gearsData, simulationDriveIndex);
var velSolver = new VelocitySolver(joints, links, FirstInputJointIndex, simulationDriveIndex, InputLinkIndex,
InputSpeed, gearsData, AverageLength);
var accelSolver = new AccelerationSolver(joints, links, FirstInputJointIndex, simulationDriveIndex,
InputLinkIndex, InputSpeed, gearsData, AverageLength);
do
{
#region Find Next Positions
if (useErrorMethod)
{
var k = 0;
double upperError;
do
{
timeStep = startingPosChange / InputSpeed;
NumericalPosition(timeStep, joints, links);
validPosition = posFinder.DefineNewPositions(startingPosChange, ref IsDyadic);
upperError = posFinder.PositionError - maxLengthError;
if (validPosition && upperError < 0)
{
startingPosChange *= Constants.ErrorSizeIncrease;
// startingPosChange = startingPosChange * maxLengthError / (maxLengthError + upperError);
}
else
{
if (Math.Abs(startingPosChange * Constants.ConservativeErrorEstimation * 0.5) <
Constants.MinimumStepSize)
{
validPosition = false;
}
else
{
startingPosChange *= Constants.ConservativeErrorEstimation * 0.5;
}
}
} while ((!validPosition || upperError > 0) && k++ < Constants.MaxItersInPositionError
&&
(Math.Abs(startingPosChange * Constants.ConservativeErrorEstimation * 0.5) >=
Constants.MinimumStepSize));
//var tempStep = startingPosChange;
//startingPosChange = (Constants.ErrorEstimateInertia * prevStep + startingPosChange) / (1 + Constants.ErrorEstimateInertia);
//prevStep = tempStep;
}
else
{
// this next function puts the xNumerical and yNumerical values in the joints
NumericalPosition(timeStep, joints, links);
var delta = InputSpeed * timeStep;
// this next function puts the x and y values in the joints
validPosition = posFinder.DefineNewPositions(delta, ref IsDyadic);
}
#endregion
if (validPosition)
{
if (Forward == (InputSpeed > 0))
{
lock (InputRange)
{
InputRange[1] = links[InputLinkIndex].Angle;
}
}
else
{
lock (InputRange)
{
InputRange[0] = links[InputLinkIndex].Angle;
}
}
#region Find Velocities for Current Position
// this next functions puts the vx and vy values as well as the vx_unit and vy_unit in the joints
if (!velSolver.Solve())
{
Status += "Instant Centers could not be found at" + currentTime + ".";
NumericalVelocity(timeStep, joints, links);
}
#endregion
#region Find Accelerations for Current Position
// this next functions puts the ax and ay values in the joints
if (!accelSolver.Solve())
{
Status += "Analytical acceleration could not be found at" + currentTime + ".";
NumericalAcceleration(timeStep, joints, links);
}
#endregion
currentTime += timeStep;
var jointParams = WriteJointStatesVariablesToMatrixAndToLast(joints);
var linkParams = WriteLinkStatesVariablesToMatrixAndToLast(links);
if (Forward == (InputSpeed > 0))
{
lock (JointParameters)
JointParameters.AddNearEnd(currentTime, jointParams);
lock (LinkParameters)
LinkParameters.AddNearEnd(currentTime, linkParams);
}
else
{
lock (JointParameters)
JointParameters.AddNearBegin(currentTime, jointParams);
lock (LinkParameters)
LinkParameters.AddNearBegin(currentTime, linkParams);
}
}
} while (validPosition && LessThanFullRotation());
}
public BodyPartInfo(List <PrimitiveSpec> shapes, JointParameters jointParameters)
{
Shapes = shapes;
JointParameters = jointParameters;
}
