X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=src%2Fvg%2Fvg_m.h;h=8e49fe55e330f4adb99d2291ae552c56fbf55274;hb=3dd767bb10e6fee9cbffeb185d1a9685810c17b5;hp=5bbb46733bbb0482d532d36d885d35ad4effecad;hpb=94b61bed97af43179b37593f4453a05d03687cbc;p=vg.git

diff --git a/src/vg/vg_m.h b/src/vg/vg_m.h
index 5bbb467..8e49fe5 100644
--- a/src/vg/vg_m.h
+++ b/src/vg/vg_m.h
@@ -45,9 +45,6 @@ static float stable_force( float current, float diff )
    return fnew;
 }
 
-#define VG_MIN( A, B ) ((A)<(B)?(A):(B))
-#define VG_MAX( A, B ) ((A)>(B)?(A):(B))
-
 static inline int vg_min( int a, int b )
 {
 	return a < b? a: b;
@@ -191,7 +188,14 @@ static inline void v2_lerp( v2f a, v2f b, float t, v2f d )
 
 static inline void v2_normalize( v2f a )
 {
-	v2_muls( a, 1.f / v2_length( a ), a );
+	v2_muls( a, 1.0f / v2_length( a ), a );
+}
+
+static void v2_normalize_clamp( v2f a )
+{
+   float l2 = v2_length2( a );
+   if( l2 > 1.0f )
+      v2_muls( a, 1.0f/sqrtf(l2), a );
 }
 
 static inline void v2_floor( v2f a, v2f b )
@@ -301,6 +305,19 @@ static inline float vg_lerpf( float a, float b, float t )
 	return a + t*(b-a);
 }
 
+static inline double vg_lerp( double a, double b, double t )
+{
+	return a + t*(b-a);
+}
+
+/* correctly lerp around circular period -pi -> pi */
+static float vg_alerpf( float a, float b, float t )
+{
+   float d = fmodf( b-a, VG_TAUf ),
+         s = fmodf( 2.0f*d, VG_TAUf ) - d;
+   return a + s*t;
+}
+
 static inline void v3_lerp( v3f a, v3f b, float t, v3f d )
 {
 	d[0] = a[0] + t*(b[0]-a[0]);
@@ -638,6 +655,45 @@ static inline void m4x3_invert_affine( m4x3f a, m4x3f b )
    v3_negate( b[3], b[3] );
 }
 
+static void m4x3_invert_full( m4x3f src, m4x3f dst )
+{
+  float t2, t4, t5,
+        det,
+        a = src[0][0], b = src[0][1], c = src[0][2],
+        e = src[1][0], f = src[1][1], g = src[1][2],
+        i = src[2][0], j = src[2][1], k = src[2][2],
+        m = src[3][0], n = src[3][1], o = src[3][2];
+
+   t2 = j*o - n*k;
+   t4 = i*o - m*k;
+   t5 = i*n - m*j;
+   
+   dst[0][0] =  f*k - g*j;
+   dst[1][0] =-(e*k - g*i);
+   dst[2][0] =  e*j - f*i;
+   dst[3][0] =-(e*t2 - f*t4 + g*t5);
+   
+   dst[0][1] =-(b*k - c*j);
+   dst[1][1] =  a*k - c*i;
+   dst[2][1] =-(a*j - b*i);
+   dst[3][1] =  a*t2 - b*t4 + c*t5;
+   
+   t2 = f*o - n*g;
+   t4 = e*o - m*g; 
+   t5 = e*n - m*f;
+   
+   dst[0][2] =  b*g - c*f ;
+   dst[1][2] =-(a*g - c*e );
+   dst[2][2] =  a*f - b*e ;
+   dst[3][2] =-(a*t2 - b*t4 + c * t5);
+
+   det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]);
+   v3_muls( dst[0], det, dst[0] );
+   v3_muls( dst[1], det, dst[1] );
+   v3_muls( dst[2], det, dst[2] );
+   v3_muls( dst[3], det, dst[3] );
+}
+
 static inline void m4x3_copy( m4x3f a, m4x3f b )
 {
 	v3_copy( a[0], b[0] );
@@ -1173,7 +1229,14 @@ static inline void q_inv( v4f q, v4f d )
 
 static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
 {
-   v4_lerp( a, b, t, d );
+   if( v4_dot(a,b) < 0.0f )
+   {
+      v4_muls( b, -1.0f, d );
+      v4_lerp( a, d, t, d );
+   }
+   else
+      v4_lerp( a, b, t, d );
+
    q_normalize( d );
 }
 
@@ -1314,4 +1377,15 @@ static inline int vg_randint(int max)
    return rand()%max;
 }
 
+static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
+{
+   float tt = t*t,
+         ttt = tt*t;
+
+   v3_muls( p1, ttt, p );
+   v3_muladds( p, h1, 3.0f*tt  -3.0f*ttt, p );
+   v3_muladds( p, h0, 3.0f*ttt -6.0f*tt  +3.0f*t, p );
+   v3_muladds( p, p0, 3.0f*tt  -ttt -3.0f*t +1.0f, p );
+}
+
 #endif /* VG_M_H */