private void KeepAttitude()
        {
            pidControllerAttitudeX.Control(globalRotation.X, ref attitude_dx);
            pidControllerAttitudeZ.Control(globalRotation.Z, ref attitude_dz);

            PhysicsEntity.ApplyTorque(new Vector3(attitude_dx, 0.0f, attitude_dz), false);
        }
        private void SimulateWind()
        {
            // Calculate wind intensity fluctuation by the settings
            float windIntensityFluctuation = settingsWindIntensityFluctuation / 1000.0f;
            float windDirectionFluctuation = settingsWindDirectionFluctuation / 1000.0f;

            // Constant wind
            float constantWind = sngConstantWind.GenerateNext_Between0And1(windIntensityFluctuation);

            constantWind /= 2.0f;
            constantWind *= (settingsWindIntensity / 2.0f);

            // Wind gust
            float windGust = sngWindGust.GenerateNext_OnlyPositive(windIntensityFluctuation * 3.0f);

            windGust *= settingsWindIntensity;

            totalWindIntensity = constantWind + windGust;

            float windDirectionChange = 5.0f * sngWindDirectionFluctuation.GenerateNext(windDirectionFluctuation);

            currentWindDirectionAngle = settingsWindDirection + windDirectionChange;

            // Create a wind force vector with length 1 and rotate so the wind affects the multicopter from a certain direction
            windForceVector = MathHelper.RotateAroundAxis_Y(
                new Vector3(1.0f, 0.0f, 0.0f),
                new Vector3(),
                xna.MathHelper.ToRadians(currentWindDirectionAngle)
                );
            windForceVector = MathHelper.RotateAroundAxis_X(
                windForceVector,
                new Vector3(),
                xna.MathHelper.ToRadians(windDirectionChange)
                );

            // Generate oscillation of the multicopter arms
            torqueVector.X = sngAttitudeDisturbances.GenerateNext(Globals.RandomGenerator.Next(100, 300) / 10000.0f) / 250.0f;
            torqueVector.Y = sngAttitudeDisturbances.GenerateNext(Globals.RandomGenerator.Next(100, 300) / 10000.0f) / 300.0f;
            torqueVector.Z = sngAttitudeDisturbances.GenerateNext(Globals.RandomGenerator.Next(100, 300) / 10000.0f) / 250.0f;

            // Scale the direction vector with the intensity
            windForceVector = Vector3.Scale(windForceVector, totalWindIntensity);
            torqueVector    = Vector3.Scale(torqueVector, totalWindIntensity + 0.5f);

            // Let the global wind force and torque affect the multicopter
            PhysicsEntity.ApplyForce(windForceVector, false);
            PhysicsEntity.ApplyTorque(torqueVector, false);

            // Sum up for debug display
            totalForceVector += windForceVector;
        }
        /// <summary>
        /// The frameUpdate method gets called every frame iteration
        /// </summary>
        /// <param name="frameUpdate">Additional data provided with the current frame</param>
        public override void Update(FrameUpdate frameUpdate)
        {
            // Update state for us and all the shapes that make up the rigid body
            PhysicsEntity.UpdateState(true);

            // Calculate the raw values by the controller input
            raw_thrust += 0.025f * MAX_THRUST * raw_leftY;
            raw_yaw     = -raw_leftX;
            raw_pitch   = raw_rightY;
            raw_roll    = raw_rightX;

            // Limit too high thrust values
            if (raw_thrust < 0.0f)
            {
                raw_thrust = 0.0f;
            }
            else if (raw_thrust > MAX_THRUST)
            {
                raw_thrust = MAX_THRUST;
            }

            // Calculate the thrust ratio (used for motor sound and rotation behaviour)
            thrust_ratio = raw_thrust / MAX_THRUST;

            // Clear total force vector for the plot
            totalForceVector.X = 0.0f;
            totalForceVector.Y = 0.0f;
            totalForceVector.Z = 0.0f;

            // Rotation torque
            torqueVector.X = raw_roll / 20;
            torqueVector.Y = raw_yaw / 10;
            torqueVector.Z = raw_pitch / 20;

            PhysicsEntity.ApplyTorque(torqueVector, true);

            // Calculate the total rotation (for sound effects an propeller)
            float rotationFactor = Math.Abs(torqueVector.X) + Math.Abs(torqueVector.Y) + Math.Abs(torqueVector.Z);

            // Let the linear force affect the multicopter
            foreach (PropellerEntity propellerEntity in propellerEntities)
            {
                // Calculate the force vector for the current propeller
                forceVector = Vector3.Scale(propellerEntity.forceDirectionVector, raw_thrust * THRUST_TO_FORCE_FACTOR);

                // Let the linear force act on every single propeller
                PhysicsEntity.ApplyForceAtPosition(
                    forceVector,
                    propellerEntity.localPosition,
                    true,
                    true
                    );

                // Calculate the force to the current force vector
                totalForceVector += forceVector;

                // Let the propellers rotate depending on the linearForce
                propellerEntity.pitchRate = (raw_thrust + (10 * rotationFactor)) * 100;

                // Update the propeller entity
                propellerEntity.Update(frameUpdate);
            }

            #region Wind Force

            if (windSimulationActivated)
            {
                SimulateWind();
            }

            #endregion

            #region Auto stabilization

            globalRotation = MathHelper.RotateAroundAxis_Y(Rotation, new xna.Vector3(), xna.MathHelper.ToRadians(-Rotation.Y));

            // Use PID algorithm to keep the attitude
            if (keepAttitude && raw_rightX == 0 && raw_rightY == 0)
            {
                KeepAttitude();
            }

            // Use PID algorithm to keep altitude
            if (keepAltitude && raw_leftY == 0.0f)
            {
                KeepAltitude();
            }

            // Keep position
            if (keepPosition && raw_rightX == 0 && raw_rightY == 0)
            {
                KeepPosition();
            }
            else
            {
                pidControllerAttitudeX.referenceValue = 0.0f;
                pidControllerAttitudeZ.referenceValue = 0.0f;
            }

            #endregion

            #region Update sound

            // Update sound
            float newVolume = thrust_ratio + rotationFactor;
            soundEntity.Volume = (newVolume > 1.0f) ? 1.0f : newVolume; // TODO In die SoundEntity machen
            soundEntity.Pitch  = thrust_ratio + rotationFactor * 2;
            soundEntity.AudioEmitterPosition  = Position;
            soundEntity.AudioListenerPosition = frameUpdate.ActiveCamera.Eye;
            //soundEntity.AudioListenerLookAt = frameUpdate.ActiveCamera.LookAt; // TODO Schauen was hier falsch läuft
            soundEntity.Update3DSound();

            #endregion

            #region Debug result

            // Debug result
            updateDebugData.applicationTime = frameUpdate.ApplicationTime;
            updateDebugData.position        = Position;
            updateDebugData.velocity        = State.Velocity;
            updateDebugData.linearForce     = totalForceVector;
            //updateDebugData.rotation = Rotation;
            updateDebugData.rotation                    = globalRotation;
            updateDebugData.angularVelocity             = State.AngularVelocity;
            updateDebugData.rotationTorque              = torqueVector;
            updateDebugData.thrust                      = raw_thrust;
            updateDebugData.pidControllerAttitudeXValue = pidControllerAttitudeX.DebugOutputValue;
            updateDebugData.pidControllerAttitudeZValue = pidControllerAttitudeZ.DebugOutputValue;
            updateDebugData.pidControllerAltitudeValue  = pidControllerAltitude.DebugOutputValue;
            updateDebugData.pidControllerPositionXValue = pidControllerPositionX.DebugOutputValue;
            updateDebugData.pidControllerPositionZValue = pidControllerPositionZ.DebugOutputValue;
            updateDebugData.windDirection               = currentWindDirectionAngle;
            updateDebugData.windIntensity               = totalWindIntensity;
            windowStatisticsEventsPort.Post(updateDebugData);

            #endregion

            #region Stats result

            // Stats result
            updateStatsData.thrust       = raw_thrust;
            updateStatsData.thrust_ratio = thrust_ratio;
            updateStatsData.altitude     = Position.Y;
            updateStatsData.speedVector  = State.Velocity;
            windowControlEventsPort.Post(updateStatsData);

            #endregion

            // Simulation engine will frameUpdate children
            base.Update(frameUpdate);
        }