+ boxf region = {{ -k_board_radius, -k_board_length, -k_board_radius },
+ { k_board_radius, k_board_length, k_board_radius }};
+ m4x3_transform_aabb( mboard, region );
+
+ u32 geo[256];
+ v3f tri[3];
+ int len = bh_select( &world.geo.bhtris, region, geo, 256 );
+
+ v3f poles[2];
+ m4x3_mulv(mboard, (v3f){0.0f,-k_board_length+k_board_radius,0.0f}, poles[0]);
+ m4x3_mulv(mboard, (v3f){0.0f, k_board_length-k_board_radius,0.0f}, poles[1]);
+
+ struct contact manifold[12];
+ int manifold_count = 0;
+
+ v3f surface_avg = {0.0f, 0.0f, 0.0f};
+
+ for( int i=0; i<len; i++ )
+ {
+ u32 *ptri = &world.geo.indices[ geo[i]*3 ];
+
+ for( int j=0; j<3; j++ )
+ v3_copy( world.geo.verts[ptri[j]].co, tri[j] );
+
+ vg_line(tri[0],tri[1],0xff00ff00 );
+ vg_line(tri[1],tri[2],0xff00ff00 );
+ vg_line(tri[2],tri[0],0xff00ff00 );
+
+ v3f co, norm;
+ float p;
+
+ for( int j=0; j<2; j++ )
+ {
+ if( sphere_vs_triangle( poles[j], k_board_radius, tri,co,norm,&p) )
+ {
+ if(manifold_count >= vg_list_size(manifold))
+ {
+ vg_error("Manifold overflow!\n");
+ break;
+ }
+
+ v3f p1;
+ v3_muladds( poles[j], norm, p, p1 );
+ vg_line( poles[j], p1, 0xffffffff );
+
+ struct contact *ct = &manifold[manifold_count ++];
+ v3_sub( co, player.co, ct->delta );
+ v3_copy( co, ct->co );
+ v3_copy( norm, ct->n );
+ ct->bias = -0.2f*k_rb_rate*vg_minf(0.0f,-p+k_board_allowance);
+ ct->norm_impulse = 0.0f;
+
+ v3_add( norm, surface_avg, surface_avg );
+ }
+ }
+ }
+
+ if( !manifold_count )
+ {
+ player_start_air();
+ }
+ else
+ {
+ v3_normalize( surface_avg );
+
+ if( v3_dot( player.v, surface_avg ) > 0.5f )
+ {
+ player_start_air();
+ }
+ else
+ player.in_air = 0;
+ }
+
+ for( int j=0; j<10; j++ )
+ {
+ for( int i=0; i<manifold_count; i++ )
+ {
+ struct contact *ct = &manifold[i];
+
+ v3f dv;
+ v3_cross( player.w, ct->delta, dv );
+ v3_add( player.v, dv, dv );
+
+ float vn = -v3_dot( dv, ct->n );
+ vn += ct->bias;
+
+ float temp = ct->norm_impulse;
+ ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
+ vn = ct->norm_impulse - temp;
+
+ v3f impulse;
+
+ v3_muls( ct->n, vn, impulse );
+ v3_add( impulse, player.v, player.v );
+
+ v3_cross( ct->delta, impulse, impulse );
+
+ /*
+ * W Impulses are limited to the Y and X axises, we don't really want
+ * roll angular velocities being included.
+ *
+ * Can also tweak the resistance of each axis here by scaling the wx,wy
+ * components.
+ */
+
+ float wy = v3_dot( player.up, impulse ),
+ wx = v3_dot( player.right, impulse );
+
+ v3_muladds( player.w, player.up, wy, player.w );
+ v3_muladds( player.w, player.right, wx, player.w );
+ }
+ }