module Reanimate.Morph.LineBend ( lineBend , lineBendRaw ) where import qualified Data.Vector as V import Linear.Matrix (det22) import Linear.Metric import Linear.V2 import Linear.Vector import Reanimate.Math.Common import Reanimate.Math.Polygon import Reanimate.Morph.Common lineBend :: Trajectory lineBend = lineBend' True lineBendRaw :: Trajectory lineBendRaw = lineBend' False -- Note: Returns polygon with n+1 elements. lineBend' :: Bool -> Trajectory lineBend' corrected (a,b) = \t -> let u = 1 - t -- phi = V.zipWith (\l r -> u*l + t*r) phi_a phi_b phi = V.zipWith (\l r -> lerpAngle l r t) phi_a phi_b -- alpha_zero = u * alpha_a_zero + t * alpha_b_zero alpha_zero = lerpAngle alpha_a_zero alpha_b_zero t alpha = V.scanl (+) alpha_zero phi bigE = V.sum $ V.zipWith (\diff alp -> squared diff * squared (cos alp)) lengths_diff alpha bigF = V.sum $ V.zipWith (\diff alp -> squared diff * sin alp * cos alp) lengths_diff alpha bigG = V.sum $ V.zipWith (\diff alp -> squared diff * squared (sin alp)) lengths_diff alpha bigU = 2 * V.sum (V.zipWith3 (\len_a len_b alp -> (u*len_a + t*len_b) * cos alp) lengths_a lengths_b alpha) bigV = 2 * V.sum (V.zipWith3 (\len_a len_b alp -> (u*len_a + t*len_b) * sin alp) lengths_a lengths_b alpha) lam1 = det22 (V2 (V2 bigU bigF) (V2 bigV bigG)) / det22 (V2 (V2 bigE bigF) (V2 bigF bigG)) lam2 = det22 (V2 (V2 bigE bigU) (V2 bigF bigV)) / det22 (V2 (V2 bigE bigF) (V2 bigF bigG)) s_vect = V.fromList [ -0.5 * squared (lengths_diff V.! n) * (lam1 * cos (alpha V.! n) + lam2 * sin (alpha V.! n)) | n <- [0 .. pSize a-1]] lengths = V.zipWith3 (\l r s -> u*l + t*r + if corrected then s else 0) lengths_a lengths_b s_vect movingCenter :: V2 Double movingCenter = lerp t (realToFrac <$> pCentroid b) (realToFrac <$> pCentroid a) centerAng :: Double centerAng = lerpAngle centoid_angle_a centoid_angle_b t centerLen = u*centoid_len_a + t*centoid_len_b -- V2 x_zero y_zero = lerp t (realToFrac <$> b V.! 0) (realToFrac <$> a V.! 0) V2 x_zero y_zero = movingCenter + V2 (cos centerAng * centerLen) (sin centerAng * centerLen) x_modifiers = V.zipWith (*) lengths (V.map cos alpha) y_modifiers = V.zipWith (*) lengths (V.map sin alpha) xs = V.scanl (+) x_zero x_modifiers ys = V.scanl (+) y_zero y_modifiers in mkPolygon $ V.map (fmap realToFrac) $ V.zipWith V2 xs ys where squared x = x*x centoid_angle_a :: Double centoid_angle_a = lineAngle (pCentroid a + V2 1 0) (pCentroid a) (pAccess a 0) centoid_angle_b = lineAngle (pCentroid b + V2 1 0) (pCentroid b) (pAccess b 0) centoid_len_a :: Double centoid_len_a = realToFrac $ approxDist (pCentroid a) (pAccess a 0) centoid_len_b = realToFrac $ approxDist (pCentroid b) (pAccess b 0) alpha_a_zero = lineAngle (pAccess a 0 + V2 1 0) (pAccess a 0) (pAccess a 1) alpha_b_zero = lineAngle (pAccess b 0 + V2 1 0) (pAccess b 0) (pAccess b 1) lengths_a = computeLength a lengths_b = computeLength b lengths_diff' = V.zipWith (\l r -> abs (l-r)) lengths_a lengths_b lengths_tol = 0.0001 * V.maximum lengths_diff' lengths_diff = V.map (max lengths_tol) lengths_diff' phi_a = computePhi a phi_b = computePhi b computeLength :: Polygon -> V.Vector Double computeLength poly = V.fromList [ realToFrac $ approxDist this next | n <- [0 .. pSize poly-1] , let this = pAccess poly n next = pAccess poly (pNext a n) ] computePhi poly = V.fromList $ [ negate $ angle' (next-this) ((prev-this) ^* (-1)) | n <- [1 .. pSize poly-1] , let prev = pAccess poly (n-1) this = pAccess poly n next = pAccess poly (pNext a n) ] lerpAngle :: Double -> Double -> (Double -> Double) lerpAngle fromAng toAng t | abs (fromAng - (toAng+2*pi)) < abs (fromAng - toAng) = (1-t)*fromAng + t*(toAng+2*pi) | abs (fromAng - (toAng-2*pi)) < abs (fromAng - toAng) = (1-t)*fromAng + t*(toAng-2*pi) | otherwise = (1-t)*fromAng + t*toAng -- Angle from a through b to c. Measured on the right-hand side, from 0 to tau lineAngle :: V2 Rational -> V2 Rational -> V2 Rational -> Double lineAngle a b c = angle' (a-b) (c-b) angle' :: V2 Rational -> V2 Rational -> Double angle' a b = atan2 (realToFrac $ crossZ a b) (realToFrac $ dot a b)