sirius_unity_sim/Assets/Scripts/ResetPosition.cs

using UnityEngine;

public class ResetPosition : MonoBehaviour
{
    [SerializeField] private Terrain currentTerrain;
    [SerializeField] private LayerMask _terrainLayer = 1 << 0;
    [SerializeField] private float _groundOffset = 1.0f;  // m, shift during raycast to stick robot to the ground

    public bool startRotation = false;
    private Vector3 targetEuler, targetPosition;
    private Quaternion targetRotation;
    private Vector3 terrainPosition, terrainNormal;
    private float normalizedX, normalizedZ;
    private Vector3 faceDirection;

    private ArticulationBody body;
    // draw ray and spawn object little up
    // Start is called once before the first execution of Update after the MonoBehaviour is created

    void Start()
    {
        body = GetComponent<ArticulationBody>();
    }

    void FixedUpdate() {
        if (startRotation)
        {
            FixPosition();
        }
    }

    void FixPosition()
    {
        terrainPosition = transform.position - currentTerrain.transform.position;
        normalizedX = Mathf.InverseLerp(0, currentTerrain.terrainData.size.x, terrainPosition.x);
        normalizedX = Mathf.Clamp01(normalizedX);
        normalizedZ = Mathf.InverseLerp(0, currentTerrain.terrainData.size.z, terrainPosition.z);
        normalizedZ = Mathf.Clamp01(normalizedZ);
        terrainNormal = currentTerrain.terrainData.GetInterpolatedNormal(normalizedX, normalizedZ);

        faceDirection = Vector3.Cross(terrainNormal, -transform.right);

        Quaternion predictedRot = Quaternion.LookRotation(faceDirection, terrainNormal);
        targetEuler = predictedRot.eulerAngles;
        targetEuler.y = transform.rotation.eulerAngles.y;

        targetRotation = Quaternion.Euler(targetEuler);
        targetPosition = new Vector3(
            transform.position.x,
            transform.position.y + _groundOffset,
            transform.position.z);

        body.TeleportRoot(targetPosition, targetRotation);

        startRotation = false;
    }
}