ball collisions

4 files changed, 149 insertions, 4 deletions

diff --git a/balls.cpp b/balls.cpp
index e7a5a28..a939053 100644
--- a/balls.cpp
+++ b/balls.cpp
@@ -6,7 +6,7 @@
 
 using namespace std;
 
-#define VMAX_INIT 0.15
+#define VMAX_INIT 0.05
 	/* max initial velocity [m/s] */
 #define RMIN 0.05
 #define RMAX 0.10
@@ -183,9 +183,14 @@ volume(double radius) {
 
 void
 animate(int v) {
-	for (Ball& ball : balls) {
-		ball.p = ptAddVec(ball.p, ball.v);
-		collideWall(&ball, bounds);
+	size_t i, j;
+
+	/* TODO: parallel */
+	for (i = 0; i < balls.size(); i++) {
+		for (j = i+1; j < balls.size(); j++)
+			collideBall(&balls[i], &balls[j]);
+		balls[i].p = ptAddVec(balls[i].p, balls[i].v);
+		collideWall(&balls[i], bounds);
 	}
 
 	display();
diff --git a/balls.h b/balls.h
index b69eea4..1a0038b 100644
--- a/balls.h
+++ b/balls.h
@@ -19,13 +19,30 @@ typedef struct {
 	double m; /* mass [kg] */
 } Ball;
 
+Point addPt(Point p, Point q);
+Point subPt(Point p, Point q);
 Point ptAddVec(Point p, Vector v);
+Point ptSubVec(Point p, Vector v);
+Point ptMulS(Point p, double s);
+Point ptDivS(Point p, double s);
 Point Pt(double x, double y);
+
+Vector addVec(Vector v1, Vector v2);
+Vector subVec(Vector v1, Vector v2);
+Vector vecMulS(Vector v, double s);
+Vector vecDivS(Vector v, double s);
+double vecDot(Vector v1, Vector v2);
+Vector unitNorm(Vector v);
+double vecLen(Vector v);
+Vector Vec(double x, double y);
+Vector VecPt(Point p, Point q);
+
 Rectangle insetRect(Rectangle r, double n);
 Point randPtInRect(Rectangle r);
 
 int isCollision(Point p1, double r1, Point p2, double r2);
 void collideWall(Ball *b, Rectangle wall);
+void collideBall(Ball *b1, Ball *b2);
 
 int randInt(int lo, int hi);
 double randDouble(double lo, double hi);
diff --git a/collision.cpp b/collision.cpp
index 1ffe957..5f0725e 100644
--- a/collision.cpp
+++ b/collision.cpp
@@ -27,6 +27,34 @@ collideWall(Ball *b, Rectangle wall) {
 	}
 }
 
+void
+collideBall(Ball *b1, Ball *b2) {
+	Point midpt;
+	Vector n, distv;
+	double mrat, coef;
+
+	if (!isCollision(b1->p, b1->r, b2->p, b2->r))
+		return;
+
+	/* set position of collision */
+	midpt = ptDivS(addPt(b1->p, b2->p), 2);
+	n = unitNorm(VecPt(b1->p, b2->p));
+	b1->p = ptSubVec(midpt, vecMulS(n, b1->r));
+	b2->p = ptAddVec(midpt, vecMulS(n, b2->r));
+
+	/* reaction velocity */
+
+	mrat = 2.0 * b2->m / (b1->m + b2->m);
+	distv = VecPt(b2->p, b1->p);
+	coef = vecDot(subVec(b1->v, b2->v), distv) / (vecLen(distv)*vecLen(distv));
+	b1->v = subVec(b1->v, vecMulS(distv, mrat*coef));
+
+	mrat = 2.0 * b1->m / (b1->m + b2->m);
+	distv = VecPt(b1->p, b2->p);
+	coef = vecDot(subVec(b2->v, b1->v), distv) / (vecLen(distv)*vecLen(distv));
+	b2->v = subVec(b2->v, vecMulS(distv, mrat*coef));
+}
+
 static double
 clamp(double v, double lo, double hi) {
 	return min(hi, max(v, lo));
diff --git a/geometry.cpp b/geometry.cpp
index 2bdad8c..542c08d 100644
--- a/geometry.cpp
+++ b/geometry.cpp
@@ -1,6 +1,22 @@
+#include <math.h>
+
 #include "balls.h"
 
 Point
+addPt(Point p, Point q) {
+	p.x += q.x;
+	p.y += q.y;
+	return p;
+}
+
+Point
+subPt(Point p, Point q) {
+	p.x -= q.x;
+	p.y -=q.y;
+	return p;
+}
+
+Point
 ptAddVec(Point p, Vector v) {
 	p.x += v.x;
 	p.y += v.y;
@@ -8,11 +24,90 @@ ptAddVec(Point p, Vector v) {
 }
 
 Point
+ptSubVec(Point p, Vector v) {
+	p.x -= v.x;
+	p.y -= v.y;
+	return p;
+}
+
+Point
+ptMulS(Point p, double s) {
+	p.x *= s;
+	p.y *= s;
+	return p;
+}
+
+Point
+ptDivS(Point p, double s) {
+	p.x /= s;
+	p.y /=s;
+	return p;
+}
+
+Point
 Pt(double x, double y) {
 	Point p = {x, y};
 	return p;
 }
 
+Vector
+addVec(Vector v1, Vector v2) {
+	v1.x += v2.x;
+	v1.y += v2.y;
+	return v1;
+}
+
+Vector
+subVec(Vector v1, Vector v2) {
+	v1.x -= v2.x;
+	v1.y -= v2.y;
+	return v1;
+}
+
+Vector
+vecMulS(Vector v, double s) {
+	v.x *= s;
+	v.y *= s;
+	return v;
+}
+
+Vector
+vecDivS(Vector v, double s) {
+	v.x /= s;
+	v.y /= s;
+	return v;
+}
+
+double
+vecDot(Vector v1, Vector v2) {
+	return v1.x*v2.x + v1.y*v2.y;
+}
+
+Vector
+unitNorm(Vector v) {
+	return vecDivS(v, vecLen(v));
+}
+
+double
+vecLen(Vector v) {
+	return sqrt(v.x*v.x + v.y*v.y);
+}
+
+Vector
+Vec(double x, double y) {
+	Vector v = {x, y};
+	return v;
+}
+
+Vector
+VecPt(Point p, Point q) {
+	double dx, dy;
+
+	dx = q.x - p.x;
+	dy = q.y - p.y;
+	return Vec(dx, dy);
+}
+
 Rectangle
 insetRect(Rectangle r, double n) {
 	r.min.x += n;