-
-
Save sjkelly/12b4753c3ef37e81f069 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| # GPREDv0.1.3 | |
| # | |
| # Fast, robust 2D and 3D geometrical predicates on generic point types. | |
| # Implementation follows algorithms described in http://arxiv.org/abs/0901.4107 | |
| # and used (for e.g.) in the Illustris Simulation | |
| # http://www.illustris-project.org/ | |
| # | |
| # Author: Ariel Keselman (skariel@gmail.com) | |
| # License: MIT | |
| # Bug reportss welcome! | |
| # | |
| # Calculations initially performed on Float64 while bounding max | |
| # absolute errors. If unable to determine result fall back to exact | |
| # calculation using BigInts. This is a form of floating point filtering. | |
| # Most calculations are cached for fast repeated testing of | |
| # incircle/intriangle | |
| # | |
| # Usage notes: | |
| # ------------- | |
| # - all point types must implement getx(p), gety(p) and (if 3D point) getz(p). | |
| # These methods should return Float64. The predicated are generic in the sense | |
| # that they accept any such point carrying additional user defined data. | |
| # - all point coordinates must be in the range [1, 2) | |
| # - beware of creating a 2D point using somthing like Point2D(3, 2) - | |
| # it will return the point belonging to a Peano-Hilbert key `3` in | |
| # a 2X2 grid. If you want to create a point located at 3, 2 use | |
| # Point2D(3.0 2.0) (currently no such problem for Point3D since peano curve | |
| # is not yet implemented for these...) | |
| # | |
| # `incircle` method: determines if the point | |
| # lies inside (ret->1), exactly on (ret->0), | |
| # outside (ret->-1), or NA (ret->2) of a circle or sphere | |
| # defined by the primitive | |
| # | |
| # `intriangle` method: determines if the point | |
| # lies inside (ret->1) or outside (ret->negative num) the primitive. | |
| # The negative number is the negative index ot the triangle point | |
| # which test point is in front of. If test point is in front of two | |
| # triangle points then one is chosen in an undefined mannar. | |
| # If point is exactly on one of the sides it returns the | |
| # index+1 of the point that is in front of said side | |
| # i.e. for a point infront of Triangle.a return 2, | |
| # for a point infront of Triangle.b return 3, etc. | |
| # If test point is exactly on one of the triangle points then one | |
| # side is chosen in an undefined mannar. | |
| # | |
| # `peanokey` method: returns the Peano-Hilbert key for the given | |
| # point in an nXn grid, starting with 0 at (1.0, 2.0) and | |
| # ending at nXn-1 at (2.0,2.0) | |
| # | |
| # Todo | |
| # ------------------ | |
| # - 3D Peano-Hilbert ordering | |
| # - Use the testing package | |
| # | |
| const float_err = eps(Float64) | |
| const abs_err_incircle_2d = 12*float_err | |
| const abs_err_incircle_3d = 48*float_err | |
| const abs_err_orientation_2d = 2*float_err | |
| const abs_err_orientation_3d = 6*float_err | |
| const abs_err_intriangle = 6*float_err | |
| const abs_err_intriangle_zero = 2*float_err | |
| const abs_err_intetra = 24*float_err | |
| const abs_err_intetra_zero = 6*float_err | |
| abstract AbstractPoint | |
| abstract AbstractPoint2D <: AbstractPoint | |
| abstract AbstractPoint3D <: AbstractPoint | |
| abstract AbstractPrimitive | |
| abstract AbstractTriangle <: AbstractPrimitive | |
| abstract AbstractTetrahedron <: AbstractPrimitive | |
| const min_coord = 1.0 | |
| const max_coord = 2.0 - eps(Float64) | |
| immutable Point2D <: AbstractPoint2D | |
| _x::Float64 | |
| _y::Float64 | |
| end | |
| getx(p::Point2D) = p._x | |
| gety(p::Point2D) = p._y | |
| immutable Point3D <: AbstractPoint3D | |
| _x::Float64 | |
| _y::Float64 | |
| _z::Float64 | |
| end | |
| getx(p::Point3D) = p._x | |
| gety(p::Point3D) = p._y | |
| getz(p::Point3D) = p._z | |
| function _exact_sign_orientation_determinant!(ax::BigInt, ay::BigInt, bx::BigInt, by::BigInt, cx::BigInt, cy::BigInt) | |
| bx -= ax; by -= ay | |
| cx -= ax; cy -= ay | |
| sign(bx*cy - by*cx) | |
| end | |
| function _exact_sign_orientation_determinant!(ax::BigInt, ay::BigInt, az::BigInt, bx::BigInt, by::BigInt, bz::BigInt, cx::BigInt, cy::BigInt, cz::BigInt, dx::BigInt, dy::BigInt, dz::BigInt) | |
| bx -= ax; by -= ay; bz -= az | |
| cx -= ax; cy -= ay; cz -= az | |
| dx -= ax; dy -= ay; dz -= az | |
| sign(+bx*cy*dz - bx*cz*dy - by*cx*dz + by*cz*dx + bz*cx*dy - bz*cy*dx) | |
| end | |
| _extract_int(n::Float64) = reinterpret(Uint64, n) & 0x000fffffffffffff | |
| _extract_bigint(n::Float64) = BigInt(_extract_int(n)) | |
| type Triangle{T<:AbstractPoint2D} <: AbstractTriangle | |
| _dirty::Bool | |
| _a::T; _b::T; _c::T | |
| _bx::Float64; _by::Float64 | |
| _cx::Float64; _cy::Float64 | |
| _px::Float64; _py::Float64 | |
| _pr2::Float64 | |
| _sz::Float64 | |
| _o::Int64 | |
| function Triangle(a::T, b::T, c::T) | |
| new(true, a, b, c, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0) | |
| end | |
| end | |
| function clean!(t::AbstractTriangle) | |
| if !t._dirty | |
| return | |
| end | |
| t._dirty = false | |
| t._bx = getx(getb(t))-getx(geta(t)); t._by = gety(getb(t))-gety(geta(t)); | |
| t._cx = getx(getc(t))-getx(geta(t)); t._cy = gety(getc(t))-gety(geta(t)); | |
| const br2 = t._bx*t._bx+t._by*t._by | |
| const cr2 = t._cx*t._cx+t._cy*t._cy | |
| t._sz = max(abs(t._bx), abs(t._by), abs(t._cx), abs(t._cy)) | |
| t._px = br2*t._cy - t._by*cr2 | |
| t._py = -br2*t._cx + t._bx*cr2 | |
| t._pr2 = -t._bx *t._cy + t._by*t._cx | |
| t._o = -1 | |
| # if t._pr2 < -abs_err_orientation_2d*t._sz | |
| # t._o = 1 | |
| # elseif t._pr2 > abs_err_orientation_2d*t._sz | |
| # t._o = -1 | |
| # else | |
| # t._o = _exact_sign_orientation_determinant!( | |
| # _extract_bigint(getx(geta(t))), _extract_bigint(gety(geta(t))), | |
| # _extract_bigint(getx(getb(t))), _extract_bigint(gety(getb(t))), | |
| # _extract_bigint(getx(getc(t))), _extract_bigint(gety(getc(t)))) | |
| # end | |
| end | |
| geta(t::AbstractTriangle) = t._a | |
| getb(t::AbstractTriangle) = t._b | |
| getc(t::AbstractTriangle) = t._c | |
| type Tetrahedron{T<:AbstractPoint3D} <: AbstractTetrahedron | |
| _a::T; _b::T; _c::T; _d::T | |
| _bx::Float64; _by::Float64; _bz::Float64 | |
| _cx::Float64; _cy::Float64; _cz::Float64 | |
| _dx::Float64; _dy::Float64; _dz::Float64 | |
| _px::Float64; _py::Float64; _pz::Float64 | |
| _pr2::Float64 | |
| _o::Int64 | |
| function Tetrahedron(a::AbstractPoint3D, b::AbstractPoint3D, c::AbstractPoint3D, d::AbstractPoint3D) | |
| const bx = getx(b)-getx(a); const by = gety(b)-gety(a); const bz = getz(b)-getz(a) | |
| const cx = getx(c)-getx(a); const cy = gety(c)-gety(a); const cz = getz(c)-getz(a) | |
| const dx = getx(d)-getx(a); const dy = gety(d)-gety(a); const dz = getz(d)-getz(a) | |
| const br2 = bx*bx+by*by+bz*bz | |
| const cr2 = cx*cx+cy*cy+cz*cz | |
| const dr2 = dx*dx+dy*dy+dz*dz | |
| const px = by*cz*dr2 - br2*cz*dy - by*cr2*dz - bz*cy*dr2 + br2*cy*dz + bz*cr2*dy | |
| const py = br2*cz*dx + bz*cx*dr2 - br2*cx*dz - bx*cz*dr2 - bz*cr2*dx + bx*cr2*dz | |
| const pz = br2*cx*dy + bx*cy*dr2 + by*cr2*dx - br2*cy*dx - bx*cr2*dy - by*cx*dr2 | |
| const pr2 = -bx*cy*dz + bx*cz*dy + by*cx*dz - by*cz*dx - bz*cx*dy + bz*cy*dx | |
| o::Int64 = 0 | |
| if pr2 < -abs_err_orientation_3d | |
| o = 1 | |
| elseif pr2 > abs_err_orientation_3d | |
| o = -1 | |
| else | |
| o = _exact_sign_orientation_determinant!( | |
| _extract_bigint(getx(a)), _extract_bigint(gety(a)), _extract_bigint(getz(a)), | |
| _extract_bigint(getx(b)), _extract_bigint(gety(b)), _extract_bigint(getz(b)), | |
| _extract_bigint(getx(c)), _extract_bigint(gety(c)), _extract_bigint(getz(c)), | |
| _extract_bigint(getx(d)), _extract_bigint(gety(d)), _extract_bigint(getz(d))) | |
| end | |
| new(a, b, c, d, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz, pr2, o) | |
| end | |
| end | |
| geta(t::Tetrahedron) = t._a | |
| getb(t::Tetrahedron) = t._b | |
| getc(t::Tetrahedron) = t._c | |
| getd(t::Tetrahedron) = t._d | |
| Triangle{T<:AbstractPoint2D}(a::T, b::T, c::T) = Triangle{T}(a, b, c) | |
| Triangle(ax::Float64, ay::Float64, bx::Float64, by::Float64, cx::Float64, cy::Float64) = | |
| Triangle(Point2D(ax, ay), Point2D(bx, by), Point2D(cx, cy)) | |
| Tetrahedron{T<:AbstractPoint3D}(a::T, b::T, c::T, d::T) = Tetrahedron{T}(a, b, c, d) | |
| Tetrahedron(ax::Float64, ay::Float64, az::Float64, bx::Float64, by::Float64, bz::Float64, cx::Float64, cy::Float64, cz::Float64, dx::Float64, dy::Float64, dz::Float64) = | |
| Tetrahedron(Point3D(ax, ay, az), Point3D(bx, by, bz), Point3D(cx, cy, cz), Point3D(dx, dy, dz)) | |
| orientation(t::Tetrahedron) = t._o | |
| function orientation(t::AbstractTriangle) | |
| clean!(t) | |
| t._o | |
| end | |
| orientation(ax::Float64, ay::Float64, bx::Float64, by::Float64, cx::Float64, cy::Float64) = | |
| orientation(Triangle(ax, ay, bx, by, cx, cy)) | |
| orientation(ax::Float64, ay::Float64, az::Float64, bx::Float64, by::Float64, bz::Float64, cx::Float64, cy::Float64, cz::Float64, dx::Float64, dy::Float64, dz::Float64) = | |
| orientation(Tetrahedron(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz)) | |
| function _exact_sign_incircle_determinant!(ax::BigInt, ay::BigInt, bx::BigInt, by::BigInt, cx::BigInt, cy::BigInt, px::BigInt, py::BigInt) | |
| bx -= ax; by -= ay; | |
| cx -= ax; cy -= ay; | |
| px -= ax; py -= ay; | |
| const br2 = bx*bx+by*by | |
| const cr2 = cx*cx+cy*cy | |
| const pr2 = px*px+py*py | |
| sign(-br2*cx*py + br2*cy*px + bx*cr2*py - bx*cy*pr2 - by*cr2*px + by*cx*pr2) | |
| end | |
| function _exact_sign_incircle_determinant!(ax::BigInt, ay::BigInt, az::BigInt, bx::BigInt, by::BigInt, bz::BigInt, cx::BigInt, cy::BigInt, cz::BigInt, dx::BigInt, dy::BigInt, dz::BigInt, px::BigInt, py::BigInt, pz::BigInt) | |
| bx -= ax; by -= ay; bz -= az | |
| cx -= ax; cy -= ay; cz -= az | |
| dx -= ax; dy -= ay; dz -= az | |
| px -= ax; py -= ay; pz -= az | |
| const br2 = bx*bx+by*by+bz*bz | |
| const cr2 = cx*cx+cy*cy+cz*cz | |
| const dr2 = dx*dx+dy*dy+dz*dz | |
| const pr2 = px*px+py*py+pz*pz | |
| sign( | |
| +br2*cx*dy*pz - br2*cx*dz*py - br2*cy*dx*pz + br2*cy*dz*px + | |
| br2*cz*dx*py - br2*cz*dy*px - bx*cr2*dy*pz + bx*cr2*dz*py + | |
| bx*cy*dr2*pz - bx*cy*dz*pr2 - bx*cz*dr2*py + bx*cz*dy*pr2 + | |
| by*cr2*dx*pz - by*cr2*dz*px - by*cx*dr2*pz + by*cx*dz*pr2 + | |
| by*cz*dr2*px - by*cz*dx*pr2 - bz*cr2*dx*py + bz*cr2*dy*px + | |
| bz*cx*dr2*py - bz*cx*dy*pr2 - bz*cy*dr2*px + bz*cy*dx*pr2) | |
| end | |
| function incircle(t::AbstractTriangle, p::AbstractPoint2D) | |
| clean!(t) | |
| t._o == 0 && return 2 | |
| px = getx(p) - getx(t._a) | |
| py = gety(p) - gety(t._a) | |
| sz = max(abs(px), abs(py), t._sz) | |
| pr2 = px*px + py*py | |
| d = t._px*px + t._py*py + t._pr2*pr2 | |
| if d < -abs_err_incircle_2d*sz | |
| return -t._o | |
| elseif d > abs_err_incircle_2d*sz | |
| return t._o | |
| end | |
| t._o*_exact_sign_incircle_determinant!( | |
| _extract_bigint(getx(t._a)), _extract_bigint(gety(t._a)), | |
| _extract_bigint(getx(t._b)), _extract_bigint(gety(t._b)), | |
| _extract_bigint(getx(t._c)), _extract_bigint(gety(t._c)), | |
| _extract_bigint(getx(p)) , _extract_bigint(gety(p))) | |
| end | |
| function incircle(t::Tetrahedron, p::AbstractPoint3D) | |
| t._o == 0 && return 2 | |
| px = getx(p) - getx(t._a) | |
| py = gety(p) - gety(t._a) | |
| pz = getz(p) - getz(t._a) | |
| pr2 = px*px + py*py + pz*pz | |
| d = t._px*px + t._py*py + t._pz*pz + t._pr2*pr2 | |
| if d < -abs_err_incircle_3d | |
| return -t._o | |
| elseif d > abs_err_incircle_3d | |
| return t._o | |
| end | |
| t._o*_exact_sign_incircle_determinant!( | |
| _extract_bigint(getx(t._a)), _extract_bigint(gety(t._a)), _extract_bigint(getz(t._a)), | |
| _extract_bigint(getx(t._b)), _extract_bigint(gety(t._b)), _extract_bigint(getz(t._b)), | |
| _extract_bigint(getx(t._c)), _extract_bigint(gety(t._c)), _extract_bigint(getz(t._c)), | |
| _extract_bigint(getx(t._d)), _extract_bigint(gety(t._d)), _extract_bigint(getz(t._d)), | |
| _extract_bigint(getx(p)) , _extract_bigint(gety(p)) , _extract_bigint(getz(p))) | |
| end | |
| incircle(ax::Float64, ay::Float64, bx::Float64, by::Float64, cx::Float64, cy::Float64, dx::Float64, dy::Float64) = | |
| incircle(Triangle(ax, ay, bx, by, cx, cy), Point2D(dx, dy)) | |
| incircle(ax::Float64, ay::Float64, az::Float64, bx::Float64, by::Float64, bz::Float64, cx::Float64, cy::Float64, cz::Float64, dx::Float64, dy::Float64, dz::Float64, ex::Float64, ey::Float64, ez::Float64) = | |
| incircle(Tetrahedron(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz), Point3D(ex, ey, ez)) | |
| function _exact_intriangle!(ax::BigInt, ay::BigInt, bx::BigInt, by::BigInt, cx::BigInt, cy::BigInt, px::BigInt, py::BigInt) | |
| bx -= ax; by -= ay; | |
| cx -= ax; cy -= ay; | |
| px -= ax; py -= ay; | |
| const nb = -cx*py + cy*px | |
| const nc = bx*py - by*px | |
| const denom = bx*cy - by*cx | |
| if sign(nb) * sign(denom) < 0 | |
| return -2 | |
| end | |
| if sign(nc) * sign(denom) < 0 | |
| return -3 | |
| end | |
| const l = nb+nc - denom | |
| const sl = sign(l)*sign(denom) | |
| if sl > 0 | |
| return -1 | |
| end | |
| if sign(nb) == 0 | |
| return 3 | |
| end | |
| if sign(nc) == 0 | |
| return 4 | |
| end | |
| if sl == 0 | |
| return 2 | |
| end | |
| -sl | |
| end | |
| _exact_intriangle(t::AbstractTriangle, p::AbstractPoint2D) = | |
| _exact_intriangle!( | |
| _extract_bigint(getx(t._a)), _extract_bigint(gety(t._a)), | |
| _extract_bigint(getx(t._b)), _extract_bigint(gety(t._b)), | |
| _extract_bigint(getx(t._c)), _extract_bigint(gety(t._c)), | |
| _extract_bigint(getx(p)) , _extract_bigint(gety(p))) | |
| function intriangle(t::AbstractTriangle, p::AbstractPoint2D) | |
| clean!(t) | |
| const px = getx(p) - getx(t._a); const py = gety(p) - gety(t._a) | |
| sz = max(abs(px), abs(py), t._sz) | |
| const nb = (-t._cx*py + t._cy*px) * sign(-t._pr2) | |
| if nb < -abs_err_intriangle_zero*sz | |
| return -2 | |
| elseif nb < abs_err_intriangle_zero*sz | |
| return _exact_intriangle(t, p) | |
| end | |
| const nc = (t._bx*py - t._by*px) * sign(-t._pr2) | |
| if nc < -abs_err_intriangle_zero*sz | |
| return -3 | |
| elseif nc < abs_err_intriangle_zero*sz | |
| return _exact_intriangle(t, p) | |
| end | |
| const l = nb+nc + t._pr2*sign(-t._pr2) | |
| if l < -abs_err_intriangle*sz | |
| return 1 | |
| elseif l > abs_err_intriangle*sz | |
| return -1 | |
| end | |
| _exact_intriangle(t, p) | |
| end | |
| intriangle(ax::Float64, ay::Float64, bx::Float64, by::Float64, cx::Float64, cy::Float64, px::Float64, py::Float64) = | |
| intriangle(Triangle(ax, ay, bx, by, cx, cy), Point2D(px, py)) | |
| function _exact_intriangle!(ax::BigInt, ay::BigInt, az::BigInt, bx::BigInt, by::BigInt, bz::BigInt, cx::BigInt, cy::BigInt, cz::BigInt, dx::BigInt, dy::BigInt, dz::BigInt, px::BigInt, py::BigInt, pz::BigInt) | |
| bx -= ax; by -= ay; bz -= az | |
| cx -= ax; cy -= ay; cz -= az | |
| dx -= ax; dy -= ay; dz -= az | |
| px -= ax; py -= ay; pz -= az | |
| const denom = bx*cy*dz-bx*cz*dy-by*cx*dz+by*cz*dx+bz*cx*dy-bz*cy*dx | |
| const nb = cx*dy*pz-cx*dz*py-cy*dx*pz+cy*dz*px+cz*dx*py-cz*dy*px | |
| if sign(nb) * sign(denom) < 0 | |
| return -2 | |
| end | |
| const nc = -bx*dy*pz+bx*dz*py+by*dx*pz-by*dz*px-bz*dx*py+bz*dy*px | |
| if sign(nc) * sign(denom) < 0 | |
| return -3 | |
| end | |
| const nd = bx*cy*pz-bx*cz*py-by*cx*pz+by*cz*px+bz*cx*py-bz*cy*px | |
| if sign(nd) * sign(denom) < 0 | |
| return -4 | |
| end | |
| const l = (nb+nc+nd - denom) * sign(denom) | |
| const sl = sign(l) | |
| if sl > 0 | |
| return -1 | |
| end | |
| if sign(nb) == 0 | |
| return 3 | |
| end | |
| if sign(nc) == 0 | |
| return 4 | |
| end | |
| if sign(nd) == 0 | |
| return 5 | |
| end | |
| if sl == 0 | |
| return 2 | |
| end | |
| -sl | |
| end | |
| _exact_intriangle(t::Tetrahedron, p::AbstractPoint3D) = | |
| _exact_intriangle!( | |
| _extract_bigint(getx(t._a)), _extract_bigint(gety(t._a)), _extract_bigint(getz(t._a)), | |
| _extract_bigint(getx(t._b)), _extract_bigint(gety(t._b)), _extract_bigint(getz(t._b)), | |
| _extract_bigint(getx(t._c)), _extract_bigint(gety(t._c)), _extract_bigint(getz(t._c)), | |
| _extract_bigint(getx(t._d)), _extract_bigint(gety(t._d)), _extract_bigint(getz(t._d)), | |
| _extract_bigint(getx(p)), _extract_bigint(gety(p)), _extract_bigint(getz(p))) | |
| function intriangle(t::Tetrahedron, p::AbstractPoint3D) | |
| const px = getx(p) - getx(t._a); const py = gety(p) - gety(t._a); const pz = getz(p) - getz(t._a) | |
| const nb = (t._cx*t._dy*pz-t._cx*t._dz*py-t._cy*t._dx*pz+t._cy*t._dz*px+t._cz*t._dx*py-t._cz*t._dy*px) * sign(-t._pr2) | |
| if nb < -abs_err_intetra_zero | |
| return -2 | |
| end | |
| if nb < abs_err_intetra_zero | |
| return _exact_intriangle(t, p) | |
| end | |
| const nc = (-t._bx*t._dy*pz+t._bx*t._dz*py+t._by*t._dx*pz-t._by*t._dz*px-t._bz*t._dx*py+t._bz*t._dy*px) * sign(-t._pr2) | |
| if nc < -abs_err_intetra_zero | |
| return -3 | |
| end | |
| if nc < abs_err_intetra_zero | |
| return _exact_intriangle(t, p) | |
| end | |
| const nd = (t._bx*t._cy*pz-t._bx*t._cz*py-t._by*t._cx*pz+t._by*t._cz*px+t._bz*t._cx*py-t._bz*t._cy*px) * sign(-t._pr2) | |
| if nd < -abs_err_intetra_zero | |
| return -4 | |
| end | |
| if nd < abs_err_intetra_zero | |
| return _exact_intriangle(t, p) | |
| end | |
| const l = nb+nc+nd + t._pr2*sign(-t._pr2) | |
| if l < -abs_err_intetra | |
| return 1 | |
| elseif l > abs_err_intetra | |
| return -1 | |
| end | |
| _exact_intriangle(t, p) | |
| end | |
| intriangle(ax::Float64, ay::Float64, az::Float64, bx::Float64, by::Float64, bz::Float64, cx::Float64, cy::Float64, cz::Float64, dx::Float64, dy::Float64, dz::Float64, px::Float64, py::Float64, pz::Float64) = | |
| intriangle(Tetrahedron(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz), Point3D(px, py, pz)) | |
| ################################################################################### | |
| # | |
| # Peano-Hilbert stuff | |
| # | |
| # | |
| const peano_2D_bin_count = 1 << 31 | |
| _extract_peano_2D_bin_num(nbins::Int64, n::Float64) = itrunc( (n-1)*nbins ) | |
| function peanokey(p::AbstractPoint2D, n::Int64=peano_2D_bin_count) | |
| s = n >> 1; d = 0 | |
| x = _extract_peano_2D_bin_num(n, getx(p)) | |
| y = _extract_peano_2D_bin_num(n, gety(p)) | |
| while true | |
| rx = (x & s) > 0 | |
| ry = (y & s) > 0 | |
| d += s * s * ((3 * rx) $ ry) | |
| s = s >> 1 | |
| (s == 0) && break | |
| if ry == 0 | |
| if rx == 1 | |
| x = n - 1 - x; | |
| y = n - 1 - y; | |
| end | |
| x, y = y, x | |
| end | |
| end | |
| d | |
| end | |
| function Point2D(peanokey::Int64, n::Int64=peano_2D_bin_count) | |
| x = 0; y = 0; s=1 | |
| while true | |
| rx = 1 & (peanokey >> 1) | |
| ry = 1 & (peanokey $ rx) | |
| if ry == 0 | |
| if rx == 1 | |
| x = s - 1 - x; | |
| y = s - 1 - y; | |
| end | |
| x, y = y, x | |
| end | |
| x += s * rx | |
| y += s * ry | |
| s = s << 1 | |
| (s >= n) && break | |
| peanokey = peanokey >> 2 | |
| end | |
| Point2D(1+x/n, 1+y/n) | |
| end | |
| ################################################################################### | |
| # | |
| # Tests! | |
| # | |
| # | |
| function test() | |
| # 2D orientation | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.2; by = 1.2 | |
| cx = 1.3; cy = 1.3 | |
| assert( orientation(ax,ay,bx,by,cx,cy) == 0 ) | |
| assert( orientation(bx,by,ax,ay,cx,cy) == 0 ) | |
| assert( orientation(bx,by,cx,cy,ax,ay) == 0 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.2; by = 1.199999999999 | |
| cx = 1.3; cy = 1.3 | |
| assert( orientation(ax,ay,bx,by,cx,cy) == 1 ) | |
| assert( orientation(bx,by,ax,ay,cx,cy) == -1 ) | |
| assert( orientation(bx,by,cx,cy,ax,ay) == 1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.2; by = 1.199999999999999 | |
| cx = 1.3; cy = 1.3 | |
| assert( orientation(ax,ay,bx,by,cx,cy) == 1 ) | |
| assert( orientation(bx,by,ax,ay,cx,cy) == -1 ) | |
| assert( orientation(bx,by,cx,cy,ax,ay) == 1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.2; by = 1.21 | |
| cx = 1.3; cy = 1.3 | |
| assert( orientation(ax,ay,bx,by,cx,cy) == -1 ) | |
| assert( orientation(bx,by,ax,ay,cx,cy) == 1 ) | |
| assert( orientation(bx,by,cx,cy,ax,ay) == -1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.2; by = 1.20000000000001 | |
| cx = 1.3; cy = 1.3 | |
| assert( orientation(ax,ay,bx,by,cx,cy) == -1 ) | |
| assert( orientation(bx,by,ax,ay,cx,cy) == 1 ) | |
| assert( orientation(bx,by,cx,cy,ax,ay) == -1 ) | |
| p1 = Point2D(1.0, 1.0) | |
| p2 = Point2D(1.9, 1.5) | |
| p3 = Point2D(1.45, 1.25) | |
| tr = Triangle(p1, p2, p3) | |
| assert(orientation(tr) == 0) | |
| p3 = Point2D(getx(p3), 1.3) | |
| tr = Triangle(p1, p2, p3) | |
| assert(orientation(tr) == 1) | |
| p3 = Point2D(getx(p3), 1.2) | |
| tr = Triangle(p1, p2, p3) | |
| assert(orientation(tr) == -1) | |
| # 2D incircle | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.2; by = 1.2 | |
| cx = 1.3; cy = 1.3 | |
| dx = 1.4; dy = 1.7 | |
| assert( incircle(ax,ay,bx,by,cx,cy,dx,dy) == 2 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.3; by = 1.1 | |
| cx = 1.3; cy = 1.3 | |
| dx = 1.1; dy = 1.3 | |
| assert( incircle(ax,ay,bx,by,cx,cy,dx,dy) == 0 ) | |
| assert( incircle(bx,by,ax,ay,cx,cy,dx,dy) == 0 ) | |
| assert( incircle(bx,by,cx,cy,ax,ay,dx,dy) == 0 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.3; by = 1.1 | |
| cx = 1.3; cy = 1.3 | |
| dx = 1.1; dy = 1.3000001 | |
| assert( incircle(ax,ay,bx,by,cx,cy,dx,dy) == -1 ) | |
| assert( incircle(bx,by,ax,ay,cx,cy,dx,dy) == -1 ) | |
| assert( incircle(bx,by,cx,cy,ax,ay,dx,dy) == -1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.3; by = 1.1 | |
| cx = 1.3; cy = 1.3 | |
| dx = 1.1; dy = 1.30000000000001 | |
| assert( incircle(ax,ay,bx,by,cx,cy,dx,dy) == -1 ) | |
| assert( incircle(bx,by,ax,ay,cx,cy,dx,dy) == -1 ) | |
| assert( incircle(bx,by,cx,cy,ax,ay,dx,dy) == -1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.3; by = 1.1 | |
| cx = 1.3; cy = 1.3 | |
| dx = 1.1; dy = 1.29999 | |
| assert( incircle(ax,ay,bx,by,cx,cy,dx,dy) == 1 ) | |
| assert( incircle(bx,by,ax,ay,cx,cy,dx,dy) == 1 ) | |
| assert( incircle(bx,by,cx,cy,ax,ay,dx,dy) == 1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.3; by = 1.1 | |
| cx = 1.3; cy = 1.3 | |
| dx = 1.1; dy = 1.29999999999999 | |
| assert( incircle(bx,by,ax,ay,cx,cy,dx,dy) == 1 ) | |
| assert( incircle(bx,by,ax,ay,cx,cy,dx,dy) == 1 ) | |
| assert( incircle(bx,by,cx,cy,ax,ay,dx,dy) == 1 ) | |
| p1 = Point2D(1.0, 1.0) | |
| p2 = Point2D(1.0625, 1.0) | |
| p3 = Point2D(1.0625, 1.0625) | |
| tr = Triangle(p1, p2, p3) | |
| p4 = Point2D(1.03, 1.003) | |
| p5 = Point2D(1.99, 1.99) | |
| p6 = Point2D(1.0, 1.0625) | |
| assert (incircle(tr, p4) == 1) | |
| assert (incircle(tr, p5) == -1) | |
| assert (incircle(tr, p6) == 0) | |
| assert (incircle(tr, p2) == 0) | |
| assert (incircle(tr, p3) == 0) | |
| assert (incircle(tr, p1) == 0) | |
| # 3D orientation | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.2; by = 1.2; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.4; dy = 1.4; dz=1.1 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == 0 ) | |
| assert( orientation(dx,dy,dz,bx,by,bz,cx,cy,cz,ax,ay,az) == 0 ) | |
| assert( orientation(dx,dy,dz,cx,cy,cz,bx,by,bz,ax,ay,az) == 0 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.2; by = 1.2; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.4; dy = 1.4; dz=1.100001 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == 0 ) | |
| assert( orientation(dx,dy,dz,bx,by,bz,cx,cy,cz,ax,ay,az) == 0 ) | |
| assert( orientation(dx,dy,dz,cx,cy,cz,bx,by,bz,ax,ay,az) == 0 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.2; by = 1.2; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.4; dy = 1.4; dz=1.100000000001 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == 0 ) | |
| assert( orientation(dx,dy,dz,bx,by,bz,cx,cy,cz,ax,ay,az) == 0 ) | |
| assert( orientation(dx,dy,dz,cx,cy,cz,bx,by,bz,ax,ay,az) == 0 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.2; by = 1.2; bz=1.1 | |
| cx = 1.3; cy = 1.15; cz=1.1 | |
| dx = 1.4; dy = 1.17; dz=1.1 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == 0 ) | |
| assert( orientation(dx,dy,dz,bx,by,bz,cx,cy,cz,ax,ay,az) == 0 ) | |
| assert( orientation(dx,dy,dz,cx,cy,cz,bx,by,bz,ax,ay,az) == 0 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.5 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == 1 ) | |
| assert( orientation(dx,dy,dz,bx,by,bz,cx,cy,cz,ax,ay,az) == -1 ) | |
| assert( orientation(dx,dy,dz,cx,cy,cz,bx,by,bz,ax,ay,az) == 1 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.10000000000001 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == 1 ) | |
| assert( orientation(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz) == -1 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.09 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == -1 ) | |
| assert( orientation(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz) == 1 ) | |
| assert( orientation(ax,ay,az,bx,by,bz,dx,dy,dz,cx,cy,cz) == 1 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.099999999999999 | |
| assert( orientation(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz) == -1 ) | |
| assert( orientation(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz) == 1 ) | |
| assert( orientation(ax,ay,az,bx,by,bz,dx,dy,dz,cx,cy,cz) == 1 ) | |
| # 3D incircle | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.4; dy = 1.4; dz=1.2 | |
| ex = 1.1; ey = 1.1; ez=1.1 | |
| assert( incircle(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz,ex,ey,ez) == 0 ) | |
| assert( incircle(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz,ex,ey,ez) == 0 ) | |
| assert( incircle(ax,ay,az,bx,by,bz,dx,dy,dz,cx,cy,cz,ex,ey,ez) == 0 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.5 | |
| ex = 1.2; ey = 1.15; ez=1.13 | |
| assert( incircle(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz,ex,ey,ez) == 1 ) | |
| assert( incircle(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz,ex,ey,ez) == 1 ) | |
| assert( incircle(ax,ay,az,bx,by,bz,dx,dy,dz,cx,cy,cz,ex,ey,ez) == 1 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.5 | |
| ex = 1.2; ey = 1.15; ez=1.1000000000001 | |
| assert( incircle(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz,ex,ey,ez) == 1 ) | |
| assert( incircle(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz,ex,ey,ez) == 1 ) | |
| assert( incircle(ax,ay,az,bx,by,bz,dx,dy,dz,cx,cy,cz,ex,ey,ez) == 1 ) | |
| ax = 1.1; ay = 1.1; az=1.1 | |
| bx = 1.3; by = 1.1; bz=1.1 | |
| cx = 1.3; cy = 1.3; cz=1.1 | |
| dx = 1.2; dy = 1.2; dz=1.5 | |
| ex = 1.9; ey = 1.15; ez=1.01 | |
| assert( incircle(ax,ay,az,bx,by,bz,cx,cy,cz,dx,dy,dz,ex,ey,ez) == -1 ) | |
| assert( incircle(bx,by,bz,ax,ay,az,cx,cy,cz,dx,dy,dz,ex,ey,ez) == -1 ) | |
| assert( incircle(ax,ay,az,bx,by,bz,dx,dy,dz,cx,cy,cz,ex,ey,ez) == -1 ) | |
| # intriangle (2D)! | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.11; dy = 1.11 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == 1 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) == 1 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) == 1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.01; dy = 1.01 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) < 0 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) < 0 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) < 0 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.31; dy = 1.01 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) < 0 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) < 0 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) < 0 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.1; dy = 1.2 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == 3 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) == 2 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) == 2 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.2; dy = 1.1 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == 4 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) == 4 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) == 3 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.25; dy = 1.25 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == 2 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) == 3 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) == 4 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.25; dy = 1.250000000000001 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) < 0 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) < 0 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) < 0 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.25; dy = 1.249999999999999 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == 1 ) | |
| assert( intriangle(bx,by,ax,ay,cx,cy,dx,dy) == 1 ) | |
| assert( intriangle(bx,by,cx,cy,ax,ay,dx,dy) == 1 ) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.7; dy = 1.1 | |
| assert(intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -1) | |
| dx = 1.1; dy = 1.7 | |
| assert(intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -1) | |
| dx = 1.01; dy = 1.1 | |
| assert(intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -2) | |
| dx = 1.1; dy = 1.01 | |
| assert(intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -3) | |
| ax = 1.1; ay = 1.1 | |
| bx = 1.4; by = 1.1 | |
| cx = 1.1; cy = 1.4 | |
| dx = 1.55; dy = 1.55 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -1 ) | |
| dx = 1.09; dy = 1.11 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -2 ) | |
| dx = 1.11; dy = 1.09 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -3 ) | |
| ax = min_coord; ay = min_coord | |
| bx = 1.1; by = 1.1 | |
| cx = max_coord; cy = min_coord | |
| dx = 1.2; dy = 1.2 | |
| assert( intriangle(ax,ay,bx,by,cx,cy,dx,dy) == -1 ) | |
| # intriangle (3D) ! | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.11; py = 1.11; pz = 1.11 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == 1) | |
| assert(intriangle(bx, by, bz, ax, ay, az, cx, cy, cz, dx, dy, dz, px, py, pz) == 1) | |
| assert(intriangle(ax, ay, az, cx, cy, cz, bx, by, bz, dx, dy, dz, px, py, pz) == 1) | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.02; py = 1.2; pz = 1.2 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) < 0) | |
| assert(intriangle(bx, by, bz, ax, ay, az, cx, cy, cz, dx, dy, dz, px, py, pz) < 0) | |
| assert(intriangle(ax, ay, az, cx, cy, cz, bx, by, bz, dx, dy, dz, px, py, pz) < 0) | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.09999999999999; py = 1.2; pz = 1.2 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) < 0) | |
| assert(intriangle(bx, by, bz, ax, ay, az, cx, cy, cz, dx, dy, dz, px, py, pz) < 0) | |
| assert(intriangle(ax, ay, az, cx, cy, cz, bx, by, bz, dx, dy, dz, px, py, pz) < 0) | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.10000000000001; py = 1.2; pz = 1.2 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == 1) | |
| assert(intriangle(bx, by, bz, ax, ay, az, cx, cy, cz, dx, dy, dz, px, py, pz) == 1) | |
| assert(intriangle(ax, ay, az, cx, cy, cz, bx, by, bz, dx, dy, dz, px, py, pz) == 1) | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.11; py = 1.11; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == 5) | |
| assert(intriangle(bx, by, bz, ax, ay, az, cx, cy, cz, dx, dy, dz, px, py, pz) == 5) | |
| assert(intriangle(ax, ay, az, cx, cy, cz, bx, by, bz, dx, dy, dz, px, py, pz) == 5) | |
| assert(intriangle(dx, dy, dz, bx, by, bz, cx, cy, cz, ax, ay, az, px, py, pz) == 2) | |
| assert(intriangle(ax, ay, az, dx, dy, dz, cx, cy, cz, bx, by, bz, px, py, pz) == 3) | |
| assert(intriangle(ax, ay, az, bx, by, bz, dx, dy, dz, cx, cy, cz, px, py, pz) == 4) | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.55; py = 1.55; pz = 1.55 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.09; py = 1.11; pz = 1.11 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -2) | |
| px = 1.11; py = 1.09; pz = 1.11 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -3) | |
| px = 1.11; py = 1.11; pz = 1.09 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -4) | |
| ax = 1.1; ay = 1.1; az = 1.1 | |
| bx = 1.4; by = 1.1; bz = 1.1 | |
| cx = 1.1; cy = 1.4; cz = 1.1 | |
| dx = 1.1; dy = 1.1; dz = 1.4 | |
| px = 1.7; py = 1.1; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.7; py = 1.1; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.1; py = 1.7; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.1; py = 1.1; pz = 1.7 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.9; py = 1.9; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.9; py = 1.1; pz = 1.9 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.1; py = 1.9; pz = 1.9 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -1) | |
| px = 1.01; py = 1.1; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -2) | |
| px = 1.1; py = 1.01; pz = 1.1 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -3) | |
| px = 1.1; py = 1.1; pz = 1.01 | |
| assert(intriangle(ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz, px, py, pz) == -4) | |
| # Peano-Hilbert stuff | |
| assert(peanokey(Point2D(1.01,1.01), 4) == 0) | |
| assert(peanokey(Point2D(1.9901,1.01), 4) == 4*4-1) | |
| assert(Point2D(15, 4) == Point2D(1.75, 1.0)) | |
| assert(Point2D(0, 4) == Point2D(1.0, 1.0)) | |
| for x in linspace(1.0,2.0,100), y in linspace(1.0,2.0,100) | |
| p = Point2D(1.23, 1.45) | |
| d = peanokey(p, 1 << 30) | |
| pp= Point2D(d, 1 << 30) | |
| assert( abs(getx(p) - getx(pp)) < 1e-7) | |
| assert( abs(gety(p) - gety(pp)) < 1e-7) | |
| end | |
| # that's it for today! | |
| println("gpred: All tests passed!") | |
| end | |
| #test() | |
| ################ | |
| ################ | |
| function select_x_f!(v::Array{Point2D,1}, k::Int, lo::Int, hi::Int) | |
| lo <= k <= hi || error("select index $k is out of range $lo:$hi") | |
| @inbounds while lo < hi | |
| if hi-lo == 1 | |
| if v[hi]._x < v[lo]._x | |
| v[lo], v[hi] = v[hi], v[lo] | |
| end | |
| return v[k] | |
| end | |
| pivot = v[(lo+hi)>>>1] | |
| i, j = lo, hi | |
| while true | |
| while v[i]._x < pivot._x; i += 1; end | |
| while pivot._x < v[j]._x; j -= 1; end | |
| i <= j || break | |
| v[i], v[j] = v[j], v[i] | |
| i += 1; j -= 1 | |
| end | |
| if k <= j | |
| hi = j | |
| elseif i <= k | |
| lo = i | |
| else | |
| return pivot | |
| end | |
| end | |
| return v[lo] | |
| end | |
| function select_x_b!(v::Array{Point2D,1}, k::Int, lo::Int, hi::Int) | |
| lo <= k <= hi || error("select index $k is out of range $lo:$hi") | |
| @inbounds while lo < hi | |
| if hi-lo == 1 | |
| if v[hi]._x > v[lo]._x | |
| v[lo], v[hi] = v[hi], v[lo] | |
| end | |
| return v[k] | |
| end | |
| pivot = v[(lo+hi)>>>1] | |
| i, j = lo, hi | |
| while true | |
| while v[i]._x > pivot._x; i += 1; end | |
| while pivot._x > v[j]._x; j -= 1; end | |
| i <= j || break | |
| v[i], v[j] = v[j], v[i] | |
| i += 1; j -= 1 | |
| end | |
| if k <= j | |
| hi = j | |
| elseif i <= k | |
| lo = i | |
| else | |
| return pivot | |
| end | |
| end | |
| return v[lo] | |
| end | |
| function select_y_f!(v::Array{Point2D,1}, k::Int, lo::Int, hi::Int) | |
| lo <= k <= hi || error("select index $k is out of range $lo:$hi") | |
| @inbounds while lo < hi | |
| if hi-lo == 1 | |
| if v[hi]._y < v[lo]._y | |
| v[lo], v[hi] = v[hi], v[lo] | |
| end | |
| return v[k] | |
| end | |
| pivot = v[(lo+hi)>>>1] | |
| i, j = lo, hi | |
| while true | |
| while v[i]._y < pivot._y; i += 1; end | |
| while pivot._y < v[j]._y; j -= 1; end | |
| i <= j || break | |
| v[i], v[j] = v[j], v[i] | |
| i += 1; j -= 1 | |
| end | |
| if k <= j | |
| hi = j | |
| elseif i <= k | |
| lo = i | |
| else | |
| return pivot | |
| end | |
| end | |
| return v[lo] | |
| end | |
| function select_y_b!(v::Array{Point2D,1}, k::Int, lo::Int, hi::Int) | |
| lo <= k <= hi || error("select index $k is out of range $lo:$hi") | |
| @inbounds while lo < hi | |
| if hi-lo == 1 | |
| if v[hi]._y > v[lo]._y | |
| v[lo], v[hi] = v[hi], v[lo] | |
| end | |
| return v[k] | |
| end | |
| pivot = v[(lo+hi)>>>1] | |
| i, j = lo, hi | |
| while true | |
| while v[i]._y > pivot._y; i += 1; end | |
| while pivot._y > v[j]._y; j -= 1; end | |
| i <= j || break | |
| v[i], v[j] = v[j], v[i] | |
| i += 1; j -= 1 | |
| end | |
| if k <= j | |
| hi = j | |
| elseif i <= k | |
| lo = i | |
| else | |
| return pivot | |
| end | |
| end | |
| return v[lo] | |
| end | |
| function hilbertsort!(a::Array{Point2D,1}, lo::Int64, hi::Int64, upx::Bool, upy::Bool, x::Bool, lim::Int64=4) const y = !x | |
| hi-lo <= lim && return a | |
| const i2 = (lo+hi)>>>1 | |
| const i1 = (lo+i2)>>>1 | |
| const i3 = (i2+hi)>>>1 | |
| upx? (x? select_x_f!(a, i2, lo, hi) : select_y_f!(a, i2, lo, hi)) : (x? select_x_b!(a, i2, lo, hi) : select_y_b!(a, i2, lo, hi)) | |
| upy? (y? select_x_f!(a, i1, lo, i2) : select_y_f!(a, i1, lo, i2)) : (y? select_x_b!(a, i1, lo, i2) : select_y_b!(a, i1, lo, i2)) | |
| upy? (y? select_x_b!(a, i3, i2, hi) : select_y_b!(a, i3, i2, hi)) : (y? select_x_f!(a, i3, i2, hi) : select_y_f!(a, i3, i2, hi)) | |
| hilbertsort!(a, lo, i1, upy, upx, y, lim) | |
| hilbertsort!(a, i1, i2, upx, upy, x, lim) | |
| hilbertsort!(a, i2, i3, upx, upy, x, lim) | |
| hilbertsort!(a, i3, hi, !upy, !upx, y, lim) | |
| return a | |
| end | |
| hilbertsort!(a::Array{Point2D,1}) = hilbertsort!(a, 1, length(a), false, false, false) | |
| hilbertsort!(a::Array{Point2D,1}, lo::Int64, hi::Int64, lim::Int64) = hilbertsort!(a, lo, hi, false, false, false, lim) | |
| function mssort!(a::Array{Point2D,1}, lim_ms::Int64=16, lim_hl::Int64=4, rat::Float64=0.25) | |
| hi = length(a) | |
| lo = 1 | |
| while true | |
| lo = hi - int((1-rat)*hi) | |
| hi-lo <= lim_ms && return a | |
| #println("lo=",lo," hi=",hi) | |
| hilbertsort!(a, lo, hi, lim_hl) | |
| hi = lo-1 | |
| end | |
| return a | |
| end | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment