Collision Response Algorithm for 3D Axis-Aligned Box Collider

Hello!

I'm writing a game that doesn't need complex physics. For that reason I am giving any object that needs physics enacted on it a box collider and a rigidbody with all constraints turned on. This way it still fires the onCollisionEnter and onCollisionStay methods which give much more information about collisions than the onTrigger equivalents.

The problem I'm having is with how I'm handling the collisions. I'll write the method out in English and include the relevant code just to cover everything. What happens when I'm using this algorithm is that the player kind of bounces back and forth just overlapping the object its colliding with. What I want to happen is for the collision handling to prevent any overlap.

My movement algorithm is: In the update function

1. Based on player input, set a movement vector.

2. Based on collisions modify the movement vector

3. Apply the movement vector to the transform of the player object

4. Clear all logged collisions

OnCollisionEnter and OnCollisionStay

1. Add the collision object to an arrayList of collision objects

My collision handling (#2) works like this:

1. Get the current center of the collider (collider.center + collider.transform)

2. Get the distance to the collision and store it in a Vector3

3. Scale the distanceToCollision vector by the collision normal (leaves just the distance to the collision along the direction of the collision)

4. Get the distance to the edge of the collider closest to the collision (Take a vector3 containing half the width, height and depth, multiply it by the collision normal)

5. If the distanceToCollision (#3) magnitude is less or equal to the distanceToEdge (#4) magnitude (ie. the player collider is inside the other collider) take the difference, and create a vector of that distance along the normal.

6. Apply this new vector to the movement vector (from the update function. (The movement vector is then applied to the transform of the object)

Ok and here's the code:

 void Update(){
 //Set Input Velocity
         if (Input.GetKey (KeyCode.W))
         {
             velocity += (Vector3.forward * speed * Time.deltaTime);
         }
         if (Input.GetKey (KeyCode.S))
         {
             velocity += (Vector3.forward * -speed * Time.deltaTime);
         }
         if (Input.GetKey (KeyCode.A))
         {
             velocity += (Vector3.right * -speed * Time.deltaTime);
         }
         if (Input.GetKey (KeyCode.D))
         {
             velocity += (Vector3.right * speed * Time.deltaTime);
         }
 
         //Rectify Velocity with any collisions
         for (int i=0;i<collisionLog.Count;i++){
             HandleCollision((Collision)(collisionLog[i]));
         }
 
         //Move Entity
         transform.Translate (velocity);
 }


 void HandleCollision(Collision col){
         Vector3 correctiveMoveVector = Vector3.zero;
         
         Vector3 colliderCenterPos = collider.center + collider.transform.position;
         
         //1. Determine Distance To Collision
         Vector3 distToCollision = new Vector3(col.contacts[0].point.x - colliderCenterPos.x,
                                               col.contacts[0].point.y - colliderCenterPos.y,
                                               col.contacts[0].point.z - colliderCenterPos.z);
         
         distToCollision = Vector3.Scale(distToCollision, col.contacts[0].normal);
         
         Vector3 distToEdgeAlongColNormal = Vector3.Scale (col.contacts[0].normal, distToEdges);
 
         //Debug.Break();
         
         if (distToCollision.magnitude <= distToEdgeAlongColNormal.magnitude){
             float backtrackDist = distToEdgeAlongColNormal.magnitude - distToCollision.magnitude;
             Vector3 backtrackVector = new Vector3(backtrackDist,backtrackDist,backtrackDist);
             backtrackVector = Vector3.Scale(backtrackVector, col.contacts[0].normal);
             correctiveMoveVector += backtrackVector;
         }
         
         velocity += correctiveMoveVector;
     }

 void OnCollisionEnter(Collision col){
         LogCollision(col);
     }
     void OnCollisionStay(Collision col){
         LogCollision(col);
     }
 
     void LogCollision(Collision col){
         collisionLog.Add(col);
     }
 
     void resetCollisionLog(){
         collisionLog.Clear();
     }