//Doing some SohCahToa to find where the sphere would safely sit within the 2 opposing normals.
public static SweepInfo SpherePositionBetween2Planes(float radius, Vector3 plane1Point, Vector3 plane1Normal, Vector3 plane2Point, Vector3 plane2Normal, bool normalizeParameter = true)
{
if (normalizeParameter)
{
plane1Normal.Normalize();
plane2Normal.Normalize();
}
Vector3 averageNormal = (plane1Normal + plane2Normal).normalized;
Vector3 p1Projected = Vector3.ProjectOnPlane(averageNormal, plane1Normal);
float angle = Mathf.Abs(ExtVector3.Angle(averageNormal, p1Projected.normalized));
if (angle > 0)
{
Vector3 p2Projected = Vector3.ProjectOnPlane(averageNormal, plane2Normal);
Vector3 intersectedPosition = Geometry.ClosestPointsOnTwoLines(plane1Point, p1Projected, plane2Point, p2Projected).first;
SweepInfo sweep = new SweepInfo();
sweep.hasHit = true;
sweep.distance = radius / Mathf.Sin(angle * Mathf.Deg2Rad);
sweep.intersectCenter = intersectedPosition + (averageNormal * sweep.distance);
sweep.intersectPoint = intersectedPosition + (averageNormal * (sweep.distance - radius));
return(sweep);
}
return(new SweepInfo());
}
//We use Soh from SohCahToa
public static SweepInfo DepenetrateSphereFromPlaneInDirection(Vector3 spherePosition, float radius, Vector3 depenetrationDirection, Vector3 planePoint, Vector3 planeNormal, bool normalizeParameter = true)
{
if (normalizeParameter)
{
depenetrationDirection.Normalize();
planeNormal.Normalize();
}
float distanceToPlane = LinePlaneDistance(spherePosition, -planeNormal, planePoint, planeNormal);
if (Mathf.Abs(distanceToPlane) < radius)
{
float depenetrationDistance = radius - distanceToPlane;
float angle = Mathf.Abs(90f - ExtVector3.Angle(depenetrationDirection, planeNormal));
if (angle > 0)
{
SweepInfo sweep = new SweepInfo();
sweep.hasHit = true;
sweep.distance = depenetrationDistance / Mathf.Sin(angle * Mathf.Deg2Rad);
sweep.intersectCenter = spherePosition + (depenetrationDirection * sweep.distance);
sweep.intersectPoint = spherePosition - (planeNormal * distanceToPlane);
return(sweep);
}
}
return(new SweepInfo());
}
public SweepInfo LinePlaneIntersect(Vector3 origin, Vector3 direction, bool normalize = true)
{
if (normalize)
{
direction.Normalize();
}
SweepInfo sweep = new SweepInfo();
sweep.distance = Geometry.LinePlaneDistance(origin, direction, origin, normal);
if (sweep.distance != 0f)
{
sweep.hasHit = true;
sweep.intersectPoint = origin + (direction * sweep.distance);
sweep.intersectCenter = sweep.intersectPoint;
}
return(sweep);
}
//This tries to stops us if we are grounded and trying to walk up a slope thats angle is higher than our slope limit. This prevents jitter due to constant isGrounded to isNotGrounded when trying to walk up non walkable slopes.
//We basically just create a wall stopping us from going up the slope.
//Problem - This isnt perfect, for example it has some jitter issues and this methods success depends on how much we are moving (less = better).
//This doesnt work well on low angled slopes. It kinda does if you check if you were previously grounded and use that normal, but it was very jittery. Since slope limits are usualy not so low it shouldnt be an issue.
//Also, if we did use the previous groundNormal, we might have an issue if we were to jump and hit the non walkable slope and wanted to slide up, but the previous groundNormal might prevent that.
//I have also seen a issue where if we are between 2 high angled slopes that we cant walk on, this will create a wall on both sides as it should and stop us, but our depenetration method wont know what to do and fail.
void TryBlockAtSlopeLimit(List <SphereCollisionInfo> collisionPoints)
{
if (isGrounded && !isOnEdge)
{
for (int j = 0; j < collisionPoints.Count; j++)
{
SphereCollisionInfo collisionPoint = collisionPoints[j];
if (ExtVector3.IsInDirection(collisionPoint.normal, transform.up, tinyOffset, false) && !CanWalkOnSlope(collisionPoint.normal) && !collisionPoint.isOnEdge)
{
SweepInfo sweep = Geometry.SpherePositionBetween2Planes(collisionPoint.sphereRadius, collisionPoint.closestPointOnSurface, collisionPoint.normal, groundPoint, groundNormal, false);
if (sweep.hasHit)
{
Vector3 depenetrateDirection = Vector3.ProjectOnPlane(collisionPoint.normal, groundNormal).normalized;
//First we allign the intersectCenter with the detectionOrigin so that our depenetration method can use it properly.
sweep.intersectCenter = sweep.intersectCenter + Vector3.Project(collisionPoint.detectionOrigin - sweep.intersectCenter, groundNormal);
collisionPoints[j] = new SphereCollisionInfo(true, collisionPoint.collider, collisionPoint.detectionOrigin, collisionPoint.sphereRadius, sweep.intersectCenter - (depenetrateDirection * (collisionPoint.sphereRadius - smallOffset)), depenetrateDirection);
}
}
}
}
}
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);
}