private void CalculateDesiredWorldVelocity(QuadcopterController.VelocityContext context)
{
if (followPath)
{
if (freeFlightDestination)
{
Vector3 worldDifference = worldDestination - controller.Rigidbody.position;
float distance = worldDifference.magnitude;
if (distance < pathLastCornerDistanceTreshold)
{
StopPathFollowing();
}
context.DesiredWorldVelocity = Vector3.ClampMagnitude(worldDifference, maxSpeed);
if (faceDestination)
{
context.DesiredDirection = new Vector3(worldDifference.x, 0, worldDifference.z).normalized;
}
}
else if (path != null && path.status != NavMeshPathStatus.PathInvalid && currentCornerIndex < path.corners.Length)
{
var currentCorner = path.corners[currentCornerIndex] + Vector3.up * minPathCornerHeight;
float treshold = currentCornerIndex < path.corners.Length - 1 ? pathCornerDistanceTreshold : pathLastCornerDistanceTreshold;
float cornerDistance = Vector3.Distance(controller.Rigidbody.position, currentCorner);
if (cornerDistance <= treshold)
{
currentCornerIndex++;
}
Vector3 worldDifference = currentCorner - controller.Rigidbody.position;
context.DesiredWorldVelocity = Vector3.ClampMagnitude(worldDifference, maxSpeed);
if (faceDestination)
{
context.DesiredDirection = new Vector3(worldDifference.x, 0, worldDifference.z).normalized;
}
}
else
{
StopPathFollowing();
}
}
}
private void CalculateDesiredWorldVelocity(QuadcopterController.VelocityContext context)
{
if (!pathController.FollowPath && !enableOnFreeFlight)
{
return;
}
Vector3 ahead = controller.Rigidbody.velocity.normalized;
if (Mathf.Approximately(ahead.sqrMagnitude, 0))
{
ahead = controller.transform.forward;
}
RaycastHit hit;
Ray ray = new Ray(controller.Rigidbody.position, ahead * maxSeeAhead);
if (Physics.SphereCast(ray, checkRadius, out hit, maxSeeAhead, checkMask))
{
avoidanceForce = Vector3.ClampMagnitude(hit.normal * (maxSeeAhead / hit.distance) * maxAvoidForce * controller.Rigidbody.velocity.magnitude, maxAvoidForce);
context.DesiredWorldVelocity += avoidanceForce;
}
}
