//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());
}
//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());
}
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);
}
bool CanWalkOnSlope(Vector3 normal)
{
if (normal == Vector3.zero)
{
return(false);
}
return(ExtVector3.Angle(normal, transform.up) < slopeLimit);
}
