build system revision
[vg.git] / labs / physics.c
1 #include "vg/vg_engine.h"
2 #include "vg/vg_opt.h"
3 #include "vg/vg_audio.h"
4 #include "vg/vg_camera.h"
5 #include "vg/vg_rigidbody.h"
6 #include "vg/vg_rigidbody_collision.h"
7 #include "vg/vg_rigidbody_view.h"
8 #include "vg/vg_profiler.h"
9 #include "vg/vg_bvh.h"
10 #include "vg/vg_input.h"
11
12 #define SHAPE_MAX 256
13 static rigidbody shapes[SHAPE_MAX];
14 static rb_capsule shapes_inf[SHAPE_MAX];
15 static v4f shapes_colour[SHAPE_MAX];
16 static boxf shapes_bbx[SHAPE_MAX];
17
18 static boxf floor_box = {{-6.999,-2.001,-6.999},{6.999,-0.999,6.999}};
19
20 static rigidbody racket;
21
22 static f32 k_iterations = 8.0f,
23 k_view_x = 0.0f,
24 k_view_y = 0.2f,
25 k_view_z = 10.0f,
26 k_shapes = 32.0f,
27 k_racket_d = 8.0f;
28
29 static rb_capsule racket_ca = { .h = 4.3f, .r = 0.6f },
30 racket_cb = { .h = 5.5f, .r = 0.9f };
31 static m4x3f racket_a_mdl, racket_b_mdl;
32 static m3x3f racket_I;
33
34 static v3f k_racket_init_w;
35 static i32 k_demo = 0;
36 static i32 k_gyro = 0;
37 static i32 k_prof_normalize = 0;
38 static i32 k_bbx = 1;
39 static i32 k_spacial = 0;
40
41 static struct vg_profile prof_refit = { .name = "Refit" },
42 prof_broad = { .name = "Broad phase",
43 .mode = k_profile_mode_accum },
44 prof_narrow = { .name = "Narrow phase",
45 .mode = k_profile_mode_accum },
46 prof_solve = { .name = "Solver" };
47
48 static void shape_bvh_expand_bound( void *user, boxf bound, u32 item_index ){
49 box_concat( bound, shapes_bbx[item_index] );
50 }
51
52 static f32 shape_bvh_centroid( void *user, u32 item_index, int axis ){
53 f32 x = shapes_bbx[item_index][0][axis] + shapes_bbx[item_index][1][axis];
54 return x*0.5f;
55 }
56
57 static void shape_bvh_closest( void *user, u32 item_index,
58 v3f point, v3f closest ){
59 closest_point_aabb( point, shapes_bbx[item_index], closest );
60 }
61
62 static void shape_bvh_swap( void *user, u32 ia, u32 ib ){
63 rigidbody temp = shapes[ib];
64 shapes[ib] = shapes[ia];
65 shapes[ia] = temp;
66
67 rb_capsule cb = shapes_inf[ib];
68 shapes_inf[ib] = shapes_inf[ia];
69 shapes_inf[ia] = cb;
70
71 v4f colourb;
72 v4_copy( shapes_colour[ib], colourb );
73 v4_copy( shapes_colour[ia], shapes_colour[ib] );
74 v4_copy( colourb, shapes_colour[ia] );
75
76 boxf boxb;
77 box_copy( shapes_bbx[ib], boxb );
78 box_copy( shapes_bbx[ia], shapes_bbx[ib] );
79 box_copy( boxb, shapes_bbx[ia] );
80 }
81
82 static bh_system shape_bvh = {
83 .expand_bound = shape_bvh_expand_bound,
84 .item_centroid = shape_bvh_centroid,
85 .item_closest = shape_bvh_closest,
86 .item_swap = shape_bvh_swap
87 };
88
89 static bh_tree *shape_bvh_tree = NULL;
90
91 int main( int argc, char *argv[] ){
92 vg_mem.use_libc_malloc = 0;
93 vg_set_mem_quota( 80*1024*1024 );
94 vg_enter( argc, argv, "Voyager Game Engine" );
95 return 0;
96 }
97
98 void vg_launch_opt(void){
99 const char *arg;
100 }
101
102 void vg_preload(void)
103 {
104 vg_audio.dsp_enabled = 0;
105 }
106
107 static void init_random(void)
108 {
109 for( u32 i=0; i<(u32)k_shapes; i ++ ){
110 f32 h = vg_randf64( &vg.rand ) * 2.0f + 1.3f,
111 r = vg_randf64( &vg.rand ) * 0.5f + 0.125f,
112 pv = vg_capsule_volume( r, h ),
113 k_density = 8.0f,
114 pm = pv * k_density;
115
116 shapes_inf[i].r = r;
117 shapes_inf[i].h = h;
118
119 m3x3f pI;
120 vg_capsule_inertia( r, h, pm, pI );
121 m3x3_inv( pI, shapes[i].iI );
122 shapes[i].inv_mass = 1.0f / pm;
123
124 v3f dir;
125 vg_rand_dir( &vg.rand, dir );
126 q_axis_angle( shapes[i].q, dir, vg_randf64(&vg.rand)*VG_TAUf );
127 vg_rand_sphere( &vg.rand, shapes[i].co );
128 v3_muladds( (v3f){0,4,0}, shapes[i].co, 4.0f, shapes[i].co );
129 v3_zero( shapes[i].v );
130 v3_zero( shapes[i].w );
131 shapes_colour[i][0] = vg_randf64(&vg.rand);
132 shapes_colour[i][1] = vg_randf64(&vg.rand);
133 shapes_colour[i][2] = vg_randf64(&vg.rand);
134 shapes_colour[i][3] = 1.0f;
135 rb_update_matrices( &shapes[i] );
136 }
137 }
138
139 static void init_racket(void){
140 f32 ma = vg_capsule_volume( racket_ca.r, racket_ca.h ) * k_racket_d,
141 mb = vg_capsule_volume( racket_cb.r, racket_cb.h ) * k_racket_d,
142 mt = ma+mb;
143 m3x3f aI, bI;
144
145 /* tensor for A */
146 vg_capsule_inertia( racket_ca.r, racket_ca.h, ma, aI );
147 m4x3_identity( racket_a_mdl );
148 racket_a_mdl[3][1] = -racket_ca.h*0.5f*(mb/mt);
149 vg_translate_inertia( aI, ma, racket_a_mdl[3] );
150
151 /* tensor for B */
152 vg_capsule_inertia( racket_cb.r, racket_cb.h, mb, bI );
153
154 v4f q;
155 q_axis_angle( q, (v4f){1,0,0}, VG_TAUf*0.25f );
156
157 m4x3_identity( racket_b_mdl );
158 q_m3x3( q, racket_b_mdl );
159 vg_rotate_inertia( bI, racket_b_mdl );
160 racket_b_mdl[3][1] = racket_ca.h*0.5f*(ma/mt);
161 vg_translate_inertia( bI, mb, racket_b_mdl[3] );
162
163 m3x3_add( aI, bI, racket_I );
164 m3x3_inv( racket_I, racket.iI );
165 racket.inv_mass = 1.0f/(mb+ma);
166 }
167
168 static void reset_racket(void){
169 q_identity( racket.q );
170 v3_zero( racket.co );
171 v3_copy( k_racket_init_w, racket.w );
172 v3_zero( racket.v );
173 rb_update_matrices( &racket );
174 }
175
176 void vg_load(void)
177 {
178 vg_bake_shaders();
179 init_random();
180 shape_bvh_tree = bh_create( NULL, &shape_bvh, NULL, SHAPE_MAX, 1 );
181 init_racket();
182 reset_racket();
183 }
184
185 void vg_pre_update(void)
186 {
187 vg_console.cheats = 1;
188 vg_lines.render = 1;
189 }
190
191 static void demo0_refit(void){
192 if( k_spacial == 0 ) return;
193
194 for( u32 i=0; i<(u32)k_shapes; i ++ ){
195 f32 h = shapes_inf[i].h,
196 r = shapes_inf[i].r;
197
198 rigidbody *rb = &shapes[i];
199
200 v3f p0, p1;
201 v3_muladds( rb->to_world[3], rb->to_world[1], -h*0.5f+r, p0 );
202 v3_muladds( rb->to_world[3], rb->to_world[1], h*0.5f-r, p1 );
203
204 v3f *bbx = shapes_bbx[i];
205 v3_minv( p0, p1, bbx[0] );
206 v3_maxv( p0, p1, bbx[1] );
207 v3_muladds( bbx[0], (v3f){-1,-1,-1}, r, bbx[0] );
208 v3_muladds( bbx[1], (v3f){ 1, 1, 1}, r, bbx[1] );
209 }
210
211 if( k_spacial == 1 ) return;
212
213 if( k_spacial == 2 )
214 bh_rebuild( shape_bvh_tree, (u32)k_shapes );
215 }
216
217 static void demo0(void){
218 vg_profile_begin( &prof_refit );
219 demo0_refit();
220 vg_profile_end( &prof_refit );
221
222 static rigidbody _null,
223 _mover;
224 rb_solver_reset();
225
226 f32 t = vg.time * 0.1f * VG_TAUf;
227 v3f sphere_pos = { sinf(t)*2.0f, -1, cosf(t)*2.0f };
228 _mover.v[0] = (sinf(t+vg.time_fixed_delta)-sinf(t))*2.0f;
229 _mover.v[2] = (cosf(t+vg.time_fixed_delta)-cosf(t))*2.0f;
230
231 for( u32 i=0; i<(u32)k_shapes; i ++ ){
232 rigidbody *rbi = &shapes[i];
233 rb_capsule *infi = &shapes_inf[i];
234 v3f *bbxi = shapes_bbx[i];
235
236 if( rb_global_has_space() ){
237 rb_ct *buf = rb_global_buffer();
238 m4x3f mtx;
239 m4x3_identity( mtx );
240 u32 l = rb_capsule__box( rbi->to_world, infi,
241 mtx, mtx, floor_box, buf );
242
243 for( u32 k=0; k<l; k ++ ){
244 buf[k].rba = rbi;
245 buf[k].rbb = &_null;
246 }
247
248 rb_contact_count += l;
249 }
250 else break;
251
252 if( rb_global_has_space() ){
253 rb_ct *buf = rb_global_buffer();
254 u32 l = rb_capsule__sphere( rbi->to_world, infi, sphere_pos, 1, buf );
255
256 for( u32 k=0; k<l; k ++ ){
257 buf[k].rba = rbi;
258 buf[k].rbb = &_mover;
259 }
260
261 rb_contact_count += l;
262 }
263
264 if( k_spacial == 2 ){
265 bh_iter it;
266 bh_iter_init_box( 0, &it, bbxi );
267 i32 idx;
268
269 while(1){
270 vg_profile_begin( &prof_broad );
271 if( !bh_next( shape_bvh_tree, &it, &idx ) ){
272 vg_profile_end( &prof_broad );
273 break;
274 }
275 vg_profile_end( &prof_broad );
276
277 if( idx <= i ) continue;
278
279 vg_profile_begin( &prof_narrow );
280
281 rigidbody *rbj = &shapes[idx];
282 v3f *bbxj = shapes_bbx[idx];
283 rb_capsule *infj = &shapes_inf[idx];
284
285 if( rb_global_has_space() ){
286 rb_ct *buf = rb_global_buffer();
287 u32 l = rb_capsule__capsule( rbi->to_world, infi,
288 rbj->to_world, infj, buf );
289
290 for( u32 k=0; k<l; k ++ ){
291 buf[k].rba = rbi;
292 buf[k].rbb = rbj;
293 }
294
295 rb_contact_count += l;
296 }
297
298 vg_profile_end( &prof_narrow );
299 }
300 }
301 else {
302 if( i == ((u32)k_shapes)-1 ){
303 break;
304 }
305 for( u32 j=i+1; j<(u32)k_shapes; j ++ ){
306 rigidbody *rbj = &shapes[j];
307 v3f *bbxj = shapes_bbx[j];
308 rb_capsule *infj = &shapes_inf[j];
309
310 if( k_spacial == 1 ){
311 vg_profile_begin( &prof_broad );
312 if( !box_overlap( bbxi, bbxj ) ){
313 vg_profile_end( &prof_broad );
314 continue;
315 }
316 }
317
318 vg_profile_begin( &prof_narrow );
319 if( rb_global_has_space() ){
320 rb_ct *buf = rb_global_buffer();
321 u32 l = rb_capsule__capsule( rbi->to_world, infi,
322 rbj->to_world, infj, buf );
323
324 for( u32 k=0; k<l; k ++ ){
325 buf[k].rba = rbi;
326 buf[k].rbb = rbj;
327 }
328
329 rb_contact_count += l;
330 }
331 vg_profile_end( &prof_narrow );
332 }
333 }
334 }
335
336 vg_profile_increment( &prof_broad );
337 vg_profile_increment( &prof_narrow );
338
339 vg_profile_begin( &prof_solve );
340 rb_presolve_contacts( rb_contact_buffer,
341 vg.time_fixed_delta, rb_contact_count );
342 for( u32 i=0; i<(u32)k_iterations; i ++ )
343 rb_solve_contacts( rb_contact_buffer, rb_contact_count );
344
345 for( u32 i=0; i<(u32)k_shapes; i ++ ){
346 rigidbody *rbi = &shapes[i];
347 if( k_gyro ){
348 m3x3f I;
349 m3x3_inv( rbi->iI, I );
350 rb_solve_gyroscopic( rbi, I, vg.time_fixed_delta );
351 }
352 rb_iter( rbi );
353 rb_update_matrices( rbi );
354 }
355 vg_profile_end( &prof_solve );
356 }
357
358 static void demo1(void){
359 vg_profile_begin( &prof_refit );
360 vg_profile_end( &prof_refit );
361
362 vg_profile_increment( &prof_broad );
363 vg_profile_increment( &prof_narrow );
364
365 vg_profile_begin( &prof_solve );
366
367 v3_muladds( racket.v, (v3f){0,9.8f,0}, vg.time_fixed_delta, racket.v );
368 if( k_gyro ) rb_solve_gyroscopic( &racket, racket_I, vg.time_fixed_delta );
369 rb_iter( &racket );
370 rb_update_matrices( &racket );
371
372 vg_profile_end( &prof_solve );
373 }
374
375 void vg_fixed_update(void)
376 {
377 if( k_demo == 0 ) demo0();
378 else if( k_demo == 1 ) demo1();
379 }
380
381 void vg_post_update(void)
382 {
383 if( vg_getkey( SDLK_8 ) )
384 init_random();
385 }
386
387 void vg_framebuffer_resize( int w, int h )
388 {
389 }
390
391 static void draw_origin_axis(void){
392 vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 1.0f, 0.0f, 0.0f }, 0xffff0000 );
393 vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 1.0f, 0.0f }, 0xff00ff00 );
394 vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 0.0f, 1.0f }, 0xff0000ff );
395 }
396
397 static void render0(void){
398 f32 t = vg.time * 0.1f * VG_TAUf;
399 m4x3f mdl;
400 m4x3_identity( mdl );
401 vg_rb_view_box( mdl, floor_box, (v4f){0.8f,0.8f,0.8f,1} );
402
403 mdl[3][0] = sinf(t)*2.0f;
404 mdl[3][1] = -1;
405 mdl[3][2] = cosf(t)*2.0f;
406 vg_rb_view_sphere( mdl, 1, (v4f){0,1,0,1} );
407
408 for( u32 i=0; i<(u32)k_shapes; i ++ ){
409 rigidbody *rbi = &shapes[i];
410 rb_capsule *infi = &shapes_inf[i];
411 f32 *coli = shapes_colour[i];
412
413 v4f q;
414 v3f co;
415 rb_extrapolate( rbi, co, q );
416 q_m3x3( q, mdl );
417 v3_copy( co, mdl[3] );
418 vg_rb_view_capsule( mdl, infi->r, infi->h, coli );
419 }
420
421 if( k_spacial && k_bbx ){
422 for( u32 i=0; i<(u32)k_shapes; i ++ ){
423 vg_line_boxf( shapes_bbx[i], VG__RED );
424 }
425
426 if( k_spacial == 2 ){
427 bh_debug_trace( shape_bvh_tree, 0, (v3f){0,0,0}, VG__GREEN );
428 }
429 }
430 }
431
432 static void render1(void){
433 m4x3f mdl, mmdl;
434 v4f q;
435 rb_extrapolate( &racket, mdl[3], q );
436 q_m3x3( q, mdl );
437
438 m4x3_mul( mdl, racket_a_mdl, mmdl );
439 vg_rb_view_capsule( mmdl, racket_ca.r, racket_ca.h, (v4f){1,0,0,1} );
440
441 m4x3_mul( mdl, racket_b_mdl, mmdl );
442 vg_rb_view_capsule( mmdl, racket_cb.r, racket_cb.h, (v4f){0,1,0,1} );
443 }
444
445 void vg_render(void)
446 {
447 glBindFramebuffer( GL_FRAMEBUFFER, 0 );
448 glViewport( 0,0, vg.window_x, vg.window_y );
449 glEnable( GL_DEPTH_TEST );
450 glDisable( GL_BLEND );
451
452 glClearColor( 0.05f, 0.05f, 0.05f, 1.0f );
453 glClear( GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT );
454
455 vg_camera cam = {
456 .angles = { -k_view_x, k_view_y, 0 },
457 .nearz = 0.01f,
458 .farz = 500.0f,
459 .fov = 90.0f,
460 .pos = { sinf(k_view_x)*k_view_z*cosf(k_view_y),
461 sinf(k_view_y)*k_view_z,
462 cosf(k_view_x)*k_view_z*cosf(k_view_y) },
463 };
464
465 vg_camera_update_transform( &cam );
466 vg_camera_update_view( &cam );
467 vg_camera_update_projection( &cam );
468 vg_camera_finalize( &cam );
469 m4x4_copy( cam.mtx.pv, vg.pv );
470
471 vg_rb_view_bind();
472 if( k_demo == 0 ) render0();
473 else if( k_demo == 1 ) render1();
474
475 draw_origin_axis();
476 vg_lines_drawall();
477
478 glDisable(GL_DEPTH_TEST);
479 }
480
481 struct ui_enum_opt spacial_mode_ui_enum[] = {
482 { 0, "None" },
483 { 1, "BBX" },
484 { 2, "BVH - Full rebuild" },
485 { 3, "BVH - Temporal fitting" }
486 };
487
488 static void gui0( ui_rect panel ){
489 ui_slider( panel, "Shapes", 2, vg_list_size(shapes), &k_shapes, "%.0f" );
490
491 if( ui_button( panel, "randomize" ) == k_ui_button_click ){
492 init_random();
493 }
494 }
495
496 static void gui1( ui_rect panel ){
497 ui_rect l, r;
498 ui_standard_widget( panel, l, 1 );
499 ui_split_ratio( l, k_ui_axis_v, 0.5f, 4, l, r );
500 ui_slider( l, "aH", 1.0f, 10.0f, &racket_ca.h, "%.1f" );
501 ui_slider( r, "aR", 0.1f, 10.0f, &racket_ca.r, "%.1f" );
502
503 ui_standard_widget( panel, l, 1 );
504 ui_split_ratio( l, k_ui_axis_v, 0.5f, 4, l, r );
505 ui_slider( l, "bH", 1.0f, 10.0f, &racket_cb.h, "%.1f" );
506 ui_slider( r, "bR", 0.1f, 10.0f, &racket_cb.r, "%.1f" );
507
508 for( u32 i=0; i<3; i ++ ){
509 ui_rect v0,v1,v2;
510 ui_standard_widget( panel, v0, 1 );
511 ui_split_ratio( v0, k_ui_axis_v, 2.0f/3.0f, 4, v0, v2 );
512 ui_split_ratio( v0, k_ui_axis_v, 1.0f/2.0f, 4, v0, v1 );
513
514 char buf[16];
515 snprintf( buf, sizeof(buf), "%.3f", racket_I[i][0] );
516 ui_text( v0, buf, 1, k_ui_align_middle_center, 0 );
517 snprintf( buf, sizeof(buf), "%.3f", racket_I[i][1] );
518 ui_text( v1, buf, 1, k_ui_align_middle_center, 0 );
519 snprintf( buf, sizeof(buf), "%.3f", racket_I[i][2] );
520 ui_text( v2, buf, 1, k_ui_align_middle_center, 0 );
521 }
522
523 init_racket();
524
525 ui_info( panel, "init conditions" );
526 ui_rect v0,v1,v2;
527 ui_standard_widget( panel, v0, 1 );
528 ui_split_ratio( v0, k_ui_axis_v, 2.0f/3.0f, 4, v0, v2 );
529 ui_split_ratio( v0, k_ui_axis_v, 1.0f/2.0f, 4, v0, v1 );
530 ui_slider( v0, "X", 0.01f, 30.0f, k_racket_init_w+0, "%.1f" );
531 ui_slider( v1, "Y", 0.01f, 30.0f, k_racket_init_w+1, "%.1f" );
532 ui_slider( v2, "Z", 0.01f, 30.0f, k_racket_init_w+2, "%.1f" );
533
534 if( ui_button( panel, "init" ) == k_ui_button_click ){
535 reset_racket();
536 }
537 }
538
539 void vg_gui(void)
540 {
541 vg_ui.wants_mouse = 1;
542 ui_rect panel = { vg.window_x-300, 0, 300, vg.window_y };
543 ui_rect_pad( panel, (ui_px[2]){ 8, 8 } );
544
545 ui_rect box;
546 ui_split( panel, k_ui_axis_h, VG_PROFILE_SAMPLE_COUNT*2 +4, 8, box, panel );
547 vg_profile_drawn( (struct vg_profile *[]){ &prof_refit,
548 &prof_broad,
549 &prof_narrow,
550 &prof_solve }, 4,
551 vg.time_fixed_delta*1000.0, box, 0, k_prof_normalize );
552
553 ui_split( panel, k_ui_axis_h, 14*2+8, 4, box, panel );
554 ui_checkbox( panel, "Normalize", &k_prof_normalize );
555
556 ui_slider( panel, "Iterations", 1.0f, 20.0f, &k_iterations, "%.0f" );
557 ui_enum( panel, "Spacial Type", spacial_mode_ui_enum,
558 vg_list_size(spacial_mode_ui_enum), &k_spacial );
559
560 static f32 rate = 60.0f;
561 ui_slider( panel, "Fixed timestep", 10, 200, &rate, "%.1f" );
562 vg.time_fixed_delta = 1.0f/rate;
563
564 ui_checkbox( panel, "Show BBX", &k_bbx );
565 ui_checkbox( panel, "Gyroscopic Term", &k_gyro );
566
567 ui_tabs( panel, panel,
568 (const char *[]){ "collision", "racket" }, 2, &k_demo );
569
570 if( k_demo == 0 ) gui0( panel );
571 else if( k_demo == 1 ) gui1( panel );
572
573 ui_rect viewport = { 0,0, vg.window_x-300, vg.window_y };
574
575 if( ui_inside_rect( viewport, vg_ui.mouse ) && ui_clicking(UI_MOUSE_LEFT) ){
576 k_view_x += -((f32)vg.mouse_delta[0] / (f32)vg.window_x) * VG_TAUf,
577 k_view_y += ((f32)vg.mouse_delta[1] / (f32)vg.window_y) * VG_PIf;
578 }
579 }