public static bool TestCollision(Circle *a, Vector2Int tilePos)
{
return(TestCircleAABB(
a->pos, a->radius, //Circle1
tilePos.ToLVector2() + LVector2.half, new LFloat(true, 707), LVector2.half //AABB2
));
}
//Circle* & Circle
public static bool TestCollision(Circle *a, Circle *b)
{
var diff = a->pos - b->pos;
var allRadius = a->radius + b->radius;
return(diff.sqrMagnitude <= allRadius * allRadius);
}
public void AddBody(Circle *body)
{
if (body == null)
{
return;
}
if (_childA != null)
{
_bodyCount++;
var child = GetQuadrant(body->pos);
child->AddBody(body);
}
else
{
if (_bodies == null)
{
_bodies = QuadTreeFactory.AllocPtrBlock(_maxBodiesPerNode);
}
CheckDebugInfo($"AddBody{body->Id}");
fixed(QuadTree *thisPtr = &this)
{
body->ParentNode = thisPtr;
body->_debugId = this._debugId;
}
//CheckDebugInfo(body);
_bodies[_bodyCount++] = body;
if (_bodyCount >= _maxBodiesPerNode && _curLevel < _maxLevel && _bounds.width > 2)
{
CheckDebugInfo($"Split{body->Id}");
Split();
}
}
}
void CheckDebugInfo(string info, Circle *body)
{
if (body->_debugId == DebugID)
{
int i = 0;
Debug.Log(info);
}
}
public void NotifyCollisionEvent(Circle *shape1, Circle *shape2, ref CollisionResult result)
{
var body1 = FindCollisionBody(shape1->Id);
var body2 = FindCollisionBody(shape2->Id);
body2.OnCollision(result, body1);
result.Normal *= -LFloat.one;
result.First = true;
body1.OnCollision(result, body2);
}
private static bool TestCollisionShapes(Circle *a, Circle *b, ref CollisionResult result)
{
//var hasCollide = a->TestCollision(b);
var aabbA = (AABB2D *)a;
var aabbB = (AABB2D *)b;
result.Collides = CollisionTest.TestAABBAABB(
aabbA->pos, aabbA->radius, aabbA->size,
aabbB->pos, aabbB->radius, aabbB->size
);
return(result.Collides);
}
public static void RemoveNode(Circle *body)
{
if (body == null)
{
return;
}
if (body->ParentNode == null)
{
NativeHelper.NullPointer();
}
body->ParentNode->RemoveBody(body);
}
private bool FindCollisionPair(Circle *a, Circle *b, bool remove = true)
{
var idx = a->Id * (MaxCollisionBodies + 1) + b->Id;
if (remove)
{
return(_pairs.Remove(idx));
}
else
{
return(_pairs.Contains(idx));
}
}
public static unsafe void DrawGizmos(EShape2D shapeType, Circle *shape, Color color)
{
switch (shapeType)
{
case EShape2D.Circle:
DrawCircle(*(Circle *)shape, color);
break;
case EShape2D.AABB:
DrawAABB(*(AABB2D *)shape, color);
break;
case EShape2D.OBB:
DrawOBB(*(OBB2D *)shape, color);
break;
}
}
/// <summary>
/// Executes collision between two bodies
/// </summary>
protected bool Test(Circle *body1, Circle *body2, bool removePair = true)
{
var result = new CollisionResult();
var paired = FindCollisionPair(body1, body2, removePair);
if (TestCollisionShapes(body1, body2, ref result))
{
result.Type = paired ? ECollisionEvent.Stay : ECollisionEvent.Enter;
CacheCollisionPair(body1, body2);
NotifyCollisionEvent(body1, body2, ref result);
return(true);
}
else
{
if (paired)
{
result.Type = ECollisionEvent.Exit;
NotifyCollisionEvent(body1, body2, ref result);
}
}
return(false);
}
void OnShapePtr(Circle *body1)
{
countDetectBodyVsBody++;
Profiler.BeginSample("GetBodies");
var bodies = _quadTree->GetBodies(new LRect(body1->pos, ((AABB2D *)body1)->size));
Profiler.EndSample();
Profiler.BeginSample("Test");
var count = bodies.Count;
for (int i = 0; i < count; i++)
{
var body2 = (Circle *)bodies[i];
if (body2 == null || body1 == body2)
{
continue;
}
Test(body1, body2);
}
Profiler.EndSample();
}
public void RemoveBody(Circle *body)
{
int i = 0;
for (; i < _bodyCount; i++)
{
if (_bodies[i] == body)
{
//swap last one
_bodies[i] = _bodies[_bodyCount - 1];
break;
}
}
if (i == _bodyCount)
{
int val = 0;
throw new Exception($"RemoveBody error : QuadTree have no that node _bodyCount{_bodyCount} " +
$"remove{body->Id} body->_debugId{body->_debugId} tree._debugId{_debugId}");
}
CheckDebugInfo($"remove{body->Id}", body);
body->ParentNode = null;
body->_debugId = 0;
--_bodyCount;
var parent = _parent;
while (parent != null)
{
--(parent->_bodyCount);
parent = parent->_parent;
}
if (_parent != null && _parent->HasSplit && _parent->_bodyCount < _maxBodiesPerNode)
{
_parent->Collapse();
}
}
private void CacheCollisionPair(Circle *a, Circle *b)
{
var idx = a->Id * (MaxCollisionBodies + 1) + b->Id;
_pairCache.Add(idx);
}
public static bool TestCollision(Circle *a, OBB2D *b)
{
return(TestCircleOBB(a->pos, a->radius
, b->pos, b->radius, b->size, b->up
));
}
public static void Free(Circle *ptr)
{
_spherePool.Return(ptr);
}
