/// <summary>
/// Applies the yaw.
/// </summary>
/// <param name="inputYaw">yaw input</param>
void ApplyYaw(HandlingInput input, DroneState currentState)
{
float targetVelocity = maxYawSpeed * input.yaw;
float velocityOffset = targetVelocity - currentState.angularVelocityLocal.y;
float acceleration = MathHelper.Aserp(velocityOffset, yawAccelerationMultiplier, maxYawTorque);
GetComponent <Rigidbody> ().AddRelativeTorque(0, acceleration, 0);
}
// Update is called once per frame
void FixedUpdate()
{
HandlingInput input = GetInput();
DroneState currentState = GetCurrentState();
// apply forces
ApplyTilt(input, currentState);
ApplyYaw(input, currentState);
ApplyThrust(input, currentState);
}
// float GetTargetLiftVelocity(float idleVelocity, HandlingInput input) {
//
// if (input.thrust < 0)
// return Mathf.Lerp (idleVelocity, -maxFallSpeed, -input.thrust);
//
// return Mathf.Lerp (idleVelocity, maxRiseSpeed, input.thrust);
// }
void GetTargetTilt(out float targetRoll,
out float targetPitch,
HandlingInput input)
{
Vector2 maxDirectionalTiltInput = new Vector2(input.roll, input.pitch);
maxDirectionalTiltInput.Normalize();
targetRoll = maxDirectionalTiltInput.x * Mathf.Abs(input.roll) * maxTiltAngle;
targetPitch = maxDirectionalTiltInput.y * Mathf.Abs(input.pitch) * maxTiltAngle;
}
// /// <summary>
// /// Gets the input.
// /// </summary>
// /// <param name="inputRoll">roll input</param>
// /// <param name="inputPitch">pitch input</param>
// /// <param name="inputYaw">yaw input</param>
// /// <param name="inputThrust">thrust input</param>
// private void GetInput(out float inputRoll,
// out float inputPitch,
// out float inputYaw,
// out float inputThrust,
// out bool breaking) {
//
// inputRoll = Input.GetAxis ("Roll");
// inputPitch = Input.GetAxis ("Pitch");
// inputYaw = Input.GetAxis ("Yaw");
// inputThrust = Input.GetAxis ("Thrust");
// breaking = Input.GetButton("Break");
// }
private HandlingInput GetInput()
{
HandlingInput input = new HandlingInput();
input.roll = Input.GetAxis("Roll");
input.pitch = Input.GetAxis("Pitch");
input.yaw = Input.GetAxis("Yaw");
input.thrust = Input.GetAxis("Thrust");
input.breaking = Input.GetButton("Break");
return(input);
}
// /// <summary>
// /// Applies the thrust.
// /// </summary>
// /// <param name="inputThrust">thrust input</param>
// void ApplyThrust (HandlingInput input, DroneState currentState){
//
// float targetForce;
// float idleVelocity;
//
// if (StabilizeHeight) {
//
// float angle = Vector3.Angle (Vector3.up, transform.up);
// angle *= Mathf.Deg2Rad;
//
// targetForce = idleLiftForce / Mathf.Cos(angle);
//
// targetForce = idleLiftForce;
//
// idleVelocity = transform.InverseTransformDirection (transform.up * -currentState.velocityWorld.y).y;
// } else {
//
// targetForce = idleLiftForce;
// idleVelocity = -currentState.velocityLocal.y;
// }
//
// float targetVelocity = GetTargetLiftVelocity (idleVelocity, input);
//
// float velocityOffset = targetVelocity - currentState.velocityLocal.y;
//
// float additionalForce = GetAdditionalLiftForce (velocityOffset);
//
// targetForce += additionalForce;
//
// MathHelper.Limit (ref targetForce, idleLiftForce - maxFallForce, idleLiftForce + maxRiseForce);
//
// GetComponent<Rigidbody> ().AddRelativeForce (0, targetForce + additionalForce, 0);
// }
void ApplyThrust(HandlingInput input, DroneState currentState)
{
float velocityOffset;
if (StabilizeHeightCom)
{
velocityOffset = transform.InverseTransformDirection(transform.up * -currentState.velocityWorld.y).y;
}
else
{
velocityOffset = -currentState.velocityLocal.y;
}
// float targetAcceleration = MathHelper.NthRoot (targetVelocity, 7) + idleLiftForce;
// float targetAcceleration = MathHelper.Aserp (targetVelocity, liftAccelerationMultiplier, idleLiftForce);
float targetAcceleration;
if (velocityOffset >= 0)
{
targetAcceleration = MathHelper.Aserp(velocityOffset, liftAccelerationMultiplier, maxRiseForce - idleLiftForce) + idleLiftForce;
}
else
{
targetAcceleration = MathHelper.Aserp(-velocityOffset, liftAccelerationMultiplier, maxFallForce - idleLiftForce) + idleLiftForce;
}
// MathHelper.Clamp (ref targetAcceleration, maxFallForce, maxRiseForce);
float newAcceleration;
if (input.thrust == 0)
{
newAcceleration = targetAcceleration;
}
else if (input.thrust > 0)
{
newAcceleration = MathHelper.Interpolate(targetAcceleration, maxRiseForce, input.thrust);
}
else
{
newAcceleration = MathHelper.Interpolate(targetAcceleration, maxFallForce, -input.thrust);
}
GetComponent <Rigidbody>().AddRelativeForce(0, newAcceleration, 0);
rotorSound.pitch = newAcceleration * rotorPitchMultiplier + rotorPitchMinimum;
}
/// <summary>
/// Applies the tilt.
/// </summary>
/// <param name="inputRoll">roll input</param>
/// <param name="inputPitch">pitch input</param>
void ApplyTilt(HandlingInput input, DroneState currentState)
{
float offsetRoll, offsetPitch;
if (StabilizeTiltCom)
{
// stabilized
float targetRoll, targetPitch;
if (input.breaking)
{
GetBreakTilt(out targetRoll, out targetPitch, currentState);
}
else
{
GetTargetTilt(out targetRoll, out targetPitch, input);
}
offsetRoll = MathHelper.AngularDistance(currentState.rotation.z, targetRoll);
offsetPitch = MathHelper.AngularDistance(currentState.rotation.x, targetPitch);
}
else
{
// unstabilized
offsetRoll = input.roll * unstabilizedTiltMultiplier;
offsetPitch = input.pitch * unstabilizedTiltMultiplier;
}
// targeted tilt velocity
float targetVelocityRoll = MathHelper.Aserp(offsetRoll, tiltVelocityMultiplier, maxTiltSpeed);
float targetVelocityPitch = MathHelper.Aserp(offsetPitch, tiltVelocityMultiplier, maxTiltSpeed);
float velocityOffsetRoll = targetVelocityRoll - currentState.angularVelocityLocal.z;
float velocityOffsetPitch = targetVelocityPitch - currentState.angularVelocityLocal.x;
// targeted tilt force
float targetForceRoll = MathHelper.Aserp(velocityOffsetRoll, tiltAccelerationMultiplier, maxTiltForce);
float targetForcePitch = MathHelper.Aserp(velocityOffsetPitch, tiltAccelerationMultiplier, maxTiltForce);
// apply force
GetComponent <Rigidbody> ().AddRelativeTorque(targetForcePitch, 0, targetForceRoll);
}
