Rigidbody collision correction with raycast

So, i have a rigidbody and a capsule collider attached to my player object. I've made a script that pretty much mimics the standard character controller using a rigidbody (I need it to walk on walls) With the help of unity answers it's come pretty far: http://pastebin.com/JdXFsBZT

I'm trying to make it so when a player jumps into a wall it's capsule is pushed back out so the player doesnt get stuck in the wall. This is done trough the UpdateVelocity function which gets called from inside FixedUpdate

void UpdateVelocity(Vector3 desiredVelocity, Vector3 jumpVelocity) {
        float Distance = CapsuleComponent.radius * 2.0f;
        RaycastHit hitInfo;

        if (Physics.Raycast(CachedTransform.position, desiredVelocity.normalized, out hitInfo, Distance, ignoreLayer))
            desiredVelocity = hitInfo.normal * hitInfo.distance;

        Vector3 VelocityDelta = desiredVelocity - RigidbodyComponent.velocity;
        Vector3 AntiGravity = GravityDirection * DotProduct(VelocityDelta, GravityDirection) * -1.0f;

        RigidbodyComponent.AddForce(VelocityDelta + AntiGravity + jumpVelocity, ForceMode.VelocityChange);
        RigidbodyComponent.AddForce(GravityDirection * Gravity * RigidbodyComponent.mass * Time.deltaTime, ForceMode.VelocityChange);
    }

This works as expected. The issue is that my character is very jerky. It will collide, be pushed back move towards the wall and collide again... I tried changing float Distance = CapsuleComponent.radius * 2.0f; to just float Distance = CapsuleComponent.radius; but then the character gets stuck in the wall.

How can i figure out the collision sweet spot where the character will be naturally pushed away from the colliding object?

Thanks ~Gabe

In case anyone has the same question, the solutions is really not that hard.

if (Physics.Raycast(CachedTransform.position, desiredVelocity.normalized, out hitInfo, Distance, ignoreLayer))
            desiredVelocity = (desiredVelocity.normalized * hitInfo.distance) + hitInfo.normal * hitInfo.distance;