public void update() { VesselController.updateBodyPosition(); var quat = VesselController.getOrientation(); var shipup = Vector3.Transform(new Vector3(1.0f, 0.0f, 0.0f), quat); var shipleft = Vector3.Transform(new Vector3(0.0f, 0.0f, 1.0f), quat); var shipforward = Vector3.Transform(new Vector3(0.0f, 1.0f, 0.0f), quat); var fdir = shipforward + targetDirection * 1.1f; fdir.Normalize(); float xt = -Vector3.Dot(shipup, fdir); float yt = -Vector3.Dot(shipleft, fdir); float zt = -Vector3.Dot(shipup, Vector3.UnitX); //VesselController.setYaw((float)clamp(xt * 0.1, -0.1, 0.1)); //VesselController.setPitch((float)clamp(-yt * 0.1, -0.1, 0.1)); VesselController.setYaw((float)clamp(orientationYawContoller.Calculate(0.0, xt), -1.0, 1.0)); VesselController.setPitch((float)clamp(-orientationPitchContoller.Calculate(0.0, yt), -1.0, 1.0)); VesselController.setRoll((float)clamp(orientationRollContoller.Calculate(0.0, zt), -1.0, 1.0)); // Console.WriteLine(VesselController.getAngularVelocity().X); // Console.WriteLine(VesselController.getAngularVelocity().Y); // Console.WriteLine(VesselController.getAngularVelocity().Z); // Console.WriteLine(" "); }