working rigidbody character controller. needs lotsa cleanup.

Author: micahpearlman

Runtime/Scripts/Controllers/Robots/Spot/ZOGroundDetector.cs

@@ -0,0 +1,95 @@
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace ZO {
+
+    public class ZOGroundDetector : MonoBehaviour {
+
+        public float _maxFloorAngle = 45;
+        public bool _debug = false;
+
+        public float MaxFloorAngle {
+            get => _maxFloorAngle;
+            set => _maxFloorAngle = value;
+        }
+
+        public CapsuleCollider CapsuleCollider {
+            get {
+                return GetComponent<CapsuleCollider>();
+            }
+        }
+
+        bool _isGrounded = false;
+        public bool IsGrounded {
+            get {
+                return _isGrounded;
+            }
+            private set {
+                _isGrounded = value;
+            }
+        }
+
+        Vector3 _groundNormal = -UnityEngine.Physics.gravity.normalized;
+        public Vector3 GroundNormal {
+            get => _groundNormal;
+            private set => _groundNormal = value;
+        }
+
+        Vector3 _groundPoint = Vector3.zero;
+        public Vector3 GroundPoint {
+            get => _groundPoint;
+            private set => _groundPoint = value;
+        }
+
+        // void OnCollisionEnter(Collision collision) {
+        //     IsGrounded = CheckIsGrounded(collision, CapsuleCollider, out _groundNormal);
+        // }
+
+        void OnCollisionStay(Collision collision) {
+            IsGrounded = CheckIsGrounded(collision);
+        }
+
+        void OnCollisionExit(Collision collision) {
+            IsGrounded = CheckIsGrounded(collision);
+        }
+
+        bool CheckIsGrounded(Collision collision) {
+            foreach (ContactPoint contactPoint in collision.contacts) {
+                // if (contactPoint.thisCollider == capsuleCollider) {
+                if (MaxFloorAngle > Vector3.Angle(contactPoint.normal, -UnityEngine.Physics.gravity.normalized)) {
+                    GroundNormal = contactPoint.normal;
+                    GroundPoint = contactPoint.point;
+                    
+                    // Debug.DrawRay(contactPoint.point, contactPoint.normal * 100, Random.ColorHSV(0f, 1f, 1f, 1f, 0.5f, 1f), 3f);
+
+                    return true;
+                }
+                // }
+            }
+
+            GroundNormal = -UnityEngine.Physics.gravity.normalized;
+
+            // foreach (var item in collision.contacts) {
+            //     Debug.DrawRay(item.point, item.normal * 100, Random.ColorHSV(0f, 1f, 1f, 1f, 0.5f, 1f), 10f);
+            // }
+
+
+            return false;
+        }
+
+        public void OnGUI() {
+            if (_debug) {
+                if (IsGrounded) {
+                    GUI.TextField(new Rect(10, 10, 500, 22), "On Ground");
+                } else {
+                    GUI.TextField(new Rect(10, 10, 500, 22), "Off Ground");
+                }
+
+                // Draw a different colored ray for every normal in the collision
+
+            }
+        }
+
+
+    }
+}

Runtime/Scripts/Controllers/Robots/Spot/ZOGroundDetector.cs.meta

