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 FabrikSolver implements ISolver { maxIteration = 15 ; effectorPos : vec3 = [ 0 , 0 , 0 ] ; _inWorldSpace = false ; _threshold = 0.0001 ** 2 ; _bonePos ! : Array < vec3 > ; initData ( pose ? , chain ? ) { return this ; } setTargetPos ( v ) { this . effectorPos [ 0 ] = v [ 0 ] ; this . effectorPos [ 1 ] = v [ 1 ] ; this . effectorPos [ 2 ] = v [ 2 ] ; return this ; } inWorldSpace ( ) { this . _inWorldSpace = true ; return this ; } resolve ( chain , pose , debug ? ) { this . _preProcess ( chain , pose , debug ) ; let i = 0 ; for ( i ; i < this . maxIteration ; i ++ ) { this . _iterateBackward ( chain , debug ) ; this . _iterateForward ( chain , debug ) ; if ( Vec3Util . lenSqr ( this . effectorPos , this . _bonePos [ chain . count ] ) <= this . _threshold ) break ; } if ( this . _inWorldSpace ) this . _update_fromWorldPose ( chain , pose , debug ) ; else this . _update_fromBindPose ( chain , pose , debug ) ; } _preProcess ( chain , pose , debug ? ) { if ( ! this . _bonePos ) { this . _bonePos = [ ] ; for ( let i = 0 ; i < chain . count ; i ++ ) this . _bonePos . push ( [ 0 , 0 , 0 ] ) ; this . _bonePos . push ( [ 0 , 0 , 0 ] ) ; } let lnk ; if ( this . _inWorldSpace ) { for ( let i = 0 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; vec3 . copy ( this . _bonePos [ i ] , pose . bones [ lnk . idx ] . world . pos ) ; } } else { const pt = new Transform ( ) ; const ct = new Transform ( ) ; lnk = chain . first ( ) ; pose . getWorldTransform ( lnk . pidx , pt ) ; ct . fromMul ( pt , lnk . bind ) ; vec3 . copy ( this . _bonePos [ 0 ] , ct . pos ) ; for ( let i = 1 ; i < chain . count ; i ++ ) { ct . mul ( chain . links [ i ] . bind ) ; vec3 . copy ( this . _bonePos [ i ] , ct . pos ) ; } } } _update_fromWorldPose ( chain , pose , debug ? ) { const pt = new Transform ( ) ; const ct = new Transform ( ) ; let lnk = chain . first ( ) ; let tail = [ 0 , 0 , 0 ] ; let from = [ 0 , 0 , 0 ] ; let to = [ 0 , 0 , 0 ] ; let q = [ 0 , 0 , 0 , 1 ] ; let b ; pose . getWorldTransform ( lnk . pidx , pt ) ; for ( let i = 0 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; b = pose . bones [ lnk . idx ] ; ct . fromMul ( pt , b . local ) ; tail [ 0 ] = 0 ; tail [ 1 ] = lnk . len ; tail [ 2 ] = 0 ; ct . transformVec3 ( tail ) ; vec3 . sub ( from , tail , ct . pos ) ; vec3 . normalize ( from , from ) ; vec3 . sub ( to , this . _bonePos [ i + 1 ] , 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 ) ; } } _update_fromBindPose ( chain , pose , debug ? ) { const pt = new Transform ( ) ; const ct = new Transform ( ) ; let lnk = chain . first ( ) ; let tail = [ 0 , 0 , 0 ] ; let from = [ 0 , 0 , 0 ] ; let to = [ 0 , 0 , 0 ] ; let q = [ 0 , 0 , 0 , 1 ] ; pose . getWorldTransform ( lnk . pidx , pt ) ; for ( let i = 0 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; ct . fromMul ( pt , lnk . bind ) ; tail [ 0 ] = 0 ; tail [ 1 ] = lnk . len ; tail [ 2 ] = 0 ; ct . transformVec3 ( tail ) ; vec3 . sub ( from , tail , ct . pos ) ; vec3 . normalize ( from , from ) ; vec3 . sub ( to , this . _bonePos [ i + 1 ] , 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 ) ; } } _iterateBackward ( chain , debug ? ) { const apos = this . _bonePos ; const lnks = chain . links ; const dir = [ 0 , 0 , 0 ] ; const prevPos = vec3 . copy ( [ 0 , 0 , 0 ] , this . effectorPos ) ; for ( let i = chain . count - 1 ; i > 0 ; i -- ) { vec3 . sub ( dir , apos [ i ] , prevPos ) ; vec3 . normalize ( dir , dir ) ; vec3 . scaleAndAdd ( apos [ i ] , prevPos , dir , lnks [ i ] . len ) ; vec3 . copy ( prevPos , apos [ i ] ) ; } } _iterateForward ( chain , debug ? ) { const apos = this . _bonePos ; const lnks = chain . links ; const dir = [ 0 , 0 , 0 ] ; const prevPos = vec3 . copy ( [ 0 , 0 , 0 ] , apos [ 0 ] ) ; let prevLen = lnks [ 0 ] . len ; for ( let i = 1 ; i < chain . count ; i ++ ) { vec3 . sub ( dir , apos [ i ] , prevPos ) ; vec3 . normalize ( dir , dir ) ; vec3 . scaleAndAdd ( apos [ i ] , prevPos , dir , prevLen ) ; vec3 . copy ( prevPos , apos [ i ] ) ; prevLen = lnks [ i ] . len ; } const ilast = chain . count - 1 ; vec3 . sub ( prevPos , this . effectorPos , apos [ ilast ] ) ; vec3 . normalize ( prevPos , prevPos ) ; vec3 . scaleAndAdd ( apos [ chain . count ] , apos [ ilast ] , prevPos , lnks [ ilast ] . len ) ; } } export default FabrikSolver ;