import type Pose from '../../armature/Pose' ; import type { IKChain , IKLink } from '../rigs/IKChain' ; import type { ISolver } from './support/ISolver' ; import type { IKData } from '..' ; import { Transform } from '../../maths' ; import { vec3 , quat } from 'gl-matrix' ; import QuatUtil from '../../maths/QuatUtil' ; class SwingTwistSolver implements ISolver { _isTarPosition : boolean = false ; _originPoleDir : vec3 = [ 0 , 0 , 0 ] ; effectorScale : number = 1 ; effectorPos : vec3 = [ 0 , 0 , 0 ] ; effectorDir : vec3 = [ 0 , 0 , 1 ] ; poleDir : vec3 = [ 0 , 1 , 0 ] ; orthoDir : vec3 = [ 1 , 0 , 0 ] ; originPos : vec3 = [ 0 , 0 , 0 ] ; initData ( pose ? , chain ? ) { if ( pose && chain ) { const lnk = chain . links [ 0 ] ; const rot = pose . bones [ lnk . idx ] . world . rot ; const eff = vec3 . transformQuat ( [ 0 , 0 , 0 ] , lnk . effectorDir , rot ) ; const pole = vec3 . transformQuat ( [ 0 , 0 , 0 ] , lnk . poleDir , rot ) ; this . setTargetDir ( eff , pole ) ; } return this ; } setTargetDir ( e , pole ? , effectorScale ? ) { this . _isTarPosition = false ; this . effectorDir [ 0 ] = e [ 0 ] ; this . effectorDir [ 1 ] = e [ 1 ] ; this . effectorDir [ 2 ] = e [ 2 ] ; if ( pole ) this . setTargetPole ( pole ) ; if ( effectorScale ) this . effectorScale = effectorScale ; return this ; } setTargetPos ( v , pole ? ) { this . _isTarPosition = true ; this . effectorPos [ 0 ] = v [ 0 ] ; this . effectorPos [ 1 ] = v [ 1 ] ; this . effectorPos [ 2 ] = v [ 2 ] ; if ( pole ) this . setTargetPole ( pole ) ; return this ; } setTargetPole ( v ) { this . _originPoleDir [ 0 ] = v [ 0 ] ; this . _originPoleDir [ 1 ] = v [ 1 ] ; this . _originPoleDir [ 2 ] = v [ 2 ] ; return this ; } resolve ( chain , pose , debug ? ) { const [ rot , pt ] = this . getWorldRot ( chain , pose , debug ) ; QuatUtil . pmulInvert ( rot , rot , pt . rot ) ; pose . setLocalRot ( chain . links [ 0 ] . idx , rot ) ; } ikDataFromPose ( chain , pose , out ) { const dir = [ 0 , 0 , 0 ] ; const lnk = chain . first ( ) ; const b = pose . bones [ lnk . idx ] ; vec3 . transformQuat ( dir , lnk . effectorDir , b . world . rot ) ; vec3 . normalize ( out . effectorDir , dir ) ; vec3 . transformQuat ( dir , lnk . poleDir , b . world . rot ) ; vec3 . normalize ( out . poleDir , dir ) ; } /** Update Target Data  */ _update ( origin ) { const v = [ 0 , 0 , 0 ] ; const o = [ 0 , 0 , 0 ] ; if ( this . _isTarPosition ) { vec3 . sub ( v , this . effectorPos , origin ) ; vec3 . normalize ( this . effectorDir , v ) ; } vec3 . cross ( v , this . _originPoleDir , this . effectorDir ) ; vec3 . normalize ( this . orthoDir , v ) ; vec3 . cross ( v , this . effectorDir , this . orthoDir ) ; vec3 . normalize ( this . poleDir , v ) ; vec3 . copy ( this . originPos , origin ) ; } getWorldRot ( chain , pose , debug ? ) { const pt = new Transform ( ) ; const ct = new Transform ( ) ; let lnk = chain . first ( ) ; if ( lnk . pidx == - 1 ) pt . copy ( pose . offset ) ; else pose . getWorldTransform ( lnk . pidx , pt ) ; ct . fromMul ( pt , lnk . bind ) ; this . _update ( ct . pos ) ; const rot = quat . copy ( [ 0 , 0 , 0 , 1 ] , ct . rot ) ; const dir = [ 0 , 0 , 0 ] ; const q = [ 0 , 0 , 0 , 1 ] ; vec3 . transformQuat ( dir , lnk . effectorDir , ct . rot ) ; quat . rotationTo ( q , dir , this . effectorDir ) ; quat . mul ( rot , q , rot ) ; vec3 . transformQuat ( dir , lnk . poleDir , rot ) ; quat . rotationTo ( q , dir , this . poleDir ) ; quat . mul ( rot , q , rot ) ; if ( ! this . _isTarPosition ) { this . effectorPos [ 0 ] = this . originPos [ 0 ] + this . effectorDir [ 0 ] * chain . length * this . effectorScale ; this . effectorPos [ 1 ] = this . originPos [ 1 ] + this . effectorDir [ 1 ] * chain . length * this . effectorScale ; this . effectorPos [ 2 ] = this . originPos [ 2 ] + this . effectorDir [ 2 ] * chain . length * this . effectorScale ; } return [ rot , pt ] ; } } export default SwingTwistSolver ;