public static Position RotatePositionFromInertialFrameToBodyFrame(Orientation orientation, Position currentPosition)
{
Position newPosition = new Position();
double[,] rotationMatrix = GetRotationMatrixFromInertialToBodyFrame(orientation);
double[,] positionMatrix = currentPosition.GetPositionMatrix();
double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
/*rotatedPosition[0,0] = rotationMatrix[0,0]*positionMatrix[0,0] + rotationMatrix[0,1]*positionMatrix[1,0] + rotationMatrix[0,2]*positionMatrix[2,0];
rotatedPosition[1,0] = rotationMatrix[1,0]*positionMatrix[0,0] + rotationMatrix[1,1]*positionMatrix[1,0] + rotationMatrix[1,2]*positionMatrix[2,0];
rotatedPosition[2,0] = rotationMatrix[2,0]*positionMatrix[0,0] + rotationMatrix[2,1]*positionMatrix[1,0] + rotationMatrix[2,2]*positionMatrix[2,0];
*/
//for each column in rotation matrix through columns
for (int i = 0; i < rotationMatrix.GetLength(1); i++)
{
for (int j = 0; j < positionMatrix.GetLength(0); j++)
{
rotatedPosition[i, 0] += rotationMatrix[i, j] * positionMatrix[j, 0];
}
}
newPosition.XLatitudePosition = rotatedPosition[0, 0];
newPosition.YLongitudePosition = rotatedPosition[1, 0];
newPosition.ZAltitudePosition = rotatedPosition[2, 0];
return newPosition;
}
//public static double[] ConvertFromECEFToLocalNED(double[] ecefReferencePosition, double[] ecefCurrentPosition)
//{
// double[,] rotationMatrix = GetRotationMatrixFromInertialToBodyFrame(orientation);
// double[,] positionMatrix = currentPosition.GetPositionMatrix();
// double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
// /*rotatedPosition[0,0] = rotationMatrix[0,0]*positionMatrix[0,0] + rotationMatrix[0,1]*positionMatrix[1,0] + rotationMatrix[0,2]*positionMatrix[2,0];
// rotatedPosition[1,0] = rotationMatrix[1,0]*positionMatrix[0,0] + rotationMatrix[1,1]*positionMatrix[1,0] + rotationMatrix[1,2]*positionMatrix[2,0];
// rotatedPosition[2,0] = rotationMatrix[2,0]*positionMatrix[0,0] + rotationMatrix[2,1]*positionMatrix[1,0] + rotationMatrix[2,2]*positionMatrix[2,0];
// */
// //for each column in rotation matrix through columns
// for (int i = 0; i < rotationMatrix.GetLength(1); i++)
// {
// for (int j = 0; j < positionMatrix.GetLength(0); j++)
// {
// rotatedPosition[i, 0] += rotationMatrix[i, j] * positionMatrix[j, 0];
// }
// }
// newPosition.XLatitudePosition = rotatedPosition[0, 0];
// newPosition.YLongitudePosition = rotatedPosition[1, 0];
// newPosition.ZAltitudePosition = rotatedPosition[2, 0];
// return newPosition;
//}
public static Position RotatePositionFromGeodedicFrameToBodyFrame(Orientation orientation, Position currentPosition, Position originalReferencePosition)
{
//Rotate from Geodedic to earth centered earth fixed
/* Position currentEcefPosition = new Position();
double[] ecef = ConvertFromGeodeticToECEF(currentPosition.YLongitudePosition, currentPosition.XLatitudePosition, currentPosition.ZAltitudePosition);
currentEcefPosition.XLatitudePosition = ecef[0];
currentEcefPosition.YLongitudePosition = ecef[1];
currentEcefPosition.ZAltitudePosition = ecef[2];*/
/*
Position referenceEcefPosition = new Position();
ecef = ConvertFromGeodeticToECEF(originalReferencePosition.YLongitudePosition, originalReferencePosition.XLatitudePosition, originalReferencePosition.ZAltitudePosition);
referenceEcefPosition.XLatitudePosition = ecef[0];
referenceEcefPosition.YLongitudePosition = ecef[1];
referenceEcefPosition.ZAltitudePosition = ecef[2];
*/
//Rotate from earth centered earth fixed to local NED (which is the same as vehicle carried NED)
double[,] ecefToNEDRotationMatrix = calculateEcefToNEDMatrix(originalReferencePosition);
Position currentMinusRef = new Position();
currentMinusRef.XLatitudePosition = currentPosition.XLatitudePosition - originalReferencePosition.XLatitudePosition;
currentMinusRef.YLongitudePosition = currentPosition.YLongitudePosition - originalReferencePosition.YLongitudePosition;
currentMinusRef.ZAltitudePosition = currentPosition.ZAltitudePosition - originalReferencePosition.ZAltitudePosition;
double[,] positionMatrix = currentMinusRef.GetPositionMatrix();
double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
Position currentPositionLocalNED = new Position();
//for each column in rotation matrix through columns
for (int i = 0; i < ecefToNEDRotationMatrix.GetLength(1); i++)
{
for (int j = 0; j < positionMatrix.GetLength(0); j++)
{
rotatedPosition[i, 0] += ecefToNEDRotationMatrix[i, j] * positionMatrix[j, 0];
}
}
currentPositionLocalNED.XLatitudePosition = rotatedPosition[0, 0];
currentPositionLocalNED.YLongitudePosition = rotatedPosition[1, 0];
currentPositionLocalNED.ZAltitudePosition = rotatedPosition[2, 0];
//Rotate from vehicle carried NED to body NED
double[,] NEDToBodyRotationMatrix = calculateNEDToBodyMatrix(orientation);
positionMatrix = currentPositionLocalNED.GetPositionMatrix();
rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
Position currentPositionBody = new Position();
//for each column in rotation matrix through columns
for (int i = 0; i < NEDToBodyRotationMatrix.GetLength(1); i++)
{
for (int j = 0; j < positionMatrix.GetLength(0); j++)
{
rotatedPosition[i, 0] += NEDToBodyRotationMatrix[i, j] * positionMatrix[j, 0];
}
}
currentPositionBody.XLatitudePosition = rotatedPosition[0, 0];
currentPositionBody.YLongitudePosition = rotatedPosition[1, 0];
currentPositionBody.ZAltitudePosition = rotatedPosition[2, 0];
return currentPositionBody;
}
