trick/include/trick/vector_macros.h

/*
PURPOSE:
    (Vector 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:
    (((Thomas J. Field) (Lockheed ESC) (January 1991) (Initial Release))
     ((Les Quiocho) (NASA/Johnson Space Center) (Jan 1991) (Initial Release))
     ((Les Quiocho) (NASA/JSC/ER3) (February 97) (Cleanup))
     ((Gavin Mendeck) (LinCom) (June 97) (V_MAG,min_zero modification))
     ((David Hammen) (Odyssey) (May 2005) (RDLaa07016) (Added new macros)))
     ((John M. Penn) (L3) (Aug 2010) (Document with doxygen)))
*/


/**
@page VECTOR_MACROS Vector Macros

$TRICK_HOME/trick_source/trick_utils/math/include/vector_macros.h

This set of macros operates on vectors. Parameters to the macros are as follows:

- 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 VECTOR_MACROS_H
#define VECTOR_MACROS_H

#include <stdio.h>
#include <math.h>
#include <limits.h>
#if (__vxworks | __APPLE__ | __linux | __CYGWIN__)
#include <float.h>
#endif

/**
@page VECTOR_MACROS
\b V_INIT(V)

Set all three elements of the vector V to 0.0.
\f[
v_i=0.0: i\in 0..2
\f]
*/
#define V_INIT( vect ) \
   vect[0] = vect[1] = vect[2] = 0.0

/**
@page VECTOR_MACROS
\b V_STORE(V, S )

Set all three elements of the vector V to scalar S.
\f[
v_i=S: i\in 0..2
\f]
*/
#define V_STORE( vect , scalar ) \
   vect[0] = vect[1] = vect[2] = scalar

/**
@page VECTOR_MACROS
\b V_COPY(COPY, V)

Assign the value of vector v to the vector copy.
\f[
copy_i = v_i: i\in 0..2
\f]
*/
#define V_COPY( copy , vect ) \
{   copy[0] = vect[0] ; copy[1] = vect[1] ; copy[2] = vect[2] ; }

/**
@page VECTOR_MACROS
\b V_ADD(S, A, B)

Assign the sum of vectors A and B to the vector S.
\f[
s_i = a_i + b_i: i \in 0..2
\f]
*/
#define V_ADD( sum , vect1 , vect2 ) { \
   sum[0] = vect1[0] + vect2[0] ; \
   sum[1] = vect1[1] + vect2[1] ; \
   sum[2] = vect1[2] + vect2[2] ; \
}

/**
@page VECTOR_MACROS
\b V_SUB(R, A, B)

Subtract vector B from vector A and assign the result to vector R.
\f[
r_i = a_i - b_i: i \in 0..2
\f]
*/
#define V_SUB( sum , vect1 , vect2 ) { \
   sum[0] = vect1[0] - vect2[0] ; \
   sum[1] = vect1[1] - vect2[1] ; \
   sum[2] = vect1[2] - vect2[2] ; \
}

/**
@page VECTOR_MACROS
\b V_SCALE(P, V, S)

Assign the product of vector V and scalar S to vector P.
\f[
p_i = v_i \cdot S: i \in 0..2
\f]
*/
#define V_SCALE( sum , vect , scalar ) { \
   sum[0] = vect[0] * scalar ; \
   sum[1] = vect[1] * scalar ; \
   sum[2] = vect[2] * scalar ; \
}

#define GSL_SQRT_DBL_MIN   1.4916681462400413e-154

/**
@page VECTOR_MACROS
\b V_MAG(V)

An expression of type double, which represents the scalar magnitude of
the vector V.
\f[
\left | V \right | \equiv \sqrt{ \sum_{i=0}^{2}v_{i}^2 }
\f]
*/
#define V_MAG( vect ) \
( sqrt( (((vect[0] < 0 ? -vect[0] : vect[0]) < GSL_SQRT_DBL_MIN) ? 0.0 : vect[0]*vect[0]) + \
        (((vect[1] < 0 ? -vect[1] : vect[1]) < GSL_SQRT_DBL_MIN) ? 0.0 : vect[1]*vect[1]) + \
        (((vect[2] < 0 ? -vect[2] : vect[2]) < GSL_SQRT_DBL_MIN) ? 0.0 : vect[2]*vect[2]) ) )

