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); }