/// <summary> /// Updates pose for our entity. We override default implementation /// since we control our own rendering when no file mesh is supplied, which means /// we dont need world transform updates /// </summary> /// <param name="update"></param> public override void Update(FrameUpdate update) { float leftFront = _leftFrontWheel.Wheel.AxleSpeed + _leftFrontTargetVelocity; float rightFront = _rightFrontWheel.Wheel.AxleSpeed + _rightFrontTargetVelocity; float leftRear = _leftRearWheel.Wheel.AxleSpeed + _leftRearTargetVelocity; float rightRear = _rightRearWheel.Wheel.AxleSpeed + _rightRearTargetVelocity; float timeStep = (float)update.ElapsedTime; ModulateWheelSpeed(leftFront, _leftFrontWheel, timeStep); ModulateWheelSpeed(rightFront, _rightFrontWheel, timeStep); ModulateWheelSpeed(leftRear, _leftRearWheel, timeStep); ModulateWheelSpeed(rightRear, _rightRearWheel, timeStep); // update state for us and all the shapes that make up the rigid body PhysicsEntity.UpdateState(true); // update entities in fields _leftFrontWheel.Update(update); _rightFrontWheel.Update(update); _leftRearWheel.Update(update); _rightRearWheel.Update(update); // sim engine will update children base.Update(update); }
/// <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); }