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