diff options
Diffstat (limited to 'redist')
| -rw-r--r-- | redist/dclapack.h | 225 | ||||
| -rw-r--r-- | redist/linmath.c | 167 | ||||
| -rw-r--r-- | redist/linmath.h | 9 | ||||
| -rw-r--r-- | redist/lintest.c | 112 |
4 files changed, 366 insertions, 147 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.c b/redist/linmath.c index 72c00a4..41fa18f 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -516,150 +516,27 @@ void matrix44copy(FLT * mout, const FLT * minm ) memcpy( mout, minm, sizeof( FLT ) * 16 ); } -///////////////////////////////////////Matrix Rotations//////////////////////////////////// -////Originally from Stack Overflow -////Under cc by-sa 3.0 -//// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw -//// Copyright 2014 by Campbell Barton -//// Copyright 2017 by Michael Turvey -// -///** -//* Calculate a rotation matrix from 2 normalized vectors. -//* -//* v1 and v2 must be unit length. -//*/ -//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) -//{ -// FLT axis[3]; -// /* avoid calculating the angle */ -// FLT angle_sin; -// FLT angle_cos; -// -// cross3d(axis, v1, v2); -// -// angle_sin = normalize_v3(axis); -// angle_cos = dot3d(v1, v2); -// -// if (angle_sin > FLT_EPSILON) { -// axis_calc: -// axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); -// } -// else { -// /* Degenerate (co-linear) vectors */ -// if (angle_cos > 0.0f) { -// /* Same vectors, zero rotation... */ -// unit_m3(m); -// } -// else { -// /* Colinear but opposed vectors, 180 rotation... */ -// get_orthogonal_vector(axis, v1); -// normalize_v3(axis); -// angle_sin = 0.0f; /* sin(M_PI) */ -// angle_cos = -1.0f; /* cos(M_PI) */ -// goto axis_calc; -// } -// } -//} - -//void get_orthogonal_vector(FLT out[3], const FLT in[3]) -//{ -//#ifdef USE_DOUBLE -// const FLT x = fabs(in[0]); -// const FLT y = fabs(in[1]); -// const FLT z = fabs(in[2]); -//#else -// const FLT x = fabsf(in[0]); -// const FLT y = fabsf(in[1]); -// const FLT z = fabsf(in[2]); -//#endif -// -// if (x > y && x > z) -// { -// // x is dominant -// out[0] = -in[1] - in[2]; -// out[1] = in[0]; -// out[2] = in[0]; -// } -// else if (y > z) -// { -// // y is dominant -// out[0] = in[1]; -// out[1] = -in[0] - in[2]; -// out[2] = in[1]; -// } -// else -// { -// // z is dominant -// out[0] = in[2]; -// out[1] = in[2]; -// out[2] = -in[0] - in[1]; -// } -//} -// -//void unit_m3(FLT mat[3][3]) -//{ -// mat[0][0] = 1; -// mat[0][1] = 0; -// mat[0][2] = 0; -// mat[1][0] = 0; -// mat[1][1] = 1; -// mat[1][2] = 0; -// mat[2][0] = 0; -// mat[2][1] = 0; -// mat[2][2] = 1; -//} - - -//FLT normalize_v3(FLT vect[3]) -//{ -// FLT distance = dot3d(vect, vect); -// -// if (distance < 1.0e-35f) -// { -// // distance is too short, just go to zero. -// vect[0] = 0; -// vect[1] = 0; -// vect[2] = 0; -// distance = 0; -// } -// else -// { -// distance = FLT_SQRT((FLT)distance); -// scale3d(vect, vect, 1.0f / distance); -// } -// -// return distance; -//} - -///* axis must be unit length */ -//void axis_angle_normalized_to_mat3_ex( -// FLT mat[3][3], const FLT axis[3], -// const FLT angle_sin, const FLT angle_cos) -//{ -// FLT nsi[3], ico; -// FLT n_00, n_01, n_11, n_02, n_12, n_22; -// -// ico = (1.0f - angle_cos); -// nsi[0] = axis[0] * angle_sin; -// nsi[1] = axis[1] * angle_sin; -// nsi[2] = axis[2] * angle_sin; -// -// n_00 = (axis[0] * axis[0]) * ico; -// n_01 = (axis[0] * axis[1]) * ico; -// n_11 = (axis[1] * axis[1]) * ico; -// n_02 = (axis[0] * axis[2]) * ico; -// n_12 = (axis[1] * axis[2]) * ico; -// n_22 = (axis[2] * axis[2]) * ico; -// -// mat[0][0] = n_00 + angle_cos; -// mat[0][1] = n_01 + nsi[2]; -// mat[0][2] = n_02 - nsi[1]; -// mat[1][0] = n_01 - nsi[2]; -// mat[1][1] = n_11 + angle_cos; -// mat[1][2] = n_12 + nsi[0]; -// mat[2][0] = n_02 + nsi[1]; -// mat[2][1] = n_12 - nsi[0]; -// mat[2][2] = n_22 + angle_cos; -//} +void matrix44transpose(FLT * mout, const FLT * minm ) +{ + mout[0] = minm[0]; + mout[1] = minm[4]; + mout[2] = minm[8]; + mout[3] = minm[12]; + + mout[4] = minm[1]; + mout[5] = minm[5]; + mout[6] = minm[9]; + mout[7] = minm[13]; + + mout[8] = minm[2]; + mout[9] = minm[6]; + mout[10] = minm[10]; + mout[11] = minm[14]; + mout[12] = minm[3]; + mout[13] = minm[7]; + mout[14] = minm[11]; + mout[15] = minm[15]; + +} diff --git a/redist/linmath.h b/redist/linmath.h index 676d182..66a38ed 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -89,9 +89,14 @@ void quatevenproduct( FLT * q, FLT * qa, FLT * qb ); void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ); void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ); 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 ); + + // Matrix Stuff typedef struct @@ -105,7 +110,7 @@ Matrix3x3 inverseM33(const Matrix3x3 mat); void matrix44copy(FLT * mout, const FLT * minm ); - +void matrix44transpose(FLT * mout, const FLT * minm ); #endif diff --git a/redist/lintest.c b/redist/lintest.c new file mode 100644 index 0000000..fa5a9d7 --- /dev/null +++ b/redist/lintest.c @@ -0,0 +1,112 @@ +#include "linmath.h" +#include <stdio.h> + +int main() +{ +#if 1 + +#define NONTRANSPOSED_DAVE +#ifdef NONTRANSPOSED_DAVE + FLT pLH1[3] = {-0.396888, 3.182945, -0.568622}; + FLT qLH1[4] = {0.668640, -0.576296, 0.103727, -0.458305}; + FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x + FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 }; + + + FLT pLH2[3] = {0.195579, 3.193770, -0.424473}; + FLT qLH2[4] = {0.401849, 0.104771, 0.580441, 0.700449}; + FLT pNLH2[3] = {-0.183505, 3.356293, 0.695688, }; + FLT qNLH2[4] = {-0.237438, 0.405213, 0.270438, 0.840410 }; +#else + + FLT pLH1[3] = {-0.321299, 3.130532, -0.786460}; + FLT qLH1[4] = {0.794180, 0.336117, -0.485668, -0.142934}; + FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x + FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 }; + + FLT pLH2[3] = {0.153580, 3.251673, -0.190491}; + FLT qLH2[4] = {0.217017, 0.482214, 0.306568, 0.791448 }; + FLT pNLH2[3] = {-0.175330, 3.351943, 0.669623 }; + FLT qNLH2[4] = {0.257241, 0.394159, 0.292555, 0.832392 }; +#endif + + FLT pOut1[3]; + FLT pOut2[3]; + + qLH1[0] *= -1; + qLH2[0] *= -1; + + quatrotatevector( pOut1, qLH1, pLH1 ); + quatrotatevector( pOut2, qLH2, pLH2 ); + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); + +// qLH1[1]*=-1; +// qLH2[0]*=-1; + +/* + sub3d( pOut1, pLH1, pNLH1 ); + sub3d( pOut2, pLH2, pNLH2 ); + + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); + + quatrotatevector( pOut1, qLH1, pOut1 ); + quatrotatevector( pOut2, qLH2, pOut2 ); + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); +*/ + return -1; + +#endif + +#if 0 + + FLT e[3] = { 1,1,3.14 }; + FLT q[4]; + FLT m[16]; + FLT pt[3] = { 1, 1, 1 }; + + q[0] = 0; + q[1] = 0; + q[2] = 0; + q[3] = 1; + + quatrotatevector( pt, q, pt ); + printf( "%f %f %f\n", PFTHREE( pt ) ); + printf( "\n" ); + + quatfromeuler( q, e ); + printf( "%f %f %f %f\n\n", PFFOUR( q ) ); + quattomatrix(m,q); + printf( "%f %f %f %f\n", PFFOUR( &m[0] ) ); + printf( "%f %f %f %f\n", PFFOUR( &m[4] ) ); + printf( "%f %f %f %f\n", PFFOUR( &m[8] ) ); + printf( "%f %f %f %f\n\n", PFFOUR( &m[12] ) ); + quatfrommatrix(q,m ); + printf( "%f %f %f %f\n\n", PFFOUR( q ) ); + quattoeuler( e,q ); + printf( "E: %f %f %f\n", e[0], e[1], e[2] ); + + + FLT pfromlh[3] = { 0, 1, 0 }; + FLT p[3] = { 0, 1, 0 }; + quatrotatevector( p, q, p ); + printf( "%f %f %f\n", PFTHREE( p ) ); + printf( "Flipping rotation\n" ); + q[0] *= -1; //Wow that was easy. + quatrotatevector( p, q, p ); + printf( "%f %f %f\n", PFTHREE( p ) ); + + //Try setting up a pose. +// FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] ); +// ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); +//void InvertPose( FLT * poseout, const FLT * pose ); + +#endif + +} + |