path: root/src/physics.c

/*
 * physics.c
 *
 * Calculate what happens
 *
 * (c) 2008 Thomas White <taw27@cam.ac.uk>
 *
 *  thrust3d - a silly game
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <SDL.h>
#include <math.h>

#include "types.h"
#include "model.h"
#include "game.h"
#include "utils.h"

/* Acceleration due to gravity in metres per millisecond per millisecond */
#define GRAVITY (9.81e-6)

/* Acceleration due to booster rocket, metres per ms per ms */
#define THRUST (GRAVITY*2.0)

/* Acceleration due to forwards/backwards thrusters, m per ms per ms */
#define FTHRUST (GRAVITY*0.5)

/* Air friction in (m per ms per ms) per (m per ms) */
#define FRICTION 0.001

/* Lander craft turning speed in radians per ms per ms */
#define YAWTHRUST (M_PI/3000000)

/* Conversion factor between friction and 'yawthrust' */
#define TORQUE 3

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];
	
//	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;
		
		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;
		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 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];
	
	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.0) && (vdotn > 0.0) ) return 0;
	
	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;
	
	}
	
	return 0;
	
}

/* Return non-zero if 'obj' will collide with 'other' with 'dt' milliseconds */
static int physics_check_collide(ModelInstance *obj, ModelInstance *other, Uint32 dt) {

	int i;
	
	const GLfloat vx = obj->vx;
	const GLfloat vy = obj->vy;
	const GLfloat vz = obj->vz;
	
	/* Consider all the primitives in 'obj' */
	for ( i=0; i<obj->model->num_primitives; i++ ) {
		
		int j;
		
		/* Consider all the vertices in this primitive */
		for ( j=0; j<obj->model->primitives[i]->num_vertices; j++ ) {
		
			int a;
			const GLfloat sx = obj->x + obj->model->primitives[i]->vertices[3*j+0];
			const GLfloat sy = obj->y + obj->model->primitives[i]->vertices[3*j+1];
			const GLfloat sz = obj->z + obj->model->primitives[i]->vertices[3*j+2];
			
			/* Consider all the primitives in 'other' */
			for ( a=0; a<other->model->num_primitives; a++ ) {
			
				const GLfloat nx = other->model->primitives[a]->normals[0];
				const GLfloat ny = other->model->primitives[a]->normals[1];
				const GLfloat nz = other->model->primitives[a]->normals[2];
				
				switch ( other->model->primitives[a]->type ) {
				
					case PRIMITIVE_QUADS : {
						
						/* Faces are quads */
						int f;
						
						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[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,
											face, 4, dt, other) != 0 ) {
									//printf("collides with %s/%p\n", other->model->name, other);
									if ( (nx == 0.0) && (ny == 0.0) && ( nz == 1.0) ) {
										obj->vz = 0.0;
										obj->landed = 1;
									} else {
										obj->vx = -obj->vx;
										obj->vy = -obj->vy;
										obj->vz = -obj->vz;
										obj->yawspeed = 0.0;
									}
									return 1;	/* Don't waste time looking anywhere else */
							}
						}
						break;
						
					}
				
				}
			
			}
			
		}
	}

	return 0;
	
}

/* Called once for each object which isn't just "scenery" */
static void physics_process(ModelInstance *obj, Uint32 dt, Game *game) {
	
	Room *room;
	
	/* 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 */
		int j;
		for ( j=0; j<room->num_objects; j++ ) {
			if ( physics_check_collide(obj, room->objects[j], dt) ) {
				return;
			}
		}
	}
	
	/* Air friction */
	if ( obj->vx > 0.0 ) {
		obj->vx -= FRICTION * dt * obj->vx;
		if ( obj->vx < 0.0 ) obj->vx = 0.0;
	} else {
		obj->vx += FRICTION * dt * -obj->vx;
		if ( obj->vx > 0.0 ) obj->vx = 0.0;
	}
	
	if ( obj->vy > 0.0 ) {
		obj->vy -= FRICTION * dt * obj->vy;
		if ( obj->vy < 0.0 ) obj->vy = 0.0;
	} else {
		obj->vy += FRICTION * dt * -obj->vy;
		if ( obj->vy > 0.0 ) obj->vy = 0.0;
	}
	
	if ( obj->vz > 0.0 ) {
		obj->vz -= FRICTION * dt * obj->vz;
		if ( obj->vz < 0.0 ) obj->vz = 0.0;
	} else {
		obj->vz += FRICTION * dt * -obj->vz;
		if ( obj->vz > 0.0 ) obj->vz = 0.0;
	}
	
	if ( obj->yawspeed > 0.0 ) {
		obj->yawspeed -= FRICTION * dt * obj->yawspeed * TORQUE;
		if ( obj->yawspeed < 0.0 ) obj->yawspeed = 0.0;
	} else {
		obj->yawspeed += FRICTION * dt * -obj->yawspeed * TORQUE;
		if ( obj->yawspeed > 0.0 ) obj->yawspeed = 0.0;
	}
	
	/* Gravity */
	if ( (obj->attribs & OBJ_GRAVITY) && (!obj->landed) ) obj->vz -= GRAVITY * dt;
	
	/* Take a step */
	obj->x += obj->vx * dt;
	obj->y += obj->vy * dt;
	obj->z += obj->vz * dt;
	obj->yaw += obj->yawspeed * dt;
	
	if ( obj->yaw < -M_PI ) obj->yaw += 2*M_PI;
	if ( obj->yaw > M_PI ) obj->yaw -= 2*M_PI;

}

void physics_step(Game *game, Uint32 t) {

	Uint32 dt;
	
	dt = t - game->tlast;
	
	/* Handle things specific to the lander craft */
	if ( game->thrusting ) {
		if ( game->fuel > 0.0 ) {
			game->lander->vz += THRUST * dt;
			game->fuel -= 0.0002;
			game->lander->landed = 0;
		}
	}
	if ( game->forward ) {
		game->lander->vx += sinf(game->lander->yaw) * FTHRUST * dt;
		game->lander->vy += cosf(game->lander->yaw) * FTHRUST * dt;
	} else if ( game->reverse ) {
		game->lander->vx -= sinf(game->lander->yaw) * FTHRUST * dt;
		game->lander->vy -= cosf(game->lander->yaw) * FTHRUST * dt;
	}
	if ( game->turn_left ) {
		game->lander->yawspeed -= YAWTHRUST * dt;	/* -ve yaw is "left" */
	}
	if ( game->turn_right ) {
		game->lander->yawspeed += YAWTHRUST *dt;	/* +ve yaw is "right" */
	}
	
	/* The number of rooms to consider for physics purposes can probably be further reduced */
	physics_process(game->lander, dt, game);
	
	game_check_handoff(game);
	
	game->tlast = t;

}