From: Sam Moore <20503628@student.uwa.edu.au>
Date: Sat, 4 Aug 2012 07:24:20 +0000 (+0800)
Subject: Added collision handling
X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;h=188f263700258d5b742d914baa25cbd316b64826;p=matches%2Fhonours.git

Added collision handling
---

diff --git a/course/semester2/pprog/assignment0/main.c b/course/semester2/pprog/assignment0/main.c
index c3357906..d355687d 100644
--- a/course/semester2/pprog/assignment0/main.c
+++ b/course/semester2/pprog/assignment0/main.c
@@ -9,20 +9,33 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <assert.h>
+#include <time.h>
 
 #include "nbody.h"
 
 int main(int argc, char ** argv)
 {
-
+	srand(time(NULL));
 	System system;
-	System_Init(&system, (float[3]){320,240,0}, 0.1, 10, 1);
+	System_Init(&system, (float[3]){320,240,0}, 1.0, 1.0, 0.4,1);
 	#ifdef _GRAPHICS_H
 		Graphics_Init("N-Body", 640, 480);
 	#endif //_GRAPHICS_H
 
-	assert(System_AddBody(&system, 1, (float[2]){0.0f,0.0f}, (float[2]){0.0f,0.0f})->exists);
-	assert(System_AddBody(&system, 0.01, (float[2]){120.0f, -120.0f}, (float[2]){-1.1f, 1.0f})->exists);
+	for (unsigned a = 0; a < 100; ++a)
+	{
+		float m = ((float)(rand() % 100))/1000.0;
+		float x[DIMENSIONS];
+		float v[DIMENSIONS];
+		for (unsigned i = 0; i < DIMENSIONS; ++i)
+		{
+			x[i] = (float)(rand() % 640 - 320);
+			v[i] = (float)(rand() % 100 - 50) / 100;
+		}
+		System_AddBody(&system, m, x, v);
+	}
+	System_AddBody(&system, 1.1, (float[2]){0,0}, (float[2]){0,0});
+
 	while (true)
 	{
 
diff --git a/course/semester2/pprog/assignment0/nbody.c b/course/semester2/pprog/assignment0/nbody.c
index 6f3ce508..76c90675 100644
--- a/course/semester2/pprog/assignment0/nbody.c
+++ b/course/semester2/pprog/assignment0/nbody.c
@@ -29,19 +29,26 @@ void Body_Init(Body * body, float m, float x[DIMENSIONS], float v[DIMENSIONS])
 		body->x[i] = x[i];
 		body->v[i] = v[i];
 		body->F[i] = 0.00;
+
+		body->x_old[i] = x[i];
+		body->v_old[i] = v[i];
 	}
+
 	body->exists = true;
+	body->collided = false;
 }
 
 /**
  * @function Body_Destroy
  * @purpose Destroy a Body
  * @param body - Target Body
+ * @param system - System to which the Body belongs
  */
