move rigidbody to vg

[vg.git] / vg_m.h

diff --git a/vg_m.h b/vg_m.h
index f5ed4051e1bc9620db00b362186fe693cebd4587..6cbac62295773393f84749c5b62b9b871efd2c4c 100644 (file)
--- a/vg_m.h
+++ b/vg_m.h
@@ -19,9 +19,10 @@
  *    5.d Raycast & Spherecasts
  *    5.e Curves
  *    5.f Volumes
+ *    5.g Inertia tensors
  *  6. Statistics
  *    6.a Random numbers
- **/
+ */
 
 #ifndef VG_M_H
 #define VG_M_H
@@ -2270,9 +2271,135 @@ static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p )
  * -----------------------------------------------------------------------------
  */
 
-static float vg_sphere_volume( float radius ){
-   float r3 = radius*radius*radius;
-   return (4.0f/3.0f) * VG_PIf * r3;
+static f32 vg_sphere_volume( f32 r ){
+   return (4.0f/3.0f) * VG_PIf * r*r*r;
+}
+
+static f32 vg_box_volume( boxf box ){
+   v3f e;
+   v3_sub( box[1], box[0], e );
+   return e[0]*e[1]*e[2];
+}
+
+static f32 vg_cylinder_volume( f32 r, f32 h ){
+   return VG_PIf * r*r * h;
+}
+
+static f32 vg_capsule_volume( f32 r, f32 h ){
+   return vg_sphere_volume( r ) + vg_cylinder_volume( r, h-r*2.0f );
+}
+
+static void vg_sphere_bound( f32 r, boxf out_box ){
+   v3_fill( out_box[0], -r );
+   v3_fill( out_box[1],  r );
+}
+
+static void vg_capsule_bound( f32 r, f32 h, boxf out_box ){
+   v3_copy( (v3f){-r,-h*0.5f,r}, out_box[0] );
+   v3_copy( (v3f){-r, h*0.5f,r}, out_box[1] );
+}
+
+
+/*
+ * -----------------------------------------------------------------------------
+ * Section 5.g                  Inertia Tensors
+ * -----------------------------------------------------------------------------
+ */
+
+/*
+ * Translate existing inertia tensor 
+ */
+static void vg_translate_inertia( m3x3f inout_inertia, f32 mass, v3f d ){
+   /* 
+    * I = I_0 + m*[(d.d)E_3 - d(X)d]
+    *
+    * I:   updated tensor
+    * I_0: original tensor
+    * m:   scalar mass
+    * d:   translation vector 
+    * (X): outer product
+    * E_3: identity matrix
+    */
+   m3x3f t, outer, scale;
+   m3x3_diagonal( t, v3_dot(d,d) );
+   m3x3_outer_product( outer, d, d );
+   m3x3_sub( t, outer, t );
+   m3x3_diagonal( scale, mass );
+   m3x3_mul( scale, t, t );
+   m3x3_add( inout_inertia, t, inout_inertia );
+}
+
+/*
+ * Rotate existing inertia tensor 
+ */
+static void vg_rotate_inertia( m3x3f inout_inertia, m3x3f rotation ){
+   /*
+    *  I = R I_0 R^T
+    *
+    *  I:   updated tensor
+    *  I_0: original tensor
+    *  R:   rotation matrix
+    *  R^T: tranposed rotation matrix
+    */
+
+   m3x3f Rt;
+   m3x3_transpose( rotation, Rt );
+   m3x3_mul( rotation, inout_inertia, inout_inertia );
+   m3x3_mul( inout_inertia, Rt, inout_inertia );
+}
+/*
+ * Create inertia tensor for box
+ */
+static void vg_box_inertia( boxf box, f32 mass, m3x3f out_inertia ){
+   v3f e, com;
+   v3_sub( box[1], box[0], e );
+   v3_muladds( box[0], e, 0.5f, com );
+
+   f32 ex2 = e[0]*e[0],
+       ey2 = e[1]*e[1],
+       ez2 = e[2]*e[2],
+       ix  = (ey2+ez2) * mass * (1.0f/12.0f),
+       iy  = (ex2+ez2) * mass * (1.0f/12.0f),
+       iz  = (ex2+ey2) * mass * (1.0f/12.0f);
+
+   m3x3_identity( out_inertia );
+   m3x3_setdiagonalv3( out_inertia, (v3f){ ix, iy, iz } );
+   vg_translate_inertia( out_inertia, mass, com );
+}
+
+/*
+ * Create inertia tensor for sphere
+ */
+static void vg_sphere_inertia( f32 r, f32 mass, m3x3f out_inertia ){
+   f32 ixyz = r*r * mass * (2.0f/5.0f);
+   
+   m3x3_identity( out_inertia );
+   m3x3_setdiagonalv3( out_inertia, (v3f){ ixyz, ixyz, ixyz } );
+}
+
+/*
+ * Create inertia tensor for capsule
+ */
+static void vg_capsule_inertia( f32 r, f32 h, f32 mass, m3x3f out_inertia ){
+   f32 density = mass / vg_capsule_volume( r, h ),
+       ch  = h-r*2.0f, /* cylinder height */
+       cm  = VG_PIf * ch*r*r * density, /* cylinder mass */
+       hm  = VG_TAUf * (1.0f/3.0f) * r*r*r * density, /* hemisphere mass */
+       
+       iy  = r*r*cm * 0.5f,
+       ixz = iy * 0.5f + cm*ch*ch*(1.0f/12.0f),
+
+       aux0= (hm*2.0f*r*r)/5.0f;
+
+   iy += aux0 * 2.0f;
+
+   f32 aux1= ch*0.5f,
+       aux2= aux0 + hm*(aux1*aux1 + 3.0f*(1.0f/8.0f)*ch*r);
+
+   ixz += aux2*2.0f;
+
+   m3x3_identity( out_inertia );
+   m3x3_setdiagonalv3( out_inertia, (v3f){ ixz, iy, ixz } );
 }
 
 /*