int physics_move_walkable(vec *pos, vec *vel, float dt, float size[2][3])
{
int result=0;
int ix,iy,iz;
float x = pos->x;
float y = pos->y;
float z = pos->z;
float dx = vel->x * dt;
float dy = vel->y * dt;
float dz = vel->z * dt;
collision_geometry cg;
ix = (int) floor(x);
iy = (int) floor(y);
iz = (int) floor(z + size[0][2] + (size[1][2] - size[0][2])/2);
gather_collision_geometry(&cg, ix - COLLIDE_BLOB_X/2, iy - COLLIDE_BLOB_Y/2, iz - COLLIDE_BLOB_Z/2);
if (!collision_test_box(&cg, x, y, z - 2*Z_EPSILON, size)) {
// falling
if (collision_test_box(&cg, x+dx, y+dy, z+dz, size)) {
result = 0;
if (!collision_test_box(&cg, x+dx, y+0, z+dz, size)) {
x += dx;
vel->y = 0;
z += dz;
} else if (!collision_test_box(&cg, x+0, y+dy, z+dz, size)) {
vel->x = 0;
y += dy;
z += dz;
} else if (!collision_test_box(&cg, x+0, y+0, z+dz, size)) {
vel->x = 0;
vel->y = 0;
z += dz;
} else {
// move bottom to floor of current voxel
float min_z = z + dz;
assert(!collision_test_box(&cg, x,y,z,size));
z = (float) floor(z + size[0][2]) - size[0][2];
assert(!collision_test_box(&cg, x,y,z,size));
while (!collision_test_box(&cg, x,y,z-1,size))
z -= 1;
assert(z >= min_z - Z_EPSILON);
vel->z = 0;
result = 1;
}
} else {
result = 0;
x += dx;
y += dy;
z += dz;
}
} else {
result = 1;
if (collision_test_box(&cg, x+dx,y+dy,z, size)) {
// step up?
if (!collision_test_box(&cg, x+dx,y+dy,z+1, size)) {
x += dx;
y += dy;
z += 1;
} else {
vel->x = 0;
vel->y = 0;
}
} else {
x += dx;
y += dy;
}
}
pos->x = x;
pos->y = y;
pos->z = z;
return result;
}
void player_physics(objid oid, player_controls *con, float dt)
{
int i;
object *o = &obj[oid];
float thrust[3] = { 0,0,0 };
float world_thrust[3];
// choose direction to apply thrust
thrust[0] = (con->buttons & 3)== 1 ? EFFECTIVE_ACCEL : (con->buttons & 3)== 2 ? -EFFECTIVE_ACCEL : 0;
thrust[1] = (con->buttons & 12)== 4 ? EFFECTIVE_ACCEL : (con->buttons & 12)== 8 ? -EFFECTIVE_ACCEL : 0;
thrust[2] = (con->buttons & 48)==16 ? EFFECTIVE_ACCEL : (con->buttons & 48)==32 ? -EFFECTIVE_ACCEL : 0;
// @TODO clamp thrust[0] & thrust[1] vector length to EFFECTIVE_ACCEL
objspace_to_worldspace(world_thrust, oid, thrust[0], thrust[1], 0);
world_thrust[2] += thrust[2];
for (i=0; i < 3; ++i) {
float acc = world_thrust[i];
(&o->velocity.x)[i] += acc*dt;
}
if (o->velocity.x || o->velocity.y || o->velocity.z)
{
float vel = (float) sqrt(square(o->velocity.x)+square(o->velocity.y)+square(o->velocity.z));
float newvel = vel;
float dec = STATIC_FRICTION + DYNAMIC_FRICTION*vel;
newvel = vel - dec*dt;
if (newvel < 0)
newvel = 0;
o->velocity.x *= newvel/vel;
o->velocity.y *= newvel/vel;
o->velocity.z *= newvel/vel;
}
{
float x,y,z;
x = o->position.x + o->velocity.x * dt;
y = o->position.y + o->velocity.y * dt;
z = o->position.z + o->velocity.z * dt;
if (!con->flying) {
if (!physics_move_walkable(&o->position, &o->velocity, dt, camera_bounds))
o->velocity.z -= 20.0f * dt;
} else {
o->position.x = x;
o->position.y = y;
o->position.z = z;
}
#if 0
if (!collision_test_box(x,y,z,camera_bounds)) {
camloc[0] = x;
camloc[1] = y;
camloc[2] = z;
} else {
cam_vel[0] = 0;
cam_vel[1] = 0;
cam_vel[2] = 0;
}
#endif
}
}
