void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
{
Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;
float terrainAltitude;
float dynPressure;
float velocity;
//Setup
terrainAltitude = (float)vessel.heightFromTerrain;
dynPressure = (float)vessel.dynamicPressurekPa;
velocity = (float)vessel.srfSpeed;
float upWeighting = pilot.UpWeighting(terrainAltitude, velocity);
//Calculating errors
ErrorData behavior = flightMode.Simulate(vesselTransform, velocityTransform, targetPosition, upDirection, upWeighting, vessel);
//Controlling
Steer steer = pilot.Simulate(behavior.pitchError, behavior.rollError, behavior.yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);
//Piloting
s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
if (s.roll == s.rollTrim)
{
s.roll = Mathf.Clamp(steer.roll, -1, 1);
}
s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
}
void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
{
Vector3d srfVel = vessel.srf_velocity;
if (srfVel != Vector3d.zero)
{
velocityTransform.rotation = Quaternion.LookRotation(srfVel, -vesselTransform.forward);
}
velocityTransform.rotation = Quaternion.AngleAxis(90, velocityTransform.right) * velocityTransform.rotation;
Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;
Vector3d targetDirection;
Vector3d targetDirectionYaw;
float yawError;
float pitchError;
float rollError;
float terrainAltitude;
float dynPressure;
float velocity;
//Setup
targetDirection = vesselTransform.InverseTransformDirection(targetPosition - velocityTransform.position).normalized;
targetDirectionYaw = targetDirection;
terrainAltitude = GetRadarAltitude();
dynPressure = (float)vessel.dynamicPressurekPa;
velocity = (float)vessel.srfSpeed;
pitchError = (float)Math.Asin(Vector3d.Dot(Vector3d.back, VectorUtils.Vector3dProjectOnPlane(targetDirection, Vector3d.right))) * Mathf.Rad2Deg;
yawError = (float)Math.Asin(Vector3d.Dot(Vector3d.right, VectorUtils.Vector3dProjectOnPlane(targetDirectionYaw, Vector3d.forward))) * Mathf.Rad2Deg;
//roll
Vector3 currentRoll = -vesselTransform.forward;
Vector3 rollTarget;
upWeighting = pilot.UpWeighting(terrainAltitude, dynPressure, velocity);
rollTarget = (targetPosition + Mathf.Clamp(upWeighting * (100f - Math.Abs(yawError * 1.6f) - (pitchError * 2.8f)), 0, float.PositiveInfinity) * upDirection) - vessel.CoM;
rollTarget = Vector3.ProjectOnPlane(rollTarget, vesselTransform.up);
rollError = VectorUtils.SignedAngle(currentRoll, rollTarget, vesselTransform.right);
Steer steer = pilot.Simulate(pitchError, rollError, yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);
s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
if (s.roll == s.rollTrim)
{
s.roll = Mathf.Clamp(steer.roll, -1, 1);
}
s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
//Debug
dynPressDebug = dynPressure;
speedFactorDebug = dynPressure * 16 / velocity;
invSpeedFactorDebug = 1 / (speedFactorDebug + Single.Epsilon);
//------------------
}
void FlyToPosition(FlightCtrlState s, Vector3 targetPosition)
{
Vector3d srfVel = vessel.srf_velocity;
if (srfVel != Vector3d.zero)
{
velocityTransform.rotation = Quaternion.LookRotation(srfVel, -vesselTransform.forward);
}
velocityTransform.rotation = Quaternion.AngleAxis(90, velocityTransform.right) * velocityTransform.rotation;
Vector3 localAngVel = vessel.angularVelocity * Mathf.Rad2Deg;
Vector3d targetDirection;
Vector3d targetDirectionYaw;
//float yawError;
//float pitchError;
//float rollError;
float terrainAltitude;
float dynPressure;
float velocity;
//Setup
targetDirection = vesselTransform.InverseTransformDirection(targetPosition - velocityTransform.position).normalized;
targetDirectionYaw = targetDirection;
terrainAltitude = GetRadarAltitude();
dynPressure = (float)vessel.dynamicPressurekPa;
velocity = (float)vessel.srfSpeed;
upWeighting = pilot.UpWeighting(terrainAltitude, dynPressure, velocity);
//Setting forced behavior for testing while there is no toggle button
flightMode.SetBehavior(1);
//Calculating errors
ErrorData behavior = flightMode.Simulate(vesselTransform, targetDirection, targetDirectionYaw, targetPosition, upDirection, upWeighting, vessel);
//Controlling
Steer steer = pilot.Simulate(behavior.pitchError, behavior.rollError, behavior.yawError, localAngVel, terrainAltitude, TimeWarp.fixedDeltaTime, dynPressure, velocity);
//Piloting
s.pitch = Mathf.Clamp(steer.pitch, -1, 1);
if (s.roll == s.rollTrim)
{
s.roll = Mathf.Clamp(steer.roll, -1, 1);
}
s.yaw = Mathf.Clamp(steer.yaw, -1, 1);
}