protected Vector2 CalculateDeltaToMoveThisFrame(Vector2 position, float distanceToEndOfPath, float deltaTime) { if (rvoController != null && rvoController.enabled) { // Use RVOController to get a processed delta position // such that collisions will be avoided if possible return(movementPlane.ToPlane(rvoController.CalculateMovementDelta(movementPlane.ToWorld(position, 0), deltaTime))); } // Direction and distance to move during this frame return(Vector2.ClampMagnitude(velocity2D * deltaTime, distanceToEndOfPath)); }
void AgentMove() { //if (seeker.destination == m_destination) // return; if (!m_walking) { return; } agent.destination = m_destination; Vector3 nextPosition; Quaternion nextRotation; agent.MovementUpdate(Time.deltaTime, out nextPosition, out nextRotation); if (!controller) { deltaPosition = new Vector3(animator.rootPosition.x, nextPosition.y, animator.rootPosition.z); } //deltaPosition = nextPosition; else { deltaPosition = new Vector3(animator.rootPosition.x, nextPosition.y, animator.rootPosition.z); //deltaPosition = nextPosition; Vector3 delta = controller.CalculateMovementDelta(animatedObject.transform.position, Time.deltaTime); deltaPosition = new Vector3(deltaPosition.x + delta.x, deltaPosition.y, deltaPosition.z + delta.z); } agent.FinalizeMovement(deltaPosition, nextRotation); transform.rotation = nextRotation; //if (agent.destination == m_destination) // return; //agent.destination = m_destination; //if (agent.pathStatus == NavMeshPathStatus.PathInvalid || agent.pathStatus == NavMeshPathStatus.PathPartial) //{ // m_walking = false; // agent.isStopped = true; // return; //} //if (agent.isStopped) // agent.isStopped = false; //m_walking = true; //agent.nextPosition = animatedObject.transform.position; //transform.rotation = agent.transform.rotation; }
public void Update() { if (Time.time >= nextRepath && canSearchAgain) { RecalculatePath(); } Vector3 pos = transform.position; if (vectorPath != null && vectorPath.Count != 0) { while ((controller.To2D(pos - vectorPath[wp]).sqrMagnitude < moveNextDist * moveNextDist && wp != vectorPath.Count - 1) || wp == 0) { wp++; } // Current path segment goes from vectorPath[wp-1] to vectorPath[wp] // We want to find the point on that segment that is 'moveNextDist' from our current position. // This can be visualized as finding the intersection of a circle with radius 'moveNextDist' // centered at our current position with that segment. var p1 = vectorPath[wp - 1]; var p2 = vectorPath[wp]; // Calculate the intersection with the circle. This involves some math. var t = VectorMath.LineCircleIntersectionFactor(controller.To2D(transform.position), controller.To2D(p1), controller.To2D(p2), moveNextDist); // Clamp to a point on the segment t = Mathf.Clamp01(t); Vector3 waypoint = Vector3.Lerp(p1, p2, t); // Calculate distance to the end of the path float remainingDistance = controller.To2D(waypoint - pos).magnitude + controller.To2D(waypoint - p2).magnitude; for (int i = wp; i < vectorPath.Count - 1; i++) { remainingDistance += controller.To2D(vectorPath[i + 1] - vectorPath[i]).magnitude; } // Set the target to a point in the direction of the current waypoint at a distance // equal to the remaining distance along the path. Since the rvo agent assumes that // it should stop when it reaches the target point, this will produce good avoidance // behavior near the end of the path. When not close to the end point it will act just // as being commanded to move in a particular direction, not toward a particular point var rvoTarget = (waypoint - pos).normalized * remainingDistance + pos; // When within [slowdownDistance] units from the target, use a progressively lower speed var desiredSpeed = Mathf.Clamp01(remainingDistance / slowdownDistance) * maxSpeed; Debug.DrawLine(transform.position, waypoint, Color.red); controller.SetTarget(rvoTarget, desiredSpeed, maxSpeed); } else { // Stand still controller.SetTarget(pos, maxSpeed, maxSpeed); } // Get a processed movement delta from the rvo controller and move the character. // This is based on information from earlier frames. var movementDelta = controller.CalculateMovementDelta(Time.deltaTime); pos += movementDelta; // Rotate the character if the velocity is not extremely small if (Time.deltaTime > 0 && movementDelta.magnitude / Time.deltaTime > 0.01f) { var rot = transform.rotation; var targetRot = Quaternion.LookRotation(movementDelta, controller.To3D(Vector2.zero, 1)); const float RotationSpeed = 5; if (controller.movementPlane == MovementPlane.XY) { targetRot = targetRot * Quaternion.Euler(-90, 180, 0); } transform.rotation = Quaternion.Slerp(rot, targetRot, Time.deltaTime * RotationSpeed); } if (controller.movementPlane == MovementPlane.XZ) { RaycastHit hit; if (Physics.Raycast(pos + Vector3.up, Vector3.down, out hit, 2, groundMask)) { pos.y = hit.point.y; } } transform.position = pos; }
public void Update() { if (Time.time >= nextRepath && canSearchAgain) { RecalculatePath(); } Vector3 pos = transform.position; if (vectorPath != null && vectorPath.Count != 0) { //Good Game //while ((controller.To2D(pos - vectorPath[wp]).sqrMagnitude < moveNextDist*moveNextDist && wp != vectorPath.Count-1) || wp == 0) //while ((controller.To2D((VInt3)pos - vectorPath[wp]).sqrMagnitude < moveNextDist*moveNextDist && wp != vectorPath.Count-1) || wp == 0) //Debug.Log($"--waypoint--{gameObject.name}--{(controller.To2D((VInt3)pos - vectorPath[wp])).sqrMagnitude / 1000000f}--{moveNextDist * moveNextDist}"); while ((((Vector2)controller.To2D((VInt3)pos - vectorPath[wp])).sqrMagnitude < moveNextDist * moveNextDist && wp != vectorPath.Count - 1) || wp == 0) { wp++; //Debug.Log($"--agent{transform.GetSiblingIndex()}--wp--{wp}"); } // Current path segment goes from vectorPath[wp-1] to vectorPath[wp] // We want to find the point on that segment that is 'moveNextDist' from our current position. // This can be visualized as finding the intersection of a circle with radius 'moveNextDist' // centered at our current position with that segment. var p1 = vectorPath[wp - 1]; var p2 = vectorPath[wp]; // Calculate the intersection with the circle. This involves some math. //Good Game //var t = VectorMath.LineCircleIntersectionFactor(controller.To2D(transform.position), controller.To2D(p1), controller.To2D(p2), moveNextDist); var t = VectorMath.LineCircleIntersectionFactor((Vector2)controller.To2D((VInt3)transform.position), (Vector2)controller.To2D(p1), (Vector2)controller.To2D(p2), moveNextDist); // Clamp to a point on the segment t = Mathf.Clamp01(t); //Good Game //Vector3 waypoint = Vector3.Lerp(p1, p2, t); VInt3 waypoint = VInt3.Lerp(p1, p2, t); // Calculate distance to the end of the path //Good Game /*float remainingDistance = controller.To2D(waypoint - pos).magnitude + controller.To2D(waypoint - p2).magnitude; * for (int i = wp; i < vectorPath.Count - 1; i++) * remainingDistance += controller.To2D(vectorPath[i+1] - vectorPath[i]).magnitude;*/ float remainingDistance = controller.To2D(waypoint - (VInt3)pos).magnitude + controller.To2D(waypoint - p2).magnitude; for (int i = wp; i < vectorPath.Count - 1; i++) { remainingDistance += controller.To2D(vectorPath[i + 1] - vectorPath[i]).magnitude; } // Set the target to a point in the direction of the current waypoint at a distance // equal to the remaining distance along the path. Since the rvo agent assumes that // it should stop when it reaches the target point, this will produce good avoidance // behavior near the end of the path. When not close to the end point it will act just // as being commanded to move in a particular direction, not toward a particular point //GG remainingDistance /= 1000000; //Debug.Log("--remian--" + remainingDistance.ToString("f2")); //var rvoTarget = (waypoint - pos).normalized * remainingDistance + pos; var rvoTarget = ((Vector3)waypoint - pos).normalized * remainingDistance + pos; // When within [slowdownDistance] units from the target, use a progressively lower speed var desiredSpeed = Mathf.Clamp01(remainingDistance / slowdownDistance) * maxSpeed; Debug.DrawLine(transform.position, waypoint, Color.red); //GG //controller.SetTarget(rvoTarget, desiredSpeed, maxSpeed); //Debug.Log($"--target--{rvoTarget}--de speed--{desiredSpeed}--maxSpeed--{maxSpeed}"); controller.SetTarget((VInt3)rvoTarget, (int)(desiredSpeed * 1000), (int)(maxSpeed * 1000)); //controller.SetTarget((VInt3)rvoTarget, 50, (int)(maxSpeed* 1000)); } else { // Stand still //GG //controller.SetTarget(pos, maxSpeed, maxSpeed); controller.SetTarget((VInt3)pos, (int)(maxSpeed * 1000), (int)(maxSpeed * 1000)); } // Get a processed movement delta from the rvo controller and move the character. // This is based on information from earlier frames. //GG //var movementDelta = controller.CalculateMovementDelta(Time.deltaTime); //var movementDelta = controller.CalculateMovementDelta((int)(Time.deltaTime * 1000)); var movementDelta = controller.CalculateMovementDelta(50); //GG //pos += movementDelta; pos += (Vector3)movementDelta; //Debug.Log("--" + movementDelta.ToString()); //Rotate the character if the velocity is not extremely small //GG //if (Time.deltaTime > 0 && movementDelta.magnitude / Time.deltaTime > 0.01f) if (Time.deltaTime > 0 && ((Vector3)movementDelta).magnitude / Time.deltaTime > 0.01f) { var rot = transform.rotation; //GG //var targetRot = Quaternion.LookRotation((Vector3)movementDelta, (Vector3)controller.To3D(Vector2.zero, 1)); var targetRot = Quaternion.LookRotation((Vector3)movementDelta, (Vector3)controller.To3D(VInt2.zero, 1000)); const float RotationSpeed = 5; if (controller.movementPlane == MovementPlane.XY) { targetRot = targetRot * Quaternion.Euler(-90, 180, 0); } transform.rotation = Quaternion.Slerp(rot, targetRot, Time.deltaTime * RotationSpeed); } if (controller.movementPlane == MovementPlane.XZ) { RaycastHit hit; if (Physics.Raycast(pos + Vector3.up, Vector3.down, out hit, 2, groundMask)) { pos.y = hit.point.y; } } transform.position = pos; //Good Game /*if(pos.x > 200 || pos.y > 200 || pos.z > 200 || pos.x > -200 || pos.z < -200) * Debug.LogError("--" + gameObject.name + "--" + pos);*/ }
/** Update is called once per frame */ protected virtual void Update() { deltaTime = Mathf.Min(Time.smoothDeltaTime * 2, Time.deltaTime); if (rp != null) { RichPathPart currentPart = rp.GetCurrentPart(); var fn = currentPart as RichFunnel; if (fn != null) { // Clamp the current position to the navmesh // and update the list of upcoming corners in the path // and store that in the 'nextCorners' variable Vector3 position = UpdateTarget(fn); // Only get walls every 5th frame to save on performance if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0) { wallBuffer.Clear(); fn.FindWalls(wallBuffer, wallDist); } // Target point int tgIndex = 0; Vector3 targetPoint = nextCorners[tgIndex]; Vector3 dir = targetPoint - position; dir.y = 0; bool passedTarget = Vector3.Dot(dir, currentTargetDirection) < 0; // Check if passed target in another way if (passedTarget && nextCorners.Count - tgIndex > 1) { tgIndex++; targetPoint = nextCorners[tgIndex]; } // Check if the target point changed compared to last frame if (targetPoint != lastTargetPoint) { currentTargetDirection = targetPoint - position; currentTargetDirection.y = 0; currentTargetDirection.Normalize(); lastTargetPoint = targetPoint; } // Direction to target dir = targetPoint - position; dir.y = 0; // Normalized direction Vector3 normdir = VectorMath.Normalize(dir, out distanceToWaypoint); // Is the endpoint of the path (part) the current target point bool targetIsEndPoint = lastCorner && nextCorners.Count - tgIndex == 1; // When very close to the target point, move directly towards the target // instead of using accelerations as they tend to be a bit jittery in this case if (targetIsEndPoint && distanceToWaypoint < 0.01f * maxSpeed) { // Velocity will be at most 1 times max speed, it will be further clamped below velocity = (targetPoint - position) * 100; } else { // Calculate force from walls Vector3 wallForceVector = CalculateWallForce(position, normdir); Vector2 accelerationVector; if (targetIsEndPoint) { accelerationVector = CalculateAccelerationToReachPoint(To2D(targetPoint - position), Vector2.zero, To2D(velocity)); //accelerationVector = Vector3.ClampMagnitude(accelerationVector, acceleration); // Reduce the wall avoidance force as we get closer to our target wallForceVector *= System.Math.Min(distanceToWaypoint / 0.5f, 1); if (distanceToWaypoint < endReachedDistance) { // END REACHED NextPart(); } } else { var nextNextCorner = tgIndex < nextCorners.Count - 1 ? nextCorners[tgIndex + 1] : (targetPoint - position) * 2 + position; var targetVelocity = (nextNextCorner - targetPoint).normalized * maxSpeed; accelerationVector = CalculateAccelerationToReachPoint(To2D(targetPoint - position), To2D(targetVelocity), To2D(velocity)); } // Update the velocity using the acceleration velocity += (new Vector3(accelerationVector.x, 0, accelerationVector.y) + wallForceVector * wallForce) * deltaTime; } var currentNode = fn.CurrentNode; Vector3 closestOnNode; if (currentNode != null) { closestOnNode = currentNode.ClosestPointOnNode(position); } else { closestOnNode = position; } // Distance to the end of the path (as the crow flies) var distToEndOfPath = (fn.exactEnd - closestOnNode).magnitude; // Max speed to use for this frame var currentMaxSpeed = maxSpeed; currentMaxSpeed *= Mathf.Sqrt(Mathf.Min(1, distToEndOfPath / (maxSpeed * slowdownTime))); // Check if the agent should slow down in case it is not facing the direction it wants to move in if (slowWhenNotFacingTarget) { // 1 when normdir is in the same direction as tr.forward // 0.2 when they point in the opposite directions float directionSpeedFactor = Mathf.Max((Vector3.Dot(normdir, tr.forward) + 0.5f) / 1.5f, 0.2f); currentMaxSpeed *= directionSpeedFactor; float currentSpeed = VectorMath.MagnitudeXZ(velocity); float prevy = velocity.y; velocity.y = 0; currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed); // Make sure the agent always moves in the forward direction // except when getting close to the end of the path in which case // the velocity can be in any direction velocity = Vector3.Lerp(velocity.normalized * currentSpeed, tr.forward * currentSpeed, Mathf.Clamp(targetIsEndPoint ? distanceToWaypoint * 2 : 1, 0.0f, 0.5f)); velocity.y = prevy; } else { velocity = VectorMath.ClampMagnitudeXZ(velocity, currentMaxSpeed); } // Apply gravity velocity += deltaTime * gravity; if (rvoController != null && rvoController.enabled) { // Send a message to the RVOController that we want to move // with this velocity. In the next simulation step, this velocity // will be processed and it will be fed back the rvo controller // and finally it will be used by this script when calling the // CalculateMovementDelta method below // Make sure that we don't move further than to the end point of the path // If the RVO simulation FPS is low and we did not do this, the agent // might overshoot the target a lot. var rvoTarget = position + VectorMath.ClampMagnitudeXZ(velocity, distToEndOfPath); rvoController.SetTarget(rvoTarget, VectorMath.MagnitudeXZ(velocity), maxSpeed); } // Direction and distance to move during this frame Vector3 deltaPosition; if (rvoController != null && rvoController.enabled) { // Use RVOController to get a processed delta position // such that collisions will be avoided if possible deltaPosition = rvoController.CalculateMovementDelta(position, deltaTime); // The RVOController does not know about gravity // so we copy it from the normal velocity calculation deltaPosition.y = velocity.y * deltaTime; } else { deltaPosition = velocity * deltaTime; } if (targetIsEndPoint) { // Rotate towards the direction that the agent was in // when the target point was seen for the first time // TODO: Some magic constants here, should probably compute them from other variables // or expose them as separate variables Vector3 trotdir = Vector3.Lerp(deltaPosition.normalized, currentTargetDirection, System.Math.Max(1 - distanceToWaypoint * 2, 0)); RotateTowards(trotdir); } else { // Rotate towards the direction we are moving in RotateTowards(deltaPosition); } if (controller != null && controller.enabled) { // Use CharacterController tr.position = position; controller.Move(deltaPosition); // Grab the position after the movement to be able to take physics into account position = tr.position; } else { // Use Transform float lastY = position.y; position += deltaPosition; // Position the character on the ground position = RaycastPosition(position, lastY); } // Clamp the position to the navmesh after movement is done var clampedPosition = fn.ClampToNavmesh(position); if (position != clampedPosition) { // The agent was outside the navmesh. Remove that component of the velocity // so that the velocity only goes along the direction of the wall, not into it var difference = clampedPosition - position; velocity -= difference * Vector3.Dot(difference, velocity) / difference.sqrMagnitude; // Make sure the RVO system knows that there was a collision here // Otherwise other agents may think this agent continued to move forwards // and avoidance quality may suffer if (rvoController != null && rvoController.enabled) { rvoController.SetCollisionNormal(difference); } } tr.position = clampedPosition; } else { if (rvoController != null && rvoController.enabled) { //Use RVOController rvoController.Move(Vector3.zero); } } if (currentPart is RichSpecial) { // The current path part is a special part, for example a link // Movement during this part of the path is handled by the TraverseSpecial coroutine if (!traversingSpecialPath) { StartCoroutine(TraverseSpecial(currentPart as RichSpecial)); } } } else { if (rvoController != null && rvoController.enabled) { // Use RVOController rvoController.Move(Vector3.zero); } else if (controller != null && controller.enabled) { } else { tr.position = RaycastPosition(tr.position, tr.position.y); } } }