import type Pose from '../../armature/Pose' ; import type { IKChain , IKLink } from "../rigs/IKChain" ; import type { ISolver } from './support/ISolver' ; import { QuatUtil , Transform , Vec3Util } from '../../maths' ; import CurveSample from '../../maths/CurveSample' ; import { vec3 , quat } from 'gl-matrix' ; class Caternary { static get ( A , x ) { return A * Math . cosh ( x / A ) ; } static computeSag ( len , maxLen , tries = 100 ) { if ( len > maxLen ) return null ; const hLen = len * 0.5 ; const hMaxLen = maxLen * 0.5 ; let e = Number . MAX_VALUE ; let a = 100 ; let tmp = 0 ; for ( let i = 0 ; i < tries ; i ++ ) { tmp = hLen / Math . asinh ( hMaxLen / a ) ; e = Math . abs ( ( tmp - a ) / a ) ; a = tmp ; if ( e < 0.001 ) break ; } return a ; } static fromEndPoints ( p0 , p1 , maxLen , segments = 5 , invert = false ) { const vecLen = Vec3Util . len ( p0 , p1 ) ; const A = this . computeSag ( vecLen , maxLen ) ; if ( A == null ) return [ ] ; segments += 1 ; const hVecLen = vecLen * 0.5 ; const offset = this . get ( A , - hVecLen ) ; const step = vecLen / segments ; const rtn = [ ] ; let pnt ; let x ; let c ; for ( let i = 1 ; i < segments ; i ++ ) { pnt = [ 0 , 0 , 0 ] ; vec3 . lerp ( pnt , p0 , p1 , i / segments ) ; x = i * step - hVecLen ; c = offset - this . get ( A , x ) ; pnt [ 1 ] = ( ! invert ) ? pnt [ 1 ] - c : pnt [ 1 ] + c ; rtn . push ( pnt ) ; } return rtn ; } } class CatenarySolver implements ISolver { effectorPos : vec3 = [ 0 , 0 , 0 ] ; sampler ! : CurveSample ; initData ( pose ? , chain ? ) { return this ; } setTargetPos ( v ) { this . effectorPos [ 0 ] = v [ 0 ] ; this . effectorPos [ 1 ] = v [ 1 ] ; this . effectorPos [ 2 ] = v [ 2 ] ; return this ; } resolve ( chain , pose , debug ? ) { const sCnt = chain . count * 2 ; if ( ! this . sampler ) this . sampler = new CurveSample ( sCnt + 2 ) ; const pt = new Transform ( ) ; const ct = new Transform ( ) ; let lnk = chain . first ( ) ; pose . getWorldTransform ( lnk . pidx , pt ) ; ct . fromMul ( pt , lnk . bind ) ; const pnts = Caternary . fromEndPoints ( ct . pos , this . effectorPos , chain . length , sCnt , false ) ; let i = 1 ; this . sampler . set ( 0 , ct . pos ) ; for ( let p of pnts ) this . sampler . set ( i ++ , p ) ; this . sampler . set ( i , this . effectorPos ) ; this . sampler . updateLengths ( ) ; const tail = [ 0 , 0 , 0 ] ; const tar = [ 0 , 0 , 0 ] ; const from = [ 0 , 0 , 0 ] ; const to = [ 0 , 0 , 0 ] ; const q = [ 0 , 0 , 0 , 1 ] ; let dist = 0 ; for ( let i = 0 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; dist += lnk . len ; ct . fromMul ( pt , lnk . bind ) ; tail [ 0 ] = 0 ; tail [ 1 ] = lnk . len ; tail [ 2 ] = 0 ; ct . transformVec3 ( tail ) ; this . sampler . atLength ( dist , tar ) ; vec3 . sub ( from , tail , ct . pos ) ; vec3 . normalize ( from , from ) ; vec3 . sub ( to , tar , ct . pos ) ; vec3 . normalize ( to , to ) ; quat . rotationTo ( q , from , to ) ; quat . mul ( q , q , ct . rot ) ; QuatUtil . pmulInvert ( q , q , pt . rot ) ; pose . setLocalRot ( lnk . idx , q ) ; pt . mul ( q , lnk . bind . pos , lnk . bind . scl ) ; } } } export default CatenarySolver ;