sdsandbox-rl-scripts/Scripts/Car.cs

using UnityEngine;
using System.Collections;


public class Car : MonoBehaviour, ICar{

	public CarSpawner carSpawner;
	public WheelCollider[] wheelColliders;
	public Transform[] wheelMeshes;

	public float maxSpeed = 30f;
	public float maxTorque = 50f;
	public float maxBreakTorque = 50f;
	public AnimationCurve torqueCurve;

	public Transform centrOfMass;

	public float requestTorque = 0f;
	public float requestBrake = 0f;
	public float requestSteering = 0f;

	public Vector3 acceleration = Vector3.zero;
	public Vector3 velocity = Vector3.zero;
	public Vector3 prevVel = Vector3.zero;

	public Vector3 startPos;
	public Quaternion startRot;
	private Quaternion rotation = Quaternion.identity;
	private Vector3 gyro = Vector3.zero;

	public Rigidbody rb;

	//for logging
	public float lastSteer = 0.0f;
	public float lastAccel = 0.0f;

	//when the car is doing multiple things, we sometimes want to sort out parts of the training
	//use this label to pull partial training samples from a run 
	public string activity = "keep_lane";

    public float maxSteer = 16.0f;

	//name of the last object we hit.
	public string last_collision = "none";
	private float last_collision_time = -999f;
	private const float collision_hold_seconds = 0.75f;

	// Pre-allocated buffer for per-wheel barrier overlap checks (avoids GC per frame).
	private Collider[] _wheelOverlapBuf = new Collider[8];


	// Use this for initialization
	void Awake () 
	{
		rb = GetComponent<Rigidbody>();

		if(rb && centrOfMass)
		{
			rb.centerOfMass = centrOfMass.localPosition;
		}

		// Continuous collision detection prevents the car from tunnelling through
		// thin or fast-moving colliders (including barrier walls) between frames.
		if (rb)
			rb.collisionDetectionMode = CollisionDetectionMode.Continuous;

		requestTorque = 0f;
		requestSteering = 0f;

		SavePosRot();
		
		// had to disable this because PID max steering was affecting the global max_steering
        // maxSteer = PlayerPrefs.GetFloat("max_steer", 16.0f);
	}

	public void SavePosRot()
	{
		startPos = transform.position;
		startRot = transform.rotation;
	}

	public void RestorePosRot()
	{
		Set(startPos, startRot);
	}

	public void RequestThrottle(float val)
	{
		requestTorque = val;
		requestBrake = 0f;
		//Debug.Log("request throttle: " + val);
	}

    public void SetMaxSteering(float val)
    {
        maxSteer = val;
		// had to disable this because PID max steering was affecting the global max_steering
        // PlayerPrefs.SetFloat("max_steer", maxSteer);
        // PlayerPrefs.Save();
    }

    public float GetMaxSteering()
    {
        return maxSteer;
    }

	public void RequestSteering(float val)
	{
		requestSteering = Mathf.Clamp(val, -maxSteer, maxSteer);
		//Debug.Log("request steering: " + val);
	}

	public void Set(Vector3 pos, Quaternion rot)
	{
		rb.position = pos;
		rb.rotation = rot;

		//just setting it once doesn't seem to work. Try setting it multiple times..
		StartCoroutine(KeepSetting(pos, rot, 1));
	}

	IEnumerator KeepSetting(Vector3 pos, Quaternion rot, int numIter)
	{
		while(numIter > 0)
		{
			rb.isKinematic = true;
			
			yield return new WaitForFixedUpdate();

			rb.position = pos;
			rb.rotation = rot;
			transform.position = pos;
			transform.rotation = rot;

			numIter--;

			rb.isKinematic = false;
		}
	}

	public float GetSteering()
	{
		return requestSteering;
	}

	public float GetThrottle()
	{
		return requestTorque;
	}

	public float GetFootBrake()
	{
		return requestBrake;
	}

	public float GetHandBrake()
	{
		return 0.0f;
	}

	public Vector3 GetVelocity()
	{
		return velocity;
	}

	public Vector3 GetAccel()
	{
		return acceleration;
	}
	public Vector3 GetGyro()
	{
	  return gyro;
  	}
	public float GetOrient ()
	{
		Vector3 dir = transform.forward;
		return Mathf.Atan2( dir.z, dir.x);
	}

	public Transform GetTransform()
	{
		return this.transform;
	}

	public bool IsStill()
	{
		return rb.IsSleeping();
	}

	public void RequestFootBrake(float val)
	{
		requestBrake = val;
	}

	public void RequestHandBrake(float val)
	{
		//todo
	}
	
	// Update is called once per frame
	void Update () {
	
		UpdateWheelPositions();
	}

	public string GetActivity()
	{
		return activity;
	}

	public void SetActivity(string act)
	{
		activity = act;
	}

