//With this you can rotate safely by doing it iteratively. The maxAngleDifference decides how often you will iterate (lower = more iterations).
        public bool SetRotationNow(Quaternion rotation, float maxAngleDifference, bool resetToOriginalIfFail, LayerMask layerMask)
        {
            Quaternion originalRotation = transform.rotation;
            Quaternion previousRotation = transform.rotation;

            int   intervals  = Mathf.CeilToInt(Quaternion.Angle(transform.rotation, rotation) / maxAngleDifference);
            float subdivided = 1f / intervals;

            for (int i = 1; i <= intervals; i++)
            {
                transform.rotation = Quaternion.Slerp(transform.rotation, rotation, subdivided * i);
                if (ExtPhysics.CheckCapsule(transform.position, transform.up, capsuleCollider.height - (smallOffset * 2f), capsuleCollider.radius - smallOffset, ignoreColliders, layerMask))
                {
                    if (resetToOriginalIfFail)
                    {
                        transform.rotation = originalRotation;
                    }
                    else
                    {
                        transform.rotation = previousRotation;
                    }

                    return(false);
                }
                else
                {
                    previousRotation = transform.rotation;
                }
            }

            return(true);
        }
Ejemplo n.º 2
0
        public static List <SphereCollisionInfo> DetectCollisions(Vector3 segment0, Vector3 segment1, float radius, int mask, IList <Component> ignoreColliders, List <SphereCollisionInfo> resultBuffer, float checkOffset = 0f, bool multipleContactsPerCollider = true)
        {
            resultBuffer.Clear();
            colliderBufferCapsule.Clear();

            ExtPhysics.OverlapCapsule(segment0, segment1, radius + checkOffset, ignoreColliders, colliderBufferCapsule, mask);
            if (colliderBufferCapsule.Count == 0)
            {
                return(resultBuffer);
            }

            for (int i = 0; i < colliderBufferCapsule.Count; i++)
            {
                contactsBufferCapsule = ExtCollider.ClosestPointsOnSurface(colliderBufferCapsule[i], segment0, segment1, radius + checkOffset, contactsBufferCapsule, multipleContactsPerCollider);

                for (int j = 0; j < contactsBufferCapsule.Count; j++)
                {
                    //We calculate sphereDetectionOriginInCapsule for our depenetration method since we need to know where the spheres detection origin would be within the capsule.
                    Vector3 sphereDetectionOriginInCapsule = Vector3.zero;
                    if ((colliderBufferCapsule[i] is CapsuleCollider || colliderBufferCapsule[i] is SphereCollider) && !ExtVector3.IsParallel(segment1 - segment0, contactsBufferCapsule[j].normal))
                    {
                        sphereDetectionOriginInCapsule = Geometry.ClosestPointsOnSegmentToLine(segment0, segment1, contactsBufferCapsule[j].point, contactsBufferCapsule[j].normal).first;
                    }
                    else
                    {
                        sphereDetectionOriginInCapsule = Geometry.ClosestPointOnLineSegmentToPoint(contactsBufferCapsule[j].point, segment0, segment1);
                    }

                    //We store just the radius, not radius + checkOffset, so that our depenetration method has the correct radius to depenetrate with.
                    resultBuffer.Add(new SphereCollisionInfo(true, colliderBufferCapsule[i], sphereDetectionOriginInCapsule, radius, contactsBufferCapsule[j].point, contactsBufferCapsule[j].normal));
                }
            }

            return(resultBuffer);
        }
