import type Pose from '../../armature/Pose' ; import type { IKChain , IKLink } 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 SpringSolver 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 qprev = quat . copy ( [ 0 , 0 , 0 , 1 ] , pt . rot ) ; const qnext = [ 0 , 0 , 0 , 1 ] ; let lnk = chain . links [ 0 ] ; const boneLen = lnk . len ; const baseLen = effLen / ( chain . count / 2 ) ; const rad_a = lawcos_sss ( boneLen , baseLen , boneLen ) ; const rad_b = Math . PI - lawcos_sss ( boneLen , boneLen , baseLen ) ; const r_axis_an = quat . setAxisAngle ( [ 0 , 0 , 0 , 1 ] , ST . orthoDir , - rad_a ) ; const r_axis_b = quat . setAxisAngle ( [ 0 , 0 , 0 , 1 ] , ST . orthoDir , rad_b ) ; quat . mul ( rot , r_axis_an , rot ) ; quat . copy ( qnext , rot ) ; QuatUtil . pmulInvert ( rot , rot , qprev ) ; pose . setLocalRot ( lnk . idx , rot ) ; quat . copy ( qprev , qnext ) ; quat . setAxisAngle ( r_axis_an , ST . orthoDir , rad_a * - 2 ) ; let r_axis ; for ( let i = 1 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; r_axis = ( ( i & 1 ) == 0 ) ? r_axis_an : r_axis_b ; quat . mul ( rot , qprev , lnk . bind . rot ) ; quat . mul ( rot , r_axis , rot ) ; quat . copy ( qnext , rot ) ; QuatUtil . pmulInvert ( rot , rot , qprev ) ; pose . setLocalRot ( lnk . idx , rot ) ; quat . copy ( qprev , qnext ) ; } } } export default SpringSolver ;