Optimize over phase angles
This commit is contained in:
Martin Asprusten
parent
d535a3fdea
commit
ef4b9b8862
@ -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;
|
||||
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];
|
||||
}
|
||||
|
||||
// If the distance function is below something like 10 metres, we are close enough
|
||||
if (Math.abs(currentDistance) < 10) {
|
||||
break;
|
||||
// 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;
|
||||
[testValue, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne + learningRate * updateOne, phaseAngleTwo);
|
||||
}
|
||||
} while (Math.abs(candidateDistance) > Math.abs(currentDistance));
|
||||
|
||||
if (!deadEndAngleOne) {
|
||||
angleOne = (angleOne + learningRate * update) % (2 * Math.PI);
|
||||
phaseAngleOne += learningRate * updateOne;
|
||||
}
|
||||
|
||||
currentDistance = anglesToDistanceFunction(angleOne, angleTwo);
|
||||
let dfD2 = anglesToDistancePartialDerivativeWrtAngleTwo(angleOne, angleTwo);
|
||||
// Now, do the same for angle two
|
||||
[value, dfd1, dfd2] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo);
|
||||
let updateTwo = -value / dfd2;
|
||||
learningRate = 1;
|
||||
|
||||
if (!currentDistance || !dfD2) {
|
||||
break;
|
||||
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;
|
||||
[testValue, _, __] = phaseAnglesToDistanceFunctionWithDerivatives(phaseAngleOne, phaseAngleTwo + learningRate * updateTwo);
|
||||
}
|
||||
} while (Math.abs(candidateDistance) > Math.abs(currentDistance));
|
||||
|
||||
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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user