Ejemplo n.º 3
0
        public static List <SphereCollisionInfo> DetectCollisions(Vector3 origin, float radius, int mask, IList <Component> ignoreColliders, List <SphereCollisionInfo> resultBuffer, float checkOffset = 0f, bool multipleContactsPerCollider = true)
        {
            resultBuffer.Clear();
            colliderBufferSphere.Clear();

            ExtPhysics.OverlapSphere(origin, radius + checkOffset, ignoreColliders, colliderBufferSphere, mask);
            if (colliderBufferSphere.Count == 0)
            {
                return(resultBuffer);
            }

            for (int i = 0; i < colliderBufferSphere.Count; i++)
            {
                contactsBufferSphere = ExtCollider.ClosestPointsOnSurface(colliderBufferSphere[i], origin, radius + checkOffset, contactsBufferSphere, multipleContactsPerCollider);

                for (int j = 0; j < contactsBufferSphere.Count; j++)
                {
                    //We store just the radius, not radius + checkOffset, so that our depenetration method has the correct radius to depenetrate with.
                    resultBuffer.Add(new SphereCollisionInfo(true, colliderBufferSphere[i], origin, radius, contactsBufferSphere[j].point, contactsBufferSphere[j].normal));
                }
            }

            return(resultBuffer);
        }
        CollisionInfo GetCollisionSafeVelocity(Vector3 targetVelocity)
        {
            if (collisionHandleInfo.abortIfFailedThisFrame && Time.frameCount == collisionFailedFrame)
            {
                return new CollisionInfo()
                       {
                           hasFailed = true
                       }
            }
            ;

            CollisionInfo collisionInfo = new CollisionInfo();

            Vector3   origin            = transform.position;
            Vector3   targetPosition    = origin + targetVelocity;
            Vector3   previousHitNormal = Vector3.zero;
            LayerMask mask        = ~ignoreLayers;
            Vector3   transformUp = transform.up;

            //We cut our velocity up into steps so that we never move more than a certain amount of our radius per step.
            //This prevents tunneling and acts as a "Continuous Collision Detection", but is not as good as using a CapsuleCast.
            int     steps        = 1;
            Vector3 stepVelocity = targetVelocity;
            float   distance     = Vector3.Distance(origin, targetPosition);

            if (distance > maxRadiusMove)
            {
                steps = Mathf.CeilToInt(distance / maxRadiusMove);

                if (steps > collisionHandleInfo.maxVelocitySteps)
                {
                    steps = collisionHandleInfo.maxVelocitySteps;

                    #region Debug
#if UNITY_EDITOR
                    if (infoDebug.printOverMaxVelocitySteps)
                    {
                        Debug.LogWarning("PlayerRigidbody GetCollisionSafeVelocity velocity steps is larger than maxVelocitySteps. To avoid major lag we are limiting the amount of steps which means unsafe collision handling.", gameObject);
                    }
#endif
                    #endregion
                }

                stepVelocity /= steps;
            }

            int attempts = 0;
            for (int i = 0; i < steps; i++)
            {
                Vector3 previousOrigin = origin;
                origin        += stepVelocity;
                targetPosition = origin;
                float negativeOffset = 0;

                for (attempts = 0; attempts < collisionHandleInfo.maxCollisionCheckIterations; attempts++)
                {
                    Vector3 hitNormal = Vector3.zero;
                    bool    hasHit    = false;
                    //It is important for us to have a negativeOffset, otherwise our collision detection methods might keep telling us we are penetrated...
                    //There seems to be float precision errors on large convex mesh colliders (not even that large, like 40 units wide) when using Physics.CheckCapsule (and possibly other physics methods) so try to allow negativeOffset to go below -.01f or so. Not sure if will work well at lower player scales.
                    if (attempts > 0 && negativeOffset > -safeCheckOffset)
                    {
                        negativeOffset += -smallOffset;
                    }

                    //It is advised to do your grounding somewhere here depending on your grounding method and I also think its better for framerate independence.
                    //Keep in mind the the way your collision system is, it can make or break your chances of framerate independence.
                    Vector3 groundAndStepDepenetration = Grounding(previousOrigin, origin, mask);
                    if (groundAndStepDepenetration != Vector3.zero && groundAndStepDepenetration.sqrMagnitude > (negativeOffset).Squared())
                    {
                        hasHit    = true;
                        hitNormal = groundNormal;
                        origin    = origin + groundAndStepDepenetration;
                    }

                    if (ExtPhysics.CheckCapsule(origin, transformUp, capsuleHeight + (negativeOffset * 2f), capsuleRadius + negativeOffset, ignoreColliders, mask))
                    {
                        List <SphereCollisionInfo> collisionPoints = SphereCollisionDetect.DetectCollisions(origin, transformUp, capsuleHeight, capsuleRadius, mask, ignoreColliders, collisionPointBuffer, safeCheckOffset);

                        if (collisionPoints.Count > 0)
                        {
                            if (collisionHandleInfo.tryBlockAtSlopeLimit)
                            {
                                TryBlockAtSlopeLimit(collisionPoints);
                            }

                            //Not tested, but might be a good idea to use this if it works...
                            if (collisionHandleInfo.cleanByIgnoreBehindPlane)
                            {
                                SphereCollisionDetect.CleanByIgnoreBehindPlane(collisionPoints);
                            }

                            #region Debug
                #if UNITY_EDITOR
                            DrawContactsDebug(collisionPoints, .5f, Color.magenta, Color.green);
                #endif
                            #endregion

                            //We do the main depenetration method
                            Vector3 depenetration = SphereCollisionDetect.Depenetrate(collisionPoints, collisionHandleInfo.maxDepenetrationIterations);
                            depenetration = Vector3.ClampMagnitude(depenetration, maxRadiusMove);                             //We clamp to make sure we dont depenetrate too much into possibly unsafe areas

                            origin = origin + depenetration;

                            hitNormal = (depenetration != Vector3.zero) ? depenetration.normalized : hitNormal;

                            //Final check if we are safe, if not then we just move a little and hope for the best.
                            if (ExtPhysics.CheckCapsule(origin, transformUp, capsuleHeight + ((negativeOffset - smallOffset) * 2f), capsuleRadius + (negativeOffset - smallOffset), ignoreColliders, mask))
                            {
                                origin += (hitNormal * smallOffset);
                            }

                            hasHit = true;
                        }
                    }

                    if (hasHit)
                    {
                        collisionInfo.attempts++;
                        previousHitNormal = hitNormal;
                        targetPosition    = origin;
                    }
                    else
                    {
                        break;
                    }
                }

                //Even if collisionHandleInfo.depenetrateEvenIfUnsafe is true, we exit early so that we dont continue trying to move when we are having issues depenetrating.
                if (attempts >= collisionHandleInfo.maxCollisionCheckIterations)
                {
                    //Failed to find a safe spot, breaking out early.
                    break;
                }
            }

            if (attempts < collisionHandleInfo.maxCollisionCheckIterations || collisionHandleInfo.depenetrateEvenIfUnsafe)
            {
                collisionInfo.hasCollided = (collisionInfo.attempts > 0);

                collisionInfo.safeMoveDirection = targetPosition - transform.position;

                //We handle redirecting our velocity. First we just default it to the targetVelocity.
                collisionInfo.velocity = targetVelocity;

                //If we are already moving in a direction that is not colliding with the normal, we dont redirect the velocity.
                if (!ExtVector3.IsInDirection(targetVelocity, previousHitNormal, tinyOffset, false))
                {
                    //If we are on an edge then we dont care if we cant walk on the slope since our grounding will count the edge as a ground and friction will slow us down.
                    if ((!isOnEdge && !CanWalkOnSlope(previousHitNormal)) || GoingOverEdge(targetVelocity))
                    {
                        collisionInfo.velocity = Vector3.ProjectOnPlane(targetVelocity, previousHitNormal);
                    }
                    else if (isGrounded)
                    {
                        //We flatten our velocity. This helps us move up and down slopes, but also has a bad side effect of not having us fly off slopes correctly.
                        collisionInfo.velocity = Vector3.ProjectOnPlane(targetVelocity, transformUp);
                    }
                }
            }

            if (attempts >= collisionHandleInfo.maxCollisionCheckIterations)
            {
                //Couldnt find a safe spot. We should hopefully not get to this point.

                #region Debug
#if UNITY_EDITOR
                if (infoDebug.printFailCollision)
                {
                    Debug.LogWarning("PlayRigidbody Collision has failed!", gameObject);
                }
                if (infoDebug.pauseOnFailCollision)
                {
                    UnityEditor.EditorApplication.isPaused = true;
                }
#endif
                #endregion

                collisionFailedFrame      = Time.frameCount;
                collisionInfo.hasCollided = true;
            }

            #region Debug
#if UNITY_EDITOR
            if (infoDebug.printAttempts && collisionInfo.attempts >= infoDebug.minAttemptsToStartPrint)
            {
                Debug.Log("(" + steps + ", " + collisionInfo.attempts + ") (Velocity SubSteps, Total Collision Attempts)", gameObject);
            }
#endif
            #endregion

            return(collisionInfo);
        }

        //Needs more work for high angles (like 70+ angles).
        //- We would need in our GetCollisionSafeVelocity to gather all detected collision points below our bottomSphere and offset their SphereCollisionInfo.radius by some small amount so that we are sure it will not detect the high slope.
        Vector3 Grounding(Vector3 previousOrigin, Vector3 origin, LayerMask layerMask)
        {
            friction = 0;

            float radius = capsuleRadius;

            GroundCastInfo hitInfo = GroundCast(previousOrigin, origin, radius, layerMask);

            if (hitInfo.hasHit)
            {
                if (CanWalkOnSlope(hitInfo.normal))
                {
                    isGrounded   = true;
                    isOnEdge     = hitInfo.onEdge;
                    groundNormal = hitInfo.normal;
                    groundPoint  = hitInfo.point;
                    friction     = SpecialPhysicsMaterial.GetFriction(hitInfo.collider);

                    //We use a spherecast because we will only trust the spherecast to detect a safe spot to depenetrate
                    //to since DepenetrateSphereFromPlaneInDirection treats the hit as an infinite plane, which is not desired.
                    //We will iterate with the DepenetrateSphereFromPlaneInDirection info to hopefully find the correct safe spot.

                    Vector3 transformUp  = transform.up;
                    Vector3 bottomSphere = GetCapsulePoint(origin, -transformUp);
                    hitInfo.depenetrationDistance += smallOffset;                     //just to give the spherecast some breathing room in case the distance is very small.
                    int     iterations    = Mathf.CeilToInt(hitInfo.depenetrationDistance / maxRadiusMove);
                    float   distance      = hitInfo.depenetrationDistance / iterations;
                    float   stepDistance  = 0;
                    float   depenetration = 0;
                    Vector3 castStart     = bottomSphere;
                    for (int i = 0; i < iterations; i++)
                    {
                        stepDistance += distance;
                        castStart    += (transformUp * stepDistance);

                        //Its important to do a checksphere first, otherwise we might start the cast inside the desired collider, miss it and detect a different collider.
                        if (!ExtPhysics.CheckSphere(castStart, radius, ignoreColliders, layerMask))
                        {
                            //I subtract minOffset from the radius since for some reason the spherecast detects scaled box colliders when right next to them.
                            RaycastHit castHitInfo = ExtPhysics.SphereCast(castStart, radius - tinyOffset, -transformUp, ignoreColliders, stepDistance + groundCheckOffset, layerMask);
                            if (castHitInfo.collider != null)
                            {
                                #region Debug
                        #if UNITY_EDITOR
                                DrawGroundDebug(castHitInfo.point, castHitInfo.normal, 1, Color.white, Color.green);
                        #endif
                                #endregion

                                //We subtract groundOffset to make sure we depenetrate enough so that our GetCollisionSafeVelocity does not detect anything, but small enough so we dont lose contact.
                                Vector3 safePosition = castStart - (transformUp * (castHitInfo.distance - groundOffset));

                                //It is important to check if we are on an edge and the castHitInfo edge hitPoint is on the same plane as the hitInfo edge since if it wasnt a walkable slope,
                                //we would have depenetrated upwards using that non walkable slope which would cause us to not be grounded anymore which can lead to
                                //an infinite loop of trying to be grounded which would increase our downward velocity infinitely.
                                //You need the isOnEdge check if you want to be able to walk on a edge that is too steep, but is an edge of a walkable slope.
                                bool isWalkable = CanWalkOnSlope(castHitInfo.normal);
                                if (isWalkable || (isOnEdge && MPlane.IsOnPlane(hitInfo.point, hitInfo.normal, castHitInfo.point, smallOffset)))
                                {
                                    depenetration = Mathf.Max(0f, ExtVector3.MagnitudeInDirection(safePosition - bottomSphere, transformUp, false));

                                    //CeilingDetected is very important to prevent going through things, as well as to help our GetCollisionSafeVelocity deal with opposing normals better.
                                    //CeilingDetected is not perfect, for example if our bottomSphere is low enough into the floor that the GroundCast highestPoint couldnt be set to points detected
                                    //above bottomSphere, we will undesirably detect a "ceiling". This shouldnt be an issue as long as we do our maxRadiusMove.
                                    Vector3 newTopSphere       = GetCapsulePoint(origin, transformUp) + (transformUp * (depenetration + smallOffset));
                                    Vector3 bottomSphereOffset = bottomSphere - (transformUp * groundCheckOffset);
                                    if (CeilingDetected(newTopSphere, bottomSphereOffset, radius, layerMask, hitInfo.highestPoint, hitInfo.highestPointNormal))
                                    {
                                        depenetration = 0;
                                    }
                                    else if (isWalkable)
                                    {
                                        groundNormal = castHitInfo.normal;
                                        groundPoint  = castHitInfo.point;
                                    }
                                }

                                //We break out after the first hit detected whether it was a good one or not since we assume the first hit is the one we are most interested in.
                                break;
                            }
                        }
                    }

                    return(transformUp * depenetration);
                }
                else if (detectFrictionOnNonWalkable)
                {
                    friction = SpecialPhysicsMaterial.GetFriction(hitInfo.collider);
                }
            }

            isGrounded   = false;
            isOnEdge     = false;
            groundNormal = Vector3.zero;
            groundPoint  = Vector3.zero;
            return(Vector3.zero);
        }

        List <SphereCollisionInfo> groundContactsBuffer = new List <SphereCollisionInfo>();
        GroundCastInfo GroundCast(Vector3 previousOrigin, Vector3 origin, float radius, LayerMask layerMask)
        {
            Vector3 transformUp = transform.up;

            Vector3 topSphere    = GetCapsulePoint(origin, transformUp);
            Vector3 bottomSphere = GetCapsulePoint(origin, -transformUp);
            //We use groundCheckOffset as a way to ensure we wont depenetrate ourselves too far off the ground to miss its detection next time.
            Vector3 bottomSphereOffset = bottomSphere - (transformUp * groundCheckOffset);

            //When we check to see if the hitpoint is below or above our bottomsphere, we want to take into account where we moved.
            //If we moved upwards, then just use our current bottomSphere, but if we moved downwards, then lets use our previous as the reference.
            Vector3 previousBottomSphere  = GetCapsulePoint(previousOrigin, -transformUp);
            Vector3 bottomHeightReference = (ExtVector3.IsInDirection(origin - previousOrigin, transformUp)) ? bottomSphere : previousBottomSphere;

            GroundCastInfo walkable             = new GroundCastInfo(float.MinValue);
            GroundCastInfo nonWalkable          = new GroundCastInfo(float.MinValue);
            GroundCastInfo averagedWalkable     = new GroundCastInfo(float.MinValue);
            float          highestPointDistance = float.MaxValue;
            Vector3        highestPoint         = Vector3.zero;
            Vector3        highestPointNormal   = Vector3.zero;

            SphereCollisionDetect.DetectCollisions(topSphere, bottomSphereOffset, radius, layerMask, ignoreColliders, groundContactsBuffer);

            //We search for the best ground.
            for (int i = 0; i < groundContactsBuffer.Count; i++)
            {
                SphereCollisionInfo collisionPoint = groundContactsBuffer[i];

                //We make sure the hit is below our bottomSphere
                if (!ExtVector3.IsInDirection(collisionPoint.closestPointOnSurface - bottomHeightReference, -transformUp, tinyOffset, false))
                {
                    continue;
                }

                Vector3 hitPoint = GetBetterHitPoint(collisionPoint.collider, collisionPoint.closestPointOnSurface);

                Vector3 normal = collisionPoint.interpolatedNormal;
                //If we are on a edge, it is possible that we penetrated far enough that the interpolatedNormal returns a too steep angle.
                //So we try to find the actual surface normal that faces our origin, and if it isnt found then we just use the hitpoint to origin as a saftey.
                if (collisionPoint.isOnEdge)
                {
                    RaycastHit hitInfo = ExtPhysics.SphereCast(hitPoint + (transformUp * .03f), .01f, -transformUp, collisionPoint.collider, .06f, layerContext: ExtPhysics.Inclusion.IncludeOnly);
                    if (hitInfo.collider != null && ExtVector3.IsInDirection(hitInfo.normal, transformUp))
                    {
                        normal = ExtVector3.ClosestDirectionTo(collisionPoint.normal, hitInfo.normal, transformUp);
                    }
                    else
                    {
                        normal = collisionPoint.normal;
                    }

                    //We want the normal that faces our transform.up the most.
                    normal = ExtVector3.ClosestDirectionTo(collisionPoint.interpolatedNormal, normal, transformUp);
                }

                //This will be useful for when we check to make sure our grounding doesnt depenetrate into or through objects.
                float pointDistance = Mathf.Max(0f, ExtVector3.MagnitudeInDirection(hitPoint - bottomHeightReference, -transformUp, false));
                if (pointDistance < highestPointDistance)
                {
                    highestPoint         = hitPoint;
                    highestPointNormal   = normal;
                    highestPointDistance = pointDistance;
                }

                float depenetrationDistance = Geometry.DepenetrateSphereFromPlaneInDirection(bottomSphereOffset, radius, transformUp, hitPoint, normal).distance;
                if (CanWalkOnSlope(normal))
                {
                    if (depenetrationDistance > walkable.depenetrationDistance)
                    {
                        walkable.Set(hitPoint, normal, collisionPoint.collider, collisionPoint.isOnEdge, depenetrationDistance);

                        #region Debug
#if UNITY_EDITOR
                        DrawGroundDebug(walkable.point, walkable.normal, 1, Color.cyan, Color.green);
#endif
                        #endregion
                    }
                }
                else
                {
                    //We try to see if we are on a platform like a V shape. If we are, then we want to count that as grounded.
                    Vector3 averageNormal = (normal + nonWalkable.normal).normalized;
                    if (CanWalkOnSlope(averageNormal) && Vector3.Dot(averageNormal, transformUp) > Vector3.Dot(averagedWalkable.normal, transformUp) + tinyOffset)
                    {
                        SweepInfo sweep = Geometry.SpherePositionBetween2Planes(radius, nonWalkable.point, nonWalkable.normal, hitPoint, normal, false);
                        if (!sweep.hasHit || sweep.distance < averagedWalkable.depenetrationDistance)
                        {
                            continue;
                        }

                        //Our grounding does not handle depenetrating us from averageNormals, we are mainly just passing the averageNormal so we can be considered grounded.
                        //Our GetCollisionSafeVelocity will handle depenetrating us. This means we dont have much controll over how we want to handle average normals.
                        //So for average normals we will just slide off edges.
                        averagedWalkable.Set(sweep.intersectPoint, averageNormal, collisionPoint.collider, false, sweep.distance);

                        #region Debug
#if UNITY_EDITOR
                        DrawGroundDebug(averagedWalkable.point, averagedWalkable.normal, 1, Color.yellow, Color.green);
#endif
                        #endregion
                    }

                    if (depenetrationDistance > nonWalkable.depenetrationDistance)
                    {
                        nonWalkable.Set(hitPoint, normal, collisionPoint.collider, collisionPoint.isOnEdge, depenetrationDistance);

                        #region Debug
        #if UNITY_EDITOR
                        DrawGroundDebug(nonWalkable.point, nonWalkable.normal, 1, Color.blue, Color.green);
        #endif
                        #endregion
                    }
                    else
                    {
                        #region Debug
        #if UNITY_EDITOR
                        DrawGroundDebug(collisionPoint.closestPointOnSurface, normal, .2f, Color.gray, Color.green);
        #endif
                        #endregion
                    }
                }
            }

            if (walkable.hasHit)
            {
                walkable.SetHighest(highestPoint, highestPointNormal);
                return(walkable);
            }
            if (averagedWalkable.hasHit)
            {
                averagedWalkable.SetHighest(highestPoint, highestPointNormal);
                return(averagedWalkable);
            }
            nonWalkable.SetHighest(highestPoint, highestPointNormal);
            return(nonWalkable);
        }

        List <SphereCollisionInfo> ceilingContactsBuffer = new List <SphereCollisionInfo>();
        GroundCastInfo GroundCast(Vector3 previousOrigin, Vector3 origin, float radius, LayerMask layerMask)
        {
            Vector3 transformUp = transform.up;

            Vector3 topSphere    = GetCapsulePoint(origin, transformUp);
            Vector3 bottomSphere = GetCapsulePoint(origin, -transformUp);
            //We use groundCheckOffset as a way to ensure we wont depenetrate ourselves too far off the ground to miss its detection next time.
            Vector3 bottomSphereOffset = bottomSphere - (transformUp * groundCheckOffset);

            //When we check to see if the hitpoint is below or above our bottomsphere, we want to take into account where we moved.
            //If we moved upwards, then just use our current bottomSphere, but if we moved downwards, then lets use our previous as the reference.
            Vector3 previousBottomSphere  = GetCapsulePoint(previousOrigin, -transformUp);
            Vector3 bottomHeightReference = (ExtVector3.IsInDirection(origin - previousOrigin, transformUp)) ? bottomSphere : previousBottomSphere;

            GroundCastInfo walkable             = new GroundCastInfo(float.MinValue);
            GroundCastInfo nonWalkable          = new GroundCastInfo(float.MinValue);
            GroundCastInfo averagedWalkable     = new GroundCastInfo(float.MinValue);
            float          highestPointDistance = float.MaxValue;
            Vector3        highestPoint         = Vector3.zero;
            Vector3        highestPointNormal   = Vector3.zero;

            SphereCollisionDetect.DetectCollisions(topSphere, bottomSphereOffset, radius, layerMask, ignoreColliders, groundContactsBuffer);

            //We search for the best ground.
            for (int i = 0; i < groundContactsBuffer.Count; i++)
            {
                SphereCollisionInfo collisionPoint = groundContactsBuffer[i];

                //We make sure the hit is below our bottomSphere
                if (!ExtVector3.IsInDirection(collisionPoint.closestPointOnSurface - bottomHeightReference, -transformUp, tinyOffset, false))
                {
                    continue;
                }

                Vector3 hitPoint = GetBetterHitPoint(collisionPoint.collider, collisionPoint.closestPointOnSurface);

                Vector3 normal = collisionPoint.interpolatedNormal;
                //If we are on a edge, it is possible that we penetrated far enough that the interpolatedNormal returns a too steep angle.
                //So we try to find the actual surface normal that faces our origin, and if it isnt found then we just use the hitpoint to origin as a saftey.
                if (collisionPoint.isOnEdge)
                {
                    RaycastHit hitInfo = ExtPhysics.SphereCast(hitPoint + (transformUp * .03f), .01f, -transformUp, collisionPoint.collider, .06f, layerContext: ExtPhysics.Inclusion.IncludeOnly);
                    if (hitInfo.collider != null && ExtVector3.IsInDirection(hitInfo.normal, transformUp))
                    {
                        normal = ExtVector3.ClosestDirectionTo(collisionPoint.normal, hitInfo.normal, transformUp);
                    }
                    else
                    {
                        normal = collisionPoint.normal;
                    }

                    //We want the normal that faces our transform.up the most.
                    normal = ExtVector3.ClosestDirectionTo(collisionPoint.interpolatedNormal, normal, transformUp);
                }

                //This will be useful for when we check to make sure our grounding doesnt depenetrate into or through objects.
                float pointDistance = Mathf.Max(0f, ExtVector3.MagnitudeInDirection(hitPoint - bottomHeightReference, -transformUp, false));
                if (pointDistance < highestPointDistance)
                {
                    highestPoint         = hitPoint;
                    highestPointNormal   = normal;
                    highestPointDistance = pointDistance;
                }

                float depenetrationDistance = Geometry.DepenetrateSphereFromPlaneInDirection(bottomSphereOffset, radius, transformUp, hitPoint, normal).distance;
                if (CanWalkOnSlope(normal))
                {
                    if (depenetrationDistance > walkable.depenetrationDistance)
                    {
                        walkable.Set(hitPoint, normal, collisionPoint.collider, collisionPoint.isOnEdge, depenetrationDistance);

                        #region Debug
#if UNITY_EDITOR
                        DrawGroundDebug(walkable.point, walkable.normal, 1, Color.cyan, Color.green);
#endif
                        #endregion
                    }
                }
                else
                {
                    //We try to see if we are on a platform like a V shape. If we are, then we want to count that as grounded.
                    Vector3 averageNormal = (normal + nonWalkable.normal).normalized;
                    if (CanWalkOnSlope(averageNormal) && Vector3.Dot(averageNormal, transformUp) > Vector3.Dot(averagedWalkable.normal, transformUp) + tinyOffset)
                    {
                        SweepInfo sweep = Geometry.SpherePositionBetween2Planes(radius, nonWalkable.point, nonWalkable.normal, hitPoint, normal, false);
                        if (!sweep.hasHit || sweep.distance < averagedWalkable.depenetrationDistance)
                        {
                            continue;
                        }

                        //Our grounding does not handle depenetrating us from averageNormals, we are mainly just passing the averageNormal so we can be considered grounded.
                        //Our GetCollisionSafeVelocity will handle depenetrating us. This means we dont have much controll over how we want to handle average normals.
                        //So for average normals we will just slide off edges.
                        averagedWalkable.Set(sweep.intersectPoint, averageNormal, collisionPoint.collider, false, sweep.distance);

                        #region Debug
#if UNITY_EDITOR
                        DrawGroundDebug(averagedWalkable.point, averagedWalkable.normal, 1, Color.yellow, Color.green);
#endif
                        #endregion
                    }

                    if (depenetrationDistance > nonWalkable.depenetrationDistance)
                    {
                        nonWalkable.Set(hitPoint, normal, collisionPoint.collider, collisionPoint.isOnEdge, depenetrationDistance);

                        #region Debug
        #if UNITY_EDITOR
                        DrawGroundDebug(nonWalkable.point, nonWalkable.normal, 1, Color.blue, Color.green);
        #endif
                        #endregion
                    }
                    else
                    {
                        #region Debug
        #if UNITY_EDITOR
                        DrawGroundDebug(collisionPoint.closestPointOnSurface, normal, .2f, Color.gray, Color.green);
        #endif
                        #endregion
                    }
                }
            }

            if (walkable.hasHit)
            {
                walkable.SetHighest(highestPoint, highestPointNormal);
                return(walkable);
            }
            if (averagedWalkable.hasHit)
            {
                averagedWalkable.SetHighest(highestPoint, highestPointNormal);
                return(averagedWalkable);
            }
            nonWalkable.SetHighest(highestPoint, highestPointNormal);
            return(nonWalkable);
        }
        //Needs more work for high angles (like 70+ angles).
        //- We would need in our GetCollisionSafeVelocity to gather all detected collision points below our bottomSphere and offset their SphereCollisionInfo.radius by some small amount so that we are sure it will not detect the high slope.
        Vector3 Grounding(Vector3 previousOrigin, Vector3 origin, LayerMask layerMask)
        {
            friction = 0;

            float radius = capsuleRadius;

            GroundCastInfo hitInfo = GroundCast(previousOrigin, origin, radius, layerMask);

            if (hitInfo.hasHit)
            {
                if (CanWalkOnSlope(hitInfo.normal))
                {
                    isGrounded   = true;
                    isOnEdge     = hitInfo.onEdge;
                    groundNormal = hitInfo.normal;
                    groundPoint  = hitInfo.point;
                    friction     = SpecialPhysicsMaterial.GetFriction(hitInfo.collider);

                    //We use a spherecast because we will only trust the spherecast to detect a safe spot to depenetrate
                    //to since DepenetrateSphereFromPlaneInDirection treats the hit as an infinite plane, which is not desired.
                    //We will iterate with the DepenetrateSphereFromPlaneInDirection info to hopefully find the correct safe spot.

                    Vector3 transformUp  = transform.up;
                    Vector3 bottomSphere = GetCapsulePoint(origin, -transformUp);
                    hitInfo.depenetrationDistance += smallOffset;                     //just to give the spherecast some breathing room in case the distance is very small.
                    int     iterations    = Mathf.CeilToInt(hitInfo.depenetrationDistance / maxRadiusMove);
                    float   distance      = hitInfo.depenetrationDistance / iterations;
                    float   stepDistance  = 0;
                    float   depenetration = 0;
                    Vector3 castStart     = bottomSphere;
                    for (int i = 0; i < iterations; i++)
                    {
                        stepDistance += distance;
                        castStart    += (transformUp * stepDistance);

                        //Its important to do a checksphere first, otherwise we might start the cast inside the desired collider, miss it and detect a different collider.
                        if (!ExtPhysics.CheckSphere(castStart, radius, ignoreColliders, layerMask))
                        {
                            //I subtract minOffset from the radius since for some reason the spherecast detects scaled box colliders when right next to them.
                            RaycastHit castHitInfo = ExtPhysics.SphereCast(castStart, radius - tinyOffset, -transformUp, ignoreColliders, stepDistance + groundCheckOffset, layerMask);
                            if (castHitInfo.collider != null)
                            {
                                #region Debug
                        #if UNITY_EDITOR
                                DrawGroundDebug(castHitInfo.point, castHitInfo.normal, 1, Color.white, Color.green);
                        #endif
                                #endregion

                                //We subtract groundOffset to make sure we depenetrate enough so that our GetCollisionSafeVelocity does not detect anything, but small enough so we dont lose contact.
                                Vector3 safePosition = castStart - (transformUp * (castHitInfo.distance - groundOffset));

                                //It is important to check if we are on an edge and the castHitInfo edge hitPoint is on the same plane as the hitInfo edge since if it wasnt a walkable slope,
                                //we would have depenetrated upwards using that non walkable slope which would cause us to not be grounded anymore which can lead to
                                //an infinite loop of trying to be grounded which would increase our downward velocity infinitely.
                                //You need the isOnEdge check if you want to be able to walk on a edge that is too steep, but is an edge of a walkable slope.
                                bool isWalkable = CanWalkOnSlope(castHitInfo.normal);
                                if (isWalkable || (isOnEdge && MPlane.IsOnPlane(hitInfo.point, hitInfo.normal, castHitInfo.point, smallOffset)))
                                {
                                    depenetration = Mathf.Max(0f, ExtVector3.MagnitudeInDirection(safePosition - bottomSphere, transformUp, false));

                                    //CeilingDetected is very important to prevent going through things, as well as to help our GetCollisionSafeVelocity deal with opposing normals better.
                                    //CeilingDetected is not perfect, for example if our bottomSphere is low enough into the floor that the GroundCast highestPoint couldnt be set to points detected
                                    //above bottomSphere, we will undesirably detect a "ceiling". This shouldnt be an issue as long as we do our maxRadiusMove.
                                    Vector3 newTopSphere       = GetCapsulePoint(origin, transformUp) + (transformUp * (depenetration + smallOffset));
                                    Vector3 bottomSphereOffset = bottomSphere - (transformUp * groundCheckOffset);
                                    if (CeilingDetected(newTopSphere, bottomSphereOffset, radius, layerMask, hitInfo.highestPoint, hitInfo.highestPointNormal))
                                    {
                                        depenetration = 0;
                                    }
                                    else if (isWalkable)
                                    {
                                        groundNormal = castHitInfo.normal;
                                        groundPoint  = castHitInfo.point;
                                    }
                                }

                                //We break out after the first hit detected whether it was a good one or not since we assume the first hit is the one we are most interested in.
                                break;
                            }
                        }
                    }

                    return(transformUp * depenetration);
                }
                else if (detectFrictionOnNonWalkable)
                {
                    friction = SpecialPhysicsMaterial.GetFriction(hitInfo.collider);
                }
            }

            isGrounded   = false;
            isOnEdge     = false;
            groundNormal = Vector3.zero;
            groundPoint  = Vector3.zero;
            return(Vector3.zero);
        }
        CollisionInfo GetCollisionSafeVelocity(Vector3 targetVelocity)
        {
            CollisionInfo collisionInfo = new CollisionInfo();

            if (collisionHandleInfo.abortIfFailedThisFrame && Time.frameCount == collisionFailedFrame)
            {
                return(collisionInfo);
            }

            Vector3   origin            = transform.position;
            Vector3   targetPosition    = origin + targetVelocity;
            Vector3   previousHitNormal = Vector3.zero;
            LayerMask mask        = ~ignoreLayers;
            Vector3   transformUp = transform.up;

            //We cut our velocity up into steps so that we never move more than a certain amount of our radius per step.
            //This prevents tunneling and acts as a "Continuous Collision Detection", but is not as good as using a CapsuleCast.
            int     steps        = 1;
            Vector3 stepVelocity = targetVelocity;
            float   distance     = Vector3.Distance(origin, targetPosition);

            if (distance > maxRadiusMove)
            {
                steps = Mathf.CeilToInt(distance / maxRadiusMove);
                if (steps > collisionHandleInfo.maxVelocitySteps)
                {
                    steps = collisionHandleInfo.maxVelocitySteps;

                    #region Debug
#if UNITY_EDITOR
                    if (infoDebug.printOverMaxVelocitySteps)
                    {
                        Debug.LogWarning("PlayerRigidbody GetCollisionSafeVelocity velocity steps is larger than maxVelocitySteps. To avoid major lag we are limiting the amount of steps which means unsafe collision handling.", gameObject);
                    }
#endif
                    #endregion
                }

                stepVelocity /= steps;
            }

            int attempts = 0;
            for (int i = 0; i < steps; i++)
            {
                Vector3 previousOrigin = origin;
                origin        += stepVelocity;
                targetPosition = origin;
                float negativeOffset = 0;

                for (attempts = 0; attempts < collisionHandleInfo.maxCollisionCheckIterations; attempts++)
                {
                    Vector3 hitNormal = Vector3.zero;
                    bool    hasHit    = false;
                    //It is important for us to have a negativeOffset, otherwise our collision detection methods might keep telling us we are penetrated...
                    if (attempts > 0 && attempts < collisionHandleInfo.addNegativeOffsetUntilAttempt)
                    {
                        negativeOffset += -smallOffset;
                    }

                    //It is advised to do your grounding somewhere here depending on your grounding method and I also think its better for framerate independence.
                    //Keep in mind the the way your collision system is, it can make or break your chances of framerate independence.
                    Vector3 groundAndStepDepenetration = Grounding(previousOrigin, origin, mask);
                    if (groundAndStepDepenetration != Vector3.zero && groundAndStepDepenetration.sqrMagnitude > (negativeOffset).Squared())
                    {
                        hasHit    = true;
                        hitNormal = groundNormal;
                        origin    = origin + groundAndStepDepenetration;
                    }

                    if (ExtPhysics.CheckCapsule(origin, transformUp, capsuleHeight + (negativeOffset * 2f), capsuleRadius + negativeOffset, ignoreColliders, mask))
                    {
                        List <SphereCollisionInfo> collisionPoints = SphereCollisionDetect.DetectCollisions(origin, transformUp, capsuleHeight, capsuleRadius, mask, ignoreColliders, collisionPointBuffer, safeCheckOffset);

                        if (collisionPoints.Count > 0)
                        {
                            if (collisionHandleInfo.tryBlockAtSlopeLimit)
                            {
                                TryBlockAtSlopeLimit(collisionPoints);
                            }

                            //Not tested, but might be a good idea to use this if it works...
                            if (collisionHandleInfo.cleanByIgnoreBehindPlane)
                            {
                                SphereCollisionDetect.CleanByIgnoreBehindPlane(collisionPoints);
                            }

                            #region Debug
                #if UNITY_EDITOR
                            DrawContactsDebug(collisionPoints, .5f, Color.magenta, Color.green);
                #endif
                            #endregion

                            //We do the main depenetration method
                            Vector3 depenetration = SphereCollisionDetect.Depenetrate(collisionPoints, collisionHandleInfo.maxDepenetrationIterations);
                            depenetration = Vector3.ClampMagnitude(depenetration, maxRadiusMove);                             //We clamp to make sure we dont depenetrate too much into possibly unsafe areas

                            origin = origin + depenetration;

                            hitNormal = (depenetration != Vector3.zero) ? depenetration.normalized : hitNormal;

                            //Final check if we are safe, if not then we just move a little and hope for the best.
                            if (ExtPhysics.CheckCapsule(origin, transformUp, capsuleHeight + ((negativeOffset - smallOffset) * 2f), capsuleRadius + negativeOffset - smallOffset, ignoreColliders, mask))
                            {
                                origin += (hitNormal * smallOffset);
                            }

                            hasHit = true;
                        }
                    }

                    if (hasHit)
                    {
                        collisionInfo.attempts++;
                        previousHitNormal = hitNormal;
                        targetPosition    = origin;
                    }
                    else
                    {
                        break;
                    }
                }

                //Even if collisionHandleInfo.depenetrateEvenIfUnsafe is true, we exit early so that we dont continue trying to move when we are having issues depenetrating.
                if (attempts >= collisionHandleInfo.maxCollisionCheckIterations)
                {
                    //Failed to find a safe spot, breaking out early.
                    break;
                }
            }

            if (attempts < collisionHandleInfo.maxCollisionCheckIterations || collisionHandleInfo.depenetrateEvenIfUnsafe)
            {
                collisionInfo.hasCollided = (collisionInfo.attempts > 0);

                collisionInfo.safeMoveDirection = targetPosition - transform.position;

                //We handle redirecting our velocity. First we just default it to the targetVelocity.
                collisionInfo.velocity = targetVelocity;

                //If we are already moving in a direction that is not colliding with the normal, we dont redirect the velocity.
                if (!ExtVector3.IsInDirection(targetVelocity, previousHitNormal, tinyOffset, false))
                {
                    //If we are on an edge then we dont care if we cant walk on the slope since our grounding will count the edge as a ground and friction will slow us down.
                    if ((!isOnEdge && !CanWalkOnSlope(previousHitNormal)) || GoingOverEdge(targetVelocity))
                    {
                        collisionInfo.velocity = Vector3.ProjectOnPlane(targetVelocity, previousHitNormal);
                    }
                    else if (isGrounded)
                    {
                        //We flatten our velocity. This helps us move up and down slopes, but also has a bad side effect of not having us fly off slopes correctly.
                        collisionInfo.velocity = Vector3.ProjectOnPlane(targetVelocity, transformUp);
                    }
                }
            }

            if (attempts >= collisionHandleInfo.maxCollisionCheckIterations)
            {
                //Couldnt find a safe spot. We should hopefully not get to this point.

                #region Debug
#if UNITY_EDITOR
                if (infoDebug.printFailCollision)
                {
                    Debug.LogWarning("PlayRigidbody Collision has failed!", gameObject);
                }
                if (infoDebug.pauseOnFailCollision)
                {
                    UnityEditor.EditorApplication.isPaused = true;
                }
#endif
                #endregion

                collisionFailedFrame      = Time.frameCount;
                collisionInfo.hasCollided = true;
            }

            #region Debug
#if UNITY_EDITOR
            if (infoDebug.printAttempts && collisionInfo.attempts >= infoDebug.minAttemptsToStartPrint)
            {
                Debug.Log("(" + steps + ", " + collisionInfo.attempts + ") (Velocity SubSteps, Total Collision Attempts)", gameObject);
            }
#endif
            #endregion

            return(collisionInfo);
        }