/// <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);
}
void FixedUpdate()
{
Vector3 currentRotation = transform.rotation.eulerAngles;
float currentRoll = currentRotation.z;
float currentPitch = currentRotation.x;
float currentYaw = currentRotation.y;
Vector3 velocity = GetComponent <Rigidbody>().velocity;
Vector3 localVelocity = transform.InverseTransformDirection(velocity);
float inputRoll = Input.GetAxis("Roll");
float inputPitch = Input.GetAxis("Pitch");
float inputYaw = Input.GetAxis("Yaw");
float inputThrust = Input.GetAxis("Thrust");
bool breaking = Input.GetButton("Break");
float targetRoll;
float targetPitch;
if (!stabilized || !stabilizeTilt || !breaking)
{
GetTargetTilt(out targetRoll, out targetPitch,
inputRoll, inputPitch,
currentRoll, currentPitch);
}
else
{
Vector3 yawDependantVelocity = Quaternion.AngleAxis(-currentRotation.y, Vector3.up) * velocity;
GetBreakTilt(out targetRoll, out targetPitch,
yawDependantVelocity.x, yawDependantVelocity.z);
}
float offsetRoll = MathHelper.AngularDistance(currentRoll, targetRoll);
MathHelper.Flatten(ref offsetRoll, 0.05f);
float offsetPitch = MathHelper.AngularDistance(currentPitch, targetPitch);
MathHelper.Flatten(ref offsetPitch, 0.05f);
float targetVelocityRoll = MathHelper.NthRoot(offsetRoll, 1) * tiltVelocityMultiplier;
// MathHelper.Flatten (ref targetVelocityRoll, 0.5f);
float targetVelocityPitch = MathHelper.NthRoot(offsetPitch, 1) * tiltVelocityMultiplier;
// MathHelper.Flatten (ref targetVelocityPitch, 0.5f);
Vector3 localAngularVelocity = GetComponent <Rigidbody>().transform.InverseTransformDirection(GetComponent <Rigidbody>().angularVelocity);
localAngularVelocity.z = targetVelocityRoll;
localAngularVelocity.x = targetVelocityPitch;
GetComponent <Rigidbody>().angularVelocity = transform.TransformDirection(localAngularVelocity);
// float velocityOffsetRoll = targetVelocityRoll - localAngularVelocity.z;
// MathHelper.Flatten (ref velocityOffsetRoll, 0.01f);
//
// float velocityOffsetPitch = targetVelocityPitch - localAngularVelocity.x;
// MathHelper.Flatten (ref velocityOffsetPitch, 0.01f);
//
//
// float rollAcceleration = MathHelper.NthRoot (velocityOffsetRoll, 1) * tiltAccelerationMultiplier;
//// MathHelper.Flatten (ref rollAcceleration, 1f);
//
// float pitchAcceleration = MathHelper.NthRoot (velocityOffsetPitch, 1) * tiltAccelerationMultiplier;
//// MathHelper.Flatten (ref pitchAcceleration, 1f);
//
//
// rigidbody.AddRelativeTorque (pitchAcceleration, 0, rollAcceleration);
// GetTiltOffset (out offsetRoll, out offsetPitch, targetRoll, targetPitch, currentRoll, currentPitch);
// transform.rotation = Quaternion.Euler (targetPitch, currentYaw, targetRoll);
float targetVelocity;
if (stabilized && stabilizeVerticalMovement)
{
Vector3 worldUp = transform.TransformDirection(Vector3.up);
worldUp *= -velocity.y;
Vector3 localUp = transform.InverseTransformDirection(worldUp);
targetVelocity = localUp.y;
}
else
{
targetVelocity = -localVelocity.y;
}
float idleAcceleration = MathHelper.NthRoot(targetVelocity, 7) + defaultUpAcceleration;
MathHelper.Clamp(ref idleAcceleration, maxAccelerationDown, maxAccelerationUp);
float newAcceleration;
if (inputThrust == 0)
{
newAcceleration = idleAcceleration;
}
else if (inputThrust > 0)
{
newAcceleration = MathHelper.Interpolate(idleAcceleration, maxAccelerationUp, inputThrust);
}
else
{
newAcceleration = MathHelper.Interpolate(idleAcceleration, maxAccelerationDown, -inputThrust);
}
GetComponent <Rigidbody>().AddRelativeForce(0, newAcceleration, 0);
rotorSound.pitch = newAcceleration * rotorPitchMultiplier + rotorPitchMinimum;
ApplyYawForce(inputYaw);
}
