Added collision handling

diff --git a/course/semester2/pprog/assignment0/main.c b/course/semester2/pprog/assignment0/main.c
index c335790..d355687 100644 (file)
--- 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 6f3ce50..76c9067 100644 (file)
--- 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 775699f..efd713e 100644 (file)
--- 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