import type Bone from '../../armature/Bone' ; import type Pose from '../../armature/Pose' ; import type { IKChain , IKLink } from '../rigs/IKChain' ; import type { ISolver } from './support/ISolver' ; import type { IKData } from '..' ; import { vec3 , quat } from 'gl-matrix' ; import QuatUtil from '../../maths/QuatUtil' ; class SwingTwistEndsSolver implements ISolver { startEffectorDir : vec3 = [ 0 , 0 , 0 ] ; startPoleDir : vec3 = [ 0 , 0 , 0 ] ; endEffectorDir : vec3 = [ 0 , 0 , 0 ] ; endPoleDir : vec3 = [ 0 , 0 , 0 ] ; initData ( pose ? , chain ? ) { if ( pose && chain ) { const pole = [ 0 , 0 , 0 ] ; const eff = [ 0 , 0 , 0 ] ; let rot ; let lnk ; lnk = chain . first ( ) ; rot = pose . bones [ lnk . idx ] . world . rot ; vec3 . transformQuat ( eff , lnk . effectorDir , rot ) ; vec3 . transformQuat ( pole , lnk . poleDir , rot ) ; this . setStartDir ( eff , pole ) ; lnk = chain . last ( ) ; rot = pose . bones [ lnk . idx ] . world . rot ; vec3 . transformQuat ( eff , lnk . effectorDir , rot ) ; vec3 . transformQuat ( pole , lnk . poleDir , rot ) ; this . setEndDir ( eff , pole ) ; } return this ; } setStartDir ( eff , pole ) { this . startEffectorDir [ 0 ] = eff [ 0 ] ; this . startEffectorDir [ 1 ] = eff [ 1 ] ; this . startEffectorDir [ 2 ] = eff [ 2 ] ; this . startPoleDir [ 0 ] = pole [ 0 ] ; this . startPoleDir [ 1 ] = pole [ 1 ] ; this . startPoleDir [ 2 ] = pole [ 2 ] ; return this ; } setEndDir ( eff , pole ) { this . endEffectorDir [ 0 ] = eff [ 0 ] ; this . endEffectorDir [ 1 ] = eff [ 1 ] ; this . endEffectorDir [ 2 ] = eff [ 2 ] ; this . endPoleDir [ 0 ] = pole [ 0 ] ; this . endPoleDir [ 1 ] = pole [ 1 ] ; this . endPoleDir [ 2 ] = pole [ 2 ] ; return this ; } resolve ( chain , pose , debug ? ) { const iEnd = chain . count - 1 ; const pRot = [ 0 , 0 , 0 , 1 ] ; const cRot = [ 0 , 0 , 0 , 1 ] ; const ikEffe = [ 0 , 0 , 0 ] ; const ikPole = [ 0 , 0 , 0 ] ; const dir = [ 0 , 0 , 0 ] ; const rot = [ 0 , 0 , 0 , 1 ] ; const tmp = [ 0 , 0 , 0 , 1 ] ; let lnk = chain . first ( ) ; let t ; if ( lnk . pidx != - 1 ) pose . getWorldRotation ( lnk . pidx , pRot ) ; else quat . copy ( pRot , pose . offset . rot ) ; for ( let i = 0 ; i <= iEnd ; i ++ ) { t = i / iEnd ; lnk = chain . links [ i ] ; vec3 . lerp ( ikEffe , this . startEffectorDir , this . endEffectorDir , t ) ; vec3 . lerp ( ikPole , this . startPoleDir , this . endPoleDir , t ) ; quat . mul ( cRot , pRot , lnk . bind . rot ) ; vec3 . transformQuat ( dir , lnk . effectorDir , cRot ) ; quat . rotationTo ( rot , dir , ikEffe ) ; quat . mul ( cRot , rot , cRot ) ; vec3 . transformQuat ( dir , lnk . poleDir , cRot ) ; quat . rotationTo ( rot , dir , ikPole ) ; quat . mul ( cRot , rot , cRot ) ; quat . copy ( tmp , cRot ) ; QuatUtil . pmulInvert ( cRot , cRot , pRot ) ; pose . setLocalRot ( lnk . idx , cRot ) ; if ( i != iEnd ) quat . copy ( pRot , tmp ) ; } } ikDataFromPose ( chain , pose , out ) { const dir = [ 0 , 0 , 0 ] ; let lnk ; let b ; lnk = chain . first ( ) ; b = pose . bones [ lnk . idx ] ; vec3 . transformQuat ( dir , lnk . effectorDir , b . world . rot ) ; vec3 . normalize ( out . startEffectorDir , dir ) ; vec3 . transformQuat ( dir , lnk . poleDir , b . world . rot ) ; vec3 . normalize ( out . startPoleDir , dir ) ; lnk = chain . last ( ) ; b = pose . bones [ lnk . idx ] ; vec3 . transformQuat ( dir , lnk . effectorDir , b . world . rot ) ; vec3 . normalize ( out . endEffectorDir , dir ) ; vec3 . transformQuat ( dir , lnk . poleDir , b . world . rot ) ; vec3 . normalize ( out . endPoleDir , dir ) ; } } export default SwingTwistEndsSolver ;