/// <summary>
/// Set the overrides of thrusters to match MoveForceRatio. Should be called on game thread.
/// </summary>
public void SetOverrides(ref DirectionGrid MoveForceRatio)
{
if (MoveForceRatio.vector.X >= 0f)
{
SetOverrides(Base6Directions.Direction.Right, MoveForceRatio.vector.X);
ClearOverrides(Base6Directions.Direction.Left);
}
else
{
ClearOverrides(Base6Directions.Direction.Right);
SetOverrides(Base6Directions.Direction.Left, -MoveForceRatio.vector.X);
}
if (MoveForceRatio.vector.Y >= 0f)
{
SetOverrides(Base6Directions.Direction.Up, MoveForceRatio.vector.Y);
ClearOverrides(Base6Directions.Direction.Down);
}
else
{
ClearOverrides(Base6Directions.Direction.Up);
SetOverrides(Base6Directions.Direction.Down, -MoveForceRatio.vector.Y);
}
if (MoveForceRatio.vector.Z >= 0f)
{
SetOverrides(Base6Directions.Direction.Backward, MoveForceRatio.vector.Z);
ClearOverrides(Base6Directions.Direction.Forward);
}
else
{
ClearOverrides(Base6Directions.Direction.Backward);
SetOverrides(Base6Directions.Direction.Forward, -MoveForceRatio.vector.Z);
}
}
/// <summary>
/// Apply the calculated force ratios to the controller.
/// </summary>
public void MoveAndRotate()
{
CheckGrid();
//Log.DebugLog("moveForceRatio: " + moveForceRatio + ", rotateForceRatio: " + rotateForceRatio + ", move length: " + moveForceRatio.Length(), "MoveAndRotate()");
// if all the force ratio values are 0, Autopilot has to stop the ship, MoveAndRotate will not
if (m_moveForceRatio == Vector3.Zero && m_rotateTargetVelocity == Vector3.Zero)
{
//Log.DebugLog("Stopping the ship, move: " + m_moveForceRatio + ", rotate: " + m_rotateTargetVelocity);
// should not toggle dampeners, grid may just have landed
MoveAndRotateStop(false);
return;
}
if (m_moveForceRatio != Vector3.Zero && CheckStuck(WriggleAfter))
{
ulong upWoMove = Globals.UpdateCount - m_lastAccel;
if (m_rotateForceRatio != Vector3.Zero)
{
// wriggle
float wriggle = (upWoMove - WriggleAfter) * 0.0001f;
//Log.DebugLog("wriggle: " + wriggle + ", updates w/o moving: " + upWoMove);
m_rotateForceRatio.X += (0.5f - (float)Globals.Random.NextDouble()) * wriggle;
m_rotateForceRatio.Y += (0.5f - (float)Globals.Random.NextDouble()) * wriggle;
m_rotateForceRatio.Z += (0.5f - (float)Globals.Random.NextDouble()) * wriggle;
}
// increase force
m_moveForceRatio *= 1f + (upWoMove - WriggleAfter) * 0.1f;
}
// clamp values
Vector3 moveControl;
moveControl.X = MathHelper.Clamp(m_moveForceRatio.X, -1f, 1f);
moveControl.Y = MathHelper.Clamp(m_moveForceRatio.Y, -1f, 1f);
moveControl.Z = MathHelper.Clamp(m_moveForceRatio.Z, -1f, 1f);
Vector3 rotateControl = -m_rotateForceRatio; // control torque is opposite of move indicator
Vector3.ClampToSphere(ref rotateControl, 1f);
m_stopped = false;
MyShipController controller = Block.Controller;
MyAPIGateway.Utilities.TryInvokeOnGameThread(() => {
//Log.DebugLog("rotate control: " + rotateControl + ", previous: " + m_prevRotateControl + ", delta: " + (rotateControl - m_prevRotateControl), "MoveAndRotate()");
if (Block.Controller.GridGyroSystem != null)
{
DirectionGrid gridRotate = ((DirectionBlock)rotateControl).ToGrid(Block.CubeBlock);
Block.Controller.GridGyroSystem.ControlTorque = gridRotate;
}
else
{
Log.DebugLog("No gyro system");
}
MyEntityThrustComponent thrustComponent = Block.CubeGrid.Components.Get <MyEntityThrustComponent>();
if (thrustComponent != null)
{
DirectionGrid gridMove = ((DirectionBlock)moveControl).ToGrid(Block.CubeBlock);
thrustComponent.ControlThrust = gridMove;
}
else
{
Log.DebugLog("No thrust component");
}
});
}
public void Update()
{
// sometimes called from Game Thread when world is loaded, has not been an issue so far
//Log.DebugLog("Not on autopilot thread: " + ThreadTracker.ThreadName + ", from: " + callerPath + "." + callerMember, Logger.severity.ERROR, condition: !ThreadTracker.ThreadName.StartsWith("Autopilot"));
if (Globals.UpdateCount < m_nextUpdate)
{
return;
}
m_nextUpdate = Globals.UpdateCount + ShipAutopilot.UpdateFrequency;
//for (int i = m_totalThrustForce.Length - 1; i >= 0; i--)
// m_totalThrustForce[i] = 0f;
CapableAtmo = false; CapableSpace = false;
Vector3D position = myGrid.GetPosition();
bool first = true;
Vector3 worldGravity = Vector3.Zero;
m_planetAtmos = null;
m_airDensity = 0f;
foreach (MyPlanet planet in Globals.AllPlanets())
{
if (planet.IsPositionInGravityWell(position))
{
if (first)
{
first = false;
m_gravStrength = planet.GetGravityMultiplier(position) * 9.81f;
Vector3 direction = planet.GetWorldGravityNormalized(ref position);
worldGravity = m_gravStrength.Value * direction;
}
else
{
worldGravity += planet.GetWorldGravity(position);
m_gravStrength = null;
}
if (planet.HasAtmosphere)
{
m_airDensity += planet.GetAirDensity(position);
m_planetAtmos = planet;
}
}
}
CalcForceInDirection(Base6Directions.Direction.Forward);
CalcForceInDirection(Base6Directions.Direction.Backward);
CalcForceInDirection(Base6Directions.Direction.Up);
CalcForceInDirection(Base6Directions.Direction.Down);
CalcForceInDirection(Base6Directions.Direction.Left);
CalcForceInDirection(Base6Directions.Direction.Right);
if (worldGravity.LengthSquared() < 0.01f)
{
//Log.DebugLog("Not in gravity well", "Update()");
WorldGravity = Vector3.Zero;
LocalGravity = Vector3.Zero;
m_gravStrength = 0f;
return;
}
WorldGravity = worldGravity;
LocalGravity = new DirectionGrid()
{
vector = Vector3.Transform(worldGravity, myGrid.WorldMatrixNormalizedInv.GetOrientation())
};
Vector3 gravityReactRatio = Vector3.Zero;
if (LocalGravity.vector.X > 0)
{
gravityReactRatio.X = -LocalGravity.vector.X * myGrid.Physics.Mass / GetForceInDirection(Base6Directions.Direction.Left);
}
else
{
gravityReactRatio.X = -LocalGravity.vector.X * myGrid.Physics.Mass / GetForceInDirection(Base6Directions.Direction.Right);
}
if (LocalGravity.vector.Y > 0)
{
gravityReactRatio.Y = -LocalGravity.vector.Y * myGrid.Physics.Mass / GetForceInDirection(Base6Directions.Direction.Down);
}
else
{
gravityReactRatio.Y = -LocalGravity.vector.Y * myGrid.Physics.Mass / GetForceInDirection(Base6Directions.Direction.Up);
}
if (LocalGravity.vector.Z > 0)
{
gravityReactRatio.Z = -LocalGravity.vector.Z * myGrid.Physics.Mass / GetForceInDirection(Base6Directions.Direction.Forward);
}
else
{
gravityReactRatio.Z = -LocalGravity.vector.Z * myGrid.Physics.Mass / GetForceInDirection(Base6Directions.Direction.Backward);
}
GravityReactRatio = gravityReactRatio;
Log.DebugLog("Gravity: " + WorldGravity + ", local: " + LocalGravity + ", react: " + gravityReactRatio + ", air density: " + m_airDensity);
}
private void CalcRotate_InGravity(RelativeDirection3F direction)
{
//Log.DebugLog("entered CalcRotate_InGravity(RelativeDirection3F direction)", "CalcRotate_InGravity()");
float secondaryAccel = Thrust.SecondaryForce / Block.Physics.Mass;
RelativeDirection3F fightGrav = RelativeDirection3F.FromLocal(Block.CubeGrid, -Thrust.LocalGravity.vector);
if (secondaryAccel > Thrust.GravityStrength)
{
// secondary thrusters are strong enough to fight gravity
if (Vector3.Dot(direction.ToLocalNormalized(), fightGrav.ToLocalNormalized()) > 0f)
{
// direction is away from gravity
Log.DebugLog("Facing primary towards direction(" + direction.ToLocalNormalized() + "), rolling secondary away from gravity(" + fightGrav.ToLocalNormalized() + ")");
CalcRotate(Thrust.Standard.LocalMatrix, direction, fightGrav, gravityAdjusted: true);
return;
}
else
{
// direction is towards gravity
Log.DebugLog("Facing secondary away from gravity(" + fightGrav.ToLocalNormalized() + "), rolling primary towards direction(" + direction.ToLocalNormalized() + ")");
CalcRotate(Thrust.Gravity.LocalMatrix, fightGrav, direction, gravityAdjusted: true);
return;
}
}
// secondary thrusters are not strong enough to fight gravity
if (secondaryAccel > 1f)
{
Log.DebugLog("Facing primary towards gravity(" + fightGrav.ToLocalNormalized() + "), rolling secondary towards direction(" + direction.ToLocalNormalized() + ")");
CalcRotate(Thrust.Standard.LocalMatrix, fightGrav, direction, gravityAdjusted: true);
return;
}
// helicopter
float primaryAccel = Math.Max(Thrust.PrimaryForce / Block.Physics.Mass - Thrust.GravityStrength, 1f); // obviously less than actual value but we do not need maximum theoretical acceleration
DirectionGrid moveAccel = m_moveAccel != Vector3.Zero ? ((DirectionBlock)m_moveAccel).ToGrid(Block.CubeBlock) : LinearVelocity.ToGrid(Block.CubeGrid) * -0.5f;
if (moveAccel.vector.LengthSquared() > primaryAccel * primaryAccel)
{
//Log.DebugLog("move accel is over available acceleration: " + moveAccel.vector.Length() + " > " + primaryAccel, "CalcRotate_InGravity()");
moveAccel = Vector3.Normalize(moveAccel.vector) * primaryAccel;
}
//Log.DebugLog("Facing primary away from gravity and towards direction, moveAccel: " + moveAccel + ", fight gravity: " + fightGrav.ToLocal() + ", direction: " + direction.ToLocalNormalized() + ", new direction: " + (moveAccel + fightGrav.ToLocal()));
Vector3 dirV = moveAccel + fightGrav.ToLocal();
direction = RelativeDirection3F.FromLocal(Block.CubeGrid, dirV);
CalcRotate(Thrust.Standard.LocalMatrix, direction, gravityAdjusted: true);
Vector3 fightGravDirection = fightGrav.ToLocal() / Thrust.GravityStrength;
// determine acceleration needed in forward direction to move to desired altitude or remain at current altitude
float projectionMagnitude = Vector3.Dot(dirV, fightGravDirection) / Vector3.Dot(Thrust.Standard.LocalMatrix.Forward, fightGravDirection);
Vector3 projectionDirection = ((DirectionGrid)Thrust.Standard.LocalMatrix.Forward).ToBlock(Block.CubeBlock);
m_moveForceRatio = projectionDirection * projectionMagnitude * Block.CubeGrid.Physics.Mass / Thrust.PrimaryForce;
Log.DebugLog("changed moveForceRatio, projectionMagnitude: " + projectionMagnitude + ", projectionDirection: " + projectionDirection + ", moveForceRatio: " + m_moveForceRatio);
}
