Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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 stored as a center with half-dimensions
- // 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 List<Particle> UpdateVelocity(Particle particle, float timeStep)
- {
- List<Particle> targets = new();
- //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;
- targets.Add(particles[i]);
- //particle.velocity += CalculateAcceleration(particle, particles[i]) * timeStep;
- }
- return targets;
- }
- //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 += CalculateAcceleration(particle, avgParticle) * timeStep;
- targets.Add(avgParticle);
- return targets;
- }
- //Otherwise, check children if the node has been divided
- targets.AddRange(northWest.UpdateVelocity(particle, timeStep));
- targets.AddRange(northEast.UpdateVelocity(particle, timeStep));
- targets.AddRange(southWest.UpdateVelocity(particle, timeStep));
- targets.AddRange(southEast.UpdateVelocity(particle, timeStep));
- return targets;
- }
- Vector2 CalculateAcceleration(Particle p1, Particle p2)
- {
- Vector2 diff = p2.position - p1.position;
- Vector2 forceDir = diff.normalized;
- float sqrDst = Mathf.Max(diff.sqrMagnitude, 0.05f);
- Vector2 force = forceDir * (p1.mass * p2.mass) / sqrDst;
- Vector2 acceleration = force / p1.mass;
- return acceleration;
- }
- 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;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement