+static int giftwrapXZ( v3f *points, int *output, int len )
+{
+ int l, p, q, count;
+
+ if( len < 3 )
+ return 0;
+
+ l = 0;
+ for( int i=1; i<len; i++ )
+ if( points[i][0] < points[l][0] )
+ l = i;
+
+ p = l;
+ count = 0;
+ do
+ {
+ if( count >= len )
+ {
+ vg_error ("MANIFOLD ERR (%d)\n", count );
+ return 0;
+ }
+ output[ count ++ ] = p;
+
+ q = (p+1)%len;
+
+ for( int i=0; i<len; i++ )
+ {
+ if( (points[i][2]-points[p][2])*(points[q][0]-points[i][0]) -
+ (points[i][0]-points[p][0])*(points[q][2]-points[i][2])
+ > 0.0001f )
+ {
+ q = i;
+ }
+ }
+ p = q;
+ }
+ while( p != l );
+
+ return count;
+}
+
+static void player_do_collision( rigidbody *rb )
+{
+ /*
+ * If point is inside box
+ * find normal (theres 8 simple pyramid regions for this, x>y/dim .. etc)
+ * find distance (same sorta thing)
+ *
+ * apply normal impulse to rotation
+ * correct position based on new penetration amount if needed
+ * apply normal impulse to velocity
+ */
+
+ v3f pfront, pback;
+ m4x3_mulv( player.to_world, (v3f){ 0.0f,0.0f,-1.0f }, pfront );
+ m4x3_mulv( player.to_world, (v3f){ 0.0f,0.0f, 1.0f }, pback );
+
+ float const kheight = 2.0f;
+
+ v3f verts[8];
+
+ v3f a, b;
+ v3_copy( rb->bbx[0], a );
+ v3_copy( rb->bbx[1], b );
+
+ m4x3f compound;
+ m4x3_mul( player.to_local, rb->to_world, compound );
+
+ m4x3_mulv( compound, (v3f){ a[0], a[1], a[2] }, verts[0] );
+ m4x3_mulv( compound, (v3f){ a[0], b[1], a[2] }, verts[1] );
+ m4x3_mulv( compound, (v3f){ b[0], b[1], a[2] }, verts[2] );
+ m4x3_mulv( compound, (v3f){ b[0], a[1], a[2] }, verts[3] );
+
+ m4x3_mulv( compound, (v3f){ a[0], a[1], b[2] }, verts[4] );
+ m4x3_mulv( compound, (v3f){ a[0], b[1], b[2] }, verts[5] );
+ m4x3_mulv( compound, (v3f){ b[0], b[1], b[2] }, verts[6] );
+ m4x3_mulv( compound, (v3f){ b[0], a[1], b[2] }, verts[7] );
+
+ int const indices[12][2] = {
+ {0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},
+ {0,4},{1,5},{2,6},{3,7}
+ };
+
+ v3f hull[12*2 + 8];
+ int hull_indices[12*2 + 8];
+ int hull_len = 0;
+
+ for( int i=0; i<vg_list_size(indices); i++ )
+ {
+ int ia = indices[i][0],
+ ib = indices[i][1];
+
+ v3f p0, p1;
+
+ float ya = verts[ia][1],
+ yb = verts[ib][1],
+ d = 1.0f/(yb-ya),
+ qa;
+
+ if( (ya-0.2f) * (yb-0.2f) < 0.0f )
+ {
+ v3_muls( verts[ia], (yb-0.2f)*d, p0 );
+ v3_muladds( p0, verts[ib], -(ya-0.2f)*d, p0 );
+
+ v3_copy( p0, hull[hull_len] );
+ hull[hull_len ++][1] = 0.2f;
+
+ m4x3_mulv( player.to_world, p0, p0 );
+ vg_line_pt3( p0, 0.1f, 0xffffff00 );
+ }
+
+ if( (ya-kheight) * (yb-kheight) < 0.0f )
+ {
+ v3_muls( verts[ia], (yb-kheight)*d, p0 );
+ v3_muladds( p0, verts[ib], -(ya-kheight)*d, p0 );
+
+ v3_copy( p0, hull[hull_len] );
+ hull[hull_len ++][1] = 0.2f;
+
+ m4x3_mulv( player.to_world, p0, p0 );
+ vg_line_pt3( p0, 0.1f, 0xff00ffff );
+ }
+ }
+ for( int i=0; i<8; i++ )
+ {
+ int ia = indices[i][0];
+ float ya = verts[ia][1];
+
+ if( ya > 0.2f && ya < kheight )
+ {
+ v3_copy( verts[ia], hull[hull_len] );
+ hull[hull_len ++][1] = 0.2f;
+ }
+ }
+
+ if( hull_len < 3 )
+ return;
+
+ int len = giftwrapXZ( hull, hull_indices, hull_len );
+ for( int i=0; i<len; i++ )
+ {
+ v3f p0, p1, p2, p3;
+ v3_copy( hull[hull_indices[i]], p0 );
+ v3_copy( hull[hull_indices[(i+1)%len]], p1 );
+ v3_add( p0, (v3f){0,kheight-0.2f,0}, p2 );
+ v3_add( p1, (v3f){0,kheight-0.2f,0}, p3 );
+
+ m4x3_mulv( player.to_world, p0, p0 );
+ m4x3_mulv( player.to_world, p1, p1 );
+ m4x3_mulv( player.to_world, p2, p2 );
+ m4x3_mulv( player.to_world, p3, p3 );
+
+ vg_line2( p0, p1, 0xff00ffff, 0xff000000 );
+ vg_line( p2, p3, 0xff00ffff );
+ vg_line( p0, p2, 0xff00ffa0 );
+ }
+
+ int collide = 1;
+ float min_dist = 99999.9f;
+ v2f normal;
+ for( int i=0; i<len; i++ )
+ {
+ v2f p0, p1;
+ p0[0] = hull[hull_indices[i]][0];
+ p0[1] = hull[hull_indices[i]][2];
+ p1[0] = hull[hull_indices[(i+1)%len]][0];
+ p1[1] = hull[hull_indices[(i+1)%len]][2];
+
+ v2f t,n, rel;
+ v2_sub( p1, p0, t );
+ n[0] = -t[1];
+ n[1] = t[0];
+ v2_normalize(n);
+
+ v2_sub( (v2f){ 0.0f, -1.0f }, p0, rel );
+ float d = -v2_dot( n, rel ) + 0.5f;
+
+ if( d < 0.0f )
+ {
+ collide = 0;
+ break;
+ }
+
+ if( d < min_dist )
+ {
+ min_dist = d;
+ v2_copy( n, normal );
+ }
+ }
+
+ if( collide )
+ {
+ v3f p0, p1;
+ p0[0] = 0.0f;
+ p0[1] = 0.2f;
+ p0[2] = -1.0f;
+
+ p1[0] = p0[0] + normal[0]*min_dist;
+ p1[1] = p0[1];
+ p1[2] = p0[2] + normal[1]*min_dist;
+
+ m4x3_mulv( player.to_world, p0, p0 );
+ m4x3_mulv( player.to_world, p1, p1 );
+
+ vg_line( p0, p1, 0xffffffff );
+
+ v2f impulse;
+ v2_muls( normal, min_dist, impulse );
+ float rotation = v2_cross( (v2f){0.0f,-1.0f}, impulse )*0.08f;
+
+ v3f vel;
+ m3x3_mulv( player.to_local, player.v, vel );
+ vel[1] = vel[2];
+
+ float vn = vg_maxf( -v2_dot( vel, normal ), 0.0f );
+ vn += -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -min_dist+0.04f );
+
+ v2_muls( normal, vn*0.03f, impulse );
+ v3f impulse_world = { impulse[0], 0.0f, impulse[1] };
+
+ m3x3_mulv( player.to_world, impulse_world, impulse_world );
+ v3_add( impulse_world, player.v, player.v );
+
+ v4f rot;
+ v3f up = {0.0f,1.0f,0.0f};
+ m3x3_mulv( player.to_world, up, up );
+ q_axis_angle( rot, up, -rotation );
+ q_mul( rot, player.rot, player.rot );
+ }
+}
+