-void Body_Destroy(Body * body)
+void Body_Destroy(Body * body, System * system)
 {
 	assert(body != NULL);
 	body->exists = false;
+	system->n -= 1;
 }
 
 /**
@@ -53,16 +60,19 @@ void Body_Destroy(Body * body)
 void Body_Step(Body * body, System * system)
 {
 	assert(body != NULL && system != NULL && body->exists);
-
-	//fprintf(stderr, "DEBUG - Body %p\t%d\n", (void*)(body), body->exists);
+	body->collided = false;
 	for (unsigned i = 0; i < DIMENSIONS; ++i)
 	{
+
+		//Each Body keeps track of position and velocity on the previous step
+		//This is used for collision handling in the overall System step.
+		body->x_old[i] = body->x[i];
+		body->v_old[i] = body->v[i];
+
 		body->x[i] += body->v[i] * system->dt; //Update the position vector
 		body->v[i] += (body->F[i] / body->m) * system->dt; //Update the velocity vector
 		body->F[i] = 0.00; //Reset the net force vector
 	}
-
-	//Do collision checking
 	
 }
 
@@ -99,8 +109,121 @@ void Body_Force(Body * A, Body * B, System * system)
 float Body_Radius(Body * body)
 {
 	assert(body != NULL && body->exists);
-	float radius = 5.0;
-	return radius;
+	return 8.0 * sqrt(body->m);
+}
+
+
+/**
+ * @function Body_CheckCollision
+ * @purpose Check for collision between bodies
+ * @param A - First body
+ * @param B - Second body
+ * @param system - System that the bodies belong to
+ * @param t - pointer to variable that will store the time of closest approach
+ * @returns true if collision occurred
+ */
+bool Body_CheckCollision(Body * A, Body * B, System * system, float * t)
+{
+	assert(A != NULL && B != NULL && system != NULL && A->exists && B->exists);
+
+	if (A == B)
+		return false;
+
+	// Determine time and position of closest approach 	
+	float x_rel[DIMENSIONS];
+	float v_rel[DIMENSIONS];
+	float coeff_t = 0.0;
+	float c = 0.0;
+	for (unsigned i = 0; i < DIMENSIONS; ++i)
+	{
+		x_rel[i] = B->x_old[i] - A->x_old[i];
+		v_rel[i] = B->v_old[i] - A->v_old[i];
+		c += v_rel[i] * x_rel[i];
+		coeff_t += v_rel[i] * v_rel[i];
+	}		
+
+	*t = -c / coeff_t;
+	if (*t <= system->dt/100.0 || *t > system->dt)
+		return false;
+
+	float r2 = 0.0;
+	for (unsigned i = 0; i < DIMENSIONS; ++i)
+	{
+		x_rel[i] += *t * v_rel[i];
+		r2 += pow(x_rel[i], 2);
+	}
+
+	return (r2 <= pow(Body_Radius(A), 2) + pow(Body_Radius(B), 2));
+}
+
+/** 
+ * @function Body_HandleCollision
+ * @purpose Handle a collision between two Bodies
+ * @param A - The first Body
+ * @param B - The second Body
+ * @param system - The System to which the bodies belong
+ * @param t - The time of closest approach
+ */
+void Body_HandleCollision(Body * A, Body * B, System * system, float t)
+{
+	fprintf(stderr, "#Collision %p %p at t %f\n", (void*)A, (void*)B, t);
+	fprintf(stderr,"# Before Collide; A velocity is %f %f, B velocity is %f %f\n", A->v[0], A->v[1], B->v[0], B->v[1]);
+	//Calculate A's new velocity and position
+	for (unsigned i = 0; i < DIMENSIONS; ++i)
+	{
+		A->v[i] = A->m * A->v_old[i] + B->m * B->v_old[i] + B->m * system->cor * (B->v_old[i] - A->v_old[i]);
+		A->v[i] = A->v[i] / (A->m + B->m); 
+
+		A->x[i] = A->x_old[i] + A->v_old[i] * t; //Move to the collision point at the original velocity
+		A->x[i] += A->v[i] * (system->dt - t); //Move the rest of the step at the new velocity
+		
+	}
+	
+	//For cor = 0, bodies always merge; B is travelling with A
+	if (system->cor == 0.0) 
+	{
+		A->m += B->m; //Increase A's mass
+		Body_Destroy(B, system); //Destroy B
+		return;
+	}
+	
+	//Calculate B's new velocity and position
+	for (unsigned i = 0; i < DIMENSIONS; ++i)
+	{
+		B->v[i] = B->m * B->v_old[i] + A->m * A->v_old[i] + A->m * system->cor * (A->v_old[i] - B->v_old[i]);
+		B->v[i] = B->v[i] / (B->m + A->m); 
+
+		B->x[i] = B->x_old[i] + B->v_old[i] * t; //Move to the collision point at the original velocity
+		B->x[i] += B->v[i] * (system->dt - t); //Move the rest of the step at the new velocity
+		
+	}
+	
+	//Determine whether Body B has escaped Body A; if it hasn't, merge the two bodies
+
+
+	fprintf(stderr, "# After Collide; A velocity is %f %f, B velocity is %f %f\n", A->v[0], A->v[1], B->v[0], B->v[1]);
+	
+	float v2 = 0.0;
+	float r2 = 0.0;
+	for (unsigned i = 0; i < DIMENSIONS; ++i)
+	{
+		v2 += pow(B->v[i], 2);
+		r2 += pow(B->x[i] - A->x[i], 2);
+	}
+	
+	fprintf(stderr, "# Collision; v is %f, escape is %f and %f\n", v2, 2.0 * system->G * A->m / sqrt(r2),2.0 * system->G * B->m / sqrt(r2));
+	if (v2 > 2.0*system->G * A->m / sqrt(r2) && v2 > 2.0*system->G * B->m / sqrt(r2)) 
+		return;
+	
+	//Inelastically merge the two bodies
+	for (unsigned i = 0; i < DIMENSIONS; ++i)
+	{
+		A->x[i] = (A->m * A->x[i] + B->m * B->x[i]) / (A->m + B->m);
+		A->v[i] = (A->m * A->v[i] + B->m * B->v[i]) / (A->m + B->m);
+	}
+	A->m += B->m;
+	Body_Destroy(B, system);
+
 }
 
 /**
@@ -134,9 +257,10 @@ void Body_WriteData(Body * body, System * system, FILE * file)
  * @param x - The origin of the System (currently used for offset of graphics)
  * @param dt - Time step
  * @param G - Universal Gravitational Constant
+ * @param cor - The coefficient of restitution for collisions
  * @param nMax - Number of Body objects this System will contain (DEFAULT = 10)
  */
