Refactor el big #1

[convexer.git] / src / cxr_math.h

diff --git a/src/cxr_math.h b/src/cxr_math.h
index d03d8862cd68eeb241a0c5bf51709dd05f8fbee6..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 )
 {
@@ -576,3 +581,67 @@ CXR_INLINE double plane_polarity( double p[4], double a[3] )
        -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2])
        ;
 }
+
+CXR_INLINE void plane_project_point( v4f plane, v3f a, v3f d )
+{
+   v3f ref, delta;
+   v3_muls( plane, plane[3], ref );
+
+   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 );
+}