Skip to content

Instantly share code, notes, and snippets.

@nilpunch

nilpunch/PhysicsUtils.cs

Last active August 1, 2025 12:02
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save nilpunch/27548a9227417839d2d085f7482a4343 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save nilpunch/27548a9227417839d2d085f7482a4343 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. nilpunch revised this gist Jul 30, 2025. 1 changed file with 18 additions and 13 deletions.
    31 changes: 18 additions & 13 deletions PhysicsUtils.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -1,3 +1,4 @@
    using System.Runtime.InteropServices;
    using UnityEngine;
    using Object = UnityEngine.Object;

    @@ -82,14 +83,17 @@ private static bool ClosestPointOnCollider(
    out float distance,
    float epsilon = 1e-5f)
    {
    const float multiplier = 0.5f;
    if (collider is not MeshCollider meshCollider || meshCollider.convex)
    {
    var direction = position - collider.ClosestPoint(position);
    distance = direction.magnitude;
    normal = direction / distance;
    return distance <= radius;
    }

    var probeSphere = ProbeSphere;

    var probeRadius = overestimate;
    var step = overestimate * multiplier;

    probeSphere.radius = probeRadius;
    probeSphere.radius = radius;

    if (!Physics.ComputePenetration(
    probeSphere, position, Quaternion.identity,
    @@ -100,31 +104,32 @@ private static bool ClosestPointOnCollider(
    return false;
    }

    distance = probeRadius - depth;
    distance = radius - depth;

    radius *= 0.5f;

    probeRadius -= step;
    step *= multiplier;
    var step = radius * 0.5f;

    while (step > epsilon)
    {
    probeSphere.radius = probeRadius;
    probeSphere.radius = radius;

    if (Physics.ComputePenetration(
    probeSphere, position, Quaternion.identity,
    collider, colliderPosition, colliderRotation,
    out var currentNormal, out depth))
    {
    distance = probeRadius - depth;
    distance = radius - depth;
    normal = currentNormal;

    probeRadius -= step;
    radius -= step;
    }
    else
    {
    probeRadius += step;
    radius += step;
    }

    step *= multiplier;
    step *= 0.5f;
    }

    return true;
  2. nilpunch created this gist Jul 24, 2025.
    142 changes: 142 additions & 0 deletions PhysicsUtils.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,142 @@
    using UnityEngine;
    using Object = UnityEngine.Object;

    public static class PhysicsUtils
    {
    private static readonly Collider[] _colliders = new Collider[512];

    private static SphereCollider _probeSphere;

    private static SphereCollider ProbeSphere
    {
    get
    {
    if (_probeSphere == null)
    {
    var go = new GameObject(nameof(ProbeSphere));
    go.hideFlags = HideFlags.HideInInspector | HideFlags.HideInHierarchy;
    Object.DontDestroyOnLoad(go);
    _probeSphere = go.AddComponent<SphereCollider>();
    _probeSphere.enabled = false;
    }

    return _probeSphere;
    }
    }

    public static bool ClosestPoint(Vector3 position, float checkRadius, out RaycastHit hit, int layerMask = ~0, float epsilon = 1e-5f)
    {
    hit = default;

    var overlaps = Physics.OverlapSphereNonAlloc(position, checkRadius + 0.001f, _colliders, layerMask, QueryTriggerInteraction.Ignore);
    if (overlaps == 0)
    {
    return false;
    }

    var minDistance = float.MaxValue;
    var closestNormal = Vector3.zero;
    Collider closestCollider = null;

    ProbeSphere.enabled = true;

    for (var i = 0; i < overlaps; i++)
    {
    var collider = _colliders[i];

    if (ClosestPointOnCollider(checkRadius, position, collider,
    collider.transform.position, collider.transform.rotation,
    out var normal, out var distance, epsilon))
    {
    if (distance < minDistance)
    {
    minDistance = distance;
    closestNormal = normal;
    closestCollider = collider;
    }
    }
    }

    ProbeSphere.enabled = false;

    if (minDistance.Equals(float.MaxValue))
    {
    return false;
    }

    hit.distance = minDistance;
    hit.point = position - closestNormal * minDistance;
    hit.normal = closestNormal;
    SetHitCollider(ref hit, closestCollider);

    return true;
    }

    private static bool ClosestPointOnCollider(
    float overestimate,
    Vector3 position,
    Collider collider,
    Vector3 colliderPosition,
    Quaternion colliderRotation,
    out Vector3 normal,
    out float distance,
    float epsilon = 1e-5f)
    {
    const float multiplier = 0.5f;

    var probeSphere = ProbeSphere;

    var probeRadius = overestimate;
    var step = overestimate * multiplier;

    probeSphere.radius = probeRadius;

    if (!Physics.ComputePenetration(
    probeSphere, position, Quaternion.identity,
    collider, colliderPosition, colliderRotation,
    out normal, out var depth))
    {
    distance = default;
    return false;
    }

    distance = probeRadius - depth;

    probeRadius -= step;
    step *= multiplier;

    while (step > epsilon)
    {
    probeSphere.radius = probeRadius;

    if (Physics.ComputePenetration(
    probeSphere, position, Quaternion.identity,
    collider, colliderPosition, colliderRotation,
    out var currentNormal, out depth))
    {
    distance = probeRadius - depth;
    normal = currentNormal;

    probeRadius -= step;
    }
    else
    {
    probeRadius += step;
    }

    step *= multiplier;
    }

    return true;
    }

    private static readonly int _sizeOfHit = Marshal.SizeOf<RaycastHit>();
    private static readonly int _colliderFieldOffset = (int)Marshal.OffsetOf<RaycastHit>("m_Collider");

    private static void SetHitCollider(ref RaycastHit hit, Collider collider)
    {
    var colliderId = collider.GetInstanceID();
    var bytes = MemoryMarshal.AsBytes(MemoryMarshal.CreateSpan(ref hit, _sizeOfHit));
    MemoryMarshal.Write(bytes.Slice(_colliderFieldOffset, sizeof(int)), ref colliderId);
    }
    }