x-ite's diary

覚え書きです。想定読者は俺

Car AI Calculate torque and steer with waypoints in C#

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class CarAI : MonoBehaviour {

    public Transform CarTransform;

    public WheelCollider WCFrontLeft;
    public WheelCollider WCFrontRight;
    public WheelCollider WCRearLeft;
    public WheelCollider WCRearRight;

    public float EnginePower = 50.0f;
    public float MaxSteerAngle = 20.0f;

    public List<Vector3> Waypoints;
    public int CurrentWaypointIndex = 0;

    void Update() {
       
       Vector3 waypoint = Waypoints[CurrentWaypointIndex];

       Vector3 relativeWaypointPosition = CarTransform.InverseTransformPoint(
            new Vector3(waypoint.x, 
                       CarTransform.position.y, 
                       waypoint.z )
        );
                                       
        float steerRatio = relativeWaypointPosition.x / relativeWaypointPosition.magnitude;
        float torqueRatio;
 
        if (Mathf.Abs(steerRatio) < 0.5f) {
            torqueRatio = relativeWaypointPosition.z / relativeWaypointPosition.magnitude - Mathf.Abs(steerRatio);
        } else {
            torqueRatio = 0.0f;
        }
        
        float motorTorque = EnginePower * torqueRatio;
        float steerAngle = MaxSteerAngle * steerRatio;

        WCFrontLeft.motorTorque  = WCFrontLeft.isGrounded ? motorTorque : 0.0f;
        WCFrontRight.motorTorque = WCFrontRight.isGrounded ? motorTorque : 0.0f;
        WCRearLeft.motorTorque   = WCRearLeft.isGrounded ? motorTorque : 0.0f;
        WCRearRight.motorTorque  = WCRearRight.isGrounded ? motorTorque : 0.0f;

        WCFrontLeft.steerAngle  = steerAngle;
        WCFrontRight.steerAngle = steerAngle;
    }
}