import type { TVec3 } from '@oito/type' ; import type { Pose } from '@oito/armature' ; import type { IKChain , Link } from '../rigs/IKChain' ; import type { ISolver } from './ISolver' ; import { Vec3 , Transform , Quat } from '@oito/core' ; class STDirectionSet { effectorDir = [ 0 , 0 , 0 ] ; poleDir = [ 0 , 0 , 0 ] ; } class SwingTwistChainSolver implements ISolver { directionSet ! : Array < STDirectionSet > ; initData ( pose ? , chain ? ) { if ( pose && chain ) { const v = new Vec3 ( ) ; let rot ; let lnk ; let ds ; this . directionSet = new Array ( chain . count ) ; for ( let i = 0 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; rot = pose . bones [ lnk . idx ] . world . rot ; ds = new STDirectionSet ( ) ; v . fromQuat ( rot , lnk . effectorDir ) . copyTo ( ds . effectorDir ) ; v . fromQuat ( rot , lnk . poleDir ) . copyTo ( ds . effectorDir ) ; this . directionSet [ i ] = ds ; } } return this ; } setChainDir ( eff , pole ) { const cnt = eff . length ; let ds ; for ( let i = 0 ; i < cnt ; i ++ ) { ds = this . directionSet [ i ] ; ds . effectorDir [ 0 ] = eff [ i ] [ 0 ] ; ds . effectorDir [ 1 ] = eff [ i ] [ 1 ] ; ds . effectorDir [ 2 ] = eff [ i ] [ 2 ] ; ds . poleDir [ 0 ] = pole [ i ] [ 0 ] ; ds . poleDir [ 1 ] = pole [ i ] [ 1 ] ; ds . poleDir [ 2 ] = pole [ i ] [ 2 ] ; } return this ; } resolve ( chain , pose , debug ? ) { const iEnd = chain . count - 1 ; const pRot = new Quat ( ) ; const cRot = new Quat ( ) ; const dir = new Vec3 ( ) ; const rot = new Quat ( ) ; const tmp = [ 0 , 0 , 0 , 1 ] ; let lnk = chain . first ( ) ; let ds ; if ( lnk . pidx != - 1 ) pose . getWorldRotation ( lnk . pidx , pRot ) ; else pRot . copy ( pose . offset . rot ) ; for ( let i = 0 ; i <= iEnd ; i ++ ) { lnk = chain . links [ i ] ; ds = this . directionSet [ i ] ; cRot . fromMul ( pRot , lnk . bind . rot ) ; dir . fromQuat ( cRot , lnk . effectorDir ) ; rot . fromUnitVecs ( dir , ds . effectorDir ) ; cRot . pmul ( rot ) ; dir . fromQuat ( cRot , lnk . poleDir ) ; rot . fromUnitVecs ( dir , ds . poleDir ) ; cRot . pmul ( rot ) ; cRot . copyTo ( tmp ) ; cRot . pmulInvert ( pRot ) ; pose . setLocalRot ( lnk . idx , cRot ) ; if ( i != iEnd ) pRot . copy ( tmp ) ; } } } export default SwingTwistChainSolver ;