	void FixedUpdate()
	{
		lastSteer = requestSteering;
		lastAccel = requestTorque;
		prevVel = velocity;
		velocity = transform.InverseTransformDirection(rb.velocity);
		acceleration = (velocity - prevVel)/Time.deltaTime;
		gyro = rb.angularVelocity;
		rotation = rb.rotation;

		// use the torque curve
		float throttle = torqueCurve.Evaluate(velocity.magnitude / maxSpeed) * requestTorque * maxTorque;
		float steerAngle = requestSteering;
        float brake = requestBrake * maxBreakTorque;

		//front two tires.
		wheelColliders[2].steerAngle = steerAngle;
		wheelColliders[3].steerAngle = steerAngle;

		//four wheel drive at the moment
		foreach(WheelCollider wc in wheelColliders)
		{
			wc.motorTorque = throttle;
			wc.brakeTorque = brake;
		}

		// WheelColliders don't fire OnCollisionStay on this MonoBehaviour. We use
		// two complementary checks to catch barrier contact from any angle:
		//
		// 1) Forward raycast — probes ahead of the car even before contact occurs,
		//    so it fires quickly when driving into a barrier nose-first.
		if (requestTorque > 0.05f)
		{
			Vector3 rayOrigin = transform.position + transform.up * 0.3f;
			RaycastHit rhit;
			if (Physics.Raycast(rayOrigin, transform.forward, out rhit, 0.8f))
			{
				if (!IsStartingLine(rhit.collider.gameObject.name))
					RegisterCollision(rhit.collider.gameObject.name);
			}
		}

		// 2) Per-wheel OverlapSphere — catches actual contact from any direction
		//    (side, rear, diagonal) regardless of throttle or car orientation.
		//    Uses barrier name filter to avoid false positives from the road surface.
		foreach (WheelCollider wc in wheelColliders)
		{
			Vector3 wheelCenter;
			Quaternion wheelRot;
			wc.GetWorldPose(out wheelCenter, out wheelRot);
			int n = Physics.OverlapSphereNonAlloc(wheelCenter, wc.radius + 0.05f, _wheelOverlapBuf);
			for (int i = 0; i < n; i++)
			{
				if (_wheelOverlapBuf[i] == null) continue;
				string hitName = _wheelOverlapBuf[i].gameObject.name;
				if (hitName.Contains("barrier"))
				{
					RegisterCollision(hitName);
					break;
				}
			}
		}

	}

	void FlipUpright()
	{
		Quaternion rot = Quaternion.Euler(180f, 0f, 0f);
		this.transform.rotation = transform.rotation * rot;
		transform.position = transform.position + Vector3.up * 2;
	}

	void UpdateWheelPositions()
	{
		Quaternion rot;
		Vector3 pos;

		for(int i = 0; i < wheelColliders.Length; i++)
		{
			WheelCollider wc = wheelColliders[i];
			Transform tm = wheelMeshes[i];

			wc.GetWorldPose(out pos, out rot);

			tm.position = pos;
			tm.rotation = rot;
		}
	}

	//get the name of the last object we collided with
	public string GetLastCollision()
	{
		return last_collision;
	}

	public void ClearLastCollision()
	{
		if (Time.time - last_collision_time > collision_hold_seconds)
		{
			last_collision = "none";
		}
	}

	bool ShouldPersistCollision(string collisionName, float speedMagnitude)
	{
		if (speedMagnitude < 1.5f)
		{
			return true;
		}

		string lowered = collisionName.ToLowerInvariant();
		return lowered.Contains("barrier") || lowered.Contains("tree") || lowered.Contains("wall");
	}

	// The starting/finish line is a physical collider used for lap counting.
	// It must not be treated as a barrier hit — lap counting is handled separately
	// via the collision_with_starting_line TCP message in TcpCarHandler.
	bool IsStartingLine(string objectName)
	{
		string lower = objectName.ToLowerInvariant();
		return lower.Contains("starting_line") || lower.Contains("startingline") || lower.Contains("finishline");
	}

	void RegisterCollision(string collisionName)
	{
		last_collision = collisionName;
		last_collision_time = Time.time;
	}

	void OnCollisionEnter(Collision col)
	{
		if (IsStartingLine(col.gameObject.name)) return;
		RegisterCollision(col.gameObject.name);
	}

	void OnCollisionStay(Collision col)
	{
		if (IsStartingLine(col.gameObject.name)) return;
		float speedMagnitude = (rb != null) ? rb.velocity.magnitude : 0.0f;
		if (ShouldPersistCollision(col.gameObject.name, speedMagnitude))
		{
			RegisterCollision(col.gameObject.name);
		}
	}

	// Trigger-based barriers (used by built-in tracks like mini-monaco) fire
	// OnTriggerEnter rather than OnCollisionEnter. Handle them here so that
	// out-of-bounds detection works on all tracks, not just our custom ones.
	void OnTriggerEnter(Collider other)
	{
		if (IsStartingLine(other.gameObject.name)) return;
		RegisterCollision(other.gameObject.name);
	}

	void OnTriggerStay(Collider other)
	{
		if (IsStartingLine(other.gameObject.name)) return;
		float speedMagnitude = (rb != null) ? rb.velocity.magnitude : 0.0f;
		if (ShouldPersistCollision(other.gameObject.name, speedMagnitude))
		{
			RegisterCollision(other.gameObject.name);
		}
	}
}