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 trapezoid_calculator ( lbase , sbase , lleg , rleg ) { if ( lbase < sbase ) { console . log ( "Long Base Must Be Greater Than Short Base" ) ; return null ; } ; let h2 = ( lbase + lleg + sbase + rleg ) * ( lbase * - 1 + lleg + sbase + rleg ) * ( lbase - lleg - sbase + rleg ) * ( lbase + lleg - sbase - rleg ) / ( 4 * ( ( lbase - sbase ) * ( lbase - sbase ) ) ) ; if ( h2 < 0 ) { console . log ( "A Trapezoid With These Dimensions Cannot Exist" ) ; return null ; } ; let median = ( lbase + sbase ) * 0.5 ; let diff = lbase - sbase ; let xval = ( lleg ** 2 + diff ** 2 - rleg ** 2 ) / ( 2 * diff ) ; let height = Math . sqrt ( lleg ** 2 - xval ** 2 ) ; let adj = diff - xval ; let angA = Math . atan ( height / xval ) ; if ( angA < 0 ) angA = angA + Math . PI ; let angB = Math . PI - angA ; let angD = Math . atan ( height / adj ) ; if ( angD < 0 ) angD = angD + Math . PI ; let angC = Math . PI - angD ; return [ angA , angB , angC , angD ] ; } class TrapezoidSolver extends SwingTwistBase { bendDir : number = 1 ; invertBend ( ) { this . bendDir = - this . bendDir ; return this ; } 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 lft_len = b0 . len ; const top_len = b1 . len ; const rit_len = b2 . len ; const bot_len = Vec3Util . len ( ST . effectorPos , ST . originPos ) ; ; const qprev = quat . copy ( [ 0 , 0 , 0 , 1 ] , pt . rot ) ; const qnext = [ 0 , 0 , 0 , 1 ] ; let ang ; if ( bot_len >= top_len ) { ang = trapezoid_calculator ( bot_len , top_len , lft_len , rit_len ) ; if ( ! ang ) return ; } else { ang = trapezoid_calculator ( top_len , bot_len , rit_len , lft_len ) ; if ( ! ang ) return ; ang = [ ang [ 2 ] , ang [ 3 ] , ang [ 0 ] , ang [ 1 ] ] ; } QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , - ang [ 0 ] * this . bendDir , rot ) ; quat . copy ( qnext , rot ) ; QuatUtil . pmulInvert ( rot , rot , qprev ) ; pose . setLocalRot ( b0 . idx , rot ) ; quat . copy ( qprev , qnext ) ; quat . mul ( rot , qprev , b1 . bind . rot ) ; QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , - ( Math . PI + ang [ 1 ] * this . bendDir ) , rot ) ; quat . copy ( qnext , rot ) ; QuatUtil . pmulInvert ( rot , rot , qprev ) ; pose . setLocalRot ( b1 . idx , rot ) ; quat . mul ( rot , qnext , b2 . bind . rot ) ; QuatUtil . pmulAxisAngle ( rot , ST . orthoDir , - ( Math . PI + ang [ 2 ] * this . bendDir ) , rot ) ; QuatUtil . pmulInvert ( rot , rot , qnext ) ; pose . setLocalRot ( b2 . idx , rot ) ; } } export default TrapezoidSolver ;