Vector3 GetInterpolatedNormal()
{
isOnEdge = false;
Vector3 interpolatedNormal = detectionOrigin - closestPointOnSurface;
//If the detection origin and closestpoint are right on eachother, we return the normal. This is important for when using a capsule.
if (interpolatedNormal.sqrMagnitude < .001f)
{
return(normal);
}
if (!ExtVector3.IsInDirection(interpolatedNormal, normal))
{
interpolatedNormal = -interpolatedNormal;
}
interpolatedNormal.Normalize();
//We check for an angle greater than 3 as a safety since our closestPointOnSurface might have been detected slightly inaccurately,
//which might lead to sliding when depenetrating (such as on the floor when standing still)
//This is a custom Vector3.Angle method that assumes the vectors are already normalized for performance reasons.
if (ExtVector3.Angle(interpolatedNormal, normal) > 3f)
{
isOnEdge = true;
return(interpolatedNormal);
}
return(normal);
}
public static float GetCollisionMagnitude(Vector3 collisionVelocity, Vector3 normal, float radius)
{
float mag = (ExtVector3.IsInDirection(collisionVelocity, normal)) ? -collisionVelocity.magnitude : collisionVelocity.magnitude;
mag = Mathf.Clamp(mag, -radius, radius); //We clamp by radius in case the original detection radius was larger than this radius
return(radius + mag);
}
public float GetCollisionMagnitudeSqr()
{
Vector3 collisionVelocity = GetCollisionVelocity(closestPointOnSurface, detectionOrigin);
float mag = (ExtVector3.IsInDirection(collisionVelocity, normal)) ? -collisionVelocity.sqrMagnitude : collisionVelocity.sqrMagnitude;
return(sphereRadius.Squared() + mag);
}
//We can avoid using .magnitude as long as collisionVelocity is the same direction as the normal (and the normal is already normalized)
public static float GetCollisionMagnitudeInDirection(Vector3 collisionVelocity, Vector3 normal, float radius)
{
float mag = -ExtVector3.MagnitudeInDirection(collisionVelocity, normal, false);
mag = Mathf.Clamp(mag, -radius, radius);
return(radius + mag);
}
//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());
}
bool CanWalkOnSlope(Vector3 normal)
{
if (normal == Vector3.zero)
{
return(false);
}
return(ExtVector3.Angle(normal, transform.up) < slopeLimit);
}
public void AddExplosionForce(float force, Vector3 position, float explosionRadius, ForceMode forceMode = ForceMode.Force)
{
float distance = Vector3.Distance(position, transform.position);
if (distance < explosionRadius)
{
AddForce(ExtVector3.Direction(position, transform.position) * (force * (distance / explosionRadius)), forceMode);
}
}
protected Vector3 CheckAndApplyFriction(Vector3 velocity, float deltaTime)
{
//If our velocity is going upwards, such as a jump, we dont want to apply friction.
if (!ExtVector3.IsInDirection(velocity, transform.up, tinyOffset, false))
{
return(ApplyFriction(velocity, friction, mass, deltaTime));
}
return(velocity);
}
void OnDrawGizmos()
{
if (!Application.isPlaying)
{
return;
}
//Keep in mind that this movement consistency test will not work if you are using FixedUpdate since OnDrawGizmos runs outside FixedUpdate.
if (infoDebug.drawMovementConsistency || infoDebug.printMovementConsistency)
{
float movement = Vector3.Distance(previousPosition, transform.position) / Time.deltaTime;
Vector3 pos = transform.position;
//We set overrideYMovementConsistency in a way so that it handles any player rotation
//This will automatically set itself the first time it runs or if our transform.up has changed.
if (infoDebug.setOverrideYToCurrentY || (!infoDebug.setOverrideYToCurrentY && overrideYMovementConsistency == 0) || transform.up != previousUp)
{
overrideYMovementConsistency = ExtVector3.MagnitudeInDirection(transform.position, transform.up, false);
}
if (overrideYMovementConsistency != 0)
{
pos -= (transform.up * ExtVector3.MagnitudeInDirection(transform.position, transform.up, false));
pos += (transform.up * overrideYMovementConsistency);
}
if (infoDebug.printMovementConsistency)
{
Debug.Log(movement);
}
if (infoDebug.drawMovementConsistency)
{
Debug.DrawRay(pos, transform.up * (1f + movement), Color.red, infoDebug.drawMovementgDuration);
Debug.DrawRay(transform.position, transform.right, Color.red, infoDebug.drawMovementgDuration);
}
}
if (infoDebug.drawGrounding && capsuleCollider != null)
{
Vector3 bottomSphere = GetCapsulePoint(transform.position, -transform.up);
if (isGrounded)
{
Debug.DrawRay(bottomSphere, groundNormal, Color.red, infoDebug.drawGroundingDuration);
}
else
{
Debug.DrawRay(bottomSphere, transform.up, Color.blue, infoDebug.drawGroundingDuration);
}
}
previousPosition = transform.position;
previousUp = transform.up;
}
//Assumes the point is already on the line somewhere
public static Vector3 ClampToSegment(Vector3 point, Vector3 linePoint1, Vector3 linePoint2)
{
Vector3 lineDirection = linePoint2 - linePoint1;
if (!ExtVector3.IsInDirection(point - linePoint1, lineDirection))
{
point = linePoint1;
}
else if (ExtVector3.IsInDirection(point - linePoint2, lineDirection))
{
point = linePoint2;
}
return(point);
}
//Ive seen bugginess when the capsule shape is actually a sphere shape (or at least when segment0 and segment1 are equal),
//it would collide when not really near something, so we try to detect that and make it a capsule by offsetting it by a tiny amount.
static Vector3 MakeRealCapsule(Vector3 segment0, Vector3 segment1, float radius)
{
float radiusDoubled = radius * 2f;
if ((segment1 - segment0).sqrMagnitude <= radiusDoubled.Squared())
{
Vector3 direction = ExtVector3.Direction(segment0, segment1);
if (direction == Vector3.zero)
{
direction = Vector3.up;
}
segment1 += (direction * .0001f);
}
return(segment1);
}
public static AABB CreateCapsuleAABB(Vector3 segment0, Vector3 segment1, float radius)
{
for (int i = 0; i < 3; i++)
{
if (segment0[i] < segment1[i])
{
segment0[i] -= radius;
segment1[i] += radius;
}
else
{
segment0[i] += radius;
segment1[i] -= radius;
}
}
return(new AABB((segment0 + segment1) * .5f, ExtVector3.Abs(segment1 - segment0) * .5f)); //It seems the extents must be positives for our overlap tests to work...
}
public bool CeilingDetected(Vector3 topSphere, Vector3 bottomSphere, float radius, LayerMask layerMask, Vector3 ignoreHitPoint, Vector3 ignoreHitNormal)
{
SphereCollisionDetect.DetectCollisions(topSphere, bottomSphere, radius, layerMask, ignoreColliders, ceilingContactsBuffer);
if (ceilingContactsBuffer.Count > 0)
{
for (int i = 0; i < ceilingContactsBuffer.Count; i++)
{
SphereCollisionInfo collisionPoint = ceilingContactsBuffer[i];
//If the detected point is not behind the ignoreHitPoints plane and above the ignoreHitPoint in our transform.up direction, we assume we detected a ceiling.
if (ExtVector3.IsInDirection(collisionPoint.closestPointOnSurface - ignoreHitPoint, ignoreHitNormal, smallOffset, false) && ExtVector3.IsInDirection(collisionPoint.closestPointOnSurface - ignoreHitPoint, transform.up, smallOffset, false))
{
return(true);
}
}
}
return(false);
}
public static Vector3 Depenetrate(List <SphereCollisionInfo> collisionPoints, int maxIterations = 1)
{
if (collisionPoints.Count > 0 && maxIterations > 0)
{
Vector3 depenetrationVelocity = Vector3.zero;
Vector3 totalDepenetrationVelocity = Vector3.zero;
//Since with each iteration we are using old collision data, higher maxIterations does not mean more accuracy. You will need to tune it to your liking.
for (int i = 0; i < maxIterations; i++)
{
for (int j = 0; j < collisionPoints.Count; j++)
{
SphereCollisionInfo cp = collisionPoints[j];
Vector3 detectOriginOffset = totalDepenetrationVelocity + depenetrationVelocity + cp.detectionOrigin;
//We check if we are already depenetrated.
if (ExtVector3.MagnitudeInDirection(detectOriginOffset - cp.closestPointOnSurface, cp.interpolatedNormal, false) > cp.sphereRadius)
{
continue;
}
//We take into account how much we already depenetrated.
Vector3 collisionVelocityOffset = Vector3.Project(detectOriginOffset - cp.closestPointOnSurface, cp.GetCollisionVelocity());
float collisionMagnitude = SphereCollisionInfo.GetCollisionMagnitudeInDirection(collisionVelocityOffset, cp.interpolatedNormal, cp.sphereRadius) + .0001f;
depenetrationVelocity += SphereCollisionInfo.GetDepenetrationVelocity(cp.interpolatedNormal, collisionMagnitude);
}
if (depenetrationVelocity == Vector3.zero)
{
break;
}
totalDepenetrationVelocity += depenetrationVelocity;
depenetrationVelocity = Vector3.zero;
}
return(totalDepenetrationVelocity);
}
return(Vector3.zero);
}
public ContactInfo ClosestPointOnSurface(Vector3 point, float radius)
{
trianglesBufferSingleSphere.Clear();
Vector3 localPoint = transform.InverseTransformPoint(point);
radius /= ExtVector3.Minimum(ExtVector3.Abs(transform.lossyScale));
AABB aabb = AABB.CreateSphereAABB(localPoint, radius + .001f);
tree.FindClosestTriangles(tree.rootNode, aabb, trianglesBufferSingleSphere, infoDebug);
Vector3 shortestPoint = Vector3.zero;
Vector3 p1, p2, p3, nearest;
float radiusSqrd = radius * radius;
float shortestDistance = float.MaxValue;
int shortestTriangleIndex = -1;
HashSet <int> .Enumerator enumerator = trianglesBufferSingleSphere.GetEnumerator();
while (enumerator.MoveNext())
{
tree.GetTriangleVertices(enumerator.Current, out p1, out p2, out p3);
nearest = Geometry.ClosestPointOnTriangleToPoint(p1, p2, p3, localPoint);
float distance = (localPoint - nearest).sqrMagnitude;
if (PointIsBetter(distance, shortestDistance, localPoint, shortestPoint, localPoint, nearest, shortestTriangleIndex, enumerator.Current, radiusSqrd))
{
shortestDistance = distance;
shortestPoint = nearest;
shortestTriangleIndex = enumerator.Current;
}
}
if (shortestPoint == Vector3.zero)
{
return(new ContactInfo());
}
return(new ContactInfo(transform.TransformPoint(shortestPoint), transform.TransformDirection(tree.GetTriangleNormal(shortestTriangleIndex))));
}
//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);
}
}
}
}
}
public List <ContactInfo> ClosestPointsOnSurface(Vector3 point, float radius, List <ContactInfo> resultsBuffer)
{
resultsBuffer.Clear();
trianglesBufferMultiSphere.Clear();
closestPointsBufferMultiSphere.Clear();
Vector3 localPoint = transform.InverseTransformPoint(point);
radius /= ExtVector3.Minimum(ExtVector3.Abs(transform.lossyScale));
AABB aabb = AABB.CreateSphereAABB(localPoint, radius + .001f);
tree.FindClosestTriangles(tree.rootNode, aabb, trianglesBufferMultiSphere, infoDebug);
Vector3 p1, p2, p3, nearest;
float radiusSqrd = radius * radius;
float distance = 0;
HashSet <int> .Enumerator enumerator = trianglesBufferMultiSphere.GetEnumerator();
while (enumerator.MoveNext())
{
tree.GetTriangleVertices(enumerator.Current, out p1, out p2, out p3);
nearest = Geometry.ClosestPointOnTriangleToPoint(p1, p2, p3, localPoint);
distance = (nearest - localPoint).sqrMagnitude;
if (distance <= radiusSqrd)
{
closestPointsBufferMultiSphere.Add(new ClosestTrianglePoint(nearest, distance, enumerator.Current, localPoint, this));
}
}
CleanUp(closestPointsBufferMultiSphere, resultsBuffer);
return(resultsBuffer);
}
public static CapsuleShape ToLocalOfUniformScale(CapsuleShape capsuleShape, Transform transform)
{
Vector3 localSegment0 = transform.InverseTransformPoint(capsuleShape.top);
Vector3 localSegment1 = transform.InverseTransformPoint(capsuleShape.bottom);
float distance = Vector3.Distance(localSegment0, localSegment1);
float difference = capsuleShape.pointsDistance / distance;
//If distance is zero, then the capsule is probably like a sphere.
//Not sure if we should just always do it the way that can handle zero.
if (Mathf.Approximately(distance, 0f) || Mathf.Approximately(difference, 0f))
{
float minimumScale = ExtVector3.Minimum(ExtVector3.Abs(transform.lossyScale));
float height = capsuleShape.height / minimumScale;
float radius = capsuleShape.radius / minimumScale;
return(new CapsuleShape((localSegment0 + localSegment1) * .5f, localSegment1 - localSegment0, height, radius));
}
else
{
return(new CapsuleShape((localSegment0 + localSegment1) * .5f, localSegment1 - localSegment0, capsuleShape.height / difference, capsuleShape.radius / difference));
}
}
public Vector3 HalfExtent()
{
return(ExtVector3.Abs((maxExtent - minExtent) * .5f));
}
public static void GetVelocityAxis(Vector3 velocity, Vector3 transformUp, out Vector3 horizontal, out Vector3 vertical)
{
vertical = transformUp * ExtVector3.MagnitudeInDirection(velocity, transformUp);
horizontal = velocity - vertical;
}
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);
}
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);
}
//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);
}
bool GoingOverEdge(Vector3 targetVelocity)
{
//There is a issue with when we are on a edge and not a slope. If we walk and jump towards a box and our feet hit the edge, we will flatten our velocity and go straight and not upwards. It feels a bit weird so this fixes it.
return(isGrounded && isOnEdge && ExtVector3.IsInDirection(targetVelocity, transform.up, tinyOffset, false));
}
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);
}
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>();