@@ -11,8 +11,50 @@ public class ZOSpotCharacterController : MonoBehaviour {
 
         public Rigidbody _rigidBody;
         public Transform _frontCollider = null;
+
+        public CapsuleCollider FrontColliderCapsule {
+            get {
+                return _frontCollider?.GetComponent<CapsuleCollider>();
+            }
+        }
+
+        public Rigidbody FrontColliderRigidBody {
+            get {
+                return _frontCollider?.GetComponent<Rigidbody>();
+            }
+        }
+
+        public ZOGroundDetector FrontGroundDetector {
+            get {
+                return _frontCollider?.GetComponent<ZOGroundDetector>();
+            }
+        }
         public Transform _rearCollider = null;
+        public CapsuleCollider RearColliderCapsule {
+            get {
+                return _rearCollider?.GetComponent<CapsuleCollider>();
+            }
+        }
+
+        public ZOGroundDetector RearGroundDetector {
+            get {
+                return _rearCollider?.GetComponent<ZOGroundDetector>();
+            }
+        }
+
+        public Rigidbody RearColliderRigidBody {
+            get {
+                return _rearCollider?.GetComponent<Rigidbody>();
+            }
+        }
+
+
         public float _maxFloorAngle = 45;
+
+        public float MaxFloorAngle {
+            get => _maxFloorAngle;
+            set => _maxFloorAngle = value;
+        }
         public float _maxForwardVelocity = 1.0f;
 
         public float MaxForwardVelocity {
@@ -34,10 +76,41 @@ public float MaxTurnVelocityDegreesSecond {
         private Vector2 _targetVelocity = Vector2.zero;
         private float _targetTurnVelocityDegreesPerSecond = 0.0f;
 
+        private float _lieDownScale = 0.5f;
+
+        private bool IsFrontGrounded {
+            get {
+                return FrontGroundDetector.IsGrounded;
+            }
+        }
+
+        private bool IsRearGrounded {
+            get {
+                return RearGroundDetector.IsGrounded;
+            }
+        }
+
+
+        private Vector3 FrontGroundNormal {
+            get {
+                return FrontGroundDetector.GroundNormal;
+            }
+        }
+
+
+        private Vector3 RearGroundNormal {
+            get {
+                return RearGroundDetector.GroundNormal;
+            }
+        }
+
+
+
         public enum StateEnum {
             Off,
-            Down,
+            Sitting,
             UpAndReady,
+            Busy,
             Error
         }
 
@@ -60,46 +133,105 @@ public ZOSpotCharacterController.ErrorEnum Error {
 
 
         public delegate void SpotControllerStatusDelegate(ZOSpotCharacterController thisClass, Vector2 linearVelocity, float turnVelocityDegreesPerSecond, float orientationDegrees, ZOSpotCharacterController.StateEnum state, ZOSpotCharacterController.ErrorEnum error);
-        private event SpotControllerStatusDelegate _spotContrllerStatusDelegate;
+        private event SpotControllerStatusDelegate _spotControllerStatusDelegate;
         public event SpotControllerStatusDelegate SpotControllerStatus {
             add {
-                _spotContrllerStatusDelegate += value;
+                _spotControllerStatusDelegate += value;
             }
             remove {
-                _spotContrllerStatusDelegate -= value;
+                _spotControllerStatusDelegate -= value;
             }
         }
 
 
 
         // Start is called before the first frame update
         void Start() {
-
+            // start sitting down
+            Sit();
         }
 
         // Update is called once per frame
         void Update() {
 
         }
 
+
+
         void FixedUpdate() {
 
-            _rigidBody.velocity = _rigidBody.transform.rotation * new Vector3(_targetVelocity.x, _rigidBody.velocity.y, _targetVelocity.y);
+            if (IsFrontGrounded || IsRearGrounded) {
+
+                Vector3 angularVelocity = _rigidBody.angularVelocity;
+                float torqueYaw = (_targetTurnVelocityDegreesPerSecond * Mathf.Deg2Rad) - angularVelocity.y;
+                _rigidBody.AddRelativeTorque(new Vector3(0, torqueYaw, 0), ForceMode.VelocityChange);
 
-            Vector3 angularVelocity = _rigidBody.angularVelocity;
-            angularVelocity.y = _targetTurnVelocityDegreesPerSecond * Mathf.Deg2Rad;
-            _rigidBody.angularVelocity = angularVelocity;
+                Vector3 localVelocity = _rigidBody.transform.InverseTransformDirection(_rigidBody.velocity);
+                Vector3 targetVelocity = new Vector3(_targetVelocity.x, 0, _targetVelocity.y);
+                Vector3 deltaVelocity = targetVelocity - localVelocity;
+                deltaVelocity = _rigidBody.transform.TransformDirection(deltaVelocity);
+                _rigidBody.AddForce(deltaVelocity, ForceMode.VelocityChange);
 
-            if (_spotContrllerStatusDelegate != null) {
-                _spotContrllerStatusDelegate.Invoke(this, new Vector2(_rigidBody.velocity.x, _rigidBody.velocity.z), _rigidBody.angularVelocity.y * Mathf.Rad2Deg, transform.rotation.eulerAngles.y, State, Error);
 
             }
+
+
+            if (_spotControllerStatusDelegate != null) {
+                _spotControllerStatusDelegate.Invoke(this, new Vector2(_rigidBody.velocity.x, _rigidBody.velocity.z), _rigidBody.angularVelocity.y * Mathf.Rad2Deg, transform.rotation.eulerAngles.y, State, Error);
+
+            }
+
+            // obey gravity
+            if (IsFrontGrounded == true) {
+                Vector3 gravity = -FrontGroundNormal * UnityEngine.Physics.gravity.magnitude * 20.2f;
+                FrontColliderRigidBody.AddForce(gravity, ForceMode.Acceleration);
+            } else {
+                FrontColliderRigidBody.AddForce(UnityEngine.Physics.gravity * 2.2f, ForceMode.Acceleration);
+            }
+
+            if (IsRearGrounded == true) {
+                Vector3 gravity = -RearGroundNormal * UnityEngine.Physics.gravity.magnitude * 20.2f;
+                RearColliderRigidBody.AddForce(gravity, ForceMode.Acceleration);
+            } else {
+                RearColliderRigidBody.AddForce(UnityEngine.Physics.gravity * 2.2f, ForceMode.Acceleration);
+            }
+
+            // if (IsFrontGrounded == false || IsRearGrounded == false) {
+            //     _rigidBody.AddForce(UnityEngine.Physics.gravity, ForceMode.Acceleration);
+            // }
+
+        }
+
+
+
+
+        public void TurnOn() {
+
+        }
+
+        public void Stand() {
+            if (State == ZOSpotCharacterController.StateEnum.Sitting) {
+                FrontColliderCapsule.height = FrontColliderCapsule.height * (1.0f / _lieDownScale);
+                RearColliderCapsule.height = RearColliderCapsule.height * (1.0f / _lieDownScale);
+                State = ZOSpotCharacterController.StateEnum.UpAndReady;
+            }
+        }
+
+        public void Sit() {
+            if (State == ZOSpotCharacterController.StateEnum.UpAndReady || State == ZOSpotCharacterController.StateEnum.Off) {
+                FrontColliderCapsule.height = FrontColliderCapsule.height * _lieDownScale;
+                RearColliderCapsule.height = RearColliderCapsule.height * _lieDownScale;
+                State = ZOSpotCharacterController.StateEnum.Sitting;
+            }
+
         }
 
 
         public void Move(Vector2 targetVelocity, float targetTurnVelocityDegreesPerSecond) {
-            _targetVelocity = targetVelocity;
-            _targetTurnVelocityDegreesPerSecond = targetTurnVelocityDegreesPerSecond;
+            if (State == ZOSpotCharacterController.StateEnum.UpAndReady) {
+                _targetVelocity = targetVelocity;
+                _targetTurnVelocityDegreesPerSecond = targetTurnVelocityDegreesPerSecond;
+            }
         }
         // void Sit(Delegate OnSitFinishedOrError)
         // void Stand(Delegate OnStandFinishedOrError)

Runtime/Scripts/Controllers/Robots/Spot/ZOSpotCharacterController.cs

@@ -1,24 +1,39 @@
 using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
-
 namespace ZO {
     public class ZOSpotJoystickCmdVel : MonoBehaviour {
 
         public ZOSpotCharacterController _spotCharacterController;
 
+
         // Start is called before the first frame update
         void Start() {
-
         }
 
+        // public void SitCB(InputAction.CallbackContext context) {
+        //     Debug.Log("YO");
+        // }
+
         // Update is called once per frame
-        void Update() {
+        void FixedUpdate() {
+
+            if (Input.GetAxis("Sit") > 0) {
+                Debug.Log("INFO: Sit");
+                _spotCharacterController.Sit();
+            }
+
+            if (Input.GetButtonDown("Stand") == true) {
+                Debug.Log("INFO: Stand");
+                _spotCharacterController.Stand();
+            }
+
 
             if (_spotCharacterController != null) {
                 Vector2 targetVelocity = new Vector2(Input.GetAxis("Roll") * _spotCharacterController.MaxSideMoveVelocity, Input.GetAxis("Pitch") * _spotCharacterController.MaxForwardVelocity);
                 _spotCharacterController.Move(targetVelocity, Input.GetAxis("Yaw") * _spotCharacterController._maxTurnVelocityDegreesSecond); // hack no turning yet
 
+
             }
 
         }