sys/src/games/galaxy/quad.c

   1 #include <u.h>
   2 #include <libc.h>
   3 #include <draw.h>
   4 #include "galaxy.h"
   5
   6 void
   7 growquads(void)
   8 {
   9         quads.max *= 2;
  10         quads.a = realloc(quads.a, sizeof(Quad) * quads.max);
  11         if(quads.a == nil)
  12                 sysfatal("could not realloc quads: %r");
  13 }
  14
  15 Quad*
  16 quad(Body *b)
  17 {
  18         Quad *q;
  19
  20         if(quads.l == quads.max)
  21                 return nil;
  22
  23         q = quads.a + quads.l++;
  24         memset(q->c, 0, sizeof(QB)*4);
  25         q->x = b->x;
  26         q->y = b->y;
  27         q->mass = b->mass;
  28         return q;
  29 }
  30
  31 int
  32 quadins(Body *nb, double size)
  33 {
  34         QB *qb;
  35         Quad *q;
  36         Body *b;
  37         double mass, qx, qy;
  38         uint qxy;
  39         int d;
  40
  41         if(space.t == EMPTY) {
  42                 space.t = BODY;
  43                 space.b = nb;
  44                 return 0;
  45         }
  46
  47         d = 0;
  48         qb = &space;
  49         qx = 0.0;
  50         qy = 0.0;
  51         for(;;) {
  52                 if(qb->t == BODY) {
  53                         b = qb->b;
  54                         qxy = b->x < qx ? 0 : 1;
  55                         qxy |= b->y < qy ? 0 : 2;
  56                         qb->t = QUAD;
  57                         if((qb->q = quad(b)) == nil)
  58                                 return -1;
  59                         qb->q->c[qxy].t = BODY;
  60                         qb->q->c[qxy].b = b;
  61                 }
  62
  63                 q = qb->q;
  64                 mass = q->mass + nb->mass;
  65                 q->x = (q->x*q->mass + nb->x*nb->mass) / mass;
  66                 q->y = (q->y*q->mass + nb->y*nb->mass) / mass;
  67                 q->mass = mass;
  68
  69                 qxy = nb->x < qx ? 0 : 1;
  70                 qxy |= nb->y < qy ? 0 : 2;
  71                 if(q->c[qxy].t == EMPTY) {
  72                         q->c[qxy].t = BODY;
  73                         q->c[qxy].b = nb;
  74                         STATS(if(d > quaddepth) quaddepth = d;)
  75                         return 0;
  76                 }
  77
  78                 STATS(d++;)
  79                 qb = &q->c[qxy];
  80                 size /= 2;
  81                 qx += qxy&1 ? size/2 : -size/2;
  82                 qy += qxy&2 ? size/2 : -size/2;
  83         }
  84 }
  85
  86 void
  87 quadcalc(QB qb, Body *b, double size)
  88 {
  89         double fx÷❨m₁m₂❩, fy÷❨m₁m₂❩, dx, dy, h, G÷h³;
  90
  91         for(;;) switch(qb.t) {
  92         default:
  93                 sysfatal("quadcalc: No such type");
  94         case EMPTY:
  95                 return;
  96         case BODY:
  97                 if(qb.b == b)
  98                         return;
  99                 dx = qb.b->x - b->x;
 100                 dy = qb.b->y - b->y;
 101                 h = hypot(hypot(dx, dy), ε);
 102                 G÷h³ = G / (h*h*h);
 103                 fx÷❨m₁m₂❩ = dx * G÷h³;
 104                 fy÷❨m₁m₂❩ = dy * G÷h³;
 105                 b->newa.x += fx÷❨m₁m₂❩ * qb.b->mass;
 106                 b->newa.y += fy÷❨m₁m₂❩ * qb.b->mass;
 107                 STATS(calcs++;)
 108                 return;
 109         case QUAD:
 110                 dx = qb.q->x - b->x;
 111                 dy = qb.q->y - b->y;
 112                 h = hypot(dx, dy);
 113                 if(h != 0.0 && size/h < θ) {
 114                         h = hypot(h, ε);
 115                         G÷h³ = G / (h*h*h);
 116                         fx÷❨m₁m₂❩ = dx * G÷h³;
 117                         fy÷❨m₁m₂❩ = dy * G÷h³;
 118                         b->newa.x += fx÷❨m₁m₂❩ * qb.q->mass;
 119                         b->newa.y += fy÷❨m₁m₂❩ * qb.q->mass;
 120                         STATS(calcs++;)
 121                         return;
 122                 }
 123                 size /= 2;
 124                 quadcalc(qb.q->c[0], b, size);
 125                 quadcalc(qb.q->c[1], b, size);
 126                 quadcalc(qb.q->c[2], b, size);
 127                 qb = qb.q->c[3];
 128                 break;  /* quadcalc(q->q[3], b, size); */
 129         }
 130 }
 131
 132 void
 133 quadsinit(void)
 134 {
 135         quads.a = calloc(5, sizeof(Body));
 136         if(quads.a == nil)
 137                 sysfatal("could not allocate quads: %r");
 138         quads.l = 0;
 139         quads.max = 5;
 140 }