brush-strokes (empty) → 0.1.0.0
raw patch · 59 files changed
+20216/−0 lines, 59 filesdep +actsdep +basedep +bifunctorsbinary-added
Dependencies added: acts, base, bifunctors, brush-strokes, code-page, containers, deepseq, directory, eigen, falsify, filepath, fp-ieee, generic-lens, ghc-prim, groups, groups-generic, hspray, optparse-applicative, parallel, primitive, rounded-hw, system-cxx-std-lib, tasty, template-haskell, time, transformers, tree-view, unordered-containers
Files
- brush-strokes.cabal +440/−0
- cbits/lp_2d.cpp +2036/−0
- cbits/lp_2d.hpp +1252/−0
- cbits/mul.S +30/−0
- cbits/rounding.c +9/−0
- changelog.md +5/−0
- img/METAFONT_logo.svg +5/−0
- img/bench_cubics.png binary
- img/bench_quadratics.png binary
- img/clenshaw_error_landscape.png binary
- img/example_stroke.svg +316/−0
- readme.md +161/−0
- src/arc-length/bench/BenchArcLength.hs +390/−0
- src/arc-length/bench/plot_bench.py +290/−0
- src/arc-length/test/TestArcLength.hs +429/−0
- src/cusps/bench/Bench/Cases.hs +136/−0
- src/cusps/bench/Bench/Types.hs +160/−0
- src/cusps/bench/Main.hs +327/−0
- src/lib/Calligraphy/Brushes.hs +524/−0
- src/lib/Debug/Utils.hs +70/−0
- src/lib/Math/Algebra/Dual.hs +1376/−0
- src/lib/Math/Algebra/Dual/Internal.hs +705/−0
- src/lib/Math/Bezier/ArcLength.hs +739/−0
- src/lib/Math/Bezier/Cubic.hs +337/−0
- src/lib/Math/Bezier/Cubic/Fit.hs +334/−0
- src/lib/Math/Bezier/Quadratic.hs +238/−0
- src/lib/Math/Bezier/Spline.hs +659/−0
- src/lib/Math/Bezier/Stroke.hs +1712/−0
- src/lib/Math/Bezier/Stroke/EnvelopeEquation.hs +686/−0
- src/lib/Math/Differentiable.hs +82/−0
- src/lib/Math/Epsilon.hs +15/−0
- src/lib/Math/Float/Utils.hs +76/−0
- src/lib/Math/Interval.hs +489/−0
- src/lib/Math/Interval/Internal.hs +252/−0
- src/lib/Math/Interval/Internal/FMA.hs +221/−0
- src/lib/Math/Interval/Internal/RoundedHW.hs +53/−0
- src/lib/Math/Interval/Internal/SIMD.hs +86/−0
- src/lib/Math/Linear.hs +222/−0
- src/lib/Math/Linear/Internal.hs +212/−0
- src/lib/Math/Linear/Solve.hs +78/−0
- src/lib/Math/Module.hs +228/−0
- src/lib/Math/Module/Internal.hs +90/−0
- src/lib/Math/Monomial.hs +253/−0
- src/lib/Math/Orientation.hs +150/−0
- src/lib/Math/Ring.hs +185/−0
- src/lib/Math/Root/Isolation.hs +282/−0
- src/lib/Math/Root/Isolation/Bisection.hs +272/−0
- src/lib/Math/Root/Isolation/Core.hs +309/−0
- src/lib/Math/Root/Isolation/Degree.hs +128/−0
- src/lib/Math/Root/Isolation/Narrowing.hs +511/−0
- src/lib/Math/Root/Isolation/Newton.hs +182/−0
- src/lib/Math/Root/Isolation/Newton/GaussSeidel.hs +238/−0
- src/lib/Math/Root/Isolation/Newton/LP.hs +132/−0
- src/lib/Math/Root/Isolation/Utils.hs +50/−0
- src/lib/Math/Roots.hs +627/−0
- src/lib/Math/Taylor.hs +933/−0
- src/lib/TH/Utils.hs +23/−0
- src/simd-interval/Math/Interval/Internal/SIMD/Internal.hs +152/−0
- src/test/Main.hs +319/−0
+ brush-strokes.cabal view
@@ -0,0 +1,440 @@+cabal-version: 3.0 +name: brush-strokes +version: 0.1.0.0 +synopsis: Toolkit for Bézier curves and brush stroking +category: Calligraphy, Geometry, Graphics +license: BSD-3-Clause +homepage: https://gitlab.com/sheaf/metabrush/-/tree/master/brush-strokes +author: Sam Derbyshire +maintainer: Sam Derbyshire +build-type: Simple +description: + A toolkit for handling quadratic and cubic Bézier curves, splines, and + stroking. +extra-doc-files: + changelog.md + readme.md + img/*.svg + img/*.png + +extra-source-files: + cbits/lp_2d.cpp + cbits/lp_2d.hpp + cbits/mul.S + cbits/rounding.c + src/arc-length/bench/plot_bench.py + +source-repository head + type: git + location: https://gitlab.com/sheaf/MetaBrush + +flag use-simd + description: Use SIMD instructions to implement interval arithmetic. + default: False + manual: True + +flag use-fma + description: Use fused-muliply add instructions to implement interval arithmetic. + default: False + manual: True + +flag asserts + description: Enable debugging assertions. + default: False + manual: True + +common common + + build-depends: + -- This package requires GHC >= 9.14 due to usage of expressions in SPECIALISE pragmas. + -- Corresponding base version: 4.22 + base + >= 4.22 && < 5 + , code-page + ^>= 0.2.1 + , containers + >= 0.6.0.1 && < 0.8 + , deepseq + >= 1.4.4.0 && < 1.6 + , primitive + ^>= 0.9 + , tree-view + ^>= 0.5 + + default-extensions: + BangPatterns + BlockArguments + ConstraintKinds + DataKinds + DeriveAnyClass + DeriveTraversable + DeriveGeneric + DerivingVia + FlexibleContexts + FlexibleInstances + FunctionalDependencies + GADTs + GeneralisedNewtypeDeriving + InstanceSigs + LambdaCase + LexicalNegation + MagicHash + MultiWayIf + NamedFieldPuns + NoStarIsType + PatternSynonyms + RankNTypes + RecordWildCards + RoleAnnotations + StandaloneDeriving + StandaloneKindSignatures + TupleSections + TypeAbstractions + TypeApplications + TypeFamilyDependencies + TypeOperators + UnboxedTuples + ViewPatterns + + ghc-options: + -fexpose-all-unfoldings + -- -funfolding-use-threshold=1000 + -fspecialise-aggressively + -fpolymorphic-specialisation + -flate-dmd-anal + -fmax-worker-args=200 + -optc-O3 + -Wall + -Wcompat + -fwarn-missing-local-signatures + -fwarn-incomplete-patterns + -fwarn-incomplete-uni-patterns + -fwarn-missing-deriving-strategies + -fno-warn-unticked-promoted-constructors + + if flag(use-fma) + cpp-options: + -DUSE_FMA + ghc-options: + -mfma + + if flag(use-simd) + cpp-options: + -DUSE_SIMD + ghc-options: + -mavx + + if flag(asserts) + cpp-options: + -DASSERTS + +common extra + + build-depends: + acts + ^>= 0.3.1.0 + , generic-lens + >= 2.2 && < 2.3 + , groups + ^>= 0.5.3 + +library + + import: + common, extra + + hs-source-dirs: + src/lib + + default-language: + Haskell2010 + + exposed-modules: + Calligraphy.Brushes + , Math.Algebra.Dual + , Math.Bezier.ArcLength + , Math.Bezier.Cubic + , Math.Bezier.Cubic.Fit + , Math.Bezier.Quadratic + , Math.Bezier.Spline + , Math.Bezier.Stroke + , Math.Bezier.Stroke.EnvelopeEquation + , Math.Differentiable + , Math.Epsilon + , Math.Float.Utils + , Math.Interval + , Math.Linear + , Math.Linear.Solve + , Math.Module + , Math.Monomial + , Math.Orientation + , Math.Ring + , Math.Roots + , Math.Root.Isolation + , Math.Root.Isolation.Bisection + , Math.Root.Isolation.Core + , Math.Root.Isolation.Degree + , Math.Root.Isolation.Narrowing + , Math.Root.Isolation.Newton + , Math.Root.Isolation.Newton.GaussSeidel + , Math.Root.Isolation.Newton.LP + , Math.Root.Isolation.Utils + , Math.Taylor + , Debug.Utils + + other-modules: + Math.Algebra.Dual.Internal + , Math.Interval.Internal + , Math.Interval.Internal.FMA + , Math.Linear.Internal + , Math.Module.Internal + , TH.Utils + + if flag(use-simd) + other-modules: + Math.Interval.Internal.SIMD + build-depends: + simd-interval + , ghc-prim + >= 0.13 && < 1.0 + + if !flag(use-fma) && !flag(use-simd) + other-modules: + Math.Interval.Internal.RoundedHW + + build-depends: + template-haskell + >= 2.18 && < 3 + , bifunctors + >= 5.5.4 && < 5.7 + , directory + >= 1.3.7.1 && < 1.4 + , eigen + ^>= 3.3.7.0 + , filepath + >= 1.4 && < 1.6 + , fp-ieee + ^>= 0.1.0.4 + , groups-generic + ^>= 0.3.1.0 + , parallel + >= 3.2.2.0 && < 3.4 + , rounded-hw + ^>= 0.4 + , time + >= 1.12.2 && < 1.15 + , transformers + >= 0.5.6.2 && < 0.7 + + include-dirs: + cbits + c-sources: + -- C code for: setting the rounding mode + cbits/rounding.c + cxx-sources: + -- Walter's C++ code for 2D linear systems of interval equations + cbits/lp_2d.cpp + cxx-options: + -std=c++20 + -mavx2 + -mfma + -frounding-math + -ffp-contract=off + -fno-math-errno + -fno-signed-zeros + -fno-trapping-math + -Wno-unused-result + -Wno-sign-compare + -Wno-switch + -march=native + -mtune=native + -DNDEBUG + build-depends: + system-cxx-std-lib == 1.0 + +-- Separate library to implement interval arithmetic using SIMD, +-- to work around TH linking bugs: +-- +-- - https://gitlab.haskell.org/ghc/ghc/-/issues/25240 +-- - https://github.com/haskell/cabal/issues/5623 +library simd-interval + + import: common + + default-language: + Haskell2010 + + hs-source-dirs: + src/simd-interval + + exposed-modules: + Math.Interval.Internal.SIMD.Internal + + include-dirs: + cbits + asm-sources: + -- X86 assembly for interval multiplication using SIMD instructions + cbits/mul.S + + build-depends: + ghc-prim + >= 0.13 && < 1 + , rounded-hw + ^>= 0.4 + +--executable convert-metafont +-- +-- import: +-- common +-- +-- hs-source-dirs: +-- src/metafont +-- +-- default-language: +-- Haskell2010 +-- +-- main-is: +-- Main.hs +-- +-- other-modules: +-- Calligraphy.MetaFont.Convert +-- +-- build-depends: +-- diagrams-contrib, +-- diagrams-lib, +-- linear, +-- parsec + +--executable inspect +-- +-- import: +-- common, extra +-- +-- hs-source-dirs: +-- src/cusps/inspect +-- +-- default-language: +-- Haskell2010 +-- +-- main-is: +-- Main.hs +-- +-- other-modules: +-- Math.Interval.Abstract +-- +-- build-depends: +-- brush-strokes +-- , data-reify +-- ^>= 0.6.3 + +test-suite tests + + import: + common + + type: + exitcode-stdio-1.0 + + hs-source-dirs: + src/test + + default-language: + Haskell2010 + + main-is: + Main.hs + + build-depends: + brush-strokes + , falsify + ^>= 0.2 + , hspray + ^>= 0.1.3 + , tasty + ^>= 1.5 + , unordered-containers + >= 0.2.15 && < 0.3 + +test-suite test-arc-length + + import: + common + + type: + exitcode-stdio-1.0 + + hs-source-dirs: + src/arc-length/test + + default-language: + Haskell2010 + + main-is: + TestArcLength.hs + + ghc-options: + -threaded -rtsopts + + build-depends: + brush-strokes + , falsify + ^>= 0.2 + , optparse-applicative + >= 0.17 && < 0.19 + , parallel + >= 3.2.2.0 && < 3.4 + , tasty + ^>= 1.5 + +benchmark bench-arc-length + + import: + common + + hs-source-dirs: + src/arc-length/bench + + main-is: + BenchArcLength.hs + + build-depends: + brush-strokes + + type: + exitcode-stdio-1.0 + + default-language: + Haskell2010 + + ghc-options: + -O2 + -threaded + -rtsopts + +benchmark cusps + + import: + common + + hs-source-dirs: + src/cusps/bench + + main-is: + Main.hs + + other-modules: + Bench.Types + Bench.Cases + + build-depends: + brush-strokes + + type: + exitcode-stdio-1.0 + + default-language: + Haskell2010 + + ghc-options: + -threaded + -rtsopts
+ cbits/lp_2d.cpp view
@@ -0,0 +1,2036 @@+//---------------------------------------------------------------------------------------- +// Copyright (c) 2024 Walter Mascarenhas +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. +// +// The MPL has a "Disclaimer of Warranty" and a "Limitation of Liability", +// and we provide no guarantee regarding the correctness of this source code. +// +//---------------------------------------------------------------------------------------- + +#include <algorithm> +#include <cfenv> +#include <cstdint> +#include <functional> +#include <iostream> + +#include "lp_2d.hpp" + +//---------------------------------------------------------------------------------------- +namespace wm::nlp::lp2d { + +__m256i bounds( __m256i* tiny, __m256i* huge ) +{ + __m256i ones = ones_i(); + __m256i sign = _mm256_slli_epi64( ones, 63 ); + __m256i a = _mm256_srli_epi64( sign, 1 ); // 0b0100'0000'0000... + __m256i b = _mm256_srli_epi64( sign, 3 ); // 0b0001'0000'0000... + __m256i c = _mm256_or_si256( a, b ); // 0b0101'0000'0000... + __m256i d = _mm256_srli_epi64( c, 4 ); // 0b0000'0101'0000... + + *huge = _mm256_or_si256( c, d ); // 0b0101'0101'0000... 0x5500... + + __m256i e = _mm256_srli_epi64( *huge, 1 ); // 0b0010'1010'1000... + __m256i f = _mm256_srli_epi64( d, 2 ); // 0b0000'0001'0100... + __m256i g = _mm256_srli_epi64( d, 4 ); // 0b0000'0000'0101... + __m256i h = _mm256_srli_epi64( ones, 1 ); + __m256i i = _mm256_srli_epi64( sign, 32 ); + + __m256i j = _mm256_or_si256( f, g ); // 0b0000'0001'0101... + + *tiny = _mm256_or_si256( e, j ); // 0b0010'1011'1101... 0x2bd... + __m256i k = and_not_i( i, h ); + *huge = blend< 0, 0, 0, 1 >( *huge, k ); + *tiny = blend< 0, 0, 0, 1 >( *tiny, _mm256_setzero_si256() ); + return sign; +} + +//---------------------------------------------------------------------------------------- +// exact_location returns the exact location of the intersection ( x, y ) of the lines +// +// b0[ 0 ] x + b0[ 1 ] y >= b0[ 2 ] +// b1[ 1 ] x + b1[ 1 ] y >= b1[ 2 ] +// +// with respect to the set +// +// e0 x + e1 y >= e2 +// +// under the assumption that +// +// d = b0[ 0 ] b1[ 1 ] - b0[ 1 ] b1[ 1 ] > 0 +// +// where ei = edge[ i ], b0 = border[ 0 ] and b1 = b[ 1 ] +// +// This amounts to evaluating the sign of +// +// p = e0 r + e1 s - e2 d +// +// r = b0[ 2 ] b1[ 1 ] - b0[ 1 ] b1[ 2 ] +// s = b0[ 0 ] b1[ 2 ] - b0[ 2 ] b1[ 0 ] , +// +// which motivates the evaluation of the six products +// +// group 1 group 2 +// e0 b0[ 2 ] b1[ 1 ], -e0 b0[ 1 ] b1[ 2 ] +// e1 b0[ 0 ] b1[ 2 ], -e1 b0[ 2 ] b1[ 0 ] +// e2 b0[ 1 ] b1[ 0 ]. -e2 b0[ 0 ] b1[ 1 ] +// +// and we want to find the sign of their sum. +// +// The function assumes that the edges entries are either normal or zero, ie., +// there are no subnormal entries. +//---------------------------------------------------------------------------------------- + +location exact_location( __m256d e, __m256d b0, __m256d b1 ) +{ + //---------------------------------------------------------------------------------------- + //---------------------------------------------------------------------------------------- + + __m256d b0_a = permute< 1, 2, 0, 3 >( b0 ); // { b0[ 1 ], b0[ 2 ], b0[ 0 ], ? } + __m256d b0_b = permute< 2, 0, 1, 3 >( b0 ); // { b0[ 2 ], b0[ 0 ], b0[ 1 ], ? } + __m256d b1_a = permute< 1, 2, 0, 3 >( b1 ); // { b1[ 1 ], b1[ 2 ], b1[ 0 ], ? } + __m256d b1_b = permute< 2, 0, 1, 3 >( b1 ); // { b1[ 2 ], b1[ 0 ], b1[ 1 ], ? } + + __m256d g1 = _mm256_mul_pd( b0_b, b1_a ); // { b0[ 2 ] b1[ 1 ], b0[ 0 ] b1[ 2 ], b0[ 1 ] b1[ 0 ], ? } + __m256d g2 = _mm256_mul_pd( b0_a, b1_b ); // { b0[ 1 ] b1[ 2 ], b0[ 2 ] b1[ 0 ], b0[ 0 ] b1[ 1 ], ? } + + __m256d g1b = _mm256_fmsub_pd( b0_b, b1_a, g1 ); // { b0[ 2 ] b1[ 1 ], b0[ 0 ] b1[ 2 ], b0[ 1 ] b1[ 0 ], ? } + __m256d g2b = _mm256_fmsub_pd( b0_a, b1_b, g2 ); // { b0[ 1 ] b1[ 2 ], b0[ 2 ] b1[ 0 ], b0[ 0 ] b1[ 1 ], ? } + + __m256i sign_i = _mm256_slli_epi64( ones_i(), 63 ); + __m256d sign = cast_d( sign_i ); + __m256d minus_e = xor_d( e, sign ); + + __m256d c[ 8 ]; + + c[ 0 ] = _mm256_mul_pd( e, g1 ); + c[ 1 ] = _mm256_mul_pd( minus_e, g2 ); + c[ 2 ] = _mm256_mul_pd( e, g1b ); + c[ 3 ] = _mm256_mul_pd( minus_e, g2b ); + + c[ 4 ] = _mm256_fmsub_pd( e, g1, c[ 0 ] ); + c[ 5 ] = _mm256_fmsub_pd( minus_e, g2, c[ 1 ] ); + c[ 6 ] = _mm256_fmsub_pd( e, g1b, c[ 2 ] ); + c[ 7 ] = _mm256_fmsub_pd( minus_e, g2b, c[ 3 ] ); + + __m256d a[ 6 ]; + + // transposing the products + + __m256d tmp_0 = gather_low( c[ 0 ], c[ 1 ] ); + __m256d tmp_1 = gather_high( c[ 0 ], c[ 1 ] ); + __m256d tmp_2 = gather_low( c[ 2 ], c[ 3 ] ); + __m256d tmp_3 = gather_high( c[ 2 ], c[ 3 ] ); + + a[ 0 ] = _mm256_permute2f128_pd( tmp_0, tmp_2, 0x20 ); + a[ 1 ] = _mm256_permute2f128_pd( tmp_1, tmp_3, 0x20 ); + a[ 2 ] = _mm256_permute2f128_pd( tmp_0, tmp_2, 0x31 ); + + __m256d tmp_4 = gather_low( c[ 4 ], c[ 5 ] ); + __m256d tmp_5 = gather_high( c[ 4 ], c[ 5 ] ); + __m256d tmp_6 = gather_low( c[ 6 ], c[ 7 ]); + __m256d tmp_7 = gather_high( c[ 6 ], c[ 7 ] ); + + a[ 3 ] = _mm256_permute2f128_pd( tmp_4, tmp_6, 0x20 ); + a[ 4 ] = _mm256_permute2f128_pd( tmp_5, tmp_7, 0x20 ); + a[ 5 ] = _mm256_permute2f128_pd( tmp_4, tmp_6, 0x31 ); + + __m256d gt_05 = is_greater( a[ 0 ], a[ 5 ] ); + __m256d tmp = blend_max( a[ 0 ], a[ 5 ], gt_05 ); + a[ 0 ] = blend_min( a[ 0 ], a[ 5 ], gt_05 ); + a[ 5 ] = tmp; + + //---------------------------------------------------------------------------------------- + // In the loop below we assume that a0[ j ] <= a5[ j ], i = 0,1,2,3 + //---------------------------------------------------------------------------------------- + + while( true ) + { + __m256d gt_12 = is_greater( a[ 1 ], a[ 2 ] ); + __m256d gt_34 = is_greater( a[ 3 ], a[ 4 ] ); + + __m256d min_12 = blend_min( a[ 1 ], a[ 2 ], gt_12 ); + __m256d max_12 = blend_max( a[ 1 ], a[ 2 ], gt_12 ); + + __m256d min_34 = blend_min( a[ 3 ], a[ 4 ], gt_34 ); + __m256d max_34 = blend_max( a[ 3 ], a[ 4 ], gt_34 ); + + __m256d gt_min = is_greater( min_12, min_34 ); + __m256d gt_max = is_greater( max_12, max_34 ); + + __m256d min_1234 = blend_min( min_12, min_34, gt_min ); + a[ 2 ] = blend_max( min_12, min_34, gt_min ); + + a[ 3 ] = blend_min( max_12, max_34, gt_max ); + __m256d max_1234 = blend_max( max_12, max_34, gt_max ); + + __m256d gt_0_min_1234 = is_greater( a[ 0 ], min_1234 ); + __m256d gt_max_1234_5 = is_greater( max_1234, a[ 5 ] ); + + a[ 1 ] = blend_max( a[ 0 ], min_1234, gt_0_min_1234 ); + a[ 0 ] = blend_min( a[ 0 ], min_1234, gt_0_min_1234 ); + + a[ 4 ] = blend_min( max_1234, a[ 5 ], gt_max_1234_5 ); + a[ 5 ] = blend_max( max_1234, a[ 5 ], gt_max_1234_5 ); + + // 1) a0[ j ] = min { ai[ j ], i = 0, .... 5 } + // 2) a5[ j ] = max { ai[ j ], i = 0, .... 5 } + + __m256d zero = _mm256_setzero_pd(); + __m256d cmp0 = _mm256_cmp_pd( a[ 0 ], zero, _CMP_LT_OQ ); + + if( _mm256_testz_pd( cmp0, cmp0 ) ) // all a0[ j ] are >= 0 + { + __m256d cmp = is_greater( a[ 5 ], zero ); + if( _mm256_testz_pd( cmp, cmp ) ) // no entry in a5 is positive + return location::border; + else + return location::in; + } + + __m256d cmp5 = _mm256_cmp_pd( a[ 5 ], zero, _CMP_GT_OQ ); + + if( _mm256_testz_pd( cmp5, cmp5 ) ) // all a5[ j ] are <= 0 + { + __m256d cmp = is_greater( zero, a[ 0 ] ); + if( _mm256_testz_pd( cmp, cmp ) ) // no entry in a0 is negative + return location::border; + else + return location::out; + } + + // now + // 1) min ai[j] has its sign bit set + // 2) max ai[j] does not have its sign bit set + + __m256d lo = gather_low( a[ 0 ], a[ 5 ] ); // { a0[ 0 ], a5[ 0 ], a0[ 2 ], a5[ 2 ] } + __m256d hi = gather_high( a[ 0 ], a[ 5 ] ); // { a0[ 1 ], a5[ 1 ], a0[ 3 ], a5[ 3 ] } + + __m256d gt_lo_hi = is_greater( lo, hi ); + __m256d b0 = blend_min( lo, hi, gt_lo_hi ); + __m256d b5 = blend_max( lo, hi, gt_lo_hi ); + + // 1) min{ ai[ j ], i,j = 0,...5 } is in { b0[ 0 ], b0[ 2 ] } + // 2) max{ ai[ j ], i,j = 0,...5 } is in { b5[ 1 ], b5[ 3 ] } + + b0 = permute< 0, 2, 1, 3 >( b0 ); + b5 = permute< 3, 1, 2, 0 >( b5 ); + + // 1) min{ ai[ j ], i,j = 0,...5 } is in { b0[ 0 ], b0[ 1 ] } + // 2) max{ ai[ j ], i,j = 0,...5 } is in { b5[ 0 ], b5[ 1 ] } + + lo = gather_low( b0, b5 ); // { b0[ 0 ], b5[ 0 ], b0[ 2 ], b5[ 2 ] } + hi = gather_high( b0, b5 ); // { b0[ 1 ], b5[ 1 ], b0[ 3 ], b5[ 3 ] } + + gt_lo_hi = is_greater( lo, hi ); + b0 = blend_min( lo, hi, gt_lo_hi ); + b5 = blend_max( lo, hi, gt_lo_hi ); + + // 1) min{ ai[ j ], i,j = 0,...5 } is b0[ 0 ] + // 2) max{ ai[ j ], i,j = 0,...5 } is b5[ 1 ] + + b5 = permute< 1, 0, 3, 2 >( b5 ); + + // 1) min{ ai[ j ], i,j = 0,...5 } is b0[ 0 ] <= 0 + // 2) max{ ai[ j ], i,j = 0,...5 } is b5[ 0 ] >= 0 s + + // fixing possible break of order in positions 1,2,3 + + __m256d chk123 = is_greater( b0, b5 ); + __m256d aux = blend_min( b0, b5, chk123 ); + b5 = blend_max( b0, b5, chk123 ); + b0 = aux; + + __m256i b0i = cast_i( b0 ); + __m256i b5i = cast_i( b5 ); + + uint64_t u0 = _mm256_extract_epi64( b0i, 0 ) & 0x7FFF'FFFF'FFFF'FFFFULL; + uint64_t u5 = _mm256_extract_epi64( b5i, 0 ); + + if( u0 >= u5 ) // | b0[ 0 | >= | b5[ 0 ] | + { + if( u0 > u5 + 0x0060'0000'0000'0000ULL ) // |b0| > 2^5 |b1| (we use 0x006... to handle subnormals properly) + return location::out; + + __m256d sum = _mm256_add_pd( b0, b5 ); // b0 + b5, rounded down + __m256d cor = _mm256_sub_pd( sum, b0 ); // sum - b0, rounded down + __m256d delta = _mm256_sub_pd( b5, cor ); // b5 - cor, rounded down. + // now b0[ 0 ] + b5[ 0 ] = sum[ 0 ] + delta[ 0 ], exactly, with sum[ 0 ] <= 0 <= delta[ 0 ] + a[ 0 ] = blend< 1, 0, 0, 0 >( b0, sum ); + a[ 5 ] = blend< 1, 0, 0, 0 >( b5, delta ); + } + else + { + if( u5 > u0 + 0x0060'0000'0000'0000ULL ) + return location::in; + + __m256i sign_i = _mm256_slli_epi64( ones_i(), 63 ); + __m256d sign = cast_d( sign_i ); + __m256d minus_b0 = xor_d( b0, sign ); + + __m256d sum = _mm256_sub_pd( minus_b0, b5 ); // (-b5) + (-b0) rounded down + __m256d cor = _mm256_add_pd( sum, b5 ); // sum - (-b5) rounded down + __m256d delta = _mm256_sub_pd( minus_b0, cor ); // (-b0) - cor, rounded down. + + // -( b0[ 0 ] + b5[ 0 ] ) = sum[ 0 ] + delta[ 0 ], exactly, with sum[ 0 ] <= 0 <= delta[ 0 ] + + sum = xor_d( sum, sign ); + delta = xor_d( delta, sign ); + + // b0[ 0 ] + b5[ 0 ] ) = sum[ 0 ] + delta[ 0 ], exactly, with delta[ 0 ] <= 0 <= sum[ 0 ] + + a[ 0 ] = blend< 1, 0, 0, 0 >( b0, delta ); + a[ 5 ] = blend< 1, 0, 0, 0 >( b5, sum ); + } + } +} + +//---------------------------------------------------------------------------------------- +// bounded_convex::box: finding the box of the bounded convex region, under the assumption +// that the normals are normalized, that is, for each edge e, either +// e[ i ] = +0.0 or e[ i ] >= tiny ~ 2^(-100), i.e. no signed zeros, underflow +// or overflow or negative coordinates. +//---------------------------------------------------------------------------------------- + +__m256d bounded_convex::box() const +{ + constexpr int non_0 = 0; + constexpr int non_1 = 2; + constexpr int inf_0 = 0; + constexpr int sup_0 = 1; + constexpr int inf_1 = 2; + constexpr int sup_1 = 3; + + assert( type != bounded_convex_type::empty ); + + __m256d den = _mm256_setzero_pd(); + __m256d num = _mm256_setzero_pd(); + __m256i sign = _mm256_slli_epi64( ones_i(), 63 ); // { -1, -1, -1, -1 } + __m256i sign_h = _mm256_slli_si256( sign, 8 ); // { 0, -1, 0, -1 } + + if( type != bounded_convex_type::polygon ) + { + if( type == bounded_convex_type::point ) + { + // { vertex_pair[ 0 ] = { x_inf, -x_sup, ? , ? } + // { vertex_pair[ 1 ] = { y_inf, -y_sup, ? , ? } + // { vertex_pair[ 2 ] = { z_inf, -z_sup, ? , ? } + + den = xor_d( vertex_p[ 2 ], sign_h ); + den = permute< sup_0, inf_0, sup_0, inf_0 >( den ); + num = gather_lanes< 0, 0 >( vertex_p[ 0 ], vertex_p[ 1 ] ); + } + else + { + __m256d num_x; + __m256d num_y; + + den = xor_d( vertex_p[ 2 ], sign_h ); + + int xy_signs = high_bits_64( edge[ 1 ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // edge[ 0 ] >= 0, edge[ 1 ] >= 0: box = { x0, x1, y1, y0 } + { + num_x = permute< inf_0, sup_1, non_1, non_1 >( vertex_p[ 0 ] ); + num_y = permute< non_0, non_0, inf_1, sup_0 >( vertex_p[ 1 ] ); + + den = permute< sup_0, inf_1, sup_1, inf_0 >( den ); + break; + } + case 1: // edge[ 0 ] < 0, edge[ 1 ] >= 0: box = { x0, x1, y0, y1 } + { + num_x = permute< inf_0, sup_1, non_1, non_1 >( vertex_p[ 0 ] ); + num_y = permute< non_0, non_0, inf_0, sup_1 >( vertex_p[ 1 ] ); + + den = permute< sup_0, inf_0, sup_0, inf_1 >( den ); + break; + } + case 2: // edge[ 0 ] >= 0, edge[ 1 ] < 0: box = {x1, x0, y1, y0 } + { + num_x = permute< inf_1, sup_0, non_1, non_1 >( vertex_p[ 0 ] ); + num_y = permute< non_0, non_0, inf_1, sup_0 >( vertex_p[ 1 ] ); + + den = permute< sup_1, inf_0, sup_1, inf_0 >( den ); + + break; + } + case 3: // edge[ 0 ] < 0, edge[ 1 ] < 0: box = { x1, x0, y0, y1 } + { + num_x = permute< inf_1, sup_0, non_1, non_1 >( vertex_p[ 0 ] ); + num_y = permute< non_0, non_0, inf_0, sup_1 >( vertex_p[ 1 ] ); + + den = permute< sup_1, inf_0, sup_0, inf_1 >( den ); + break; + } + } + + num = gather_lanes< 0, 1 >( num_x, num_y ); + } + } + else + { + int index = begin; + + auto set_x_max = [ & ]() + { + int previous_vertex = ( ( index > 0 ) ? index : end ); + int previous_pair = 3 * ( --previous_vertex / 2 ); + + __m256d z = xor_d( vertex_p[ previous_pair + 2 ], sign_h ); + + if( previous_vertex & 0x1 ) + { + z = permute< non_0, inf_1, non_1, non_1 >( z ); + __m256d x = gather_lanes< 1, 1 >( vertex_p[ previous_pair ] ); + num = blend< 0, 1, 0, 0 >( num, x ); + } + else + { + z = permute< non_0, inf_0, non_1, non_1 >( z ); + num = blend< 0, 1, 0, 0 >( num, vertex_p[ previous_pair ] ); + } + + den = blend< 0, 1, 0, 0 >( den, z ); + }; + + auto set_x_min = [ & ]() + { + int previous_vertex = ( ( index > 0 ) ? index : end ); + int previous_pair = 3 * ( --previous_vertex / 2 ); + + __m256d z = xor_d( vertex_p[ previous_pair + 2 ], sign_h ); + + if( previous_vertex & 0x1 ) + { + z = permute< sup_1, non_0, non_1, non_1 >( z ); + __m256d x = gather_lanes< 1, 1 >( vertex_p[ previous_pair ] ); + num = blend< 1, 0, 0, 0 >( num, x ); + } + else + { + z = permute< sup_0, non_0, non_1, non_1 >( z ); + num = blend< 1, 0, 0, 0 >( num, vertex_p[ previous_pair ] ); + } + + den = blend< 1, 0, 0, 0 >( den, z ); + }; + + auto set_y_max = [ & ]() + { + int previous_vertex = ( ( index > 0 ) ? index : end ); + int previous_pair = 3 * ( --previous_vertex / 2 ); + + __m256d z = xor_d( vertex_p[ previous_pair + 2 ], sign_h ); + + if( previous_vertex & 0x1 ) + { + z = permute< non_0, non_0, non_1, inf_1 >( z ); + num = blend< 0, 0, 0, 1 >( num, vertex_p[ previous_pair + 1 ] ); + } + else + { + z = permute< non_0, non_0, non_1, inf_0 >( z ); + __m256d y = gather_lanes< 0, 0 >( vertex_p[ previous_pair + 1 ] ); + num = blend< 0, 0, 0, 1 >( num, y ); + } + + den = blend< 0, 0, 0, 1 >( den, z ); + }; + + auto set_y_min = [ & ]() + { + int previous_vertex = ( ( index > 0 ) ? index : end ); + int previous_pair = 3 * ( --previous_vertex / 2 ); + + __m256d z = xor_d( vertex_p[ previous_pair + 2 ], sign_h ); + + if( previous_vertex & 0x1 ) + { + z = permute< non_1, non_1, sup_1, non_1 >( z ); + num = blend< 0, 0, 1, 0 >( num, vertex_p[ previous_pair + 1 ] ); + } + else + { + z = permute< non_0, non_0, sup_0, non_0 >( z ); + __m256d y = gather_lanes< 0, 0 >( vertex_p[ previous_pair + 1 ] ); + num = blend< 0, 0, 1, 0 >( num, y ); + } + + den = blend< 0, 0, 1, 0 >( den, z ); + }; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // normal[ 0 ] >= 0, normal[ 1 ] >= 0: first quadrant + { + if( index & 0x1 ) + goto quadrant_1_odd; + else + goto quadrant_1_even; + } + case 1: // normal[ 0 ] < 0, normal[ 1 ] >= 0: second quadrant + { + if( index & 0x1 ) + goto quadrant_2_odd; + else + goto quadrant_2_even; + } + case 2: // normal[ 0 ] >= 0, normal[ 1 ] < 0: fourth quadrant + { + if( index & 0x1 ) + goto quadrant_4_odd; + else + goto quadrant_4_even; + } + case 3: // normal[ 0 ] < 0, normal[ 1 ] < 0: third quadrant + { + if( index & 0x1 ) + goto quadrant_3_odd; + else + goto quadrant_3_even; + } + } + + quadrant_1_even: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // quadrant_1 + goto quadrant_1_odd; + case 1: // quadrant_2 + { + set_y_min(); + goto quadrant_2_odd; + } + case 3: // quadrant_3 + { + set_x_max(); + set_y_min(); + goto quadrant_3_odd; + } + } + } + + quadrant_1_odd: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // quadrant_1 + goto quadrant_1_even; + case 1: // quadrant_2 + { + set_y_min(); + goto quadrant_2_even; + } + case 3: // quadrant_3 + { + set_x_max(); + set_y_min(); + goto quadrant_3_even; + } + } + } + + quadrant_2_even: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 1: // quadrant_2 + goto quadrant_2_odd; + case 2:// quadrant_4 + { + set_x_max(); + set_y_max(); + goto quadrant_4_odd; + } + case 3: // quadrant_3 + { + set_x_max(); + goto quadrant_3_odd; + } + } + } + + quadrant_2_odd: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 1: // quadrant_2 + goto quadrant_2_even; + case 2:// quadrant_4 + { + set_x_max(); + set_y_max(); + goto quadrant_4_even; + } + case 3: // quadrant_3 + { + set_x_max(); + goto quadrant_3_even; + } + } + } + + quadrant_3_even: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // quadrant_1 + { + set_y_max(); + set_x_min(); + goto quadrant_1_odd; + } + case 2:// quadrant_4 + { + set_y_max(); + goto quadrant_4_odd; + } + case 3: // quadrant_3 + goto quadrant_3_odd; + } + } + + quadrant_3_odd: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // quadrant_1 + { + set_x_min(); + set_y_max(); + goto quadrant_1_even; + } + case 2:// quadrant_4 + { + set_y_max(); + goto quadrant_4_even; + } + case 3: // quadrant_3 + goto quadrant_3_even; + } + } + + quadrant_4_even: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // quadrant_1 + { + set_x_min(); + goto quadrant_1_odd; + } + case 1: // quadrant_2 + { + set_x_min(); + set_y_min(); + goto quadrant_2_odd; + } + case 2:// quadrant_4 + goto quadrant_4_odd; + } + } + + quadrant_4_odd: + { + if( ++index > end ) + goto done; + + int xy_signs = high_bits_64( edge[ index ] ) & 0x3; + + switch( xy_signs ) + { + case 0: // quadrant_1 + { + set_x_min(); + goto quadrant_1_even; + } + case 1: // quadrant_2 + { + set_x_min(); + set_y_min(); + goto quadrant_2_even; + } + case 2:// quadrant_4 + goto quadrant_4_even; + } + } + + done: {} + } + + num = _mm256_div_pd( num, den ); + num = xor_d( num, sign_h ); + return num; +} + +int bounded_convex::insert_vertex( int at ) +{ + int high = end - at; + int low = at - begin; + assert( ( low > 0 ) && ( high > 0 ) ); + + if( low <= high ) + { + __m256d* ed = edge + ( begin - 1 ); + __m256d* ed_max = ed + low; + + for( ; ed < ed_max; ++ed ) + { + __m256d tmp = _mm256_load_pd( reinterpret_cast< double* >( ed + 1 ) ); + _mm256_store_pd( reinterpret_cast< double* >( ed ), tmp ); + } + + int to_load; + + __m256d* vp = vertex_pair( begin ); + + __m256d a0 = _mm256_load_pd( reinterpret_cast< double* >( vp ) ); + __m256d a1 = _mm256_load_pd( reinterpret_cast< double* >( vp + 1 ) ); + __m256d a2 = _mm256_load_pd( reinterpret_cast< double* >( vp + 2 ) ); + + if( ( begin & 0x1 ) == 0 ) + { + __m256d c0 = gather_lanes< clear_lane, 0 >( a0 ); + __m256d c1 = gather_lanes< clear_lane, 0 >( a1 ); + __m256d c2 = gather_lanes< clear_lane, 0 >( a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp - 3 ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp - 2 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp - 1 ), c2 ); + + if( low == 1 ) + { + --begin; + return at - 1; + } + to_load = low - 2; + } + else + to_load = low - 1; + + int q = to_load / 4; + __m256d* vp_max = vp + 6 * q; + + while( vp < vp_max ) + { + __m256d b0 = _mm256_load_pd( reinterpret_cast< double* >( vp + 3 ) ); + __m256d b1 = _mm256_load_pd( reinterpret_cast< double* >( vp + 4 ) ); + __m256d b2 = _mm256_load_pd( reinterpret_cast< double* >( vp + 5 ) ); + + __m256d c0 = gather_lanes< 1, 0 >( a0, b0 ); + __m256d c1 = gather_lanes< 1, 0 >( a1, b1 ); + __m256d c2 = gather_lanes< 1, 0 >( a2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + + vp += 3; + + a0 = _mm256_load_pd( reinterpret_cast< double* >( vp + 3 ) ); + a1 = _mm256_load_pd( reinterpret_cast< double* >( vp + 4 ) ); + a2 = _mm256_load_pd( reinterpret_cast< double* >( vp + 5 ) ); + + __m256d d0 = gather_lanes< 1, 0 >( b0, a0 ); + __m256d d1 = gather_lanes< 1, 0 >( b1, a1 ); + __m256d d2 = gather_lanes< 1, 0 >( b2, a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), d2 ); + + vp += 3; + } + + int r = to_load % 4; + + if( r == 0 ) + { + __m256d c0 = gather_lanes< 1, clear_lane >( a0 ); + __m256d c1 = gather_lanes< 1, clear_lane >( a1 ); + __m256d c2 = gather_lanes< 1, clear_lane >( a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + } + else + { + __m256d b0 = _mm256_load_pd( reinterpret_cast< double* >( vp + 3 ) ); + __m256d b1 = _mm256_load_pd( reinterpret_cast< double* >( vp + 4 ) ); + __m256d b2 = _mm256_load_pd( reinterpret_cast< double* >( vp + 5 ) ); + + __m256d c0 = gather_lanes< 1, 0 >( a0, b0 ); + __m256d c1 = gather_lanes< 1, 0 >( a1, b1 ); + __m256d c2 = gather_lanes< 1, 0 >( a2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + + switch( r ) + { + case 1: + goto low_end; + case 2: + { + __m256d c0 = gather_lanes< 1, clear_lane >( b0 ); + __m256d c1 = gather_lanes< 1, clear_lane >( b1 ); + __m256d c2 = gather_lanes< 1, clear_lane >( b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp + 3 ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 4 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 5 ), c2 ); + goto low_end; + } + default: + { + vp += 3; + a0 = _mm256_load_pd( reinterpret_cast< double* >( vp + 3 ) ); + a1 = _mm256_load_pd( reinterpret_cast< double* >( vp + 4 ) ); + a2 = _mm256_load_pd( reinterpret_cast< double* >( vp + 5 ) ); + + __m256d c3 = gather_lanes< 1, 0 >( b0, a0 ); + __m256d c4 = gather_lanes< 1, 0 >( b1, a1 ); + __m256d c5 = gather_lanes< 1, 0 >( b2, a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c3 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c4 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c5 ); + } + } + } + + low_end: + { + --begin; + return at - 1; + } + } + else + { + __m256d* ed = edge + end + 1; + __m256d* ed_min = ed - ( high + 1 ); + + for( ; ed > ed_min; --ed ) + { + __m256d tmp = _mm256_load_pd( reinterpret_cast< double* >( ed - 1 ) ); + _mm256_store_pd( reinterpret_cast< double* >( ed ), tmp ); + } + + __m256d* vp = vertex_pair( end ); + + __m256d a0; + __m256d a1; + __m256d a2; + + int to_load; + + if( ( end & 0x1 ) == 0 ) + { + a0 = _mm256_load_pd( reinterpret_cast< double* >( vp - 3 ) ); + a1 = _mm256_load_pd( reinterpret_cast< double* >( vp - 2 ) ); + a2 = _mm256_load_pd( reinterpret_cast< double* >( vp - 1 ) ); + + __m256d c0 = gather_lanes< 1, clear_lane >( a0 ); + __m256d c1 = gather_lanes< 1, clear_lane >( a1 ); + __m256d c2 = gather_lanes< 1, clear_lane >( a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + + if( high == 1 ) + { + ++end; + return at; + } + + vp -= 3; + to_load = high - 2; + } + else + { + to_load = high - 1; + a0 = _mm256_load_pd( reinterpret_cast< double* >( vp ) ); + a1 = _mm256_load_pd( reinterpret_cast< double* >( vp + 1 ) ); + a2 = _mm256_load_pd( reinterpret_cast< double* >( vp + 2 ) ); + } + + int q = to_load / 4; + + __m256d* vp_min = vp - 6 * q; + + while( vp > vp_min ) + { + __m256d b0 = _mm256_load_pd( reinterpret_cast< double* >( vp - 3 ) ); + __m256d b1 = _mm256_load_pd( reinterpret_cast< double* >( vp - 2 ) ); + __m256d b2 = _mm256_load_pd( reinterpret_cast< double* >( vp - 1 ) ); + + __m256d c0 = gather_lanes< 1, 0 >( b0, a0 ); + __m256d c1 = gather_lanes< 1, 0 >( b1, a1 ); + __m256d c2 = gather_lanes< 1, 0 >( b2, a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + + vp -= 3; + + a0 = _mm256_load_pd( reinterpret_cast< double* >( vp - 3 ) ); + a1 = _mm256_load_pd( reinterpret_cast< double* >( vp - 2 ) ); + a2 = _mm256_load_pd( reinterpret_cast< double* >( vp - 1 ) ); + + __m256d d0 = gather_lanes< 1, 0 >( a0, b0 ); + __m256d d1 = gather_lanes< 1, 0 >( a1, b1 ); + __m256d d2 = gather_lanes< 1, 0 >( a2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), d2 ); + + vp -= 3; + } + + int r = to_load % 4; + + if( r == 0 ) + { + __m256d c0 = gather_lanes< clear_lane, 0 >( a0 ); + __m256d c1 = gather_lanes< clear_lane, 0 >( a1 ); + __m256d c2 = gather_lanes< clear_lane, 0 >( a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + } + else + { + __m256d b0 = _mm256_load_pd( reinterpret_cast< double* >( vp - 3 ) ); + __m256d b1 = _mm256_load_pd( reinterpret_cast< double* >( vp - 2 ) ); + __m256d b2 = _mm256_load_pd( reinterpret_cast< double* >( vp - 1 ) ); + + __m256d c0 = gather_lanes< 1, 0 >( b0, a0 ); + __m256d c1 = gather_lanes< 1, 0 >( b1, a1 ); + __m256d c2 = gather_lanes< 1, 0 >( b2, a2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), c2 ); + + switch( r ) + { + case 1: + goto high_end; + case 2: + { + __m256d d0 = gather_lanes< clear_lane, 0 >( b0 ); + __m256d d1 = gather_lanes< clear_lane, 0 >( b1 ); + __m256d d2 = gather_lanes< clear_lane, 0 >( b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp - 3 ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp - 2 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp - 1 ), d2 ); + goto high_end; + } + default: // case 3 + { + vp -= 3; + + a0 = _mm256_load_pd( reinterpret_cast< double* >( vp - 3 ) ); + a1 = _mm256_load_pd( reinterpret_cast< double* >( vp - 2 ) ); + a2 = _mm256_load_pd( reinterpret_cast< double* >( vp - 1 ) ); + + __m256d d0 = gather_lanes< 1, 0 >( a0, b0 ); + __m256d d1 = gather_lanes< 1, 0 >( a1, b1 ); + __m256d d2 = gather_lanes< 1, 0 >( a2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( vp ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 1 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( vp + 2 ), d2 ); + } + } + } + + high_end: + { + ++end; + return at; + } + } +} + +void bounded_convex::push( __m256d* begin, __m256d* end ) +{ + for( ; begin < end; ++begin ) + { + int sign = _mm256_movemask_pd( *begin ) & 0x7; + + switch( sign ) + { + case 0b000: { locator< 0, 0, 0 >::insert( *this, *begin ); break; } + case 0b001: { locator< 1, 0, 0 >::insert( *this, *begin ); break; } + case 0b010: { locator< 0, 1, 0 >::insert( *this, *begin ); break; } + case 0b011: { locator< 1, 1, 0 >::insert( *this, *begin ); break; } + case 0b100: { locator< 0, 0, 1 >::insert( *this, *begin ); break; } + case 0b101: { locator< 1, 0, 1 >::insert( *this, *begin ); break; } + case 0b110: { locator< 0, 1, 1 >::insert( *this, *begin ); break; } + case 0b111: { locator< 1, 1, 1 >::insert( *this, *begin ); break; } + } + + if( type == bounded_convex_type::empty ) + return; + } +} + +int bounded_convex::remove_vertices( int begin_vertex, int end_vertex ) +{ + int re = end_vertex & 0x1; + int rb = begin_vertex & 0x1; + int low = begin_vertex - begin; + int high = end - end_vertex; + + if( low <= high ) + { + __m256d* e_dst = edge + end_vertex; + __m256d* e_dst_min = e_dst - low; + __m256d* e_src = edge + begin_vertex; + + while( --e_dst >= e_dst_min ) + { + __m256d tmp = _mm256_load_pd( reinterpret_cast< double* >( --e_src ) ); + _mm256_store_pd( reinterpret_cast< double* >( e_dst ), tmp ); + } + + __m256d* v_dst = vertex_pair( end_vertex ); + __m256d* v_src = vertex_pair( begin_vertex ); + + if( re == rb ) + { + int q; + + if( re & 0x1 ) + { + __m256d s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + __m256d s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + __m256d s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d d0 = _mm256_load_pd( reinterpret_cast< double* >( v_dst ) ); + __m256d d1 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 1 ) ); + __m256d d2 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 2 ) ); + + __m256d b0 = blend< 0, 0, 1, 1 >( s0, d0 ); + __m256d b1 = blend< 0, 0, 1, 1 >( s1, d1 ); + __m256d b2 = blend< 0, 0, 1, 1 >( s2, d2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), b0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), b1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), b2 ); + + q = low; + } + else + q = ( low + 1 ); + + q /= 2; + __m256d* v_src_min = v_src - 3 * q; + + while( v_src > v_src_min ) + { + --v_src; + --v_dst; + __m256d tmp = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), tmp ); + } + } + else + { + __m256d s0; + __m256d s1; + __m256d s2; + + int to_load; + + if( rb == 0 ) + { + v_src -= 3; + + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d p0 = gather_lanes< 1, clear_lane >( s0 ); + __m256d p1 = gather_lanes< 1, clear_lane >( s1 ); + __m256d p2 = gather_lanes< 1, clear_lane >( s2 ); + + __m256d d0 = _mm256_load_pd( reinterpret_cast< double* >( v_dst ) ); + __m256d d1 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 1 ) ); + __m256d d2 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 2 ) ); + + __m256d b0 = blend< 0, 0, 1, 1 >( p0, d0 ); + __m256d b1 = blend< 0, 0, 1, 1 >( p1, d1 ); + __m256d b2 = blend< 0, 0, 1, 1 >( p2, d2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), b0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), b1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), b2 ); + + if( low == 1 ) + goto low_end; + + to_load = low - 2; + } + else + { + to_load = low - 1; + + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + } + + int q = to_load / 4; + __m256d* v_src_min = v_src - 6 * q; + + while( v_src > v_src_min ) + { + v_src -= 3; + v_dst -= 3; + + __m256d b0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + __m256d b1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + __m256d b2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d c0 = gather_lanes< 1, 0 >( b0, s0 ); + __m256d c1 = gather_lanes< 1, 0 >( b1, s1 ); + __m256d c2 = gather_lanes< 1, 0 >( b2, s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), c2 ); + + v_dst -= 3; + v_src -= 3; + + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d d0 = gather_lanes< 1, 0 >( s0, b0 ); + __m256d d1 = gather_lanes< 1, 0 >( s1, b1 ); + __m256d d2 = gather_lanes< 1, 0 >( s2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), d2 ); + } + + v_dst -= 3; + int r = to_load % 4; + + if( r == 0 ) + { + __m256d c0 = gather_lanes< clear_lane, 0 >( s0 ); + __m256d c1 = gather_lanes< clear_lane, 0 >( s1 ); + __m256d c2 = gather_lanes< clear_lane, 0 >( s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), c2 ); + } + else + { + v_src -= 3; + __m256d b0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + __m256d b1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + __m256d b2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d c0 = gather_lanes< 1, 0 >( b0, s0 ); + __m256d c1 = gather_lanes< 1, 0 >( b1, s1 ); + __m256d c2 = gather_lanes< 1, 0 >( b2, s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), c2 ); + + switch( r ) + { + case 1: + goto low_end; + case 2: + { + v_dst -= 3; + + __m256d d0 = gather_lanes< clear_lane, 0 >( b0 ); + __m256d d1 = gather_lanes< clear_lane, 0 >( b1 ); + __m256d d2 = gather_lanes< clear_lane, 0 >( b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), d2 ); + goto low_end; + } + default: + { + v_dst -= 3; + v_src -= 3; + + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d d0 = gather_lanes< 1, 0 >( s0, b0 ); + __m256d d1 = gather_lanes< 1, 0 >( s1, b1 ); + __m256d d2 = gather_lanes< 1, 0 >( s2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), d0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), d1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), d2 ); + + v_dst -= 3; + + s0 = gather_lanes< clear_lane, 0 >( s0, b0 ); + s1 = gather_lanes< clear_lane, 0 >( s1, b1 ); + s2 = gather_lanes< clear_lane, 0 >( s2, b2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), s0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), s1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), s2 ); + } + } + } + } + + low_end: + { + begin += ( end_vertex - begin_vertex ); + return end_vertex - 1; + } + } + else + { + __m256d* e_dst = edge + begin_vertex; + __m256d* e_src = edge + end_vertex; + __m256d* e_src_max = e_src + high + 1; + + for( ; e_src < e_src_max; ++e_src, ++e_dst ) + { + __m256d tmp = _mm256_load_pd( reinterpret_cast< double* >( e_src ) ); + _mm256_store_pd( reinterpret_cast< double* >( e_dst ), tmp ); + } + + __m256d s0; + __m256d s1; + __m256d s2; + + __m256d* v_dst = vertex_pair( begin_vertex ); + __m256d* v_src = vertex_pair( end_vertex ); + + if( re == rb ) + { + if( re & 0x1 ) + { + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d d0 = _mm256_load_pd( reinterpret_cast< double* >( v_dst ) ); + __m256d d1 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 1 ) ); + __m256d d2 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 2 ) ); + + __m256d c0 = blend< 0, 0, 1, 1 >( d0, s0 ); + __m256d c1 = blend< 0, 0, 1, 1 >( d1, s1 ); + __m256d c2 = blend< 0, 0, 1, 1 >( d2, s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), c0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), c1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), c2 ); + + --high; + v_src += 3; + v_dst += 3; + } + + int q = 3 * ( ( high + 1 ) / 2 ); + + __m256d* v_src_max = vertex_pair( end + 1 ); + for( ; v_src < v_src_max; ++v_src, ++v_dst ) + { + __m256d tmp = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), tmp ); + } + } + else + { + int to_load; + + __m256d s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + __m256d s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + __m256d s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + if( rb ) // re = 0 =: + { + __m256d d0 = _mm256_load_pd( reinterpret_cast< double* >( v_dst ) ); + __m256d d1 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 1 ) ); + __m256d d2 = _mm256_load_pd( reinterpret_cast< double* >( v_dst + 2 ) ); + + __m256d g0 = gather_lanes< 0, 0 >( d0, s0 ); + __m256d g1 = gather_lanes< 0, 0 >( d1, s1 ); + __m256d g2 = gather_lanes< 0, 0 >( d2, s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), g0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), g1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), g2 ); + + if( high == 1 ) + goto high_end; + + v_dst += 3; + to_load = high - 2; + } + else // re = 1 => only the second lane of the si are used + to_load = high - 1; + + int q = to_load / 4; + __m256d* v_src_max = v_src + 6 * q; + + while( v_src < v_src_max ) + { + v_src += 3; + + __m256d t0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + __m256d t1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + __m256d t2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d u0 = gather_lanes< 1, 0 >( s0, t0 ); + __m256d u1 = gather_lanes< 1, 0 >( s1, t1 ); + __m256d u2 = gather_lanes< 1, 0 >( s2, t2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), u0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), u1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), u2 ); + + v_dst += 3; + v_src += 3; + + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d v0 = gather_lanes< 1, 0 >( t0, s0 ); + __m256d v1 = gather_lanes< 1, 0 >( t1, s1 ); + __m256d v2 = gather_lanes< 1, 0 >( t2, s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), v0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), v1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), v2 ); + + v_dst += 3; + } + + int r = to_load % 4; + + if( r == 0 ) + { + __m256d u0 = gather_lanes< 1, clear_lane >( s0 ); + __m256d u1 = gather_lanes< 1, clear_lane >( s1 ); + __m256d u2 = gather_lanes< 1, clear_lane >( s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), u0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), u1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), u2 ); + } + else + { + v_src += 3; + + __m256d t0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + __m256d t1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + __m256d t2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d u0 = gather_lanes< 1, 0 >( s0, t0 ); + __m256d u1 = gather_lanes< 1, 0 >( s1, t1 ); + __m256d u2 = gather_lanes< 1, 0 >( s2, t2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), u0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), u1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), u2 ); + + switch( r ) + { + case 1: + goto high_end; + case 2: + { + v_dst += 3; + __m256d v0 = gather_lanes< 1, clear_lane >( t0 ); + __m256d v1 = gather_lanes< 1, clear_lane >( t1 ); + __m256d v2 = gather_lanes< 1, clear_lane >( t2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), v0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), v1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), v2 ); + + goto high_end; + } + default: // case 3 + { + v_dst += 3; + v_src += 3; + + s0 = _mm256_load_pd( reinterpret_cast< double* >( v_src ) ); + s1 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 1 ) ); + s2 = _mm256_load_pd( reinterpret_cast< double* >( v_src + 2 ) ); + + __m256d v0 = gather_lanes< 1, 0 >( t0, s0 ); + __m256d v1 = gather_lanes< 1, 0 >( t1, s1 ); + __m256d v2 = gather_lanes< 1, 0 >( t2, s2 ); + + _mm256_store_pd( reinterpret_cast< double* >( v_dst ), v0 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 1 ), v1 ); + _mm256_store_pd( reinterpret_cast< double* >( v_dst + 2 ), v2 ); + } + } + } + } + + high_end: + { + end -= ( end_vertex - begin_vertex ); + return begin_vertex - 1; + } + } +} + +void bounded_convex::set_to_last_segment() +{ + int last = end - 1; + edge[ 0 ] = edge[ last ]; + edge[ 1 ] = edge[ end ]; + edge[ 2 ] = edge[ begin + 1 ]; + + __m256d* vl = vertex_pair( last ); + __m256d* vb = vertex_pair( begin ); + + if( last & 0x1 ) + { + if( begin & 0x1 ) + { + vertex_p[ 0 ] = gather_lanes< 1, 1 >( vl[ 0 ], vb[ 0 ] ); + vertex_p[ 1 ] = gather_lanes< 1, 1 >( vl[ 1 ], vb[ 1 ] ); + vertex_p[ 2 ] = gather_lanes< 1, 1 >( vl[ 2 ], vb[ 2 ] ); + } + else + { + vertex_p[ 0 ] = gather_lanes< 1, 0 >( vl[ 0 ], vb[ 0 ] ); + vertex_p[ 1 ] = gather_lanes< 1, 0 >( vl[ 1 ], vb[ 1 ] ); + vertex_p[ 2 ] = gather_lanes< 1, 0 >( vl[ 2 ], vb[ 2 ] ); + } + } + else + { + if( begin & 0x1 ) + { + vertex_p[ 0 ] = blend< 0, 0, 1, 1 >( vl[ 0 ], vb[ 0 ] ); + vertex_p[ 1 ] = blend< 0, 0, 1, 1 >( vl[ 1 ], vb[ 1 ] ); + vertex_p[ 2 ] = blend< 0, 0, 1, 1 >( vl[ 2 ], vb[ 2 ] ); + } + else + { + vertex_p[ 0 ] = gather_lanes< 0, 0 >( vl[ 0 ], vb[ 0 ] ); + vertex_p[ 1 ] = gather_lanes< 0, 0 >( vl[ 1 ], vb[ 1 ] ); + vertex_p[ 2 ] = gather_lanes< 0, 0 >( vl[ 2 ], vb[ 2 ] ); + } + } + + end = 2; + begin = 0; + type = bounded_convex_type::segment; +} + +void bounded_convex::setup( __m256d box ) +{ + type = bounded_convex_type::polygon; + + __m256d zero = _mm256_setzero_pd(); + __m256i ones = ones_i(); + __m256i signi = _mm256_slli_epi64( ones, 63 ); + __m256i signi_h = _mm256_slli_si256( signi, 8 ); + __m256d sign = _mm256_castsi256_pd( signi ); + __m256d sign_h = _mm256_castsi256_pd( signi_h ); + __m256i onei = _mm256_srli_epi64( ones, 54 ); + onei = _mm256_slli_epi64( onei, 52 ); // { 1.0, 1.0, 1.0, 1.0 } + __m256d one = _mm256_castsi256_pd( onei ); + + box = _mm256_max_pd( box, zero ); + + begin = alloc / 2; + end = begin + 4; + __m256d* e = edge + begin; + __m256d x_box = gather_lanes< 0, 0 >( box ); // { x_min, x_max, x_min, x_max } + __m256d y_box = gather_lanes< 1, 1 >( box ); // { y_min, y_max, y_min, y_max } + + e[ 0 ] = blend< 0, 1, 0, 0 >( one, zero ); // { 1, 0, 0, 0}, + e[ 0 ] = blend< 0, 0, 1, 1 >( e[ 0 ], x_box ); // { 1, 0, x_min, x_max }, for x >= x_min + + e[ 1 ] = permute< 1, 0, 2, 3 >( e[ 0 ] ); // { 0 , 1, x_min, x_max } // y >= y_min + e[ 1 ] = blend< 0, 0, 1, 1 >( e[ 1 ], y_box ); // { 0 , 1, y_min, y_max } // y >= y_min + + e[ 2 ] = permute< 0, 1, 3, 2 >( e[ 0 ] ); // { 1 , 0, x_max, x_min } + e[ 2 ] = _mm256_xor_pd( e[ 2 ], sign ); // { -1 , -, -x_max, -y_min } -x >= -x_max + + e[ 3 ] = permute< 0, 1, 3, 2 >( e[ 1 ] ); // { 0, 1, y_max, ? } + e[ 3 ] = _mm256_xor_pd( e[ 3 ], sign ); // { 0, 1, -y_max, ? } -y >= -y_max + + e[ 4 ] = e[ 0 ]; // { 1, 0, x_min, } } + + // initial vertices + + __m256d* vp = vertex_pair( begin ); + + vp[ 0 ] = permute< 0, 0, 3, 3 >( x_box ); // { x_min, x_min, x_max, x_max } : vertices 0 and 1: + vp[ 0 ] = _mm256_xor_pd( vp[ 0 ], sign_h ); // { x_min, -x_min, x_max, -x_max } : vertices 0 and 1: + + vp[ 1 ] = permute< 0, 0, 2, 2 >( y_box ); // { y_min, y_min, y_min, y_min } + vp[ 1 ] = _mm256_xor_pd( vp[ 1 ], sign_h ); // { y_min, -y_min, y_min, -y_min } + + vp[ 2 ] = _mm256_xor_pd( one, sign_h ); // { 1.0, -1.0, 1.0, -1.0 } + + vp[ 3 ] = permute< 1, 1, 2, 2 >( x_box ); // { x_max, x_max, x_min, x_min } : vertices 2 and 3: + vp[ 3 ] = _mm256_xor_pd( vp[ 3 ], sign_h ); // { x_max, -x_max, x_min, -x_min } + + vp[ 4 ] = permute< 1, 1, 3, 3 >( y_box ); // { y_max, y_max, y_max, y_max } + vp[ 4 ] = _mm256_xor_pd( vp[ 4 ], sign_h ); // { y_max, -y_max, y_max, -y_max } + + vp[ 5 ] = vp[ 2 ]; +} + +int normalize_equations( __m256d* quadrant[ 4 ], int n, __m256i tiny, __m256i huge ) +{ + constexpr unsigned long long x_index_epi8 = 0x1 << 4; + constexpr unsigned long long y_index_epi8 = 0x1 << 12; + constexpr unsigned long long z_index_epi8 = 0x1 << 16; + + int ret = 0; + __m256i ones = ones_i(); // { 0xFFFF'FFFF'FFFF'FFFF, ... } + __m256i exp_mask = _mm256_srli_epi64( ones, 53 ); // { 0x0000'0000'0000'07FF, .... } + __m256i one = _mm256_srli_epi64( ones, 54 ); // { 0x0000'0000'0000'03FF, .... } + __m256i sign = _mm256_slli_epi64( ones, 63 ); // { 0x8000'0000'0000'0000, .... } + exp_mask = _mm256_slli_epi64( exp_mask, 52 ); // { 0x7FF0'0000'0000'0000, .... } + one = _mm256_slli_epi64( one, 52 ); // { 0x3FF0'0000'0000'0000, .... } + sign = gather_lanes< 1, clear_lane >( sign ); // { sign_bit, sign_bit, 0, 0 } + + for( int iq = 0; iq < 4; ++iq ) + { + __m256d* end = quadrant[ iq ] + n; + + while( quadrant[ iq ] < end ) + { + __m256d* q = quadrant[ iq ]; + __m256i exp = and_i( *q, exp_mask ); + __m256i cmp_huge = _mm256_cmpgt_epi32( exp, huge ); + + if( _mm256_testz_si256( cmp_huge, cmp_huge ) ) // no huge entries + { + __m256i cmp_tiny = _mm256_cmpgt_epi32( tiny, exp ); + + if( _mm256_testz_si256( cmp_tiny, cmp_tiny ) ) // no entries are huge or tiny + { + ++quadrant[ iq ]; + continue; + } + + // no entries are huge, but aome are tiny. We scale by 2^(huge - max_exp[ i ] ) + + __m256i exp_b = permute< 1, 0, 2, 3 >( exp ); // { exp[ 1 ], exp[ 0 ], exp[ 2 ], ? } + __m256i exp_c = gather_lanes< 1, 1 >( exp ); // { exp[ 2 ], exp[ 3 ], exp[ 2 ], ? } + __m256i max = _mm256_max_epi32( exp, exp_b ); // { max( exp[ 0 ], exp[ 1 ] ), ?. ?. ? } + max = _mm256_max_epi32( max, exp_c ); // { max{ exp[ 0 ], exp[ 1 ], exp[ 2 ] ), ....} + + int64_t int_max = _mm256_extract_epi64( max, 0 ); + if( int_max == 0 ) // all exponents are zero. + { + __m256d cmp = _mm256_cmp_pd( *q, _mm256_setzero_pd(), _CMP_EQ_OQ ); + if( ( _mm256_movemask_pd( cmp ) & 0x7 ) == 0x7 ) // all coefficients are zero + { // drop the constraint + *q = *( --end ); + continue; + } + } + + max = permute< 0, 0, 0, 0 >( max ); + __m256i de = _mm256_sub_epi32( huge, max ); + + // de may be larger than 0x3ff and we must scale in two steps + + *q = xor_d( *q, sign ); + + __m256i hs = _mm256_srli_epi64( de, 1 ); // de/2 + hs = _mm256_and_si256( hs, exp_mask ); // throwing away the remainder + __m256d scale = cast_d( _mm256_add_epi64( hs, one ) ); + *q = _mm256_mul_pd( scale, *q ); + + __m256i ls = _mm256_sub_epi64( de, hs ); // de - hs + scale = cast_d( _mm256_add_epi64( ls, one ) ); + *q = _mm256_mul_pd( scale, *q ); + } + else // some huge entries + { + __m256i exp_b = permute< 1, 0, 2, 3 >( exp ); // { exp[ 1 ], exp[ 0 ], exp[ 2 ], ? } + __m256i exp_c = gather_lanes< 1, 1 >( exp ); // { exp[ 2 ], exp[ 3 ], exp[ 2 ], ? } + __m256i max = _mm256_max_epi32( exp, exp_b ); // { max( exp[ 0 ], exp[ 1 ] ), ?. ?. ? } + max = _mm256_max_epi32( max, exp_c ); // { max{ exp[ 0 ], exp[ 1 ], exp[ 2 ] ), ....} + + max = permute< 0, 0, 0, 0 >( max ); + __m256i de = _mm256_sub_epi32( huge, max ); // de > -0x3ff + __m256d scale = cast_d( _mm256_add_epi64( de, one ) ); // this does not underflow + + *q = xor_d( *q, sign ); + *q = _mm256_mul_pd( scale, *q ); // this may be inexact, but the inexact entries will be dropped later + } + + // cleaning up the remaining tiny entries + + __m256i exp_d = and_i( *q, exp_mask ); + __m256i cmp_d = _mm256_cmpgt_epi32( exp_d, tiny ); // 8 x (4 byte masks) + int cmp_d_mask = _mm256_movemask_epi8( cmp_d ); // 8 bits for x, 8 bits for y, 8 bits for z + + if( cmp_d_mask & ( x_index_epi8 | y_index_epi8 ) ) + { + // In order to preserve the solution set, we must round the right hand side down and + // the left hand side up, so that every solution of the old problem satisfies the new contraints. + // To do that, we want to round the left hand side up and the right hand side down. Since + // the lhs were flipped, we simply round all entries down, that is: + // + // we replace x in ( -tiny, 0 ) by -tiny and x in [ 0, tiny ) by +0.0 + // (and fix the zeros latter ) + + __m256d zero = _mm256_setzero_pd(); + __m256i exp_f = and_i( *q, exp_mask ); + __m256i full_sign = gather_lanes< 0, 0 >( sign ); + __m256i minus_tiny = _mm256_or_si256( tiny, full_sign ); + __m256i is_tiny_f = _mm256_cmpgt_epi32( tiny, exp_f ); + __m256d non_negative = _mm256_cmp_pd( *q, zero, _CMP_GE_OQ ); + + // tiny negatives should be replaced by -tiny, tiny non negatives are replace by 0. + __m256d fix = blend( cast_d( minus_tiny ), zero, non_negative ); + + // fixing the tiny entries + *q = blend( *q, fix, is_tiny_f ); + + // restoring the rhs sign + *q = xor_d( *q, sign ); + + // cleaning up the zeros + __m256i abs = _mm256_slli_epi64( cast_i( *q ), 1 ); // throwing away the sign bit + __m256i is_zero = _mm256_cmpeq_epi64( abs, cast_i( zero ) ); + + *q = blend( *q, zero, is_zero ); + + ++quadrant[ iq ]; + } + else + { + // | a | <= tiny and | b | <= tiny: and |c| >= huge, + // The constraint is either irrelevant or fatal. + + int pb_sign = _mm256_movemask_pd( *q ); + if( !( pb_sign & 0x4 ) ) // drop the quadrant: the right hand side is too large + { + ret |= ( 0x1 << iq ); + break; + } + + // else, drop the constraint: the right hand side is too small + + *q = *( --end ); + } + } + } + + return ret; +} + +} // namespace wm::nlp::lp2d +//---------------------------------------------------------------------------------------- + +extern "C" +{ + +int interval_system_2d( double* solution, double const* box, double const* eq, + unsigned int n_equations ) +{ + constexpr int max_quadrant_size = 32; + + using namespace wm::nlp::lp2d; + + double* solsPtr = solution; + + //printf("box: x_min = %f, x_max = %f, y_min = %f, y_max = %f\n", box[0], box[1], box[2], box[3]); + //printf("n_equations = %u\n", n_equations); + //for (int i = 0; i < n_equations; i++) { + // printf("equation %u: a_min = %f, a_max = %f, b_min = %f, b_max = %f, c_min = %f, c_max = %f\n", i, eq[6*i], eq[6*i + 1], eq[6*i + 2], eq[6*i + 3], eq[6*i + 4], eq[6*i + 5]); + //} + + if( n_equations > ( max_quadrant_size / 2 ) ) + return -1; + + if( std::bit_cast< uint64_t >( box ) & 0x1F ) + return -2; + + if( std::bit_cast< uint64_t >( eq ) & 0x1F ) + return -3; + + + + int ret = 0; + int n_equations_q = n_equations / 2; + int n_equations_r = n_equations % 2; + + double const* eq_max = eq + n_equations_q; + + __m256i huge; + __m256i tiny; + __m256i signi = bounds( &tiny, &huge ); + __m256i exp_mask = _mm256_srli_epi64( ones_i(), 53 ); + exp_mask = _mm256_slli_epi64( exp_mask, 52 ); // { 0x7FF0'0000'0000'0000... } + + __m256d sign = cast_d( signi ); + __m256d zero = _mm256_setzero_pd(); + __m256d sign_01 = blend< 0, 0, 1, 1 >( sign, zero ); // { -1: -1, 0, 0 } + + int old_rounding_mode = std::fegetround(); + + if( std::fesetround( FE_DOWNWARD ) ) + return -4; + + __m256d quadrants[ 4 * max_quadrant_size ]; + + __m256d* q[ 4 ] = { quadrants, + quadrants + max_quadrant_size, + quadrants + 2 * max_quadrant_size, + quadrants + 3 * max_quadrant_size }; + + int empty_quadrant = 0; + double const* p_eq = eq; + int quadrant_is_not_empty = 0xF; + + for( ; p_eq < eq_max; p_eq += 12 ) + { + __m256d a0 = _mm256_load_pd( p_eq ); + __m256d aux = _mm256_load_pd( p_eq + 4 ); + __m256d bc1 = _mm256_load_pd( p_eq + 8 ); + + // the equations in this group are + // + // a0[ 0, 1 ] x + a0[ 2, 3 ] y = aux[ 0, 1 ] + // aux[ 2, 3 ] x + bc1[ 0, 1 ] y = bc1[ 2, 3 ] + + __m256d bc0 = gather_lanes< 1, 0 >( a0, aux ); + __m256d a1 = gather_lanes< 1, 0 >( aux ); + + // the equations in this group are now + // + // a0[ 0, 1 ] x + bc0[ 0, 1 ] y = bc0[ 2, 3 ] + // a1[ 0, 1 ] x + bc1[ 0, 1 ] y = bc1[ 2, 3 ] + + __m256d max_c0 = permute< 0, 1, 3, 2 >( bc0 ); //{ ?, ?, bc0[ 3 ], ? } + __m256d max_c1 = permute< 0, 1, 3, 2 >( bc1 ); //{ ?, ?, bc1[ 3 ], ? } + + for( int iq = 0; iq < 4; ++iq ) // iqth quadrant + { + q[ iq ][ 0 ] = gather_low( a0, bc0 ); // { a0[ 0 ], bc0[ 0 ], ?, ? } + q[ iq ][ 1 ] = gather_high( a0, bc0 ); // { a0[ 1 ], bc0[ 1 ], ?, ? } + q[ iq ][ 2 ] = gather_low( a1, bc1 ); // { a1[ 0 ], bc1[ 0 ], ?, ? } + q[ iq ][ 3 ] = gather_high( a1, bc1 ); // { a1[ 1 ], bc1[ 1 ], ?, ? } + + q[ iq ][ 0 ] = blend< 0, 0, 1, 1 >( q[ iq ][ 0 ], max_c0 ); // { a0[ 0 ], bc0[ 0 ], bc0[ 3 ], ? } + q[ iq ][ 1 ] = blend< 0, 0, 1, 1 >( q[ iq ][ 1 ], bc0 ); // { a0[ 1 ], bc0[ 1 ], bc0[ 2 ], ? } + q[ iq ][ 2 ] = blend< 0, 0, 1, 1 >( q[ iq ][ 2 ], max_c1 ); // { a1[ 0 ], bc1[ 0 ], bc1[ 3 ], ? } + q[ iq ][ 3 ] = blend< 0, 0, 1, 1 >( q[ iq ][ 3 ], bc1 ); // { a1[ 1 ], bc1[ 1 ], bc1[ 2 ], ? } + + q[ iq ][ 0 ] = xor_d( q[ iq ][ 0 ], sign ); // { -a0[ 0 ], -bc0[ 0 ], -bc0[ 3 ], ? } + q[ iq ][ 2 ] = xor_d( q[ iq ][ 2 ], sign ); // { -a1[ 0 ], -bc1[ 0 ], -bc1[ 3 ], ? } + + // moving to the next quadrant + + if( iq & 0x1 ) // replacing y by - y + { + bc0 = xor_d( bc0, sign_01 ); + bc1 = xor_d( bc1, sign_01 ); + bc0 = permute< 1, 0, 2, 3 >( bc0 ); + bc1 = permute< 1, 0, 2, 3 >( bc1 ); + } + else // replacing x by - x + { + a0 = xor_d( a0, sign_01 ); + a1 = xor_d( a1, sign_01 ); + a0 = permute< 1, 0, 2, 3 >( a0 ); + a1 = permute< 1, 0, 2, 3 >( a1 ); + } + } + + //---------------------------------------------------------------------------------------- + // Checking for overflow and underflow: to avoid headaches with underflow and overflow, we + // normalize equation in which some elements are either +0.0 or have absolute value outside + // the range [ tiny, huge ]. In this process we may detect that the intersection of the + // feasible region with some quadrants is empty, or that some equations are irrelevant. + //---------------------------------------------------------------------------------------- + + __m256d ra = gather_lanes< 0, 0 >( a0, a1 ); // { row[ 0 ][ 0 ], row[ 0 ][ 1 ], row[ 3 ][ 0 ], row[ 3 ][ 1 ] } + __m256i expa = and_i( ra, exp_mask ); + __m256i exp0 = and_i( bc0, exp_mask ); + __m256i exp1 = and_i( bc1, exp_mask ); + + __m256i min = _mm256_min_epi32( expa, exp0 ); + __m256i max = _mm256_max_epi32( expa, exp0 ); + min = _mm256_min_epi32( min, exp1 ); + max = _mm256_max_epi32( max, exp1 ); + + __m256i c_min = _mm256_cmpgt_epi32( tiny, min ); + __m256i c_max = _mm256_cmpgt_epi32( max, huge ); + __m256i bad = or_i( c_min, c_max ); + + if( _mm256_testz_si256( bad, bad ) ) + { + q[ 0 ] += 4; + q[ 1 ] += 4; + q[ 2 ] += 4; + q[ 3 ] += 4; + } + else + empty_quadrant |= normalize_equations( q, 4, tiny, huge ); + } + + if( n_equations_r ) + { + __m256d ab = _mm256_load_pd( p_eq ); + __m128d aux = _mm_load_pd( p_eq + 4 ); + + __m256d sign_23 = gather_lanes< 1, 0 >( sign_01 ); + + // the equations in this group are + // ab[ 0, 1 ] x + ab[ 2, 3 ] y = aux[ 0, 1 ] + + __m256d c = _mm256_castpd128_pd256( aux ); + c = gather_lanes< 0, 0 >( c ); + // the equations in this group are now + // ab[ 0, 1 ] x + ab[ 2, 3 ] y = c[ 0, 1 ] + + __m256d max_c = permute< 0, 1, 3, 3 >( c ); //{ ?, ?, c[ 1 ], ? } + + for( int iq = 0; iq < 4; ++iq ) // iqth quadrant + { + q[ iq ][ 0 ] = permute< 0, 2, 2, 3 >( ab ); // { ab[ 0 ], ab[ 2 ], ?, ? } + q[ iq ][ 1 ] = permute< 1, 3, 2, 3 >( ab ); // { ab[ 1 ], ab[ 3 ], ?, ? } + + q[ iq ][ 0 ] = blend< 0, 0, 1, 1 >( q[ iq ][ 0 ], max_c ); // { ab[ 0 ], ab[ 2 ], c[ 1 ], ? } + q[ iq ][ 1 ] = blend< 0, 0, 1, 1 >( q[ iq ][ 1 ], c ); // { ab[ 1 ], ab[ 3 ], c[ 0 ], ? } + + q[ iq ][ 0 ] = xor_d( q[ iq ][ 0 ], sign ); // { -ab[ 0 ], -ab[ 2 ], -c[ 1 ], ? } + + // moving to the next quadrant + + if( iq & 0x1 ) // replacing y by - y + { + ab = xor_d( ab, sign_23 ); + ab = permute< 0, 1, 3, 2 >( ab ); + } + else // replacing x by - x + { + ab = xor_d( ab, sign_01 ); + ab = permute< 1, 0, 2, 3 >( ab ); + } + } + + //---------------------------------------------------------------------------------------- + // Checking for overflow and underflow: to avoid headaches with underflow and overflow, we + // normalize equation in which some elements are either +0.0 or have absolute value outside + // the range [ tiny, huge ]. In this process we may detect that the intersection of the + // feasible region with some quadrants is empty, or that some equations are irrelevant. + //---------------------------------------------------------------------------------------- + + __m256i exp_ab = and_i( ab, exp_mask ); + __m256i exp_c = and_i( c, exp_mask ); + exp_c = blend< 0, 0, 1, 1 >( exp_c, exp_ab ); + + __m256i min = _mm256_min_epi32( exp_ab, exp_c ); + __m256i max = _mm256_max_epi32( exp_ab, exp_c ); + + __m256i c_min = _mm256_cmpgt_epi32( tiny, min ); + __m256i c_max = _mm256_cmpgt_epi32( max, huge ); + __m256i bad = or_i( c_min, c_max ); + + if( _mm256_testz_si256( bad, bad ) ) + { + q[ 0 ] += 2; + q[ 1 ] += 2; + q[ 2 ] += 2; + q[ 3 ] += 2; + } + else + empty_quadrant |= normalize_equations( q, 2, tiny, huge ); + } + + __m256d avx_box = _mm256_load_pd( box ); + __m256d cmp = _mm256_cmp_pd( avx_box, _mm256_setzero_pd(), _CMP_GE_OQ ); + int positive_box = _mm256_movemask_pd( cmp ); + + __m256d edge[ 2 * ( max_quadrant_size + 2 ) ]; + __m256d vertex_pair[ 3 * ( max_quadrant_size + 2 ) ]; + bounded_convex bc( 2 * ( max_quadrant_size + 2 ), edge, vertex_pair ); + + constexpr uint x_min_field = 0x1; + constexpr uint x_max_field = 0x2; + constexpr uint y_min_field = 0x4; + constexpr uint y_max_field = 0x8; + + constexpr uint first_quadrant_field = 0x1; + constexpr uint second_quadrant_field = 0x2; + constexpr uint third_quadrant_field = 0x4; + constexpr uint fourth_quadrant_field = 0x8; + + if( positive_box & x_max_field ) // x_max >= 0 + { + if( !( empty_quadrant & first_quadrant_field ) && + ( positive_box & y_max_field ) ) // y_max >= 0, first quadrant + { + bc.setup( avx_box ); + bc.push( quadrants, q[ 0 ] ); + if( bc.type != bounded_convex_type::empty ) + { + ++ret; + __m256d bc_box = bc.box(); + _mm256_store_pd( solution, bc_box ); + solution += 4; + } + } + + if( !( empty_quadrant & fourth_quadrant_field ) && + !( positive_box & y_min_field ) ) // y_min < 0, fourth quadrant + { + __m256i y_sign = _mm256_slli_epi64( ones_i(), 63 ); + y_sign = gather_lanes< clear_lane, 1 >( y_sign ); + + __m256d q_box = permute< 0, 1, 3, 2 >( avx_box ); + q_box = xor_d( q_box, y_sign ); + + bc.setup( q_box ); + bc.push( quadrants + 3 * max_quadrant_size, q[ 3 ] ); + + if( bc.type != bounded_convex_type::empty ) + { + ++ret; + __m256i y_sign = _mm256_slli_epi64( ones_i(), 63 ); + y_sign = gather_lanes< clear_lane, 1 >( y_sign ); + + __m256d bc_box = bc.box(); + bc_box = xor_d( bc_box, y_sign ); + bc_box = permute< 0, 1, 3, 2 >( bc_box ); + _mm256_store_pd( solution, bc_box ); + solution += 4; + } + } + } + + if( !( positive_box & x_min_field ) ) // x_min < 0 + { + if( !( empty_quadrant & second_quadrant_field) && + ( positive_box & y_max_field ) ) // y_max >= 0, second quadrant + { + __m256i x_sign = _mm256_slli_epi64( ones_i(), 63 ); + x_sign = gather_lanes< 0, clear_lane >( x_sign ); + + __m256d q_box = permute< 1, 0, 2, 3 >( avx_box ); + q_box = xor_d( q_box, x_sign ); + + bc.setup( q_box ); + bc.push( quadrants + max_quadrant_size, q[ 1 ] ); + + if( bc.type != bounded_convex_type::empty ) + { + ++ret; + __m256d bc_box = bc.box(); + __m256i x_sign = _mm256_slli_epi64( ones_i(), 63 ); + x_sign = gather_lanes< 0, clear_lane >( x_sign ); + bc_box = xor_d( bc_box, x_sign ); + bc_box = permute< 1, 0, 2, 3 >( bc_box ); + _mm256_store_pd( solution, bc_box ); + solution += 4; + } + } + + if( !( empty_quadrant & third_quadrant_field ) && + !( positive_box & y_min_field ) ) // y_min < 0, third quadrant + { + __m256i sign = _mm256_slli_epi64( ones_i(), 63 ); + avx_box = permute< 1, 0, 3, 2 >( avx_box ); + avx_box = xor_d( avx_box, sign ); + bc.setup( avx_box ); + bc.push( quadrants + 2 * max_quadrant_size, q[ 2 ] ); + + if( bc.type != bounded_convex_type::empty ) + { + ++ret; + __m256d bc_box = bc.box(); + __m256i sign = _mm256_slli_epi64( ones_i(), 63 ); + bc_box = xor_d( bc_box, sign ); + bc_box = permute< 1, 0, 3, 2 >( bc_box ); + _mm256_store_pd( solution, bc_box ); + } + } + } + + std::fesetround( old_rounding_mode ); + + //for (int i = 0; i < ret; i++) { + // printf("solution %u: x_min = %f, x_max = %f, y_min = %f, y_max = %f\n", i, solsPtr[4*i], solsPtr[4*i + 1], solsPtr[4*i + 2], solsPtr[4*i + 3]); + //} + + return ret; +} + +} // extern "C" +//----------------------------------------------------------------------------------------
+ cbits/lp_2d.hpp view
@@ -0,0 +1,1252 @@+#pragma once + +//---------------------------------------------------------------------------------------- +// Copyright (c) 2024 Walter Mascarenhas +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. +// +// The MPL has a "Disclaimer of Warranty" and a "Limitation of Liability", +// and we provide no guarantee regarding the correctness of this source code. +// +//---------------------------------------------------------------------------------------- + +#include <bit> +#include <cassert> +#include <immintrin.h> + +typedef unsigned int uint; + +//---------------------------------------------------------------------------------------- +namespace wm::nlp::lp2d { + +//---------------------------------------------------------------------------------------- +// avx2 wrappers, to simplify the notation a bit and clarify the use of immediates +//---------------------------------------------------------------------------------------- + +inline __m256d and_d( __m256d x, __m256d y ) +{ + return _mm256_and_pd( x, y ); +} + +inline __m256i and_i( __m256i x, __m256d y ) +{ + __m256i yi = _mm256_castpd_si256( y ); + return _mm256_and_si256( x, yi ); +} + +inline __m256i and_i( __m256d x, __m256i y ) +{ + __m256i xi = _mm256_castpd_si256( x ); + __m256i r = _mm256_and_si256( xi, y ); + return r; +} + +inline __m256i and_i( __m256i x, __m256i y ) +{ + return _mm256_and_si256( x, y ); +} + +inline __m256d and_not_d( __m256i x, __m256i y ) +{ + __m256d xd = _mm256_castsi256_pd( x ); + __m256d yd = _mm256_castsi256_pd( y ); + return _mm256_andnot_pd( xd, yd ); +} + +inline __m256d and_not_d( __m256d x, __m256d y ) +{ + return _mm256_andnot_pd( x, y ); +} + +inline __m256i and_not_i( __m256i x, __m256i y ) +{ + __m256d xd = _mm256_castsi256_pd( x ); + __m256d yd = _mm256_castsi256_pd( y ); + __m256d rd = _mm256_andnot_pd( xd, yd ); + __m256i r = _mm256_castpd_si256( rd ); + return r; +} + +template < uint I0, uint I1, uint I2, uint I3 > +requires ( ( I0 < 2 ) && ( I1 < 2 ) && ( I2 < 2 ) && ( I3 < 2 ) ) +__m256d blend( __m256d x, __m256d y ) +{ + __m256d p = _mm256_blend_pd( x, y, ( I0 | ( I1 << 1 ) | ( I2 << 2 ) | ( I3 << 3 ) ) ); + return p; +} + +template < uint I0, uint I1, uint I2, uint I3 > +requires ( ( I0 < 2 ) && ( I1 < 2 ) && ( I2 < 2 ) && ( I3 < 2 ) ) +__m256i blend( __m256i x, __m256i y ) +{ + __m256d xd = _mm256_castsi256_pd( x ); + __m256d yd = _mm256_castsi256_pd( y ); + __m256d pd = _mm256_blend_pd( xd, yd, ( I0 | ( I1 << 1 ) | ( I2 << 2 ) | ( I3 << 3 ) ) ); + __m256i p = _mm256_castpd_si256( pd ); + return p; +} + +inline __m256d blend( __m256d a, __m256d b, __m256d mask ) +{ + __m256d blended = _mm256_blendv_pd( a, b, mask ); + return blended; +} + +inline __m256d blend( __m256d a, __m256d b, __m256i mask ) +{ + __m256d mask_d = _mm256_castsi256_pd( mask ); + __m256d blended = _mm256_blendv_pd( a, b, mask_d ); + return blended; +} + +inline __m256i blend( __m256i a, __m256i b, __m256i mask ) +{ + __m256d a_d = _mm256_castsi256_pd( a ); + __m256d b_d = _mm256_castsi256_pd( b ); + __m256d mask_d = _mm256_castsi256_pd( mask ); + __m256d blended_d = _mm256_blendv_pd( a_d, b_d, mask_d ); + __m256i blended = _mm256_castpd_si256( blended_d ); + return blended; +} + +inline __m256d blend_max( __m256d x, __m256d y, __m256d greater_mask ) +{ + __m256d max = blend( y, x, greater_mask ); + return max; +} + +inline __m256d blend_min( __m256d x, __m256d y, __m256d greater_mask ) +{ + __m256d min = blend( x, y, greater_mask ); + return min; +} + +inline __m256d cast_d( __m256i x ) +{ + return _mm256_castsi256_pd( x ); +} + +inline __m256i cast_i( __m256d x ) +{ + return _mm256_castpd_si256( x ); +} + +//---------------------------------------------------------------------------------------- +// +// gather_lanes< a, b >( x ), with a,b in { 0, 1, 2 } returns { x.lane( a ), x.lane( b ) } +// with x.lane( 2 ) = 0 +// +//---------------------------------------------------------------------------------------- + +constexpr int clear_lane = 8; + +template < uint I0, uint I1 > +requires ( ( ( I0 < 2 ) || ( I0 == clear_lane ) ) && ( ( I1 < 2 ) || ( I1 == clear_lane ) ) ) +__m256i gather_lanes( __m256i x ) +{ + constexpr int lane_0 = I0; + constexpr int lane_1 = I1 << 4; + __m256d xd = _mm256_castsi256_pd( x ); + __m256d gd = _mm256_permute2f128_pd( xd, xd, lane_0 | lane_1 ); + __m256i g = _mm256_castpd_si256( gd ); + return g; +} + +template < uint I0, uint I1 > +requires ( ( ( I0 < 2 ) || ( I0 == clear_lane ) ) && ( ( I1 < 2 ) || ( I1 == clear_lane ) ) ) +__m256d gather_lanes( __m256d x ) +{ + constexpr int lane_0 = I0; + constexpr int lane_1 = I1 << 4; + + __m256d p; + if constexpr ( ( I0 == 0 ) && ( I1 == 1 ) ) + p = x; + else + p = _mm256_permute2f128_pd( x, x, lane_0 | lane_1 ); + return p; +} + +//---------------------------------------------------------------------------------------- +// +// gather_lanes< a, b >( x, y ), with a,b in { 0, 1, clear_lane } returns { x.lane( a ), y.lane( b ) } +// with x.lane( clear_lane ) = 0 +// +//---------------------------------------------------------------------------------------- + +template < uint I0, uint I1 > +requires ( ( ( I0 < 2 ) || ( I0 == clear_lane ) ) && ( ( I1 < 2 ) || ( I1 == clear_lane ) ) ) +__m256d gather_lanes( __m256d x, __m256d y ) +{ + constexpr int lane_0 = I0; + constexpr int lane_1 = ( (I1 == clear_lane) ? clear_lane : ( I1 + 2 ) ) << 4; + + __m256d p; + if( ( I0 == 0 ) && ( I1 == 1 ) ) + p = blend< 0, 0, 1, 1 >( x, y ); + else + p = _mm256_permute2f128_pd( x, y, lane_0 | lane_1 ); + return p; +} + +//---------------------------------------------------------------------------------------- +// gather_high returns { x[ 1 ], y[ 1 ], x[ 3 ], y[ 3 ] } +//---------------------------------------------------------------------------------------- + +inline __m256d gather_high( __m256d x, __m256d y ) +{ + return _mm256_unpackhi_pd( x, y ); +} + +//---------------------------------------------------------------------------------------- +// gather_low returns { x[ 0 ], y[ 0 ], x[ 2 ], y[ 2 ] } +//---------------------------------------------------------------------------------------- + +inline __m256d gather_low( __m256d x, __m256d y ) +{ + return _mm256_unpacklo_pd( x, y ); +} + +//---------------------------------------------------------------------------------------- +// high_bits_64 returns the bits 63, 64 + 63, 128 + 63 and 196 + 63 of x +//---------------------------------------------------------------------------------------- + +inline int high_bits_64( __m256d x ) +{ + return _mm256_movemask_pd( x ); +} + +inline int high_bits_64( __m256i x ) +{ + __m256d xd = cast_d( x ); + return _mm256_movemask_pd( xd ); +} + +//---------------------------------------------------------------------------------------- +// gather_low returns { x[ 0 ], y[ 0 ], x[ 2 ], y[ 2 ] } +//---------------------------------------------------------------------------------------- + +inline __m256d is_greater( __m256d x, __m256d y ) +{ + __m256d gt = _mm256_cmp_pd( x, y, _CMP_GT_OQ ); + return gt; +} + +inline __m256i ones_i() +{ + __m256i r = _mm256_setzero_si256(); + return _mm256_cmpeq_epi64( r, r ); +} + +inline __m256d or_d( __m256d x, __m256d y ) +{ + return _mm256_or_pd( x, y ); +} + +inline __m256i or_i( __m256i x, __m256i y ) +{ + return _mm256_or_si256( x, y ); +} + +template < uint I0, uint I1, uint I2, uint I3 > +requires ( ( I0 < 4 ) && ( I1 < 4 ) && ( I2 < 4 ) && ( I3 < 4 ) ) +__m256d permute( __m256d x ) +{ + __m256d p; + if constexpr ( ( I0 < 2 ) && ( I1 < 2 ) && ( I2 > 1 ) && ( I3 > 1 ) ) + p = _mm256_permute_pd( x, ( I0 | ( I1 << 1 ) | ( ( I2 - 2 ) << 2 ) | ( ( I3 - 2 ) << 3 ) ) ); + else + p = _mm256_permute4x64_pd( x, ( I0 | ( I1 << 2 ) | ( I2 << 4 ) | ( I3 << 6 ) ) ); + return p; +} + +template < uint I0, uint I1, uint I2, uint I3 > +requires ( ( I0 < 4 ) && ( I1 < 4 ) && ( I2 < 4 ) && ( I3 < 4 ) ) +__m256i permute( __m256i x ) +{ + __m256d xd = _mm256_castsi256_pd( x ); + __m256d pd = permute< I0, I1, I2, I3 >( xd ); + __m256i p = _mm256_castpd_si256( pd ); + return p; +} + +inline __m256d xor_d( __m256d x, __m256d y ) +{ + return _mm256_xor_pd( x, y ); +} + +inline __m256d xor_d( __m256d x, __m256i y ) +{ + __m256d yd = cast_d( y ); + return _mm256_xor_pd( x, yd ); +} + +inline __m256d xor_d( __m256i x, __m256d y ) +{ + __m256d xd = cast_d( x ); + return _mm256_xor_pd( xd, y ); +} + +inline __m256i xor_i( __m256i x, __m256i y ) +{ + return _mm256_xor_si256( x, y ); +} + +//---------------------------------------------------------------------------------------- +// avx2 wrappers, to simplify the notation a bit and clarify the use of immediates +//---------------------------------------------------------------------------------------- + +enum class location +{ + null = -1024, + out = -1, + border = 0, + in = 1 +}; + +enum class bounded_convex_type +{ + empty = 0, + point = 1, + segment = 2, + polygon = 3 +}; + +//---------------------------------------------------------------------------------------- +// vertex_0 and vertex_1 scatter the arrays +// +// { inf_y, inf_z, inf_x, ? } and +// { m_sup_y, m_sup_z, m_sup_x, ? } +// +// to vertex_pair, in the x, y, z order +//---------------------------------------------------------------------------------------- + +struct vertex_0 +{ + static void scatter( __m256d* vertex_pair, __m256d inf, __m256d m_sup ) + { + __m256d y = gather_low( inf, m_sup ); // { inf( y ), m_sup( y ), inf( x ), m_sup( x ) } + __m256d z = gather_high( inf, m_sup ); // { inf( z ), m_sup( z ), ? ? } + __m256d x = gather_lanes< 1, 1 >( y ); // { inf( x ), m_sup( x ), ? ? } + + vertex_pair[ 0 ] = blend< 0, 0, 1, 1 >( x, vertex_pair[ 0 ] ); + vertex_pair[ 1 ] = blend< 0, 0, 1, 1 >( y, vertex_pair[ 1 ] ); + vertex_pair[ 2 ] = blend< 0, 0, 1, 1 >( z, vertex_pair[ 2 ] ); + } +}; + +struct vertex_1 +{ + static void scatter( __m256d* vertex_pair, __m256d inf, __m256d m_sup ) + { + __m256d x = gather_low( inf, m_sup ); // { inf( y ), m_sup( y ), inf( x ), m_sup( x ) } + __m256d z = gather_high( inf, m_sup ); // { inf( z ), m_sup( z ), ?, ? } + __m256d y = gather_lanes< 0, 0 >( x ); // { ?, ?, inf( y ), m_sup( y ) } + z = gather_lanes< 0, 0 >( z ); // { ?. . inf( z ), m_sup( z ) } + + vertex_pair[ 0 ] = blend< 0, 0, 1, 1 >( vertex_pair[ 0 ], x ); + vertex_pair[ 1 ] = blend< 0, 0, 1, 1 >( vertex_pair[ 1 ], y ); + vertex_pair[ 2 ] = blend< 0, 0, 1, 1 >( vertex_pair[ 2 ], z ); + } +}; + +//---------------------------------------------------------------------------------------- +// cross computes the solution ( x / z, y / z ) of the equation +// +// a[ 0 ] x + a[ 1 ] y = a[ 2 ] +// b[ 0 ] x + b[ 1 ] y = b[ 2 ] +// +// which is +// +// x = a[ 2 ] b[ 1 ] - a[ 1 ] b[ 2 ] +// y = a[ 0 ] b[ 2 ] - a[ 2 ] b[ 0 ] +// z = a[ 0 ] b[ 1 ] - a[ 1 ] b[ 0 ] +// +// which is convenient to order as +// +// y = a[ 0 ] b[ 2 ] - a[ 2 ] b[ 0 ] +// z = a[ 0 ] b[ 1 ] - a[ 1 ] b[ 0 ] +// x = a[ 2 ] b[ 1 ] - a[ 1 ] b[ 2 ] +// +// It assumes that x, y >= 0 and z > 0 and makes sure that the rounded values of +// x, y and z also satisfy these lower bounds. +//---------------------------------------------------------------------------------------- + +template < class Vertex > +inline void cross( __m256d* vertex_pair, __m256d a, __m256d b ) +{ + __m256d inf_a = _mm256_movedup_pd( a ); // { a[ 0 ], a[ 0 ], a[ 2 ], ? } + __m256d m_sup_b = _mm256_movedup_pd( b ); // { b[ 0 ], b[ 0 ], b[ 2 ], ? } + + __m256d inf_b = permute< 2, 1, 1, 3 >( b ); // { b[ 2 ], b[ 1 ], b[ 1 ], ? } + __m256d m_sup_a = permute< 2, 1, 1, 3 >( a ); // { a[ 2 ], a[ 1 ], a[ 1 ], ? } + + __m256d inf_h = _mm256_mul_pd( inf_a, inf_b ); + __m256d m_sup_h = _mm256_mul_pd( m_sup_a, m_sup_b ); + + __m256d inf = _mm256_fnmadd_pd( m_sup_a, m_sup_b, inf_h ); // inf(x) = rounded_down( x ) + __m256d m_sup = _mm256_fnmadd_pd( inf_a, inf_b, m_sup_h ); // -sup(x) = rounded_down( -x ) + + // x,y must be >= 0 and z must be positive and the computed values violate this due to + // rounding. In this rare case in which inf[ i ] <= 0 I can prove that the exact value + // of inf[ i ] is m_sup[ i ] - inf[ i ] + + __m256d cmp = _mm256_cmp_pd( inf, _mm256_setzero_pd(), _CMP_LE_OQ ); // rounded_down( x ) <= 0 ? + __m256d ds = _mm256_sub_pd( m_sup, inf ); + inf = blend( inf, ds, cmp ); + + Vertex::scatter( vertex_pair, inf, m_sup ); +} + +//---------------------------------------------------------------------------------------- +// exact_location returns the exact location of the intersection ( x, y ) of the lines +// +// b0[ 0 ] x + b0[ 1 ] y >= b0[ 2 ] +// b1[ 1 ] x + b1[ 1 ] y >= b1[ 2 ] +// +// with respect to the set +// +// e0 x + e1 y >= e2 +// +// under the assumption that +// +// d = b0[ 0 ] b1[ 1 ] - b0[ 1 ] b1[ 1 ] > 0 +// +// where ei = edge[ i ], b0 = border[ 0 ] and b1 = b[ 1 ] +// +// This amounts to evaluating the sign of +// +// p = e0 r + e1 s - e2 d +// +// r = b0[ 2 ] b1[ 1 ] - b0[ 1 ] b1[ 2 ] +// s = b0[ 0 ] b1[ 2 ] - b0[ 2 ] b1[ 0 ] , +// +// which motivates the evaluation of the six products +// +// group 1 group 2 +// e0 b0[ 2 ] b1[ 1 ], -e0 b0[ 1 ] b1[ 2 ] +// e1 b0[ 0 ] b1[ 2 ], -e1 b0[ 2 ] b1[ 0 ] +// e2 b0[ 1 ] b1[ 0 ]. -e2 b0[ 0 ] b1[ 1 ] +// +// and we want to find the sign of their sum. +// +// The function assumes that the edges entries are either normal or zero, ie., +// there are no subnormal entries. +//---------------------------------------------------------------------------------------- + +location exact_location( __m256d e, __m256d b0, __m256d b1 ); + +//---------------------------------------------------------------------------------------- +// The struct bounded convex represents a bounded convex set on the plane, which can +// have one of the following types: +// +// a) empty +// b) ṕoint +// c) segment +// d) polygon +// +//---------------------------------------------------------------------------------------- + +struct bounded_convex final +{ + constexpr bounded_convex( int alloc, __m256d* edge, __m256d* vertex_p ) + : alloc( alloc ), edge( edge ), vertex_p( vertex_p ){} + + void setup( __m256d box ); + + int insert_vertex( int at ); + int remove_vertices( int begin_vertex, int end_vertex ); + + void set_empty() + { + end = 0; + begin = 0; + type = bounded_convex_type::empty; + } + + void set_point( int first_vertex ) + { + end = 1; + begin = 0; + type = bounded_convex_type::point; + edge[ 0 ] = edge[ first_vertex ]; + edge[ 1 ] = edge[ first_vertex + 1 ]; + + __m256d* src = vertex_pair( first_vertex ); + + if( first_vertex & 0x1 ) + { + vertex_p[ 0 ] = gather_lanes< 1, 1 >( src[ 0 ] ); + vertex_p[ 1 ] = gather_lanes< 1, 1 >( src[ 1 ] ); + vertex_p[ 2 ] = gather_lanes< 1, 1 >( src[ 2 ] ); + } + else + { + vertex_p[ 0 ] = src[ 0 ]; + vertex_p[ 1 ] = src[ 1 ]; + vertex_p[ 2 ] = src[ 2 ]; + } + } + + void set_to_last_segment(); + + void set_segment( int first_vertex ) + { + end = 2; + begin = 0; + type = bounded_convex_type::segment; + edge[ 0 ] = edge[ first_vertex ]; + edge[ 1 ] = edge[ first_vertex + 1 ]; + edge[ 2 ] = edge[ first_vertex + 2 ]; + + __m256d* src = vertex_pair( first_vertex ); + + if( first_vertex & 0x1 ) + { + vertex_p[ 0 ] = gather_lanes< 1, 0 >( src[ 0 ], src[ 3 ] ); + vertex_p[ 1 ] = gather_lanes< 1, 0 >( src[ 1 ], src[ 4 ] ); + vertex_p[ 2 ] = gather_lanes< 1, 0 >( src[ 2 ], src[ 5 ] ); + } + else + { + vertex_p[ 0 ] = *src; + vertex_p[ 1 ] = *( ++src ); + vertex_p[ 2 ] = *( ++src ); + } + } + + __m256d box() const; + void push( __m256d* begin, __m256d* end ); + + __m256d plain_edge( int i ) const + { + return edge[ begin + i ]; + } + + void plain_vertex( __m256d* inf_msup, int i ) const + { + int ri = ( begin + i ); + __m256d const* vp = vertex_pair( ri ); + int di = 2 * ( ( begin + i ) & 0x1 ); + inf_msup[ 0 ] = _mm256_setr_pd( vp[ 0 ][ di ], vp[ 1 ][ di ], vp[ 2 ][ di ], 0.0 ); + inf_msup[ 1 ] = _mm256_setr_pd( vp[ 0 ][ di + 1 ], vp[ 1 ][ di + 1 ], vp[ 2 ][ di + 1 ], 0.0 ); + } + + __m256d* vertex_pair( int index ) + { + __m256d* p = vertex_p + 3 * ( index / 2 ); + return p; + } + + __m256d const* vertex_pair( int index ) const + { + __m256d* p = vertex_p + 3 * ( index / 2 ); + return p; + } + + int end; + int begin; + int const alloc; + __m256d* const edge; + __m256d* const vertex_p; + bounded_convex_type type; +}; + +// huge = { 0x5500'0000'0000'0000, 0x5500'0000'0000'0000, 0x5500'0000'0000'0000, 0x7FFF'7FFF'7FFF' } +// tiny = { 0x2bd0'0000'0000'0000, 0x2bd0'0000'0000'0000, 0x2bd0'0000'0000'0000, 0x0000'0000'0000'00000 } + +__m256i bounds( __m256i* tiny, __m256i* huge ); + +int normalize_equations( __m256d* quadrant[ 4 ], int n, __m256i tiny, __m256i huge ); + +//---------------------------------------------------------------------------------------- +// +// The class locator is used to find the location of a point with respect to the half +// plane. +// +// a x + b y >= c z +// +// The template parameters indicate the signs of the coefficients a, b and c. +// +//---------------------------------------------------------------------------------------- + +template < int flip_x, int flip_y, int flip_z > +struct locator final +{ + //---------------------------------------------------------------------------------------- + // The raw method assumes that + // + // edges[ 0 ] = { a, a, a, a } + // edges[ 1 ] = { b, b, b, b } + // edges[ 2 ] = { c, c, c, c } + // + // vertex_pair[ 0 ] = { x0_inf, x0_msup, x1_inf, x1_msup } + // vertex_pair[ 1 ] = { y0_inf, y0_msup, y1_inf, y1_msup } + // vertex_pair[ 2 ] = { z0_inf, z0_msup, z1_inf, z1_msup } + // + // and checks the location of the vertices ( x0, y0 ) and ( x1, y1 ) with respect to + // the half plane + // + // (-1)^flip_x a x + (-1)^flip_y b y >= (-1)^flip_z c z + // + // This is done by computing a x + b y - c z rounded up and down. + // + //---------------------------------------------------------------------------------------- + + static int raw( __m256d const* edge, __m256d const* vertex_pair ) + { + __m256d chk; + + if constexpr ( flip_x ) + chk = _mm256_mul_pd( edge[ 0 ], permute< 1, 0, 3, 2 >( vertex_pair[ 0 ] ) ); + else + chk = _mm256_mul_pd( edge[ 0 ], vertex_pair[ 0 ] ); + + if constexpr ( flip_y) + chk = _mm256_fmadd_pd( edge[ 1 ], permute< 1, 0, 3, 2 >( vertex_pair[ 1 ] ), chk ); + else + chk = _mm256_fmadd_pd( edge[ 1 ], vertex_pair[ 1 ], chk ); + + if constexpr ( flip_z ) + chk = _mm256_fmadd_pd( edge[ 2 ], vertex_pair[ 2 ], chk ); + else + chk = _mm256_fmadd_pd( edge[ 2 ], permute< 1, 0, 3, 2 >( vertex_pair[ 2 ] ), chk ); + + __m256d cmp = _mm256_cmp_pd( chk, _mm256_setzero_pd(), _CMP_GT_OQ ); + + int cm = _mm256_movemask_pd( cmp ); + + return cm; + } + + static location locate_vertex_0( __m256d const* edge, __m256d const* vertex_pair ) + { + location loc; + int cm = raw( edge, vertex_pair ); + + if( cm & 0x1 ) // inf( x[ 0 ] ) > 0 + loc = location::in; + else + { + if( cm & 0x2 ) + loc = location::out; + else + loc = location::null; + } + + return loc; + } + + static location locate_vertex_1( __m256d const* edge, __m256d const* vertex_pair ) + { + location loc; + int cm = raw( edge, vertex_pair ); + + if( cm & 0x4 ) // the second vertex is in + loc = location::in; + else + { + if( cm & 0x8 ) // the second vertex is out + loc = location::out; + else + loc = location::null; + } + + return loc; + } + + static void locate_pair( location loc[ 2 ], __m256d const* edge, __m256d const* vertex_pair ) + { + // computing a vertex[ 0 ] + b vertex[ 1 ] + c vertex[ 2 ] + + int cm = raw( edge, vertex_pair ); + if( cm & 0x1 ) // the first vertex is in + loc[ 0 ] = location::in; + else + { + if( cm & 0x2 ) // the first vertex is out + loc[ 0 ] = location::out; + else + loc[ 0 ] = location::null; + } + + if( cm & 0x4 ) // the second vertex is in + loc[ 1 ] = location::in; + else + { + if( cm & 0x8 ) // the second vertex is out + loc[ 1 ] = location::out; + else + loc[ 1 ] = location::null; + } + } + + static void insert( bounded_convex& r, __m256d new_edge ) + { + __m256i ones = ones_i(); + ones = _mm256_slli_epi64( ones, 63 ); + __m256d sign = _mm256_castsi256_pd( ones ); + + __m256d coefficients[ 3 ] = { permute< 0, 0, 0, 0 >( new_edge ), + permute< 1, 1, 1, 1 >( new_edge ), + permute< 2, 2, 2, 2 >( new_edge ) }; + + coefficients[ 0 ] = _mm256_andnot_pd( sign, coefficients[ 0 ] ); + coefficients[ 1 ] = _mm256_andnot_pd( sign, coefficients[ 1 ] ); + coefficients[ 2 ] = _mm256_andnot_pd( sign, coefficients[ 2 ] ); + + if( r.type != bounded_convex_type::polygon ) + { + if( r.type == bounded_convex_type::point ) + { + location loc = locate_vertex_0( coefficients, r.vertex_p ); + + if( loc == location::null ) + loc = exact_location( new_edge, r.edge[ 0 ], r.edge[ 1 ] ); + + if( loc == location::out ) + r.set_empty(); + + return; + } + + location loc[ 2 ]; + locate_pair( loc, coefficients, r.vertex_p ); + + if( loc[ 0 ] == location::null ) + loc[ 0 ] = exact_location( new_edge, r.edge[ 0 ], r.edge[ 1 ] ); + + if( loc[ 1 ] == location::null ) + loc[ 1 ] = exact_location( new_edge, r.edge[ 1 ], r.edge[ 2 ] ); + + switch( loc[ 0 ] ) + { + case location::in: + { + if( loc[ 1 ] == location::out ) + { + r.edge[ 2 ] = new_edge; + cross< vertex_1 >( r.vertex_p, r.edge[ 1 ], new_edge ); + } + return; + } + case location::border: + { + if( loc[ 1 ] == location::out ) + { + r.end = 1; + r.type = bounded_convex_type::point; + } + return; + } + default: // location::out + { + switch( loc[ 1 ] ) + { + case location::in: + { + r.edge[ 0 ] = new_edge; + cross< vertex_0 >( r.vertex_p, new_edge, r.edge[ 1 ] ); + return; + } + case location::border: + { + r.end = 1; + r.edge[ 0 ] = r.edge[ 1 ]; + r.edge[ 1 ] = r.edge[ 2 ]; + r.type = bounded_convex_type::point; + r.vertex_p[ 0 ] = gather_lanes< 1, 1 >( r.vertex_p[ 0 ] ); + r.vertex_p[ 1 ] = gather_lanes< 1, 1 >( r.vertex_p[ 1 ] ); + r.vertex_p[ 2 ] = gather_lanes< 1, 1 >( r.vertex_p[ 2 ] ); + return; + } + default: + { + r.set_empty(); + return; + } + } + } + } + } + + location loc[ 2 ]; + + int index = r.begin; + int intersection = -1; + bool intersection_at_vertex; + + __m256d* vertex_pair_at_index = r.vertex_pair( index ); + + auto scan = [ & ]() + { + if( ++index >= r.end ) + return location::null; + + if( index & 0x1 ) + return loc[ 1 ]; + + vertex_pair_at_index += 3; + + if( index + 1 >= r.end ) + { + loc[ 0 ] = locate_vertex_0( coefficients, vertex_pair_at_index ); + + if( loc[ 0 ] == location::null ) + loc[ 0 ] = exact_location( new_edge, r.edge[ index ], r.edge[ index + 1 ] ); + } + else + { + locate_pair( loc, coefficients, vertex_pair_at_index ); + + if( loc[ 0 ] == location::null ) + loc[ 0 ] = exact_location( new_edge, r.edge[ index ], r.edge[ index + 1 ] ); + + if( loc[ 1 ] == location::null ) + loc[ 1 ] = exact_location( new_edge, r.edge[ index + 1 ], r.edge[ index + 2 ] ); + } + + return loc[ 0 ]; + }; + + if( index & 0x1 ) + { + location begin = locate_vertex_1( coefficients, vertex_pair_at_index ); + + if( begin == location::null ) + begin = exact_location( new_edge, r.edge[ index ], r.edge[ index + 1 ] ); + + switch( begin ) + { + case location::in: + goto cursor_in; + case location::border: + { + intersection = index; + intersection_at_vertex = true; + + ++index; + vertex_pair_at_index += 3; + + locate_pair( loc, coefficients, vertex_pair_at_index ); + + if( loc[ 0 ] == location::null ) + loc[ 0 ] = exact_location( new_edge, r.edge[ index ], r.edge[ index + 1 ] ); + + if( loc[ 1 ] == location::null ) + loc[ 1 ] = exact_location( new_edge, r.edge[ index + 1 ], r.edge[ index + 2 ] ); + + switch( loc[ 0 ] ) + { + case location::in: + goto cursor_in; + case location::border: + { + if( loc[ 1 ] == location::out ) + r.set_segment( r.begin ); + return; + } + case location::out: + goto cursor_out; + } + } + default: + goto cursor_out; + } + } + + locate_pair( loc, coefficients, vertex_pair_at_index ); + + if( loc[ 0 ] == location::null ) + loc[ 0 ] = exact_location( new_edge, r.edge[ index ], r.edge[ index + 1 ] ); + + if( loc[ 1 ] == location::null ) + loc[ 1 ] = exact_location( new_edge, r.edge[ index + 1 ], r.edge[ index + 2 ] ); + + switch( loc[ 0 ] ) + { + case location::in: + goto cursor_in; + case location::border: + { + intersection = index; + intersection_at_vertex = true; + + ++index; + + switch( loc[ 1 ] ) + { + case location::in: + goto cursor_in; + case location::border: + { + location next = locate_vertex_0( coefficients, vertex_pair_at_index + 3 ); + + if( next == location::null ) + next = exact_location( new_edge, r.edge[ index ], r.edge[ index + 1 ] ); + + if( next == location::out ) + r.set_segment( r.begin ); + + return; + } + default: + goto cursor_out; + } + } + default: + goto cursor_out; + } + + cursor_in: + { + location next_loc = scan(); + + switch( next_loc ) + { + case location::in: + goto cursor_in; + case location::border: + { + if( intersection >= 0 ) + { + if( !intersection_at_vertex ) + { + __m256d* vp = r.vertex_pair( intersection - 1 ); + if( intersection & 0x1 ) + cross< vertex_0 >( vp, new_edge, r.edge[ intersection ] ); + else + cross< vertex_1 >( vp, new_edge, r.edge[ intersection ] ); + + --intersection; + } + + r.end = index + 1; + r.begin = intersection; + r.edge[ r.end ] = new_edge; + r.edge[ r.begin ] = new_edge; + } + else + { + next_loc = scan(); + + if( next_loc == location::out ) + { + intersection = index - 1; + intersection_at_vertex = true; + goto cursor_out; + } + } + return; + } + case location::out: + { + if( intersection < 0 ) + { + intersection = index; + intersection_at_vertex = false; + goto cursor_out; + } + + if( intersection_at_vertex ) + r.begin = intersection; + else + { + r.begin = intersection - 1; + __m256d* vp = r.vertex_pair( r.begin ); + + if( r.begin & 0x1 ) + cross< vertex_1 >( vp, new_edge, r.edge[ intersection ] ); + else + cross< vertex_0 >( vp, new_edge, r.edge[ intersection ] ); + } + + __m256d* vp = r.vertex_pair( index ); + if( index & 0x1 ) + cross< vertex_1 >( vp, r.edge[ index ], new_edge ); + else + cross< vertex_0 >( vp, r.edge[ index ], new_edge ); + + r.end = index + 1; + r.edge[ r.end ] = new_edge; + r.edge[ r.begin ] = new_edge; + return; + } + case location::null: + { + assert( index == r.end ); + + if( intersection >= 0 ) + { + if( intersection_at_vertex ) + { + if( intersection == r.begin ) + return; + } + else + { + __m256d* first_pair = r.vertex_pair( intersection - 1 ); + + if( intersection & 0x1 ) + cross< vertex_0 >( first_pair, new_edge, r.edge[ intersection ] ); + else + cross< vertex_1 >( first_pair, new_edge, r.edge[ intersection ] ); + + --intersection; + } + + __m256d* last_pair = r.vertex_pair( index ); + + if( index & 0x1 ) + cross< vertex_1 >( last_pair, r.edge[ index ], new_edge ); + else + cross< vertex_0 >( last_pair, r.edge[ index ], new_edge ); + + ++r.end; + r.begin = intersection; + r.edge[ r.end ] = new_edge; + r.edge[ r.begin ] = new_edge; + } + + return; + } + } + } + + cursor_out: + { + location new_loc = scan(); + + switch( new_loc ) + { + case location::in: + { + if( intersection < 0 ) + { + intersection = index; + intersection_at_vertex = false; + goto cursor_in; + } + + if( intersection + 1 == index ) + { + assert( !intersection_at_vertex ); + int new_vertex = r.insert_vertex( intersection ); + + if( new_vertex == intersection ) + { + __m256d* vp = r.vertex_pair( intersection ); + if( intersection & 0x1 ) + { + cross< vertex_1 >( vp, r.edge[ intersection ], new_edge ); + cross< vertex_0 >( vp + 3, new_edge, r.edge[ intersection + 2 ] ); + } + else + { + cross< vertex_0 >( vp, r.edge[ intersection ], new_edge ); + cross< vertex_1 >( vp, new_edge, r.edge[ intersection + 2 ] ); + } + + r.edge[ intersection + 1 ] = new_edge; + } + else + { + assert( new_vertex == intersection - 1 ); + + __m256d* vp = r.vertex_pair( intersection ); + + if( intersection & 0x1 ) + { + cross< vertex_1 >( vp, new_edge, r.edge[ index ] ); + cross< vertex_0 >( vp, r.edge[ intersection ], new_edge) ; + } + else + { + cross< vertex_0 >( vp, new_edge, r.edge[ index ] ); + cross< vertex_1 >( vp - 3, r.edge[ intersection ], new_edge) ; + } + + r.edge[ new_vertex ] = r.edge[ intersection ]; + r.edge[ intersection ] = new_edge; + } + return; + } + + if( !intersection_at_vertex ) + { + __m256d* vp = r.vertex_pair( intersection ); + + if( intersection & 0x1 ) + cross< vertex_1 >( vp, r.edge[ intersection ], new_edge ); + else + cross< vertex_0 >( vp, r.edge[ intersection ], new_edge ); + } + + if( intersection + 2 < index ) + index = r.remove_vertices( intersection + 1, index - 1 ) + 2; + + __m256d* vp = r.vertex_pair( index - 1 ); + if( index & 0x1 ) + cross< vertex_0 >( vp, new_edge, r.edge[ index ] ); + else + cross< vertex_1 >( vp, new_edge, r.edge[ index ] ); + + r.edge[ index - 1 ] = new_edge; + return; + } + case location::border: + { + if( intersection >= 0 ) + { + if( intersection_at_vertex ) + { + if( ( intersection == r.begin ) && ( index + 1 == r.end ) ) + { + r.set_to_last_segment(); + return; + } + } + else + { + __m256d* vp = r.vertex_pair( intersection ); + if( intersection & 0x1 ) + cross< vertex_1 >( vp, r.edge[ intersection ], new_edge ); + else + cross< vertex_0 >( vp, r.edge[ intersection ], new_edge ); + } + + if( intersection + 1 < index ) + intersection = r.remove_vertices( intersection + 1, index ); + + r.edge[ intersection + 1 ] = new_edge; + return; + } + + new_loc = scan(); + switch( new_loc ) + { + case location::in: + { + intersection = index - 1; + intersection_at_vertex = true; + goto cursor_in; + } + case location::border: + { + r.set_segment( index - 1 ); + return; + } + default: // location::out or location::null: + { + r.set_point( index - 1 ); + return; + } + } + } + case location::out: + goto cursor_out; + case location::null: + { + assert( index == r.end ); + + if( intersection < 0 ) + { + r.set_empty(); + return; + } + + if( intersection_at_vertex ) + { + if( intersection == r.begin ) + { + r.set_point( r.begin ); + return; + } + } + else + { + __m256d* vp = r.vertex_pair( intersection ); + + if( intersection & 0x1 ) + cross< vertex_1 >( vp, r.edge[ intersection ], new_edge ); + else + cross< vertex_0 >( vp, r.edge[ intersection ], new_edge ); + } + + ++intersection; + __m256d* vp = r.vertex_pair( intersection ); + + if( intersection & 0x1 ) + cross< vertex_1 >( vp, new_edge, r.edge[ index ] ); + else + cross< vertex_0 >( vp, new_edge, r.edge[ index ] ); + + r.end = intersection + 1; + r.edge[ intersection ] = new_edge; + r.edge[ r.end ] = r.edge[ r.begin ]; + } + + return; + } + } + } +}; + +} // namespace wm::nlp::lp2d + +//---------------------------------------------------------------------------------------- +// +// The function interval_system_2d solves the system of interval equations +// +// ai x + bi y = ci +// +// for 0 <= i < n_equations and +// +// ai = [ eq[ 6 * i ], eq[ 6 * i + 1 ] ] with eq[ 6 * i ] <= eq[ 6 * i + 1 ] +// bi = [ eq[ 6 * i + 2 ], eq[ 6 * i + 3 ] ] with eq[ 6 * i + 2 ] <= eq[ 6 * i + 3 ] +// ci = [ eq[ 6 * i + 4 ], eq[ 6 * i + 5 ] ] with eq[ 6 * i + 4 ] <= eq[ 6 * i + 5 ] +// +// The function finds the solutions inside the box +// +// box[ 0 ] <= x <= box[ 1 ] +// box[ 2 ] <= y <= box[ 3 ] +// +// The solution consists of up to 4 boxes bx[ 0 ], bx[ 1 ], bx[ 2 ] and bx[ 3 ], with +// +// bx[ i ] = inf_x[ i ] <= x <= sup_x[ i ] +// inf_y[ i ] <= y <= sup_y[ i ] +// for +// +// inf_x[ i ] = solution[ 4 * i ] +// sup_x[ i ] = solution[ 4 * i + 1 ] +// inf_y[ i ] = solution[ 4 * i + 2 ] +// sup_y[ i ] = solution[ 4 * i + 3 ] +// +// The function returns the number of boxes in the solutions, which can be 0,1,2,3 or 4, +// or a negative number if the input is invalid or if it cannot change the rounding mode. +// +// It assumes that: +// +// a) n_equations <= 16 +// b) all doubles are finite, ie., not nan or +/-inf +// +// c) box and eq are such that +// +// box[ 0 ] <= box[ 1 ] +// box[ 2 ] <= box[ 3] +// eq[ 2 * i ] <= eq[ 2 i + 1 ] for i = 0,..., 3 * n_equations. +// +// d) box and eq are aligned at a 32 byte boundary +// +// It does not verify the input and its results should not be trusted if the inputs are +// invalid. +// +//---------------------------------------------------------------------------------------- + +extern "C" +{ + +int interval_system_2d( double* solution, double const* box, double const* eq, unsigned int n_equations ); +}
+ cbits/mul.S view
@@ -0,0 +1,30 @@+.global in2_mul8 +in2_mul8: + vshufpd $3, %xmm1, %xmm1, %xmm4 + vshufpd $3, %xmm2, %xmm2, %xmm3 + vxorpd %xmm1, %xmm4, %xmm5 + vxorpd %xmm2, %xmm3, %xmm6 + vtestpd %xmm5, %xmm6 + je .LBB1_2 + vxorpd %xmm5, %xmm5, %xmm5 + vcmplepd %xmm5, %xmm3, %xmm3 + vcmplepd %xmm5, %xmm4, %xmm4 + vmovsd .LCPI1_1(%rip), %xmm5 + vxorpd %xmm5, %xmm1, %xmm1 + vshufps $78, %xmm1, %xmm1, %xmm6 + vblendvpd %xmm3, %xmm6, %xmm1, %xmm1 + vxorpd %xmm5, %xmm2, %xmm2 + vshufps $78, %xmm2, %xmm2, %xmm3 + vblendvpd %xmm4, %xmm3, %xmm2, %xmm2 + vxorpd %xmm5, %xmm2, %xmm2 + vmulpd %xmm2, %xmm1, %xmm1 + jmp *(%rbp) +.LCPI1_1: + .quad -9223372036854775808 +.LBB1_2: + vshufpd $1, %xmm1, %xmm1, %xmm4 + vmovddup %xmm2, %xmm2 + vmulpd %xmm4, %xmm2, %xmm2 + vmulpd %xmm1, %xmm3, %xmm1 + vmaxpd %xmm1, %xmm2, %xmm1 + jmp *(%rbp)
+ cbits/rounding.c view
@@ -0,0 +1,9 @@+#include <xmmintrin.h> + +// Function to set the CSR rounding mode for SSE operations +void set_sse_rounding_mode(unsigned int rounding_mode) { + unsigned int csr = _mm_getcsr(); + csr &= ~_MM_ROUND_MASK; // Clear existing rounding mode bits + csr |= rounding_mode; // Set new rounding mode + _mm_setcsr(csr); +}
+ changelog.md view
@@ -0,0 +1,5 @@+# Changelog for `brush-strokes` + +## 0.1.0.0 – 2026-03-28 + +Initial Hackage release.
+ img/METAFONT_logo.svg view
@@ -0,0 +1,5 @@+<?xml version="1.0" encoding="UTF-8" standalone="no"?> +<!-- Created with Inkscape (http://www.inkscape.org/) --> +<svg xmlns:svg="http://www.w3.org/2000/svg" xmlns="http://www.w3.org/2000/svg" version="1.0" width="250" height="40" viewBox="0 0 231.88 28.524" id="Layer_1" xml:space="preserve"><defs id="defs2208"/> +<path d="M 29.049192,26.855797 L 29.049192,7.2687353 L 18.968117,24.801632 C 18.701494,25.248274 18.207171,25.518789 17.630137,25.518789 C 17.048237,25.518789 16.557806,25.248274 16.291183,24.801632 L 6.207189,7.2687353 L 6.207189,26.855797 C 6.207189,27.613823 5.5805276,28.195724 4.7320047,28.195724 C 3.9331086,28.195724 3.2636319,27.613823 3.2636319,26.855797 L 3.2636319,1.7796095 C 3.2636319,1.023529 3.9331086,0.44162904 4.7320047,0.44162904 C 5.3139046,0.44162904 5.8043352,0.70825206 6.0758236,1.1548943 L 17.630137,21.277148 L 29.185423,1.1548943 C 29.452046,0.70825206 29.94053,0.44162904 30.52243,0.44162904 C 31.327165,0.44162904 31.997614,1.023529 31.997614,1.7796095 L 31.997614,26.85677 C 31.997614,27.614797 31.327165,28.196696 30.52243,28.196696 C 29.67488,28.195724 29.049192,27.613823 29.049192,26.855797 z M 40.429324,27.700427 C 39.62459,27.700427 38.95414,27.123393 38.95414,26.363419 L 38.95414,2.2690671 C 38.95414,1.5139596 39.62459,0.93108663 40.429324,0.93108663 L 58.765982,0.93108663 C 59.615479,0.93108663 60.283982,1.5129865 60.283982,2.2690671 C 60.283982,3.0319593 59.614505,3.611913 58.765982,3.611913 L 41.89867,3.611913 L 41.89867,14.318676 L 56.090021,14.318676 C 56.934652,14.318676 57.603156,14.900576 57.603156,15.656657 C 57.603156,16.413711 56.934652,16.996584 56.090021,16.996584 L 41.89867,16.996584 L 41.89867,25.024467 L 58.766955,25.024467 C 59.616451,25.024467 60.284955,25.606367 60.284955,26.364393 C 60.284955,27.124366 59.615479,27.701401 58.766955,27.701401 L 40.429324,27.701401 L 40.429324,27.700427 z M 74.704006,26.855797 L 74.704006,3.611913 L 64.710508,3.611913 C 63.905773,3.611913 63.236297,3.0319593 63.236297,2.2690671 C 63.236297,1.5139596 63.905773,0.93108663 64.710508,0.93108663 L 87.596299,0.93108663 C 88.445795,0.93108663 89.114299,1.5129865 89.114299,2.2690671 C 89.114299,3.0319593 88.445795,3.611913 87.596299,3.611913 L 77.64659,3.611913 L 77.64659,26.855797 C 77.64659,27.613823 76.977113,28.195724 76.172378,28.195724 C 75.328721,28.195724 74.704006,27.613823 74.704006,26.855797 z M 112.08378,3.8347476 C 114.66925,7.4477814 114.31407,12.175961 114.31407,15.655684 L 114.31407,26.855797 C 114.31407,27.613823 113.6446,28.195724 112.83986,28.195724 C 111.99133,28.195724 111.32186,27.613823 111.32186,26.855797 L 111.32186,16.99561 L 94.458437,16.99561 L 94.458437,26.855797 C 94.458437,27.613823 93.831775,28.195724 92.983252,28.195724 C 92.183384,28.195724 91.51488,27.613823 91.51488,26.855797 L 91.51488,15.655684 C 91.51488,12.175961 91.155814,7.4468084 93.745171,3.8347476 C 95.932648,0.75593282 99.902807,0.44162904 102.89112,0.44162904 C 105.88236,0.44162904 109.8963,0.75593282 112.08378,3.8347476 z M 94.45941,14.318676 L 111.32283,14.318676 C 111.37051,10.706616 111.32283,7.670616 109.58687,5.260305 C 108.33451,3.5194707 105.92518,3.1175901 102.89112,3.1175901 C 99.902807,3.1175901 97.493463,3.5194707 96.242086,5.260305 C 94.502225,7.670616 94.45941,10.705642 94.45941,14.318676 z M 122.73119,28.195724 C 121.92742,28.195724 121.2599,27.613823 121.2599,26.855797 L 121.2599,2.2690671 C 121.2599,1.5139596 121.92742,0.93108663 122.73119,0.93108663 L 141.06979,0.93108663 C 141.91929,0.93108663 142.58876,1.5129865 142.58876,2.2690671 C 142.58876,3.0319593 141.91929,3.611913 141.06979,3.611913 L 124.20637,3.611913 L 124.20637,14.318676 L 138.39578,14.318676 C 139.24137,14.318676 139.9128,14.900576 139.9128,15.656657 C 139.9128,16.413711 139.24235,16.996584 138.39578,16.996584 L 124.20637,16.996584 L 124.20637,26.85677 C 124.20637,27.613823 123.58068,28.195724 122.73119,28.195724 z M 145.00783,15.655684 C 145.00783,12.043622 144.73828,7.1821315 147.72952,3.7423053 C 150.49404,0.4854175 155.08793,0.44162904 158.38861,0.44162904 C 161.73502,0.44162904 166.33281,0.4854175 169.09829,3.7432783 C 172.04184,7.1821315 171.77814,12.043622 171.77814,15.656657 C 171.77814,18.644003 171.86183,22.837963 169.00682,25.564524 C 165.97764,28.371851 161.51316,28.196696 158.38861,28.196696 L 155.84791,28.196696 C 154.91083,28.14707 153.97375,28.105228 153.03474,27.925208 C 151.16254,27.61577 149.33023,26.988135 147.82002,25.564524 C 144.96209,22.836989 145.00783,18.644003 145.00783,15.655684 z M 150.04934,5.3488549 C 147.90565,7.8924775 147.99323,11.418908 147.99323,15.655684 C 147.99323,19.137352 148.12848,21.944679 149.95884,23.685513 C 150.94166,24.577824 152.14437,25.07312 153.57383,25.294982 C 154.64226,25.471109 155.84888,25.517816 157.14502,25.517816 L 158.34774,25.517816 C 162.09409,25.517816 164.90627,25.517816 166.86994,23.68454 C 168.69641,21.943705 168.82777,19.136379 168.82777,15.654711 C 168.82777,11.417934 168.8774,7.8915044 166.73372,5.3478819 C 164.99288,3.3404246 162.05224,3.116617 158.39055,3.116617 C 154.73471,3.1175901 151.78628,3.3413977 150.04934,5.3488549 z M 178.23256,28.195724 C 177.42686,28.195724 176.75738,27.613823 176.75738,26.855797 L 176.75738,1.7796095 C 176.75738,1.024502 177.42686,0.44162904 178.23256,0.44162904 C 178.72104,0.44162904 179.12389,0.62067516 179.38955,0.93108663 C 185.14627,7.403993 190.85726,13.872034 196.56825,20.340075 L 196.56825,1.7796095 C 196.56825,1.024502 197.23578,0.44162904 198.08527,0.44162904 C 198.88515,0.44162904 199.55461,1.023529 199.55461,1.7796095 L 199.55461,26.85677 C 199.55461,27.614797 198.88515,28.196696 198.08527,28.196696 C 197.23578,28.196696 196.56825,27.614797 196.56825,26.85677 L 196.56825,24.67124 C 190.94873,18.290776 185.32239,11.953126 179.70482,5.6203433 L 179.70482,26.85677 C 179.7058,27.613823 179.07816,28.195724 178.23256,28.195724 z M 214.49134,26.855797 L 214.49134,3.611913 L 204.49784,3.611913 C 203.69214,3.611913 203.02267,3.0319593 203.02267,2.2690671 C 203.02267,1.5139596 203.69214,0.93108663 204.49784,0.93108663 L 227.38462,0.93108663 C 228.23411,0.93108663 228.90164,1.5129865 228.90164,2.2690671 C 228.90164,3.0319593 228.23411,3.611913 227.38462,3.611913 L 217.4349,3.611913 L 217.4349,26.855797 C 217.4349,27.613823 216.76542,28.195724 215.96556,28.195724 C 215.11606,28.195724 214.49134,27.613823 214.49134,26.855797 z " id="path2205"/> +</svg>
+ img/bench_cubics.png view
binary file changed (absent → 758764 bytes)
+ img/bench_quadratics.png view
binary file changed (absent → 333889 bytes)
+ img/clenshaw_error_landscape.png view
binary file changed (absent → 402865 bytes)
+ img/example_stroke.svg view
@@ -0,0 +1,316 @@+<?xml version="1.0" encoding="UTF-8" standalone="no"?> +<svg + width="655.83875" + height="534.828" + viewBox="0 0 655.83875 534.828" + version="1.1" + id="svg382" + sodipodi:docname="example_stroke.svg" + inkscape:version="1.2.1 (9c6d41e410, 2022-07-14)" + xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" + xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" + xmlns="http://www.w3.org/2000/svg" + xmlns:svg="http://www.w3.org/2000/svg"> + <defs + id="defs386"> + <marker + style="overflow:visible" + id="marker1543" + refX="0" + refY="0" + orient="auto" + inkscape:stockid="Dot" + markerWidth="2.5" + markerHeight="2.5" + viewBox="0 0 5.6666667 5.6666667" + inkscape:isstock="true" + inkscape:collect="always" + preserveAspectRatio="xMidYMid"> + <path + transform="scale(0.5)" + style="fill:context-stroke;fill-rule:evenodd;stroke:context-stroke;stroke-width:1pt" + d="M 5,0 C 5,2.76 2.76,5 0,5 -2.76,5 -5,2.76 -5,0 c 0,-2.76 2.3,-5 5,-5 2.76,0 5,2.24 5,5 z" + id="path1541" + sodipodi:nodetypes="sssss" /> + </marker> + <marker + style="overflow:visible" + id="Dot" + refX="0" + refY="0" + orient="auto" + inkscape:stockid="Dot" + markerWidth="2.5" + markerHeight="2.5" + viewBox="0 0 5.6666667 5.6666667" + inkscape:isstock="true" + inkscape:collect="always" + preserveAspectRatio="xMidYMid"> + <path + transform="scale(0.5)" + style="fill:context-stroke;fill-rule:evenodd;stroke:context-stroke;stroke-width:1pt" + d="M 5,0 C 5,2.76 2.76,5 0,5 -2.76,5 -5,2.76 -5,0 c 0,-2.76 2.3,-5 5,-5 2.76,0 5,2.24 5,5 z" + id="Dot1" + sodipodi:nodetypes="sssss" /> + </marker> + </defs> + <sodipodi:namedview + id="namedview384" + pagecolor="#ffffff" + bordercolor="#000000" + borderopacity="0.25" + inkscape:showpageshadow="2" + inkscape:pageopacity="0.0" + inkscape:pagecheckerboard="0" + inkscape:deskcolor="#d1d1d1" + showgrid="false" + inkscape:zoom="1.6775372" + inkscape:cx="339.78382" + inkscape:cy="252.75148" + inkscape:window-width="3840" + inkscape:window-height="2054" + inkscape:window-x="3829" + inkscape:window-y="-11" + inkscape:window-maximized="1" + inkscape:current-layer="svg382" /> + <g + id="g2367" + transform="translate(-141.81688,-142.41298)"> + <path + fill-rule="nonzero" + fill="#eb76db" + fill-opacity="0.66" + d="m 218.73828,365.40625 c -8.57031,2.32812 -18.11719,0.12891 -24.84375,-6.59375 -9.98828,-9.99219 -9.98828,-26.21875 0,-36.20703 53.93359,-53.9336 92.46875,-85.3086 115.60547,-94.13281 5.86719,-2.23829 11.4375,-4.25 16.6875,-6.02735 26.03906,-8.82812 44.91797,-12.16797 60.20703,-11.60937 11.91797,0.43359 21,4.13672 27.90625,10.33203 4.01563,3.59765 7.59766,7.83203 10.90234,12.80859 2.75782,4.14844 5.32422,8.82031 7.80469,14.15235 13.92969,29.95703 19.0586,69.04687 47.49219,133.61718 0.44531,1.01563 0.94531,1.98828 1.40625,3.01953 0.38672,0.86329 0.8125,1.73438 1.23828,2.67188 2,4.38672 5.44531,11.67969 9.26563,19.11328 0.0117,0.0195 0.0195,0.043 0.0312,0.0625 6.30859,12.28516 12.60937,24.94922 19.42578,37.26953 5.69531,10.28906 11.77734,20.41406 18.57422,30.14063 33.59375,48.08203 84.95312,87.74218 199.30859,96.05468 6.43359,0.46875 12.14844,4.96875 13.84375,11.5586 2.12891,8.25781 -2.84766,16.6875 -11.10547,18.8164 C 659.01562,619.375 615.70312,626.17578 602.54687,620.85156 483.83594,572.78906 449.93359,494.01562 439.33203,424.88672 c -2.08984,-13.6211 -3.35937,-27.66797 -4.13672,-39.79688 -0.98047,-15.22265 -1.39453,-29.78515 -2.0039,-41.3789 0,-0.0195 -0.004,-0.0352 -0.004,-0.0547 -0.49219,-9.42578 1.01562,2.76953 2.45312,7.85937 22.16509,38.86335 11.9908,7.54009 21.42969,58.78907 -2.48828,-4.62891 -22.67578,-61.4375 -15.05078,-38.23047 2.19531,6.68359 -0.31641,-0.96094 0,0.004 16.52734,50.29297 -4.125,-0.73437 -31.10156,-16.17968 -3.69141,-2.11328 -7.65625,-4 -11.97266,-5.64453 -5.1914,-1.97657 -10.84765,-3.59375 -17.1875,-4.82032 -14.36719,-2.78125 -32.27734,-3.53125 -54.73828,-1.52734 -28.31641,2.52734 -63.85547,9.44141 -108.28125,21.5" + id="path262" + sodipodi:nodetypes="ccscccccccccccccccccccccccccccccc" + style="fill:#69afd5;fill-opacity:0.66" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m 33.052734,-33.050819 c 3.015625,9.050781 -12.984375,31.085937 -48,66.103516" + transform="matrix(2,0,0,2,260,292.70711)" + id="path264" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m -14.947266,33.052697 c -4.996093,4.99414 -13.109375,4.99414 -18.105468,0" + id="path266" + transform="matrix(2,0,0,2,260,292.70711)" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m -33.052734,33.052697 c -4.994141,-4.996094 -4.994141,-13.109375 0,-18.103516" + transform="matrix(2,0,0,2,260,292.70711)" + id="path268" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M -33.052734,14.949181 C 1.964844,-20.068397 24,-36.068397 33.052734,-33.050819" + transform="matrix(2,0,0,2,260,292.70711)" + id="path270" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="3" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(72.156864%, 31.37255%, 31.37255%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M -142.75,-88.250038 C 16.757812,-134.97465 -102.42969,28.769493 63,65.499962" + transform="matrix(2,0,0,2,545.5,469.20711)" + id="path272" + style="stroke:#4a66b1;stroke-width:2;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:none;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m -0.187944,-13.998487 c 3.355468,1.621094 5.125,9.265625 5.308593,22.929688" + transform="matrix(2,0,0,2,579.01651,199.25869)" + id="path274" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m 5.120649,8.931201 c 0.03711,2.759766 -2.173828,5.03125 -4.933593,5.068359" + transform="matrix(2,0,0,2,579.01651,199.25869)" + id="path276" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M 0.187056,13.99956 C -2.570757,14.03667 -4.842241,11.825732 -4.879351,9.065967" + transform="matrix(2,0,0,2,579.01651,199.25869)" + id="path278" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m -4.879351,9.065967 c -0.183593,-13.666016 1.38086,-21.353516 4.691407,-23.064454" + transform="matrix(2,0,0,2,579.01651,199.25869)" + id="path280" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m -24.935972,-17.640018 c 6.763672,-3.621094 22.56836,3.357422 47.414063,20.933594" + transform="matrix(2,0,0,2,340.64147,526.74488)" + id="path282" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m 22.478091,3.293576 c 4.638672,3.28125 5.740234,9.708984 2.458984,14.347656" + transform="matrix(2,0,0,2,340.64147,526.74488)" + id="path284" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m 24.937075,17.641232 c -3.28125,4.636719 -9.710937,5.738281 -14.347656,2.458984" + transform="matrix(2,0,0,2,340.64147,526.74488)" + id="path286" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M 10.589419,20.100216 C -14.256284,2.522091 -26.098081,-10.057987 -24.935972,-17.640018" + transform="matrix(2,0,0,2,340.64147,526.74488)" + id="path288" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m -36.046875,9.285118 c 1.208984,-5.628906 22.103516,-13.669921 62.6875,-24.125" + transform="matrix(2,0,0,2,671.5,600.20711)" + id="path290" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m 26.640625,-14.839882 c 4.126953,-1.0625 8.34375,1.425782 9.40625,5.554688" + transform="matrix(2,0,0,2,671.5,600.20711)" + id="path292" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="m 36.046875,-9.285194 c 1.064453,4.128906 -1.423828,8.34375 -5.552734,9.408203" + transform="matrix(2,0,0,2,671.5,600.20711)" + id="path294" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(41.568628%, 24.705882%, 87.058824%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M 30.494141,0.123009 C -10.087891,10.576134 -32.269531,13.630822 -36.046875,9.285118" + transform="matrix(2,0,0,2,671.5,600.20711)" + id="path296" + style="stroke:#6c2ccb;stroke-width:1.25;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1.25, 2.5;stroke-dashoffset:0;stroke-opacity:1" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(63.137257%, 86.666667%, 91.37255%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M -142.75,-88.250038 16.757812,-134.97465" + transform="matrix(2,0,0,2,545.5,469.20711)" + id="path362" + style="stroke:#395dae;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1, 2;stroke-dashoffset:0;stroke-opacity:1;marker-start:url(#Dot);marker-end:url(#marker1543)" /> + <path + fill="none" + stroke-width="1.5" + stroke-linecap="butt" + stroke-linejoin="miter" + stroke="rgb(63.137257%, 86.666667%, 91.37255%)" + stroke-opacity="1" + stroke-miterlimit="10" + d="M -102.42969,28.769493 63,65.499962" + transform="matrix(2,0,0,2,545.5,469.20711)" + id="path364" + style="stroke:#395dae;stroke-width:1;stroke-linecap:round;stroke-linejoin:round;stroke-dasharray:1, 2;stroke-dashoffset:0;stroke-opacity:1;marker-start:url(#Dot);marker-end:url(#marker1543)" /> + </g> +</svg>
+ readme.md view
@@ -0,0 +1,161 @@+# brush-strokes + + +This library provides a toolkit for dealing with Bézier curves and Bézier splines, +with a particular focus on computing the outline of a brush stroke in the spirit +of Knuth's [METAFONT](https://en.wikipedia.org/wiki/Metafont). + +<p align="center"><img src="img/example_stroke.svg" width="400" alt="Brush stroke example"/></p> + +--- + +## Bézier curves + +This library mainly deals with quadratic and cubic Bézier curves, +with `Math.Bezier.Quadratic` and `Math.Bezier.Cubic` providing basic operations +such as differentiation, curvature, subdivision, and closest-point queries. + +### Bézier splines + +`Math.Bezier.Spline` contains functionality for working with Bézier splines +(made up of straight lines and quadratic/cubic Bézier curve). For example: + + - subdividing a spline, + - deleting points from a spline, + - distinguishing between open and closed splines. + +### Arc-length parametrisation + +`Math.Bezier.ArcLength` implements several numerical integration strategies +for computing the arc length of a Bézier curve, and inverts the arc-length +map via Newton–Raphson for arc-length re-parametrisation: + +| Integrator | Description | +|-------------------|-------------| +| `GaussLegendre` | Gauss–Legendre quadrature | +| `ClenshawCurtis` | Clenshaw–Curtis quadrature | +| `TanhSinh` | Tanh–sinh (double-exponential) quadrature | +| `Gravesen` | Gravesen's adaptive polygon/chord method | + +These methods are also extended to Bézier splines. + +## Brush stroking + +`Math.Bezier.Stroke` implements brush stroking, in which: + + - the base path is a Bézier spline, + - the brush shape is a closed Bézier spline whose parameters can vary as the + brush moves along the path; the parameters are themselves Bézier functions + of the base path Bézier parameter. + +Brush shapes are defined in `Calligraphy.Brushes`; predefined shapes include +circular, elliptical, and teardrop brushes. + +The stroking algorithm: + + - Computes outline points by numerically solving the envelope equation + using (forward-mode) automatic differentiation. + (See `Math.Bezier.Stroke.EnvelopeEquation` for details.) + - Detects cusps via real root isolation algorithms implemented using + interval arithmetic. + - Splits up the base path at cuspidal points. + - Fits a cubic Bézier spline to the computed outline, using Hoschek's + least-squares curve fitting algorithm (see `Math.Bezier.Cubic.Fit`). + - Joins up adjacent stroke segments at corners. + +### Cusps + +The most complex (and computationally intensive) step in the above algorithm is +the computation of cusps, using real root isolation with interval arithmetic. + +The cusp equation is formulated in `Math.Bezier.Stroke.EnvelopeEquation` using +implicit differentiation of the envelope equation. (There is also a secondary +cusp equation corresponding to corners of the brush, if the brush has corners.) + +The `cusps` benchmark suite compares the performance of the root isolation +algorithms for cusp finding on a selection of brush strokes that arose in +testing. + +## Root finding + +`Math.Roots` implements a few basic 1D root-finding algorithms: + + - A quadratic equation solver, + - A Newton–Raphson implementation with Armijo line search, + - Laguerre's method, + - A modified Halley M2 method by [Cordero, Ramos, Torregrosa (2020)](https://doi.org/10.1007/s10910-020-01108-3). + +A very basic `n`-dimensional Newton's method is also implemented, +calling out to `eigen` for inversion of the Jacobian. + +### Real root isolation + +`Math.Root.Isolation` provides general-purpose real root isolation algorithms +for general multi-dimensional non-linear systems. + +Available algorithms include bisection, the interval Newton method, +and narrowing methods (e.g. [Kubica (2017)](https://doi.org/10.1016/j.jpdc.2017.03.009) +and [Goldsztejn & Goualard (2010)](https://dl.acm.org/doi/10.1145/1774088.1774519)). + +For interval Newton, two methods are available for inverting the interval +Jacobian: + + - Gauss–Seidel step (with option to use the complete Gauss–Seidel method, + [Montanher, Domes, Schichl, Neumaier (2017)](https://link.springer.com/article/10.1007/s10543-017-0657-x)), + - (2D only) a highly-performant linear-programming approach, kindly contributed by + Walter Figueiredo Mascarenhas; see [`cbits/lp_2d.cpp`](cbits/lp_2d.cpp). + +It is possible to customise the pipeline to choose in which order and when to apply the different algorithms when performing branch-and-bound style search. + +### Interval arithmetic + +`Math.Interval` provides rigorously rounded intervals and *n*-dimensional +interval boxes. + +Three back-ends for computing correctly-rounded interval arithmetic operations +are selectable at build time via Cabal flags: + +| Flag | Description | +|-------------|-------------| +| *(default)* | Use `rounded-hw` | +| `use-fma` | Use Kahan TwoSum and FMA TwoProd | +| `use-simd` | Use 128-bit SIMD registers | + +### Automatic differentiation + +`Math.Algebra.Dual` implements *k*-th order dual numbers for forward-mode +automatic differentiation in *n* variables, used throughout the library for +derivatives, curvature, and critical-point detection. + +--- + +## Other utilities + +### Arc-length test suite + +The `test-arc-length` test executable can output relative error data to a CSV +file, useful for visualising which Béziers each integrator struggles with: + +``` +cabal run test-arc-length -- csv --output errors.csv INTEGRATOR [--grid-size STEP] +``` + +<p align="center"><img src="img/clenshaw_error_landscape.png" alt="Clenshaw–Curtis arc-length integrator error landscape"/></p> + + +### Arc-length benchmark suite + +The `bench-arc-length` executable compares the speed and accuracy of various +arc-length integrators across a selection of quadratic and cubic Béziers. + +On top of command-line output, it also writes the results to `bench_results.csv`. +These results can be visualised using the included helper Python script +`plot_bench.py` (requires `numpy` and `matplotlib`): + +``` +python src/arc-length/bench/plot_bench.py bench_results.csv +``` + +<p align="center"><img src="img/bench_quadratics.png" alt="Integrator time & error histograms – Quadratic Béziers"/></p> + +<p align="center"><img src="img/bench_cubics.png" alt="Benchmark time & error histograms – Cubic Béziers"/></p>
+ src/arc-length/bench/BenchArcLength.hs view
@@ -0,0 +1,390 @@+{-# OPTIONS_GHC -Wno-x-partial #-} + +module Main where + +-- base +import Control.Exception + ( evaluate ) +import Data.Traversable + ( for ) +import GHC.Clock + ( getMonotonicTime ) +import GHC.Exts + ( SPEC(..) ) +import Numeric + ( showEFloat, showFFloat ) + +-- code-page +import System.IO.CodePage + ( withCP65001 ) + +-- base +import Data.List + ( intercalate ) + +-- brush-strokes +import Math.Bezier.ArcLength + ( ArcLengthOptions(..), defaultArcLengthOptions + , Integrator(..) + , Regulariser(..), defaultRegulariser + , ArcLengthParametrisation(..), curveArcLengthParametrisation + ) +import qualified Math.Bezier.Cubic as Cubic +import qualified Math.Bezier.Quadratic as Quadratic +import Math.Bezier.Spline + ( Curve(..), NextPoint(..) ) +import Math.Linear + ( ℝ(..), T ) +import Math.Module + ( distance ) + +-------------------------------------------------------------------------------- +-- Polyline arc-length + +-- | Approximate the arc length of a parametric curve as a polyline with @n@ +-- chords. +polylineArcLength :: Int -> (Double -> ℝ 2) -> Double +polylineArcLength n f = + sum ( zipWith ( distance @( T (ℝ 2) ) ) pts ( tail pts ) ) + where + pts = [ f ( fromIntegral i / fromIntegral n ) | i <- [0 .. n] ] + +-------------------------------------------------------------------------------- +-- Arc-length test case + +-- | A benchmark case for arc-length estimation +data ArcLengthCase = ArcLengthCase + { caseName :: !String + , caseCurve :: !( Double -> ℝ 2 ) + , caseIntegrate :: !( ArcLengthOptions -> Double ) + } + +highAccuracyRef :: ( Double -> ℝ 2 ) -> Double +highAccuracyRef = romberg 50000 1 + +romberg :: Int -> Int -> (Double -> ℝ 2) -> Double +romberg nBase depth f = + let + baseLengths = [ polylineArcLength (nBase * (2 ^ k)) f | k <- [0 .. depth] ] + + -- Tableau generation on a tiny list of Doubles (no performance hit here) + rombergTableau :: [Double] -> [[Double]] + rombergTableau col0 = col0 : go 1 col0 + where + go :: Int -> [Double] -> [[Double]] + go k col = + let factor = 4 ** fromIntegral k + nextCol = zipWith (\prev curr -> (factor * curr - prev) / (factor - 1)) col (tail col) + in nextCol : go (k + 1) nextCol + + -- Grab the fully extrapolated value (the single element in the final column) + in head (rombergTableau baseLengths !! depth) + +-- | Construct a quadratic Bézier test case. +quadCase :: String -> ℝ 2 -> ℝ 2 -> ℝ 2 -> ArcLengthCase +quadCase name p0 p1 p2 = + ArcLengthCase + { caseName = name + , caseCurve = Quadratic.bezier @( T (ℝ 2) ) + $ Quadratic.Bezier { p0, p1, p2 } + , caseIntegrate = \ opts -> + totalArcLength $ + curveArcLengthParametrisation @( T (ℝ 2) ) opts p0 + Bezier2To + { controlPoint = p1 + , curveEnd = NextPoint p2 + , curveData = () + } + } + +-- | Construct a cubic Bézier test case. +cubicCase :: String -> ℝ 2 -> ℝ 2 -> ℝ 2 -> ℝ 2 -> ArcLengthCase +cubicCase name p0 p1 p2 p3 = + ArcLengthCase + { caseName = name + , caseCurve = Cubic.bezier @( T (ℝ 2) ) + $ Cubic.Bezier { p0, p1, p2, p3 } + , caseIntegrate = \ opts -> + totalArcLength $ + curveArcLengthParametrisation @( T (ℝ 2) ) opts p0 + Bezier3To + { controlPoint1 = p1 + , controlPoint2 = p2 + , curveEnd = NextPoint p3 + , curveData = () + } + } + +-------------------------------------------------------------------------------- +-- Integration methods + +-- | The integration methods to compare. +data Method + = Polyline !Int -- ^ polyline with @n@ chords + | Romberg !Int !Int -- ^ Romberg method + | CC !Bool !( Maybe Regulariser ) !Int -- ^ Clenshaw–Curtis: split near crits, regulariser, degree @d@ + | TS !Bool !Int !Double -- ^ tanh–sinh: split near crits, @N@ nodes per side, step size @h@ + | GL !Bool !( Maybe Regulariser ) !Int -- ^ Gauss–Legendre: split near crits, regulariser, @n@ nodes + | GV !Double -- ^ Gravesen adaptive polygon/chord, stopping tolerance + +methodLabel :: Method -> String +methodLabel ( Polyline n ) = + "Polyline, n=" ++ show n +methodLabel ( Romberg n d ) = + "Romberg, n=" ++ show n ++ " d=" ++ show d +methodLabel ( CC cs reg d ) = + "Clenshaw-Curtis, d=" ++ show d ++ pprSplitting cs ++ pprRegulariser reg +methodLabel ( TS cs n h ) = + "TanhSinh, N=" ++ show n ++ " h=" ++ showFFloat ( Just 2 ) h "" ++ pprSplitting cs +methodLabel ( GL cs reg n ) = + "Gauss-Legendre, n=" ++ show n ++ pprSplitting cs ++ pprRegulariser reg +methodLabel ( GV eps ) = + "Gravesen, tol=" ++ showEFloat ( Just 0 ) eps "" + +pprSplitting :: Bool -> String +pprSplitting False = "" +pprSplitting True = " [S]" + +pprRegulariser :: Maybe Regulariser -> String +pprRegulariser Nothing = "" +pprRegulariser ( Just _ ) = " [R:√]" + +-- | Compute the arc length for a given test case using the specified method. +computeArcLength :: ArcLengthCase -> Method -> Double +computeArcLength ( ArcLengthCase { caseCurve = crv, caseIntegrate } ) = \case + Polyline n -> + polylineArcLength n crv + Romberg n d -> + romberg n d crv + CC cs reg d -> + caseIntegrate + ( defaultArcLengthOptions + { arcLengthIntegrator = ClenshawCurtis + { clenshawCurtisDegree = d + , regulariser = reg + } + , splitAtCriticalPoints = cs + } ) + TS cs n h -> + caseIntegrate + ( defaultArcLengthOptions + { arcLengthIntegrator = TanhSinh { tanhSinhNodes = n, tanhSinhStepSize = h } + , splitAtCriticalPoints = cs + } ) + GL cs reg n -> + caseIntegrate + ( defaultArcLengthOptions + { arcLengthIntegrator = GaussLegendre + { gaussLegendreDegree = n + , regulariser = reg + } + , splitAtCriticalPoints = cs + } ) + GV eps -> + caseIntegrate + ( defaultArcLengthOptions + { arcLengthIntegrator = Gravesen { gravesenTol = eps } + } ) + +-------------------------------------------------------------------------------- +-- Timing + +-- | Core timing loop, following the tasty-bench technique. +-- +-- @f@ and @x@ are explicit parameters so that GHC's full-laziness transformation +-- cannot hoist @f x@ outside the loop. +-- The @SPEC@ argument defeats the static-argument transformation (SAT) that +-- would otherwise share the computation across recursive calls. +{-# NOINLINE benchLoop #-} +benchLoop :: SPEC -> (a -> Double) -> a -> Int -> IO Double +benchLoop !_ f x n + | n <= 1 = evaluate (f x) + | otherwise = evaluate (f x) >> benchLoop SPEC f x (n - 1) + +-- | Time @f x@ for @n@ repetitions, preceded by one warm-up call. +-- Returns (result of last run, seconds per run). +timeN :: Int -> (a -> Double) -> a -> IO ( Double, Double ) +timeN n f x = do + _ <- benchLoop SPEC f x 1 -- warm up + before <- getMonotonicTime + result <- benchLoop SPEC f x n + after <- getMonotonicTime + return ( result, ( after - before ) / fromIntegral n ) + +-- | Number of timed repetitions per (case, method) pair. +numReps :: Int +numReps = 500 + +-- | Time a single (case, method) pair. +timeMethod :: ArcLengthCase -> Method -> IO ( Double, Double ) +timeMethod tc method = timeN numReps (computeArcLength tc) method + +-------------------------------------------------------------------------------- +-- Benchmark groups + +-- | A named collection of test cases. +data BenchGroup = BenchGroup + { groupName :: !String + , groupCases :: ![ArcLengthCase] + } + +-- | The methods benchmarked for every (group, case) pair. +allMethods :: [Method] +allMethods = + concat + [ [ Polyline n | n <- [ 40, 200, 1000 ] ] + , [ GV e | e <- [ 1e-1, 1e-3, 1e-5 ] ] + , [ Romberg n d | (n,d) <- [ (40, 1), (20, 2), (20, 3), (100, 2) ] ] + , [ GL cs reg n | n <- [ 8, 20 ], cs <- [ False, True ], reg <- [ Nothing, Just defaultRegulariser ], maybe True ( const cs ) reg ] + , [ CC cs reg n | n <- [ 8, 20 ], cs <- [ False, True ], reg <- [ Nothing, Just defaultRegulariser ], maybe True ( const cs ) reg ] + , [ TS cs n h | (n,h) <- [ (8, 0.4), (32, 0.1) ], cs <- [ False, True ] ] + ] + +benchGroups :: [BenchGroup] +benchGroups = + [ BenchGroup + { groupName = "Quadratic Béziers" + , groupCases = + [ quadCase "gentle arc" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 1 1 ) + , quadCase "sharp arc" ( ℝ2 0 0 ) ( ℝ2 0.5 3.0 ) ( ℝ2 0.5 0 ) + , quadCase "line" ( ℝ2 0 0 ) ( ℝ2 0.5 0.5 ) ( ℝ2 1 1 ) + , quadCase "nearly flat" ( ℝ2 0 0 ) ( ℝ2 4.8 0 ) ( ℝ2 4 -0.01 ) + , quadCase "(P1 = P0)" ( ℝ2 0 0 ) ( ℝ2 0 0 ) ( ℝ2 1 1 ) + , quadCase "degenerate" ( ℝ2 0 0 ) ( ℝ2 1 1 ) ( ℝ2 -1 -1 ) + ] + } + , BenchGroup + { groupName = "Cubic Béziers" + , groupCases = + [ cubicCase "simple arc" ( ℝ2 0 0 ) ( ℝ2 0 1 ) ( ℝ2 1 1 ) ( ℝ2 1 0 ) + , cubicCase "circular arc" ( ℝ2 1 0 ) ( ℝ2 1 κ ) ( ℝ2 κ 1 ) ( ℝ2 0 1 ) + , cubicCase "loose S" ( ℝ2 0 0 ) ( ℝ2 0.33 0 ) ( ℝ2 0.67 1 ) ( ℝ2 1 1 ) + , cubicCase "tight S" ( ℝ2 0 0 ) ( ℝ2 2 0 ) ( ℝ2 -1 1 ) ( ℝ2 1 1 ) + , cubicCase "inflection S" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 0 1 ) ( ℝ2 1 1 ) + , cubicCase "loop" ( ℝ2 0 0 ) ( ℝ2 1 1 ) ( ℝ2 -1 1 ) ( ℝ2 0 0 ) + , cubicCase "gamma shape" ( ℝ2 1 2 ) ( ℝ2 0.16 -1 ) ( ℝ2 3.5 1.4 ) ( ℝ2 0.25 1.46 ) + , cubicCase "simple cusp" ( ℝ2 0 0 ) ( ℝ2 1 1 ) ( ℝ2 0 1 ) ( ℝ2 1 0 ) + , cubicCase "tricky cusp" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 -0.82 -0.18 ) ( ℝ2 1 1 ) + , cubicCase "tricky cusp 2" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 1.22 2.52 ) ( ℝ2 1 1 ) + , cubicCase "near cusp" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 -0.7 -0.6 ) ( ℝ2 1 1 ) + , cubicCase "near cusp 2" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 -0.25 -1.88 ) ( ℝ2 1 1 ) + , cubicCase "line" ( ℝ2 0 0 ) ( ℝ2 0.3 0.3 ) ( ℝ2 0.5 0.5 ) ( ℝ2 1 1 ) + , cubicCase "degenerate (collinear)" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 -1 0 ) ( ℝ2 0 0 ) + , cubicCase "very degenerate" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 1 0 ) ( ℝ2 0 0 ) + , cubicCase "(P0 = P1)" ( ℝ2 0 0 ) ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 1 1 ) + , cubicCase "(P1 = P2)" ( ℝ2 0 0 ) ( ℝ2 1 0 ) ( ℝ2 1 0 ) ( ℝ2 1 1 ) + ] + } + ] + +κ :: Double +κ = 0.5519150244935105707435627227925 + +-------------------------------------------------------------------------------- +-- Benchmark result record + +data BenchResult = BenchResult + { resGroup :: !String + , resCase :: !String + , resMethod :: !String + , resTimeUs :: !Double + , resRelErr :: !Double + } + +-------------------------------------------------------------------------------- +-- Running and reporting + +-- | Print the header row and separator for the result table. +printTableHeader :: IO () +printTableHeader = do + let row = " " + ++ lAlign 42 "Method" + ++ rAlign 11 "Time" + ++ rAlign 13 "Rel. error" + putStrLn row + putStrLn $ " " ++ replicate ( 42 + 11 + 13 ) '─' + +-- | Print one result row. +printResultRow :: String -> Double -> Double -> Double -> IO () +printResultRow label usPerCall _arcLen relErr = + putStrLn $ " " + ++ lAlign 42 label + ++ rAlign 11 ( showFFloat ( Just 2 ) usPerCall "μs" ) + ++ rAlign 13 ( showRelErr relErr ) + +-- | Run and print all benchmarks for a group, returning all results. +runGroup :: BenchGroup -> IO [BenchResult] +runGroup BenchGroup { groupName, groupCases } = do + putStrLn "" + putStrLn $ replicate 80 '=' + putStrLn $ " " ++ groupName + fmap concat $ for groupCases $ \tc -> do + -- Compute the reference value without benchmarking its time. + let refLen = highAccuracyRef ( caseCurve tc ) + putStrLn "" + putStrLn $ " ── " ++ caseName tc +-- putStrLn $ " reference: " ++ showFFloat ( Just 15 ) refLen "" + putStrLn "" + printTableHeader + for allMethods $ \method -> do + ( result, dt ) <- timeMethod tc method + let relErr = abs ( result - refLen ) / max refLen 1e-15 + usPerCall = dt * 1e6 + printResultRow ( methodLabel method ) usPerCall result relErr + return BenchResult + { resGroup = groupName + , resCase = caseName tc + , resMethod = methodLabel method + , resTimeUs = usPerCall + , resRelErr = relErr + } + +main :: IO () +main = withCP65001 $ do + putStrLn "Arc-length quadrature benchmarks" + putStrLn $ unlines + [ "" + , "Legend:" + , " R: regularisation method (change of variables)" + , " S: splitting (subdividing at near-critical points)" + ] + results <- fmap concat $ mapM runGroup benchGroups + putStrLn "" + putStrLn $ replicate 80 '=' + let csvPath = "bench_results.csv" + writeCSV csvPath results + putStrLn $ "CSV results written to: " ++ csvPath + +-- | Write benchmark results to a CSV file. +writeCSV :: FilePath -> [BenchResult] -> IO () +writeCSV path results = writeFile path $ unlines ( header : map row results ) + where + header = "group,case,method,time_us,rel_error" + row BenchResult { resGroup, resCase, resMethod, resTimeUs, resRelErr } = + intercalate "," + [ csvQuote resGroup + , csvQuote resCase + , csvQuote resMethod + , show resTimeUs + , show resRelErr + ] + csvQuote s + | any ( `elem` ",\"\n" ) s = "\"" ++ concatMap esc s ++ "\"" + | otherwise = s + where esc '"' = "\"\"" + esc c = [c] + +-------------------------------------------------------------------------------- +-- Formatting helpers + +-- | Left-align a string in a field of width @n@ (pad on the right). +lAlign :: Int -> String -> String +lAlign n s = take n ( s ++ repeat ' ' ) + +-- | Right-align a string in a field of width @n@ (pad on the left). +rAlign :: Int -> String -> String +rAlign n s = replicate ( max 0 ( n - length s ) ) ' ' ++ s + +-- | Format a relative error in scientific notation, 2 significant figures. +showRelErr :: Double -> String +showRelErr x = showEFloat ( Just 2 ) x ""
+ src/arc-length/bench/plot_bench.py view
@@ -0,0 +1,290 @@+""" +plot_bench.py – Visualise arc-length benchmark results from bench_results.csv + +Usage: + python plot_bench.py [bench_results.csv] + +Produces one PNG per benchmark group, e.g.: + bench_Quadratic_Beziers.png + bench_Cubic_Beziers.png + +Layout (per figure) +------------------- +One panel per test case arranged in a grid. +Within each panel: + • Bars going UP = time (µs, log scale) + • Bars going DOWN = relative error (log scale, flipped) +Each half is independently normalised to the same pixel height, so both +halves always use their full visual space. + +Colours / variants +------------------ + Colour family = method family (Polyline / Romberg / CC / TanhSinh / GL) + Shade within family = base parameters (coarser = lighter) + Edge overlay = [S] split variant (thick coloured border) + Edge + xhatch = [S][R:√] split+regularised variant + Error bars = same colour, reduced alpha, soft same-colour hatching +""" + +import sys, re, csv, math +from pathlib import Path +from collections import defaultdict + +import numpy as np +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import matplotlib.patches as mpatches +import matplotlib.ticker as mticker + + +# ────────────────────────────────────────────────────────────────────────────── +# Colour helpers +# ────────────────────────────────────────────────────────────────────────────── + +FAMILY_HUE = { + "Polyline": "#59a14f", + "Gravesen": "#76b7b2", + "Romberg": "#f28e2b", + "Gauss-Legendre": "#e15759", + "Clenshaw-Curtis": "#4e79a7", + "TanhSinh": "#b07aa1", +} + +def _lighten(h, t): + r,g,b = int(h[1:3],16), int(h[3:5],16), int(h[5:7],16) + return "#{:02x}{:02x}{:02x}".format(int(r+(255-r)*t), int(g+(255-g)*t), int(b+(255-b)*t)) + +def _darken(h, t): + r,g,b = int(h[1:3],16), int(h[3:5],16), int(h[5:7],16) + return "#{:02x}{:02x}{:02x}".format(int(r*(1-t)), int(g*(1-t)), int(b*(1-t))) + + +# ────────────────────────────────────────────────────────────────────────────── +# Method parsing +# ────────────────────────────────────────────────────────────────────────────── + +def _family(label): + if label.startswith("Polyline"): return "Polyline" + if label.startswith("Romberg"): return "Romberg" + if label.startswith("Clenshaw"): return "Clenshaw-Curtis" + if label.startswith("TanhSinh"): return "TanhSinh" + if label.startswith("Gauss"): return "Gauss-Legendre" + if label.startswith("Gravesen"): return "Gravesen" + return "Other" + +def _parse(label): + """Returns (family, base_label, has_split, has_reg).""" + has_split = "[S]" in label + has_reg = "[R:√]" in label + base = label.replace(" [R:√]", "").replace(" [S]", "").strip() + return _family(base), base, has_split, has_reg + +def build_colour_map(all_labels): + """ + Assign fill colour by (family, base_label). + Within a family, earlier (coarser) base params get a lighter shade. + All variants of the same base params share exactly one colour. + """ + fam_bases: dict[str, list] = defaultdict(list) + for lbl in all_labels: + _, base, _, _ = _parse(lbl) + fam = _family(base) + if base not in fam_bases[fam]: + fam_bases[fam].append(base) + + base_col: dict[str, str] = {} + for fam, bases in fam_bases.items(): + hue = FAMILY_HUE.get(fam, "#999999") + n = max(len(bases), 1) + for i, base in enumerate(bases): + fade = 0.40 * (1.0 - i / n) # coarser → lighter + base_col[base] = _lighten(hue, fade) + + return {lbl: base_col[_parse(lbl)[1]] for lbl in all_labels} + + +# ────────────────────────────────────────────────────────────────────────────── +# CSV loading +# ────────────────────────────────────────────────────────────────────────────── + +def load(path): + data = defaultdict(lambda: defaultdict(list)) + with open(path, newline="", encoding="utf-8") as f: + for row in csv.DictReader(f): + data[row["group"]][row["case"]].append({ + "method": row["method"], + "time_us": float(row["time_us"]), + "rel_error": float(row["rel_error"]), + }) + return data + + +# ────────────────────────────────────────────────────────────────────────────── +# Axis tick helpers +# ────────────────────────────────────────────────────────────────────────────── + +def nice_log_ticks(lo, hi, max_n=5): + """Integer log10 values spanning [lo, hi], spaced to give ≤ max_n ticks.""" + lo_i, hi_i = int(math.floor(lo)), int(math.ceil(hi)) + step = max(1, math.ceil((hi_i - lo_i) / max_n)) + start = math.ceil(lo_i / step) * step + return list(range(start, hi_i + 1, step)) + +def fmt_time(lv): + v = 10 ** lv # value in µs + if v < 1: return f"{v*1000:.0f} ns" + if v < 1000: return f"{v:.0f} µs" + return f"{v/1000:.1f} ms" + +def fmt_err(lv): + return f"1e{int(lv)}" + + +# ────────────────────────────────────────────────────────────────────────────── +# Main plot routine +# ────────────────────────────────────────────────────────────────────────────── + +HALF = 1.0 # each axis half (time / error) is normalised to [0, HALF] + +def plot_group(group_name, cases, out_dir): + case_names = list(cases.keys()) + all_methods = [r["method"] for r in next(iter(cases.values()))] + n_methods = len(all_methods) + n_cases = len(case_names) + + col_map = build_colour_map(all_methods) + parsed = {m: _parse(m) for m in all_methods} + + ncols = min(3, n_cases) + nrows = math.ceil(n_cases / ncols) + fig, axes = plt.subplots( + nrows, ncols, + figsize=(max(ncols * 5.5, 8), nrows * 4.8), + constrained_layout=True, + ) + axes_flat = np.array(axes).reshape(-1) if n_cases > 1 else [axes] + + x = np.arange(n_methods) + bar_w = 0.72 + + # ── Compute shared log-range across all test cases ──────────────────── + all_lt, all_le = [], [] + for rows in cases.values(): + for r in rows: + if r["time_us"] > 0: all_lt.append(math.log10(r["time_us"])) + if r["rel_error"] > 0: all_le.append(math.log10(r["rel_error"])) + + lt_lo, lt_hi = min(all_lt) - 0.3, max(all_lt) + 0.3 + le_lo, le_hi = min(all_le) - 0.3, max(all_le) + 0.3 + t_span = lt_hi - lt_lo + e_span = le_hi - le_lo + + # ── Build explicit tick positions on the normalised axis ────────────── + # + # Time ticks: normalised position = HALF * (log_t - lt_lo) / t_span → [0, HALF] + # Error ticks: normalised position = -HALF * (le_hi - log_e) / e_span → [-HALF, 0] + # + # These are the values we pass to ax.set_yticks() — no implicit conversion + # through the axis scale at all, so no rounding/extrapolation bugs. + # Filter to strictly within the mapped range so no tick crosses the + # zero line onto the wrong half. + t_ticks = [t for t in nice_log_ticks(lt_lo, lt_hi) if lt_lo <= t <= lt_hi] + e_ticks = [e for e in nice_log_ticks(le_lo, le_hi) if le_lo <= e <= le_hi] + tick_y = ([ HALF * (t - lt_lo) / t_span for t in t_ticks] + + [-HALF * (le_hi - e) / e_span for e in e_ticks]) + tick_lbl = [fmt_time(t) for t in t_ticks] + [fmt_err(e) for e in e_ticks] + + # ── Draw panels ─────────────────────────────────────────────────────── + for ax_idx, case_name in enumerate(case_names): + ax = axes_flat[ax_idx] + rows = cases[case_name] + + lts = [math.log10(max(r["time_us"], 1e-12)) for r in rows] + les = [math.log10(max(r["rel_error"], 1e-20)) for r in rows] + + bu = np.array([HALF * (t - lt_lo) / t_span for t in lts]) # height above 0 + bd = np.array([HALF * (le_hi - e) / e_span for e in les]) # depth below 0 + + for i, m in enumerate(all_methods): + col = col_map[m] + _, _, has_split, has_reg = parsed[m] + if has_reg: + hatch, ec = "///\\\\\\", _darken(col, 0.3) + elif has_split: + hatch, ec = "///", _darken(col, 0.3) + else: + hatch, ec = None, "none" + + # ── Time bar: hatch encodes variant, no border on any bar ──── + + ax.bar(x[i], bu[i], width=bar_w, color=col, + edgecolor=ec, linewidth=0, hatch=hatch, zorder=2) + + # ── Error bar: faded colour only, no extra styling ──────────── + ax.bar(x[i], -bd[i], width=bar_w, color=col, alpha=0.45, + edgecolor=ec, linewidth=0, hatch=hatch, zorder=2) + + ax.axhline(0, color="#333", linewidth=0.9, zorder=4) + ax.set_xlim(-0.6, n_methods - 0.4) + ax.set_ylim(-HALF * 1.12, HALF * 1.12) + + ax.set_yticks(tick_y) + ax.set_yticklabels(tick_lbl, fontsize=6.5) + ax.yaxis.set_minor_locator(mticker.NullLocator()) + + ax.set_title(case_name, fontsize=9, pad=3) + ax.tick_params(axis="x", bottom=False, labelbottom=False) + ax.set_facecolor("#f8f8f8") + ax.grid(axis="y", color="white", linewidth=0.7, zorder=1) + ax.text(0.01, 0.98, "time ↑", transform=ax.transAxes, + fontsize=7, va="top", color="#555") + ax.text(0.01, 0.02, "accuracy ↓", transform=ax.transAxes, + fontsize=7, va="bottom", color="#555") + + for ax in axes_flat[n_cases:]: + ax.set_visible(False) + + # ── Legend ──────────────────────────────────────────────────────────── + handles = [] + seen_bases = [] + for m in all_methods: + _, base, _, _ = parsed[m] + if base not in seen_bases: + seen_bases.append(base) + handles.append(mpatches.Patch(color=col_map[m], label=base)) + + grey = "#b0b0b0" + grey_dk = _darken(grey, 0.3) + handles += [ + mpatches.Patch(facecolor=grey, edgecolor="none", linewidth=0, + label="basic"), + mpatches.Patch(facecolor=grey, edgecolor=grey_dk, linewidth=0, + hatch="///", label="split at cusps"), + mpatches.Patch(facecolor=grey, edgecolor=grey_dk, linewidth=0, + hatch="\\\\\\", label="regularisation"), + ] + + fig.legend(handles=handles, loc="lower center", + ncol=min(len(handles), 5), fontsize=7.5, + frameon=False, bbox_to_anchor=(0.5, -0.04)) + + safe = re.sub(r"[^\w]+", "_", group_name).strip("_") + out = out_dir / f"bench_{safe}.png" + fig.suptitle(group_name, fontsize=13, y=1.01) + fig.savefig(out, dpi=150, bbox_inches="tight") + plt.close(fig) + print(f"Saved: {out}") + + +# ────────────────────────────────────────────────────────────────────────────── + +def main(): + csv_path = sys.argv[1] if len(sys.argv) > 1 else "bench_results.csv" + out_dir = Path(csv_path).parent + data = load(csv_path) + for group_name, cases in data.items(): + plot_group(group_name, cases, out_dir) + +if __name__ == "__main__": + main()
+ src/arc-length/test/TestArcLength.hs view
@@ -0,0 +1,429 @@+{-# OPTIONS_GHC -Wno-x-partial #-}++module Main where++-- base+import Control.Applicative+ ( (<|>) )+import Control.Monad+ ( guard )+import Data.Char+ ( toLower )+import Data.List+ ( intercalate, maximumBy )+import qualified Data.List.NonEmpty as NE+import Data.Maybe+ ( mapMaybe )+import Data.Ord+ ( comparing )+import GHC.Generics+ ( Generic )+import System.Environment+ ( withArgs )++-- deepseq+import Control.DeepSeq+ ( NFData )++-- parallel+import Control.Parallel.Strategies+ ( parListChunk, rdeepseq, withStrategy )++-- code-page+import System.IO.CodePage+ ( withCP65001 )++-- optparse-applicative+import qualified Options.Applicative as OptParse++-- falsify+import Test.Tasty.Falsify+import Test.Falsify.Predicate+ ( (.$) )+import qualified Test.Falsify.Generator as Gen+import qualified Test.Falsify.Predicate as Falsify.Prop+import qualified Test.Falsify.Range as Range+import qualified Test.Tasty.Falsify as Falsify++-- tasty+import qualified Test.Tasty as Tasty++-- brush-strokes+import Math.Bezier.ArcLength+ ( ArcLengthOptions(..), defaultArcLengthOptions+ , Integrator(..)+ , defaultRegulariser+ , ArcLengthParametrisation(..), curveArcLengthParametrisation+ )+import qualified Math.Bezier.Cubic as Cubic ( Bezier(..), bezier )+import qualified Math.Bezier.Quadratic as Quadratic ( Bezier(..), bezier )+import Math.Bezier.Spline+ ( Curve(..), NextPoint(..) )+import Math.Linear+ ( ℝ(..), T )+import Math.Module+ ( distance )++--------------------------------------------------------------------------------+-- Generator+--------------------------------------------------------------------------------++-- | Generate a 'Double' uniformly in @[lo, hi]@ on a grid of 10⁶ steps.+genDouble :: Double -> Double -> Gen.Gen Double+genDouble lo hi =+ fmap (\n -> lo + (hi - lo) * fromIntegral n / 1e6) $+ Gen.inRange $ Range.between (0, 1000000 :: Word)++--------------------------------------------------------------------------------+-- Polyline reference+--------------------------------------------------------------------------------++-- | Approximate the arc length of a parametric curve by a polyline with @n@+-- segments.+polylineArcLength :: Int -> (Double -> ℝ 2) -> Double+polylineArcLength n f =+ sum ( zipWith ( distance @( T (ℝ 2) ) ) pts ( tail pts ) )+ where+ pts = [ f ( fromIntegral i / fromIntegral n )+ | i <- [0 .. n]+ ]++-- | Reference arc-length implementation, using Romberg's method.+referenceArcLength :: ( Double -> ℝ 2 ) -> Double+referenceArcLength f = ( 4 * l2 - l1 ) / 3+ where+ n = 50000 -- Number of samples used for reference implementation.+ l1 = polylineArcLength n f+ l2 = polylineArcLength ( 2 * n ) f++--------------------------------------------------------------------------------+-- Error computation+--------------------------------------------------------------------------------++data Result+ = Result+ { referenceResult :: !Double+ , methodResult :: !Double+ , methodRelativeError :: !Double+ }+ deriving stock ( Show, Generic )+ deriving anyclass NFData++-- | Compute (impl, ref, relErr) for the quadratic arc-length test.+--+-- Endpoints fixed at p0=(0,0) and p2=(1,0); control point p1=(cx,cy).+quadResult :: ArcLengthOptions -> ℝ 2 -> Result+quadResult opts p1 =+ Result+ { referenceResult = ref+ , methodResult = impl+ , methodRelativeError = relErr+ }+ where+ p0 = ℝ2 0 0+ p2 = ℝ2 1 0+ bez = Quadratic.Bezier { p0, p1, p2 }+ curve = Bezier2To+ { controlPoint = p1+ , curveEnd = NextPoint p2+ , curveData = ()+ }+ impl = totalArcLength+ $ curveArcLengthParametrisation @( T ( ℝ 2 ) ) opts p0 curve+ ref = referenceArcLength $ Quadratic.bezier @( T ( ℝ 2 ) ) bez+ relErr = abs (impl - ref) / max ref 1e-15++-- | Compute (impl, ref, relErr) for the cubic arc-length test.+--+-- Fixed points p0=(0,0), p1=(1,0), p3=(1,1); free control point p2=(cx,cy).+cubicResult :: ArcLengthOptions -> ℝ 2 -> Result+cubicResult opts p2 =+ Result+ { referenceResult = ref+ , methodResult = impl+ , methodRelativeError = relErr+ }+ where+ p0 = ℝ2 0 0+ p1 = ℝ2 1 0+ p3 = ℝ2 1 1+ bez = Cubic.Bezier { p0, p1, p2, p3 }+ curve = Bezier3To+ { controlPoint1 = p1+ , controlPoint2 = p2+ , curveEnd = NextPoint p3+ , curveData = ()+ }+ impl = totalArcLength+ $ curveArcLengthParametrisation @( T ( ℝ 2 ) ) opts p0 curve+ ref = referenceArcLength $ Cubic.bezier @( T ( ℝ 2 ) ) bez+ relErr = abs (impl - ref) / max ref 1e-15++--------------------------------------------------------------------------------+-- Properties+--------------------------------------------------------------------------------++-- | Relative-error tolerance for arc-length tests.+arcLengthTol :: Double+arcLengthTol = 1e-5++testOptions :: Falsify.TestOptions+testOptions = Falsify.TestOptions+ { Falsify.expectFailure = Falsify.DontExpectFailure+ , Falsify.overrideVerbose = Nothing+ , Falsify.overrideNumTests = Just 400+ , Falsify.overrideMaxShrinks = Just 0 -- shrinking isn't really useful here+ , Falsify.overrideMaxRatio = Nothing+ }++-- | Arc length of a quadratic Bézier with endpoints (0,0), (1,0) and a random+-- control point (cx, cy) in [-3, 3]².+prop_quadratic :: ArcLengthOptions -> Property ()+prop_quadratic opts = do+ cx <- Falsify.genWith (\v -> Just $ "cx = " ++ show v) $ genDouble -3 3+ cy <- Falsify.genWith (\v -> Just $ "cy = " ++ show v) $ genDouble -3 3++ let+ c = ℝ2 cx cy+ Result+ { referenceResult = ref+ , methodResult = impl+ , methodRelativeError = relErr+ } = quadResult opts c++ Falsify.info $+ "{ c = " ++ show c +++ ", impl = " ++ show impl +++ ", ref = " ++ show ref +++ ", relErr = " ++ show relErr ++ " }"++ Falsify.assert $+ Falsify.Prop.relatedBy ("< tolerance", (<))+ .$ ("relErr", relErr)+ .$ ("tolerance", arcLengthTol)++-- | Arc length of a cubic Bézier with fixed points p0=(0,0), p1=(1,0), p3=(1,1)+-- and a random free control point p2=(cx, cy) in [-3, 3]².+prop_cubic :: ArcLengthOptions -> Property ()+prop_cubic opts = do+ cx <- Falsify.genWith (\v -> Just $ "cx = " ++ show v) $ genDouble -3 3+ cy <- Falsify.genWith (\v -> Just $ "cy = " ++ show v) $ genDouble -3 3++ let+ c = ℝ2 cx cy+ Result+ { referenceResult = ref+ , methodResult = impl+ , methodRelativeError = relErr+ } = cubicResult opts c++ Falsify.info $+ "{ c = " ++ show c +++ ", impl = " ++ show impl +++ ", ref = " ++ show ref +++ ", relErr = " ++ show relErr ++ " }"++ Falsify.assert $+ Falsify.Prop.relatedBy ("< tolerance", (<))+ .$ ("relErr", relErr)+ .$ ("tolerance", arcLengthTol)++testGroup :: ArcLengthOptions -> Tasty.TestTree+testGroup opts =+ Tasty.testGroup ( integratorName opts )+ [ testPropertyWith testOptions "Quadratic Bezier" $ prop_quadratic opts+ , testPropertyWith testOptions "Cubic Bezier" $ prop_cubic opts+ ]++allTests :: [ Tasty.TestTree ]+allTests = map ( testGroup . snd ) integrators++--------------------------------------------------------------------------------+-- CSV output+--------------------------------------------------------------------------------++writeArcLengthData :: FilePath -> ArcLengthOptions -> Double -> IO ()+writeArcLengthData fp opts gridSize = do+ putStrLn $+ unlines+ [ "Computing arc-length data for integrator" ++ integratorName opts ++ "."+ , "Writing to: " ++ fp+ ]+ writeFile fp $ displayDataPoints dataPoints+ putStrLn $+ unlines+ [ " Worst error, at " ++ show wp ++ ": " ++ show werr+ , " Mean error: " ++ show avg_err+ ]++ where+ chunkSize = 2500 -- one row per spark++ dataPoints :: [ ( ℝ 2, Result ) ]+ dataPoints =+ withStrategy ( parListChunk chunkSize rdeepseq )+ [ ( p, res )+ | x <- [ -2, -2 + gridSize .. 3 ]+ , y <- [ -2, -2 + gridSize .. 3 ]+ , let p = ℝ2 x y+ res = cubicResult opts p+ ]++ getErr :: ( ℝ 2, Result ) -> Double+ getErr ( _xy, res ) = methodRelativeError res++ ( wp, werr ) = maximumBy ( comparing getErr ) dataPoints++ log_avgerr = sum (map (logBase 10 . getErr) dataPoints) / fromIntegral ( length dataPoints )+ avg_err = 10 ** log_avgerr++displayDataPoints+ :: [ ( ℝ 2, Result ) ]+ -> String+displayDataPoints pts =+ intercalate "\n" ( map displayDataPoint pts ) ++ "\n"++displayDataPoint+ :: ( ℝ 2, Result )+ -> String+displayDataPoint ( ℝ2 x y, res ) =+ intercalate ", " $ map displayDouble [ x, y, methodRelativeError res ]++displayDouble :: Double -> String+displayDouble d = show d++--------------------------------------------------------------------------------+-- Command-line interface+--------------------------------------------------------------------------------++data Command+ = RunTests+ | WriteCSV+ { csvOutputPath :: !FilePath+ , csvIntegrator :: !ArcLengthOptions+ , csvGridSize :: !Double+ }++commandParser :: OptParse.Parser Command+commandParser =+ OptParse.subparser+ ( OptParse.command "test"+ ( OptParse.info ( pure RunTests )+ ( OptParse.progDesc "(default) Run arc-length integrator testsuite" ) )+ <> OptParse.command "csv"+ ( OptParse.info ( csvParser OptParse.<**> OptParse.helper )+ ( OptParse.progDesc+ "Write arc-length error data to a CSV file" ) )+ )+ -- No command: run the tests.+ <|> pure RunTests+ where+ csvParser :: OptParse.Parser Command+ csvParser =+ WriteCSV+ <$> OptParse.option OptParse.str+ ( OptParse.long "output"+ <> OptParse.short 'o'+ <> OptParse.metavar "FILE"+ <> OptParse.help "Path of CSV output file" )+ <*> OptParse.argument ( OptParse.eitherReader parseIntegrator )+ ( OptParse.metavar "INTEGRATOR"+ <> OptParse.help ( "Integration method to use: "+ ++ intercalate ", " ( map ( NE.head . fst ) integrators ) ) )+ <*> OptParse.option OptParse.auto+ ( OptParse.long "grid-size"+ <> OptParse.short 'g'+ <> OptParse.metavar "STEP"+ <> OptParse.value 5e-2+ <> OptParse.showDefault+ <> OptParse.help "Grid step size" )++optParseOpts :: OptParse.ParserInfo Command+optParseOpts = OptParse.info ( commandParser OptParse.<**> OptParse.helper )+ ( OptParse.fullDesc+ <> OptParse.header "test-arc-length – test-suite for arc-length integrators"+ )++integrators :: [ ( NE.NonEmpty String, ArcLengthOptions ) ]+integrators =+ map ( \ i -> ( integratorNames i, i ) )+ [ gravesenOpts+ , gaussLegendreOpts+ , clenshawCurtisOpts+ , tanhSinhOpts+ ]++ where+ gravesenOpts =+ defaultArcLengthOptions+ { arcLengthIntegrator = Gravesen { gravesenTol = 1e-5 }+ }+ gaussLegendreOpts =+ defaultArcLengthOptions+ { arcLengthIntegrator = GaussLegendre+ { gaussLegendreDegree = 20+ , regulariser = Just defaultRegulariser+ }+ , splitAtCriticalPoints = True+ }+ clenshawCurtisOpts =+ defaultArcLengthOptions+ { arcLengthIntegrator = ClenshawCurtis+ { clenshawCurtisDegree = 20+ , regulariser = Just defaultRegulariser+ }+ , splitAtCriticalPoints = True+ }+ tanhSinhOpts =+ defaultArcLengthOptions+ { arcLengthIntegrator = TanhSinh+ { tanhSinhNodes = 32+ , tanhSinhStepSize = 0.1+ }+ , splitAtCriticalPoints = True+ }++integratorName :: ArcLengthOptions -> String+integratorName = NE.head . integratorNames++integratorNames :: ArcLengthOptions -> NE.NonEmpty String+integratorNames opts =+ case arcLengthIntegrator opts of+ Gravesen {} -> "Gravesen" NE.:| [ "AdaptiveGravesen" ]+ GaussLegendre {} -> "Gauss–Legendre" NE.:| [ "Gauss-Legendre", "GaussLegendre", "Gauss", "Legendre" ]+ ClenshawCurtis {} -> "Clenshaw–Curtis" NE.:| [ "Clenshaw-Curtis", "ClenshawCurtis", "Clenshaw", "Curtis" ]+ TanhSinh {} -> "TanhSinh" NE.:| [ "Tanh–Sinh", "Tanh-Sinh", "Tanh", "Sinh" ]++parseIntegrator :: String -> Either String ArcLengthOptions+parseIntegrator intStr =+ case mapMaybe parseOne integrators of+ [ opts ] -> Right opts+ [ ] ->+ Left $ "Unknown integrator " ++ show intStr+ ++ ".\n Available integrators: "+ ++ intercalate ", " integratorNms ++ "."+ _ ->+ Left $ "Ambiguous integrator " ++ show intStr+ ++ ".\n Available integrators: "+ ++ intercalate ", " integratorNms ++ "."+ where+ integratorNms = map ( NE.head . fst ) integrators+ parseOne :: ( NE.NonEmpty String, ArcLengthOptions ) -> Maybe ArcLengthOptions+ parseOne ( nms, integrator ) = do+ guard $ any ( ( map toLower intStr == ) . map toLower ) nms+ return integrator++--------------------------------------------------------------------------------+-- Entry point+--------------------------------------------------------------------------------++main :: IO ()+main = withCP65001 $ do+ cmd <- OptParse.execParser optParseOpts+ case cmd of+ RunTests ->+ withArgs [] $+ Tasty.defaultMain $+ Tasty.testGroup "Arc-length integrators" allTests+ WriteCSV { csvOutputPath, csvIntegrator, csvGridSize } ->+ writeArcLengthData csvOutputPath csvIntegrator csvGridSize
+ src/cusps/bench/Bench/Cases.hs view
@@ -0,0 +1,136 @@+module Bench.Cases where + +-- brush-strokes +import Calligraphy.Brushes +import Math.Bezier.Spline +import Math.Bezier.Stroke +import Math.Interval +import Math.Linear +import Math.Module +import Math.Root.Isolation + +-- brush-strokes bench:cusps +import Bench.Types + +-------------------------------------------------------------------------------- + +defaultStartBoxes :: [ Int ] -> [ ( Int, [ Box 2 ] ) ] +defaultStartBoxes is = + [ ( i, [ 𝕀ℝ2 ( 𝕀 0 1 ) ( 𝕀 0 1 ) ] ) | i <- is ] + +-------------------------------------------------------------------------------- + +ellipseTestCase :: RootIsolationOptions N 3 -> ( Double, Double ) -> Double -> [ ( Int, [ Box 2 ] ) ] -> TestCase +ellipseTestCase opts k0k1 rot startBoxes = + TestCase + { testDescription = "" + , testBrushStroke = ellipseBrushStroke k0k1 rot + , testCuspOptions = opts + , testStartBoxes = startBoxes + } + +ellipseBrushStroke :: ( Double, Double ) -> Double -> BrushStroke +ellipseBrushStroke ( k0, k1 ) rot = + BrushStroke + { brush = ellipseBrush + , stroke = ( p0, LineTo ( NextPoint p1 ) () ) } + where + mkPt x y w h phi = + Point + { pointCoords = ℝ2 x y + , pointParams = Params $ ℝ3 w h phi + } + l :: Double -> Double -> Double -> Double + l k = lerp @( T Double ) k + p k = mkPt ( l k 0 100 ) 0 ( l k 10 15 ) ( l k 25 40 ) ( l k 0 rot ) + p0 = p k0 + p1 = p k1 + +trickyCusp2TestCase :: TestCase +trickyCusp2TestCase = + TestCase + { testDescription = "" + , testBrushStroke = trickyCusp2BrushStroke + , testCuspOptions = defaultRootIsolationOptions + , testStartBoxes = defaultStartBoxes [ 0 .. 3 ] + } + +trickyCusp2BrushStroke :: BrushStroke +trickyCusp2BrushStroke = + BrushStroke + { brush = circleBrush + , stroke = ( p0, Bezier3To p1 p2 ( NextPoint p3 ) () ) + } + where + mkPt x y = + Point + { pointCoords = ℝ2 x y + , pointParams = Params $ ℝ1 5.0 + } + p0 = mkPt 5e+1 -5e+1 + p1 = mkPt -7.72994362904069e+1 -3.124468786098509e+1 + p2 = mkPt -5.1505430313958364e+1 -3.9826386521527986e+1 + p3 = mkPt -5e+1 -5e+1 + +rotationTestCase :: RootIsolationOptions N 3 -> TestCase +rotationTestCase opts = + TestCase + { testDescription = "" + , testBrushStroke = rotationCuspBrushStroke + , testCuspOptions = opts + , testStartBoxes = defaultStartBoxes [0 .. 3 ] + } +{- +Expecting exactly two cusps. I computed one of them with Mathematica: + + - t = 0.551437802322262119984220362508195622632 + - s = 0.4493675650062704654881343190165751812803 + - i = 2 +-} + +rotationCuspBrushStroke :: BrushStroke +rotationCuspBrushStroke = + BrushStroke + { brush = ellipseBrush + , stroke = ( p0, Bezier2To p1 ( NextPoint p2 ) () ) + } + where + mkPt x y a b phi = + Point + { pointCoords = ℝ2 x y + , pointParams = Params $ ℝ3 a b phi + } + p0 = mkPt -100 -200 40 20 ( -pi / 4 ) + p1 = mkPt 0 100 40 20 0 + p2 = mkPt 100 -200 40 20 ( pi / 4 ) + +missingCusp2TestCase :: TestCase +missingCusp2TestCase = + TestCase + { testDescription = "" + , testBrushStroke = missingCusp2BrushStroke + , testCuspOptions = defaultRootIsolationOptions + , testStartBoxes = defaultStartBoxes [ 0 ] + } + +{- +Two cusps expected: + - cusp 1: t = 0.32055820458595224, s = 0.3575339430663827 + - cusp 2: t = 0.527772951374112, s = 0.6271234239242184 +-} +missingCusp2BrushStroke :: BrushStroke +missingCusp2BrushStroke = + BrushStroke + { brush = tearDropBrush + , stroke = ( p0, Bezier3To p1 p2 ( NextPoint p3 ) () ) + } + where + mkPt x y = + Point + { pointCoords = ℝ2 x y + , pointParams = Params $ ℝ3 18.90303454818744 7.655839954858216 2.356194490192345 + } + p0 = mkPt -174.8456710674493 58.621320343559674 + p1 = mkPt -181.07339185171972 91.21769601156711 + p2 = mkPt -187.7254115637175 92.08897689831758 + p3 = mkPt -233.5 106.5
+ src/cusps/bench/Bench/Types.hs view
@@ -0,0 +1,160 @@+{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Bench.Types + ( BrushStroke(..) + , TestCase(..) + , Point(..) + , Params(..) + , brushStrokeFunctions + ) where + +-- base +import Data.Coerce + ( coerce ) +import Data.Proxy + ( Proxy(..) ) +import Data.Type.Equality + ( (:~:)(Refl) ) +import GHC.Generics + ( Generic ) +import GHC.TypeNats + ( sameNat ) + +-- containers +import Data.Sequence + ( Seq ) + +-- brush-strokes +import Calligraphy.Brushes +import Math.Algebra.Dual +import Math.Bezier.Spline +import Math.Bezier.Stroke +import Math.Bezier.Stroke.EnvelopeEquation +import Math.Differentiable +import Math.Interval +import Math.Linear +import Math.Module +import Math.Ring + ( Transcendental ) +import Math.Root.Isolation + +-------------------------------------------------------------------------------- + +data BrushStroke = + forall nbParams. ParamsCt nbParams => + BrushStroke + { brush :: !( Brush nbParams ) + , stroke :: !( Point nbParams, Curve Open () ( Point nbParams ) ) + } + +data TestCase = + TestCase + { testDescription :: String + , testBrushStroke :: BrushStroke + , testCuspOptions :: RootIsolationOptions N 3 + , testStartBoxes :: [ ( Int, [ Box 2 ] ) ] + } + +-------------------------------------------------------------------------------- + +type ParamsCt nbParams + = ( Show ( ℝ nbParams ) + , HasChainRule Double 2 ( ℝ nbParams ) + , HasChainRule 𝕀 3 ( 𝕀ℝ nbParams ) + , Applicative ( D 2 nbParams ) + , Applicative ( D 3 nbParams ) + , Traversable ( D 2 nbParams ) + , Traversable ( D 3 nbParams ) + , Representable Double ( ℝ nbParams ) + , Representable 𝕀 ( 𝕀ℝ nbParams ) + , Module Double ( T ( ℝ nbParams ) ) + , Module 𝕀 ( T ( 𝕀ℝ nbParams ) ) + , Module ( D 2 nbParams Double ) ( D 2 nbParams ( ℝ 2 ) ) + , Module ( D 3 nbParams 𝕀 ) ( D 3 nbParams ( 𝕀ℝ 2 ) ) + , Transcendental ( D 2 nbParams Double ) + , Transcendental ( D 3 nbParams 𝕀 ) + ) + +newtype Params nbParams = Params { getParams :: ( ℝ nbParams ) } +deriving newtype instance Show ( ℝ nbParams ) => Show ( Params nbParams ) + +data Point nbParams = + Point + { pointCoords :: !( ℝ 2 ) + , pointParams :: !( Params nbParams ) } + deriving stock Generic +deriving stock instance Show ( ℝ nbParams ) => Show ( Point nbParams ) + +getStrokeFunctions + :: forall nbParams + . ParamsCt nbParams + => Brush nbParams + -- ^ brush shape + -> Point nbParams + -- ^ start point + -> Curve Open () ( Point nbParams ) + -- ^ curve points + -> ( ℝ 1 -> ( Seq ( CornerStrokeDatum 2 NonIV ) + , Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) ) + , 𝕀ℝ 1 -> ( Seq ( CornerStrokeDatum 3 IsIV ) + , Seq ( 𝕀ℝ 1 -> StrokeDatum 3 IsIV ) + ) + ) +getStrokeFunctions + ( Brush { brushBaseShape + , brushBaseShapeI + , brushCorners + , brushCornersI + , mbRotation + } ) + sp0 crv = + let + usedParams :: C 2 ( ℝ 1 ) ( ℝ nbParams ) + path :: C 2 ( ℝ 1 ) ( ℝ 2 ) + ( path, usedParams ) = + pathAndUsedParams @2 @NonIV @nbParams coerce id id ( getParams . pointParams ) + sp0 crv + usedParamsI :: C 3 ( 𝕀ℝ 1 ) ( 𝕀ℝ nbParams ) + pathI :: C 3 ( 𝕀ℝ 1 ) ( 𝕀ℝ 2 ) + ( pathI, usedParamsI ) = + pathAndUsedParams @3 @IsIV @nbParams coerce point point ( getParams . pointParams ) + sp0 crv + in ( brushStrokeData @2 @nbParams SNonIV + path usedParams + brushBaseShape + brushCorners + mbRotation + , brushStrokeData @3 @nbParams SIsIV + pathI usedParamsI + brushBaseShapeI + brushCornersI + ( fmap ( \ rot -> un𝕀ℝ1 . nonDecreasing ( ℝ1 . rot ) ) mbRotation ) + ) +{-# INLINEABLE getStrokeFunctions #-} +{-# SPECIALISE getStrokeFunctions @0 #-} +{-# SPECIALISE getStrokeFunctions @1 #-} +{-# SPECIALISE getStrokeFunctions @2 #-} +{-# SPECIALISE getStrokeFunctions @3 #-} +{-# SPECIALISE getStrokeFunctions @4 #-} + +brushStrokeFunctions + :: BrushStroke + -> ( ℝ 1 -> ( Seq ( CornerStrokeDatum 2 NonIV ) + , Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) ) + , 𝕀ℝ 1 -> ( Seq ( CornerStrokeDatum 3 IsIV ) + , Seq ( 𝕀ℝ 1 -> StrokeDatum 3 IsIV ) + ) + ) +brushStrokeFunctions ( BrushStroke { stroke = ( sp0, crv ), brush = brush :: Brush nbParams } ) + -- Trick for triggering specialisation... TODO improve this. + | Just Refl <- sameNat @nbParams @1 Proxy Proxy + = getStrokeFunctions @1 brush sp0 crv + | Just Refl <- sameNat @nbParams @2 Proxy Proxy + = getStrokeFunctions @2 brush sp0 crv + | Just Refl <- sameNat @nbParams @3 Proxy Proxy + = getStrokeFunctions @3 brush sp0 crv + | Just Refl <- sameNat @nbParams @4 Proxy Proxy + = getStrokeFunctions @4 brush sp0 crv + | otherwise + = getStrokeFunctions @nbParams brush sp0 crv
+ src/cusps/bench/Main.hs view
@@ -0,0 +1,327 @@+{-# LANGUAGE ScopedTypeVariables #-} + +module Main where + +-- base +import Control.Arrow + ( second ) +import Control.Monad + ( when ) +import Data.Foldable + ( for_ ) +import qualified Data.List.NonEmpty as NE + ( NonEmpty(..) + , fromList, head, length, sort + ) +import Data.Semigroup + ( Arg(..), Sum(..), Product(..) ) +import Data.Traversable + ( for ) +import GHC.Clock + ( getMonotonicTime ) +import Numeric + ( showFFloat ) + +-- code-page +import System.IO.CodePage + ( withCP65001 ) + +-- containers +import qualified Data.IntMap.Strict as IntMap + ( fromList, toList ) +import Data.Tree + ( foldTree ) + +-- deepseq +import Control.DeepSeq + ( rnf ) + +-- brush-strokes +import Math.Bezier.Stroke +import Math.Interval +import Math.Linear +import Math.Root.Isolation +import Math.Root.Isolation.Core +import Math.Root.Isolation.Newton +import Math.Root.Isolation.Newton.GaussSeidel + +-- brush-strokes bench:cusps +import Bench.Cases +import Bench.Types + +-------------------------------------------------------------------------------- + +pattern ROUND_UP :: Word +pattern ROUND_UP = 0x4000 + +foreign import ccall "set_sse_rounding_mode" + setSSERoundingMode :: Word -> IO () + +main :: IO () +main = withCP65001 $ do + setSSERoundingMode ROUND_UP + putStrLn "Running cusp-finding benchmarks." + for_ benchGroups $ \ ( groupName, benchGroupCases ) -> do + putStrLn $ unlines + [ replicate 40 '=' + , "Benchmark group '" ++ groupName ++ "':" ] + testsWithTime <- for benchGroupCases $ \ tst -> do + dt <- benchTestCase groupName tst + return $ Arg dt tst + let Arg bestTime bestTest = NE.head $ NE.sort testsWithTime + when ( NE.length benchGroupCases >= 1 ) $ + putStrLn $ unlines $ + [ "Best time in '" ++ groupName ++ "' group: " ++ show bestTime ++ "s" ] + ++ [ " (" ++ descr ++ ")" + | let descr = testDescription bestTest + , not ( null descr ) + ] + +-- TODO: run the benchmark multiple times to reduce noise. +benchTestCase :: String -> TestCase -> IO Double +benchTestCase testName ( TestCase { testDescription, testBrushStroke, testCuspOptions, testStartBoxes } ) = do + putStr $ unlines + [ " " ++ replicate 38 '-' + , " -- Test case: " ++ testName ++ ( if null testDescription then "" else " (" ++ testDescription ++ ")" ) + , " --" ] + before <- getMonotonicTime + let ( _, cornerAndCuspFnI ) = brushStrokeFunctions testBrushStroke + ( trees, dunno, sols ) = + foldMap + ( \ ( i, ( trees, DoneBoxes { doneSolBoxes = defCusps, doneGiveUpBoxes = mbCusps } ) ) -> + ( map ( i, ) trees, map ( ( i , ) . fst ) mbCusps, map ( i, ) defCusps ) ) $ + IntMap.toList $ + findCuspsIn testCuspOptions ( snd . cornerAndCuspFnI ) $ + IntMap.fromList + [ ( i, boxes ) + | ( i, boxes ) <- testStartBoxes + ] + rnf dunno `seq` rnf sols `seq` return () + after <- getMonotonicTime + let dt = after - before + putStrLn $ unlines $ + showResultsList "sols" sols + ++ + showResultsList "dunno" dunno + ++ + [ " -- Time elapsed: " ++ show dt ++ "s" + , " -- Tree size: " ++ show (getSum $ foldMap (foldMap sizeRootIsolationTree) trees) + , " -- Newton stats: " + ] ++ map ( " -- " ++ ) ( showNewtonStats (foldMap (foldMap newtonStats) trees) ) + return dt + +-- | Format a list of results with empty set symbol for empty lists +showResultsList :: Show a => String -> [a] -> [String] +showResultsList label items = + [ " -- " ++ label ++ ":" ++ (if null items then " ∅" else "") ] + ++ + [ " -- • " ++ show item | item <- items ] + +sizeRootIsolationTree :: ( Box 2, RootIsolationTree ( Box 2 ) ) -> Sum Int +sizeRootIsolationTree ( box, tree ) = foldMap ( const $ Sum 1 ) ( showRootIsolationTree box tree ) + + +data NewtonStats + = NewtonStats + { newtonImprovement :: !( Sum Double ) + , newtonElimination :: !( Sum Int, BigBoxes ) + , newtonTime :: !( Sum Double ) + , newtonTotal :: !( Sum Int ) + } +instance Semigroup NewtonStats where + NewtonStats imp1 el1 t1 tot1 <> NewtonStats imp2 el2 t2 tot2 = + NewtonStats ( imp1 <> imp2 ) ( el1 <> el2 ) ( t1 <> t2 ) ( tot1 <> tot2 ) +instance Monoid NewtonStats where + mempty = NewtonStats mempty mempty mempty mempty + +showNewtonStats :: NewtonStats -> [ String ] +showNewtonStats ( NewtonStats ( Sum improv ) ( Sum elims, big ) ( Sum time ) ( Sum tot ) ) + | tot == 0 + = [ ] + | otherwise + = [ "average improvement: " ++ showPercent ( improv / fromIntegral tot ) + , "average eliminations: " ++ showPercent ( fromIntegral elims / fromIntegral tot ) + , "time elapsed: " ++ show ( time / fromIntegral tot ) ++ "s" + ] + +showPercent :: Double -> String +showPercent x = fixed 3 2 ( 100 * x ) <> "%" + +fixed :: Int -> Int -> Double -> String +fixed digitsBefore digitsAfter x = + case second ( drop 1 ) . break ( == '.' ) $ showFFloat ( Just digitsAfter ) x "" of + ( as, bs ) -> + let + l, r :: Int + l = length as + r = length bs + in + replicate ( digitsBefore - l ) ' ' <> as <> "." <> bs <> replicate ( digitsAfter - r ) '0' + +data BigBoxes = + BigBoxes + { biggestArea :: Maybe ( Box 2 ) + , biggestX :: Maybe ( Box 2 ) + , biggestY :: Maybe ( Box 2 ) + } + deriving stock ( Show, Eq ) +instance Semigroup BigBoxes where + BigBoxes area1 x1 y1 <> BigBoxes area2 x2 y2 = + BigBoxes + ( pickBig boxArea area1 area2 ) + ( pickBig widthX x1 x2 ) + ( pickBig widthY y1 y2 ) + where + pickBig _ Nothing Nothing = Nothing + pickBig _ (Just x1) Nothing = Just x1 + pickBig _ Nothing (Just x2) = Just x2 + pickBig f j1@(Just x1) j2@(Just x2) + | f x1 >= f x2 + = j1 + | otherwise + = j2 + widthX ( 𝕀ℝ2 x _y ) = width x + widthY ( 𝕀ℝ2 _x y ) = width y +instance Monoid BigBoxes where + mempty = BigBoxes Nothing Nothing Nothing + + +newtonStats :: ( Box 2, RootIsolationTree ( Box 2 ) ) -> NewtonStats +newtonStats ( _box, RootIsolationLeaf {} ) = mempty +newtonStats ( box, RootIsolationStep step boxes ) + | IsolationStep @Newton ( _, TimeInterval dt ) <- step + = thisImprovement dt <> foldMap newtonStats boxes + | otherwise + = foldMap newtonStats boxes + where + thisImprovement dt = + NewtonStats + { newtonImprovement = Sum $ ( old - new ) / old + , newtonElimination = + if new == 0 + then ( Sum 1, BigBoxes ( Just box ) ( Just box ) ( Just box ) ) + else mempty + , newtonTime = Sum dt + , newtonTotal = Sum 1 + } + where + old = boxArea box + new = sum ( map ( boxArea . fst ) boxes ) + +benchGroups :: [ ( String, NE.NonEmpty TestCase ) ] +benchGroups = + [ ( "ellipse" + , NE.fromList + [ ( ellipseTestCase ( mkOpts gs ) + ( 0, 1 ) pi + ( defaultStartBoxes [ 0 .. 3 ] ) + ) { testDescription = show gs } + | gs <- [ GS_Partial, GS_Complete ] + ] + ) + , ( "trickyCusp2", NE.fromList [ trickyCusp2TestCase ] ) + , ( "rotation", NE.fromList [ ( rotationTestCase ( mkOpts gs ) ) { testDescription = show gs } + | gs <- [ GS_Partial, GS_Complete ] ] ) + , ( "missingCusp2", NE.fromList [ missingCusp2TestCase ] ) + ] + where + minWidth = 1e-5 + ε_bis = 5e-3 + mkOpts gs_opts = + RootIsolationOptions + { rootIsolationAlgorithms = \ hist -> + defaultRootIsolationAlgorithms minWidth ε_bis + ( newtOpts gs_opts hist ) hist + } + newtOpts gs_opts hist = + NewtonLP + --NewtonGaussSeidel $ + -- ( defaultGaussSeidelOptions @2 @3 hist ) + -- { gsUpdate = gs_opts } + +-------------------------------------------------------------------------------- + +{- -- Old testing code. + +getR1 (ℝ1 u) = u + +eval + :: ( I i 1 -> Seq ( I i 1 -> StrokeDatum k i ) ) + -> ( I i 1, Int, I i 1 ) + -> StrokeDatum k i +eval f ( t, i, s ) = ( f t `Seq.index` i ) s + +mkVal :: Double -> Int -> Double -> ( ℝ 1, Int, ℝ 1 ) +mkVal t i s = ( ℝ1 t, i, ℝ1 s ) + +mkI :: ( Double, Double ) -> 𝕀 +mkI ( lo, hi ) = 𝕀 lo hi + +mkBox :: ( Double, Double ) -> ( Double, Double ) -> Box 2 +mkBox ( t_min, t_max ) ( s_min, s_max ) = + ( 𝕀 ( ℝ2 t_min s_min ) ( ℝ2 t_max s_max ) ) + +potentialCusp :: StrokeDatum 3 𝕀 -> Bool +potentialCusp + ( StrokeDatum + { ee = D22 { _D22_v = 𝕀 ( ℝ1 ee_min ) ( ℝ1 ee_max ) } + , 𝛿E𝛿sdcdt = D12 { _D12_v = T ( 𝕀 ( ℝ2 vx_min vy_min ) ( ℝ2 vx_max vy_max ) )} + } + ) = ee_min <= 0 && ee_max >= 0 + && vx_min <= 0 && vx_max >= 0 + && vy_min <= 0 && vy_max >= 0 + +dEdsdcdt :: StrokeDatum k i -> D ( k - 2 ) ( I i 2 ) ( T ( I i 2 ) ) +dEdsdcdt ( StrokeDatum { 𝛿E𝛿sdcdt = v } ) = v + +width :: 𝕀ℝ 1 -> Double +width (𝕀 (ℝ1 lo) (ℝ1 hi)) = hi - lo + +negV2 :: 𝕀ℝ 2 -> 𝕀ℝ 2 +negV2 ( 𝕀 ( ℝ2 x_lo y_lo ) ( ℝ2 x_hi y_hi ) ) = + let !( 𝕀 x'_lo x'_hi ) = negate $ 𝕀 x_lo x_hi + !( 𝕀 y'_lo y'_hi ) = negate $ 𝕀 y_lo y_hi + in 𝕀 ( ℝ2 x'_lo y'_lo ) ( ℝ2 x'_hi y'_hi ) + +midV2 :: 𝕀ℝ 2 -> ℝ 2 +midV2 ( 𝕀 ( ℝ2 x_lo y_lo ) ( ℝ2 x_hi y_hi ) ) = + ℝ2 ( 0.5 * ( x_lo + x_hi ) ) ( 0.5 * ( y_lo + y_hi ) ) + + +logLines :: [ String ] +logLines = + [ "E, dE/ds * dc/dt" + , "{" ++ + (intercalate "," + [ "{" ++ showD t ++ "," ++ showD s ++ ",{" ++ intercalate "," vals ++ "}}" + | t <- [ 0.5484, 0.5484 + 0.00001 .. 0.5488 ] + , s <- [ 0.5479, 0.5479 + 0.00001 .. 0.5483 ] + , let StrokeDatum + { ee = D22 ee _ _ _ _ _ + , 𝛿E𝛿sdcdt = D12 (T f) (T (T f_t)) (T (T f_s)) + } = (curvesI (singleton (ℝ1 t)) `Seq.index` i) (singleton (ℝ1 s)) + ℝ2 vx vy = midPoint2 f + --ℝ2 vx_t vy_t = midPoint2 f_t + --ℝ2 vx_s vy_s = midPoint2 f_s + vals = [ showD ( midPoint ee ) + , "{" ++ showD vx ++ "," ++ showD vy ++ "}" + -- , "{" ++ showD vx_t ++ "," ++ showD vy_t ++ "}" + -- , "{" ++ showD vx_s ++ "," ++ showD vy_s ++ "}" + ] + ] + ) ++ "}" + ] + where + i = 2 + ( curves, curvesI ) = brushStrokeFunctions $ ellipseBrushStroke ( 0, 1 ) pi + midPoint (𝕀 (ℝ1 lo) (ℝ1 hi)) = 0.5 * ( lo + hi ) + midPoint2 (𝕀 (ℝ2 lo_x lo_y) (ℝ2 hi_x hi_y)) + = ℝ2 ( 0.5 * ( lo_x + hi_x ) ) ( 0.5 * ( lo_y + hi_y ) ) + +_x ( ℝ2 x _ ) = x +_y ( ℝ2 _ y ) = y + +showD :: Double -> String +showD float = showFFloat (Just 6) float "" + +-}
+ src/lib/Calligraphy/Brushes.hs view
@@ -0,0 +1,524 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE OverloadedStrings #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE RebindableSyntax #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Calligraphy.Brushes + ( BrushFn, Brush(..), Corner(..) + , Unrotated(..) + , circleBrush + , ellipseBrush + , tearDropBrush, tearDropHeightOffset + , roundTearDropBrush + ) where + +-- base +import Prelude + hiding + ( Num(..), Floating(..), (^), (/), fromInteger, fromRational ) +import Data.Kind + ( Type, Constraint ) +import GHC.Exts + ( Proxy#, proxy# ) +import GHC.TypeNats + ( Nat ) + +-- acts +import Data.Act + ( Torsor((-->)) ) + +-- containers +import Data.Sequence + ( Seq ) +import qualified Data.Sequence as Seq + ( empty, fromList, singleton ) + +-- brush-strokes +import Math.Algebra.Dual +import Math.Bezier.Spline +import Math.Differentiable + ( I, IVness(..), SingIVness (..) ) +import Math.Interval + ( 𝕀ℝ, singleton, point ) +import Math.Linear +import Math.Module +import Math.Ring + +-------------------------------------------------------------------------------- + +-- | The shape of a brush (before applying any rotation). +type BrushFn :: IVness -> Nat -> Nat -> Type +type BrushFn i k nbBrushParams = + Unrotated + ( C k ( I i nbBrushParams ) ( Spline Closed () ( I i 2 ) ) ) + +-- | A newtype wrapper to enforce applying rotation. +newtype Unrotated a = Unrotated { getUnrotated :: a } + deriving stock ( Eq, Ord, Show, Functor, Foldable, Traversable ) + +-- | A corner of a brush. +type Corner :: Type -> Type +data Corner a + = Corner + { cornerPoint :: a + , cornerStartTangent, cornerEndTangent :: T a + } + deriving stock ( Eq, Show, Functor, Foldable, Traversable ) + +instance Applicative Corner where + pure a = Corner a ( T a ) ( T a ) + Corner f ( T g ) ( T h ) <*> Corner a ( T b ) ( T c ) = + Corner ( f a ) ( T $ g b ) ( T $ h c ) + +instance forall r a. ( Ring r, Module r ( T a ) ) => Module r ( T ( Corner a ) ) where + origin = T $ pure $ unT $ origin @r @( T a ) + ( T f ) ^+^ ( T g ) = T $ liftA2 ( \ x y -> unT $ T x ^+^ T y ) f g + ( T f ) ^-^ ( T g ) = T $ liftA2 ( \ x y -> unT $ T x ^+^ T y ) f g + a *^ T f = T $ fmap ( \ x -> unT $ a *^ T x ) f + +-- | A brush, described as a base shape + an optional rotation. +data Brush nbBrushParams + = Brush + { -- | Base brush shape, before applying rotation (if any). + brushBaseShape :: BrushFn NonIV 2 nbBrushParams + , -- | Base brush shape, before applying rotation (if any). + brushBaseShape3 :: BrushFn NonIV 3 nbBrushParams + -- | Base brush shape, before applying rotation (if any). + , brushBaseShapeI :: BrushFn IsIV 3 nbBrushParams + + -- | Brush corners, before applying rotation (if any). + , brushCorners :: Seq ( Unrotated ( C 2 ( ℝ nbBrushParams ) ( Corner ( I NonIV 2 ) ) ) ) + -- | Brush corners, before applying rotation (if any). + , brushCorners3 :: Seq ( Unrotated ( C 3 ( ℝ nbBrushParams ) ( Corner ( I NonIV 2 ) ) ) ) + -- | Brush corners, before applying rotation (if any). + , brushCornersI :: Seq ( Unrotated ( C 3 ( 𝕀ℝ nbBrushParams ) ( Corner ( I IsIV 2 ) ) ) ) + + -- | Optional rotation angle function + -- (assumed to be a linear function). + , mbRotation :: Maybe ( ℝ nbBrushParams -> Double ) + } + +-------------------------------------------------------------------------------- +-- Brushes + +-- Some convenience type synonyms for brush types... a bit horrible +type ParamsICt :: Nat -> IVness -> Nat -> Constraint +type ParamsICt k i nbParams = + ( Module + ( D k nbParams ( I i Double ) ) + ( D k nbParams ( I i 2 ) ) + , Module ( I i Double ) ( T ( I i Double ) ) + , HasChainRule ( I i Double ) k ( I i nbParams ) + , Representable ( I i Double ) ( I i nbParams ) + , Applicative ( D k nbParams ) + , Dim ( I i nbParams ) ~ nbParams + ) + +{-# INLINEABLE circleBrush #-} +circleBrush :: Brush 1 +circleBrush = + Brush + { brushBaseShape = Unrotated $ circleBrushFn @2 SNonIV + , brushBaseShape3 = Unrotated $ circleBrushFn @3 SNonIV + , brushBaseShapeI = Unrotated $ circleBrushFn @3 SIsIV + , brushCorners = Seq.empty + , brushCorners3 = Seq.empty + , brushCornersI = Seq.empty + , mbRotation = Nothing + } + +{-# INLINEABLE ellipseBrush #-} +ellipseBrush :: Brush 3 +ellipseBrush = + Brush + { brushBaseShape = Unrotated $ ellipseBrushFn @2 SNonIV + , brushBaseShape3 = Unrotated $ ellipseBrushFn @3 SNonIV + , brushBaseShapeI = Unrotated $ ellipseBrushFn @3 SIsIV + , brushCorners = Seq.empty + , brushCorners3 = Seq.empty + , brushCornersI = Seq.empty + , mbRotation = Just ( `index` ( Fin 3 ) ) + } + +{-# INLINEABLE tearDropBrush #-} +tearDropBrush :: Brush 3 +tearDropBrush = + Brush + { brushBaseShape = Unrotated $ tearDropBrushFn @2 SNonIV + , brushBaseShape3 = Unrotated $ tearDropBrushFn @3 SNonIV + , brushBaseShapeI = Unrotated $ tearDropBrushFn @3 SIsIV + , brushCorners = Seq.singleton $ Unrotated $ tearDropCorner @2 proxy# SNonIV + , brushCorners3 = Seq.singleton $ Unrotated $ tearDropCorner @3 proxy# SNonIV + , brushCornersI = Seq.singleton $ Unrotated $ tearDropCorner @3 proxy# SIsIV + , mbRotation = Just ( `index` ( Fin 3 ) ) + } + +tearDropHeightOffset :: Double +tearDropHeightOffset = 2 / 14 + +-------------------------------------------------------------------------------- + +type SplineFn k i nbParams + = SingIVness i + -> C k ( I i nbParams ) ( Spline 'Closed () ( I i 2 ) ) + +type CornerFn k i nbParams + = Proxy# k + -> SingIVness i + -> C k ( I i nbParams ) ( Corner ( I i 2 ) ) + +-------------------------------------------------------------------------------- +-- Circle & ellipse + +-- | Root of @(Sqrt[2] (4 + 3 κ) - 16) (2 - 3 κ)^2 - 8 (1 - 3 κ) Sqrt[8 - 24 κ + 12 κ^2 + 8 κ^3 + 3 κ^4]@. +-- +-- Used to approximate circles and ellipses with Bézier curves. +κ :: Double +κ = 0.5519150244935105707435627227925 + +circleSpline :: forall d v + . Applicative d + => ( Double -> Double -> d v ) + -> d ( Spline 'Closed () v ) +circleSpline p = sequenceA $ + Spline { splineStart = p 1 0 + , splineCurves = ClosedCurves crvs lastCrv } + where + crvs = Seq.fromList + [ Bezier3To ( p 1 κ ) ( p κ 1 ) ( NextPoint ( p 0 1 ) ) () + , Bezier3To ( p -κ 1 ) ( p -1 κ ) ( NextPoint ( p -1 0 ) ) () + , Bezier3To ( p -1 -κ ) ( p -κ -1 ) ( NextPoint ( p 0 -1 ) ) () + ] + lastCrv = + Bezier3To ( p κ -1 ) ( p 1 -κ ) BackToStart () +{-# INLINE circleSpline #-} + +mkI1 :: SingIVness i -> Double -> I i Double +mkI1 = \case + SNonIV -> id + SIsIV -> singleton +{-# INLINE mkI1 #-} + +mkI2 :: SingIVness i -> ℝ 2 -> I i 2 +mkI2 = \case + SNonIV -> id + SIsIV -> point +{-# INLINE mkI2 #-} + +circleBrushFn :: forall k i nbParams + . ( nbParams ~ 1 + , ParamsICt k i nbParams + ) + => SplineFn k i nbParams +circleBrushFn ivness = + D \ params -> + let r :: D k nbParams ( I i Double ) + r = runD ( var @_ @k $ Fin 1 ) params + mkPt :: Double -> Double -> D k nbParams ( I i 2 ) + mkPt x y + = ( r `scaledBy` x ) *^ e_x + ^+^ ( r `scaledBy` y ) *^ e_y + in circleSpline mkPt + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE circleBrushFn @2 @NonIV #-} +{-# SPECIALISE circleBrushFn @3 @IsIV #-} + +ellipseBrushFn :: forall k i nbParams + . ( nbParams ~ 3 + , ParamsICt k i nbParams + ) + => SplineFn k i nbParams +ellipseBrushFn ivness = + D \ params -> + let a, b :: D k nbParams ( I i Double ) + a = runD ( var @_ @k $ Fin 1 ) params + b = runD ( var @_ @k $ Fin 2 ) params + mkPt :: Double -> Double -> D k nbParams ( I i 2 ) + mkPt x y + = let !x' = a `scaledBy` x + !y' = b `scaledBy` y + in x' *^ e_x ^+^ y' *^ e_y + in circleSpline mkPt + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE ellipseBrushFn @2 @NonIV #-} +{-# SPECIALISE ellipseBrushFn @3 @IsIV #-} + +-------------------------------------------------------------------------------- +-- Tear drop + +-- | y-coordinate of the center of mass of the cubic Bézier teardrop +-- with control points at (0,0), (±0.5,√3/2). +tearCenter :: Double +tearCenter = 3 * sqrt 3 / 14 + +-- | Width of the cubic Bézier teardrop with control points at (0,0), (±0.5,√3/2). +tearWidth :: Double +tearWidth = 1 / ( 2 * sqrt 3 ) + +-- | Height of the cubic Bézier teardrop with control points at (0,0), (±0.5,√3/2). +tearHeight :: Double +tearHeight = 3 * sqrt 3 / 8 + +-- √3/2 +sqrt3_over_2 :: Double +sqrt3_over_2 = 0.5 * sqrt 3 + +tearDropBrushFn :: forall k i nbParams + . ( nbParams ~ 3 + , ParamsICt k i nbParams + ) + => SplineFn k i nbParams +tearDropBrushFn ivness = + D \ params -> + let w, h :: D k nbParams ( I i Double ) + w = 2 * runD ( var @_ @k ( Fin 1 ) ) params + h = 2 * runD ( var @_ @k ( Fin 2 ) ) params + mkPt :: Double -> Double -> D k nbParams ( I i 2 ) + mkPt x y + -- 1. translate the teardrop so that the centre of mass is at the origin + -- 2. scale the teardrop so that it has the requested width/height + -- 3. rotate (done separately) + = let !x' = w `scaledBy` ( x / tearWidth ) + !y' = h `scaledBy` ( ( y - tearCenter ) / tearHeight ) + in x' *^ e_x ^+^ y' *^ e_y + + in sequenceA $ + Spline { splineStart = mkPt 0 0 + , splineCurves = ClosedCurves Seq.empty $ + Bezier3To + ( mkPt 0.5 sqrt3_over_2 ) + ( mkPt -0.5 sqrt3_over_2 ) + BackToStart () } + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE tearDropBrushFn @2 @NonIV #-} +{-# SPECIALISE tearDropBrushFn @2 @IsIV #-} + +tearDropCorner :: forall k i nbParams. ( nbParams ~ 3, ParamsICt k i nbParams, Torsor ( T ( I i 2 ) ) ( I i 2 ) ) => CornerFn k i nbParams +tearDropCorner _ ivness = + D \ params -> + let w, h :: D k nbParams ( I i Double ) + w = 2 * runD ( var @_ @k ( Fin 1 ) ) params + h = 2 * runD ( var @_ @k ( Fin 2 ) ) params + mkPt :: Double -> Double -> D k nbParams ( I i 2 ) + mkPt x y + -- 1. translate the teardrop so that the centre of mass is at the origin + -- 2. scale the teardrop so that it has the requested width/height + -- 3. rotate (done separately) + = let !x' = w `scaledBy` ( x / tearWidth ) + !y' = h `scaledBy` ( ( y - tearCenter ) / tearHeight ) + in x' *^ e_x ^+^ y' *^ e_y + !cornerPoint = mkPt 0 0 + in sequenceA $ + Corner + { cornerPoint + , cornerStartTangent = T $ liftA2 ( \ x y -> unT $ x --> y ) ( mkPt -0.5 sqrt3_over_2 ) cornerPoint + , cornerEndTangent = T $ liftA2 ( \ x y -> unT $ x --> y ) cornerPoint ( mkPt 0.5 sqrt3_over_2 ) + } + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE tearDropCorner @2 @NonIV #-} +{-# SPECIALISE tearDropCorner @3 @IsIV #-} + +-------------------------------------------------------------------------------- +-- Curl Drop + +{-# INLINEABLE roundTearDropBrush #-} +roundTearDropBrush :: Brush 3 +roundTearDropBrush = + Brush + { brushBaseShape = Unrotated $ roundTearDropBrushFn @2 SNonIV + , brushBaseShape3 = Unrotated $ roundTearDropBrushFn @3 SNonIV + , brushBaseShapeI = Unrotated $ roundTearDropBrushFn @3 SIsIV + , brushCorners = Seq.fromList + [ Unrotated $ roundTearDropCorner @2 proxy# SNonIV + , Unrotated $ roundTearDropSideCorner @2 proxy# SNonIV True + , Unrotated $ roundTearDropSideCorner @2 proxy# SNonIV False + ] + , brushCorners3 = Seq.fromList + [ Unrotated $ roundTearDropCorner @3 proxy# SNonIV + , Unrotated $ roundTearDropSideCorner @3 proxy# SNonIV True + , Unrotated $ roundTearDropSideCorner @3 proxy# SNonIV False + ] + , brushCornersI = Seq.fromList + [ Unrotated $ roundTearDropCorner @3 proxy# SIsIV + , Unrotated $ roundTearDropSideCorner @3 proxy# SIsIV True + , Unrotated $ roundTearDropSideCorner @3 proxy# SIsIV False + ] + , mbRotation = Just ( `index` ( Fin 3 ) ) + } + +roundTearDropBrushFn :: forall k i nbParams + . ( nbParams ~ 3 + , ParamsICt k i nbParams + ) + => SplineFn k i nbParams +roundTearDropBrushFn ivness = + D \ params -> + let h, r :: D k nbParams ( I i Double ) + h = 2 * runD ( var @_ @k ( Fin 1 ) ) params + r = runD ( var @_ @k ( Fin 2 ) ) params + + mkPt :: D k nbParams ( I i Double ) -> D k nbParams ( I i Double ) -> D k nbParams ( I i 2 ) + mkPt x y = ( x - h `scaledBy` 0.5 ) *^ e_x ^+^ y *^ e_y + + curves = Seq.fromList + [ + Bezier2To + ( mkPt ( r `scaledBy` 0.5 ) -r ) + ( NextPoint ( mkPt ( h - r ) -r ) ) + () + + , Bezier3To + ( mkPt ( h + r `scaledBy` ( κ - 1 ) ) -r ) + ( mkPt h ( -r `scaledBy` κ ) ) + ( NextPoint ( mkPt h 0 ) ) + () + + , Bezier3To + ( mkPt h ( r `scaledBy` κ ) ) + ( mkPt ( h + r `scaledBy` ( κ - 1 ) ) r ) + ( NextPoint ( mkPt ( h - r ) r ) ) + () + ] + + lastCrv = + Bezier2To + ( mkPt ( r `scaledBy` 0.5 ) r ) + BackToStart + () + + in sequenceA $ + Spline { splineStart = mkPt 0 0 + , splineCurves = ClosedCurves curves lastCrv + } + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE roundTearDropBrushFn @2 @NonIV #-} +{-# SPECIALISE roundTearDropBrushFn @2 @IsIV #-} + +-- | Main corner of the round tear-drop. +roundTearDropCorner :: forall k i nbParams. ( nbParams ~ 3, ParamsICt k i nbParams, Torsor ( T ( I i 2 ) ) ( I i 2 ) ) => CornerFn k i nbParams +roundTearDropCorner _ ivness = + D \ params -> + let h, r :: D k nbParams ( I i Double ) + h = 2 * runD ( var @_ @k ( Fin 1 ) ) params + r = runD ( var @_ @k ( Fin 2 ) ) params + + mkPt :: D k nbParams ( I i Double ) -> D k nbParams ( I i Double ) -> D k nbParams ( I i 2 ) + mkPt x y = ( x - h `scaledBy` 0.5 ) *^ e_x ^+^ y *^ e_y + + -- Corner point at the origin + cornerPoint = mkPt 0 0 + + -- First and last control points as defined in roundTearDropBrushFn + firstControlPoint = mkPt ( r `scaledBy` 0.5 ) -r + lastControlPoint = mkPt ( r `scaledBy` 0.5 ) r + + in sequenceA $ + Corner + { cornerPoint + -- Define tangent directions based on the control points + -- Start tangent: from last control point to corner + , cornerStartTangent = T $ liftA2 ( \ x y -> unT $ x --> y ) lastControlPoint cornerPoint + -- End tangent: from corner to first control point + , cornerEndTangent = T $ liftA2 ( \ x y -> unT $ x --> y ) cornerPoint firstControlPoint + } + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE roundTearDropCorner @2 @NonIV #-} +{-# SPECIALISE roundTearDropCorner @3 @IsIV #-} + +-- | Corner at the side connection points (h - r, ±r). +-- Takes a Boolean parameter to determine if it's the top (True) or bottom (False) corner. +-- +-- Weirdly, this isn't really a corner (as it's flat), but because the derivative +-- is not continuous it can cause cusps in the envelope, so we need to +-- treat it as a corner. +roundTearDropSideCorner :: forall k i nbParams. + ( nbParams ~ 3 + , ParamsICt k i nbParams + , Torsor ( T ( I i 2 ) ) ( I i 2 ) + ) + => Proxy# k + -> SingIVness i + -> Bool -- ^ True for top corner, False for bottom corner + -> C k ( I i nbParams ) ( Corner ( I i 2 ) ) +roundTearDropSideCorner _ ivness isTop = + D \ params -> + let h, r :: D k nbParams ( I i Double ) + h = 2 * runD ( var @_ @k ( Fin 1 ) ) params + r = runD ( var @_ @k ( Fin 2 ) ) params + + mkPt :: D k nbParams ( I i Double ) -> D k nbParams ( I i Double ) -> D k nbParams ( I i 2 ) + mkPt x y = ( x - h `scaledBy` 0.5 ) *^ e_x ^+^ y *^ e_y + + -- Sign factor based on whether it's top or bottom + signFactor = if isTop then 1 else -1 + + -- Corner point at (h - r, ±r) + cornerPoint = mkPt (h - r) (r `scaledBy` signFactor) + + -- Control points for the adjacent curves + -- For top: prevControlPoint is from cubic, nextControlPoint is from quadratic + -- For bottom: prevControlPoint is from quadratic, nextControlPoint is from cubic + (prevControlPoint, nextControlPoint) = + if isTop + then + ( mkPt (h + r `scaledBy` (κ - 1)) (r `scaledBy` signFactor) -- From cubic Bézier + , mkPt (r `scaledBy` 0.5) (r `scaledBy` signFactor) -- From quadratic Bézier + ) + else + ( mkPt (r `scaledBy` 0.5) (r `scaledBy` signFactor) -- From quadratic Bézier + , mkPt (h + r `scaledBy` (κ - 1)) (r `scaledBy` signFactor) -- From cubic Bézier + ) + + in sequenceA $ + Corner + { cornerPoint + , cornerStartTangent = T $ liftA2 ( \ x y -> unT $ x --> y ) prevControlPoint cornerPoint + , cornerEndTangent = T $ liftA2 ( \ x y -> unT $ x --> y ) cornerPoint nextControlPoint + } + where + e_x, e_y :: D k nbParams ( I i 2 ) + e_x = pure $ mkI2 ivness $ ℝ2 1 0 + e_y = pure $ mkI2 ivness $ ℝ2 0 1 + + scaledBy :: D k nbParams ( I i Double ) -> Double -> D k nbParams ( I i Double ) + scaledBy d x = fmap ( mkI1 ivness x * ) d +{-# SPECIALISE roundTearDropSideCorner @2 @NonIV #-} +{-# SPECIALISE roundTearDropSideCorner @3 @IsIV #-}
+ src/lib/Debug/Utils.hs view
@@ -0,0 +1,70 @@+module Debug.Utils + ( trace + , logToFile + ) where + +-- base +import Control.Concurrent.MVar + ( MVar, withMVarMasked ) +import Control.Monad + ( void ) +import System.IO + ( BufferMode(..), hSetBuffering, hFlush, hPutStrLn, stdout ) +import System.IO.Unsafe + ( unsafePerformIO ) + +-- code-page +import System.IO.CodePage + ( withCP65001 ) + +-- deepseq +import Control.DeepSeq + ( force ) + +-- directory +import qualified System.Directory as Directory + ( createDirectoryIfMissing ) + +-- filepath +import qualified System.FilePath as FilePath + ( takeDirectory ) + +-- time +import qualified Data.Time.Clock as Time + ( getCurrentTime ) +import qualified Data.Time.Format as Time + ( defaultTimeLocale, formatTime ) + +-------------------------------------------------------------------------------- + +-- | Like 'Debug.Trace.trace', but using 'withCP65001` in order to handle +-- Unicode without crashing on Windows. +trace :: String -> a -> a +trace ( force -> !str ) expr = + unsafePerformIO $ withCP65001 do + hSetBuffering stdout ( BlockBuffering $ Just 50 ) + hPutStrLn stdout str + hFlush stdout + return expr + +-- | Log the second argument to the file stored in the 'MVar' in the first +-- argument. +-- +-- The 'MVar' is used to avoid jumbled contents when attempting to write to +-- the file concurrently. +logToFile :: MVar FilePath -> String -> () +logToFile logFileMVar logContents = + unsafePerformIO $ withCP65001 $ void $ withMVarMasked logFileMVar $ \ logFile -> do + now <- Time.getCurrentTime + let timeString = Time.formatTime Time.defaultTimeLocale "%0Y-%m-%d (%Hh %Mm %Ss)" now + logContentsWithHeader = unlines + [ replicate 80 '=' + , timeString + , logContents + ] + Directory.createDirectoryIfMissing True $ + FilePath.takeDirectory logFile + appendFile logFile logContentsWithHeader + return logFile +{-# NOINLINE logToFile #-} +
+ src/lib/Math/Algebra/Dual.hs view
@@ -0,0 +1,1376 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE RebindableSyntax #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +{-# OPTIONS_GHC -Wno-orphans -O2 #-} + +module Math.Algebra.Dual + ( C(..), D, Dim + , Differential(..) + , HasChainRule(..), chainRule + , uncurryD2, uncurryD3 + , linear, fun, var + + , chainRuleN1Q, chainRule1NQ + + , ApModule(..) + , DiffModule(..) + + , D𝔸0(..) + , D1𝔸1(..), D2𝔸1(..), D3𝔸1(..) + , D1𝔸2(..), D2𝔸2(..), D3𝔸2(..) + , D1𝔸3(..), D2𝔸3(..), D3𝔸3(..) + , D1𝔸4(..), D2𝔸4(..), D3𝔸4(..) + ) where + +-- base +import Prelude + hiding + ( Num(..), Floating(..) + , (^), recip, fromInteger, fromRational ) +import Data.Coerce + ( coerce ) +import Data.Kind + ( Constraint, Type ) +import GHC.TypeNats + ( Nat ) + +-- brush-strokes +import Math.Algebra.Dual.Internal +import Math.Interval.Internal +import Math.Linear +import Math.Module +import Math.Monomial +import Math.Ring + +-------------------------------------------------------------------------------- + +-- | @C n u v@ is the space of @C^k@-differentiable maps from @u@ to @v@. +type C :: Nat -> Type -> Type -> Type +newtype C k u v = D { runD :: u -> D k ( Dim u ) v } +deriving stock instance Functor ( D k ( Dim u ) ) => Functor ( C k u ) + +type Dim :: k -> Nat +type family Dim u +type instance Dim ( ℝ n ) = n + +-- | @D k u v@ is the space of @k@-th order germs of functions from @u@ to @v@, +-- represented by the algebra: +-- +-- \[ \mathbb{Z}[x_1, \ldots, x_n]/(x_1, \ldots, x_n)^{k+1} \otimes_\mathbb{Z} v \] +-- +-- when @u@ is of dimension @n@. +type D :: Nat -> Nat -> Type -> Type +type family D k u + +type instance D k 0 = D𝔸0 +type instance D 0 1 = D𝔸0 +type instance D 0 2 = D𝔸0 +type instance D 0 3 = D𝔸0 +type instance D 0 4 = D𝔸0 + +type instance D 1 1 = D1𝔸1 +type instance D 1 2 = D1𝔸2 +type instance D 1 3 = D1𝔸3 +type instance D 1 4 = D1𝔸4 + +type instance D 2 1 = D2𝔸1 +type instance D 2 2 = D2𝔸2 +type instance D 2 3 = D2𝔸3 +type instance D 2 4 = D2𝔸4 + +type instance D 3 1 = D3𝔸1 +type instance D 3 2 = D3𝔸2 +type instance D 3 3 = D3𝔸3 +type instance D 3 4 = D3𝔸4 + +-------------------------------------------------------------------------------- + +-- Weird instance needed in just one place; +-- see use of chain in 'Math.Bezier.Stroke.brushStrokeData'. +instance ( Applicative ( D k ( Dim u ) ), Module r ( T v ) ) + => Module r ( T ( C k u v ) ) where + origin = T $ D \ _ -> pure $ coerce $ origin @r @( T v ) + T ( D f ) ^+^ T ( D g ) = T $ D \ t -> liftA2 ( coerce $ (^+^) @r @( T v ) ) ( f t ) ( g t ) + T ( D f ) ^-^ T ( D g ) = T $ D \ t -> liftA2 ( coerce $ (^-^) @r @( T v ) ) ( f t ) ( g t ) + a *^ T ( D f ) = T $ D \ t -> fmap ( coerce $ (*^) @r @( T v ) a ) $ f t + +newtype DiffModule k n v = DiffModule { getDiffModule :: D k n v } + +instance ( Applicative ( D k n ), Module r ( T v ) ) + => Module r ( T ( DiffModule k n v ) ) where + origin = T $ DiffModule $ pure $ coerce $ origin @r @( T v ) + T ( DiffModule f ) ^+^ T ( DiffModule g ) = T $ DiffModule $ liftA2 ( coerce $ (^+^) @r @( T v ) ) f g + T ( DiffModule f ) ^-^ T ( DiffModule g ) = T $ DiffModule $ liftA2 ( coerce $ (^-^) @r @( T v ) ) f g + a *^ T ( DiffModule f ) = T $ DiffModule $ fmap ( coerce $ (*^) @r @( T v ) a ) f + +-------------------------------------------------------------------------------- + +type Differential :: Nat -> Nat -> Constraint +class Differential k n where + konst :: AbelianGroup w => w -> D k n w + value :: D k n w -> w + +-- | @HasChainRule r k v@ means we have a chain rule +-- with @D k v w@ in the middle, for any @r@-module @w@. +class Differential k ( Dim v ) => HasChainRule r k v where + chain :: Module r ( T w ) + => D k 1 v -> D k ( Dim v ) w -> D k 1 w + linearD :: Module r ( T w ) => ( v -> w ) -> v -> D k ( Dim v ) w + +linear :: forall k r v w + . ( HasChainRule r k v, Module r ( T w ) ) + => ( v -> w ) -> C k v w +linear f = D \ x -> linearD @r @k @v @w f x + +chainRule :: forall r k u v w + . ( HasChainRule r k v, Module r ( T w ) + , Dim u ~ 1, HasChainRule r k u + ) + => C k u v -> C k v w -> C k u w +chainRule ( D df ) ( D dg ) = + D \ x -> + case df x of + df_x -> + chain @r @k @v df_x ( dg $ value @k @( Dim u ) df_x ) + +uncurryD2 :: D 2 1 ( D 2 1 b ) -> D 2 2 b +uncurryD2 ( D21 ( b_t0 ) ( T ( dbdt_t0 ) ) ( T ( d2bdt2_t0 ) ) ) = + let D21 b_t0s0 dbds_t0s0 d2bds2_t0s0 = b_t0 + D21 dbdt_t0s0 d2bdtds_t0s0 _ = dbdt_t0 + D21 d2bdt2_t0s0 _ _ = d2bdt2_t0 + in D22 + b_t0s0 + ( T dbdt_t0s0 ) dbds_t0s0 + ( T d2bdt2_t0s0 ) d2bdtds_t0s0 d2bds2_t0s0 + +uncurryD3 :: D 3 1 ( D 3 1 b ) -> D 3 2 b +uncurryD3 ( D31 b_t0 ( T dbdt_t0 ) ( T d2bdt2_t0 ) ( T d3bdt3_t0 ) ) = + let D31 b_t0s0 dbds_t0s0 d2bds2_t0s0 d3bds3_t0s0 = b_t0 + D31 dbdt_t0s0 d2bdtds_t0s0 d3bdtds2_t0s0 _ = dbdt_t0 + D31 d2bdt2_t0s0 d3bdt2ds_t0s0 _ _ = d2bdt2_t0 + D31 d3bdt3_t0s0 _ _ _ = d3bdt3_t0 + in D32 + b_t0s0 + ( T dbdt_t0s0 ) dbds_t0s0 + ( T d2bdt2_t0s0 ) d2bdtds_t0s0 d2bds2_t0s0 + ( T d3bdt3_t0s0 ) d3bdt2ds_t0s0 d3bdtds2_t0s0 d3bds3_t0s0 + +-- | Recover the underlying function, discarding all infinitesimal information. +fun :: forall k v w. Differential k ( Dim v ) => C k v w -> ( v -> w ) +fun ( D df ) = value @k @( Dim v ) . df +{-# INLINE fun #-} + +-- | The differentiable germ of a coordinate variable. +var :: forall r k v + . ( Module r ( T r ), Representable r v, HasChainRule r k v ) + => Fin ( RepDim v ) -> C k v r +var i = D $ linearD @r @k @v ( `index` i ) +{-# INLINE var #-} + +-------------------------------------------------------------------------------- + +-- | Newtype for the module instance @Module r v => Module ( dr r ) ( dr v )@. +type ApModule :: Type -> ( Type -> Type ) -> Type -> Type +newtype ApModule r dr v = ApModule { unApModule :: dr v } + +instance ( Ring ( dr r ), Module r ( T v ), Applicative dr ) + => Module ( dr r ) ( ApModule r dr v ) where + ApModule !u ^+^ ApModule !v = ApModule $ liftA2 ( coerce $ (^+^) @r @( T v ) ) u v + ApModule !u ^-^ ApModule !v = ApModule $ liftA2 ( coerce $ (^-^) @r @( T v ) ) u v + origin = ApModule $ pure $ coerce $ origin @r @( T v ) + !k *^ ApModule !u = ApModule $ liftA2 ( coerce $ (*^) @r @( T v ) ) k u + + +deriving via ApModule r D𝔸0 v + instance Module r ( T v ) => Module ( D𝔸0 r ) ( D𝔸0 v ) +--deriving via ApModule r D1𝔸1 v +-- instance Module r ( T v ) => Module ( D1𝔸1 r ) ( D1𝔸1 v ) +deriving via ApModule r D2𝔸1 v + instance Module r ( T v ) => Module ( D2𝔸1 r ) ( D2𝔸1 v ) +deriving via ApModule r D3𝔸1 v + instance Module r ( T v ) => Module ( D3𝔸1 r ) ( D3𝔸1 v ) +--deriving via ApModule r D1𝔸2 v +-- instance Module r ( T v ) => Module ( D1𝔸2 r ) ( D1𝔸2 v ) +deriving via ApModule r D2𝔸2 v + instance Module r ( T v ) => Module ( D2𝔸2 r ) ( D2𝔸2 v ) +deriving via ApModule r D3𝔸2 v + instance Module r ( T v ) => Module ( D3𝔸2 r ) ( D3𝔸2 v ) +--deriving via ApModule r D1𝔸3 v +-- instance Module r ( T v ) => Module ( D1𝔸3 r ) ( D1𝔸3 v ) +deriving via ApModule r D2𝔸3 v + instance Module r ( T v ) => Module ( D2𝔸3 r ) ( D2𝔸3 v ) +deriving via ApModule r D3𝔸3 v + instance Module r ( T v ) => Module ( D3𝔸3 r ) ( D3𝔸3 v ) +--deriving via ApModule r D1𝔸4 v +-- instance Module r ( T v ) => Module ( D1𝔸4 r ) ( D1𝔸4 v ) +deriving via ApModule r D2𝔸4 v + instance Module r ( T v ) => Module ( D2𝔸4 r ) ( D2𝔸4 v ) +deriving via ApModule r D3𝔸4 v + instance Module r ( T v ) => Module ( D3𝔸4 r ) ( D3𝔸4 v ) + +-------------------------------------------------------------------------------- +-- AbelianGroup instances + +newtype ApAp2 k u r = ApAp2 { unApAp2 :: D k ( Dim u ) r } + +instance ( Applicative ( D k ( Dim u ) ) + , AbelianGroup r + , HasChainRule Double k u + ) => AbelianGroup ( ApAp2 k u r ) where + ApAp2 !x + ApAp2 !y = ApAp2 $ liftA2 ( (+) @r ) x y + ApAp2 !x - ApAp2 !y = ApAp2 $ liftA2 ( (-) @r ) x y + negate ( ApAp2 !x ) = ApAp2 $ fmap ( negate @r ) x + + -- DO NOT USE PURE!! + fromInteger !i = ApAp2 $ konst @k @( Dim u ) ( fromInteger @r i ) + +deriving newtype instance AbelianGroup r => AbelianGroup ( D𝔸0 r ) + +deriving via ApAp2 2 ( ℝ 1 ) r + instance AbelianGroup r => AbelianGroup ( D2𝔸1 r ) +deriving via ApAp2 3 ( ℝ 1 ) r + instance AbelianGroup r => AbelianGroup ( D3𝔸1 r ) +deriving via ApAp2 2 ( ℝ 2 ) r + instance AbelianGroup r => AbelianGroup ( D2𝔸2 r ) +deriving via ApAp2 3 ( ℝ 2 ) r + instance AbelianGroup r => AbelianGroup ( D3𝔸2 r ) +deriving via ApAp2 2 ( ℝ 3 ) r + instance AbelianGroup r => AbelianGroup ( D2𝔸3 r ) +deriving via ApAp2 3 ( ℝ 3 ) r + instance AbelianGroup r => AbelianGroup ( D3𝔸3 r ) +deriving via ApAp2 2 ( ℝ 4 ) r + instance AbelianGroup r => AbelianGroup ( D2𝔸4 r ) +deriving via ApAp2 3 ( ℝ 4 ) r + instance AbelianGroup r => AbelianGroup ( D3𝔸4 r ) + +-------------------------------------------------------------------------------- +-- Ring instances. + +d1pow :: Ring r => Word -> r -> D1𝔸1 r +d1pow i x = + let !j = fromInteger $ fromIntegral i + in D11 + ( x ^ i ) + ( T $ j * x ^ (i - 1) ) +{-# INLINE d1pow #-} +d2pow :: Ring r => Word -> r -> D2𝔸1 r +d2pow i x = + let !j = fromInteger $ fromIntegral i + in D21 + ( x ^ i ) + ( T $ j * x ^ (i - 1) ) + ( T $ j * (j - 1) * x ^ ( i - 2 ) ) +{-# INLINE d2pow #-} +d3pow :: Ring r => Word -> r -> D3𝔸1 r +d3pow i x = + let !j = fromInteger $ fromIntegral i + in D31 + ( x ^ i ) + ( T $ j * x ^ (i - 1) ) + ( T $ j * (j - 1) * x ^ ( i - 2 ) ) + ( T $ j * (j - 1) * (j - 2) * x ^ ( i - 3 ) ) +{-# INLINE d3pow #-} + +deriving newtype instance Ring r => Ring ( D𝔸0 r ) + +--instance Ring r => Ring ( D1𝔸1 r ) where +--instance Ring r => Ring ( D1𝔸2 r ) where +--instance Ring r => Ring ( D1𝔸3 r ) where +--instance Ring r => Ring ( D1𝔸4 r ) where + +instance Ring r => Ring ( D2𝔸1 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2pow @r i ( _D21_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D2𝔸1 Double ) #-} + {-# SPECIALISE instance Ring ( D2𝔸1 𝕀 ) #-} + +instance Ring r => Ring ( D2𝔸2 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2pow @r i ( _D22_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D2𝔸2 Double ) #-} + {-# SPECIALISE instance Ring ( D2𝔸2 𝕀 ) #-} + +instance Ring r => Ring ( D2𝔸3 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2pow @r i ( _D23_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D2𝔸3 Double ) #-} + {-# SPECIALISE instance Ring ( D2𝔸3 𝕀 ) #-} + +instance Ring r => Ring ( D2𝔸4 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2pow @r i ( _D24_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D2𝔸4 Double ) #-} + {-# SPECIALISE instance Ring ( D2𝔸4 𝕀 ) #-} + +instance Ring r => Ring ( D3𝔸1 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3pow @r i ( _D31_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D3𝔸1 Double ) #-} + {-# SPECIALISE instance Ring ( D3𝔸1 𝕀 ) #-} + +instance Ring r => Ring ( D3𝔸2 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3pow @r i ( _D32_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D3𝔸2 Double ) #-} + {-# SPECIALISE instance Ring ( D3𝔸2 𝕀 ) #-} + +instance Ring r => Ring ( D3𝔸3 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3pow @r i ( _D33_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D3𝔸3 Double ) #-} + {-# SPECIALISE instance Ring ( D3𝔸3 𝕀 ) #-} + +instance Ring r => Ring ( D3𝔸4 r ) where + !dr1 * !dr2 = + let + o :: r + !o = fromInteger 0 + p :: r -> r -> r + !p = (+) @r + m :: r -> r -> r + !m = (*) @r + in + $$( prodRuleQ + [|| o ||] [|| p ||] [|| m ||] + [|| dr1 ||] [|| dr2 ||] ) + df ^ i = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2pow @r i ( _D34_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Ring ( D3𝔸4 Double ) #-} + {-# SPECIALISE instance Ring ( D3𝔸4 𝕀 ) #-} + +-------------------------------------------------------------------------------- +-- Field, floating & transcendental instances + +d1recip :: Field r => r -> D1𝔸1 r +d1recip x = + let !d = recip x + in D11 d ( T $ -1 * d ^ 2 ) +{-# INLINE d1recip #-} +d2recip :: Field r => r -> D2𝔸1 r +d2recip x = + let !d = recip x + in D21 d ( T $ -1 * d ^ 2 ) ( T $ 2 * d ^ 3 ) +{-# INLINE d2recip #-} +d3recip :: Field r => r -> D3𝔸1 r +d3recip x = + let !d = recip x + in D31 d ( T $ -1 * d ^ 2 ) ( T $ 2 * d ^ 3 ) ( T $ -6 * d ^ 4 ) +{-# INLINE d3recip #-} + +deriving newtype instance Field r => Field ( D𝔸0 r ) + +--instance Field r => Field ( D1𝔸1 r ) where +--instance Field r => Field ( D1𝔸2 r ) where +--instance Field r => Field ( D1𝔸3 r ) where +--instance Field r => Field ( D1𝔸4 r ) where +instance Field r => Field ( D2𝔸1 r ) where + fromRational q = konst @2 @1 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2recip @r ( _D21_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D2𝔸1 Double ) #-} + {-# SPECIALISE instance Field ( D2𝔸1 𝕀 ) #-} +instance Field r => Field ( D2𝔸2 r ) where + fromRational q = konst @2 @2 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2recip @r ( _D22_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D2𝔸2 Double ) #-} + {-# SPECIALISE instance Field ( D2𝔸2 𝕀 ) #-} +instance Field r => Field ( D2𝔸3 r ) where + fromRational q = konst @2 @3 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2recip @r ( _D23_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D2𝔸3 Double ) #-} + {-# SPECIALISE instance Field ( D2𝔸3 𝕀 ) #-} +instance Field r => Field ( D2𝔸4 r ) where + fromRational q = konst @2 @4 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2recip @r ( _D24_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D2𝔸4 Double ) #-} + {-# SPECIALISE instance Field ( D2𝔸4 𝕀 ) #-} +instance Field r => Field ( D3𝔸1 r ) where + fromRational q = konst @3 @1 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3recip @r ( _D31_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D3𝔸1 Double ) #-} + {-# SPECIALISE instance Field ( D3𝔸1 𝕀 ) #-} +instance Field r => Field ( D3𝔸2 r ) where + fromRational q = konst @3 @2 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3recip @r ( _D32_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D3𝔸2 Double ) #-} + {-# SPECIALISE instance Field ( D3𝔸2 𝕀 ) #-} +instance Field r => Field ( D3𝔸3 r ) where + fromRational q = konst @3 @3 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3recip @r ( _D33_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D3𝔸3 Double ) #-} + {-# SPECIALISE instance Field ( D3𝔸3 𝕀 ) #-} +instance Field r => Field ( D3𝔸4 r ) where + fromRational q = konst @3 @4 ( fromRational q ) + recip df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3recip @r ( _D34_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Field ( D3𝔸4 Double ) #-} + {-# SPECIALISE instance Field ( D3𝔸4 𝕀 ) #-} + +d1sqrt :: Floating r => r -> D1𝔸1 r +d1sqrt x = + let !v = sqrt x + !d = recip v + in D11 v ( T $ 0.5 * d ) +{-# INLINE d1sqrt #-} +d2sqrt :: Floating r => r -> D2𝔸1 r +d2sqrt x = + let !v = sqrt x + !d = recip v + in D21 v ( T $ 0.5 * d ) ( T $ -0.25 * d ^ 2 ) +{-# INLINE d2sqrt #-} +d3sqrt :: Floating r => r -> D3𝔸1 r +d3sqrt x = + let !v = sqrt x + !d = recip v + in D31 v ( T $ 0.5 * d ) ( T $ -0.25 * d ^ 2 ) ( T $ 0.375 * d ^ 3 ) +{-# INLINE d3sqrt #-} + +deriving newtype instance Floating r => Floating ( D𝔸0 r ) +--instance Floating r => Floating ( D1𝔸1 r ) where +--instance Floating r => Floating ( D1𝔸2 r ) where +--instance Floating r => Floating ( D1𝔸3 r ) where +--instance Floating r => Floating ( D1𝔸4 r ) where +instance Floating r => Floating ( D2𝔸1 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sqrt @r ( _D21_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D2𝔸1 Double ) #-} + {-# SPECIALISE instance Floating ( D2𝔸1 𝕀 ) #-} +instance Floating r => Floating ( D2𝔸2 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sqrt @r ( _D22_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D2𝔸2 Double ) #-} + {-# SPECIALISE instance Floating ( D2𝔸2 𝕀 ) #-} +instance Floating r => Floating ( D2𝔸3 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sqrt @r ( _D23_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D2𝔸3 Double ) #-} + {-# SPECIALISE instance Floating ( D2𝔸3 𝕀 ) #-} +instance Floating r => Floating ( D2𝔸4 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sqrt @r ( _D24_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D2𝔸4 Double ) #-} + {-# SPECIALISE instance Floating ( D2𝔸4 𝕀 ) #-} +instance Floating r => Floating ( D3𝔸1 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sqrt @r ( _D31_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D3𝔸1 Double ) #-} + {-# SPECIALISE instance Floating ( D3𝔸1 𝕀 ) #-} +instance Floating r => Floating ( D3𝔸2 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sqrt @r ( _D32_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D3𝔸2 Double ) #-} + {-# SPECIALISE instance Floating ( D3𝔸2 𝕀 ) #-} +instance Floating r => Floating ( D3𝔸3 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sqrt @r ( _D33_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D3𝔸3 Double ) #-} + {-# SPECIALISE instance Floating ( D3𝔸3 𝕀 ) #-} +instance Floating r => Floating ( D3𝔸4 r ) where + sqrt df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sqrt @r ( _D34_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Floating ( D3𝔸4 Double ) #-} + {-# SPECIALISE instance Floating ( D3𝔸4 𝕀 ) #-} + +d1sin, d1cos, d1atan :: Transcendental r => r -> D1𝔸1 r +d1sin x = + let !s = sin x + !c = cos x + in D11 s ( T c ) +d1cos x = + let !s = sin x + !c = cos x + in D11 c ( T -s ) +d1atan x + = let !d = recip $ 1 + x ^ 2 + in D11 + ( atan x ) + ( T $ d ) +{-# INLINE d1sin #-} +{-# INLINE d1cos #-} +{-# INLINE d1atan #-} + +d2sin, d2cos, d2atan :: Transcendental r => r -> D2𝔸1 r +d2sin x = + let !s = sin x + !c = cos x + in D21 s ( T c ) ( T -s ) +d2cos x = + let !s = sin x + !c = cos x + in D21 c ( T -s ) ( T -c ) +d2atan x + = let !d = recip $ 1 + x ^ 2 + in D21 + ( atan x ) + ( T $ d ) + ( T $ -2 * x * d ^ 2 ) +{-# INLINE d2sin #-} +{-# INLINE d2cos #-} +{-# INLINE d2atan #-} + +d3sin, d3cos, d3atan :: Transcendental r => r -> D3𝔸1 r +d3sin x = + let !s = sin x + !c = cos x + in D31 s ( T c ) ( T -s ) ( T -c ) +d3cos x = + let !s = sin x + !c = cos x + in D31 c ( T -s ) ( T -c ) ( T s ) +d3atan x + = let !d = recip $ 1 + x ^ 2 + in D31 + ( atan x ) + ( T $ d ) + ( T $ -2 * x * d ^ 2 ) + ( T $ ( -2 + 6 * x ^ 2 ) * d ^ 3 ) +{-# INLINE d3sin #-} +{-# INLINE d3cos #-} +{-# INLINE d3atan #-} + +deriving newtype instance Transcendental r => Transcendental ( D𝔸0 r ) +--instance Transcendental r => Transcendental ( D1𝔸1 r ) where +--instance Transcendental r => Transcendental ( D1𝔸2 r ) where +--instance Transcendental r => Transcendental ( D1𝔸3 r ) where +--instance Transcendental r => Transcendental ( D1𝔸4 r ) where +instance Transcendental r => Transcendental ( D2𝔸1 r ) where + pi = konst @2 @1 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sin @r ( _D21_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2cos @r ( _D21_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2atan @r ( _D21_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D2𝔸1 Double ) #-} + {-# SPECIALISE instance Transcendental ( D2𝔸1 𝕀 ) #-} +instance Transcendental r => Transcendental ( D2𝔸2 r ) where + pi = konst @2 @2 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sin @r ( _D22_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2cos @r ( _D22_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2atan @r ( _D22_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D2𝔸2 Double ) #-} + {-# SPECIALISE instance Transcendental ( D2𝔸2 𝕀 ) #-} +instance Transcendental r => Transcendental ( D2𝔸3 r ) where + pi = konst @2 @3 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sin @r ( _D23_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2cos @r ( _D23_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2atan @r ( _D23_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D2𝔸3 Double ) #-} + {-# SPECIALISE instance Transcendental ( D2𝔸3 𝕀 ) #-} + +instance Transcendental r => Transcendental ( D2𝔸4 r ) where + pi = konst @2 @4 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2sin @r ( _D24_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2cos @r ( _D24_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d2atan @r ( _D24_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D2𝔸4 Double ) #-} + {-# SPECIALISE instance Transcendental ( D2𝔸4 𝕀 ) #-} + +instance Transcendental r => Transcendental ( D3𝔸1 r ) where + pi = konst @3 @1 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sin @r ( _D31_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3cos @r ( _D31_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3atan @r ( _D31_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D3𝔸1 Double ) #-} + {-# SPECIALISE instance Transcendental ( D3𝔸1 𝕀 ) #-} + +instance Transcendental r => Transcendental ( D3𝔸2 r ) where + pi = konst @3 @2 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sin @r ( _D32_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3cos @r ( _D32_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3atan @r ( _D32_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D3𝔸2 Double ) #-} + {-# SPECIALISE instance Transcendental ( D3𝔸2 𝕀 ) #-} + +instance Transcendental r => Transcendental ( D3𝔸3 r ) where + pi = konst @3 @3 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sin @r ( _D33_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3cos @r ( _D33_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3atan @r ( _D33_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D3𝔸3 Double ) #-} + {-# SPECIALISE instance Transcendental ( D3𝔸3 𝕀 ) #-} + +instance Transcendental r => Transcendental ( D3𝔸4 r ) where + pi = konst @3 @4 pi + sin df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3sin @r ( _D34_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + cos df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3cos @r ( _D34_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + atan df = + let + fromInt = fromInteger @r + add = (+) @r + times = (*) @r + pow = (^) @r + dg = d3atan @r ( _D34_v df ) + in $$( chainRuleN1Q [|| fromInt ||] [|| add ||] [|| times ||] [|| pow ||] + [|| df ||] [|| dg ||] ) + {-# SPECIALISE instance Transcendental ( D3𝔸4 Double ) #-} + {-# SPECIALISE instance Transcendental ( D3𝔸4 𝕀 ) #-} + +-------------------------------------------------------------------------------- +-- HasChainRule instances. + +instance Differential 2 0 where + konst w = D0 w + value ( D0 v ) = v + {-# INLINEABLE konst #-} + + {-# INLINEABLE value #-} +instance HasChainRule Double 2 ( ℝ 0 ) where + linearD f v = D0 ( f v ) + chain _ ( D0 gfx ) = D21 gfx origin origin + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 3 0 where + konst w = D0 w + + value ( D0 v ) = v + {-# INLINEABLE konst #-} + + {-# INLINEABLE value #-} +instance HasChainRule Double 3 ( ℝ 0 ) where + linearD f v = D0 ( f v ) + chain _ ( D0 gfx ) = D31 gfx origin origin origin + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 2 1 where + konst :: forall w. AbelianGroup w => w -> D2𝔸1 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 2 ( ℝ 1 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 1 -> w ) -> ℝ 1 -> D2𝔸1 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D2𝔸1 ( ℝ 1 ) -> D2𝔸1 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 3 1 where + konst :: forall w. AbelianGroup w => w -> D3𝔸1 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 3 ( ℝ 1 ) where + + chain :: forall w. Module Double ( T w ) => D3𝔸1 ( ℝ 1 ) -> D3𝔸1 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + linearD :: forall w. Module Double ( T w ) => ( ℝ 1 -> w ) -> ℝ 1 -> D3𝔸1 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 2 2 where + konst :: forall w. AbelianGroup w => w -> D2𝔸2 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 2 ( ℝ 2 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 2 -> w ) -> ℝ 2 -> D2𝔸2 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D2𝔸1 ( ℝ 2 ) -> D2𝔸2 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 3 2 where + konst :: forall w. AbelianGroup w => w -> D3𝔸2 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 3 ( ℝ 2 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 2 -> w ) -> ℝ 2 -> D3𝔸2 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D3𝔸1 ( ℝ 2 ) -> D3𝔸2 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 2 3 where + konst :: forall w. AbelianGroup w => w -> D2𝔸3 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 2 ( ℝ 3 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 3 -> w ) -> ℝ 3 -> D2𝔸3 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D2𝔸1 ( ℝ 3 ) -> D2𝔸3 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 3 3 where + konst :: forall w. AbelianGroup w => w -> D3𝔸3 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 3 ( ℝ 3 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 3 -> w ) -> ℝ 3 -> D3𝔸3 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D3𝔸1 ( ℝ 3 ) -> D3𝔸3 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE chain #-} + {-# INLINEABLE linearD #-} + +instance Differential 2 4 where + konst :: forall w. AbelianGroup w => w -> D2𝔸4 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} + +instance HasChainRule Double 2 ( ℝ 4 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 4 -> w ) -> ℝ 4 -> D2𝔸4 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D2𝔸1 ( ℝ 4 ) -> D2𝔸4 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-} + +instance Differential 3 4 where + konst :: forall w. AbelianGroup w => w -> D3𝔸4 w + konst w = + let !o = fromInteger @w 0 + in $$( monTabulateQ \ mon -> if isZeroMonomial mon then [|| w ||] else [|| o ||] ) + + value df = $$( monIndexQ [|| df ||] zeroMonomial ) + {-# INLINEABLE konst #-} + {-# INLINEABLE value #-} +instance HasChainRule Double 3 ( ℝ 4 ) where + linearD :: forall w. Module Double ( T w ) => ( ℝ 4 -> w ) -> ℝ 4 -> D3𝔸4 w + linearD f v = + let !o = origin @Double @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 1 ||] + | otherwise + -> [|| 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + chain :: forall w. Module Double ( T w ) => D3𝔸1 ( ℝ 4 ) -> D3𝔸4 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @Double @( T w ) + !p = (^+^) @Double @( T w ) + !s = (^*) @Double @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + {-# INLINEABLE linearD #-} + {-# INLINEABLE chain #-}
+ src/lib/Math/Algebra/Dual/Internal.hs view
@@ -0,0 +1,705 @@+ +{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE QuantifiedConstraints #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Algebra.Dual.Internal + ( D𝔸0(..) + , D1𝔸1(..), D2𝔸1(..), D3𝔸1(..) + , D1𝔸2(..), D2𝔸2(..), D3𝔸2(..) + , D1𝔸3(..), D2𝔸3(..), D3𝔸3(..) + , D1𝔸4(..), D2𝔸4(..), D3𝔸4(..) + + , chainRule1NQ, chainRuleN1Q + ) where + +-- base +import GHC.Generics + ( Generic, Generic1, Generically1(..) ) +import GHC.TypeNats + ( KnownNat ) + +-- deepseq +import Control.DeepSeq + ( NFData ) + +-- template-haskell +import Language.Haskell.TH + ( CodeQ ) +import Language.Haskell.TH.Syntax + ( Lift(..) ) + +-- brush-strokes +import Math.Linear + ( Vec(..), T(..) + , RepresentableQ(..), RepDim + , zipIndices + ) +import Math.Monomial + ( Mon(..), MonomialBasisQ(..), MonomialBasis(..) + , Vars, Deg + , mons, zeroMonomial, isZeroMonomial, totalDegree + , multiSubsetSumFaà, multiSubsetsSum + , partitionFaà, partitions + ) +import Math.Ring + ( Ring ) +import qualified Math.Ring as Ring +import TH.Utils + ( foldQ, powQ ) + +-------------------------------------------------------------------------------- + +-- | \( \mathbb{Z} \). +newtype D𝔸0 v = D0 { _D0_v :: v } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D𝔸0 + +-- | \( \mathbb{Z}[x]/(x)^2 \). +data D1𝔸1 v = + D11 { _D11_v :: v + , _D11_dx :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D1𝔸1 + +-- | \( \mathbb{Z}[x]/(x)^3 \). +data D2𝔸1 v = + D21 { _D21_v :: v + , _D21_dx :: ( T v ) + , _D21_dxdx :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D2𝔸1 + +-- | \( \mathbb{Z}[x]/(x)^4 \). +data D3𝔸1 v = + D31 { _D31_v :: v + , _D31_dx :: ( T v ) + , _D31_dxdx :: ( T v ) + , _D31_dxdxdx :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D3𝔸1 + +-- | \( \mathbb{Z}[x, y]/(x, y)^2 \). +data D1𝔸2 v = + D12 { _D12_v :: v + , _D12_dx, _D12_dy :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D1𝔸2 + +-- | \( \mathbb{Z}[x, y]/(x, y)^3 \). +data D2𝔸2 v = + D22 { _D22_v :: v + , _D22_dx, _D22_dy :: ( T v ) + , _D22_dxdx, _D22_dxdy, _D22_dydy :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D2𝔸2 + +-- | \( \mathbb{Z}[x, y]/(x, y)^4 \). +data D3𝔸2 v = + D32 { _D32_v :: v + , _D32_dx, _D32_dy :: ( T v ) + , _D32_dxdx, _D32_dxdy, _D32_dydy :: ( T v ) + , _D32_dxdxdx, _D32_dxdxdy, _D32_dxdydy, _D32_dydydy :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D3𝔸2 + +-- | \( \mathbb{Z}[x, y, z]/(x, y, z)^2 \). +data D1𝔸3 v = + D13 { _D13_v :: v + , _D13_dx, _D13_dy, _D13_dz :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D1𝔸3 + +-- | \( \mathbb{Z}[x, y, z]/(x, y, z)^3 \). +data D2𝔸3 v = + D23 { _D23_v :: v + , _D23_dx, _D23_dy, _D23_dz :: ( T v ) + , _D23_dxdx, _D23_dxdy, _D23_dydy, _D23_dxdz, _D23_dydz, _D23_dzdz :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D2𝔸3 + +-- | \( \mathbb{Z}[x, y, z]/(x, y, z)^4 \). +data D3𝔸3 v = + D33 { _D33_v :: v + , _D33_dx, _D33_dy, _D33_dz :: ( T v ) + , _D33_dxdx, _D33_dxdy, _D33_dydy, _D33_dxdz, _D33_dydz, _D33_dzdz :: ( T v ) + , _D33_dxdxdx, _D33_dxdxdy, _D33_dxdydy, _D33_dydydy + , _D33_dxdxdz, _D33_dxdydz, _D33_dxdzdz, _D33_dydydz, _D33_dydzdz, _D33_dzdzdz :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D3𝔸3 + +-- | \( \mathbb{Z}[x, y, z, w]/(x, y, z, w)^2 \). +data D1𝔸4 v = + D14 { _D14_v :: v + , _D14_dx, _D14_dy, _D14_dz, _D14_dw :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D1𝔸4 + +-- | \( \mathbb{Z}[x, y, z, w]/(x, y, z, w)^3 \). +data D2𝔸4 v = + D24 { _D24_v :: v + , _D24_dx, _D24_dy, _D24_dz, _D24_dw :: ( T v ) + , _D24_dxdx, _D24_dxdy, _D24_dydy, _D24_dxdz + , _D24_dydz, _D24_dzdz, _D24_dxdw, _D24_dydw, _D24_dzdw, _D24_dwdw :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D2𝔸4 + +-- | \( \mathbb{Z}[x, y, z, w]/(x, y, z, w)^4 \). +data D3𝔸4 v = + D34 { _D34_v :: v + , _D34_dx, _D34_dy, _D34_dz, _D34_dw :: ( T v ) + , _D34_dxdx, _D34_dxdy, _D34_dydy, _D34_dxdz, _D34_dydz, _D34_dzdz + , _D34_dxdw, _D34_dydw, _D34_dzdw, _D34_dwdw :: ( T v ) + , _D34_dxdxdx, _D34_dxdxdy, _D34_dxdydy, _D34_dydydy, + _D34_dxdxdz, _D34_dxdydz, _D34_dxdzdz, _D34_dydzdz, _D34_dydydz, _D34_dzdzdz, + _D34_dxdxdw, _D34_dxdydw, _D34_dydydw, _D34_dxdzdw, _D34_dydzdw, _D34_dzdzdw, + _D34_dxdwdw, _D34_dydwdw, _D34_dzdwdw, _D34_dwdwdw :: ( T v ) + } + deriving stock ( Show, Eq, Functor, Foldable, Traversable, Generic, Generic1 ) + deriving anyclass NFData + deriving Applicative + via Generically1 D3𝔸4 + +-------------------------------------------------------------------------------- + +-- | The chain rule for a composition \( \mathbb{R}^1 \to \mathbb{R}^n \to W \) +-- +-- (To be spliced in using Template Haskell.) +chainRule1NQ :: forall dr1 dv v r w d + . ( Ring r, RepresentableQ r v + , MonomialBasisQ dr1, Vars dr1 ~ 1 + , MonomialBasisQ dv , Vars dv ~ RepDim v + , Deg dr1 ~ Deg dv + , d ~ Vars dv, KnownNat d + ) + => CodeQ ( T w ) -- Module r ( T w ) + -> CodeQ ( T w -> T w -> T w ) -- + -> CodeQ ( T w -> r -> T w ) -- (circumvent TH constraint issue) + -> CodeQ ( dr1 v ) + -> CodeQ ( dv w ) + -> CodeQ ( dr1 w ) +chainRule1NQ zero_w sum_w scale_w code_df code_dg = + -- Share df/dg to avoid repeatedly inlining the same large piece of code. + [|| let { !df0 = $$code_df; !dg0 = $$code_dg } in $$( + let { df = [|| df0 ||]; dg = [|| dg0 ||] } in + monTabulateQ @dr1 \ mon -> + case totalDegree mon of + -- Set the value of the composition separately, + -- as that isn't handled by the Faà di Bruno formula. + 0 -> monIndexQ @dv dg zeroMonomial + k -> + -- TODO: explicitly share subexpressions, such as powers of monomials of df. + [|| unT $ + $$( foldQ sum_w zero_w + [ [|| $$scale_w ( T $$( monIndexQ @dv dg m_g ) ) + $$( foldQ [|| (Ring.+) ||] [|| Ring.fromInteger ( 0 :: Integer ) ||] + [ [|| Ring.fromInteger $$( liftTyped $ fromIntegral $ multiSubsetSumFaà k is ) Ring.* + $$( foldQ [|| (Ring.*) ||] [|| Ring.fromInteger ( 1 :: Integer ) ||] + [ foldQ [|| (Ring.*) ||] [|| Ring.fromInteger ( 1 :: Integer ) ||] + [ ( indexQ @r @v ( monIndexQ @dr1 df ( Mon $ Vec [ f_deg ] ) ) v_index ) + `powQ` pow + | ( f_deg, pow ) <- pows + ] + | ( v_index, pows ) <- zipIndices is + ] + ) ||] + | is <- mss + ] + ) + ||] + | m_g <- mons @_ @d k + , let mss = multiSubsetsSum [ 1 .. k ] k ( monDegs m_g ) + , not ( null mss ) -- avoid terms of the form x * 0 + ] + ) ||] + ) ||] + +-- | The chain rule for a composition \( \mathbb{R}^n \to \mathbb{R}^1 \to \mathbb{R}^1 \) +-- +-- (To be spliced in using Template Haskell.) +chainRuleN1Q :: forall du dr1 r + . ( MonomialBasisQ du + , MonomialBasisQ dr1, Vars dr1 ~ 1 + ) + => CodeQ ( Integer -> r ) -- ^ fromInteger (circumvent TH constraint issue) + -> CodeQ ( r -> r -> r ) -- ^ (+) (circumvent TH constraint issue) + -> CodeQ ( r -> r -> r ) -- ^ (*) (circumvent TH constraint issue) + -> CodeQ ( r -> Word -> r ) -- ^ (^) (circumvent TH constraint issue) + -> CodeQ ( du r ) + -> CodeQ ( dr1 r ) + -> CodeQ ( du r ) +chainRuleN1Q fromInt add times pow code_df code_dg = + -- Share df/dg to avoid repeatedly inlining the same large piece of code. + [|| let { !df0 = $$code_df; !dg0 = $$code_dg } in $$( + let { df = [|| df0 ||]; dg = [|| dg0 ||] } in + monTabulateQ @du \ mon -> + if + | isZeroMonomial mon + -- Set the value of the composition separately, + -- as that isn't handled by the Faà di Bruno formula. + -> monIndexQ @dr1 dg zeroMonomial + | otherwise + -> foldQ add [|| $$fromInt ( 0 :: Integer ) ||] + [ [|| $$times $$( monIndexQ @dr1 dg ( Mon ( Vec [ k ] ) ) ) + $$( foldQ add [|| $$fromInt ( 0 :: Integer ) ||] + [ [|| $$times ( $$fromInt $$( liftTyped $ fromIntegral $ partitionFaà mon is ) ) + $$( foldQ times [|| $$fromInt ( 1 :: Integer ) ||] + [ [|| $$pow $$( monIndexQ @du df f_mon ) p ||] + | ( f_mon, p ) <- is + ] + ) ||] + | is <- mss + ] + ) + ||] + | k <- [ 1 .. totalDegree mon ] + , let mss = partitions k mon + ] ) ||] + +-------------------------------------------------------------------------------- +-- MonomialBasisQ instances follow (nothing else). + +type instance Deg D𝔸0 = 0 +type instance Vars D𝔸0 = 0 +instance MonomialBasisQ D𝔸0 where + monTabulateQ f = + [|| let + _D0_v = $$( f $ Mon ( Vec [ ] ) ) + in D0 { .. } + ||] + + monIndexQ d _ = [|| _D0_v $$d ||] + +type instance Deg D1𝔸1 = 1 +type instance Vars D1𝔸1 = 1 +instance MonomialBasisQ D1𝔸1 where + monTabulateQ f = + [|| let + _D11_v = $$( f $ Mon ( Vec [ 0 ] ) ) + _D11_dx = T $$( f $ Mon ( Vec [ 1 ] ) ) + in D11 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1 ] ) -> [|| unT $ _D11_dx $$d ||] + _ -> [|| _D11_v $$d ||] + +instance MonomialBasis D1𝔸1 where + monTabulate f = + let + _D11_v = f $ Mon ( Vec [ 0 ] ) + _D11_dx = T $ f $ Mon ( Vec [ 1 ] ) + in D11 { .. } + {-# INLINE monTabulate #-} + + monIndex d = \ case + Mon ( Vec [ 1 ] ) -> unT $ _D11_dx d + _ -> _D11_v d + {-# INLINE monIndex #-} + +type instance Deg D2𝔸1 = 2 +type instance Vars D2𝔸1 = 1 +instance MonomialBasisQ D2𝔸1 where + monTabulateQ f = + [|| let + _D21_v = $$( f $ Mon ( Vec [ 0 ] ) ) + _D21_dx = T $$( f $ Mon ( Vec [ 1 ] ) ) + _D21_dxdx = T $$( f $ Mon ( Vec [ 2 ] ) ) + in D21 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1 ] ) -> [|| unT $ _D21_dx $$d ||] + Mon ( Vec [ 2 ] ) -> [|| unT $ _D21_dxdx $$d ||] + _ -> [|| _D21_v $$d ||] + +type instance Deg D3𝔸1 = 3 +type instance Vars D3𝔸1 = 1 +instance MonomialBasisQ D3𝔸1 where + monTabulateQ f = + [|| let + _D31_v = $$( f $ Mon ( Vec [ 0 ] ) ) + _D31_dx = T $$( f $ Mon ( Vec [ 1 ] ) ) + _D31_dxdx = T $$( f $ Mon ( Vec [ 2 ] ) ) + _D31_dxdxdx = T $$( f $ Mon ( Vec [ 3 ] ) ) + in D31 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1 ] ) -> [|| unT $ _D31_dx $$d ||] + Mon ( Vec [ 2 ] ) -> [|| unT $ _D31_dxdx $$d ||] + Mon ( Vec [ 3 ] ) -> [|| unT $ _D31_dxdxdx $$d ||] + _ -> [|| _D31_v $$d ||] + +type instance Deg D1𝔸2 = 1 +type instance Vars D1𝔸2 = 2 +instance MonomialBasisQ D1𝔸2 where + monTabulateQ f = + [|| let + _D12_v = $$( f $ Mon ( Vec [ 0, 0 ] ) ) + _D12_dx = T $$( f $ Mon ( Vec [ 1, 0 ] ) ) + _D12_dy = T $$( f $ Mon ( Vec [ 0, 1 ] ) ) + in D12 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0 ] ) -> [|| unT $ _D12_dx $$d ||] + Mon ( Vec [ 0, 1 ] ) -> [|| unT $ _D12_dy $$d ||] + _ -> [|| _D12_v $$d ||] + + +instance MonomialBasis D1𝔸2 where + monTabulate f = + let + _D12_v = f $ Mon ( Vec [ 0, 0 ] ) + _D12_dx = T $ f $ Mon ( Vec [ 1, 0 ] ) + _D12_dy = T $ f $ Mon ( Vec [ 0, 1 ] ) + in D12 { .. } + {-# INLINE monTabulate #-} + + monIndex d = \ case + Mon ( Vec [ 1, 0 ] ) -> unT $ _D12_dx d + Mon ( Vec [ 0, 1 ] ) -> unT $ _D12_dy d + _ -> _D12_v d + {-# INLINE monIndex #-} + +type instance Deg D2𝔸2 = 2 +type instance Vars D2𝔸2 = 2 +instance MonomialBasisQ D2𝔸2 where + monTabulateQ f = + [|| let + _D22_v = $$( f $ Mon ( Vec [ 0, 0 ] ) ) + _D22_dx = T $$( f $ Mon ( Vec [ 1, 0 ] ) ) + _D22_dy = T $$( f $ Mon ( Vec [ 0, 1 ] ) ) + _D22_dxdx = T $$( f $ Mon ( Vec [ 2, 0 ] ) ) + _D22_dxdy = T $$( f $ Mon ( Vec [ 1, 1 ] ) ) + _D22_dydy = T $$( f $ Mon ( Vec [ 0, 2 ] ) ) + in D22 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0 ] ) -> [|| unT $ _D22_dx $$d ||] + Mon ( Vec [ 0, 1 ] ) -> [|| unT $ _D22_dy $$d ||] + Mon ( Vec [ 2, 0 ] ) -> [|| unT $ _D22_dxdx $$d ||] + Mon ( Vec [ 1, 1 ] ) -> [|| unT $ _D22_dxdy $$d ||] + Mon ( Vec [ 0, 2 ] ) -> [|| unT $ _D22_dydy $$d ||] + _ -> [|| _D22_v $$d ||] + +type instance Deg D3𝔸2 = 3 +type instance Vars D3𝔸2 = 2 +instance MonomialBasisQ D3𝔸2 where + monTabulateQ f = + [|| let + _D32_v = $$( f $ Mon ( Vec [ 0, 0 ] ) ) + _D32_dx = T $$( f $ Mon ( Vec [ 1, 0 ] ) ) + _D32_dy = T $$( f $ Mon ( Vec [ 0, 1 ] ) ) + _D32_dxdx = T $$( f $ Mon ( Vec [ 2, 0 ] ) ) + _D32_dxdy = T $$( f $ Mon ( Vec [ 1, 1 ] ) ) + _D32_dydy = T $$( f $ Mon ( Vec [ 0, 2 ] ) ) + _D32_dxdxdx = T $$( f $ Mon ( Vec [ 3, 0 ] ) ) + _D32_dxdxdy = T $$( f $ Mon ( Vec [ 2, 1 ] ) ) + _D32_dxdydy = T $$( f $ Mon ( Vec [ 1, 2 ] ) ) + _D32_dydydy = T $$( f $ Mon ( Vec [ 0, 3 ] ) ) + in D32 { .. } ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0 ] ) -> [|| unT $ _D32_dx $$d ||] + Mon ( Vec [ 0, 1 ] ) -> [|| unT $ _D32_dy $$d ||] + Mon ( Vec [ 2, 0 ] ) -> [|| unT $ _D32_dxdx $$d ||] + Mon ( Vec [ 1, 1 ] ) -> [|| unT $ _D32_dxdy $$d ||] + Mon ( Vec [ 0, 2 ] ) -> [|| unT $ _D32_dydy $$d ||] + Mon ( Vec [ 3, 0 ] ) -> [|| unT $ _D32_dxdxdx $$d ||] + Mon ( Vec [ 2, 1 ] ) -> [|| unT $ _D32_dxdxdy $$d ||] + Mon ( Vec [ 1, 2 ] ) -> [|| unT $ _D32_dxdydy $$d ||] + Mon ( Vec [ 0, 3 ] ) -> [|| unT $ _D32_dydydy $$d ||] + _ -> [|| _D32_v $$d ||] + +type instance Deg D1𝔸3 = 1 +type instance Vars D1𝔸3 = 3 +instance MonomialBasisQ D1𝔸3 where + monTabulateQ f = + [|| let + !_D13_v = $$( f ( Mon ( Vec [ 0, 0, 0 ] ) ) ) + !_D13_dx = T $$( f ( Mon ( Vec [ 1, 0, 0 ] ) ) ) + !_D13_dy = T $$( f ( Mon ( Vec [ 0, 1, 0 ] ) ) ) + !_D13_dz = T $$( f ( Mon ( Vec [ 0, 0, 1 ] ) ) ) + in D13 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0, 0 ] ) -> [|| unT $ _D13_dx $$d ||] + Mon ( Vec [ 0, 1, 0 ] ) -> [|| unT $ _D13_dy $$d ||] + Mon ( Vec [ 0, 0, 1 ] ) -> [|| unT $ _D13_dz $$d ||] + _ -> [|| _D13_v $$d ||] + +instance MonomialBasis D1𝔸3 where + monTabulate f = + let + !_D13_v = f ( Mon ( Vec [ 0, 0, 0 ] ) ) + !_D13_dx = T $ f ( Mon ( Vec [ 1, 0, 0 ] ) ) + !_D13_dy = T $ f ( Mon ( Vec [ 0, 1, 0 ] ) ) + !_D13_dz = T $ f ( Mon ( Vec [ 0, 0, 1 ] ) ) + in D13 { .. } + {-# INLINE monTabulate #-} + + monIndex d = \ case + Mon ( Vec [ 1, 0, 0 ] ) -> unT $ _D13_dx d + Mon ( Vec [ 0, 1, 0 ] ) -> unT $ _D13_dy d + Mon ( Vec [ 0, 0, 1 ] ) -> unT $ _D13_dz d + _ -> _D13_v d + {-# INLINE monIndex #-} + +type instance Deg D2𝔸3 = 2 +type instance Vars D2𝔸3 = 3 +instance MonomialBasisQ D2𝔸3 where + monTabulateQ f = + [|| let + _D23_v = $$( f $ Mon ( Vec [ 0, 0, 0 ] ) ) + _D23_dx = T $$( f $ Mon ( Vec [ 1, 0, 0 ] ) ) + _D23_dy = T $$( f $ Mon ( Vec [ 0, 1, 0 ] ) ) + _D23_dz = T $$( f $ Mon ( Vec [ 0, 0, 1 ] ) ) + _D23_dxdx = T $$( f $ Mon ( Vec [ 2, 0, 0 ] ) ) + _D23_dxdy = T $$( f $ Mon ( Vec [ 1, 1, 0 ] ) ) + _D23_dydy = T $$( f $ Mon ( Vec [ 0, 2, 0 ] ) ) + _D23_dxdz = T $$( f $ Mon ( Vec [ 1, 0, 1 ] ) ) + _D23_dydz = T $$( f $ Mon ( Vec [ 0, 1, 1 ] ) ) + _D23_dzdz = T $$( f $ Mon ( Vec [ 0, 0, 2 ] ) ) + in D23 { .. } + ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0, 0 ] ) -> [|| unT $ _D23_dx $$d ||] + Mon ( Vec [ 0, 1, 0 ] ) -> [|| unT $ _D23_dy $$d ||] + Mon ( Vec [ 0, 0, 1 ] ) -> [|| unT $ _D23_dz $$d ||] + Mon ( Vec [ 2, 0, 0 ] ) -> [|| unT $ _D23_dxdx $$d ||] + Mon ( Vec [ 1, 1, 0 ] ) -> [|| unT $ _D23_dxdy $$d ||] + Mon ( Vec [ 0, 2, 0 ] ) -> [|| unT $ _D23_dydy $$d ||] + Mon ( Vec [ 1, 0, 1 ] ) -> [|| unT $ _D23_dxdz $$d ||] + Mon ( Vec [ 0, 1, 1 ] ) -> [|| unT $ _D23_dydz $$d ||] + Mon ( Vec [ 0, 0, 2 ] ) -> [|| unT $ _D23_dzdz $$d ||] + _ -> [|| _D23_v $$d ||] + + +type instance Deg D3𝔸3 = 3 +type instance Vars D3𝔸3 = 3 +instance MonomialBasisQ D3𝔸3 where + monTabulateQ f = + [|| let + _D33_v = $$( f $ Mon ( Vec [ 0, 0, 0 ] ) ) + _D33_dx = T $$( f $ Mon ( Vec [ 1, 0, 0 ] ) ) + _D33_dy = T $$( f $ Mon ( Vec [ 0, 1, 0 ] ) ) + _D33_dz = T $$( f $ Mon ( Vec [ 0, 0, 1 ] ) ) + _D33_dxdx = T $$( f $ Mon ( Vec [ 2, 0, 0 ] ) ) + _D33_dxdy = T $$( f $ Mon ( Vec [ 1, 1, 0 ] ) ) + _D33_dydy = T $$( f $ Mon ( Vec [ 0, 2, 0 ] ) ) + _D33_dxdz = T $$( f $ Mon ( Vec [ 1, 0, 1 ] ) ) + _D33_dydz = T $$( f $ Mon ( Vec [ 0, 1, 1 ] ) ) + _D33_dzdz = T $$( f $ Mon ( Vec [ 0, 0, 2 ] ) ) + _D33_dxdxdx = T $$( f $ Mon ( Vec [ 3, 0, 0 ] ) ) + _D33_dxdxdy = T $$( f $ Mon ( Vec [ 2, 1, 0 ] ) ) + _D33_dxdydy = T $$( f $ Mon ( Vec [ 1, 2, 0 ] ) ) + _D33_dydydy = T $$( f $ Mon ( Vec [ 0, 3, 0 ] ) ) + _D33_dxdxdz = T $$( f $ Mon ( Vec [ 2, 0, 1 ] ) ) + _D33_dxdydz = T $$( f $ Mon ( Vec [ 1, 1, 1 ] ) ) + _D33_dxdzdz = T $$( f $ Mon ( Vec [ 1, 0, 2 ] ) ) + _D33_dydydz = T $$( f $ Mon ( Vec [ 0, 2, 1 ] ) ) + _D33_dydzdz = T $$( f $ Mon ( Vec [ 0, 1, 2 ] ) ) + _D33_dzdzdz = T $$( f $ Mon ( Vec [ 0, 0, 3 ] ) ) + in D33 { .. } ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0, 0 ] ) -> [|| unT $ _D33_dx $$d ||] + Mon ( Vec [ 0, 1, 0 ] ) -> [|| unT $ _D33_dy $$d ||] + Mon ( Vec [ 0, 0, 1 ] ) -> [|| unT $ _D33_dz $$d ||] + Mon ( Vec [ 2, 0, 0 ] ) -> [|| unT $ _D33_dxdx $$d ||] + Mon ( Vec [ 1, 1, 0 ] ) -> [|| unT $ _D33_dxdy $$d ||] + Mon ( Vec [ 0, 2, 0 ] ) -> [|| unT $ _D33_dydy $$d ||] + Mon ( Vec [ 1, 0, 1 ] ) -> [|| unT $ _D33_dxdz $$d ||] + Mon ( Vec [ 0, 1, 1 ] ) -> [|| unT $ _D33_dydz $$d ||] + Mon ( Vec [ 0, 0, 2 ] ) -> [|| unT $ _D33_dzdz $$d ||] + Mon ( Vec [ 3, 0, 0 ] ) -> [|| unT $ _D33_dxdxdx $$d ||] + Mon ( Vec [ 2, 1, 0 ] ) -> [|| unT $ _D33_dxdxdy $$d ||] + Mon ( Vec [ 1, 2, 0 ] ) -> [|| unT $ _D33_dxdydy $$d ||] + Mon ( Vec [ 0, 3, 0 ] ) -> [|| unT $ _D33_dydydy $$d ||] + Mon ( Vec [ 2, 0, 1 ] ) -> [|| unT $ _D33_dxdxdz $$d ||] + Mon ( Vec [ 1, 1, 1 ] ) -> [|| unT $ _D33_dxdydz $$d ||] + Mon ( Vec [ 1, 0, 2 ] ) -> [|| unT $ _D33_dxdzdz $$d ||] + Mon ( Vec [ 0, 2, 1 ] ) -> [|| unT $ _D33_dydydz $$d ||] + Mon ( Vec [ 0, 1, 2 ] ) -> [|| unT $ _D33_dydzdz $$d ||] + Mon ( Vec [ 0, 0, 3 ] ) -> [|| unT $ _D33_dzdzdz $$d ||] + _ -> [|| _D33_v $$d ||] + +type instance Deg D1𝔸4 = 1 +type instance Vars D1𝔸4 = 4 +instance MonomialBasisQ D1𝔸4 where + monTabulateQ f = + [|| let + _D14_v = $$( f $ Mon ( Vec [ 0, 0, 0, 0 ] ) ) + _D14_dx = T $$( f $ Mon ( Vec [ 1, 0, 0, 0 ] ) ) + _D14_dy = T $$( f $ Mon ( Vec [ 0, 1, 0, 0 ] ) ) + _D14_dz = T $$( f $ Mon ( Vec [ 0, 0, 1, 0 ] ) ) + _D14_dw = T $$( f $ Mon ( Vec [ 0, 0, 0, 1 ] ) ) + in D14 { .. } ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0, 0, 0 ] ) -> [|| unT $ _D14_dx $$d ||] + Mon ( Vec [ 0, 1, 0, 0 ] ) -> [|| unT $ _D14_dy $$d ||] + Mon ( Vec [ 0, 0, 1, 0 ] ) -> [|| unT $ _D14_dz $$d ||] + Mon ( Vec [ 0, 0, 0, 1 ] ) -> [|| unT $ _D14_dw $$d ||] + _ -> [|| _D14_v $$d ||] + +type instance Deg D2𝔸4 = 2 +type instance Vars D2𝔸4 = 4 +instance MonomialBasisQ D2𝔸4 where + monTabulateQ f = + [|| let + _D24_v = $$( f $ Mon ( Vec [ 0, 0, 0, 0 ] ) ) + _D24_dx = T $$( f $ Mon ( Vec [ 1, 0, 0, 0 ] ) ) + _D24_dy = T $$( f $ Mon ( Vec [ 0, 1, 0, 0 ] ) ) + _D24_dz = T $$( f $ Mon ( Vec [ 0, 0, 1, 0 ] ) ) + _D24_dw = T $$( f $ Mon ( Vec [ 0, 0, 0, 1 ] ) ) + _D24_dxdx = T $$( f $ Mon ( Vec [ 2, 0, 0, 0 ] ) ) + _D24_dxdy = T $$( f $ Mon ( Vec [ 1, 1, 0, 0 ] ) ) + _D24_dydy = T $$( f $ Mon ( Vec [ 0, 2, 0, 0 ] ) ) + _D24_dxdz = T $$( f $ Mon ( Vec [ 1, 0, 1, 0 ] ) ) + _D24_dydz = T $$( f $ Mon ( Vec [ 0, 1, 1, 0 ] ) ) + _D24_dzdz = T $$( f $ Mon ( Vec [ 0, 0, 2, 0 ] ) ) + _D24_dxdw = T $$( f $ Mon ( Vec [ 1, 0, 0, 1 ] ) ) + _D24_dydw = T $$( f $ Mon ( Vec [ 0, 1, 0, 1 ] ) ) + _D24_dzdw = T $$( f $ Mon ( Vec [ 0, 0, 1, 1 ] ) ) + _D24_dwdw = T $$( f $ Mon ( Vec [ 0, 0, 0, 2 ] ) ) + in D24 { .. } ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0, 0, 0 ] ) -> [|| unT $ _D24_dx $$d ||] + Mon ( Vec [ 0, 1, 0, 0 ] ) -> [|| unT $ _D24_dy $$d ||] + Mon ( Vec [ 0, 0, 1, 0 ] ) -> [|| unT $ _D24_dz $$d ||] + Mon ( Vec [ 0, 0, 0, 1 ] ) -> [|| unT $ _D24_dw $$d ||] + Mon ( Vec [ 2, 0, 0, 0 ] ) -> [|| unT $ _D24_dxdx $$d ||] + Mon ( Vec [ 1, 1, 0, 0 ] ) -> [|| unT $ _D24_dxdy $$d ||] + Mon ( Vec [ 0, 2, 0, 0 ] ) -> [|| unT $ _D24_dydy $$d ||] + Mon ( Vec [ 1, 0, 1, 0 ] ) -> [|| unT $ _D24_dxdz $$d ||] + Mon ( Vec [ 0, 1, 1, 0 ] ) -> [|| unT $ _D24_dydz $$d ||] + Mon ( Vec [ 0, 0, 2, 0 ] ) -> [|| unT $ _D24_dzdz $$d ||] + Mon ( Vec [ 1, 0, 0, 1 ] ) -> [|| unT $ _D24_dxdw $$d ||] + Mon ( Vec [ 0, 1, 0, 1 ] ) -> [|| unT $ _D24_dydw $$d ||] + Mon ( Vec [ 0, 0, 1, 1 ] ) -> [|| unT $ _D24_dzdw $$d ||] + Mon ( Vec [ 0, 0, 0, 2 ] ) -> [|| unT $ _D24_dwdw $$d ||] + _ -> [|| _D24_v $$d ||] + + +type instance Deg D3𝔸4 = 3 +type instance Vars D3𝔸4 = 4 +instance MonomialBasisQ D3𝔸4 where + monTabulateQ f = + [|| let + _D34_v = $$( f $ Mon ( Vec [ 0, 0, 0, 0 ] ) ) + _D34_dx = T $$( f $ Mon ( Vec [ 1, 0, 0, 0 ] ) ) + _D34_dy = T $$( f $ Mon ( Vec [ 0, 1, 0, 0 ] ) ) + _D34_dz = T $$( f $ Mon ( Vec [ 0, 0, 1, 0 ] ) ) + _D34_dw = T $$( f $ Mon ( Vec [ 0, 0, 0, 1 ] ) ) + _D34_dxdx = T $$( f $ Mon ( Vec [ 2, 0, 0, 0 ] ) ) + _D34_dxdy = T $$( f $ Mon ( Vec [ 1, 1, 0, 0 ] ) ) + _D34_dydy = T $$( f $ Mon ( Vec [ 0, 2, 0, 0 ] ) ) + _D34_dxdz = T $$( f $ Mon ( Vec [ 1, 0, 1, 0 ] ) ) + _D34_dydz = T $$( f $ Mon ( Vec [ 0, 1, 1, 0 ] ) ) + _D34_dzdz = T $$( f $ Mon ( Vec [ 0, 0, 2, 0 ] ) ) + _D34_dxdw = T $$( f $ Mon ( Vec [ 1, 0, 0, 1 ] ) ) + _D34_dydw = T $$( f $ Mon ( Vec [ 0, 1, 0, 1 ] ) ) + _D34_dzdw = T $$( f $ Mon ( Vec [ 0, 0, 1, 1 ] ) ) + _D34_dwdw = T $$( f $ Mon ( Vec [ 0, 0, 0, 2 ] ) ) + _D34_dxdxdx = T $$( f $ Mon ( Vec [ 3, 0, 0, 0 ] ) ) + _D34_dxdxdy = T $$( f $ Mon ( Vec [ 2, 1, 0, 0 ] ) ) + _D34_dxdydy = T $$( f $ Mon ( Vec [ 1, 2, 0, 0 ] ) ) + _D34_dydydy = T $$( f $ Mon ( Vec [ 0, 3, 0, 0 ] ) ) + _D34_dxdxdz = T $$( f $ Mon ( Vec [ 2, 0, 1, 0 ] ) ) + _D34_dxdydz = T $$( f $ Mon ( Vec [ 1, 1, 1, 0 ] ) ) + _D34_dxdzdz = T $$( f $ Mon ( Vec [ 1, 0, 2, 0 ] ) ) + _D34_dydydz = T $$( f $ Mon ( Vec [ 0, 2, 1, 0 ] ) ) + _D34_dydzdz = T $$( f $ Mon ( Vec [ 0, 1, 2, 0 ] ) ) + _D34_dzdzdz = T $$( f $ Mon ( Vec [ 0, 0, 3, 0 ] ) ) + _D34_dxdxdw = T $$( f $ Mon ( Vec [ 2, 0, 0, 1 ] ) ) + _D34_dxdydw = T $$( f $ Mon ( Vec [ 1, 1, 0, 1 ] ) ) + _D34_dydydw = T $$( f $ Mon ( Vec [ 0, 2, 0, 1 ] ) ) + _D34_dxdzdw = T $$( f $ Mon ( Vec [ 1, 0, 1, 1 ] ) ) + _D34_dydzdw = T $$( f $ Mon ( Vec [ 0, 1, 1, 1 ] ) ) + _D34_dzdzdw = T $$( f $ Mon ( Vec [ 0, 0, 2, 1 ] ) ) + _D34_dxdwdw = T $$( f $ Mon ( Vec [ 1, 0, 0, 2 ] ) ) + _D34_dydwdw = T $$( f $ Mon ( Vec [ 0, 1, 0, 2 ] ) ) + _D34_dzdwdw = T $$( f $ Mon ( Vec [ 0, 0, 1, 2 ] ) ) + _D34_dwdwdw = T $$( f $ Mon ( Vec [ 0, 0, 0, 3 ] ) ) + in D34 { .. } ||] + + monIndexQ d = \ case + Mon ( Vec [ 1, 0, 0, 0 ] ) -> [|| unT $ _D34_dx $$d ||] + Mon ( Vec [ 0, 1, 0, 0 ] ) -> [|| unT $ _D34_dy $$d ||] + Mon ( Vec [ 0, 0, 1, 0 ] ) -> [|| unT $ _D34_dz $$d ||] + Mon ( Vec [ 0, 0, 0, 1 ] ) -> [|| unT $ _D34_dw $$d ||] + Mon ( Vec [ 2, 0, 0, 0 ] ) -> [|| unT $ _D34_dxdx $$d ||] + Mon ( Vec [ 1, 1, 0, 0 ] ) -> [|| unT $ _D34_dxdy $$d ||] + Mon ( Vec [ 0, 2, 0, 0 ] ) -> [|| unT $ _D34_dydy $$d ||] + Mon ( Vec [ 1, 0, 1, 0 ] ) -> [|| unT $ _D34_dxdz $$d ||] + Mon ( Vec [ 0, 1, 1, 0 ] ) -> [|| unT $ _D34_dydz $$d ||] + Mon ( Vec [ 0, 0, 2, 0 ] ) -> [|| unT $ _D34_dzdz $$d ||] + Mon ( Vec [ 1, 0, 0, 1 ] ) -> [|| unT $ _D34_dxdw $$d ||] + Mon ( Vec [ 0, 1, 0, 1 ] ) -> [|| unT $ _D34_dydw $$d ||] + Mon ( Vec [ 0, 0, 1, 1 ] ) -> [|| unT $ _D34_dzdw $$d ||] + Mon ( Vec [ 0, 0, 0, 2 ] ) -> [|| unT $ _D34_dwdw $$d ||] + Mon ( Vec [ 3, 0, 0, 0 ] ) -> [|| unT $ _D34_dxdxdx $$d ||] + Mon ( Vec [ 2, 1, 0, 0 ] ) -> [|| unT $ _D34_dxdxdy $$d ||] + Mon ( Vec [ 1, 2, 0, 0 ] ) -> [|| unT $ _D34_dxdydy $$d ||] + Mon ( Vec [ 0, 3, 0, 0 ] ) -> [|| unT $ _D34_dydydy $$d ||] + Mon ( Vec [ 2, 0, 1, 0 ] ) -> [|| unT $ _D34_dxdxdz $$d ||] + Mon ( Vec [ 1, 1, 1, 0 ] ) -> [|| unT $ _D34_dxdydz $$d ||] + Mon ( Vec [ 1, 0, 2, 0 ] ) -> [|| unT $ _D34_dxdzdz $$d ||] + Mon ( Vec [ 0, 2, 1, 0 ] ) -> [|| unT $ _D34_dydydz $$d ||] + Mon ( Vec [ 0, 1, 2, 0 ] ) -> [|| unT $ _D34_dydzdz $$d ||] + Mon ( Vec [ 0, 0, 3, 0 ] ) -> [|| unT $ _D34_dzdzdz $$d ||] + Mon ( Vec [ 2, 0, 0, 1 ] ) -> [|| unT $ _D34_dxdxdw $$d ||] + Mon ( Vec [ 1, 1, 0, 1 ] ) -> [|| unT $ _D34_dxdydw $$d ||] + Mon ( Vec [ 0, 2, 0, 1 ] ) -> [|| unT $ _D34_dydydw $$d ||] + Mon ( Vec [ 1, 0, 1, 1 ] ) -> [|| unT $ _D34_dxdzdw $$d ||] + Mon ( Vec [ 0, 1, 1, 1 ] ) -> [|| unT $ _D34_dydzdw $$d ||] + Mon ( Vec [ 0, 0, 2, 1 ] ) -> [|| unT $ _D34_dzdzdw $$d ||] + Mon ( Vec [ 1, 0, 0, 2 ] ) -> [|| unT $ _D34_dxdwdw $$d ||] + Mon ( Vec [ 0, 1, 0, 2 ] ) -> [|| unT $ _D34_dydwdw $$d ||] + Mon ( Vec [ 0, 0, 1, 2 ] ) -> [|| unT $ _D34_dzdwdw $$d ||] + Mon ( Vec [ 0, 0, 0, 3 ] ) -> [|| unT $ _D34_dwdwdw $$d ||] + _ -> [|| _D34_v $$d ||]
+ src/lib/Math/Bezier/ArcLength.hs view
@@ -0,0 +1,739 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE BangPatterns #-} +{-# LANGUAGE DuplicateRecordFields #-} +{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Bezier.ArcLength + ( -- * Arc-length reparametrisation + ArcLengthParametrisation(..) + , curveArcLengthParametrisation + + -- ** Options + , ArcLengthOptions(..), defaultArcLengthOptions + , Integrator(..) + , Regulariser(..), defaultRegulariser + , RegularisationMethod(..) + , NewtonRaphsonOptions(..), defaultNewtonRaphsonOptions + + -- * Spline arc-length parametrisation + , SplineArcLengthParam(..) + , splineArcLengthParam + , splineTotalArcLength + , splineParameterAtArcLength + , splitSplineAtArcLengths + ) + where + +-- base +import Data.Complex + ( realPart ) +import Data.Maybe + ( fromMaybe ) +import GHC.Exts + ( fromList, proxy# ) +import GHC.TypeNats + ( KnownNat, natVal', SomeNat (..), someNatVal ) + +-- acts +import Data.Act + ( Torsor (..) ) + +-- containers +import Data.Sequence + ( Seq ) +import qualified Data.Sequence as Seq + ( Seq(..) ) + +-- primitive +import Data.Primitive.PrimArray + ( PrimArray ) +import qualified Data.Primitive.PrimArray as Prim + ( indexPrimArray ) + +-- brush-strokes +import qualified Math.Bezier.Cubic as Cubic + ( Bezier(..), bezier', speedCriticalPoints, subdivide ) +import qualified Math.Bezier.Quadratic as Quadratic + ( Bezier(..), bezier', speedCriticalPoints, subdivide ) +import Math.Bezier.Spline + ( Curve(..), Curves(..), NextPoint(..), Spline(..), SplineType(..) + , SplineTypeI + , adjustSplineType, openCurveEnd, subdivideSplineAt + ) +import Math.Epsilon + ( epsilon ) +import Math.Module + ( Module, Inner, norm ) +import Math.Roots + ( newtonRaphson ) + +-------------------------------------------------------------------------------- +-- Curve arc-length + +data ArcLengthOptions + = ArcLengthOptions + { arcLengthIntegrator :: !Integrator + -- ^ Quadrature method used for arc-length integration. + , splitAtCriticalPoints :: !Bool + -- ^ Whether to split at (near) critical points. + -- (A "near" critical point is the real part of a possibly complex critical point.) + -- + -- (Ignored for the Gravesen method.) + , newtonRaphsonOptions :: !NewtonRaphsonOptions + -- ^ Options for the Newton–Raphson method used to compute the + -- inverse of the arc-length function. + } + +-- | Method used to regularise the integrand near a low-speed endpoint. +data RegularisationMethod + = SquareRoot + -- ^ Substitute \( t = u^2 \) near the endpoint, so that the integrand + -- \( \|B'(t)\| \sim \sqrt{t} \) becomes smooth in \( u \). + -- (No other methods yet; more may be added in the future.) + +-- | Cusp-endpoint regularisation settings. +data Regulariser + = Regulariser + { regularisationMethod :: !RegularisationMethod + , regularisationThreshold :: !Double + -- ^ Speed threshold below which the substitution is applied at an endpoint. + } + +defaultRegulariser :: Regulariser +defaultRegulariser = Regulariser + { regularisationMethod = SquareRoot + , regularisationThreshold = 0.5 + } + +data Integrator + = ClenshawCurtis + { clenshawCurtisDegree :: !Int -- ^ degree (must be even) + , regulariser :: !( Maybe Regulariser ) + -- ^ Regularisation at cuspidal endpoints? + } + | TanhSinh + { tanhSinhNodes :: !Int + -- ^ @N@: nodes per side (2N+1 nodes total). + -- + -- Recommended value: such that @N*h ≥ 3.2@. + , tanhSinhStepSize :: !Double + -- ^ @h@: step size. + -- + -- Recommended value: between @0.5@ (fast) and @0.1@ (precise). + } + | GaussLegendre + { gaussLegendreDegree :: !Int + -- ^ Number of quadrature nodes. + -- + -- An @n@-point rule is exact for polynomials of degree up to @2n−1@. + , regulariser :: !( Maybe Regulariser ) + -- ^ Cusp-endpoint regularisation (@Nothing@ disables it). + } + | Gravesen + { gravesenTol :: !Double + -- ^ Stopping tolerance on the polygon-length minus chord-length gap + -- for each piece. + -- The tolerance is halved at each subdivision level, so the total + -- arc-length error is bounded by @gravesenTol@. + } + +defaultArcLengthOptions :: ArcLengthOptions +defaultArcLengthOptions = + ArcLengthOptions + { arcLengthIntegrator = ClenshawCurtis + { clenshawCurtisDegree = 16 + , regulariser = Just defaultRegulariser + } + , splitAtCriticalPoints = True + , newtonRaphsonOptions = defaultNewtonRaphsonOptions + } + +data NewtonRaphsonOptions + = NewtonRaphsonOptions + { newtonRaphsonDigits :: !Int -- ^ desired bits of precision + , newtonRaphsonMaxIterations :: !Word -- ^ maximum number of iterations (at least 1) + } + +defaultNewtonRaphsonOptions :: NewtonRaphsonOptions +defaultNewtonRaphsonOptions = + NewtonRaphsonOptions + { newtonRaphsonDigits = 50 + , newtonRaphsonMaxIterations = 64 + } + +data ArcLengthParametrisation + = ArcLengthParametrisation + { totalArcLength :: !Double + , parameterFromArcLength :: !( Double -> Double ) + } + +-- | Compute the arc-length parametrisation of a 'Curve'. +curveArcLengthParametrisation + :: forall diff ptData crvData + . ( Torsor diff ptData, Module Double diff, Inner Double diff + , Show ptData ) + => ArcLengthOptions + -> ptData + -> Curve Open crvData ptData + -> ArcLengthParametrisation +curveArcLengthParametrisation opts p0 = \case + + LineTo { curveEnd = NextPoint p1 } -> + let !len = norm @diff ( p0 --> p1 ) + in ArcLengthParametrisation + { totalArcLength = len + , parameterFromArcLength = + \ s -> + if len > 0 + then max 0 $ min 1 $ s / len + else 0 + } + + Bezier2To { controlPoint = p1, curveEnd = NextPoint p2 } -> + let !bez = Quadratic.Bezier { p0, p1, p2 } + speed = norm @diff . Quadratic.bezier' @diff bez + !critPts = + if splitAtCriticalPoints opts + then Quadratic.speedCriticalPoints @diff bez + else [] + in build speed $ + pickIntegrator ( gravesenQuadIntegrator @diff bez ) speed critPts + + Bezier3To { controlPoint1 = p1, controlPoint2 = p2, curveEnd = NextPoint p3 } -> + let !bez = Cubic.Bezier { p0, p1, p2, p3 } + speed = norm @diff . Cubic.bezier' @diff bez + !critPts = + if splitAtCriticalPoints opts + then map realPart $ Cubic.speedCriticalPoints @diff bez + else [] + in build speed $ + pickIntegrator ( gravesenCubicIntegrator @diff bez ) speed critPts + + where + pickIntegrator + :: ( Double -> Double -> Double ) -- ^ Gravesen cumulative arc-length integrator + -> ( Double -> Double ) -- ^ speed function + -> [ Double ] -- ^ critical points + -> ( Double -> Double ) -- ^ cumulative arc-length function + pickIntegrator gravesenFn speed critPts = + case arcLengthIntegrator opts of + Gravesen { gravesenTol = eps } + -> gravesenFn eps + ClenshawCurtis { clenshawCurtisDegree = d, regulariser = reg } + | SomeNat @n _ <- someNatVal ( fromIntegral d ) + -> makeCompositeCCIntegrator @n reg speed critPts + TanhSinh { tanhSinhNodes = nodesPerSide, tanhSinhStepSize = tsH } + -> makeCompositeTSIntegrator nodesPerSide tsH speed critPts + GaussLegendre { gaussLegendreDegree = d, regulariser = reg } + -> makeCompositeGLIntegrator d reg speed critPts + + -- | Build the final parametrisation from speed and cumulative arc-length. + build speed integrate = + let !lg = integrate 1 + in + ArcLengthParametrisation + { totalArcLength = lg + , parameterFromArcLength = + \ s -> + if | s <= 0 + -> 0 + | s >= lg + -> 1 + | otherwise + -> let t_0 = s / lg + in case newtonRaphson + ( newtonRaphsonMaxIterations $ newtonRaphsonOptions opts ) + ( newtonRaphsonDigits $ newtonRaphsonOptions opts ) + ( \ t -> (# integrate t - s, speed t #) ) + t_0 + of + Just t' -> t' + Nothing -> t_0 + } +{-# INLINEABLE curveArcLengthParametrisation #-} + +-------------------------------------------------------------------------------- +-- Spline arc-length + +-- | Precomputed arc-length data for all curves of a spline. +data SplineArcLengthParam = SplineArcLengthParam + { splineArcLengthCurveEnds :: !( Seq Double ) + -- ^ Cumulative arc lengths at the end of each curve. + , splineArcLengthCurveParams :: !( Seq ArcLengthParametrisation ) + -- ^ Per-curve arc-length parametrisations. + } + +-- | Compute the arc-length parametrisation of a spline. +splineArcLengthParam + :: forall diff clo crvData ptData + . ( SplineTypeI clo + , Torsor diff ptData, Module Double diff, Inner Double diff + , Show ptData ) + => ArcLengthOptions + -> Spline clo crvData ptData + -> SplineArcLengthParam +splineArcLengthParam opts spl = + let Spline { splineStart, splineCurves = OpenCurves curves } = + adjustSplineType @Open spl + pairs = go Seq.Empty 0 splineStart curves + in SplineArcLengthParam + { splineArcLengthCurveEnds = fmap fst pairs + , splineArcLengthCurveParams = fmap snd pairs + } + where + go :: Seq (Double, ArcLengthParametrisation) + -> Double + -> ptData + -> Seq ( Curve Open crvData ptData ) + -> Seq (Double, ArcLengthParametrisation) + go acc _ _ Seq.Empty = acc + go acc cum pt ( crv Seq.:<| rest ) = + let param = curveArcLengthParametrisation @diff opts pt crv + cum' = cum + totalArcLength param + in go ( acc Seq.:|> ( cum', param ) ) cum' ( openCurveEnd crv ) rest + +-- | Total arc length of a spline. +splineTotalArcLength :: SplineArcLengthParam -> Double +splineTotalArcLength ( SplineArcLengthParam { splineArcLengthCurveEnds = ends } ) = + case ends of + Seq.Empty -> 0 + _ Seq.:|> x -> x + +-- | Find the @(curve index, Bézier parameter t ∈ [0,1])@ within the spline +-- at the given arc length. +splineParameterAtArcLength + :: SplineArcLengthParam + -> Double -- ^ target arc length + -> ( Int, Double ) -- ^ (curve index, Bézier parameter t ∈ [0,1]) +splineParameterAtArcLength + ( SplineArcLengthParam + { splineArcLengthCurveEnds = ends + , splineArcLengthCurveParams = crvParams + } + ) + s = go 0 0 ends crvParams + where + go i _cumStart Seq.Empty _ + = ( max 0 ( i - 1 ), 1 ) + go i cumStart ( end Seq.:<| restEnds ) ( param Seq.:<| restParams ) + | s <= end || null restEnds + = ( i, parameterFromArcLength param ( s - cumStart ) ) + | otherwise + = go ( i + 1 ) end restEnds restParams + go i _cumStart _ _ + = ( max 0 ( i - 1 ), 1 ) + +-- | Split an open spline at a sorted list of absolute arc-lengths. +splitSplineAtArcLengths + :: forall diff crvData ptData + . ( Torsor diff ptData, Module Double diff, Inner Double diff + , Show ptData -- debugging + ) + => ArcLengthOptions + -> [ Double ] -- ^ sorted arc-length positions + -> Spline Open crvData ptData + -> [ Spline Open crvData ptData ] +splitSplineAtArcLengths opts positions spl = subdivideSplineAt @diff spl splitParams + where + !param = splineArcLengthParam @diff opts spl + splitParams = map ( splineParameterAtArcLength param ) positions + +-------------------------------------------------------------------------------- +-- Clenshaw–Curtis quadrature + +-- | Chebyshev node of the second kind. +chebyshevNode + :: forall n + . KnownNat n + => Int -- ^ 0 <= k <= n + -> Double +chebyshevNode k = cos ( pi * fromIntegral k / fromIntegral n ) + where + n :: Int + n = fromIntegral $ natVal' @n proxy# +{-# INLINEABLE chebyshevNode #-} + +-- | Clenshaw–Curtis nodes. +ccNodes :: forall n. KnownNat n => PrimArray Double +ccNodes = + fromList + [ 0.5 * ( 1 + chebyshevNode @n k ) + | k <- [ 0 .. n ] + ] + where + n :: Int + n = fromIntegral $ natVal' @n proxy# +{-# INLINEABLE ccNodes #-} + +-- | Pre computation of a Clenshaw–Curtis integrator. +makeCCIntegrator + :: forall n + . KnownNat n + => PrimArray Double + -> ( Double -> Double ) +makeCCIntegrator ys = + -- Pre-computation + let + n :: Int + n = fromIntegral $ natVal' @n proxy# + ep :: Int -> Double + ep k = if k == 0 || k == n then 0.5 else 1 + + -- Chebyshev coefficients (pre-computation) + cs :: PrimArray Double + cs = + fromList + [ ( 2 / fromIntegral n ) + * sum [ ep k + * ( ys `Prim.indexPrimArray` k ) + * cos ( fromIntegral j * fromIntegral k * pi / fromIntegral n ) + | k <- [0..n] + ] + | j <- [0..n] + ] + + -- The integrator proper + in \t -> + let y0 = 2*t - 1 -- map t ∈ [0,1] to y₀ ∈ [−1,1] + + -- Chebyshev recurrence (T_{j−1}, T_j) as plain + go :: Double -> Double -> Double -> Int -> Double + go !acc !tm1 !tj !j + | j > n + = acc + | otherwise + = let + !tj1 = 2 * y0 * tj - tm1 + !cj = cs `Prim.indexPrimArray` j + !ej = cj * ( if j == 0 || j == n then 0.5 else 1 ) + a | j == 0 = ej * ( y0 + 1) + | j == 1 = ej * ( tj1 - 1 ) / 4 + | otherwise = ej * ( tj1 / fromIntegral ( 2 * ( j + 1 )) + - tm1 / fromIntegral ( 2 * ( j - 1 )) + - ( if even j then 1 else -1 ) / fromIntegral ( j * j - 1 ) ) + in go ( acc + a ) tj tj1 ( j + 1 ) + in 0.5 * go 0 y0 1 0 +{-# INLINEABLE makeCCIntegrator #-} + +-------------------------------------------------------------------------------- +-- Endpoint regularisation + +-- | Select the regularisation substitution for a single piece @[a,b]@. +-- Returns the transformed integrand on @[0,1]@ and the map @t ↦ u@. +applyRegulariser + :: Maybe Regulariser + -> Double -- ^ @a@ + -> Double -- ^ @b@ + -> ( Double -> Double ) -- ^ speed + -> ( Double -> Double -- ^ transformed integrand on @[0,1]@ + , Double -> Double ) -- ^ \( t \in [a,b] \mapsto u \in [0,1] \) +applyRegulariser Nothing a b speed = + ( \u -> speed ( a + (b-a)*u ) * (b-a) + , \x -> (x-a) / (b-a) ) +applyRegulariser + ( Just Regulariser { regularisationMethod = meth, regularisationThreshold = tol } ) + a b speed + | sa <= tol && sa <= sb = case meth of + SquareRoot -> + ( \u -> speed ( a + (b-a)*u*u ) * 2*(b-a)*u + , \x -> sqrt ( max 0 ( (x-a) / (b-a) ) ) ) + | sb <= tol && sb < sa = case meth of + SquareRoot -> + ( \u -> speed ( b - (b-a)*(1-u)*(1-u) ) * 2*(b-a)*(1-u) + , \x -> 1 - sqrt ( max 0 ( (b-x) / (b-a) ) ) ) + | otherwise = + ( \u -> speed ( a + (b-a)*u ) * (b-a) + , \x -> (x-a) / (b-a) ) + where + sa = speed a + sb = speed b +{-# INLINEABLE applyRegulariser #-} + +-------------------------------------------------------------------------------- +-- Clenshaw–Curtis quadrature (continued) + +-- | Composite Clenshaw–Curtis integrator that splits @[0,1]@ at the critical +-- points of @|B′(t)|²@ and optionally applies an endpoint regularisation +-- substitution on each piece at the low-speed endpoint. +-- +-- The splitting avoids bad convergence around critical points; the +-- regularisation restores exponential convergence when a boundary point is a +-- critical point (Bernstein ellipse expansion). +makeCompositeCCIntegrator + :: forall n + . KnownNat n + => Maybe Regulariser -- ^ cusp-endpoint regularisation + -> ( Double -> Double ) -- ^ speed function on @[0,1]@ + -> [ Double ] -- ^ critical points of @|speed|²@ in @(0,1)@, sorted ascending + -> ( Double -> Double ) -- ^ \( t \mapsto \int_0^t speed(x) dx \) +makeCompositeCCIntegrator reg speed critPts = + let + n = fromIntegral $ natVal' @n proxy# + bps = 0 : critPts ++ [1] + pieceData = + [ ( a, b, integr, toU, integr 1 ) + | ( a, b ) <- zip bps ( drop 1 bps ) + , b - a > 1e-8 + , let ( f01, toU ) = applyRegulariser reg a b speed + ys = fromList + [ f01 ( ccNodes @n `Prim.indexPrimArray` k ) + | k <- [ 0 .. n ] ] + integr = makeCCIntegrator @n ys + ] + !totalLen = sum [ fullLen | (_, _, _, _, fullLen) <- pieceData ] + + in \ t -> + if | t <= 0 + -> 0 + | t >= 1 + -> totalLen + | otherwise + -> sum [ if b <= t then fullLen else integr ( toU t ) + | ( a, b, integr, toU, fullLen ) <- pieceData + , a < t + ] +{-# INLINEABLE makeCompositeCCIntegrator #-} + +-------------------------------------------------------------------------------- +-- Tanh–Sinh quadrature + +-- | Composite Tanh–Sinh integrator that splits @[0,1]@ at the given subdivision +-- points (e.g. cusps), and applies Tanh–Sinh (double-exponential) quadrature +-- on each piece. +makeCompositeTSIntegrator + :: Int -- ^ @N@: nodes per side (@2N+1@ nodes total) + -> Double -- ^ @h@: step size + -> ( Double -> Double ) -- ^ speed function on @[0,1]@ + -> [ Double ] -- ^ subdivision points + -> ( Double -> Double ) -- ^ \( t \mapsto \int_0^t speed(x) dx \) +makeCompositeTSIntegrator nodesPerSide h speed subdivPts = + let + pi_2 :: Double + pi_2 = pi / 2 + + bps :: [ Double ] + bps = 0 : subdivPts ++ [1] + + n :: Int + n = 2 * nodesPerSide + 1 -- total number of nodes + + -- Pre-compute normalised Tanh–Sinh nodes and weights for [-1,1]: + -- φ_k = tanh(π/2 · sinh(k·h)) node, ∈ (−1,1) + -- ψ_k = π/2 · cosh(k·h) / cosh²(π/2 · sinh(k·h)) weight kernel + -- + -- Integral on [a,b] ≈ h · (b−a)/2 · Σ_k ψ_k · f( (a+b)/2 + (b−a)/2 · φ_k ) + φs :: PrimArray Double + !φs = fromList + [ tanh ( pi_2 * sinh ( fromIntegral k * h ) ) + | k <- [ -nodesPerSide .. nodesPerSide ] + ] + + ψs :: PrimArray Double + !ψs = fromList + [ let skh = sinh ( fromIntegral k * h ) + ckh = cosh ( fromIntegral k * h ) + c = cosh ( pi_2 * skh ) + in pi_2 * ckh / ( c * c ) + | k <- [ -nodesPerSide .. nodesPerSide ] + ] + + -- Tanh–Sinh quadrature of 'speed' on [a, b]. + tsIntegral :: Double -> Double -> Double + tsIntegral a b = + let mid = ( a + b ) * 0.5 + half = ( b - a ) * 0.5 + go :: Double -> Int -> Double + go !acc !i + | i >= n = acc + | otherwise = + let φ = φs `Prim.indexPrimArray` i + ψ = ψs `Prim.indexPrimArray` i + in go ( acc + ψ * speed ( mid + half * φ ) ) ( i + 1 ) + in h * half * go 0 0 + + pieceData :: [ ( Double, Double, Double ) ] + pieceData = + [ ( a, b, tsIntegral a b ) + | ( a, b ) <- zip bps ( drop 1 bps ) + , b - a > 1e-8 + ] + + totalLen :: Double + !totalLen = sum [ fullLen | ( _, _, fullLen ) <- pieceData ] + + in \ t -> + if + | t <= 0 + -> 0 + | t >= 1 + -> totalLen + | otherwise + -> sum [ if t >= b then fullLen else tsIntegral a t + | ( a, b, fullLen ) <- pieceData + , a < t + ] +{-# INLINEABLE makeCompositeTSIntegrator #-} + +-------------------------------------------------------------------------------- +-- Gauss–Legendre quadrature + +-- | Compute @n@-point Gauss–Legendre nodes (on [−1,1]) and weights +-- using Newton iteration on the Legendre polynomial recurrence. +computeGLNodesWeights :: Int -> ( PrimArray Double, PrimArray Double ) +computeGLNodesWeights n = + let + -- Evaluate (P_n(x), P_n′(x)) via the three-term recurrence: + -- P_0 = 1, P_0′ = 0 + -- P_1 = x, P_1′ = 1 + -- (k+1) P_{k+1} = (2k+1) x P_k − k P_{k−1} + legendre :: Double -> (# Double, Double #) + legendre x + | n == 0 = (# 1, 0 #) + | otherwise = go 1 1 x 0 1 + where + go :: Int -> Double -> Double -> Double -> Double -> (# Double, Double #) + go !k !p_0 !p_1 !dp_0 !dp_1 + | k == n = (# p_1, dp_1 #) + | otherwise = + let + kd, p_2, dp_2 :: Double + !kd = fromIntegral k + !p_2 = ( ( 2 * kd + 1 ) * x * p_1 - kd * p_0 ) / ( kd + 1 ) + !dp_2 = ( ( 2 * kd + 1 ) * ( p_1 + x * dp_1 ) - kd * dp_0 ) / ( kd + 1 ) + in go (k+1) p_1 p_2 dp_1 dp_2 + + -- Newton iteration to refine a root of P_n. + refineRoot :: Double -> Double + refineRoot x0 = fromMaybe x0 $ newtonRaphson 50 50 legendre x0 + + -- Compute the m largest positive roots (including 0 for odd n). + m = ( n + 1 ) `div` 2 + + halfData :: [ ( Double, Double ) ] + halfData = + [ let x_i = refineRoot ( cos ( pi * ( fromIntegral k - 0.25 ) + / ( fromIntegral n + 0.5 ) ) ) + (# _, dp_i #) = legendre x_i + w_i = 2 / ( ( 1 - x_i * x_i ) * dp_i * dp_i ) + in ( x_i, w_i ) + | k <- [ 1 .. m ] + ] + + -- Use symmetry to obtain all nodes + ( xs, ws ) = unzip $ + concatMap + ( \ ( x_i, w_i ) -> + if abs x_i < epsilon + then [ ( 0 , w_i ) ] + else [ ( x_i, w_i ) + , ( -x_i, w_i ) ] + ) halfData + + in ( fromList xs + , fromList ws ) +{-# INLINEABLE computeGLNodesWeights #-} + +-- | Composite Gauss–Legendre integrator that splits @[0,1]@ at the critical +-- points of @|B′(t)|²@ and optionally applies an endpoint regularisation +-- substitution at the low-speed endpoint of each piece. +makeCompositeGLIntegrator + :: Int -- ^ number of quadrature nodes + -> Maybe Regulariser -- ^ cusp-endpoint regularisation + -> ( Double -> Double ) -- ^ speed function on @[0,1]@ + -> [ Double ] -- ^ critical points of @|speed|²@ in @(0,1)@, sorted ascending + -> ( Double -> Double ) -- ^ \( t \mapsto \int_0^t speed(x) dx \) +makeCompositeGLIntegrator nPts reg speed critPts = + let + bps = 0 : critPts ++ [ 1 ] + + ( glXs, glWs ) = computeGLNodesWeights nPts + + -- GL quadrature of f on [a, b]. + glQuad :: ( Double -> Double ) -> Double -> Double -> Double + glQuad f a b = + let mid = ( a + b ) * 0.5 + half = ( b - a ) * 0.5 + go :: Double -> Int -> Double + go !acc !i + | i >= nPts = acc + | otherwise = + let xi = glXs `Prim.indexPrimArray` i + wi = glWs `Prim.indexPrimArray` i + in go ( acc + wi * f ( mid + half * xi ) ) ( i + 1 ) + in half * go 0 0 + + pieceData :: [ ( Double, Double, Double -> Double -> Double, Double -> Double, Double ) ] + pieceData = + [ ( a, b, glQuad f01, toU, fullLen ) + | ( a, b ) <- zip bps ( drop 1 bps ) + , b - a > 1e-8 + , let ( f01, toU ) = applyRegulariser reg a b speed + , let fullLen = glQuad f01 0 1 + ] + + !totalLen = sum [ fullLen | ( _, _, _, _, fullLen ) <- pieceData ] + + in \ t -> + if | t <= 0 + -> 0 + | t >= 1 + -> totalLen + | otherwise + -> sum [ if b <= t then fullLen else intgr 0 ( toU t ) + | ( a, b, intgr, toU, fullLen ) <- pieceData + , a < t + ] +{-# INLINEABLE makeCompositeGLIntegrator #-} + +-------------------------------------------------------------------------------- +-- Gravesen adaptive subdivision + +-- | Cumulative arc-length integrator for a quadratic Bézier using the +-- Gravesen (1993) adaptive polygon/chord method. +gravesenQuadIntegrator + :: forall diff ptData + . ( Torsor diff ptData, Module Double diff, Inner Double diff ) + => Quadratic.Bezier ptData + -> Double -- ^ @eps@: stopping tolerance + -> ( Double -> Double ) +gravesenQuadIntegrator bez eps = + let !total = go eps bez + in \ t -> + if t <= 0 then 0 + else if t >= 1 then total + else go eps $ fst $ Quadratic.subdivide @diff bez t + where + go :: Double -> Quadratic.Bezier ptData -> Double + go e b@( Quadratic.Bezier { p0, p1, p2 } ) = + let l0 = norm @diff ( p0 --> p2 ) + l1 = norm @diff ( p0 --> p1 ) + norm @diff ( p1 --> p2 ) + in if l1 - l0 < e + then ( 2 * l0 + l1 ) / 3 + else let ( lb, rb ) = Quadratic.subdivide @diff b 0.5 + in go ( e / 2 ) lb + go ( e / 2 ) rb +{-# INLINEABLE gravesenQuadIntegrator #-} + +-- | Cumulative arc-length integrator for a cubic Bézier using the +-- Gravesen (1993) adaptive polygon/chord method. +gravesenCubicIntegrator + :: forall diff ptData + . ( Torsor diff ptData, Module Double diff, Inner Double diff ) + => Cubic.Bezier ptData + -> Double -- ^ @eps@: stopping tolerance + -> ( Double -> Double ) +gravesenCubicIntegrator bez eps = + let !total = go eps bez + in \ t -> + if t <= 0 then 0 + else if t >= 1 then total + else go eps $ fst $ Cubic.subdivide @diff bez t + where + go :: Double -> Cubic.Bezier ptData -> Double + go e b@( Cubic.Bezier { p0, p1, p2, p3 } ) = + let l0 = norm @diff ( p0 --> p3 ) + l1 = norm @diff ( p0 --> p1 ) + norm @diff ( p1 --> p2 ) + norm @diff ( p2 --> p3 ) + in if l1 - l0 < e + then ( l0 + l1 ) / 2 -- = (2·l0 + 2·l1) / 4, degree-3 Gravesen estimate + else let ( lb, rb ) = Cubic.subdivide @diff b 0.5 + in go ( e / 2 ) lb + go ( e / 2 ) rb +{-# INLINEABLE gravesenCubicIntegrator #-}
+ src/lib/Math/Bezier/Cubic.hs view
@@ -0,0 +1,337 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Bezier.Cubic + ( Bezier(..) + , fromQuadratic + , bezier, bezier', bezier'', bezier''' + , derivative + , curvature, squaredCurvature, signedCurvature + , subdivide, restrict + , ddist, closestPoint + , drag, selfIntersectionParameters + , extrema + , speedCriticalPoints + ) + where + +-- base +import Data.Complex + ( realPart, Complex ) +import Data.Function + ( on ) +import Data.List + ( sortOn, nubBy ) +import Data.List.NonEmpty + ( NonEmpty(..) ) +import Data.Monoid + ( Ap(..) ) +import Data.Semigroup + ( ArgMin, Min(..), Arg(..) ) +import GHC.Generics + ( Generic, Generic1 + , Generically(..), Generically1(..) + ) + +-- acts +import Data.Act + ( Act(..) + , Torsor + ( (-->) ) + ) + +-- deepseq +import Control.DeepSeq + ( NFData, NFData1 ) + +-- fp-ieee +import Numeric.Floating.IEEE.NaN + ( RealFloatNaN ) + +-- groups +import Data.Group + ( Group ) + +-- groups-generic +import Data.Group.Generics + () + +-- primitive +import Data.Primitive.Types + ( Prim ) + +-- brush-strokes +import qualified Math.Bezier.Quadratic as Quadratic + ( Bezier(..), bezier ) +import Math.Epsilon + ( epsilon ) +import Math.Module + ( Module (..) + , lerp + , Inner((^.^)), norm, squaredNorm + , Cross((×)) + ) +import Math.Roots + ( realRoots, solveQuadratic, roots ) +import Math.Linear + ( ℝ(..), T(..) ) +import qualified Math.Ring as Ring + +-------------------------------------------------------------------------------- + +-- | Points defining a cubic Bézier curve (Bernstein form). +-- +-- @ p0 @ and @ p3 @ are endpoints, whereas @ p1 @ and @ p2 @ are control points. +data Bezier p + = Bezier + { p0, p1, p2, p3 :: !p } + deriving stock ( Generic, Generic1, Functor, Foldable, Traversable ) + deriving ( Semigroup, Monoid, Group ) + via Generically ( Bezier p ) + deriving Applicative + via Generically1 Bezier + deriving anyclass ( NFData, NFData1 ) + +deriving via Ap Bezier p + instance {-# OVERLAPPING #-} Act v p => Act v ( Bezier p ) +deriving via Ap Bezier ( T b ) + instance Module r ( T b ) => Module r ( T ( Bezier b ) ) + +instance Show p => Show (Bezier p) where + show (Bezier p1 p2 p3 p4) = + show p1 ++ "--" ++ show p2 ++ "--" ++ show p3 ++ "->" ++ show p4 + +-- | Degree raising: convert a quadratic Bézier curve to a cubic Bézier curve. +fromQuadratic :: forall v r p. ( Torsor v p, Module r v, Fractional r ) => Quadratic.Bezier p -> Bezier p +fromQuadratic ( Quadratic.Bezier { p0 = q0, p1 = q1, p2 = q2 } ) = Bezier {..} + where + !p0 = q0 + !p1 = lerp @v (2/3) q0 q1 + !p2 = lerp @v (1/3) q1 q2 + !p3 = q2 +{-# INLINEABLE fromQuadratic #-} + +-- | Cubic Bézier curve. +bezier :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> p +bezier ( Bezier {..} ) t = + lerp @v t + ( Quadratic.bezier @v ( Quadratic.Bezier p0 p1 p2 ) t ) + ( Quadratic.bezier @v ( Quadratic.Bezier p1 p2 p3 ) t ) +{-# INLINEABLE bezier #-} + +-- | The derivative of a Cubic Bézier curve, as a quadratic Bézier curve. +derivative :: ( Group v, Module r v ) => Bezier v -> Quadratic.Bezier v +derivative ( Bezier {..} ) = ( Ring.fromInteger 3 *^ ) + <$> Quadratic.Bezier ( p0 --> p1 ) ( p1 --> p2 ) ( p2 --> p3 ) +{-# INLINEABLE derivative #-} + +-- | Derivative of a cubic Bézier curve. +bezier' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> v +bezier' ( Bezier {..} ) + = ( Ring.fromInteger 3 *^ ) + . Quadratic.bezier @v ( Quadratic.Bezier ( p0 --> p1 ) ( p1 --> p2 ) ( p2 --> p3 ) ) +{-# INLINEABLE bezier' #-} + +-- | Second derivative of a cubic Bézier curve. +bezier'' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> v +bezier'' ( Bezier {..} ) t + = ( Ring.fromInteger 6 *^ ) + $ lerp @v t + ( p1 --> p0 ^+^ p1 --> p2 ) + ( p2 --> p1 ^+^ p2 --> p3 ) +{-# INLINEABLE bezier'' #-} + +-- | Third derivative of a cubic Bézier curve. +bezier''' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> v +bezier''' ( Bezier {..} ) + = ( Ring.fromInteger 6 *^ ) + $ ( ( p0 --> p3 ) ^+^ Ring.fromInteger 3 *^ ( p2 --> p1 ) ) +{-# INLINEABLE bezier''' #-} + +-- | Curvature of a cubic Bézier curve. +curvature :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> r -> r +curvature bez t = sqrt $ squaredCurvature @v bez t +{-# INLINEABLE curvature #-} + +-- | Square of curvature of a cubic Bézier curve. +squaredCurvature :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> r -> r +squaredCurvature bez t + | sq_nm_g' < epsilon + = 1 / 0 + | otherwise + = ( sq_nm_g' * squaredNorm @v g'' - ( g' ^.^ g'' ) ^ ( 2 :: Int ) ) + / ( sq_nm_g' ^ ( 3 :: Int ) ) + where + g', g'' :: v + !g' = bezier' @v bez t + !g'' = bezier'' @v bez t + sq_nm_g' :: r + !sq_nm_g' = squaredNorm @v g' +{-# INLINEABLE squaredCurvature #-} + +-- | Signed curvature of a planar cubic Bézier curve. +signedCurvature :: Bezier ( ℝ 2 ) -> Double -> Double +signedCurvature bez t = ( g' × g'' ) / norm g' ^ ( 3 :: Int ) + where + g', g'' :: T ( ℝ 2 ) + !g' = bezier' @( T ( ℝ 2 ) ) bez t + !g'' = bezier'' @( T ( ℝ 2 ) ) bez t + +-- | Subdivide a cubic Bézier curve into two parts. +subdivide :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> ( Bezier p, Bezier p ) +subdivide ( Bezier {..} ) t = ( Bezier p0 q1 q2 pt, Bezier pt r1 r2 p3 ) + where + pt, s, q1, q2, r1, r2 :: p + !q1 = lerp @v t p0 p1 + !s = lerp @v t p1 p2 + !r2 = lerp @v t p2 p3 + !q2 = lerp @v t q1 s + !r1 = lerp @v t s r2 + !pt = lerp @v t q2 r1 +{-# INLINEABLE subdivide #-} + +-- | Restrict a cubic Bézier curve to a sub-interval, re-parametrising +-- to \( [0,1] \). +restrict :: forall v r p. ( Torsor v p, Ring.Field r, Module r v ) => Bezier p -> ( r , r ) -> Bezier p +restrict bez ( a, b ) = fst $ ( flip ( subdivide @v ) b' ) $ snd $ subdivide @v bez a + where + b' = ( b Ring.- a ) Ring./ ( Ring.fromInteger 1 Ring.- a ) + -- TODO: this could be made more efficient. + -- See e.g. "https://math.stackexchange.com/questions/4172835/cubic-b%C3%A9zier-spline-multiple-split" + -- or the paper "On the numerical condition of Bernstein-Bézier subdivision process". +{-# INLINEABLE restrict #-} + +-- | Polynomial coefficients of the derivative of the distance to a cubic Bézier curve. +ddist :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> [ r ] +ddist ( Bezier {..} ) c = [ a5, a4, a3, a2, a1, a0 ] + where + v, v', v'', v''' :: v + !v = c --> p0 + !v' = p0 --> p1 + !v'' = p1 --> p0 ^+^ p1 --> p2 + !v''' = p0 --> p3 ^+^ 3 *^ ( p2 --> p1 ) + + a0, a1, a2, a3, a4, a5 :: r + !a0 = v ^.^ v' + !a1 = 3 * squaredNorm v' + 2 * v ^.^ v'' + !a2 = 9 * v' ^.^ v'' + v ^.^ v''' + !a3 = 6 * squaredNorm v'' + 4 * v' ^.^ v''' + !a4 = 5 * v'' ^.^ v''' + !a5 = squaredNorm v''' +{-# INLINEABLE ddist #-} + +-- | Finds the closest point to a given point on a cubic Bézier curve. +closestPoint + :: forall v r p. ( Torsor v p, Inner r v, RealFloat r, Prim r, NFData r ) + => Bezier p -> p -> ArgMin r ( r, p ) +closestPoint pts c = pickClosest ( 0 :| 1 : distRoots ) + where + distRoots :: [ r ] + distRoots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots 50 $ ddist @v pts c ) + + pickClosest :: NonEmpty r -> ArgMin r ( r, p ) + pickClosest ( s :| ss ) = go s q nm0 ss + where + q :: p + !q = bezier @v pts s + nm0 :: r + !nm0 = squaredNorm ( c --> q :: v ) + go t p nm [] = Min ( Arg nm ( t, p ) ) + go t p nm ( t' : ts ) + | nm' < nm = go t' p' nm' ts + | otherwise = go t p nm ts + where + p' :: p + !p' = bezier @v pts t' + nm' :: r + !nm' = squaredNorm ( c --> p' :: v ) +{-# INLINEABLE closestPoint #-} + +-- | Drag a cubic Bézier curve to pass through a given point. +-- +-- Given a cubic Bézier curve, a time \( 0 < t < 1 \) and a point `q`, +-- modifies the control points to make the curve pass through `q` at time `t`. +-- +-- Affects the two control points depending on how far along the dragged point is. +-- For instance, dragging near the middle moves both control points equally, +-- while dragging near an endpoint will mostly affect the control point associated with that endpoint. +drag :: forall v r p. ( Torsor v p, Module r v, Fractional r ) => Bezier p -> r -> p -> Bezier p +drag ( Bezier {..} ) t q = Bezier { p0, p1 = p1', p2 = p2', p3 } + where + v0, v1, v2, v3, delta :: v + !v0 = q --> p0 + !v1 = q --> p1 + !v2 = q --> p2 + !v3 = q --> p3 + !delta = ( recip $ t * ( -3 + t * ( 9 + t * ( -12 + 6 * t ) ) ) ) + *^ bezier @v ( Bezier v0 v1 v2 v3 ) t + p1', p2' :: p + !p1' = ( ( 1 - t ) *^ delta ) • p1 + !p2' = ( t *^ delta ) • p2 +{-# INLINEABLE drag #-} + +-- | Compute parameter values for the self-intersection of a planar cubic Bézier curve, if such exist. +-- +-- The parameter values might lie outside the interval [0,1], +-- indicating a self-intersection of the extended curve. +-- +-- Formula taken from: +-- "A Basis for the Implicit Representation of Planar Rational Cubic Bézier Curves" +-- – Oliver J. D. Barrowclough, 2016 +selfIntersectionParameters :: Bezier ( ℝ 2 ) -> [ Double ] +selfIntersectionParameters ( Bezier {..} ) = solveQuadratic c0 c1 c2 + where + areaConstant :: ℝ 2 -> ℝ 2 -> ℝ 2 -> Double + areaConstant ( ℝ2 x1 y1 ) ( ℝ2 x2 y2 ) ( ℝ2 x3 y3 ) = + x1 * ( y2 - y3 ) + x2 * ( y3 - y1 ) + x3 * ( y1 - y2 ) + l0, l1, l2, l3, f1, f2, f3, c0, c1, c2 :: Double + !l0 = areaConstant p3 p2 p1 + !l1 = areaConstant p2 p3 p0 + !l2 = areaConstant p1 p0 p3 + !l3 = areaConstant p0 p1 p2 + !f1 = 3 * ( l1 * l1 - 3 * l0 * l2 ) + !f2 = 3 * ( l2 * l2 - 3 * l1 * l3 ) + !f3 = 3 * ( 9 * l0 * l3 - l1 * l2 ) + !c0 = f2 + !c1 = f3 - 2 * f2 + !c2 = f1 + f2 - f3 + +-- | Critical points of the speed @|B′(t)|@ in @(0,1)@ for a cubic Bézier. +-- +-- These are the roots of @d\/dt |B′(t)|²\/18 = 0@, a cubic polynomial whose +-- coefficients come from expanding @H(t)·H′(t) = 0@ where +-- @H(t) = d₀ + 2t·f + t²·e@ is the (scaled) hodograph, +-- @f = d₁ − d₀@ and @e = d₀ − 2d₁ + d₂@ its first and second differences. +speedCriticalPoints + :: forall diff ptData + . ( Torsor diff ptData, Module Double diff, Inner Double diff ) + => Bezier ptData -> [ Complex Double ] +speedCriticalPoints ( Bezier {..} ) = + let + d0, d1, d2 :: diff + !d0 = p0 --> p1 + !d1 = p1 --> p2 + !d2 = p2 --> p3 + !f = d1 ^-^ d0 + !e = ( d0 ^-^ ( 2 *^ d1 ) ) ^+^ d2 + !c3 = e ^.^ e + !c2 = 3 * ( f ^.^ e ) + !c1 = 2 * ( f ^.^ f ) + ( d0 ^.^ e ) + !c0 = d0 ^.^ f + in nubBy ( (==) `on` realPart ) + $ sortOn realPart + $ filter ( \ t -> realPart t > 0 && realPart t < 1 ) + $ roots epsilon 64 [c3, c2, c1, c0] +{-# INLINEABLE speedCriticalPoints #-} + +-- | Extremal values of the Bézier parameter for a cubic Bézier curve. +extrema :: RealFloatNaN r => Bezier r -> [ r ] +extrema ( Bezier {..} ) = solveQuadratic c b a + where + !a = p3 - 3 * p2 + 3 * p1 - p0 + !b = 2 * ( p0 - 2 * p1 + p2 ) + !c = p1 - p0 +{-# INLINEABLE extrema #-} +{-# SPECIALISE extrema @Float #-} +{-# SPECIALISE extrema @Double #-}
+ src/lib/Math/Bezier/Cubic/Fit.hs view
@@ -0,0 +1,334 @@+{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Bezier.Cubic.Fit + ( FitParameters(..), FitPoint(..), FwdBwd(..) + , OutlinePoint(..) + , fitSpline, fitPiece + ) + where + +-- base +import Control.Arrow + ( first, second ) +import Control.Monad + ( when ) +import Control.Monad.ST + ( ST, runST ) +import Data.Complex + ( Complex(..) ) +import Data.Foldable + ( for_ ) +import Data.Functor + ( ($>) ) +import Data.Semigroup + ( Arg(..), Max(..), ArgMax ) +import GHC.Generics + ( Generic ) + +-- acts +import Data.Act + ( Act + ( (•) ) + ) + +-- containers +import Data.Sequence + ( Seq(..) ) +import qualified Data.Sequence as Seq + ( fromList ) + +-- deepseq +import Control.DeepSeq + ( NFData ) + +-- parallel +import qualified Control.Parallel.Strategies as Parallel.Strategy + ( rdeepseq, parTuple2, using ) + +-- primitive +import Data.Primitive.PrimArray + ( MutablePrimArray + , generatePrimArray + , primArrayFromListN + , unsafeThawPrimArray, unsafeFreezePrimArray + , readPrimArray, writePrimArray, primArrayToList + ) + +-- transformers +import Control.Monad.Trans.State.Strict + ( execStateT, modify' ) +import Control.Monad.Trans.Class + ( lift ) + +-- brush-strokes +import qualified Math.Bezier.Cubic as Cubic + ( Bezier(..), bezier, ddist ) +import Math.Bezier.Spline + ( SplineType(..), SplinePts + , openCubicBezierCurveSpline + ) +import Math.Epsilon + ( epsilon ) +import Math.Linear.Solve + ( linearSolve ) +import Math.Module + ( Module((*^), (^-^)), lerp + , Inner((^.^)), quadrance + ) +import Math.Roots + ( laguerre ) --, eval, derivative ) +import Math.Linear + ( Mat22(..), ℝ(..), T(..) ) + +-------------------------------------------------------------------------------- + +-- | Parameters to the curve fitting algorithm. +data FitParameters + = FitParameters + { maxSubdiv :: !Int -- ^ maximum subdivision recursion number + , nbSegments :: !Int -- ^ number of segments to split the curve into for fitting + , dist_tol :: !Double -- ^ tolerance for the distance + , t_tol :: !Double -- ^ the tolerance for the Bézier parameter (for the fitting process) + , maxIters :: !Int -- ^ maximum number of iterations (for the fitting process) + } + deriving stock Show + +data FwdBwd + = Fwd + | Bwd + deriving stock ( Eq, Show, Generic ) + deriving anyclass NFData + +-- | Point computed as being on the outline. +-- +-- Output of the envelope equation calculation, and input to the +-- curve fitting algorithm. +data OutlinePoint + = OutlinePoint + { outlinePoint :: ℝ 2 + , outlineParam :: Double + , outlineTangent :: T ( ℝ 2 ) + , outlineDotProduct :: Double + } + deriving stock ( Eq, Show, Generic ) + +data FitPoint + = FitPoint + { fitDir :: !FwdBwd + , fitPoint :: !( ℝ 2 ) + , fitParam :: !( ℝ 1, ℝ 1 ) + , fitDotProduct :: !Double + } + | FitTangent + { fitDir :: !FwdBwd + , fitPoint :: !( ℝ 2 ) + , fitTangent :: !( T ( ℝ 2 ) ) + , fitDotProduct :: !Double + } + | JoinPoint + { joinDir :: !FwdBwd + , joinPoint :: !( ℝ 2 ) + } + deriving stock ( Show, Generic ) + deriving anyclass NFData + +-- | Fits a cubic Bézier spline to the given curve \( t \mapsto C(t), 0 \leqslant t \leqslant 1 \), +-- assumed to be G1-continuous. +-- +-- Additionally returns the points that were used to perform the fit, for debugging purposes. +-- +-- Subdivides the given curve into the specified number of segments \( \texttt{nbSegments} \), +-- and tries to fit the resulting points with a cubic Bézier curve using 'fitPiece'. +-- +-- When the fit is too poor: subdivide at the worst fitting point, and recurse. +-- \( \texttt{maxSubdiv} \) controls the maximum recursion depth for subdivision. +-- +-- See 'fitPiece' for more information on the fitting process, +-- including the meaning of \( \texttt{t_tol} \) and \( \texttt{maxIters} \). +fitSpline + :: FwdBwd + -> FitParameters + -> ( ℝ 1 -> OutlinePoint ) -- ^ curve \( t \mapsto ( C(t), C'(t) ) \) to fit + -> ( ℝ 1, ℝ 1 ) -- ^ interval \( [t_min, t_max] \) + -> ( SplinePts Open, Seq FitPoint ) +fitSpline fwdOrBwd ( FitParameters {..} ) curveFn = go 0 + where + dt :: Double + dt = recip ( fromIntegral nbSegments ) + go + :: Int + -> ( ℝ 1, ℝ 1 ) + -> ( SplinePts Open, Seq FitPoint ) + go subdiv ( t_min, t_max ) = + let + p, r :: ℝ 2 + tp, tr :: T ( ℝ 2 ) + qs :: [ ( ℝ 1, ( ℝ 2, Double ) ) ] + OutlinePoint { outlinePoint = p, outlineTangent = tp, outlineDotProduct = dot_p } = curveFn t_min + OutlinePoint { outlinePoint = r, outlineTangent = tr, outlineDotProduct = dot_r } = curveFn t_max + qs = [ ( t0, ( q, dot_q ) ) + | j <- [ 1 .. nbSegments - 1 ] + , let t0 = lerp @( T ( ℝ 1 ) ) ( dt * fromIntegral j ) t_min t_max + OutlinePoint { outlinePoint = q, outlineDotProduct = dot_q } + = curveFn t0 + ] + in + case fitPiece dist_tol t_tol maxIters p tp ( fmap ( fst . snd ) qs ) r tr of + ( bez, ts', Max ( Arg max_sq_error t_split_0 ) ) + | subdiv >= maxSubdiv + || max_sq_error <= dist_tol ^ ( 2 :: Int ) + -> -- trace ( unlines [ "fitSpline: piece is OK", "t_min = " ++ show t_min, "start = " ++ show p, "start tgt = " ++ show tp, "t_max = " ++ show t_max, "end = " ++ show r, "end tgt = " ++ show tr ] ) $ + ( openCubicBezierCurveSpline () bez + , ( FitTangent fwdOrBwd p tp dot_p + :<| Seq.fromList ( zipWith ( \ ( t0, ( pt, dot ) ) δ -> FitPoint fwdOrBwd pt ( t0, lerp @( T ( ℝ 1 ) ) δ t_min t_max ) dot ) qs ts' ) ) + :|> FitTangent fwdOrBwd r tr dot_r + ) + | let + t_split :: ℝ 1 + t_split = ℝ1 $ min ( unℝ1 $ lerp @( T ( ℝ 1 ) ) ( dt * 1 ) t_min t_max ) + $ max ( unℝ1 $ lerp @( T ( ℝ 1 ) ) ( dt * fromIntegral ( nbSegments - 1 ) ) t_min t_max ) + $ t_split_0 + c1, c2 :: SplinePts Open + ps1, ps2 :: Seq FitPoint + ( ( c1, ps1 ), ( c2, ps2 ) ) + = ( go ( subdiv + 1 ) ( t_min , t_split ) + , go ( subdiv + 1 ) ( t_split, t_max ) + ) `Parallel.Strategy.using` + ( Parallel.Strategy.parTuple2 Parallel.Strategy.rdeepseq Parallel.Strategy.rdeepseq ) + -> ( c1 <> c2, ps1 <> ps2 ) + +-- | Fits a single cubic Bézier curve to the given data. +-- +-- This will consist of a cubic Bézier curve which: +-- +-- * starts at \( p \) with tangent \( \textrm{t}_p \), +-- * ends at \( r \) with tangent \( \textrm{t}_r \), +-- * best fits the intermediate sequence of points \( \left ( q_i \right )_{i=1}^n \). +-- +-- This function also returns \( \textrm{ArgMax}\ d^2_\textrm{max}\ t_\textrm{max}: \) +-- the parameter and squared distance of the worst-fitting point. +-- It is guaranteed that all points to fit lie within the tubular neighbourhood +-- of radius \( d_\textrm{max} \) of the fitted curve. +-- +-- /Note/: the order of the intermediate points is important. +-- +-- Proceeds by fitting a cubic Bézier curve \( B(t) \), \( 0 \leqslant t \leqslant 1 \), +-- with given endpoints and tangents, which minimises the sum of squares functional +-- +-- \[ \sum_{i=1}^n \Big \| B(t_i) - q_i \Big \|^2. \] +-- +-- The values of the parameters \( \left ( t_i \right )_{i=1}^n \) are recursively estimated, +-- starting from uniform parametrisation (this will be the fit when \( \texttt{maxIters} = 0 \)). +-- +-- The iteration ends when any of the following conditions are satisfied: +-- +-- * each new estimated parameter value \( t_i' \) differs from +-- its previous value \( t_i \) by less than \( \texttt{t_tol} \), +-- * each on-curve point \( B(t_i) \) is within distance \( \texttt{dist_tol} \) +-- of its corresponding point to fit \( q_i \), +-- * the maximum iteration limit \( \texttt{maxIters} \) has been reached. +fitPiece + :: Double -- ^ \( \texttt{dist_tol} \), tolerance for the distance + -> Double -- ^ \( \texttt{t_tol} \), the tolerance for the Bézier parameter + -> Int -- ^ \( \texttt{maxIters} \), maximum number of iterations + -> ℝ 2 -- ^ \( p \), start point + -> T ( ℝ 2 ) -- ^ \( \textrm{t}_p \), start tangent vector (length is ignored) + -> [ ℝ 2 ] -- ^ \( \left ( q_i \right )_{i=1}^n \), points to fit + -> ℝ 2 -- ^ \( r \), end point + -> T ( ℝ 2 ) -- ^ \( \textrm{t}_r \), end tangent vector (length is ignored) + -> ( Cubic.Bezier ( ℝ 2 ), [ Double ], ArgMax Double Double ) +fitPiece dist_tol t_tol maxIters p tp qs r tr = + runST do + -- Initialise the parameter values to a uniform subdivision. + ts <- unsafeThawPrimArray $ generatePrimArray n uniform + loop ts 0 + where + n :: Int + n = length qs + uniform :: Int -> Double + uniform i = fromIntegral ( i + 1 ) / fromIntegral ( n + 1 ) + + f0, f1, f2, f3 :: Double -> T ( ℝ 2 ) + f0 t = h1 t *^ tp + f1 t = h2 t *^ tr + f2 t = h0 t *^ ( T p ) + f3 t = h3 t *^ ( T r ) + + loop :: forall s. MutablePrimArray s Double -> Int -> ST s ( Cubic.Bezier ( ℝ 2 ), [ Double ], ArgMax Double Double ) + loop ts count = do + let + hermiteParameters :: Mat22 -> T ( ℝ 2 ) -> Int -> [ ℝ 2 ] -> ST s ( T ( ℝ 2 ) ) + hermiteParameters ( Mat22 a11 a12 _ a22 ) ( V2 b1 b2 ) i ( q : rest ) = do + ti <- readPrimArray ts i + let + f0i, f1i, f2i, f3i :: T ( ℝ 2 ) + f0i = f0 ti + f1i = f1 ti + f2i = f2 ti + f3i = f3 ti + q' = T q ^-^ f2i ^-^ f3i + a11', a12', a21', a22', b1', b2' :: Double + a11' = a11 + ( f0i ^.^ f0i ) + a12' = a12 + ( f1i ^.^ f0i ) + a21' = a12' + a22' = a22 + ( f1i ^.^ f1i ) + b1' = b1 + ( q' ^.^ f0i ) + b2' = b2 + ( q' ^.^ f1i ) + hermiteParameters ( Mat22 a11' a12' a21' a22' ) ( V2 b1' b2' ) ( i + 1 ) rest + hermiteParameters a b _ [] = pure ( linearSolve a b ) + + ~(V2 s1 s2) <- hermiteParameters ( Mat22 0 0 0 0 ) ( V2 0 0 ) 0 qs + + let + -- Convert from Hermite form to Bézier form. + cp1, cp2 :: ℝ 2 + cp1 = ( ( s1 / 3 ) *^ tp ) • p + cp2 = ( ( -s2 / 3 ) *^ tr ) • r + + bez :: Cubic.Bezier ( ℝ 2 ) + bez = Cubic.Bezier p cp1 cp2 r + + -- Run one iteration of Laguerre's method to improve the parameter values t_i, + -- so that t_i' is a better approximation of the parameter + -- at which the curve is closest to q_i. + ( dts_changed, argmax_sq_dist ) <- ( `execStateT` ( False, Max ( Arg 0 0 ) ) ) $ for_ ( zip qs [ 0 .. ] ) \( q, i ) -> do + ti <- lift ( readPrimArray ts i ) + let + laguerreStepResult :: Complex Double + laguerreStepResult = runST do + coeffs <- unsafeThawPrimArray . primArrayFromListN 6 + $ Cubic.ddist @( T ( ℝ 2 ) ) bez q + laguerre epsilon 1 coeffs ( ti :+ 0 ) + ti' <- case laguerreStepResult of + x :+ y + | isNaN x + || isNaN y + || abs y > epsilon + || x < -epsilon + || x > 1 + epsilon + -> modify' ( first ( const True ) ) $> ti + | otherwise + -> when ( abs ( x - ti ) > t_tol ) + ( modify' ( first ( const True ) ) ) + $> ( min 1 $ max 0 x ) + let + sq_dist :: Double + sq_dist = quadrance @( T ( ℝ 2 ) ) q ( Cubic.bezier @( T ( ℝ 2 ) ) bez ti' ) + modify' ( second ( <> Max ( Arg sq_dist ti' ) ) ) + lift ( writePrimArray ts i ti' ) + + case argmax_sq_dist of + Max ( Arg max_sq_dist _ ) + | count < maxIters + && ( dts_changed || max_sq_dist > dist_tol ^ ( 2 :: Int ) ) + -> loop ts ( count + 1 ) + _ -> do + tsList <- primArrayToList <$> unsafeFreezePrimArray ts + pure ( bez, tsList, argmax_sq_dist ) + +-- | Cubic Hermite polynomial. +h0, h1, h2, h3 :: Num t => t -> t +h0 t = ( 1 + 2 * t ) * ( t - 1 ) ^ ( 2 :: Int ) +h1 t = t * ( t - 1 ) ^ ( 2 :: Int ) +h2 t = t ^ ( 2 :: Int ) * ( t - 1 ) +h3 t = t ^ ( 2 :: Int ) * ( 3 - 2 * t )
+ src/lib/Math/Bezier/Quadratic.hs view
@@ -0,0 +1,238 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Bezier.Quadratic + ( Bezier(..) + , bezier, bezier', bezier'' + , curvature, squaredCurvature, signedCurvature + , subdivide, restrict + , ddist, closestPoint + , interpolate + , extrema + , speedCriticalPoints + ) + where + +-- base +import Data.List.NonEmpty + ( NonEmpty(..) ) +import Data.Monoid + ( Ap(..) ) +import Data.Semigroup + ( ArgMin, Min(..), Arg(..) ) +import GHC.Generics + ( Generic, Generic1 + , Generically(..), Generically1(..) + ) + +-- acts +import Data.Act + ( Act(..) + , Torsor + ( (-->) ) + ) + +-- deepseq +import Control.DeepSeq + ( NFData, NFData1 ) + +-- groups +import Data.Group + ( Group ) + +-- groups-generic +import Data.Group.Generics + () + +-- primitive +import Data.Primitive.Types + ( Prim ) + +-- brush-strokes +import Math.Epsilon + ( epsilon ) +import Math.Module + ( Module (..) + , lerp + , Inner(..), norm, squaredNorm + , Cross((×)) + ) +import Math.Roots + ( realRoots ) +import Math.Linear + ( ℝ(..), T(..) ) +import qualified Math.Ring as Ring + +-------------------------------------------------------------------------------- + +-- | Points defining a quadratic Bézier curve (Bernstein form). +-- +-- @ p0 @ and @ p2 @ are endpoints, whereas @ p1 @ is a control point. +data Bezier p + = Bezier + { p0, p1, p2 :: !p } + deriving stock ( Generic, Generic1, Functor, Foldable, Traversable ) + deriving ( Semigroup, Monoid, Group ) + via Generically ( Bezier p ) + deriving Applicative + via Generically1 Bezier + deriving anyclass ( NFData, NFData1 ) + +deriving via Ap Bezier p + instance {-# OVERLAPPING #-} Act v p => Act v ( Bezier p ) +deriving via Ap Bezier ( T b ) + instance Module r ( T b ) => Module r ( T ( Bezier b ) ) + +instance Show p => Show (Bezier p) where + show (Bezier p1 p2 p3) = + show p1 ++ "--" ++ show p2 ++ "->" ++ show p3 + +-- | Quadratic Bézier curve. +bezier :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> p +bezier ( Bezier {..} ) t = lerp @v t ( lerp @v t p0 p1 ) ( lerp @v t p1 p2 ) +{-# INLINEABLE bezier #-} + +-- | Derivative of a quadratic Bézier curve. +bezier' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> v +bezier' ( Bezier {..} ) t = Ring.fromInteger 2 *^ lerp @v t ( p0 --> p1 ) ( p1 --> p2 ) +{-# INLINEABLE bezier' #-} + +-- | Second derivative of a quadratic Bézier curve. +bezier'' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> v +bezier'' ( Bezier {..} ) = Ring.fromInteger 2 *^ ( p1 --> p0 ^+^ p1 --> p2 ) +{-# INLINEABLE bezier'' #-} + +-- | Curvature of a quadratic Bézier curve. +curvature :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> r -> r +curvature bez t = sqrt $ squaredCurvature @v bez t +{-# INLINEABLE curvature #-} + +-- | Square of curvature of a quadratic Bézier curve. +squaredCurvature :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> r -> r +squaredCurvature bez t + | sq_nm_g' < epsilon + = 1 / 0 + | otherwise + = ( sq_nm_g' * squaredNorm @v g'' - ( g' ^.^ g'' ) ^ ( 2 :: Int ) ) + / ( sq_nm_g' ^ ( 3 :: Int ) ) + where + g', g'' :: v + !g' = bezier' @v bez t + !g'' = bezier'' @v bez + sq_nm_g' :: r + !sq_nm_g' = squaredNorm @v g' +{-# INLINEABLE squaredCurvature #-} + +-- | Signed curvature of a planar quadratic Bézier curve. +signedCurvature :: Bezier ( ℝ 2 ) -> Double -> Double +signedCurvature bez t = ( g' × g'' ) / norm g' ^ ( 3 :: Int ) + where + g', g'' :: T ( ℝ 2 ) + !g' = bezier' @( T ( ℝ 2 ) ) bez t + !g'' = bezier'' @( T ( ℝ 2 ) ) bez + +-- | Subdivide a quadratic Bézier curve into two parts. +subdivide :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> ( Bezier p, Bezier p ) +subdivide ( Bezier {..} ) t = ( Bezier p0 q1 pt, Bezier pt r1 p2 ) + where + pt, q1, r1 :: p + !q1 = lerp @v t p0 p1 + !r1 = lerp @v t p1 p2 + !pt = lerp @v t q1 r1 +{-# INLINEABLE subdivide #-} + +-- | Restrict a quadratic Bézier curve to a sub-interval, re-parametrising +-- to \( [0,1] \). +restrict :: forall v r p. ( Torsor v p, Ring.Field r, Module r v ) => Bezier p -> ( r , r ) -> Bezier p +restrict bez ( a, b ) = fst $ ( flip ( subdivide @v ) b' ) $ snd $ subdivide @v bez a + where + !b' = ( b Ring.- a ) Ring./ ( Ring.fromInteger 1 Ring.- a ) + -- TODO: this could be made more efficient. + -- See e.g. "https://math.stackexchange.com/questions/4172835/cubic-b%C3%A9zier-spline-multiple-split" + -- or the paper "On the numerical condition of Bernstein-Bézier subdivision process". +{-# INLINEABLE restrict #-} + +-- | Polynomial coefficients of the derivative of the distance to a quadratic Bézier curve. +ddist :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> [ r ] +ddist ( Bezier {..} ) c = [ a3, a2, a1, a0 ] + where + v, v', v'' :: v + !v = c --> p0 + !v' = p0 --> p1 + !v'' = p1 --> p0 ^+^ p1 --> p2 + + a0, a1, a2, a3 :: r + !a0 = v ^.^ v' + !a1 = v ^.^ v'' + 2 * squaredNorm v' + !a2 = 3 * v' ^.^ v'' + !a3 = squaredNorm v'' +{-# INLINEABLE ddist #-} + +-- | Finds the closest point to a given point on a quadratic Bézier curve. +closestPoint + :: forall v r p. ( Torsor v p, Inner r v, RealFloat r, Prim r, NFData r ) + => Bezier p -> p -> ArgMin r ( r, p ) +closestPoint pts c = pickClosest ( 0 :| 1 : roots ) + where + roots :: [ r ] + roots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots 50 $ ddist @v pts c ) + + pickClosest :: NonEmpty r -> ArgMin r ( r, p ) + pickClosest ( s :| ss ) = go s q nm0 ss + where + q :: p + !q = bezier @v pts s + nm0 :: r + !nm0 = squaredNorm ( c --> q :: v ) + go t p nm [] = Min ( Arg nm ( t, p ) ) + go t p nm ( t' : ts ) + | nm' < nm = go t' p' nm' ts + | otherwise = go t p nm ts + where + p' :: p + !p' = bezier @v pts t' + nm' :: r + !nm' = squaredNorm ( c --> p' :: v ) +{-# INLINEABLE closestPoint #-} + +-- | Interpolation of a quadratic Bézier control point, given path points and Bézier parameter. +-- +-- That is, given points `p0`, `p2`, and `q`, and parameter \( 0 < t < 1 \), +-- this function finds the unique control point `p1` such that +-- the quadratic Bézier curve with parameters `p0`, `p1`, `p2` passes through +-- the point `q` at time `t`. +interpolate :: forall v r p. ( Torsor v p, Module r v, Fractional r ) => p -> p -> r -> p -> Bezier p +interpolate p0 p2 t q = Bezier {..} + where + p1 :: p + !p1 = ( ( 0.5 * ( t - 1 ) / t ) *^ ( q --> p0 :: v ) + ^+^ ( 0.5 * t / ( t - 1 ) ) *^ ( q --> p2 :: v ) + ) • q +{-# INLINEABLE interpolate #-} + +-- | Critical points of the speed @|B′(t)|@ in @(0,1)@ for a quadratic Bézier. +-- +-- These are the roots of @d\/dt |B′(t)|² = 0@, i.e. where the hodograph +-- @H(t) = d₀ + t·f@ is perpendicular to its (constant) derivative @f = d₁ − d₀@. +speedCriticalPoints + :: forall diff ptData + . ( Torsor diff ptData, Module Double diff, Inner Double diff ) + => Bezier ptData -> [ Double ] +speedCriticalPoints ( Bezier {..} ) = + let + d0, d1 :: diff + !d0 = p0 --> p1 + !d1 = p1 --> p2 + !f = d1 ^-^ d0 + !ff = f ^.^ f + !tc = -(f ^.^ d0) / ff + in [ tc | ff > 0, tc > 0, tc < 1 ] +{-# INLINEABLE speedCriticalPoints #-} + +-- | Extremal values of the Bézier parameter for a quadratic Bézier curve. +extrema :: Fractional r => Bezier r -> [ r ] +extrema ( Bezier {..} ) = [ t ] + where + !t = ( p0 - p1 ) / ( p0 - 2 * p1 + p2 ) +{-# INLINEABLE extrema #-}
+ src/lib/Math/Bezier/Spline.hs view
@@ -0,0 +1,659 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE QuantifiedConstraints #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Bezier.Spline where + +-- base +import Control.Monad + ( guard ) +import Data.Bifoldable + ( Bifoldable(..) ) +import Data.Bifunctor + ( Bifunctor(..) ) +import Data.Bitraversable + ( Bitraversable(..) ) +import Data.Coerce + ( coerce ) +import Data.Functor.Const + ( Const(..) ) +import Data.Functor.Identity + ( Identity(..) ) +import Data.Kind + ( Type, Constraint ) +import Data.Monoid + ( Ap(..) ) +import Data.Semigroup + ( First(..) ) +import GHC.Generics + ( Generic, Generic1, Generically1(..) ) + +-- bifunctors +import qualified Data.Bifunctor.Tannen as Biff + ( Tannen(..) ) + +-- containers +import Data.Sequence + ( Seq(..) ) +import qualified Data.Sequence as Seq + ( drop, length, singleton, splitAt ) + +-- deepseq +import Control.DeepSeq + ( NFData(..), NFData1, deepseq ) + +-- generic-lens +import Data.Generics.Product.Fields + ( field ) +import Data.Generics.Internal.VL + ( set ) + +-- transformers +import Control.Monad.Trans.Class + ( lift ) +import Control.Monad.Trans.State.Strict + ( StateT(runStateT), modify' ) + +-- acts +import Data.Act + ( Torsor (..) ) + +-- brush-strokes +import qualified Math.Bezier.Cubic as Cubic + ( Bezier(..), subdivide ) +import qualified Math.Bezier.Quadratic as Quadratic + ( Bezier(..), subdivide ) +import Math.Linear + ( ℝ(..) ) +import Math.Module + ( Module, lerp ) + + +-------------------------------------------------------------------------------- + +data PointType + = PathPoint + | ControlPoint ControlPoint + deriving stock ( Eq, Ord, Show, Generic ) + deriving anyclass NFData + +data ControlPoint + = Bez2Cp + | Bez3Cp1 + | Bez3Cp2 + deriving stock ( Eq, Ord, Show, Generic ) + deriving anyclass NFData + +-------------------------------------------------------------------------------- + +data SplineType = Open | Closed + +data SSplineType ( clo :: SplineType ) where + SOpen :: SSplineType Open + SClosed :: SSplineType Closed + +class ( Traversable ( NextPoint clo ) + , forall crvData. Traversable ( Curves clo crvData ) + , Bitraversable ( Curves clo ) + , forall ptData. Show ptData => Show ( NextPoint clo ptData ) + , forall ptData crvData. ( Show ptData, Show crvData ) => Show ( Curves clo crvData ptData ) + , forall ptData. NFData ptData => NFData ( NextPoint clo ptData ) + , forall ptData crvData. ( NFData ptData, NFData crvData ) => NFData ( Curves clo crvData ptData ) + ) + => SplineTypeI ( clo :: SplineType ) where + -- | Singleton for the spline type + ssplineType :: SSplineType clo +instance SplineTypeI Open where + ssplineType = SOpen +instance SplineTypeI Closed where + ssplineType = SClosed + +type NextPoint :: SplineType -> Type -> Type +data family NextPoint clo +newtype instance NextPoint Open ptData = NextPoint { nextPoint :: ptData } + deriving stock ( Show, Generic, Generic1, Functor, Foldable, Traversable ) + deriving anyclass ( NFData, NFData1 ) + deriving Applicative + via ( Generically1 ( NextPoint Open ) ) +data instance NextPoint Closed ptData = BackToStart + deriving stock ( Show, Generic, Generic1, Functor, Foldable, Traversable ) + deriving anyclass ( NFData, NFData1 ) + deriving Applicative + via ( Generically1 ( NextPoint Closed ) ) + +fromNextPoint :: forall clo ptData. SplineTypeI clo => ptData -> NextPoint clo ptData -> ptData +fromNextPoint pt nxt + | SOpen <- ssplineType @clo + = case nxt of { NextPoint q -> q } + | otherwise + = pt + +toNextPoint :: forall clo ptData. SplineTypeI clo => ptData -> NextPoint clo ptData +toNextPoint pt = case ssplineType @clo of + SOpen -> NextPoint pt + SClosed -> BackToStart + +type Curve :: SplineType -> Type -> Type -> Type +data Curve clo crvData ptData + = LineTo + { curveEnd :: !( NextPoint clo ptData ) + , curveData :: !crvData + } + | Bezier2To + { controlPoint :: !ptData + , curveEnd :: !( NextPoint clo ptData ) + , curveData :: !crvData + } + | Bezier3To + { controlPoint1 :: !ptData + , controlPoint2 :: !ptData + , curveEnd :: !( NextPoint clo ptData ) + , curveData :: !crvData + } + deriving stock ( Generic, Generic1 ) + +deriving stock instance ( Show ptData, Show crvData, Show ( NextPoint clo ptData ) ) => Show ( Curve clo crvData ptData ) +deriving anyclass instance ( NFData ptData, NFData crvData, NFData ( NextPoint clo ptData ) ) => NFData ( Curve clo crvData ptData ) + +deriving stock instance Functor ( NextPoint clo ) => Functor ( Curve clo crvData ) +deriving stock instance Foldable ( NextPoint clo ) => Foldable ( Curve clo crvData ) +deriving stock instance Traversable ( NextPoint clo ) => Traversable ( Curve clo crvData ) + +instance Functor ( NextPoint clo ) => Bifunctor ( Curve clo ) where + bimap f g ( LineTo np d ) = LineTo ( fmap g np ) ( f d ) + bimap f g ( Bezier2To cp np d ) = Bezier2To ( g cp ) ( fmap g np ) ( f d ) + bimap f g ( Bezier3To cp1 cp2 np d ) = Bezier3To ( g cp1 ) ( g cp2 ) ( fmap g np ) ( f d ) +instance Foldable ( NextPoint clo ) => Bifoldable ( Curve clo ) where + bifoldMap f g ( LineTo np d ) = foldMap g np <> f d + bifoldMap f g ( Bezier2To cp np d ) = g cp <> foldMap g np <> f d + bifoldMap f g ( Bezier3To cp1 cp2 np d ) = g cp1 <> g cp2 <> foldMap g np <> f d +instance Traversable ( NextPoint clo ) => Bitraversable ( Curve clo ) where + bitraverse f g ( LineTo np d ) = LineTo <$> traverse g np <*> f d + bitraverse f g ( Bezier2To cp np d ) = Bezier2To <$> g cp <*> traverse g np <*> f d + bitraverse f g ( Bezier3To cp1 cp2 np d ) = Bezier3To <$> g cp1 <*> g cp2 <*> traverse g np <*> f d + +openCurveEnd :: Curve Open crvData ptData -> ptData +openCurveEnd = nextPoint . curveEnd + +openCurveStart :: Curve Open crvData ptData -> ptData +openCurveStart ( LineTo ( NextPoint p ) _ ) = p +openCurveStart ( Bezier2To p _ _ ) = p +openCurveStart ( Bezier3To p _ _ _ ) = p + +type Curves :: SplineType -> Type -> Type -> Type +data family Curves clo + +newtype instance Curves Open crvData ptData + = OpenCurves { openCurves :: Seq ( Curve Open crvData ptData ) } + deriving stock ( Show, Generic, Generic1, Functor, Foldable, Traversable ) + deriving newtype ( Semigroup, Monoid, NFData ) + +deriving via Biff.Tannen Seq ( Curve Open ) + instance Bifunctor ( Curves Open ) +deriving via Biff.Tannen Seq ( Curve Open ) + instance Bifoldable ( Curves Open ) +instance Bitraversable ( Curves Open ) where + bitraverse f g ( OpenCurves { openCurves = curves } ) + = OpenCurves <$> traverse ( bitraverse f g ) curves + +data instance Curves Closed crvData ptData + = NoCurves + | ClosedCurves + { prevOpenCurves :: !( Seq ( Curve Open crvData ptData ) ) + , lastClosedCurve :: !( Curve Closed crvData ptData ) + } + deriving stock ( Show, Generic, Generic1, Functor, Foldable, Traversable ) + deriving anyclass NFData + +nbCurves :: Curves Closed crvData ptData -> Int +nbCurves NoCurves = 0 +nbCurves ( ClosedCurves { prevOpenCurves } ) = 1 + Seq.length prevOpenCurves + +instance Bifunctor ( Curves Closed ) where + bimap _ _ NoCurves = NoCurves + bimap f g ( ClosedCurves p l ) = ClosedCurves ( fmap ( bimap f g ) p ) ( bimap f g l ) +instance Bifoldable ( Curves Closed ) where + bifoldMap _ _ NoCurves = mempty + bifoldMap f g ( ClosedCurves p l ) = foldMap ( bifoldMap f g ) p <> bifoldMap f g l +instance Bitraversable ( Curves Closed ) where + bitraverse _ _ NoCurves = pure NoCurves + bitraverse f g ( ClosedCurves p l ) + = ClosedCurves <$> ( traverse ( bitraverse f g ) p ) <*> bitraverse f g l + + +data Spline ( clo :: SplineType ) crvData ptData + = Spline + { splineStart :: !ptData + , splineCurves :: !( Curves clo crvData ptData ) + } + deriving stock ( Generic, Generic1 ) + +deriving stock instance ( Show ptData, Show ( Curves clo crvData ptData ) ) + => Show ( Spline clo crvData ptData ) +deriving anyclass instance ( NFData ptData, NFData ( Curves clo crvData ptData ) ) + => NFData ( Spline clo crvData ptData ) +deriving stock instance Functor ( Curves clo crvData ) => Functor ( Spline clo crvData ) +deriving stock instance Foldable ( Curves clo crvData ) => Foldable ( Spline clo crvData ) +deriving stock instance Traversable ( Curves clo crvData ) => Traversable ( Spline clo crvData ) + +instance KnownSplineType clo => Bifunctor ( Spline clo ) where + bimap fc fp = bimapSpline ( const $ bimap fc fp ) fp +instance KnownSplineType clo => Bifoldable ( Spline clo ) where + bifoldMap fc fp = runIdentity . bifoldSpline @_ @Identity ( const $ bifoldMap ( coerce fc ) ( coerce fp ) ) ( coerce fp ) +instance KnownSplineType clo => Bitraversable ( Spline clo ) where + bitraverse fc fp = bitraverseSpline ( const $ bitraverse fc fp ) fp + +type SplinePts clo = Spline clo () ( ℝ 2 ) + +bimapCurve + :: Functor ( NextPoint clo ) + => ( crvData -> crvData' ) -> ( PointType -> ptData -> ptData' ) + -> Curve clo crvData ptData -> Curve clo crvData' ptData' +bimapCurve f g ( LineTo p1 d ) = LineTo ( g PathPoint <$> p1 ) ( f d ) +bimapCurve f g ( Bezier2To p1 p2 d ) = Bezier2To ( g ( ControlPoint Bez2Cp ) p1 ) ( g PathPoint <$> p2 ) ( f d ) +bimapCurve f g ( Bezier3To p1 p2 p3 d ) = Bezier3To ( g ( ControlPoint Bez3Cp1 ) p1 ) ( g ( ControlPoint Bez3Cp2 ) p2 ) ( g PathPoint <$> p3 ) ( f d ) + +bifoldMapCurve + :: forall m clo crvData ptData + . ( Monoid m, Foldable ( NextPoint clo ) ) + => ( crvData -> m ) -> ( PointType -> ptData -> m ) + -> Curve clo crvData ptData -> m +bifoldMapCurve f g ( LineTo p1 d ) = ( foldMap ( g PathPoint ) p1 ) <> f d +bifoldMapCurve f g ( Bezier2To p1 p2 d ) = g ( ControlPoint Bez2Cp ) p1 <> ( foldMap ( g PathPoint ) p2 ) <> f d +bifoldMapCurve f g ( Bezier3To p1 p2 p3 d ) = g ( ControlPoint Bez3Cp1 ) p1 <> g ( ControlPoint Bez3Cp2 ) p2 <> ( foldMap ( g PathPoint ) p3 ) <> f d + +bitraverseCurve + :: forall f clo crvData crvData' ptData ptData' + . ( Applicative f, Traversable ( NextPoint clo ) ) + => ( crvData -> f crvData' ) -> ( PointType -> ptData -> f ptData' ) + -> Curve clo crvData ptData -> f ( Curve clo crvData' ptData' ) +bitraverseCurve f g ( LineTo p1 d ) = LineTo <$> traverse ( g PathPoint ) p1 <*> f d +bitraverseCurve f g ( Bezier2To p1 p2 d ) = Bezier2To <$> g ( ControlPoint Bez2Cp ) p1 <*> traverse ( g PathPoint ) p2 <*> f d +bitraverseCurve f g ( Bezier3To p1 p2 p3 d ) = Bezier3To <$> g ( ControlPoint Bez3Cp1 ) p1 <*> g ( ControlPoint Bez3Cp2 ) p2 <*> traverse ( g PathPoint ) p3 <*> f d + +dropCurves :: Int -> Spline Open crvData ptData -> Maybe ( Spline Open crvData ptData ) +dropCurves i spline@( Spline { splineCurves = OpenCurves curves } ) + | i < 1 + = Just spline + | otherwise + = case Seq.drop ( i - 1 ) curves of + prev :<| next -> Just $ Spline { splineStart = openCurveEnd prev, splineCurves = OpenCurves next } + Empty -> Nothing + +splitSplineAt :: Int -> Spline Open crvData ptData -> ( Spline Open crvData ptData, Spline Open crvData ptData ) +splitSplineAt i ( Spline { splineStart, splineCurves = OpenCurves curves } ) = case Seq.splitAt i curves of + ( Empty, next ) -> + ( Spline { splineStart, splineCurves = OpenCurves Empty }, Spline { splineStart, splineCurves = OpenCurves next } ) + ( prev@( _ :|> lastPrev ), next ) -> + ( Spline { splineStart, splineCurves = OpenCurves prev }, Spline { splineStart = openCurveEnd lastPrev, splineCurves = OpenCurves next } ) + +-- | Subdivide a single curve at the given Bézier parameter. +subdivideCurveAt + :: forall diff ptData crvData + . ( Torsor diff ptData, Module Double diff ) + => ptData + -> Curve Open crvData ptData + -> Double -- ^ Bézier parameter of split + -> ( Curve Open crvData ptData, ptData, Curve Open crvData ptData ) +subdivideCurveAt p0 curve t = case curve of + LineTo { curveEnd = NextPoint p1, curveData } -> + let splitPt = lerp @diff t p0 p1 + in ( LineTo { curveEnd = NextPoint splitPt, curveData } + , splitPt + , LineTo { curveEnd = NextPoint p1, curveData } + ) + Bezier2To { controlPoint = p1, curveEnd = NextPoint p2, curveData } -> + let ( Quadratic.Bezier _ q1 splitPt, Quadratic.Bezier _ r1 _ ) = + Quadratic.subdivide @diff ( Quadratic.Bezier { p0, p1, p2 } ) t + in ( Bezier2To { controlPoint = q1, curveEnd = NextPoint splitPt, curveData } + , splitPt + , Bezier2To { controlPoint = r1, curveEnd = NextPoint p2, curveData } + ) + Bezier3To { controlPoint1 = p1, controlPoint2 = p2, curveEnd = NextPoint p3, curveData } -> + let ( Cubic.Bezier _ q1 q2 splitPt, Cubic.Bezier _ r1 r2 _ ) = + Cubic.subdivide @diff ( Cubic.Bezier { p0, p1, p2, p3 } ) t + in ( Bezier3To { controlPoint1 = q1, controlPoint2 = q2, curveEnd = NextPoint splitPt, curveData } + , splitPt + , Bezier3To { controlPoint1 = r1, controlPoint2 = r2, curveEnd = NextPoint p3, curveData } + ) + +-- | Subdivide a spline at multiple positions, each given as a +-- @(curveIndex, t)@ pair (assumed sorted and without duplicates). +subdivideSplineAt + :: forall diff ptData crvData + . ( Torsor diff ptData, Module Double diff ) + => Spline Open crvData ptData + -> [ ( Int, Double ) ] -- ^ @(curveIndex, t)@, where @t@ is the Bézier parameter for that curve + -> [ Spline Open crvData ptData ] +subdivideSplineAt ( Spline { splineStart, splineCurves = OpenCurves curves } ) splits = + go splineStart splineStart Empty 0 0 curves splits + where + go :: ptData + -> ptData + -> Seq ( Curve Open crvData ptData ) + -> Int -> Double + -> Seq ( Curve Open crvData ptData ) + -> [ ( Int, Double ) ] + -> [ Spline Open crvData ptData ] + go segStart _curPt acc _idx _tPrev cs [] = + -- No more splits: absorb all remaining curves into the current segment. + [ Spline { splineStart = segStart, splineCurves = OpenCurves ( acc <> cs ) } ] + go segStart curPt acc idx tPrev cs ( ( i, t ) : rest_splits ) = + case cs of + Empty -> + -- No more curves (shouldn't happen for valid split positions). + [ Spline { splineStart = segStart, splineCurves = OpenCurves acc } ] + c :<| rest + | i > idx + -> -- No split in this curve; add it to the accumulator and + -- go to the next curve, starting with Bézier parameter t=0. + go segStart ( openCurveEnd c ) ( acc :|> c ) ( idx + 1 ) 0 rest + ( ( i, t ) : rest_splits ) + | otherwise + -> -- Split falls in this curve: reparametrise the Bézier parameter + -- for further splits. + let t' = ( t - tPrev ) / ( 1 - tPrev ) + ( leftC, splitPt, rightC ) = subdivideCurveAt @diff curPt c t' + in Spline { splineStart = segStart, splineCurves = OpenCurves ( acc :|> leftC ) } + : go splitPt splitPt Empty idx t ( rightC :<| rest ) rest_splits + -- NB: pass t, not t', here. + +reverseSpline :: forall crvData ptData. Spline Open crvData ptData -> Spline Open crvData ptData +reverseSpline spline@( Spline { splineStart = p0, splineCurves = OpenCurves curves } ) = case curves of + Empty -> spline + prev :|> lst -> Spline { splineStart = openCurveEnd lst, splineCurves = OpenCurves ( go prev lst ) } + where + go :: Seq ( Curve Open crvData ptData ) -> Curve Open crvData ptData -> Seq ( Curve Open crvData ptData ) + go Empty ( LineTo _ dat ) = LineTo ( NextPoint p0 ) dat :<| Empty + go Empty ( Bezier2To p1 _ dat ) = Bezier2To p1 ( NextPoint p0 ) dat :<| Empty + go Empty ( Bezier3To p1 p2 _ dat ) = Bezier3To p2 p1 ( NextPoint p0 ) dat :<| Empty + go ( crvs :|> crv ) ( LineTo _ dat ) = LineTo ( curveEnd crv ) dat :<| go crvs crv + go ( crvs :|> crv ) ( Bezier2To p1 _ dat ) = Bezier2To p1 ( NextPoint $ openCurveEnd crv ) dat :<| go crvs crv + go ( crvs :|> crv ) ( Bezier3To p1 p2 _ dat ) = Bezier3To p2 p1 ( NextPoint $ openCurveEnd crv ) dat :<| go crvs crv + +splineEnd :: Spline Open crvData ptData -> ptData +splineEnd ( Spline { splineStart, splineCurves = OpenCurves curves } ) = case curves of + Empty -> splineStart + _ :|> lastCurve -> openCurveEnd lastCurve + +catMaybesSpline :: ( ptData -> ptData -> Bool ) -> crvData -> ptData -> Maybe ptData -> Maybe ptData -> ptData -> Spline Open crvData ptData +catMaybesSpline eq dat p0 mbCp1 mbCp2 p3 = + let + mbCp1' = do { cp1 <- mbCp1; guard ( not $ eq cp1 p0 ); return cp1 } + mbCp2' = do { cp2 <- mbCp2; guard ( not $ eq cp2 p3 ); return cp2 } + mbCp1'' = do { cp1 <- mbCp1'; guard ( case mbCp2' of { Just {} -> True ; Nothing -> not ( eq cp1 p3 ) } ); return cp1 } + mbCp2'' = do { cp2 <- mbCp2'; guard ( case mbCp1' of { Just {} -> True ; Nothing -> not ( eq cp2 p0 ) } ); return cp2 } + in go mbCp1'' mbCp2'' + where + go Nothing Nothing = Spline { splineStart = p0, splineCurves = OpenCurves $ Seq.singleton ( LineTo ( NextPoint p3 ) dat ) } + go ( Just p1 ) Nothing = Spline { splineStart = p0, splineCurves = OpenCurves $ Seq.singleton ( Bezier2To p1 ( NextPoint p3 ) dat ) } + go Nothing ( Just p2 ) = Spline { splineStart = p0, splineCurves = OpenCurves $ Seq.singleton ( Bezier2To p2 ( NextPoint p3 ) dat ) } + go ( Just p1 ) ( Just p2 ) = Spline { splineStart = p0, splineCurves = OpenCurves $ Seq.singleton ( Bezier3To p1 p2 ( NextPoint p3 ) dat ) } + +-- | Connect two open curves. +-- +-- It is assumed (not checked) that the end of the first curve is the start of the second curve. +instance Semigroup ( Spline Open crvData ptData ) where + spline1@( Spline { splineStart, splineCurves = segs1 } ) <> spline2@( Spline { splineCurves = segs2 } ) + | null segs1 = spline2 + | null segs2 = spline1 + | otherwise = Spline { splineStart, splineCurves = segs1 <> segs2 } + +-- | Create a curve containing a single (open) cubic Bézier segment. +openCubicBezierCurveSpline :: crvData -> Cubic.Bezier ptData -> Spline Open crvData ptData +openCubicBezierCurveSpline crvData ( Cubic.Bezier {..} ) + = Spline + { splineStart = p0 + , splineCurves = OpenCurves . Seq.singleton $ + Bezier3To + { controlPoint1 = p1 + , controlPoint2 = p2 + , curveEnd = NextPoint p3 + , curveData = crvData + } + } + +-- | Drop the end of an open curve segment to create a "closed" curve (i.e. one that returns to the start). +dropCurveEnd :: Curve Open crvData ptData -> Curve Closed crvData ptData +dropCurveEnd ( LineTo _ dat ) = LineTo BackToStart dat +dropCurveEnd ( Bezier2To cp _ dat ) = Bezier2To cp BackToStart dat +dropCurveEnd ( Bezier3To cp1 cp2 _ dat ) = Bezier3To cp1 cp2 BackToStart dat + +-- | A 'Maybe' value which keeps track of whether it is 'Nothing' or 'Just' with a type-level boolean. +-- +-- This is used to enable writing 'biwitherSpline' with a dependent type signature, +-- as the result type depends on whether a starting point has been found yet or not. +data CurrentStart ( hasStart :: Bool ) ptData where + NoStartFound :: CurrentStart False ptData + CurrentStart :: + { wasOriginalStart :: Bool + , startPoint :: !ptData + } -> CurrentStart True ptData + +deriving stock instance Show ptData => Show ( CurrentStart hasStart ptData ) +deriving stock instance Functor ( CurrentStart hasStart ) +deriving stock instance Foldable ( CurrentStart hasStart ) +deriving stock instance Traversable ( CurrentStart hasStart ) +instance NFData ptData => NFData ( CurrentStart hasStart ptData ) where + rnf NoStartFound = () + rnf ( CurrentStart orig ptData ) = rnf orig `seq` rnf ptData + +-- | The result of a wither operation on a spline. +-- +-- - When a starting point has been found, we can choose whether to keep the current segment or not. +-- - When a starting point has not yet been found, we first need to determine a new start point, +-- before we can consider keeping or dismissing the curve. +data WitherResult ( hasStart :: Bool ) ( clo' :: SplineType ) crvData' ptData' where + Dismiss :: WitherResult hasStart clo' crvData' ptData' + UseStartPoint :: ptData' -> Maybe ( Curve clo' crvData' ptData' ) -> WitherResult False clo' crvData' ptData' + UseCurve :: Curve clo' crvData' ptData' -> WitherResult True clo' crvData' ptData' + +deriving stock instance ( Show ptData', Show crvData', SplineTypeI clo' ) => Show ( WitherResult hasStart clo' crvData' ptData' ) +instance ( NFData ptData', NFData crvData', SplineTypeI clo' ) => NFData ( WitherResult hasStart clo' crvData' ptData' ) where + rnf Dismiss = () + rnf ( UseStartPoint ptData mbCurve ) = ptData `deepseq` mbCurve `deepseq` () + rnf ( UseCurve curve ) = rnf curve + +class SplineTypeI clo => KnownSplineType clo where + + type TraversalCt clo ( clo' :: SplineType ) :: Constraint + + -- | Last point of a spline. + lastPoint :: Spline clo crvData ptData -> ptData + + -- | Close a spline if necessary. + adjustSplineType :: forall clo' crvData ptData. SplineTypeI clo' => Spline clo' crvData ptData -> Spline clo crvData ptData + + -- | Indexed traversal of a spline. + ibitraverseSpline + :: forall f crvData ptData crvData' ptData' + . Applicative f + => ( forall clo'. ( TraversalCt clo clo', SplineTypeI clo' ) + => Int -> ptData -> Curve clo' crvData ptData -> f ( Curve clo' crvData' ptData' ) + ) + -> ( ptData -> f ptData' ) + -> Spline clo crvData ptData + -> f ( Spline clo crvData' ptData' ) + + -- | Bi-witherable traversal of a spline. + biwitherSpline + :: forall f crvData ptData crvData' ptData' + . Monad f + => ( forall clo' hasStart. ( TraversalCt clo clo', SplineTypeI clo' ) + => CurrentStart hasStart ptData' -> Curve clo' crvData ptData -> f ( WitherResult hasStart clo' crvData' ptData' ) + ) + -> ( ptData -> f ( Maybe ptData' ) ) + -> Spline clo crvData ptData + -> f ( Maybe ( Spline clo crvData' ptData' ) ) + + -- | Traversal of a spline. + bitraverseSpline + :: forall f crvData ptData crvData' ptData' + . Applicative f + => ( forall clo'. ( TraversalCt clo clo', SplineTypeI clo' ) + => ptData -> Curve clo' crvData ptData -> f ( Curve clo' crvData' ptData' ) + ) + -> ( ptData -> f ptData' ) + -> Spline clo crvData ptData + -> f ( Spline clo crvData' ptData' ) + + bitraverseSpline fc fp = ibitraverseSpline ( const fc ) fp + + -- | Indexed fold of a spline. + ibifoldSpline + :: forall f m crvData ptData + . ( Applicative f, Monoid m ) + => ( forall clo'. ( TraversalCt clo clo', SplineTypeI clo' ) + => Int -> ptData -> Curve clo' crvData ptData -> f m + ) + -> ( ptData -> f m ) + -> Spline clo crvData ptData + -> f m + + ibifoldSpline fc fp + = coerce + . ibitraverseSpline @clo @( Const ( Ap f m ) ) ( coerce fc ) ( coerce fp ) + + + -- | Fold of a spline. + bifoldSpline + :: forall f m crvData ptData + . ( Applicative f, Monoid m ) + => ( forall clo'. ( TraversalCt clo clo', SplineTypeI clo' ) + => ptData -> Curve clo' crvData ptData -> f m ) + -> ( ptData -> f m ) + -> Spline clo crvData ptData + -> f m + + bifoldSpline fc fp = ibifoldSpline ( const fc ) fp + + -- | Bifunctor fmap of a spline. + bimapSpline + :: forall crvData ptData crvData' ptData' + . ( forall clo'. ( TraversalCt clo clo', SplineTypeI clo' ) + => ptData -> Curve clo' crvData ptData -> Curve clo' crvData' ptData' + ) + -> ( ptData -> ptData' ) + -> Spline clo crvData ptData + -> Spline clo crvData' ptData' + bimapSpline fc fp + = runIdentity + . bitraverseSpline @clo @Identity ( coerce fc ) ( coerce fp ) + + + +instance KnownSplineType Open where + + type TraversalCt Open clo' = clo' ~ Open + + lastPoint ( Spline { splineStart, splineCurves = OpenCurves curves } ) = + case curves of + Empty -> splineStart + _ :|> lastCurve -> openCurveEnd lastCurve + + adjustSplineType :: forall clo' crvData ptData. SplineTypeI clo' => Spline clo' crvData ptData -> Spline Open crvData ptData + adjustSplineType spline@( Spline { splineStart, splineCurves } ) = case ssplineType @clo' of + SOpen -> spline + SClosed -> case splineCurves of + NoCurves -> Spline { splineStart, splineCurves = OpenCurves Empty } + ClosedCurves prev lst -> Spline { splineStart, splineCurves = OpenCurves $ prev :|> set ( field @"curveEnd" ) ( NextPoint splineStart ) lst } + + ibitraverseSpline fc fp ( Spline { splineStart, splineCurves = OpenCurves curves } ) = + ( \ p cs -> Spline p ( OpenCurves cs ) ) <$> fp splineStart <*> go 0 splineStart curves + where + go _ _ Empty = pure Empty + go i p ( seg :<| segs ) = (:<|) <$> fc i p seg <*> go ( i + 1 ) ( openCurveEnd seg ) segs + + biwitherSpline + :: forall f crvData ptData crvData' ptData' + . Monad f + => ( forall clo' hasStart. ( clo' ~ Open, SplineTypeI clo' ) + => CurrentStart hasStart ptData' -> Curve clo' crvData ptData -> f ( WitherResult hasStart clo' crvData' ptData' ) + ) + -> ( ptData -> f ( Maybe ptData' ) ) + -> Spline Open crvData ptData + -> f ( Maybe ( Spline Open crvData' ptData' ) ) + biwitherSpline fc fp ( Spline { splineStart, splineCurves = OpenCurves curves } ) = do + mbStart' <- fp splineStart + ( curves', mbStart'' ) <- ( `runStateT` ( fmap First mbStart' ) ) $ go ( (, True ) <$> mbStart' ) curves + case mbStart'' of + Nothing -> pure Nothing + Just ( First start' ) -> + pure ( Just $ Spline { splineStart = start', splineCurves = OpenCurves curves' } ) + where + go :: Maybe ( ptData', Bool ) -> Seq ( Curve Open crvData ptData ) -> StateT ( Maybe ( First ptData' ) ) f ( Seq ( Curve Open crvData' ptData' ) ) + go _ Empty = pure Empty + go Nothing ( crv :<| crvs ) = do + mbCrv' <- lift $ fc NoStartFound crv + case mbCrv' of + Dismiss -> go Nothing crvs + UseStartPoint ptData'' mbCrv'' -> do + modify' ( <> Just ( First ptData'' ) ) + case mbCrv'' of + Nothing -> go ( Just ( ptData'', False ) ) crvs + Just crv'' -> ( crv'' :<| ) <$> go ( Just ( ptData'', False ) ) crvs + go ( Just ( ptData', orig ) ) ( crv :<| crvs ) = do + mbCrv' <- lift $ fc ( CurrentStart orig ptData' ) crv + case mbCrv' of + Dismiss -> go ( Just ( ptData', False ) ) crvs + UseCurve crv'' -> + ( crv'' :<| ) <$> go ( Just ( openCurveEnd crv'', True ) ) crvs + +instance KnownSplineType Closed where + + type TraversalCt Closed clo' = () + + lastPoint ( Spline { splineStart } ) = splineStart + + adjustSplineType :: forall clo' crvData ptData. SplineTypeI clo' => Spline clo' crvData ptData -> Spline Closed crvData ptData + adjustSplineType spline@( Spline { splineStart, splineCurves } ) = case ssplineType @clo' of + SClosed -> spline + SOpen -> case splineCurves of + OpenCurves ( Empty ) -> Spline { splineStart, splineCurves = NoCurves } + OpenCurves ( prev :|> lst ) -> Spline { splineStart, splineCurves = ClosedCurves prev ( set ( field @"curveEnd" ) BackToStart lst ) } + + ibitraverseSpline + :: forall f crvData ptData crvData' ptData' + . Applicative f + => ( forall clo'. ( (), SplineTypeI clo' ) + => Int -> ptData -> Curve clo' crvData ptData -> f ( Curve clo' crvData' ptData' ) + ) + -> ( ptData -> f ptData' ) + -> Spline Closed crvData ptData + -> f ( Spline Closed crvData' ptData' ) + ibitraverseSpline _ fp ( Spline { splineStart = p0, splineCurves = NoCurves } ) = ( \ p -> Spline p NoCurves ) <$> fp p0 + ibitraverseSpline fc fp ( Spline { splineStart = p0, splineCurves = ClosedCurves prevCurves lastCurve } ) = + ( \ p cs lst -> Spline p ( ClosedCurves cs lst ) ) <$> fp p0 <*> go 0 p0 prevCurves <*> fc n pn lastCurve + where + n :: Int + n = length prevCurves + pn :: ptData + pn = case prevCurves of + Empty -> p0 + _ :|> lastPrev -> openCurveEnd lastPrev + go _ _ Empty = pure Empty + go i p ( seg :<| segs ) = (:<|) <$> fc i p seg <*> go ( i + 1 ) ( openCurveEnd seg ) segs + + biwitherSpline fc fp closedSpline = do + spline' <- biwitherSpline fc fp ( adjustSplineType @Open closedSpline ) + return $ adjustSplineType @Closed <$> spline' + +showSplinePoints :: forall clo ptData crvData + . (KnownSplineType clo, Show ptData) + => Spline clo crvData ptData -> String +showSplinePoints + = runIdentity + . bifoldSpline + ( \ _pt crv -> Identity $ f crv ) + ( \ pt -> Identity $ "[ " <> show pt ) + where + f :: SplineTypeI clo' => Curve clo' crvData ptData -> String + f (LineTo end _) = " -> " ++ showEnd end + f (Bezier2To cp end _) = " -- " ++ show cp ++ " -> " ++ showEnd end + f (Bezier3To cp1 cp2 end _) = " -- " ++ show cp1 ++ " -- " ++ show cp2 ++ " -> " ++ showEnd end + + showEnd :: forall clo'. SplineTypeI clo' => NextPoint clo' ptData -> String + showEnd = case ssplineType @clo' of + SOpen -> \ ( NextPoint pt ) -> show pt <> "\n, " + SClosed -> \ BackToStart -> ". ]"
+ src/lib/Math/Bezier/Stroke.hs view
@@ -0,0 +1,1712 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE PartialTypeSignatures #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TypeOperators #-} + +{-# OPTIONS_GHC -fspecialise-aggressively #-} + +module Math.Bezier.Stroke + ( Offset(..), Cusp(..) + , CachedStroke(..), discardCache, newCache, invalidateCache + , computeStrokeOutline_specialise + , computeStrokeOutline, joinWithBrush + , withTangent + + , RootSolvingAlgorithm(..) -- TODO: move this? + + -- * Brush stroking + + , envelopeEquation + , line, bezier2, bezier3 + , brushStrokeData, pathAndUsedParams + + -- ** Cusp finding + , findCusps, findCuspsIn + + -- TODO: hack for switching between 2 and 3 dim formulations of cusp-finding + , N + ) + where + +-- base +import Prelude + hiding ( unzip ) +import Control.Arrow + ( first ) +import Control.Monad + ( unless ) +import Control.Monad.ST + ( RealWorld, ST ) +import Data.Bifunctor + ( Bifunctor(bimap) ) +import Data.Coerce + ( coerce ) +import Data.Fixed + ( divMod' ) +import Data.Foldable + ( for_ ) +import Data.Functor + ( (<&>), unzip ) +import Data.Functor.Identity + ( Identity(..) ) +import Data.List + ( sort ) +import Data.List.NonEmpty + ( NonEmpty ) +import qualified Data.List.NonEmpty as NE +import Data.Maybe + ( catMaybes, fromMaybe, isNothing, listToMaybe, mapMaybe ) +import Data.Monoid + ( First(..) ) +import Data.Proxy + ( Proxy(..)) +import Data.Semigroup + ( sconcat ) +import Data.Type.Equality + ( (:~:)(Refl) ) +import GHC.Exts + ( newMutVar#, runRW#, proxy# ) +import GHC.STRef + ( STRef(..), readSTRef, newSTRef, writeSTRef ) +import GHC.Generics + ( Generic, Generic1, Generically(..) ) +import GHC.TypeNats + ( Nat, KnownNat + , type (<=) + , sameNat, natVal' + ) + +-- acts +import Data.Act + ( Act + ( (•) ) + , Torsor + ( (-->) ) + ) + +-- containers +import Data.IntMap.Strict + ( IntMap ) +import qualified Data.IntMap.Strict as IntMap + ( fromList, mapWithKey, toList ) +import Data.Sequence + ( Seq(..) ) +import qualified Data.Sequence as Seq + +-- deepseq +import Control.DeepSeq + ( NFData(..), NFData1, deepseq, force ) + +-- generic-lens +import Data.Generics.Product.Fields + ( field' ) +import Data.Generics.Product.Typed + ( HasType(typed) ) +import qualified Data.Generics.Internal.VL as Lens + ( set, view, over ) + +-- parallel +import qualified Control.Parallel.Strategies as Strats + ( rdeepseq, parTuple2, using ) + +-- transformers +import Control.Monad.Trans.Class + ( lift ) +import Control.Monad.Trans.Except + ( Except, runExcept, throwE ) +import Control.Monad.Trans.State.Strict as State + ( StateT, runStateT, evalStateT, get, put ) +import Control.Monad.Trans.Writer.CPS + ( WriterT, execWriterT, runWriter, tell ) + +-- brush-strokes +import Calligraphy.Brushes + ( Brush(..), Corner(..), Unrotated(..), BrushFn ) +import Math.Algebra.Dual +import qualified Math.Bezier.Cubic as Cubic +import Math.Bezier.Cubic.Fit + ( FwdBwd(..), FitPoint(..), OutlinePoint(..) + , FitParameters, fitSpline + ) +import Math.Bezier.Spline +import qualified Math.Bezier.Quadratic as Quadratic +import Math.Bezier.Stroke.EnvelopeEquation +import Math.Differentiable + ( I, IVness(..), SingIVness (..) + , Differentiable, DiffInterp + ) +import Math.Epsilon + ( epsilon ) +import Math.Interval +import Math.Linear +import Math.Module +import Math.Ring + ( Transcendental ) +import qualified Math.Ring as Ring +import Math.Orientation + ( Orientation(..), splineOrientation + , between + ) +import Math.Roots +import Math.Root.Isolation +import Math.Root.Isolation.Utils + ( boxMidpoint ) + +import Debug.Utils + +-------------------------------------------------------------------------------- + +data Offset + = Offset + { offsetIndex :: !Int + , offsetParameter :: !( Maybe Double ) + , offset :: !( T ( ℝ 2 ) ) + } + deriving stock ( Show, Generic ) + deriving anyclass NFData + +data TwoSided a + = TwoSided + { fwd :: !a + , bwd :: !a + } + deriving stock ( Show, Generic, Generic1, Functor, Foldable, Traversable ) + deriving anyclass ( NFData, NFData1 ) + +data OutlineData = + OutlineData + { outline :: !( TwoSided ( SplinePts Open, Seq FitPoint ) ) + , cusps :: ![ Cusp ] } + deriving stock Generic + deriving ( Semigroup, Monoid ) + via Generically OutlineData + deriving anyclass NFData + +instance Semigroup ( TwoSided ( SplinePts Open, Seq FitPoint ) ) where + TwoSided ( fwdSpline1, fwdPts1 ) ( bwdSpline1, bwdPts1 ) <> TwoSided ( fwdSpline2, fwdPts2 ) ( bwdSpline2, bwdPts2 ) = + TwoSided + ( fwdSpline1 <> fwdSpline2, fwdPts1 <> fwdPts2 ) + ( bwdSpline2 <> bwdSpline1, bwdPts2 <> bwdPts1 ) +instance Monoid ( TwoSided ( SplinePts Open, Seq FitPoint ) ) where + mempty = TwoSided empt empt + where + empt :: ( SplinePts Open, Seq FitPoint ) + empt = ( Spline { splineStart = ℝ2 0 0, splineCurves = OpenCurves Empty }, Empty ) + +data CachedStroke s = NoCache | CachedStroke { cachedStrokeRef :: STRef s ( Maybe OutlineData ) } +instance Show ( CachedStroke s ) where + show _ = "<<CachedStroke>>" +instance NFData ( CachedStroke s ) where + rnf _ = () + +discardCache :: forall crvData s. HasType ( CachedStroke s ) crvData => crvData -> ST s () +discardCache ( Lens.view ( typed @( CachedStroke s ) ) -> cache ) = + case cache of + NoCache -> return () + CachedStroke { cachedStrokeRef } -> + writeSTRef cachedStrokeRef Nothing + +{-# INLINE invalidateCache #-} +invalidateCache :: forall crvData. HasType ( CachedStroke RealWorld ) crvData => crvData -> crvData +invalidateCache = runRW# \ s -> + case newMutVar# Nothing s of + (# _, mutVar #) -> + Lens.set ( typed @( CachedStroke RealWorld ) ) + ( CachedStroke $ STRef mutVar ) + +newCache :: forall s. ST s ( CachedStroke s ) +newCache = CachedStroke <$> newSTRef Nothing + +coords :: forall ptData. HasType ( ℝ 2 ) ptData => ptData -> ℝ 2 +coords = Lens.view typed + +-------------------------------------------------------------------------------- + +-- | Forward and backward outlines. +type OutlineFn = ℝ 1 -> TwoSided OutlinePoint + +data Cusp + = Cusp + { cuspParameters :: !( ℝ 2 ) + -- ^ @(t,s)@ parameter values of the cusp + , cuspPathCoords :: !( D 2 1 ( ℝ 2 ) ) + -- ^ path point coordinates and tangent + , cuspStrokeCoords :: !( ℝ 2 ) + -- ^ brush stroke point coordinates + , cornerCusp :: !Bool + } + deriving stock ( Generic, Show ) + deriving anyclass NFData + +data OutlineInfo = + OutlineInfo + { outlineFn :: OutlineFn + , outlineDefiniteCusps, outlinePotentialCusps :: [ Cusp ] } + +type N = 2 + +-- | A version of 'computeStrokeOutline' which dispatches to the correctly +-- specialised version at runtime. +computeStrokeOutline_specialise :: + forall ( clo :: SplineType ) ( nbUsedParams :: Nat ) ( nbBrushParams :: Nat ) crvData ptData s + . ( KnownSplineType clo + , HasType ( ℝ 2 ) ptData + , HasType ( CachedStroke s ) crvData + , NFData ptData, NFData crvData + + -- Debugging. + , Show ptData, Show crvData + + -- Constraints for runtime dispatch to specialisations + , KnownNat nbUsedParams, KnownNat nbBrushParams + , nbUsedParams <= 4, nbBrushParams <= 4, 1 <= nbBrushParams + , nbUsedParams <= nbBrushParams + ) + => RootSolvingAlgorithm + -> Maybe ( RootIsolationOptions 1 1, RootIsolationOptions 1 2, RootIsolationOptions 2 3 ) + -> FitParameters + -> ( ptData -> ℝ nbUsedParams ) + -> ( ℝ nbUsedParams -> ℝ nbBrushParams ) + -> Brush nbBrushParams + -> Spline clo crvData ptData + -> ST s + ( Either ( SplinePts Closed ) ( SplinePts Closed, SplinePts Closed ) + , Seq FitPoint + , [ Cusp ] + ) +computeStrokeOutline_specialise rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @0 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @1 Proxy Proxy + = computeStrokeOutline @0 @1 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @1 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @1 Proxy Proxy + = computeStrokeOutline @1 @1 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @0 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @2 Proxy Proxy + = computeStrokeOutline @0 @2 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @1 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @2 Proxy Proxy + = computeStrokeOutline @1 @2 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @2 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @2 Proxy Proxy + = computeStrokeOutline @2 @2 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @0 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @3 Proxy Proxy + = computeStrokeOutline @0 @3 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @1 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @3 Proxy Proxy + = computeStrokeOutline @1 @3 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @2 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @3 Proxy Proxy + = computeStrokeOutline @2 @3 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @3 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @3 Proxy Proxy + = computeStrokeOutline @3 @3 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @0 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @4 Proxy Proxy + = computeStrokeOutline @0 @4 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @1 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @4 Proxy Proxy + = computeStrokeOutline @1 @4 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @2 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @4 Proxy Proxy + = computeStrokeOutline @2 @4 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @3 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @4 Proxy Proxy + = computeStrokeOutline @3 @4 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | Just Refl <- sameNat @nbUsedParams @4 Proxy Proxy + , Just Refl <- sameNat @nbBrushParams @4 Proxy Proxy + = computeStrokeOutline @4 @4 rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline + | otherwise + = error $ unlines + [ "computeStrokeOutline_specialise: expected 'nbUsedParams <= 4', 'nbBrushParams <= 4'" + , " nbUsedParams: " ++ show (natVal' @nbUsedParams proxy#) + , "nbBrushParams: " ++ show (natVal' @nbBrushParams proxy#) + ] + +computeStrokeOutline :: + forall { clo :: SplineType } ( nbUsedParams :: Nat ) ( nbBrushParams :: Nat ) crvData ptData s + . ( KnownSplineType clo + , HasType ( ℝ 2 ) ptData + , HasType ( CachedStroke s ) crvData + , NFData ptData, NFData crvData + + -- Differentiability. + , Interpolatable Double ( ℝ nbUsedParams ) + , DiffInterp 2 NonIV nbBrushParams + , DiffInterp 3 IsIV nbBrushParams + , HasChainRule Double 3 ( ℝ nbBrushParams ) + , Module 𝕀 ( T ( 𝕀ℝ nbUsedParams ) ) + + -- AABB computations + , Representable Double ( ℝ nbUsedParams ) + , Representable 𝕀 ( 𝕀ℝ nbUsedParams ) + + -- Debugging. + , Show ptData, Show crvData, Show ( ℝ nbBrushParams ) + ) + => RootSolvingAlgorithm + -> Maybe ( RootIsolationOptions 1 1, RootIsolationOptions 1 2, RootIsolationOptions 2 3 ) + -> FitParameters + -> ( ptData -> ℝ nbUsedParams ) + -> ( ℝ nbUsedParams -> ℝ nbBrushParams ) + -- ^ assumed to be linear and non-decreasing + -> Brush nbBrushParams + -> Spline clo crvData ptData + -> ST s + ( Either ( SplinePts Closed ) ( SplinePts Closed, SplinePts Closed ) + , Seq FitPoint + , [ Cusp ] + ) +computeStrokeOutline rootAlgo mbCuspOptions fitParams ptParams toBrushParams brush spline@( Spline { splineStart = spt0 } ) = case ssplineType @clo of + -- Open brush path with at least one segment. + -- Need to add caps at both ends of the path. + SOpen + | firstCurve :<| _ <- openCurves $ splineCurves spline + , prevCurves :|> lastCurve <- openCurves $ splineCurves spline + , firstOutlineFn :<| _ <- outlineFns + , _ :|> lastOutlineFn <- outlineFns + , let + endPt :: ptData + endPt = openCurveEnd lastCurve + startTgtFwd, startTgtBwd, endTgtFwd, endTgtBwd :: T ( ℝ 2 ) + TwoSided + { fwd = OutlinePoint { outlineTangent = startTgtFwd } + , bwd = OutlinePoint { outlineTangent = startTgtBwd } + } = outlineFn firstOutlineFn $ ℝ1 0 + TwoSided + { fwd = OutlinePoint { outlineTangent = endTgtFwd } + , bwd = OutlinePoint { outlineTangent = endTgtBwd } + } = outlineFn lastOutlineFn $ ℝ1 1 + startBrush, endBrush :: SplinePts Closed + startBrush = brushShape spt0 + endBrush = brushShape endPt + + -- Computation of which brush segment to use for the end caps. + startTgt, endTgt :: T ( ℝ 2 ) + startTgt = coords spt0 --> coords ( openCurveStart firstCurve ) + endTgt = case prevCurves of + Empty -> endTangent spt0 spt0 lastCurve + _ :|> lastPrev -> endTangent spt0 ( openCurveEnd lastPrev ) lastCurve + + startCap = + brushJoin ( Just CCW ) Bwd startTgtBwd ( coords spt0 , ( -1 *^ startTgt, startTgt ), startBrush ) startTgtFwd + endCap = + brushJoin ( Just CCW ) Fwd endTgtFwd ( coords endPt, ( endTgt , -1 *^ endTgt ), endBrush ) endTgtBwd + + -> do + OutlineData + ( TwoSided ( fwdPts, fwdFits ) ( bwdPts, bwdFits ) ) + cusps + <- updateSpline ( startTgt, startTgtFwd, startTgtBwd ) + pure + -- Add on start/end caps. + ( Left ( adjustSplineType @Closed $ sconcat $ NE.fromList $ [ startCap, fwdPts, endCap, bwdPts ] ) + , ( Seq.fromList $ + [ JoinPoint Fwd $ splineStart endCap + , JoinPoint Fwd $ splineEnd startCap + , JoinPoint Bwd $ splineEnd endCap + , JoinPoint Bwd $ splineStart startCap + ] + ) + <> fwdFits <> bwdFits + , cusps + ) + -- Closed brush path with at least one segment. + SClosed + | ClosedCurves prevCurves lastCurve <- splineCurves spline + , firstOutlineFn :<| _ <- outlineFns + , _ :|> lastOutlineFn <- outlineFns + , let + startTgt, endTgt, startTgtFwd, startTgtBwd, endTgtFwd, endTgtBwd :: T ( ℝ 2 ) + startTgt = case prevCurves of + Empty -> startTangent spt0 spt0 lastCurve + firstCrv :<| _ -> startTangent spt0 spt0 firstCrv + endTgt = case prevCurves of + Empty -> endTangent spt0 spt0 lastCurve + _ :|> lastPrev -> endTangent spt0 ( openCurveEnd lastPrev ) lastCurve + TwoSided + { fwd = OutlinePoint { outlineTangent = startTgtFwd } + , bwd = OutlinePoint { outlineTangent = startTgtBwd } + } = outlineFn firstOutlineFn $ ℝ1 0 + TwoSided + { fwd = OutlinePoint { outlineTangent = endTgtFwd } + , bwd = OutlinePoint { outlineTangent = endTgtBwd } + } = outlineFn lastOutlineFn $ ℝ1 1 + fwdStartCap, bwdStartCap :: SplinePts Open + OutlineData ( fmap fst -> TwoSided fwdStartCap bwdStartCap ) _ + = snd . runWriter + $ tellBrushJoin ( endTgt, endTgtFwd, endTgtBwd ) spt0 ( startTgt, startTgtFwd, startTgtBwd ) + -> do + OutlineData + ( TwoSided ( fwdPts, fwdFits ) ( bwdPts, bwdFits ) ) + cusps + <- updateSpline ( endTgt, endTgtFwd, endTgtBwd ) + pure + ( Right ( adjustSplineType @Closed ( fwdStartCap <> fwdPts ), adjustSplineType @Closed ( bwdPts <> bwdStartCap ) ) + , fwdFits <> bwdFits + , cusps + ) + -- Single point. + _ -> + pure + ( Left $ fmap ( T ( coords spt0 ) • ) ( brushShape spt0 ) + , Empty + , [] + ) + where + + outlineInfo :: ptData -> Curve Open crvData ptData -> OutlineInfo + outlineInfo = outlineFunction rootAlgo mbCuspOptions ptParams toBrushParams brush + + outlineFns :: Seq OutlineInfo + outlineFns = go spt0 ( openCurves $ splineCurves ( adjustSplineType @Open spline ) ) + where + go + :: ptData + -> Seq ( Curve Open crvData ptData ) + -> Seq OutlineInfo + go _ Empty = Empty + go p0 ( crv :<| crvs ) = + outlineInfo p0 crv :<| go ( openCurveEnd crv ) crvs + + brushShape :: ptData -> SplinePts Closed + brushShape pt = + let Brush { brushBaseShape = brushFn, mbRotation = mbRot } = brush + brushParams = toBrushParams $ ptParams pt + in applyRotation @NonIV brushParams brushFn mbRot + + updateSpline :: ( T ( ℝ 2 ), T ( ℝ 2 ), T ( ℝ 2 ) ) -> ST s OutlineData + updateSpline ( lastTgt, lastTgtFwd, lastTgtBwd ) + = execWriterT + . ( `evalStateT` ( lastTgt, lastTgtFwd, lastTgtBwd ) ) + $ bifoldSpline + ( \ ptData curve -> do + ( prevTgt, prev_tgtFwd, prev_tgtBwd ) <- get + let + fwdBwd :: OutlineInfo + fwdBwd = outlineInfo ptData curve + tgt, next_tgt, tgtFwd, next_tgtFwd, tgtBwd, next_tgtBwd :: T ( ℝ 2 ) + tgt = startTangent spt0 ptData curve + next_tgt = endTangent spt0 ptData curve + TwoSided + { fwd = OutlinePoint { outlineTangent = tgtFwd } + , bwd = OutlinePoint { outlineTangent = tgtBwd } + } = outlineFn fwdBwd $ ℝ1 0 + TwoSided + { fwd = OutlinePoint { outlineTangent = next_tgtFwd } + , bwd = OutlinePoint { outlineTangent = next_tgtBwd } + } = outlineFn fwdBwd $ ℝ1 1 + lift $ tellBrushJoin ( prevTgt, prev_tgtFwd, tgtBwd ) ptData ( tgt, tgtFwd, prev_tgtBwd ) + lift $ updateCurveData ( curveData curve ) fwdBwd + put ( next_tgt, next_tgtFwd, next_tgtBwd ) + ) + ( const ( pure () ) ) + ( adjustSplineType @Open spline ) + + updateCurveData + :: crvData + -> OutlineInfo + -> WriterT OutlineData ( ST s ) () + updateCurveData ( Lens.view ( typed @( CachedStroke s ) ) -> cache ) fwdBwd = do + let + mbCacheRef = + case cache of + NoCache -> Nothing + CachedStroke { cachedStrokeRef } -> Just cachedStrokeRef + mbOutline <- case mbCacheRef of + Nothing -> return Nothing + Just cacheRef -> lift ( readSTRef cacheRef ) + + case mbOutline of + -- Cached fit data is available: use it. + Just outline -> tell outline + -- No cached fit: compute the fit anew. + Nothing -> do + let + -- Split up the path at the cusps + cusps :: [ Cusp ] + cusps = outlineDefiniteCusps fwdBwd + ++ outlinePotentialCusps fwdBwd + intervals :: NonEmpty ( ℝ 1, ℝ 1 ) + intervals = splitInterval ( ℝ1 0, ℝ1 1 ) + $ sort [ ℝ1 t | Cusp { cuspParameters = ℝ2 t _ } <- cusps ] + + fwdData, bwdData :: ( SplinePts Open, Seq FitPoint ) + ( fwdData, bwdData ) = + ( sconcat $ fmap ( fitSpline Fwd fitParams ( fwd . outlineFn fwdBwd ) ) intervals + , sconcat $ fmap ( fitSpline Bwd fitParams ( bwd . outlineFn fwdBwd ) ) intervals + ) + -- TODO: use foldMap1 once that's in base. + `Strats.using` + ( Strats.parTuple2 Strats.rdeepseq Strats.rdeepseq ) + outlineData :: OutlineData + outlineData = + OutlineData + ( TwoSided fwdData ( bimap reverseSpline Seq.reverse bwdData ) ) + cusps + outlineData `deepseq` tell outlineData + + -- Write the computed outline to the cache. + case mbCacheRef of + Nothing -> return () + Just cacheRef -> + lift $ writeSTRef cacheRef ( Just outlineData ) + + -- Connecting paths at a point of discontinuity of the tangent vector direction (G1 discontinuity). + -- This happens at corners of the brush path (including endpoints of an open brush path, where the tangent flips direction). + tellBrushJoin + :: Monad m + => ( T ( ℝ 2 ), T ( ℝ 2 ), T ( ℝ 2 ) ) + -> ptData + -> ( T ( ℝ 2 ), T ( ℝ 2 ), T ( ℝ 2 ) ) + -> WriterT OutlineData m () + tellBrushJoin ( prevTgt, prevTgtFwd, prevTgtBwd ) sp0 ( tgt, tgtFwd, tgtBwd ) = + unless noJoin $ + tell $ OutlineData + ( TwoSided + ( fwdJoin, Seq.fromList $ map ( JoinPoint Fwd ) [ splineStart fwdJoin, splineEnd fwdJoin ] ) + ( bwdJoin, Seq.fromList $ map ( JoinPoint Bwd ) [ splineStart bwdJoin, splineEnd bwdJoin ] ) + ) + [] + where + noJoin = prevTgt `strictlyParallel` tgt + joinPointData = ( coords sp0, ( prevTgt, tgt ), brushShape sp0 ) + + fwdJoin = brushJoin Nothing Fwd prevTgtFwd joinPointData tgtFwd + bwdJoin = brushJoin Nothing Bwd prevTgtBwd joinPointData tgtBwd + + +-- | Compute a brush join, i.e. a section of a brush that connects up +-- at a corner or cap of the base path. +brushJoin :: Maybe Orientation + -> FwdBwd + -> T ( ℝ 2 ) + -> ( ℝ 2, ( T ( ℝ 2 ), T ( ℝ 2 ) ), SplinePts Closed ) + -> T ( ℝ 2 ) + -> SplinePts Open +brushJoin mbJoinOri fwdBwd incTgt ( pt, ( incPathTgt, outPathTgt ), brushShape ) outTgt = + let + + -- Flip tangents to account for regions where the outline moves + -- backwards relative to the base path (e.g. in between two cusps). + -- + -- This is admittedly rather ad-hoc, but some logic is necessary to handle + -- such situations. + ( incTgt', outTgt' ) = + ( if case fwdBwd of { Fwd -> incTgt ^.^ incPathTgt < 0; Bwd -> incTgt ^.^ outPathTgt > 0 } + then -1 *^ incTgt + else incTgt + , if case fwdBwd of { Fwd -> outTgt ^.^ outPathTgt < 0; Bwd -> outTgt ^.^ incPathTgt > 0 } + then -1 *^ outTgt + else outTgt + ) + + brushOri, joinOri :: Orientation + brushOri = splineOrientation brushShape + joinOri = + case mbJoinOri of + Just ori -> ori + Nothing -> + if incTgt' × outTgt' >= 0 then CCW else CW + in + + if joinOri == brushOri + then + fmap ( T pt • ) + $ joinWithBrush brushShape incTgt' outTgt' + else + fmap ( T pt • ) + . reverseSpline + $ joinWithBrush brushShape outTgt' incTgt' + +{-# SPECIALISE computeStrokeOutline @0 @1 #-} +{-# SPECIALISE computeStrokeOutline @1 @1 #-} +{-# SPECIALISE computeStrokeOutline @0 @2 #-} +{-# SPECIALISE computeStrokeOutline @1 @2 #-} +{-# SPECIALISE computeStrokeOutline @2 @2 #-} +{-# SPECIALISE computeStrokeOutline @0 @3 #-} +{-# SPECIALISE computeStrokeOutline @1 @3 #-} +{-# SPECIALISE computeStrokeOutline @2 @3 #-} +{-# SPECIALISE computeStrokeOutline @3 @3 #-} +{-# SPECIALISE computeStrokeOutline @0 @4 #-} +{-# SPECIALISE computeStrokeOutline @1 @4 #-} +{-# SPECIALISE computeStrokeOutline @2 @4 #-} +{-# SPECIALISE computeStrokeOutline @3 @4 #-} +{-# SPECIALISE computeStrokeOutline @4 @4 #-} + +-- | Computes the forward and backward stroke outline functions for a single curve. +outlineFunction + :: forall (nbUsedParams :: Nat) (nbBrushParams :: Nat) crvData ptData + . ( HasType ( ℝ 2 ) ptData + + -- Differentiability. + , Interpolatable Double ( ℝ nbUsedParams ) + , DiffInterp 2 NonIV nbBrushParams + , DiffInterp 3 IsIV nbBrushParams + , HasChainRule Double 3 ( ℝ nbBrushParams ) + , Module 𝕀 ( T ( 𝕀ℝ nbUsedParams ) ) + + -- Computing AABBs + , Representable Double ( ℝ nbUsedParams ) + , Representable 𝕀 ( 𝕀ℝ nbUsedParams ) + + -- Debugging. + , Show ptData, Show crvData, Show ( ℝ nbBrushParams ) + ) + => RootSolvingAlgorithm + -> Maybe ( RootIsolationOptions 1 1, RootIsolationOptions 1 2, RootIsolationOptions 2 3 ) + -> ( ptData -> ℝ nbUsedParams ) + -> ( ℝ nbUsedParams -> ℝ nbBrushParams ) -- ^ assumed to be linear and non-decreasing + -> Brush nbBrushParams + -> ptData + -> Curve Open crvData ptData + -> OutlineInfo +outlineFunction rootAlgo mbCuspOptions ptParams toBrushParams + ( Brush { brushBaseShape, brushBaseShape3, brushBaseShapeI + , brushCorners, brushCorners3, brushCornersI + , mbRotation } ) = \ sp0 crv -> + let + + usedParams :: C 2 ( ℝ 1 ) ( ℝ nbUsedParams ) + path :: C 2 ( ℝ 1 ) ( ℝ 2 ) + ( path, usedParams ) + = pathAndUsedParams @2 @NonIV @nbUsedParams coerce id id ptParams sp0 crv + + curves :: ℝ 1 -- t + -> ( Seq ( CornerStrokeDatum 2 NonIV ) + , Seq ( ℝ 1 {- s -} -> StrokeDatum 2 NonIV ) ) + curves = + brushStrokeData @2 @nbBrushParams SNonIV + path + ( fmap toBrushParams usedParams ) + brushBaseShape + brushCorners + mbRotation + + usedParams3 :: C 3 ( ℝ 1 ) ( ℝ nbUsedParams ) + path3 :: C 3 ( ℝ 1 ) ( ℝ 2 ) + ( path3, usedParams3 ) + = pathAndUsedParams @3 @NonIV @nbUsedParams coerce id id ptParams sp0 crv + + curves3 :: ℝ 1 -- t + -> ( Seq ( CornerStrokeDatum 3 NonIV ) + , Seq ( ℝ 1 {- s -} -> StrokeDatum 3 NonIV ) ) + curves3 = + brushStrokeData @3 @nbBrushParams SNonIV + path3 + ( fmap toBrushParams usedParams3 ) + brushBaseShape3 + brushCorners3 + mbRotation + + curvesI :: 𝕀ℝ 1 -- t + -> ( Seq ( CornerStrokeDatum 3 IsIV ) + , Seq ( 𝕀ℝ 1 {- s -} -> StrokeDatum 3 IsIV ) + ) + curvesI = + brushStrokeData @3 @nbBrushParams SIsIV + pathI + ( fmap ( nonDecreasing toBrushParams ) usedParamsI ) + brushBaseShapeI + brushCornersI + ( fmap ( \ rot -> un𝕀ℝ1 . nonDecreasing ( ℝ1 . rot ) ) mbRotation ) + + usedParamsI :: C 3 ( 𝕀ℝ 1 ) ( 𝕀ℝ nbUsedParams ) + pathI :: C 3 ( 𝕀ℝ 1 ) ( 𝕀ℝ 2 ) + ( pathI, usedParamsI ) = pathAndUsedParams @3 @IsIV @nbUsedParams coerce point point ptParams sp0 crv + + fwdBwd :: OutlineFn + fwdBwd t + = solveEnvelopeEquations rootAlgo t path_t path'_t ( fwdOffset, bwdOffset ) + ( curves t ) + where + + fwdOffset = withTangent path'_t brush_t + bwdOffset = withTangent ( -1 *^ path'_t ) brush_t + + D21 path_t path'_t _ = runD path t + D21 params_t _ _ = runD usedParams t + brush_t = + applyRotation @NonIV + ( toBrushParams params_t ) + brushBaseShape + mbRotation + + ( potentialCusps :: [Cusp], definiteCusps :: [Cusp] ) = + case mbCuspOptions of + Just cuspOpts + -- Don't try to compute cusps for a trivial curve + -- (e.g. a line segment with identical start- and end-points), + -- as the root isolation code chokes on this. + | not ( trivialCurve sp0 crv ) + -- TODO: introduce a maximum time budget for the cusp computation, + -- and bail out if the computation exceeds the budget. + -- (Record such bailing out and warn the user if this happens often.) + , let ( ( cornerCusps, startCornerCusps, endCornerCusps ), normalCusps ) = findCusps cuspOpts curvesI + -> + -- TODO: we are applying Newton's method to the returned boxes. + -- + -- This is correct for "definite cusp" boxes: Newton's method will converge. + -- + -- For "potential cusp" boxes: + -- + -- 1. There might not be a cusp. + -- 2. There might be multiple cusps. + foldMap + ( \ ( i, ( _trees, DoneBoxes { doneSolBoxes = defCusps, doneGiveUpBoxes = potCusps } ) ) -> + ( map ( \ ( box, _ ) -> fst $ cornerCuspCoords cornerCuspEqnPiece' ( fst . curves3 ) ( i, box ) ) potCusps + , map ( \ box -> fst $ cornerCuspCoords cornerCuspEqnPiece' ( fst . curves3 ) ( i, box ) ) defCusps ) + ) + ( IntMap.toList cornerCusps ) + <> + foldMap + ( \ ( i, ( _trees, DoneBoxes { doneSolBoxes = defCusps, doneGiveUpBoxes = potCusps } ) ) -> + ( map ( \ ( box, _ ) -> fst $ cornerCuspCoords startCornerEqnPiece ( fst . curves3 ) ( i, box ) ) potCusps + , map ( \ box -> fst $ cornerCuspCoords startCornerEqnPiece ( fst . curves3 ) ( i, box ) ) defCusps ) + ) + ( IntMap.toList startCornerCusps ) + <> + foldMap + ( \ ( i, ( _trees, DoneBoxes { doneSolBoxes = defCusps, doneGiveUpBoxes = potCusps } ) ) -> + ( map ( \ ( box, _ ) -> fst $ cornerCuspCoords endCornerEqnPiece ( fst . curves3 ) ( i, box ) ) potCusps + , map ( \ box -> fst $ cornerCuspCoords endCornerEqnPiece ( fst . curves3 ) ( i, box ) ) defCusps ) + ) + ( IntMap.toList endCornerCusps ) + <> + foldMap + ( \ ( i, ( _trees, DoneBoxes { doneSolBoxes = defCusps, doneGiveUpBoxes = potCusps } ) ) -> + ( map ( \ ( box, _ ) -> fst $ cuspCoords ( snd . curves3 ) ( i, box ) ) potCusps + , map ( \ box -> fst $ cuspCoords ( snd . curves3 ) ( i, box ) ) defCusps ) + ) + ( IntMap.toList normalCusps ) + _ -> + ( [], [] ) + +{- DEBUG code + + !_ = trace ( unlines [ "cusp finding results:" + , "definite cusps: " ++ show ( map cuspParameters definiteCusps ) + , "potential cusps: " ++ show ( map cuspParameters potentialCusps ) + ] + ) () + + !_ = trace + ( unlines + [ "guessed cusp: " ++ show ( cuspParameters $ fst $ cuspCoords ( snd . curves3 ) ( 0, point $ ℝ2 0.33 0.35 ) ) + ] + ) + () + +-} + + in + + OutlineInfo + { outlineFn = fwdBwd + , outlineDefiniteCusps = definiteCusps + , outlinePotentialCusps = potentialCusps + } +{-# INLINEABLE outlineFunction #-} + +-- TODO: move this out +trivialCurve :: HasType ( ℝ 2 ) ptData => ptData -> Curve Open crvData ptData -> Bool +trivialCurve p0 = \case + LineTo ( NextPoint p1 ) _ -> coords p0 == coords p1 + Bezier2To cp1 ( NextPoint p2 ) _ -> all ( ( == coords p0 ) . coords ) [ cp1, p2 ] + Bezier3To cp1 cp2 ( NextPoint p3 ) _ -> all ( ( == coords p0 ) . coords ) [ cp1, cp2, p3 ] + +pathAndUsedParams :: forall k i (nbUsedParams :: Nat) arr crvData ptData + . ( HasType ( ℝ 2 ) ptData + , HasBézier k i + , arr ~ C k + , Module ( I i Double ) ( T ( I i 2 ) ) + , Torsor ( T ( I i 2 ) ) ( I i 2 ) + , Module ( I i Double ) ( T ( I i nbUsedParams ) ) + , Torsor ( T ( I i nbUsedParams ) ) ( I i nbUsedParams ) + , Module Double ( T ( ℝ nbUsedParams ) ) + , Representable Double ( ℝ nbUsedParams ) + , Representable 𝕀 ( 𝕀ℝ nbUsedParams ) + ) + => ( I i 1 -> I i Double ) + -> ( ℝ 2 -> I i 2 ) + -> ( ℝ nbUsedParams -> I i nbUsedParams ) + -> ( ptData -> ℝ nbUsedParams ) + -> ptData + -> Curve Open crvData ptData + -> ( I i 1 `arr` I i 2, I i 1 `arr` I i nbUsedParams ) +pathAndUsedParams co toI1 toI2 ptParams sp0 crv = + case crv of + LineTo { curveEnd = NextPoint sp1 } + | let seg = Segment sp0 sp1 + -> ( line @k @i @2 co ( fmap ( toI1 . coords ) seg ) + , line @k @i @nbUsedParams co ( fmap ( toI2 . ptParams ) seg ) ) + Bezier2To { controlPoint = sp1, curveEnd = NextPoint sp2 } + | let bez2 = Quadratic.Bezier sp0 sp1 sp2 + -> ( bezier2 @k @i @2 co ( fmap ( toI1 . coords ) bez2 ) + , bezier2 @k @i @nbUsedParams co ( fmap ( toI2 . ptParams ) bez2 ) ) + Bezier3To { controlPoint1 = sp1, controlPoint2 = sp2, curveEnd = NextPoint sp3 } + | let bez3 = Cubic.Bezier sp0 sp1 sp2 sp3 + -> ( bezier3 @k @i @2 co ( fmap ( toI1 . coords ) bez3 ) + , bezier3 @k @i @nbUsedParams co ( fmap ( toI2 . ptParams ) bez3 ) ) +{-# INLINEABLE pathAndUsedParams #-} + +----------------------------------- +-- Various utility functions +-- used in the "stroke" function. +----- + +startTangent, endTangent + :: ( SplineTypeI clo, HasType ( ℝ 2 ) ptData ) + => ptData -- ^ start point of the spline (to handle 'NextPoint' correctly) + -> ptData -- ^ start of the curve segment + -> Curve clo crvData ptData + -> T ( ℝ 2 ) +startTangent sp p0 ( LineTo mp1 _ ) = coords p0 --> coords ( fromNextPoint sp mp1 ) +startTangent _ p0 ( Bezier2To p1 _ _ ) = coords p0 --> coords p1 +startTangent _ p0 ( Bezier3To p1 _ _ _ ) = coords p0 --> coords p1 +endTangent sp p0 ( LineTo mp1 _ ) = coords p0 --> coords ( fromNextPoint sp mp1 ) +endTangent sp _ ( Bezier2To p0 mp1 _ ) = coords p0 --> coords ( fromNextPoint sp mp1 ) +endTangent sp _ ( Bezier3To _ p0 mp1 _ ) = coords p0 --> coords ( fromNextPoint sp mp1 ) + +lastTangent :: HasType ( ℝ 2 ) ptData => Spline Closed crvData ptData -> Maybe ( T ( ℝ 2 ) ) +lastTangent ( Spline { splineCurves = NoCurves } ) = Nothing +lastTangent ( Spline { splineStart, splineCurves = ClosedCurves Empty lst } ) = Just $ endTangent splineStart splineStart lst +lastTangent ( Spline { splineStart, splineCurves = ClosedCurves ( _ :|> prev ) lst } ) = Just $ endTangent splineStart ( openCurveEnd prev ) lst + +-- | Split up the given interval at the list of points provided. +-- +-- Assumes the list is sorted. +splitInterval :: Ord a => ( a, a ) -> [ a ] -> NonEmpty ( a, a ) +splitInterval ( start, end ) [] + = NE.singleton ( start, end ) +splitInterval ( start, end ) ( split : splits ) + | split >= end + = NE.singleton ( start, end ) + | split <= start + = splitInterval ( start, end ) splits + | otherwise + = NE.cons ( start, split ) $ splitInterval ( split, end ) splits + +-------------------------------------------------------------------------------- + +-- | Compute the join at a point of discontinuity of the tangent vector direction (G1 discontinuity). +joinWithBrush + :: SplinePts Closed -> T ( ℝ 2 ) -> T ( ℝ 2 ) -> SplinePts Open +joinWithBrush brush startTgt endTgt = + joinBetweenOffsets brush startOffset endOffset + where + startOffset, endOffset :: Offset + startOffset = withTangent startTgt brush + endOffset = withTangent endTgt brush + +-- | Select the section of a spline in between two offsets. +joinBetweenOffsets :: SplinePts Closed -> Offset -> Offset -> SplinePts Open +joinBetweenOffsets + spline + ( Offset { offsetIndex = i1, offsetParameter = fromMaybe 1 -> t1 } ) + ( Offset { offsetIndex = i2, offsetParameter = fromMaybe 0 -> t2 } ) + | i2 > i1 + = let + pcs, lastAndRest :: Maybe ( SplinePts Open ) + ( pcs, lastAndRest ) = + unzip $ + fmap ( splitSplineAt ( i2 - i1 ) ) ( dropCurves i1 openSpline ) + in + fromMaybe empty $ + mconcat + [ snd <$> ( splitFirstPiece t1 =<< pcs ) + , dropFirstPiece =<< pcs + , fst <$> ( splitFirstPiece t2 =<< lastAndRest ) + ] + | i2 == i1 && t2 >= t1 + = let + pcs :: Maybe ( SplinePts Open ) + pcs = dropCurves i1 openSpline + -- We want to split: 0 -- t1 -- t2 -- 1 + -- First split at t1, then split again but after rescaling + -- using t |-> ( t - t1 ) / ( 1 - t1 ). + t2' + | t1 >= 1 + = 1 + | otherwise + = min 1 $ ( t2 - t1 ) / ( 1 - t1 ) + in + maybe empty fst + ( splitFirstPiece t2' . snd =<< ( splitFirstPiece t1 =<< pcs ) ) + | otherwise + = let + start, middle, end :: SplinePts Open + ( ( middle, end ), start ) + = first ( splitSplineAt i2 ) + $ splitSplineAt i1 openSpline + in + sconcat $ NE.fromList $ catMaybes $ + [ snd <$> splitFirstPiece t1 start + , dropFirstPiece start + , Just middle + , fst <$> splitFirstPiece t2 ( if i1 == i2 then start else end ) + ] + where + empty :: SplinePts Open + empty = Spline { splineStart = ℝ2 0 0, splineCurves = OpenCurves Empty } + openSpline :: Spline Open () ( ℝ 2 ) + openSpline = adjustSplineType spline + +-- | Drop the first curve in a Bézier spline. +dropFirstPiece :: SplinePts Open -> Maybe ( SplinePts Open ) +dropFirstPiece ( Spline { splineCurves = OpenCurves curves } ) = case curves of + Empty -> Nothing + fstPiece :<| laterPieces -> + Just $ Spline + { splineStart = openCurveEnd fstPiece + , splineCurves = OpenCurves laterPieces + } + +-- | Subdivide the first piece at the given parameter, discarding the subsequent pieces. +splitFirstPiece :: Double -> SplinePts Open -> Maybe ( SplinePts Open, SplinePts Open ) +splitFirstPiece t ( Spline { splineStart = p0, splineCurves = OpenCurves curves } ) = + case curves of + Empty -> Nothing + fstPiece :<| _ + -- Handle t = 0 and t = 1 first. + | t <= 0 + -- t = 0: single point, then first piece. + -> Just ( Spline { splineStart = p0, splineCurves = OpenCurves Empty } + , Spline { splineStart = p0, splineCurves = OpenCurves ( Seq.singleton fstPiece ) } ) + | t >= 1 + -- t = 1: first piece, then single point. + -> Just ( Spline { splineStart = p0, splineCurves = OpenCurves ( Seq.singleton fstPiece ) } + , Spline { splineStart = openCurveEnd fstPiece, splineCurves = OpenCurves Empty } + ) + | otherwise + -- Normal code path: actual splitting. + -> let ( leftCurve, splitPt, rightCurve ) = subdivideCurveAt @( T ( ℝ 2 ) ) p0 fstPiece t + in Just + ( Spline { splineStart = p0 , splineCurves = OpenCurves ( Seq.singleton leftCurve ) } + , Spline { splineStart = splitPt, splineCurves = OpenCurves ( Seq.singleton rightCurve ) } + ) + +-------------------------------------------------------------------------------- + +-- | Finds the point at which a convex nib (given by a piecewise Bézier curve) has the given tangent vector. +-- +-- Does /not/ check that the provided nib shape is convex. +withTangent + :: forall crvData ptData + . ( HasType ( ℝ 2 ) ptData, Show crvData, Show ptData ) + => T ( ℝ 2 ) -> Spline Closed crvData ptData -> Offset +withTangent tgt_wanted spline@( Spline { splineStart } ) + -- only allow non-empty splines + | Just tgt_last <- lastTangent spline + -- only allow well-defined query tangent vectors + , not $ badTangent tgt_wanted + = case runExcept . ( `runStateT` tgt_last ) $ ibifoldSpline go ( \ _ -> pure () ) $ adjustSplineType @Open spline of + Left off -> + off + Right _ -> + trace + ( unlines + [ "withTangent: could not find any point with given tangent vector" + , "tangent vector: " <> show tgt_wanted + , "spline:" + , showSplinePoints spline + ] ) $ + Offset { offsetIndex = 0, offsetParameter = Just 0.5, offset = T ( coords splineStart ) } + | otherwise + = Offset { offsetIndex = 0, offsetParameter = Just 0.5, offset = T ( coords splineStart ) } + + where + badTangent :: T ( ℝ 2 ) -> Bool + badTangent ( V2 tx ty ) = + isNaN tx || isNaN ty || isInfinite tx || isInfinite ty + || ( abs tx < epsilon && abs ty < epsilon ) + ori :: Orientation + ori = splineOrientation spline + go :: Int -> ptData -> Curve Open crvData ptData -> StateT ( T ( ℝ 2 ) ) ( Except Offset ) () + go i cp cseg = do + tgt_prev <- get + let + p :: ℝ 2 + p = coords cp + seg :: Curve Open crvData ( ℝ 2 ) + seg = fmap coords cseg + tgt_start, tgt_end :: T ( ℝ 2 ) + tgt_start = startTangent splineStart cp cseg + tgt_end = endTangent splineStart cp cseg + -- Handle corner. + unless ( tgt_prev `strictlyParallel` tgt_start ) do + for_ ( between ori tgt_prev tgt_start tgt_wanted ) \ a -> do + let + -- Decide whether the offset should be at the start of this + -- segment or at the end of the previous segment, depending + -- on which tangent is closer. + -- + -- This is important to get the best initial guess possible + -- for Newton's method when solving the envelope equation. + off + | a < 0.5 + = Offset + { offsetIndex = + if i == 0 + then nbCurves ( splineCurves spline ) - 1 + else i - 1 + , offsetParameter = Just 1 + , offset = T p + } + | otherwise + = Offset + { offsetIndex = i + , offsetParameter = Just 0 + , offset = T p + } + lift $ throwE $ off + -- Handle segment. + lift $ handleSegment i p seg tgt_start + put tgt_end + + handleSegment :: Int -> ℝ 2 -> Curve Open crvData ( ℝ 2 ) -> T ( ℝ 2 ) -> Except Offset () + handleSegment i p0 ( LineTo ( NextPoint p1 ) _ ) tgt0 + | tgt_wanted `strictlyParallel` tgt0 + , let + offset :: T ( ℝ 2 ) + offset = T $ lerp @( T ( ℝ 2 ) ) 0.5 p0 p1 + = throwE ( Offset { offsetIndex = i, offsetParameter = Nothing, offset } ) + | otherwise + = pure () + handleSegment i p0 ( Bezier2To p1 ( NextPoint p2 ) _ ) tgt0 = + let + tgt1 :: T ( ℝ 2 ) + tgt1 = p1 --> p2 + in for_ ( convexCombination tgt0 tgt1 tgt_wanted ) \ s -> + throwE $ + Offset + { offsetIndex = i + , offsetParameter = Just s + , offset = T $ Quadratic.bezier @( T ( ℝ 2 ) ) ( Quadratic.Bezier {..} ) s + } + handleSegment i p0 ( Bezier3To p1 p2 ( NextPoint p3 ) _ ) tgt0 = + let + tgt1, tgt2 :: T ( ℝ 2 ) + tgt1 = p1 --> p2 + tgt2 = p2 --> p3 + bez :: Cubic.Bezier ( ℝ 2 ) + bez = Cubic.Bezier {..} + c01, c12, c23 :: Double + c01 = tgt_wanted × tgt0 + c12 = tgt_wanted × tgt1 + c23 = tgt_wanted × tgt2 + correctTangentParam :: Double -> Maybe Double + correctTangentParam s + | s > -epsilon && s < 1 + epsilon + , tgt_wanted ^.^ Cubic.bezier' bez s > epsilon + = Just ( max 0 ( min 1 s ) ) + | otherwise + = Nothing + in + let + mbParam :: Maybe Double + mbParam = listToMaybe + . mapMaybe correctTangentParam + $ solveQuadratic c01 ( 2 * ( c12 - c01 ) ) ( c01 + c23 - 2 * c12 ) + in for_ mbParam \ s -> + throwE $ + Offset + { offsetIndex = i + , offsetParameter = Just s + , offset = T $ Cubic.bezier @( T ( ℝ 2 ) ) bez s + } + +-------------------------------------------------------------------------------- + +splineCurveFns :: forall k i + . ( HasBézier k i + , Module ( I i Double ) ( T ( I i 2 ) ) + , Torsor ( T ( I i 2 ) ) ( I i 2 ) ) + => ( I i 1 -> I i Double ) + -> Spline Closed () ( I i 2 ) + -> Seq ( C k ( I i 1 ) ( I i 2 ) ) +splineCurveFns co spls + = runIdentity + . bifoldSpline + ( \ pt crv -> Identity . Seq.singleton $ curveFn pt crv ) + ( const $ Identity Seq.empty ) + . adjustSplineType @Open + $ spls + where + curveFn :: I i 2 + -> Curve Open () ( I i 2 ) + -> C k ( I i 1 ) ( I i 2 ) + curveFn p0 = \case + LineTo { curveEnd = NextPoint p1 } + -> line @k @i @2 co $ Segment p0 p1 + Bezier2To { controlPoint = p1, curveEnd = NextPoint p2 } + -> bezier2 @k @i @2 co $ Quadratic.Bezier p0 p1 p2 + Bezier3To { controlPoint1 = p1, controlPoint2 = p2, curveEnd = NextPoint p3 } + -> bezier3 @k @i @2 co $ Cubic.Bezier p0 p1 p2 p3 + +newtype ZipSeq a = ZipSeq { getZipSeq :: Seq a } + deriving stock Functor +instance Applicative ZipSeq where + pure _ = error "only use Applicative ZipSeq with non-empty Traversable functors" + liftA2 f ( ZipSeq xs ) ( ZipSeq ys ) = ZipSeq ( Seq.zipWith f xs ys ) + {-# INLINE liftA2 #-} + +brushStrokeData :: forall (k :: Nat) (nbBrushParams :: Nat) (i :: IVness) arr + . ( arr ~ C k + , HasBézier k i, HasEnvelopeEquation k + , Differentiable k i nbBrushParams + , HasChainRule ( I i Double ) k ( I i 1 ) + , HasChainRule ( I i Double ) k ( I i nbBrushParams ) + , Applicative ( D k 1 ) + , Dim ( I i 1 ) ~ 1 + , Dim ( I i nbBrushParams ) ~ nbBrushParams + , Traversable ( D k nbBrushParams ) + , Traversable ( D k 1 ) + , Module ( D k 1 𝕀 ) ( D k 1 ( 𝕀ℝ 2 ) ) + , Transcendental ( D k 1 𝕀 ) + + , Transcendental ( I i Double ) + , Module ( I i Double ) ( T ( I i 1 ) ) + , Cross ( I i Double ) ( T ( I i 2 ) ) + , Torsor ( T ( I i 2 ) ) ( I i 2 ) + , Show ( ℝ nbBrushParams ) + , Show ( StrokeDatum k i ) + , Show ( StrokeDatum k IsIV ) + , Show ( CornerStrokeDatum k i ) + , Show ( CornerStrokeDatum k IsIV ) + , Representable ( I i Double ) ( I i 2 ), RepDim ( I i 2 ) ~ 2 + + ) + => SingIVness i + -> ( I i 1 `arr` I i 2 ) + -- ^ path + -> ( I i 1 `arr` I i nbBrushParams ) + -- ^ brush parameters + -> ( Unrotated ( I i nbBrushParams `arr` Spline Closed () ( I i 2 ) ) ) + -- ^ brush from parameters + -> ( Seq ( Unrotated ( I i nbBrushParams `arr` Corner ( I i 2 ) ) ) ) + -- ^ brush corners from parameters + -> ( Maybe ( I i nbBrushParams -> I i Double ) ) + -- ^ rotation parameter + -> ( I i 1 -> ( Seq ( CornerStrokeDatum k i ), Seq ( I i 1 -> StrokeDatum k i ) ) ) +brushStrokeData ivness path params brush brushCorners mbBrushRotation = + \ t -> + let + + co :: I i 1 -> I i Double + !co = case ivness of + SNonIV -> coerce + SIsIV -> coerce + + dpath_t :: D k 1 ( I i 2 ) + !dpath_t = runD path t + dparams_t :: D k 1 ( I i nbBrushParams ) + !dparams_t = runD params t + brushParams :: I i nbBrushParams + !brushParams = value @k @1 dparams_t + dbrush_params :: D k nbBrushParams ( Spline Closed () ( I i 2 ) ) + !dbrush_params = runD ( getUnrotated brush ) brushParams + splines :: Seq ( D k nbBrushParams ( I i 1 `arr` I i 2 ) ) + !splines = getZipSeq $ traverse ( ZipSeq . splineCurveFns @k @i co ) dbrush_params + dbrushes_t :: Seq ( I i 1 -> Unrotated ( D k 2 ( I i 2 ) ) ) + !dbrushes_t = + force $ + fmap + ( fmap ( Unrotated . uncurryD @k ) + . ( \ db s -> fmap ( `runD` s ) db ) + . chain @( I i Double ) @k dparams_t + -- This is the crucial use of the chain rule. + ) + splines + + in ( brushCorners <&> \ cornerFn -> + let !dcorner = runD ( getUnrotated cornerFn ) brushParams + !dcorner_t = Unrotated $ chain @( I i Double ) @k dparams_t dcorner + in mkCornerStrokeDatum dpath_t dparams_t dcorner_t + , fmap ( mkStrokeDatum dpath_t dparams_t . ) dbrushes_t ) + + where + + mkStrokeDatum :: D k 1 ( I i 2 ) + -> D k 1 ( I i nbBrushParams ) + -> Unrotated ( D k 2 ( I i 2 ) ) + -> StrokeDatum k i + mkStrokeDatum dpath_t dparams_t dbrush_t = + let mbRotation = mbBrushRotation <&> \ getTheta -> fmap getTheta dparams_t + in envelopeEquation @k ivness dpath_t dbrush_t mbRotation + + mkCornerStrokeDatum :: D k 1 ( I i 2 ) + -> D k 1 ( I i nbBrushParams ) + -> Unrotated ( D k 1 ( Corner ( I i 2 ) ) ) + -> CornerStrokeDatum k i + mkCornerStrokeDatum dpath_t dparams_t dcorner_t = + let mbRotation = mbBrushRotation <&> \ getTheta -> fmap getTheta dparams_t + in cornerEnvelopeEquation @k dpath_t dcorner_t mbRotation + +{-# INLINEABLE brushStrokeData #-} +{-# SPECIALISE brushStrokeData @2 @1 @NonIV #-} +{-# SPECIALISE brushStrokeData @2 @2 @NonIV #-} +{-# SPECIALISE brushStrokeData @2 @3 @NonIV #-} +{-# SPECIALISE brushStrokeData @2 @4 @NonIV #-} +{-# SPECIALISE brushStrokeData @3 @1 @IsIV #-} +{-# SPECIALISE brushStrokeData @3 @2 @IsIV #-} +{-# SPECIALISE brushStrokeData @3 @3 @IsIV #-} +{-# SPECIALISE brushStrokeData @3 @4 @IsIV #-} + +-------------------------------------------------------------------------------- +-- Solving the envelolpe equation: root-finding. + +-- | Which method to use to solve the envelope equation? +data RootSolvingAlgorithm + -- | Use the Newton–Raphson method. + = NewtonRaphson { maxIters :: Word, precision :: Int } + -- | Use the modified Halley M2 method. + | HalleyM2 { maxIters :: Word, precision :: Int } + +-- | Solve the envelope equations at a given point \( t = t_0 \), to find +-- \( s_0 \) such that \( c(t_0, s_0) \) is on the envelope of the brush stroke. +solveEnvelopeEquations :: RootSolvingAlgorithm + -> ℝ 1 -- ^ @t@ (for debugging only) + -> ℝ 2 + -> T ( ℝ 2 ) + -> ( Offset, Offset ) + -> ( Seq ( CornerStrokeDatum 2 NonIV ), Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) ) + -> TwoSided OutlinePoint +solveEnvelopeEquations rootAlgo ( ℝ1 _t ) path_t path'_t ( fwdOffset, bwdOffset ) ( corners, strokeData ) + = TwoSided + { fwd = fromMaybe fwdSol mbFwdCornerSol + , bwd = Lens.over ( field' @"outlineTangent" ) ( -1 *^ ) + $ fromMaybe bwdSol mbBwdCornerSol + } + + where + + fwdSol = findSolFrom Fwd fwdOffset + bwdSol = findSolFrom Bwd bwdOffset + + mbFwdCornerSol = getFirst $ foldMap ( First . cornerSol Fwd ) corners + mbBwdCornerSol = getFirst $ foldMap ( First . cornerSol Bwd ) corners + + findSolFrom :: FwdBwd -> Offset -> OutlinePoint + findSolFrom fwdOrBwd ( Offset { offsetIndex = i00, offsetParameter = s00 } ) = + go ( fromIntegral i00 + maybe 0.5 ( min 0.99999999999999989 ) s00 ) + -- NB: the 'fromDomain' function requires values in the + -- half-open interval [0,1[, hence the @min 0.99999999999999989@ above. + where + + go :: Double -> OutlinePoint + go is0 = + case sol fwdOrBwd strokeData is0 of + ( goodSoln, pt ) + | goodSoln + -> pt + | otherwise + -> trace + ( unlines + [ "solveEnvelopeEquations: bad solution" + , "falling back to naive guess; this will lead to inaccurate fitting" + , " t: " ++ show _t + , " dir: " ++ show fwdOrBwd + , " p: " ++ show path_t + , " p': " ++ show path'_t + , " is0: " ++ show is0 + , " BAD pt: " ++ show pt + ] + ) finish fwdOrBwd strokeData is0 + + sol :: FwdBwd -> Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) -> Double -> ( Bool, OutlinePoint ) + sol fwdOrBwd f is0 = + let solRes = runSolveMethod ( eqn fwdOrBwd f ) is0 + ( good, is ) = + case solRes of + Nothing -> ( False, is0 ) + Just is1 -> ( case fwdOrBwd of + Fwd -> outlineDotProduct outlinePt >= 0 + Bwd -> outlineDotProduct outlinePt <= 0 + , is1 + ) + outlinePt = finish fwdOrBwd f is + in ( good, outlinePt ) + + runSolveMethod = case rootAlgo of + HalleyM2 { maxIters, precision } -> + halleyM2 maxIters precision + NewtonRaphson { maxIters, precision } -> + newtonRaphson maxIters precision + + finish :: FwdBwd -> Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) -> Double -> OutlinePoint + finish _fwdOrBwd fs is = + let (_i, s) = fromDomain is in + case evalStrokeDatum fs is of -- TODO: a bit redundant to have to compute this again... + StrokeDatum + { stroke + , mbRotation + , ee = D12 ( ℝ1 _ee ) _ ( T ( ℝ1 𝛿E𝛿s ) ) + , du = D12 u _ _ + , dv = D12 v _ _ + , 𝛿E𝛿s_unrot_dcdt = Unrotated ( D0 𝛿E𝛿s_unrot_dcdt ) + } -> + let + -- The total derivative dc/dt is computed by dividing by ∂E/∂s. + -- However: + -- + -- - the outline fitting algorithm only cares about the direction + -- of the tangent and not its magnitude, + -- - ∂E/∂s is a scalar + -- + -- Thus we only care about the sign of ∂E/∂s. + unrot_dcdt = ( if 𝛿E𝛿s < 0 then (-1 *^) else id ) 𝛿E𝛿s_unrot_dcdt + dcdt = case mbRotation of + Nothing -> unrot_dcdt + Just ( D21 θ _ _ ) -> + let cosθ = cos θ + sinθ = sin θ + in rotate cosθ sinθ $ unrot_dcdt + -- TODO: reduce duplication with 'cornerSol'. + in OutlinePoint + { outlinePoint = stroke + , outlineParam = s + , outlineTangent = dcdt + , outlineDotProduct = T u ^.^ T v + } + -- Compute the dot product of u and v (which are rotated versions of ∂c/∂t and ∂c/∂s). + -- The sign of this quantity determines which side of the envelope + -- we are on. + + evalStrokeDatum :: Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) -> ( Double -> StrokeDatum 2 NonIV ) + evalStrokeDatum fs is = + let (i, s) = fromDomain is + in ( fs `Seq.index` i ) ( ℝ1 $ max 0 $ min 1 $ s ) + + eqn :: FwdBwd -> Seq ( ℝ 1 -> StrokeDatum 2 NonIV ) -> ( Double -> (# Double, Double #) ) + eqn fwdOrBwd fs is = + case evalStrokeDatum fs is of + StrokeDatum + { du = D12 u _ u_s + , dv = D12 v _ v_s + , ee = D12 ( ℝ1 ee ) _ ( T ( ℝ1 ee_s ) ) } -> + let dp = T u ^.^ T v + dp_s = u_s ^.^ T v + T u ^.^ v_s + sgn = case fwdOrBwd of + Fwd -> 1 + Bwd -> -1 + in + if sgn * dp >= 0 + then (# ee, ee_s #) + else + -- Perturb the envelope equation to avoid solutions + -- on the "wrong" side (e.g. with the wrong sign of the dot product). + -- + -- NB: use a square root to avoid introducing double roots, + -- as roots with multiplicity >= 2 cause the Newton-Raphson algorithm + -- to only converge linearly instead of quadratically. + let ee_perturbed = sqrt $ ee * ee + dp * dp + in (# ee_perturbed, ( ee * ee_s + dp * dp_s ) / ee_perturbed #) + + cornerSol :: FwdBwd -> CornerStrokeDatum 2 NonIV -> Maybe OutlinePoint + cornerSol fwdOrBwd + ( CornerStrokeDatum + { dstroke = + D21 + ( Corner pt startTgt endTgt ) + ( T ( Corner ( T -> tgt ) _ _ ) ) + _ + } + ) = + let flipWhenBwd = + case fwdOrBwd of + Fwd -> id + Bwd -> ( -1 *^ ) + tgt_flipped = flipWhenBwd tgt + in + if isNothing $ between CCW startTgt endTgt tgt_flipped + then Nothing + else + let midTgt = startTgt ^+^ endTgt + in + Just $ + OutlinePoint + { outlinePoint = pt + , outlineParam = 0 + , outlineTangent = tgt + , outlineDotProduct = tgt ^.^ midTgt + } + + n :: Int + n = Seq.length strokeData + + fromDomain :: Double -> ( Int, Double ) + fromDomain is = + let ( i0, s ) = is `divMod'` 1 + in ( i0 `mod` n, s ) + +-------------------------------------------------------------------------------- +-- Finding cusps in the envelope equation using root isolation. + +-- | Computes the brush stroke coordinates of a cusp from +-- the @(t,s)@ parameter values. +cuspCoords :: ( ℝ 1 -> Seq ( ℝ 1 -> StrokeDatum 3 NonIV ) ) + -> ( Int, 𝕀ℝ 2 ) + -> ( Cusp, Bool ) +cuspCoords eqs ( i, box ) + = let p0 = boxMidpoint box + ( ℝ2 t_final s_final ) + = newton ( cuspEqnPiece eqs i ) p0 + StrokeDatum + { ee = D22 ( ℝ1 ee ) _ _ _ _ _ + , 𝛿E𝛿s_unrot_dcdt = Unrotated ( D12 { _D12_v = T ( ℝ2 f g ) } ) + , dpath + , stroke + } + = ( eqs ( ℝ1 t_final ) `Seq.index` i ) ( ℝ1 s_final ) + in + ( Cusp + { cuspParameters = ℝ2 t_final s_final + , cuspPathCoords = + case dpath of + D31 p p' p'' _ -> D21 p p' p'' + , cuspStrokeCoords = stroke + , cornerCusp = False + } + , abs ee < 1e-3 && abs f < 1e-2 && abs g < 1e-2 + ) + +cornerCuspCoords :: ( ( I NonIV 1 -> Seq ( CornerStrokeDatum 3 NonIV ) ) -> Int -> I NonIV 1 -> D1𝔸1 Double ) + -> ( ℝ 1 -> Seq ( CornerStrokeDatum 3 NonIV ) ) + -> ( Int, 𝕀ℝ 1 ) + -> ( Cusp, Double ) +cornerCuspCoords mk_eq eqs ( i, box ) + = let p0@( ℝ1 t0 ) = boxMidpoint box + f t = + case mk_eq eqs i ( ℝ1 t ) of + D11 x ( T x' ) -> (# x, x' #) + mb_t_final + = newtonRaphson 40 8 f t0 + ℝ1 t_final = + case mb_t_final of { Nothing -> p0; Just t -> ℝ1 t } + (# ok, _ #) = f t_final + CornerStrokeDatum + { dpath, dstroke = D31 ( Corner stroke _ _ ) _ _ _ } + = eqs ( ℝ1 t_final ) `Seq.index` i + in + ( Cusp + { cuspParameters = ℝ2 t_final 0 + , cuspPathCoords = + case dpath of + D31 p p' p'' _ -> D21 p p' p'' + , cuspStrokeCoords = stroke + , cornerCusp = True + } + , ok + ) + +-- | Find cusps in the envelope for values of the parameters in +-- \( 0 \leqslant t, s \leqslant 1 \), using interval arithmetic. +-- +-- See 'isolateRootsIn' for details of the algorithms used. +findCusps + :: ( RootIsolationOptions 1 1, RootIsolationOptions 1 2, RootIsolationOptions 2 3 ) + -> ( 𝕀ℝ 1 -> ( Seq ( CornerStrokeDatum 3 IsIV ), Seq ( 𝕀ℝ 1 -> StrokeDatum 3 IsIV ) ) ) + -> ( ( IntMap ( [ ( Box 1, RootIsolationTree ( Box 1 ) ) ], DoneBoxes 1 ) + , IntMap ( [ ( Box 1, RootIsolationTree ( Box 1 ) ) ], DoneBoxes 1 ) + , IntMap ( [ ( Box 1, RootIsolationTree ( Box 1 ) ) ], DoneBoxes 1 ) + ) + , IntMap ( [ ( Box 2, RootIsolationTree ( Box 2 ) ) ], DoneBoxes 2 ) + ) +findCusps ( opts11, opts12, opts23 ) boxStrokeData = + ( findCornerCuspsIn opts11 opts12 ( fst . boxStrokeData ) + ( IntMap.fromList + [ ( i, [ 𝕀ℝ1 unit ] ) + | i <- [ 0 .. length ( fst $ boxStrokeData ( 𝕀ℝ1 unit ) ) - 1 ] + ] + ) + , findCuspsIn opts23 ( snd . boxStrokeData ) + ( IntMap.fromList + [ ( i, [ 𝕀ℝ2 unit unit ] ) + | i <- [ 0 .. length ( snd $ boxStrokeData ( 𝕀ℝ1 unit ) ) - 1 ] + ] + ) + ) + where + unit :: 𝕀 + unit = 𝕀 0 1 + {-# INLINE unit #-} + +-- | Like 'findCusps', but parametrised over the initial boxes for the +-- root isolation method. +findCuspsIn + :: RootIsolationOptions N 3 + -> ( 𝕀ℝ 1 -> Seq ( 𝕀ℝ 1 -> StrokeDatum 3 IsIV ) ) + -> IntMap [ Box 2 ] + -> IntMap ( [ ( Box N, RootIsolationTree ( Box N ) ) ], DoneBoxes N ) +findCuspsIn opts eqs = + IntMap.mapWithKey + ( \ i -> foldMap ( isolateRootsIn opts ( cuspEqnPieceI eqs i ) ) ) + + +-- TODO: code duplication between 'cuspEqnPiece' and 'cuspEqnPieceI'. +cuspEqnPiece :: ( ℝ 1 -> Seq ( ℝ 1 -> StrokeDatum 3 NonIV ) ) + -> Int + -> ℝ 2 + -> D1𝔸2 (ℝ 3) +cuspEqnPiece eqs i ( ℝ2 t s ) = + let StrokeDatum + { ee = + D22 { _D22_v = ℝ1 ee + , _D22_dx = T ( ℝ1 ee_t ) + , _D22_dy = T ( ℝ1 ee_s ) } + , 𝛿E𝛿s_unrot_dcdt = + Unrotated + D12 { _D12_v = T ( ℝ2 f g ) + , _D12_dx = T ( T ( ℝ2 f_t g_t ) ) + , _D12_dy = T ( T ( ℝ2 f_s g_s ) ) } + } = ( eqs ( ℝ1 t ) `Seq.index` i ) ( ℝ1 s ) + in D12 ( ℝ3 ee f g ) + ( T $ ℝ3 ee_t f_t g_t ) + ( T $ ℝ3 ee_s f_s g_s ) + +cuspEqnPieceI :: ( 𝕀ℝ 1 -> Seq ( 𝕀ℝ 1 -> StrokeDatum 3 IsIV ) ) + -> Int -> 𝕀ℝ 2 -> D1𝔸2 ( 𝕀ℝ 3 ) +cuspEqnPieceI eqs i ( 𝕀ℝ2 t s ) = + let StrokeDatum + { ee = + D22 { _D22_v = 𝕀ℝ1 ee + , _D22_dx = T ( 𝕀ℝ1 ee_t ) + , _D22_dy = T ( 𝕀ℝ1 ee_s ) } + , 𝛿E𝛿s_unrot_dcdt = + Unrotated + D12 { _D12_v = T ( 𝕀ℝ2 f g ) + , _D12_dx = T ( T ( 𝕀ℝ2 f_t g_t ) ) + , _D12_dy = T ( T ( 𝕀ℝ2 f_s g_s ) ) } + } = ( eqs ( 𝕀ℝ1 t ) `Seq.index` i ) ( 𝕀ℝ1 s ) + in D12 ( 𝕀ℝ3 ee f g ) + ( T $ 𝕀ℝ3 ee_t f_t g_t ) + ( T $ 𝕀ℝ3 ee_s f_s g_s ) + + +-- | Like 'findCuspsIn' but in the case that the envelope is traced out +-- by a brush corner. +findCornerCuspsIn + :: RootIsolationOptions 1 1 + -> RootIsolationOptions 1 2 + -> ( 𝕀ℝ 1 -> Seq ( CornerStrokeDatum 3 IsIV ) ) + -> IntMap [ Box 1 ] + -> ( IntMap ( [ ( Box 1, RootIsolationTree ( Box 1 ) ) ], DoneBoxes 1 ) + , IntMap ( [ ( Box 1, RootIsolationTree ( Box 1 ) ) ], DoneBoxes 1 ) + , IntMap ( [ ( Box 1, RootIsolationTree ( Box 1 ) ) ], DoneBoxes 1 ) + ) +findCornerCuspsIn opts1 opts2 eqs initBoxes = + ( IntMap.mapWithKey + ( \ i -> foldMap ( isolateRootsIn opts2 ( cornerCuspEqnPiece eqs i ) ) ) + initBoxes + , IntMap.mapWithKey + ( \ i -> foldMap ( isolateRootsIn opts1 ( fmap 𝕀ℝ1 . startCornerEqnPiece eqs i ) ) ) + initBoxes + , IntMap.mapWithKey + ( \ i -> foldMap ( isolateRootsIn opts1 ( fmap 𝕀ℝ1 . endCornerEqnPiece eqs i ) ) ) + initBoxes + ) + +cornerCuspEqnPiece + :: ( I i 1 -> Seq ( CornerStrokeDatum 3 i ) ) + -> Int -> I i 1 -> D1𝔸1 ( I i 2 ) +cornerCuspEqnPiece eqs i t = + let CornerStrokeDatum + { dstroke = + D31 _ + ( T ( Corner ( T -> c' ) _ _ ) ) + ( T ( Corner ( T -> c'' ) _ _ ) ) + _ + } = ( eqs t `Seq.index` i ) + in D11 ( unT c' ) c'' + +cornerCuspEqnPiece' + :: ( ℝ 1 -> Seq ( CornerStrokeDatum 3 NonIV ) ) + -> Int -> ℝ 1 -> D1𝔸1 Double +cornerCuspEqnPiece' eqs i t = + case cornerCuspEqnPiece eqs i t of + D11 ( ℝ2 e1 e2 ) ( T ( ℝ2 de1 de2 ) ) -> + let v = Ring.sqrt ( e1 * e1 + e2 * e2 ) + in D11 v ( T $ if v < Ring.fromRational 1e-12 + then Ring.fromInteger 0 + else ( e1 * de1 + e2 * de2 ) / v + ) + +startCornerEqnPiece + :: forall i. Cross ( I i Double ) ( T ( I i 2 ) ) + => ( I i 1 -> Seq ( CornerStrokeDatum 3 i ) ) + -> Int -> I i 1 -> D1𝔸1 ( I i Double ) +startCornerEqnPiece eqs i t = + let CornerStrokeDatum + { dstroke = + D31 ( Corner _ t_s _ ) + ( T ( Corner c' t'_s _ ) ) + ( T ( Corner ( T -> c'' ) _ _ ) ) + _ + } = ( eqs t `Seq.index` i ) + in parallelEqn @i ( D11 c' c'' ) ( D11 ( unT t_s ) t'_s ) + +endCornerEqnPiece + :: forall i. Cross ( I i Double ) ( T ( I i 2 ) ) + => ( I i 1 -> Seq ( CornerStrokeDatum 3 i ) ) + -> Int -> I i 1 -> D1𝔸1 ( I i Double ) +endCornerEqnPiece eqs i t = + let CornerStrokeDatum + { dstroke = + D31 ( Corner _ _ t_e ) + ( T ( Corner c' _ t'_e ) ) + ( T ( Corner ( T -> c'' ) _ _ ) ) + _ + } = ( eqs t `Seq.index` i ) + in parallelEqn @i ( D11 c' c'' ) ( D11 ( unT t_e ) t'_e ) + + +-- | An equation whose roots are when the input vectors are parallel. +parallelEqn + :: Cross ( I i Double ) ( T ( I i 2 ) ) + => D1𝔸1 ( I i 2 ) -> D1𝔸1 ( I i 2 ) -> D1𝔸1 ( I i Double ) +parallelEqn ( D11 ( T -> u ) u' ) ( D11 ( T -> v ) v' ) = + D11 ( u × v ) ( T $ u' × v Ring.+ u × v' ) + -- TODO: this allows anti-parallel vectors, but the equations below + -- (which require strict parallelism) seem to perform worse + -- with interval arithmetic. Perhaps due to the square roots? +{- + D11 ( c Ring.- n ) ( T $ r Ring.- ( c Ring.* r Ring.+ s Ring.* t ) Ring./ n ) + where + + c = u ^.^ v + s = u × v + n = Ring.sqrt ( c Ring.^ 2 Ring.+ s Ring.^ 2 ) + + p = c Ring.- n + + r = u ^.^ v' Ring.+ u' ^.^ v + t = u × v' Ring.+ u' × v +-} + + +-------------------------------------------------------------------------------- +-- Dealing with rotation + +applyRotation :: forall i nbBrushParams + . ( Representable ( I i Double ) ( I i 2 ) + , RepDim ( I i 2 ) ~ 2 + , Ring.Transcendental ( I i Double ) + , Differential 2 ( Dim ( I i nbBrushParams ) ) + ) + => I i nbBrushParams + -> BrushFn i 2 nbBrushParams + -> Maybe ( I i nbBrushParams -> I i Double ) + -> Spline Closed () ( I i 2 ) +applyRotation brushParams ( Unrotated unrotatedShapeFn ) mbRot = + let unrotatedShape = fun unrotatedShapeFn brushParams + in case mbRot of + Nothing -> unrotatedShape + Just getθ -> + let θ = getθ brushParams + cosθ = Ring.cos θ + sinθ = Ring.sin θ + in fmap ( unT . rotate cosθ sinθ . T ) unrotatedShape +{-# INLINEABLE applyRotation #-}
+ src/lib/Math/Bezier/Stroke/EnvelopeEquation.hs view
@@ -0,0 +1,686 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE DuplicateRecordFields #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE RebindableSyntax #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Bezier.Stroke.EnvelopeEquation + ( StrokeDatum(..), CornerStrokeDatum(..) + , HasBézier(..) + , HasEnvelopeEquation(..) + ) where + +-- base +import Prelude hiding ( Num(..), (^), pi, sin, cos ) +import Data.Coerce + ( coerce ) +import Data.Kind + ( Type, Constraint ) +import Data.List.NonEmpty + ( NonEmpty(..) ) +import GHC.TypeNats + ( Nat, type (-) ) + +-- acts +import Data.Act + ( Torsor ((-->)) ) + +-- brush-strokes +import Calligraphy.Brushes + ( Corner (..), Unrotated (..) ) +import Math.Algebra.Dual +import qualified Math.Bezier.Cubic as Cubic +import qualified Math.Bezier.Quadratic as Quadratic +import Math.Differentiable + ( I, IVness(..), SingIVness (..) ) +import Math.Interval +import Math.Linear +import Math.Module + ( Module(..), Cross((×)), lerp ) +import Math.Ring +import qualified Math.Ring as Ring + +-------------------------------------------------------------------------------- + +-- | Quantities related to the envelope equation for a brush stroke. +-- +-- These are used to formulate: +-- +-- - the envelope equation, that determines whether a point is on the outline, +-- - the cusp equation, that determines the presence of cusps in the outline. +type StrokeDatum :: Nat -> IVness -> Type +data StrokeDatum k i + = StrokeDatum + { -- | Path \( p(t) \). + dpath :: D k 1 ( I i 2 ) + -- | Brush shape \( b(t, s) \), not including rotation. + , dbrush :: Unrotated ( D k 2 ( I i 2 ) ) + -- | (Optional) rotation angle \( \theta(t) \). + , mbRotation :: Maybe ( D k 1 ( I i Double ) ) + + -- Everything below is computed in terms of the first three fields. + + -- | Stroke shape, including rotation (if any): + -- + -- \( c(t,s) = p(t) + R(\theta(t)) b(t,s) \). + , stroke :: I i 2 + -- TODO: return this as D 2 1 ( I i 2 ), i.e. include + -- the total derivative with respect to t. + + -- | \( u(t,s) = R(-\theta(t)) \frac{\partial c}{\partial t} \), + -- \( v(t,s) = R(-\theta(t)) \frac{\partial c}{\partial s} \) + , du, dv :: D ( k - 1 ) 2 ( I i 2 ) + + -- | Envelope function + -- + -- \[ \begin{align} E(t,s) &= \frac{\partial c}{\partial t} \times \frac{\partial c}{\partial s} \\ + -- &= \left ( R(-\theta(t)) \frac{\partial c}{\partial t} \right ) \times \left ( R(-\theta(t)) \frac{\partial c}{\partial s} \right ) \\ + -- &= \left ( R(-\theta(t)) p'(t) + \theta'(t) S b(t,s) + \frac{\partial b}{\partial t} \right ) \times \frac{\partial b}{\partial s} \] + -- + -- where \( S = R(\pi/2) \) is a constant rotation. + , ee :: D ( k - 1 ) 2 ( I i 1 ) + + -- | \[ \frac{\partial E}{\partial s} R(-\theta(t)) \frac{\mathrm{d} c}{\mathrm{d} t}, \] + -- + -- where \( \frac{\mathrm{d} c}{\mathrm{d} t} \) + -- + -- denotes a total derivative, which is computed by implicit differentiation + -- of the envelope equation \( E(t,s) = 0 \): + -- + -- \[ \frac{\mathrm{d} c}{\mathrm{d} t} = \frac{\partial c}{\partial t} - \frac{\partial c}{\partial s} \frac{\partial E / \partial t }{ \partial E / \partial s } \] + -- + -- \[ R(-\theta(t)) \frac{\mathrm{d} c}{\mathrm{d} t} = u - v \frac{\partial E / \partial t }{ \partial E / \partial s } \] + , 𝛿E𝛿s_unrot_dcdt :: Unrotated ( D ( k - 2 ) 2 ( T ( I i 2 ) ) ) + } + +deriving stock instance + ( Show ( I i 2 ) + , Show ( D k 1 ( I i Double ) ) + , Show ( D k 1 ( I i 2 ) ) + , Show ( D k 2 ( I i 2 ) ) + , Show ( D ( k - 1 ) 2 ( I i 1 ) ) + , Show ( D ( k - 1 ) 2 ( I i 2 ) ) + , Show ( D ( k - 2 ) 2 ( T ( I i 2 ) ) ) + ) + => Show ( StrokeDatum k i ) + + +-- | Like 'StrokeDatum', except that it applies for the parts of the stroke +-- which are drawn out by a corner of a brush. +type CornerStrokeDatum :: Nat -> IVness -> Type +data CornerStrokeDatum k i + = CornerStrokeDatum + { -- | Path \( p(t) \). + dpath :: D k 1 ( I i 2 ) + -- | The coordinate of the brush corner \( b(t) \) (not including rotation), + -- together with the start/end angles of the corner (not including rotation). + , dcorner :: Unrotated ( D k 1 ( Corner ( I i 2 ) ) ) + -- | (Optional) rotation angle \( \theta(t) \). + , mbRotation :: Maybe ( D k 1 ( I i Double ) ) + + -- Everything below is computed in terms of the first three fields. + + -- | The path being traced out by the corner of the brush, + -- including the rotation (if any): + -- + -- \( c(t) = p(t) + R(\theta(t)) b(t) \) + , dstroke :: D k 1 ( Corner ( I i 2 ) ) + } + +deriving stock instance + ( Show ( I i Double ) + , Show ( I i 2 ) + , Show ( D k 1 ( I i Double ) ) + , Show ( D k 1 ( I i 2 ) ) + , Show ( D k 1 ( Corner ( I i 2 ) ) ) + ) + => Show ( CornerStrokeDatum k i ) + +-------------------------------------------------------------------------------- + +type HasBézier :: Nat -> IVness -> Constraint +class HasBézier k i where + + -- | Linear interpolation, as a differentiable function. + line :: forall ( n :: Nat ) + . ( Module Double ( T ( ℝ n ) ), Torsor ( T ( ℝ n ) ) ( ℝ n ) + , Module ( I i Double ) ( T ( I i n ) ), Torsor ( T ( I i n ) ) ( I i n ) + ) + => ( I i 1 -> I i Double ) + -> Segment ( I i n ) -> C k ( I i 1 ) ( I i n ) + + -- | A quadratic Bézier curve, as a differentiable function. + bezier2 :: forall ( n :: Nat ) + . ( Module Double ( T ( ℝ n ) ), Torsor ( T ( ℝ n ) ) ( ℝ n ) + , Module ( I i Double ) ( T ( I i n ) ), Torsor ( T ( I i n ) ) ( I i n ) + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + ) + => ( I i 1 -> I i Double ) + -> Quadratic.Bezier ( I i n ) -> C k ( I i 1 ) ( I i n ) + + -- | A cubic Bézier curve, as a differentiable function. + bezier3 :: forall ( n :: Nat ) + . ( Module Double ( T ( ℝ n ) ), Torsor ( T ( ℝ n ) ) ( ℝ n ) + , Module ( I i Double ) ( T ( I i n ) ), Torsor ( T ( I i n ) ) ( I i n ) + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + ) + => ( I i 1 -> I i Double ) + -> Cubic.Bezier ( I i n ) -> C k ( I i 1 ) ( I i n ) + +type HasEnvelopeEquation :: Nat -> Constraint +class HasEnvelopeEquation k where + + uncurryD :: D k 1 ( D k 1 b ) -> D k 2 b + + -- | The envelope function + -- + -- \[ E = \frac{\partial c}{\partial t} \times \frac{\partial c}{\partial s}, \] + -- + -- together with the 2-vector + -- + -- \[ \frac{\partial E}{\partial s} \frac{\mathrm{d} c}{\mathrm{d} t}, \] + -- + -- where \( \frac{\mathrm{d} c}{\mathrm{d} t} \) + -- + -- denotes a total derivative. + envelopeEquation :: forall i + . ( Module ( I i Double ) ( T ( I i 1 ) ) + , Cross ( I i Double ) ( T ( I i 2 ) ) + , Transcendental ( I i Double ) + , Representable ( I i Double ) ( I i 2 ) + , RepDim ( I i 2 ) ~ 2 + + , Show ( StrokeDatum k i ) + ) + => SingIVness i + -> D k 1 ( I i 2 ) + -> Unrotated ( D k 2 ( I i 2 ) ) + -> Maybe ( D k 1 ( I i Double ) ) + -> StrokeDatum k i + + cornerEnvelopeEquation + :: ( Module ( I i Double ) ( T ( I i 1 ) ) + , Cross ( I i Double ) ( T ( I i 2 ) ) + , Transcendental ( I i Double ) + , Representable ( I i Double ) ( I i 2 ) + , RepDim ( I i 2 ) ~ 2 + + , Show ( CornerStrokeDatum k i ) + ) + => D k 1 ( I i 2 ) + -> Unrotated ( D k 1 ( Corner ( I i 2 ) ) ) + -> Maybe ( D k 1 ( I i Double ) ) + -> CornerStrokeDatum k i + +instance HasBézier 2 NonIV where + line co ( Segment a b :: Segment b ) = + D \ ( co -> t ) -> + D21 ( lerp @( T b ) t a b ) + ( a --> b ) + origin + {-# INLINEABLE line #-} + + bezier2 co ( bez :: Quadratic.Bezier b ) = + D \ ( co -> t ) -> + D21 ( Quadratic.bezier @( T b ) bez t ) + ( Quadratic.bezier' bez t ) + ( Quadratic.bezier'' bez ) + {-# INLINEABLE bezier2 #-} + + bezier3 co ( bez :: Cubic.Bezier b ) = + D \ ( co -> t ) -> + D21 ( Cubic.bezier @( T b ) bez t ) + ( Cubic.bezier' bez t ) + ( Cubic.bezier'' bez t ) + {-# INLINEABLE bezier3 #-} + +instance HasEnvelopeEquation 2 where + + uncurryD = uncurryD2 + {-# INLINEABLE uncurryD #-} + + envelopeEquation ( ivness :: SingIVness i ) + dp@( D21 ( T -> p ) p_t p_tt ) + db@( Unrotated ( D22 ( T -> b ) b_t b_s b_tt b_ts b_ss ) ) + mbRotation = + StrokeDatum + { dpath = dp + , dbrush = db + , mbRotation + , stroke = c + , du, dv, ee, 𝛿E𝛿s_unrot_dcdt + } + where + co :: I i Double -> I i 1 + co = case ivness of + SNonIV -> coerce + SIsIV -> coerce + (ee, 𝛿E𝛿s_unrot_dcdt) = + let + D12 (T -> u) u_t u_s = du + D12 (T -> v) v_t v_s = dv + ee_val, ee_t, ee_s :: I i Double + ee_val= u × v + ee_t = u_t × v + + u × v_t + ee_s = u_s × v + + u × v_s + 𝛿E𝛿s_unrot_dcdt_val = ee_s *^ u ^-^ ee_t *^ v + in ( D12 + ( co ee_val ) + ( T $ co ee_t ) ( T $ co ee_s ) + , Unrotated $ D0 𝛿E𝛿s_unrot_dcdt_val + ) + (c, du, dv) = case mbRotation of + Nothing -> + -- c(t,s) = p(t) + b(t,s) + ( unT $ p ^+^ b + , D12 ( unT $ p_t ^+^ b_t ) + ( p_tt ^+^ b_tt ) b_ts + , D12 ( unT $ b_s ) + b_ts b_ss + ) + Just ( D21 θ ( T θ_t ) ( T θ_tt ) ) -> + -- c(t,s) = p(t) + R(θ(t)) b(t,s) + -- E = ∂c/∂t × ∂c/ds + -- = ( R(-θ(t)) ∂c/∂t ) × ( R(-θ(t)) ∂c/ds ) + -- = ( R(-θ(t)) p'(t) + θ'(t) S b(t,s) + ∂b/∂t ) × ∂b/ds + -- + let rot, rot' :: T ( I i 2 ) -> T ( I i 2 ) + cosθ = cos θ + sinθ = sin θ + -- rot = R(-θ), rot' = R'(-θ) + -- NB: rot' is not the derivative of f(θ) = R(-θ) + rot = rotate cosθ -sinθ + rot' = rotate sinθ cosθ + rot90 = rotate 0 1 + + u, v, u_t, u_s, v_t, v_s :: T ( I i 2 ) + u = rot p_t ^+^ θ_t *^ rot90 b ^+^ b_t + v = b_s + u_t = ( -θ_t *^ rot' p_t + ^+^ rot p_tt + ) + ^+^ + ( θ_tt *^ rot90 b + ^+^ θ_t *^ rot90 b_t + ) + ^+^ b_tt + u_s = θ_t *^ rot90 b_s ^+^ b_ts + v_t = b_ts + v_s = b_ss + + in ( unT $ p ^+^ rotate cosθ sinθ b + , D12 ( unT u ) u_t u_s + , D12 ( unT v ) v_t v_s + ) + {-# INLINEABLE envelopeEquation #-} + + cornerEnvelopeEquation + dpath@( D21 ( T -> p ) p' p'' ) + dcorner@( + Unrotated + ( D21 + ( Corner ( T -> b ) t_s t_e ) + ( T ( Corner ( T -> b' ) t'_s t'_e ) ) + ( T ( Corner ( T -> b'' ) t''_s t''_e ) ) + ) ) + mbRotation + = + let + dstroke = case mbRotation of + Nothing -> + D21 ( Corner ( unT $ p ^+^ b ) t_s t_e ) + ( T $ Corner ( unT $ p' ^+^ b' ) t'_s t'_e ) + ( T $ Corner ( unT $ p'' ^+^ b'' ) t''_s t''_e ) + Just ( D21 θ ( T θ' ) ( T θ'' ) ) -> + let cosθ = Ring.cos θ + sinθ = Ring.sin θ + rot = rotate cosθ sinθ + rot90 = rotate 0 1 + rot' = rot . rot90 + + -- TODO: I shouldn't have to do this by hand. + -- + -- c(t) = p(t) + rot(θ(t)) b(t) + -- + -- abbreviate; let R = rot(θ(t)), R'=rot'(θ(t)), ... + -- + -- c = p + R b + -- c' = p' + R b' + θ' R' b + -- c'' = p'' + R b'' + 2 θ' R' b' + θ'² R'' b + θ'' R' b + d0 v = rot v + d1 v v' = rot v' ^+^ θ' *^ rot' v + d2 v v' v'' = rot v'' + ^+^ ( 2 * θ' ) *^ rot' v' + ^-^ ( θ' Ring.^ 2 ) *^ rot v + ^+^ θ'' *^ rot' v + in + D21 + ( Corner + ( unT $ p ^+^ d0 b ) + ( d0 t_s ) + ( d0 t_e ) + ) + ( T $ Corner + ( unT $ p' ^+^ d1 b b' ) + ( d1 t_s t'_s ) + ( d1 t_e t'_e ) + ) + ( T $ Corner + ( unT $ p'' ^+^ d2 b b' b'' ) + ( d2 t_s t'_s t''_s ) + ( d2 t_e t'_e t''_e ) + ) + in + CornerStrokeDatum + { dpath, dcorner, dstroke, mbRotation } + {-# INLINEABLE cornerEnvelopeEquation #-} + +instance HasBézier 3 NonIV where + + line co ( Segment a b :: Segment b ) = + D \ ( co -> t ) -> + D31 ( lerp @( T b ) t a b ) + ( a --> b ) + origin + origin + {-# INLINEABLE line #-} + + bezier2 co ( bez :: Quadratic.Bezier b ) = + D \ ( co -> t ) -> + D31 ( Quadratic.bezier @( T b ) bez t ) + ( Quadratic.bezier' bez t ) + ( Quadratic.bezier'' bez ) + origin + {-# INLINEABLE bezier2 #-} + + bezier3 co ( bez :: Cubic.Bezier b ) = + D \ ( co -> t ) -> + D31 ( Cubic.bezier @( T b ) bez t ) + ( Cubic.bezier' bez t ) + ( Cubic.bezier'' bez t ) + ( Cubic.bezier''' bez ) + {-# INLINEABLE bezier3 #-} + +instance HasEnvelopeEquation 3 where + + uncurryD = uncurryD3 + {-# INLINEABLE uncurryD #-} + + envelopeEquation ( ivness :: SingIVness i ) + dp@( D31 ( T -> p ) p_t p_tt p_ttt ) + db@( Unrotated ( D32 ( T -> b ) b_t b_s b_tt b_ts b_ss b_ttt b_tts b_tss b_sss ) ) + mbRotation = + StrokeDatum + { dpath = dp + , dbrush = db + , mbRotation + , stroke = c + , du, dv, ee, 𝛿E𝛿s_unrot_dcdt + } + where + co :: I i Double -> I i 1 + co = case ivness of + SNonIV -> coerce + SIsIV -> coerce + (ee, 𝛿E𝛿s_unrot_dcdt) = + let + D22 (T -> u) u_t u_s u_tt u_ts u_ss = du + D22 (T -> v) v_t v_s v_tt v_ts v_ss = dv + ee_val, ee_t, ee_s, ee_tt, ee_ts, ee_ss :: I i Double + ee_val= u × v + ee_t = u_t × v + + u × v_t + ee_s = u_s × v + + u × v_s + ee_tt = u_tt × v + + 2 * ( u_t × v_t ) + + u × v_tt + ee_ts = u_ts × v + + u_t × v_s + + u_s × v_t + + u × v_ts + ee_ss = u_ss × v + + 2 * ( u_s × v_s ) + + u × v_ss + + 𝛿E𝛿s_unrot_dcdt_val = + ee_s *^ u ^-^ ee_t *^ v + 𝛿E𝛿s_unrot_dcdt_t = + ee_ts *^ u ^+^ ee_s *^ u_t + ^-^ ( ee_tt *^ v ^+^ ee_t *^ v_t ) + 𝛿E𝛿s_unrot_dcdt_s = + ee_ss *^ u ^+^ ee_s *^ u_s + ^-^ ( ee_ts *^ v ^+^ ee_t *^ v_s ) + in ( D22 + ( co ee_val ) + ( T $ co ee_t ) ( T $ co ee_s ) + ( T $ co ee_tt) ( T $ co ee_ts ) ( T $ co ee_ss ) + , Unrotated $ D12 𝛿E𝛿s_unrot_dcdt_val ( T 𝛿E𝛿s_unrot_dcdt_t ) ( T 𝛿E𝛿s_unrot_dcdt_s ) + ) + (c, du, dv) = case mbRotation of + Nothing -> + -- c(t,s) = p(t) + b(t,s) + ( unT $ p ^+^ b + , D22 ( unT $ p_t ^+^ b_t ) + ( p_tt ^+^ b_tt ) b_ts + ( p_ttt ^+^ b_ttt ) b_tts b_tss + , D22 ( unT $ b_s ) + b_ts b_ss + b_tts b_tss b_sss + ) + Just ( D31 θ ( T θ_t ) ( T θ_tt ) ( T θ_ttt ) ) -> + -- c(t,s) = p(t) + R(θ(t)) b(t,s) + -- E = ∂c/∂t × ∂c/ds + -- = ( R(-θ(t)) ∂c/∂t ) × ( R(-θ(t)) ∂c/ds ) + -- = ( R(-θ(t)) p'(t) + θ'(t) S b(t,s) + ∂b/∂t ) × ∂b/ds + -- + let rot, rot' :: T ( I i 2 ) -> T ( I i 2 ) + cosθ = cos θ + sinθ = sin θ + -- rot = R(-θ), rot' = R'(-θ), rot'' = R''(-θ) + -- NB: rot' is not the derivative of f(θ) = R(-θ) + rot = rotate cosθ -sinθ + rot' = rotate sinθ cosθ + rot90 = rotate 0 1 + + u, v, u_t, u_s, u_tt, u_ts, u_ss, v_t, v_s, v_tt, v_ts, v_ss :: T ( I i 2 ) + + u = rot p_t ^+^ θ_t *^ rot90 b ^+^ b_t + v = b_s + u_t = ( -θ_t *^ rot' p_t + ^+^ rot p_tt + ) + ^+^ + ( θ_tt *^ rot90 b + ^+^ θ_t *^ rot90 b_t + ) + ^+^ b_tt + u_s = θ_t *^ rot90 b_s ^+^ b_ts + u_tt = ( -( θ_t ^ 2 ) *^ rot p_t + ^-^ θ_tt *^ rot' p_t + ^-^ ( 2 * θ_t ) *^ rot' p_tt + ^+^ rot p_ttt + ) + ^+^ ( θ_ttt *^ rot90 b + ^+^ ( 2 * θ_tt ) *^ rot90 b_t + ^+^ θ_t *^ rot90 b_tt + ) + ^+^ b_ttt + u_ts = θ_tt *^ rot90 b_s + ^+^ θ_t *^ rot90 b_ts + ^+^ b_tts + u_ss = θ_t *^ rot90 b_ss + ^+^ b_tss + v_t = b_ts + v_s = b_ss + v_tt = b_tts + v_ts = b_tss + v_ss = b_sss + + in ( unT $ p ^+^ rotate cosθ sinθ b + , D22 ( unT u ) u_t u_s u_tt u_ts u_ss + , D22 ( unT v ) v_t v_s v_tt v_ts v_ss + ) + {-# INLINEABLE envelopeEquation #-} + + cornerEnvelopeEquation + dpath@( D31 ( T -> p ) p' p'' p''' ) + dcorner@( + Unrotated + ( D31 + ( Corner ( T -> b ) t_s t_e ) + ( T ( Corner ( T -> b' ) t'_s t'_e ) ) + ( T ( Corner ( T -> b'' ) t''_s t''_e ) ) + ( T ( Corner ( T -> b''' ) t'''_s t'''_e ) ) + ) ) + mbRotation = + let + dstroke = case mbRotation of + Nothing -> + D31 ( Corner ( unT $ p ^+^ b ) t_s t_e ) + ( T $ Corner ( unT $ p' ^+^ b' ) t'_s t'_e ) + ( T $ Corner ( unT $ p'' ^+^ b'' ) t''_s t''_e ) + ( T $ Corner ( unT $ p''' ^+^ b''' ) t'''_s t'''_e ) + Just ( D31 θ ( T θ' ) ( T θ'' ) ( T θ''' ) ) -> + let cosθ = Ring.cos θ + sinθ = Ring.sin θ + rot90 = rotate 0 1 + rot = rotate cosθ sinθ + rot' = rot . rot90 + + -- TODO: I shouldn't have to do this by hand. + -- + -- c(t) = p(t) + rot(θ(t)) b(t) + -- + -- abbreviate; let R = rot(θ(t)), R'=rot'(θ(t)), ... + -- + -- c = p + R b + -- c' = p' + R b' + θ' R' b + -- c'' = p'' + R b'' + 2 θ' R' b' + θ'² R'' b + θ'' R' b + -- c''' = p''' + R b''' + 3 θ' R'' ( θ' b' + θ'' b ) + -- + R' ( θ''' b + 3 θ'' b' + 3 θ' b'' ) + -- + θ'³ R''' b + d0 v = rot v + d1 v v' = rot v' ^+^ θ' *^ rot' v + d2 v v' v'' = rot v'' + ^+^ ( 2 * θ' ) *^ rot' v' + ^-^ ( θ' Ring.^ 2 ) *^ rot v + ^+^ θ'' *^ rot' v + d3 v v' v'' v''' + = rot v''' + ^-^ ( 3 * θ' ) *^ ( rot ( θ' *^ v' ^+^ θ'' *^ v ) ) + ^+^ rot' ( θ''' *^ v ^+^ ( 3 * θ'' ) *^ v' ^+^ ( 3 * θ' ) *^ v'' ) + ^-^ ( θ' Ring.^ 3 ) *^ rot' v + in + + D31 + ( Corner + ( unT $ p ^+^ d0 b ) + ( d0 t_s ) + ( d0 t_e ) + ) + ( T $ Corner + ( unT $ p' ^+^ d1 b b' ) + ( d1 t_s t'_s ) + ( d1 t_e t'_e ) + ) + ( T $ Corner + ( unT $ p'' ^+^ d2 b b' b'' ) + ( d2 t_s t'_s t''_s ) + ( d2 t_e t'_e t''_e ) + ) + ( T $ Corner + ( unT $ p''' ^+^ d3 b b' b'' b''' ) + ( d3 t_s t'_s t''_s t'''_s ) + ( d3 t_e t'_e t''_e t'''_e ) + ) + in + CornerStrokeDatum + { dpath, dcorner, dstroke, mbRotation } + {-# INLINEABLE cornerEnvelopeEquation #-} + +instance HasBézier 3 IsIV where + + line co ( Segment a b :: Segment b ) = + D \ ( co -> t ) -> + D31 ( lerp @( T b ) t a b ) + ( a --> b ) + origin + origin + {-# INLINEABLE line #-} + + bezier2 co ( bez :: Quadratic.Bezier b ) = + D \ ( co -> t ) -> + D31 ( aabb bez ( `evaluateQuadratic` t ) ) + ( Quadratic.bezier' bez t ) + ( Quadratic.bezier'' bez ) + origin + {-# INLINEABLE bezier2 #-} + + bezier3 co ( bez :: Cubic.Bezier b ) = + D \ ( co -> t ) -> + D31 ( aabb bez ( `evaluateCubic` t ) ) + ( T $ aabb ( fmap unT $ Cubic.derivative ( fmap T bez ) ) ( `evaluateQuadratic` t ) ) + ( Cubic.bezier'' bez t ) + ( Cubic.bezier''' bez ) + {-# INLINEABLE bezier3 #-} + +{- Note [Computing Béziers over intervals] +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Here's how we evaluate a Bézier curve when the coefficients are intervals. +As we are using axis-aligned interval arithmetic, we reduce to the 1D situation. + +Now, the formulas for Bézier curves (with value of the t parameter in [0,1]) +are convex combinations + + b_0(t) [p_0,q_0] + b_1(t) [p_1,q_1] + ... + b_n(t) [p_n,q_n] + + -- Here b_0, ..., b_n are Bernstein polynomials. + +This means that the minimum value attained by the Bézier curve as we vary +both the time parameter and the values of the points within their respective +intervals will occur when we take + + b_0(t) p_0 + b_1(t) p_1 + ... + b_n(t) p_n + +and the maximum when we take + + b_0(t) q_0 + b_1(t) q_1 + ... + b_n(t) q_n + +For each of these, we can compute a minimum/maximum by computing an axis-aligned +bounding box for the Bézier curve. This is done by computing the derivative +with respect to t and root-finding. +-} + +-- | Evaluate a cubic Bézier curve, when both the coefficients and the +-- parameter are intervals. +evaluateCubic :: Cubic.Bezier 𝕀 -> 𝕀 -> 𝕀 +evaluateCubic bez t = + -- assert (inf t >= 0 && sup t <= 1) "evaluateCubic: t ⊊ [0,1]" $ -- Requires t ⊆ [0,1] + let inf_bez = fmap inf bez + sup_bez = fmap sup bez + mins = fmap (Cubic.bezier @( T Double ) inf_bez) + $ inf t :| ( sup t : filter ( ∈ t ) ( Cubic.extrema inf_bez ) ) + maxs = fmap (Cubic.bezier @( T Double ) sup_bez) + $ inf t :| ( sup t : filter ( ∈ t ) ( Cubic.extrema sup_bez ) ) + in 𝕀 ( minimum mins ) ( maximum maxs ) +{-# INLINEABLE evaluateCubic #-} + +-- | Evaluate a quadratic Bézier curve, when both the coefficients and the +-- parameter are intervals. +evaluateQuadratic :: Quadratic.Bezier 𝕀 -> 𝕀 -> 𝕀 +evaluateQuadratic bez t = + -- assert (inf t >= 0 && sup t <= 1) "evaluateCubic: t ⊊ [0,1]" $ -- Requires t ⊆ [0,1] + let inf_bez = fmap inf bez + sup_bez = fmap sup bez + mins = fmap (Quadratic.bezier @( T Double ) inf_bez) + $ inf t :| ( sup t : filter ( ∈ t ) ( Quadratic.extrema inf_bez ) ) + maxs = fmap (Quadratic.bezier @( T Double ) sup_bez) + $ inf t :| ( sup t : filter ( ∈ t ) ( Quadratic.extrema sup_bez ) ) + in 𝕀 ( minimum mins ) ( maximum maxs ) +{-# INLINEABLE evaluateQuadratic #-}
+ src/lib/Math/Differentiable.hs view
@@ -0,0 +1,82 @@+{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE UnliftedDatatypes #-} + +module Math.Differentiable + ( IVness(..), SingIVness(..) + , I, Differentiable, DiffInterp + ) + where + +-- base +import Data.Kind + ( Type, Constraint ) +import GHC.Exts + ( UnliftedType ) +import GHC.TypeNats + ( Nat ) + +-- brush-strokes +import Math.Algebra.Dual + ( D, HasChainRule ) +import Math.Interval + ( 𝕀, 𝕀ℝ ) +import Math.Linear +import Math.Module +import Math.Ring + ( Transcendental ) + +-------------------------------------------------------------------------------- + +data IVness + = NonIV + | IsIV + +type SingIVness :: IVness -> UnliftedType +data SingIVness iv where + SNonIV :: SingIVness NonIV + SIsIV :: SingIVness IsIV + +-- | Type family to parametrise over both pointwise and interval computations. +type I :: forall l. IVness -> l -> Type +type family I i t +type instance I @Type NonIV Double = Double +type instance I @Type IsIV Double = 𝕀 +type instance I @Nat NonIV n = ℝ n +type instance I @Nat IsIV n = 𝕀ℝ n + +type Differentiable :: Nat -> IVness -> Nat -> Constraint +class + ( Module ( I i Double ) ( T ( I i u ) ) + , HasChainRule ( I i Double ) k ( I i u ) + , Traversable ( D k u ) + , Representable ( I i Double ) ( I i u ) + ) => Differentiable k i u +instance + ( Module ( I i Double ) ( T ( I i u ) ) + , HasChainRule ( I i Double ) k ( I i u ) + , Traversable ( D k u ) + , Representable ( I i Double ) ( I i u ) + ) => Differentiable k i u + +type DiffInterp :: Nat -> IVness -> Nat -> Constraint +class + ( Differentiable k i u + , Interpolatable ( I i Double ) ( I i u ) + , Module ( I i Double ) ( T ( I i Double ) ) + , Module ( D k u ( I i Double ) ) + ( D k u ( I i 2 ) ) + , Transcendental ( D k u ( I i Double ) ) + , Applicative ( D k u ) + , Representable ( I i Double ) ( I i u ) + ) => DiffInterp k i u +instance + ( Differentiable k i u + , Interpolatable ( I i Double ) ( I i u ) + , Module ( I i Double ) ( T ( I i Double ) ) + , Module ( D k u ( I i Double ) ) + ( D k u ( I i 2 ) ) + , Transcendental ( D k u ( I i Double ) ) + , Applicative ( D k u ) + , Representable ( I i Double ) ( I i u ) + ) => DiffInterp k i u
+ src/lib/Math/Epsilon.hs view
@@ -0,0 +1,15 @@+{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Epsilon + ( epsilon, nearZero ) + where + +-------------------------------------------------------------------------------- + +{-# SPECIALISE INLINE epsilon @Float #-} +{-# SPECIALISE INLINE epsilon @Double #-} +epsilon :: forall r. RealFloat r => r +epsilon = encodeFloat 1 ( 1 - floatDigits ( 0 :: r ) ) + +nearZero :: RealFloat r => r -> Bool +nearZero x = abs x < epsilon
+ src/lib/Math/Float/Utils.hs view
@@ -0,0 +1,76 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Float.Utils + ( FPBits(..) + , nextAfter, succFP, prevFP + ) + where + +-- base +import Data.Bits + ( Bits((.&.), shiftL) ) +import Data.Word + ( Word32, Word64 ) +import GHC.Float + ( castFloatToWord32 , castWord32ToFloat + , castDoubleToWord64, castWord64ToDouble + ) + +-------------------------------------------------------------------------------- + +class ( RealFloat f, Num b, Bits b ) => FPBits f b | f -> b, b -> f where + toBits :: f -> b + fromBits :: b -> f + -- | Size in bytes. + sizeOf :: Int + +instance FPBits Float Word32 where + toBits = castFloatToWord32 + fromBits = castWord32ToFloat + sizeOf = 4 + +instance FPBits Double Word64 where + toBits = castDoubleToWord64 + fromBits = castWord64ToDouble + sizeOf = 8 + + +{-# SPECIALISE nextAfter @Float #-} +{-# SPECIALISE nextAfter @Double #-} +{-# INLINEABLE nextAfter #-} +-- | @nextAfter a b@ computes the next floating-point value after @a@ +-- in the direction of @b@. +nextAfter :: forall f b. FPBits f b => f -> f -> f +nextAfter a b + | isNaN a + = a + | isNaN b + = b + | a == b + = b + | otherwise + = let !res_bits + | a == 0 + , let !sgn_mask = 1 `shiftL` ( sizeOf @f * 8 - 1 ) + = ( toBits b .&. sgn_mask ) + 1 + | ( a < b ) == ( a > 0 ) + = toBits a + 1 + | otherwise + = toBits a - 1 + in fromBits res_bits + +{-# SPECIALISE succFP @Float #-} +{-# SPECIALISE succFP @Double #-} +{-# INLINEABLE succFP #-} +-- | The next floating-point number. +succFP :: forall f b. FPBits f b => f -> f +succFP x = nextAfter x (1/0) + + +{-# SPECIALISE prevFP @Float #-} +{-# SPECIALISE prevFP @Double #-} +{-# INLINEABLE prevFP #-} +-- | The previous floating-point number. +prevFP :: forall f b. FPBits f b => f -> f +prevFP x = nextAfter x (-1/0)
+ src/lib/Math/Interval.hs view
@@ -0,0 +1,489 @@+{-# LANGUAGE CPP #-} + +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +{-# OPTIONS_GHC -Wno-orphans #-} + +module Math.Interval + ( 𝕀(𝕀), inf, sup + , midpoint , scaleInterval, width + + , hull + , addWithErr, subWithErr, prodWithErr, divWithErr + + , isCanonical + , (∩), (⊘), (∈), (∈∈), (⊖) + , extendedRecip + + , singleton, point + , nonDecreasing + , aabb, bisect + + , 𝕀ℝ(..) + ) + where + +-- base +import Prelude hiding ( Num(..), Fractional(..) ) +import qualified Data.List.NonEmpty as NE + +-- acts +import Data.Act + ( Act((•)), Torsor((-->)) ) + +-- groups +import Data.Group + ( Group(..) ) + +-- groups-generic +import Data.Group.Generics + ( ) + +-- brush-strokes +import Math.Algebra.Dual +import Math.Float.Utils + ( prevFP ) +import Math.Interval.Internal + ( 𝕀(𝕀), inf, sup + , scaleInterval, width, singleton + , hull + , addWithErr, subWithErr, prodWithErr + , 𝕀ℝ(..), divWithErr + ) +import Math.Linear + ( ℝ(..), T(..) + , RepresentableQ(..), Representable(..), coordinates + ) +import Math.Module +import Math.Monomial +import Math.Ring + +-------------------------------------------------------------------------------- +-- Interval arithmetic. + +type instance Dim ( 𝕀ℝ n ) = n + +-- | Turn a non-decreasing function into a function on intervals. +nonDecreasing :: forall n m + . ( Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , Representable Double ( ℝ m ) + , Representable 𝕀 ( 𝕀ℝ m ) ) + => ( ℝ n -> ℝ m ) -> 𝕀ℝ n -> 𝕀ℝ m +nonDecreasing f u = + let + lo, hi :: ℝ n + lo = tabulate \ i -> inf ( u `index` i ) + hi = tabulate \ i -> sup ( u `index` i ) + lo', hi' :: ℝ m + lo' = f lo + hi' = f hi + in tabulate \ j -> 𝕀 ( lo' `index` j ) ( hi' `index` j ) +{-# INLINEABLE nonDecreasing #-} + +-- | Does the given value lie inside the specified interval? +(∈) :: Double -> 𝕀 -> Bool +x ∈ 𝕀 lo hi = x >= lo && x <= hi +infix 4 ∈ + +-- | Does the given point lie inside the specified box? +(∈∈) :: ( Representable Double ( ℝ n ), Representable 𝕀 ( 𝕀ℝ n ) ) => ℝ n -> 𝕀ℝ n -> Bool +p ∈∈ i = and $ (∈) <$> coordinates p <*> coordinates i +infix 4 ∈∈ +{-# INLINEABLE (∈∈) #-} +{-# SPECIALISE INLINE (∈∈) @1 #-} +{-# SPECIALISE INLINE (∈∈) @2 #-} +{-# SPECIALISE INLINE (∈∈) @3 #-} +{-# SPECIALISE INLINE (∈∈) @4 #-} + +-- | Is this interval canonical, i.e. it consists of either 1 or 2 floating point +-- values only? +isCanonical :: 𝕀 -> Bool +isCanonical ( 𝕀 x_inf x_sup ) = x_inf >= prevFP x_sup + +-- | The midpoint of an interval. +-- +-- NB: assumes the endpoints are neither @NaN@ nor infinity. +midpoint :: 𝕀 -> Double +midpoint ( 𝕀 x_inf x_sup ) = 0.5 * ( x_inf + x_sup ) + +point :: ( Representable Double ( ℝ n ), Representable 𝕀 ( 𝕀ℝ n ) ) + => ℝ n -> 𝕀ℝ n +point x = tabulate \ i -> singleton ( x `index` i ) +{-# INLINEABLE point #-} + +-- | Compute the intersection of two intervals. +-- +-- Returns whether the first interval is a strict subset of the second interval +-- (or the intersection is a single point). +(∩) :: 𝕀 -> 𝕀 -> [ ( 𝕀, Bool ) ] +𝕀 lo1 hi1 ∩ 𝕀 lo2 hi2 + | lo > hi + = [ ] + | otherwise + = [ ( 𝕀 lo hi, ( lo1 > lo2 && hi1 < hi2 ) || ( lo == hi ) ) ] + where + lo = max lo1 lo2 + hi = min hi1 hi2 +infix 3 ∩ + +infixl 6 ⊖ +(⊖) :: 𝕀 -> 𝕀 -> 𝕀 +(⊖) a@( 𝕀 lo1 hi1 ) b@( 𝕀 lo2 hi2 ) + | width a >= width b + = 𝕀 ( lo1 - lo2 ) ( hi1 - hi2 ) + | otherwise + = 𝕀 ( hi1 - hi2 ) ( lo1 - lo2 ) + +-- | Bisect an interval. +-- +-- Generally returns two sub-intervals, but returns the input interval if +-- it is canonical (and thus bisection would be pointless). +bisect :: 𝕀 -> NE.NonEmpty 𝕀 +bisect x@( 𝕀 x_inf x_sup ) + | isCanonical x + = NE.singleton x + | otherwise + = 𝕀 x_inf x_mid NE.:| [ 𝕀 x_mid x_sup ] + where x_mid = midpoint x + +deriving via ViaAbelianGroup ( T 𝕀 ) + instance Semigroup ( T 𝕀 ) +deriving via ViaAbelianGroup ( T 𝕀 ) + instance Monoid ( T 𝕀 ) +deriving via ViaAbelianGroup ( T 𝕀 ) + instance Group ( T 𝕀 ) + +instance Act ( T 𝕀 ) 𝕀 where + T g • a = g + a +instance Torsor ( T 𝕀 ) 𝕀 where + a --> b = T $ b - a + +-------------------------------------------------------------------------------- +-- Extended division + +-- | Extended division, returning either 1 or 2 intervals. +-- +-- NB: this function can return overlapping intervals if both arguments contain 0. +-- Otherwise, the returned intervals are guaranteed to be disjoint. +(⊘) :: 𝕀 -> 𝕀 -> [ 𝕀 ] +x ⊘ y +#ifdef ASSERTS + | 0 ∈ x && 0 ∈ y + = error $ unlines + [ "x ⊘ y: both arguments contain zero" + , "x: " ++ show x, "y: " ++ show y ] + | otherwise +#endif + = map ( x * ) ( extendedRecip y ) +infixl 7 ⊘ + +-- | Extended reciprocal, returning either 1 or 2 intervals. +extendedRecip :: 𝕀 -> [ 𝕀 ] +extendedRecip x@( 𝕀 lo hi ) + | lo == 0 && hi == 0 + = [ 𝕀 negInf posInf ] + | lo >= 0 || hi <= 0 + = [ recip x ] + | otherwise + = [ 𝕀 negInf ( recip lo ), 𝕀 ( recip hi ) posInf ] + where + +negInf, posInf :: Double +negInf = -1 / 0 +posInf = 1 / 0 + +------------------------------------------------------------------------------- +-- Lattices. + +aabb :: ( Representable 𝕀 ( 𝕀ℝ n ), Functor f ) + => f ( 𝕀ℝ n ) -> ( f 𝕀 -> 𝕀 ) -> 𝕀ℝ n +aabb fv extrema = tabulate \ i -> extrema ( fmap ( `index` i ) fv ) +{-# INLINEABLE aabb #-} + +-------------------------------------------------------------------------------- + +instance Module 𝕀 ( T ( 𝕀ℝ 0 ) ) where + origin = T 𝕀ℝ0 + _ ^+^ _ = T 𝕀ℝ0 + _ ^-^ _ = T 𝕀ℝ0 + _ *^ _ = T 𝕀ℝ0 + +instance Module 𝕀 ( T ( 𝕀ℝ 1 ) ) where + origin = T $ 𝕀ℝ1 ( unT origin ) + T ( 𝕀ℝ1 x1 ) ^+^ T ( 𝕀ℝ1 x2 ) = T $ 𝕀ℝ1 ( x1 + x2 ) + T ( 𝕀ℝ1 x1 ) ^-^ T ( 𝕀ℝ1 x2 ) = T $ 𝕀ℝ1 ( x1 - x2 ) + k *^ T ( 𝕀ℝ1 x1 ) = T $ 𝕀ℝ1 ( k * x1 ) + +instance Module 𝕀 ( T ( 𝕀ℝ 2 ) ) where + origin = T $ 𝕀ℝ2 ( unT origin ) ( unT origin ) + T ( 𝕀ℝ2 x1 y1 ) ^+^ T ( 𝕀ℝ2 x2 y2 ) = T $ 𝕀ℝ2 ( x1 + x2 ) ( y1 + y2 ) + T ( 𝕀ℝ2 x1 y1 ) ^-^ T ( 𝕀ℝ2 x2 y2 ) = T $ 𝕀ℝ2 ( x1 - x2 ) ( y1 - y2 ) + k *^ T ( 𝕀ℝ2 x1 y1 ) = T $ 𝕀ℝ2 ( k * x1 ) ( k * y1 ) + +instance Module 𝕀 ( T ( 𝕀ℝ 3 ) ) where + origin = T $ 𝕀ℝ3 ( unT origin ) ( unT origin ) ( unT origin ) + T ( 𝕀ℝ3 x1 y1 z1 ) ^+^ T ( 𝕀ℝ3 x2 y2 z2 ) = T $ 𝕀ℝ3 ( x1 + x2 ) ( y1 + y2 ) ( z1 + z2 ) + T ( 𝕀ℝ3 x1 y1 z1 ) ^-^ T ( 𝕀ℝ3 x2 y2 z2 ) = T $ 𝕀ℝ3 ( x1 - x2 ) ( y1 - y2 ) ( z1 - z2 ) + k *^ T ( 𝕀ℝ3 x1 y1 z1 ) = T $ 𝕀ℝ3 ( k * x1 ) ( k * y1 ) ( k * z1 ) + +instance Module 𝕀 ( T ( 𝕀ℝ 4 ) ) where + origin = T $ 𝕀ℝ4 ( unT origin ) ( unT origin ) ( unT origin ) ( unT origin ) + T ( 𝕀ℝ4 x1 y1 z1 w1 ) ^+^ T ( 𝕀ℝ4 x2 y2 z2 w2 ) = T $ 𝕀ℝ4 ( x1 + x2 ) ( y1 + y2 ) ( z1 + z2 ) ( w1 + w2 ) + T ( 𝕀ℝ4 x1 y1 z1 w1 ) ^-^ T ( 𝕀ℝ4 x2 y2 z2 w2 ) = T $ 𝕀ℝ4 ( x1 - x2 ) ( y1 - y2 ) ( z1 - z2 ) ( w1 - w2 ) + k *^ T ( 𝕀ℝ4 x1 y1 z1 w1 ) = T $ 𝕀ℝ4 ( k * x1 ) ( k * y1 ) ( k * z1 ) ( k * w1 ) + +instance Inner 𝕀 ( T ( 𝕀ℝ 2 ) ) where + T ( 𝕀ℝ2 x1 y1 ) ^.^ T ( 𝕀ℝ2 x2 y2 ) = x1 * x2 + y1 * y2 + +instance Cross 𝕀 ( T ( 𝕀ℝ 2 ) ) where + T ( 𝕀ℝ2 x1 y1 ) × T ( 𝕀ℝ2 x2 y2 ) = x1 * y2 - x2 * y1 + +deriving via ViaModule 𝕀 ( T ( 𝕀ℝ n ) ) + instance Module 𝕀 ( T ( 𝕀ℝ n ) ) => Semigroup ( T ( 𝕀ℝ n ) ) +deriving via ViaModule 𝕀 ( T ( 𝕀ℝ n ) ) + instance Module 𝕀 ( T ( 𝕀ℝ n ) ) => Monoid ( T ( 𝕀ℝ n ) ) +deriving via ViaModule 𝕀 ( T ( 𝕀ℝ n ) ) + instance Module 𝕀 ( T ( 𝕀ℝ n ) ) => Group ( T ( 𝕀ℝ n ) ) + +deriving via ViaModule 𝕀 ( 𝕀ℝ n ) + instance Module 𝕀 ( T ( 𝕀ℝ n ) ) => Act ( T ( 𝕀ℝ n ) ) ( 𝕀ℝ n ) +deriving via ( ViaModule 𝕀 ( 𝕀ℝ n ) ) + instance Module 𝕀 ( T ( 𝕀ℝ n ) ) => Torsor ( T ( 𝕀ℝ n ) ) ( 𝕀ℝ n ) + +-------------------------------------------------------------------------------- +-- HasChainRule instances. + +instance HasChainRule 𝕀 2 ( 𝕀ℝ 0 ) where + linearD f v = D0 ( f v ) + chain _ ( D0 gfx ) = D21 gfx origin origin + +instance HasChainRule 𝕀 3 ( 𝕀ℝ 0 ) where + linearD f v = D0 ( f v ) + chain _ ( D0 gfx ) = D31 gfx origin origin origin + +instance HasChainRule 𝕀 2 ( 𝕀ℝ 1 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 1 -> w ) -> 𝕀ℝ 1 -> D2𝔸1 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D2𝔸1 ( 𝕀ℝ 1 ) -> D2𝔸1 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 3 ( 𝕀ℝ 1 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 1 -> w ) -> 𝕀ℝ 1 -> D3𝔸1 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D3𝔸1 ( 𝕀ℝ 1 ) -> D3𝔸1 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 2 ( 𝕀ℝ 2 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 2 -> w ) -> 𝕀ℝ 2 -> D2𝔸2 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D2𝔸1 ( 𝕀ℝ 2 ) -> D2𝔸2 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 3 ( 𝕀ℝ 2 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 2 -> w ) -> 𝕀ℝ 2 -> D3𝔸2 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D3𝔸1 ( 𝕀ℝ 2 ) -> D3𝔸2 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 2 ( 𝕀ℝ 3 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 3 -> w ) -> 𝕀ℝ 3 -> D2𝔸3 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D2𝔸1 ( 𝕀ℝ 3 ) -> D2𝔸3 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 3 ( 𝕀ℝ 3 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 3 -> w ) -> 𝕀ℝ 3 -> D3𝔸3 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D3𝔸1 ( 𝕀ℝ 3 ) -> D3𝔸3 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 2 ( 𝕀ℝ 4 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 4 -> w ) -> 𝕀ℝ 4 -> D2𝔸4 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D2𝔸1 ( 𝕀ℝ 4 ) -> D2𝔸4 w -> D2𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] ) + +instance HasChainRule 𝕀 3 ( 𝕀ℝ 4 ) where + + linearD :: forall w. Module 𝕀 ( T w ) => ( 𝕀ℝ 4 -> w ) -> 𝕀ℝ 4 -> D3𝔸4 w + linearD f v = + let !o = origin @𝕀 @( T w ) + in $$( monTabulateQ \ mon -> + if | isZeroMonomial mon + -> [|| f v ||] + | Just i <- isLinear mon + -> [|| f $$( tabulateQ \ j -> + if | j == i + -> [|| 𝕀 1 1 ||] + | otherwise + -> [|| 𝕀 0 0 ||] + ) ||] + | otherwise + -> [|| unT o ||] + ) + + chain :: forall w. Module 𝕀 ( T w ) => D3𝔸1 ( 𝕀ℝ 4 ) -> D3𝔸4 w -> D3𝔸1 w + chain !df !dg = + let !o = origin @𝕀 @( T w ) + !p = (^+^) @𝕀 @( T w ) + !s = (^*) @𝕀 @( T w ) + in $$( chainRule1NQ + [|| o ||] [|| p ||] [|| s ||] + [|| df ||] [|| dg ||] )
+ src/lib/Math/Interval/Internal.hs view
@@ -0,0 +1,252 @@+{-# LANGUAGE CPP #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +{-# OPTIONS_GHC -Wno-orphans #-} + +module Math.Interval.Internal + ( 𝕀(𝕀, inf, sup) + , scaleInterval, width, singleton + , hull + , addWithErr, subWithErr, prodWithErr, divWithErr + , 𝕀ℝ(..) + ) + where + +-- base +import Prelude hiding ( Num(..), Fractional(..), Floating(..), (^) ) +import Data.Kind + ( Type ) +import Data.Monoid + ( Sum(..) ) +import GHC.Generics + ( Generic ) +import GHC.Show + ( showCommaSpace ) +import GHC.TypeNats + ( Nat ) +import GHC.Exts + ( (+#), (*#) ) + +-- deepseq +import Control.DeepSeq + ( NFData(..) ) + +-- primitive +import Data.Primitive + ( Prim(..) ) + +-- brush-strokes +#if defined(USE_SIMD) +import Math.Interval.Internal.SIMD +#elif defined(USE_FMA) +import Math.Interval.Internal.FMA +#else +import Math.Interval.Internal.RoundedHW +#endif + +import Math.Interval.Internal.FMA + ( addI, subI, prodI, divI ) + +-- brush-strokes +import Math.Float.Utils + ( succFP, prevFP ) +import Math.Linear + ( T(..) + , RepDim, RepresentableQ(..), Representable(..) + , Fin(..) + ) +import Math.Module + ( Module(..) ) +import Math.Ring + +-------------------------------------------------------------------------------- + +-- | An interval reduced to a single point. +singleton :: Double -> 𝕀 +singleton x = 𝕀 x x + +-- | The width of an interval. +-- +-- NB: assumes the endpoints are neither @NaN@ nor infinity. +width :: 𝕀 -> Double +width ( 𝕀 lo hi ) = hi - lo + +-- | Union (hull) of two intervals. +hull :: 𝕀 -> 𝕀 -> 𝕀 +hull ( 𝕀 lo1 hi1 ) ( 𝕀 lo2 hi2 ) = 𝕀 ( min lo1 lo2 ) ( max hi1 hi2 ) + +addWithErr, subWithErr, prodWithErr, divWithErr :: Double -> Double -> (# Double, 𝕀 #) +addWithErr = addI withError2 +subWithErr = subI withError2 +prodWithErr = prodI withError2 +divWithErr = divI withError2 + +-- TODO: not sure why but these don't work properly right now +--{-# SPECIALISE INLINE addI withError2 #-} +--{-# SPECIALISE INLINE subI withError2 #-} +--{-# SPECIALISE INLINE prodI withError2 #-} +--{-# SPECIALISE INLINE divI withError2 #-} + +{-# INLINE withError2 #-} +withError2 :: Double -> Double -> (# Double, 𝕀 #) +withError2 f e = (# f , #) + case compare e 0 of + LT -> 𝕀 ( prevFP f - f ) 0 + EQ -> 𝕀 0 0 + GT -> 𝕀 0 ( succFP f - f) + +-------------------------------------------------------------------------------- +-- Instances for (1D) intervals. + +instance Eq 𝕀 where + 𝕀 a b == 𝕀 c d = + a == c && b == d + +instance Show 𝕀 where + showsPrec _ ( 𝕀 x y ) + = showString "[" + . showsPrec 0 x + . showCommaSpace + . showsPrec 0 y + . showString "]" + +deriving via ViaPrelude 𝕀 + instance AbelianGroup ( T 𝕀 ) +deriving via Sum 𝕀 + instance Module 𝕀 ( T 𝕀 ) + + +-- NB: the SIMD implementation of 𝕀 uses DoubleX2#, and has a custom 'Prim' instance. +#if !defined(USE_SIMD) +instance Prim 𝕀 where + sizeOf# _ = 2# *# sizeOf# (undefined :: Double) + + alignment# _ = alignment# (undefined :: Double) + + indexByteArray# arr# i# = + let infVal = indexByteArray# arr# (2# *# i#) + supVal = indexByteArray# arr# (2# *# i# +# 1#) + in 𝕀 infVal supVal + + readByteArray# arr# i# s0 = + case readByteArray# arr# (2# *# i#) s0 of + (# s1, infVal #) -> case readByteArray# arr# (2# *# i# +# 1#) s1 of + (# s2, supVal #) -> (# s2, 𝕀 infVal supVal #) + + writeByteArray# arr# i# (𝕀 infVal supVal) s0 = + case writeByteArray# arr# (2# *# i#) infVal s0 of + s1 -> writeByteArray# arr# (2# *# i# +# 1#) supVal s1 + + indexOffAddr# addr# i# = + let infVal = indexOffAddr# addr# (2# *# i#) + supVal = indexOffAddr# addr# (2# *# i# +# 1#) + in 𝕀 infVal supVal + + readOffAddr# addr# i# s0 = + case readOffAddr# addr# (2# *# i#) s0 of + (# s1, infVal #) -> case readOffAddr# addr# (2# *# i# +# 1#) s1 of + (# s2, supVal #) -> (# s2, 𝕀 infVal supVal #) + + writeOffAddr# addr# i# (𝕀 infVal supVal) s0 = + case writeOffAddr# addr# (2# *# i#) infVal s0 of + s1 -> writeOffAddr# addr# (2# *# i# +# 1#) supVal s1 +#endif + +-------------------------------------------------------------------------------- + +type 𝕀ℝ :: Nat -> Type +data family 𝕀ℝ n + +data instance 𝕀ℝ 0 = 𝕀ℝ0 + deriving stock ( Show, Eq, Ord, Generic ) + deriving anyclass NFData +newtype instance 𝕀ℝ 1 = 𝕀ℝ1 { un𝕀ℝ1 :: 𝕀 } + deriving stock ( Show, Generic ) + deriving newtype ( Eq, NFData ) +data instance 𝕀ℝ 2 = 𝕀ℝ2 { _𝕀ℝ2_x, _𝕀ℝ2_y :: !𝕀 } + deriving stock Generic + deriving anyclass NFData + deriving stock ( Show, Eq ) +data instance 𝕀ℝ 3 = 𝕀ℝ3 { _𝕀ℝ3_x, _𝕀ℝ3_y, _𝕀ℝ3_z :: !𝕀 } + deriving stock Generic + deriving anyclass NFData + deriving stock ( Show, Eq ) +data instance 𝕀ℝ 4 = 𝕀ℝ4 { _𝕀ℝ4_x, _𝕀ℝ4_y, _𝕀ℝ4_z, _𝕀ℝ4_w :: !𝕀 } + deriving stock Generic + deriving anyclass NFData + deriving stock ( Show, Eq ) + +type instance RepDim ( 𝕀ℝ n ) = n + +instance RepresentableQ 𝕀 ( 𝕀ℝ 0 ) where + tabulateQ _ = [|| 𝕀ℝ0 ||] + indexQ _ _ = [|| 0 ||] + +instance RepresentableQ 𝕀 ( 𝕀ℝ 1 ) where + tabulateQ f = [|| 𝕀ℝ1 $$( f ( Fin 1 ) ) ||] + indexQ p = \ case + _ -> [|| un𝕀ℝ1 $$p ||] + +instance RepresentableQ 𝕀 ( 𝕀ℝ 2 ) where + tabulateQ f = [|| 𝕀ℝ2 $$( f ( Fin 1 ) ) $$( f ( Fin 2 ) ) ||] + indexQ p = \ case + Fin 1 -> [|| _𝕀ℝ2_x $$p ||] + _ -> [|| _𝕀ℝ2_y $$p ||] + +instance RepresentableQ 𝕀 ( 𝕀ℝ 3 ) where + tabulateQ f = [|| 𝕀ℝ3 $$( f ( Fin 1 ) ) $$( f ( Fin 2 ) ) $$( f ( Fin 3 ) ) ||] + indexQ p = \ case + Fin 1 -> [|| _𝕀ℝ3_x $$p ||] + Fin 2 -> [|| _𝕀ℝ3_y $$p ||] + _ -> [|| _𝕀ℝ3_z $$p ||] + +instance RepresentableQ 𝕀 ( 𝕀ℝ 4 ) where + tabulateQ f = [|| 𝕀ℝ4 $$( f ( Fin 1 ) ) $$( f ( Fin 2 ) ) $$( f ( Fin 3 ) ) $$( f ( Fin 4 ) ) ||] + indexQ p = \ case + Fin 1 -> [|| _𝕀ℝ4_x $$p ||] + Fin 2 -> [|| _𝕀ℝ4_y $$p ||] + Fin 3 -> [|| _𝕀ℝ4_z $$p ||] + _ -> [|| _𝕀ℝ4_w $$p ||] + + +instance Representable 𝕀 ( 𝕀ℝ 0 ) where + tabulate _ = 𝕀ℝ0 + {-# INLINE tabulate #-} + index _ _ = 0 + {-# INLINE index #-} + +instance Representable 𝕀 ( 𝕀ℝ 1 ) where + tabulate f = 𝕀ℝ1 ( f ( Fin 1 ) ) + {-# INLINE tabulate #-} + index p = \ case + _ -> un𝕀ℝ1 p + {-# INLINE index #-} + +instance Representable 𝕀 ( 𝕀ℝ 2 ) where + tabulate f = 𝕀ℝ2 ( f ( Fin 1 ) ) ( f ( Fin 2 ) ) + {-# INLINE tabulate #-} + index p = \ case + Fin 1 -> _𝕀ℝ2_x p + _ -> _𝕀ℝ2_y p + {-# INLINE index #-} + +instance Representable 𝕀 ( 𝕀ℝ 3 ) where + tabulate f = 𝕀ℝ3 ( f ( Fin 1 ) ) ( f ( Fin 2 ) ) ( f ( Fin 3 ) ) + {-# INLINE tabulate #-} + index p = \ case + Fin 1 -> _𝕀ℝ3_x p + Fin 2 -> _𝕀ℝ3_y p + _ -> _𝕀ℝ3_z p + {-# INLINE index #-} + +instance Representable 𝕀 ( 𝕀ℝ 4 ) where + tabulate f = 𝕀ℝ4 ( f ( Fin 1 ) ) ( f ( Fin 2 ) ) ( f ( Fin 3 ) ) ( f ( Fin 4 ) ) + {-# INLINE tabulate #-} + index p = \ case + Fin 1 -> _𝕀ℝ4_x p + Fin 2 -> _𝕀ℝ4_y p + Fin 3 -> _𝕀ℝ4_z p + _ -> _𝕀ℝ4_w p + {-# INLINE index #-}
+ src/lib/Math/Interval/Internal/FMA.hs view
@@ -0,0 +1,221 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Interval.Internal.FMA + ( 𝕀(..) + , scaleInterval + , addI, subI, prodI, divI + , withError1 + ) where + +-- base +import Prelude hiding ( Num(..), Fractional(..), Floating(..) ) +import qualified Prelude +import GHC.Exts + ( Double(D#), fmsubDouble#, fnmaddDouble#, TYPE ) + +-- brush-strokes +import Math.Float.Utils + ( prevFP, succFP ) +import Math.Ring + +-- deepseq +import Control.DeepSeq + ( NFData(..) ) + +-- rounded-hw +import Numeric.Rounded.Hardware + ( Rounded(..) ) +import Numeric.Rounded.Hardware.Class + ( intervalFromRational, intervalFromInteger ) +import qualified Numeric.Rounded.Hardware.Interval.NonEmpty as Interval + ( Interval(..) ) + +-------------------------------------------------------------------------------- + +{-# INLINE withError1 #-} +withError1 :: Double -> Double -> ( Double, Double ) +withError1 f e = + case compare e 0 of + LT -> ( prevFP f, f ) + EQ -> ( f, f ) + GT -> ( f, succFP f ) + +addI :: forall rep (r :: TYPE rep). ( Double -> Double -> r ) -> Double -> Double -> r +addI withErr a b = + let !s = a + b + !a' = s - b + !b' = s - a' + !da = a - a' + !db = b - b' + !e = da + db + in s `withErr` e +{-# SPECIALISE addI withError1 #-} +{-# INLINEABLE addI #-} + +subI :: forall rep (r :: TYPE rep). ( Double -> Double -> r ) -> Double -> Double -> r +subI withErr a b = + let !s = a - b + !a' = s - b + !b' = s - a' + !da = a - a' + !db = b - b' + !e = da + db + in s `withErr` e +{-# SPECIALISE subI withError1 #-} +{-# INLINEABLE subI #-} + +{-# INLINE fmsubDouble #-} +fmsubDouble :: Double -> Double -> Double -> Double +fmsubDouble ( D# x ) ( D# y ) ( D# z ) = D# ( fmsubDouble# x y z ) +{-# INLINE fnmaddDouble #-} +fnmaddDouble :: Double -> Double -> Double -> Double +fnmaddDouble ( D# x ) ( D# y ) ( D# z ) = D# ( fnmaddDouble# x y z ) + +prodI :: forall rep (r :: TYPE rep). ( Double -> Double -> r ) -> Double -> Double -> r +prodI withErr a b = + let !p = a * b + !e = fmsubDouble a b p + in p `withErr` e +{-# SPECIALISE prodI withError1 #-} +{-# INLINEABLE prodI #-} + +divI :: forall rep (r :: TYPE rep). ( Double -> Double -> r ) -> Double -> Double -> r +divI withErr a b = + let !r = a / b + !e = fnmaddDouble r b a + in r `withErr` e +{-# SPECIALISE divI withError1 #-} +{-# INLINEABLE divI #-} + +-- | Power of a **non-negative** number to a natural power. +posPowI :: Double -- ^ Assumed to be non-negative! + -> Word -> ( Double, Double ) +posPowI _ 0 = ( 1, 1 ) +posPowI f 1 = ( f, f ) +posPowI f 2 = prodI withError1 f f +posPowI f n + | even n + , let m = n `quot` 2 + ( f²_lo, f²_hi ) = prodI withError1 f f + = ( fst $ posPowI f²_lo m, snd $ posPowI f²_hi m ) + | otherwise + , let m = n `quot` 2 + ( f²_lo, f²_hi ) = prodI withError1 f f + = ( fst $ posPowAcc f²_lo m f, snd $ posPowAcc f²_hi m f ) + +posPowAcc :: Double -> Word -> Double -> ( Double, Double ) +posPowAcc f 1 x = prodI withError1 f x +posPowAcc f n x + | even n + , let m = n `quot` 2 + ( f²_lo, f²_hi ) = prodI withError1 f f + = ( fst $ posPowAcc f²_lo m x, snd $ posPowAcc f²_hi m x ) + | otherwise + , let m = n `quot` 2 + ( f²_lo, f²_hi ) = prodI withError1 f f + ( y_lo, y_hi ) = prodI withError1 f x + = ( fst $ posPowAcc f²_lo m y_lo, snd $ posPowAcc f²_hi m y_hi ) + +-------------------------------------------------------------------------------- + +-- | A non-empty interval of real numbers (possibly unbounded). +data 𝕀 = 𝕀 { inf, sup :: !Double } + +instance NFData 𝕀 where + rnf ( 𝕀 lo hi ) = rnf lo `seq` rnf hi + +instance Prelude.Num 𝕀 where + 𝕀 x_lo x_hi + 𝕀 y_lo y_hi + | let !z_lo = fst $ addI withError1 x_lo y_lo + !z_hi = snd $ addI withError1 x_hi y_hi + = 𝕀 z_lo z_hi + 𝕀 x_lo x_hi - 𝕀 y_lo y_hi + | let !z_lo = fst $ subI withError1 x_lo y_hi + !z_hi = snd $ subI withError1 x_hi y_lo + = 𝕀 z_lo z_hi + negate (𝕀 lo hi) = 𝕀 (Prelude.negate hi) (Prelude.negate lo) + (*) = (*) + fromInteger i = + case intervalFromInteger i of + ( Rounded lo, Rounded hi ) -> 𝕀 lo hi + abs (𝕀 lo hi) + | 0 <= lo + = 𝕀 lo hi + | hi <= 0 + = 𝕀 (Prelude.negate hi) (Prelude.negate lo) + | otherwise + = 𝕀 0 (max (Prelude.negate lo) hi) + signum _ = error "No implementation of signum for intervals" + +instance Ring 𝕀 where + 𝕀 lo1 hi1 * 𝕀 lo2 hi2 + | let !( x_min, x_max ) = prodI withError1 lo1 lo2 + !( y_min, y_max ) = prodI withError1 lo1 hi2 + !( z_min, z_max ) = prodI withError1 hi1 lo2 + !( w_min, w_max ) = prodI withError1 hi1 hi2 + = 𝕀 ( min ( min x_min y_min ) ( min z_min w_min ) ) + ( max ( max x_max y_max ) ( max z_max w_max ) ) + _ ^ 0 = 𝕀 1 1 + iv ^ 1 = iv + 𝕀 lo hi ^ n + | odd n || 0 <= lo + , let !lo' = fst $ posPowI lo n + !hi' = snd $ posPowI hi n + = 𝕀 lo' hi' + | hi <= 0 + , let !lo' = fst $ posPowI (negate hi) n + !hi' = snd $ posPowI (negate lo) n + = 𝕀 lo' hi' + | otherwise + , let !hi1 = snd $ posPowI (negate lo) n + , let !hi2 = snd $ posPowI hi n + = 𝕀 0 ( max hi1 hi2 ) + +instance Prelude.Fractional 𝕀 where + fromRational r = + case intervalFromRational r of + ( Rounded lo, Rounded hi ) -> 𝕀 lo hi + recip ( 𝕀 lo hi ) +-- #ifdef ASSERTS + | lo == 0 + = 𝕀 ( fst $ divI withError1 1 hi ) ( 1 Prelude./ 0 ) + | hi == 0 + = 𝕀 ( -1 Prelude./ 0 ) ( snd $ divI withError1 1 lo ) + | lo > 0 || hi < 0 +-- #endif + = 𝕀 ( fst $ divI withError1 1 hi ) ( snd $ divI withError1 1 lo ) +-- #ifdef ASSERTS + | otherwise + = error "BAD interval recip; should use extendedRecip instead" +-- #endif + p / q = p * Prelude.recip q + +instance Floating 𝕀 where + sqrt = withHW Prelude.sqrt + +instance Transcendental 𝕀 where + pi = 𝕀 3.141592653589793 3.1415926535897936 + cos = withHW Prelude.cos + sin = withHW Prelude.sin + atan = withHW Prelude.atan + +deriving via ViaPrelude 𝕀 + instance AbelianGroup 𝕀 +deriving via ViaPrelude 𝕀 + instance Field 𝕀 + +{-# INLINE withHW #-} +-- | Internal function: use @rounded-hw@ to define a function on intervals. +withHW :: (Interval.Interval Double -> Interval.Interval Double) -> 𝕀 -> 𝕀 +withHW f = \ ( 𝕀 lo hi ) -> + case f ( Interval.I ( Rounded lo ) ( Rounded hi ) ) of + Interval.I ( Rounded x ) ( Rounded y ) -> 𝕀 x y + +scaleInterval :: Double -> 𝕀 -> 𝕀 +scaleInterval s ( 𝕀 lo hi ) = + case compare s 0 of + LT -> 𝕀 ( fst $ prodI withError1 s hi ) ( snd $ prodI withError1 s lo ) + EQ -> 𝕀 0 0 + GT -> 𝕀 ( fst $ prodI withError1 s lo ) ( snd $ prodI withError1 s hi )
+ src/lib/Math/Interval/Internal/RoundedHW.hs view
@@ -0,0 +1,53 @@+ +module Math.Interval.Internal.RoundedHW + ( 𝕀(𝕀, inf, sup) + , scaleInterval + ) + where + +-- base +import Prelude hiding ( Num(..), Fractional(..), Floating(..) ) +import qualified Prelude + +-- brush-strokes +import Math.Ring + +-- deepseq +import Control.DeepSeq + ( NFData(..) ) + +-- rounded-hw +import Numeric.Rounded.Hardware + ( Rounded(..) ) +import qualified Numeric.Rounded.Hardware.Interval.NonEmpty as Interval + ( Interval(..), powInt ) + +-------------------------------------------------------------------------------- + +-- | A non-empty interval of real numbers (possibly unbounded). +newtype 𝕀 = MkI { ival :: Interval.Interval Double } + deriving newtype ( Prelude.Num, Prelude.Fractional, Prelude.Floating ) + deriving newtype NFData + +{-# COMPLETE 𝕀 #-} +pattern 𝕀 :: Double -> Double -> 𝕀 +pattern 𝕀 { inf, sup } = MkI ( Interval.I ( Rounded inf ) ( Rounded sup ) ) + +instance Ring 𝕀 where + MkI i1 * MkI i2 = MkI $ i1 Prelude.* i2 + MkI x ^ n = MkI { ival = Interval.powInt x ( Prelude.fromIntegral n ) } + -- This is very important, as x^2 is not the same as x * x + -- in interval arithmetic. This ensures we don't + -- accidentally use (^) from Prelude. + +deriving via ViaPrelude 𝕀 + instance AbelianGroup 𝕀 +deriving via ViaPrelude 𝕀 + instance Field 𝕀 +deriving via ViaPrelude 𝕀 + instance Floating 𝕀 +deriving via ViaPrelude 𝕀 + instance Transcendental 𝕀 + +scaleInterval :: Double -> 𝕀 -> 𝕀 +scaleInterval s iv = 𝕀 s s * iv -- TODO: could be better
+ src/lib/Math/Interval/Internal/SIMD.hs view
@@ -0,0 +1,86 @@+{-# LANGUAGE CPP #-} +{-# LANGUAGE GHCForeignImportPrim #-} +{-# LANGUAGE UnliftedFFITypes #-} + +{-# OPTIONS_GHC -Wno-orphans #-} + +module Math.Interval.Internal.SIMD + ( 𝕀(𝕀, inf, sup) + , scaleInterval + ) + where + +-- base +import Prelude hiding ( Num(..), Fractional(..), Floating(..) ) +import qualified Prelude +import GHC.Exts + ( (<=##), isTrue# + , Double(D#) + ) + +-- ghc-prim +import GHC.Prim + ( maxDouble# ) + +-- rounded-hw +import Numeric.Rounded.Hardware + ( Rounded(..) ) +import qualified Numeric.Rounded.Hardware.Interval.NonEmpty as Interval + ( Interval(..) ) + +-- brush-strokes +import Math.Ring + +-- brush-strokes:simd-interval +import Math.Interval.Internal.SIMD.Internal + +-------------------------------------------------------------------------------- + +deriving via ViaPrelude 𝕀 + instance AbelianGroup 𝕀 +deriving via ViaPrelude 𝕀 + instance Field 𝕀 + +instance Ring 𝕀 where + (*) = (Prelude.*) + iv ^ 1 = iv + PackI m_lo hi ^ n + | odd n || isTrue# (m_lo <=## 0.0##) + , let !(D# m_lo') = D# m_lo Prelude.^ n + !(D# hi' ) = D# hi Prelude.^ n + = PackI m_lo' hi' + | isTrue# (hi <=## 0.0##) + , let !(D# m_lo') = Prelude.negate $ (D# hi) Prelude.^ n + !(D# hi') = (D# m_lo) Prelude.^ n + = PackI m_lo' hi' + | otherwise + , let !(D# hi1) = (D# m_lo) Prelude.^ n + !(D# hi2) = (D# hi ) Prelude.^ n + hi' = maxDouble# hi1 hi2 + = PackI 0.0## hi' + +instance Floating 𝕀 where + sqrt = withHW Prelude.sqrt + +instance Transcendental 𝕀 where + pi = 𝕀 3.141592653589793 3.1415926535897936 + cos = withHW Prelude.cos + sin = withHW Prelude.sin + atan = withHW Prelude.atan + +{- +TODO: consider alternatives for sin/cos, such as: + + - https://github.com/JishinMaster/simd_utils/blob/160c50f07e08d2077ae4368f0aed2f10f7173c67/simd_utils_sse_double.h#L530 + - Intel SVML + - metalibm + - sleef + +-} + +{-# INLINE withHW #-} +-- | Internal function: use @rounded-hw@ to define a function on intervals. +withHW :: (Interval.Interval Double -> Interval.Interval Double) -> 𝕀 -> 𝕀 +withHW f = \ ( 𝕀 lo hi ) -> + case f ( Interval.I ( Rounded lo ) ( Rounded hi ) ) of + Interval.I ( Rounded x ) ( Rounded y ) -> 𝕀 x y
+ src/lib/Math/Linear.hs view
@@ -0,0 +1,222 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Linear + ( -- * Points and vectors + Segment(..), Mat22(..) + + -- * Points and vectors (second version) + , ℝ(..), T(.., V2, V3, V4) + , Fin(..), MFin(..) + , universe, coordinates, choose + , RepDim, RepresentableQ(..) + , Representable(..), set, injection, projection + , Vec(..), (!), find, zipIndices + + , swap, rotate + + ) where + +-- base +import Prelude hiding ( unzip ) +import Control.Applicative + ( ZipList(..) ) +import Data.Coerce + ( coerce ) +import Data.Kind + ( Type ) +import GHC.Exts + ( proxy# ) +import GHC.Generics + ( Generic, Generic1, Generically(..), Generically1(..) ) +import GHC.Stack + ( HasCallStack ) +import GHC.TypeNats + ( Nat, KnownNat, type (<=) + , natVal' + ) + +-- acts +import Data.Act + ( Act((•)), Torsor((-->)) ) + +-- deepseq +import Control.DeepSeq + ( NFData(..), NFData1 ) + +-- groups +import Data.Group + ( Group(..) ) + +-- groups-generic +import Data.Group.Generics + ( ) + +-- brush-strokes +import Math.Linear.Internal +import Math.Ring + ( Ring ) +import qualified Math.Ring as Ring + +-------------------------------------------------------------------------------- + +data Mat22 = Mat22 !Double !Double !Double !Double +data Segment p = + Segment + { segmentStart :: !p + , segmentEnd :: !p + } + deriving stock ( Generic, Generic1, Functor, Foldable, Traversable ) + deriving ( Semigroup, Monoid, Group ) + via Generically ( Segment p ) + deriving Applicative + via Generically1 Segment + deriving anyclass ( NFData, NFData1 ) + +instance Show p => Show ( Segment p ) where + show ( Segment s e ) = show s ++ " -> " ++ show e + +-------------------------------------------------------------------------------- + +-- | Tangent space to Euclidean space. +type T :: Type -> Type +newtype T e = T { unT :: e } + deriving stock ( Eq, Functor, Foldable, Traversable ) + deriving newtype NFData + +instance Semigroup ( T Double ) where + (<>) = coerce ( (+) @Double ) +instance Monoid ( T Double ) where + mempty = T 0 + +instance Group ( T Double ) where + invert ( T x ) = T ( negate x ) + +instance Act ( T Double ) Double where + T u • v = u + v +instance Torsor ( T Double ) Double where + a --> b = T ( b - a ) + +instance {-# OVERLAPPING #-} Show ( ℝ n ) => Show ( T ( ℝ n ) ) where + show ( T p ) = "V" ++ drop 1 ( show p ) +instance {-# INCOHERENT #-} Show v => Show ( T v ) where + showsPrec p ( T v ) + = showParen ( p >= 11 ) + $ showString "T" + . showsPrec 0 v + +instance Applicative T where + pure = T + T f <*> T a = T ( f a ) + +{-# COMPLETE V2 #-} +pattern V2 :: Double -> Double -> T ( ℝ 2 ) +pattern V2 x y = T ( ℝ2 x y ) + +{-# COMPLETE V3 #-} +pattern V3 :: Double -> Double -> Double -> T ( ℝ 3 ) +pattern V3 x y z = T ( ℝ3 x y z ) + +{-# COMPLETE V4 #-} +pattern V4 :: Double -> Double -> Double -> Double -> T ( ℝ 4 ) +pattern V4 x y z w = T ( ℝ4 x y z w ) + +-------------------------------------------------------------------------------- + +type Vec :: Nat -> Type -> Type +newtype Vec n a = Vec { vecList :: [ a ] } +type role Vec nominal representational + +deriving newtype instance Show a => Show ( Vec n a ) +deriving newtype instance Eq a => Eq ( Vec n a ) +deriving newtype instance Ord a => Ord ( Vec n a ) +deriving newtype instance Functor ( Vec n ) +deriving newtype instance Foldable ( Vec n ) +deriving via ZipList + instance Applicative ( Vec n ) +instance Traversable ( Vec n ) where + traverse f ( Vec as ) = Vec <$> traverse f as +instance NFData a => NFData ( Vec n a ) where + rnf ( Vec as ) = rnf as + +universe :: forall n. KnownNat n => Vec n ( Fin n ) +universe = Vec [ Fin i | i <- [ 1 .. fromIntegral ( natVal' @n proxy# ) ] ] +{-# INLINEABLE universe #-} + +coordinates :: forall r u. ( Representable r u ) => u -> Vec ( RepDim u ) r +coordinates u = fmap ( index u ) $ universe @( RepDim u ) +{-# INLINEABLE coordinates #-} + +-- | Binomial coefficient: choose all subsets of size @k@ of the given set +-- of size @n@. +choose + :: forall n k + . ( KnownNat n, KnownNat k + , 1 <= n, 1 <= k, k <= n + ) + => [ Vec k ( Fin n ) ] +choose = coerce $ go ( fromIntegral $ natVal' @n proxy# ) + ( fromIntegral $ natVal' @k proxy# ) + where + go :: Word -> Word -> [ [ Word ] ] + go n k + | k == 1 + = [ [ i ] | i <- [ 1 .. n ] ] + | n == k + = [ [ 1 .. n ] ] + go n k = go ( n - 1 ) k + ++ ( map ( ++ [ n ] ) $ go ( n - 1 ) ( k - 1 ) ) +{-# INLINEABLE choose #-} + +infixl 9 ! +(!) :: forall l a. HasCallStack => Vec l a -> Fin l -> a +( Vec l ) ! Fin i = l !! ( fromIntegral i - 1 ) + +find :: forall l a. ( a -> Bool ) -> Vec l a -> MFin l +find f ( Vec v ) = MFin ( find_ 1 v ) + where + find_ :: Word -> [ a ] -> Word + find_ j ( a : as ) + | f a + = j + | otherwise + = find_ ( j + 1 ) as + find_ _ [] = 0 + +zipIndices :: forall n a. Vec n a -> [ ( Fin n, a ) ] +zipIndices ( Vec v ) = zipIndices_ 1 v + where + zipIndices_ :: Word -> [ a ] -> [ ( Fin n, a ) ] + zipIndices_ _ [] = [] + zipIndices_ i (a : as) = ( Fin i, a ) : zipIndices_ ( i + 1 ) as + +-------------------------------------------------------------------------------- + +-- | Rotate a vector by the given angle (counter-clockwise), +-- given the cosine and sine of the angle (in that order). +rotate :: ( Representable r m, RepDim m ~ 2, Ring r ) + => r -- \( \cos \theta \) + -> r -- \( \sin \theta \) + -> T m + -> T m +rotate cosθ sinθ ( T xy ) = + let x = xy `index` Fin 1 + y = xy `index` Fin 2 + in T $ tabulate \ case + Fin 1 -> x Ring.* cosθ Ring.- y Ring.* sinθ + _ -> y Ring.* cosθ Ring.+ x Ring.* sinθ +{-# INLINEABLE rotate #-} + +-- | Swap the two coordinates of a 2D vector. +swap :: ( Representable r m, RepDim m ~ 2, Ring r ) + => T m + -> T m +swap ( T xy ) = + let x = xy `index` Fin 1 + y = xy `index` Fin 2 + in T $ tabulate \ case + Fin 1 -> y + _ -> x +{-# INLINEABLE swap #-}
+ src/lib/Math/Linear/Internal.hs view
@@ -0,0 +1,212 @@+{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} + +module Math.Linear.Internal + ( ℝ(..) + , Fin(..), MFin(..) + , RepDim + , RepresentableQ(..) + , Representable(..) + , set + , projection, injection + ) + where + +-- base +import Data.Kind + ( Type, Constraint ) +import Data.List + ( intersperse ) +import GHC.Generics + ( Generic ) +import GHC.Show + ( showSpace ) +import GHC.TypeNats + ( Nat, KnownNat ) + +-- template-haskell +import Language.Haskell.TH + ( CodeQ ) + +-- deepseq +import Control.DeepSeq + ( NFData) + +-------------------------------------------------------------------------------- + +-- | Euclidean space \( \mathbb{R}^n \). +type ℝ :: Nat -> Type +data family ℝ n + +data instance ℝ 0 = ℝ0 + deriving stock ( Show, Eq, Ord, Generic ) + deriving anyclass NFData +newtype instance ℝ 1 = ℝ1 { unℝ1 :: Double } + deriving stock ( Generic ) + deriving newtype ( Eq, Ord, NFData ) +data instance ℝ 2 = ℝ2 { _ℝ2_x, _ℝ2_y :: Double } + deriving stock Generic + deriving anyclass NFData + deriving stock ( Eq, Ord ) +data instance ℝ 3 = ℝ3 { _ℝ3_x, _ℝ3_y, _ℝ3_z :: Double } + deriving stock Generic + deriving anyclass NFData + deriving stock ( Eq, Ord ) +data instance ℝ 4 = ℝ4 { _ℝ4_x, _ℝ4_y, _ℝ4_z, _ℝ4_w :: Double } + deriving stock Generic + deriving anyclass NFData + deriving stock ( Eq, Ord ) + +instance Show ( ℝ 1 ) where + showsPrec p ( ℝ1 x ) + = showParen ( p >= 11 ) + $ showString "ℝ1" + . foldr (.) id ( showSpace : intersperse showSpace coords ) + where + coords = map ( showsPrec 0 ) [ x ] + +instance Show ( ℝ 2 ) where + showsPrec p ( ℝ2 x y ) + = showParen ( p >= 11 ) + $ showString "ℝ2" + . foldr (.) id ( showSpace : intersperse showSpace coords ) + where + coords = map ( showsPrec 0 ) [ x, y ] + +instance Show ( ℝ 3 ) where + showsPrec p ( ℝ3 x y z ) + = showParen ( p >= 11 ) + $ showString "ℝ3" + . foldr (.) id ( showSpace : intersperse showSpace coords ) + where + coords = map ( showsPrec 0 ) [ x, y, z ] + +instance Show ( ℝ 4 ) where + showsPrec p ( ℝ4 x y z w ) + = showParen ( p >= 11 ) + $ showString "ℝ4" + . foldr (.) id ( showSpace : intersperse showSpace coords ) + where + coords = map ( showsPrec 0 ) [ x, y, z, w ] + +-------------------------------------------------------------------------------- + +-- | 1, ..., n +type Fin :: Nat -> Type +newtype Fin n = Fin Word + deriving stock ( Eq, Ord, Show ) + +-- | 0, ..., n +type MFin :: Nat -> Type +newtype MFin n = MFin Word + +type RepDim :: k -> Nat +type family RepDim v + +type RepresentableQ :: Type -> Type -> Constraint +class RepresentableQ r v | v -> r where + tabulateQ :: ( Fin ( RepDim v ) -> CodeQ r ) -> CodeQ v + indexQ :: CodeQ v -> Fin ( RepDim v ) -> CodeQ r + +type Representable :: Type -> Type -> Constraint +class KnownNat ( RepDim v ) => Representable r v | v -> r where + tabulate :: ( Fin ( RepDim v ) -> r ) -> v + index :: v -> Fin ( RepDim v ) -> r + +set :: Representable r v => Fin ( RepDim v ) -> r -> v -> v +set i r u = tabulate \ j -> + if i == j + then r + else index u j +{-# INLINEABLE set #-} + +projection :: ( Representable r u, Representable r v ) + => ( Fin ( RepDim v ) -> Fin ( RepDim u ) ) + -> u -> v +projection f = \ u -> + tabulate \ i -> index u ( f i ) +{-# INLINEABLE projection #-} + +injection :: ( Representable r u, Representable r v ) + => ( Fin ( RepDim v ) -> MFin ( RepDim u ) ) + -> u -> v -> v +injection f = \ u v -> + tabulate \ i -> case f i of + MFin 0 -> index v i + MFin j -> index u ( Fin j ) +{-# INLINEABLE injection #-} + +-------------------------------------------------------------------------------- + +type instance RepDim ( ℝ n ) = n + +instance RepresentableQ Double ( ℝ 0 ) where + tabulateQ _ = [|| ℝ0 ||] + indexQ _ _ = [|| 0 ||] + +instance RepresentableQ Double ( ℝ 1 ) where + tabulateQ f = [|| ℝ1 $$( f ( Fin 1 ) ) ||] + indexQ p = \ case + _ -> [|| unℝ1 $$p ||] + +instance RepresentableQ Double ( ℝ 2 ) where + tabulateQ f = [|| ℝ2 $$( f ( Fin 1 ) ) $$( f ( Fin 2 ) ) ||] + indexQ p = \ case + Fin 1 -> [|| _ℝ2_x $$p ||] + _ -> [|| _ℝ2_y $$p ||] + +instance RepresentableQ Double ( ℝ 3 ) where + tabulateQ f = [|| ℝ3 $$( f ( Fin 1 ) ) $$( f ( Fin 2 ) ) $$( f ( Fin 3 ) ) ||] + indexQ p = \ case + Fin 1 -> [|| _ℝ3_x $$p ||] + Fin 2 -> [|| _ℝ3_y $$p ||] + _ -> [|| _ℝ3_z $$p ||] + +instance RepresentableQ Double ( ℝ 4 ) where + tabulateQ f = [|| ℝ4 $$( f ( Fin 1 ) ) $$( f ( Fin 2 ) ) $$( f ( Fin 3 ) ) $$( f ( Fin 4 ) ) ||] + indexQ p = \ case + Fin 1 -> [|| _ℝ4_x $$p ||] + Fin 2 -> [|| _ℝ4_y $$p ||] + Fin 3 -> [|| _ℝ4_z $$p ||] + _ -> [|| _ℝ4_w $$p ||] + +instance Representable Double ( ℝ 0 ) where + tabulate _ = ℝ0 + {-# INLINE tabulate #-} + index _ _ = 0 + {-# INLINE index #-} + +instance Representable Double ( ℝ 1 ) where + tabulate f = ℝ1 ( f ( Fin 1 ) ) + {-# INLINE tabulate #-} + index p = \ case + _ -> unℝ1 p + {-# INLINE index #-} + +instance Representable Double ( ℝ 2 ) where + tabulate f = ℝ2 ( f ( Fin 1 ) ) ( f ( Fin 2 ) ) + {-# INLINE tabulate #-} + index p = \ case + Fin 1 -> _ℝ2_x p + _ -> _ℝ2_y p + {-# INLINE index #-} + +instance Representable Double ( ℝ 3 ) where + tabulate f = ℝ3 ( f ( Fin 1 ) ) ( f ( Fin 2 ) ) ( f ( Fin 3 ) ) + {-# INLINE tabulate #-} + index p = \ case + Fin 1 -> _ℝ3_x p + Fin 2 -> _ℝ3_y p + _ -> _ℝ3_z p + {-# INLINE index #-} + +instance Representable Double ( ℝ 4 ) where + tabulate f = ℝ4 ( f ( Fin 1 ) ) ( f ( Fin 2 ) ) ( f ( Fin 3 ) ) ( f ( Fin 4 ) ) + {-# INLINE tabulate #-} + index p = \ case + Fin 1 -> _ℝ4_x p + Fin 2 -> _ℝ4_y p + Fin 3 -> _ℝ4_z p + _ -> _ℝ4_w p + {-# INLINE index #-}
+ src/lib/Math/Linear/Solve.hs view
@@ -0,0 +1,78 @@+{-# LANGUAGE ScopedTypeVariables #-} + +{-# OPTIONS_GHC -Wno-incomplete-uni-patterns #-} + +module Math.Linear.Solve + ( linearSolve, fromEigen, toEigen, withEigenSem ) + where + +-- base +import Control.Concurrent.QSem +import Control.Exception + ( evaluate ) +import Data.Maybe + ( fromJust ) +import GHC.TypeNats + ( KnownNat ) +import System.IO.Unsafe + ( unsafePerformIO ) + +-- deepseq +import Control.DeepSeq + +-- eigen +import qualified Eigen.Matrix as Eigen + ( Matrix, toList, fromList, generate, unsafeCoeff ) +import qualified Eigen.Solver.LA as Eigen + ( Decomposition(..), solve ) + +-- brush-strokes +import Math.Linear + +-------------------------------------------------------------------------------- + +linearSolve :: Mat22 -> T ( ℝ 2 ) -> T ( ℝ 2 ) +linearSolve ( Mat22 a b c d ) ( V2 p q ) = V2 u v + where + [[u],[v]] = withEigenSem + $ Eigen.toList + $ Eigen.solve Eigen.JacobiSVD + ( fromJust ( Eigen.fromList [[a,b],[c,d]] ) + :: Eigen.Matrix 2 2 Double ) + ( fromJust ( Eigen.fromList [[p],[q]] ) + :: Eigen.Matrix 2 1 Double ) + +toEigen :: forall n d + . ( KnownNat d, Representable Double ( ℝ n ) ) + => Vec d ( ℝ n ) -> Eigen.Matrix n d Double +toEigen cols = + Eigen.generate $ \ r c -> + ( cols ! Fin ( fromIntegral c + 1 ) ) `index` ( Fin ( fromIntegral r + 1 ) ) +{-# INLINEABLE toEigen #-} + +fromEigen :: forall n d + . ( KnownNat d, Representable Double ( ℝ n ) ) + => Eigen.Matrix n d Double -> Vec d ( ℝ n ) +fromEigen mat = + fmap + ( \ ( Fin c ) -> + tabulate $ \ ( Fin r ) -> + Eigen.unsafeCoeff ( fromIntegral r - 1 ) ( fromIntegral c - 1 ) mat + ) + ( universe @d ) +{-# INLINEABLE fromEigen #-} + +-- | Semaphore to ensure we don't ever call Eigen concurrently, which +-- seems to cause random crashes. +eigenSem :: QSem +eigenSem = unsafePerformIO $ newQSem 1 +{-# NOINLINE eigenSem #-} + +-- | Take a lock on the 'eigenSem' semaphore, to ensure we don't call Eigen +-- concurrently (which seems to cause random crashes). +withEigenSem :: NFData a => a -> a +withEigenSem v = unsafePerformIO $ do + waitQSem eigenSem + res <- evaluate v + signalQSem eigenSem + return res
+ src/lib/Math/Module.hs view
@@ -0,0 +1,228 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +{-# OPTIONS_GHC -Wno-orphans #-} + +module Math.Module + ( Module(..), lerp + , Inner(..), Cross(..) + , Interpolatable + , norm, squaredNorm, normalise + , quadrance, distance + , proj, projC, closestPointOnSegment + , strictlyParallel, convexCombination + + , ViaModule(..) + ) + where + +-- base +import Control.Monad + ( guard ) +import Data.Coerce + ( coerce ) +import Data.Kind + ( Type, Constraint ) +import Data.Monoid + ( Ap(..), Sum(..) ) + +-- acts +import Data.Act + ( Act + ( (•) ) + , Torsor + ( (-->) ) + ) + +-- groups +import Data.Group + ( Group(..) ) + +-- brush-strokes +import Math.Epsilon + ( epsilon ) +import Math.Linear +import Math.Module.Internal + ( Module(..), Inner(..) ) +import Math.Ring + ( Ring ) +import qualified Math.Ring as Ring + +-------------------------------------------------------------------------------- + +instance ( Applicative f, Module r m ) => Module r ( Ap f m ) where + origin = pure origin + (^+^) = liftA2 (^+^) + (^-^) = liftA2 (^-^) + (*^) r = fmap ( r *^ ) + +lerp :: forall v r p. ( Module r v, Torsor v p ) => r -> p -> p -> p +lerp t p0 p1 = ( t *^ ( p0 --> p1 :: v ) ) • p0 + +class Module r m => Cross r m where + (×) :: m -> m -> r + +-- | Norm of a vector, computed using the inner product. +norm :: forall m r. ( Floating r, Inner r m ) => m -> r +norm = sqrt . squaredNorm + +-- | Squared norm of a vector, computed using the inner product. +squaredNorm :: forall m r. Inner r m => m -> r +squaredNorm v = v ^.^ v + +normalise :: ( Floating r, Inner r m ) => m -> m +normalise v = recip ( norm v ) *^ v + +-- | Squared distance between two points. +quadrance :: forall v r p. ( Inner r v, Torsor v p ) => p -> p -> r +quadrance p1 p2 = squaredNorm ( p1 --> p2 :: v ) + +-- | Distance between two points. +distance :: forall v r p. ( Floating r, Inner r v, Torsor v p ) => p -> p -> r +distance p1 p2 = sqrt ( quadrance @v p1 p2 ) + +-- | Projects the first argument onto the second. +proj :: forall m r. ( Inner r m, Fractional r ) => m -> m -> m +proj x y = projC x y *^ y + +-- | Projection constant: how far along the projection of the first vector lands along the second vector. +projC :: forall m r. ( Inner r m, Fractional r ) => m -> m -> r +projC x y = x ^.^ y / squaredNorm y + +closestPointOnSegment + :: forall v r p + . ( Inner r v, Torsor v p, Fractional r, Ord r ) + => p -> Segment p -> ( r, p ) +closestPointOnSegment c ( Segment p0 p1 ) + | t <= 0 + = ( 0, p0 ) + | t >= 1 + = ( 1, p1 ) + | otherwise + = ( t, ( t *^ v01 ) • p0 ) + where + v01 :: v + v01 = p0 --> p1 + t :: r + t = projC ( p0 --> c ) v01 + +-------------------------------------------------------------------------------- + +-- | A convenient constraint synonym for types that support interpolation. +type Interpolatable :: Type -> Type -> Constraint +class ( Torsor ( T u ) u, Module r ( T u ) ) => Interpolatable r u +instance ( Torsor ( T u ) u, Module r ( T u ) ) => Interpolatable r u + +-------------------------------------------------------------------------------- + +instance Ring a => Inner a ( Sum a ) where + Sum a ^.^ Sum b = a Ring.* b + +instance Inner Double ( T ( ℝ 2 ) ) where + V2 x1 y1 ^.^ V2 x2 y2 = x1 Ring.* x2 + y1 Ring.* y2 + +instance Cross Double ( T ( ℝ 2 ) ) where + V2 x1 y1 × V2 x2 y2 = x1 Ring.* y2 Ring.- x2 Ring.* y1 + +-- | Compute whether two vectors point in the same direction, +-- that is, whether each vector is a (strictly) positive multiple of the other. +-- +-- Returns @False@ if either of the vectors is zero (or very close to zero). +strictlyParallel :: T ( ℝ 2 ) -> T ( ℝ 2 ) -> Bool +strictlyParallel u v + = abs ( u × v ) < tol -- vectors are collinear + && u ^.^ v > tol -- vectors point in the same direction (parallel and not anti-parallel) + where + tol = norm u * norm v * epsilon + +-- | Finds whether the query vector @ u @ is a convex combination of the two provided vectors @ v0 @, @ v1 @. +-- +-- If so, returns @ t @ in @ [ 0, 1 ] @ such that @ ( 1 - t ) v0 + t v1 @ is a positive multiple of @ u @. +convexCombination + :: T ( ℝ 2 ) -- ^ first vector + -> T ( ℝ 2 ) -- ^ second vector + -> T ( ℝ 2 ) -- ^ query vector + -> Maybe Double +convexCombination v0 v1 u + | abs c10 < epsilon + = if strictlyParallel u v0 + then Just 0 + else if strictlyParallel u v1 + then Just 1 + else Nothing + | otherwise + = do + let + t :: Double + t = c0 / c10 + guard ( t > -epsilon && t < 1 + epsilon ) + guard ( epsilon < u ^.^ ( lerp @( T ( ℝ 2 ) ) t v0 v1 ) ) + Just $ min 1 ( max 0 t ) + + where + c0, c10 :: Double + c0 = v0 × u + c10 = ( v0 ^-^ v1 ) × u + +-------------------------------------------------------------------------------- +-- Not sure how to set things up to automate the following... + +instance Module Double ( T ( ℝ 0 ) ) where + origin = T $$( tabulateQ \ _ -> [|| unT $ origin ||] ) + T a ^+^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^+^ T $$( indexQ [|| b ||] i ) ||] ) + T a ^-^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^-^ T $$( indexQ [|| b ||] i ) ||] ) + k *^ T a = T $$( tabulateQ \ i -> [|| unT $ k *^ T $$( indexQ [|| a ||] i ) ||] ) + +instance Module Double ( T ( ℝ 1 ) ) where + origin = T $$( tabulateQ \ _ -> [|| unT $ origin ||] ) + T a ^+^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^+^ T $$( indexQ [|| b ||] i ) ||] ) + T a ^-^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^-^ T $$( indexQ [|| b ||] i ) ||] ) + k *^ T a = T $$( tabulateQ \ i -> [|| unT $ k *^ T $$( indexQ [|| a ||] i ) ||] ) + +instance Module Double ( T ( ℝ 2 ) ) where + origin = T $$( tabulateQ \ _ -> [|| unT $ origin ||] ) + T a ^+^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^+^ T $$( indexQ [|| b ||] i ) ||] ) + T a ^-^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^-^ T $$( indexQ [|| b ||] i ) ||] ) + k *^ T a = T $$( tabulateQ \ i -> [|| unT $ k *^ T $$( indexQ [|| a ||] i ) ||] ) + +instance Module Double ( T ( ℝ 3 ) ) where + origin = T $$( tabulateQ \ _ -> [|| unT $ origin ||] ) + T a ^+^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^+^ T $$( indexQ [|| b ||] i ) ||] ) + T a ^-^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^-^ T $$( indexQ [|| b ||] i ) ||] ) + k *^ T a = T $$( tabulateQ \ i -> [|| unT $ k *^ T $$( indexQ [|| a ||] i ) ||] ) + +instance Module Double ( T ( ℝ 4 ) ) where + origin = T $$( tabulateQ \ _ -> [|| unT $ origin ||] ) + T a ^+^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^+^ T $$( indexQ [|| b ||] i ) ||] ) + T a ^-^ T b = T $$( tabulateQ \ i -> [|| unT $ T $$( indexQ [|| a ||] i ) ^-^ T $$( indexQ [|| b ||] i ) ||] ) + k *^ T a = T $$( tabulateQ \ i -> [|| unT $ k *^ T $$( indexQ [|| a ||] i ) ||] ) + +deriving via ViaModule Double ( T ( ℝ n ) ) + instance Module Double ( T ( ℝ n ) ) => Semigroup ( T ( ℝ n ) ) +deriving via ViaModule Double ( T ( ℝ n ) ) + instance Module Double ( T ( ℝ n ) ) => Monoid ( T ( ℝ n ) ) +deriving via ViaModule Double ( T ( ℝ n ) ) + instance Module Double ( T ( ℝ n ) ) => Group ( T ( ℝ n ) ) + +deriving via ViaModule Double ( ℝ n ) + instance Module Double ( T ( ℝ n ) ) => Act ( T ( ℝ n ) ) ( ℝ n ) +deriving via ( ViaModule Double ( ℝ n ) ) + instance Module Double ( T ( ℝ n ) ) => Torsor ( T ( ℝ n ) ) ( ℝ n ) + +-------------------------------------------------------------------------------- + +newtype ViaModule r m = ViaModule { unViaModule :: m } + +instance Module r m => Semigroup ( ViaModule r m ) where + (<>) = coerce $ (^+^) @r @m +instance Module r m => Monoid ( ViaModule r m ) where + mempty = coerce $ origin @r @m +instance Module r m => Group ( ViaModule r m ) where + invert = coerce $ ( \ x -> (^-^) @r @m ( origin @r @m ) x ) + +instance ( Semigroup ( T m ), Module r ( T m ) ) => Act ( T m ) ( ViaModule r m ) where + g • ViaModule m = ViaModule ( unT $ g ^+^ T m ) +instance ( Group ( T m ), Module r ( T m ) ) => Torsor ( T m ) ( ViaModule r m ) where + ViaModule a --> ViaModule b = T ( unT $ T b ^-^ T a )
+ src/lib/Math/Module/Internal.hs view
@@ -0,0 +1,90 @@+{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Module.Internal where + +-- base +import Data.Coerce + ( coerce ) +import Data.Monoid + ( Sum(..) ) + +-- brush-strokes +import Math.Ring + ( Ring ) +import qualified Math.Ring as Ring +import Math.Linear + +-------------------------------------------------------------------------------- + +infixl 6 ^+^, ^-^ +infix 9 ^*, *^ + +class Ring r => Module r m | m -> r where + + {-# MINIMAL origin, (^+^), ( (^*) | (*^) ) #-} + + origin :: m + (^+^) :: m -> m -> m + (^-^) :: m -> m -> m + (*^) :: r -> m -> m + (^*) :: m -> r -> m + + (*^) = flip (^*) + (^*) = flip (*^) + m ^-^ n = m ^+^ Ring.fromInteger ( -1 :: Integer ) *^ n + +infixl 8 ^.^ + +class Module r m => Inner r m where + (^.^) :: m -> m -> r + +-------------------------------------------------------------------------------- + +instance Ring a => Module a ( Sum a ) where + + origin = Sum $ Ring.fromInteger ( 0 :: Integer ) + + (^+^) = coerce $ (Ring.+) @a + (^-^) = coerce $ (Ring.-) @a + + c *^ Sum x = Sum ( c Ring.* x ) + Sum x ^* c = Sum ( x Ring.* c ) + +deriving via Sum Double instance Module Double ( T Double ) + +{- +moduleViaRepresentable :: Q TH.Type -> Q TH.Type -> TH.DecsQ +moduleViaRepresentable r m = do + i <- TH.newName "i" + let tab b = ( ( TH.varE 'tabulateQ `TH.appTypeE` r ) `TH.appTypeE` m ) `TH.appE` TH.lamE [ TH.varP i ] b + idx a = ( ( TH.varE 'indexQ `TH.appTypeE` r ) `TH.appTypeE` m ) `TH.appE` TH.varE i + + boo1 <- tab ( TH.varE 'origin `TH.appTypeE` r `TH.appTypeE` m ) + + + let + + meths = + [ mkMeth 'origin $ pure boo1 -- $ tab ( TH.varE 'origin `TH.appTypeE` r `TH.appTypeE` m ) + --, mkMeth '(^+^) $ TH.varE '(^+^) `TH.appE` ( TH ) `TH.appE` () + ] + + i1 <- TH.instanceD ( pure [] ) ( TH.conT ''Module `TH.appT` r `TH.appT` m ) meths + return [i1] + + where + mkMeth nm body = TH.funD nm [ TH.clause [] ( TH.normalB body ) [] ] + + +moduleViaRepresentable :: ( ( i -> a ) -> TH.CodeQ b ) -> ( b -> i -> TH.CodeQ a ) -> Q TH.Type -> Q TH.Type -> TH.DecsQ +moduleViaRepresentable tab idx r m = do + [d| + instance Module $r $m where + origin = $( TH.unTypeCode $ tab \ _ -> origin ) + a ^+^ b = $( TH.unTypeCode $ tab \ i -> idx [|| a ||] i ^+^ idx [|| b ||] i ) + a ^-^ b = $( TH.unTypeCode $ tab \ i -> idx [|| a ||] i ^-^ idx [|| b ||] i ) + k *^ a = $( TH.unTypeCode $ tab \ i -> [|| {- k *^ -} $$( idx [|| a ||] i ) ||] ) + |] +-}
+ src/lib/Math/Monomial.hs view
@@ -0,0 +1,253 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE QuantifiedConstraints #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE ParallelListComp #-} +{-# LANGUAGE TemplateHaskell #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Monomial + ( Mon(..) + , MonomialBasis(..), MonomialBasisQ(..), Deg, Vars + , zeroMonomial, isZeroMonomial + , totalDegree, isLinear, linearMonomial + + , split, mons + + , multiSubsetSumFaà, multiSubsetsSum, multiSubsetSum + , partitionFaà, partitions + + , prodRuleQ + + ) where + +-- base +import Data.Foldable + ( toList ) +import Data.Kind + ( Type, Constraint ) +import GHC.Exts + ( proxy# ) +import GHC.TypeNats +-- ( KnownNat, Nat, natVal' ) +import Unsafe.Coerce + ( unsafeCoerce ) + +-- template-haskell +import Language.Haskell.TH + ( CodeQ ) + +-- brush-strokes +import Math.Linear + ( Vec(..), Fin(..) ) +import TH.Utils + ( foldQ ) + +-------------------------------------------------------------------------------- + +-- | @Mon k n@ is the set of monomials in @n@ variables of degree less than or equal to @k@. +type Mon :: Nat -> Nat -> Type +newtype Mon k n = Mon { monDegs :: Vec n Word } -- sum <= k + deriving stock ( Show, Eq, Ord ) + +type Deg :: ( Type -> Type ) -> Nat +type Vars :: ( Type -> Type ) -> Nat +type family Deg f +type family Vars f + +zeroMonomial :: forall k n. KnownNat n => Mon k n +zeroMonomial = Mon $ Vec $ replicate ( fromIntegral $ word @n ) 0 +{-# INLINE zeroMonomial #-} + +isZeroMonomial :: Mon k n -> Bool +isZeroMonomial ( Mon ( Vec pows ) ) = all ( == 0 ) pows +{-# INLINE isZeroMonomial #-} + +totalDegree :: Mon k n -> Word +totalDegree ( Mon ( Vec pows ) ) = sum pows + +linearMonomial :: forall k n. ( KnownNat n, 1 <= k ) => Fin n -> Mon k n +linearMonomial ( Fin i' ) = + Mon $ Vec $ replicate ( i - 1 ) 0 ++ [ 1 ] ++ replicate ( n - i ) 0 + where + n, i :: Int + n = fromIntegral $ word @n + i = fromIntegral i' +{-# INLINE linearMonomial #-} + +isLinear :: Mon k n -> Maybe ( Fin n ) +isLinear ( Mon ( Vec pows ) ) = fmap Fin $ go 1 pows + where + go :: Word -> [ Word ] -> Maybe Word + go _ [] = Nothing + go j ( i : is ) + | i == 0 + = go ( j + 1 ) is + | i == 1 && all ( == 0 ) is + = Just j + | otherwise + = Nothing + +-- | Co-multiplication of monomials. +split :: Mon k n -> [ ( Mon k n, Mon k n ) ] +split ( Mon ( Vec [] ) ) = [ ( Mon ( Vec [] ), Mon ( Vec [] ) ) ] +split ( Mon ( Vec ( d : ds ) ) ) = + [ ( Mon ( Vec ( i : as ) ), Mon ( Vec ( ( ( d - i ) : bs ) ) ) ) + | i <- [ 0 .. d ] + , ( Mon ( Vec as ), Mon ( Vec bs ) ) <- ( split ( Mon ( Vec ds ) ) ) + ] + +-- | All monomials of degree less than or equal to @k@ in @n@ variables, +-- in lexicographic order. +mons :: forall k n. ( KnownNat n ) => Word -> [ Mon k n ] +mons k = unsafeCoerce ( mons' k ( word @n ) ) + +mons' :: Word -> Word -> [ [ Word ] ] +mons' k _ | k < 0 = [] +mons' _ 0 = [ [] ] +mons' 0 n = [ replicate ( fromIntegral n ) 0 ] +mons' k n = [ i : is | i <- reverse [ 0 .. k ], is <- mons' ( k - i ) ( n - 1 ) ] + +subs :: Mon k n -> [ ( Mon k n, Word ) ] +subs ( Mon ( Vec []) ) = [ ( Mon ( Vec [] ), maxBound ) ] +subs ( Mon ( Vec ( i : is ) ) ) + = [ ( Mon ( Vec ( j : js ) ) + , if j == 0 then mult else min ( i `quot` j ) mult ) + | j <- [ 0 .. i ] + , ( Mon ( Vec js ), mult ) <- subs ( Mon ( Vec is ) ) + ] + +word :: forall n. KnownNat n => Word +word = fromIntegral $ natVal' @n proxy# + +-- | The factorial function \( n! = n \cdot (n-1) \cdot `ldots` \cdot 2 `cdot` 1 \). +factorial :: Word -> Word +factorial i = product [ 1 .. i ] + +vecFactorial :: Vec n Word -> Word +vecFactorial ( Vec [] ) = 1 +vecFactorial ( Vec ( i : is ) ) = factorial i * vecFactorial ( Vec is ) + +-------------------------------------------------------------------------------- +-- Computations for the chain rule R^1 -> R^n -> R^m + +-- | Faà di Bruno coefficient (naive implementation). +multiSubsetSumFaà :: Word -> Vec n [ ( Word, Word ) ] -> Word +multiSubsetSumFaà k multisubsets = + factorial k `div` + product [ factorial p * ( factorial i ) ^ p + | multisubset <- toList multisubsets + , ( i, p ) <- multisubset ] + +-- | Computes the multisubsets of the given set which have the specified sum +-- and number of elements. +multiSubsetSum :: Word -- ^ size of multisubset + -> Word -- ^ desired sum + -> [ Word ] -- ^ set to pick from + -> [ [ ( Word, Word ) ] ] +multiSubsetSum 0 0 _ = [ [] ] +multiSubsetSum 0 _ _ = [] +multiSubsetSum _ _ [] = [] +multiSubsetSum n s ( i : is ) = + [ if j == 0 then js else ( i, j ) : js + | j <- [ 0 .. n ] + , js <- multiSubsetSum ( n - j ) ( s - i * j ) is + ] + +-- | @multiSubsetsSum is s ns@ computes all collection of multisubsets of @is@, +-- with sizes specified by @ns@, such that the total sum is @s@. +multiSubsetsSum :: forall n + . [ Word ] -- ^ set to pick from + -> Word -- ^ desired total sum + -> Vec n Word -- ^ sizes of each multisets + -> [ Vec n [ ( Word, Word ) ] ] +multiSubsetsSum is = goMSS + where + goMSS :: forall i. Word -> Vec i Word -> [ Vec i [ ( Word, Word ) ] ] + goMSS 0 ( Vec [] ) = [ Vec [] ] + goMSS _ ( Vec [] ) = [ ] + goMSS s ( Vec ( n : ns ) ) = + [ Vec ( multi : rest ) + | s_i <- [ n * i_min .. s ] + , multi <- multiSubsetSum n s_i is + , Vec rest <- goMSS ( s - s_i ) ( Vec ns ) ] + i_min = case is of + [] -> 0 + _ -> max 0 $ minimum is + +-------------------------------------------------------------------------------- +-- Computations for the chain rule R^n -> R^1 -> R^1 + +partitionFaà :: Mon k n -> [ ( Mon k n, Word ) ] -> Word +partitionFaà ( Mon mon ) multiIndexes = + vecFactorial mon `div` + product [ factorial p * ( vecFactorial i ) ^ p + | ( Mon i, p ) <- toList multiIndexes ] + +-- | @partitions p mon@ computes all partitions of the monomial @mon@ into +-- @p@ (non-zero) parts, allowing repetition. +partitions :: forall k n + . Word -- ^ number of parts + -> Mon k n -- ^ monomial to sum to + -> [ [ ( Mon k n, Word ) ] ] +partitions n_parts0 mon0 = go n_parts0 Nothing mon0 + where + go :: Word -> Maybe ( Mon k n ) -> Mon k n -> [ [ ( Mon k n , Word ) ] ] + go 0 _ mon + | isZeroMonomial mon + = [ [] ] + | otherwise + = [] + go 1 mbMaxMon mon + | isZeroMonomial mon + || case mbMaxMon of { Just maxMon -> mon >= maxMon ; _ -> False } + = [] + | otherwise + = [ [ ( mon, 1 ) ] ] + go n_parts mbMaxMon mon + = [ ( part, l ) : parts + | ( part, l_max ) <- subs mon + , not ( isZeroMonomial part ) -- parts must be non-empty + -- use a total ordering on monomials to ensure uniqueness + , case mbMaxMon of + Just maxMon -> part < maxMon + Nothing -> True + , l <- [ 1 .. min n_parts l_max ] + , parts <- go ( n_parts - l ) ( Just part ) ( subPart l mon part ) + ] + +subPart :: Word -> Mon k n -> Mon k n -> Mon k n +subPart = unsafeCoerce subPart' + +subPart' :: Word -> [ Word ] -> [ Word ] -> [ Word ] +subPart' m = zipWith ( \ i j -> i - m * j ) + +-------------------------------------------------------------------------------- + +-- | @'MonomialBasis' f@ exhibits @f u@ as a free @r@-module with basis the +-- monomials in @Vars u@ variables, of degree up to (and including) @Deg u@. +type MonomialBasis :: ( Type -> Type ) -> Constraint +class MonomialBasis f where + monTabulate :: ( ( Mon ( Deg f ) ( Vars f ) ) -> u ) -> f u + monIndex :: f u -> Mon ( Deg f ) ( Vars f ) -> u + +-- | @'MonomialBasisQ' f@ exhibits @f u@ as a free @r@-module with basis the +-- monomials in @Vars u@ variables, of degree up to (and including) @Deg u@, +-- at compile-time (to be spliced in using TemplateHaskell). +type MonomialBasisQ :: ( Type -> Type ) -> Constraint +class MonomialBasisQ f where + monTabulateQ :: ( ( Mon ( Deg f ) ( Vars f ) ) -> CodeQ u ) -> CodeQ ( f u ) + monIndexQ :: CodeQ ( f u ) -> Mon ( Deg f ) ( Vars f ) -> CodeQ u + +-------------------------------------------------------------------------------- + +prodRuleQ :: forall f r. MonomialBasisQ f + => CodeQ r -> CodeQ ( r -> r -> r ) -> CodeQ ( r -> r -> r ) + -- Ring r constraint (circumvent TH constraint problem) + -> CodeQ ( f r ) -> CodeQ ( f r ) -> CodeQ ( f r ) +prodRuleQ zero plus times df1 df2 = + monTabulateQ @f \ mon -> + [|| $$( foldQ plus zero + [ [|| $$times $$( monIndexQ @f df1 m1 ) $$( monIndexQ @f df2 m2 ) ||] + | (m1, m2) <- split mon + ] ) + ||]
+ src/lib/Math/Orientation.hs view
@@ -0,0 +1,150 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Orientation + ( Orientation(..), reverseOrientation + , convexOrientation, splineOrientation, splineTangents + , between + ) + where + +-- base +import Control.Monad + ( guard ) +import Data.Fixed + ( mod' ) + +-- acts +import Data.Act + ( Torsor((-->)) ) + +-- containers +import Data.Sequence + ( Seq(..) ) + +-- generic-lens +import Data.Generics.Product.Typed + ( HasType(typed) ) +import Data.Generics.Internal.VL + ( view ) + +-- brush-strokes +import Math.Epsilon + ( nearZero ) +import Math.Module + ( (×) ) +import Math.Bezier.Spline + ( Spline(..), Curves(..), Curve(..), NextPoint(..) + , SplineType(..), KnownSplineType(..), SSplineType(..) + , ssplineType + ) +import Math.Linear + ( ℝ(..), T(..) ) + +-------------------------------------------------------------------------------- + +-- | An orientation in the plane: counter-clockwise or clockwise. +data Orientation = CCW | CW + deriving stock ( Show, Eq, Ord ) + +-- | Reverse an orientation, turning counter-clockwise into clockwise and vice-versa. +reverseOrientation :: Orientation -> Orientation +reverseOrientation CCW = CW +reverseOrientation CW = CCW + +-- | Compute an orientation from a sequence of tangent vectors (assumed to have monotone angle). +convexOrientation :: [ T ( ℝ 2 ) ] -> Orientation +convexOrientation ( v1 : v2 : vs ) + | nearZero crossProduct + = convexOrientation ( v2 : vs ) + | crossProduct < 0 + = CW + | otherwise + = CCW + where + crossProduct :: Double + crossProduct = v1 × v2 +convexOrientation _ = CCW -- default + +-- | Compute the orientation of a spline, assuming tangent vectors have a monotone angle. +splineOrientation + :: forall clo crvData ptData + . ( KnownSplineType clo, HasType ( ℝ 2 ) ptData ) + => Spline clo crvData ptData + -> Orientation +splineOrientation = convexOrientation . splineTangents + +-- | Compute the sequence of tangent vectors given by the control points of a Bézier spline. +splineTangents + :: forall clo crvData ptData + . ( KnownSplineType clo, HasType ( ℝ 2 ) ptData ) + => Spline clo crvData ptData + -> [ T ( ℝ 2 ) ] +splineTangents spline@( Spline { splineStart = sp0, splineCurves = curves } ) + | let + p0 :: ℝ 2 + p0 = view typed sp0 + = case ssplineType @clo of + SOpen + | OpenCurves { openCurves = cs } <- curves + -> go p0 cs + SClosed + | OpenCurves cs@( c :<| _ ) <- splineCurves $ adjustSplineType @Open spline + -> go p0 ( cs :|> c ) + _ -> [] + where + go :: ℝ 2 -> Seq ( Curve Open crvData ptData ) -> [ T ( ℝ 2 ) ] + go _ Empty = [] + go p ( crv :<| crvs ) = + case crv of + LineTo { curveEnd = NextPoint sq } + | let + q :: ℝ 2 + q = view typed sq + -> ( p --> q ) : go q crvs + Bezier2To { controlPoint = scp, curveEnd = NextPoint sq } + | let + cp, q :: ℝ 2 + cp = view typed scp + q = view typed sq + -> ( p --> cp ) : ( cp --> q ) : go q crvs + Bezier3To { controlPoint1 = scp1, controlPoint2 = scp2, curveEnd = NextPoint sq } + | let + cp1, cp2, q :: ℝ 2 + cp1 = view typed scp1 + cp2 = view typed scp2 + q = view typed sq + -> ( p --> cp1 ) : ( cp1 --> cp2 ) : ( cp2 --> q ) : go q crvs + +-- | Checks whether a 2D vector lies "in between" two other vectors according to a given orientation, +-- i.e. whether the angle of the query vector lies in between the angles of the start and end vectors. +-- +-- The input vectors are expected to be within π radians of each other (as we are dealing +-- with convex shapes). +-- +-- Returns the proportion of the angle the vector is in between, or @Nothing@ if the query vector +-- is not in between. +-- +-- >>> between CCW ( V2 1 0 ) ( V2 -1 1 ) ( V2 1 1 ) +-- Just 0.3333333333333333 +between + :: Orientation + -> T ( ℝ 2 ) -- ^ start vector + -> T ( ℝ 2 ) -- ^ end vector + -> T ( ℝ 2 ) -- ^ query vector: is it in between the start and end vectors w.r.t. the provided orientation? + -> Maybe Double +between CCW ( V2 x1 y1 ) ( V2 x2 y2 ) ( V2 a b ) = + let + τ, η, φ, θ :: Double + τ = 2 * pi + η = atan2 y1 x1 + φ = ( atan2 y2 x2 - η ) `mod'` τ + θ = ( atan2 b a - η ) `mod'` τ + + in do + guard ( φ < pi ) -- deals with possible floating-point inaccuracies, + -- which can cause the angles of the two reference vectors + -- to be flipped, e.g. arg(v2) = arg(v1) - 0.000000001 + guard ( θ <= φ ) + pure ( θ / φ ) +between CW v1 v2 u = ( 1 - ) <$> between CCW v2 v1 u
+ src/lib/Math/Ring.hs view
@@ -0,0 +1,185 @@+{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Ring + ( AbelianGroup(..), Signed(..), Ring(..), Field(..) + , Floating(..), Transcendental(..) + + , ViaPrelude(..), ViaAbelianGroup(..) + + , ifThenElse + ) + where + +-- base +import Prelude ( Num, Fractional ) +import Prelude hiding ( Num(..), Fractional(..), Floating(..) ) +import qualified Prelude +import Data.Coerce + ( coerce ) +import Data.Monoid + ( Ap(..) ) + +-- groups +import Data.Group + ( Group(invert) ) + +-------------------------------------------------------------------------------- + +infixl 6 +, - +infixl 7 *, / +infixr 8 ^ + +class AbelianGroup a where + {-# MINIMAL (+), fromInteger, ( (-) | negate ) #-} + + (+) :: a -> a -> a + (-) :: a -> a -> a + negate :: a -> a + fromInteger :: Integer -> a + + a - b = a + negate b + negate b = fromInteger 0 - b + +class AbelianGroup a => Signed a where + abs :: a -> a + signum :: a -> a + +class AbelianGroup a => Ring a where + (*) :: a -> a -> a + (^) :: a -> Word -> a + (^) = defaultPow + +class Ring a => Field a where + {-# MINIMAL fromRational, ( recip | (/) ) #-} + + fromRational :: Rational -> a + (/) :: a -> a -> a + recip :: a -> a + + recip x = fromInteger 1 / x + x / y = x * recip y + +class Field a => Floating a where + sqrt :: a -> a + +class Floating a => Transcendental a where + pi :: a + cos :: a -> a + sin :: a -> a + atan :: a -> a + +-------------------------------------------------------------------------------- + +ifThenElse :: Bool -> a -> a -> a +ifThenElse b x y = if b then x else y + +-------------------------------------------------------------------------------- +-- A default power function, taken from base (GHC.Real). +-- +-- DO NOT use with interval arithmetic! + +{-# RULES +"defaultPow 2" forall x. defaultPow x 2 = x*x +"defaultPow 3" forall x. defaultPow x 3 = x*x*x +"defaultPow 4" forall x. defaultPow x 4 = let u = x*x in u*u +"defaultPow 5" forall x. defaultPow x 5 = let u = x*x in u*u*x + #-} + +{-# INLINE [1] defaultPow #-} +defaultPow :: Ring a => a -> Word -> a +defaultPow x0 y0 + | y0 < 0 = errorWithoutStackTrace "Negative exponent" + | y0 == 0 = fromInteger 1 + | otherwise = powImpl x0 y0 + +powImpl :: Ring a => a -> Word -> a +-- powImpl : x0 ^ y0 = (x ^ y) +powImpl x y + | even y = powImpl (x * x) (y `quot` 2) + | y == 1 = x + | otherwise = powImplAcc (x * x) (y `quot` 2) x + +{-# INLINABLE powImplAcc #-} +powImplAcc :: Ring a => a -> Word -> a -> a +-- powImplAcc : x0 ^ y0 = (x ^ y) * z +powImplAcc x y z + | even y = powImplAcc (x * x) (y `quot` 2) z + | y == 1 = x * z + | otherwise = powImplAcc (x * x) (y `quot` 2) (x * z) + +-------------------------------------------------------------------------------- + +newtype ViaPrelude a = ViaPrelude { viaPrelude :: a } + +instance Num a => AbelianGroup ( ViaPrelude a ) where + (+) = coerce $ (Prelude.+) @a + (-) = coerce $ (Prelude.-) @a + fromInteger = coerce $ Prelude.fromInteger @a + +instance Num a => Signed ( ViaPrelude a ) where + abs = coerce $ Prelude.abs @a + signum = coerce $ Prelude.signum @a + +instance Num a => Ring ( ViaPrelude a ) where + (*) = coerce $ (Prelude.*) @a + (^) = coerce $ (Prelude.^) @a @Word + +instance Fractional a => Field ( ViaPrelude a ) where + fromRational = coerce $ Prelude.fromRational @a + recip = coerce $ Prelude.recip @a + (/) = coerce $ (Prelude./) @a + +instance Prelude.Floating a => Floating ( ViaPrelude a ) where + sqrt = coerce $ Prelude.sqrt @a + +instance Prelude.Floating a => Transcendental ( ViaPrelude a ) where + pi = coerce $ Prelude.pi @a + sin = coerce $ Prelude.sin @a + cos = coerce $ Prelude.cos @a + atan = coerce $ Prelude.atan @a + +-------------------------------------------------------------------------------- + +newtype ViaAbelianGroup a = ViaAbelianGroup { viaAbelianGroup :: a } + +instance AbelianGroup a => Semigroup ( ViaAbelianGroup a ) where + (<>) = coerce $ (+) @a +instance AbelianGroup a => Monoid ( ViaAbelianGroup a ) where + mempty = ViaAbelianGroup $ fromInteger 0 +instance AbelianGroup a => Group ( ViaAbelianGroup a ) where + invert = coerce $ negate @a + +-------------------------------------------------------------------------------- + +deriving via ViaPrelude Integer instance AbelianGroup Integer +deriving via ViaPrelude Integer instance Ring Integer +deriving via ViaPrelude Integer instance Signed Integer + +deriving via ViaPrelude Int instance AbelianGroup Int +deriving via ViaPrelude Int instance Ring Int +deriving via ViaPrelude Int instance Signed Int + +deriving via ViaPrelude Word instance AbelianGroup Word +deriving via ViaPrelude Word instance Ring Word + +deriving via ViaPrelude Float instance AbelianGroup Float +deriving via ViaPrelude Float instance Ring Float +deriving via ViaPrelude Float instance Signed Float +deriving via ViaPrelude Float instance Field Float +deriving via ViaPrelude Float instance Floating Float +deriving via ViaPrelude Float instance Transcendental Float + +deriving via ViaPrelude Double instance AbelianGroup Double +deriving via ViaPrelude Double instance Ring Double +deriving via ViaPrelude Double instance Signed Double +deriving via ViaPrelude Double instance Field Double +deriving via ViaPrelude Double instance Floating Double +deriving via ViaPrelude Double instance Transcendental Double + +-------------------------------------------------------------------------------- + +instance ( AbelianGroup r, Applicative f ) => AbelianGroup ( Ap f r ) where + (+) = liftA2 $ (+) @r + (-) = liftA2 $ (-) @r + negate = fmap $ negate @r + fromInteger = pure . fromInteger @r
+ src/lib/Math/Root/Isolation.hs view
@@ -0,0 +1,282 @@+{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Root.Isolation + ( + -- * Main entry point: execute root isolation strategies + isolateRootsIn + , Box + , DoneBoxes(..) + + -- * General API for root isolation algorithms + , BoxCt, BoxHistory + , RootIsolationAlgorithm(..) + , RootIsolationAlgorithmWithOptions(..) + + -- ** Inspecting history + , RootIsolationStep(..) + + -- ** Visualising history + , RootIsolationTree(..) + , showRootIsolationTree + + -- * Configuration of the root isolation methods + , RootIsolationOptions(..), defaultRootIsolationOptions + , defaultRootIsolationAlgorithms + + -- * The bisection method + , Bisection + , BisectionOptions(..), BisectionCoordPicker + , defaultBisectionOptions + + -- * The interval Newton method + , NewtonOptions(..) + , defaultNewtonOptions + -- ** Options for the Gauss–Seidel step + , GaussSeidelOptions(..), Preconditioner(..) + , GaussSeidelUpdateMethod(..) + + -- * Box-consistency methods + + -- ** The @box(1)@-consistency algorithm + , Box1 + , Box1Options(..) + , defaultBox1Options + + -- *** Narrowing operators for the @box(1)@-consistency algorithm + , NarrowingMethod(..) + , narrowingMethods + , AdaptiveShavingOptions(..) + , defaultAdaptiveShavingOptions + + -- ** The @box(2)@-consistency algorithm + , Box2 + , Box2Options(..) + , defaultBox2Options + ) + where + +-- base +import Data.Kind + ( Type ) +import Data.List.NonEmpty + ( NonEmpty ) +import qualified Data.List.NonEmpty as NE + ( NonEmpty(..), last, singleton ) +import GHC.TypeNats + ( Nat ) + +-- parallel +--import Control.Parallel.Strategies +-- ( Strategy, using, parTraversable, rpar ) + +-- transformers +import Control.Monad.Trans.Writer.CPS + ( Writer, runWriter, tell ) + +-- brush-strokes +import Math.Algebra.Dual + ( D ) +import Math.Interval +import Math.Linear +import Math.Module + ( Module(..) ) +import Math.Monomial + ( MonomialBasis(..), zeroMonomial ) + +import Math.Root.Isolation.Bisection +import Math.Root.Isolation.Core +import Math.Root.Isolation.Narrowing +import Math.Root.Isolation.Newton +import Math.Root.Isolation.Newton.GaussSeidel + +-------------------------------------------------------------------------------- + +-- | Options for the root isolation methods in 'isolateRootsIn'. +type RootIsolationOptions :: Nat -> Nat -> Type +newtype RootIsolationOptions n d + = RootIsolationOptions + { -- | Given a box and its history, return a round of root isolation strategies + -- to run, in sequence, on this box. + -- + -- A return value of @Left whyStop@ indicates stopping any further iterations + -- on this box, e.g. because it is too small. + rootIsolationAlgorithms + :: BoxHistory n + -> Box n + -> Either String ( NE.NonEmpty ( RootIsolationAlgorithmWithOptions n d ) ) + } + +defaultRootIsolationOptions :: forall n d. BoxCt n d => RootIsolationOptions n d +defaultRootIsolationOptions = + RootIsolationOptions + { rootIsolationAlgorithms = \ hist -> + defaultRootIsolationAlgorithms minWidth ε_eq + ( defaultNewtonOptions @n @d hist ) hist + } + where + minWidth = 1e-7 + ε_eq = 5e-3 +{-# INLINEABLE defaultRootIsolationOptions #-} + +defaultRootIsolationAlgorithms + :: forall n d + . BoxCt n d + => Double -- ^ minimum width of boxes (don't bisect further) + -> Double -- ^ threshold for progress + -> NewtonOptions n d -- ^ options for Newton's method + -> BoxHistory n + -> Box n + -> Either String ( NE.NonEmpty ( RootIsolationAlgorithmWithOptions n d ) ) +defaultRootIsolationAlgorithms minWidth ε_eq newtonOptions history box = + case history of + lastRoundBoxes : _ + -- If, in the last round of strategies, we didn't try bisection... + | all ( \case { IsolationStep @Bisection _ -> False; _ -> True } . fst ) lastRoundBoxes + , ( _step, lastRoundFirstBox ) <- NE.last lastRoundBoxes + -- ...and the last round didn't sufficiently reduce the size of the box... + , not $ box `sufficientlySmallerThan` lastRoundFirstBox + -- ...then try bisecting the box... + -- ...unless the box is already too small, in which case we give up. + -> if verySmall + then Left $ "widths <= " ++ show minWidth + else Right $ NE.singleton $ AlgoWithOptions @Bisection _bisOptions + -- Otherwise, do a normal round. + -- Currently: we try an interval Gauss–Seidel. + -- (box(1)- and box(2)-consistency don't seem to help when using + -- the complete interval union Gauss–Seidel step) + _ -> Right $ AlgoWithOptions @Newton newtonOptions + NE.:| [] + + where + verySmall = and $ ( \ cd -> width cd <= minWidth ) <$> coordinates box + + _bisOptions = defaultBisectionOptions @n @d minWidth ε_eq box + _box1Options = defaultBox1Options @n @d ( minWidth * 100 ) ε_eq + _box2Options = ( defaultBox2Options @n @d minWidth ε_eq ) { box2LambdaMin = 0.001 } + + -- Did we reduce the box width by at least ε_eq + -- in at least one of the coordinates? + sufficientlySmallerThan :: Box n -> Box n -> Bool + b1 `sufficientlySmallerThan` b2 = + or $ + ( \ cd1 cd2 -> width cd2 - width cd1 >= ε_eq ) + <$> coordinates b1 + <*> coordinates b2 +{-# INLINEABLE defaultRootIsolationAlgorithms #-} + +-------------------------------------------------------------------------------- +-- Main driver + +-- | Use various methods using interval arithmetic in order to solve a system of +-- non-linear equations. +-- +-- Currently supported algorithms: +-- +-- - interval Newton method with Gauss–Seidel step for inversion +-- of the interval Jacobian, +-- - coordinate-wise Newton method (@box(1)@-consistency algorithm), +-- - @box(2)@-consistency algorithm, +-- - bisection. +-- +-- Returns @(tree, DoneBoxes sols dunno)@ where @tree@ is the full tree +-- (useful for debugging), sols are boxes that contain a unique solution +-- (and to which Newton's method will converge starting from anywhere inside +-- the box), and @dunno@ are boxes that we have given up processing (usually +-- because they are too small); they may or may not enclose solutions. +isolateRootsIn + :: forall n d + . BoxCt n d + => RootIsolationOptions n d + -- ^ configuration (which algorithms to use, and with what parameters) + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -- ^ equations to solve + -> Box n + -- ^ initial search domain + -> ( [ ( Box n, RootIsolationTree ( Box n ) ) ], DoneBoxes n ) +isolateRootsIn ( RootIsolationOptions { rootIsolationAlgorithms } ) + eqs initBox = runWriter $ map ( initBox , ) <$> go [] initBox + + where + + go :: BoxHistory n + -> Box n -- box to work on + -> Writer ( DoneBoxes n ) + [ RootIsolationTree ( Box n ) ] + go history cand + | -- Check the range of the equations contains zero. + not $ unT ( origin :: T ( ℝ d ) ) ∈∈ iRange + -- Box doesn't contain a solution: discard it. + = return [ RootIsolationLeaf "rangeTest" cand ] + | otherwise + = case rootIsolationAlgorithms history cand of + Right strats -> doStrategies history strats cand + Left whyStop -> do + -- We are giving up on this box (e.g. because it is too small). + tell $ noDoneBoxes { doneGiveUpBoxes = [ ( cand, whyStop ) ] } + return [ RootIsolationLeaf whyStop cand ] + where + iRange :: Box d + iRange = eqs cand `monIndex` zeroMonomial + + -- Run a round of root isolation strategies, then recur. + doStrategies + :: BoxHistory n + -> NonEmpty ( RootIsolationAlgorithmWithOptions n d ) + -> Box n + -> Writer ( DoneBoxes n ) + [ RootIsolationTree ( Box n ) ] + doStrategies prevRoundsHist = do_strats [] + where + do_strats :: [ ( RootIsolationStep, Box n ) ] + -> NE.NonEmpty ( RootIsolationAlgorithmWithOptions n d ) + -> Box n + -> Writer ( DoneBoxes n ) + [ RootIsolationTree ( Box n )] + do_strats thisRoundHist ( algo NE.:| algos ) box = do + -- Run one strategy in the round. + ( step, boxes ) <- doStrategy algo thisRoundHist prevRoundsHist eqs box + case algos of + -- If there are other algorithms to run in this round, run them next. + nextAlgo : otherAlgos -> + let thisRoundHist' = ( step, box ) : thisRoundHist + in recur step ( do_strats thisRoundHist' ( nextAlgo NE.:| otherAlgos ) ) boxes + -- Otherwise, we have done one full round of strategies. + -- Recur back to the top (calling 'go'). + [] -> + let thisRoundHist' = ( step, box ) NE.:| thisRoundHist + history = thisRoundHist' : prevRoundsHist + in recur step ( go history ) boxes + + recur :: RootIsolationStep + -> ( Box n -> Writer ( DoneBoxes n ) [ RootIsolationTree ( Box n ) ] ) + -> [ Box n ] + -> Writer ( DoneBoxes n ) [ RootIsolationTree ( Box n ) ] + recur step r cands = do + let rest :: [ ( [ ( Box n, RootIsolationTree ( Box n ) ) ], DoneBoxes n ) ] + rest = + map ( \ c -> runWriter $ do { trees <- r c; return [ (c, tree) | tree <- trees ] } ) cands + ( rest2, dones ) = unzip rest --( rest `using` myParTraversable rpar ) + tell ( mconcat dones ) + return [ RootIsolationStep step $ concat rest2 ] +{-# INLINEABLE isolateRootsIn #-} + +--myParTraversable :: Strategy a -> Strategy [a] +--myParTraversable _ [] = return [] +--myParTraversable strat (a:as) = (a:) <$> parTraversable strat as + +-- | Execute a root isolation strategy, replacing the input box with +-- (hopefully smaller) output boxes. +doStrategy + :: forall n d + . RootIsolationAlgorithmWithOptions n d + -> [ ( RootIsolationStep, Box n ) ] + -> BoxHistory n + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -> Box n + -> Writer ( DoneBoxes n ) + ( RootIsolationStep, [ Box n ] ) +doStrategy algo thisRoundHist prevRoundsHist eqs cand = + case algo of + AlgoWithOptions @algo options -> do + ( step, res ) <- rootIsolationAlgorithm @algo options thisRoundHist prevRoundsHist eqs cand + return ( SomeRootIsolationStep @algo step, res )
+ src/lib/Math/Root/Isolation/Bisection.hs view
@@ -0,0 +1,272 @@+ +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Root.Isolation.Bisection + ( -- * The bisection method for root isolation + Bisection + , bisection + + -- ** Configuration options + , BisectionCoordPicker + , BisectionOptions(..), defaultBisectionOptions + + -- *** Helper code for picking which dimension to bisect + , CoordBisectionData(..) + , spread, normVI, maxVI + , sortOnArgNE + ) + where + +-- base +import Data.Kind + ( Type ) +import Data.Foldable + ( toList ) +import Data.Functor + ( (<&>) ) +import qualified Data.List.NonEmpty as NE + ( NonEmpty(..), cons, filter + , head, nonEmpty, singleton, sort + ) +import Data.Semigroup + ( Arg(..), Dual(..) ) +import GHC.TypeNats + ( Nat, KnownNat + , type (<=) + ) + +-- brush-strokes +import Math.Algebra.Dual + ( D ) +import Math.Interval +import Math.Linear +import Math.Module + ( Module(..) ) +import Math.Monomial + ( MonomialBasis(..), linearMonomial, zeroMonomial ) +import qualified Math.Ring as Ring +import Math.Root.Isolation.Core + +-------------------------------------------------------------------------------- +-- Bisection + +-- | The bisection algorithm; see 'bisection'. +data Bisection +instance BoxCt n d => RootIsolationAlgorithm Bisection n d where + type instance StepDescription Bisection = ( String, Double ) + type instance RootIsolationAlgorithmOptions Bisection n d = BisectionOptions n d + rootIsolationAlgorithm + ( BisectionOptions { canHaveSols, fallbackBisectionCoord } ) + thisRoundHist prevRoundsHist eqs box = do + let ( boxes, whatBis ) = + bisection + ( canHaveSols eqs ) + ( fallbackBisectionCoord thisRoundHist prevRoundsHist eqs ) + box + return ( whatBis, boxes ) + {-# INLINEABLE rootIsolationAlgorithm #-} + {-# SPECIALISE rootIsolationAlgorithm @2 @3 #-} + -- NB: including this to be safe. The specialiser seems to sometimes + -- be able to generate this specialisation on its own, and sometimes not. + +-- | Options for the bisection method. +type BisectionOptions :: Nat -> Nat -> Type +data BisectionOptions n d = + BisectionOptions + { -- | Custom function to check whether the given box might contain solutions to the + -- given equations. + -- + -- If you always return @True@, then we will always bisect along + -- the dimension picked by the 'fallbackBisectionCoord' function. + -- + -- NB: only return 'False' if non-existence of solutions is guaranteed + -- (otherwise, the root isolation algorithm might not be consistent). + canHaveSols :: !( ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) -> Box n -> Bool ) + -- | Heuristic to choose which coordinate dimension to bisect. + -- + -- It's only a fallback, as we prefer to bisect along coordinate dimensions + -- that minimise the number of sub-boxes created. + , fallbackBisectionCoord :: !( BisectionCoordPicker n d ) + } + +-- | A function to choose along which coordination dimension we should bisect. +type BisectionCoordPicker n d + = [ ( RootIsolationStep, Box n ) ] + -> BoxHistory n + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -> forall r. Show r => ( NE.NonEmpty ( Fin n, r ) -> ( r, String ) ) + +-- | Default options for the bisection method. +defaultBisectionOptions + :: forall n d + . ( 1 <= n, BoxCt n d, Show ( 𝕀ℝ d ) ) + => Double -> Double + -> Box n -> BisectionOptions n d +defaultBisectionOptions minWidth _ε_eq box = + BisectionOptions + { canHaveSols = + \ eqs box' -> + -- box(0)-consistency + let iRange' :: Box d + iRange' = eqs box' `monIndex` zeroMonomial + in all ( unT ( origin @Double ) ∈ ) ( coordinates iRange' ) + + -- box(1)-consistency + --let box1Options = Box1Options _ε_eq ( toList $ universe @n ) ( toList $ universe @d ) + --in not $ null $ makeBox1Consistent _minWidth box1Options eqs box' + + -- box(2)-consistency + --let box2Options = Box2Options _ε_eq 0.001 ( toList $ universe @n ) ( toList $ universe @d ) + -- box'' = makeBox2Consistent _minWidth box2Options eqs box' + -- iRange'' :: Box d + -- iRange'' = eqs box'' `monIndex` zeroMonomial + --in unT ( origin @Double ) ∈ iRange'' + , fallbackBisectionCoord = + \ _thisRoundHist _prevRoundsHist eqs possibleCoordChoices -> + let datPerCoord = + possibleCoordChoices <&> \ ( i, r ) -> + CoordBisectionData + { coordIndex = i + , coordInterval = box `index` i + , coordJacobianColumn = eqs box `monIndex` ( linearMonomial i ) + , coordBisectionData = r + } + + -- First, check if the largest dimension is over 20 times larger + -- than the smallest dimension; if so bisect that dimension. + in case sortOnArgNE ( width . coordInterval ) datPerCoord of + Arg _ d NE.:| [] -> + ( coordBisectionData d, "" ) + ( Arg w0 _ NE.:| ds ) -> + let Arg w1 d1 = last ds + in if w1 >= 20 * w0 + then ( coordBisectionData d1, "tooWide" ) + -- Otherwise, pick the coordination dimension with maximum spread + -- (spread = width * Jacobian column norm). + else + let isTooSmall ( Arg ( Dual w ) _ ) = w < minWidth + in case NE.filter ( not . isTooSmall ) $ sortOnArgNE ( Dual . spread ) datPerCoord of + [] -> ( coordBisectionData d1, "tooWide'" ) + Arg _ d : _ -> ( coordBisectionData d, "spread" ) + -- TODO: pick a dimension that previous Newton steps did not + -- manage to narrow well? + } +{-# INLINEABLE defaultBisectionOptions #-} + +sortOnArgNE :: Ord b => ( a -> b ) -> NE.NonEmpty a -> NE.NonEmpty ( Arg b a ) +sortOnArgNE f = NE.sort . fmap ( \ a -> Arg ( f a ) a ) +{-# INLINEABLE sortOnArgNE #-} + +-- | A utility datatype that is useful in implementing bisection dimension picker +-- functions ('fallbackBisectionCoord'). +type CoordBisectionData :: Nat -> Nat -> Type -> Type +data CoordBisectionData n d r = + CoordBisectionData + { coordIndex :: !( Fin n ) + , coordInterval :: !𝕀 + , coordJacobianColumn :: !( 𝕀ℝ d ) + , coordBisectionData :: !r + } +deriving stock instance ( Show ( 𝕀ℝ d ), Show r ) + => Show ( CoordBisectionData n d r ) + +spread :: ( BoxCt n d, Representable 𝕀 ( 𝕀ℝ d ) ) + => CoordBisectionData n d r -> Double +spread ( CoordBisectionData { coordInterval = cd, coordJacobianColumn = j_cd } ) + = width cd * normVI j_cd +{-# INLINEABLE spread #-} + +normVI :: ( Applicative ( Vec d ), Representable 𝕀 ( 𝕀ℝ d ) ) => 𝕀ℝ d -> Double +normVI box = + sqrt $ sum ( nm1 <$> coordinates box ) + where + nm1 :: 𝕀 -> Double + nm1 ( 𝕀 lo hi ) = max ( abs lo ) ( abs hi ) Ring.^ 2 +{-# INLINEABLE normVI #-} + +maxVI :: ( Applicative ( Vec d ), Representable 𝕀 ( 𝕀ℝ d ) ) => 𝕀ℝ d -> Double +maxVI box = + maximum ( maxAbs <$> coordinates box ) + where + maxAbs :: 𝕀 -> Double + maxAbs ( 𝕀 lo hi ) = max ( abs lo ) ( abs hi ) +{-# INLINEABLE maxVI #-} + +-------------------------------------------------------------------------------- + +-- | Bisect the given box. +-- +-- (The difficult part lies in determining along which coordinate +-- dimension to bisect.) +bisection + :: forall n + . ( 1 <= n, KnownNat n, Show ( 𝕀ℝ n ), Representable 𝕀 ( 𝕀ℝ n ) ) + => ( Box n -> Bool ) + -- ^ how to check whether a box contains solutions + -> ( forall r. Show r => NE.NonEmpty ( Fin n, r ) -> ( r, String ) ) + -- ^ heuristic bisection coordinate picker + -> Box n + -- ^ the box to bisect + -> ( [ Box n ], ( String, Double ) ) +bisection canHaveSols pickFallbackBisCoord box = + case NE.nonEmpty solsList of + Nothing -> + -- We discarded dimensions along which bisection was useless + -- (because the interval was canonical in that dimension). + -- If there are no dimensions left, then don't do any bisection. + -- (TODO: we shouldn't really ever get here.) + ( [ box ], ( "noBis", 0 / 0 ) ) + Just solsNE -> + case findFewestSols solsNE of + -- If there is a coordinate for which bisection results in no solutions, + -- or in fewer sub-boxes with solutions than any other coordinate choice, + -- pick that coordinate for bisection. + Arg nbSols ( ( i, ( mid, subBoxesWithSols ) ) NE.:| [] ) -> + ( subBoxesWithSols, ( "cd = " ++ show i ++ "(#subs=" ++ show nbSols ++ ")", mid ) ) + -- Otherwise, fall back to the provided heuristic. + Arg _nbSols is -> + let ( ( mid, subBoxesWithSols ), why ) = pickFallbackBisCoord is + in ( subBoxesWithSols, ( why, mid ) ) + where + solsList = + [ Arg ( fromIntegral $ length subBoxesWithSols ) ( i, ( mid, subBoxesWithSols ) ) + | i <- toList $ universe @n + , let ( mid, subBoxes ) = bisectInCoord box i + subBoxesWithSols = NE.filter canHaveSols subBoxes + -- discard coordinate dimensions in which the box is a singleton + , length subBoxes >= 2 || null subBoxesWithSols + ] +{-# INLINEABLE bisection #-} + +-- | Bisect a box in the given coordinate dimension. +bisectInCoord + :: Representable 𝕀 ( 𝕀ℝ n ) + => Box n -> Fin n -> ( Double, NE.NonEmpty ( Box n ) ) +bisectInCoord box i = + let z = box `index` i + zs' = bisect z + in ( sup ( NE.head zs' ) + , fmap ( \ z' -> set i z' box ) zs' ) +{-# INLINEABLE bisectInCoord #-} + +-- | Return the elements with the least argument. +-- +-- NB: this function shortcuts as soon as it finds an element with argument 0. +findFewestSols :: forall a. NE.NonEmpty ( Arg Word a ) -> Arg Word ( NE.NonEmpty a ) +findFewestSols ( Arg nbSols arg NE.:| args ) + | nbSols == 0 + = Arg 0 $ NE.singleton arg + | otherwise + = go nbSols ( NE.singleton arg ) args + where + go :: Word -> NE.NonEmpty a -> [ Arg Word a ] -> Arg Word ( NE.NonEmpty a ) + go bestNbSolsSoFar bestSoFar [] = Arg bestNbSolsSoFar bestSoFar + go bestNbSolsSoFar bestSoFar ( ( Arg nbSols' arg' ) : args' ) + | nbSols' == 0 + = Arg 0 $ NE.singleton arg' + | otherwise + = case compare nbSols' bestNbSolsSoFar of + LT -> go nbSols' ( NE.singleton arg' ) args' + GT -> go bestNbSolsSoFar bestSoFar args' + EQ -> go bestNbSolsSoFar ( arg' `NE.cons` bestSoFar ) args'
+ src/lib/Math/Root/Isolation/Core.hs view
@@ -0,0 +1,309 @@+ +{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} +{-# LANGUAGE UndecidableSuperClasses #-} + +-- | Core definitions and utilities common to root isolation methods. +module Math.Root.Isolation.Core + ( -- * Root isolation types + Box + , DoneBoxes(..), noDoneBoxes + + -- * General typeclass for root isolation methods + , BoxCt + , RootIsolationAlgorithm(..) + , RootIsolationAlgorithmWithOptions(..) + + -- ** Inspecting history + , RootIsolationStep(IsolationStep, ..) + , BoxHistory + + -- ** Visualising history + , RootIsolationTree(..) + , boxArea + , showRootIsolationTree + + -- * Utility functions + , pipeFunctionsWhileTrue + , forEachCoord + + -- * Timing + , TimeInterval(..), timeInterval + ) where + +-- base +import Data.Foldable + ( toList ) +import Data.Kind + ( Type, Constraint ) +import qualified Data.List.NonEmpty as NE + ( NonEmpty ) +import Data.Type.Equality + ( (:~~:)(HRefl) ) +import Data.Typeable + ( Typeable, heqT ) +import Numeric + ( showFFloat ) +import GHC.Clock + ( getMonotonicTime ) +import GHC.TypeNats + ( Nat, KnownNat, type (<=) ) +import System.IO.Unsafe + ( unsafePerformIO ) + +-- containers +import Data.Tree + ( Tree(..) ) + +-- deepseq +import Control.DeepSeq + ( NFData(..), deepseq ) + +-- transformers +import Control.Monad.Trans.State.Strict as State + ( State, get, put ) +import Control.Monad.Trans.Writer.CPS + ( Writer ) + +-- tree-view +import Data.Tree.View + ( showTree ) + +-- brush-strokes +import Math.Algebra.Dual + ( D ) +import Math.Interval +import Math.Linear +import Math.Module + ( Module(..) ) +import Math.Monomial + ( MonomialBasis(..), Deg, Vars ) + +-------------------------------------------------------------------------------- + +-- | An axis-aligned box in @n@-dimensions. +type Box n = 𝕀ℝ n + +-- | Dimension constraints for root isolation in a system of equations: +-- +-- - @n@: number of variables +-- - @d@: number of equations +-- +-- NB: we require n <= d (no support for under-constrained systems). +-- +-- NB: in practice, this constraint should specialise away. +type BoxCt n d = + ( KnownNat n, KnownNat d + , 1 <= n, 1 <= d, n <= d + + , Show ( 𝕀ℝ n ), Show ( ℝ n ) + , Show ( 𝕀ℝ d ), Show ( ℝ d ) + , Eq ( ℝ n ) + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , MonomialBasis ( D 1 n ) + , Deg ( D 1 n ) ~ 1 + , Vars ( D 1 n ) ~ n + , Module Double ( T ( ℝ n ) ) + , Module 𝕀 ( T ( 𝕀ℝ n ) ) + , NFData ( ℝ n ) + , NFData ( 𝕀ℝ n ) + + , Ord ( ℝ d ) + , Module Double ( T ( ℝ d ) ) + , Representable Double ( ℝ d ) + , Representable 𝕀 ( 𝕀ℝ d ) + , NFData ( ℝ d ) + , NFData ( 𝕀ℝ d ) + ) +-- | Boxes we are done with and will not continue processing. +data DoneBoxes n = + DoneBoxes + { -- | Boxes which definitely contain a unique solution. + doneSolBoxes :: ![ Box n ] + -- | Boxes which may or may not contain solutions, + -- and that we have stopped processing for some reason. + , doneGiveUpBoxes :: ![ ( Box n, String ) ] + } +deriving stock instance Show ( Box n ) => Show ( DoneBoxes n ) + +instance Semigroup ( DoneBoxes n ) where + DoneBoxes a1 b1 <> DoneBoxes a2 b2 = DoneBoxes ( a1 <> a2 ) ( b1 <> b2 ) +instance Monoid ( DoneBoxes n ) where + mempty = noDoneBoxes +noDoneBoxes :: DoneBoxes n +noDoneBoxes = DoneBoxes [] [] + +-------------------------------------------------------------------------------- +-- Class for all root isolation algorithms. +-- +-- This keeps the implementation open-ended, and allows inspection of +-- other root isolation methods, so that heuristics can look at +-- what happened in previous steps to decide what to do. + +-- | Existential wrapper over any root isolation algorithm, +-- with the options necessary to run it. +type RootIsolationAlgorithmWithOptions :: Nat -> Nat -> Type +data RootIsolationAlgorithmWithOptions n d where + AlgoWithOptions + :: forall {k :: Type} {n :: Nat} {d :: Nat} ( ty :: k ) + . RootIsolationAlgorithm ty n d + => RootIsolationAlgorithmOptions ty n d + -> RootIsolationAlgorithmWithOptions n d + +-- | Type-class for root isolation algorithms. +-- +-- This design keeps the set of root isolation algorithms open-ended, +-- while retaining the ability to inspect previous steps (using the +-- 'IsolationStep' pattern). +type RootIsolationAlgorithm :: forall {k :: Type}. k -> Nat -> Nat -> Constraint +class ( Typeable ty, Show ( StepDescription ty ), BoxCt n d ) + => RootIsolationAlgorithm ty n d where + -- | The type of additional information about an algorithm step. + -- + -- Only really useful for debugging; gets stored in 'RootIsolationTree's. + type StepDescription ty + -- | Configuration options expected by this root isolation method. + type RootIsolationAlgorithmOptions ty n d = r | r -> ty n d + -- | Run one step of the root isolation method. + -- + -- This gets given the equations and a box, and should attempt to + -- shrink the box in some way, returning smaller boxes. + -- + -- Should return: + -- + -- - a description of the step taken (see 'StepDescription'), + -- - new boxes to process (the return value of type @['Box' n]@), + -- which can be empty if the algorithm can prove that the input + -- bix does not contain any solutions; + -- - (as a writer side-effect) boxes to definitely stop processing; see 'DoneBoxes'. + rootIsolationAlgorithm + :: RootIsolationAlgorithmOptions ty n d + -- ^ options for this root isolation algorithm + -> [ ( RootIsolationStep, Box n ) ] + -- ^ history of the current round + -> BoxHistory n + -- ^ previous rounds history + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -- ^ equations + -> Box n + -- ^ box + -> Writer ( DoneBoxes n ) ( StepDescription ty, [ Box n ] ) + +-- | Match on an unknown root isolation algorithm step with a known algorithm. +pattern IsolationStep + :: forall ( ty :: Type ) + . Typeable ty + => StepDescription ty + -> RootIsolationStep +pattern IsolationStep stepDescr + <- ( rootIsolationAlgorithmStep_maybe @ty -> Just stepDescr ) + -- NB: this pattern could also return @RootIsolationAlgorithm n d ty@ evidence, + -- but it's simpler to not do so for now. + +-- | Helper function used to define the 'IsolationStep' pattern. +-- +-- Inspects whether an existential 'RootIsolationStep' packs a step for +-- the given algorithm. +rootIsolationAlgorithmStep_maybe + :: forall ty + . Typeable ty + => RootIsolationStep -> Maybe ( StepDescription ty ) +rootIsolationAlgorithmStep_maybe ( SomeRootIsolationStep @existential descr ) + | Just HRefl <- heqT @existential @ty + = Just descr + | otherwise + = Nothing +{-# INLINEABLE rootIsolationAlgorithmStep_maybe #-} + +-- | History for a given box: what was the outcome of previous root isolation +-- methods? +type BoxHistory n = [ NE.NonEmpty ( RootIsolationStep, Box n ) ] + +-- | A description of a step taken when isolating roots. +data RootIsolationStep where + SomeRootIsolationStep + :: forall step + . ( Typeable step + , Show ( StepDescription step ) + ) + => StepDescription step + -> RootIsolationStep + +instance Show RootIsolationStep where + showsPrec p ( SomeRootIsolationStep stepDescr ) = showsPrec p stepDescr + +-------------------------------------------------------------------------------- +-- Trees recording steps taken by the algorithm, for visualisation & debugging. + +-- | A tree recording the steps taken when isolating roots. +data RootIsolationTree d + = RootIsolationLeaf String d + | RootIsolationStep RootIsolationStep [ ( d, RootIsolationTree d ) ] + +instance {-# OVERLAPPING #-} ( Representable 𝕀 ( 𝕀ℝ n ), Show ( Box n ) ) => Show ( Box n, RootIsolationTree ( Box n ) ) where + show ( cand, tree ) = showTree $ showRootIsolationTree cand tree + +showRootIsolationTree + :: ( Representable 𝕀 ( 𝕀ℝ n ), Show ( Box n ) ) + => Box n -> RootIsolationTree ( Box n ) -> Tree String +showRootIsolationTree cand (RootIsolationLeaf why l) = Node (show cand ++ " " ++ showArea (boxArea cand) ++ " " ++ why ++ " " ++ show l) [] +showRootIsolationTree cand (RootIsolationStep s ts) + = Node (show cand ++ " abc " ++ showArea (boxArea cand) ++ " " ++ show s) $ map (\ (c,t) -> showRootIsolationTree c t) ts + +{-# INLINEABLE boxArea #-} +boxArea :: Representable 𝕀 ( 𝕀ℝ n ) => Box n -> Double +boxArea box = product ( width <$> coordinates box ) + +showArea :: Double -> String +showArea area = "(area " ++ showFFloat (Just 6) area "" ++ ")" + +-------------------------------------------------------------------------------- +-- Utilities for feeding one computation into the next. + +-- NB: I tried adding RULES on these functions in order to perform +-- "loop unrolling"-like optimisations, but this does not seem to +-- improve the performance. + +-- | Run an effectful computation several times in sequence, piping its output +-- into the next input, once for each coordinate dimension. +forEachCoord :: forall n a m. ( KnownNat n, Monad m ) => a -> ( Fin n -> a -> m a ) -> m a +forEachCoord a0 f = go ( toList $ universe @n ) a0 + where + go [] a = return a + go ( i : is ) a = do + a' <- f i a + go is a' +{-# INLINEABLE forEachCoord #-} + +-- | Apply each function in turn, piping the output of one function into +-- the next. +-- +-- Once all the functions have been applied, check whether the Bool is True. +-- If it is, go around again with all the functions; otherwise, stop. +pipeFunctionsWhileTrue :: [ a -> State Bool [ a ] ] -> a -> State Bool [ a ] +pipeFunctionsWhileTrue fns = go fns + where + go [] x = do + doAnotherRound <- State.get + if doAnotherRound + then do { State.put False ; go fns x } + else return [ x ] + go ( f : fs ) x = do + xs <- f x + concat <$> traverse ( go fs ) xs + +-------------------------------------------------------------------------------- + +newtype TimeInterval = TimeInterval Double + deriving newtype Show + +timeInterval :: NFData b => ( a -> b ) -> a -> ( b, TimeInterval ) +timeInterval f a = unsafePerformIO $ do + bef <- getMonotonicTime + let !b = f a + b `deepseq` return () + aft <- getMonotonicTime + return $ ( b, TimeInterval ( aft - bef ) )
+ src/lib/Math/Root/Isolation/Degree.hs view
@@ -0,0 +1,128 @@+{-# LANGUAGE ScopedTypeVariables #-} + +module Math.Root.Isolation.Degree + ( -- * Computing the topological degree + topologicalDegree + ) + where + +-- base +import Data.Kind + ( Type ) +import GHC.TypeNats + ( Nat ) + +-- primitive +import Data.Primitive.SmallArray + +-- brush-strokes +import Math.Interval +import Math.Linear + +-------------------------------------------------------------------------------- +-- Topological degree. + +-- | Compute the topological degree of the given interval function (2D only). +topologicalDegree + :: Double + -- ^ minimum width when bisecting facets + -> ( 𝕀ℝ 2 -> 𝕀ℝ 2 ) + -- ^ function + -> 𝕀ℝ 2 + -- ^ box + -> Maybe Int +topologicalDegree ε_bis f ( 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_lo y_hi ) ) = + topologicalDegreeFromContour $ + smallArrayFromList $ + concatMap ( tagFacet ε_bis f ) + [ ( Tag ( Fin 2 ) P, 𝕀ℝ2 ( 𝕀 x_hi x_hi ) ( 𝕀 y_lo y_hi ) ) + , ( Tag ( Fin 1 ) N, 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_hi y_hi ) ) + , ( Tag ( Fin 2 ) N, 𝕀ℝ2 ( 𝕀 x_lo x_lo ) ( 𝕀 y_lo y_hi ) ) + , ( Tag ( Fin 1 ) P, 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_lo y_lo ) ) ] + +-- | Tag a facet with the sign of one of the equations that is non-zero +-- on that facet. +-- +-- If all equations take the value 0 on that facet, bisect and recur. +tagFacet + :: Double + -> ( 𝕀ℝ 2 -> 𝕀ℝ 2 ) + -> ( Tag 2, 𝕀ℝ 2 ) + -> [ Tag 2 ] +tagFacet ε_bis f ( tag@( Tag i _ ), facet0 ) = go facet0 + where + go facet + | f1_b_lo > 0 + = [ Tag ( Fin 1 ) P ] + | f1_b_hi < 0 + = [ Tag ( Fin 1 ) N ] + | f2_b_lo > 0 + = [ Tag ( Fin 2 ) P ] + | f2_b_hi < 0 + = [ Tag ( Fin 2 ) N ] + | width ( facet `index` i ) < ε_bis + = [ NoTag ] -- TODO: shortcut the whole computation; + -- if we fail to tag one segment, the final answer could + -- be off by { -2, -1, 0, 1, 2 } (I believe) + | otherwise + = concatMap go $ bisectFacet tag facet + where + 𝕀 f1_b_lo f1_b_hi = f facet `index` Fin 1 + 𝕀 f2_b_lo f2_b_hi = f facet `index` Fin 2 +tagFacet _ _ ( NoTag, _ ) = error "impossible: input always tagged" + +bisectFacet :: Tag 2 -> 𝕀ℝ 2 -> [ 𝕀ℝ 2 ] +bisectFacet ( Tag i s ) x = + ( if s == P then id else reverse ) $ bisectInCoord i x +bisectFacet NoTag _ = error "impossible: input always tagged" + +bisectInCoord :: Fin n -> 𝕀ℝ 2 -> [ 𝕀ℝ 2 ] +bisectInCoord ( Fin 1 ) ( 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_lo y_hi ) ) = + [ 𝕀ℝ2 ( 𝕀 x_lo x_mid ) ( 𝕀 y_lo y_hi ) + , 𝕀ℝ2 ( 𝕀 x_mid x_hi ) ( 𝕀 y_lo y_hi ) ] + where x_mid = 0.5 * ( x_lo + x_hi ) +bisectInCoord _ ( 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_lo y_hi ) ) = + [ 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_lo y_mid ) + , 𝕀ℝ2 ( 𝕀 x_lo x_hi ) ( 𝕀 y_mid y_hi ) ] + where y_mid = 0.5 * ( y_lo + y_hi ) + +-- | Compute the topological degree of \( f \colon \mathbb{IR}^n \to \mathbb{IR}^n \) +-- on a box \( D \subset \mathbb{IR}^n \) by using the signs of the components +-- of \( f \) on a counter-clockwise polygonal contour describing the boundary of \( D \). +topologicalDegreeFromContour :: SmallArray ( Tag 2 ) -> Maybe Int +topologicalDegreeFromContour facetTags = go 0 0 + where + n = sizeofSmallArray facetTags + go :: Int -> Int -> Maybe Int + go !d !i + | i >= n + = Just d + | otherwise + = case indexSmallArray facetTags i of + NoTag -> Nothing + Tag ( Fin 1 ) P -> go ( d + δ ) ( i + 1 ) + where + δ = if indexSmallArray facetTags ( if i == n - 1 then 0 else i + 1 ) == Tag ( Fin 2 ) P + then 1 else 0 + - if indexSmallArray facetTags ( if i == 0 then n - 1 else i - 1 ) == Tag ( Fin 2 ) P + then 1 else 0 + _ -> go d ( i + 1 ) + +-------------------------------------------------------------------------------- +-- Tagging edges with signs of certain components of the function. + +data Sign = P | N + deriving stock ( Eq, Ord ) +instance Show Sign where + showsPrec _ = \case { P -> showString "+"; N -> showString "-" } +type Tag :: Nat -> Type +newtype Tag d = MkTag Int + deriving newtype ( Eq, Ord ) +pattern NoTag :: Tag d +pattern NoTag = MkTag 0 +pattern Tag :: Fin d -> Sign -> Tag d +pattern Tag c s <- ( getTag -> (# c, s #) ) + where Tag ( Fin c ) s = MkTag $ case s of { P -> fromIntegral c; N -> -( fromIntegral c ) } +{-# COMPLETE Tag, NoTag #-} +getTag :: Tag d -> (# Fin d, Sign #) +getTag ( MkTag t ) = (# Fin ( fromIntegral $ abs t ), if t >= 0 then P else N #)
+ src/lib/Math/Root/Isolation/Narrowing.hs view
@@ -0,0 +1,511 @@+{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Root.Isolation.Narrowing + ( -- * @box(1)@-consistency + Box1 + , makeBox1Consistent + -- ** Configuration options + , Box1Options(..) + , defaultBox1Options + + , -- *** Narrowing methods for @box(1)@-consistency + NarrowingMethod(..) + , narrowingMethods + + -- **** Options for the adaptive shaving method + , AdaptiveShavingOptions(..) + , defaultAdaptiveShavingOptions + + -- * @box(2)@-consistency + , Box2 + , makeBox2Consistent + + -- ** Configuration options + , Box2Options(..) + , defaultBox2Options + ) + where + +-- base +import Control.Monad + ( when ) +import Data.Foldable + ( toList ) +import qualified Data.List.NonEmpty as NE + ( toList ) +import GHC.TypeNats + ( KnownNat ) + +-- transformers +import Control.Monad.Trans.State.Strict as State + ( State, evalState, get, put ) + +-- brush-strokes +import Math.Algebra.Dual + ( D ) +import Math.Float.Utils + ( succFP, prevFP ) +import Math.Interval +import Math.Linear + ( ℝ + , Representable + , Fin, set, index, universe + ) +import Math.Monomial + ( MonomialBasis(..), Deg, Vars + , zeroMonomial, linearMonomial + ) +import Math.Root.Isolation.Core + +-------------------------------------------------------------------------------- +-- Box-consistency driver code + +-- | A @box(1)@-consistency enforcing algorithm; see 'makeBox1Consistent'. +data Box1 +instance BoxCt n d => RootIsolationAlgorithm Box1 n d where + type instance StepDescription Box1 = String + type instance RootIsolationAlgorithmOptions Box1 n d = Box1Options n d + rootIsolationAlgorithm opts _thisRoundHist _prevRoundsHist eqs box = + return ( "box(1) consistency", makeBox1Consistent opts eqs box ) + {-# INLINEABLE rootIsolationAlgorithm #-} + {-# SPECIALISE rootIsolationAlgorithm @2 @3 #-} + -- NB: including this to be safe. The specialiser seems to sometimes + -- be able to generate this specialisation on its own, and sometimes not. + +-- | A @box(2)@-consistency enforcing algorithm; see 'makeBox1Consistent'. +data Box2 +instance BoxCt n d => RootIsolationAlgorithm Box2 n d where + type instance StepDescription Box2 = String + type instance RootIsolationAlgorithmOptions Box2 n d = Box2Options n d + rootIsolationAlgorithm opts _thisRoundHist _prevRoundsHist eqs box = + return ( "box(2) consistency", [ makeBox2Consistent opts eqs box ] ) + {-# INLINEABLE rootIsolationAlgorithm #-} + {-# SPECIALISE rootIsolationAlgorithm @2 @3 #-} + -- NB: including this to be safe. The specialiser seems to sometimes + -- be able to generate this specialisation on its own, and sometimes not. + +-- | Options for the @box(1)@-consistency method. +data Box1Options n d = + Box1Options + { box1EpsEq :: !Double + , box1EpsBis :: !Double + , box1CoordsToNarrow :: !( [ Fin n ] ) + , box1EqsToUse :: !( [ Fin d ] ) + , box1NarrowingMethod :: !NarrowingMethod + } + deriving stock Show + +-- | Options for the @box(2)@-consistency method. +data Box2Options n d = + Box2Options + { box2Box1Options :: !( Box1Options n d ) + , box2EpsEq :: !Double + , box2LambdaMin :: !Double + } + deriving stock Show + +-- | Default options for the @box(1)@-consistency method. +defaultBox1Options + :: forall n d + . ( KnownNat n, KnownNat d ) + => Double -- ^ minimum width of boxes (don't bisect further) + -> Double -- ^ threshold for progress + -> Box1Options n d +defaultBox1Options minWidth ε_eq = + Box1Options + { box1EpsEq = ε_eq + , box1EpsBis = minWidth + , box1CoordsToNarrow = toList $ universe @n + , box1EqsToUse = toList $ universe @d + , box1NarrowingMethod = Kubica + } +{-# INLINEABLE defaultBox1Options #-} + +-- | Default options for the @box(2)@-consistency method. +defaultBox2Options + :: forall n d + . ( KnownNat n, KnownNat d ) + => Double -- ^ minimum width of boxes (don't bisect further) + -> Double -- ^ threshold for progress + -> Box2Options n d +defaultBox2Options minWidth ε_eq = + Box2Options + { box2Box1Options = defaultBox1Options minWidth ε_eq + , box2EpsEq = ε_eq + , box2LambdaMin = 0.001 + } +{-# INLINEABLE defaultBox2Options #-} + +-- | An implementation of "bc_enforce" from the paper +-- "Parallelization of a bound-consistency enforcing procedure and its application in solving nonlinear systems" +-- +-- See also +-- "Presentation of a highly tuned multithreaded interval solver for underdetermined and well-determined nonlinear systems" +makeBox1Consistent + :: ( KnownNat n + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , MonomialBasis ( D 1 n ) + , Deg ( D 1 n ) ~ 1 + , Vars ( D 1 n ) ~ n + , Representable Double ( ℝ d ) + , Representable 𝕀 ( 𝕀ℝ d ) + ) + => Box1Options n d + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -> 𝕀ℝ n -> [ 𝕀ℝ n ] +makeBox1Consistent box1Options eqs x = + ( `State.evalState` False ) $ + pipeFunctionsWhileTrue ( allNarrowingOperators box1Options eqs ) x +{-# INLINEABLE makeBox1Consistent #-} +{-# SPECIALISE makeBox1Consistent @2 @3 #-} + +-- | An implementation of "bound-consistency" from the paper +-- "Parallelization of a bound-consistency enforcing procedure and its application in solving nonlinear systems" +makeBox2Consistent + :: forall n d + . ( KnownNat n + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , MonomialBasis ( D 1 n ) + , Deg ( D 1 n ) ~ 1 + , Vars ( D 1 n ) ~ n + , Representable Double ( ℝ d ) + , Representable 𝕀 ( 𝕀ℝ d ) + ) + => Box2Options n d + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -> 𝕀ℝ n -> 𝕀ℝ n +makeBox2Consistent (Box2Options box1Options ε_eq λMin) eqs x0 + = ( `State.evalState` False ) $ doLoop 0.25 x0 + where + doBox1 :: 𝕀ℝ n -> [ 𝕀ℝ n ] + doBox1 = makeBox1Consistent box1Options eqs + doLoop :: Double -> 𝕀ℝ n -> State Bool ( 𝕀ℝ n ) + doLoop λ x = do + x'' <- forEachCoord @n x $ boundConsistency λ + modified <- State.get + let λ' = if modified then λ else 0.5 * λ + if λ' < λMin + then return x'' + else do { State.put False ; doLoop λ' x'' } + + boundConsistency :: Double -> Fin n -> 𝕀ℝ n -> State Bool ( 𝕀ℝ n ) + boundConsistency λ i box = do + let x@( 𝕀 x_inf x_sup ) = getter box + c1 = ( 1 - λ ) * x_inf + λ * x_sup + c2 = λ * x_inf + ( 1 - λ ) * x_sup + x'_inf = + case doBox1 ( setter ( 𝕀 x_inf c1 ) box ) of + [] -> c1 + x's -> minimum $ map ( inf . getter ) x's + x'_sup = + case doBox1 ( setter ( 𝕀 c2 x_sup ) box ) of + [] -> c2 + x's -> maximum $ map ( sup . getter ) x's + x' = 𝕀 x'_inf x'_sup + when ( width x - width x' >= ε_eq ) $ + State.put True + return $ setter x' box + where + getter = ( `index` i ) + setter = set i +{-# INLINEABLE makeBox2Consistent #-} +{-# SPECIALISE makeBox2Consistent @2 @3 #-} + +-------------------------------------------------------------------------------- +-- Narrowing methods + +-- | The narrowing method to use to enforce @box(1)@-consistency. +data NarrowingMethod + -- | Algorithm 5 from the paper + -- "Parallelization of a bound-consistency enforcing procedure and its application in solving nonlinear systems" + -- (Bartłomiej Jacek Kubica, 2017) + = Kubica + -- | Two-sided shaving @sbc@ from the paper + -- "A Data-Parallel Algorithm to Reliably Solve Systems of Nonlinear Equations", + -- (Goldsztejn & Goualard, 2008) + | TwoSidedShaving + -- | @sbc3ag@ (adaptive guessing) shaving, from the paper + -- "Box Consistency through Adaptive Shaving" + -- (Goldsztejn & Goualard, 2010). + | AdaptiveShaving AdaptiveShavingOptions + deriving stock Show + +narrowingMethods + :: Double -> Double + -> NarrowingMethod + -> [ ( 𝕀 -> ( 𝕀, 𝕀 ) ) -> 𝕀 -> [ 𝕀 ] ] +narrowingMethods ε_eq ε_bis Kubica + = [ leftNarrow ε_eq ε_bis, rightNarrow ε_eq ε_bis ] +narrowingMethods ε_eq ε_bis ( AdaptiveShaving opts ) + = [ leftShave ε_eq ε_bis opts, rightNarrow ε_eq ε_bis ] + -- TODO: haven't implemented right shaving yet +narrowingMethods _ε_eq ε_bis TwoSidedShaving + = [ sbc ε_bis ] +{-# INLINEABLE narrowingMethods #-} + +allNarrowingOperators + :: forall n d + . ( KnownNat n + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , MonomialBasis ( D 1 n ) + , Deg ( D 1 n ) ~ 1 + , Vars ( D 1 n ) ~ n + , Representable Double ( ℝ d ) + , Representable 𝕀 ( 𝕀ℝ d ) + ) + => Box1Options n d + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -> [ 𝕀ℝ n -> State Bool [ 𝕀ℝ n ] ] +allNarrowingOperators ( Box1Options ε_eq ε_bis coordsToNarrow eqsToUse narrowingMethod ) eqs = + [ \ cand -> + let getter = ( `index` coordIndex ) + setter = set coordIndex + newCands = map ( `setter` cand ) + $ narrowFn ( \ box -> ff' coordIndex eqnIndex $ setter box cand ) + ( getter cand ) + in do + -- Record when we achieved a meaningful reduction, + -- so that we continue trying further narrowings. + when ( ( width ( getter cand ) - sum ( map ( width . getter ) newCands ) ) >= ε_eq ) $ + -- NB: making this return 'True' less often seems slightly beneficial? + -- Further investigation needed. + State.put True + return newCands + | narrowFn <- narrowingMethods ε_eq ε_bis narrowingMethod + , coordIndex <- coordsToNarrow + , eqnIndex <- eqsToUse + ] + where + ff' :: Fin n -> Fin d -> 𝕀ℝ n -> ( 𝕀, 𝕀 ) + ff' i d ts = + let df = eqs ts + f, f' :: 𝕀ℝ d + f = df `monIndex` zeroMonomial + f' = df `monIndex` linearMonomial i + in ( f `index` d, f' `index` d ) +{-# INLINEABLE allNarrowingOperators #-} +{-# SPECIALISE allNarrowingOperators @2 @3 #-} + +-------------------------------------------------------------------------------- +-- Kubica's algorithm. + +-- Use the univariate interval Newton method to narrow from the left +-- a candidate interval. +-- +-- See Algorithm 5 (Procedure left_narrow) in +-- "Parallelization of a bound-consistency enforcing procedure and its application in solving nonlinear systems" +-- by Bartłomiej Jacek Kubica, 2017 +leftNarrow :: Double + -> Double + -> ( 𝕀 -> ( 𝕀, 𝕀 ) ) + -> 𝕀 + -> [ 𝕀 ] +leftNarrow ε_eq ε_bis ff' = left_narrow + where + left_narrow ( 𝕀 x_inf x_sup ) = + go x_sup ( 𝕀 x_inf ( if x_inf == x_sup then x_inf else succFP x_inf ) ) + go x_sup x_left = + let ( f_x_left, _f'_x_left ) = ff' x_left + in + if inf f_x_left <= 0 && sup f_x_left >= 0 + then [ 𝕀 ( inf x_left ) x_sup ] + else + let x = 𝕀 ( sup x_left ) x_sup + ( _f_x, f'_x ) = ff' x + x's = do δ <- f_x_left ⊘ f'_x + let x_new = x_left - δ + map fst $ x_new ∩ x + in + if | null x's + -> [] + | ( width x - sum ( map width x's ) ) < ε_eq + -> x's + | otherwise + -> do + x' <- x's + if sup x' - inf x' < ε_bis + then return x' + else left_narrow =<< NE.toList ( bisect x' ) + +-- TODO: de-duplicate with 'leftNarrow'? +rightNarrow :: Double + -> Double + -> ( 𝕀 -> ( 𝕀, 𝕀 ) ) + -> 𝕀 + -> [ 𝕀 ] +rightNarrow ε_eq ε_bis ff' = right_narrow + where + right_narrow ( 𝕀 x_inf x_sup ) = + go x_inf ( 𝕀 ( if x_inf == x_sup then x_sup else prevFP x_sup ) x_sup ) + go x_inf x_right = + let ( f_x_right, _f'_x_right ) = ff' x_right + in + if inf f_x_right <= 0 && sup f_x_right >= 0 + then [ 𝕀 x_inf ( sup x_right ) ] + else + let x = 𝕀 x_inf ( inf x_right ) + ( _f_x, f'_x ) = ff' x + x's = do δ <- f_x_right ⊘ f'_x + let x_new = x_right - δ + map fst $ x_new ∩ x + in + if | null x's + -> [] + | ( width x - sum ( map width x's ) ) < ε_eq + -> x's + | otherwise + -> do + x' <- x's + if sup x' - inf x' < ε_bis + then return x' + else right_narrow =<< NE.toList ( bisect x' ) + +-------------------------------------------------------------------------------- +-- Adaptive shaving. + +data AdaptiveShavingOptions + = AdaptiveShavingOptions + { γ_init, σ_good, σ_bad, β_good, β_bad :: !Double + } + deriving stock Show + +defaultAdaptiveShavingOptions :: AdaptiveShavingOptions +defaultAdaptiveShavingOptions = + AdaptiveShavingOptions + { γ_init = 0.25 + , σ_good = 0.25 + , σ_bad = 0.75 + , β_good = 1.5 + , β_bad = 0.7 + } + +-- | Algorithm @lnar_sbc3ag@ (adaptive guessing) from the paper +-- "Box Consistency through Adaptive Shaving" (Goldsztejn & Goualard, 2010). +leftShave :: Double + -> Double + -> AdaptiveShavingOptions + -> ( 𝕀 -> ( 𝕀, 𝕀 ) ) + -> 𝕀 + -> [ 𝕀 ] +leftShave ε_eq ε_bis + ( AdaptiveShavingOptions { γ_init, σ_good, σ_bad, β_good, β_bad } ) + ff' i0 = + left_narrow γ_init i0 + where + w0 = width i0 + left_narrow :: Double -> 𝕀 -> [ 𝕀 ] + left_narrow γ i@( 𝕀 x_inf x_sup ) + -- Stop if the box is too small. + | width i < ε_bis + = [ i ] + | otherwise + = go γ x_sup ( 𝕀 x_inf ( if x_inf == x_sup then x_inf else succFP x_inf ) ) + go :: Double -> Double -> 𝕀 -> [ 𝕀 ] + go γ x_sup x_left = + let ( f_x_left, _f'_x_left ) = ff' x_left + in + if 0 ∈ f_x_left + -- Box-consistency achieved; finish. + then [ 𝕀 ( inf x_left ) x_sup ] + else + -- Otherwise, try to shave off a chunk on the left of the interval. + let x' = 𝕀 ( sup x_left ) x_sup + inf_guess = sup x_left + sup_guess = min x_sup ( inf_guess + γ * w0 ) -- * width x' ) + guess = 𝕀 inf_guess sup_guess + -- NB: this always uses the initial width (to "avoid asymptotic behaviour" according to the paper) + ( f_guess, f'_guess ) = ff' guess + x_minus_guess = 𝕀 ( min x_sup ( succFP $ sup guess ) ) x_sup + in if not ( 0 ∈ f_guess ) + then + -- We successfully shaved "guess" off; go round again after removing it. + -- TODO: here we could go back to the top with a new "w" maybe? + left_narrow ( β_good * γ ) x_minus_guess + else + -- Do a Newton step to try to reduce the guess interval. + -- Starting from "guess", we get a collection of sub-intervals + -- "guesses'" refining where the function can be zero. + let guesses' :: [ 𝕀 ] + guesses' = do + δ <- f_x_left ⊘ f'_guess + let guess' = singleton ( inf guess ) - δ + map fst $ guess' ∩ guess + w_guess = width guess + w_guesses' + | null guesses' + = 0 + | otherwise + = sup_guess - minimum ( map inf guesses' ) + γ' | w_guesses' < σ_good * w_guess + -- Good improvement, try larger guesses in the future. + = β_good * γ + | w_guesses' > σ_bad * w_guess + -- Poor improvement, try smaller guesses in the future. + = β_bad * γ + | otherwise + -- Otherwise, keep the γ factor the same. + = γ + xs' = x_minus_guess : guesses' + in if ( width x' - sum ( map width xs' ) ) < ε_eq + then xs' + else left_narrow γ' =<< xs' +{-# INLINEABLE leftShave #-} + +-------------------------------------------------------------------------------- +-- Two-sided shaving. + +-- | @sbc@ algorithm from the paper +-- +-- "A Data-Parallel Algorithm to Reliably Solve Systems of Nonlinear Equations", +-- (Frédéric Goualard, Alexandre Goldsztejn, 2008). +sbc :: Double + -> ( 𝕀 -> ( 𝕀, 𝕀 ) ) + -> 𝕀 -> [ 𝕀 ] +sbc ε ff' = go + where + go :: 𝕀 -> [ 𝕀 ] + go x@( 𝕀 x_l x_r ) + | width x <= ε + = [ x ] + | otherwise + = let x_mid = 0.5 * ( x_l + x_r ) + ( left_done, left_todo ) + | 0 ∈ ( fst $ ff' ( 𝕀 x_l x_lp ) ) + = ( True, [ ] ) + | not $ 0 ∈ ( fst $ ff' i_l ) + = ( False, [ ] ) + | otherwise + = let + xls = do + let l = 𝕀 x_lp x_lp + δ <- fst ( ff' l ) ⊘ snd ( ff' i_l ) + map fst $ ( l - δ ) ∩ i_l + in ( False, xls ) + where x_lp = min ( succFP x_l ) x_mid + i_l = 𝕀 x_lp x_mid + ( right_done, right_todo ) + | 0 ∈ ( fst $ ff' ( 𝕀 x_rm x_r ) ) + = ( True, [ ] ) + | not $ 0 ∈ ( fst $ ff' i_r ) + = ( False, [ ] ) + | otherwise + = let + xrs = do + let r = 𝕀 x_rm x_rm + δ <- fst ( ff' r ) ⊘ snd ( ff' i_r ) + map fst $ ( r - δ ) ∩ i_r + in ( False, xrs ) + where x_rm = max ( prevFP x_r ) x_mid + i_r = 𝕀 x_mid x_rm + in do let lefts' = if left_done + then [ 𝕀 x_l x_mid ] + else go =<< left_todo + rights' = if right_done + then [ 𝕀 x_mid x_r ] + else go =<< right_todo + lefts' ++ rights' +{-# INLINEABLE sbc #-}
+ src/lib/Math/Root/Isolation/Newton.hs view
@@ -0,0 +1,182 @@+{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Root.Isolation.Newton + ( -- * The interval Newton method, + -- with Gauss–Seidel step or explicit linear programming + Newton + , intervalNewton + + -- ** Configuration options + , NewtonOptions(..) + , defaultNewtonOptions + ) + where + +-- base +import Prelude hiding ( unzip ) +import Control.Arrow + ( first ) +import Data.Bifunctor + ( Bifunctor(bimap) ) +import Data.Kind + ( Type ) +import Data.List + ( partition ) +import GHC.TypeNats + ( Nat, KnownNat, type (<=) ) + +-- deepseq +import Control.DeepSeq + ( force ) + +-- transformers +import Control.Monad.Trans.Writer.CPS + ( Writer, tell ) + +-- brush-strokes +import Math.Algebra.Dual + ( D ) +import Math.Interval +import Math.Linear +import Math.Module + ( Module(..) ) +import Math.Monomial + ( MonomialBasis(..), linearMonomial, zeroMonomial ) +import Math.Root.Isolation.Core +import Math.Root.Isolation.Newton.GaussSeidel +import Math.Root.Isolation.Newton.LP +import Math.Root.Isolation.Utils + +-------------------------------------------------------------------------------- +-- Interval Newton + +-- | The interval Newton method; see 'intervalNewton'. +data Newton +instance (Show (NewtonOptions n d), BoxCt n d) => RootIsolationAlgorithm Newton n d where + type instance StepDescription Newton = (String, TimeInterval) + type instance RootIsolationAlgorithmOptions Newton n d = NewtonOptions n d + rootIsolationAlgorithm opts _thisRoundHist _prevRoundsHist eqs box = + first ( "Newton " ++ show opts, ) <$> intervalNewton @n @d opts eqs box + {-# INLINEABLE rootIsolationAlgorithm #-} + {-# SPECIALISE rootIsolationAlgorithm @2 @3 #-} + -- NB: including this to be safe. The specialiser seems to sometimes + -- be able to generate this specialisation on its own, and sometimes not. + +-- | Options for the interval Newton method. +type NewtonOptions :: Nat -> Nat -> Type +data NewtonOptions n d where + -- | Use the Gauss–Seidel method to solve linear systems. + NewtonGaussSeidel + :: GaussSeidelOptions n d -> NewtonOptions n d + -- | Use linear programming to solve linear systems (2 dimensions only). + NewtonLP + :: NewtonOptions 2 d + +deriving stock instance Show ( NewtonOptions n d ) + +-- | Default options for the interval Newton method. +defaultNewtonOptions + :: forall n d + . ( KnownNat n, KnownNat d + , 1 <= n, 1 <= d, n <= d + , Representable Double ( ℝ n ) + , Representable Double ( ℝ d ) + , Representable 𝕀 ( 𝕀ℝ n ) + , Representable 𝕀 ( 𝕀ℝ d ) + ) + => BoxHistory n + -> NewtonOptions n d +defaultNewtonOptions history = + NewtonGaussSeidel $ defaultGaussSeidelOptions history +{-# INLINEABLE defaultNewtonOptions #-} + +-- | Interval Newton method with Gauss–Seidel step. +intervalNewton + :: forall n d + . BoxCt n d + => NewtonOptions n d + -> ( 𝕀ℝ n -> D 1 n ( 𝕀ℝ d ) ) + -- ^ equations + -> 𝕀ℝ n + -- ^ box + -> Writer ( DoneBoxes n ) ( TimeInterval, [ 𝕀ℝ n ] ) +intervalNewton opts eqs x = case opts of + NewtonGaussSeidel + ( GaussSeidelOptions + { gsPreconditioner = precondMeth + , gsPickEqs = pickEqs + , gsUpdate + } ) -> + let x_mid = point $ boxMidpoint x + f :: 𝕀ℝ n -> 𝕀ℝ n + f = \ x_0 -> pickEqs $ eqs x_0 `monIndex` zeroMonomial + f'_x :: Vec n ( 𝕀ℝ n ) + f'_x = fmap ( \ i -> pickEqs $ eqs x `monIndex` linearMonomial i ) ( universe @n ) + + -- Interval Newton method: take one Gauss–Seidel step + -- for the system of equations f'(x) ( x - x_mid ) = - f(x_mid). + minus_f_x_mid = unT $ -1 *^ T ( boxMidpoint $ f x_mid ) + + -- Precondition the above linear system into A ( x - x_mid ) = B. + !( !a, !b ) = force $ + precondition precondMeth + f'_x ( point minus_f_x_mid ) + !x'_0 = force ( T x ^-^ T x_mid ) + + -- NB: we have to change coordinates, putting the midpoint of the box + -- at the origin, in order to take a Gauss–Seidel step. + ( x's, dt ) = + timeInterval + ( gaussSeidelUpdate gsUpdate a b ) + x'_0 + gsGuesses = map ( first ( \ x' -> unT $ x' ^+^ T x_mid ) ) x's + ( done, todo ) = bimap ( map fst ) ( map fst ) + $ partition snd gsGuesses + in -- If the Gauss–Seidel step was a contraction, then the box + -- contains a unique solution (by the Banach fixed point theorem). + -- + -- These boxes can thus be directly added to the solution set: + -- Newton's method is guaranteed to converge to the unique solution. + do tell $ noDoneBoxes { doneSolBoxes = done } + return ( dt, todo ) + NewtonLP -> + -- TODO: reduce duplication with the above. + let x_mid = point $ boxMidpoint x + f :: 𝕀ℝ 2 -> 𝕀ℝ d + f = \ x_0 -> eqs x_0 `monIndex` zeroMonomial + f'_x :: Vec 2 ( 𝕀ℝ d ) + f'_x = fmap ( \ i -> eqs x `monIndex` linearMonomial i ) ( universe @2 ) + + minus_f_x_mid = unT $ -1 *^ T ( boxMidpoint $ f x_mid ) + !( !a, !b ) = force ( f'_x, point minus_f_x_mid ) + !x'_0 = force ( T x ^-^ T x_mid ) + ( x's, dt ) = + timeInterval + ( solveIntervalLinearEquations a b ) x'_0 + lpGuesses = map ( first ( \ x' -> unT $ x' ^+^ T x_mid ) ) x's + ( done, todo ) = bimap ( map fst ) ( map fst ) + $ partition snd lpGuesses + in do tell $ noDoneBoxes { doneSolBoxes = done } + return ( dt, todo ) +{-# INLINEABLE intervalNewton #-} +{-# SPECIALISE intervalNewton @2 @3 #-} + +{- + +mbDeg = topologicalDegree 0.005 f x +det = case f'_x of + Vec [ c1, c2 ] -> + let a_11 = c1 `index` Fin 1 + a_12 = c2 `index` Fin 1 + a_21 = c1 `index` Fin 2 + a_22 = c2 `index` Fin 2 + in a_11 * a_22 - a_12 * a_21 + _ -> error "TODO: just testing n=2 here" + +if | not $ 0 ∈ det + , mbDeg == Just 0 + -> return [] + -- If the Jacobian is invertible over the box, then the topological + -- degree tells us exactly how many solutions there are in the box. +-}
+ src/lib/Math/Root/Isolation/Newton/GaussSeidel.hs view
@@ -0,0 +1,238 @@+{-# LANGUAGE ScopedTypeVariables #-} + +-- | The Gauss–Seidel method for solving systems of +-- interval linear equations +module Math.Root.Isolation.Newton.GaussSeidel + ( -- * Gauss–Seidel step + gaussSeidelUpdate + -- ** Options for the Gauss–Seidel method + , GaussSeidelOptions(..) + , Preconditioner(..), GaussSeidelUpdateMethod(..) + , defaultGaussSeidelOptions + , precondition + ) + where + +-- base +import Prelude hiding ( unzip ) +import Data.Coerce + ( coerce ) +import Data.Kind + ( Type ) +import Data.Foldable + ( toList ) +import Data.Proxy + ( Proxy(..) ) +import Data.Type.Ord + ( OrderingI(..) ) +import GHC.TypeNats + ( Nat, KnownNat, type (<=), cmpNat ) + +-- eigen +import qualified Eigen.Matrix as Eigen + ( inverse ) + +-- brush-strokes +import Math.Interval +import Math.Linear +import Math.Linear.Solve + ( fromEigen, toEigen ) +import Math.Root.Isolation.Core +import Math.Root.Isolation.Utils + +-------------------------------------------------------------------------------- +-- Gauss–Seidel + +-- | Options for the Gauss–Seidel method. +type GaussSeidelOptions :: Nat -> Nat -> Type +data GaussSeidelOptions n d = + GaussSeidelOptions + { -- | Which preconditioner to use? + gsPreconditioner :: !Preconditioner + -- | Function that projects over the equations we will consider + -- (the identity for a well-determined problem, or a projection for + -- an overdetermined system). + , gsPickEqs :: !( 𝕀ℝ d -> 𝕀ℝ n ) + -- | Whether to use a partial or a complete Gauss–Seidel update + , gsUpdate :: !GaussSeidelUpdateMethod + } +instance Show ( GaussSeidelOptions n d ) where + show ( GaussSeidelOptions { gsPreconditioner, gsUpdate } ) = + unlines + [ "GaussSeidelOptions" + , " preconditioner: " ++ show gsPreconditioner + , " update: " ++ show gsUpdate + ] + +-- | Whether to use a partial or a complete Gauss–Seidel update. +data GaussSeidelUpdateMethod + = GS_Partial + | GS_Complete + deriving stock Show + +-- | Default options for the Gauss–Seidel step. +defaultGaussSeidelOptions + :: forall n d + . ( KnownNat n, KnownNat d + , 1 <= n, 1 <= d, n <= d + , Representable Double ( ℝ n ), Representable 𝕀 ( 𝕀ℝ n ) + , Representable Double ( ℝ d ), Representable 𝕀 ( 𝕀ℝ d ) + ) + => BoxHistory n + -> GaussSeidelOptions n d +defaultGaussSeidelOptions history = + GaussSeidelOptions + { gsPreconditioner = InverseMidpoint + , gsPickEqs = + case cmpNat @n @d Proxy Proxy of + EQI -> id + LTI -> + -- If there are more equations (d) than variables (n), + -- pick a size n subset of the variables + -- (go through all combinations cyclically). + let choices :: [ Vec n ( Fin d ) ] + choices = choose @d @n + choice :: Vec n ( Fin d ) + choice = choices !! ( length history `mod` length choices ) + in \ u -> tabulate \ i -> index u ( choice ! i ) + , gsUpdate = GS_Complete + } +{-# INLINEABLE defaultGaussSeidelOptions #-} + +-- | Preconditioner to use with the interval Gauss–Seidel method. +data Preconditioner + = NoPreconditioning + | InverseMidpoint + deriving stock Show + +-- | A partial or complete Gauss–Seidel step for the equation \( A X = B \), +-- refining the initial guess box for \( X \) into up to \( 2^n \) (disjoint) new boxes. +gaussSeidelUpdate + :: forall n + . ( Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , Eq ( ℝ n ) + , Show ( 𝕀ℝ n ) + ) + => GaussSeidelUpdateMethod -- ^ which step method to use + -> Vec n ( 𝕀ℝ n ) -- ^ columns of \( A \) + -> 𝕀ℝ n -- ^ \( B \) + -> T ( 𝕀ℝ n ) -- ^ initial box \( X \) + -> [ ( T ( 𝕀ℝ n ), Bool ) ] +gaussSeidelUpdate upd as b x = + case upd of + GS_Partial -> gaussSeidelStep as b x + GS_Complete -> gaussSeidelStep_Complete as b x +{-# INLINEABLE gaussSeidelUpdate #-} + +-- | Take one interval Gauss–Seidel step for the equation \( A X = B \), +-- refining the initial guess box for \( X \) into up to \( 2^n \) (disjoint) new boxes. +-- +-- The boolean indicates whether the Gauss–Seidel step was a contraction. +gaussSeidelStep + :: forall n + . ( Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , Eq ( ℝ n ) + ) + => Vec n ( 𝕀ℝ n ) -- ^ columns of \( A \) + -> 𝕀ℝ n -- ^ \( B \) + -> T ( 𝕀ℝ n ) -- ^ initial box \( X \) + -> [ ( T ( 𝕀ℝ n ), Bool ) ] +gaussSeidelStep as b ( T x0 ) = coerce $ + forEachCoord @n ( x0, True ) $ \ i ( x, contraction ) -> do + -- For each i, we have an equation: sum_j a_ij * x_j = b_i + -- + -- Re-arrange this with x_i on the left to get an iteration: + -- x_i' = ( b_i - sum { j /= i } a_ij * x_j ) / a_ii + -- + -- We perform each iteration in turn (for i = 1, ..., n), + -- **using the latest updated value of each x_j** in each iteration. + let s = b `index` i - sum [ ( as ! j ) `index` i * x `index` j + | j <- toList ( universe @n ), j /= i ] + x_i = x `index` i + a_ii = ( as ! i ) `index` i + -- Take a shortcut before performing the division if possible. + if | not $ 0 ∈ ( s - a_ii * x_i ) + -- No solutions: don't bother performing a division. + -> [ ] + | 0 ∈ s && 0 ∈ a_ii + -- The division would produce [-oo,+oo]: don't do anything. + -> [ ( x, False ) ] + -- Otherwise, perform the division. + | otherwise + -> do + x_i'0 <- s ⊘ a_ii + ( x_i', sub_i ) <- x_i'0 ∩ x_i + return $ ( set i x_i' x, sub_i && contraction ) +{-# INLINEABLE gaussSeidelStep #-} + +-- | The complete interval-union Gauss–Seidel step. +-- +-- Algorithm 2 from: +-- "Using interval unions to solve linear systems of equations with uncertainties" +-- (Montanher, Domes, Schichl, Neumaier) (2017) +gaussSeidelStep_Complete + :: forall n + . ( Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + , Show ( 𝕀ℝ n ) + ) + => Vec n ( 𝕀ℝ n ) -- ^ columns of \( A \) + -> 𝕀ℝ n -- ^ \( B \) + -> T ( 𝕀ℝ n ) -- ^ initial box \( X \) + -> [ ( T ( 𝕀ℝ n ), Bool ) ] +gaussSeidelStep_Complete as b ( T x0 ) = coerce $ do + ( x', subs ) <- + forEachCoord @n ( x0, pure False ) $ \ i ( x, contractions ) -> do + let s = b `index` i - sum [ ( as ! k ) `index` i * x `index` k + | k <- toList ( universe @n ) ] + ( x', subs ) <- fromComponents \ j -> do + let x_j = x `index` j + a_ij = ( as ! j ) `index` i + s_j = s ⊖ ( -a_ij * x_j ) + -- NB: the paper is missing the minus sign here. + -- In the definition of s we subtract all terms including + -- the a_ij * x_j term, so we need to add it back. + + -- Shortcut division if possible (see gaussSeidelStep for commentary). + -- NB: there is a typo in the paper here, it should NOT be + -- + -- 0 ∈ ( s_j - a_ii * x_j ) + -- + -- because we are dividing by a_ij, not a_ii. + if | not $ 0 ∈ ( s_j - a_ij * x_j ) + -> [ ] + | 0 ∈ s_j && 0 ∈ a_ij + -> [ ( x_j, False ) ] + | otherwise + -> do + x_j'0 <- s_j ⊘ a_ij + ( x_j', sub_j ) <- x_j'0 ∩ x_j + return $ ( x_j', sub_j ) + return ( x', (||) <$> contractions <*> subs ) + return ( x', and subs ) +{-# INLINEABLE gaussSeidelStep_Complete #-} + +-- | Pre-condition the system \( AX = B \). +precondition + :: forall n + . ( KnownNat n + , Representable Double ( ℝ n ) + , Representable 𝕀 ( 𝕀ℝ n ) + ) + => Preconditioner -- ^ pre-conditioning method to use + -> Vec n ( 𝕀ℝ n ) -- ^ columns of \( A \) + -> 𝕀ℝ n -- ^ \( B \) + -> ( Vec n ( 𝕀ℝ n ), 𝕀ℝ n ) +precondition meth as b = + case meth of + NoPreconditioning + -> ( as, b ) + InverseMidpoint + | let mat = toEigen $ fmap boxMidpoint as + , let precond = Eigen.inverse mat + doPrecond = matMulVec ( fromEigen precond ) + -- TODO: avoid this when condition number is small? + -> ( fmap doPrecond as, doPrecond b ) +{-# INLINEABLE precondition #-}
+ src/lib/Math/Root/Isolation/Newton/LP.hs view
@@ -0,0 +1,132 @@+{-# LANGUAGE CApiFFI #-} +{-# LANGUAGE ForeignFunctionInterface #-} +{-# LANGUAGE ParallelListComp #-} +{-# LANGUAGE ScopedTypeVariables #-} + +-- | A linear programming approach for solving systems of +-- interval linear equations. +module Math.Root.Isolation.Newton.LP + ( solveIntervalLinearEquations ) + where + +-- base +import Control.Arrow + ( (&&&) ) +import Data.Coerce + ( coerce ) +import Data.Foldable + ( toList ) +import Foreign.C.Types + ( CDouble(..), CInt(..), CUInt(..) ) +import Foreign.Marshal + ( allocaArray, peekArray, pokeArray, with, withArray ) +import Foreign.Ptr + ( Ptr, castPtr ) +import Foreign.Storable + ( Storable(..) ) +import GHC.TypeNats + ( KnownNat, natVal' ) +import GHC.Exts + ( proxy# ) +import System.IO.Unsafe + ( unsafePerformIO ) + +-- brush-strokes +import Math.Interval +import Math.Linear + +-------------------------------------------------------------------------------- +-- Linear programming approach to solving systems of linear equations +-- (in two variables). + +-- | Solve the system of linear equations \( A X = B \) +-- using linear programming. +solveIntervalLinearEquations + :: ( KnownNat d, Representable 𝕀 ( 𝕀ℝ d ) ) + => Vec 2 ( 𝕀ℝ d ) -- ^ columns of \( A \) + -> 𝕀ℝ d -- ^ \( B \) + -> T ( 𝕀ℝ 2 ) -- ^ initial box \( X \) + -> [ ( T ( 𝕀ℝ 2 ), Bool ) ] +solveIntervalLinearEquations a b x = + let !sols = unsafePerformIO $ intervalSystem2D_LP a b x + in if + | any hasNaNs sols + -> [ ( x, False ) ] + | length sols <= 1 + -> map ( id &&& isSubBox x ) sols + | otherwise + -> map ( , False ) sols + +-- Assuming the second box is a subset of the second box, returns whether it +-- is in fact a strict subset. +isSubBox :: T ( 𝕀ℝ 2 ) -> T ( 𝕀ℝ 2 ) -> Bool +isSubBox + ( T ( 𝕀ℝ2 ( 𝕀 x1_min x1_max ) ( 𝕀 y1_min y1_max ) ) ) + ( T ( 𝕀ℝ2 ( 𝕀 x2_min x2_max ) ( 𝕀 y2_min y2_max ) ) ) + = ( ( x2_min > x1_min && x2_max < x1_max ) || x2_min == x2_max ) + && ( ( y2_min > y1_min && y2_max < y1_max ) || y2_min == y2_max ) + +hasNaNs :: T (𝕀ℝ 2) -> Bool +hasNaNs ( T ( 𝕀ℝ2 ( 𝕀 x_min x_max ) ( 𝕀 y_min y_max ) ) ) = + any ( \ x -> isNaN x || isInfinite x ) [ x_min, y_min, x_max, y_max ] + +intervalSystem2D_LP + :: forall d + . ( KnownNat d, Representable 𝕀 ( 𝕀ℝ d ) ) + => Vec 2 ( 𝕀ℝ d ) + -> 𝕀ℝ d + -> T ( 𝕀ℝ 2 ) + -> IO [ T ( 𝕀ℝ 2 ) ] +intervalSystem2D_LP a b x = + allocaArray 4 \ ptrSolutions -> + with ( CBox $ unT $ x ) \ ptrBox -> + withArray ( mkEquationArray a b ) \ ptrEqs -> do + CInt nbSols <- + interval_system_2d ptrSolutions ptrBox ptrEqs ( fromIntegral d ) + if nbSols < 0 + then + error $ unlines + [ "interval_system_2d returned with exit code " ++ show nbSols + , "This probably means it was given invalid input." ] + else + coerce <$> peekArray ( fromIntegral nbSols ) ptrSolutions + where + d = natVal' @d proxy# + +mkEquationArray + :: ( KnownNat d, Representable 𝕀 ( 𝕀ℝ d ) ) + => Vec 2 ( 𝕀ℝ d ) -> 𝕀ℝ d -> [ CEqn ] +mkEquationArray ( Vec [ a_x, a_y ] ) b = + [ CEqn a_x_i a_y_i b_i + | a_x_i <- toList $ coordinates a_x + | a_y_i <- toList $ coordinates a_y + | b_i <- toList $ coordinates b + ] +mkEquationArray _ _ = error "impossible" + +foreign import ccall "interval_system_2d" + interval_system_2d :: Ptr CBox -> Ptr CBox -> Ptr CEqn -> CUInt -> IO CInt + +data CEqn = CEqn !𝕀 !𝕀 !𝕀 +instance Storable CEqn where + sizeOf _ = 6 * sizeOf @Double undefined + alignment _ = 4 * alignment @Double undefined + peek ptr = do + [ CDouble a_min, CDouble a_max, CDouble b_min, CDouble b_max, CDouble c_min, CDouble c_max ] + <- peekArray 6 ( castPtr ptr :: Ptr CDouble ) + return $ + CEqn ( 𝕀 a_min a_max ) ( 𝕀 b_min b_max ) ( 𝕀 c_min c_max ) + poke ptr ( CEqn ( 𝕀 a_min a_max ) ( 𝕀 b_min b_max ) ( 𝕀 c_min c_max ) ) + = pokeArray ( castPtr ptr ) [ CDouble a_min, CDouble a_max, CDouble b_min, CDouble b_max, CDouble c_min, CDouble c_max ] + + +newtype CBox = CBox ( 𝕀ℝ 2 ) +instance Storable CBox where + sizeOf _ = 4 * sizeOf @Double undefined + alignment _ = 4 * alignment @Double undefined + peek ptr = do + [ CDouble x_min, CDouble x_max, CDouble y_min, CDouble y_max ] <- peekArray 4 ( castPtr ptr :: Ptr CDouble ) + return $ + CBox ( 𝕀ℝ2 ( 𝕀 x_min x_max ) ( 𝕀 y_min y_max ) ) + poke ptr ( CBox ( 𝕀ℝ2 ( 𝕀 x_min x_max ) ( 𝕀 y_min y_max ) ) ) = + pokeArray ( castPtr ptr ) [ CDouble x_min, CDouble x_max, CDouble y_min, CDouble y_max ]
+ src/lib/Math/Root/Isolation/Utils.hs view
@@ -0,0 +1,50 @@+{-# LANGUAGE ScopedTypeVariables #-} + +-- | Utilities for root isolation +module Math.Root.Isolation.Utils + ( fromComponents, matMulVec, boxMidpoint ) + where + +-- base +import Prelude hiding ( unzip ) +import Data.Foldable + ( toList ) +import Data.Functor + ( unzip ) + +-- brush-strokes +import Math.Interval +import Math.Linear + +-------------------------------------------------------------------------------- + +fromComponents + :: forall n + . ( Representable 𝕀 ( 𝕀ℝ n ), n ~ RepDim ( 𝕀ℝ n ) ) + => ( Fin n -> [ ( 𝕀, Bool ) ] ) -> [ ( 𝕀ℝ n, Vec n Bool ) ] +fromComponents f = do + ( xs, bs ) <- unzip <$> traverse f ( universe @n ) + return $ ( tabulate $ \ i -> xs ! i, bs ) + -- TODO: this could be more efficient. +{-# INLINEABLE fromComponents #-} + +-- | The midpoint of a box. +boxMidpoint :: ( Representable 𝕀 ( 𝕀ℝ n ), Representable Double ( ℝ n ) ) => 𝕀ℝ n -> ℝ n +boxMidpoint box = + tabulate $ \ i -> midpoint ( box `index` i ) +{-# INLINEABLE boxMidpoint #-} + +-- | Matrix multiplication \( A v \). +matMulVec + :: forall n m + . ( Representable 𝕀 ( 𝕀ℝ n ), Representable 𝕀 ( 𝕀ℝ m ) + , Representable Double ( ℝ n ), Representable Double ( ℝ m ) + ) + => Vec m ( ℝ n ) -- ^ columns of the matrix \( A ) + -> 𝕀ℝ m -- ^ vector \( v \) + -> 𝕀ℝ n +matMulVec as v = tabulate $ \ r -> + sum [ scaleInterval ( a `index` r ) ( index v c ) + | ( c, a ) <- toList ( (,) <$> universe @m <*> as ) + ] +{-# INLINEABLE matMulVec #-}
+ src/lib/Math/Roots.hs view
@@ -0,0 +1,627 @@+{-# LANGUAGE DuplicateRecordFields #-} +{-# LANGUAGE ScopedTypeVariables #-} + +{-# OPTIONS_GHC -Wno-name-shadowing #-} + +module Math.Roots + ( -- * Quadratic solver + solveQuadratic + + -- * Laguerre method + , roots, realRoots + , laguerre + + -- * Newton–Raphson + , newtonRaphson + + -- * Modified Halley's method M2 + , halleyM2 + + -- * N-dimensional Newton's method + , newton + ) + where + +-- base +import Control.Monad + ( unless, when ) +import Control.Monad.ST + ( ST, runST ) +import Data.Complex + ( Complex(..), conjugate, magnitude, realPart, imagPart ) +import Data.Maybe + ( mapMaybe ) +import GHC.TypeNats + ( KnownNat ) + +-- deepseq +import Control.DeepSeq + ( NFData, force ) + +-- eigen +import qualified Eigen.Matrix as Eigen + ( Matrix ) +import qualified Eigen.Solver.LA as Eigen + ( Decomposition(..), solve ) + +-- fp-ieee +import Numeric.Floating.IEEE.NaN + ( RealFloatNaN(copySign) ) + +-- primitive +import Control.Monad.Primitive + ( PrimMonad(PrimState) ) +import Data.Primitive.PrimArray + ( PrimArray, MutablePrimArray + , primArrayFromList + , getSizeofMutablePrimArray, sizeofPrimArray + , unsafeThawPrimArray + , shrinkMutablePrimArray, readPrimArray, writePrimArray + ) +import Data.Primitive.Types + ( Prim ) + +-- rounded-hw +import Numeric.Rounded.Hardware.Internal + ( fusedMultiplyAdd ) + +-- brush-strokes +import Math.Algebra.Dual +import Math.Epsilon + ( epsilon, nearZero ) +import Math.Linear +import Math.Linear.Solve + ( fromEigen, toEigen, withEigenSem ) +import Math.Module +import Math.Monomial + +-------------------------------------------------------------------------------- + +-- | Real solutions to a quadratic equation. +-- +-- Coefficients are given in order of increasing degree. +-- +-- Implementation taken from https://pbr-book.org/4ed/Utilities/Mathematical_Infrastructure#Quadratic. +solveQuadratic + :: forall a. RealFloatNaN a + => a -- ^ constant coefficient + -> a -- ^ linear coefficient + -> a -- ^ quadratic coefficient + -> [ a ] +solveQuadratic c b a + | isNaN a || isNaN b || isNaN c + || isInfinite a || isInfinite b || isInfinite c + = [] + | otherwise + = case (a == 0, c == 0) of + -- First, handle all cases in which a or c is zero. + + -- bx = 0 + (True , True ) + | b == 0 + -> [ 0, 0.5, 1 ] -- convention + | otherwise + -> [ 0 ] + -- bx + c = 0 + (True , False) + | b == 0 + -> [ ] + | otherwise + -> [ -c / b ] + -- ax² + bx = 0 + (False, True ) + | b == 0 + -> [ 0 ] + | signum a == signum b + -> [ -b / a, 0 ] + | otherwise + -> [ 0, -b / a ] + -- General case: ax² + bx + c = 0 + (False, False) + | discr < 0 + -> [] + | otherwise + -> let rootDiscr = sqrt discr + q = -0.5 * ( b + copySign rootDiscr b ) + x1 = q / a + x2 = c / q + in if x1 > x2 + then [ x2, x1 ] + else [ x1, x2 ] + where discr = discriminant a b c +{-# INLINEABLE solveQuadratic #-} +{-# SPECIALISE solveQuadratic @Float #-} +{-# SPECIALISE solveQuadratic @Double #-} +-- TODO: implement the version from the paper +-- "The Ins and Outs of Solving Quadratic Equations with Floating-Point Arithmetic" +-- which is even more robust. + +-- | Kahan's method for computing the discriminant \( b^2 - 4ac \), +-- using a fused multiply-add operation to avoid cancellation in the naive +-- formula (if \( b^2 \) and \( 4ac \) are close). +-- +-- From "The Ins and Outs of Solving Quadratic Equations with Floating-Point Arithmetic", +-- (Frédéric Goualard, 2023). +discriminant :: RealFloat a => a -> a -> a -> a +discriminant a b c + -- b² and 4ac are different enough that b² - 4ac gives a good answer + | 3 * abs d_naive >= b² - m4ac + = d_naive + | otherwise + = let dp = fma b b -b² + dq = fma ( 4 * a ) c m4ac + in d_naive + ( dp - dq ) + where + b² = b * b + m4ac = -4 * a * c + d_naive = b² + m4ac + fma = fusedMultiplyAdd +{-# INLINEABLE discriminant #-} +{-# SPECIALISE discriminant @Float #-} +{-# SPECIALISE discriminant @Double #-} + +-------------------------------------------------------------------------------- +-- Root finding using Laguerre's method + +-- | Find real roots of a polynomial with real coefficients. +-- +-- Coefficients are given in order of decreasing degree, e.g.: +-- x² + 7 is given by [ 1, 0, 7 ]. +realRoots :: forall a. ( RealFloat a, Prim a, NFData a ) => Int -> [ a ] -> [ a ] +realRoots maxIters coeffs = mapMaybe isReal ( roots epsilon maxIters coeffs ) + where + isReal :: Complex a -> Maybe a + isReal ( a :+ b ) + | abs b < epsilon = Just a + | otherwise = Nothing + +-- | Compute all roots of a polynomial with real coefficients using Laguerre's method and (forward) deflation. +-- +-- Polynomial coefficients are given in order of descending degree (e.g. constant coefficient last). +-- +-- N.B. The forward deflation process is only guaranteed to be numerically stable +-- if Laguerre's method finds roots in increasing order of magnitude +-- (which this function attempts to do). +roots :: forall a. ( RealFloat a, Prim a, NFData a ) => a -> Int -> [ a ] -> [ Complex a ] +roots eps maxIters coeffs = runST do + let + coeffPrimArray :: PrimArray a + coeffPrimArray = primArrayFromList coeffs + sz :: Int + !sz = sizeofPrimArray coeffPrimArray + n :: Int + !n = sz - 1 + ( p :: MutablePrimArray s a ) <- unsafeThawPrimArray coeffPrimArray + let + go :: Int -> [ Complex a ] -> ST s [ Complex a ] + go 0 rs = return rs + go d rs = do + -- Start at 0, attempting to find the root with smallest absolute value. + -- This improves numerical stability of the forward deflation process. + !r <- force <$> laguerre eps maxIters p 0 + if d == 1 + then pure ( r : rs ) + else + -- real root + if abs ( imagPart r ) < epsilon + then do + deflate ( realPart r ) p + go ( d - 1 ) ( r : rs ) + else do + deflateConjugatePair r p + go ( d - 2 ) ( r : conjugate r : rs ) + go n [] + +-- | Forward deflation of a polynomial by a root: factors out the root. +-- +-- Mutates the input polynomial. +deflate :: forall a m s. ( Num a, Prim a, PrimMonad m, s ~ PrimState m ) => a -> MutablePrimArray s a -> m () +deflate r p = do + deg <- subtract 1 <$> getSizeofMutablePrimArray p + when ( deg >= 2 ) do + shrinkMutablePrimArray p deg + let + go :: a -> Int -> m () + go !b !i = unless ( i >= deg ) do + ai <- readPrimArray p i + let + bi :: a + !bi = ai + r * b + writePrimArray p i bi + go bi ( i + 1 ) + a0 <- readPrimArray p 0 + go a0 1 + +-- | Forward deflation of a polynomial with real coefficients by a pair of complex-conjugate roots. +-- +-- Mutates the input polynomial. +deflateConjugatePair :: forall a m s. ( Num a, Prim a, PrimMonad m, s ~ PrimState m ) => Complex a -> MutablePrimArray s a -> m () +deflateConjugatePair ( x :+ y ) p = do + deg <- subtract 1 <$> getSizeofMutablePrimArray p + when ( deg >= 3 ) do + shrinkMutablePrimArray p ( deg - 1 ) + let + c1, c2 :: a + !c1 = 2 * x + !c2 = x * x + y * y + a0 <- readPrimArray p 0 + a1 <- readPrimArray p 1 + let + b1 :: a + !b1 = a1 + c1 * a0 + writePrimArray p 1 b1 + let + go :: a -> a -> Int -> m () + go !b !b' !i = unless ( i >= deg - 1 ) do + ai <- readPrimArray p i + let + bi :: a + !bi = ai + c1 * b - c2 * b' + writePrimArray p i bi + go bi b ( i + 1 ) + go b1 a0 2 + +-- | Laguerre's method. +-- +-- Does not perform any mutation. +laguerre + :: forall a m s + . ( RealFloat a, Prim a, PrimMonad m, s ~ PrimState m ) + => a -- ^ error tolerance + -> Int -- ^ max number of iterations + -> MutablePrimArray s a -- ^ polynomial + -> Complex a -- ^ initial point + -> m ( Complex a ) +laguerre eps maxIters p x0 = do + let + go :: Int -> Complex a -> m ( Complex a ) + go !stepNo !x = do + x' <- laguerreStep stepNo p x + if stepNo >= maxIters || magnitude ( x' - x ) < eps + then pure x' + else go ( stepNo + 1 ) x' + go 1 x0 + +-- | Take a single step in Laguerre's method. +-- +-- Does not perform any mutation. +laguerreStep + :: forall a m s + . ( RealFloat a, Prim a, PrimMonad m, s ~ PrimState m ) + => Int -- ^ step number + -> MutablePrimArray s a -- ^ polynomial + -> Complex a -- ^ initial point + -> m ( Complex a ) +laguerreStep stepNo p x = do + sz <- getSizeofMutablePrimArray p + let + n :: a + !n = fromIntegral $ sz - 1 + ( px, p'x, p''x ) <- evalDerivatives p x + if magnitude px == 0 + then pure x + else do + let + !g = p'x / px + !g² = g * g + !h = g² - p''x / px + !m = multiplicityEstimator n ( g² / h ) + !delta = sqrt $ ( ( n - m ) / m ) *: ( n *: h - g² ) + !gp = g + delta + !gm = g - delta + !denom + | sqMag gm > sqMag gp + = gm + | otherwise + = gp + pure $ + if sqMag denom == 0 + then x + ( 0.001 :+ 0.001 ) -- random perturbation to escape saddle point + else x - ( n :+ 0 ) / denom + + where + (*:) :: a -> Complex a -> Complex a + r *: (u :+ v) = ( r * u ) :+ ( r * v ) + + sqMag :: Complex a -> a + sqMag (u :+ v) = u * u + v * v + + multiplicityEstimator :: a -> Complex a -> a + multiplicityEstimator n est + + -- Burn-in period: unmodified Laguerre for the first few steps + | stepNo < 5 + + -- Don't use an estimator if it is NaN/Infinity + || isNaN realEst || isInfinite realEst + || isNaN imagEst || isInfinite imagEst + + -- Only use an estimate if it is near an integer in [1..n] + || sqMag ( est - ( rounded :+ 0 ) ) > 0.01 + || rounded >= n + 0.5 + || rounded <= 1 + = 1 + + | otherwise + = rounded + where + realEst, imagEst, rounded :: a + realEst = realPart est + imagEst = imagPart est + rounded = fromIntegral ( round realEst :: Int ) + +-- | Evaluate @p(x)@, @p'(x)@ and @p''(x)@, for a polynomial with real +-- coefficients @p@, at a complex number. +-- +-- Does not perform any mutation. +evalDerivatives + :: forall a m s + . ( RealFloat a, Prim a, PrimMonad m, s ~ PrimState m ) + => MutablePrimArray s a + -> Complex a + -> m ( Complex a, Complex a, Complex a ) +evalDerivatives p x = do + n <- getSizeofMutablePrimArray p + a0 <- readPrimArray p 0 + let + -- Generalised Horner's method + go :: Int -> Complex a -> Complex a -> Complex a -> m ( Complex a, Complex a, Complex a ) + go !i !px !dpx !ddpx + | i >= n + = pure (px, dpx, ddpx * 2) -- Multiply second derivative by 2 at the end + | otherwise + = do + ai <- readPrimArray p i + let + !ddpx' = ddpx * x + dpx + !dpx' = dpx * x + px + !px' = px * x + (ai :+ 0) + go (i + 1) px' dpx' ddpx' + go 1 (a0 :+ 0) 0 0 + +{-# SPECIALISE maxRealFloat @Float #-} +{-# SPECIALISE maxRealFloat @Double #-} +maxRealFloat :: forall r. RealFloat r => r +maxRealFloat = encodeFloat m n + where + !b = floatRadix @r 0 + !e = floatDigits @r 0 + !(_, !e') = floatRange @r 0 + !m = b ^ e - 1 + !n = e' - e + +{- +for some reason GHC isn't able to optimise maxRealFloat @Double into + +maxDouble :: Double +maxDouble = D# 1.7976931348623157e308## +-} + +-------------------------------------------------------------------------------- +-- Newton–Raphson + +-- | Newton–Raphson root-finding with Armijo backtracking line search and +-- quadratic interpolation fallback. +newtonRaphson + :: forall r + . ( RealFloat r, Show r ) + => Word -- ^ maximum number of iterations + -> Int -- ^ desired digits of precision + -> ( r -> (# r, r #) ) -- ^ function and its derivative + -> r -- ^ initial guess + -> Maybe r +newtonRaphson maxIters digits f x0 = + let (# f_x0, f'_x0 #) = f x0 + in go 0 x0 f_x0 f'_x0 + where + !factor = encodeFloat 1 ( 1 - digits ) + + go :: Word -> r -> r -> r -> Maybe r + go !iters !x !f_x !f'_x + | f_x == 0 + = Just x + | iters >= maxIters || f'_x == 0 -- trapped at a local extremum + = Nothing + | otherwise + = case newtonRaphsonStep factor f x f_x f'_x of + (# done | #) -> done + (# | (# x', fx', f'x' #) #) -> + go ( iters + 1 ) x' fx' f'x' +{-# SPECIALISE newtonRaphson @Float #-} +{-# SPECIALISE newtonRaphson @Double #-} +{-# INLINEABLE newtonRaphson #-} + +newtonRaphsonStep + :: ( RealFloat r, Show r ) + => r + -> ( r -> (# r, r #) ) + -> r -> r -> r + -> (# Maybe r | (# r, r, r #) #) +newtonRaphsonStep !factor f !x !f_x !f'_x = lineSearch 1 + where + -- Armijo "sufficient decrease" parameter + !c1 = 1e-4 + + !g_0 = f_x * f_x + !p = -f_x / f'_x + -- Directional derivative of f(x)^2 along the Newton step + !slope = -2 * g_0 + + -- Backtracking line search + lineSearch !α + + -- Termination criteria: + -- 1. Close enough to a solution. + | abs δ <= max epsilon ( abs ( x' * factor ) ) + = (# Just x' | #) + -- 2. α got too small, the algorithm has stalled + | α < 1e-4 + = (# Nothing | #) + + -- Otherwise, either accept step or shrink α. + | otherwise + = let + !(# f_x', f'_x' #) = f x' + !g_α = f_x' * f_x' + in + -- Armijo condition for accepting step + if g_α <= g_0 + c1 * α * slope + then (# | (# x', f_x', f'_x' #) #) + else + -- Quadratic interpolation fallback + let !α_tmp = ( g_0 * α * α ) / ( g_α - g_0 + 2 * g_0 * α ) + -- Clamp α to [0.1 * α, 0.5 * α] to prevent numerical instability + !α' = max ( 0.1 * α ) ( min ( 0.5 * α ) α_tmp ) + in lineSearch α' + where + !δ = α * p + !x' = x + δ +{-# SPECIALISE newtonRaphsonStep @Float #-} +{-# SPECIALISE newtonRaphsonStep @Double #-} +{-# INLINEABLE newtonRaphsonStep #-} + +-------------------------------------------------------------------------------- +-- Modified Halley method + +-- | Take a single step in the M2 modified Halley method. +-- +-- Taken from @Some variants of Halley’s method with memory and their applications for solving several chemical problems@ +-- by A. Cordero, H. Ramos & J.R. Torregrosa, J Math Chem 58, 751–774 (2020). +-- +-- @https://doi.org/10.1007/s10910-020-01108-3@ +halleyM2Step + :: ( RealFloat a, Show a ) + => (# a, (# a, a #) #) -> (# a, (# a, a #) #) -> a +halleyM2Step (# x_nm1, (# f_nm1, f'_nm1 #) #) (# x_n, (# f_n, f'_n #) #) + | nearZero dx + = if nearZero f'_n + then x_n -- Stalled: Derivative is zero, cannot take a Newton step + else x_n - ( f_n / f'_n ) -- simple Newton step fallback + | nearZero num && nearZero denom + = 0.001 * signum num * signum denom + | nearZero num + = num + | otherwise + = num / denom + where + !u = f_n * f_nm1 * (f_n - f_nm1) + !dx = x_n - x_nm1 + !g1 = f_nm1 ^ ( 2 :: Int ) * f'_n + !g2 = f_n ^ ( 2 :: Int ) * f'_nm1 + !num = (x_n + x_nm1) * u - dx * ( g1 * x_n + g2 * x_nm1) + !denom = 2 * u - dx * ( g1 + g2 ) + +{-# SPECIALISE halleyM2Step @Float #-} +{-# SPECIALISE halleyM2Step @Double #-} +{-# INLINEABLE halleyM2Step #-} + +{-# SPECIALISE halleyM2 @Float #-} +{-# SPECIALISE halleyM2 @Double #-} +{-# INLINEABLE halleyM2 #-} +halleyM2 + :: forall r + . ( RealFloat r, Show r ) + => Word -- ^ maximum number of iterations + -> Int -- ^ desired digits of precision + -> ( r -> (# r, r #) ) -- ^ function and its derivative + -> r -- ^ initial guess + -> Maybe r +halleyM2 maxIters digits f x0 = + let + y0 = (# x0, df_x0 #) + !df_x0@(# f_x0, f'_x0 #) = f x0 + !factor = encodeFloat 1 ( 1 - digits ) + + -- Start off with a Newton step to get the second initial x value. + in case newtonRaphsonStep factor f x0 f_x0 f'_x0 of + -- Newton converged immediately on the first step + (# Just x1 | #) -> Just x1 + + -- Newton stalled immediately (e.g., zero derivative) + (# Nothing | #) -> Nothing + + -- Newton took a successful step, we have x1. Proceed with Halley M2. + (# | (# x1, f_x1, f'_x1 #) #) -> + let y1 = (# x1, (# f_x1, f'_x1 #) #) + in go 0 y0 y1 + where + !factor = encodeFloat 1 ( 1 - digits ) + + go :: Word -> (# r, (# r, r #) #) -> (# r, (# r, r #) #) -> Maybe r + go i y_nm1 y_n@(# x_n, _ #) = + let !x_np1 = halleyM2Step y_nm1 y_n + in + if + | i >= maxIters + || abs ( x_np1 - x_n ) <= abs ( x_n * factor ) + -> Just x_np1 + | otherwise + -> go (i+1) y_n (# x_np1, f x_np1 #) + +-------------------------------------------------------------------------------- + +-- | @n@-dimensional Newton's method. +newton + :: forall n d + . ( KnownNat n, KnownNat d + , Show ( ℝ n ), Show ( ℝ d ) + , NFData ( ℝ n ) + , Module Double ( T ( ℝ n ) ) + , MonomialBasis ( D 1 n ), Deg ( D 1 n ) ~ 1, Vars ( D 1 n ) ~ n + , Representable Double ( ℝ d ) + , Representable Double ( ℝ n ) + ) + => ( ℝ n -> D 1 n ( ℝ d ) ) + -- ^ equations + -> ℝ n + -- ^ initial guess + -> ℝ n +newton f x0 = go 0 x0 + where + maxIters :: Int + maxIters = 40 + relError, absError :: Double + relError = 1e-9 + absError = 1e-11 + + go :: Int -> ℝ n -> ℝ n + go !i !x + | i >= maxIters + = x + | norm ( unT δ ) < absError + relError * norm x + = x' + | otherwise + = go ( i + 1 ) x' + where + ( _norm_fx, δ ) = newtonStep x + x' = unT $ T x ^-^ δ + + norm :: forall i. Representable Double ( ℝ i ) => ℝ i -> Double + norm x = + sqrt $ sum ( ( \ x -> x * x ) <$> coordinates x ) + {-# INLINEABLE norm #-} + + newtonStep :: ℝ n -> ( Double, T ( ℝ n ) ) + newtonStep x = + let df_x = f x + f_x :: ℝ d + f_x = df_x `monIndex` zeroMonomial + j_x :: Vec n ( ℝ d ) + j_x = fmap ( \ i -> df_x `monIndex` linearMonomial i ) ( universe @n ) + δ :: ℝ n + !δ = withEigenSem $ + ( \ ( Vec cols ) -> case cols of [ !c ] -> c; _ -> error "internal error in Newton's method" ) $ + fromEigen $ + Eigen.solve Eigen.JacobiSVD + ( toEigen j_x :: Eigen.Matrix d n Double ) + ( toEigen ( Vec [ f_x ] ) :: Eigen.Matrix d 1 Double ) + in ( norm f_x, T δ ) + +{-# INLINEABLE newton #-} +{-# SPECIALISE newton @1 @1 #-} +{-# SPECIALISE newton @1 @2 #-} +{-# SPECIALISE newton @1 @3 #-} +{-# SPECIALISE newton @2 @2 #-} +{-# SPECIALISE newton @2 @3 #-} + +-- TODO: implement the W4 method?
+ src/lib/Math/Taylor.hs view
@@ -0,0 +1,933 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE PolyKinds #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE UndecidableInstances #-} + +module Math.Taylor where + +-- base +import Prelude + hiding ( Num(..), (/), fromRational ) +import Control.Monad.ST + ( ST, runST ) +import Data.Kind + ( Type, Constraint ) +import Data.Ratio + ( (%) ) +import Data.Semigroup + ( Product(..) ) +import GHC.Exts + ( Double(D#), IsList(..) + , proxy#, fmaddDouble# + ) +import GHC.TypeNats + ( Nat, KnownNat + , natVal' + , type (+), type (-) + ) + +-- primitive +import qualified Data.Primitive.Array as Prim +import qualified Data.Primitive.PrimArray as Prim + +-- brush-strokes +import Math.Interval + ( 𝕀(𝕀), 𝕀ℝ + , hull, addWithErr, subWithErr, prodWithErr, scaleInterval + ) +import Math.Linear + ( ℝ, Fin (..), Representable (index) ) +import Math.Ring +import Math.Root.Isolation.Core + ( forEachCoord ) + +-------------------------------------------------------------------------------- + +-- | The shape of a tensor of the given order. +type Shape :: Nat -> Type +data Shape order where + Nil :: Shape 0 + (:.) :: Nat -> Shape ( n - 1 ) -> Shape n + +type KnownShape :: forall {order}. Shape order -> Constraint +class KnownShape shape where + knownShape :: [ Int ] +instance KnownShape Nil where + knownShape = [] +instance ( KnownNat n, KnownShape sh ) => KnownShape ( n :. sh ) where + knownShape = fromIntegral ( natVal' @n proxy# ) : knownShape @sh + +nbCoefficients :: forall sh. KnownShape sh => Int +nbCoefficients = getProduct $ foldMap ( Product . nbCoeffs ) ( knownShape @sh ) + where + -- A degree d polynomial has (d+1) coefficients. + nbCoeffs :: Int -> Int + nbCoeffs deg = deg + 1 + +-- | A Taylor model in Bernstein form, consisting of a multivariate polynomial +-- given as a Bernstein tensor + an error term (interval). +-- +-- Invariant: the interval always contains @0@ (although it is not necessarily +-- symmetric around @0@). +type BernsteinModel :: Shape order -> Type +data BernsteinModel degs + = BernsteinModel + { bernsteinCoefficients :: !( Prim.PrimArray Double ) + -- Tensor coefficients, stored in row-major order. + , bernsteinModelRemainder :: !𝕀 + } + +instance KnownShape degs => AbelianGroup ( BernsteinModel degs ) where + BernsteinModel c1 e1 + BernsteinModel c2 e2 = + let (fpErr, cNew) = zipWithCoeffs addWithErr c1 c2 + in BernsteinModel cNew (e1 + e2 + fpErr) + + BernsteinModel c1 e1 - BernsteinModel c2 e2 = + let (fpErr, cNew) = zipWithCoeffs subWithErr c1 c2 + in BernsteinModel cNew (e1 + e2 + fpErr) + + negate ( BernsteinModel coeffs err ) = + -- Floating-point negation is exact, so no change in error. + BernsteinModel ( Prim.mapPrimArray negate coeffs ) ( negate err ) + fromInteger i = + case fromInteger i of + 𝕀 lo hi -> + if lo == hi + then + BernsteinModel + ( Prim.replicatePrimArray ( nbCoefficients @degs ) ( fromIntegral i ) ) + 0 + else + let !x = 0.5 * ( lo + hi ) + in + BernsteinModel + ( Prim.replicatePrimArray ( nbCoefficients @degs ) x ) + ( 𝕀 ( lo - x ) ( hi - x ) ) + +-- | Bernstein coefficient strides (assuming a row-major layout). +getStrides :: [ Int ] -> [ Int ] +getStrides shape = + let degs = map (+1) shape + in drop 1 $ scanr (*) 1 degs + +choose :: Int -> Int -> Integer +choose n k + | k < 0 || k > n = 0 + | k == 0 || k == n = 1 + | k > n `div` 2 = choose n (n - k) + | otherwise = go 1 1 + where + n', k' :: Integer + n' = fromIntegral n + k' = fromIntegral k + go :: Integer -> Integer -> Integer + go acc i + | i > k' = acc + | otherwise = go (acc * (n' - i + 1) `div` i) (i + 1) + +-- | Compute the Bernstein weight \( \frac { { n \choose i } { m \choose j } }{ { {n + m} \choose { i + j } } } \). +bernsteinWeight :: Int -> Int -> Int -> Int -> 𝕀 +bernsteinWeight n m i j = + let + -- Compute the exact rational using integer arithmetic, + -- then use 'fromRational'. This avoids any accumulation of floating-point + -- errors, at the cost of using large integers. + num = choose n i * choose m j + den = choose (n + m) (i + j) + + in + -- NB: the 'fromRational' implementation for 𝕀 produces an enclosure of + -- the exact rational number by 'Double's. + fromRational $ num % den + +-- | Helper function for computing Bernstein weights in the computation of +-- multiplication of two Bernstein models. +bernsteinWeights + :: forall { order } ( degs1 :: Shape order ) ( degs2 :: Shape order ) + . ( KnownNat order, KnownShape degs1, KnownShape degs2 ) + => ( Prim.Array ( Prim.PrimArray Double ) + , Prim.Array ( Prim.PrimArray 𝕀 ) + ) +bernsteinWeights = + let + !dims1 = fromList $ knownShape @degs1 + !dims2 = fromList $ knownShape @degs2 + !order = fromIntegral $ natVal' @order proxy# + in runST \ @s -> do + marrBoxedVal <- Prim.newArray @( ST s ) @( Prim.PrimArray Double ) order undefined + marrBoxedErr <- Prim.newArray @( ST s ) @( Prim.PrimArray 𝕀 ) order undefined + + forLoop 0 order \ d -> do + let + n = Prim.indexPrimArray dims1 d + m = Prim.indexPrimArray dims2 d + rowSize = m + 1 + totalSize = (n + 1) * rowSize + + marrWeightsVal <- Prim.newPrimArray @( ST s ) @Double totalSize + marrWeightsErr <- Prim.newPrimArray @( ST s ) @𝕀 totalSize + + forLoop 0 ( n + 1 ) \ i -> do + let + !offsetI = i * rowSize + forLoop 0 ( m + 1 ) \ j -> do + let + !( 𝕀 lo hi ) = bernsteinWeight n m i j + !val = 0.5 * ( lo + hi ) + !err = 𝕀 ( lo - val ) ( hi - val ) + + Prim.writePrimArray marrWeightsVal (offsetI + j) val + Prim.writePrimArray marrWeightsErr (offsetI + j) err + + frozenVal <- Prim.unsafeFreezePrimArray marrWeightsVal + frozenErr <- Prim.unsafeFreezePrimArray marrWeightsErr + + Prim.writeArray marrBoxedVal d frozenVal + Prim.writeArray marrBoxedErr d frozenErr + + weightsVal <- Prim.unsafeFreezeArray marrBoxedVal + weightsErr <- Prim.unsafeFreezeArray marrBoxedErr + + return ( weightsVal, weightsErr ) + +type ZipSum :: Shape order -> Shape order -> Shape order +type family ZipSum as bs where + ZipSum Nil Nil = Nil + ZipSum (a :. as) (b :. bs) = (a + b) :. ZipSum as bs + +-- | Multiply two Bernstein models (without any degree reduction). +timesBernsteinModel + :: forall { order } ( degs1 :: Shape order ) ( degs2 :: Shape order ) + . ( KnownNat order + , KnownShape degs1, KnownShape degs2 + , KnownShape ( ZipSum degs1 degs2 ) + ) + => BernsteinModel degs1 + -> BernsteinModel degs2 + -> BernsteinModel ( ZipSum degs1 degs2 ) +timesBernsteinModel = + let + -- 1. Setup constants + !env = ConvolutionEnv + { envOrder = fromIntegral $ natVal' @order proxy# + , envDims1 = fromList $ knownShape @degs1 + , envDims2 = fromList $ knownShape @degs2 + , envDimsOut = fromList $ knownShape @( ZipSum degs1 degs2 ) + , envStrides1 = fromList $ getStrides $ knownShape @degs1 + , envStrides2 = fromList $ getStrides $ knownShape @degs2 + , envStridesOut = fromList $ getStrides $ knownShape @( ZipSum degs1 degs2 ) + , envWeightsVal = fst $ bernsteinWeights @degs1 @degs2 + , envWeightsErr = snd $ bernsteinWeights @degs1 @degs2 + } + !nbOut = nbCoefficients @( ZipSum degs1 degs2 ) + + in + \ (BernsteinModel coeffs1 err1) (BernsteinModel coeffs2 err2) -> + runST $ do + + -- 2. Initialise output coefficient array & output FP-error array + marrVal <- Prim.newPrimArray nbOut + marrErr <- Prim.newPrimArray nbOut + Prim.setPrimArray marrVal 0 nbOut 0 + Prim.setPrimArray marrErr 0 nbOut 0 + + -- 3. Run the gathering weighted convolution + runOutputWalker env coeffs1 coeffs2 marrVal marrErr + + -- 4. Compute the final error term and finish. + resultCoeffs <- Prim.unsafeFreezePrimArray marrVal + fpErrs <- Prim.unsafeFreezePrimArray marrErr + + let + !( 𝕀 min1 max1 ) = minMaxCoeffs coeffs1 + !( 𝕀 min2 max2 ) = minMaxCoeffs coeffs2 + !bound1 = max (abs min1) (abs max1) + !bound2 = max (abs min2) (abs max2) + !properErr = scaleInterval bound2 err1 + scaleInterval bound1 err2 + err1 * err2 + !finalErr = properErr + hulls fpErrs + + return $ BernsteinModel resultCoeffs finalErr +{-# INLINEABLE timesBernsteinModel #-} + +data ConvolutionEnv = ConvolutionEnv + { envOrder :: !Int + , envDims1 :: !( Prim.PrimArray Int ) + , envDims2 :: !( Prim.PrimArray Int ) + , envDimsOut :: !( Prim.PrimArray Int ) + , envStrides1 :: !( Prim.PrimArray Int ) + , envStrides2 :: !( Prim.PrimArray Int ) + , envStridesOut :: !( Prim.PrimArray Int ) + , envWeightsVal :: !( Prim.Array ( Prim.PrimArray Double ) ) + , envWeightsErr :: !( Prim.Array ( Prim.PrimArray 𝕀 ) ) + } + +data FiberBuffer s = FiberBuffer + { fbVal :: !( Prim.MutablePrimArray s Double ) + , fbErr :: !( Prim.MutablePrimArray s 𝕀 ) + } + +-- | Lightweight grouping for the current traversal state. +data Cursor = Cursor + { curOff1 :: !Int -- ^ Current offset in input 1 + , curOff2 :: !Int -- ^ Current offset in input 2 + , curWVal :: !Double -- ^ Accumulated Bernstein weight product + , curWErr :: !𝕀 -- ^ Accumulated weight error + } + +runOutputWalker + :: forall s + . ConvolutionEnv + -> Prim.PrimArray Double -- ^ Coeffs 1 + -> Prim.PrimArray Double -- ^ Coeffs 2 + -> Prim.MutablePrimArray s Double -- ^ Output Coeffs + -> Prim.MutablePrimArray s 𝕀 -- ^ Output Errors + -> ST s () +{-# INLINE runOutputWalker #-} +runOutputWalker env c1 c2 marrVal marrErr = + if envOrder env == 0 + then do + -- Trivial scalar codepath + let !val1 = Prim.indexPrimArray c1 0 + !val2 = Prim.indexPrimArray c2 0 + !(# pVal, pErr #) = weightedProduct val1 val2 1 0 + oldVal <- Prim.readPrimArray marrVal 0 + oldErr <- Prim.readPrimArray marrErr 0 + let !(# newVal, sumErr #) = addWithErr oldVal pVal + !newErr = oldErr + pErr + sumErr + Prim.writePrimArray marrVal 0 newVal + Prim.writePrimArray marrErr 0 newErr + else do + + let lastDimLen = Prim.indexPrimArray (envDimsOut env) (envOrder env - 1) + + -- Allocate reusable buffers for the fiber dimension + tmpVal <- Prim.newPrimArray lastDimLen + tmpErr <- Prim.newPrimArray lastDimLen + let fiberBuf = FiberBuffer tmpVal tmpErr + + -- Array to track output indices (k) for prefix dimensions + kIndices <- Prim.newPrimArray (envOrder env) + + -- Iterate over every output fiber + iterateOutputFibers env kIndices 0 0 $ \outputOffset -> do + + -- A. Reset the temporary accumulation buffer + Prim.setPrimArray tmpVal 0 lastDimLen 0 + Prim.setPrimArray tmpErr 0 lastDimLen 0 + + -- B. Accumulate all input contributions into the buffer + let startCursor = Cursor 0 0 1 0 + accumulateFiberInputs env c1 c2 kIndices fiberBuf 0 startCursor + + -- C. Write the fiber into the main output array + commitFiber marrVal marrErr fiberBuf outputOffset + +-- | Iterates over all output indices for dimensions 0 to N-2. +-- Calls the 'action' callback when a fiber (dimension N-1) is identified. +iterateOutputFibers + :: ConvolutionEnv + -> Prim.MutablePrimArray s Int -- ^ current output indices + -> Int -- ^ current depth + -> Int -- ^ current output offset + -> ( Int -> ST s () ) -- ^ action to run on the fiber (receives offset) + -> ST s () +{-# INLINE iterateOutputFibers #-} +iterateOutputFibers env kIndices depth offOut action + | depth == envOrder env - 1 = action offOut + | otherwise = do + let + !outDeg = Prim.indexPrimArray (envDimsOut env) depth + !stride = Prim.indexPrimArray (envStridesOut env) depth + + -- Loop over k for this dimension + forLoop 0 ( outDeg + 1 ) \ k -> do + Prim.writePrimArray kIndices depth k + iterateOutputFibers env kIndices (depth + 1) (offOut + k * stride) action + +-- | Finds all (i, j) pairs such that i + j = k for the prefix dimensions. +-- Updates the Cursor (offsets and weights) and recurses. +accumulateFiberInputs + :: ConvolutionEnv + -> Prim.PrimArray Double -- ^ argument 1 coefficients + -> Prim.PrimArray Double -- ^ argument 2 coefficients + -> Prim.MutablePrimArray s Int -- ^ output indices + -> FiberBuffer s + -> Int -- ^ current depth + -> Cursor -- ^ current accumulated state (offsets/weights) + -> ST s () +{-# INLINE accumulateFiberInputs #-} +accumulateFiberInputs env c1 c2 kIndices buf depth cur + -- Base case: run the 1D convolution. + | depth == envOrder env - 1 = + weightedConvolve1D env c1 c2 buf cur + + -- Recursive Step: match i + j = k for this dimension + | otherwise = do + targetK <- Prim.readPrimArray kIndices depth + + let + !n = Prim.indexPrimArray (envDims1 env) depth + !m = Prim.indexPrimArray (envDims2 env) depth + !s1 = Prim.indexPrimArray (envStrides1 env) depth + !s2 = Prim.indexPrimArray (envStrides2 env) depth + + -- Bernstein weights for this dimension + !wArrVal = Prim.indexArray (envWeightsVal env) depth + !wArrErr = Prim.indexArray (envWeightsErr env) depth + !wRowLen = m + 1 + + -- Valid range for i + !startI = max 0 (targetK - m) + !endI = min n targetK + + forLoop startI (endI + 1) \i -> do + let + !j = targetK - i + !wIdx = (i * wRowLen) + j + + -- Fetch weight for this (i, j) pair + !dimWVal = Prim.indexPrimArray wArrVal wIdx + !dimWErr = Prim.indexPrimArray wArrErr wIdx + + -- Update accumulated weight via product: wNew = wOld * wDim + !(# nextWVal, wProdErr #) = prodWithErr (curWVal cur) dimWVal + !nextWErr = scaleInterval (curWVal cur) dimWErr + + scaleInterval dimWVal (curWErr cur) + + (curWErr cur) * dimWErr + + wProdErr + + -- Update offsets + !nextCursor = + Cursor + { curOff1 = curOff1 cur + (i * s1) + , curOff2 = curOff2 cur + (j * s2) + , curWVal = nextWVal + , curWErr = nextWErr + } + + accumulateFiberInputs env c1 c2 kIndices buf (depth + 1) nextCursor + +-- | Performs a weighted convolution for the innermost dimension and adds +-- the result into the FiberBuffer. +weightedConvolve1D + :: ConvolutionEnv + -> Prim.PrimArray Double + -> Prim.PrimArray Double + -> FiberBuffer s + -> Cursor + -> ST s () +{-# INLINE weightedConvolve1D #-} +weightedConvolve1D env coeffs1 coeffs2 (FiberBuffer bufVal bufErr) (Cursor off1 off2 wVal wErr) = do + let + !lastDim = envOrder env - 1 + !n = Prim.indexPrimArray (envDims1 env) lastDim + !m = Prim.indexPrimArray (envDims2 env) lastDim + !s1 = Prim.indexPrimArray (envStrides1 env) lastDim + !s2 = Prim.indexPrimArray (envStrides2 env) lastDim + + !wArrVal = Prim.indexArray (envWeightsVal env) lastDim + !wArrErr = Prim.indexArray (envWeightsErr env) lastDim + !wRowLen = m + 1 + !outDeg = n + m + + -- Standard 1D convolution + forLoop 0 (outDeg + 1) \k -> do + let + !startI = max 0 (k - m) + !endI = min n k + + -- Inner reduction loop: sum( c1[i]*c2[j]*w[i,j] ) + !(fiberVal, fiberErr) = + foldLoop startI (endI + 1) (0, 0) \i (accVal, accErr) -> + let + !j = k - i + + !c1 = Prim.indexPrimArray coeffs1 (off1 + i * s1) + !c2 = Prim.indexPrimArray coeffs2 (off2 + j * s2) + + !wIdx = i * wRowLen + j + !dimWVal = Prim.indexPrimArray wArrVal wIdx + !dimWErr = Prim.indexPrimArray wArrErr wIdx + + -- Compute term: (c1 * c2) * w + !(# p, pErr #) = prodWithErr c1 c2 + !(# termVal, termProdErr #) = prodWithErr p dimWVal + !termErr = scaleInterval p dimWErr + + scaleInterval dimWVal pErr + + pErr * dimWErr + + termProdErr + + !(# nextAcc, sumErr #) = addWithErr accVal termVal + !nextAccErr = accErr + termErr + sumErr + in (nextAcc, nextAccErr) + + -- Apply the prefix weight (wVal) accumulated from higher dimensions. + -- result = fiber_sum * prefix_weight + let !(# finalVal, finalProdErr #) = prodWithErr fiberVal wVal + !finalErr = scaleInterval fiberVal wErr + + scaleInterval wVal fiberErr + + wErr * fiberErr + + finalProdErr + + -- Accumulate into temporary buffer + oldBufVal <- Prim.readPrimArray bufVal k + oldBufErr <- Prim.readPrimArray bufErr k + let !(# newBufVal, bufSumErr #) = addWithErr oldBufVal finalVal + !newBufErr = oldBufErr + finalErr + bufSumErr + + Prim.writePrimArray bufVal k newBufVal + Prim.writePrimArray bufErr k newBufErr + +commitFiber + :: Prim.MutablePrimArray s Double + -> Prim.MutablePrimArray s 𝕀 + -> FiberBuffer s + -> Int + -> ST s () +{-# INLINE commitFiber #-} +commitFiber marrVal marrErr ( FiberBuffer bufVal bufErr ) outputOffset = do + len <- Prim.getSizeofMutablePrimArray bufVal + Prim.copyMutablePrimArray marrVal outputOffset bufVal 0 len + Prim.copyMutablePrimArray marrErr outputOffset bufErr 0 len + +weightedProduct + :: Double -- ^ coefficient from tensor 1 + -> Double -- ^ coefficient from tensor 2 + -> Double -- ^ accumulated weight + -> 𝕀 -- ^ accumulated weight error + -> (# Double, 𝕀 #) +{-# INLINE weightedProduct #-} +weightedProduct c1 c2 wVal wErr = + let + -- Compute ( c1 * c2 ) * w, keeping track of FP error. + !(# p, pErr #) = prodWithErr c1 c2 + !(# termVal, termProdErr #) = prodWithErr p wVal + + !termErr = scaleInterval p wErr + + scaleInterval wVal pErr + + pErr * wErr + + termProdErr + in + (# termVal, termErr #) + +fma :: Double -> Double -> Double -> Double +fma ( D# x ) ( D# y ) ( D# z ) = D# ( fmaddDouble# x y z ) + +boundingBox :: forall order ( degs :: Shape order ). BernsteinModel degs -> 𝕀 +boundingBox ( BernsteinModel coeffs err ) = + err + minMaxCoeffs coeffs + +minMaxCoeffs :: Prim.PrimArray Double -> 𝕀 +minMaxCoeffs arr = loop 0 $ 𝕀 (1/0) (-1/0) + where + len = Prim.sizeofPrimArray arr + loop !i ival@( 𝕀 mn mx ) + | i >= len = ival + | otherwise = + let !x = Prim.indexPrimArray arr i + in loop (i+1) $ 𝕀 (min mn x) (max mx x) + +hulls :: Prim.PrimArray 𝕀 -> 𝕀 +hulls arr = loop 0 $ 𝕀 (1/0) (-1/0) + where + len = Prim.sizeofPrimArray arr + loop !i ival + | i >= len = ival + | otherwise = + let !x = Prim.indexPrimArray arr i + in loop (i+1) $ hull x ival + + +refineBernsteinModel + :: forall { order } ( degs :: Shape order ) + . ( KnownNat order + , KnownShape degs + , Representable 𝕀 ( 𝕀ℝ order ) + ) + => 𝕀ℝ order -- ^ The domains @[u_i, v_i]@ for each dimension + -> BernsteinModel degs + -> BernsteinModel degs +refineBernsteinModel = + let + !dims = knownShape @degs + !maxDeg = if null dims then 0 else maximum dims + !strides = getStrides dims + in \ domains ( BernsteinModel coeffs inputErr ) -> runST \ @s -> do + + -- Allocate a scratch buffer for 'restrict1D' + scratchBuf <- Prim.newPrimArray @( ST s ) @Double ( maxDeg + 1 ) + + -- 1. Copy coefficients to a mutable array (we modify in place) + let len = Prim.sizeofPrimArray coeffs + marr <- Prim.newPrimArray len + Prim.copyPrimArray marr 0 coeffs 0 len + + + let + -- Iterate over all 1D lines for a specific dimension 'd' + -- This loops over all indices *except* dimension 'd'. + applyToDim :: Fin order -> 𝕀 -> 𝕀 -> ST s 𝕀 + applyToDim ( Fin d_one_ixed ) ( 𝕀 u v ) currentAccErr = do + let + -- Fin uses 1-based indexing (not for any good reason), so account for that + !d = fromIntegral $ d_one_ixed - 1 + !n = dims !! d + !stride = strides !! d + !dimSize = n + 1 + !block = stride * dimSize + + loopBlock !base !acc + | base >= len = return acc + | otherwise = do + let loopOffset !off !innerAcc + | off >= stride = return innerAcc + | otherwise = do + let !idx = base + off + e <- restrict1D scratchBuf ( Strided marr idx stride ) n (𝕀 u v) + loopOffset (off + 1) (innerAcc + e) + errChunk <- loopOffset 0 0 + loopBlock (base + block) (acc + errChunk) + + loopBlock 0 currentAccErr + + -- 2. Apply refinement dimension by dimension + totalRefinementErr <- + forEachCoord @order 0 \ d accErr -> do + let !dom = domains `index` d + applyToDim d dom accErr + + finalCoeffs <- Prim.unsafeFreezePrimArray marr + + -- 3. Update error + -- The accumulated floating point error from De Casteljau must be added. + return $ BernsteinModel finalCoeffs (inputErr + totalRefinementErr) + +-- | Restricts a 1D Bernstein polynomial from domain [0,1] to [u,v]. +-- +-- Returns the floating-point error incurred during the transformation. +restrict1D + :: forall s + . Prim.MutablePrimArray s Double -- ^ scratch buffer (size >= degree + 1) + -> Strided s -- coefficient buffer + -> Int -- ^ degree + -> 𝕀 + -> ST s 𝕀 +restrict1D buf coeffs deg ( 𝕀 u v ) = do + + -- Use the passed-in scratch buffer for the computaiton. + let !count = deg + 1 + forLoop 0 count \ k -> do + val <- readStrided coeffs k + Prim.writePrimArray buf k val + + -- Do right Casteljau on our local buffer, [0,1] --> [u,0]. + err1 <- + if u == 0 + then return 0 + else deCasteljauRight buf deg u + + -- Do left Casteljau on our local buffer, [u,0] --> [u,v]. + -- As we have already re-scaled once, the new right edge is not v but ( v - u ) / ( 1 - u ). + err2 <- + if v == 1 + then return 0 + else do + let v' = ( v - u ) / ( 1 - u ) + deCasteljauLeft buf deg v' + + -- Propagate the changes from the temporary buffer to the parent array + forLoop 0 count \k -> do + val <- Prim.readPrimArray buf k + writeStrided coeffs k val + + return $ err1 + err2 + +-- | Restrict a Bernstein polynomial on [0, 1] to [0, t]. +-- +-- Returns accumulated floating-point error. +deCasteljauLeft + :: Prim.MutablePrimArray s Double + -> Int -- ^ degree + -> Double -- ^ split point @t@ + -> ST s 𝕀 +deCasteljauLeft marr deg t = do + let + tInv = 1 - t + + loopJ j errAcc + | j > deg = return errAcc + | otherwise = do + + -- NB: backwards iteration. + let + loopI i currentErr + | i < j = return currentErr + | otherwise = do + let + val <- Prim.readPrimArray marr i + valPrev <- Prim.readPrimArray marr ( i - 1 ) + + let + !(# term1 , e1 #) = prodWithErr valPrev tInv + !(# term2 , e2 #) = prodWithErr val t + !(# newVal, e3 #) = addWithErr term1 term2 + + stepErr = e1 + e2 + e3 + + Prim.writePrimArray marr i newVal + loopI (i - 1) (currentErr + stepErr) + + newErr <- loopI deg 0 + loopJ (j + 1) (errAcc + newErr) + + loopJ 1 0 + +-- | Restrict a Bernstein polynomial on [0, 1] to [t, 1]. +-- +-- Returns accumulated floating-point error. +deCasteljauRight + :: Prim.MutablePrimArray s Double + -> Int -- ^ degree + -> Double -- ^ split point @t@ + -> ST s 𝕀 +deCasteljauRight marr deg t = do + let + tInv = 1.0 - t + + loopJ j errAcc + | j > deg = return errAcc + | otherwise = do + + -- NB: forwards iteration. + let + limit = deg - j + loopI i currentErr + | i > limit = return currentErr + | otherwise = do + val <- Prim.readPrimArray marr i + valNext <- Prim.readPrimArray marr ( i + 1 ) + + let + !(# term1 , e1 #) = prodWithErr val tInv + !(# term2 , e2 #) = prodWithErr valNext t + !(# newVal, e3 #) = addWithErr term1 term2 + + stepErr = e1 + e2 + e3 + + Prim.writePrimArray marr i newVal + loopI (i + 1) (currentErr + stepErr) + + newErr <- loopI 0 0 + loopJ (j + 1) (errAcc + newErr) + + loopJ 1 0 + +evaluateBernsteinModel + :: forall { order } ( degs :: Shape order ) + . ( KnownNat order + , KnownShape degs + , Representable Double ( ℝ order ) + ) + => BernsteinModel degs -> ℝ order -> 𝕀 +evaluateBernsteinModel ( BernsteinModel coeffs startingErr ) x = + runST \ @s -> do + + let + !dims = knownShape @degs + !dimsArray = fromList dims + !maxDeg = if null dims then 0 else maximum dims + !totalLen = Prim.sizeofPrimArray coeffs + + -- Copy input into our a local buffer. + bufA <- Prim.newPrimArray @( ST s ) @Double totalLen + Prim.copyPrimArray bufA 0 coeffs 0 totalLen + + -- A second buffer for ping/pong. + bufB <- Prim.newPrimArray @( ST s ) @Double totalLen + + -- Small scratch buffer for 1D evaluations. + scratchBuf <- Prim.newPrimArray @( ST s ) @Double $ maxDeg + 1 + + let + -- We return the error accumulated in this step AND the buffer containing the result + processDim + :: Prim.MutablePrimArray s Double -- src + -> Prim.MutablePrimArray s Double -- dst + -> Int -- current (logical) size of src + -> ( Int, Double ) -- @(deg, t)@ + -> ST s ( Int, 𝕀 ) -- @(new_size, err)@ + processDim src dst srcSize (deg, t) = do + let + !rowSize = deg + 1 + !outputSize = srcSize `div` rowSize + + stepErr <- foldMLoop 0 outputSize 0 \ i accErr -> do + let offset = i * rowSize + -- eval1D reads from 'src' and uses 'scratchBuf' internally + (val, evalErr) <- eval1D scratchBuf src offset deg t + Prim.writePrimArray dst i val + return $! accErr + evalErr + return (outputSize, stepErr) + + -- Ping-pong + ( finalBuf, _, _, totalRefinementErr) <- + forEachCoord @order ( bufA, bufB, totalLen, 0 ) \ ( Fin i ) ( curr, other, sz, err ) -> do + let + order = Prim.sizeofPrimArray dimsArray + + -- Process the last dimension first, because it has stride 1. + -- + -- Apologies for the index math, caused by 'Fin' being 1-indexed + -- while all the array indices are 0-based. + !j = order - fromIntegral i -- 0-based index from end + !deg_j = dimsArray `Prim.indexPrimArray` j + !x_j = x `index` ( Fin $ fromIntegral j + 1 ) + + ( newSize, stepErr ) <- processDim curr other sz ( deg_j, x_j ) + -- Swap buffers + return ( other, curr, newSize, err + stepErr ) + + finalVal <- Prim.readPrimArray finalBuf 0 + return $ 𝕀 finalVal finalVal + totalRefinementErr + startingErr + + +-- | Evaluates a 1D Bernstein polynomial at 't' using de Casteljau. +eval1D + :: Prim.MutablePrimArray s Double -- ^ scratch buffer (size >= degree + 1) + -> Prim.MutablePrimArray s Double -- ^ coefficients (only read, not written) + -> Int + -> Int + -> Double -- @t@ + -> ST s ( Double, 𝕀 ) +eval1D buf marr off deg t = do + + -- Copy the coefficients into the scratch buffer. + let + !count = deg + 1 + forLoop 0 count \ k -> do + v <- Prim.readPrimArray marr ( off + k ) + Prim.writePrimArray buf k v + + let + !tInv = 1 - t + + -- Triangle loop: Reduce degree from 'deg' down to 1 + finalErr <- + foldMLoop 1 ( deg + 1 ) 0 \ r accErr -> do + -- Inner loop: compute degree r coefficients + -- We write to indices 0 .. (deg - r) + let !limit = deg - r + stepErr <- foldMLoop 0 ( limit + 1 ) 0 \ i currentErr -> do + c0 <- Prim.readPrimArray buf i + c1 <- Prim.readPrimArray buf $ i + 1 + + let + !(# p1 , e1 #) = prodWithErr c0 tInv + !(# p2 , e2 #) = prodWithErr c1 t + !(# val, e3 #) = addWithErr p1 p2 + + Prim.writePrimArray buf i val + return $ currentErr + e1 + e2 + e3 + + return ( accErr + stepErr ) + + finalVal <- Prim.readPrimArray buf 0 + return (finalVal, finalErr) + + +-- TODO: degree lowering, degree raising + + +{- TODO: Bézier clipping + +Start with multivariate Taylor model in Bernstein basis + + 1. Choose a dimension/variable. + 2. Perform column reduction to that dimension, taking max/min of all coefficients across other dimensions. + This gives a 1D Taylor model in Bernstein basis. + 3. Use Andrew's monotone chain algorithm to compute a floor and roof, + including the Taylor error term (negative part for floor, positive part for roof). + 4. Clip the floor and roof against y=0 (using interval arithmetic to account for rounding errors). + For example, for the ceiling: + - start with the first edge that starts above y=0 or crosses y=0, + - end with the last edge that ends above y=0 or crosses y=0. + This (potentially) refines the original interval in that dimension. + +Then iterate across all dimensions. It may be beneficial to refine the Bézier patch +after handling each dimension, as long as we shrink enough in the dimension to +warrant doing this computation (e.g. if we shrink to 99% of the orginal size in the first +dimension, probably not worth doing de Casteljau). + +-} + +{- FUTURE ENHANCEMENTS + + - composition of Bernstein tensors/Bernstein models + +-} + +-------------------------------------------------------------------------------- +-- Helpers + +data Strided s = Strided + { stridedArray :: !( Prim.MutablePrimArray s Double ) + , stridedOffset :: !Int + , stridedStride :: !Int + } + +readStrided :: Strided s -> Int -> ST s Double +readStrided ( Strided arr off str) i = + Prim.readPrimArray arr ( off + i * str ) + +writeStrided :: Strided s -> Int -> Double -> ST s () +writeStrided ( Strided arr off str ) i x = + Prim.writePrimArray arr ( off + i * str ) x + +{-# INLINEABLE forLoop #-} +{-# SPECIALISE INLINE forall s. forall. forLoop @( ST s ) #-} +-- | @C@-style for loop, from start to end (but not including the end). +forLoop :: Monad m => Int -> Int -> (Int -> m ()) -> m () +forLoop start end body = go start + where + go !i | i >= end = return () + | otherwise = body i >> go (i + 1) + +{-# INLINE foldLoop #-} +foldLoop :: Int -> Int -> a -> ( Int -> a -> a ) -> a +foldLoop start end initVal body = go start initVal + where + go !i !acc + | i >= end = acc + | otherwise = let acc' = body i acc in go (i + 1) acc' + +{-# INLINEABLE foldMLoop #-} +{-# SPECIALISE INLINE forall s. forall. foldMLoop @( ST s ) #-} +foldMLoop :: Monad m => Int -> Int -> a -> ( Int -> a -> m a ) -> m a +foldMLoop start end initVal body = go start initVal + where + go !i !acc | i >= end = return acc + | otherwise = body i acc >>= go (i + 1) + + +zipWithCoeffs + :: ( Double -> Double -> (# Double, 𝕀 #) ) + -> Prim.PrimArray Double + -> Prim.PrimArray Double + -> ( 𝕀, Prim.PrimArray Double ) +zipWithCoeffs op = \ c1 c2 -> runST $ do + let !len = Prim.sizeofPrimArray c1 + marr <- Prim.newPrimArray len + + -- Compute the hull of local floating-point errors + totalFpErr <- foldMLoop 0 len ( 𝕀 (1/0) (-1/0) ) \ i accErr -> do + let + !v1 = Prim.indexPrimArray c1 i + !v2 = Prim.indexPrimArray c2 i + !(# res, err #) = op v1 v2 + Prim.writePrimArray marr i res + return $! hull accErr err + + resArr <- Prim.unsafeFreezePrimArray marr + return ( totalFpErr, resArr ) +{-# INLINE zipWithCoeffs #-} + -- inline to expose the 'op' function
+ src/lib/TH/Utils.hs view
@@ -0,0 +1,23 @@+{-# LANGUAGE TemplateHaskell #-} + +module TH.Utils where + +-- template-haskell +import Language.Haskell.TH + ( CodeQ ) + +-- brush-strokes +import Math.Ring ( Ring ) +import qualified Math.Ring as Ring + +-------------------------------------------------------------------------------- + +foldQ :: CodeQ ( a -> a -> a ) -> CodeQ a -> [ CodeQ a ] -> CodeQ a +foldQ _ a0 [] = a0 +foldQ _ _ [a] = a +foldQ f a0 (a:as) = [|| $$f $$a $$( foldQ f a0 as ) ||] + +powQ :: Ring a => CodeQ a -> Word -> CodeQ a +powQ _ 0 = [|| Ring.fromInteger ( 1 :: Integer ) ||] +powQ x 1 = x +powQ x n = [|| $$x Ring.^ ( n :: Word ) ||]
+ src/simd-interval/Math/Interval/Internal/SIMD/Internal.hs view
@@ -0,0 +1,152 @@+{-# LANGUAGE CPP #-} +{-# LANGUAGE ForeignFunctionInterface #-} +{-# LANGUAGE GHCForeignImportPrim #-} +{-# LANGUAGE UnliftedFFITypes #-} + +module Math.Interval.Internal.SIMD.Internal + ( 𝕀(𝕀, MkI, PackI, inf, sup) + , scaleInterval + ) + where + +-- base +import Prelude hiding ( Num(..), Fractional(..), Floating(..) ) +import qualified Prelude +import GHC.Exts + ( Double(D#), Double# + , negateDouble# + , DoubleX2# + , packDoubleX2#, unpackDoubleX2# + , broadcastDoubleX2# + , plusDoubleX2#, timesDoubleX2# + + , indexDoubleX2Array# + , readDoubleX2Array# + , writeDoubleX2Array# + , indexDoubleX2OffAddr# + , readDoubleX2OffAddr# + , writeDoubleX2OffAddr# + ) + +-- primitive +import Data.Primitive + +-- rounded-hw +import Numeric.Rounded.Hardware + ( Rounded(..) ) +import Numeric.Rounded.Hardware.Internal + ( intervalFromInteger, intervalFromRational ) + +-- ghc-prim +import GHC.Prim + ( maxDouble#, shuffleDoubleX2# ) + +-- deepseq +import Control.DeepSeq + ( NFData(..) ) + +-------------------------------------------------------------------------------- + +-- | A non-empty interval of real numbers (possibly unbounded). +data 𝕀 = MkI DoubleX2# + -- Interval [lo..hi] represented as (-lo, hi) + +instance NFData 𝕀 where + rnf ( MkI !_ ) = () + +foreign import prim "in2_mul8" + mulI :: DoubleX2# -> DoubleX2# -> DoubleX2# + +{-# COMPLETE PackI #-} +{-# INLINE PackI #-} +pattern PackI :: Double# -> Double# -> 𝕀 +pattern PackI m_lo hi <- ( ( \ ( MkI x ) -> unpackDoubleX2# x ) -> (# m_lo, hi #) ) + where + PackI m_lo hi = MkI ( packDoubleX2# (# m_lo, hi #) ) + + +{-# COMPLETE 𝕀 #-} +{-# INLINE 𝕀 #-} +pattern 𝕀 :: Double -> Double -> 𝕀 +pattern 𝕀 { inf, sup } <- ( ( \ ( PackI m_lo hi ) -> (# D# ( negateDouble# m_lo ), D# hi #) ) + -> (# inf, sup #) ) + where + 𝕀 (D# lo) (D# hi) = + let m_lo = negateDouble# lo + in PackI m_lo hi + +instance Prelude.Num 𝕀 where + MkI x + MkI y = MkI ( x `plusDoubleX2#` y ) + negate ( MkI x ) = MkI ( shuffleDoubleX2# x x (# 1#, 0# #) ) + ( MkI x ) * ( MkI y ) = MkI ( mulI x y ) + -- This uses the C code from Lockless Inc. + -- TODO: compare with + -- "Fast and Correct SIMD Algorithms for Interval Arithmetic" + -- Frédéric Goualard, 2008 + fromInteger i = + case intervalFromInteger i of + ( Rounded lo, Rounded hi ) -> 𝕀 lo hi + abs x@(PackI m_lo hi) + | D# m_lo <= 0 + = PackI m_lo hi + | D# hi <= 0 + = Prelude.negate x + | otherwise + = 𝕀 0 ( D# ( maxDouble# m_lo hi ) ) + signum _ = error "No implementation of signum for intervals" + +instance Prelude.Fractional 𝕀 where + fromRational r = + case intervalFromRational r of + ( Rounded lo, Rounded hi ) -> 𝕀 lo hi + recip ( 𝕀 lo hi ) +#ifdef ASSERTS + | lo == 0 + = 𝕀 ( Prelude.recip hi ) ( 1 Prelude./ 0 ) + | hi == 0 + = 𝕀 ( -1 Prelude./ 0 ) ( Prelude.recip lo ) + | lo > 0 || hi < 0 +#endif + = 𝕀 ( Prelude.recip hi ) ( Prelude.recip lo ) +#ifdef ASSERTS + | otherwise + = error "BAD interval recip; should use extendedRecip instead" +#endif + p / q = p Prelude.* Prelude.recip q + +scaleInterval :: Double -> 𝕀 -> 𝕀 +scaleInterval s@(D# s#) i = + case compare s 0 of + LT -> case Prelude.negate i of { MkI x -> MkI ( broadcastDoubleX2# ( negateDouble# s# ) `timesDoubleX2#` x ) } + EQ -> 𝕀 0 0 + GT -> case i of { MkI x -> MkI ( broadcastDoubleX2# s# `timesDoubleX2#` x ) } + +-------------------------------------------------------------------------------- + +instance Prim 𝕀 where + + sizeOf# _ = 16# + + alignment# _ = 16# + + indexByteArray# arr# i# = + let v = indexDoubleX2Array# arr# i# + in MkI v + + readByteArray# arr# i# s0 = + case readDoubleX2Array# arr# i# s0 of + (# s1, v #) -> (# s1, MkI v #) + + writeByteArray# arr# i# (MkI v) s0 = + writeDoubleX2Array# arr# i# v s0 + + indexOffAddr# addr# i# = + let v = indexDoubleX2OffAddr# addr# i# + in MkI v + + readOffAddr# addr# i# s0 = + case readDoubleX2OffAddr# addr# i# s0 of + (# s1, v #) -> (# s1, MkI v #) + + writeOffAddr# addr# i# (MkI v) s0 = + writeDoubleX2OffAddr# addr# i# v s0
+ src/test/Main.hs view
@@ -0,0 +1,319 @@+{-# LANGUAGE AllowAmbiguousTypes #-} +{-# LANGUAGE NumericUnderscores #-} +{-# LANGUAGE ScopedTypeVariables #-} +{-# LANGUAGE TemplateHaskell #-} + +module Main (main) where + +-- base +import Prelude hiding + ( Num(..), (^) ) +import Data.Foldable + ( toList ) +import Data.List.NonEmpty + ( NonEmpty(..) ) +import Data.Maybe + ( catMaybes ) +import Data.Traversable + ( for ) +import Unsafe.Coerce + ( unsafeCoerce ) + +-- brush-strokes +import Math.Algebra.Dual +import Math.Linear +import Math.Module +import Math.Monomial + ( multiSubsetSum, multiSubsetsSum + , MonomialBasisQ ( monTabulateQ, monIndexQ ) + ) +import Math.Ring + +-- hspray +import Math.Algebra.Hspray + ( Spray ) +import qualified Math.Algebra.Hspray as Spray + +-- falsify +import Test.Tasty.Falsify +import qualified Test.Falsify.Generator as Falsify + ( Gen ) +import qualified Test.Falsify.Generator as Falsify.Gen +import Test.Falsify.Predicate + ( (.$) ) +import qualified Test.Falsify.Predicate as Falsify.Prop +import qualified Test.Falsify.Property as Falsify + ( Property + , assert + , discard + , gen, genWith + ) +import qualified Test.Falsify.Range as Falsify + +-- tasty +import qualified Test.Tasty as Tasty + +-- unordered-containers +import qualified Data.HashMap.Lazy as HashMap + +-------------------------------------------------------------------------------- + + +main :: IO () +main = + Tasty.defaultMain $ + Tasty.testGroup "brush-strokes property tests" + [ Tasty.testGroup "Automatic differentiation" + [ Tasty.testGroup "Monomial basis" + [ testProperty "Round trip D33" testMonomialBasisQD33 + ] + , Tasty.testGroup "Monomials" + [ Tasty.testGroup "multiSubsetSum" + [ testProperty "multiSubsetSum valid" testMultiSubsetSumValid + , testProperty "multiSubsetSum exhaustive" testMultiSubsetSumExhaustive + ] + -- , Tasty.testGroup "multiSubsetsSum" + -- [ testProperty "multiSubsetsSum exhaustive" testMultiSubsetsSumExhaustive + -- ] + ] + , Tasty.testGroup "chainRule1NQ" + [ testProperty "chainRule1NQ_1" testChainRule1NQ_1 + , testProperty "chainRule1NQ_2" testChainRule1NQ_2 + , testProperty "chainRule1NQ_3" testChainRule1NQ_3 + ] + ] + ] + +-- | Check that the 'multiSubsetSum' function returns valid answers, i.e. +-- all returned multisubsets have the desired size and sum. +testMultiSubsetSumValid :: Falsify.Property () +testMultiSubsetSumValid = do + rg <- Falsify.genWith (\ rg -> Just $ "range = " ++ show rg ) $ Falsify.Gen.inRange $ Falsify.between ( 1, 6 ) + sz <- Falsify.genWith (\ sz -> Just $ "size = " ++ show sz ) $ Falsify.Gen.inRange $ Falsify.between ( 0, 20 ) + tot <- Falsify.genWith (\ tot -> Just $ "tot = " ++ show tot) $ Falsify.Gen.inRange $ Falsify.between ( sz, sz * rg ) + let range = [ 1 .. rg ] + mss = multiSubsetSum sz tot range + case mss of + [] -> Falsify.discard + r:rs -> do + ms <- Falsify.gen $ Falsify.Gen.elem ( r :| rs ) + Falsify.assert + $ Falsify.Prop.eq + .$ ("(sz, tot)", (sz, tot) ) + .$ ("computed (sz, tot)", (size ms, total ms)) + where + size, total :: [ ( Word, Word ) ] -> Word + size [] = 0 + size ((_,n):ins) = n + size ins + total [] = 0 + total ((i,n):ins) = i * n + total ins + +-- | Check that the 'multiSubsetSum' function returns all multisubsets of +-- the given set, by generating a random multisubset, computing its size, and +-- checking it belongs to the output of the 'multiSubsetSum' function. +testMultiSubsetSumExhaustive :: Falsify.Property () +testMultiSubsetSumExhaustive = do + rg <- Falsify.genWith (\ rg -> Just $ "range = " ++ show rg) $ Falsify.Gen.inRange $ Falsify.between ( 1, 6 ) + sz <- Falsify.genWith (\ sz -> Just $ "size = " ++ show sz) $ Falsify.Gen.inRange $ Falsify.between ( 0, 10 ) + let range = [ 1 .. rg ] + (multiSubset, tot) <- Falsify.genWith (\ ms -> Just $ "multisubset = " ++ show ms) $ genMultiSubset range sz + Falsify.assert + $ Falsify.Prop.elem + .$ ("all multisubsets", multiSubsetSum sz tot range ) + .$ ("random multisubset", multiSubset) + +genMultiSubset :: [ Word ] -> Word -> Falsify.Gen ( [ ( Word, Word ) ] , Word ) +genMultiSubset [i] sz = + return $ + if sz == 0 + then ( [], 0 ) + else ( [ ( i, sz ) ], i * sz ) +genMultiSubset (i:is) sz = do + nb <- Falsify.Gen.inRange $ Falsify.between ( 0, sz ) + (rest, tot) <- genMultiSubset is ( sz - nb ) + return $ ( if nb == 0 then rest else ( i, nb ) : rest, tot + nb * i ) +genMultiSubset [] _ = error "impossible" + +coerceVec1 :: [ a ] -> Vec n a +coerceVec1 = unsafeCoerce + +coerceVec2 :: Vec n a -> [ a ] +coerceVec2 = toList + +-- | Check that the 'multiSubsetSums' function returns all collections of +-- multisubsets of the given set (see 'testMultiSubsetSumExhaustive'). +testMultiSubsetsSumExhaustive :: Falsify.Property () +testMultiSubsetsSumExhaustive = do + rg <- Falsify.genWith (\ rg -> Just $ "range = " ++ show rg) $ Falsify.Gen.inRange $ Falsify.between ( 1, 5 ) + let range = [ 1 .. rg ] + n <- Falsify.genWith (\ n -> Just $ "n = " ++ show n ) $ Falsify.Gen.inRange $ Falsify.between ( 1, 10 ) + multiSubsets <- for ( [ 0 .. n - 1 ] :: [ Word ] ) \ i -> do + sz <- Falsify.gen $ Falsify.Gen.inRange $ Falsify.between ( 0, 5 ) + ( ms, tot ) <- Falsify.genWith ( \ ms -> Just $ "ms_" ++ show i ++ " = " ++ show ms ) $ genMultiSubset range sz + return ( ms, sz, tot ) + let mss = map ( \ (ms, _,_) -> ms ) multiSubsets + szs = map ( \ (_,sz,_) -> sz) multiSubsets + tot = sum $ map ( \(_,_,t) -> t) multiSubsets + Falsify.assert + $ Falsify.Prop.elem + .$ ("all multisubsets", map coerceVec2 $ multiSubsetsSum range tot $ coerceVec1 szs ) + .$ ("random multisubset", mss) + + +testRoundTrip + :: ( Show a, Eq a ) + => Falsify.Gen a + -> ( a -> a ) + -> Falsify.Property () +testRoundTrip g roundTrip = do + d <- Falsify.gen g + Falsify.assert + $ Falsify.Prop.eq + .$ ("value", d ) + .$ ("round tripped", roundTrip d ) + +testMonomialBasisQD33 :: Falsify.Property () +testMonomialBasisQD33 = + testRoundTrip genD33 \ d -> $$( monTabulateQ \ mon -> monIndexQ [|| d ||] mon ) + where + genD33 :: Falsify.Gen ( D3𝔸3 Double ) + genD33 = + D33 <$> (unT <$> g) + <*> g <*> g <*> g + <*> g <*> g <*> g <*> g <*> g <*> g + <*> g <*> g <*> g <*> g <*> g <*> g <*> g <*> g <*> g <*> g + + g :: Falsify.Gen ( T Double ) + g = T . fromIntegral <$> Falsify.Gen.inRange ( Falsify.withOrigin ( -100, 100 ) ( 0 :: Int ) ) + +-- | Test the Faà di Bruno formula on polynomials, with a composition +-- \( g(f_1(x), f_2(x), .., f_n(x)) \). +testChainRule1NQ_1 :: Falsify.Property () +testChainRule1NQ_1 = do + f <- genSpray "f" 1 + g <- genSpray "g" 1 + let gof_spray = Spray.composeSpray g [f] + gof_chain = + chain @_ @3 @( ℝ 1 ) ( ℝ1 <$> fromSpray @3 @( ℝ 1 ) f ) ( fromSpray @3 @( ℝ 1 ) g ) + Falsify.assert + $ Falsify.Prop.eq + .$ ("direct", fromSpray @3 @( ℝ 1 ) gof_spray ) + .$ ("chain rule", gof_chain ) + +-- | Test the Faà di Bruno formula on polynomials, with a composition +-- \( g(f_1(x), f_2(x), .., f_n(x)) \). +testChainRule1NQ_2 :: Falsify.Property () +testChainRule1NQ_2 = do + f1 <- genSpray "f1" 1 + f2 <- genSpray "f2" 1 + g <- genSpray "g" 2 + let gof_spray = Spray.composeSpray g [f1, f2] + f = ℝ2 <$> fromSpray @3 @( ℝ 1 ) f1 + <*> fromSpray @3 @( ℝ 1 ) f2 + gof_chain = + chain @_ @3 @( ℝ 2 ) f ( fromSpray @3 @( ℝ 2 ) g ) + Falsify.assert + $ Falsify.Prop.eq + .$ ("direct", fromSpray @3 @( ℝ 1 ) gof_spray ) + .$ ("chain rule", gof_chain ) + +-- | Test the Faà di Bruno formula on polynomials, with a composition +-- \( g(f_1(x), f_2(x), .., f_n(x)) \). +testChainRule1NQ_3 :: Falsify.Property () +testChainRule1NQ_3 = do + f1 <- genSpray "f1" 1 + f2 <- genSpray "f2" 1 + f3 <- genSpray "f3" 1 + g <- genSpray "g" 3 + let gof_spray = Spray.composeSpray g [f1, f2, f3] + f = ℝ3 <$> fromSpray @3 @( ℝ 1 ) f1 + <*> fromSpray @3 @( ℝ 1 ) f2 + <*> fromSpray @3 @( ℝ 1 ) f3 + gof_chain = + chain @_ @3 @( ℝ 3 ) f ( fromSpray @3 @( ℝ 3 ) g ) + Falsify.assert + $ Falsify.Prop.eq + .$ ("direct", fromSpray @3 @( ℝ 1 ) gof_spray ) + .$ ("chain rule", gof_chain ) + +class FromSpray v where + varFn :: Int -> v + linFn :: v -> Int -> Double + +instance FromSpray ( ℝ 1 ) where + varFn = \case + 0 -> ℝ1 1 + i -> error $ "fromSpray in 1d but variable " ++ show i + linFn ( ℝ1 x ) = \case + 0 -> x + i -> error $ "fromSpray in 1d but variable " ++ show i +instance FromSpray ( ℝ 2 ) where + varFn = \case + 0 -> ℝ2 1 0 + 1 -> ℝ2 0 1 + i -> error $ "fromSpray in 2d but variable " ++ show i + linFn ( ℝ2 x y ) = \case + 0 -> x + 1 -> y + i -> error $ "fromSpray in 2d but variable " ++ show i +instance FromSpray ( ℝ 3 ) where + varFn = \case + 0 -> ℝ3 1 0 0 + 1 -> ℝ3 0 1 0 + 2 -> ℝ3 0 0 1 + i -> error $ "fromSpray in 3d but variable " ++ show i + linFn ( ℝ3 x y z ) = \case + 0 -> x + 1 -> y + 2 -> z + i -> error $ "fromSpray in 3d but variable " ++ show i + +genSpray :: String -> Word -> Falsify.Property ( Spray Double ) +genSpray lbl nbVars = Falsify.genWith (\ p -> Just $ lbl ++ " = " ++ Spray.prettySpray show "x" p) $ do + deg <- Falsify.Gen.inRange $ Falsify.between ( 0, 10 ) + let mons = allMonomials deg nbVars + coeffs <- + for mons $ \ mon -> do + if all (== 0) mon + then return Nothing + else do + nonZero <- Falsify.Gen.bool False + if nonZero + then return Nothing + else do + -- Just use (small) integral values in tests for now, + -- to avoid errors arising from rounding. + c <- Falsify.Gen.inRange $ Falsify.withOrigin ( -100, 100 ) ( 0 :: Int ) + return $ Just ( map fromIntegral mon, fromIntegral c ) + return $ Spray.fromList $ catMaybes coeffs + +allMonomials :: Word -> Word -> [ [ Word ] ] +allMonomials k _ | k < 0 = [] +allMonomials _ 0 = [ [] ] +allMonomials 0 n = [ replicate ( fromIntegral n ) 0 ] +allMonomials k n = [ i : is | i <- reverse [ 0 .. k ], is <- allMonomials ( k - i ) ( n - 1 ) ] + +-- | Convert a multivariate polynomial from the @hspray@ library to the dual algebra. +fromSpray + :: forall k v + . ( HasChainRule Double k v + , Module Double (T v) + , Applicative ( D k v ) + , Ring ( D k v Double ) + , FromSpray v + ) + => Spray Double + -> D k v Double +fromSpray coeffs = HashMap.foldlWithKey' addMonomial ( konst @Double @k @v $ HashMap.lookupDefault 0 (Spray.Powers mempty 0) coeffs ) coeffs + where + addMonomial :: D k v Double -> Spray.Powers -> Double -> D k v Double + addMonomial a xs c = a + monomial c ( toList $ Spray.exponents xs ) + monomial :: Double -> [ Int ] -> D k v Double + monomial _ [] = konst @Double @k @v 0 + monomial c is = fmap ( c * ) $ go 0 is + go :: Int -> [ Int ] -> D k v Double + go _ [] = konst @Double @k @v 1 + go d (i : is) = pow d i * go ( d + 1 ) is + pow :: Int -> Int -> D k v Double + pow _ 0 = konst @Double @k @v 1 + pow d i = linearD @Double @k @v ( \ x -> linFn @v x d ) ( unT origin :: v ) ^ ( fromIntegral i )