Refactor el big #1

[convexer.git] / src / cxr_math.h

diff --git a/src/cxr_math.h b/src/cxr_math.h
index 17c9902b390e3539a8a46af711f4d601727288a8..031c61e4d2f7d787dc53a0efe7c58e9660083956 100644 (file)
--- a/src/cxr_math.h
+++ b/src/cxr_math.h
@@ -24,6 +24,11 @@ CXR_INLINE int cxr_max( int a, int b )
        return a > b? a: b;
 }
 
+CXR_INLINE double cxr_clampf( double v, double a, double b )
+{
+   return cxr_minf( b, cxr_maxf( a, v ) );
+}
+
 // Convert degrees to radians
 CXR_INLINE double cxr_rad( double deg )
 {
@@ -585,3 +590,58 @@ CXR_INLINE void plane_project_point( v4f plane, v3f a, v3f d )
    v3_sub( a, ref, delta );
    v3_muladds( a, plane, -v3_dot(delta,plane), d );
 }
+
+CXR_INLINE double line_line_dist( v3f pa0, v3f pa1, v3f pb0, v3f pb1 )
+{
+   v3f va, vb, n, delta;
+   v3_sub( pa1, pa0, va );
+   v3_sub( pb1, pb0, vb );
+
+   v3_cross( va, vb, n );
+   v3_normalize( n );
+
+   v3_sub( pb0, pa0, delta );
+
+   return fabs( v3_dot( n, delta ) );
+}
+
+CXR_INLINE double segment_segment_dist( v3f a0, v3f a1, v3f b0, v3f b1, 
+   v3f a, v3f b )
+{
+   v3f r,u,v;
+   v3_sub( b0, a0, r );
+   v3_sub( a1, a0, u );
+   v3_sub( b1, b0, v );
+
+   double ru = v3_dot( r,u ),
+          rv = v3_dot( r,v ),
+          uu = v3_dot( u,u ),
+          uv = v3_dot( u,v ),
+          vv = v3_dot( v,v );
+   
+   double det = uu*vv - uv*uv,
+          s,
+          t;
+
+   if( det < 1e-6 *uu*vv )
+   {
+      s = ru/uu;
+      t = 0.0;
+   }
+   else
+   {
+      s = (ru*vv - rv*uv)/det;
+      t = (ru*uv - rv*uu)/det;
+   }
+
+   s = cxr_clampf( s, 0.0, 1.0 );
+   t = cxr_clampf( t, 0.0, 1.0 );
+
+   double S = cxr_clampf((t*uv + ru)/uu, 0.0, 1.0),
+          T = cxr_clampf((s*uv - rv)/vv, 0.0, 1.0);
+   
+   v3_muladds( a0, u, S, a );
+   v3_muladds( b0, v, T, b );
+
+   return v3_dist( a, b );
+}