+ f32 u = 1.0f-t;
+
+ v3_muls( p0, u*u, p );
+ v3_muladds( p, p1, 2.0f*u*t, p );
+ v3_muladds( p, p2, t*t, p );
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * Section 5.f Volumes
+ * -----------------------------------------------------------------------------
+ */
+
+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 } );
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * Section 6.a PSRNG and some distributions
+ * -----------------------------------------------------------------------------
+ */
+
+/* An implementation of the MT19937 Algorithm for the Mersenne Twister
+ * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and
+ * T. Nishimura, "Mersenne Twister: A 623-dimensionally
+ * equidistributed uniform pseudorandom number generator," ACM
+ * Transactions on Modeling and Computer Simulation Vol. 8, No. 1,
+ * January pp.3-30 1998.
+ *
+ * http://www.sultanik.com/Mersenne_twister
+ * https://github.com/ESultanik/mtwister/blob/master/mtwister.c
+ */
+
+#define MT_UPPER_MASK 0x80000000
+#define MT_LOWER_MASK 0x7fffffff
+#define MT_TEMPERING_MASK_B 0x9d2c5680
+#define MT_TEMPERING_MASK_C 0xefc60000
+
+#define MT_STATE_VECTOR_LENGTH 624
+
+/* changes to STATE_VECTOR_LENGTH also require changes to this */
+#define MT_STATE_VECTOR_M 397
+
+typedef struct vg_rand vg_rand;
+struct vg_rand {
+ u32 mt[MT_STATE_VECTOR_LENGTH];
+ i32 index;
+};
+
+static void vg_rand_seed( vg_rand *rand, unsigned long seed ) {
+ /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator
+ * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer
+ * Programming," Vol. 2 (2nd Ed.) pp.102.
+ */
+ rand->mt[0] = seed & 0xffffffff;
+ for( rand->index=1; rand->index<MT_STATE_VECTOR_LENGTH; rand->index++){
+ rand->mt[rand->index] = (6069 * rand->mt[rand->index-1]) & 0xffffffff;
+ }
+}
+
+/*
+ * Generates a pseudo-randomly generated long.
+ */
+static u32 vg_randu32( vg_rand *rand ) {
+ u32 y;
+ /* mag[x] = x * 0x9908b0df for x = 0,1 */
+ static u32 mag[2] = {0x0, 0x9908b0df};
+ if( rand->index >= MT_STATE_VECTOR_LENGTH || rand->index < 0 ){
+ /* generate STATE_VECTOR_LENGTH words at a time */
+ int kk;
+ if( rand->index >= MT_STATE_VECTOR_LENGTH+1 || rand->index < 0 ){
+ vg_rand_seed( rand, 4357 );
+ }
+ for( kk=0; kk<MT_STATE_VECTOR_LENGTH-MT_STATE_VECTOR_M; kk++ ){
+ y = (rand->mt[kk] & MT_UPPER_MASK) |
+ (rand->mt[kk+1] & MT_LOWER_MASK);
+ rand->mt[kk] = rand->mt[kk+MT_STATE_VECTOR_M] ^ (y>>1) ^ mag[y & 0x1];
+ }
+ for( ; kk<MT_STATE_VECTOR_LENGTH-1; kk++ ){
+ y = (rand->mt[kk] & MT_UPPER_MASK) |
+ (rand->mt[kk+1] & MT_LOWER_MASK);
+ rand->mt[kk] =
+ rand->mt[ kk+(MT_STATE_VECTOR_M-MT_STATE_VECTOR_LENGTH)] ^
+ (y >> 1) ^ mag[y & 0x1];
+ }
+ y = (rand->mt[MT_STATE_VECTOR_LENGTH-1] & MT_UPPER_MASK) |
+ (rand->mt[0] & MT_LOWER_MASK);
+ rand->mt[MT_STATE_VECTOR_LENGTH-1] =
+ rand->mt[MT_STATE_VECTOR_M-1] ^ (y >> 1) ^ mag[y & 0x1];
+ rand->index = 0;
+ }
+ y = rand->mt[rand->index++];
+ y ^= (y >> 11);
+ y ^= (y << 7) & MT_TEMPERING_MASK_B;
+ y ^= (y << 15) & MT_TEMPERING_MASK_C;
+ y ^= (y >> 18);
+ return y;
+}
+
+/*
+ * Generates a pseudo-randomly generated f64 in the range [0..1].
+ */
+static inline f64 vg_randf64( vg_rand *rand ){
+ return (f64)vg_randu32(rand)/(f64)0xffffffff;
+}
+
+static inline f64 vg_randf64_range( vg_rand *rand, f64 min, f64 max ){
+ return vg_lerp( min, max, (f64)vg_randf64(rand) );
+}
+
+static inline void vg_rand_dir( vg_rand *rand, v3f dir ){
+ dir[0] = vg_randf64(rand);
+ dir[1] = vg_randf64(rand);
+ dir[2] = vg_randf64(rand);
+
+ /* warning: *could* be 0 length.
+ * very unlikely.. 1 in (2^32)^3. but its mathematically wrong. */