Recursive quadtree

using System.Collections.Generic;
using UnityEngine;
using Vector2 = UnityEngine.Vector2;
using Particle = Simulation.Particle;
public class QuadTree
{
    // Arbitrary constant to indicate how many elements can be stored in this quad
    public static int Capacity;
    //How many times tree can split
    public static float MaxDepth;
    public static float OriginalSize;
    //Handles edge cases
    public static float OverlapTolerance;
    //How much the algorithm should group together particles
    public static float ApproximationThreshold;

    //The average center of mass and mass in a node
    Vector2 centerOfMass;
    float mass;

    // Axis-aligned bounding box to represent the
    //boundaries of this quad tree
    AABB boundary;

    //Objects in this quad tree node
    List<Particle> particles;
    int particleCount;

    // Children
    QuadTree northWest;
    QuadTree northEast;
    QuadTree southWest;
    QuadTree southEast;

    Particle avgParticle;

    //True if the quad subdivides
    bool divided;

    public QuadTree(AABB boundary)
    {
        this.boundary = boundary;
        avgParticle = new();
        particles = new();
    }

    public void DrawTree()
    {
        DrawSquare(boundary.position, boundary.size);
        Debug.DrawLine(centerOfMass, centerOfMass + new Vector2(0.1f, 0), Color.blue);

        if (divided)
        {
            northWest.DrawTree();
            northEast.DrawTree();
            southWest.DrawTree();
            southEast.DrawTree();
        }
    }
    static void DrawSquare(Vector2 position, float size)
    {
        Debug.DrawLine(position, new Vector2(position.x, position.y + size), Color.red);
        Debug.DrawLine(new Vector2(position.x, position.y + size), new Vector2(position.x + size, position.y + size), Color.red);
        Debug.DrawLine(new Vector2(position.x + size, position.y + size), new Vector2(position.x + size, position.y), Color.red);
        Debug.DrawLine(new Vector2(position.x + size, position.y), new Vector2(position.x, position.y), Color.red);
    }

    //TODO: Update velocity in Simulation instead
    public Particle UpdateVelocity(Particle particle, float timeStep)
    {
        //If the node hasn't been divided, calculate the force exerted by all particles in this node
        if (!divided)
        {
            for (int i = 0; i < particleCount; i++)
            {
                if(particles[i].position == particle.position) continue;
                particle.velocity += Simulation.CalculateAcceleration(particle, particles[i]) * timeStep;
            }

            return particle;
        }
        //Otherwise, calculate the ratio s/d. If node is sufficiently far away, treat it as a single particle
        float ratio = boundary.size / (particle.position - centerOfMass).magnitude;
        if (ratio < ApproximationThreshold)
        {
            particle.velocity += Simulation.CalculateAcceleration(particle, avgParticle) * timeStep;
            return particle;
        }
        //Otherwise, check children if the node has been divided
        particle = northWest.UpdateVelocity(particle, timeStep);
        particle = northEast.UpdateVelocity(particle, timeStep);
        particle = southWest.UpdateVelocity(particle, timeStep);
        particle = southEast.UpdateVelocity(particle, timeStep);
        return particle;
    }

    public void CalculateMassAndCenterOfMass()
    {
        for (int i = 0; i < particleCount; i++)
        {
            mass += particles[i].mass;
        }

        float x = 0;
        float y = 0;
        for (int i = 0; i < particleCount; i++)
        {
            x += particles[i].position.x * particles[i].mass;
            y += particles[i].position.y * particles[i].mass;
        }

        x /= mass;
        y /= mass;
        centerOfMass = new Vector2(x, y);

        if (divided)
        {
            northWest.CalculateMassAndCenterOfMass();
            northEast.CalculateMassAndCenterOfMass();
            southWest.CalculateMassAndCenterOfMass();
            southEast.CalculateMassAndCenterOfMass();
        }

        avgParticle.position = centerOfMass;
        avgParticle.mass = mass;
    }

    public void Insert(Particle particle)
    {
        //If the quad has already divided, insert it into on of the children
        if (divided)
        {
            InsertIntoChildren(particle);
        }
        else
        {
            // Ignore objects that do not belong in this quad tree
            if (!boundary.Intersects(particle, OverlapTolerance)) return;

            // If there is space in this quad tree and if doesn't have subdivisions, add the object here
            if (particleCount < Capacity)
            {
                particles.Add(particle);
                particleCount++;
            }
            // Otherwise, subdivide and then add the point to whichever node will accept it
            else
            {
                Subdivide(particle);
            }

        }
    }

    void Subdivide(Particle particle)
    {
        float size = boundary.size / 2;
        //If the node is big enough
        if (OriginalSize / size < MaxDepth * 4)
        {
            //Create 4 children
            northWest = new(new AABB
            {
                position = new Vector2(boundary.position.x, boundary.position.y + boundary.size / 2f),
                size = size
            });
            northEast = new(new AABB
            {
                position = new Vector2(boundary.position.x + boundary.size / 2, boundary.position.y + boundary.size / 2f),
                size = size
            });
            southWest = new(new AABB
            {
                position = new Vector2(boundary.position.x, boundary.position.y),
                size = size
            });
            southEast = new(new AABB
            {
                position = new Vector2(boundary.position.x + boundary.size / 2, boundary.position.y),
                size = size
            });

            //Move all objects from this quad into children
            InsertIntoChildren(particle);
            for (int i = 0; i < particleCount; i++)
            {
                InsertIntoChildren(particles[i]);
            }

            divided = true;
        }
    }

    void InsertIntoChildren(Particle particle)
    {
        northWest.Insert(particle);
        northEast.Insert(particle);
        southWest.Insert(particle);
        southEast.Insert(particle);
    }
}

public struct AABB
{
    //Bottom left of rectangle
    public Vector2 position;
    public float size;

    public bool Intersects(Particle p, float margin)
    {
        return p.position.x > position.x - margin && p.position.x < position.x + size + margin &&
               p.position.y > position.y - margin && p.position.y < position.y + size + margin;
    }
}