Mostly fix collision detection (collision response now needs work...)

git-svn-id: svn://cook.msm.cam.ac.uk:745/thrust3d/thrust3d@46 84d2e878-0bd5-11dd-ad15-13eda11d74c5

1 files changed, 26 insertions, 16 deletions

diff --git a/src/physics.c b/src/physics.c
index 33a0417..b289b06 100644
--- a/src/physics.c
+++ b/src/physics.c
@@ -49,16 +49,27 @@ int physics_point_is_inside_hull(GLfloat cx, GLfloat cy, GLfloat cz, GLfloat *fv
 	p1y = fvert[3*0 + 1];
 	p1z = fvert[3*0 + 2];
 	
-	for ( i=1; i<nfvert; i++ ) {
+//	printf("Testing if (%5.2f %5.2f %5.2f) is inside plane: \n", cx, cy, cz);
+//	for ( i=0; i<nfvert; i++ ) {
+//		printf("%3i : %5.2f %5.2f %5.2f\n", i, fvert[3*i + 0], fvert[3*i + 1], fvert[3*i + 2]);
+//	}
+	
+	for ( i=1; i<=nfvert; i++ ) {
 		
 		GLfloat lx, ly, lz;
 		GLfloat qx, qy, qz;
 		GLfloat px, py, pz;
 		
-		p2x = fvert[3*i + 0];
-		p2y = fvert[3*i + 1];
-		p2z = fvert[3*i + 2];
-		
+		if ( i < nfvert ) {
+			p2x = fvert[3*i + 0];
+			p2y = fvert[3*i + 1];
+			p2z = fvert[3*i + 2];
+		} else {
+			p2x = fvert[3*0 + 0];
+			p2y = fvert[3*0 + 1];
+			p2z = fvert[3*0 + 2];
+		}
+			
 		/* Calculate 'left' vector = n ^ p1->p2 */
 		qx = p2x-p1x;  qy = p2y-p1y;  qz = p2z-p1z;
 		lx = ny*qz - nz*qy;
@@ -115,16 +126,14 @@ int physics_will_collide_face(GLfloat sx, GLfloat sy, GLfloat sz, GLfloat vx, GL
 			name = obj->model->name;
 		}
 		
-		printf("Testing vertex %8f %8f %8f, velocity %8f %8f %8f against face %8f %8f %8f (%s/%p)\n",
-				sx, sy, sz, vx, vy, vz, px, py, pz, name, obj);
-		printf("(%f - %f) / %f\n", pdotn, sdotn, vdotn);
-		printf("Collision happens at t + %f\n", t);
-		exit(0);
+	//	printf("Testing vertex %8f %8f %8f, velocity %8f %8f %8f against face %8f %8f %8f (%s/%p)\n",
+	//			sx, sy, sz, vx, vy, vz, px, py, pz, name, obj);
+	//	printf("(%f - %f) / %f\n", pdotn, sdotn, vdotn);
+	//	printf("Collision happens at t + %f\n", t);
+	//	exit(0);
 		
 		return 1;
 	
-	} else {
-		printf("!");
 	}
 	
 	return 0;
@@ -164,18 +173,18 @@ static int physics_will_collide(ModelInstance *obj, ModelInstance *other, Uint32
 				
 					case PRIMITIVE_QUADS : {
 						
+						/* Faces are quads */
 						int f;
 						
-						/* Faces are quads */
 						for ( f=0; f<other->model->primitives[a]->num_vertices/4; f++ ) {
 						
 							GLfloat face[3*4];
 							int q;
 							
 							for ( q=0; q<4; q++ ) {
-								face[3*q + 0] = other->x + other->model->primitives[a]->vertices[4*(f+q) + 0];
-								face[3*q + 1] = other->y + other->model->primitives[a]->vertices[4*(f+q) + 1];
-								face[3*q + 2] = other->z + other->model->primitives[a]->vertices[4*(f+q) + 2];
+								face[3*q + 0] = other->x + other->model->primitives[a]->vertices[3*((4*f)+q) + 0];
+								face[3*q + 1] = other->y + other->model->primitives[a]->vertices[3*((4*f)+q) + 1];
+								face[3*q + 2] = other->z + other->model->primitives[a]->vertices[3*((4*f)+q) + 2];
 							}
 							
 							if ( physics_will_collide_face(sx, sy, sz, vx, vy, vz, nx, ny, nz,
@@ -185,6 +194,7 @@ static int physics_will_collide(ModelInstance *obj, ModelInstance *other, Uint32
 							}
 						}
 						break;
+						
 					}
 				
 				}