Archived
KenanZhu111
a3fb64a657
1. KNZ_GeoTrackLab: Fix some bug, Modify some UI logic.
2. KNZ_GeoTrackLab: Remove 3D plot function.
3. KNZ_GeoTrackLab: Merge some functions and concentrated.
4. KNZ_GeoTrackLab: New added:
Sat track projection ui(not ok on function).
232 lines
7.7 KiB
C++
232 lines
7.7 KiB
C++
#include "brdm2pos.h"
|
|
/* new matrix ------------------------------------------------------------------
|
|
* allocate memory of matrix
|
|
* args : int n,m I number of rows and columns of matrix
|
|
* return : matrix pointer (if n<=0 or m<=0, return NULL)
|
|
*-----------------------------------------------------------------------------*/
|
|
double* mat(int n, int m)
|
|
{
|
|
double* p;
|
|
|
|
if (n <= 0 || m <= 0) return NULL;
|
|
if (!(p = (double*)malloc(sizeof(double) * n * m))) {
|
|
return NULL;
|
|
}
|
|
return p;
|
|
}
|
|
/* new integer matrix ----------------------------------------------------------
|
|
* allocate memory of integer matrix
|
|
* args : int n,m I number of rows and columns of matrix
|
|
* return : matrix pointer (if n<=0 or m<=0, return NULL)
|
|
*-----------------------------------------------------------------------------*/
|
|
int* imat(int n, int m)
|
|
{
|
|
int* p;
|
|
|
|
if (n <= 0 || m <= 0) return NULL;
|
|
if (!(p = (int*)malloc(sizeof(int) * n * m))) {
|
|
return NULL;
|
|
}
|
|
return p;
|
|
}
|
|
/* zero matrix -----------------------------------------------------------------
|
|
* generate new zero matrix
|
|
* args : int n,m I number of rows and columns of matrix
|
|
* return : matrix pointer (if n<=0 or m<=0, return NULL)
|
|
*-----------------------------------------------------------------------------*/
|
|
double* zeros(int n, int m)
|
|
{
|
|
double* p;
|
|
|
|
#if NOCALLOC
|
|
if ((p = mat(n, m))) for (n = n * m - 1; n >= 0; n--) p[n] = 0.0;
|
|
#else
|
|
if (n <= 0 || m <= 0) return NULL;
|
|
if (!(p = (double*)calloc(sizeof(double), n * m))) {
|
|
return NULL;
|
|
}
|
|
#endif
|
|
return p;
|
|
}
|
|
/* identity matrix -------------------------------------------------------------
|
|
* generate new identity matrix
|
|
* args : int n I number of rows and columns of matrix
|
|
* return : matrix pointer (if n<=0, return NULL)
|
|
*-----------------------------------------------------------------------------*/
|
|
double* eye(int n)
|
|
{
|
|
double* p;
|
|
int i;
|
|
|
|
if ((p = zeros(n, n))) for (i = 0; i < n; i++) p[i + i * n] = 1.0;
|
|
return p;
|
|
}
|
|
/* inner product ---------------------------------------------------------------
|
|
* inner product of vectors
|
|
* args : double *a,*b I vector a,b (n x 1)
|
|
* int n I size of vector a,b
|
|
* return : a'*b
|
|
*-----------------------------------------------------------------------------*/
|
|
double dot(const double* va, const double* vb, int n)
|
|
{
|
|
double c = 0.0;
|
|
|
|
while (--n >= 0) c += va[n] * vb[n];
|
|
return c;
|
|
}
|
|
/* euclid norm -----------------------------------------------------------------
|
|
* euclid norm of vector
|
|
* args : double *a I vector a (n x 1)
|
|
* int n I size of vector a
|
|
* return : || a ||
|
|
*-----------------------------------------------------------------------------*/
|
|
double norm(const double* va, int n)
|
|
{
|
|
return sqrt(dot(va, va, n));
|
|
}
|
|
/* outer product of 3d vectors -------------------------------------------------
|
|
* outer product of 3d vectors
|
|
* args : double *a,*b I vector a,b (3 x 1)
|
|
* double *c O outer product (a x b) (3 x 1)
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
void cross3(const double* va, const double* vb, double* vc)
|
|
{
|
|
vc[0] = va[1] * vb[2] - va[2] * vb[1];
|
|
vc[1] = va[2] * vb[0] - va[0] * vb[2];
|
|
vc[2] = va[0] * vb[1] - va[1] * vb[0];
|
|
}
|
|
/* normalize 3d vector ---------------------------------------------------------
|
|
* normalize 3d vector
|
|
* args : double *a I vector a (3 x 1)
|
|
* double *b O normlized vector (3 x 1) || b || = 1
|
|
* return : status (1:ok,0:error)
|
|
*-----------------------------------------------------------------------------*/
|
|
int normv3(const double* va, double* vb)
|
|
{
|
|
double r;
|
|
if ((r = norm(va, 3)) <= 0.0) return 0;
|
|
vb[0] = va[0] / r;
|
|
vb[1] = va[1] / r;
|
|
vb[2] = va[2] / r;
|
|
return 1;
|
|
}
|
|
/* copy matrix -----------------------------------------------------------------
|
|
* copy matrix
|
|
* args : double *A O destination matrix A (n x m)
|
|
* double *B I source matrix B (n x m)
|
|
* int n,m I number of rows and columns of matrix
|
|
* return : none
|
|
*-----------------------------------------------------------------------------*/
|
|
void matcpy(double* A, const double* B, int n, int m)
|
|
{
|
|
memcpy(A, B, sizeof(double) * n * m);
|
|
}
|
|
/*multiply matrix*/
|
|
void matmul(const char* tr, int n, int k, int m, double alpha,
|
|
const double* A, const double* B, double beta, double* C)
|
|
{
|
|
double d;
|
|
int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4);
|
|
|
|
for (i = 0; i < n; i++) for (j = 0; j < k; j++) {
|
|
d = 0.0;
|
|
switch (f) {
|
|
case 1: for (x = 0; x < m; x++) d += A[i + x * n] * B[x + j * m]; break;
|
|
case 2: for (x = 0; x < m; x++) d += A[i + x * n] * B[j + x * k]; break;
|
|
case 3: for (x = 0; x < m; x++) d += A[x + i * m] * B[x + j * m]; break;
|
|
case 4: for (x = 0; x < m; x++) d += A[x + i * m] * B[j + x * k]; break;
|
|
}
|
|
if (beta == 0.0) C[i + j * n] = alpha * d; else C[i + j * n] = alpha * d + beta * C[i + j * n];
|
|
}
|
|
}
|
|
/*LU decomposition*/
|
|
static int ludcmp(double* A, int n, int* indx, double* d)
|
|
{
|
|
double big, s, tmp, * vv = mat(n, 1);
|
|
int i, imax = 0, j, k;
|
|
|
|
*d = 1.0;
|
|
for (i = 0; i < n; i++) {
|
|
big = 0.0; for (j = 0; j < n; j++) if ((tmp = fabs(A[i + j * n])) > big) big = tmp;
|
|
if (big > 0.0) vv[i] = 1.0 / big; else { free(vv); return -1; }
|
|
}
|
|
for (j = 0; j < n; j++) {
|
|
for (i = 0; i < j; i++) {
|
|
s = A[i + j * n]; for (k = 0; k < i; k++) s -= A[i + k * n] * A[k + j * n]; A[i + j * n] = s;
|
|
}
|
|
big = 0.0;
|
|
for (i = j; i < n; i++) {
|
|
s = A[i + j * n]; for (k = 0; k < j; k++) s -= A[i + k * n] * A[k + j * n]; A[i + j * n] = s;
|
|
if ((tmp = vv[i] * fabs(s)) >= big) { big = tmp; imax = i; }
|
|
}
|
|
if (j != imax) {
|
|
for (k = 0; k < n; k++) {
|
|
tmp = A[imax + k * n]; A[imax + k * n] = A[j + k * n]; A[j + k * n] = tmp;
|
|
}
|
|
*d = -(*d); vv[imax] = vv[j];
|
|
}
|
|
indx[j] = imax;
|
|
if (A[j + j * n] == 0.0) { free(vv); return -1; }
|
|
if (j != n - 1) {
|
|
tmp = 1.0 / A[j + j * n]; for (i = j + 1; i < n; i++) A[i + j * n] *= tmp;
|
|
}
|
|
}
|
|
free(vv);
|
|
return 0;
|
|
}
|
|
/*LU back-substitution*/
|
|
static void lubksb(const double* A, int n, const int* indx, double* b)
|
|
{
|
|
double s;
|
|
int i, ii = -1, ip, j;
|
|
|
|
for (i = 0; i < n; i++) {
|
|
ip = indx[i]; s = b[ip]; b[ip] = b[i];
|
|
if (ii >= 0) for (j = ii; j < i; j++) s -= A[i + j * n] * b[j]; else if (s) ii = i;
|
|
b[i] = s;
|
|
}
|
|
for (i = n - 1; i >= 0; i--) {
|
|
s = b[i]; for (j = i + 1; j < n; j++) s -= A[i + j * n] * b[j]; b[i] = s / A[i + i * n];
|
|
}
|
|
}
|
|
/*inverse of matrix*/
|
|
int matinv(double* A, int n)
|
|
{
|
|
double d, * B;
|
|
int i, j, * indx;
|
|
|
|
indx = imat(n, 1); B = mat(n, n); matcpy(B, A, n, n);
|
|
if (ludcmp(B, n, indx, &d)) { free(indx); free(B); return -1; }
|
|
for (j = 0; j < n; j++) {
|
|
for (i = 0; i < n; i++) A[i + j * n] = 0.0;
|
|
A[j + j * n] = 1.0;
|
|
lubksb(B, n, indx, A + j * n);
|
|
}
|
|
free(indx); free(B);
|
|
return 0;
|
|
}
|
|
/*solve linear equation*/
|
|
int solve(const char* tr, const double* A, const double* Y, int n,
|
|
int m, double* X)
|
|
{
|
|
double* B = mat(n, n);
|
|
int info;
|
|
|
|
matcpy(B, A, n, n);
|
|
if (!(info = matinv(B, n))) matmul(tr[0] == 'N' ? "NN" : "TN", n, m, n, 1.0, B, Y, 0.0, X);
|
|
free(B);
|
|
return info;
|
|
}
|
|
void matprint(const double A[], int n, int m, int p, int q)
|
|
{
|
|
int i, j;
|
|
|
|
for (i = 0; i < n; i++) {
|
|
for (j = 0; j < m; j++) printf(" %*.*f", p, q, A[i + j * n]);
|
|
printf("\n");
|
|
}
|
|
}
|
|
|
|
|