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