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 LimbSolver extends SwingTwistBase { resolve ( chain , pose , debug ? ) { const ST = this . _swingTwist ; const [ rot , pt ] = ST . getWorldRot ( chain , pose , debug ) ; let b0 = chain . links [ 0 ] , b1 = chain . links [ 1 ] , alen = b0 . len , blen = b1 . len , clen = Vec3Util . len ( ST . effectorPos , ST . originPos ) , prot = [ 0 , 0 , 0 , 0 ] , rad ; rad = lawcos_sss ( alen , clen , blen ) ; 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 ( alen , blen , clen ) ; quat . mul ( rot , prot , b1 . bind . rot ) ; QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , rad , rot ) ; QuatUtil . pmulInvert ( rot , rot , prot ) ; pose . setLocalRot ( b1 . idx , rot ) ; } } export default LimbSolver ;