Physics work

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

1 files changed, 86 insertions, 30 deletions

diff --git a/src/physics.c b/src/physics.c
index 0c33e63..33a0417 100644
--- a/src/physics.c
+++ b/src/physics.c
@@ -25,10 +25,10 @@
 #define GRAVITY (9.81e-6)
 
 /* Acceleration due to booster rocket, metres per ms per ms */
-#define THRUST (GRAVITY*1.5)
+#define THRUST (GRAVITY*2.0)
 
 /* Acceleration due to forwards/backwards thrusters, m per ms per ms */
-#define FTHRUST (GRAVITY*0.2)
+#define FTHRUST (GRAVITY*0.5)
 
 /* Air friction in (m per ms per ms) per (m per ms) */
 #define FRICTION 0.001
@@ -39,39 +39,95 @@
 /* Conversion factor between friction and 'yawthrust' */
 #define TORQUE 3
 
-static int physics_will_collide_face(GLfloat sx, GLfloat sy, GLfloat sz, GLfloat vx, GLfloat vy, GLfloat vz,
-					GLfloat nx, GLfloat ny, GLfloat nz, GLfloat *fvert, int nfvert, Uint32 dt) {
+int physics_point_is_inside_hull(GLfloat cx, GLfloat cy, GLfloat cz, GLfloat *fvert, int nfvert, GLfloat nx, GLfloat ny, GLfloat nz) {
+
+	int i;
+	GLfloat p1x, p1y, p1z;
+	GLfloat p2x, p2y, p2z;
+	
+	p1x = fvert[3*0 + 0];
+	p1y = fvert[3*0 + 1];
+	p1z = fvert[3*0 + 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];
+		
+		/* Calculate 'left' vector = n ^ p1->p2 */
+		qx = p2x-p1x;  qy = p2y-p1y;  qz = p2z-p1z;
+		lx = ny*qz - nz*qy;
+		ly = - nx*qz + nz*qx;
+		lz = nx*qy - ny*qx;
+		px = cx - p1x;  py = cy - p1y;  pz = cz - p1z;
+		if ( px*lx + py*ly + pz*lz < 0.0 ) return 0;
+		
+		p1x = p2x;  p1y = p2y;  p1z = p2z;
+		
+	}
+	
+	return 1;
+
+}
+
+int physics_will_collide_face(GLfloat sx, GLfloat sy, GLfloat sz, GLfloat vx, GLfloat vy, GLfloat vz,
+					GLfloat nx, GLfloat ny, GLfloat nz, GLfloat *fvert, int nfvert, Uint32 dt,
+					ModelInstance *obj) {
 
 	GLfloat px, py, pz;
-	GLfloat t1, t2, b;
-	int t;
+	GLfloat pdotn, sdotn, vdotn;
+	GLfloat cx, cy, cz;
+	GLfloat t;
 	
 	/* Range test */
 	px = fvert[3*0 + 0];
 	py = fvert[3*0 + 1];
 	pz = fvert[3*0 + 2];
 	
-	t1 = px*nx + py*ny + pz*nz;
-	t2 = sx*nx + sy*ny + sz*nz;
-	b = vx*nx + vy*ny + vz*nz;
-	if ( b == 0 ) return 0;		/* Collision happens infinitely far in the future */
-	t = (t1 - t2) / b;
-	
-//	printf("Testing vertex %8f %8f %8f, velocity %8f %8f %8f against face %8f %8f %8f\n",
-//			sx, sy, sz, vx, vy, vz, px, py, pz);
-//	printf("Collision happens at t + %i\n", t);
-//	exit(0);
+	pdotn = px*nx + py*ny + pz*nz;
+	sdotn = sx*nx + sy*ny + sz*nz;
+	vdotn = vx*nx + vy*ny + vz*nz;
+	if ( vdotn == 0.0 ) return 0;		/* Collision happens infinitely far in the future */
+	t = (pdotn - sdotn) / vdotn;
 	
 	/* If the vertex is exactly on the plane, then moving away from the plane is OK */
-	if ( (t == 0) && (b > 0) ) return 0;
+	if ( (t == 0.0) && (vdotn > 0.0) ) return 0;
 	
-	if ( t < 0 ) return 0;		/* Collided in the past */
+	if ( t < 0.0 ) return 0;		/* Collided in the past */
 	if ( t > dt ) return 0;		/* Not going to collide this step */
 	
 	/* Boundary test */
+	cx = sx + vx * t;
+	cy = sy + vy * t;
+	cz = sz + vz * t;
+	if ( physics_point_is_inside_hull(cx, cy, cz, fvert, nfvert, nx, ny, nz) == 1 ) {
+		
+		char *name;
+		
+		if ( obj == NULL ) {
+			name = "unknown";
+		} else {
+			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);
+		
+		return 1;
 	
+	} else {
+		printf("!");
+	}
 	
-	return 1;
+	return 0;
 	
 }
 
@@ -123,7 +179,8 @@ static int physics_will_collide(ModelInstance *obj, ModelInstance *other, Uint32
 							}
 							
 							if ( physics_will_collide_face(sx, sy, sz, vx, vy, vz, nx, ny, nz,
-											face, 4, dt) != 0 ) {
+											face, 4, dt, other) != 0 ) {
+									printf("collides with %s/%p\n", other->model->name, other);
 									return 1;	/* Don't waste time looking anywhere else */
 							}
 						}
@@ -142,17 +199,17 @@ static int physics_will_collide(ModelInstance *obj, ModelInstance *other, Uint32
 }
 
 /* Called once for each object which isn't just "scenery" */
-static void physics_process(ModelInstance *obj, Uint32 dt, Room **rooms, int num_rooms) {
+static void physics_process(ModelInstance *obj, Uint32 dt, Game *game) {
 	
-	int i;
+	Room *room;
 	
-	#if 0
-	/* Consider all the currently relevant rooms */
-	for ( i=0; i<num_rooms; i++ ) {
-		int j;
+	/* Consider only the current room */
+	room = game_find_room(game, game->cur_room_x, game->cur_room_y, game->cur_room_z);
+	if ( room != NULL ) {
 		/* Consider all the objects in this room */
-		for ( j=0; j<rooms[i]->num_objects; j++ ) {
-			if ( physics_will_collide(obj, rooms[i]->objects[j], dt) ) {
+		int j;
+		for ( j=0; j<room->num_objects; j++ ) {
+			if ( physics_will_collide(obj, room->objects[j], dt) ) {
 				/* Stop the object in its tracks and return */
 				obj->vx = 0.0;
 				obj->vy = 0.0;
@@ -162,7 +219,6 @@ static void physics_process(ModelInstance *obj, Uint32 dt, Room **rooms, int num
 			}
 		}
 	}
-	#endif
 	
 	/* Air friction */
 	if ( obj->vx > 0.0 ) {
@@ -239,7 +295,7 @@ void physics_step(Game *game, Uint32 t) {
 	}
 	
 	/* The number of rooms to consider for physics purposes can probably be further reduced */
-	physics_process(game->lander, dt, game->rooms, game->num_rooms);
+	physics_process(game->lander, dt, game);
 	
 	game_check_handoff(game);