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 , vec3 } from 'gl-matrix' ; import SwingTwistBase from './support/SwingTwistBase' ; const PI_2 = Math . PI * 2 ; const OFFSETS = [ 0 , 0.025 , 0.07 , 0.12808739578845105 , 0.19558255045541925 , 0.2707476090618156 , 0.35128160042581186 , 0.43488355336557927 , 0.5192524966992895 , 0.6051554208208854 , 0.6887573737606529 , 0.7692913651246491 , 0.8444564237310455 , 0.9119515783980137 , 0.9694758579437253 , 1.0124273200045233 , 1.034670041428865 , 1.026233147095494 , 0.966407896367954 , 0.8053399136399616 , 0 ] ; function getOffset ( t ) { if ( t <= 0.03 || t >= 1 ) return 0 ; const i = 20 * t ; const idx = Math . floor ( i ) ; const fract = i - idx ; const a = OFFSETS [ idx ] ; const b = OFFSETS [ idx + 1 ] ; return a * ( 1 - fract ) + b * fract ; } class ArcSolver extends SwingTwistBase { bendDir : number = 1 ; resolve ( chain , pose , debug ? ) { const ST = this . _swingTwist ; const [ rot , pt ] = ST . getWorldRot ( chain , pose , debug ) ; const eff_len = Vec3Util . len ( ST . effectorPos , ST . originPos ) ; let len_scl = eff_len / chain . length ; if ( len_scl >= 0.999 ) { QuatUtil . pmulInvert ( rot , rot , pt . rot ) ; pose . setLocalRot ( chain . links [ 0 ] . idx , rot ) ; return ; } const arc_ang = PI_2 * ( 1 - len_scl ) + getOffset ( len_scl ) ; const arc_inc = arc_ang / chain . count * this . bendDir ; const q_inc = quat . setAxisAngle ( [ 0 , 0 , 0 , 0 ] , ST . orthoDir , arc_inc ) ; const final_inv = quat . invert ( [ 0 , 0 , 0 , 0 ] , pt . rot ) ; let lnk = chain . links [ 0 ] ; quat . mul ( rot , q_inc , rot ) ; const init_rot = quat . copy ( [ 0 , 0 , 0 , 0 ] , rot ) ; QuatUtil . pmulInvert ( rot , rot , pt . rot ) ; pt . mul ( rot , lnk . bind . pos , lnk . bind . scl ) ; let i ; for ( i = 1 ; i < chain . count ; i ++ ) { lnk = chain . links [ i ] ; quat . mul ( rot , pt . rot , lnk . bind . rot ) ; quat . mul ( rot , q_inc , rot ) ; QuatUtil . pmulInvert ( rot , rot , pt . rot ) ; pose . setLocalRot ( lnk . idx , rot ) ; pt . mul ( rot , lnk . bind . pos , lnk . bind . scl ) ; } const tail_pos = pt . transformVec3 ( [ 0 , lnk . len , 0 ] ) ; const tail_dir = vec3 . sub ( [ 0 , 0 , 0 ] , tail_pos , ST . originPos ) ; vec3 . normalize ( tail_dir , tail_dir ) ; quat . rotationTo ( rot , tail_dir , ST . effectorDir ) ; quat . mul ( rot , rot , init_rot ) ; quat . mul ( rot , final_inv , rot ) ; pose . setLocalRot ( chain . links [ 0 ] . idx , rot ) ; } } export default ArcSolver ;