shithub: riscv

ref: 89cd4a0e67376d69d42a17bc3135ac9206c4e332
dir: /sys/src/games/galaxy/quad.c/

#include <u.h>
#include <libc.h>
#include <draw.h>
#include "galaxy.h"

void
growquads(void)
{
	quads.max *= 2;
	quads.a = realloc(quads.a, sizeof(Quad) * quads.max);
	if(quads.a == nil)
		sysfatal("could not realloc quads: %r");
}

Quad*
quad(Body *b)
{
	Quad *q;

	if(quads.l == quads.max)
		return nil;

	q = quads.a + quads.l++;
	memset(q->c, 0, sizeof(QB)*4);
	q->x = b->x;
	q->y = b->y;
	q->mass = b->mass;
	return q;
}

int
quadins(Body *nb, double size)
{
	QB *qb;
	Quad *q;
	Body *b;
	double mass, qx, qy;
	uint qxy;
	int d;

	if(space.t == EMPTY) {
		space.t = BODY;
		space.b = nb;
		return 0;
	}

	d = 0;
	qb = &space;
	qx = 0.0;
	qy = 0.0;
	for(;;) {
		if(qb->t == BODY) {
			b = qb->b;
			qxy = b->x < qx ? 0 : 1;
			qxy |= b->y < qy ? 0 : 2;
			qb->t = QUAD;
			if((qb->q = quad(b)) == nil)
				return -1;
			qb->q->c[qxy].t = BODY;
			qb->q->c[qxy].b = b;
		}

		q = qb->q;
		mass = q->mass + nb->mass;
		q->x = (q->x*q->mass + nb->x*nb->mass) / mass;
		q->y = (q->y*q->mass + nb->y*nb->mass) / mass;
		q->mass = mass;

		qxy = nb->x < qx ? 0 : 1;
		qxy |= nb->y < qy ? 0 : 2;
		if(q->c[qxy].t == EMPTY) {
			q->c[qxy].t = BODY;
			q->c[qxy].b = nb;
			STATS(if(d > quaddepth) quaddepth = d;)
			return 0;
		}

		STATS(d++;)
		qb = &q->c[qxy];
		size /= 2;
		qx += qxy&1 ? size/2 : -size/2;
		qy += qxy&2 ? size/2 : -size/2;
	}
}

void
quadcalc(Body *b, QB qb, double size)
{
	double fx÷❨m₁m₂❩, fy÷❨m₁m₂❩, dx, dy, h, G÷h³;

	for(;;) switch(qb.t) {
	default:
		sysfatal("quadcalc: No such type");
	case EMPTY:
		return;
	case BODY:
		if(qb.b == b)
			return;
		dx = qb.b->x - b->x;
		dy = qb.b->y - b->y;
		h = hypot(hypot(dx, dy), ε);
		G÷h³ = G / (h*h*h);
		fx÷❨m₁m₂❩ = dx * G÷h³;
		fy÷❨m₁m₂❩ = dy * G÷h³;
		b->newa.x += fx÷❨m₁m₂❩ * qb.b->mass;
		b->newa.y += fy÷❨m₁m₂❩ * qb.b->mass;
		STATS(calcs++;)
		return;
	case QUAD:
		dx = qb.q->x - b->x;
		dy = qb.q->y - b->y;
		h = hypot(dx, dy);
		if(h != 0.0 && size/h < θ) {
			h = hypot(h, ε);
			G÷h³ = G / (h*h*h);
			fx÷❨m₁m₂❩ = dx * G÷h³;
			fy÷❨m₁m₂❩ = dy * G÷h³;
			b->newa.x += fx÷❨m₁m₂❩ * qb.q->mass;
			b->newa.y += fy÷❨m₁m₂❩ * qb.q->mass;
			STATS(calcs++;)
			return;
		}
		size /= 2;
		quadcalc(b, qb.q->c[0], size);
		quadcalc(b, qb.q->c[1], size);
		quadcalc(b, qb.q->c[2], size);
		qb = qb.q->c[3];
		break;	/* quadcalc(q->q[3], b, size); */
	}
}

void
quadsinit(void)
{
	quads.a = calloc(5, sizeof(Body));
	if(quads.a == nil)
		sysfatal("could not allocate quads: %r");
	quads.l = 0;
	quads.max = 5;
}