import type Pose from '../../armature/Pose' ; import type { IKChain , IKLink } from '../rigs/IKChain' ; import QuatUtil from '../../maths/QuatUtil' ; import Vec3Util from '../../maths/Vec3Util' ; import { vec3 } from 'gl-matrix' ; import SwingTwistBase from './support/SwingTwistBase' ; class PistonSolver extends SwingTwistBase { resolve ( chain , pose , debug ? ) { const ST = this . _swingTwist ; const [ rot , pt ] = ST . getWorldRot ( chain , pose , debug ) ; const effLen = Vec3Util . len ( ST . effectorPos , ST . originPos ) ; const v = [ 0 , 0 , 0 ] ; let lnk = chain . first ( ) ; let i ; QuatUtil . pmulInvert ( rot , rot , pt . rot ) ; pose . setLocalRot ( lnk . idx , rot ) ; for ( lnk of chain . links ) { if ( lnk . len >= effLen ) { for ( i = 1 ; i < chain . count ; i ++ ) pose . setLocalPos ( chain . links [ i ] . idx , [ 0 , 0 , 0 ] ) ; return ; } } const endIdx = chain . count - 1 ; const deltaLen = chain . length - effLen ; const incInv = 1 / ( chain . length - chain . links [ endIdx ] . len ) ; for ( i = 0 ; i < endIdx ; i ++ ) { lnk = chain . links [ i ] ; v [ 1 ] = lnk . len - deltaLen * ( lnk . len * incInv ) ; pose . setLocalPos ( chain . links [ i + 1 ] . idx , v ) ; } } } export default PistonSolver ;