euler stuff

[vg.git] / vg_m.h

diff --git a/vg_m.h b/vg_m.h
index 090c4873ed723ad90244e8786842d047fa808a32..46cf38c173d09111ed17fc86176707ece7249845 100644 (file)
--- a/vg_m.h
+++ b/vg_m.h
@@ -32,6 +32,7 @@
 
 #define VG_PIf  3.14159265358979323846264338327950288f
 #define VG_TAUf 6.28318530717958647692528676655900576f
+
 /*
  * -----------------------------------------------------------------------------
  * Section 0.                    Misc Operations
@@ -106,6 +107,31 @@ static inline f32 vg_rad( f32 deg )
    return deg * VG_PIf / 180.0f;
 }
 
+/* angle to reach b from a */
+static f32 vg_angle_diff( f32 a, f32 b ){
+   f32 d = fmod(b,VG_TAUf)-fmodf(a,VG_TAUf);
+   if( fabsf(d) > VG_PIf )
+      d = -vg_signf(d) * (VG_TAUf - fabsf(d));
+
+   return d;
+}
+
+/*
+ * quantize float to bit count
+ */
+static u32 vg_quantf( f32 a, u32 bits, f32 min, f32 max ){
+   u32 mask = (0x1 << bits) - 1;
+   return vg_clampf((a - min) * ((f32)mask/(max-min)), 0.0f, mask );
+}
+
+/*
+ * un-quantize discreet to float
+ */
+static f32 vg_dequantf( u32 q, u32 bits, f32 min, f32 max ){
+   u32 mask = (0x1 << bits) - 1;
+   return min + (f32)q * ((max-min) / (f32)mask);
+}
+
 /*
  * -----------------------------------------------------------------------------
  * Section 2.a                   2D Vectors
@@ -514,6 +540,34 @@ static void v3_tangent_basis( v3f n, v3f tx, v3f ty ){
    v3_cross( n, tx, ty );
 }
 
+/*
+ * Compute yaw and pitch based of a normalized vector representing forward
+ * forward: -z
+ * result -> (YAW,PITCH,0.0)
+ */
+static void v3_angles( v3f v, v3f out_angles ){
+   float yaw = atan2f( v[0], -v[2] ),
+       pitch = atan2f( 
+                   -v[1], 
+                   sqrtf(
+                     v[0]*v[0] + v[2]*v[2]
+                   )
+               );
+
+   out_angles[0] = yaw;
+   out_angles[1] = pitch;
+   out_angles[2] = 0.0f;
+}
+
+/*
+ * Compute the forward vector from (YAW,PITCH,ROLL)
+ * forward: -z
+ */
+static void v3_angles_vector( v3f angles, v3f out_v ){
+   out_v[0] =  sinf( angles[0] ) * cosf( angles[1] );
+   out_v[1] = -sinf( angles[1] );
+   out_v[2] = -cosf( angles[0] ) * cosf( angles[1] );
+}
 
 /*
  * -----------------------------------------------------------------------------
@@ -673,6 +727,10 @@ static void q_mulv( v4f q, v3f v, v3f d )
    v3_add( v1, v2, d );
 }
 
+static f32 q_dist( v4f q0, v4f q1 ){
+   return acosf( 2.0f * v4_dot(q0,q1) -1.0f );
+}
+
 /*
  * -----------------------------------------------------------------------------
  * Section 4.a                  2x2 matrices
@@ -1589,7 +1647,7 @@ static f32 ray_plane( v4f plane, v3f co, v3f dir ){
  * These closest point tests were learned from Real-Time Collision Detection by 
  * Christer Ericson 
  */
-VG_STATIC f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
+static f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
    f32 *s, f32 *t, v3f c1, v3f c2)
 {
    v3f d1,d2,r;
@@ -1666,7 +1724,7 @@ VG_STATIC f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
    return v3_length2( v0 );
 }
 
-VG_STATIC int point_inside_aabb( boxf box, v3f point )
+static int point_inside_aabb( boxf box, v3f point )
 {
    if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) &&
       (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) )
@@ -1675,13 +1733,13 @@ VG_STATIC int point_inside_aabb( boxf box, v3f point )
       return 0;
 }
 
-VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
+static void closest_point_aabb( v3f p, boxf box, v3f dest )
 {
    v3_maxv( p, box[0], dest );
    v3_minv( dest, box[1], dest );
 }
 
-VG_STATIC void closest_point_obb( v3f p, boxf box, 
+static void closest_point_obb( v3f p, boxf box, 
                                   m4x3f mtx, m4x3f inv_mtx, v3f dest )
 {
    v3f local;
@@ -1690,7 +1748,7 @@ VG_STATIC void closest_point_obb( v3f p, boxf box,
    m4x3_mulv( mtx, local, dest );
 }
 
-VG_STATIC f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+static f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
 {
    v3f v0, v1;
    v3_sub( b, a, v0 );
@@ -1702,7 +1760,7 @@ VG_STATIC f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
    return t;
 }
 
-VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
 {
    v3f ab, ac, ap;
    f32 d1, d2;
@@ -1798,7 +1856,7 @@ enum contact_type
    k_contact_type_edge
 };
 
-VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
+static enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
 {
    v3f ab, ac, ap;
    f32 d1, d2;