/**
@page VECTOR_MACROS
\b V_NORM(N, V)

Assign the unit vector, generated by dividing V by its length to N.
\f[
N=\frac{V}{\left |V  \right |}
\f]
*/
#define V_NORM( norm , vect ) \
{ \
   double tmp_vector_magnitude ; \
   tmp_vector_magnitude = V_MAG( vect ) ; \
   if( tmp_vector_magnitude > 1.0e-12 ) { \
      norm[0] = vect[0] / tmp_vector_magnitude ;\
      norm[1] = vect[1] / tmp_vector_magnitude ;\
      norm[2] = vect[2] / tmp_vector_magnitude ; \
   } \
      else norm[0] = norm[1] = norm[2] = 0.0 ; \
}

/**
@page VECTOR_MACROS
\b V_SKEW(SKEW, V)

Transform a vector V to a skew symmetric matrix form,
storing it in the matrix skew ( double[3][3] ).
\f[
SKEW =
\begin{bmatrix}
   0 & -v_2 &  v_1 \\
 v_2 &    0 & -v_0 \\
-v_1 &  v_0 &  0
\end{bmatrix}
\f]
*/
#define V_SKEW( skew , vect ) { \
   skew[0][0] = 0.0 ; skew[1][1] = 0.0 ; skew[2][2] = 0.0 ; \
   skew[0][1] = - vect[2] ; skew[0][2] = vect[1] ; \
   skew[1][0] = vect[2] ; skew[1][2] = - vect[0] ; \
   skew[2][0] = - vect[1] ; skew[2][1] = vect[0] ; \
}

/**
@page VECTOR_MACROS
\b V_DOT(A, B)

An expression returning type double, which represents the
dot product (scalar product) of A and B.
It is equivalent to:
\f[
\sum_{i=0}^{2} a_i \cdot b_i
\f]
*/
#define V_DOT( vect1 , vect2 ) \
( vect1[0]*vect2[0] + vect1[1]*vect2[1] + vect1[2]* vect2[2] )

/**
@page VECTOR_MACROS
\b V_OUTER(OUTER, V)

\f[
OUTER = V \cdot V^T
\f]
\f[
outer_{i,j} = v_i \cdot v_j: i,j \in 0..2
\f]
*/
#define V_OUTER( outer , vec ) { \
   outer[0][0] = vec[0] * vec[0] ; \
   outer[1][0] = vec[1] * vec[0] ; \
   outer[2][0] = vec[2] * vec[0] ; \
   outer[0][1] = vec[0] * vec[1] ; \
   outer[1][1] = vec[1] * vec[1] ; \
   outer[2][1] = vec[2] * vec[1] ; \
   outer[0][2] = vec[0] * vec[2] ; \
   outer[1][2] = vec[1] * vec[2] ; \
   outer[2][2] = vec[2] * vec[2] ; \
}

/**
@page VECTOR_MACROS
\b V_CROSS(CROSS, A, B )

Assign cross product of vectors A and B to vector CROSS.
This is equivalent to:
\f[
CROSS =
\left \langle
\left ( a_1 \cdot b_2 - a_2 \cdot b_1 \right ),
\left ( a_2 \cdot b_0 - a_0 \cdot b_2 \right ),
\left ( a_0 \cdot b_1 - a_1 \cdot b_0 \right )
\right \rangle
\f]

*/
#define V_CROSS( cross, vect1, vect2 ) { \
   cross[0] = ((vect1[1]*vect2[2]) - (vect1[2]*vect2[1])) ; \
   cross[1] = ((vect1[2]*vect2[0]) - (vect1[0]*vect2[2])) ; \
   cross[2] = ((vect1[0]*vect2[1]) - (vect1[1]*vect2[0])) ; \
}

/**
@page VECTOR_MACROS
\b VxV_ADD(C, A, B)

\f[
C = C + A * B
\f]
*/
#define VxV_ADD( sum, vect1, vect2 ) { \
   sum[0] += ((vect1[1]*vect2[2]) - (vect1[2]*vect2[1])) ; \
   sum[1] += ((vect1[2]*vect2[0]) - (vect1[0]*vect2[2])) ; \
   sum[2] += ((vect1[0]*vect2[1]) - (vect1[1]*vect2[0])) ; \
}

