diff --git a/GeomPredicates-SSE.cabal b/GeomPredicates-SSE.cabal new file mode 100644 --- /dev/null +++ b/GeomPredicates-SSE.cabal @@ -0,0 +1,27 @@ +name: GeomPredicates-SSE +version: 0.2 +synopsis: Geometric predicates (Intel SSE) +description: Exact, hardware based computation of geometric predicates using an SSE based interval filter and the ESSA algorithm. + See \"Exact computation of Voronoi diagram and Delaunay triangulation\" by Marina Gavrilova, Helmut Ratschek and Jon Rokne. + This package is a specialization of the @GeomPredicates@ package and uses it's primitives defined under @Numeric.Geometric.Primitives@. + This package requires a CPU with @Streaming SIMD Extensions 2@. +category: Math +license: BSD3 +license-file: LICENSE +author: Neal Alexander +maintainer: NHAlxr@gmail.com +Build-Type: Simple +Cabal-Version: >=1.6 + +Library + Build-Depends: base >= 3 && < 5, GeomPredicates + Exposed-modules: Numeric.Geometric.Predicates.Interval, Numeric.Geometric.Predicates.ESSA + ghc-options: -Wall + c-sources: Numeric/Geometric/Predicates/ESSA/ESSAPrimitives.c, Numeric/Geometric/Predicates/Interval/IntervalFilterPrimitives.c + cc-options: -msse2 -Wall + +source-repository head + type: darcs + location: http://code.haskell.org/~hexpuem/GeomPredicates-SSE + + diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -0,0 +1,25 @@ +Copyright (c) 2010, Neal Alexander +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/Numeric/Geometric/Predicates/ESSA.hs b/Numeric/Geometric/Predicates/ESSA.hs new file mode 100644 --- /dev/null +++ b/Numeric/Geometric/Predicates/ESSA.hs @@ -0,0 +1,178 @@ +{-# LANGUAGE UnicodeSyntax, ForeignFunctionInterface #-} +{-# CFILES Numeric/Geometric/Predicates/ESSA/ESSAPrimitives.c #-} + +{- | Hardware based, exact computation using the ESSA algorithm in double precision (1) + + * We're using Float inputs on Double precision ESSA at the moment. Hopefully later we can add support for Double inputs on Quadruple ESSA + + * Line intersection is based on the algorithm presented in (4) + + * ccw and incircle based on (5) + + * See (2) and (3) for more information on the splitDouble operation + + * We assume realToFrac is broken and that CFloat == Float and CDouble == Double + + (1) Helmut Ratschek, Jon Rokne. \"Exact computation of the sign of a finite sum\". Applied Mathematics and Computation, Volume 99, Issue 2-3, Pages: 99-127, ISSN:0096-3003, 1999. + + (2) Siegfried M. Rump. \"High precision evaluation of nonlinear functions\" + + (3) T.J. Dekker. \"A Floating-Point Technique for Extending the Available Precision\". Numerische Mathematik, 18:224-242, 1971. + + (4) Marina Gavrilova, Jon Rokne. \"Reliable line segment intersection testing\" + + (5) Marina Gavrilova, Helmut Ratschek and Jon Rokne. \"Exact computation of Voronoi diagram and Delaunay triangulation\" +-} + +module Numeric.Geometric.Predicates.ESSA (cinttESSA, intersectESSA_SS2D, ccwESSA, incircleESSA, essa, splitDouble) where +import Numeric.Geometric.Primitives + +import Data.Foldable (toList,Foldable) +import Control.Applicative +import Foreign.Ptr +import Foreign.Marshal.Alloc +import Foreign.Storable +import Foreign.Marshal.Array +import Foreign.C.Types +import System.IO.Unsafe + + +foreign import ccall unsafe ccw_essa ∷ Float → Float → Float → Float → Float → Float → IO CInt +foreign import ccall unsafe incircle_essa ∷ Float → Float → Float → Float → Float → Float → Float → Float → IO CInt +foreign import ccall unsafe intersect2D_essa ∷ Float → Float → Float → Float → Float → Float → Float → Float → Ptr CInt → Ptr CInt → IO CInt +foreign import ccall unsafe cintt_essa ∷ Float → Float → Float → IO CInt + +foreign import ccall unsafe split_double ∷ Double → Ptr Double → Ptr Double → IO () +foreign import ccall unsafe essa_double ∷ Ptr Double → CSize → IO CInt + +-- | Test if p3 is within the closed interval specified by [p1,p2] + +cinttESSA ∷ Float → Float → Float → Bool +cinttESSA lo hi p = (unsafePerformIO $ cintt_essa (cast lo) (cast hi) (cast p)) /= 0 + +-- | Intersect two line segments + +intersectESSA_SS2D ∷ LineSegment (Vector2 Float) → LineSegment (Vector2 Float) → LineIntersection +intersectESSA_SS2D (a,b) (c,d) = unsafePerformIO $ + alloca (\ip1p → + alloca (\ip2p → do + x ← intersect2D_essa xi yi xj yj xk yk xl yl ip1p ip2p + case x of + 0 → return NINP + 1 → return Coincident + 2 → return Parallel + 3 → do + ip1 ← ip <$> peek ip1p + ip2 ← ip <$> peek ip2p + return (Intersecting (ip1,ip2)) + + _ → error "intersectESSA_SS2D: unexpected result from FFI" + )) + + + where + ip 0 = Endpoint0 + ip 1 = Endpoint1 + ip 2 = Between + ip _ = error "intersectESSA_SS2D: unexpected intersection result from FFI" + + (xi,yi) = castVector a + (xj,yj) = castVector b + (xk,yk) = castVector c + (xl,yl) = castVector d + + + +{- | Counter-clockwise orientation test. Classifies p3 in relation to the line formed by p1 and p2. + + Result: LT=Right, GT=Left, EQ=Coincident +-} + +ccwESSA ∷ Vector2 Float → Vector2 Float → Vector2 Float → Ordering +ccwESSA p1 p2 p3 = compare (unsafePerformIO $ ccw_essa x1 y1 x2 y2 x3 y3) 0 + where + (x1,y1) = castVector p1 + (x2,y2) = castVector p2 + (x3,y3) = castVector p3 + + + +{- | Test the relation of a point to the circle formed by (p1..p3). (p1..p3) must be in counterclockwise order. + + Result: GT=inside, EQ=border, LT=outside. + + Note: this is the sum of 192 multiplications. +-} + +incircleESSA ∷ (Vector2 Float, Vector2 Float, Vector2 Float) → Vector2 Float → Ordering +incircleESSA (a,b,c) d = compare (unsafePerformIO (incircle_essa xi yi xj yj xk yk xl yl)) 0 + + where + (xi,yi) = castVector a + (xj,yj) = castVector b + (xk,yk) = castVector c + (xl,yl) = castVector d + + +{- | Compute the exact sign of the sum of the input sequence. + It is the caller's responsibility to ensure that the inputs have not suffered a loss of precision. +-} + +essa ∷ (Functor t, Foldable t) => t Double → Ordering +essa = doubleESSA . fmap realToFrac -- maybe support 128bit quad later + + + +{- | Split a 53 bit double into two 26 bit halves so that: @ let (lo,hi) = splitDouble x in x == lo + hi @ + + The trick is that the sign is used as the additional bit. + + Note that the multiplication of two 26-bit floating point numbers is exact in double precision. + + If you're new to this function you may want to read paper (5), as using this function properly may be trickier than it seems. +-} + +splitDouble ∷ Double → (Double,Double) +splitDouble a = unsafePerformIO (alloca (\xp → + alloca (\yp → do + split_double a xp yp + x ← peek xp + y ← peek yp + return (x,y)))) + + + + +{-- + +foreign import ccall unsafe slope_essa ∷ Double → Double → Double → Double → IO CInt + +slopeESSA ∷ LineSegment (Vector2 Float) → Slope +slopeESSA (a,b) = case unsafePerformIO $ slope_essa x1 y1 x2 y2 of + 0 → ZeroSlope + 1 → UndefinedSlope + 2 → NegativeSlope + 3 → PositiveSlope + + where + (x1,y1) = castVector a + (x2,y2) = castVector b + +--} + +---------------------------------------------------- + +-- At the time realToFrac couldn't be trusted to do the right thing +cast ∷ Float → Float +cast = id + +castVector ∷ Vector2 Float → Vector2 Float +castVector = id + +doubleESSA ∷ (Functor a, Foldable a) => a Double → Ordering +doubleESSA xs = compare v 0 + + where + v = unsafePerformIO $ withArrayLen (toList xs) f + f = (\i p → essa_double p (fromIntegral i)) + diff --git a/Numeric/Geometric/Predicates/ESSA/ESSAPrimitives.c b/Numeric/Geometric/Predicates/ESSA/ESSAPrimitives.c new file mode 100644 --- /dev/null +++ b/Numeric/Geometric/Predicates/ESSA/ESSAPrimitives.c @@ -0,0 +1,476 @@ +#include +#include +#include +#include +#include + +#include "List.h" +#include "ESSAPrimitives.h" +/* +//////////////////// IMPORTANT NOTE //////////////////// + + Most of these functions assume that the inputs were *floats* that were casted to *doubles* prior to the call. + These functions compute the exact result of single precision float inputs by using double precision multiplications + with some splitting operations. + + They will not work with full double precision inputs, for this you would need quad precision in these routines (i think). + + Read the paper, it explains things better: + + "Exact computation of Voronoi diagram and Delaunay triangulation" + M Gavrilova, H Ratschek, J Rokne - Reliable Computing, 2000 - cpsc.ucalgary.ca + +////////////////////////// - /////////////////////////// +*/ +static inline Ordering essaR(Sequence*,Sequence*); +static inline Sequence * place(Sequence * s, double x1, double x2, Node *n1, Node *n2); +static int cmp_d(const void *, const void *); +static inline LineIntersection coincidenceTest(double a1x, double a1y, double a2x, double a2y, double b1x, double b1y, double b2x, double b2y); +static inline int interval_test(double * topS, Ordering bottom, double * bottomS, IntersectionPoint * ip); + +////////////////////// + + +void split_double(double a, double *x, double *y) +{ + __m128d factor = _mm_set_sd(pow(2.0,27)+1); + __m128d a_ = _mm_set_sd(a); + __m128d b_ = _mm_mul_sd(factor,a_); + __m128d c_ = _mm_sub_sd(b_, a_); + __m128d d_ = _mm_sub_sd(b_, c_); + __m128d e_ = _mm_sub_sd(a_, d_); + _mm_store_sd(x,d_); + _mm_store_sd(y,e_); +} + + +// This is 20x slower than a straight floating point sum over the values + +Ordering +essa_double(double* xs, size_t n) +{ + int i; + double add[n]; // overestimating the space required - how bad could it be + double sub[n]; + size_t an = 0; + size_t sn = 0; + + for (i = 0; i < n; ++i) + { + if (xs[i] > 0) { add[an] = xs[i]; ++an; } + else if (xs[i] < 0) { sub[sn] = -xs[i]; ++sn; } + } + + qsort(add, an, sizeof (double), &cmp_d); + qsort(sub, sn, sizeof (double), &cmp_d); + + Node as_storage[an]; + Node ss_storage[sn]; + Sequence as = toSequence(add,an,as_storage); + Sequence ss = toSequence(sub,sn,ss_storage); + return essaR(&as, &ss); +} + +///////////////////////// + +static int +cmp_d(const void *ap, const void *bp) +{ + double a = *(double*)ap; + double b = *(double*)bp; + + // return (b - a); + + if (a > b) return -1; + else if (a < b) return 1; + else return 0; + +} + +static inline +double exponent(double x) +{ + int e; + + assert (isfinite(x)); + + frexp(x, &e); + return (double) e; +} + +static inline +Ordering +essaR(Sequence * s1, Sequence * s2) +{ + double a1 = 0, a2 = 0, b1 = 0, b2 = 0; + + { + size_t m = length(s1); + size_t n = length(s2); + + if (m == 0 && n == 0) return EQ; + if (m > n && n == 0) return GT; + if (n > m && m == 0) return LT; + + double a = head(s1); + double b = head(s2); + double e = exponent(a); + double f = exponent(b); + + if (a >= (double) (n*pow(2,f))) return GT; + if (b >= (double) (m*pow(2,e))) return LT; + + if (e == f) + { + if (a >= b) a1 = a - b; + else b1 = b - a; + } + else if (e > f) + { + double p = pow(2,f-1); + double u = (b == p) ? p : pow(2,f); + a1 = a - u; + a2 = u - b; + } + else if (f > e) + { + double p = pow(2,e-1); + double u = (a == p) ? p : pow(2,e); + b1 = b - u; + b2 = u - a; + } + } + + + Node ns[4]; + return essaR(place(tail(s1),a1,a2, &ns[0],&ns[1]), + place(tail(s2),b1,b2, &ns[2],&ns[3])); +} + + +static inline +Sequence * +place(Sequence * s, double x1, double x2, Node *n1, Node *n2) +{ + if (x1 != 0 && x2 != 0) + { + assert(x1>0); + assert(x2>0); + + return insert(insert(s,x1,n1),x2,n2); + } + else if (x1 != 0) + { + assert(x1>0); + return insert(s,x1,n1); + } + else if (x2 != 0) + { + assert(x2>0); + return insert(s,x2,n2); + } + else + return s; +} + +/* +void +insertion_sort(double *xs, size_t n) +{ + int i; + + for (i=1; i < n; ++i) + { + double value = xs[i]; + int j; + + for (j = i - 1; j >= 0 && xs[j] < value; --j) + xs[j+1] = xs[j]; + + xs[j+1] = value; + } +} +*/ + +// 0.6 seconds -- about 15% faster than insert_sort and 300% faster than qsort. +/* + s.s_head = malloc(sizeof (Node)); + s.s_head->value = v; + s.s_head->next = NULL; +*/ +///////////////// + /* +int +main() +{ + int i = 0; + double xs[] = { -4, 1, 2147483649 }; + size_t n = (sizeof xs) / (sizeof (double)); + Ordering x; + double y = 0; + + // for (i = 0; i < 99999; ++i) + x = essa_double(xs, n); + + + //insertion_sort(xs,n); + //printSequence((Sequence){xs,n}); + + printf("x=%d,y=%f,n=%ld\n",x,y,n); + return 0; +} +*/ + + + +/* + Ignore this note: + + _MM_SET_ROUNDING_MODE + int mode = _MM_GET_ROUNDING_MODE(); + + _MM_ROUND_NEAREST + _MM_ROUND_DOWN + _MM_ROUND_UP + _MM_ROUND_TOWARD_ZERO + + _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_OFF); + +*/ + + +typedef struct { + int p_sign; + + struct { + int i; + int j; + int k; + } p_idx; + +} LeviCivita; + +#define PERMUTATION_N 24 +const LeviCivita permutations[PERMUTATION_N] = {{1,{0,1,2}},{-1,{0,1,3}},{-1,{0,2,1}},{1,{0,2,3}}, + {1,{0,3,1}},{-1,{0,3,2}},{-1,{1,0,2}},{1,{1,0,3}}, + {1,{1,2,0}},{-1,{1,2,3}},{-1,{1,3,0}},{1,{1,3,2}}, + {1,{2,0,1}},{-1,{2,0,3}},{-1,{2,1,0}},{1,{2,1,3}}, + {1,{2,3,0}},{-1,{2,3,1}},{-1,{3,0,1}},{1,{3,0,2}}, + {1,{3,1,0}},{-1,{3,1,2}},{-1,{3,2,0}},{1,{3,2,1}}}; +Ordering +incircle_essa(float fxi, float fyi, float fxj, float fyj, float fxk, float fyk, float fxl, float fyl) +{ + double xi=(double)fxi, yi=(double)fyi, + xj=(double)fxj, yj=(double)fyj, + xk=(double)fxk, yk=(double)fyk, + xl=(double)fxl, yl=(double)fyl; + + #define SUMS_N (PERMUTATION_N*4) + + int index, cursor; + double xs[] = { xi, xj, xk, xl }; + double ys[] = { yi, yj, yk, yl }; + double sums[SUMS_N]; + + for (index = 0, cursor = 0; index < PERMUTATION_N; ++index) + { + const LeviCivita p = permutations[index]; + + double x = xs[p.p_idx.i]; + double y = ys[p.p_idx.j]; + double a = xs[p.p_idx.k]; + double b = ys[p.p_idx.k]; + + // Would require quadrouple precision ESSA if we diddnt split + // these into lower precision combinations + double xyL, xyR; split_double (x*y, &xyL, &xyR); + double a2L, a2R; split_double (a*a, &a2L, &a2R); + double b2L, b2R; split_double (b*b, &b2L, &b2R); + + if (p.p_sign == -1) + { + sums[cursor++] = -(xyL*a2L); + sums[cursor++] = -(xyR*a2R); + sums[cursor++] = -(xyL*b2L); + sums[cursor++] = -(xyR*b2R); + } + else + { + sums[cursor++] = xyL*a2L; + sums[cursor++] = xyR*a2R; + sums[cursor++] = xyL*b2L; + sums[cursor++] = xyR*b2R; + } + } + + return essa_double(sums, SUMS_N); + #undef SUMS_N +} + + + +Ordering +ccw_essa(float fx1, float fy1, float fx2, float fy2, float fx3, float fy3) +{ + double x1=(double)fx1, y1=(double)fy1, + x2=(double)fx2, y2=(double)fy2, + x3=(double)fx3, y3=(double)fy3; + + double sums[6] = { x1*y2, x2*y3, x3*y1, -(x1*y3), -(x2*y1), -(x3*y2) }; + return essa_double(sums, 6); +} + + +// Exactly test if a point is within a closed interval +int +cintt_essa(float fx1, float fx2, float fp) +{ + double x1=(double)fx1, x2=(double)fx2, p=(double)fp; + + double s_top[2] = { x1, -p }; + double s_bot[2] = { -x2, x1 }; + double s_sign[4] = { x1, -p, x2, -x1 }; + Ordering top = essa_double(s_top, 2); + Ordering bottom = essa_double(s_bot, 2); + Ordering sign = essa_double(s_sign, 4); + + if (top == LT && sign != EQ) // need to flip the sign if were subtracting negative values + sign = (sign == LT) ? GT : LT; + + if (top == EQ) return 1; // = 0 + else if (top != bottom) return 0; // < 0 + else if (sign == EQ) return 1; // = 1 + else if (sign == LT) return 1; // < 1 + else if (sign == GT) return 0; // > 1 + + else return 0; // This shouldn't be reachable + +} + + +/////////// Exact Line intersection test ///////////// + + +#define BOTTOM_N 8 +#define TOP_N 6 + +LineIntersection +intersect2D_essa(float fa1x, float fa1y, + float fa2x, float fa2y, + float fb1x, float fb1y, + float fb2x, float fb2y, + int * ip1, int * ip2) +{ + double a1x=(double)fa1x, a1y=(double)fa1y, + a2x=(double)fa2x, a2y=(double)fa2y, + b1x=(double)fb1x, b1y=(double)fb1y, + b2x=(double)fb2x, b2y=(double)fb2y; + + int i; + double bottom_sm[BOTTOM_N] = { b1y*a1x, -(b2y*a1x), -(b1y*a2x), b2y*a2x, -(a1y*b1x), a1y*b2x, a2y*b1x, -(a2y*b2x) }; + Ordering bottom = essa_double(bottom_sm, BOTTOM_N); + + if (bottom == EQ) + return (ccw_essa(a1x,a1y,a2x,a2y,b1x,b1y) != EQ) + ? PARALLEL + : coincidenceTest(a1x,a1y,a2x,a2y,b1x,b1y,b2x,b2y); + + double top1_sm[6] = { -(a2y*b1x), b1x*b2y, a2y*b2x, b1y*a2x, -(b2y*a2x), -(b1y*b2x) }; + double top2_sm[6] = { a1x*a2y, b2y*a2x, -(b2y*a1x), a1y*b2x, -(a1y*a2x), -(a2y*b2x) }; + + // negate top1 and bottom + for (i = 0; i < TOP_N; ++i) + top1_sm[i] = -top1_sm[i]; + + for (i = 0; i < BOTTOM_N; ++i) + bottom_sm[i] = -bottom_sm[i]; + + if (interval_test(top1_sm,bottom,bottom_sm, ip1) && + interval_test(top2_sm,bottom,bottom_sm, ip2)) + + return INTERSECTING; + + else + + return NINP; +} + + +static +int +interval_test(double * topS, Ordering bottom, double * bottomS, IntersectionPoint * ip) +{ + double sign_sm[TOP_N+BOTTOM_N]; + + // TODO: make sure these two copies dont overflow + + memcpy(sign_sm, topS, TOP_N * sizeof (double)); + memcpy(sign_sm+TOP_N, bottomS, BOTTOM_N * sizeof (double)); + + Ordering top = essa_double(topS, TOP_N); + Ordering sign = essa_double(sign_sm, TOP_N+BOTTOM_N); + + if (top == LT && sign != EQ) // need to flip the sign if were subtracting negative values + sign = (sign == LT) ? GT : LT; + + if (top == EQ) { *ip = ENDPOINT_1; return 1; } // = 0 + else if (top != bottom) return 0; // < 0 + else if (sign == EQ){ *ip = ENDPOINT_0; return 1; } // = 1 + else if (sign == LT){ *ip = BETWEEN; return 1; } // < 1 + else if (sign == GT) return 0; // > 1 + + else return 0; // This shouldn't be reachable +} + + +static +LineIntersection +coincidenceTest(double a1x, double a1y, + double a2x, double a2y, + double b1x, double b1y, + double b2x, double b2y) +{ + // Vertical line. compare Y values. + if ((a1x == a2x && a2x == b1x && b1x == b2x) && + ( + (a1y <= b1y && b1y <= a2y) || + (a1y <= b2y && b2y <= a2y) || + (a1y >= b1y && b1y >= a2y) || + (a1y >= b2y && b2y >= a2y) + )) + return COINCIDENT; + + else if ((a1x <= b1x && b1x <= a2x) || + (a1x <= b2x && b2x <= a2x) || + (a1x >= b1x && b1x >= a2x) || + (a1x >= b2x && b2x >= a2x)) + + return COINCIDENT; + + else + return NINP; +} + +#undef BOTTOM_N +#undef TOP_N + +/////////////////////////////////////// + +/* + +Slope +slope_essa(float fx1, float fy1, float fx2, float fy2) +{ + double x1=(double)fx1, y1=(double)fy1, + x2=(double)fx2, y2=(double)fy2; + + Ordering top = essa_double((double[]){y2, -y1}, 2); + Ordering bottom = essa_double((double[]){x2, -x1}, 2); + + if (top == EQ) return ZERO_SLOPE; + else if (bottom == EQ) return UNDEFINED_SLOPE; + else if (top != bottom) return NEGATIVE_SLOPE; + else return POSITIVE_SLOPE; +} + +*/ diff --git a/Numeric/Geometric/Predicates/Interval.hs b/Numeric/Geometric/Predicates/Interval.hs new file mode 100644 --- /dev/null +++ b/Numeric/Geometric/Predicates/Interval.hs @@ -0,0 +1,137 @@ +{-# LANGUAGE UnicodeSyntax, ForeignFunctionInterface #-} +{-# CFILES Numeric/Geometric/Predicates/Interval/IntervalFilterPrimitives.c #-} + +{- | These predicates use hardware (SSE) based interval arithmetic based on the algorithms presented in (1). + They are intended to be used as a filter before resorting to slower exact computation. + + * These routines return Nothing if the result could not be determined + exactly from the calculated interval. + + * Each call toggles the SSE rounding mode to -infinity and back. + + * All computations are done in Double precision. + + * Rewrite specializations are in place for Float and Double that greatly reduce allocations compared to Real. + Using anything but Float or Double is probably absurdly slow thanks to realToFrac. + + * For performance reasons we assume CDouble == Double. + + (1) BRANIMIR LAMBOV. \"INTERVAL ARITHMETIC USING SSE-2\" +-} + +module Numeric.Geometric.Predicates.Interval (cinttSSE, incircleSSE, ccwSSE) where +import Numeric.Geometric.Primitives +import System.IO.Unsafe +import Foreign.Ptr +import Foreign.Marshal +import Foreign.C.Types +import Control.Exception (assert) +import GHC.Float + +foreign import ccall unsafe ccw_d ∷ Double → Double → Double → Double → Double → Double → Ptr Double → IO () +foreign import ccall unsafe incircle_d ∷ Double → Double → Double → Double → Double → Double → Double → Double → Ptr Double → IO () +foreign import ccall unsafe cintt_d ∷ Double → Double → Double → Ptr Double → IO CInt + + +{-# RULES "incircleSSE/Double" incircleSSE = incircleSSE_D #-} +{-# RULES "cinttSSE/Double" cinttSSE = cinttSSE_D #-} +{-# RULES "ccwSSE/Double" ccwSSE = ccwSSE_D #-} + +{-# RULES "incircleSSE/Float" incircleSSE = incircleSSE_F #-} +{-# RULES "cinttSSE/Float" cinttSSE = cinttSSE_F #-} +{-# RULES "ccwSSE/Float" ccwSSE = ccwSSE_F #-} + + +-- | Test if p3 is within the closed interval specified by [p1,p2] + +cinttSSE ∷ Real a => a → a → a → Maybe Bool +cinttSSE a b c = cinttSSE_D (realToFrac a) (realToFrac b) (realToFrac c) + +-- | Counter-clockwise orientation test. Classifies p3 in relation to the line formed by p1 and p2. +-- Result: LT=Right, GT=Left, EQ=Coincident + +ccwSSE ∷ Real a => Vector2 a → Vector2 a → Vector2 a → Maybe Ordering +ccwSSE (xa,ya) (xb,yb) (xc,yc) = ccwSSE_D (realToFrac xa,realToFrac ya) + (realToFrac xb,realToFrac yb) + (realToFrac xc,realToFrac yc) + +-- | Test the relation of a point to the circle formed by (p1..p3). (p1..p3) must be in counterclockwise order. +-- Result: GT=inside, EQ=border, LT=outside + +incircleSSE ∷ Real a => (Vector2 a, Vector2 a, Vector2 a) → Vector2 a → Maybe Ordering +incircleSSE ((x1,y1), (x2,y2), (x3,y3)) (x4,y4) = incircleSSE_D ((realToFrac x1, realToFrac y1), + (realToFrac x2, realToFrac y2), + (realToFrac x3, realToFrac y3)) + (realToFrac x4, realToFrac y4) +--------------------------------------------------- + +cinttSSE_F ∷ Float → Float → Float → Maybe Bool +cinttSSE_F a b c = cinttSSE_D (float2Double a) (float2Double b) (float2Double c) + +ccwSSE_F ∷ Vector2 Float → Vector2 Float → Vector2 Float → Maybe Ordering +ccwSSE_F (xa,ya) (xb,yb) (xc,yc) = ccwSSE_D (float2Double xa,float2Double ya) + (float2Double xb,float2Double yb) + (float2Double xc,float2Double yc) + +incircleSSE_F ∷ (Vector2 Float, Vector2 Float, Vector2 Float) → Vector2 Float → Maybe Ordering +incircleSSE_F ((x1,y1), (x2,y2), (x3,y3)) (x4,y4) = incircleSSE_D ((float2Double x1, float2Double y1), + (float2Double x2, float2Double y2), + (float2Double x3, float2Double y3)) + (float2Double x4, float2Double y4) + +--------------------------------------------------- + +cinttSSE_D ∷ Double → Double → Double → Maybe Bool +cinttSSE_D l h p + | l == h = Just (p == l) + | otherwise = unsafePerformIO $ allocaArray 2 $ \out → do + + x ← cintt_d l h p out + + if x == 0 + then return Nothing + else do + + [hi,lo] ← peekArray 2 out + return . assert (lo <= hi) $ check lo hi + where + check lo hi + | hi < 0 = Just False + | lo > 1 = Just False + | lo >= 0 && hi <= 1 = Just True + | otherwise = Nothing + + +incircleSSE_D ∷ (Vector2 Double, Vector2 Double, Vector2 Double) → Vector2 Double → Maybe Ordering +incircleSSE_D ((x1,y1), (x2,y2), (x3,y3)) (x4,y4) = unsafePerformIO $ allocaArray 2 $ \out → do + + incircle_d x1 y1 + x2 y2 + x3 y3 + x4 y4 out + + [hi,lo] ← peekArray 2 out + return . assert (lo <= hi) $ check lo hi + where + check lo hi + | lo > 0 = Just GT + | hi < 0 = Just LT + | lo == 0 && hi == 0 = Just EQ + | otherwise = Nothing + + +ccwSSE_D ∷ Vector2 Double → Vector2 Double → Vector2 Double → Maybe Ordering +ccwSSE_D (x1,y1) (x2,y2) (x3,y3) = unsafePerformIO $ allocaArray 2 $ \out → do + + ccw_d x1 y1 + x2 y2 + x3 y3 out + + [hi,lo] ← peekArray 2 out + return . assert (lo <= hi) $ check lo hi + where + check lo hi + | lo > 0 = Just GT + | hi < 0 = Just LT + | lo == 0 && hi == 0 = Just EQ + | otherwise = Nothing diff --git a/Numeric/Geometric/Predicates/Interval/IntervalFilterPrimitives.c b/Numeric/Geometric/Predicates/Interval/IntervalFilterPrimitives.c new file mode 100644 --- /dev/null +++ b/Numeric/Geometric/Predicates/Interval/IntervalFilterPrimitives.c @@ -0,0 +1,148 @@ +#include +#include +#include +#include "IntervalSSE.h" + + +static inline +void fromInterval(double output[2], __m128d interval) +{ + __m128d signmask = _mm_set_pd(0.0, -1.0 * 0.0); + _mm_storeu_pd(output, _mm_xor_pd(interval, signmask)); +} + +static inline +__m128d toInterval(double x) +{ + __m128d signmask = _mm_set_pd(0.0, -1.0 * 0.0); + return _mm_xor_pd(_mm_set1_pd(x), signmask); + // return _mm_set_pd(x,-x); +} + + + +//////////////////////////// + +static __m128d +ccw(__m128d x1, __m128d y1, __m128d x2, __m128d y2, __m128d x3, __m128d y3) +{ + __m128d s1 = interval_add(interval_add(interval_mul(x1,y2), + interval_mul(x2,y3)), + interval_mul(x3,y1)); + + __m128d s2 = interval_add(interval_add(interval_mul(x1,y3), + interval_mul(x2,y1)), + interval_mul(x3,y2)); + + return interval_sub(s1,s2); +} + +static __m128d +incircle(__m128d x1, __m128d y1, __m128d x2, __m128d y2, __m128d x3, __m128d y3, __m128d x4, __m128d y4) +{ + #define DDD(x,y) interval_add(interval_mul(x,x),interval_mul(y,y)) + __m128d a = interval_mul(DDD(x1,y1), ccw(x2,y2, x3,y3, x4,y4)); + __m128d b = interval_mul(DDD(x2,y2), ccw(x1,y1, x3,y3, x4,y4)); + __m128d c = interval_mul(DDD(x3,y3), ccw(x1,y1, x2,y2, x4,y4)); + __m128d d = interval_mul(DDD(x4,y4), ccw(x1,y1, x2,y2, x3,y3)); + + return interval_add(interval_sub(a,b),interval_sub(c,d)); + #undef DDD +} + +static __m128d +cintt(__m128d lo, __m128d hi, __m128d p, int * error) +{ + __m128d n = interval_sub(lo, p); + __m128d v = interval_sub(hi,lo); + + return interval_div(n, interval_negate(v), error); +} + +//////////////////////////// + +void +ccw_d(double ax, double ay, double bx, double by, double cx, double cy, double output[2]) +{ + int mode = _MM_GET_ROUNDING_MODE(); + _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); + + __m128d x1 = toInterval(ax); + __m128d y1 = toInterval(ay); + __m128d x2 = toInterval(bx); + __m128d y2 = toInterval(by); + __m128d x3 = toInterval(cx); + __m128d y3 = toInterval(cy); + + fromInterval(output, ccw(x1,y1,x2,y2,x3,y3)); + _MM_SET_ROUNDING_MODE(mode); +} + + +void +incircle_d(double ax, double ay, double bx, double by, double cx, double cy, double dx, double dy, double output[2]) +{ + int mode = _MM_GET_ROUNDING_MODE(); + _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); + + __m128d x1 = toInterval(ax); + __m128d y1 = toInterval(ay); + __m128d x2 = toInterval(bx); + __m128d y2 = toInterval(by); + __m128d x3 = toInterval(cx); + __m128d y3 = toInterval(cy); + __m128d x4 = toInterval(dx); + __m128d y4 = toInterval(dy); + + fromInterval(output, incircle(x1,y1,x2,y2,x3,y3,x4,y4)); + _MM_SET_ROUNDING_MODE(mode); +} + +// Input interval must not be degenerate + +int +cintt_d(double lo, double hi, double p, double output[2]) +{ + if (lo == hi) + return 0; + + int mode = _MM_GET_ROUNDING_MODE(); + int error = 0; + + _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); + __m128d result = cintt(toInterval(lo),toInterval(hi),toInterval(p),&error); + + if (error == 1) + { + _MM_SET_ROUNDING_MODE(mode); + return 0; + + } else + { + fromInterval(output, result); + + _MM_SET_ROUNDING_MODE(mode); + return 1; + } +} + + + + +/* +void +main() +{ + _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); + + double output[2]; + + __m128d x1 = toInterval(3); + __m128d y1 = toInterval(4); + + fromInterval(output, interval_sub(x1,y1)); + + + printf("x=%.16f,x2=%.16f\n", output[0],output[1]); +} +*/ diff --git a/Setup.lhs b/Setup.lhs new file mode 100644 --- /dev/null +++ b/Setup.lhs @@ -0,0 +1,3 @@ +#!/usr/bin/env runhaskell +> import Distribution.Simple +> main = defaultMain