mirror of
https://github.com/nasa/trick.git
synced 2025-01-09 22:42:47 +00:00
14a75508a3
Changed all header file once include variables to follow the same naming convention and not start with any underscores. Also deleted old incorrect copyright notices. Also removed $Id: tags from all files. Fixes #14. Fixes #22.
139 lines
3.0 KiB
C
139 lines
3.0 KiB
C
/*
|
|
PURPOSE:
|
|
(Quaternion math in line code macros.)
|
|
|
|
REFERENCE:
|
|
((None))
|
|
|
|
PROGRAMMERS:
|
|
(((David Hammen) (Odyssey) (May 2005) (RDLaa07016) (Initial release.)))
|
|
*/
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS Quaternian Macros
|
|
|
|
Include:
|
|
$TRICK_HOME/trick_source/trick_utils/math/include/quat_macros.h
|
|
|
|
This set of macros operates on quaternians which are represented
|
|
as an array of four floating point types (i,e., double quat[4] or float quat[4]).
|
|
|
|
- Element 0 of the array represents the scalar part of the quaternian.
|
|
- Elements 1..3 of the array represent the vector part of the quaternian.
|
|
*/
|
|
|
|
#ifndef _QUAT_MACROS_H_
|
|
#define _QUAT_MACROS_H_
|
|
|
|
#include "vector_macros.h"
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b Q_IDENT( Q)
|
|
|
|
Set Q to the identity quaternian.
|
|
Q = (1.0, 0.0, 0.0, 0.0)
|
|
*/
|
|
#define Q_IDENT(quat) { \
|
|
*quat = 1.0; \
|
|
V_INIT ((quat+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b Q_COPY(D, S)
|
|
|
|
Copy quaternian S to quaternian D.
|
|
*/
|
|
#define Q_COPY(dest,src) { \
|
|
*dest = *src; \
|
|
V_COPY ((dest+1), (src+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b Q_SCALE(D, Q, C)
|
|
|
|
Assign the product of quaternian Q and scalar C to quaternian D.
|
|
*/
|
|
#define Q_SCALE(dest,src,scale) { \
|
|
*dest = *src * scale; \
|
|
V_SCALE ((dest+1), (src+1), scale); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b Q_TRANS(D, Q)
|
|
|
|
Assign the conjugate of quaternian Q to quaternian D.
|
|
*/
|
|
#define Q_TRANS(dest,src) { \
|
|
*dest = *src; \
|
|
V_NEGATE ((dest+1), (src+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b QxQ(D, A, B)
|
|
|
|
Assign the product of quaternian A and quaternian B to quaternian D.
|
|
*/
|
|
#define QxQ(dest,quat1,quat2) { \
|
|
*dest = *quat1 * *quat2 - V_DOT ((quat1+1), (quat2+1)); \
|
|
V_SCALE ((dest+1), (quat2+1), *quat1); \
|
|
VxS_ADD ((dest+1), (quat1+1), *quat2); \
|
|
VxV_ADD ((dest+1), (quat1+1), (quat2+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b QtxQ(D, A, B)
|
|
|
|
Assign the product of quaternian A conjugate and B to quaternian D.
|
|
*/
|
|
#define QtxQ(dest,quat1,quat2) { \
|
|
*dest = *quat1 * *quat2 + V_DOT ((quat1+1), (quat2+1)); \
|
|
V_SCALE ((dest+1), (quat2+1), *quat1); \
|
|
VxS_SUB ((dest+1), (quat1+1), *quat2); \
|
|
VxV_SUB ((dest+1), (quat1+1), (quat2+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b QxQt(D, A, B)
|
|
|
|
Assign the product of quaternian A and quaternian B conjugate to quaternian D.
|
|
*/
|
|
#define QxQt(dest,quat1,quat2) { \
|
|
*dest = *quat1 * *quat2 + V_DOT ((quat1+1), (quat2+1)); \
|
|
V_SCALE ((dest+1), (quat1+1), *quat2); \
|
|
VxS_SUB ((dest+1), (quat2+1), *quat1); \
|
|
VxV_SUB ((dest+1), (quat1+1), (quat2+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b VxQ(D, V, Q)
|
|
|
|
Assign the product of vector V and quaternian Q to quaternian D.
|
|
*/
|
|
#define VxQ(dest,vec,quat) { \
|
|
*dest = - V_DOT (vec, (quat+1)); \
|
|
V_SCALE ((dest+1), vec, *quat); \
|
|
VxV_ADD ((dest+1), vec, (quat+1)); \
|
|
}
|
|
|
|
/**
|
|
@page QUATERNIAN_MACROS
|
|
\b QxV(D, Q, V)
|
|
|
|
Assign the product of quaternian Q and vector V to quaternian D.
|
|
*/
|
|
#define QxV(dest,quat,vec) { \
|
|
*dest = - V_DOT ((quat+1), vec); \
|
|
V_SCALE ((dest+1), vec, *quat); \
|
|
VxV_ADD ((dest+1), (quat+1), vec); \
|
|
}
|
|
|
|
#endif /* _QUAT_MACROS_H_ DO NOT PUT ANYTHING AFTER THIS LINE */
|