Advertisement
JontePonte

slow tree gravity

Nov 10th, 2024
55
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 7.51 KB | None | 0 0
  1. using System.Collections.Generic;
  2. using UnityEngine;
  3. using Vector2 = UnityEngine.Vector2;
  4. using Particle = Simulation.Particle;
  5. public class QuadTree
  6. {
  7.     // Arbitrary constant to indicate how many elements can be stored in this quad
  8.     public static int Capacity;
  9.     //How many times tree can split
  10.     public static float MaxDepth;
  11.     public static float OriginalSize;
  12.     //Handles edge cases
  13.     public static float OverlapTolerance;
  14.     //How much the algorithm should group together particles
  15.     public static float ApproximationThreshold;
  16.    
  17.     //The average center of mass and mass in a node
  18.     Vector2 centerOfMass;
  19.     float mass;
  20.    
  21.     // Axis-aligned bounding box stored as a center with half-dimensions
  22.     // to represent the boundaries of this quad tree
  23.     AABB boundary;
  24.    
  25.     //Objects in this quad tree node
  26.     List<Particle> particles;
  27.     int particleCount;
  28.    
  29.     // Children
  30.     QuadTree northWest;
  31.     QuadTree northEast;
  32.     QuadTree southWest;
  33.     QuadTree southEast;
  34.  
  35.     Particle avgParticle;
  36.    
  37.     //True if the quad subdivides
  38.     bool divided;
  39.  
  40.     public QuadTree(AABB boundary)
  41.     {
  42.         this.boundary = boundary;
  43.         avgParticle = new();
  44.         particles = new();
  45.     }
  46.  
  47.     public void DrawTree()
  48.     {
  49.         DrawSquare(boundary.position, boundary.size);
  50.         Debug.DrawLine(centerOfMass, centerOfMass + new Vector2(0.1f, 0), Color.blue);
  51.        
  52.         if (divided)
  53.         {
  54.             northWest.DrawTree();
  55.             northEast.DrawTree();
  56.             southWest.DrawTree();
  57.             southEast.DrawTree();
  58.         }
  59.     }
  60.     static void DrawSquare(Vector2 position, float size)
  61.     {
  62.         Debug.DrawLine(position, new Vector2(position.x, position.y + size), Color.red);
  63.         Debug.DrawLine(new Vector2(position.x, position.y + size), new Vector2(position.x + size, position.y + size), Color.red);
  64.         Debug.DrawLine(new Vector2(position.x + size, position.y + size), new Vector2(position.x + size, position.y), Color.red);
  65.         Debug.DrawLine(new Vector2(position.x + size, position.y), new Vector2(position.x, position.y), Color.red);
  66.     }
  67.    
  68.     //TODO: Update velocity in Simulation instead
  69.     public List<Particle> UpdateVelocity(Particle particle, float timeStep)
  70.     {
  71.         List<Particle> targets = new();
  72.         //If the node hasn't been divided, calculate the force exerted by all particles in this node
  73.         if (!divided)
  74.         {
  75.             for (int i = 0; i < particleCount; i++)
  76.             {
  77.                 if(particles[i].position == particle.position) continue;
  78.                 targets.Add(particles[i]);
  79.                 //particle.velocity += CalculateAcceleration(particle, particles[i]) * timeStep;
  80.             }
  81.  
  82.             return targets;
  83.         }
  84.         //Otherwise, calculate the ratio s/d. If node is sufficiently far away, treat it as a single particle
  85.         float ratio = boundary.size / (particle.position - centerOfMass).magnitude;
  86.         if (ratio < ApproximationThreshold)
  87.         {
  88.             //particle.velocity += CalculateAcceleration(particle, avgParticle) * timeStep;
  89.             targets.Add(avgParticle);
  90.             return targets;
  91.         }
  92.         //Otherwise, check children if the node has been divided
  93.         targets.AddRange(northWest.UpdateVelocity(particle, timeStep));
  94.         targets.AddRange(northEast.UpdateVelocity(particle, timeStep));
  95.         targets.AddRange(southWest.UpdateVelocity(particle, timeStep));
  96.         targets.AddRange(southEast.UpdateVelocity(particle, timeStep));
  97.         return targets;
  98.     }
  99.    
  100.     Vector2 CalculateAcceleration(Particle p1, Particle p2)
  101.     {
  102.         Vector2 diff = p2.position - p1.position;
  103.         Vector2 forceDir = diff.normalized;
  104.         float sqrDst = Mathf.Max(diff.sqrMagnitude, 0.05f);
  105.         Vector2 force = forceDir * (p1.mass * p2.mass) / sqrDst;
  106.         Vector2 acceleration = force / p1.mass;
  107.  
  108.         return acceleration;
  109.     }
  110.  
  111.     public void CalculateMassAndCenterOfMass()
  112.     {
  113.         for (int i = 0; i < particleCount; i++)
  114.         {
  115.             mass += particles[i].mass;
  116.         }
  117.  
  118.         float x = 0;
  119.         float y = 0;
  120.         for (int i = 0; i < particleCount; i++)
  121.         {
  122.             x += particles[i].position.x * particles[i].mass;
  123.             y += particles[i].position.y * particles[i].mass;
  124.         }
  125.  
  126.         x /= mass;
  127.         y /= mass;
  128.         centerOfMass = new Vector2(x, y);
  129.  
  130.         if (divided)
  131.         {
  132.             northWest.CalculateMassAndCenterOfMass();
  133.             northEast.CalculateMassAndCenterOfMass();
  134.             southWest.CalculateMassAndCenterOfMass();
  135.             southEast.CalculateMassAndCenterOfMass();
  136.         }
  137.  
  138.         avgParticle.position = centerOfMass;
  139.         avgParticle.mass = mass;
  140.     }
  141.    
  142.     public void Insert(Particle particle)
  143.     {
  144.         //If the quad has already divided, insert it into on of the children
  145.         if (divided)
  146.         {
  147.             InsertIntoChildren(particle);
  148.         }
  149.         else
  150.         {
  151.             // Ignore objects that do not belong in this quad tree
  152.             if (!boundary.Intersects(particle, OverlapTolerance)) return;
  153.            
  154.             // If there is space in this quad tree and if doesn't have subdivisions, add the object here
  155.             if (particleCount < Capacity)
  156.             {
  157.                 particles.Add(particle);
  158.                 particleCount++;
  159.             }
  160.             // Otherwise, subdivide and then add the point to whichever node will accept it
  161.             else
  162.             {
  163.                 Subdivide(particle);
  164.             }
  165.  
  166.         }
  167.     }
  168.  
  169.     void Subdivide(Particle particle)
  170.     {
  171.         float size = boundary.size / 2;
  172.         //If the node is big enough
  173.         if (OriginalSize / size < MaxDepth * 4)
  174.         {
  175.             //Create 4 children
  176.             northWest = new(new AABB
  177.             {
  178.                 position = new Vector2(boundary.position.x, boundary.position.y + boundary.size / 2f),
  179.                 size = size
  180.             });
  181.             northEast = new(new AABB
  182.             {
  183.                 position = new Vector2(boundary.position.x + boundary.size / 2, boundary.position.y + boundary.size / 2f),
  184.                 size = size
  185.             });
  186.             southWest = new(new AABB
  187.             {
  188.                 position = new Vector2(boundary.position.x, boundary.position.y),
  189.                 size = size
  190.             });
  191.             southEast = new(new AABB
  192.             {  
  193.                 position = new Vector2(boundary.position.x + boundary.size / 2, boundary.position.y),
  194.                 size = size
  195.             });
  196.        
  197.             //Move all objects from this quad into children
  198.             InsertIntoChildren(particle);
  199.             for (int i = 0; i < particleCount; i++)
  200.             {
  201.                 InsertIntoChildren(particles[i]);
  202.             }
  203.  
  204.             divided = true;
  205.         }
  206.     }
  207.  
  208.     void InsertIntoChildren(Particle particle)
  209.     {
  210.         northWest.Insert(particle);
  211.         northEast.Insert(particle);
  212.         southWest.Insert(particle);
  213.         southEast.Insert(particle);
  214.     }
  215. }
  216.  
  217. public struct AABB
  218. {
  219.     //Bottom left of rectangle
  220.     public Vector2 position;
  221.     public float size;
  222.  
  223.     public bool Intersects(Particle p, float margin)
  224.     {
  225.         return p.position.x > position.x - margin && p.position.x < position.x + size + margin &&
  226.                p.position.y > position.y - margin && p.position.y < position.y + size + margin;
  227.     }
  228. }
  229.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement