ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-28 08:58:53 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
678cc4b877
trick/trick_source/trick_utils/math/include/matrix_macros.h
Alex Lin f0c594f841 Initial commit of everything.
2015-02-26 09:02:31 -06:00

329 lines
11 KiB
C
Raw Blame History

/*******************************************************************************
* *
* Trick Simulation Environment Software *
* *
* Copyright (c) 1996,1997 LinCom Corporation, Houston, TX *
* All rights reserved. *
* *
* Copyrighted by LinCom Corporation and proprietary to it. Any unauthorized *
* use of Trick Software including source code, object code or executables is *
* strictly prohibited and LinCom assumes no liability for such actions or *
* results thereof. *
* *
* Trick Software has been developed under NASA Government Contracts and *
* access to it may be granted for Government work by the following contact: *
* *
* Contact: Charles Gott, Branch Chief *
* Simulation and Graphics Branch *
* Automation, Robotics, & Simulation Division *
* NASA, Johnson Space Center, Houston, TX *
* *
*******************************************************************************/
/*
PURPOSE:
(Matrix math in line code macros)
REFERENCE:
(((Bailey, R.W)
(Trick Simulation Environment Developer's Guide - Beta Release)
(NASA:JSC #......)
(JSC / Engineering Directorate / Automation and Robotics Division)
(February 1991) ()))
PROGRAMMERS:
(((Les Quiocho) (NASA/Johnson Space Center) (Jan 1991) (Initial Release))
((Les Quiocho) (NASA/JSC/ER3) (February 97) (Cleanup)))
((John M. Penn) (L3) (Aug 2010) (Documented for Doxygen)))
*/
/*
* $Id: matrix_macros.h 1062 2010-09-08 21:55:54Z penn $
*/
/**
@page MATRIX_MACROS Matrix Macros
$TRICK_HOME/trick_source/trick_utils/math/include/matrix_macros.h
- Scalar parameters are expected to be of type float or double.
- Vector parameters are expected to be of type float[3] or double[3];
- Matrix parameters are expected to be of type float[3,3] or double[3,3];
*/
#ifndef _MATRIX_MACROS_H_
#define _MATRIX_MACROS_H_
#include <stdio.h>
/**
@page MATRIX_MACROS Matrix Macros
\b M_INIT( M )
Set all nine elements of the matrix m to 0.0.
\f[
M =
\begin{bmatrix}
0.0 & 0.0 & 0.0 \\
0.0 & 0.0 & 0.0 \\
0.0 & 0.0 & 0.0
\end{bmatrix}
\f]
*/
#define M_INIT( mat ) { \
mat[0][0] = mat[1][1] = mat[2][2] = 0.0 ; \
mat[0][1] = mat[1][0] = 0.0 ; \
mat[1][2] = mat[2][1] = 0.0 ; \
mat[2][0] = mat[0][2] = 0.0 ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b M_IDENT( M )
Set M to the identity matrix.
\f[
M =
\begin{bmatrix}
1.0 & 0.0 & 0.0 \\
0.0 & 1.0 & 0.0 \\
0.0 & 0.0 & 1.0
\end{bmatrix}
\f]
*/
#define M_IDENT( mat ) { \
mat[0][0] = mat[1][1] = mat[2][2] = 1.0 ; \
mat[0][1] = mat[1][0] = 0.0 ; \
mat[1][2] = mat[2][1] = 0.0 ; \
mat[2][0] = mat[0][2] = 0.0 ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b M_COPY(COPY, M)
Copy matrix M into matrix COPY.
\f[
copy_{i,j} = m_{i,j}:{i,j}\in 0..2
\f]
*/
#define M_COPY( copy , mat ) { \
copy[0][0] = mat[0][0] ; copy[0][1] = mat[0][1] ; copy[0][2] = mat[0][2] ; \
copy[1][0] = mat[1][0] ; copy[1][1] = mat[1][1] ; copy[1][2] = mat[1][2] ; \
copy[2][0] = mat[2][0] ; copy[2][1] = mat[2][1] ; copy[2][2] = mat[2][2] ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b M_TRANS(TRANSPOSE ,M )
Transpose matrix M (swap rows and columns) and assign to matrix TRANSPOSE.
\f[
transpose_{i,j} = m_{j,i}:{i,j}\in 0..2
\f]
*/
#define M_TRANS( trans , mat ) { \
trans[0][0]=mat[0][0] ; trans[0][1]=mat[1][0] ; trans[0][2]=mat[2][0] ; \
trans[1][0]=mat[0][1] ; trans[1][1]=mat[1][1] ; trans[1][2]=mat[2][1] ; \
trans[2][0]=mat[0][2] ; trans[2][1]=mat[1][2] ; trans[2][2]=mat[2][2] ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b M_ADD(S, A ,B)
Assign the sum of matrices A and B to the matrix S.
\f[
s_{i,j} = a_{i,j} + b_{i,j}: {i,j}\in 0..2
\f]
*/
#define M_ADD( sum , mat1 , mat2 ) { \
sum[0][0] = mat1[0][0] + mat2[0][0] ; \
sum[0][1] = mat1[0][1] + mat2[0][1] ; \
sum[0][2] = mat1[0][2] + mat2[0][2] ; \
sum[1][0] = mat1[1][0] + mat2[1][0] ; \
sum[1][1] = mat1[1][1] + mat2[1][1] ; \
sum[1][2] = mat1[1][2] + mat2[1][2] ; \
sum[2][0] = mat1[2][0] + mat2[2][0] ; \
sum[2][1] = mat1[2][1] + mat2[2][1] ; \
sum[2][2] = mat1[2][2] + mat2[2][2] ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b M_SUB(D, A, B)
Subtract matrix B from matrix A and assign the difference the matrix D.
\f[
d_{i,j} = a_{i,j} - b_{i,j}: {i,j}\in 0..2
\f]
*/
#define M_SUB( diff , mat1 , mat2 ) { \
diff[0][0] = mat1[0][0] - mat2[0][0] ; \
diff[0][1] = mat1[0][1] - mat2[0][1] ; \
diff[0][2] = mat1[0][2] - mat2[0][2] ; \
diff[1][0] = mat1[1][0] - mat2[1][0] ; \
diff[1][1] = mat1[1][1] - mat2[1][1] ; \
diff[1][2] = mat1[1][2] - mat2[1][2] ; \
diff[2][0] = mat1[2][0] - mat2[2][0] ; \
diff[2][1] = mat1[2][1] - mat2[2][1] ; \
diff[2][2] = mat1[2][2] - mat2[2][2] ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b MxV(P, M, V)
Assign the product of matrix M and vector V to vector P.
\f[
p_i = \sum_{j=0}^{2} m_{i,j} v_i: i\in 0..2
\f]
*/
#define MxV( prod , mat , vect ) { \
prod[0] = mat[0][0] * vect[0] + mat[0][1] * vect[1] + mat[0][2] * vect[2] ; \
prod[1] = mat[1][0] * vect[0] + mat[1][1] * vect[1] + mat[1][2] * vect[2] ; \
prod[2] = mat[2][0] * vect[0] + mat[2][1] * vect[1] + mat[2][2] * vect[2] ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b MtxV(P, M, V)
Assign the product of the transpose of matrix M and vector V to vector P.
\f[
p_i = \sum_{j=0}^{2} m_{j,i} \cdot v_j: i\in 0..2
\f]
*/
#define MtxV( prod , mat , vect ) { \
prod[0] = mat[0][0] * vect[0] + mat[1][0] * vect[1] + mat[2][0] * vect[2] ; \
prod[1] = mat[0][1] * vect[0] + mat[1][1] * vect[1] + mat[2][1] * vect[2] ; \
prod[2] = mat[0][2] * vect[0] + mat[1][2] * vect[1] + mat[2][2] * vect[2] ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b MxSCALAR(P, M, S)
Assign the product of the matrix M and scalar S to vector P.
\f[
p_i = m_{i,j} \cdot S: i\in 0..2
\f]
*/
#define MxSCALAR( prod , mat , scalar ) { \
prod[0][0]=mat[0][0] * scalar ; \
prod[0][1]=mat[0][1] * scalar ; \
prod[0][2]=mat[0][2] * scalar ; \
prod[1][0]=mat[1][0] * scalar ; \
prod[1][1]=mat[1][1] * scalar ; \
prod[1][2]=mat[1][2] * scalar ; \
prod[2][0]=mat[2][0] * scalar ; \
prod[2][1]=mat[2][1] * scalar ; \
prod[2][2]=mat[2][2] * scalar ; \
}
/**
@page MATRIX_MACROS Matrix Macros
\b MxM(P, A, B)
Assign the product of the matrices A and B to matrix P.
\f[
p_{i,j} = \sum_{k=0}^{2}a_{i,k} \cdot b_{k,j}: i,j\in 0..2
\f]
*/
#define MxM( prod , mat1 , mat2 ) { \
prod[0][0]=mat1[0][0]*mat2[0][0]+mat1[0][1]*mat2[1][0]+mat1[0][2]*mat2[2][0] ;\
prod[0][1]=mat1[0][0]*mat2[0][1]+mat1[0][1]*mat2[1][1]+mat1[0][2]*mat2[2][1] ;\
prod[0][2]=mat1[0][0]*mat2[0][2]+mat1[0][1]*mat2[1][2]+mat1[0][2]*mat2[2][2] ;\
prod[1][0]=mat1[1][0]*mat2[0][0]+mat1[1][1]*mat2[1][0]+mat1[1][2]*mat2[2][0] ;\
prod[1][1]=mat1[1][0]*mat2[0][1]+mat1[1][1]*mat2[1][1]+mat1[1][2]*mat2[2][1] ;\
prod[1][2]=mat1[1][0]*mat2[0][2]+mat1[1][1]*mat2[1][2]+mat1[1][2]*mat2[2][2] ;\
prod[2][0]=mat1[2][0]*mat2[0][0]+mat1[2][1]*mat2[1][0]+mat1[2][2]*mat2[2][0] ;\
prod[2][1]=mat1[2][0]*mat2[0][1]+mat1[2][1]*mat2[1][1]+mat1[2][2]*mat2[2][1] ;\
prod[2][2]=mat1[2][0]*mat2[0][2]+mat1[2][1]*mat2[1][2]+mat1[2][2]*mat2[2][2] ;\
}
/**
@page MATRIX_MACROS Matrix Macros
\b MtxM(P, A, B)
Assign the product of the A transpose and B to matrix P.
\f[
P = A^T \cdot B
\f]
\f[
p_{i,j} = \sum_{k=0}^{2}a_{k,i} \cdot b_{k,j}: i,j\in 0..2
\f]
*/
#define MtxM( prod , mat1 , mat2 ) { \
prod[0][0]=mat1[0][0]*mat2[0][0]+mat1[1][0]*mat2[1][0]+mat1[2][0]*mat2[2][0] ;\
prod[0][1]=mat1[0][0]*mat2[0][1]+mat1[1][0]*mat2[1][1]+mat1[2][0]*mat2[2][1] ;\
prod[0][2]=mat1[0][0]*mat2[0][2]+mat1[1][0]*mat2[1][2]+mat1[2][0]*mat2[2][2] ;\
prod[1][0]=mat1[0][1]*mat2[0][0]+mat1[1][1]*mat2[1][0]+mat1[2][1]*mat2[2][0] ;\
prod[1][1]=mat1[0][1]*mat2[0][1]+mat1[1][1]*mat2[1][1]+mat1[2][1]*mat2[2][1] ;\
prod[1][2]=mat1[0][1]*mat2[0][2]+mat1[1][1]*mat2[1][2]+mat1[2][1]*mat2[2][2] ;\
prod[2][0]=mat1[0][2]*mat2[0][0]+mat1[1][2]*mat2[1][0]+mat1[2][2]*mat2[2][0] ;\
prod[2][1]=mat1[0][2]*mat2[0][1]+mat1[1][2]*mat2[1][1]+mat1[2][2]*mat2[2][1] ;\
prod[2][2]=mat1[0][2]*mat2[0][2]+mat1[1][2]*mat2[1][2]+mat1[2][2]*mat2[2][2] ;\
}
/**
@page MATRIX_MACROS Matrix Macros
\b MxMt(P, A, B)
Assign the product of the A and B transpose to matrix P.
\f[
P = A \cdot B^T
\f]
\f[
prod_{i,j} = \sum_{k=0}^{2}a_{i,k} \cdot b_{j,k}: i,j\in 0..2
\f]
*/
#define MxMt( prod , mat1 , mat2 ) { \
prod[0][0]=mat1[0][0]*mat2[0][0]+mat1[0][1]*mat2[0][1]+mat1[0][2]*mat2[0][2] ;\
prod[0][1]=mat1[0][0]*mat2[1][0]+mat1[0][1]*mat2[1][1]+mat1[0][2]*mat2[1][2] ;\
prod[0][2]=mat1[0][0]*mat2[2][0]+mat1[0][1]*mat2[2][1]+mat1[0][2]*mat2[2][2] ;\
prod[1][0]=mat1[1][0]*mat2[0][0]+mat1[1][1]*mat2[0][1]+mat1[1][2]*mat2[0][2] ;\
prod[1][1]=mat1[1][0]*mat2[1][0]+mat1[1][1]*mat2[1][1]+mat1[1][2]*mat2[1][2] ;\
prod[1][2]=mat1[1][0]*mat2[2][0]+mat1[1][1]*mat2[2][1]+mat1[1][2]*mat2[2][2] ;\
prod[2][0]=mat1[2][0]*mat2[0][0]+mat1[2][1]*mat2[0][1]+mat1[2][2]*mat2[0][2] ;\
prod[2][1]=mat1[2][0]*mat2[1][0]+mat1[2][1]*mat2[1][1]+mat1[2][2]*mat2[1][2] ;\
prod[2][2]=mat1[2][0]*mat2[2][0]+mat1[2][1]*mat2[2][1]+mat1[2][2]*mat2[2][2] ;\
}
/**
@page MATRIX_MACROS Matrix Macros
\b MtxMt(P, A, B)
Assign the product of the A transpose and B transpose to matrix P.
\f[
P = A^T \cdot B^T
\f]
\f[
prod_{i,j} = \sum_{k=0}^{2}a_{k,i} \cdot b_{j,k}: i,j\in 0..2
\f]
*/
#define MtxMt( prod , mat1 , mat2 ) { \
prod[0][0]=mat1[0][0]*mat2[0][0]+mat1[1][0]*mat2[0][1]+mat1[2][0]*mat2[0][2] ;\
prod[0][1]=mat1[0][0]*mat2[1][0]+mat1[1][0]*mat2[1][1]+mat1[2][0]*mat2[1][2] ;\
prod[0][2]=mat1[0][0]*mat2[2][0]+mat1[1][0]*mat2[2][1]+mat1[2][0]*mat2[2][2] ;\
prod[1][0]=mat1[0][1]*mat2[0][0]+mat1[1][1]*mat2[0][1]+mat1[2][1]*mat2[0][2] ;\
prod[1][1]=mat1[0][1]*mat2[1][0]+mat1[1][1]*mat2[1][1]+mat1[2][1]*mat2[1][2] ;\
prod[1][2]=mat1[0][1]*mat2[2][0]+mat1[1][1]*mat2[2][1]+mat1[2][1]*mat2[2][2] ;\
prod[2][0]=mat1[0][2]*mat2[0][0]+mat1[1][2]*mat2[0][1]+mat1[2][2]*mat2[0][2] ;\
prod[2][1]=mat1[0][2]*mat2[1][0]+mat1[1][2]*mat2[1][1]+mat1[2][2]*mat2[1][2] ;\
prod[2][2]=mat1[0][2]*mat2[2][0]+mat1[1][2]*mat2[2][1]+mat1[2][2]*mat2[2][2] ;\
}
/**
@page MATRIX_MACROS Matrix Macros
\b M_PRINT(M)
Print matrix M to stderr.
*/
#define M_PRINT( mat ) { \
fprintf( stderr, "\n%f %f %f\n" , mat[0][0] , mat[0][1] , mat[0][2] ) ; \
fprintf( stderr, "%f %f %f\n" , mat[1][0] , mat[1][1] , mat[1][2] ) ; \
fprintf( stderr, "%f %f %f\n" , mat[2][0] , mat[2][1] , mat[2][2] ) ; \
}
#endif
Reference in New Issue View Git Blame Copy Permalink