import { add , distance2 , set , sub , v2 , magnitude , scale } from "./v2" ; import { Integratable } from "./common-types" ; export function solveDistanceConstraint ( p1 , p1mass , p2 , p2mass , goal , stiffness = 1 , impartEnergy = true ) { const mass1 = p1mass > 0 ? p1mass : 1 ; const mass2 = p2mass > 0 ? p2mass : 1 ; const imass1 = 1 / ( mass1 || 1 ) ; const imass2 = 1 / ( mass2 || 1 ) ; const imass = imass1 + imass2 ; const delta = sub ( v2 ( ) , p2 . cpos , p1 . cpos ) ; const deltaMag = magnitude ( delta ) ; if ( deltaMag === 0 ) return ; const diff = ( deltaMag - goal ) / deltaMag ; scale ( delta , delta , diff / imass ) ; const p1correction = scale ( v2 ( ) , delta , imass1 * stiffness ) ; const p2correction = scale ( v2 ( ) , delta , imass2 * stiffness ) ; if ( p1mass > 0 ) { add ( p1 . cpos , p1 . cpos , p1correction ) ; if ( ! impartEnergy ) add ( p1 . ppos , p1 . ppos , p1correction ) ; } else if ( p2mass > 0 ) { sub ( p2 . cpos , p2 . cpos , p1correction ) ; if ( ! impartEnergy ) sub ( p2 . ppos , p2 . ppos , p1correction ) ; } if ( p2mass > 0 ) { sub ( p2 . cpos , p2 . cpos , p2correction ) ; if ( ! impartEnergy ) sub ( p2 . ppos , p2 . ppos , p2correction ) ; } else if ( p1mass > 0 ) { add ( p1 . cpos , p1 . cpos , p2correction ) ; if ( ! impartEnergy ) add ( p1 . ppos , p1 . ppos , p2correction ) ; } }