private void autoPilot(AutoPilot pilot, FlightCtrlState controls)
{
controls.killRot = false;
if (!controls.isIdle)
{
Enabled = false;
}
if (attachedVessel == null || !Enabled)
{
return;
}
Vessel vessel = attachedVessel;
CelestialBody body = vessel.mainBody;
Vector3d pos = vessel.GetWorldPos3D() - body.position;
Vector3d airVelocity = vessel.obt_velocity - body.getRFrmVel(body.position + pos);
Transform vesselTransform = vessel.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
Vector3d localVel = new Vector3d(Vector3d.Dot(vesselRight, airVelocity), Vector3d.Dot(vesselUp, airVelocity), Vector3d.Dot(vesselBackward, airVelocity));
Vector3d prograde = localVel.normalized;
Vector3d localGravityUp = new Vector3d(Vector3d.Dot(vesselRight, pos), Vector3d.Dot(vesselUp, pos), Vector3d.Dot(vesselBackward, pos)).normalized;
Vector3d localProgradeRight = Vector3d.Cross(prograde, localGravityUp).normalized;
localGravityUp = Vector3d.Cross(localProgradeRight, prograde);
Vector3d vel = vessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
double AoA = (float)DescentProfile.fetch.GetAngleOfAttack(body, pos, vel);
Vector3d worldTargetDirection = CorrectedDirection;
Vector3d targetDirection = new Vector3d(Vector3d.Dot(vesselRight, worldTargetDirection), Vector3d.Dot(vesselUp, worldTargetDirection), Vector3d.Dot(vesselBackward, worldTargetDirection));
Vector3d targetUp = prograde * (-Math.Sin(AoA)) + localGravityUp * Math.Cos(AoA);
Vector2 correction = Correction;
float dirx = targetDirection.z > 0.0f ? Mathf.Sign((float)targetDirection.x) : (float)targetDirection.x;
float diry = targetDirection.z > 0.0f ? Mathf.Sign((float)targetDirection.y) : (float)targetDirection.y;
float dirz = targetUp.y < 0.0f ? Mathf.Sign((float)targetUp.x) : (float)targetUp.x;
if (targetUp.y > 0.0f)
{
dirz += correction.x; // in case the craft has wings, roll it to use lift for left/right correction
}
float maxSteer = Mathf.Clamp(2.0f - Smoothness * 0.2f, 0.1f, 1.0f);
float warpDamp = 1.0f / TimeWarp.CurrentRate;
controls.pitch = Mathf.Clamp(diry * Strength + vessel.angularVelocity.x * Smoothness, -maxSteer, maxSteer) * warpDamp;
controls.yaw = Mathf.Clamp(-dirx * Strength + vessel.angularVelocity.z * Smoothness, -maxSteer, maxSteer) * warpDamp;
controls.roll = Mathf.Clamp(-dirz * Strength + vessel.angularVelocity.y * Smoothness, -maxSteer, maxSteer) * warpDamp;
}
public void Start()
{
fetch_ = this;
}
private void autoPilot(AutoPilot pilot, FlightCtrlState controls)
{
controls.killRot = false;
if (!controls.isIdle)
Enabled = false;
if (attachedVessel == null || !Enabled)
return;
Vessel vessel = attachedVessel;
CelestialBody body = vessel.mainBody;
Vector3d pos = vessel.GetWorldPos3D() - body.position;
Vector3d airVelocity = vessel.obt_velocity - body.getRFrmVel(body.position + pos);
Transform vesselTransform = vessel.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
Vector3d localVel = new Vector3d(Vector3d.Dot(vesselRight, airVelocity), Vector3d.Dot(vesselUp, airVelocity), Vector3d.Dot(vesselBackward, airVelocity));
Vector3d prograde = localVel.normalized;
Vector3d localGravityUp = new Vector3d(Vector3d.Dot(vesselRight, pos), Vector3d.Dot(vesselUp, pos), Vector3d.Dot(vesselBackward, pos)).normalized;
Vector3d localProgradeRight = Vector3d.Cross(prograde, localGravityUp).normalized;
localGravityUp = Vector3d.Cross(localProgradeRight, prograde);
Vector3d vel = vessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
double AoA = (float)DescentProfile.fetch.GetAngleOfAttack(body, pos, vel);
Vector3d worldTargetDirection = CorrectedDirection;
Vector3d targetDirection = new Vector3d(Vector3d.Dot(vesselRight, worldTargetDirection), Vector3d.Dot(vesselUp, worldTargetDirection), Vector3d.Dot(vesselBackward, worldTargetDirection));
Vector3d targetUp = prograde * (-Math.Sin(AoA)) + localGravityUp * Math.Cos(AoA);
Vector2 correction = Correction;
float dirx = targetDirection.z > 0.0f ? Mathf.Sign((float)targetDirection.x) : (float)targetDirection.x;
float diry = targetDirection.z > 0.0f ? Mathf.Sign((float)targetDirection.y) : (float)targetDirection.y;
float dirz = targetUp.y < 0.0f ? Mathf.Sign((float)targetUp.x) : (float)targetUp.x;
if(targetUp.y > 0.0f)
{
dirz += correction.x; // in case the craft has wings, roll it to use lift for left/right correction
}
float maxSteer = Mathf.Clamp(2.0f - Smoothness*0.2f, 0.1f, 1.0f);
float warpDamp = 1.0f / TimeWarp.CurrentRate;
controls.pitch = Mathf.Clamp(diry * Strength + vessel.angularVelocity.x * Smoothness, -maxSteer, maxSteer) * warpDamp;
controls.yaw = Mathf.Clamp(-dirx * Strength + vessel.angularVelocity.z * Smoothness, -maxSteer, maxSteer) * warpDamp;
controls.roll = Mathf.Clamp(-dirz * Strength + vessel.angularVelocity.y * Smoothness, -maxSteer, maxSteer) * warpDamp;
}
public void Start()
{
fetch_ = this;
}
