//This is very slow depending on how many contacts there are...
        void CleanUp(List <ClosestTrianglePoint> closestPoints, List <ContactInfo> resultsBuffer)
        {
            if (closestPoints.Count > 0)
            {
                ignoreBehindPlanes.Clear();

                //Taking advantage of C# built in QuickSort algorithm.
                closestPoints.Sort(ClosestTrianglePointComparerAscend.defaultComparer);

                //Note - If we are in a corner and our sphere origin is behind 1 wall while also large enough to touch the other wall, the first wall normal would block the second due to the .0001 offset below since the points lie on the same plane.
                //This is actually desired since the interpolatednormal of that point would have been going towards inside the other wall, which means we would be try to depenetrate into inside the wall...
                //Problem - I have noticed cases where this removes points we actually wanted, such as when we are on an edge of a single mesh touching its ceiling and floor. We would get the ceiling and floor normals as
                //well as the edges side normals. One of the side normals would end up removing 1 of the ceiling / floor normals, giving our depenetration method trouble and possibly getting us stuck.
                for (int i = 0; i < closestPoints.Count; i++)
                {
                    ClosestTrianglePoint closestPoint = closestPoints[i];
                    Vector3 planeNormal = tree.GetTriangleNormal(closestPoint.triangleIndex);

                    if (planeNormal == Vector3.zero)
                    {
                        continue;
                    }

                    if (!MPlane.IsBehindPlanes(closestPoint.position, ignoreBehindPlanes, -.0001f))                    //thanks to the .0001 offset we avoid duplicates
                    {
                        resultsBuffer.Add(new ContactInfo(transform.TransformPoint(closestPoint.position), transform.TransformDirection(planeNormal)));
                    }

                    ignoreBehindPlanes.Add(new MPlane(planeNormal, closestPoint.position, false));
                }
            }
        }
        void Split(List <int> triangles, Vector3 partitionPoint, Vector3 partitionNormal, out List <int> positiveTriangles, out List <int> negativeTriangles)
        {
            positiveTriangles = new List <int>();
            negativeTriangles = new List <int>();

            for (int i = 0; i < triangles.Count; i++)
            {
                int triangle = triangles[i];

                Vector3 triangleCenter = (vertices[tris[triangle]] + vertices[tris[triangle + 1]] + vertices[tris[triangle + 2]]) / 3f;
                bool    pointAbove     = MPlane.PointAbovePlane(partitionPoint, partitionNormal, triangleCenter);

                if (pointAbove)
                {
                    positiveTriangles.Add(triangle);
                }
                else
                {
                    negativeTriangles.Add(triangle);
                }
            }
        }
Ejemplo n.º 3
0
        public static List <SphereCollisionInfo> CleanByIgnoreBehindPlane(List <SphereCollisionInfo> collisionPoints)
        {
            if (collisionPoints.Count > 1)
            {
                ignoreBehindPlanes.Clear();

                //Taking advantage of C# built in QuickSort algorithm
                collisionPoints.Sort(SphereCollisionInfo.SphereCollisionComparerDescend.defaultComparer);

                for (int i = collisionPoints.Count - 1; i >= 0; i--)
                {
                    if (!MPlane.IsBehindPlanes(collisionPoints[i].closestPointOnSurface, ignoreBehindPlanes, -.0001f))
                    {
                        ignoreBehindPlanes.Add(new MPlane(collisionPoints[i].interpolatedNormal, collisionPoints[i].closestPointOnSurface, false));
                    }
                    else
                    {
                        collisionPoints.RemoveAt(i);
                    }
                }
            }

            return(collisionPoints);
        }
        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>();
        //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);
        }