better collisions

5 files changed, 278 insertions, 98 deletions

diff --git a/balls.c b/balls.c
index a26605d..ccc3e8e 100644
--- a/balls.c
+++ b/balls.c
@@ -1,10 +1,8 @@
-#include <u.h>
-#include <libc.h>
+#include "balls.h"
+
 #include <stdio.h>
-#include <draw.h>
 #include <thread.h>
 #include <mouse.h>
-#include <cursor.h>
 #include <keyboard.h>
 
 #define NELEMS(arr) (sizeof(arr) / sizeof(arr[0]))
@@ -19,10 +17,10 @@ enum {
 
 	KEY_QUIT = 'q' ,
 
-	NBALLS = 2,
-	RADIUS = 20,
+	NBALLS = 3,
 	VMAX = 5,
 	G = 1,
+	MASS = 1,
 
 	FPS = 60,
 	NS_PER_SEC = 1000000000,
@@ -33,14 +31,14 @@ enum {
 };
 
 typedef struct {
-	Point pos;
-	int vx, vy;
-	int col;
+	Ball b;
+
+	int color;
 
 	Channel *frametick;
 
-	Channel **posin; /* receive positions of other balls */
-	Channel **posout; /* send position to other balls */
+	Channel **in; /* <-chan Ball */
+	Channel **out; /* chan<- Ball */
 	int nothers; /* number of other balls */
 } BallArg;
 
@@ -57,8 +55,6 @@ void vcopyskip(Channel *dst[], Channel *src[], int n, int skip);
 void nooverlapcircles(Point centers[], int n);
 Point randptinrect(Rectangle r);
 int randint(int lo, int hi);
-int isoverlapcircle(Point p, Point q);
-int dist(Point p, Point q);
 void ball(void *arg);
 Image *alloccircle(int fg, int bg);
 void drawcircle(Image *m, Point pos);
@@ -137,61 +133,75 @@ drawbg(Image *walls, Image *bg) {
 void
 spawnballs(int n) {
 	Channel **ticks;
-	Point *pos;
+	Point *ps;
 	int i, j;
-	BallArg *b;
-	Channel ***poschans;
+	BallArg *arg;
+	Channel ***cs;
 
 	if ((ticks = allocchans(n, sizeof(int), TICK_BUFSIZE)) == nil)
 		sysfatal("failed to allocate frame ticker channels");
 
 	threadcreate(frametick, ticks, mainstacksize);
 
-	if ((pos = malloc(n*sizeof(Point))) == nil)
+	if ((ps = malloc(n*sizeof(Point))) == nil)
 		sysfatal("failed to allocate position array");
-	nooverlapcircles(pos, n);
+	nooverlapcircles(ps, n);
 
-	if ((poschans = malloc(n*sizeof(Channel **))) == nil)
-		sysfatal("failed to allocate position channel matrix");
+	if ((cs = malloc(n*sizeof(Channel **))) == nil)
+		sysfatal("failed to allocate channel matrix");
 	for (i = 0; i < n; i++) {
-		if ((poschans[i] = malloc(n*sizeof(Channel *))) == nil)
-			sysfatal("failed to allocate row of position channel matrix");
+		if ((cs[i] = malloc(n*sizeof(Channel *))) == nil)
+			sysfatal("failed to allocate row of channel matrix");
 		for (j = 0; j < n; j++) {
 			if (j == i)
 				continue;
-			if ((poschans[i][j] = chancreate(sizeof(Point), POS_BUFSIZE)) == nil)
-				sysfatal("failed to create position channel");
+			if ((cs[i][j] = chancreate(sizeof(Ball), POS_BUFSIZE)) == nil)
+				sysfatal("failed to create channel");
 		}
 	}
 
 	for (i = 0; i < n; i++) {
-		if ((b = malloc(sizeof(BallArg))) == nil)
+		for (j = 0; j < n; j++)
+			printf("%16p ", cs[i][j]);
+		printf("\n\n");
+	}
+
+	for (i = 0; i < n; i++) {
+		if ((arg = malloc(sizeof(BallArg))) == nil)
 			sysfatal("failed to allocate ball");
 
-		b->pos = pos[i];
-		b->vx = randint(-VMAX, VMAX+1);
-		b->vy = randint(-VMAX, VMAX+1);
-		b->col = ballcolors[randint(0, NELEMS(ballcolors))];
+		arg->b.p = ps[i];
+		arg->b.v = V(randint(-VMAX, VMAX+1), randint(-VMAX, VMAX+1));
+		arg->b.m = MASS;
 
-		b->frametick = ticks[i];
+		arg->color = ballcolors[randint(0, NELEMS(ballcolors))];
 
-		if ((b->posin = malloc((n-1)*sizeof(Channel *))) == nil)
-			sysfatal("failed to allocate array of incoming position channels");
-		mcopycolskip(b->posin, poschans, n, i, i);
+		arg->frametick = ticks[i];
 
-		if ((b->posout = malloc((n-1)*sizeof(Channel *))) == nil)
-			sysfatal("failed to allocate array of outgoing position channels");
-		vcopyskip(b->posout, poschans[i], n, i);
+		if ((arg->in = malloc((n-1)*sizeof(Channel *))) == nil)
+			sysfatal("failed to allocate array of incoming channels");
+		mcopycolskip(arg->in, cs, n, i, i);
+		printf("%d in:\n", i);
+		for (j = 0; j < n-1; j++)
+			printf("%16p ", arg->in[j]);
+		printf("\n");
 
-		b->nothers = n-1;
+		if ((arg->out = malloc((n-1)*sizeof(Channel *))) == nil)
+			sysfatal("failed to allocate array of outgoing channels");
+		vcopyskip(arg->out, cs[i], n, i);
+		printf("%d: out\n", i);
+		for (j = 0; j < n-1; j++)
+			printf("%16p ", arg->out[j]);
+		printf("\n\n");
+		arg->nothers = n-1;
 
-		threadcreate(ball, b, mainstacksize);
+		threadcreate(ball, arg, mainstacksize);
 	}
 
-	free(pos);
+	free(ps);
 	for (i = 0; i < n; i++)
-		free(poschans[i]);
-	free(poschans);
+		free(cs[i]);
+	free(cs);
 }
 
 Channel **
@@ -230,8 +240,12 @@ mcopycolskip(Channel *vec[], Channel **matrix[], int n, int col, int skip) {
 /* copy each element in n-length src, except for element skip, into n-1 length dst */
 void
 vcopyskip(Channel *dst[], Channel *src[], int n,  int skip) {
-	memmove(dst, src, skip*sizeof(Channel *));
-	memmove(dst, src+skip+1, (n-skip-1)*sizeof(Channel *));
+	int i;
+
+	for (i = 0; i < skip; i++)
+		dst[i] = src[i];
+	for (i = skip+1; i < n; i++)
+		dst[i-1] = src[i];
 }
 
 void
@@ -242,7 +256,7 @@ nooverlapcircles(Point centers[], int n) {
 	for (i = 0; i < n; i++) {
 		centers[i] = randptinrect(insetrect(bounds, RADIUS));
 		for (j = 0; j < i; j++)
-			if (isoverlapcircle(centers[j], centers[i]))
+			if (iscollision(centers[j], centers[i]))
 				break;
 		if (j < i) { /* overlapping */
 			i--;
@@ -261,74 +275,71 @@ randint(int lo, int hi) {
 	return (rand() % (hi-lo)) + lo;
 }
 
-int
-isoverlapcircle(Point p, Point q) {
-	return dist(p, q) < 2*RADIUS;
-}
-
-int
-dist(Point p, Point q) {
-	Rectangle r;
-
-	r = Rpt(p, q);
-	return sqrt(Dx(r)*Dx(r) + Dy(r)*Dy(r));
-}
-
 void
 ball(void *arg) {
-	BallArg *b;
-	Point p1, p2;
+	BallArg *barg;
+	Point p, oldp;
+	Vec v;
+	int m;
 	Image *fill, *erase;
-	int t, i, vsum;
-	float rx, ry;
-	Point otherpos, vdir;
-
-	b = (BallArg *) arg;
-	p1 = p2 = b->pos;
-
-	fill = alloccircle(b->col, DTransparent);
+	int i;
+	Ball other;
+	Point midpoint;
+	Vec d, n;
+	int magnitude;
+	int t;
+
+	barg = (BallArg *) arg;
+	p = oldp = barg->b.p;
+	v = barg->b.v;
+	m = barg->b.m;
+
+	fill = alloccircle(barg->color, DTransparent);
 	erase = alloccircle(BG, DTransparent);
 	if (fill == nil ||erase == nil)
 		sysfatal("failed to allocate image");
 
 	for (;;) {
-		b->vy += G;
+		v.y += G;
 
-		p2.x += b->vx;
-		p2.y += b->vy;
+		p = ptaddv(p, v);
 
-		printf("(%d,%d) %d %d\n", b->pos.x, b->pos.y, b->vx, b->vy);
+		printf("(%d,%d) %d %d\n", p.x, p.y, v.x, v.y);
 
 		/* check for wall collision */
-		if (p2.x < bounds.min.x+RADIUS || p2.x > bounds.max.x-RADIUS) {
-			p2.x = clamp(p2.x, bounds.min.x+RADIUS, bounds.max.x-RADIUS);
-			printf("clamped to %d\n", b->pos.x);
-			b->vx = -b->vx;
+		if (p.x < bounds.min.x+RADIUS || p.x > bounds.max.x-RADIUS) {
+			p.x = clamp(p.x, bounds.min.x+RADIUS, bounds.max.x-RADIUS);
+			printf("clamped to %d\n", p.x);
+			v.x = -v.x;
 		}
-		if (p2.y < bounds.min.y+RADIUS || p2.y > bounds.max.y-RADIUS) {
-			p2.y = clamp(p2.y, bounds.min.y+RADIUS, bounds.max.y-RADIUS);
-			b->vy = -b->vy;
+		if (p.y < bounds.min.y+RADIUS || p.y > bounds.max.y-RADIUS) {
+			p.y = clamp(p.y, bounds.min.y+RADIUS, bounds.max.y-RADIUS);
+			v.y = -v.y;
 		}
 
-		broadcast(p2, b->posout, b->nothers);
+		broadcast(p, barg->out, barg->nothers);
 
 		/* check for ball collision */
-		for (i = 0; i < b->nothers; i++) {
-			recv(b->posin[i], &otherpos);
-			if (isoverlapcircle(p2, otherpos)) {
-				vdir = subpt(p2, otherpos);
-				rx = (float) vdir.x / (float) (vdir.x + vdir.y);
-				ry = (float) vdir.y / (float) (vdir.x + vdir.y);
-				vsum = b->vx + b->vy;
-				b->vx = rx * (float) vsum;
-				b->vy = ry * (float) vsum;
+		for (i = 0; i < barg->nothers; i++) {
+			printf("recv %16p\n", barg->in[i]);
+			recv(barg->in[i], &other);
+			if (iscollision(p, other.p)) {
+				midpoint = divpt(addpt(p, other.p), 2);
+				d = Vpt(p, other.p);
+				p = ptaddv(midpoint, vmuls(unitnorm(d), -RADIUS));
+
+				n = unitnorm(Vpt(other.p, p));
+				magnitude = 2*vdot(v, n) / (m + other.m);
+				v = vsub(v, vmuls(
+					vsub(vmuls(n, m), vmuls(n, other.m)),
+					magnitude));
 			}
 		}		
-
-		recv(b->frametick, &t);
-		drawcircle(erase, p1);
-		drawcircle(fill, p2);
-		p1 = p2;
+		
+		recv(barg->frametick, &t);
+		drawcircle(erase, oldp);
+		drawcircle(fill, p);
+		oldp = p;
 
 	}	
 }
@@ -352,7 +363,6 @@ alloccircle(int fg, int bg) {
 	return m;
 }
 
-
 void
 drawcircle(Image *m, Point pos) {
 	draw(screen, rectaddpt(bounds, subpt(pos, Pt(RADIUS, RADIUS))), m, nil, ZP);
@@ -375,8 +385,10 @@ max(int a, int b) {
 
 void
 broadcast(Point p, Channel *cs[], int n) {
-	while (n-- > 0)
+	while (n-- > 0) {
+		printf("send %16p\n", cs[n]);
 		send(cs[n], &p);
+	}
 }
 
 void
diff --git a/balls.h b/balls.h
new file mode 100644
index 0000000..0fa93a7
--- /dev/null
+++ b/balls.h
@@ -0,0 +1,34 @@
+#include <u.h>
+#include <libc.h>
+#include <draw.h>
+#include <cursor.h>
+
+enum {
+	RADIUS = 20,
+};
+
+typedef struct {
+	int x, y;
+} Vec;
+
+typedef struct {
+	Point p1, p2;
+} Line;
+
+typedef struct {
+	Point p; /* position */
+	Vec v; /* velocity */
+	int m; /* mass */
+} Ball;
+
+Vec vsub(Vec v1, Vec v2);
+Vec vmuls(Vec v, int a);
+Vec vdivs(Vec v, int a);
+int vdot(Vec v1, Vec v2);
+int vlen(Vec v);
+Vec unitnorm(Vec v);
+Point ptaddv(Point p, Vec v);
+Vec V(int x, int y);
+Vec Vpt(Point p, Point q);
+
+int iscollision(Point p, Point q);
\ No newline at end of file
diff --git a/collision.c b/collision.c
new file mode 100644
index 0000000..d377d39
--- /dev/null
+++ b/collision.c
@@ -0,0 +1,81 @@
+#include "balls.h"
+
+int
+iscollision(Point p, Point q) {
+	int dx, dy;
+
+	dx = p.x - q.x;
+	dy = p.y-q.y;
+	return (dx*dx + dy*dy) < 4*RADIUS*RADIUS;
+}
+
+/* TODO: remove
+int
+collision(Ball b1, Ball b2, Point *p) {
+	Point l1, l2, d;
+	int dx, dy, closestdistsq, backdist, mvmtveclen;
+
+	l1 = b1.p;
+	l2 = Pt(b.p.x+b.v.x, b.p.y+b.v.y);
+	d = closestpointonline(L(l1, l2), b2.p));
+
+	dx = b2.p.x - d.x;
+	dy = b2.p.y - d.y;
+	closestdistsq = dx*dx + dy*dy;
+
+	if (closestdistsq > 4*RADIUS*RADIUS)
+		return 0;
+
+	backdist = sqrt(4*RADIUS*RADIUS - closestdistsq);
+	mvmtveclen = vlen(b1.v);
+	p->x = d.x - backdist * (b1.v.x / mvmtveclen);
+	p->y = d.y - backdist * (b1.v.y / mvmtveclen);
+	return 1;
+}
+
+Vec
+vpostcollision(Ball b1, Ball b2) {
+	Point c1, c2;
+	int dcx, dcy, d;
+	Vec n;
+
+	if (!collision(b1, b2, &c1)) {
+		printf("warning: vpostcollision called, but no collision\n");
+		return b1.v;
+	}
+	if (!collision(b2, b1, &c2)) {
+		printf("warning: vpostcollision called, but no collision\n");
+		return b1.v;
+	}
+
+	d = norm(vsub(c1, c2));
+	n = V(dcx/d, dcy/d);
+	p = 2 * (vdot(b1.v, n) - vdot(b2.v, n)) / (b1.m + b2.m);
+	
+	b1.v.x = b1.v.x - p * b1.m * n.x;
+	b1.v.y = b1.v.y - p * b1.m * n.y;
+	return b1.v;
+}
+
+Point
+closestpointonline(Line l, Point p) {
+	int a, b, c1, c2, d;
+	Point c;
+	
+	a = l.p2.y - l.p1.y;
+	b = l.p1.x - l.p2.x;
+
+	c1 = a*l.p1.x + b*l.p1.y;
+	c2 = -b*p.x + a*p.y;
+
+	d = a*a - -b*b;
+	if (d != 0) {
+		c.x = (a*c1 - b*c2) / d;
+		c.y = (a*c2 + b*c1) / d;
+	} else {
+		c = p;
+	}
+
+	return c;
+}
+*/
diff --git a/mkfile b/mkfile
index 945037d..f5c1922 100644
--- a/mkfile
+++ b/mkfile
@@ -2,11 +2,13 @@ CC=9c
 LD=9l
 O=o
 
-balls: balls.$O
-	$LD -o balls balls.$O
+OBJ = balls.$O vec.$O collision.$O
+
+balls: $OBJ
+	$LD -o balls $OBJ
 
 %.$O: %.c
 	$CC $CFLAGS $stem.c
 
 clean:V:
-	rm -f balls.$O balls
\ No newline at end of file
+	rm -f ./*.$O balls
\ No newline at end of file
diff --git a/vec.c b/vec.c
new file mode 100644
index 0000000..ba8edfa
--- /dev/null
+++ b/vec.c
@@ -0,0 +1,51 @@
+#include "balls.h"
+
+Vec
+vsub(Vec v1, Vec v2) {
+	return V(v1.x-v2.x, v1.y-v2.y);
+}
+
+Vec
+vmuls(Vec v, int a) {
+	return V(v.x*a, v.y*a);
+}
+
+Vec
+vdivs(Vec v, int a) {
+	if (a == 0)
+		return V(0, 0);
+	return V(v.x/a, v.y/a);
+}
+
+int
+vdot(Vec v1, Vec v2) {
+	return v1.x*v2.x + v1.y*v2.y;
+}
+
+int
+vlen(Vec v) {
+	return sqrt(v.x*v.x + v.y*v.y);
+}
+
+Vec
+unitnorm(Vec v) {
+	return vdivs(v, vlen(v));
+}
+
+Point
+ptaddv(Point p, Vec v) {
+	p.x += v.x;
+	p.y += v.y;
+	return p;
+}
+
+Vec
+V(int x, int y) {
+	Vec v = {x, y};
+	return v;
+}
+
+Vec
+Vpt(Point p, Point q) {
+	return V(p.x-q.x, p.y-q.y);
+}