/// <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>
/// 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>
/// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect.
/// </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>
private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity)
{
m_logger.debugLog(Direction == null, "Direction == null", Logger.severity.ERROR);
m_gyro.Update();
float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor);
if (minimumMoment <= 0f)
{
// == 0f, not calculated yet. < 0f, we have math failure
StopRotate();
m_logger.debugLog(minimumMoment < 0f, "minimumMoment < 0f", Logger.severity.FATAL);
return;
}
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();
Vector3 localDirect = Direction.ToLocalNormalized();
Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);
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.ToLocalNormalized();
Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
upRotBlock.Z = 0f;
upRotBlock.Normalize();
float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f));
Vector3 rotaBackward = localMatrix.Backward;
Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));
//m_logger.debugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()");
displacement += roll * NFR_backward;
}
m_lastMoveAttempt = Globals.UpdateCount;
Pathfinder.TestRotate(displacement);
switch (Pathfinder.m_rotateState)
{
case Autopilot.Pathfinder.Pathfinder.PathState.Not_Running:
m_logger.debugLog("Pathfinder not run yet: " + Pathfinder.m_rotateState);
m_lastMove = Globals.UpdateCount;
return;
case Autopilot.Pathfinder.Pathfinder.PathState.No_Obstruction:
break;
default:
m_logger.debugLog("Pathfinder not allowing rotation: " + Pathfinder.m_rotateState);
return;
}
float distanceAngle = displacement.Length();
if (distanceAngle < m_bestAngle || float.IsNaN(m_navSet.Settings_Current.DistanceAngle))
{
m_bestAngle = distanceAngle;
m_lastMove = Globals.UpdateCount;
}
m_navSet.Settings_Task_NavWay.DistanceAngle = distanceAngle;
//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()");
m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null);
// adjustment to face a moving entity
if (targetEntity != null)
{
Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - LinearVelocity;
float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
//myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()");
float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down);
float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right);
float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);
m_logger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw));
m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw;
}
//m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()");
// angular velocity is reversed
Vector3 angularVelocity = AngularVelocity.ToBlock(Block.CubeBlock);// ((DirectionWorld)(-Block.Physics.AngularVelocity)).ToBlock(Block.CubeBlock);
m_rotateForceRatio = (m_rotateTargetVelocity + angularVelocity) / (minimumMoment * m_gyro.GyroForce);
m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + (m_rotateTargetVelocity + angularVelocity));
m_logger.debugLog("minimumMoment: " + minimumMoment + ", force: " + m_gyro.GyroForce + ", rotateForceRatio: " + m_rotateForceRatio);
// dampeners
for (int index = 0; index < 3; index++)
{
// if targetVelocity is close to 0, use dampeners
float target = m_rotateTargetVelocity.GetDim(index);
if (target > -0.01f && target < 0.01f)
{
//m_logger.debugLog("target near 0 for " + i + ", " + target, "in_CalcRotate()");
m_rotateTargetVelocity.SetDim(index, 0f);
m_rotateForceRatio.SetDim(index, 0f);
continue;
}
}
}
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 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, IMyEntity targetEntity)
{
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();
float secondsSinceLast = (float)(DateTime.UtcNow - updated_prevAngleVel).TotalSeconds;
updated_prevAngleVel = DateTime.UtcNow;
if (rotateForceRatio != Vector3.Zero)
{
if (secondsSinceLast <= MaxUpdateSeconds)
{
Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce * secondsSinceLast);
//myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");
myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
}
else
myLogger.debugLog("prevAngleVel is old: " + secondsSinceLast, "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)
{
// if cannot rotate and not calculating move, move away from obstruction
if (myPathfinder.RotateObstruction != null && Globals.UpdateCount >= m_notCalcMove)
{
Vector3 position = Block.CubeBlock.GetPosition();
Vector3 away = position - myPathfinder.RotateObstruction.GetCentre();
away.Normalize();
myLogger.debugLog("Cannot rotate and not calculating move, creating GOLIS to move away from obstruction", "CalcRotate()", Logger.severity.INFO);
new GOLIS(this, NavSet, position + away * (10f + NavSet.Settings_Current.DestinationRadius), true);
}
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, secondsSinceLast);
if (targetEntity != null)
{
Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - Block.Physics.LinearVelocity;
float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
//Vector3 tangentialVelocity = Vector3.Reject(relativeLinearVelocity, targetEntity.GetCentre() - Block.CubeBlock.GetPosition());
//Vector3 localTangVel = Vector3.Transform(tangentialVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());
//myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "CalcRotate()");
float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Up);
float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Left);
float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);
myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch, "CalcRotate()");
targetVelocity += new Vector3(angl_pitch, angl_yaw, 0f);
}
Vector3 accel = (targetVelocity - angularVelocity) / secondsSinceLast;
rotateForceRatio = accel / (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce);
myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + accel, "CalcRotate()");
myLogger.debugLog("accel: " + accel + ", torque: " + (myGyro.torqueAccelRatio * secondsSinceLast * 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;
}
}
}
/// <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()");
}