+#ifndef PHYSICS_TEST_H
+#define PHYSICS_TEST_H
+
+#include "rigidbody.h"
+#include "player.h"
+
+rigidbody ground = { .type = k_rb_shape_box,
+ .bbx = {{-100.0f,-1.0f,-100.0f},{100.0f,0.0f,100.0f}},
+ .co = {0.0f, 0.0f, 0.0f},
+ .q = {0.0f,0.0f,0.0f,1.0f},
+ .is_world = 1 };
+rigidbody funnel[4] = {
+ {
+ .type = k_rb_shape_box,
+ .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}},
+ .co = {-10.0f,5.0f,0.0f},
+ .is_world = 1
+ },
+ {
+ .type = k_rb_shape_box,
+ .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}},
+ .co = { 10.0f,5.0f,0.0f},
+ .is_world = 1
+ },
+ {
+ .type = k_rb_shape_box,
+ .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}},
+ .co = { 0.0f,5.0f,10.0f},
+ .is_world = 1
+ },
+ {
+ .type = k_rb_shape_box,
+ .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}},
+ .co = {0.0f,5.0f,-10.0f},
+ .is_world = 1
+ }
+};
+
+
+rigidbody ball = { .type = k_rb_shape_sphere,
+ .inf.sphere = { .radius = 2.0f },
+ .co = {0.0f,6.0f,0.0f},
+ .q = {0.0f,0.0f,0.0f,1.0f}},
+
+ ball1= { .type = k_rb_shape_sphere,
+ .inf.sphere = { .radius = 1.0f },
+ .co = {0.1f,9.0f,0.2f},
+ .q = {0.0f,0.0f,0.0f,1.0f}};
+
+static void physics_test_start(void)
+{
+ q_axis_angle( funnel[0].q, (v3f){1.0f,0.0f,0.0f}, 0.3f );
+ q_axis_angle( funnel[1].q, (v3f){1.0f,0.0f,0.0f}, -0.3f );
+ q_axis_angle( funnel[2].q, (v3f){0.0f,0.0f,1.0f}, 0.3f );
+ q_axis_angle( funnel[3].q, (v3f){0.0f,0.0f,1.0f}, -0.3f );
+
+ for( int i=0; i<4; i++ )
+ rb_init( &funnel[i] );
+
+ rb_init( &ground );
+ rb_init( &ball );
+ rb_init( &ball1 );
+}
+
+static void physics_test_update(void)
+{
+ player_freecam();
+ player_camera_update();
+
+ {
+
+ rb_iter( &ball );
+ rb_iter( &ball1 );
+
+ rb_solver_reset();
+
+ for( int i=0; i<4; i++ )
+ {
+ rb_contact_count += rb_sphere_vs_box( &ball, &funnel[i], rb_global_ct());
+ rb_contact_count += rb_sphere_vs_box( &ball1, &funnel[i], rb_global_ct());
+ }
+
+ rb_contact_count += rb_sphere_vs_box( &ball, &ground, rb_global_ct() );
+ rb_contact_count += rb_sphere_vs_box( &ball1, &ground, rb_global_ct() );
+ rb_contact_count += rb_sphere_vs_sphere( &ball, &ball1, rb_global_ct() );
+
+ rb_presolve_contacts();
+
+ for( int i=0; i<5; i++ )
+ rb_solve_contacts();
+
+ rb_update_transform( &ball );
+ rb_update_transform( &ball1 );
+
+ }
+
+ if(glfwGetKey( vg_window, GLFW_KEY_L ))
+ {
+ v3_copy( player.camera_pos, ball.co );
+ v3_zero( ball.v );
+ v3_zero( ball.w );
+ }
+
+ for( int i=0; i<4; i++ )
+ rb_debug( &funnel[i], 0xff0060e0 );
+ rb_debug( &ground, 0xff00ff00 );
+ rb_debug( &ball, 0xffe00040 );
+ rb_debug( &ball1, 0xff00e050 );
+}
+
+static void physics_test_render(void)
+{
+ m4x4f world_4x4;
+ m4x3_expand( player.camera_inverse, world_4x4 );
+
+ gpipeline.fov = freecam? 60.0f: 135.0f; /* 120 */
+ m4x4_projection( vg_pv, gpipeline.fov,
+ (float)vg_window_x / (float)vg_window_y,
+ 0.1f, 2100.0f );
+
+ m4x4_mul( vg_pv, world_4x4, vg_pv );
+ glEnable( GL_DEPTH_TEST );
+
+ glDisable( GL_DEPTH_TEST );
+ vg_lines_drawall( (float *)vg_pv );
+}
+
+#endif /* PHYSICS_TEST_H */