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; } }