diff options
Diffstat (limited to 'redist')
-rw-r--r-- | redist/dclapack.h | 225 | ||||
-rw-r--r-- | redist/linmath.h | 1 | ||||
-rw-r--r-- | redist/lintest.c | 3 |
3 files changed, 227 insertions, 2 deletions
diff --git a/redist/dclapack.h b/redist/dclapack.h new file mode 100644 index 0000000..4e209d3 --- /dev/null +++ b/redist/dclapack.h @@ -0,0 +1,225 @@ +#ifndef __DCLAPACK_H__ +#define __DCLAPACK_H__ + +#ifndef ORDER +#define ORDER 50 +#endif + +#ifndef FLOAT +#define FLOAT float +#endif + +#include<stdio.h> + +#define _ABS(a) ( (a)<=0 ? 0-(a) : (a) ) + +/* + * Prints a matrix A (n by m) + */ +#define PRINT(A,n,m) { \ + int i,j; \ + printf(#A "\n"); \ + for (i=0; i<n; i++) { \ + for (j=0; j<m; j++) { \ + printf("%4.3f\t", A[i][j]); \ + } \ + printf("\n"); \ + } \ + printf("\n"); \ +} + +/* + * Returns the identity matrix + */ +#define IDENTITY(I,n) { \ + int i,j; \ + for (i=0; i<n; i++) { \ + for (j=0; j<i; j++) { I[i][j]=0.0f; } \ + I[i][i] = 1.0f; \ + for (j=i+1; j<n; j++) { I[i][j]=0.0f; } \ + } \ +} + +/* + * B = Transpose(A) + * A is (n by m) + * B is (m by n) + */ +#define TRANSP(A,B,n,m) { \ + int i,j; \ + for (i=0; i<n; i++) { \ + for (j=0; j<m; j++) { \ + B[j][i] = A[i][j]; \ + } \ + } \ +} + +/* + * Calculate L,U of a matrix A with pivot table + */ +#define LU(A,L,U,Piv,n) { \ + int i,j,k,_tempi; float _tempf; \ + for (i=0; i<n; i++) { Piv[i]=i; } \ + for (i=0; i<n; i++) { \ + for (j=0; j<n; j++) { \ + U[i][j] = A[i][j]; \ + } \ + } \ + IDENTITY(L,n); \ + \ + for (i=0; i<n-1; i++) { \ + \ + int max=i; \ + for (j=i+1; j<n; j++) { \ + if (_ABS(U[j][i]) > _ABS(U[max][i])) { max = j; } \ + } \ + _tempi=Piv[i]; Piv[i]=Piv[max]; Piv[max]=_tempi; \ + for (k=i; k<n; k++) { \ + _tempf=U[i][k]; U[i][k]=U[max][k]; U[max][k]=_tempf; \ + } \ + for (k=0; k<i; k++) { \ + _tempf=L[i][k]; L[i][k]=L[max][k]; L[max][k]=_tempf; \ + } \ + \ + FLOAT invDiag = 1.0 / U[i][i]; \ + for (j=i+1; j<n; j++) { \ + FLOAT scale = U[j][i] * invDiag; \ + U[j][i] = 0.0; \ + for (k=i+1; k<n; k++) { U[j][k] -= U[i][k]*scale; } \ + L[j][i] = scale; \ + } \ + } \ +} + +/* + * Pivots a matrix to a different matrix + * B = Pivot(A) given table 'Piv' + * A and B are (n by m) + */ +#define PIVOT(A,B,Piv,n,m) { \ + int i,j; \ + for (j=0; j<n; j++) { \ + for (i=0; i<m; i++) { \ + B[j][i] = A[Piv[j]][i]; \ + } \ + } \ +} + +/* + * Solve LX=B for matrix X and B + * L is n by n (lower triangular) + * B is n by m + */ +#define L_SUB(L,X,B,n,m) { \ + int i,j,k; \ + for (i=0; i<m; i++) { \ + for (j=0; j<n; j++) { \ + float sum=0.0; \ + for (k=0; k<j; k++) { sum += L[j][k]*X[k][i]; } \ + X[j][i] = (B[j][i] - sum) / L[j][j]; \ + } \ + } \ +} + +/* + * Solve UX=B for matrix X and B + * U is n by n (upper triangular) + * B is n by m + */ +#define U_SUB(U,X,B,n,m) { \ + int i,j,k; \ + for (i=0; i<m; i++) { \ + for (j=n-1; j>=0; j--) { \ + float sum=0.0; \ + for (k=n-1; k>j; k--) { sum += U[j][k]*X[k][i]; } \ + X[j][i] = (B[j][i] - sum) / U[j][j]; \ + } \ + } \ +} + +/* + * Inverts a matrix X (n by n) using the method of LU decomposition + */ +#define INV(A,Ainv,n) { \ + FLOAT Ipiv[ORDER][ORDER]; \ + FLOAT L[ORDER][ORDER]; \ + FLOAT U[ORDER][ORDER]; \ + FLOAT I[ORDER][ORDER]; \ + FLOAT C[ORDER][ORDER]; \ + int Piv[ORDER]; \ + IDENTITY(I,n); \ + LU(A,L,U,Piv,n); \ + PIVOT(I,Ipiv,Piv,n,n); \ + L_SUB(L,C,Ipiv,n,n); \ + U_SUB(U,Ainv,C,n,n); \ +} + +/* +PRINT(A,n,n); \ +PRINT(L,n,n); \ +PRINT(U,n,n); \ +MUL(L,U,LU,n,n,n);\ +PRINT(LU,n,n);\ +PRINT(C,n,n); \ +PRINT(Ainv,n,n); \ +*/ + +/* + * Matrix Multiply C = A * B + * A (n by m) + * B (m by p) + * C (n by p) + */ +#define MUL(A,B,C,n,m,p) { \ + int i,j,k; \ + for (i=0; i<n; i++) { \ + for (j=0; j<p; j++) { \ + C[i][j] = 0.0f; \ + for (k=0; k<m; k++) { \ + C[i][j] += A[i][k] * B[k][j]; \ + } \ + } \ + } \ +} + +/* + * Matrix Multiply D = A * B + C + * A (n by m) + * B (m by p) + * C (n by p) + * D (n by p) + */ +#define MULADD(A,B,C,D,n,m,p) { \ + int i,j,k; \ + for (i=0; i<n; i++) { \ + for (j=0; j<p; j++) { \ + D[i][j] = C[i][j]; \ + for (k=0; k<m; k++) { \ + D[i][j] += A[i][k] * B[k][j]; \ + } \ + } \ + } \ +} + +/* + * Matrix Multiply D = alpha * A * B + beta * C + * A (n by m) + * B (m by p) + * C (n by p) + * D (n by p) + */ +#define GMULADD(A,B,C,D,alpha,beta,n,m,p) { \ + int i,j,k; \ + float sum; \ + for (i=0; i<n; i++) { \ + for (j=0; j<p; j++) { \ + sum = 0.0f; \ + for (k=0; k<m; k++) { \ + sum += A[i][k] * B[k][j]; \ + } \ + D[i][j] = alpha * sum + beta * C[i][j]; \ + } \ + } \ +} + +#endif diff --git a/redist/linmath.h b/redist/linmath.h index a58b2bf..aeaca46 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -92,6 +92,7 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ); void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest); //Poses are Position: [x, y, z] Quaternion: [q, x, y, z] +//XXX TODO Write these! void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); void InvertPose( FLT * poseout, const FLT * pose ); diff --git a/redist/lintest.c b/redist/lintest.c index 377824e..a767670 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -31,8 +31,7 @@ int main() printf( "E: %f %f %f\n", e[0], e[1], e[2] ); - FLT p[3] = { 0, 0, 1 }; - printf( "%f %f %f\n", PFTHREE( p ) ); + FLT pfromlh[3] = { 0, 1, 0 }; quatrotatevector( p, q, p ); printf( "%f %f %f\n", PFTHREE( p ) ); printf( "Flipping rotation\n" ); |