diff --git a/src/calculations/orbit-calculations.ts b/src/calculations/orbit-calculations.ts index 3c6a89e..0d52bbc 100644 --- a/src/calculations/orbit-calculations.ts +++ b/src/calculations/orbit-calculations.ts @@ -1454,18 +1454,6 @@ export function findOrbitThroughInterpolation(ownCoordinates: OrbitalCoordinates const [firstDistanceAlongDirection, firstDistancePerpendicularToDirection] = getDistances(interpolationParameters.firstOwnAltitude, interpolationParameters.firstTargetAltitude, interpolationParameters.firstDistance); const [secondDistanceAlongDirection, secondDistancePerpendicularToDirection] = getDistances(interpolationParameters.secondOwnAltitude, interpolationParameters.secondTargetAltitude, interpolationParameters.secondDistance); - let cosOfAngleOne = firstDistanceAlongDirection * Math.tan(interpolationParameters.firstPhaseAngle) / firstDistancePerpendicularToDirection;; - let cosOfAngleTwo = secondDistanceAlongDirection * Math.tan(interpolationParameters.secondPhaseAngle) / secondDistancePerpendicularToDirection;; - - // If either of these angles are outside the allowable values, clip them down - if (Math.abs(cosOfAngleOne) > 1) { - cosOfAngleOne /= Math.abs(cosOfAngleOne); - } - - if (Math.abs(cosOfAngleTwo) > 1) { - cosOfAngleTwo /= Math.abs(cosOfAngleTwo); - } - let results: [OrbitalCoordinates, number][] = []; let epochTrueAnomaly = ownCoordinates.trueAnomaly; while (epochTrueAnomaly + 2 * Math.PI < ownFirstTrueAnomaly) { @@ -1473,297 +1461,125 @@ export function findOrbitThroughInterpolation(ownCoordinates: OrbitalCoordinates } const timeElapsed = getTimeBetweenTrueAnomalies(epochTrueAnomaly, ownSecondTrueAnomaly, ownCoordinates.orbit, body); - const anglesToDistanceFunction = (angleOne: number, angleTwo: number): number => { - let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]), - addVector( - multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]), - multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2]) - ) - ); - - let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]), - addVector( - multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]), - multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2]) - ) - ); - - let distance = getVectorMagnitude(addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne))); - return distance - expectedDistance; - }; - - const anglesToDistancePartialDerivativeWrtAngleOne = (angleOne: number, angleTwo: number): number => { - let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]), - addVector( - multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]), - multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2]) - ) - ); - - let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]), - addVector( - multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]), - multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2]) - ) - ); - - let difference = addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne)); - let distance = Math.abs(getVectorMagnitude(difference)); - - let positionOneDerivative = addVector( - multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]), - multiplyMatrixWithScalar(-Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2]) - ); - - return vectorDotProduct(difference, positionOneDerivative) / distance; - } - - const anglesToDistancePartialDerivativeWrtAngleTwo = (angleOne: number, angleTwo: number): number => { - let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]), - addVector( - multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]), - multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2]) - ) - ); - - let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]), - addVector( - multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]), - multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2]) - ) - ); - - let difference = addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne)); - let distance = Math.abs(getVectorMagnitude(difference)); - - let positionTwoDerivative = addVector( - multiplyMatrixWithScalar(-Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]), - multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2]) - ); - - return vectorDotProduct(difference, positionTwoDerivative) / distance; - }; - - let firstExpectedPlaneAngle = (ownFirstTrueAnomaly + interpolationParameters.firstPhaseAngle); - let secondExpectedPlaneAngle = (ownSecondTrueAnomaly + interpolationParameters.secondPhaseAngle); - let planeAngleDifference = secondExpectedPlaneAngle - firstExpectedPlaneAngle; - while (planeAngleDifference <= -Math.PI) { - planeAngleDifference += 2 * Math.PI; - } - while (planeAngleDifference > Math.PI) { - planeAngleDifference -= 2 * Math.PI; - } - - const planeAngleFunction = (angleOne: number, angleTwo: number) => { - const horizontalOne = addVector( - multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]), - multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]) - ); - - const horizontalTwo = addVector( - multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]), - multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]) - ); - - return vectorDotProduct(horizontalOne, horizontalTwo) / (getVectorMagnitude(horizontalOne) * getVectorMagnitude(horizontalTwo)) - Math.cos(planeAngleDifference); - } - - const planeAnglePartialDerivativeAngleOne = (angleOne: number, angleTwo: number) => { - let directionOne = multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]); - let perpendicularOne = multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]); - - let directionTwo = multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[1]); - let perpendicularTwo = multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]); - - const horizontalOne = addVector( - directionOne, - perpendicularOne - ); - - const horizontalTwo = addVector( - directionTwo, - perpendicularTwo - ); - - return ( - -Math.sin(angleOne) * vectorDotProduct(perpendicularOne, directionTwo) - -Math.sin(angleOne) * Math.cos(angleTwo) * vectorDotProduct(perpendicularOne, perpendicularTwo) - -Math.cos(angleOne) * Math.sin(angleOne) * vectorDotProduct(perpendicularOne, perpendicularOne) * vectorDotProduct(horizontalOne, horizontalTwo) / getVectorMagnitude(horizontalOne) - ) / (getVectorMagnitude(horizontalOne) * getVectorMagnitude(horizontalTwo)); - } - - const planeAnglePartialDerivativeAngleTwo = (angleOne: number, angleTwo: number) => { - let directionOne = multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]); - let perpendicularOne = multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]); - - let directionTwo = multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[1]); - let perpendicularTwo = multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]); - - const horizontalOne = addVector( - directionOne, - perpendicularOne - ); - - const horizontalTwo = addVector( - directionTwo, - perpendicularTwo - ); - - return ( - -Math.sin(angleTwo) * vectorDotProduct(perpendicularTwo, directionOne) - -Math.sin(angleTwo) * Math.cos(angleOne) * vectorDotProduct(perpendicularTwo, perpendicularOne) - -Math.cos(angleTwo) * Math.sin(angleTwo) * vectorDotProduct(perpendicularTwo, perpendicularOne) * vectorDotProduct(horizontalOne, horizontalTwo) / getVectorMagnitude(horizontalTwo) - ) / (getVectorMagnitude(horizontalOne) * getVectorMagnitude(horizontalTwo)) - } - - - - - let previouslyFoundAngles: [number, number][] = []; - // Try all four possible arrangements of angles [-1, 1].forEach(angleOneMultiplier => { [-1, 1].forEach(angleTwoMultiplier => { - let angleOne = angleOneMultiplier * Math.acos(cosOfAngleOne); - let angleTwo = angleTwoMultiplier * Math.acos(cosOfAngleTwo); - // Do some gradient descent to further optimize the angles - for (var i = 0; i < 10000; i++) { - // We'll descend one dimension at a time - let currentDistance = anglesToDistanceFunction(angleOne, angleTwo); - let dfD1 = anglesToDistancePartialDerivativeWrtAngleOne(angleOne, angleTwo); + const phaseAnglesToDistanceFunctionWithDerivatives = (phaseAngleOne: number, phaseAngleTwo: number): [number, number, number] => { + let angleOne = angleOneMultiplier * Math.acos(firstDistanceAlongDirection*Math.tan(phaseAngleOne) / firstDistancePerpendicularToDirection); + let angleTwo = angleTwoMultiplier * Math.acos(secondDistanceAlongDirection*Math.tan(phaseAngleTwo) / secondDistancePerpendicularToDirection); - if (!dfD1 || !currentDistance) { - break; - } - - // If the distance function is below something like 10 metres, we are close enough - if (Math.abs(currentDistance) < 10) { - break; + let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]), + addVector( + multiplyMatrixWithScalar(firstDistancePerpendicularToDirection * Math.cos(angleOne), firstVectors[1]), + multiplyMatrixWithScalar(firstDistancePerpendicularToDirection * Math.sin(angleOne), firstVectors[2]) + ) + ); + + let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]), + addVector( + multiplyMatrixWithScalar(secondDistancePerpendicularToDirection * Math.cos(angleTwo), secondVectors[1]), + multiplyMatrixWithScalar(secondDistancePerpendicularToDirection * Math.sin(angleTwo), secondVectors[2]) + ) + ); + + let difference = addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne)); + let distance = getVectorMagnitude(difference); + + // Find derivative with respect to phase angles + let dAngleOneByPhaseAngleOne = -firstDistanceAlongDirection / (firstDistancePerpendicularToDirection * Math.cos(phaseAngleOne)**2 * Math.sin(angleOne)); + let dAngleTwoByPhaseAngleTwo = -secondDistanceAlongDirection / (secondDistancePerpendicularToDirection * Math.cos(phaseAngleTwo)**2 * Math.sin(angleTwo)); + + let dPositionOneByPhaseAngleOne = addVector( + multiplyMatrixWithScalar(-Math.sin(angleOne)*dAngleOneByPhaseAngleOne*firstDistancePerpendicularToDirection, firstVectors[1]), + multiplyMatrixWithScalar(Math.cos(angleOne)*dAngleOneByPhaseAngleOne*firstDistancePerpendicularToDirection, firstVectors[2]) + ); + + let dPositionTwoByPhaseAngleTwo = addVector( + multiplyMatrixWithScalar(-Math.sin(angleTwo)*dAngleTwoByPhaseAngleTwo*secondDistancePerpendicularToDirection, secondVectors[1]), + multiplyMatrixWithScalar(Math.cos(angleTwo)*dAngleTwoByPhaseAngleTwo*secondDistancePerpendicularToDirection, secondVectors[2]) + ); + + let dDistanceByPhaseAngleOne = -vectorDotProduct(difference, dPositionOneByPhaseAngleOne) / distance; + let dDistanceByPhaseAngleTwo = vectorDotProduct(difference, dPositionTwoByPhaseAngleTwo) / distance; + + return [distance - expectedDistance, dDistanceByPhaseAngleOne, dDistanceByPhaseAngleTwo]; + } + + // Try to find the phase angles that minimise the function above + let phaseAngleOne = interpolationParameters.firstPhaseAngle; + let phaseAngleTwo = interpolationParameters.secondPhaseAngle; + + for (var i = 0; i < 1000; i++) { + // Minimise phase angles one by one + let [value, dfd1, dfd2] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo); + + let updateOne = -value / dfd1; + let learningRate = 1; + + while(learningRate*updateOne > Math.PI / 3600 && Math.abs(phaseAngleOne + learningRate * updateOne - interpolationParameters.firstPhaseAngle) > Math.PI / 3600) { + learningRate *= 0.5; } + // We'll also check if the update skips over a minimum of the function let deadEndAngleOne = false; + let [testValue, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne + learningRate * updateOne, phaseAngleTwo); let counter = 0; - let update = -currentDistance / dfD1; - let learningRate = 2; - let candidateDistance; - do { + while (Math.abs(testValue) > Math.abs(value)) { counter++; - if (counter == 32) { + if (counter >= 32) { deadEndAngleOne = true; break; } learningRate *= 0.5; - candidateDistance = anglesToDistanceFunction(angleOne + learningRate * update, angleTwo); - if (!candidateDistance) { - deadEndAngleOne = true; - break; - } - } while (Math.abs(candidateDistance) > Math.abs(currentDistance)); - - if (!deadEndAngleOne) { - angleOne = (angleOne + learningRate * update) % (2 * Math.PI); + [testValue, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne + learningRate * updateOne, phaseAngleTwo); } - currentDistance = anglesToDistanceFunction(angleOne, angleTwo); - let dfD2 = anglesToDistancePartialDerivativeWrtAngleTwo(angleOne, angleTwo); + if (!deadEndAngleOne) { + phaseAngleOne += learningRate * updateOne; + } - if (!currentDistance || !dfD2) { - break; + // Now, do the same for angle two + [value, dfd1, dfd2] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo); + let updateTwo = -value / dfd2; + learningRate = 1; + + while (learningRate * updateTwo > Math.PI / 36000 && Math.abs(phaseAngleTwo + learningRate * updateTwo - interpolationParameters.secondPhaseAngle) > Math.PI / 3600) { + learningRate *= 0.5; } let deadEndAngleTwo = false; + [testValue, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo + learningRate * updateTwo); counter = 0; - update = -currentDistance / dfD2; - learningRate = 2; - do { + while (Math.abs(testValue) > Math.abs(value)) { counter++; - if (counter == 32) { + if (counter >= 32) { deadEndAngleTwo = true; break; } learningRate *= 0.5; - candidateDistance = anglesToDistanceFunction(angleOne, angleTwo + learningRate * update); - if (!candidateDistance) { - deadEndAngleTwo = true; - break; - } - } while (Math.abs(candidateDistance) > Math.abs(currentDistance)); + [testValue, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo + learningRate * updateTwo); + } if (!deadEndAngleTwo) { - angleTwo = (angleTwo + learningRate * update) % (2 * Math.PI); - while (angleTwo < 0) { - angleTwo += 2 * Math.PI; - } + phaseAngleTwo += learningRate * updateTwo; } if (deadEndAngleOne && deadEndAngleTwo) { break; } - } + }; - let distanceAway = anglesToDistanceFunction(angleOne, angleTwo); - if (!distanceAway || Math.abs(distanceAway) > 1000) { + // If we're still far away from a good solution, stop here + let [value, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo); + if (Math.abs(value) > 1000 || isNaN(value)) { return; } - // Now that we've found an optimal expected distance between the two positions, try to optimize for the correct phase angle - // Newton's method in two dimensions, baby! - const getFunctionVector = (anglesVector: number[][]) => { - return [ - [anglesToDistanceFunction(anglesVector[0][0], anglesVector[1][0])], - [planeAngleFunction(anglesVector[0][0], anglesVector[1][0])] - ] - }; - - const getJacobianMatrix = (anglesVector: number[][]) => { - let angleOne = anglesVector[0][0]; - let angleTwo = anglesVector[1][0]; - return[ - [anglesToDistancePartialDerivativeWrtAngleOne(angleOne, angleTwo), anglesToDistancePartialDerivativeWrtAngleTwo(angleOne, angleTwo)], - [planeAnglePartialDerivativeAngleOne(angleOne, angleTwo), planeAnglePartialDerivativeAngleTwo(angleOne, angleTwo)] - ] - }; - - - const performNewtonMethod = (initialGuess: number[][], iterations: number) => { - let anglesVector = [ - [initialGuess[0][0]], - [initialGuess[1][0]] - ]; - - for (var i = 0; i < iterations; i++) { - let functionVector = getFunctionVector(anglesVector); - let jacobianMatrix = getJacobianMatrix(anglesVector); - - let update = matrixMultiply(invertTwoByTwoMatrix(jacobianMatrix), functionVector); - - // Don't make updates too big - while (getVectorMagnitude(update) > Math.PI / 100) { - update = multiplyMatrixWithScalar(0.5, update); - } - anglesVector = addVector(anglesVector, multiplyMatrixWithScalar(-1, update)); - }; - return anglesVector; - } - - [[angleOne], [angleTwo]] = performNewtonMethod([[angleOne + Math.random()*0.00001], [angleTwo + Math.random()*0.00001]], 1000); - - // Check if we have found these angles before - let foundAnglesBefore = false; - previouslyFoundAngles.forEach(([otherAngleOne, otherAngleTwo]) => { - if (Math.abs(otherAngleOne - angleOne) < 0.0001 && Math.abs(otherAngleTwo - angleTwo) < 0.0001) { - foundAnglesBefore = true; - } - }); - - if (foundAnglesBefore) { - return; - } - - previouslyFoundAngles.push([angleOne, angleTwo]); + let angleOne = angleOneMultiplier * Math.acos(firstDistanceAlongDirection * Math.tan(phaseAngleOne) / firstDistancePerpendicularToDirection) + let angleTwo = angleTwoMultiplier * Math.acos(secondDistanceAlongDirection * Math.tan(phaseAngleTwo) / secondDistancePerpendicularToDirection); let targetPositionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]), @@ -1780,21 +1596,6 @@ export function findOrbitThroughInterpolation(ownCoordinates: OrbitalCoordinates ) ); - let firstMeasuredHorizontalAnomaly = Math.atan2(vectorDotProduct(targetPositionOne, ownCoordinates.orbit.coordinateAxes[1]), vectorDotProduct(targetPositionOne, ownCoordinates.orbit.coordinateAxes[0])); - let secondMeasuredHorizontalAnomaly = Math.atan2(vectorDotProduct(targetPositionTwo, ownCoordinates.orbit.coordinateAxes[1]), vectorDotProduct(targetPositionTwo, ownCoordinates.orbit.coordinateAxes[0])); - let measuredHorizontalAnomalyChange = secondMeasuredHorizontalAnomaly - firstMeasuredHorizontalAnomaly; - while (measuredHorizontalAnomalyChange <= -Math.PI) { - measuredHorizontalAnomalyChange += 2 * Math.PI; - } - while (measuredHorizontalAnomalyChange > Math.PI) { - measuredHorizontalAnomalyChange -= 2 * Math.PI; - } - - // These need to change in the same way, or the orbit we've found is going in the wrong direction - if (Math.sign(planeAngleDifference) != Math.sign(measuredHorizontalAnomalyChange)) { - return; - } - let normalVector = normalizeVector(vectorCrossProduct(targetPositionOne, targetPositionTwo)); // Rotate the position vector about this normal vector to get the direction of the periapsis