void Correct()
{
//var flight = vessel.Flight(reference);
currentDirection = Vector3.Transform(new Vector3(0, 1, 0), data.GetRotation(reference));
Quaternion invRotation = Quaternion.Invert(data.GetRotation(reference));
Vector3 localTarget = Vector3.Transform(direction, invRotation);
Vector3 localAngularVel = Vector3.Transform(data.GetAngularVelocity(reference), invRotation);
Vector3 localAngularAcc = (localAngularVel - lastLocalAngularVel) / (float)Time.gameDeltaTime;
localExternalAngularAcc = localAngularAcc - lastAppliedAcc;
//Log(localExternalAngularAccFilter.ToString() + "///" + localExternalAngularAcc.ToString() + "///--///" + Time.gameDeltaTime.ToString());
localExternalAngularAccFilter = Vector3.Lerp(localExternalAngularAccFilter, localExternalAngularAcc, (float)Time.gameDeltaTime * 0.2f);//adjustable
Vector3 interia = data.GetInteria();
var torque = data.GetTorque();
localMaxAngularAccP = Vector3.Divide(torque[0], interia);
localMaxAngularAccN = Vector3.Divide(torque[1], interia);
var pitchVel = localAngularVel.X;
var pitchWeight = (float)Math.Sqrt(localTarget.Z * localTarget.Z + localTarget.Y * localTarget.Y);
var pitchTarget = (float)Math.Atan2(localTarget.Z, localTarget.Y);
var pitchBrakeAcc = InX2Acc(-Math.Sign(pitchTarget));
//var pitchForwardAcc = InX2Acc(Math.Sign(pitchTarget));
var pitchTargetVel = DecideVel(pitchTarget, pitchBrakeAcc);
var pitchDiffVel = pitchTargetVel - pitchVel;
var pitchNeedAcc = pitchDiffVel / (float)Time.gameDeltaTime * Kp;
if (pitchWeight > 0.2f)
{
vessel.Control.Pitch = AccX2In(pitchNeedAcc);
}
var yawVel = localAngularVel.Z;
var yawWeight = (float)Math.Sqrt(localTarget.X * localTarget.X + localTarget.Y * localTarget.Y);
var yawTarget = (float)Math.Atan2(-localTarget.X, localTarget.Y);
var yawBrakeAcc = InZ2Acc(-Math.Sign(yawTarget));
var yawTargetVel = DecideVel(yawTarget, yawBrakeAcc);
var yawDiffVel = yawTargetVel - yawVel;
var yawNeedAcc = yawDiffVel / (float)Time.gameDeltaTime * Kp;
if (yawWeight > 0.2f)
{
vessel.Control.Yaw = AccZ2In(yawNeedAcc);
}
if (showline)
{
line.End = (direction * 20).ToTuple();
line.ReferenceFrame = reference;
}
//LogInfo("localDir ", localTarget.ToString());
//LogInfo("localVel ", localAngularVel.ToString());
LogInfo("localTarget", "yaw=" + yawTarget + " pitch=" + pitchTarget);
LogInfo("maxAcc", "pos=" + localMaxAngularAccP.ToString() + " neg=" + localMaxAngularAccN.ToString());
LogInfo("Acc ", "yaw=" + localAngularAcc.Z + " pitch=" + localAngularAcc.X);
LogInfo("ExtAcc", "yaw=" + localExternalAngularAcc.Z + " pitch=" + localExternalAngularAcc.X);
LogInfo("ExtAccf", "yaw=" + localExternalAngularAccFilter.Z + " pitch=" + localExternalAngularAccFilter.X);
LogInfo("targetVel", "yaw=" + yawTargetVel + " pitch=" + pitchTargetVel);
lastLocalAngularVel = localAngularVel;
lastAppliedAcc = Input2Acc(new Vector3(-vessel.Control.Pitch, -vessel.Control.Roll, -vessel.Control.Yaw));
}
