import type Pose from '../../armature/Pose' ; import type { IKChain } from '../rigs/IKChain' ; import QuatUtil from '../../maths/QuatUtil' ; import Vec3Util from '../../maths/Vec3Util' ; import { quat } from 'gl-matrix' ; import SwingTwistBase from './support/SwingTwistBase' ; function lawcos_sss ( aLen , bLen , cLen ) { let v = ( aLen * aLen + bLen * bLen - cLen * cLen ) / ( 2 * aLen * bLen ) ; if ( v < - 1 ) v = - 1 ; else if ( v > 1 ) v = 1 ; return Math . acos ( v ) ; } class ZSolver extends SwingTwistBase { resolve ( chain , pose , debug ? ) { const ST = this . _swingTwist ; const [ rot , pt ] = ST . getWorldRot ( chain , pose , debug ) ; const b0 = chain . links [ 0 ] ; const b1 = chain . links [ 1 ] ; const b2 = chain . links [ 2 ] ; const a_len = b0 . len ; const b_len = b1 . len ; const c_len = b2 . len ; const mh_len = b1 . len * 0.5 ; const eff_len = Vec3Util . len ( ST . effectorPos , ST . originPos ) ; const t_ratio = ( a_len + mh_len ) / ( a_len + b_len + c_len ) ; const ta_len = eff_len * t_ratio ; const tb_len = eff_len - ta_len ; const prot = [ 0 , 0 , 0 , 0 ] ; const prot2 = [ 0 , 0 , 0 , 0 ] ; let rad ; rad = lawcos_sss ( a_len , ta_len , mh_len ) ; QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , - rad , rot ) ; quat . copy ( prot , rot ) ; QuatUtil . pmulInvert ( rot , rot , pt . rot ) ; pose . setLocalRot ( b0 . idx , rot ) ; rad = Math . PI - lawcos_sss ( a_len , mh_len , ta_len ) ; quat . mul ( rot , prot , b1 . bind . rot ) ; QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , rad , rot ) ; quat . copy ( prot2 , rot ) ; QuatUtil . pmulInvert ( rot , rot , prot ) ; pose . setLocalRot ( b1 . idx , rot ) ; rad = Math . PI - lawcos_sss ( c_len , mh_len , tb_len ) ; quat . mul ( rot , prot2 , b2 . bind . rot ) ; QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , - rad , rot ) ; QuatUtil . pmulInvert ( rot , rot , prot2 ) ; pose . setLocalRot ( b2 . idx , rot ) ; } } export default ZSolver ;