void AttitudeControl(FlightCtrlState s)
{
const float terrainOffset = 5;
Vector3 yawTarget = Vector3.ProjectOnPlane(targetDirection, vesselTransform.forward);
// limit "aoa" if we're moving
float driftMult = 1;
if (vessel.horizontalSrfSpeed * 10 > CruiseSpeed)
{
driftMult = Mathf.Max(Vector3.Angle(vessel.srf_velocity, yawTarget) / MaxDrift, 1);
yawTarget = Vector3.RotateTowards(vessel.srf_velocity, yawTarget, MaxDrift * Mathf.Deg2Rad, 0);
}
float yawError = VectorUtils.SignedAngle(vesselTransform.up, yawTarget, vesselTransform.right) + (aimingMode ? 0 : weaveAdjustment);
DebugLine($"yaw target: {yawTarget}, yaw error: {yawError}");
DebugLine($"drift multiplier: {driftMult}");
Vector3 baseForward = vessel.transform.up * terrainOffset;
float basePitch = Mathf.Atan2(
AIUtils.GetTerrainAltitude(vessel.CoM + baseForward, vessel.mainBody, false)
- AIUtils.GetTerrainAltitude(vessel.CoM - baseForward, vessel.mainBody, false),
terrainOffset * 2) * Mathf.Rad2Deg;
float pitchAngle = basePitch + TargetPitch * Mathf.Clamp01((float)vessel.horizontalSrfSpeed / CruiseSpeed);
if (aimingMode)
{
pitchAngle = VectorUtils.SignedAngle(vesselTransform.up, Vector3.ProjectOnPlane(targetDirection, vesselTransform.right), -vesselTransform.forward);
}
DebugLine($"terrain fw slope: {basePitch}, target pitch: {pitchAngle}");
float pitch = 90 - Vector3.Angle(vesselTransform.up, upDir);
float pitchError = pitchAngle - pitch;
Vector3 baseLateral = vessel.transform.right * terrainOffset;
float baseRoll = Mathf.Atan2(
AIUtils.GetTerrainAltitude(vessel.CoM + baseLateral, vessel.mainBody, false)
- AIUtils.GetTerrainAltitude(vessel.CoM - baseLateral, vessel.mainBody, false),
terrainOffset * 2) * Mathf.Rad2Deg;
float drift = VectorUtils.SignedAngle(vesselTransform.up, Vector3.ProjectOnPlane(vessel.GetSrfVelocity(), upDir), vesselTransform.right);
float bank = VectorUtils.SignedAngle(-vesselTransform.forward, upDir, -vesselTransform.right);
float targetRoll = baseRoll + BankAngle * Mathf.Clamp01(drift / MaxDrift) * Mathf.Clamp01((float)vessel.srfSpeed / CruiseSpeed);
float rollError = targetRoll - bank;
DebugLine($"terrain sideways slope: {baseRoll}, target roll: {targetRoll}");
Vector3 localAngVel = vessel.angularVelocity;
s.roll = steerMult * 0.006f * rollError - 0.4f * steerDamping * -localAngVel.y;
s.pitch = ((aimingMode ? 0.02f : 0.015f) * steerMult * pitchError) - (steerDamping * -localAngVel.x);
s.yaw = (((aimingMode ? 0.007f : 0.005f) * steerMult * yawError) - (steerDamping * 0.2f * -localAngVel.z)) * driftMult;
s.wheelSteer = -(((aimingMode ? 0.005f : 0.003f) * steerMult * yawError) - (steerDamping * 0.1f * -localAngVel.z));
if (ManeuverRCS && (Mathf.Abs(s.roll) >= 1 || Mathf.Abs(s.pitch) >= 1 || Mathf.Abs(s.yaw) >= 1))
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
}
}
