- float scale = k_inertia_scale;
- float ex2 = scale*extent[0]*extent[0],
- ey2 = scale*extent[1]*extent[1],
- ez2 = scale*extent[2]*extent[2];
-
- obj->rb.I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
- obj->rb.I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
- obj->rb.I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
-
- m3x3_identity( obj->rb.iI );
- obj->rb.iI[0][0] = obj->rb.I[0];
- obj->rb.iI[1][1] = obj->rb.I[1];
- obj->rb.iI[2][2] = obj->rb.I[2];
- m3x3_inv( obj->rb.iI, obj->rb.iI );
+ f32 ex2 = extent[0]*extent[0],
+ ey2 = extent[1]*extent[1],
+ ez2 = extent[2]*extent[2];
+
+ /* compute inertia tensor */
+ v3f I;
+
+ if( obj->type == k_rb_shape_box ){
+ I[0] = inertia_scale * (ey2+ez2) * mass * (1.0f/12.0f);
+ I[1] = inertia_scale * (ex2+ez2) * mass * (1.0f/12.0f);
+ I[2] = inertia_scale * (ex2+ey2) * mass * (1.0f/12.0f);
+ }
+ else if( obj->type == k_rb_shape_sphere ){
+ f32 r = obj->inf.sphere.radius;
+ v3_fill( I, inertia_scale * r*r * mass * (2.0f/5.0f) );
+ }
+ else if( obj->type == k_rb_shape_capsule ){
+ f32 r = obj->inf.capsule.radius;
+ I[1] = inertia_scale * r*r * mass * (2.0f/5.0f);
+ I[0] = inertia_scale * (ey2+ez2) * mass * (1.0f/12.0f);
+ I[2] = inertia_scale * (ey2+ex2) * mass * (1.0f/12.0f);
+ }
+ else {
+ vg_fatal_error( "" );
+ }
+
+ m3x3f i;
+ m3x3_identity( i );
+ m3x3_setdiagonalv3( i, I );
+
+ /* compute translation */
+ m3x3f i_t, i_t_outer, i_t_scale;
+ m3x3_diagonal( i_t, v3_dot(com,com) );
+ m3x3_outer_product( i_t_outer, com, com );
+ m3x3_sub( i_t, i_t_outer, i_t );
+ m3x3_diagonal( i_t_scale, mass );
+ m3x3_mul( i_t_scale, i_t, i_t );
+
+ /* TODO: compute rotation */
+
+ /* add Ic and Ict */
+ m3x3_add( i, i_t, i );
+
+ /* store as inverted */
+ m3x3_inv( i, obj->rb.iI );