/**
@page VECTOR_MACROS
\b VxV_SUB(C , A, B )

\f[
C = C - A * B
\f]
*/
#define VxV_SUB( diff , vect1, vect2 ) { \
   diff[0] -= ((vect1[1]*vect2[2]) - (vect1[2]*vect2[1])) ; \
   diff[1] -= ((vect1[2]*vect2[0]) - (vect1[0]*vect2[2])) ; \
   diff[2] -= ((vect1[0]*vect2[1]) - (vect1[1]*vect2[0])) ; \
}

/**
@page VECTOR_MACROS
\b VxM(P, V, M)

Assign the product of vector V and matrix M to vector P.
\f[
p_i = \sum_{j=0}^{2} v_j \cdot m_{j,i}: i\in 0..2
\f]
*/
#define VxM( prod , vect , mat ) { \
   prod[0] = vect[0]*mat[0][0] + vect[1]*mat[1][0] + vect[2]*mat[2][0] ; \
   prod[1] = vect[0]*mat[0][1] + vect[1]*mat[1][1] + vect[2]*mat[2][1] ; \
   prod[2] = vect[0]*mat[0][2] + vect[1]*mat[1][2] + vect[2]*mat[2][2] ; \
}

/**
@page VECTOR_MACROS
\b V_PRINT( v )

Print to stderr the three elements of vector v.
*/
#define V_PRINT( vect ) { \
   fprintf( stderr, " %f %f %f " , vect[0] , vect[1] , vect[2] ) ; }

/**
@page VECTOR_MACROS
\b V_ADD3(S, A, B, C)

Assign the sum of vectors A, B and C to sum.
\f[
S=A+B+C
\f]
*/
#define V_ADD3(sum,vec1,vec2,vec3) { \
   sum[0] = vec1[0] + vec2[0] + vec3[0]; \
   sum[1] = vec1[1] + vec2[1] + vec3[1]; \
   sum[2] = vec1[2] + vec2[2] + vec3[2]; \
}

/**
@page VECTOR_MACROS
\b V_DECR(S, D)

Subtract vector D from vector S, leaving the result in S.
\f[
S=S-D
\f]
*/
#define V_DECR(sum,decr) { \
   sum[0] -= decr[0]; \
   sum[1] -= decr[1]; \
   sum[2] -= decr[2]; \
}

/**
@page VECTOR_MACROS
\b V_INCR(S, I)

Add the vector I to vector S, leaving the result in S.
\f[
S=S+I
\f]
*/
#define V_INCR(sum,incr) { \
   sum[0] += incr[0]; \
   sum[1] += incr[1]; \
   sum[2] += incr[2]; \
}

/**
@page VECTOR_MACROS
\b V_NEGATE(D, S)

Assign the negative of vector S to the vector D.
\f[
D=-S
\f]
*/
#define V_NEGATE(dest,src) { \
   dest[0] = -src[0]; \
   dest[1] = -src[1]; \
   dest[2] = -src[2]; \
}

/**
@page VECTOR_MACROS
\b VxS_ADD(DEST, SRC, SCALE)

Add the product of the scalar SCALE and the vector SRC to the vector DEST.
\f[
DEST = DEST + SCALE * SRC
\f]
*/
#define VxS_ADD(dest,src,scale) { \
   dest[0] += src[0]*scale; \
   dest[1] += src[1]*scale; \
   dest[2] += src[2]*scale; \
}

/**
@page VECTOR_MACROS
\b VxS_SUB(DEST, SRC, SCALE)

Subtract the product of the scalar SCALE and the vector SRC from the vector DEST.
\f[
DEST = DEST - SCALE * SRC
\f]
*/
#define VxS_SUB(dest,src,scale) { \
   dest[0] -= src[0]*scale; \
   dest[1] -= src[1]*scale; \
   dest[2] -= src[2]*scale; \
}

/**
@page VECTOR_MACROS
\b V_ZERO_SMALL(V, LIMIT)

For each element of vector V, set its value to 0 if it is less than LIMIT.
*/
#define V_ZERO_SMALL(vec,lim) { \
   if ((vec[0] > -(lim)) && (vec[0] < (lim))) vec[0] = 0.0; \
   if ((vec[1] > -(lim)) && (vec[1] < (lim))) vec[1] = 0.0; \
   if ((vec[2] > -(lim)) && (vec[2] < (lim))) vec[2] = 0.0; \
}

#endif /* _VECTOR_MACROS_H_ */