Rotate Quadruped Character based on terrain angle

Hello,

I have a four legged character that I’m trying to make it rotate depending on the terrain (like when going up a hill the character should face up), see image below:

Here is my current code, I am using Character Controller component.

public class PlayerController1 : MonoBehaviour
{
     [System.Serializable]
     public class MoveSettings
     {
         public float forwardVel = 10f;      // walk speed
         public float rotateVel = 100;       // character rotation speed, character can walk 360 degree
         public float jumpVel = 25f;
         public LayerMask ground;
         public Transform backLeft;   // back left feet
         public Transform backRight;  // back right feet
         public Transform frontLeft;  // front left feet
         public Transform frontRight; // front left feet
     }
     [System.Serializable]
     public class PhysicsSettings
     {
         public float downAccel = 0.75f;     // down speed when not grounded
     }
     public MoveSettings moveSettings = new MoveSettings();
     public PhysicsSettings physicsSettings = new PhysicsSettings();
     private Vector3 velocity = Vector3.zero;
     private Quaternion targetRotation;
     private CharacterController cc;
     private float forwardInput, turnInput, jumpInput = 0;
     private RaycastHit lr;
     private RaycastHit rr;
     private RaycastHit lf;
     private RaycastHit rf;
     private Vector3 upDir;
     /// <summary>
     /// Rotate towards terrain slope
     /// </summary>
     public void RotateTowardsGround()
     {
         // we have four feet
         Physics.Raycast(moveSettings.backLeft.position + Vector3.up, Vector3.down, out lr);
         Physics.Raycast(moveSettings.backRight.position + Vector3.up, Vector3.down, out rr);
         Physics.Raycast(moveSettings.frontLeft.position + Vector3.up, Vector3.down, out lf);
         Physics.Raycast(moveSettings.frontRight.position + Vector3.up, Vector3.down, out rf);
         upDir = (Vector3.Cross(rr.point - Vector3.up, lr.point - Vector3.up) +
                  Vector3.Cross(lr.point - Vector3.up, lf.point - Vector3.up) +
                  Vector3.Cross(lf.point - Vector3.up, rf.point - Vector3.up) +
                  Vector3.Cross(rf.point - Vector3.up, rr.point - Vector3.up)
                 ).normalized;
         Debug.DrawRay(rr.point, Vector3.up);
         Debug.DrawRay(lr.point, Vector3.up);
         Debug.DrawRay(lf.point, Vector3.up);
         Debug.DrawRay(rf.point, Vector3.up);
         transform.forward = upDir;
     }
     public bool Grounded()
     {
         return cc.isGrounded;
     }
     private void Start()
     {
         targetRotation = transform.rotation;
         cc = GetComponent<CharacterController>();
     }
     private void Update()
     {
         GetInput();     // movement input keys
         Turn();         // character movement direction input
     }
     private void FixedUpdate()
     {
         Run();  // calculate the velocity to be applied on character controller, stored in the velocity variable
         Jump(); // code for jumping
        
         cc.Move(transform.TransformDirection(velocity) * Time.deltaTime);
         RotateTowardsGround();
     }
     private void GetInput()
     {
         forwardInput = Input.GetAxis("Vertical");
         turnInput = Input.GetAxis("Horizontal");
         jumpInput = Input.GetAxisRaw("Jump");
     }
     private void Turn()
     {
         targetRotation *= Quaternion.AngleAxis(moveSettings.rotateVel * turnInput * Time.deltaTime, Vector3.up);
         transform.rotation = targetRotation;
     }
     public void Jump()
     {
         if (jumpInput > 0 && Grounded())
         {
             velocity.y = moveSettings.jumpVel;
         }
         else if (jumpInput == 0 && Grounded())
         {
             velocity.y = 0;
         }
         else
         {
             velocity.y -= physicsSettings.downAccel;
         }
     }
     private void Run()
     {
         velocity.z = moveSettings.forwardVel * forwardInput;
     }

Currently the RotateTowardsGround() is not working correctly or is conflicting with the movement code, the character stutters and doesn’t move correctly, here are my values for the public variables and character controller:

Transform.forward will make an object face the upDir, are you sure it’s what you want? If UpDir is the normal of the surface then setting Transform.forward this way would make the character look up.

You’re doing crossproducts when you can get the normal of the surface from RaycastHit.normal.

Are you using readings from 4 appendages because you want the character to not only remain upright relative to the surface, but also take into account cases where not all of the feet are on the same surface? Have you tried doing a simple scalar average of the four normals?

Well its conflicting with my other movement code, the Y-axis turning in particular, I’m trying to achieve this: http://answers.unity3d.com/questions/168097/orient-vehicle-to-ground-normal.html

I could make it into one ray cast only but I thought one on each feet would be more accurate, however not sure if I want this anymore, I mean it would work good for a vehicle, but for a character it seems unfitting, but at the same time appearing clipped into terrain or floating mid-air a bit is also not good…