seperation of body initialization, glider model

[carveJwlIkooP6JGAAIwe30JlM.git] / player_glide.c

diff --git a/player_glide.c b/player_glide.c
index 3ec1fac58ad9266c0e697d7fb716fb60f1dec208..3091e57e2173c123c37206ad8eb6a10492102046 100644 (file)
--- a/player_glide.c
+++ b/player_glide.c
@@ -60,8 +60,7 @@ static void calculate_drag( v3f vl, f32 cd, v3f out_force ){
 }
 
 static void player_glide_update(void){
-   rigidbody *rb = &localplayer.rb;
-   vg_line_sphere( rb->to_world, 1.0f, 0 );
+   rigidbody *rb = &player_glide.rb;
 
    v2f steer;
    joystick_state( k_srjoystick_steer, steer );
@@ -116,11 +115,68 @@ static void player_glide_update(void){
    m3x3_mulv( rb->to_world, Fw, Fw );
    v3_muladds( rb->w, Fw, k_rb_delta, rb->w );
 
+   /* 
+    * collisions & constraints
+    */
+   world_instance *world = world_current_instance();
+   rb_solver_reset();
+
+   rigidbody _null = {0};
+   _null.inv_mass = 0.0f;
+   m3x3_zero( _null.iI );
+   for( u32 i=0; i < vg_list_size(player_glide.parts); i ++ ){
+      m4x3f mmdl;
+      m4x3_mul( rb->to_world, player_glide.parts[i].mdl, mmdl );
+
+      if( player_glide.parts[i].shape == k_rb_shape_capsule ){
+         vg_line_capsule( mmdl,
+                          player_glide.parts[i].inf.r,
+                          player_glide.parts[i].inf.h,
+                          VG__BLACK );
+      }
+      else if( player_glide.parts[i].shape == k_rb_shape_sphere ){
+         vg_line_sphere( mmdl, player_glide.parts[i].r, 0 );
+      }
+
+      if( rb_global_has_space() ){
+         rb_ct *buf = rb_global_buffer();
+
+         u32 l = 0;
+         
+         if( player_glide.parts[i].shape == k_rb_shape_capsule ){
+            l = rb_capsule__scene( mmdl, &player_glide.parts[i].inf,
+                                   NULL, world->geo_bh, buf, 
+                                   k_material_flag_ghosts );
+         }
+         else if( player_glide.parts[i].shape == k_rb_shape_sphere ){
+            l = rb_sphere__scene( mmdl, player_glide.parts[i].r,
+                                  NULL, world->geo_bh, buf,
+                                  k_material_flag_ghosts );
+         }
+
+         for( u32 j=0; j<l; j ++ ){
+            buf[j].rba = rb;
+            buf[j].rbb = &_null;
+         }
+
+         rb_contact_count += l;
+      }
+   }
+
+   rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+   for( u32 i=0; i<10; i ++ )
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+
    rb_iter( rb );
-   rb_update_transform( rb );
+   rb_update_matrices( rb );
 }
 
 static void player_glide_post_update(void){
+   v3_copy( player_glide.rb.co, localplayer.rb.co );
+   v4_copy( player_glide.rb.q, localplayer.rb.q );
+   v3_copy( player_glide.rb.v, localplayer.rb.v );
+   v3_copy( player_glide.rb.w, localplayer.rb.w );
+   rb_update_matrices( &localplayer.rb );
 }
 
 static void player_glide_animate(void){
@@ -180,6 +236,58 @@ static void player_glide_bind(void){
    VG_VAR_I32( k_glide_pause, flags=mask );
    VG_VAR_F32( k_glide_balance, flags=mask );
    VG_VAR_F32( k_glide_wing_orient, flags=mask );
+
+   f32 mass = 0.0f,
+       k_density = 8.0f,
+       k_inertia_scale = 1.0f;
+   m3x3f I;
+   m3x3_zero( I );
+
+   for( u32 i=0; i<vg_list_size(player_glide.parts); i ++ ){
+      /* create part transform matrix */
+      v4f qp, qy, qr, q;
+      q_axis_angle( qp, (v3f){1,0,0}, player_glide.parts[i].euler[0] );
+      q_axis_angle( qy, (v3f){0,1,0}, player_glide.parts[i].euler[1] );
+      q_axis_angle( qr, (v3f){0,0,1}, player_glide.parts[i].euler[2] );
+
+      q_mul( qr, qy, q );
+      q_mul( q, qp, q );
+
+      q_m3x3( q, player_glide.parts[i].mdl );
+      v3_copy( player_glide.parts[i].co, player_glide.parts[i].mdl[3] );
+
+      /* add it to inertia model */
+
+      if( player_glide.parts[i].shape == k_rb_shape_capsule ){
+         f32 r  = player_glide.parts[i].inf.r,
+             h  = player_glide.parts[i].inf.h,
+             pv = vg_capsule_volume( r, h ),
+             pm = pv * k_density;
+
+         mass += pm;
+
+         m3x3f pI;
+         rb_capsule_inertia( r, h, pm, pI );
+         rb_rotate_inertia( pI, player_glide.parts[i].mdl );
+         rb_translate_inertia( pI, pm, player_glide.parts[i].co );
+         m3x3_add( I, pI, I );
+      }
+      else if( player_glide.parts[i].shape == k_rb_shape_sphere ){
+         f32 r  = player_glide.parts[i].r,
+             pv = vg_sphere_volume( r ),
+             pm = pv * k_density;
+
+         mass += pm;
+         m3x3f pI;
+         rb_sphere_inertia( r, pm, pI );
+         rb_translate_inertia( pI, pm, player_glide.parts[i].co );
+         m3x3_add( I, pI, I );
+      }
+   }
+
+   /* set inverses */
+   m3x3_inv( I, player_glide.rb.iI );
+   player_glide.rb.inv_mass = 1.0f / mass;
 }
 
 #endif /* PLAYER_GLIDE_C */