/// <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);
}
void GetBreakTilt(out float targetRoll,
out float targetPitch,
DroneState currentState)
{
Vector3 rotatedVelocityWorld = Quaternion.AngleAxis(-currentState.rotation.y, Vector3.up) * currentState.velocityWorld;
targetRoll = MathHelper.Aserp(rotatedVelocityWorld.x, breakTiltMultiplier, maxBreakTiltAngle);
targetPitch = MathHelper.Aserp(-rotatedVelocityWorld.z, breakTiltMultiplier, maxBreakTiltAngle);
}
float GetAdditionalLiftForce(float velocityOffset)
{
if (velocityOffset < 0)
{
return(MathHelper.Aserp(velocityOffset, liftAccelerationMultiplier, maxFallForce));
}
return(MathHelper.Aserp(velocityOffset, liftAccelerationMultiplier, maxRiseForce));
}
// /// <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);
}
