import type Pose from '../../armature/Pose' ; import type { IKChain , IKLink } from "../rigs/IKChain" ; import type { ISolver } from './support/ISolver' ; import type Bone from '../../armature/Bone' ; import { QuatUtil , Transform , Vec3Util } from '../../maths' ; import { vec3 , quat } from 'gl-matrix' ; class NaturalCCDSolver implements ISolver { effectorPos : vec3 = [ 0 , 0 , 0 ] ; _inWorldSpace = false ; _tries = 30 ; _minEffRng = 0.001 ** 2 ; _chainCnt = 0 ; _local ! : Transform [ ] ; _world ! : Transform [ ] ; _kFactor ! : any ; initData ( pose ? , chain ? ) { if ( pose && chain ) { const lnk = chain . last ( ) ; const eff = [ 0 , lnk . len , 0 ] ; pose . bones [ lnk . idx ] . world . transformVec3 ( eff ) ; vec3 . copy ( this . effectorPos , eff ) ; } if ( chain ) { const cnt = chain . count ; this . _chainCnt = cnt ; this . _world = new Array ( cnt + 1 ) ; this . _local = new Array ( cnt + 1 ) ; for ( let i = 0 ; i < cnt ; i ++ ) { this . _world [ i ] = new Transform ( ) ; this . _local [ i ] = new Transform ( ) ; } this . _world [ cnt ] = new Transform ( ) ; this . _local [ cnt ] = new Transform ( [ 0 , 0 , 0 , 1 ] , [ 0 , chain . last ( ) . len , 0 ] , [ 1 , 1 , 1 ] ) ; } return this ; } setTargetPos ( v ) { this . effectorPos [ 0 ] = v [ 0 ] ; this . effectorPos [ 1 ] = v [ 1 ] ; this . effectorPos [ 2 ] = v [ 2 ] ; return this ; } useArcSqrFactor ( c , offset , useInv = false ) { this . _kFactor = new KFactorArcSqr ( c , offset , useInv ) ; return this ; } inWorldSpace ( ) { this . _inWorldSpace = true ; return this ; } setTries ( v ) { this . _tries = v ; return this ; } resolve ( chain , pose , debug ? ) { if ( ! this . _local ) { const cnt = chain . count ; this . _world = new Array ( cnt + 1 ) ; this . _local = new Array ( cnt + 1 ) ; for ( let i = 0 ; i < cnt ; i ++ ) { this . _world [ i ] = new Transform ( ) ; this . _local [ i ] = new Transform ( ) ; } this . _world [ cnt ] = new Transform ( ) ; this . _local [ cnt ] = new Transform ( [ 0 , 0 , 0 , 1 ] , [ 0 , chain . last ( ) . len , 0 ] , [ 1 , 1 , 1 ] ) ; } const root = new Transform ( ) ; let lnk = chain . first ( ) ; pose . getWorldTransform ( lnk . pidx , root ) ; let i ; for ( i = 0 ; i < chain . count ; i ++ ) { if ( ! this . _inWorldSpace ) this . _local [ i ] . copy ( chain . links [ i ] . bind ) ; else this . _local [ i ] . copy ( pose . bones [ chain . links [ i ] . idx ] . local ) ; } this . _updateWorld ( 0 , root ) ; if ( Vec3Util . lenSqr ( this . effectorPos , this . _getTailPos ( ) ) < this . _minEffRng ) { return ; } for ( i = 0 ; i < this . _tries ; i ++ ) { if ( this . _iteration ( chain , pose , root , debug ) ) break ; } for ( i = 0 ; i < chain . count ; i ++ ) { pose . setLocalRot ( chain . links [ i ] . idx , this . _local [ i ] . rot ) ; } } _updateWorld ( startIdx , root ) { const w = this . _world ; const l = this . _local ; let i ; if ( startIdx == 0 ) { w [ 0 ] . fromMul ( root , l [ 0 ] ) ; startIdx ++ ; } for ( i = startIdx ; i < w . length ; i ++ ) { w [ i ] . fromMul ( w [ i - 1 ] , l [ i ] ) ; } } _getTailPos ( ) { return this . _world [ this . _world . length - 1 ] . pos ; } _iteration ( chain , pose , root , debug ? ) { const w = this . _world ; const l = this . _local ; const cnt = w . length - 1 ; const tail = w [ cnt ] ; const tailDir = [ 0 , 0 , 0 ] ; const effDir = [ 0 , 0 , 0 ] ; const lerpDir = [ 0 , 0 , 0 ] ; const q = [ 0 , 0 , 0 , 1 ] ; const k = this . _kFactor ; let i ; let diff ; let b ; if ( k ) k . reset ( ) ; for ( i = cnt - 1 ; i >= 0 ; i -- ) { diff = Vec3Util . lenSqr ( tail . pos , this . effectorPos ) ; if ( diff <= this . _minEffRng ) return true ; b = w [ i ] ; vec3 . sub ( tailDir , tail . pos , b . pos ) ; vec3 . normalize ( tailDir , tailDir ) ; vec3 . sub ( effDir , this . effectorPos , b . pos ) ; vec3 . normalize ( effDir , effDir ) ; if ( k ) k . apply ( chain , chain . links [ i ] , tailDir , effDir , lerpDir ) ; else vec3 . copy ( lerpDir , effDir ) ; quat . rotationTo ( q , tailDir , lerpDir ) ; quat . mul ( q , q , b . rot ) ; if ( i != 0 ) QuatUtil . pmulInvert ( q , q , w [ i - 1 ] . rot ) ; else QuatUtil . pmulInvert ( q , q , root . rot ) ; quat . copy ( l [ i ] . rot , q ) ; this . _updateWorld ( i , root ) ; } return false ; } } class KFactorArcSqr { c : number ; offset : number ; arcLen = 0 ; useInv = false ; constructor ( c , offset , useInv = false ) { this . c = c ; this . offset = offset ; this . useInv = useInv ; } reset ( ) { this . arcLen = 0 ; } apply ( chain , lnk , tailDir , effDir , out ) { this . arcLen += lnk . len ; const k = ( ! this . useInv ) ? this . c / Math . sqrt ( this . arcLen + this . offset ) : this . c / Math . sqrt ( ( chain . length - this . arcLen ) + this . offset ) ; vec3 . lerp ( out , tailDir , effDir , k ) ; vec3 . normalize ( out , out ) ; } } export default NaturalCCDSolver ;