public void Update_torqueAccelRatio(Vector3 command, Vector3 ratio)
{
if (Globals.UpdateCount >= m_nextCheck_attachedGrids)
CheckAttachedGrids();
float mass = myGrid.Physics.Mass;
if (!dirty_torqueAccelRatio)
{
dirty_torqueAccelRatio = mass > m_mass;
myLogger.debugLog(dirty_torqueAccelRatio, "mass increased from " + m_mass + " to " + mass, "Update_torqueAccelRatio()");
}
else
myLogger.debugLog("torqueAccelRatio is dirty", "Update_torqueAccelRatio()");
m_mass = mass;
for (int i = 0; i < 3; i++)
{
// where ratio is from damping, ignore it
if (Math.Abs(command.GetDim(i)) < 0.01f)
continue;
float dim = ratio.GetDim(i);
if (dim.IsValid() && (dirty_torqueAccelRatio || dim > torqueAccelRatio))
{
if (dim > 0f)
{
myLogger.debugLog("torqueAccelRatio changed from " + torqueAccelRatio + " to " + dim, "Update_torqueAccelRatio()", Logger.severity.DEBUG);
torqueAccelRatio = dim;
dirty_torqueAccelRatio = false;
}
else // caused by bumping into things
myLogger.debugLog("dim <= 0 : " + dim, "Update_torqueAccelRatio()", Logger.severity.DEBUG);
}
}
}
/// <summary>
/// Calculates the force necessary to move the grid.
/// </summary>
/// <param name="block">To get world position from.</param>
/// <param name="destPoint">The world position of the destination</param>
/// <param name="destVelocity">The speed of the destination</param>
/// <param name="landing">Puts an emphasis on not overshooting the target.</param>
public void CalcMove(PseudoBlock block, Vector3 destPoint, Vector3 destVelocity, bool landing = false)
{
CheckGrid();
// using world vectors
if (NavSet.Settings_Current.CollisionAvoidance)
{
myPathfinder.TestPath(destPoint, landing);
if (!myPathfinder.CanMove)
{
myLogger.debugLog("Pathfinder not allowing movement", "CalcMove()");
return;
}
}
Vector3 destDisp = destPoint - block.WorldPosition;
Vector3 velocity = Block.CubeGrid.Physics.LinearVelocity;
// switch to using local vectors
Matrix positionToLocal = Block.CubeBlock.WorldMatrixNormalizedInv;
Matrix directionToLocal = positionToLocal.GetOrientation();
destDisp = Vector3.Transform(destDisp, directionToLocal);
destVelocity = Vector3.Transform(destVelocity, directionToLocal);
velocity = Vector3.Transform(velocity, directionToLocal);
float distance = destDisp.Length();
NavSet.Settings_Task_NavWay.Distance = distance;
Vector3 targetVelocity = MaximumVelocity(destDisp) * 0.5f;
// project targetVelocity onto destination direction (take shortest path)
Vector3 destDir = destDisp / distance;
targetVelocity = Vector3.Dot(targetVelocity, destDir) * destDir;
// apply relative speed limit
float relSpeedLimit = NavSet.Settings_Current.SpeedMaxRelative;
if (landing)
{
float landingSpeed = distance * 0.5f;
if (relSpeedLimit > landingSpeed)
relSpeedLimit = landingSpeed;
}
if (relSpeedLimit < float.MaxValue)
{
float tarSpeedSq_1 = targetVelocity.LengthSquared();
if (tarSpeedSq_1 > relSpeedLimit * relSpeedLimit)
{
targetVelocity *= relSpeedLimit / (float)Math.Sqrt(tarSpeedSq_1);
myLogger.debugLog("imposing relative speed limit: " + relSpeedLimit + ", targetVelocity: " + targetVelocity, "CalcMove()");
}
}
targetVelocity += destVelocity;
// apply speed limit
float tarSpeedSq = targetVelocity.LengthSquared();
float speedRequest = NavSet.Settings_Current.SpeedTarget;
if (tarSpeedSq > speedRequest * speedRequest)
targetVelocity *= speedRequest / (float)Math.Sqrt(tarSpeedSq);
Vector3 accel = targetVelocity - velocity;
moveForceRatio = ToForceRatio(accel);
// dampeners
bool enableDampeners = false;
for (int i = 0; i < 3; i++)
{
// if target velocity is close to 0, use dampeners
float targetDim = targetVelocity.GetDim(i);
if (targetDim < 0.1f && targetDim > -0.1f)
{
//myLogger.debugLog("close to 0, i: " + i + ", targetDim: " + targetDim, "CalcMove()");
moveForceRatio.SetDim(i, 0);
enableDampeners = true;
continue;
}
// if there is not enough force available for braking, use dampeners
float forceRatio = moveForceRatio.GetDim(i);
if (forceRatio < 1f && forceRatio > -1f)
continue;
float velDim = velocity.GetDim(i);
if (velDim < 1f && velDim > -1f)
continue;
if (Math.Sign(forceRatio) * Math.Sign(velDim) < 0)
{
myLogger.debugLog("damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
moveForceRatio.SetDim(i, 0);
enableDampeners = true;
}
else
myLogger.debugLog("not damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
}
//if (enableDampeners)
// Logger.debugNotify("Damping", 16);
Block.SetDamping(enableDampeners);
myLogger.debugLog("destDisp: " + destDisp
+ ", destDir: " + destDir
+ ", destVelocity: " + destVelocity
//+ ", relaVelocity: " + relaVelocity
+ ", targetVelocity: " + targetVelocity
//+ ", diffVel: " + diffVel
+ ", accel: " + accel
+ ", moveForceRatio: " + moveForceRatio, "CalcMove()");
}
/// <summary>
/// Calulates the maximum angular velocity to stop at the destination.
/// </summary>
/// <param name="disp">Displacement to the destination.</param>
/// <returns>The maximum angular velocity to stop at the destination.</returns>
private Vector3 MaxAngleVelocity(Vector3 disp)
{
Vector3 result = Vector3.Zero;
// S.E. provides damping for angular motion, we will ignore this
float accel = -myGyro.torqueAccelRatio * myGyro.TotalGyroForce();
//myLogger.debugLog("torqueAccelRatio: " + myGyro.torqueAccelRatio + ", TotalGyroForce: " + myGyro.TotalGyroForce() + ", accel: " + accel, "MaxAngleVelocity()");
//myLogger.debugLog("speed cap: " + speedCap, "MaxAngleVelocity()");
for (int i = 0; i < 3; i++)
{
float dim = disp.GetDim(i);
if (dim > 0)
//result.SetDim(i, Math.Min(MaxAngleSpeed(accel, dim), speedCap));
result.SetDim(i, MaxAngleSpeed(accel, dim));
else if (dim < 0)
//result.SetDim(i, -Math.Min(MaxAngleSpeed(accel, -dim), speedCap));
result.SetDim(i, -MaxAngleSpeed(accel, -dim));
}
return result;
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity)
{
CheckGrid();
myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);
angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());
//myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
float gyroForce = myGyro.TotalGyroForce();
const ulong UpdateFrequency = ShipController_Autopilot.UpdateFrequency;
if (rotateForceRatio != Vector3.Zero)
if (Globals.UpdateCount - updated_prevAngleVel == UpdateFrequency) // needs to be == because displacment is not divided by frequency
{
Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce);
//myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");
myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
}
else
myLogger.debugLog("prevAngleVel is old: " + (Globals.UpdateCount - updated_prevAngleVel), "CalcRotate()", Logger.severity.DEBUG);
localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);
localMatrix = localMatrix.GetOrientation();
inverted = inverted.GetOrientation();
//myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
//myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
//myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
//myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");
Vector3 localDirect = Direction.ToLocal();
Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);
rotBlockDirect.Normalize();
float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);
Vector3 rotaRight = localMatrix.Right;
Vector3 rotaUp = localMatrix.Up;
Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));
Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;
if (UpDirect != null)
{
Vector3 upLocal = UpDirect.ToLocal();
Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);
Vector3 rotaBackward = localMatrix.Backward;
Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));
myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (-roll * NFR_backward), "CalcRotate()");
displacement += roll * NFR_backward;
}
NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();
if (NavSet.Settings_Current.CollisionAvoidance)
{
myPathfinder.TestRotate(displacement);
if (!myPathfinder.CanRotate)
{
Logger.debugNotify("Cannot Rotate", 50);
myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
return;
}
}
//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");
if (myGyro.torqueAccelRatio == 0)
{
// do a test
myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
rotateForceRatio = new Vector3(0, 1f, 0);
return;
}
Vector3 targetVelocity = MaxAngleVelocity(displacement);
// Adjust for moving target by measuring changes in displacement. Part of the change in displacement is attributable to ship rotation.
const float dispToVel = (float)Globals.UpdatesPerSecond / (float)ShipController_Autopilot.UpdateFrequency;
Vector3 addVelocity = angularVelocity - (prevAngleDisp - displacement) * dispToVel;
if (addVelocity.LengthSquared() < 0.1f)
{
myLogger.debugLog("Adjust for moving, adding to target velocity: " + addVelocity, "CalcRotate()");
targetVelocity += addVelocity;
}
else
myLogger.debugLog("Not adjusting for moving, assuming target changed: " + addVelocity, "CalcRotate()");
prevAngleDisp = displacement;
Vector3 diffVel = targetVelocity - angularVelocity;
rotateForceRatio = diffVel / (myGyro.torqueAccelRatio * gyroForce);
myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", diffVel: " + diffVel, "CalcRotate()");
//myLogger.debugLog("diffVel: " + diffVel + ", torque: " + (myGyro.torqueAccelRatio * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()");
// dampeners
for (int i = 0; i < 3; i++)
{
// if targetVelocity is close to 0, use dampeners
float target = targetVelocity.GetDim(i);
if (target > -0.01f && target < 0.01f)
{
myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()");
rotateForceRatio.SetDim(i, 0f);
continue;
}
float velDim = angularVelocity.GetDim(i);
if (velDim < 0.01f && velDim > -0.01f)
continue;
// where rotateForceRatio opposes angularVelocity, use dampeners
float dim = rotateForceRatio.GetDim(i);
if (Math.Sign(dim) * Math.Sign(angularVelocity.GetDim(i)) < 0)
//{
// myLogger.debugLog("force ratio(" + dim + ") opposes velocity(" + angularVelocity.GetDim(i) + "), index: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()");
rotateForceRatio.SetDim(i, 0f);
//}
//else
// myLogger.debugLog("force ratio is aligned with velocity: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()");
}
}
/// <summary>
/// Calulates the maximum angular velocity to stop at the destination.
/// </summary>
/// <param name="disp">Displacement to the destination.</param>
/// <returns>The maximum angular velocity to stop at the destination.</returns>
private Vector3 MaxAngleVelocity(Vector3 disp, float minimumMoment, bool fast)
{
Vector3 result = Vector3.Zero;
// S.E. provides damping for angular motion, we will ignore this
float accel = -minimumMoment * m_gyro.GyroForce;
for (int index = 0; index < 3; index++)
{
float dim = disp.GetDim(index);
if (dim > 0)
result.SetDim(index, MaxAngleSpeed(accel, dim, fast));
else if (dim < 0)
result.SetDim(index, -MaxAngleSpeed(accel, -dim, fast));
}
return result;
}
private void CalcMove(ref Vector3 velocity)
{
DirectionBlock gravBlock = Thrust.WorldGravity.ToBlock(Block.CubeBlock);
m_moveForceRatio = ToForceRatio(m_moveAccel - gravBlock.vector);
// dampeners
bool enableDampeners = false;
m_thrustHigh = false;
for (int index = 0; index < 3; index++)
{
//const float minForceRatio = 0.1f;
//const float zeroForceRatio = 0.01f;
float velDim = velocity.GetDim(index);
// dampeners are useful for precise stopping but they do not always work properly
if (velDim < 1f && velDim > -1f)
{
float targetVelDim = m_moveAccel.GetDim(index) + velDim;
if (targetVelDim < 0.01f && targetVelDim > -0.01f)
{
m_logger.debugLog("for dim: " + index + ", target velocity near zero: " + targetVelDim);
m_moveForceRatio.SetDim(index, 0f);
enableDampeners = true;
continue;
}
}
float forceRatio = m_moveForceRatio.GetDim(index);
if (forceRatio < 1f && forceRatio > -1f)
{
//if (forceRatio > zeroForceRatio && forceRatio < minForceRatio)
// moveForceRatio.SetDim(i, minForceRatio);
//else if (forceRatio < -zeroForceRatio && forceRatio > -minForceRatio)
// moveForceRatio.SetDim(i, -minForceRatio);
continue;
}
m_thrustHigh = true;
// force ratio is > 1 || < -1. If it is useful, use dampeners
if (velDim < 1f && velDim > -1f)
continue;
if (Math.Sign(forceRatio) * Math.Sign(velDim) < 0)
{
m_logger.debugLog("damping, i: " + index + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim));
m_moveForceRatio.SetDim(index, 0);
enableDampeners = true;
}
//else
// myLogger.debugLog("not damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
}
SetDamping(enableDampeners);
}
/// <summary>
/// Calculates the force necessary to move the grid.
/// </summary>
/// <param name="block">To get world position from.</param>
/// <param name="destPoint">The world position of the destination</param>
/// <param name="destVelocity">The speed of the destination</param>
/// <param name="landing">Puts an emphasis on not overshooting the target.</param>
public void CalcMove(PseudoBlock block, Vector3 destPoint, Vector3 destVelocity, bool landing = false)
{
CheckGrid();
// using world vectors
if (NavSet.Settings_Current.CollisionAvoidance)
{
myPathfinder.TestPath(destPoint, landing);
if (!myPathfinder.CanMove)
{
myLogger.debugLog("Pathfinder not allowing movement", "CalcMove()");
return;
}
}
myThrust.Update();
Vector3 destDisp = destPoint - block.WorldPosition;
Vector3 velocity = Block.CubeGrid.Physics.LinearVelocity;
// switch to using local vectors
Matrix positionToLocal = Block.CubeBlock.WorldMatrixNormalizedInv;
Matrix directionToLocal = positionToLocal.GetOrientation();
destDisp = Vector3.Transform(destDisp, directionToLocal);
destVelocity = Vector3.Transform(destVelocity, directionToLocal);
velocity = Vector3.Transform(velocity, directionToLocal);
float distance = destDisp.Length();
NavSet.Settings_Task_NavWay.Distance = distance;
m_targetVelocity = MaximumVelocity(destDisp) * 0.5f;
// project targetVelocity onto destination direction (take shortest path)
Vector3 destDir = destDisp / distance;
m_targetVelocity = Vector3.Dot(m_targetVelocity, destDir) * destDir;
// apply relative speed limit
float relSpeedLimit = NavSet.Settings_Current.SpeedMaxRelative;
if (landing)
{
float landingSpeed = distance * 0.5f;
if (relSpeedLimit > landingSpeed)
relSpeedLimit = landingSpeed;
}
if (relSpeedLimit < float.MaxValue)
{
float tarSpeedSq_1 = m_targetVelocity.LengthSquared();
if (tarSpeedSq_1 > relSpeedLimit * relSpeedLimit)
{
m_targetVelocity *= relSpeedLimit / (float)Math.Sqrt(tarSpeedSq_1);
myLogger.debugLog("imposing relative speed limit: " + relSpeedLimit + ", targetVelocity: " + m_targetVelocity, "CalcMove()");
}
}
m_targetVelocity += destVelocity;
// apply speed limit
float tarSpeedSq = m_targetVelocity.LengthSquared();
float speedRequest = NavSet.Settings_Current.SpeedTarget;
if (tarSpeedSq > speedRequest * speedRequest)
m_targetVelocity *= speedRequest / (float)Math.Sqrt(tarSpeedSq);
m_moveAccel = m_targetVelocity - velocity - myThrust.m_localGravity;
moveForceRatio = ToForceRatio(m_moveAccel);
// dampeners
m_moveEnableDampeners = false;
bool checkNearZero = m_targetVelocity.LengthSquared() < 1f || m_moveAccel.LengthSquared() > 1f;
for (int i = 0; i < 3; i++)
{
float targetDim = m_targetVelocity.GetDim(i);
if (checkNearZero)
{
// if target velocity is close to 0, use dampeners
if (targetDim < 0.1f && targetDim > -0.1f)
{
myLogger.debugLog("close to 0, i: " + i + ", targetDim: " + targetDim, "CalcMove()");
moveForceRatio.SetDim(i, 0);
m_moveEnableDampeners = true;
continue;
}
}
float forceRatio = moveForceRatio.GetDim(i);
if (forceRatio < 1f && forceRatio > -1f)
{
// minimum force ratio is needed because SE is being strange about gravity
if (forceRatio < MinForceRatio && forceRatio > -MinForceRatio && myThrust.m_worldGravity.LengthSquared() > 0.1f)
{
if (targetDim > 0.1f)
moveForceRatio.SetDim(i, MinForceRatio);
else
moveForceRatio.SetDim(i, -MinForceRatio);
}
continue;
}
// force ratio is > 1 || < -1. If it is useful, use dampeners
float velDim = velocity.GetDim(i);
if (velDim < 1f && velDim > -1f)
continue;
if (Math.Sign(forceRatio) * Math.Sign(velDim) < 0)
{
myLogger.debugLog("damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
moveForceRatio.SetDim(i, 0);
m_moveEnableDampeners = true;
}
else
myLogger.debugLog("not damping, i: " + i + ", force ratio: " + forceRatio + ", velocity: " + velDim + ", sign of forceRatio: " + Math.Sign(forceRatio) + ", sign of velocity: " + Math.Sign(velDim), "CalcMove()");
}
Block.SetDamping(m_moveEnableDampeners);
if (destDisp.LengthSquared() > 1f)
m_notCalcMove = Globals.UpdateCount + CalcMoveIdle;
myLogger.debugLog("destDisp: " + destDisp
//+ ", destDir: " + destDir
+ ", destVelocity: " + destVelocity
//+ ", relaVelocity: " + relaVelocity
+ ", targetVelocity: " + m_targetVelocity
+ ", velocity: " + velocity
//+ ", diffVel: " + diffVel
+ ", m_moveAccel: " + m_moveAccel
+ ", moveForceRatio: " + moveForceRatio, "CalcMove()");
}