Advertisement
cajphrase

EntityCollider

Jul 31st, 2024
36
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 7.24 KB | None | 0 0
  1. public struct EntityCollider
  2. {
  3.     public enum Type
  4.     {
  5.         box,
  6.         capsule,
  7.         sphere,
  8.     }
  9.     public Type type;
  10.     public float3 size; //use xy to store radius / height for capsule
  11.     public Pose localTransform;
  12.  
  13.     public EntityCollider(BoxCollider b, EntityTransform localTransform)
  14.     {
  15.         type = EntityCollider.Type.box,
  16.         size = b.size * localTransform.scale,
  17.         this.localTransform = (Pose)localTransform,
  18.     }
  19.  
  20.     public EntityCollider(CapsuleCollider c, EntityTransform localTransform)
  21.     {
  22.         type = EntityCollider.Type.capsule,
  23.         size = new float3(c.radius, c.height, 0) * localTransform.scale,
  24.         this.localTransform = (Pose)localTransform,
  25.     }
  26.  
  27.     public EntityCollider(SphereCollider s, EntityTransform localTransform)
  28.     {
  29.         type = EntityCollider.Type.sphere,
  30.         size = new float3(s.radius, 0, 0) * localTransform.scale,
  31.         this.localTransform = (Pose)localTransform,
  32.     }
  33. }
  34.  
  35. public static class EntityCollisionUtility
  36. {
  37.     public class Loader : ILoader
  38.     {
  39.         public bool IsComplete => true;
  40.         public int progress => 1;
  41.         public int totalCount => 1;
  42.         public void Load()
  43.         {
  44.             EntityCollisionUtility.Initialize();
  45.         }
  46.     }
  47.  
  48.     public static NativeParallelMultiHashMap<PrefabReference, EntityCollider> entityColliders;
  49.  
  50.     public static void Initialize()
  51.     {
  52.         entityColliders = new(PrefabList.prefabs.Count, Allocator.Persistent);
  53.         Lifecycle.onShutDown += Dispose;
  54.  
  55.         foreach (var prefab in PrefabList.prefabs)
  56.         {
  57.             var rootTx = new EntityTransform(prefab.transform);
  58.             var colliders = prefab.GetComponentsInChildren<Collider>();
  59.             foreach (var collider in colliders)
  60.             {
  61.                 if (collider.IsTrigger)
  62.                     continue;
  63.  
  64.                 var localTx = rootTx / new EntityTransform(collider.transform);
  65.                 switch (collider)
  66.                 {
  67.                     case BoxCollider b:
  68.                         entityColliders.Add(prefab, new(b));
  69.                         break;
  70.                     case CapsuleCollider c:
  71.                         entityColliders.Add(prefab, new(c));
  72.                         break;
  73.                     case SphereCollider s:
  74.                         entityColliders.Add(prefab, new(s));
  75.                         break;
  76.                     default: // fail silently on mesh colliders, not supported
  77.                         break;
  78.                 }
  79.             }
  80.         }
  81.     }
  82.  
  83.     public static void Dispose()
  84.     {
  85.         entityColliders.Dispose();
  86.     }
  87.  
  88.     public static bool CalculateCollision(EntityCollider a, EntityTransform aTransform, EntityCollider b, EntityTransform bTransform, out float penetrationDepth, out float3 penetrationNormal)
  89.     {
  90.         switch (a.type)
  91.         {
  92.             case EntityCollider.Type.box:
  93.                 var aBox = a.AsBox(aTransform);
  94.                 switch (b.type)
  95.                 {
  96.                     case EntityCollider.Type.box:
  97.                     break;
  98.                     case EntityCollider.Type.capsule:
  99.                         return CollisionUtils.CapsuleBoxIntersection(b.AsCapsule(bTransform), aBox, out penetrationDepth, out penetrationNormal);
  100.                     case EntityCollider.Type.sphere:
  101.                         return CollisionUtils.SphereBoxIntersection(b.AsSphere(bTransform), aBox, out penetrationDepth, out penetrationNormal);
  102.                 }
  103.                 break;
  104.             case EntityCollider.Type.capsule:
  105.                 switch (b.type)
  106.                 {
  107.                     case EntityCollider.Type.box:
  108.                     break;
  109.                     case EntityCollider.Type.capsule:
  110.                     break:
  111.                     case EntityCollider.Type.sphere:
  112.                     break:
  113.                 }
  114.                 break;
  115.             case EntityCollider.Type.sphere:
  116.                 switch (b.type)
  117.                 {
  118.                     case EntityCollider.Type.box:
  119.                     break;
  120.                     case EntityCollider.Type.capsule:
  121.                     break:
  122.                     case EntityCollider.Type.sphere:
  123.                     break:
  124.                 }
  125.                 break;
  126.         }
  127.     }
  128. }
  129.  
  130.     public static bool SphereBoxIntersection(float3 sphereCenter, float radius, box box, out float3 penetrationNormal, out float penetrationDepth)
  131.     {
  132.         penetrationNormal = default;
  133.         penetrationDepth = default;
  134.         var localCenter = sphereCenter / box.localTransform;
  135.         var aabb = new AABB(0, box.halfExtents);
  136.         var cp = ClosestPointOnOrInAABB(localCenter, aabb);
  137.         var dist = mafs.distance(localCenter, cp);
  138.         if (dist > radius)
  139.             return false;
  140.         if (cp == localCenter) // center of sphere is inside the box
  141.         {
  142.             var edgePt = ClosestPointOnAABB(localCenter, aabb);
  143.             penetrationNormal = mafs.normalize(localCenter - edgePt);
  144.             penetrationDepth = mafs.distance(localCenter, edgePt) + radius;
  145.         }
  146.         else // center of sphere is outside the box
  147.         {
  148.             penetrationNormal = mafs.normalize(cp - localCenter);
  149.             penetrationDepth = dist;
  150.         }
  151.         return true;
  152.     }
  153.        
  154.         public static bool CapsuleBoxIntersection(capsule cap, box box, out float3 penetrationNormal, out float penetrationDepth)
  155.         {
  156.             penetrationNormal = default;
  157.             penetrationDepth = default;
  158.  
  159.             // transform the capsule into box space so the box becomes an AABB
  160.             var localCapTx = box.transform / cap.transform;
  161.             AABB aabb = new AABB(0, box.halfExtents);
  162.             var localCap = new capsule(localCapTx, cap.height, cap.radius);
  163.  
  164.             // Expand the AABB by the radius. This will give false positives on the corners which we will find later.
  165.             var expandedAABB = new AABB(0, box.halfExtents + cap.radius);
  166.  
  167.             // Check if the ray intersects the box
  168.             var rayHit = RayBoxIntersection(
  169.                 in localCap.baseCenter,
  170.                 in localCap.normal * localCap.height,
  171.                 in expandedAABB.halfExtents,
  172.                 out float2 hits,
  173.                 out float3 normal);
  174.  
  175.             // return early if there is no chance of intersection
  176.             if (!rayHit)
  177.                 return false;
  178.  
  179.             hits = mafs.clamp01(hits);
  180.             var pointA = localCap.baseCenter + localCap.normal * hits.x;
  181.             var cpA = ClosestPointOnOrInAABB(pointA, aabb);
  182.             var distA = mafs.distance(pointA, cpA);
  183.             if (distA < cap.radius)
  184.             {
  185.                 penetrationDepth = distA;
  186.                 penetrationNormal = normal;
  187.                 return true;
  188.             }
  189.             var pointB = localCap.baseCenter + localCap.normal * hits.y;
  190.             var cpB = ClosestPointOnOrInAABB(pointB, aabb);
  191.             var distB = mafs.distance(pointA, cpA);
  192.             if (distB < cap.radius)
  193.             {
  194.                 penetrationDepth = distB;
  195.                 penetrationNormal = normal;
  196.                 return true;
  197.             }
  198.  
  199.             return false;
  200.         }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement