// apply control forces
void FixedUpdate()
{
if (!_robot.moveEnabled)
{
return;
}
float displacement2d = _target2d.magnitude;
_targetSpeed = lateralSpeedController.output(0f, -displacement2d);
_targetSpeed = Mathf.Clamp(_targetSpeed, 0f, maxLateralSpeed);
_targetTilt = tiltController.output(_targetSpeed, rigidbody.velocity.magnitude);
_targetTilt = Mathf.Clamp(_targetTilt, 0f, maxTilt);
Vector3 forward2d = new Vector3(transform.forward.x, 0f, transform.forward.z).normalized;
Vector3 right2d = new Vector3(transform.right.x, 0f, transform.right.z).normalized;
// PITCH
float forwardAxisAngle = Vector3.Angle(forward2d, _target2d);
float forwardCosine = Mathf.Cos(forwardAxisAngle * Mathf.Deg2Rad);
_actualPitch = Vector3.Angle(transform.forward, Vector3.up) - 90f; // avoids the 0-360 boundary
_targetPitch = forwardCosine * _targetTilt;
// ROLL
float rightAxisAngle = Vector3.Angle(right2d, _target2d);
float rightCosine = Mathf.Cos(rightAxisAngle * Mathf.Deg2Rad);
_actualRoll = Vector3.Angle(transform.right, Vector3.up) - 90f; // avoids the 0-360 boundary
_targetRoll = rightCosine * _targetTilt;
float pitchOutput = pitchController.output(_targetPitch, _actualPitch);
float rollOutput = rollController.output(_targetRoll, _actualRoll);
// THROTTLE
float throttle = throttleController.output(_targetHeight, transform.position.y);
// adapted from here
// https://ghowen.me/build-your-own-quadcopter-autopilot/
float flForce = Mathf.Clamp(throttle + rollOutput - pitchOutput, 0f, maxThrottle);
float blForce = Mathf.Clamp(throttle + rollOutput + pitchOutput, 0f, maxThrottle);
float frForce = Mathf.Clamp(throttle - rollOutput - pitchOutput, 0f, maxThrottle);
float brForce = Mathf.Clamp(throttle - rollOutput + pitchOutput, 0f, maxThrottle);
_flControlThrust = FL.up * flForce;
_blControlThrust = BL.up * blForce;
_frControlThrust = FR.up * frForce;
_brControlThrust = BR.up * brForce;
rigidbody.AddForceAtPosition(_flControlThrust, FL.position);
rigidbody.AddForceAtPosition(_frControlThrust, FR.position);
rigidbody.AddForceAtPosition(_blControlThrust, BL.position);
rigidbody.AddForceAtPosition(_brControlThrust, BR.position);
rigidbody.AddTorque(_controlTorque, ForceMode.Acceleration);
}