private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D?targetUp,
out double yawError, out double pitchError,
out double rollError)
{
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
// Determine projection of targetVector onto our reference unit vectors
var dotZ = targetVector.Dot(referenceForward);
var dotX = targetVector.Dot(referenceLeft);
var dotY = targetVector.Dot(referenceUp);
var projZ = dotZ * referenceForward;
var projX = dotX * referenceLeft;
var projY = dotY * referenceUp;
// Determine yaw/pitch error by calculating angle between our forward
// vector and targetVector
var z = projZ.Length() * Math.Sign(dotZ);
var x = projX.Length() * Math.Sign(dotX);
var y = projY.Length() * Math.Sign(-dotY); // NB inverted
yawError = Math.Atan2(x, z);
pitchError = Math.Atan2(y, z);
var gyroYaw = yawPID.Compute(yawError);
var gyroPitch = pitchPID.Compute(pitchError);
gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch);
if (targetUp != null)
{
// Also adjust roll
dotX = ((Vector3D)targetUp).Dot(referenceLeft);
dotY = ((Vector3D)targetUp).Dot(referenceUp);
projX = dotX * referenceLeft;
projY = dotY * referenceUp;
x = projX.Length() * Math.Sign(-dotX);
y = projY.Length() * Math.Sign(dotY);
rollError = Math.Atan2(x, y);
var gyroRoll = rollPID.Compute(rollError);
gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll);
}
else
{
rollError = default(double);
}
return(gyroControl);
}
private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D? targetUp,
out double yawError, out double pitchError,
out double rollError)
{
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
// Determine projection of targetVector onto our reference unit vectors
var dotZ = targetVector.Dot(referenceForward);
var dotX = targetVector.Dot(referenceLeft);
var dotY = targetVector.Dot(referenceUp);
var projZ = dotZ * referenceForward;
var projX = dotX * referenceLeft;
var projY = dotY * referenceUp;
// Determine yaw/pitch error by calculating angle between our forward
// vector and targetVector
var z = projZ.Length() * Math.Sign(dotZ);
var x = projX.Length() * Math.Sign(dotX);
var y = projY.Length() * Math.Sign(-dotY); // NB inverted
yawError = Math.Atan2(x, z);
pitchError = Math.Atan2(y, z);
var gyroYaw = yawPID.Compute(yawError);
var gyroPitch = pitchPID.Compute(pitchError);
gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch);
if (targetUp != null)
{
// Also adjust roll
dotX = ((Vector3D)targetUp).Dot(referenceLeft);
dotY = ((Vector3D)targetUp).Dot(referenceUp);
projX = dotX * referenceLeft;
projY = dotY * referenceUp;
x = projX.Length() * Math.Sign(-dotX);
y = projY.Length() * Math.Sign(dotY);
rollError = Math.Atan2(x, y);
var gyroRoll = rollPID.Compute(rollError);
gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll);
}
else
{
rollError = default(double);
}
return gyroControl;
}
private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D?targetUp,
out double yawPitchError, out double rollError)
{
var angularVelocity = shipControl.AngularVelocity;
if (angularVelocity == null)
{
// No ship controller, no action
yawPitchError = rollError = Math.PI;
return(shipControl.GyroControl);
}
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
targetVector = Vector3D.Normalize(targetVector);
// Invert our world matrix
var toLocal = MatrixD.Transpose(MatrixD.CreateWorld(Vector3D.Zero, referenceForward, referenceUp));
// And bring targetVector & angular velocity into local space
var localTarget = Vector3D.TransformNormal(-targetVector, toLocal);
var localVel = Vector3D.TransformNormal((Vector3D)angularVelocity, toLocal);
// Use simple trig to get the error angles
var yawError = Math.Atan2(localTarget.X, localTarget.Z);
var pitchError = Math.Atan2(localTarget.Y, localTarget.Z);
// Set desired angular velocity
var desiredYawVel = yawPID.Compute(yawError);
var desiredPitchVel = pitchPID.Compute(pitchError);
//shipControl.Echo(string.Format("desiredVel = {0:F3}, {1:F3}", desiredYawVel, desiredPitchVel));
// Translate to gyro outputs
double gyroYaw = 0.0;
if (Math.Abs(desiredYawVel) >= ControlThreshold)
{
gyroYaw = yawVPID.Compute(desiredYawVel - localVel.X);
}
double gyroPitch = 0.0;
if (Math.Abs(desiredPitchVel) >= ControlThreshold)
{
gyroPitch = pitchVPID.Compute(desiredPitchVel - localVel.Y);
}
//shipControl.Echo(string.Format("yaw, pitch = {0:F3}, {1:F3}", gyroYaw, gyroPitch));
gyroControl.SetAxisVelocity(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocity(GyroControl.Pitch, (float)gyroPitch);
// Determine total yaw/pitch error
yawPitchError = Math.Acos(MathHelperD.Clamp(Vector3D.Dot(targetVector, referenceForward), -1.0, 1.0));
if (targetUp != null)
{
// Also adjust roll by rotating targetUp vector
localTarget = Vector3D.TransformNormal((Vector3D)targetUp, toLocal);
rollError = Math.Atan2(localTarget.X, localTarget.Y);
var desiredRollVel = rollPID.Compute(rollError);
//shipControl.Echo(string.Format("desiredRollVel = {0:F3}", desiredRollVel));
double gyroRoll = 0.0;
if (Math.Abs(desiredRollVel) >= ControlThreshold)
{
gyroRoll = rollVPID.Compute(desiredRollVel - localVel.Z);
}
//shipControl.Echo(string.Format("roll = {0:F3}", gyroRoll));
gyroControl.SetAxisVelocity(GyroControl.Roll, (float)gyroRoll);
// Only care about absolute error
rollError = Math.Abs(rollError);
}
else
{
rollError = 0.0;
}
return(gyroControl);
}