-void System_Init(System * system, float x[DIMENSIONS], float dt, float G, unsigned nMax)
+void System_Init(System * system, float x[DIMENSIONS], float dt, float G, float cor, unsigned nMax)
 {
 	assert(system != NULL);
 	for (unsigned i = 0; i < DIMENSIONS; ++i)
@@ -145,6 +269,7 @@ void System_Init(System * system, float x[DIMENSIONS], float dt, float G, unsign
 	
 	system->dt = dt;
 	system->G = G;
+	system->cor = cor;
 	system->nMax = nMax;
 	system->n = 0;
 	
@@ -179,19 +304,17 @@ void System_Step(System * system)
 	// Update forces
 	for (unsigned a = 0; a < system->nMax; ++a)
 	{
-		if (system->bodies[a].exists)
+		if (!(system->bodies+a)->exists)
+			continue;
+
+		for (unsigned b = 0; b < system->nMax; ++b)
 		{
-			for (unsigned b = 0; b < system->nMax; ++b)
-			{
-				//fprintf(stderr, "DEBUG1 - %p\t%p\t%d\t%d\n", (void*)(system->bodies+a), (void*)(system->bodies+b), system->bodies[a].exists, system->bodies[b].exists);
-				if (system->bodies[b].exists)
-				{
-					//fprintf(stderr, "DEBUG2 - %p\t%p\t%d\t%d\n", (void*)(system->bodies+a), (void*)(system->bodies+b), system->bodies[a].exists, system->bodies[b].exists);
-					Body_Force(&(system->bodies[a]), &(system->bodies[b]), system); //Update force on bodies[a] due to bodies[b]
-					//fprintf(stderr, "DEBUG3 - %p\t%p\t%d\t%d\n", (void*)(system->bodies+a), (void*)(system->bodies+b), system->bodies[a].exists, system->bodies[b].exists);
-				}
-			}
+			if (!(system->bodies+b)->exists)
+				continue;
+
+			Body_Force(system->bodies+a, system->bodies+b, system); //Update force on bodies[a] due to 
 		}
+
 	}
 
 	// Perform steps
@@ -199,7 +322,43 @@ void System_Step(System * system)
 	{
 		if ((system->bodies+a)->exists)
 			Body_Step(system->bodies+a, system);
+	
+
+		
 	}
+
+	//Check for collisions
+	//Note: Only allows for one collision per step
+
+	for (unsigned a = 0; a < system->nMax; ++a)
+	{
+		if ((system->bodies+a)->exists == false || (system->bodies+a)->collided)
+			continue;
+
+		for (unsigned b = 0; b < system->nMax; ++b)
+		{
+			if ((system->bodies+b)->exists == false || (system->bodies+b)->collided)
+				continue;
+		
+			float t = 0.0;
+			if (Body_CheckCollision(system->bodies+a, system->bodies+b, system, &t))
+			{
+				Body_HandleCollision(system->bodies+a, system->bodies+b, system, t);
+				(system->bodies+a)->collided = true;
+				(system->bodies+b)->collided = true;
+				break;
+			}
+				
+		}
+
+		if ((system->bodies+a)->x[0] + system->x[0] < 0 || (system->bodies+a)->x[0] + system->x[0] > 640)
+			(system->bodies+a)->v[0] = -(system->bodies+a)->v[0];
+		if ((system->bodies+a)->x[1] + system->x[1] < 0 || (system->bodies+a)->x[1] + system->x[1] > 480)
+			(system->bodies+a)->v[1] = -(system->bodies+a)->v[1];
+
+	}
+
+	
 }
 
 /**
diff --git a/course/semester2/pprog/assignment0/nbody.h b/course/semester2/pprog/assignment0/nbody.h
index 775699f0..efd713ed 100644
--- a/course/semester2/pprog/assignment0/nbody.h
+++ b/course/semester2/pprog/assignment0/nbody.h
@@ -26,7 +26,13 @@ typedef struct
 	float v[DIMENSIONS]; //Velocity vector
 	float F[DIMENSIONS]; //Net force due to all other bodies in the System
 
+	// Used for detecting and handling collisions
+	float x_old[DIMENSIONS]; //Position on previous step
+	float v_old[DIMENSIONS]; //Velocity on previous step
+
 	bool exists; //Used by memory management
+	bool collided; //Used to enforce "one collision per step"
+
 } Body;
 
 /**
@@ -34,15 +40,24 @@ typedef struct
  */
 typedef struct
 {
+
+	//--- Physical parameters ---
+
+	float G; //Universal Gravitational Constant; used to calculate forces between Bodies
+	float dt; //Time interval
+	float cor; //Coefficient of restitution for objects in the system
+
+	float x[DIMENSIONS]; //The origin of the system
+
+
+	//--- Memory management and simulation parameters ---
+
 	Body * bodies; //Array of Bodies
 	unsigned nMax; //Size of array; maximum number of Bodies before Array needs to be resized
 	unsigned n; //Number of bodies currently in the system
-	float G; //Universal Gravitational Constant; used to calculate forces between Bodies
-
-	float dt; //Time interval
 	unsigned step; //Number of steps executed by the System
 
-	float x[DIMENSIONS]; //The origin of the system
+	
 
 } System;
 
@@ -50,15 +65,17 @@ typedef struct
  * Functions for operating with Body objects
  */
 extern void Body_Init(Body * body, float m, float x[DIMENSIONS], float v[DIMENSIONS]); //Initialise Body
-extern void Body_Destroy(Body * body); //Destroy Body
+extern void Body_Destroy(Body * body, System * system); //Destroy Body
 extern void Body_Step(Body * body, System * system); //Step Body
 extern void Body_Force(Body * A, Body * B, System * system); //Calculate Force between Bodies
 
+extern bool Body_CheckCollision(Body * A, Body * B, System * system, float * t); //Check for collision between Bodies
+extern void Body_HandleCollision(Body * A, Body * B, System * system, float t); //Handle collision between Bodies
 
 /**
  * Functions for operating with System object
  */
-extern void System_Init(System * system, float x[DIMENSIONS], float dt, float G, unsigned nMax); //Initialise System
+extern void System_Init(System * system, float x[DIMENSIONS], float dt, float G, float cor, unsigned nMax); //Initialise System
 extern void System_Destroy(System * system); //Destroy System
 extern void System_Step(System * system); //Step all Bodies in System
 extern Body * System_AddBody(System * system, float m, float x[DIMENSIONS], float v[DIMENSIONS]); //Add Body to System