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