export function radiansToDegrees ( radians ) { return radians * ( 180 / Math . PI ) ; } /**
 * Transform coords on the middle of a SVG's Rect to top left coords.
 */ export const transformMiddleCoordsOfRectToSVGBeginPoint = ( x , y , width , depth ) : [ x : number , y : number ] => { return [ x - width / 2 , y + depth / 2 ] ; } ; export interface Pose2D { x : number ; y : number ; theta : number ; } export interface Segment { initialPose : number ; finalPose : number ; initialVelocity : number ; finalVelocity : number ; initialTime : number ; finalTime : number ; } export interface CoefficientSet { a : number ; b : number ; c : number ; d : number ; } export interface Velocity { x : number ; y : number ; theta : number ; } export interface Knot { pose : Pose2D ; velocity : Velocity ; time : number ; } export interface SegmentCoefficients { x : CoefficientSet ; y : CoefficientSet ; theta : CoefficientSet ; initialTime : number ; finalTime : number ; } export function segmentToCoefficientSet ( segment ) { const { initialPose : x0 , finalPose : x1 , initialVelocity : v0 , finalVelocity : v1 , initialTime , finalTime , } = segment ; const dt = finalTime - initialTime ; const w0 = v0 / dt ; const w1 = v1 / dt ; const a = w1 + w0 - x1 * 2 + x0 * 2 ; const b = - w1 - w0 * 2 + x1 * 3 - x0 * 3 ; return { a , b , c : w0 , d : x0 , } ; } export function assignKnotsToSegment ( knot , nextKnot , forCoordinate ) { return { initialPose : knot . pose [ forCoordinate ] , finalPose : nextKnot . pose [ forCoordinate ] , initialVelocity : knot . velocity [ forCoordinate ] , finalVelocity : nextKnot . velocity [ forCoordinate ] , initialTime : knot . time , finalTime : nextKnot . time , } ; } export function knotsToSegmentCoefficientsArray ( knots ) { const scs = [ ] ; for ( let i = 0 ; i < knots . length - 1 ; ++ i ) { const knot = knots [ i ] ; const nextKnot = knots [ i + 1 ] ; const sc = { x : segmentToCoefficientSet ( assignKnotsToSegment ( knot , nextKnot , 'x' ) ) , y : segmentToCoefficientSet ( assignKnotsToSegment ( knot , nextKnot , 'y' ) ) , theta : segmentToCoefficientSet ( assignKnotsToSegment ( knot , nextKnot , 'theta' ) ) , initialTime : knot . time , finalTime : nextKnot . time , } ; scs . push ( sc ) ; } return scs ; } export function getInterpolatedTime ( initialTime , finalTime , time ) { return ( time - initialTime ) / ( finalTime - initialTime ) ; } export function resolveSpline ( coefficientSet , interpolatedTime , interpolatedTimePow2 = interpolatedTime ** 2 , interpolatedTimePow3 = interpolatedTime ** 3 ) { return ( coefficientSet . a * interpolatedTimePow3 + coefficientSet . b * interpolatedTimePow2 + coefficientSet . c * interpolatedTime + coefficientSet . d ) ; } export function getPositionFromSegmentCoefficientsArray ( time , scs ) { let sc ; for ( sc of scs ) { if ( time >= sc . initialTime && time <= sc . finalTime ) { break ; } } if ( ! sc ) { return null ; } const { x : xCoeff , y : yCoeff , theta : thetaCoeff , initialTime , finalTime } = sc ; const interpolatedTime = getInterpolatedTime ( initialTime , finalTime , time ) ; const interpolatedTimePow2 = interpolatedTime ** 2 ; const interpolatedTimePow3 = interpolatedTime ** 3 ; return { x : resolveSpline ( xCoeff , interpolatedTime , interpolatedTimePow2 , interpolatedTimePow3 ) , y : resolveSpline ( yCoeff , interpolatedTime , interpolatedTimePow2 , interpolatedTimePow3 ) , theta : resolveSpline ( thetaCoeff , interpolatedTime , interpolatedTimePow2 , interpolatedTimePow3 ) , } ; } function bezierHelper ( coeffs ) { const a = coeffs . a ; const b = coeffs . b ; const c = coeffs . c ; const d = coeffs . d ; const p0 = d ; const p1 = ( c + p0 * 3 ) / 3 ; const p2 = ( b - p0 * 3 + p1 * 6 ) / 3 ; const p3 = a + p0 - p1 * 3 + p2 * 3 ; return [ p0 , p1 , p2 , p3 ] ; } export function bezierControlPoints ( segmentCoefficients ) { const px = bezierHelper ( segmentCoefficients . x ) ; const py = bezierHelper ( segmentCoefficients . y ) ; return px . map ( ( x , i ) => [ x , py [ i ] ] ) ; } param pos export function toRmfCoords ( pos ) { return [ pos [ 0 ] , - pos [ 1 ] ] ; } param pos export function fromRmfCoords ( pos ) { return [ pos [ 0 ] , - pos [ 1 ] ] ; } param yaw export function toRmfYaw ( yaw ) { return - yaw ; } param yaw export function fromRmfYaw ( yaw ) { return - yaw ; }