This commit is contained in:
@ -64,12 +64,13 @@ class MathOperations {
|
||||
}
|
||||
}
|
||||
|
||||
static void convertDateToJD2000(const T1 time, T2 julianDate){
|
||||
// time = { Y, M, D, h, m,s}
|
||||
// time in sec and microsec -> The Epoch (unixtime)
|
||||
julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12))
|
||||
+floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440;
|
||||
}
|
||||
static void convertDateToJD2000(const T1 time, T2 julianDate) {
|
||||
// time = { Y, M, D, h, m,s}
|
||||
// time in sec and microsec -> The Epoch (unixtime)
|
||||
julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
|
||||
floor(275 * time[1] / 9) + time[2] +
|
||||
(60 * time[3] + time[4] + (time(5) / 60)) / 1440;
|
||||
}
|
||||
|
||||
static T1 convertUnixToJD2000(timeval time) {
|
||||
// time = {{s},{us}}
|
||||
@ -93,43 +94,42 @@ class MathOperations {
|
||||
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
|
||||
}
|
||||
|
||||
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){
|
||||
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
|
||||
* longitude and altitude
|
||||
* @param: lat geodetic latitude [rad]
|
||||
* longi longitude [rad]
|
||||
* alt altitude [m]
|
||||
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double radiusPolar = 6356752.314;
|
||||
double radiusEqua = 6378137;
|
||||
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
|
||||
T2 *cartesianOutput) {
|
||||
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
|
||||
* longitude and altitude
|
||||
* @param: lat geodetic latitude [rad]
|
||||
* longi longitude [rad]
|
||||
* alt altitude [m]
|
||||
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double radiusPolar = 6356752.314;
|
||||
double radiusEqua = 6378137;
|
||||
|
||||
double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2));
|
||||
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2));
|
||||
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
|
||||
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
|
||||
|
||||
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
|
||||
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
|
||||
cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat);
|
||||
|
||||
}
|
||||
static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){
|
||||
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* @param: time Current time
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double JD2000Floor = 0;
|
||||
double JD2000 = convertUnixToJD2000(time);
|
||||
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
|
||||
JD2000Floor = floor(JD2000);
|
||||
if ( ( JD2000 - JD2000Floor) < 0.5) {
|
||||
JD2000Floor -= 0.5;
|
||||
}
|
||||
else {
|
||||
JD2000Floor += 0.5;
|
||||
}
|
||||
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
|
||||
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
|
||||
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
|
||||
}
|
||||
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
|
||||
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* @param: time Current time
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double JD2000Floor = 0;
|
||||
double JD2000 = convertUnixToJD2000(time);
|
||||
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
|
||||
JD2000Floor = floor(JD2000);
|
||||
if ((JD2000 - JD2000Floor) < 0.5) {
|
||||
JD2000Floor -= 0.5;
|
||||
} else {
|
||||
JD2000Floor += 0.5;
|
||||
}
|
||||
|
||||
double JC2000 = JD2000Floor / 36525;
|
||||
double sec = (JD2000 - JD2000Floor) * 86400;
|
||||
@ -152,150 +152,152 @@ class MathOperations {
|
||||
outputDcmEJ[7] = 0;
|
||||
outputDcmEJ[8] = 1;
|
||||
|
||||
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
|
||||
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
|
||||
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
|
||||
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
|
||||
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
|
||||
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
|
||||
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
|
||||
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
|
||||
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
|
||||
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
|
||||
}
|
||||
|
||||
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* give also the back the derivative of this matrix
|
||||
* @param: unixTime Current time in Unix format
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
|
||||
Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
|
||||
* Oliver Zeile
|
||||
*
|
||||
https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
|
||||
static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
|
||||
// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
|
||||
//(initial Offset) International Atomic Time (TAI)
|
||||
|
||||
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* give also the back the derivative of this matrix
|
||||
* @param: unixTime Current time in Unix format
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
|
||||
Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
|
||||
* Oliver Zeile
|
||||
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
|
||||
static void ecfToEciWithNutPre(timeval unixTime, T1 * outputDcmEJ, T1 * outputDotDcmEJ ) {
|
||||
double JD2000UTC1 = convertUnixToJD2000(unixTime);
|
||||
|
||||
// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset)
|
||||
// International Atomic Time (TAI)
|
||||
// Julian Date / century from TT
|
||||
timeval terestrialTime = unixTime;
|
||||
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
|
||||
double JD2000TT = convertUnixToJD2000(terestrialTime);
|
||||
double JC2000TT = JD2000TT / 36525;
|
||||
|
||||
double JD2000UTC1 = convertUnixToJD2000(unixTime);
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth rotation Theta
|
||||
//-------------------------------------------------------------------------------------
|
||||
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// Earth Rotation angle
|
||||
double era = 0;
|
||||
era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
|
||||
// Greenwich Mean Sidereal Time
|
||||
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
|
||||
0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
|
||||
double arcsecFactor = 1 * PI / (180 * 3600);
|
||||
gmst2000 *= arcsecFactor;
|
||||
gmst2000 += era;
|
||||
|
||||
// Julian Date / century from TT
|
||||
timeval terestrialTime = unixTime;
|
||||
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
|
||||
double JD2000TT = convertUnixToJD2000(terestrialTime);
|
||||
double JC2000TT = JD2000TT / 36525;
|
||||
theta[0][0] = cos(gmst2000);
|
||||
theta[0][1] = sin(gmst2000);
|
||||
theta[0][2] = 0;
|
||||
theta[1][0] = -sin(gmst2000);
|
||||
theta[1][1] = cos(gmst2000);
|
||||
theta[1][2] = 0;
|
||||
theta[2][0] = 0;
|
||||
theta[2][1] = 0;
|
||||
theta[2][2] = 1;
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth rotation Theta
|
||||
//-------------------------------------------------------------------------------------
|
||||
double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
// Earth Rotation angle
|
||||
double era = 0;
|
||||
era = 2* PI *(0.779057273264 + 1.00273781191135448 * JD2000UTC1);
|
||||
// Greenwich Mean Sidereal Time
|
||||
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT,2) -
|
||||
0.00009344 * pow(JC2000TT,3) + 0.00001882 * pow(JC2000TT,4);
|
||||
double arcsecFactor = 1 * PI / (180 * 3600);
|
||||
gmst2000 *= arcsecFactor;
|
||||
gmst2000 += era;
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Precession P
|
||||
//-------------------------------------------------------------------------------------
|
||||
double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
theta[0][0] = cos(gmst2000);
|
||||
theta[0][1] = sin(gmst2000);
|
||||
theta[0][2] = 0;
|
||||
theta[1][0] = -sin(gmst2000);
|
||||
theta[1][1] = cos(gmst2000);
|
||||
theta[1][2] = 0;
|
||||
theta[2][0] = 0;
|
||||
theta[2][1] = 0;
|
||||
theta[2][2] = 1;
|
||||
double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
|
||||
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
|
||||
double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Precession P
|
||||
//-------------------------------------------------------------------------------------
|
||||
double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
zeta *= arcsecFactor;
|
||||
theta2 *= arcsecFactor;
|
||||
ze *= arcsecFactor;
|
||||
|
||||
double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT,2) + 0.017998 * pow(JC2000TT,3);
|
||||
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT,2) - 0.041833 * pow(JC2000TT,3);
|
||||
double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3);
|
||||
precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
|
||||
precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
|
||||
precession[2][0] = sin(theta2) * cos(zeta);
|
||||
precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
|
||||
precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
|
||||
precession[2][1] = -sin(theta2) * sin(zeta);
|
||||
precession[0][2] = -cos(ze) * sin(theta2);
|
||||
precession[1][2] = -sin(ze) * sin(theta2);
|
||||
precession[2][2] = cos(theta2);
|
||||
|
||||
zeta *= arcsecFactor;
|
||||
theta2 *= arcsecFactor;
|
||||
ze *= arcsecFactor;
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Nutation N
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// lunar asc node
|
||||
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
|
||||
7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
|
||||
Om *= arcsecFactor;
|
||||
// delta psi approx
|
||||
double dp = -17.2 * arcsecFactor * sin(Om);
|
||||
|
||||
precession[0][0]=-sin(ze)*sin(zeta)+cos(ze)*cos(theta2)*cos(zeta);
|
||||
precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta);
|
||||
precession[2][0]=sin(theta2)*cos(zeta);
|
||||
precession[0][1]=-sin(ze)*cos(zeta)-cos(ze)*cos(theta2)*sin(zeta);
|
||||
precession[1][1]=cos(ze)*cos(zeta)-sin(ze)*cos(theta2)*sin(zeta);
|
||||
precession[2][1]=-sin(theta2)*sin(zeta);
|
||||
precession[0][2]=-cos(ze)*sin(theta2);
|
||||
precession[1][2]=-sin(ze)*sin(theta2);
|
||||
precession[2][2]=cos(theta2);
|
||||
// delta eps approx
|
||||
double de = 9.203 * arcsecFactor * cos(Om);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Nutation N
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
// lunar asc node
|
||||
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
|
||||
7.455 * pow(JC2000TT,2) + 0.008 * pow(JC2000TT,3);
|
||||
Om *= arcsecFactor;
|
||||
// delta psi approx
|
||||
double dp = -17.2 * arcsecFactor *sin(Om);
|
||||
// % true obliquity of the ecliptic eps p.71 (simplified)
|
||||
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
|
||||
|
||||
// delta eps approx
|
||||
double de = 9.203 * arcsecFactor *cos(Om);
|
||||
nutation[0][0] = cos(dp);
|
||||
nutation[1][0] = cos(e + de) * sin(dp);
|
||||
nutation[2][0] = sin(e + de) * sin(dp);
|
||||
nutation[0][1] = -cos(e) * sin(dp);
|
||||
nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
|
||||
nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
|
||||
nutation[0][2] = -sin(e) * sin(dp);
|
||||
nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
|
||||
nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
|
||||
|
||||
// % true obliquity of the ecliptic eps p.71 (simplified)
|
||||
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Derivative of rotation matrix from earth
|
||||
//-------------------------------------------------------------------------------------
|
||||
double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
|
||||
|
||||
nutation[0][0]=cos(dp);
|
||||
nutation[1][0]=cos(e+de)*sin(dp);
|
||||
nutation[2][0]=sin(e+de)*sin(dp);
|
||||
nutation[0][1]=-cos(e)*sin(dp);
|
||||
nutation[1][1]=cos(e)*cos(e+de)*cos(dp)+sin(e)*sin(e+de);
|
||||
nutation[2][1]=cos(e)*sin(e+de)*cos(dp)-sin(e)*cos(e+de);
|
||||
nutation[0][2]=-sin(e)*sin(dp);
|
||||
nutation[1][2]=sin(e)*cos(e+de)*cos(dp)-cos(e)*sin(e+de);
|
||||
nutation[2][2]=sin(e)*sin(e+de)*cos(dp)+cos(e)*cos(e+de);
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of transformation matrix and Derivative of transformation matrix
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Derivative of rotation matrix from earth
|
||||
//-------------------------------------------------------------------------------------
|
||||
double thetaDot[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
double dotMatrix[3][3] = {{0,1,0},{-1,0,0},{0,0,0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
|
||||
}
|
||||
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
|
||||
int i, j;
|
||||
double determinant;
|
||||
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
|
||||
{matrix[3], matrix[4], matrix[5]},
|
||||
{matrix[6], matrix[7], matrix[8]}};
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of transformation matrix and Derivative of transformation matrix
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
|
||||
|
||||
}
|
||||
static void inverseMatrixDimThree(const T1 *matrix, T1 * output){
|
||||
int i,j;
|
||||
double determinant;
|
||||
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]},
|
||||
{matrix[6], matrix[7], matrix[8]}};
|
||||
|
||||
for(i = 0; i < 3; i++) {
|
||||
determinant = determinant + (mat[0][i] * (mat[1][(i+1)%3] * mat[2][(i+2)%3] - mat[1][(i+2)%3] * mat[2][(i+1)%3]));
|
||||
}
|
||||
// cout<<"\n\ndeterminant: "<<determinant;
|
||||
// cout<<"\n\nInverse of matrix is: \n";
|
||||
for(i = 0; i < 3; i++){
|
||||
for(j = 0; j < 3; j++){
|
||||
output[i*3+j] = ((mat[(j+1)%3][(i+1)%3] * mat[(j+2)%3][(i+2)%3]) - (mat[(j+1)%3][(i+2)%3] * mat[(j+2)%3][(i+1)%3]))/ determinant;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < 3; i++) {
|
||||
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
|
||||
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
|
||||
}
|
||||
// cout<<"\n\ndeterminant: "<<determinant;
|
||||
// cout<<"\n\nInverse of matrix is: \n";
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < 3; j++) {
|
||||
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
|
||||
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
|
||||
determinant;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
|
||||
float det = 0;
|
||||
|
||||
Reference in New Issue
Block a user