void DriveActuator(AI.Goal goal) { // min and max refers to the speed of the car not the size of the angle float minForwardAngle = 120f * Mathf.Deg2Rad; float maxForwardAngle = car.steerMax * Mathf.Deg2Rad / 4; float minBackAngle = 120f * Mathf.Deg2Rad; float maxBackAngle = 170f * Mathf.Deg2Rad; float speedFactor = Mathf.Clamp01(car.speed * 0.036f - 0.2f); // scales from 20 to 120 kph float backAngle = Mathf.Lerp(minBackAngle, maxBackAngle, speedFactor); Vector3 backArc = car.transform.TransformDirection(new Vector3(Mathf.Sin(backAngle), 0f, Mathf.Cos(backAngle))); Debug.DrawRay(car.transform.position, backArc, Color.cyan); float forwardAngle = Mathf.Lerp(minForwardAngle, maxForwardAngle, speedFactor); Vector3 forwardArc = car.transform.TransformDirection(new Vector3(Mathf.Sin(forwardAngle), 0f, Mathf.Cos(forwardAngle))); Debug.DrawRay(car.transform.position, forwardArc, Color.cyan); Vector2 forward2D = new Vector2(car.transform.forward.x, car.transform.forward.z).normalized; Vector2 carPos2D = new Vector2(car.transform.position.x, car.transform.position.z); ControlProxy control = new ControlProxy(); // New steer based on direction Vector2 targetDir = (goal.position - carPos2D).normalized; float goalAngle = Vector2.Angle(goal.direction, forward2D) * Mathf.Deg2Rad; if (goalAngle < forwardAngle) { // targetAngle will try to match position with the goal float targetAngle = Vector2.Angle(targetDir, forward2D) * Mathf.Deg2Rad; Vector3 localTarget = car.transform.InverseTransformDirection(new Vector3(targetDir.x, 0f, targetDir.y)); control.steer = Mathf.Clamp(targetAngle * Mathf.Sign(localTarget.x) / (car.steerMax * Mathf.Deg2Rad), -1f, 1f); // note Mathf.Sign() is not Mathf.Sin() control.throttle = Mathf.Max(0.25f, Mathf.Abs(localTarget.z)); if (car.speed < 0) { control.steer *= -1; } } else if (goalAngle > backAngle) { if (debugUseBreak) { Debug.Break(); } float targetAngle = Vector2.Angle(targetDir, forward2D) * Mathf.Deg2Rad; Vector3 localTarget = car.transform.InverseTransformDirection(new Vector3(targetDir.x, 0f, targetDir.y)); control.steer = Mathf.Clamp(targetAngle * Mathf.Sign(localTarget.x) / (car.steerMax * Mathf.Deg2Rad), -1f, 1f); // note Mathf.Sign() is not Mathf.Sin() if (car.speed < 5.0f) { control.steer *= -1; control.throttle = 1.0f; control.reverse = true; } else { control.throttle = 0.0f; control.brake = 1.0f; } } else { // steep turns will try to match direction with the goal not position Vector2 velo2D = new Vector2(car.rigidbody.velocity.x, car.rigidbody.velocity.z); float approachVelo = Vector2.Dot(velo2D, targetDir); float distToNextCorner = (goal.cornerPosition - carPos2D).magnitude; float turnScale = approachVelo / distToNextCorner; Vector3 localTarget = car.transform.InverseTransformDirection(new Vector3(goal.direction.x, 0f, goal.direction.y)); control.steer = Mathf.Clamp(goalAngle * Mathf.Sign(localTarget.x) / (car.steerMax * Mathf.Deg2Rad) * turnScale, -1f, 1f); control.throttle = 1f; } control.gear = car.currentGear; if (car.engineRPM > car.autoShiftUp) { if (car.currentGear < car.gearRatios.Length - 1) { car.currentGear += 1; } } else if (car.engineRPM < car.autoShiftDown) { if (car.currentGear > 1) { car.currentGear -= 1; } } if (!debugUseAI) { return; } car.SetControls(control); }
/* In this context a path param is a 1D measurement along the path. * This calculates the nearest param on the path to position. currentParam is used * so that the search will start at a specific spot on the path and continue * consecutivly rather than search the whole path every time. */ public float GetNearestParam(ref AI.Goal goal, Vector2 position, float currentParam) { goal.hasPosition = true; float segmentTotal = 0f; int targetSegment = 0; currentParam %= totalLength; // this just any overflow after each lap if (currentParam < segmentLengths[0]) { targetSegment = 0; } else { for (int i = 0; i < segmentLengths.Length; i++) { if (segmentTotal + segmentLengths[i] > currentParam) { targetSegment = i; break; } segmentTotal += segmentLengths[i]; } } float shortestDistance2 = Mathf.Infinity; //shortest distance squared goal.position = segments2D[targetSegment]; float newDistance2; // new distanceSquared int searchSegment = targetSegment; bool fullLap = false; do { int nextSegment = searchSegment == segments2D.Length - 1 ? 0 : searchSegment + 1; //if current segment is the last segment then next is 0 Vector2 nearestPoint = NearestPointOnLine(position, segments2D[searchSegment], segments2D[nextSegment]); newDistance2 = (nearestPoint - position).sqrMagnitude; if (newDistance2 > shortestDistance2) { break; } if (nextSegment == 0) { fullLap = true; } targetSegment = searchSegment; shortestDistance2 = newDistance2; goal.position = nearestPoint; searchSegment = nextSegment; //currentSegment == segments2D.Length - 1 ? 0 : currentSegment + 1;// increment unless it is the last segment then go to 0 } while (true); float newParam = 0; for (int i = 0; i < targetSegment; i++) { newParam += segmentLengths[i]; } float sumOfSegments = newParam; newParam += (goal.position - segments2D[targetSegment]).magnitude; int directionSegment = targetSegment == segments2D.Length - 1 ? 0 : targetSegment + 1; goal.hasDirection = true; goal.direction = (segments2D[directionSegment] - segments2D[targetSegment]).normalized; int nextDirectionSegment = directionSegment == segments2D.Length - 1 ? 0 : directionSegment + 1; Vector2 nextDirection = (segments2D[nextDirectionSegment] - segments2D[directionSegment]).normalized; goal.hasCornerWarning = true; goal.cornerPosition = segments2D[directionSegment]; goal.cornerAngle = 1 - Vector2.Angle(goal.direction, nextDirection) / 90f; if (newParam >= currentParam) { // moving forward normally. use position calculated above return(newParam); } else if (fullLap) { // moving forward edge case where hte new point is just past the zero point in the track. Use position from above. return(newParam); } else { // moving backwards in not allowed. Recalculate position based on param passed in. float partialSegment = currentParam - sumOfSegments; goal.position = segments2D[targetSegment] + goal.direction * partialSegment; return(currentParam); } }