jcrussell/libsst
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
James Russell
2026-04-03 00:22:39 -05:00
commit eca1e8c458
945 changed files with 218160 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

382
Lib/Include/CML/mathlib/checking.h Normal file
View File

@@ -0,0 +1,382 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef checking_h
#define checking_h
#include <cml/vector/vector_expr.h>
#include <cml/matrix/matrix_expr.h>
#include <cml/quaternion/quaternion_expr.h>
/* Run- and compile-time checking of argument types, values and sizes. */
struct function_expects_vector_arg_error;
struct function_expects_matrix_arg_error;
struct function_expects_quaternion_arg_error;
struct function_expects_2D_vector_arg_error;
struct function_expects_3D_vector_arg_error;
struct function_expects_4D_vector_arg_error;
struct function_expects_2D_or_3D_vector_arg_error;
struct function_expects_2x2_matrix_arg_error;
struct function_expects_3x3_matrix_arg_error;
struct function_expects_4x4_matrix_arg_error;
struct function_expects_square_matrix_arg_error;
struct matrix_arg_fails_minimum_size_requirement;
namespace cml {
namespace detail {
//////////////////////////////////////////////////////////////////////////////
// Vector argument checking
//////////////////////////////////////////////////////////////////////////////
/** Compile-time check for a vector argument */
template< class VecT > inline void
CheckVec(const VecT&)
{
typedef et::ExprTraits<VecT> vector_traits;
typedef typename vector_traits::result_tag result_type;
CML_STATIC_REQUIRE_M(
(same_type<result_type, et::vector_result_tag>::is_true),
function_expects_vector_arg_error);
}
/** Compile-time check for a vector of size N */
template< class VecT, size_t N, class ErrorT > inline void
CheckVecN(const VecT& v, fixed_size_tag) {
CheckVec(v);
CML_STATIC_REQUIRE_M(((size_t)VecT::array_size == N), ErrorT);
}
/** Run-time check for a vector of size N */
template< class VecT, size_t N, class /*ErrorT*/ > inline void
CheckVecN(const VecT& v, dynamic_size_tag) {
CheckVec(v);
et::GetCheckedSize<VecT,VecT,dynamic_size_tag>()
.equal_or_fail(v.size(),size_t(N));
}
/** Check for a vector of size N */
template< class VecT, size_t N, class ErrorT > inline void
CheckVecN(const VecT& v) {
typedef et::ExprTraits<VecT> vector_traits;
typedef typename vector_traits::size_tag size_tag;
detail::CheckVecN<VecT,N,ErrorT>(v, size_tag());
}
/** Check for a vector of size 2 */
template< class VecT > inline void
CheckVec2(const VecT& v) {
detail::CheckVecN<VecT,2,function_expects_2D_vector_arg_error>(v);
}
/** Check for a vector of size 3 */
template< class VecT > inline void
CheckVec3(const VecT& v) {
detail::CheckVecN<VecT,3,function_expects_3D_vector_arg_error>(v);
}
/** Check for a vector of size 4 */
template< class VecT > inline void
CheckVec4(const VecT& v) {
CheckVecN<VecT,4,function_expects_4D_vector_arg_error>(v);
}
/** Compile-time check for a vector of size 2 or 3 */
template< class VecT > inline void
CheckVec2Or3(const VecT& v, fixed_size_tag) {
CheckVec(v);
CML_STATIC_REQUIRE_M(
(VecT::array_size == 2 || VecT::array_size == 3),
function_expects_2D_or_3D_vector_arg_error);
}
/** Run-time check for a vector of size 2 or 3 */
template< class VecT > inline void
CheckVec2Or3(const VecT& v, dynamic_size_tag) {
CheckVec(v);
if (v.size() != 2 && v.size() != 3) {
throw std::invalid_argument("2d or 3d vector arg expected");
}
}
/** Check for a vector of size 2 or 3 */
template< class VecT > inline void
CheckVec2Or3(const VecT& v) {
typedef et::ExprTraits<VecT> vector_traits;
typedef typename vector_traits::size_tag size_tag;
detail::CheckVec2Or3(v, size_tag());
}
//////////////////////////////////////////////////////////////////////////////
// Matrix argument checking
//////////////////////////////////////////////////////////////////////////////
/** Compile-time check for a matrix argument */
template< class MatT > inline void
CheckMat(const MatT&)
{
typedef et::ExprTraits<MatT> matrix_traits;
typedef typename matrix_traits::result_tag result_type;
CML_STATIC_REQUIRE_M(
(same_type<result_type, et::matrix_result_tag>::is_true),
function_expects_matrix_arg_error);
}
/** Compile-time check for a matrix of size NxM */
template< class MatT, size_t N, size_t M, class ErrorT > inline void
CheckMatNxM(const MatT& m, fixed_size_tag) {
CheckMat(m);
CML_STATIC_REQUIRE_M(
(MatT::array_rows == N && MatT::array_cols == M), ErrorT);
}
/** Run-time check for a matrix of size NxM */
template< class MatT, size_t N, size_t M, class /*ErrorT*/ > inline void
CheckMatNxM(const MatT& m, dynamic_size_tag) {
CheckMat(m);
et::GetCheckedSize<MatT,MatT,dynamic_size_tag>()
.equal_or_fail(m.rows(),N);
et::GetCheckedSize<MatT,MatT,dynamic_size_tag>()
.equal_or_fail(m.cols(),M);
}
/** Check for a matrix of size NxM */
template< class MatT, size_t N, size_t M, class ErrorT > inline void
CheckMatNxM(const MatT& m) {
typedef et::ExprTraits<MatT> matrix_traits;
typedef typename matrix_traits::size_tag size_tag;
CheckMatNxM<MatT,N,M,ErrorT>(m, size_tag());
}
/** Check for a square matrix of size NxN */
template< class MatT, size_t N, class ErrorT > inline void
CheckMatN(const MatT& m) {
CheckMatNxM<MatT,N,N,ErrorT>(m);
}
/** Check for a square matrix of size 2x2 */
template< class MatT > inline void
CheckMat2x2(const MatT& m) {
CheckMatN<MatT,2,function_expects_2x2_matrix_arg_error>(m);
}
/** Check for a square matrix of size 3x3 */
template< class MatT > inline void
CheckMat3x3(const MatT& m) {
CheckMatN<MatT,3,function_expects_3x3_matrix_arg_error>(m);
}
/** Check for a square matrix of size 4x4 */
template< class MatT > inline void
CheckMat4x4(const MatT& m) {
CheckMatN<MatT,4,function_expects_4x4_matrix_arg_error>(m);
}
/** Compile-time check for a matrix with minimum dimensions NxM */
template< class MatT, size_t N, size_t M, class ErrorT > inline void
CheckMatMinNxM(const MatT& m, fixed_size_tag) {
CheckMat(m);
CML_STATIC_REQUIRE_M(
(MatT::array_rows >= N && MatT::array_cols >= M), ErrorT);
}
/** Run-time check for a matrix with minimum dimensions NxM */
template< class MatT, size_t N, size_t M, class /*ErrorT*/ > inline void
CheckMatMinNxM(const MatT& m, dynamic_size_tag) {
CheckMat(m);
if (m.rows() < N || m.cols() < M) {
throw std::invalid_argument(
"matrix does not meet minimum size requirement");
}
}
/** Check for a matrix with minimum dimensions NxM */
template< class MatT, size_t N, size_t M, class ErrorT > inline void
CheckMatMinNxM(const MatT& m) {
typedef et::ExprTraits<MatT> matrix_traits;
typedef typename matrix_traits::size_tag size_tag;
CheckMatMinNxM<MatT,N,M,ErrorT>(m, size_tag());
}
/** Check for a matrix with minimum dimensions NxN */
template< class MatT, size_t N, class ErrorT > inline void
CheckMatMinN(const MatT& m) {
CheckMatMinNxM<MatT,N,N,ErrorT>(m);
}
/** Check for a matrix with minimum dimensions 2x2 */
template< class MatT > inline void
CheckMatMin2x2(const MatT& m) {
CheckMatMinN<MatT,2,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix with minimum dimensions 3x3 */
template< class MatT > inline void
CheckMatMin3x3(const MatT& m) {
CheckMatMinN<MatT,3,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix with minimum dimensions 4x4 */
template< class MatT > inline void
CheckMatMin4x4(const MatT& m) {
CheckMatMinN<MatT,4,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix that can represent a 3D linear transform */
template< class MatT > inline void
CheckMatLinear3D(const MatT& m) {
CheckMatMin3x3(m);
}
/** Check for a matrix that can represent a 2D linear transform */
template< class MatT > inline void
CheckMatLinear2D(const MatT& m) {
CheckMatMin2x2(m);
}
/** Check for a matrix that can represent a 3D row-basis affine transform */
template< class MatT > inline void
CheckMatAffine3D(const MatT& m, row_basis) {
CheckMatMinNxM<MatT,4,3,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix that can represent a 3D col-basis affine transform */
template< class MatT > inline void
CheckMatAffine3D(const MatT& m, col_basis) {
CheckMatMinNxM<MatT,3,4,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix that can represent a 2D row-basis affine transform */
template< class MatT > inline void
CheckMatAffine2D(const MatT& m, row_basis) {
CheckMatMinNxM<MatT,3,2,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix that can represent a 2D col-basis affine transform */
template< class MatT > inline void
CheckMatAffine2D(const MatT& m, col_basis) {
CheckMatMinNxM<MatT,2,3,matrix_arg_fails_minimum_size_requirement>(m);
}
/** Check for a matrix that can represent a 3D affine transform */
template< class MatT > inline void
CheckMatAffine3D(const MatT& m) {
CheckMatAffine3D(m, typename MatT::basis_orient());
}
/** Check for a matrix that can represent a 2D affine transform */
template< class MatT > inline void
CheckMatAffine2D(const MatT& m) {
CheckMatAffine2D(m, typename MatT::basis_orient());
}
/** Check for a matrix that can represent a 3D homogenous transform */
template< class MatT > inline void
CheckMatHomogeneous3D(const MatT& m) {
CheckMatMin4x4(m);
}
/** Compile-time check for a square matrix */
template< class MatT, class ErrorT> inline void
CheckMatSquare(const MatT& m, fixed_size_tag) {
CheckMat(m);
CML_STATIC_REQUIRE_M(
(MatT::array_rows == MatT::array_cols), ErrorT);
}
/** Run-time check for a square matrix */
template< class MatT, class /*ErrorT*/ > inline void
CheckMatSquare(const MatT& m, dynamic_size_tag) {
CheckMat(m);
if (m.rows() != m.cols()) {
throw std::invalid_argument(
"function expects square matrix as argument");
}
}
/** Check for a square matrix */
template< class MatT > inline void
CheckMatSquare(const MatT& m) {
typedef et::ExprTraits<MatT> matrix_traits;
typedef typename matrix_traits::size_tag size_tag;
detail::CheckMatSquare<
MatT,function_expects_square_matrix_arg_error>(m, size_tag());
}
//////////////////////////////////////////////////////////////////////////////
// Quaternion argument checking
//////////////////////////////////////////////////////////////////////////////
/** Compile-time check for a quaternion argument*/
template< class QuatT > inline void
CheckQuat(const QuatT& /*q*/)
{
typedef et::ExprTraits<QuatT> quaternion_traits;
typedef typename quaternion_traits::result_tag result_type;
CML_STATIC_REQUIRE_M(
(same_type<result_type, et::quaternion_result_tag>::is_true),
function_expects_quaternion_arg_error);
}
//////////////////////////////////////////////////////////////////////////////
// Index argument checking
//////////////////////////////////////////////////////////////////////////////
/** Run-time check for a valid argument */
inline void CheckValidArg(bool valid)
{
if (!valid) {
throw std::invalid_argument("invalid function argument");
}
}
/** Check for a valid integer index with value < N */
template < size_t N >
inline void CheckIndexN(size_t index) {
CheckValidArg(index < N);
}
/** Check for a valid integer index with value < 2 */
inline void CheckIndex2(size_t index) {
CheckIndexN<2>(index);
}
/** Check for a valid integer index with value < 3 */
inline void CheckIndex3(size_t index) {
CheckIndexN<3>(index);
}
} // namespace detail
} // namespace cml
#endif

162
Lib/Include/CML/mathlib/coord_conversion.h Normal file
View File

@@ -0,0 +1,162 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef coord_conversion_h
#define coord_conversion_h
#include <cml/mathlib/checking.h>
#include <cml/mathlib/epsilon.h>
#include <cml/mathlib/helper.h>
/* Functions for converting between Cartesian, polar, cylindrical and
* spherical coordinates.
*
* The 3D conversion functions take an integer axis index argument. For
* cylindrical coordinates this determines the axis of the cylinder, and for
* spherical it determines which cardinal axis is normal to the azimuth plane.
*
* For spherical coordinates the option of whether to treat phi as latitude
* or colatitude is also available. The 'type' argument takes either of the
* enumerants cml::latitude and cml::colatitude to reflect this.
*/
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// Conversion to Cartesian coordinates
//////////////////////////////////////////////////////////////////////////////
/* Convert cylindrical coordinates to Cartesian coordinates in R3 */
template < typename E, class A > void
cylindrical_to_cartesian(
E radius, E theta, E height, size_t axis, vector<E,A>& v)
{
typedef vector<E,A> vector_type;
typedef typename vector_type::value_type value_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
v[i] = height;
v[j] = std::cos(theta) * radius;
v[k] = std::sin(theta) * radius;
}
/* Convert spherical coordinates to Cartesian coordinates in R3 */
template < typename E, class A > void
spherical_to_cartesian(E radius, E theta, E phi, size_t axis,
SphericalType type, vector<E,A>& v)
{
typedef vector<E,A> vector_type;
typedef typename vector_type::value_type value_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckIndex3(axis);
if (type == latitude) {
phi = constants<value_type>::pi_over_2() - phi;
}
value_type sin_phi = std::sin(phi);
value_type cos_phi = std::cos(phi);
value_type sin_phi_r = sin_phi * radius;
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
v[i] = cos_phi * radius;
v[j] = sin_phi_r * std::cos(theta);
v[k] = sin_phi_r * std::sin(theta);
}
/* Convert polar coordinates to Cartesian coordinates in R2 */
template < typename E, class A > void
polar_to_cartesian(E radius, E theta, vector<E,A>& v)
{
/* Checking handled by set() */
v.set(std::cos(theta) * radius, std::sin(theta) * radius);
}
//////////////////////////////////////////////////////////////////////////////
// Conversion from Cartesian coordinates
//////////////////////////////////////////////////////////////////////////////
/* Convert Cartesian coordinates to cylindrical coordinates in R3 */
template < class VecT, typename Real > void
cartesian_to_cylindrical(const VecT& v, Real& radius, Real& theta,
Real& height, size_t axis, Real tolerance = epsilon<Real>::placeholder())
{
typedef Real value_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
radius = length(v[j],v[k]);
theta = radius < tolerance ? value_type(0) : std::atan2(v[k],v[j]);
height = v[i];
}
/* Convert Cartesian coordinates to spherical coordinates in R3 */
template < class VecT, typename Real > void
cartesian_to_spherical(const VecT& v, Real& radius, Real& theta, Real& phi,
size_t axis, SphericalType type,
Real tolerance = epsilon<Real>::placeholder())
{
typedef Real value_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
value_type len = length(v[j],v[k]);
theta = len < tolerance ? value_type(0) : std::atan2(v[k],v[j]);
radius = length(v[i], len);
if (radius < tolerance) {
phi = value_type(0);
} else {
phi = std::atan2(len,v[i]);
//phi = type.convert(phi);
if (type == latitude) {
phi = constants<value_type>::pi_over_2() - phi;
}
}
}
/* Convert Cartesian coordinates to polar coordinates in R2 */
template < class VecT, typename Real > void
cartesian_to_polar(const VecT& v, Real& radius, Real& theta,
Real tolerance = epsilon<Real>::placeholder())
{
typedef Real value_type;
/* Checking */
detail::CheckVec2(v);
radius = v.length();
theta = radius < tolerance ? value_type(0) : std::atan2(v[1],v[0]);
}
} // namespace cml
#endif

44
Lib/Include/CML/mathlib/epsilon.h Normal file
View File

@@ -0,0 +1,44 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef epsilon_h
#define epsilon_h
namespace cml {
/* @todo: epsilon and tolerance handling.
*
* @note This is a placeholder for a more sophisticated epsilon/tolerance
* system.
*/
template < typename Real >
struct epsilon
{
typedef Real value_type;
private:
/** For convenience */
typedef value_type T;
public:
static T placeholder() {
/* Completely arbitrary placeholder value: */
return T(0.0001);
}
};
} // namespace cml
#endif

258
Lib/Include/CML/mathlib/frustum.h Normal file
View File

@@ -0,0 +1,258 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef frustum_h
#define frustum_h
#include <cml/mathlib/matrix_concat.h>
#include <cml/mathlib/checking.h>
namespace cml {
/* @todo: plane class, and perhaps named arguments instead of an array. */
/* Extract the planes of a frustum given a modelview matrix and a projection
* matrix with the given near z-clipping range. The planes are normalized by
* default, but this can be turned off with the 'normalize' argument.
*
* The planes are in ax+by+cz+d = 0 form, and are in the order:
* left
* right
* bottom
* top
* near
* far
*/
template < class MatT, typename Real > void
extract_frustum_planes(
const MatT& modelview,
const MatT& projection,
Real planes[6][4],
ZClip z_clip,
bool normalize = true)
{
extract_frustum_planes(
detail::matrix_concat_transforms_4x4(modelview,projection),
planes,
z_clip,
normalize
);
}
/* Extract the planes of a frustum from a single matrix assumed to contain any
* model and view transforms followed by a projection transform with the given
* near z-cliping range. The planes are normalized by default, but this can be
* turned off with the 'normalize' argument.
*
* The planes are in ax+by+cz+d = 0 form, and are in the order:
* left
* right
* bottom
* top
* near
* far
*/
template < class MatT, typename Real > void
extract_frustum_planes(
const MatT& m,
Real planes[6][4],
ZClip z_clip,
bool normalize = true)
{
detail::CheckMatHomogeneous3D(m);
/* Left: [03+00, 13+10, 23+20, 33+30] */
planes[0][0] = m.basis_element(0,3) + m.basis_element(0,0);
planes[0][1] = m.basis_element(1,3) + m.basis_element(1,0);
planes[0][2] = m.basis_element(2,3) + m.basis_element(2,0);
planes[0][3] = m.basis_element(3,3) + m.basis_element(3,0);
/* Right: [03-00, 13-10, 23-20, 33-30] */
planes[1][0] = m.basis_element(0,3) - m.basis_element(0,0);
planes[1][1] = m.basis_element(1,3) - m.basis_element(1,0);
planes[1][2] = m.basis_element(2,3) - m.basis_element(2,0);
planes[1][3] = m.basis_element(3,3) - m.basis_element(3,0);
/* Bottom: [03+01, 13+11, 23+21, 33+31] */
planes[2][0] = m.basis_element(0,3) + m.basis_element(0,1);
planes[2][1] = m.basis_element(1,3) + m.basis_element(1,1);
planes[2][2] = m.basis_element(2,3) + m.basis_element(2,1);
planes[2][3] = m.basis_element(3,3) + m.basis_element(3,1);
/* Top: [03-01, 13-11, 23-21, 33-31] */
planes[3][0] = m.basis_element(0,3) - m.basis_element(0,1);
planes[3][1] = m.basis_element(1,3) - m.basis_element(1,1);
planes[3][2] = m.basis_element(2,3) - m.basis_element(2,1);
planes[3][3] = m.basis_element(3,3) - m.basis_element(3,1);
/* Far: [03-02, 13-12, 23-22, 33-32] */
planes[5][0] = m.basis_element(0,3) - m.basis_element(0,2);
planes[5][1] = m.basis_element(1,3) - m.basis_element(1,2);
planes[5][2] = m.basis_element(2,3) - m.basis_element(2,2);
planes[5][3] = m.basis_element(3,3) - m.basis_element(3,2);
/* Near: [03+02, 13+12, 23+22, 33+32] : [02, 12, 22, 32] */
extract_near_frustum_plane(m, planes[4], z_clip);
/* @todo: This will be handled by the plane class */
if (normalize) {
for (size_t i = 0; i < 6; ++i) {
Real invl = inv_sqrt(planes[i][0] * planes[i][0] +
planes[i][1] * planes[i][1] +
planes[i][2] * planes[i][2]);
planes[i][0] *= invl;
planes[i][1] *= invl;
planes[i][2] *= invl;
planes[i][3] *= invl;
}
}
}
/** Extract the near plane of a frustum given a concatenated modelview and
* projection matrix with the given near z-clipping range. The plane is
* not normalized.
*
* @note The plane is in ax+by+cz+d = 0 form.
*
* @warning The matrix is assumed to be a homogeneous transformation
* matrix.
*/
template < class MatT, class PlaneT > void
extract_near_frustum_plane(
const MatT& m,
PlaneT& plane,
ZClip z_clip
)
{
/* Near: [03+02, 13+12, 23+22, 33+32] : [02, 12, 22, 32] */
if (z_clip == z_clip_neg_one) {
plane[0] = m.basis_element(0,3) + m.basis_element(0,2);
plane[1] = m.basis_element(1,3) + m.basis_element(1,2);
plane[2] = m.basis_element(2,3) + m.basis_element(2,2);
plane[3] = m.basis_element(3,3) + m.basis_element(3,2);
} else { // z_clip == z_clip_zero
plane[0] = m.basis_element(0,2);
plane[1] = m.basis_element(1,2);
plane[2] = m.basis_element(2,2);
plane[3] = m.basis_element(3,2);
}
}
namespace detail {
/* This is currently only in support of finding the corners of a frustum.
* The input planes are assumed to have a single unique intersection, so
* no tolerance is used.
*/
template < typename Real > vector< Real, fixed<3> >
intersect_planes(Real p1[4], Real p2[4], Real p3[4])
{
typedef vector< Real, fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
vector_type n1(p1[0],p1[1],p1[2]);
vector_type n2(p2[0],p2[1],p2[2]);
vector_type n3(p3[0],p3[1],p3[2]);
value_type d1 = -p1[3];
value_type d2 = -p2[3];
value_type d3 = -p3[3];
vector_type numer =
d1*cross(n2,n3) + d2*cross(n3,n1) + d3*cross(n1,n2);
value_type denom = triple_product(n1,n2,n3);
return numer/denom;
}
} // namespace detail
/* Get the corners of a frustum defined by 6 planes. The planes are in
* ax+by+cz+d = 0 form, and are in the order:
* left
* right
* bottom
* top
* near
* far
*
* The corners are in CCW order starting in the lower-left, first at the near
* plane, then at the far plane.
*/
template < typename Real, typename E, class A > void
get_frustum_corners(Real planes[6][4], vector<E,A> corners[8])
{
// NOTE: Prefixed with 'PLANE_' due to symbol conflict with Windows
// macros PLANE_LEFT and PLANE_RIGHT.
enum {
PLANE_LEFT,
PLANE_RIGHT,
PLANE_BOTTOM,
PLANE_TOP,
PLANE_NEAR,
PLANE_FAR
};
corners[0] = detail::intersect_planes(
planes[PLANE_LEFT],
planes[PLANE_BOTTOM],
planes[PLANE_NEAR]
);
corners[1] = detail::intersect_planes(
planes[PLANE_RIGHT],
planes[PLANE_BOTTOM],
planes[PLANE_NEAR]
);
corners[2] = detail::intersect_planes(
planes[PLANE_RIGHT],
planes[PLANE_TOP],
planes[PLANE_NEAR]
);
corners[3] = detail::intersect_planes(
planes[PLANE_LEFT],
planes[PLANE_TOP],
planes[PLANE_NEAR]
);
corners[4] = detail::intersect_planes(
planes[PLANE_LEFT],
planes[PLANE_BOTTOM],
planes[PLANE_FAR]
);
corners[5] = detail::intersect_planes(
planes[PLANE_RIGHT],
planes[PLANE_BOTTOM],
planes[PLANE_FAR]
);
corners[6] = detail::intersect_planes(
planes[PLANE_RIGHT],
planes[PLANE_TOP],
planes[PLANE_FAR]
);
corners[7] = detail::intersect_planes(
planes[PLANE_LEFT],
planes[PLANE_TOP],
planes[PLANE_FAR]
);
}
} // namespace cml
#endif

158
Lib/Include/CML/mathlib/helper.h Normal file
View File

@@ -0,0 +1,158 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef helper_h
#define helper_h
#include <cstddef>
#include <cml/constants.h>
namespace cml {
/* Helper classes for axis order, coordinate system handedness, z-clipping
* range and spherical coordinate type.
*/
//////////////////////////////////////////////////////////////////////////////
// Euler order
//////////////////////////////////////////////////////////////////////////////
enum EulerOrder {
euler_order_xyz, // 0x00 [0000]
euler_order_xyx, // 0x01 [0001]
euler_order_xzy, // 0x02 [0010]
euler_order_xzx, // 0x03 [0011]
euler_order_yzx, // 0x04 [0100]
euler_order_yzy, // 0x05 [0101]
euler_order_yxz, // 0x06 [0110]
euler_order_yxy, // 0x07 [0111]
euler_order_zxy, // 0x08 [1000]
euler_order_zxz, // 0x09 [1001]
euler_order_zyx, // 0x0A [1010]
euler_order_zyz // 0x0B [1011]
};
namespace detail {
inline void unpack_euler_order(
EulerOrder order,
size_t& i,
size_t& j,
size_t& k,
bool& odd,
bool& repeat)
{
enum { REPEAT = 0x01, ODD = 0x02, AXIS = 0x0C };
repeat = order & REPEAT;
odd = ((order & ODD) == ODD);
size_t offset = size_t(odd);
i = (order & AXIS) % 3;
j = (i + 1 + offset) % 3;
k = (i + 2 - offset) % 3;
}
} // namespace detail
//////////////////////////////////////////////////////////////////////////////
// Axis order
//////////////////////////////////////////////////////////////////////////////
enum AxisOrder {
axis_order_xyz = euler_order_xyz, // 0x00 [0000]
axis_order_xzy = euler_order_xzy, // 0x02 [0010]
axis_order_yzx = euler_order_yzx, // 0x04 [0100]
axis_order_yxz = euler_order_yxz, // 0x06 [0110]
axis_order_zxy = euler_order_zxy, // 0x08 [1000]
axis_order_zyx = euler_order_zyx // 0x0A [1010]
};
namespace detail {
inline void unpack_axis_order(
AxisOrder order,
size_t& i,
size_t& j,
size_t& k,
bool& odd)
{
enum { ODD = 0x02, AXIS = 0x0C };
odd = ((order & ODD) == ODD);
size_t offset = size_t(odd);
i = (order & AXIS) % 3;
j = (i + 1 + offset) % 3;
k = (i + 2 - offset) % 3;
}
inline AxisOrder pack_axis_order(size_t i, bool odd) {
return AxisOrder((i << 2) | (size_t(odd) << 1));
}
inline AxisOrder swap_axis_order(AxisOrder order)
{
size_t i, j, k;
bool odd;
unpack_axis_order(order, i, j, k, odd);
return pack_axis_order(j, !odd);
}
} // namespace detail
//////////////////////////////////////////////////////////////////////////////
// Axis order 2D
//////////////////////////////////////////////////////////////////////////////
enum AxisOrder2D {
axis_order_xy = axis_order_xyz, // 0x00 [0000]
axis_order_yx = axis_order_yxz // 0x06 [0110]
};
namespace detail {
inline void unpack_axis_order_2D(
AxisOrder2D order,
size_t& i,
size_t& j,
bool& odd)
{
enum { ODD = 0x02, AXIS = 0x0C };
odd = ((order & ODD) == ODD);
size_t offset = size_t(odd);
i = (order & AXIS) % 3;
j = (i + 1 + offset) % 3;
}
} // namespace detail
//////////////////////////////////////////////////////////////////////////////
// Handedness
//////////////////////////////////////////////////////////////////////////////
enum Handedness { left_handed, right_handed };
//////////////////////////////////////////////////////////////////////////////
// Z clip
//////////////////////////////////////////////////////////////////////////////
enum ZClip { z_clip_neg_one, z_clip_zero };
//////////////////////////////////////////////////////////////////////////////
// Spherical coordinate type
//////////////////////////////////////////////////////////////////////////////
enum SphericalType { latitude, colatitude };
} // namespace cml
#endif

1129
Lib/Include/CML/mathlib/interpolation.h Normal file
View File

File diff suppressed because it is too large Load Diff

33
Lib/Include/CML/mathlib/mathlib.h Normal file
View File

@@ -0,0 +1,33 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef mathlib_h
#define mathlib_h
#include <cml/mathlib/typedef.h>
#include <cml/mathlib/epsilon.h>
#include <cml/mathlib/vector_angle.h>
#include <cml/mathlib/vector_ortho.h>
#include <cml/mathlib/vector_transform.h>
#include <cml/mathlib/matrix_ortho.h>
#include <cml/mathlib/matrix_rotation.h>
#include <cml/mathlib/matrix_transform.h>
#include <cml/mathlib/matrix_projection.h>
#include <cml/mathlib/quaternion_basis.h>
#include <cml/mathlib/quaternion_rotation.h>
#include <cml/mathlib/coord_conversion.h>
#include <cml/mathlib/interpolation.h>
#include <cml/mathlib/frustum.h>
#include <cml/mathlib/projection.h>
#include <cml/mathlib/picking.h>
#endif

364
Lib/Include/CML/mathlib/matrix_basis.h Normal file
View File

@@ -0,0 +1,364 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_basis_h
#define matrix_basis_h
#include <cml/mathlib/checking.h>
/* This file contains functions for setting and retrieving the basis vectors
* or transposed basis vectors of a matrix representing a 3D or 2D transform,
* either by index (0,1,2) or name (x,y,z).
*
* In addition to being a convenience for the user, the functions are also
* in support of other matrix functions which are best implemented in vector
* form (such as orthogonalization and construction of orthonormal bases).
*
* Note that matrix expression arguments are allowed to have dimensions larger
* than the minimum requirement. For example, matrix_get_basis_vector() can be
* called on any NxM matrix with N,M >= 3.
*
* As with other matrix functions, the following template argument notation is
* used for conciseness:
*
* E = vector or matrix element type
* A = vector or matrix array storage type
* B = matrix basis orientation type
* L = matrix layout type
*/
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// Functions for setting the basis vectors of a 3D or 2D transform matrix
//////////////////////////////////////////////////////////////////////////////
/** Set the i'th basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_basis_vector(matrix<E,A,B,L>& m, size_t i, const VecT& v)
{
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckVec3(v);
detail::CheckIndex3(i);
m.set_basis_element(i,0,v[0]);
m.set_basis_element(i,1,v[1]);
m.set_basis_element(i,2,v[2]);
}
/** Set the i'th transposed basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_basis_vector(matrix<E,A,B,L>& m,size_t i,const VecT& v)
{
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckVec3(v);
detail::CheckIndex3(i);
m.set_basis_element(0,i,v[0]);
m.set_basis_element(1,i,v[1]);
m.set_basis_element(2,i,v[2]);
}
/** Set the i'th basis vector of a 2D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_basis_vector_2D(matrix<E,A,B,L>& m, size_t i, const VecT& v)
{
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckVec2(v);
detail::CheckIndex2(i);
m.set_basis_element(i,0,v[0]);
m.set_basis_element(i,1,v[1]);
}
/** Set the i'th transposed basis vector of a 2D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_basis_vector_2D(
matrix<E,A,B,L>& m, size_t i, const VecT& v)
{
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckVec2(v);
detail::CheckIndex2(i);
m.set_basis_element(0,i,v[0]);
m.set_basis_element(1,i,v[1]);
}
/** Set the x basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_x_basis_vector(matrix<E,A,B,L>& m, const VecT& x) {
matrix_set_basis_vector(m,0,x);
}
/** Set the y basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_y_basis_vector(matrix<E,A,B,L>& m, const VecT& y) {
matrix_set_basis_vector(m,1,y);
}
/** Set the z basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_z_basis_vector(matrix<E,A,B,L>& m, const VecT& z) {
matrix_set_basis_vector(m,2,z);
}
/** Set the transposed x basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_x_basis_vector(matrix<E,A,B,L>& m, const VecT& x) {
matrix_set_transposed_basis_vector(m,0,x);
}
/** Set the transposed y basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_y_basis_vector(matrix<E,A,B,L>& m, const VecT& y) {
matrix_set_transposed_basis_vector(m,1,y);
}
/** Set the transposed z basis vector of a 3D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_z_basis_vector(matrix<E,A,B,L>& m, const VecT& z) {
matrix_set_transposed_basis_vector(m,2,z);
}
/** Set the x basis vector of a 2D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_x_basis_vector_2D(matrix<E,A,B,L>& m, const VecT& x) {
matrix_set_basis_vector_2D(m,0,x);
}
/** Set the y basis vector of a 2D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_y_basis_vector_2D(matrix<E,A,B,L>& m, const VecT& y) {
matrix_set_basis_vector_2D(m,1,y);
}
/** Set the transposed x basis vector of a 2D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_x_basis_vector_2D(matrix<E,A,B,L>& m,const VecT& x) {
matrix_set_transposed_basis_vector_2D(m,0,x);
}
/** Set the transposed y basis vector of a 2D transform */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_transposed_y_basis_vector_2D(matrix<E,A,B,L>& m,const VecT& y) {
matrix_set_transposed_basis_vector_2D(m,1,y);
}
/** Set the basis vectors of a 3D transform */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_set_basis_vectors(
matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y, const VecT_3& z)
{
matrix_set_x_basis_vector(m,x);
matrix_set_y_basis_vector(m,y);
matrix_set_z_basis_vector(m,z);
}
/** Set the transposed basis vectors of a 3D transform */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_set_transposed_basis_vectors(
matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y, const VecT_3& z)
{
matrix_set_transposed_x_basis_vector(m,x);
matrix_set_transposed_y_basis_vector(m,y);
matrix_set_transposed_z_basis_vector(m,z);
}
/** Set the basis vectors of a 2D transform */
template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
matrix_set_basis_vectors_2D(
matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y)
{
matrix_set_x_basis_vector_2D(m,x);
matrix_set_y_basis_vector_2D(m,y);
}
/** Set the transposed basis vectors of a 2D transform */
template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
matrix_set_transposed_basis_vectors_2D(
matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y)
{
matrix_set_transposed_x_basis_vector_2D(m,x);
matrix_set_transposed_y_basis_vector_2D(m,y);
}
//////////////////////////////////////////////////////////////////////////////
// Functions for getting the basis vectors of a 3D or 2D transform matrix
//////////////////////////////////////////////////////////////////////////////
#define TEMP_VEC3 vector< typename MatT::value_type, fixed<3> >
#define TEMP_VEC2 vector< typename MatT::value_type, fixed<2> >
/** Get the i'th basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_basis_vector(const MatT& m, size_t i)
{
typedef TEMP_VEC3 vector_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(i);
return vector_type(
m.basis_element(i,0), m.basis_element(i,1), m.basis_element(i,2));
}
/** Get the i'th transposed basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_transposed_basis_vector(const MatT& m, size_t i)
{
typedef typename MatT::value_type value_type;
typedef vector< value_type, fixed<3> > vector_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(i);
return vector_type(
m.basis_element(0,i), m.basis_element(1,i), m.basis_element(2,i));
}
/** Get the i'th basis vector of a 2D transform */
template < class MatT > TEMP_VEC2
matrix_get_basis_vector_2D(const MatT& m, size_t i)
{
typedef TEMP_VEC2 vector_type;
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckIndex2(i);
return vector_type(m.basis_element(i,0), m.basis_element(i,1));
}
/** Get the i'th transposed basis vector of a 2D transform */
template < class MatT > TEMP_VEC2
matrix_get_transposed_basis_vector_2D(const MatT& m, size_t i)
{
typedef TEMP_VEC2 vector_type;
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckIndex2(i);
return vector_type(m.basis_element(0,i), m.basis_element(1,i));
}
/** Get the x basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_x_basis_vector(const MatT& m) {
return matrix_get_basis_vector(m,0);
}
/** Get the y basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_y_basis_vector(const MatT& m) {
return matrix_get_basis_vector(m,1);
}
/** Get the z basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_z_basis_vector(const MatT& m) {
return matrix_get_basis_vector(m,2);
}
/** Get the transposed x basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_transposed_x_basis_vector(const MatT& m) {
return matrix_get_transposed_basis_vector(m,0);
}
/** Get the transposed y basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_transposed_y_basis_vector(const MatT& m) {
return matrix_get_transposed_basis_vector(m,1);
}
/** Get the transposed z basis vector of a 3D transform */
template < class MatT > TEMP_VEC3
matrix_get_transposed_z_basis_vector(const MatT& m) {
return matrix_get_transposed_basis_vector(m,2);
}
/** Get the x basis vector of a 2D transform */
template < class MatT > TEMP_VEC2
matrix_get_x_basis_vector_2D(const MatT& m) {
return matrix_get_basis_vector_2D(m,0);
}
/** Get the y basis vector of a 2D transform */
template < class MatT > TEMP_VEC2
matrix_get_y_basis_vector_2D(const MatT& m) {
return matrix_get_basis_vector_2D(m,1);
}
/** Get the transposed x basis vector of a 2D transform */
template < class MatT > TEMP_VEC2
matrix_get_transposed_x_basis_vector_2D(const MatT& m) {
return matrix_get_transposed_basis_vector_2D(m,0);
}
/** Get the transposed y basis vector of a 2D transform */
template < class MatT > TEMP_VEC2
matrix_get_transposed_y_basis_vector_2D(const MatT& m) {
return matrix_get_transposed_basis_vector_2D(m,1);
}
/** Get the basis vectors of a 3D transform */
template < class MatT, class E, class A > void
matrix_get_basis_vectors(
const MatT& m, vector<E,A>& x, vector<E,A>& y, vector<E,A>& z)
{
x = matrix_get_x_basis_vector(m);
y = matrix_get_y_basis_vector(m);
z = matrix_get_z_basis_vector(m);
}
/** Get the transposed basis vectors of a 3D transform */
template < class MatT, typename E, class A > void
matrix_get_transposed_basis_vectors(
const MatT& m, vector<E,A>& x, vector<E,A>& y, vector<E,A>& z)
{
x = matrix_get_transposed_x_basis_vector(m);
y = matrix_get_transposed_y_basis_vector(m);
z = matrix_get_transposed_z_basis_vector(m);
}
/** Get the basis vectors of a 2D transform */
template < class MatT, typename E, class A > void
matrix_get_basis_vectors_2D(const MatT& m,vector<E,A>& x,vector<E,A>& y)
{
x = matrix_get_x_basis_vector_2D(m);
y = matrix_get_y_basis_vector_2D(m);
}
/** Get the transposed basis vectors of a 2D transform */
template < class MatT, typename E, class A > void
matrix_get_transposed_basis_vectors_2D(
const MatT& m, vector<E,A>& x, vector<E,A>& y)
{
x = matrix_get_transposed_x_basis_vector_2D(m);
y = matrix_get_transposed_y_basis_vector_2D(m);
}
#undef TEMP_VEC3
#undef TEMP_VEC2
} // namespace cml
#endif

62
Lib/Include/CML/mathlib/matrix_concat.h Normal file
View File

@@ -0,0 +1,62 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_concat_h
#define matrix_concat_h
#include <cml/matrix/matrix_expr.h>
/* This will all most likely be abstracted away in a future version of the
* CML. For now, this file provides support for functions that need to
* concatenate transformation matrices in a basis-independent manner.
*
* @todo: The 2x2 and 3x3 versions of these functions are currently in
* matrix_rotation.h. They should be moved here.
*/
namespace cml {
namespace detail {
/** A fixed-size temporary 4x4 matrix */
#define MAT_TEMP_4X4 matrix< \
typename et::ScalarPromote< \
typename MatT_1::value_type, \
typename MatT_2::value_type \
>::type, \
fixed<4,4>, \
typename MatT_1::basis_orient, \
typename MatT_1::layout \
>
template < class MatT_1, class MatT_2 > MAT_TEMP_4X4
matrix_concat_transforms_4x4(const MatT_1& m1, const MatT_2& m2, row_basis) {
return m1*m2;
}
/** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_4X4
matrix_concat_transforms_4x4(const MatT_1& m1, const MatT_2& m2, col_basis) {
return m2*m1;
}
/** Concatenate two 3D rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_4X4
matrix_concat_transforms_4x4(const MatT_1& m1, const MatT_2& m2) {
return matrix_concat_transforms_4x4(m1,m2,typename MatT_1::basis_orient());
}
#undef MAT_TEMP_4x4
} // namespace detail
} // namespace cml
#endif

135
Lib/Include/CML/mathlib/matrix_misc.h Normal file
View File

@@ -0,0 +1,135 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_misc_h
#define matrix_misc_h
#include <cml/mathlib/checking.h>
/* Miscellaneous matrix functions. */
namespace cml {
/** Set a (possibly non-square) matrix to represent an identity transform */
template < typename E, class A, class B, class L > void
identity_transform(matrix<E,A,B,L>& m)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
for (size_t i = 0; i < m.rows(); ++i) {
for (size_t j = 0; j < m.cols(); ++j) {
m(i,j) = value_type((i == j) ? 1 : 0);
}
}
}
/** Trace of a square matrix */
template < class MatT > typename MatT::value_type
trace(const MatT& m)
{
typedef typename MatT::value_type value_type;
/* Checking */
detail::CheckMatSquare(m);
value_type t = value_type(0);
for (size_t i = 0; i < m.rows(); ++i) {
t += m(i,i);
}
return t;
}
/** Trace of the upper-left 3x3 part of a matrix */
template < class MatT > typename MatT::value_type
trace_3x3(const MatT& m)
{
/* Checking */
detail::CheckMatMin3x3(m);
return m(0,0) + m(1,1) + m(2,2);
}
/** Trace of the upper-left 2x2 part of a matrix */
template < class MatT > typename MatT::value_type
trace_2x2(const MatT& m)
{
/* Checking */
detail::CheckMatMin2x2(m);
return m(0,0) + m(1,1);
}
/** 3D skew-symmetric matrix */
template < typename E, class A, class B, class L, class VecT > void
matrix_skew_symmetric(matrix<E,A,B,L>& m, const VecT& v)
{
/* Checking */
detail::CheckMatMin3x3(m);
detail::CheckVec3(v);
m.zero();
m.set_basis_element(1,2, v[0]);
m.set_basis_element(2,1,-v[0]);
m.set_basis_element(2,0, v[1]);
m.set_basis_element(0,2,-v[1]);
m.set_basis_element(0,1, v[2]);
m.set_basis_element(1,0,-v[2]);
}
/** 2D skew-symmetric matrix */
template < typename E, class A, class B, class L > void
matrix_skew_symmetric_2D(matrix<E,A,B,L>& m, E s)
{
/* Checking */
detail::CheckMatMin2x2(m);
m.zero();
m.set_basis_element(0,1, s);
m.set_basis_element(1,0,-s);
}
/* @todo: Clean this up, and implement SRT as well */
/** Invert a matrix consisting of a 3D rotation and translation */
template < typename E, class A, class B, class L > void
matrix_invert_RT_only(matrix<E,A,B,L>& m)
{
typedef vector< E, fixed<3> > vector_type;
vector_type x, y, z;
matrix_get_basis_vectors(m,x,y,z);
matrix_set_transposed_basis_vectors(m,x,y,z);
vector_type p = matrix_get_translation(m);
matrix_set_translation(m,-dot(p,x),-dot(p,y),-dot(p,z));
}
/** Invert a matrix consisting of a 2D rotation and ranslation */
template < typename E, class A, class B, class L > void
matrix_invert_RT_only_2D(matrix<E,A,B,L>& m)
{
typedef vector< E, fixed<2> > vector_type;
vector_type x, y;
matrix_get_basis_vectors_2D(m,x,y);
matrix_set_transposed_basis_vectors_2D(m,x,y);
vector_type p = matrix_get_translation_2D(m);
matrix_set_translation_2D(m,-dot(p,x),-dot(p,y));
}
} // namespace cml
#endif

60
Lib/Include/CML/mathlib/matrix_ortho.h Normal file
View File

@@ -0,0 +1,60 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_ortho_h
#define matrix_ortho_h
#include <cml/mathlib/vector_ortho.h>
/* Functions for orthogonalizing a matrix.
*
* matrix_orthogonalize_3x3() and _2x2() operate on the upper-left-hand part
* of any matrix of suitable size; this is to allow orthonormalization of the
* rotation part of an affine transform matrix.
*
* Note: These functions pass off to the orthonormalization functions in
* vector_ortho.h, so see that file for details on the optional parameters.
*
* @todo: General NxN matrix orthogonalization.
*/
namespace cml {
/** Orthogonalize the upper-left 3x3 portion of a matrix */
template < typename E, class A, class B, class L > void
matrix_orthogonalize_3x3(matrix<E,A,B,L>& m, size_t stable_axis = 2,
size_t num_iter = 0, E s = E(1))
{
typedef vector< E, fixed<3> > vector_type;
vector_type x, y, z;
matrix_get_basis_vectors(m,x,y,z);
orthonormalize(x,y,z,stable_axis,num_iter,s);
matrix_set_basis_vectors(m,x,y,z);
}
/** Orthogonalize the upper-left 2x2 portion of a matrix */
template < typename E, class A, class B, class L > void
matrix_orthogonalize_2x2(matrix<E,A,B,L>& m, size_t stable_axis = 0,
size_t num_iter = 0, E s = E(1))
{
typedef vector< E, fixed<2> > vector_type;
vector_type x, y;
matrix_get_basis_vectors_2D(m,x,y);
orthonormalize(x,y,stable_axis,num_iter,s);
matrix_set_basis_vectors_2D(m,x,y);
}
} // namespace cml
#endif

348
Lib/Include/CML/mathlib/matrix_projection.h Normal file
View File

@@ -0,0 +1,348 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_projection_h
#define matrix_projection_h
#include <cml/mathlib/checking.h>
#include <cml/mathlib/helper.h>
/* Functions for building matrix transforms other than rotations
* (matrix_rotation.h) and viewing projections (matrix_projection.h).
*
* @todo: Clean up comments and documentation throughout.
*/
// NOTE: Changed 'near' and 'far' to 'n' and 'f' throughout to work around
// windows.h 'near' and 'far' macros.
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// 3D perspective projection from frustum
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a perspective projection, specified by frustum
* bounds in l,r,b,t,n,f form, and with the given handedness and z clipping
* range
*/
template < typename E, class A, class B, class L > void
matrix_perspective(matrix<E,A,B,L>& m, E left, E right, E bottom, E top,
E n, E f, Handedness handedness,
ZClip z_clip)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatHomogeneous3D(m);
identity_transform(m);
value_type inv_width = value_type(1) / (right - left);
value_type inv_height = value_type(1) / (top - bottom);
value_type inv_depth = value_type(1) / (f - n);
value_type near2 = value_type(2) * n;
value_type s = handedness == left_handed
? value_type(1) : value_type(-1);
if (z_clip == z_clip_neg_one) {
m.set_basis_element(2,2,s * (f + n) * inv_depth);
m.set_basis_element(3,2,value_type(-2) * f * n * inv_depth);
} else { // z_clip == z_clip_zero
m.set_basis_element(2,2,s * f * inv_depth);
m.set_basis_element(3,2,-s * n * m.basis_element(2,2));
}
m.set_basis_element(0,0,near2 * inv_width );
m.set_basis_element(1,1,near2 * inv_height );
m.set_basis_element(2,0,-s * (right + left) * inv_width );
m.set_basis_element(2,1,-s * (top + bottom) * inv_height);
m.set_basis_element(2,3,s );
m.set_basis_element(3,3,value_type(0) );
}
/** Build a matrix representing a perspective projection, specified by frustum
* bounds in w,h,n,f form, and with the given handedness and z clipping
* range
*/
template < typename E, class A, class B, class L > void
matrix_perspective(matrix<E,A,B,L>& m, E width, E height, E n, E f,
Handedness handedness, ZClip z_clip)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
value_type half_width = width * value_type(.5);
value_type half_height = height * value_type(.5);
matrix_perspective(m, -half_width, half_width,
-half_height, half_height, n, f, handedness, z_clip);
}
/** Build a left-handedness frustum perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_LH(matrix<E,A,B,L>& m, E left, E right, E bottom,
E top, E n, E f, ZClip z_clip)
{
matrix_perspective(m, left, right, bottom, top, n, f,
left_handed, z_clip);
}
/** Build a right-handedness frustum perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_RH(matrix<E,A,B,L>& m, E left, E right, E bottom,
E top, E n, E f, ZClip z_clip)
{
matrix_perspective(m, left, right, bottom, top, n, f,
right_handed, z_clip);
}
/** Build a left-handedness frustum perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_LH(matrix<E,A,B,L>& m, E width, E height, E n,
E f, ZClip z_clip)
{
matrix_perspective(m, width, height, n, f, left_handed, z_clip);
}
/** Build a right-handedness frustum perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_RH(matrix<E,A,B,L>& m, E width, E height, E n,
E f, ZClip z_clip)
{
matrix_perspective(m, width, height, n, f, right_handed, z_clip);
}
//////////////////////////////////////////////////////////////////////////////
// 3D perspective projection from horizontal field of view
//////////////////////////////////////////////////////////////////////////////
/** Build a perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_xfov(matrix<E,A,B,L>& m, E xfov, E aspect, E n,
E f, Handedness handedness, ZClip z_clip)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
value_type width = value_type(2) * std::tan(xfov * value_type(.5)) * n;
matrix_perspective(m, width, width / aspect, n, f,
handedness, z_clip);
}
/** Build a left-handedness perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_xfov_LH(matrix<E,A,B,L>& m, E xfov, E aspect, E n,
E f, ZClip z_clip)
{
matrix_perspective_xfov(m,xfov,aspect,n,f,left_handed,z_clip);
}
/** Build a right-handedness perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_xfov_RH(matrix<E,A,B,L>& m, E xfov, E aspect, E n,
E f, ZClip z_clip)
{
matrix_perspective_xfov(m,xfov,aspect,n,f,right_handed,z_clip);
}
//////////////////////////////////////////////////////////////////////////////
// 3D perspective projection from vertical field of view
//////////////////////////////////////////////////////////////////////////////
/** Build a perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_yfov(matrix<E,A,B,L>& m, E yfov, E aspect, E n,
E f, Handedness handedness, ZClip z_clip)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
value_type height = value_type(2) * std::tan(yfov * value_type(.5)) * n;
matrix_perspective(m, height * aspect, height, n, f,
handedness, z_clip);
}
/** Build a left-handedness perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_yfov_LH(matrix<E,A,B,L>& m, E yfov, E aspect, E n,
E f, ZClip z_clip)
{
matrix_perspective_yfov(m,yfov,aspect,n,f,left_handed,z_clip);
}
/** Build a right-handedness perspective matrix */
template < typename E, class A, class B, class L > void
matrix_perspective_yfov_RH(matrix<E,A,B,L>& m, E yfov, E aspect, E n,
E f, ZClip z_clip)
{
matrix_perspective_yfov(m,yfov,aspect,n,f,right_handed,z_clip);
}
//////////////////////////////////////////////////////////////////////////////
// 3D orthographic projection from frustum
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing an orthographic projection, specified by
* frustum bounds in l,r,b,t,n,f form, and with the given handedness and z
* clipping range
*/
template < typename E, class A, class B, class L > void
matrix_orthographic(matrix<E,A,B,L>& m, E left, E right, E bottom, E top,
E n, E f, Handedness handedness,
ZClip z_clip)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatHomogeneous3D(m);
identity_transform(m);
value_type inv_width = value_type(1) / (right - left);
value_type inv_height = value_type(1) / (top - bottom);
value_type inv_depth = value_type(1) / (f - n);
value_type s = handedness == left_handed
? value_type(1) : value_type(-1);
if (z_clip == z_clip_neg_one) {
m.set_basis_element(2,2,s * value_type(2) * inv_depth);
m.set_basis_element(3,2,-(f + n) * inv_depth);
} else { // z_clip.z_clip() == 0
m.set_basis_element(2,2,s * inv_depth);
m.set_basis_element(3,2,-n * inv_depth);
}
m.set_basis_element(0,0,value_type(2) * inv_width );
m.set_basis_element(1,1,value_type(2) * inv_height );
m.set_basis_element(3,0,-(right + left) * inv_width );
m.set_basis_element(3,1,-(top + bottom) * inv_height);
}
/** Build an orthographic projection matrix */
template < typename E, class A, class B, class L > void
matrix_orthographic(matrix<E,A,B,L>& m, E width, E height, E n, E f,
Handedness handedness, ZClip z_clip)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
value_type half_width = width * value_type(.5);
value_type half_height = height * value_type(.5);
matrix_orthographic(m, -half_width, half_width,
-half_height, half_height, n, f, handedness, z_clip);
}
/** Build a left-handedness orthographic projection matrix */
template < typename E, class A, class B, class L > void
matrix_orthographic_LH(matrix<E,A,B,L>& m, E left, E right, E bottom,
E top, E n, E f, ZClip z_clip)
{
matrix_orthographic(m, left, right, bottom, top, n, f,
left_handed, z_clip);
}
/** Build a right-handedness orthographic projection matrix */
template < typename E, class A, class B, class L > void
matrix_orthographic_RH(matrix<E,A,B,L>& m, E left, E right, E bottom,
E top, E n, E f, ZClip z_clip)
{
matrix_orthographic(m, left, right, bottom, top, n, f,
right_handed, z_clip);
}
/** Build a left-handedness orthographic projection matrix */
template < typename E, class A, class B, class L > void
matrix_orthographic_LH(matrix<E,A,B,L>& m, E width, E height, E n,
E f, ZClip z_clip)
{
matrix_orthographic(m, width, height, n, f, left_handed,
z_clip);
}
/** Build a right-handedness orthographic projection matrix */
template < typename E, class A, class B, class L > void
matrix_orthographic_RH(matrix<E,A,B,L>& m, E width, E height, E n,
E f, ZClip z_clip)
{
matrix_orthographic(m, width, height, n, f, right_handed,
z_clip);
}
//////////////////////////////////////////////////////////////////////////////
// 3D viewport
//////////////////////////////////////////////////////////////////////////////
/* Build a viewport matrix
*
* Note: A viewport matrix is in a sense the opposite of an orthographics
* projection matrix, and can be build by constructing and inverting the
* latter.
*
* @todo: Need to look into D3D viewport conventions and see if this needs to
* be adapted accordingly.
*/
template < typename E, class A, class B, class L > void
matrix_viewport(matrix<E,A,B,L>& m, E left, E right, E bottom,
E top, ZClip z_clip, E n = E(0), E f = E(1))
{
matrix_orthographic_LH(m, left, right, bottom, top, n, f, z_clip);
/* @todo: invert(m), when available */
m = inverse(m);
}
//////////////////////////////////////////////////////////////////////////////
// 3D picking volume
//////////////////////////////////////////////////////////////////////////////
/* Build a pick volume matrix
*
* When post-concatenated with a projection matrix, the pick matrix modifies
* the view volume to create a 'picking volume'. This volume corresponds to
* a screen rectangle centered at (pick_x, pick_y) and with dimensions
* pick_widthXpick_height.
*
* @todo: Representation of viewport between this function and
* matrix_viewport() is inconsistent (position and dimensions vs. bounds).
* Should this be addressed?
*/
template < typename E, class A, class B, class L > void
matrix_pick(
matrix<E,A,B,L>& m, E pick_x, E pick_y, E pick_width, E pick_height,
E viewport_x, E viewport_y, E viewport_width, E viewport_height)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatHomogeneous3D(m);
identity_transform(m);
value_type inv_width = value_type(1) / pick_width;
value_type inv_height = value_type(1) / pick_height;
m.set_basis_element(0,0,viewport_width*inv_width);
m.set_basis_element(1,1,viewport_height*inv_height);
m.set_basis_element(3,0,
(viewport_width+value_type(2)*(viewport_x-pick_x))*inv_width);
m.set_basis_element(3,1,
(viewport_height+value_type(2)*(viewport_y-pick_y))*inv_height);
}
} // namespace cml
#endif

980
Lib/Include/CML/mathlib/matrix_rotation.h Normal file
View File

@@ -0,0 +1,980 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_rotation_h
#define matrix_rotation_h
#include <cml/mathlib/matrix_misc.h>
#include <cml/mathlib/vector_ortho.h>
/* Functions related to matrix rotations in 3D and 2D. */
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// 3D rotation about world axes
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D rotation about the given world axis */
template < typename E, class A, class B, class L > void
matrix_rotation_world_axis( matrix<E,A,B,L>& m, size_t axis, E angle)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
identity_transform(m);
m.set_basis_element(j,j, c);
m.set_basis_element(j,k, s);
m.set_basis_element(k,j,-s);
m.set_basis_element(k,k, c);
}
/** Build a matrix representing a 3D rotation about the world x axis */
template < typename E, class A, class B, class L > void
matrix_rotation_world_x(matrix<E,A,B,L>& m, E angle) {
matrix_rotation_world_axis(m,0,angle);
}
/** Build a matrix representing a 3D rotation about the world y axis */
template < typename E, class A, class B, class L > void
matrix_rotation_world_y(matrix<E,A,B,L>& m, E angle) {
matrix_rotation_world_axis(m,1,angle);
}
/** Build a matrix representing a 3D rotation about the world z axis */
template < typename E, class A, class B, class L > void
matrix_rotation_world_z(matrix<E,A,B,L>& m, E angle) {
matrix_rotation_world_axis(m,2,angle);
}
//////////////////////////////////////////////////////////////////////////////
// 3D rotation from an axis-angle pair
//////////////////////////////////////////////////////////////////////////////
/** Build a rotation matrix from an axis-angle pair */
template < typename E, class A, class B, class L, class VecT > void
matrix_rotation_axis_angle(matrix<E,A,B,L>& m, const VecT& axis, E angle)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckVec3(axis);
identity_transform(m);
value_type s = std::sin(angle);
value_type c = std::cos(angle);
value_type omc = value_type(1) - c;
value_type xomc = axis[0] * omc;
value_type yomc = axis[1] * omc;
value_type zomc = axis[2] * omc;
value_type xxomc = axis[0] * xomc;
value_type yyomc = axis[1] * yomc;
value_type zzomc = axis[2] * zomc;
value_type xyomc = axis[0] * yomc;
value_type yzomc = axis[1] * zomc;
value_type zxomc = axis[2] * xomc;
value_type xs = axis[0] * s;
value_type ys = axis[1] * s;
value_type zs = axis[2] * s;
m.set_basis_element(0,0, xxomc + c );
m.set_basis_element(0,1, xyomc + zs);
m.set_basis_element(0,2, zxomc - ys);
m.set_basis_element(1,0, xyomc - zs);
m.set_basis_element(1,1, yyomc + c );
m.set_basis_element(1,2, yzomc + xs);
m.set_basis_element(2,0, zxomc + ys);
m.set_basis_element(2,1, yzomc - xs);
m.set_basis_element(2,2, zzomc + c );
}
//////////////////////////////////////////////////////////////////////////////
// 3D rotation from a quaternion
//////////////////////////////////////////////////////////////////////////////
/** Build a rotation matrix from a quaternion */
template < typename E, class A, class B, class L, class QuatT > void
matrix_rotation_quaternion(matrix<E,A,B,L>& m, const QuatT& q)
{
typedef matrix<E,A,B,L> matrix_type;
typedef QuatT quaternion_type;
typedef typename quaternion_type::order_type order_type;
typedef typename matrix_type::value_type value_type;
enum {
W = order_type::W,
X = order_type::X,
Y = order_type::Y,
Z = order_type::Z
};
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckQuat(q);
identity_transform(m);
value_type x2 = q[X] + q[X];
value_type y2 = q[Y] + q[Y];
value_type z2 = q[Z] + q[Z];
value_type xx2 = q[X] * x2;
value_type yy2 = q[Y] * y2;
value_type zz2 = q[Z] * z2;
value_type xy2 = q[X] * y2;
value_type yz2 = q[Y] * z2;
value_type zx2 = q[Z] * x2;
value_type xw2 = q[W] * x2;
value_type yw2 = q[W] * y2;
value_type zw2 = q[W] * z2;
m.set_basis_element(0,0, value_type(1) - yy2 - zz2);
m.set_basis_element(0,1, xy2 + zw2);
m.set_basis_element(0,2, zx2 - yw2);
m.set_basis_element(1,0, xy2 - zw2);
m.set_basis_element(1,1, value_type(1) - zz2 - xx2);
m.set_basis_element(1,2, yz2 + xw2);
m.set_basis_element(2,0, zx2 + yw2);
m.set_basis_element(2,1, yz2 - xw2);
m.set_basis_element(2,2, value_type(1) - xx2 - yy2);
}
//////////////////////////////////////////////////////////////////////////////
// 3D rotation from Euler angles
//////////////////////////////////////////////////////////////////////////////
/** Build a rotation matrix from an Euler-angle triple
*
* The rotations are applied about the cardinal axes in the order specified by
* the 'order' argument, where 'order' is one of the following enumerants:
*
* euler_order_xyz
* euler_order_xzy
* euler_order_xyx
* euler_order_xzx
* euler_order_yzx
* euler_order_yxz
* euler_order_yzy
* euler_order_yxy
* euler_order_zxy
* euler_order_zyx
* euler_order_zxz
* euler_order_zyz
*
* e.g. euler_order_xyz means compute the column-basis rotation matrix
* equivalent to R_x * R_y * R_z, where R_i is the rotation matrix above
* axis i (the row-basis matrix would be R_z * R_y * R_x).
*/
template < typename E, class A, class B, class L > void
matrix_rotation_euler(matrix<E,A,B,L>& m, E angle_0, E angle_1, E angle_2,
EulerOrder order)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
identity_transform(m);
size_t i, j, k;
bool odd, repeat;
detail::unpack_euler_order(order, i, j, k, odd, repeat);
if (odd) {
angle_0 = -angle_0;
angle_1 = -angle_1;
angle_2 = -angle_2;
}
value_type s0 = std::sin(angle_0);
value_type c0 = std::cos(angle_0);
value_type s1 = std::sin(angle_1);
value_type c1 = std::cos(angle_1);
value_type s2 = std::sin(angle_2);
value_type c2 = std::cos(angle_2);
value_type s0s2 = s0 * s2;
value_type s0c2 = s0 * c2;
value_type c0s2 = c0 * s2;
value_type c0c2 = c0 * c2;
if (repeat) {
m.set_basis_element(i,i, c1 );
m.set_basis_element(i,j, s1 * s2 );
m.set_basis_element(i,k,-s1 * c2 );
m.set_basis_element(j,i, s0 * s1 );
m.set_basis_element(j,j,-c1 * s0s2 + c0c2);
m.set_basis_element(j,k, c1 * s0c2 + c0s2);
m.set_basis_element(k,i, c0 * s1 );
m.set_basis_element(k,j,-c1 * c0s2 - s0c2);
m.set_basis_element(k,k, c1 * c0c2 - s0s2);
} else {
m.set_basis_element(i,i, c1 * c2 );
m.set_basis_element(i,j, c1 * s2 );
m.set_basis_element(i,k,-s1 );
m.set_basis_element(j,i, s1 * s0c2 - c0s2);
m.set_basis_element(j,j, s1 * s0s2 + c0c2);
m.set_basis_element(j,k, s0 * c1 );
m.set_basis_element(k,i, s1 * c0c2 + s0s2);
m.set_basis_element(k,j, s1 * c0s2 - s0c2);
m.set_basis_element(k,k, c0 * c1 );
}
}
/** Build a matrix of derivatives of Euler angles about the specified axis.
*
* The rotation derivatives are applied about the cardinal axes in the
* order specified by the 'order' argument, where 'order' is one of the
* following enumerants:
*
* euler_order_xyz
* euler_order_xzy
* euler_order_yzx
* euler_order_yxz
* euler_order_zxy
* euler_order_zyx
*
* e.g. euler_order_xyz means compute the column-basis rotation matrix
* equivalent to R_x * R_y * R_z, where R_i is the rotation matrix above
* axis i (the row-basis matrix would be R_z * R_y * R_x).
*
* The derivative is taken with respect to the specified 'axis', which is
* the position of the axis in the triple; e.g. if order = euler_order_xyz,
* then axis = 0 would mean take the derivative with respect to x. Note
* that repeated axes are not currently supported.
*/
template < typename E, class A, class B, class L > void
matrix_rotation_euler_derivatives(
matrix<E,A,B,L>& m, int axis, E angle_0, E angle_1, E angle_2,
EulerOrder order)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
size_t i, j, k;
bool odd, repeat;
detail::unpack_euler_order(order, i, j, k, odd, repeat);
if(repeat) throw std::invalid_argument(
"matrix_rotation_euler_derivatives does not support repeated axes");
if (odd) {
angle_0 = -angle_0;
angle_1 = -angle_1;
angle_2 = -angle_2;
}
value_type s0 = std::sin(angle_0);
value_type c0 = std::cos(angle_0);
value_type s1 = std::sin(angle_1);
value_type c1 = std::cos(angle_1);
value_type s2 = std::sin(angle_2);
value_type c2 = std::cos(angle_2);
value_type s0s2 = s0 * s2;
value_type s0c2 = s0 * c2;
value_type c0s2 = c0 * s2;
value_type c0c2 = c0 * c2;
if(axis == 0) {
m.set_basis_element(i,i, 0. );
m.set_basis_element(i,j, 0. );
m.set_basis_element(i,k, 0. );
m.set_basis_element(j,i, s1 * c0*c2 + s0*s2);
m.set_basis_element(j,j, s1 * c0*s2 - s0*c2);
m.set_basis_element(j,k, c0 * c1 );
m.set_basis_element(k,i,-s1 * s0*c2 + c0*s2);
m.set_basis_element(k,j,-s1 * s0*s2 - c0*c2);
m.set_basis_element(k,k,-s0 * c1 );
} else if(axis == 1) {
m.set_basis_element(i,i,-s1 * c2 );
m.set_basis_element(i,j,-s1 * s2 );
m.set_basis_element(i,k,-c1 );
m.set_basis_element(j,i, c1 * s0*c2 );
m.set_basis_element(j,j, c1 * s0*s2 );
m.set_basis_element(j,k,-s0 * s1 );
m.set_basis_element(k,i, c1 * c0*c2 );
m.set_basis_element(k,j, c1 * c0*s2 );
m.set_basis_element(k,k,-c0 * s1 );
} else if(axis == 2) {
m.set_basis_element(i,i,-c1 * s2 );
m.set_basis_element(i,j, c1 * c2 );
m.set_basis_element(i,k, 0. );
m.set_basis_element(j,i,-s1 * s0*s2 - c0*c2);
m.set_basis_element(j,j, s1 * s0*c2 - c0*s2);
m.set_basis_element(j,k, 0. );
m.set_basis_element(k,i,-s1 * c0*s2 + s0*c2);
m.set_basis_element(k,j, s1 * c0*c2 + s0*s2);
m.set_basis_element(k,k, 0. );
}
}
//////////////////////////////////////////////////////////////////////////////
// 3D rotation to align with a vector, multiple vectors, or the view plane
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
matrix_rotation_align(
matrix<E,A,B,L>& m,
const VecT_1& align,
const VecT_2& reference,
bool normalize = true,
AxisOrder order = axis_order_zyx)
{
typedef vector< E,fixed<3> > vector_type;
identity_transform(m);
vector_type x, y, z;
orthonormal_basis(align, reference, x, y, z, normalize, order);
matrix_set_basis_vectors(m, x, y, z);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L, class VecT > void
matrix_rotation_align(matrix<E,A,B,L>& m, const VecT& align,
bool normalize = true, AxisOrder order = axis_order_zyx)
{
typedef vector< E,fixed<3> > vector_type;
identity_transform(m);
vector_type x, y, z;
orthonormal_basis(align, x, y, z, normalize, order);
matrix_set_basis_vectors(m, x, y, z);
}
/** See vector_ortho.h for details */
template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
matrix_rotation_align_axial(matrix<E,A,B,L>& m, const VecT_1& align,
const VecT_2& axis, bool normalize = true,
AxisOrder order = axis_order_zyx)
{
typedef vector< E,fixed<3> > vector_type;
identity_transform(m);
vector_type x, y, z;
orthonormal_basis_axial(align, axis, x, y, z, normalize, order);
matrix_set_basis_vectors(m, x, y, z);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L, class MatT > void
matrix_rotation_align_viewplane(
matrix<E,A,B,L>& m,
const MatT& view_matrix,
Handedness handedness,
AxisOrder order = axis_order_zyx)
{
typedef vector< E, fixed<3> > vector_type;
identity_transform(m);
vector_type x, y, z;
orthonormal_basis_viewplane(view_matrix, x, y, z, handedness, order);
matrix_set_basis_vectors(m, x, y, z);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L, class MatT > void
matrix_rotation_align_viewplane_LH(
matrix<E,A,B,L>& m,
const MatT& view_matrix,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_align_viewplane(
m,view_matrix,left_handed,order);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L, class MatT > void
matrix_rotation_align_viewplane_RH(
matrix<E,A,B,L>& m,
const MatT& view_matrix,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_align_viewplane(
m,view_matrix,right_handed,order);
}
//////////////////////////////////////////////////////////////////////////////
// 3D rotation to aim at a target
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_rotation_aim_at(
matrix<E,A,B,L>& m,
const VecT_1& pos,
const VecT_2& target,
const VecT_3& reference,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_align(m, target - pos, reference, true, order);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2 > void
matrix_rotation_aim_at(
matrix<E,A,B,L>& m,
const VecT_1& pos,
const VecT_2& target,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_align(m, target - pos, true, order);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_rotation_aim_at_axial(
matrix<E,A,B,L>& m,
const VecT_1& pos,
const VecT_2& target,
const VecT_3& axis,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_align_axial(m, target - pos, axis, true, order);
}
//////////////////////////////////////////////////////////////////////////////
// 2D rotation
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D rotation */
template < typename E, class A, class B, class L > void
matrix_rotation_2D( matrix<E,A,B,L>& m, E angle)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear2D(m);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
identity_transform(m);
m.set_basis_element(0,0, c);
m.set_basis_element(0,1, s);
m.set_basis_element(1,0,-s);
m.set_basis_element(1,1, c);
}
//////////////////////////////////////////////////////////////////////////////
// 2D rotation to align with a vector
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L, class VecT > void
matrix_rotation_align_2D(matrix<E,A,B,L>& m, const VecT& align,
bool normalize = true, AxisOrder2D order = axis_order_xy)
{
typedef vector< E, fixed<2> > vector_type;
identity_transform(m);
vector_type x, y;
orthonormal_basis_2D(align, x, y, normalize, order);
matrix_set_basis_vectors_2D(m, x, y);
}
//////////////////////////////////////////////////////////////////////////////
// 3D relative rotation about world axes
//////////////////////////////////////////////////////////////////////////////
/** Rotate a rotation matrix about the given world axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_world_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
value_type ij = c * m.basis_element(i,j) - s * m.basis_element(i,k);
value_type jj = c * m.basis_element(j,j) - s * m.basis_element(j,k);
value_type kj = c * m.basis_element(k,j) - s * m.basis_element(k,k);
m.set_basis_element(i,k, s*m.basis_element(i,j) + c*m.basis_element(i,k));
m.set_basis_element(j,k, s*m.basis_element(j,j) + c*m.basis_element(j,k));
m.set_basis_element(k,k, s*m.basis_element(k,j) + c*m.basis_element(k,k));
m.set_basis_element(i,j,ij);
m.set_basis_element(j,j,jj);
m.set_basis_element(k,j,kj);
}
/** Rotate a rotation matrix about the world x axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_world_x(matrix<E,A,B,L>& m, E angle) {
matrix_rotate_about_world_axis(m,0,angle);
}
/** Rotate a rotation matrix about the world y axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_world_y(matrix<E,A,B,L>& m, E angle) {
matrix_rotate_about_world_axis(m,1,angle);
}
/** Rotate a rotation matrix about the world z axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_world_z(matrix<E,A,B,L>& m, E angle) {
matrix_rotate_about_world_axis(m,2,angle);
}
//////////////////////////////////////////////////////////////////////////////
// 3D relative rotation about local axes
//////////////////////////////////////////////////////////////////////////////
/** Rotate a rotation matrix about the given local axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_local_axis(matrix<E,A,B,L>& m, size_t axis, E angle)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
value_type j0 = c * m.basis_element(j,0) + s * m.basis_element(k,0);
value_type j1 = c * m.basis_element(j,1) + s * m.basis_element(k,1);
value_type j2 = c * m.basis_element(j,2) + s * m.basis_element(k,2);
m.set_basis_element(k,0, c*m.basis_element(k,0) - s*m.basis_element(j,0));
m.set_basis_element(k,1, c*m.basis_element(k,1) - s*m.basis_element(j,1));
m.set_basis_element(k,2, c*m.basis_element(k,2) - s*m.basis_element(j,2));
m.set_basis_element(j,0,j0);
m.set_basis_element(j,1,j1);
m.set_basis_element(j,2,j2);
}
/** Rotate a rotation matrix about its local x axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_local_x(matrix<E,A,B,L>& m, E angle) {
matrix_rotate_about_local_axis(m,0,angle);
}
/** Rotate a rotation matrix about its local y axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_local_y(matrix<E,A,B,L>& m, E angle) {
matrix_rotate_about_local_axis(m,1,angle);
}
/** Rotate a rotation matrix about its local z axis */
template < typename E, class A, class B, class L > void
matrix_rotate_about_local_z(matrix<E,A,B,L>& m, E angle) {
matrix_rotate_about_local_axis(m,2,angle);
}
//////////////////////////////////////////////////////////////////////////////
// 2D relative rotation
//////////////////////////////////////////////////////////////////////////////
template < typename E, class A, class B, class L > void
matrix_rotate_2D(matrix<E,A,B,L>& m, E angle)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear2D(m);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
value_type m00 = c * m.basis_element(0,0) - s * m.basis_element(0,1);
value_type m10 = c * m.basis_element(1,0) - s * m.basis_element(1,1);
m.set_basis_element(0,1, s*m.basis_element(0,0) + c*m.basis_element(0,1));
m.set_basis_element(1,1, s*m.basis_element(1,0) + c*m.basis_element(1,1));
m.set_basis_element(0,0,m00);
m.set_basis_element(1,0,m10);
}
//////////////////////////////////////////////////////////////////////////////
// Rotation from vector to vector
//////////////////////////////////////////////////////////////////////////////
/** Build a rotation matrix to rotate from one vector to another
*
* Note: The quaternion algorithm is more stable than the matrix algorithm, so
* we simply pass off to the quaternion function here.
*/
template < class E,class A,class B,class L,class VecT_1,class VecT_2 > void
matrix_rotation_vec_to_vec(
matrix<E,A,B,L>& m,
const VecT_1& v1,
const VecT_2& v2,
bool unit_length_vectors = false)
{
typedef quaternion< E,fixed<>,vector_first,positive_cross >
quaternion_type;
quaternion_type q;
quaternion_rotation_vec_to_vec(q,v1,v2,unit_length_vectors);
matrix_rotation_quaternion(m,q);
}
//////////////////////////////////////////////////////////////////////////////
// Scale the angle of a rotation matrix
//////////////////////////////////////////////////////////////////////////////
/** Scale the angle of a 3D rotation matrix */
template < typename E, class A, class B, class L > void
matrix_scale_rotation_angle(matrix<E,A,B,L>& m, E t,
E tolerance = epsilon<E>::placeholder())
{
typedef vector< E,fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
vector_type axis;
value_type angle;
matrix_to_axis_angle(m, axis, angle, tolerance);
matrix_rotation_axis_angle(m, axis, angle * t);
}
/** Scale the angle of a 2D rotation matrix */
template < typename E, class A, class B, class L > void
matrix_scale_rotation_angle_2D(
matrix<E,A,B,L>& m, E t, E tolerance = epsilon<E>::placeholder())
{
typedef vector< E,fixed<2> > vector_type;
typedef typename vector_type::value_type value_type;
value_type angle = matrix_to_rotation_2D(m);
matrix_rotation_2D(m, angle * t);
}
//////////////////////////////////////////////////////////////////////////////
// Support functions for uniform handling of row- and column-basis matrices
//////////////////////////////////////////////////////////////////////////////
/* Note: The matrix rotation slerp, difference and concatenation functions do
* not use et::MatrixPromote<M1,M2>::temporary_type as the return type, even
* though that is the return type of the underlying matrix multiplication.
* This is because the sizes of these matrices are known at compile time (3x3
* and 2x2), and using fixed<> obviates the need for resizing of intermediate
* temporaries.
*
* Also, no size- or type-checking is done on the arguments to these
* functions, as any such errors will be caught by the matrix multiplication
* and assignment to the 3x3 temporary.
*/
/** A fixed-size temporary 3x3 matrix */
#define MAT_TEMP_3X3 matrix< \
typename et::ScalarPromote< \
typename MatT_1::value_type, \
typename MatT_2::value_type \
>::type, \
fixed<3,3>, \
typename MatT_1::basis_orient, \
row_major \
>
/** A fixed-size temporary 2x2 matrix */
#define MAT_TEMP_2X2 matrix< \
typename et::ScalarPromote< \
typename MatT_1::value_type, \
typename MatT_2::value_type \
>::type, \
fixed<2,2>, \
typename MatT_1::basis_orient, \
row_major \
>
namespace detail {
/** Concatenate two 3D row-basis rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, row_basis) {
return m1*m2;
}
/** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2, col_basis) {
return m2*m1;
}
/** Concatenate two 3D rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
matrix_concat_rotations(const MatT_1& m1, const MatT_2& m2) {
return matrix_concat_rotations(m1,m2,typename MatT_1::basis_orient());
}
/** Concatenate two 2D row-basis rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, row_basis) {
return m1*m2;
}
/** Concatenate two 2D col-basis rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2, col_basis) {
return m2*m1;
}
/** Concatenate two 2D rotation matrices in the order m1->m2 */
template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
matrix_concat_rotations_2D(const MatT_1& m1, const MatT_2& m2) {
return matrix_concat_rotations_2D(m1,m2,typename MatT_1::basis_orient());
}
} // namespace detail
//////////////////////////////////////////////////////////////////////////////
// Matrix rotation difference
//////////////////////////////////////////////////////////////////////////////
/** Return the rotational 'difference' between two 3D rotation matrices */
template < class MatT_1, class MatT_2 > MAT_TEMP_3X3
matrix_rotation_difference(const MatT_1& m1, const MatT_2& m2) {
return detail::matrix_concat_rotations(transpose(m1),m2);
}
/** Return the rotational 'difference' between two 2D rotation matrices */
template < class MatT_1, class MatT_2 > MAT_TEMP_2X2
matrix_rotation_difference_2D(const MatT_1& m1, const MatT_2& m2) {
return detail::matrix_concat_rotations_2D(transpose(m1),m2);
}
//////////////////////////////////////////////////////////////////////////////
// Spherical linear interpolation of rotation matrices
//////////////////////////////////////////////////////////////////////////////
/* @todo: It might be as fast or faster to simply convert the matrices to
* quaternions, interpolate, and convert back.
*
* @todo: The behavior of matrix slerp is currently a little different than
* for quaternions: in the matrix function, when the two matrices are close
* to identical the first is returned, while in the quaternion function the
* quaternions are nlerp()'d in this case.
*
* I still need to do the equivalent of nlerp() for matrices, in which case
* these functions could be revised to pass off to nlerp() when the matrices
* are nearly aligned.
*/
/** Spherical linear interpolation of two 3D rotation matrices */
template < class MatT_1, class MatT_2, typename E > MAT_TEMP_3X3
matrix_slerp(const MatT_1& m1, const MatT_2& m2, E t,
E tolerance = epsilon<E>::placeholder())
{
typedef MAT_TEMP_3X3 temporary_type;
temporary_type m = matrix_rotation_difference(m1,m2);
matrix_scale_rotation_angle(m,t,tolerance);
return detail::matrix_concat_rotations(m1,m);
}
/** Spherical linear interpolation of two 2D rotation matrices */
template < class MatT_1, class MatT_2, typename E > MAT_TEMP_2X2
matrix_slerp_2D(const MatT_1& m1, const MatT_2& m2, E t,
E tolerance = epsilon<E>::placeholder())
{
typedef MAT_TEMP_2X2 temporary_type;
temporary_type m = matrix_rotation_difference_2D(m1,m2);
matrix_scale_rotation_angle_2D(m,t,tolerance);
return detail::matrix_concat_rotations_2D(m1,m);
}
#undef MAT_TEMP_3X3
#undef MAT_TEMP_2X2
//////////////////////////////////////////////////////////////////////////////
// Conversions
//////////////////////////////////////////////////////////////////////////////
/** Convert a 3D rotation matrix to an axis-angle pair */
template < class MatT, typename E, class A > void
matrix_to_axis_angle(
const MatT& m,
vector<E,A >& axis,
E& angle,
E tolerance = epsilon<E>::placeholder())
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
axis.set(
m.basis_element(1,2) - m.basis_element(2,1),
m.basis_element(2,0) - m.basis_element(0,2),
m.basis_element(0,1) - m.basis_element(1,0)
);
value_type l = length(axis);
value_type tmo = trace_3x3(m) - value_type(1);
if (l > tolerance) {
axis /= l;
angle = std::atan2(l, tmo); // l=2sin(theta),tmo=2cos(theta)
} else if (tmo > value_type(0)) {
axis.zero();
angle = value_type(0);
} else {
size_t largest_diagonal_element =
index_of_max(
m.basis_element(0,0),
m.basis_element(1,1),
m.basis_element(2,2)
);
size_t i, j, k;
cyclic_permutation(largest_diagonal_element, i, j, k);
axis[i] =
std::sqrt(
m.basis_element(i,i) -
m.basis_element(j,j) -
m.basis_element(k,k) +
value_type(1)
) * value_type(.5);
value_type s = value_type(.5) / axis[i];
axis[j] = m.basis_element(i,j) * s;
axis[k] = m.basis_element(i,k) * s;
angle = constants<value_type>::pi();
}
}
/** Convert a 3D rotation matrix to an Euler-angle triple */
template < class MatT, typename Real >
void matrix_to_euler(
const MatT& m,
Real& angle_0,
Real& angle_1,
Real& angle_2,
EulerOrder order,
Real tolerance = epsilon<Real>::placeholder())
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
size_t i, j, k;
bool odd, repeat;
detail::unpack_euler_order(order, i, j, k, odd, repeat);
if (repeat) {
value_type s1 = length(m.basis_element(j,i),m.basis_element(k,i));
value_type c1 = m.basis_element(i,i);
angle_1 = std::atan2(s1, c1);
if (s1 > tolerance) {
angle_0 = std::atan2(m.basis_element(j,i),m.basis_element(k,i));
angle_2 = std::atan2(m.basis_element(i,j),-m.basis_element(i,k));
} else {
angle_0 = value_type(0);
angle_2 = sign(c1) *
std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
}
} else {
value_type s1 = -m.basis_element(i,k);
value_type c1 = length(m.basis_element(i,i),m.basis_element(i,j));
angle_1 = std::atan2(s1, c1);
if (c1 > tolerance) {
angle_0 = std::atan2(m.basis_element(j,k),m.basis_element(k,k));
angle_2 = std::atan2(m.basis_element(i,j),m.basis_element(i,i));
} else {
angle_0 = value_type(0);
angle_2 = -sign(s1) *
std::atan2(-m.basis_element(k,j),m.basis_element(j,j));
}
}
if (odd) {
angle_0 = -angle_0;
angle_1 = -angle_1;
angle_2 = -angle_2;
}
}
/** Convenience function to return a 3D vector containing the Euler angles
* in the requested order.
*/
template < class MatT > vector< typename MatT::value_type, fixed<3> >
matrix_to_euler(
const MatT& m,
EulerOrder order,
const typename MatT::value_type&
tolerance = epsilon<typename MatT::value_type>::placeholder())
{
typename MatT::value_type e0, e1, e2;
matrix_to_euler(m, e0, e1, e2, order, tolerance);
return vector< typename MatT::value_type, fixed<3> >(e0, e1, e2);
}
/** Convert a 2D rotation matrix to a rotation angle */
template < class MatT > typename MatT::value_type
matrix_to_rotation_2D(const MatT& m)
{
/* Checking */
detail::CheckMatLinear2D(m);
return std::atan2(m.basis_element(0,1),m.basis_element(0,0));
}
} // namespace cml
#endif

984
Lib/Include/CML/mathlib/matrix_transform.h Normal file
View File

@@ -0,0 +1,984 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_transform_h
#define matrix_transform_h
#include <cml/mathlib/matrix_basis.h>
#include <cml/mathlib/matrix_rotation.h>
#include <cml/mathlib/matrix_translation.h>
/* Functions for building matrix transforms other than rotations
* (matrix_rotation.h) and viewing projections (matrix_projection.h).
*/
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// 3D translation
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D translation */
template < typename E, class A, class B, class L > void
matrix_translation(matrix<E,A,B,L>& m, E x, E y, E z)
{
identity_transform(m);
matrix_set_translation(m,x,y,z);
}
/** Build a matrix representing a 3D translation with z set to 0 */
template < typename E, class A, class B, class L > void
matrix_translation(matrix<E,A,B,L>& m, E x, E y)
{
identity_transform(m);
matrix_set_translation(m,x,y);
}
/** Build a matrix representing a 3D translation */
template < typename E, class A, class B, class L, class VecT > void
matrix_translation(matrix<E,A,B,L>& m, const VecT& translation)
{
identity_transform(m);
matrix_set_translation(m,translation);
}
//////////////////////////////////////////////////////////////////////////////
// 2D translation
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D translation */
template < typename E, class A, class B, class L > void
matrix_translation_2D(matrix<E,A,B,L>& m, E x, E y)
{
identity_transform(m);
matrix_set_translation_2D(m,x,y);
}
/** Build a matrix representing a 2D translation */
template < typename E, class A, class B, class L, class VecT > void
matrix_translation_2D(matrix<E,A,B,L>& m, const VecT& translation)
{
identity_transform(m);
matrix_set_translation_2D(m, translation);
}
//////////////////////////////////////////////////////////////////////////////
// 3D scale
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a uniform 3D scale */
template < typename E, class A, class B, class L > void
matrix_uniform_scale(matrix<E,A,B,L>& m, E scale) {
matrix_scale(m,scale,scale,scale);
}
/** Build a matrix representing a non-uniform 3D scale */
template < typename E, class A, class B, class L > void
matrix_scale(matrix<E,A,B,L>& m, E scale_x, E scale_y, E scale_z)
{
/* Checking */
detail::CheckMatLinear3D(m);
identity_transform(m);
m.set_basis_element(0,0,scale_x);
m.set_basis_element(1,1,scale_y);
m.set_basis_element(2,2,scale_z);
}
/** Build a matrix representing a non-uniform 3D scale */
template < typename E, class A, class B, class L, class VecT > void
matrix_scale(matrix<E,A,B,L>& m, const VecT& scale)
{
/* Checking */
detail::CheckVec3(scale);
matrix_scale(m, scale[0], scale[1], scale[2]);
}
//////////////////////////////////////////////////////////////////////////////
// 2D scale
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a uniform 2D scale */
template < typename E, class A, class B, class L > void
matrix_uniform_scale_2D(matrix<E,A,B,L>& m, E scale) {
matrix_scale_2D(m,scale,scale);
}
/** Build a matrix representing a non-uniform 2D scale */
template < typename E, class A, class B, class L > void
matrix_scale_2D(matrix<E,A,B,L>& m, E scale_x, E scale_y)
{
/* Checking */
detail::CheckMatLinear2D(m);
identity_transform(m);
m.set_basis_element(0,0,scale_x);
m.set_basis_element(1,1,scale_y);
}
/** Build a matrix representing a non-uniform 2D scale */
template < typename E, class A, class B, class L, class VecT > void
matrix_scale_2D(matrix<E,A,B,L>& m, const VecT& scale)
{
/* Checking */
detail::CheckVec2(scale);
matrix_scale_2D(m, scale[0], scale[1]);
}
//////////////////////////////////////////////////////////////////////////////
// 3D scale along axis
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D scale along an arbitrary axis */
template < typename E, class A, class B, class L, class VecT > void
matrix_scale_along_axis(matrix<E,A,B,L>&m, const VecT& axis, E scale)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckVec3(axis);
matrix<E,fixed<3,3>,B,L> outer_p = outer(axis,axis)*(scale-value_type(1));
outer_p(0,0) += value_type(1);
outer_p(1,1) += value_type(1);
outer_p(2,2) += value_type(1);
matrix_linear_transform(m, outer_p);
}
//////////////////////////////////////////////////////////////////////////////
// 2D scale along axis
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D scale along an arbitrary axis */
template < typename E, class A, class B, class L, class VecT >
void matrix_scale_along_axis_2D(matrix<E,A,B,L>& m, const VecT& axis,
E scale)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckVec2(axis);
matrix<E,fixed<2,2>,B,L> outer_p = outer(axis,axis)*(scale-value_type(1));
outer_p(0,0) += value_type(1);
outer_p(1,1) += value_type(1);
matrix_linear_transform_2D(m, outer_p);
}
//////////////////////////////////////////////////////////////////////////////
// 3D shear
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D shear along the specified world axis */
template < typename E, class A, class B, class L > void
matrix_shear(matrix<E,A,B,L>& m, size_t axis, E shear_s, E shear_t)
{
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(axis);
identity_transform(m);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
m.set_basis_element(i,j,shear_s);
m.set_basis_element(i,k,shear_t);
}
/** Build a matrix representing a 3D shear along the world x axis */
template < typename E, class A, class B, class L > void
matrix_shear_x(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
matrix_shear(m,0,shear_s,shear_t);
}
/** Build a matrix representing a 3D shear along the world y axis */
template < typename E, class A, class B, class L > void
matrix_shear_y(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
matrix_shear(m,1,shear_s,shear_t);
}
/** Build a matrix representing a 3D shear along the world z axis */
template < typename E, class A, class B, class L > void
matrix_shear_z(matrix<E,A,B,L>& m, E shear_s, E shear_t) {
matrix_shear(m,2,shear_s,shear_t);
}
//////////////////////////////////////////////////////////////////////////////
// 2D shear
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D shear along the specified world axis */
template < typename E, class A, class B, class L > void
matrix_shear_2D(matrix<E,A,B,L>& m, size_t axis, E shear)
{
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckIndex2(axis);
identity_transform(m);
size_t i, j;
cyclic_permutation(axis, i, j);
m.set_basis_element(i,j,shear);
}
/** Build a matrix representing a 2D shear along the world x axis */
template < typename E, class A, class B, class L > void
matrix_shear_x_2D(matrix<E,A,B,L>& m, E shear) {
matrix_shear_2D(m,0,shear);
}
/** Build a matrix representing a 2D shear along the world y axis */
template < typename E, class A, class B, class L > void
matrix_shear_y_2D(matrix<E,A,B,L>& m, E shear) {
matrix_shear_2D(m,1,shear);
}
//////////////////////////////////////////////////////////////////////////////
// 3D reflection
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D reflection along the given world axis */
template < typename E, class A, class B, class L > void
matrix_reflect(matrix<E,A,B,L>& m, size_t axis)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(axis);
identity_transform(m);
m(axis,axis) = value_type(-1);
}
/** Build a matrix representing a 3D reflection along the world x axis */
template < typename E, class A, class B, class L > void
matrix_reflect_x(matrix<E,A,B,L>& m) {
matrix_reflect(m,0);
}
/** Build a matrix representing a 3D reflection along the world y axis */
template < typename E, class A, class B, class L > void
matrix_reflect_y(matrix<E,A,B,L>& m) {
matrix_reflect(m,1);
}
/** Build a matrix representing a 3D reflection along the world z axis */
template < typename E, class A, class B, class L > void
matrix_reflect_z(matrix<E,A,B,L>& m) {
matrix_reflect(m,2);
}
//////////////////////////////////////////////////////////////////////////////
// 2D reflection
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D reflection along the given world axis */
template < typename E, class A, class B, class L > void
matrix_reflect_2D(matrix<E,A,B,L>& m, size_t axis)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckIndex2(axis);
identity_transform(m);
m(axis,axis) = value_type(-1);
}
/** Build a matrix representing a 2D reflection along the world x axis */
template < typename E, class A, class B, class L > void
matrix_reflect_x_2D(matrix<E,A,B,L>& m) {
matrix_reflect_2D(m,0);
}
/** Build a matrix representing a 2D reflection along the world y axis */
template < typename E, class A, class B, class L > void
matrix_reflect_y_2D(matrix<E,A,B,L>& m) {
matrix_reflect_2D(m,1);
}
//////////////////////////////////////////////////////////////////////////////
// 3D reflection about hyperplane
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D reflection about the given hyperplane */
template < typename E, class A, class B, class L, class VecT > void
matrix_reflect_about_hplane(matrix<E,A,B,L>& m, const VecT& normal)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
matrix_scale_along_axis(m, normal, value_type(-1));
}
//////////////////////////////////////////////////////////////////////////////
// 2D reflection about hyperplane
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D reflection about the given hyperplane */
template < typename E, class A, class B, class L, class VecT > void
matrix_reflect_about_hplane_2D(matrix<E,A,B,L>&m, const VecT& normal)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
matrix_scale_along_axis_2D(m, normal, value_type(-1));
}
//////////////////////////////////////////////////////////////////////////////
// 3D orthographic projection to cardinal hyperplane
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing an orthographic projection onto a plane */
template < typename E, class A, class B, class L > void
matrix_ortho_project(matrix<E,A,B,L>& m, size_t axis)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckIndex3(axis);
identity_transform(m);
m(axis,axis) = value_type(0);
}
/** Build a matrix representing an orthographic projection onto the yz plane*/
template < typename E, class A, class B, class L > void
matrix_ortho_project_yz(matrix<E,A,B,L>& m) {
matrix_ortho_project(m,0);
}
/** Build a matrix representing an orthographic projection onto the zx plane*/
template < typename E, class A, class B, class L > void
matrix_ortho_project_zx(matrix<E,A,B,L>& m) {
matrix_ortho_project(m,1);
}
/** Build a matrix representing an orthographic projection onto the zy plane*/
template < typename E, class A, class B, class L > void
matrix_ortho_project_xy(matrix<E,A,B,L>& m) {
matrix_ortho_project(m,2);
}
//////////////////////////////////////////////////////////////////////////////
// 2D orthographic projection to cardinal hyperplane
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D orthographic projection */
template < typename E, class A, class B, class L > void
matrix_ortho_project_2D(matrix<E,A,B,L>& m, size_t axis)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckIndex2(axis);
identity_transform(m);
m(axis,axis) = value_type(0);
}
/** Build a matrix representing an orthographic projection onto the y axis */
template < typename E, class A, class B, class L > void
matrix_ortho_project_y_2D(matrix<E,A,B,L>& m) {
matrix_ortho_project_2D(m,0);
}
/** Build a matrix representing an orthographic projection onto the x axis */
template < typename E, class A, class B, class L > void
matrix_ortho_project_x_2D(matrix<E,A,B,L>& m) {
matrix_ortho_project_2D(m,1);
}
//////////////////////////////////////////////////////////////////////////////
// 3D orthographic projection to hyperplane
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 3D orthographic projection about the given
* hyperplane passing through the origin.
*/
template < typename E, class A, class B, class L, class VecT > void
matrix_ortho_project_to_hplane(matrix<E,A,B,L>& m, const VecT& normal)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
matrix_scale_along_axis(m, normal, value_type(0));
}
//////////////////////////////////////////////////////////////////////////////
// 2D orthographic projection to hyperplane
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 2D orthographic projection about the given
* hyperplane passing through the origin.
*/
template < typename E, class A, class B, class L, class VecT > void
matrix_ortho_project_to_hplane_2D(matrix<E,A,B,L>& m, const VecT& normal)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
matrix_scale_along_axis_2D(m, normal, value_type(0));
}
//////////////////////////////////////////////////////////////////////////////
// 3D 'aim at'
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_aim_at(matrix<E,A,B,L>& m, const VecT_1& pos, const VecT_2& target,
const VecT_3& reference,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_aim_at(m, pos, target, reference, order);
matrix_set_translation(m, pos);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2 > void
matrix_aim_at(matrix<E,A,B,L>& m, const VecT_1& pos, const VecT_2& target,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_aim_at(m, pos, target, order);
matrix_set_translation(m, pos);
}
/** See vector_ortho.h for details */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_aim_at_axial(
matrix<E,A,B,L>& m,
const VecT_1& pos,
const VecT_2& target,
const VecT_3& axis,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_aim_at_axial(m, pos, target, axis, order);
matrix_set_translation(m, pos);
}
/** See vector_ortho.h for details */
template < typename E,class A,class B,class L,class VecT,class MatT > void
matrix_aim_at_viewplane(
matrix<E,A,B,L>& m,
const VecT& pos,
const MatT& view_matrix,
Handedness handedness,
AxisOrder order = axis_order_zyx)
{
matrix_rotation_align_viewplane(m, view_matrix, handedness, order);
matrix_set_translation(m, pos);
}
//////////////////////////////////////////////////////////////////////////////
// 2D 'aim at'
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E,class A,class B,class L,class VecT_1,class VecT_2 > void
matrix_aim_at_2D(
matrix<E,A,B,L>& m,
const VecT_1& pos,
const VecT_2& target,
AxisOrder2D order = axis_order_xy)
{
matrix_rotation_align_2D(m, target - pos, true, order);
matrix_set_translation_2D(m, pos);
}
//////////////////////////////////////////////////////////////////////////////
// 3D 'look at' view matrix
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix representing a 'look at' view transform */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_look_at(
matrix<E,A,B,L>& m,
const VecT_1& eye,
const VecT_2& target,
const VecT_3& up,
Handedness handedness)
{
typedef matrix<E,A,B,L> matrix_type;
typedef vector< E,fixed<3> > vector_type;
typedef typename matrix_type::value_type value_type;
/* Checking */
detail::CheckMatAffine3D(m);
identity_transform(m);
value_type s = handedness == left_handed ?
static_cast<value_type>(1) : static_cast<value_type>(-1);
vector_type z = s * normalize(target - eye);
vector_type x = unit_cross(up,z);
vector_type y = cross(z,x);
matrix_set_transposed_basis_vectors(m,x,y,z);
matrix_set_translation(m,-dot(eye,x),-dot(eye,y),-dot(eye,z));
}
/** Build a matrix representing a left-handedness 'look at' view transform */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_look_at_LH(matrix<E,A,B,L>& m, const VecT_1& eye,
const VecT_2& target, const VecT_3& up)
{
matrix_look_at(m, eye, target, up, left_handed);
}
/** Build a matrix representing a right-handedness 'look at' view transform */
template < typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_look_at_RH(matrix<E,A,B,L>& m, const VecT_1& eye,
const VecT_2& target, const VecT_3& up)
{
matrix_look_at(m, eye, target, up, right_handed);
}
/** Build a matrix representing a 'look at' view transform */
template < typename E, class A, class B, class L > void
matrix_look_at(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z, E target_x,
E target_y, E target_z, E up_x, E up_y, E up_z,
Handedness handedness)
{
typedef vector< E, fixed<3> > vector_type;
matrix_look_at(m,
vector_type(eye_x,eye_y,eye_z),
vector_type(target_x,target_y,target_z),
vector_type(up_x,up_y,up_z),
handedness
);
}
/** Build a matrix representing a left-handed'look at' view transform */
template < typename E, class A, class B, class L > void
matrix_look_at_LH(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z,
E target_x, E target_y, E target_z, E up_x, E up_y, E up_z)
{
matrix_look_at(m,eye_x,eye_y,eye_z,target_x,target_y,target_z,up_x,up_y,
up_z,left_handed);
}
/** Build a matrix representing a right-handed'look at' view transform */
template < typename E, class A, class B, class L > void
matrix_look_at_RH(matrix<E,A,B,L>& m, E eye_x, E eye_y, E eye_z,
E target_x, E target_y, E target_z, E up_x, E up_y, E up_z)
{
matrix_look_at(m,eye_x,eye_y,eye_z,target_x,target_y,target_z,up_x,up_y,
up_z,right_handed);
}
//////////////////////////////////////////////////////////////////////////////
// 3D linear transform
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix from the 3x3 linear transform part of another matrix */
template < typename E, class A, class B, class L, class MatT > void
matrix_linear_transform(matrix<E,A,B,L>& m, const MatT& linear)
{
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckMatLinear3D(linear);
identity_transform(m);
for(size_t i = 0; i < 3; ++i) {
for(size_t j = 0; j < 3; ++j) {
m.set_basis_element(i,j,linear.basis_element(i,j));
}
}
}
//////////////////////////////////////////////////////////////////////////////
// 2D linear transform
//////////////////////////////////////////////////////////////////////////////
/** Build a matrix from the 2x2 linear transform part of another matrix */
template < typename E, class A, class B, class L, class MatT > void
matrix_linear_transform_2D(matrix<E,A,B,L>& m, const MatT& linear)
{
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckMatLinear2D(linear);
identity_transform(m);
for(size_t i = 0; i < 2; ++i) {
for(size_t j = 0; j < 2; ++j) {
m.set_basis_element(i,j,linear.basis_element(i,j));
}
}
}
//////////////////////////////////////////////////////////////////////////////
// 3D affine transform
//////////////////////////////////////////////////////////////////////////////
/** 3D affine transform from three basis vectors and a translation */
template <typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3, class VecT_4 > void
matrix_affine_transform(matrix<E,A,B,L>& m, const VecT_1& x, const VecT_2& y,
const VecT_3& z, const VecT_4& translation)
{
identity_transform(m);
matrix_set_basis_vectors(m,x,y,z);
matrix_set_translation(m,translation);
}
/** 3D affine transform from a quaternion and a translation */
template <
typename E, class A, class B, class L,
typename QE, class QA, class O, class C, class VecT > void
matrix_affine_transform(
matrix<E,A,B,L>& m, const quaternion<QE,QA,O,C>& q,
const VecT& translation)
{
matrix_rotation_quaternion(m,q);
matrix_set_translation(m,translation);
}
/** 3D affine transform from a quaternion expression and a translation */
template < typename E,class A,class B,class L,class XprT,class VecT > void
matrix_affine_transform(
matrix<E,A,B,L>& m, const et::QuaternionXpr<XprT>& q,
const VecT& translation)
{
matrix_rotation_quaternion(m,q);
matrix_set_translation(m,translation);
}
/** 3D affine transform from an axis-angle pair and a translation */
template <
typename E, class A, class B, class L, class VecT_1, class VecT_2 > void
matrix_affine_transform(
matrix<E,A,B,L>& m,const VecT_1& axis,E angle,const VecT_2& translation)
{
matrix_rotation_axis_angle(m,axis,angle);
matrix_set_translation(m,translation);
}
/** 3D affine transform from an Euler-angle triple and a translation */
template < typename E, class A, class B, class L, class VecT > void
matrix_affine_transform(matrix<E,A,B,L>& m, E angle_0, E angle_1,
E angle_2, EulerOrder order, const VecT& translation)
{
matrix_rotation_euler(m,angle_0,angle_1,angle_2,order);
matrix_set_translation(m,translation);
}
/** 3D affine transform from a matrix and a translation */
template <
typename E, class A, class B, class L,
typename ME, class MA, class MB, class ML, class VecT > void
matrix_affine_transform(matrix<E,A,B,L>& m,
const matrix<ME,MA,MB,ML>& linear, const VecT& translation)
{
matrix_linear_transform(m,linear);
matrix_set_translation(m,translation);
}
/** 3D affine transform from a matrix expression and a translation */
template < typename E,class A,class B,class L,class XprT,class VecT > void
matrix_affine_transform(
matrix<E,A,B,L>& m, const et::MatrixXpr<XprT>& linear,
const VecT& translation)
{
matrix_linear_transform(m,linear);
matrix_set_translation(m,translation);
}
//////////////////////////////////////////////////////////////////////////////
// 2D affine transform
//////////////////////////////////////////////////////////////////////////////
/** 2D affine transform from two basis vectors and a translation */
template <typename E, class A, class B, class L,
class VecT_1, class VecT_2, class VecT_3 > void
matrix_affine_transform_2D(matrix<E,A,B,L>& m, const VecT_1& x,
const VecT_2& y, const VecT_3& translation)
{
identity_transform(m);
matrix_set_basis_vectors_2D(m,x,y);
matrix_set_translation_2D(m,translation);
}
/** 2D affine transform from a rotation angle and a translation */
template <typename E, class A, class B, class L, class VecT >
void matrix_affine_transform_2D(matrix<E,A,B,L>& m, E angle,
const VecT& translation)
{
matrix_rotation_2D(m,angle);
matrix_set_translation_2D(m,translation);
}
/** 2D affine transform from a matrix and a translation */
template < typename E,class A,class B,class L,class MatT,class VecT > void
matrix_affine_transform_2D(
matrix<E,A,B,L>& m, const MatT& linear, const VecT& translation)
{
matrix_linear_transform_2D(m, linear);
matrix_set_translation_2D(m,translation);
}
//////////////////////////////////////////////////////////////////////////////
// 3D affine from 2D affine
//////////////////////////////////////////////////////////////////////////////
/** Construct a 3D affine transform from a 2D affine transform */
template < typename E, class A, class B, class L, class MatT > void
matrix_3D_affine_from_2D_affine(matrix<E,A,B,L>& m, const MatT& affine_2D)
{
typedef vector< E, fixed<2> > vector_type;
vector_type x = matrix_get_x_basis_vector_2D(affine_2D);
vector_type y = matrix_get_y_basis_vector_2D(affine_2D);
vector_type p = matrix_get_translation_2D(affine_2D);
identity_transform(m);
matrix_set_basis_vectors_2D(m,x,y);
matrix_set_translation(m,p);
}
//////////////////////////////////////////////////////////////////////////////
// 3D affine from 3D affine
//////////////////////////////////////////////////////////////////////////////
/** Construct a 3D affine transform from another 3D affine transform */
template < typename E, class A, class B, class L, class MatT > void
matrix_3D_affine_from_3D_affine(matrix<E,A,B,L>& m, const MatT& affine_3D)
{
typedef vector< E, fixed<3> > vector_type;
vector_type x = matrix_get_x_basis_vector(affine_3D);
vector_type y = matrix_get_y_basis_vector(affine_3D);
vector_type z = matrix_get_z_basis_vector(affine_3D);
vector_type p = matrix_get_translation(affine_3D);
identity_transform(m);
matrix_set_basis_vectors(m,x,y,z);
matrix_set_translation(m,p);
}
//////////////////////////////////////////////////////////////////////////////
// Matrix decomposition (scale->rotate->translate)
//////////////////////////////////////////////////////////////////////////////
/* 3x3 matrix version */
template <
class MatT,
typename Real,
typename ME,
class MA,
class B,
class L,
typename VE,
class VA
>
void matrix_decompose_SRT(
const MatT& m,
Real& scale_x,
Real& scale_y,
Real& scale_z,
matrix<ME,MA,B,L>& rotation,
vector<VE,VA>& translation)
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
typedef vector<value_type, fixed<3> > vector_type;
/* Checking */
detail::CheckMatAffine3D(m);
detail::CheckMatLinear3D(rotation);
vector_type x, y, z;
matrix_get_basis_vectors(m, x, y, z);
scale_x = x.length();
scale_y = y.length();
scale_z = z.length();
x /= scale_x;
y /= scale_y;
z /= scale_z;
matrix_set_basis_vectors(rotation, x, y, z);
translation = matrix_get_translation(m);
}
/* Quaternion version */
template <
class MatT,
typename Real,
typename QE,
class QA,
class O,
class C,
typename VE,
class VA
>
void matrix_decompose_SRT(
const MatT& m,
Real& scale_x,
Real& scale_y,
Real& scale_z,
quaternion<QE,QA,O,C>& rotation,
vector<VE,VA>& translation)
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
typedef matrix< value_type, fixed<3,3> > rotation_type;
rotation_type rotation_matrix;
matrix_decompose_SRT(
m, scale_x, scale_y, scale_z, rotation_matrix, translation);
quaternion_rotation_matrix(rotation, rotation_matrix);
}
/* Euler angle version */
template < class MatT, typename Real, typename E, class A >
void matrix_decompose_SRT(
const MatT& m,
Real& scale_x,
Real& scale_y,
Real& scale_z,
Real& angle_0,
Real& angle_1,
Real& angle_2,
EulerOrder order,
vector<E,A>& translation,
Real tolerance = epsilon<Real>::placeholder())
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
typedef matrix< value_type, fixed<3,3> > rotation_type;
rotation_type rotation_matrix;
matrix_decompose_SRT(
m, scale_x, scale_y, scale_z, rotation_matrix, translation);
matrix_to_euler(
rotation_matrix, angle_0, angle_1, angle_2, order, tolerance);
}
/* Axis-angle version */
template < class MatT, typename Real, typename E, class A >
void matrix_decompose_SRT(
const MatT& m,
Real& scale_x,
Real& scale_y,
Real& scale_z,
vector<E,A>& axis,
Real& angle,
vector<E,A>& translation,
Real tolerance = epsilon<Real>::placeholder())
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
typedef matrix< value_type, fixed<3,3> > rotation_type;
rotation_type rotation_matrix;
matrix_decompose_SRT(
m, scale_x, scale_y, scale_z, rotation_matrix, translation);
matrix_to_axis_angle(rotation_matrix, axis, angle, tolerance);
}
/* 2x2 matrix version, 2-d */
template <
class MatT,
typename Real,
typename ME,
class MA,
class B,
class L,
typename VE,
class VA
>
void matrix_decompose_SRT_2D(
const MatT& m,
Real& scale_x,
Real& scale_y,
matrix<ME,MA,B,L>& rotation,
vector<VE,VA>& translation)
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
typedef vector<value_type, fixed<2> > vector_type;
/* Checking */
detail::CheckMatAffine2D(m);
detail::CheckMatLinear2D(rotation);
vector_type x, y;
matrix_get_basis_vectors_2D(m, x, y);
scale_x = x.length();
scale_y = y.length();
x /= scale_x;
y /= scale_y;
matrix_set_basis_vectors_2D(rotation, x, y);
translation = matrix_get_translation_2D(m);
}
/* Angle version, 2-d */
template < class MatT, typename Real, typename E, class A >
void matrix_decompose_SRT_2D(
const MatT& m,
Real& scale_x,
Real& scale_y,
Real& angle,
vector<E,A>& translation)
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
typedef matrix< value_type, fixed<2,2> > rotation_type;
rotation_type rotation_matrix;
matrix_decompose_SRT_2D(
m, scale_x, scale_y, rotation_matrix, translation);
angle = matrix_to_rotation_2D(rotation_matrix);
}
} // namespace cml
#endif

177
Lib/Include/CML/mathlib/matrix_translation.h Normal file
View File

@@ -0,0 +1,177 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef matrix_translation_h
#define matrix_translation_h
#include <cml/mathlib/checking.h>
/* Functions for getting and setting the translation of a 3D or 2D affine
* transform.
*/
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// Functions for setting the translation of a 3D or 2D affine transform matrix
//////////////////////////////////////////////////////////////////////////////
/** Set the translation of a 3D affine transform */
template < typename E, class A, class B, class L > void
matrix_set_translation(matrix<E,A,B,L>& m, E x, E y, E z)
{
/* Checking */
detail::CheckMatAffine3D(m);
m.set_basis_element(3,0,x);
m.set_basis_element(3,1,y);
m.set_basis_element(3,2,z);
}
/** Set the translation of a 3D affine transform with z set to 0 */
template < typename E, class A, class B, class L > void
matrix_set_translation(matrix<E,A,B,L>& m, E x, E y)
{
typedef matrix<E,A,B,L> matrix_type;
typedef typename matrix_type::value_type value_type;
matrix_set_translation(m, x, y, value_type(0));
}
/** Set the translation of a 3D affine transform from a 3D or 2D vector */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_translation(matrix<E,A,B,L>& m, const VecT& translation)
{
/* Checking */
detail::CheckVec2Or3(translation);
if (translation.size() == 3) {
matrix_set_translation(
m,translation[0], translation[1], translation[2]);
} else { // translation.size() == 2
matrix_set_translation(m, translation[0], translation[1]);
}
}
/** Set the translation of a 2D affine transform */
template < typename E, class A, class B, class L > void
matrix_set_translation_2D(matrix<E,A,B,L>& m, E x, E y)
{
/* Checking */
detail::CheckMatAffine2D(m);
m.set_basis_element(2,0,x);
m.set_basis_element(2,1,y);
}
/** Set the translation of a 2D affine transform from a 2D vector */
template < typename E, class A, class B, class L, class VecT > void
matrix_set_translation_2D(matrix<E,A,B,L>& m, const VecT& translation)
{
/* Checking */
detail::CheckVec2(translation);
matrix_set_translation_2D(m, translation[0], translation[1]);
}
//////////////////////////////////////////////////////////////////////////////
// Functions for getting the translation of a 3D or 2D affine transform matrix
//////////////////////////////////////////////////////////////////////////////
/** Get the translation of a 3D affine transform */
template < class MatT > vector< typename MatT::value_type, fixed<3> >
matrix_get_translation(const MatT& m)
{
typedef typename MatT::value_type value_type;
typedef vector< value_type, fixed<3> > vector_type;
/* Checking */
detail::CheckMatAffine3D(m);
return vector_type(
m.basis_element(3,0),
m.basis_element(3,1),
m.basis_element(3,2)
);
}
/** Get the translation of a 3D affine transform */
template < class MatT > void
matrix_get_translation(
const MatT& m,
typename MatT::value_type& t1,
typename MatT::value_type& t2,
typename MatT::value_type& t3
)
{
typedef typename MatT::value_type value_type;
typedef vector< value_type, fixed<3> > vector_type;
/* Checking */
detail::CheckMatAffine3D(m);
t1 = m.basis_element(3,0);
t2 = m.basis_element(3,1);
t3 = m.basis_element(3,2);
}
/** Get the translation of a 2D affine transform */
template < class MatT > vector< typename MatT::value_type, fixed<2> >
matrix_get_translation_2D(const MatT& m)
{
typedef typename MatT::value_type value_type;
typedef vector< value_type, fixed<2> > vector_type;
/* Checking */
detail::CheckMatAffine2D(m);
return vector_type(m.basis_element(2,0), m.basis_element(2,1));
}
/** Get the translation of a 2D affine transform */
template < class MatT > void
matrix_get_translation_2D(
const MatT& m,
typename MatT::value_type& t1,
typename MatT::value_type& t2
)
{
typedef typename MatT::value_type value_type;
typedef vector< value_type, fixed<2> > vector_type;
/* Checking */
detail::CheckMatAffine2D(m);
t1 = m.basis_element(2,0);
t2 = m.basis_element(2,1);
}
//////////////////////////////////////////////////////////////////////////////
// Function for getting the translation of a 3D view matrix
//////////////////////////////////////////////////////////////////////////////
/** Get the translation of a 3D affine transform */
template < class MatT > vector< typename MatT::value_type, fixed<3> >
matrix_get_view_translation(const MatT& m)
{
typedef typename MatT::value_type value_type;
typedef vector< value_type, fixed<3> > vector_type;
vector_type x, y, z;
matrix_get_basis_vectors(m,x,y,z);
vector_type p = matrix_get_translation(m);
return vector_type(-dot(p,x),-dot(p,y),-dot(p,z));
}
} // namespace cml
#endif

210
Lib/Include/CML/mathlib/misc.h Normal file
View File

@@ -0,0 +1,210 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef misc_h
#define misc_h
#include <cml/mathlib/checking.h>
/* A few miscellaneous functions and helper classes.
*
* @note: This is somewhat ad-hoc and will probably all be replaced in a future
* version of the CML (I don't think I even bothered to document these functions
* on the website).
*/
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// N-d functions
//////////////////////////////////////////////////////////////////////////////
/** Return an N-d zero vector */
template < size_t N >
vector< double, fixed<N> > zero()
{
typedef vector< double, fixed<N> > vector_type;
vector_type result;
result.zero();
return result;
}
/** Return an N-d cardinal axis by index */
template < size_t N >
vector< double, fixed<N> > axis(size_t i)
{
/* Checking */
detail::CheckValidArg(i < N);
typedef vector< double, fixed<N> > vector_type;
vector_type result;
result.cardinal(i);
return result;
}
/** Return an NxM zero matrix */
template < size_t N, size_t M >
matrix< double, fixed<N,M>, row_basis, row_major > zero()
{
typedef matrix< double, fixed<N,M>, row_basis, row_major > matrix_type;
matrix_type result;
result.zero();
return result;
}
/** Return an NxN identity matrix */
template < size_t N >
matrix< double, fixed<N,N>, row_basis, row_major > identity()
{
typedef matrix< double, fixed<N,N>, row_basis, row_major > matrix_type;
matrix_type result;
result.identity();
return result;
}
/** Return an NxM identity transform */
template < size_t N, size_t M >
matrix< double, fixed<N,M>, row_basis, row_major > identity_transform()
{
typedef matrix< double, fixed<N,M>, row_basis, row_major > matrix_type;
matrix_type result;
identity_transform(result);
return result;
}
//////////////////////////////////////////////////////////////////////////////
// Zero vector
//////////////////////////////////////////////////////////////////////////////
/** Return the 2D zero vector */
inline vector< double, fixed<2> > zero_2D() {
return zero<2>();
}
/** Return the 3D zero vector */
inline vector< double, fixed<3> > zero_3D() {
return zero<3>();
}
/** Return the 4D zero vector */
inline vector< double, fixed<4> > zero_4D() {
return zero<4>();
}
//////////////////////////////////////////////////////////////////////////////
// Cardinal axis
//////////////////////////////////////////////////////////////////////////////
/** Return a 2D cardinal axis by index */
inline vector< double, fixed<2> > axis_2D(size_t i) {
return axis<2>(i);
}
/** Return a 3D cardinal axis by index */
inline vector< double, fixed<3> > axis_3D(size_t i) {
return axis<3>(i);
}
/** Return a the 2D x cardinal axis */
inline vector< double, fixed<2> > x_axis_2D() {
return axis_2D(0);
}
/** Return a the 2D y cardinal axis */
inline vector< double, fixed<2> > y_axis_2D() {
return axis_2D(1);
}
/** Return a the 3D x cardinal axis */
inline vector< double, fixed<3> > x_axis_3D() {
return axis_3D(0);
}
/** Return a the 3D y cardinal axis */
inline vector< double, fixed<3> > y_axis_3D() {
return axis_3D(1);
}
/** Return a the 3D z cardinal axis */
inline vector< double, fixed<3> > z_axis_3D() {
return axis_3D(2);
}
//////////////////////////////////////////////////////////////////////////////
// Zero matrix
//////////////////////////////////////////////////////////////////////////////
/** Return the 2x2 zero matrix */
inline matrix< double, fixed<2,2>, row_basis, row_major > zero_2x2() {
return zero<2,2>();
}
/** Return the 3x3 zero matrix */
inline matrix< double, fixed<3,3>, row_basis, row_major > zero_3x3() {
return zero<3,3>();
}
/** Return the 4x4 zero matrix */
inline matrix< double, fixed<4,4>, row_basis, row_major > zero_4x4() {
return zero<4,4>();
}
//////////////////////////////////////////////////////////////////////////////
// Identity matrix
//////////////////////////////////////////////////////////////////////////////
/** Return the 2x2 identity matrix */
inline matrix< double, fixed<2,2>, row_basis, row_major > identity_2x2() {
return identity<2>();
}
/** Return the 3x3 identity matrix */
inline matrix< double, fixed<3,3>, row_basis, row_major > identity_3x3() {
return identity<3>();
}
/** Return the 4x4 identity matrix */
inline matrix< double, fixed<4,4>, row_basis, row_major > identity_4x4() {
return identity<4>();
}
//////////////////////////////////////////////////////////////////////////////
// Identity transform matrix
//////////////////////////////////////////////////////////////////////////////
/** Return a 3x2 identity transform */
inline matrix< double,fixed<3,2>,row_basis,row_major > identity_transform_3x2() {
return identity_transform<3,2>();
}
/** Return a 2x3 identity transform */
inline matrix< double,fixed<2,3>,col_basis,col_major > identity_transform_2x3() {
return identity_transform<2,3>();
}
/** Return a 4x3 identity transform */
inline matrix< double,fixed<4,3>,row_basis,row_major > identity_transform_4x3() {
return identity_transform<4,3>();
}
/** Return a 3x4 identity transform */
inline matrix< double,fixed<3,4>,col_basis,col_major > identity_transform_3x4() {
return identity_transform<3,4>();
}
} // namespace cml
#endif

195
Lib/Include/CML/mathlib/picking.h Normal file
View File

@@ -0,0 +1,195 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef picking_h
#define picking_h
#include <cml/mathlib/projection.h>
/* Functions for picking with rays, volumes, and drag-enclosed volumes. */
namespace cml {
/* Support function for extracting the near and far depth range values from
* a viewport matrix.
*/
namespace detail {
// NOTE: Changed 'near' and 'far' to 'n' and 'f' to work around windows.h
// 'near' and 'far' macros.
template < class MatT, typename Real > void
depth_range_from_viewport_matrix(const MatT& viewport, Real& n, Real& f)
{
detail::CheckMatHomogeneous3D(viewport);
n = viewport.basis_element(3,2);
f = viewport.basis_element(2,2) + n;
}
} // namespace detail
/* Make a pick ray given screen coordinates and view, projection, and viewport
* matrices. The origin of the ray lies in the near plane of the frustum; the
* direction vector extends to the far plane if 'normalize' is false, and is
* made unit-length if 'normalize' is true (its default value).
*
* Note that the origin of the ray lies in the near plane rather than
* coinciding with the position of the virtual camera, as the latter gives
* incorrect results when the projection is orthographic.
*
* Note also that the screen y coordinate increases from bottom to top rather
* than top to bottom. If mouse coordinates are returned in window space where
* the y coordinate increases from top to bottom (as is often the case), the
* y value should be recomputed as 'y = <window height> - y' before being
* submitted to this function.
*/
template < class MatT_1, class MatT_2, class MatT_3, typename E, class A >
void make_pick_ray(
E pick_x,
E pick_y,
const MatT_1& view,
const MatT_2& projection,
const MatT_3& viewport,
vector<E,A>& origin,
vector<E,A>& direction,
bool normalize = true)
{
typedef vector<E,A> vector_type;
typedef typename vector_type::value_type value_type;
// NOTE: Changed 'near' and 'far' to 'n' and 'f' to work around
// windows.h 'near' and 'far' macros.
value_type n, f;
detail::depth_range_from_viewport_matrix(viewport, n, f);
origin =
unproject_point(
view,projection,viewport,vector_type(pick_x,pick_y,n)
);
direction =
unproject_point(
view,projection,viewport,vector_type(pick_x,pick_y,f)
) - origin;
if (normalize) {
direction.normalize();
}
}
/* Make a pick volume given the screen coordinates of the center of the
* picking rect, the width and height of the picking rect, and view and
* projection matrices.
*
* The volume is loaded into the 'planes' array. The planes are of the form
* ax+by+cz+d = 0, and are in the order left, right, bottom, top, near, far.
*
* The z_clip argument should be either z_clip_neg_one or z_clip_zero, and
* should correspond to the near z-clipping range of the projection matrix
* argument.
*
* The 'normalize' argument indicates whether the output planes should be
* normalized; its default value is 'true'.
*
* Note that the screen y coordinate increases from bottom to top rather
* than top to bottom. If mouse coordinates are returned in window space where
* the y coordinate increases from top to bottom (as is often the case), the
* y value should be recomputed as 'y = <window height> - y' before being
* submitted to this function.
*/
template < class MatT_1, class MatT_2, typename Real >
void make_pick_volume(
Real pick_x,
Real pick_y,
Real pick_width,
Real pick_height,
Real viewport_x,
Real viewport_y,
Real viewport_width,
Real viewport_height,
const MatT_1& view,
const MatT_2& projection,
Real planes[6][4],
ZClip z_clip,
bool normalize = true)
{
// FIXME: Should be promoted type...
typedef matrix<
Real, fixed<4,4>,
typename MatT_1::basis_orient, typename MatT_1::layout >
matrix_type;
matrix_type pick;
matrix_pick(
pick, pick_x, pick_y, pick_width, pick_height,
viewport_x, viewport_y, viewport_width, viewport_height
);
cml::extract_frustum_planes(
view,detail::matrix_concat_transforms_4x4(projection,pick),
planes,z_clip,normalize);
}
/* Make a pick volume given two opposite corners of a rectangle in screen
* space, and view and projection matrices. The corners of the screen rect
* need not be in any particular 'order' with regard to the values of the
* coordinates.
*
* The volume is loaded into the 'planes' array. The planes are of the form
* ax+by+cz+d = 0, and are in the order left, right, bottom, top, near, far.
*
* The z_clip argument should be either z_clip_neg_one or z_clip_zero, and
* should correspond to the near z-clipping range of the projection matrix
* argument.
*
* The 'normalize' argument indicates whether the output planes should be
* normalized; its default value is 'true'.
*
* Note that the screen y coordinate increases from bottom to top rather
* than top to bottom. If mouse coordinates are returned in window space where
* the y coordinate increases from top to bottom (as is often the case), the
* y value should be recomputed as 'y = <window height> - y' before being
* submitted to this function.
*/
template < class MatT_1, class MatT_2, typename Real >
void make_pick_drag_volume(
Real pick_x1,
Real pick_y1,
Real pick_x2,
Real pick_y2,
Real viewport_x,
Real viewport_y,
Real viewport_width,
Real viewport_height,
const MatT_1& view,
const MatT_2& projection,
Real planes[6][4],
ZClip z_clip,
bool normalize = true)
{
typedef Real value_type;
make_pick_volume(
(pick_x1+pick_x2)*value_type(.5),
(pick_y1+pick_y2)*value_type(.5),
std::fabs(pick_x2-pick_x1),
std::fabs(pick_y2-pick_y1),
viewport_x, viewport_y, viewport_width, viewport_height,
view, projection, planes, z_clip, normalize
);
}
} // namespace cml
#endif

142
Lib/Include/CML/mathlib/projection.h Normal file
View File

@@ -0,0 +1,142 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef projection_h
#define projection_h
#include <cml/mathlib/matrix_concat.h>
#include <cml/mathlib/vector_transform.h>
/* Functions for projection and 'unprojection' of points in 3D. */
namespace cml {
namespace detail {
template < typename E > void
divide_by_w(vector< E,fixed<4> >& v) {
v *= E(1) / v[3];
}
} // namespace detail
/* Project a point to screen space using the given model, view, projection,
* and viewport matrices. The z value of the returned point is a depth value
* in the range specified by the viewport matrix.
*/
template <class MatT_1, class MatT_2, class MatT_3, class MatT_4, class VecT>
vector< typename VecT::value_type, fixed<3> > project_point(
const MatT_1& model,
const MatT_2& view,
const MatT_3& projection,
const MatT_4& viewport,
const VecT& p)
{
return project_point(
detail::matrix_concat_transforms_4x4(model,view),
projection,
viewport,
p
);
}
/* Project a point to screen space using the given modelview, projection, and
* viewport matrices. The z value of the returned point is a depth value in
* the range specified by the viewport matrix.
*/
template < class MatT_1, class MatT_2, class MatT_3, class VecT >
vector< typename VecT::value_type, fixed<3> > project_point(
const MatT_1& modelview,
const MatT_2& projection,
const MatT_3& viewport,
const VecT& p)
{
typedef vector< typename VecT::value_type, fixed<3> > vector3_type;
typedef vector< typename VecT::value_type, fixed<4> > vector4_type;
typedef typename vector3_type::value_type value_type;
detail::CheckVec3(p);
vector4_type result = transform_vector_4D(
detail::matrix_concat_transforms_4x4(
modelview,
detail::matrix_concat_transforms_4x4(
projection,
viewport
)
),
vector4_type(p[0],p[1],p[2],value_type(1))
);
detail::divide_by_w(result);
return vector3_type(result[0],result[1],result[2]);
}
/* 'Unproject' a point from screen space using the given model, view,
* projection, and viewport matrices. The z value of the input point is a
* depth value in the range specified by the viewport matrix.
*/
template <class MatT_1, class MatT_2, class MatT_3, class MatT_4, class VecT>
vector< typename VecT::value_type, fixed<3> > unproject_point(
const MatT_1& model,
const MatT_2& view,
const MatT_3& projection,
const MatT_4& viewport,
const VecT& p)
{
return unproject_point(
detail::matrix_concat_transforms_4x4(model,view),
projection,
viewport,
p
);
}
/* 'Unproject' a point from screen space using the given modelview,
* projection, and viewport matrices. The z value of the input point is a
* depth value in the range specified by the viewport matrix.
*/
template < class MatT_1, class MatT_2, class MatT_3, class VecT >
vector< typename VecT::value_type, fixed<3> > unproject_point(
const MatT_1& modelview,
const MatT_2& projection,
const MatT_3& viewport,
const VecT& p)
{
typedef vector< typename VecT::value_type, fixed<3> > vector3_type;
typedef vector< typename VecT::value_type, fixed<4> > vector4_type;
typedef typename vector3_type::value_type value_type;
detail::CheckVec3(p);
vector4_type result = transform_vector_4D(
inverse(
detail::matrix_concat_transforms_4x4(
modelview,
detail::matrix_concat_transforms_4x4(
projection,
viewport
)
)
),
vector4_type(p[0],p[1],p[2],value_type(1))
);
detail::divide_by_w(result);
return vector3_type(result[0],result[1],result[2]);
}
} // namespace cml
#endif

89
Lib/Include/CML/mathlib/quaternion_basis.h Normal file
View File

@@ -0,0 +1,89 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef quaternion_basis_h
#define quaternion_basis_h
#include <cml/mathlib/checking.h>
/* Functions for getting the basis vectors of a quaternion rotation. */
namespace cml {
/** Get the i'th basis vector of a quaternion rotation */
template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
quaternion_get_basis_vector(const QuatT& q, size_t i)
{
typedef QuatT quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
typedef vector< value_type, fixed<3> > vector_type;
/* Checking */
detail::CheckQuat(q);
detail::CheckIndex3(i);
size_t j, k;
cyclic_permutation(i, i, j, k);
/* @todo: Clean this up. */
const size_t W = order_type::W;
const size_t I = order_type::X + i;
const size_t J = order_type::X + j;
const size_t K = order_type::X + k;
value_type j2 = q[J] + q[J];
value_type k2 = q[K] + q[K];
/* @todo: use set_permuted() for the following when available. */
vector_type result;
result[i] = value_type(1) - q[J] * j2 - q[K] * k2;
result[j] = q[I] * j2 + q[W] * k2;
result[k] = q[I] * k2 - q[W] * j2;
return result;
}
/** Get the x basis vector of a quaternion rotation */
template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
quaternion_get_x_basis_vector(const QuatT& q) {
return quaternion_get_basis_vector(q,0);
}
/** Get the y basis vector of a quaternion rotation */
template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
quaternion_get_y_basis_vector(const QuatT& q) {
return quaternion_get_basis_vector(q,1);
}
/** Get the z basis vector of a quaternion rotation */
template < class QuatT > vector< typename QuatT::value_type, fixed<3> >
quaternion_get_z_basis_vector(const QuatT& q) {
return quaternion_get_basis_vector(q,2);
}
/** Get the basis vectors of a quaternion rotation */
template < class QuatT, typename E, class A > void
quaternion_get_basis_vectors(
const QuatT& q,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z)
{
x = quaternion_get_x_basis_vector(q);
y = quaternion_get_y_basis_vector(q);
z = quaternion_get_z_basis_vector(q);
}
} // namespace cml
#endif

635
Lib/Include/CML/mathlib/quaternion_rotation.h Normal file
View File

@@ -0,0 +1,635 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef quaternion_rotation_h
#define quaternion_rotation_h
#include <cml/mathlib/checking.h>
/* Functions related to quaternion rotations.
*
* Note: A number of these functions simply wrap calls to the corresponding
* matrix functions. Some of them (the 'aim-at' and 'align' functions in
* particular) might be considered a bit superfluous, since the resulting
* quaternion will most likely be converted to a matrix at some point anyway.
* However, they're included here for completeness, and for convenience in
* cases where a quaternion is being used as the primary rotation
* representation.
*/
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// Rotation about world axes
//////////////////////////////////////////////////////////////////////////////
/** Build a quaternion representing a rotation about the given world axis */
template < class E, class A, class O, class C > void
quaternion_rotation_world_axis(quaternion<E,A,O,C>& q, size_t axis, E angle)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
/* Checking */
detail::CheckIndex3(axis);
q.identity();
const size_t W = order_type::W;
const size_t I = order_type::X + axis;
angle *= value_type(.5);
q[I] = std::sin(angle);
q[W] = std::cos(angle);
}
/** Build a quaternion representing a rotation about the world x axis */
template < class E, class A, class O, class C > void
quaternion_rotation_world_x(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotation_world_axis(q,0,angle);
}
/** Build a quaternion representing a rotation about the world y axis */
template < class E, class A, class O, class C > void
quaternion_rotation_world_y(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotation_world_axis(q,1,angle);
}
/** Build a quaternion representing a rotation about the world z axis */
template < class E, class A, class O, class C > void
quaternion_rotation_world_z(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotation_world_axis(q,2,angle);
}
//////////////////////////////////////////////////////////////////////////////
// Rotation from an axis-angle pair
//////////////////////////////////////////////////////////////////////////////
/** Build a quaternion from an axis-angle pair */
template < class E, class A, class O, class C, class VecT > void
quaternion_rotation_axis_angle(
quaternion<E,A,O,C>& q, const VecT& axis, E angle)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
/* Checking */
detail::CheckVec3(axis);
enum {
W = order_type::W,
X = order_type::X,
Y = order_type::Y,
Z = order_type::Z
};
angle *= value_type(.5);
/* @todo: If and when we have a set() function that takes a vector and a
* scalar, this can be written as:
*
* q.set(std::cos(angle), axis * std::sin(angle));
*
* In which case the enum will also not be necessary.
*/
q[W] = std::cos(angle);
value_type s = std::sin(angle);
q[X] = axis[0] * s;
q[Y] = axis[1] * s;
q[Z] = axis[2] * s;
}
//////////////////////////////////////////////////////////////////////////////
// Rotation from a matrix
//////////////////////////////////////////////////////////////////////////////
/** Build a quaternion from a rotation matrix */
template < class E, class A, class O, class C, class MatT > void
quaternion_rotation_matrix(quaternion<E,A,O,C>& q, const MatT& m)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
/* Checking */
detail::CheckMatLinear3D(m);
enum {
W = order_type::W,
X = order_type::X,
Y = order_type::Y,
Z = order_type::Z
};
value_type tr = trace_3x3(m);
if (tr >= value_type(0)) {
q[W] = std::sqrt(tr + value_type(1)) * value_type(.5);
value_type s = value_type(.25) / q[W];
q[X] = (m.basis_element(1,2) - m.basis_element(2,1)) * s;
q[Y] = (m.basis_element(2,0) - m.basis_element(0,2)) * s;
q[Z] = (m.basis_element(0,1) - m.basis_element(1,0)) * s;
} else {
size_t largest_diagonal_element =
index_of_max(
m.basis_element(0,0),
m.basis_element(1,1),
m.basis_element(2,2)
);
size_t i, j, k;
cyclic_permutation(largest_diagonal_element, i, j, k);
const size_t I = X + i;
const size_t J = X + j;
const size_t K = X + k;
q[I] =
std::sqrt(
m.basis_element(i,i) -
m.basis_element(j,j) -
m.basis_element(k,k) +
value_type(1)
) * value_type(.5);
value_type s = value_type(.25) / q[I];
q[J] = (m.basis_element(i,j) + m.basis_element(j,i)) * s;
q[K] = (m.basis_element(i,k) + m.basis_element(k,i)) * s;
q[W] = (m.basis_element(j,k) - m.basis_element(k,j)) * s;
}
}
//////////////////////////////////////////////////////////////////////////////
// Rotation from Euler angles
//////////////////////////////////////////////////////////////////////////////
/** Build a quaternion from an Euler-angle triple */
template < class E, class A, class O, class C > void
quaternion_rotation_euler(
quaternion<E,A,O,C>& q, E angle_0, E angle_1, E angle_2,
EulerOrder order)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
size_t i, j, k;
bool odd, repeat;
detail::unpack_euler_order(order, i, j, k, odd, repeat);
const size_t W = order_type::W;
const size_t I = order_type::X + i;
const size_t J = order_type::X + j;
const size_t K = order_type::X + k;
if (odd) {
angle_1 = -angle_1;
}
angle_0 *= value_type(.5);
angle_1 *= value_type(.5);
angle_2 *= value_type(.5);
value_type s0 = std::sin(angle_0);
value_type c0 = std::cos(angle_0);
value_type s1 = std::sin(angle_1);
value_type c1 = std::cos(angle_1);
value_type s2 = std::sin(angle_2);
value_type c2 = std::cos(angle_2);
value_type s0s2 = s0 * s2;
value_type s0c2 = s0 * c2;
value_type c0s2 = c0 * s2;
value_type c0c2 = c0 * c2;
if (repeat) {
q[I] = c1 * (c0s2 + s0c2);
q[J] = s1 * (c0c2 + s0s2);
q[K] = s1 * (c0s2 - s0c2);
q[W] = c1 * (c0c2 - s0s2);
} else {
q[I] = c1 * s0c2 - s1 * c0s2;
q[J] = c1 * s0s2 + s1 * c0c2;
q[K] = c1 * c0s2 - s1 * s0c2;
q[W] = c1 * c0c2 + s1 * s0s2;
}
if (odd) {
q[J] = -q[J];
}
}
//////////////////////////////////////////////////////////////////////////////
// Rotation to align with a vector, multiple vectors, or the view plane
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
quaternion_rotation_align(
quaternion<E,A,O,C>& q,
const VecT_1& align,
const VecT_2& reference,
bool normalize = true,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_align(m,align,reference,normalize,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C, class VecT > void
quaternion_rotation_align(quaternion<E,A,O,C>& q, const VecT& align,
bool normalize = true, AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_align(m,align,normalize,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
quaternion_rotation_align_axial(quaternion<E,A,O,C>& q, const VecT_1& align,
const VecT_2& axis, bool normalize = true,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_align_axial(m,align,axis,normalize,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C, class MatT > void
quaternion_rotation_align_viewplane(
quaternion<E,A,O,C>& q,
const MatT& view_matrix,
Handedness handedness,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_align_viewplane(m,view_matrix,handedness,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C, class MatT > void
quaternion_rotation_align_viewplane_LH(
quaternion<E,A,O,C>& q,
const MatT& view_matrix,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_align_viewplane_LH(m,view_matrix,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C, class MatT > void
quaternion_rotation_align_viewplane_RH(
quaternion<E,A,O,C>& q,
const MatT& view_matrix,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_align_viewplane_RH(m,view_matrix,order);
quaternion_rotation_matrix(q,m);
}
//////////////////////////////////////////////////////////////////////////////
// Rotation to aim at a target
//////////////////////////////////////////////////////////////////////////////
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C,
class VecT_1, class VecT_2, class VecT_3 > void
quaternion_rotation_aim_at(
quaternion<E,A,O,C>& q,
const VecT_1& pos,
const VecT_2& target,
const VecT_3& reference,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_aim_at(m,pos,target,reference,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C,
class VecT_1, class VecT_2 > void
quaternion_rotation_aim_at(
quaternion<E,A,O,C>& q,
const VecT_1& pos,
const VecT_2& target,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_aim_at(m,pos,target,order);
quaternion_rotation_matrix(q,m);
}
/** See vector_ortho.h for details */
template < typename E, class A, class O, class C,
class VecT_1, class VecT_2, class VecT_3 > void
quaternion_rotation_aim_at_axial(
quaternion<E,A,O,C>& q,
const VecT_1& pos,
const VecT_2& target,
const VecT_3& axis,
AxisOrder order = axis_order_zyx)
{
typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_aim_at_axial(m,pos,target,axis,order);
quaternion_rotation_matrix(q,m);
}
//////////////////////////////////////////////////////////////////////////////
// Relative rotation about world axes
//////////////////////////////////////////////////////////////////////////////
/* Rotate a quaternion about the given world axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_world_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
/* Checking */
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
const size_t W = order_type::W;
const size_t I = order_type::X + i;
const size_t J = order_type::X + j;
const size_t K = order_type::X + k;
angle *= value_type(.5);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
quaternion_type result;
result[I] = c * q[I] + s * q[W];
result[J] = c * q[J] - s * q[K];
result[K] = c * q[K] + s * q[J];
result[W] = c * q[W] - s * q[I];
q = result;
}
/* Rotate a quaternion about the world x axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_world_x(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotate_about_world_axis(q,0,angle);
}
/* Rotate a quaternion about the world y axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_world_y(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotate_about_world_axis(q,1,angle);
}
/* Rotate a quaternion about the world z axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_world_z(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotate_about_world_axis(q,2,angle);
}
//////////////////////////////////////////////////////////////////////////////
// Relative rotation about local axes
//////////////////////////////////////////////////////////////////////////////
/* Rotate a quaternion about the given local axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_local_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
/* Checking */
detail::CheckIndex3(axis);
size_t i, j, k;
cyclic_permutation(axis, i, j, k);
const size_t W = order_type::W;
const size_t I = order_type::X + i;
const size_t J = order_type::X + j;
const size_t K = order_type::X + k;
angle *= value_type(.5);
value_type s = value_type(std::sin(angle));
value_type c = value_type(std::cos(angle));
quaternion_type result;
result[I] = c * q[I] + s * q[W];
result[J] = c * q[J] + s * q[K];
result[K] = c * q[K] - s * q[J];
result[W] = c * q[W] - s * q[I];
q = result;
}
/* Rotate a quaternion about its local x axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_local_x(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotate_about_local_axis(q,0,angle);
}
/* Rotate a quaternion about its local y axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_local_y(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotate_about_local_axis(q,1,angle);
}
/* Rotate a quaternion about its local z axis */
template < class E, class A, class O, class C > void
quaternion_rotate_about_local_z(quaternion<E,A,O,C>& q, E angle) {
quaternion_rotate_about_local_axis(q,2,angle);
}
//////////////////////////////////////////////////////////////////////////////
// Rotation from vector to vector
//////////////////////////////////////////////////////////////////////////////
/* http://www.martinb.com/maths/algebra/vectors/angleBetween/index.htm. */
/** Build a quaternion to rotate from one vector to another */
template < class E,class A,class O,class C,class VecT_1,class VecT_2 > void
quaternion_rotation_vec_to_vec(
quaternion<E,A,O,C>& q,
const VecT_1& v1,
const VecT_2& v2,
bool unit_length_vectors = false)
{
typedef quaternion<E,A,O,C> quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef vector< value_type, fixed<3> > vector_type;
/* Checking handled by cross() */
/* @todo: If at some point quaternion<> has a set() function that takes a
* vector and a scalar, this can then be written as:
*
* if (...) {
* q.set(value_type(1)+dot(v1,v2), cross(v1,v2));
* } else {
* q.set(std::sqrt(...)+dot(v1,v2), cross(v1,v2));
* }
*/
vector_type c = cross(v1,v2);
if (unit_length_vectors) {
q = quaternion_type(value_type(1) + dot(v1,v2), c.data());
} else {
q = quaternion_type(
std::sqrt(v1.length_squared() * v2.length_squared()) + dot(v1,v2),
c/*.data()*/
);
}
q.normalize();
}
//////////////////////////////////////////////////////////////////////////////
// Scale the angle of a rotation matrix
//////////////////////////////////////////////////////////////////////////////
template < typename E, class A, class O, class C > void
quaternion_scale_angle(quaternion<E,A,O,C>& q, E t,
E tolerance = epsilon<E>::placeholder())
{
typedef vector< E,fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
vector_type axis;
value_type angle;
quaternion_to_axis_angle(q, axis, angle, tolerance);
quaternion_rotation_axis_angle(q, axis, angle * t);
}
//////////////////////////////////////////////////////////////////////////////
// Support functions for uniform handling of pos- and neg-cross quaternions
//////////////////////////////////////////////////////////////////////////////
namespace detail {
/** Concatenate two quaternions in the order q1->q2 */
template < class QuatT_1, class QuatT_2 >
typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
quaternion_rotation_difference(
const QuatT_1& q1, const QuatT_2& q2, positive_cross)
{
return q2 * conjugate(q1);
}
/** Concatenate two quaternions in the order q1->q2 */
template < class QuatT_1, class QuatT_2 >
typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
quaternion_rotation_difference(
const QuatT_1& q1, const QuatT_2& q2, negative_cross)
{
return conjugate(q1) * q2;
}
} // namespace detail
//////////////////////////////////////////////////////////////////////////////
// Quaternions rotation difference
//////////////////////////////////////////////////////////////////////////////
/** Return the rotational 'difference' between two quaternions */
template < class QuatT_1, class QuatT_2 >
typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
quaternion_rotation_difference(const QuatT_1& q1, const QuatT_2& q2) {
return detail::quaternion_rotation_difference(
q1, q2, typename QuatT_1::cross_type());
}
//////////////////////////////////////////////////////////////////////////////
// Conversions
//////////////////////////////////////////////////////////////////////////////
/** Convert a quaternion to an axis-angle pair */
template < class QuatT, typename E, class A > void
quaternion_to_axis_angle(
const QuatT& q,
vector<E,A>& axis,
E& angle,
E tolerance = epsilon<E>::placeholder())
{
typedef QuatT quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef typename quaternion_type::order_type order_type;
/* Checking */
detail::CheckQuat(q);
axis = q.imaginary();
value_type l = length(axis);
if (l > tolerance) {
axis /= l;
angle = value_type(2) * std::atan2(l,q.real());
} else {
axis.zero();
angle = value_type(0);
}
}
/** Convert a quaternion to an Euler-angle triple
*
* Note: I've implemented direct quaternion-to-Euler conversion, but as far as
* I can tell it more or less reduces to converting the quaternion to a matrix
* as you go. The direct method is a little more efficient in that it doesn't
* require a temporary and only the necessary matrix elements need be
* computed. However, the implementation is complex and there's considerable
* opportunity for error, so from a development and debugging standpoint I
* think it's better to just perform the conversion via matrix_to_euler(),
* which is already known to be correct.
*/
template < class QuatT, typename Real > void
quaternion_to_euler(
const QuatT& q,
Real& angle_0,
Real& angle_1,
Real& angle_2,
EulerOrder order,
Real tolerance = epsilon<Real>::placeholder())
{
typedef QuatT quaternion_type;
typedef typename quaternion_type::value_type value_type;
typedef matrix< value_type,fixed<3,3>,row_basis,row_major > matrix_type;
matrix_type m;
matrix_rotation_quaternion(m, q);
matrix_to_euler(m, angle_0, angle_1, angle_2, order, tolerance);
}
} // namespace cml
#endif

134
Lib/Include/CML/mathlib/typedef.h Normal file
View File

@@ -0,0 +1,134 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef typedef_h
#define typedef_h
#include <cml/vector.h>
#include <cml/matrix.h>
#include <cml/quaternion.h>
#include <cml/constants.h>
#include <cml/mathlib/epsilon.h>
namespace cml {
/* fixed-size vectors */
typedef vector< int, fixed<2> > vector2i;
typedef vector< float, fixed<2> > vector2f;
typedef vector< double, fixed<2> > vector2d;
typedef vector< int, fixed<3> > vector3i;
typedef vector< float, fixed<3> > vector3f;
typedef vector< double, fixed<3> > vector3d;
typedef vector< int, fixed<4> > vector4i;
typedef vector< float, fixed<4> > vector4f;
typedef vector< double, fixed<4> > vector4d;
/* fixed-size matrices */
typedef matrix< int, fixed<2,2> > matrix22i;
typedef matrix< float, fixed<2,2> > matrix22f;
typedef matrix< double, fixed<2,2> > matrix22d;
typedef matrix< int, fixed<2,2>, row_basis, row_major > matrix22i_r;
typedef matrix< int, fixed<2,2>, col_basis, col_major > matrix22i_c;
typedef matrix< float, fixed<2,2>, row_basis, row_major > matrix22f_r;
typedef matrix< float, fixed<2,2>, col_basis, col_major > matrix22f_c;
typedef matrix< double, fixed<2,2>, row_basis, row_major > matrix22d_r;
typedef matrix< double, fixed<2,2>, col_basis, col_major > matrix22d_c;
typedef matrix< int, fixed<3,3> > matrix33i;
typedef matrix< float, fixed<3,3> > matrix33f;
typedef matrix< double, fixed<3,3> > matrix33d;
typedef matrix< int, fixed<3,3>, row_basis, row_major > matrix33i_r;
typedef matrix< int, fixed<3,3>, col_basis, col_major > matrix33i_c;
typedef matrix< float, fixed<3,3>, row_basis, row_major > matrix33f_r;
typedef matrix< float, fixed<3,3>, col_basis, col_major > matrix33f_c;
typedef matrix< double, fixed<3,3>, row_basis, row_major > matrix33d_r;
typedef matrix< double, fixed<3,3>, col_basis, col_major > matrix33d_c;
typedef matrix< int, fixed<4,4> > matrix44i;
typedef matrix< float, fixed<4,4> > matrix44f;
typedef matrix< double, fixed<4,4> > matrix44d;
typedef matrix< int, fixed<4,4>, row_basis, row_major > matrix44i_r;
typedef matrix< int, fixed<4,4>, col_basis, col_major > matrix44i_c;
typedef matrix< float, fixed<4,4>, row_basis, row_major > matrix44f_r;
typedef matrix< float, fixed<4,4>, col_basis, col_major > matrix44f_c;
typedef matrix< double, fixed<4,4>, row_basis, row_major > matrix44d_r;
typedef matrix< double, fixed<4,4>, col_basis, col_major > matrix44d_c;
typedef matrix< int, fixed<3,2>, row_basis, row_major > matrix32i_r;
typedef matrix< float, fixed<3,2>, row_basis, row_major > matrix32f_r;
typedef matrix< double, fixed<3,2>, row_basis, row_major > matrix32d_r;
typedef matrix< int, fixed<2,3>, col_basis, col_major > matrix23i_c;
typedef matrix< float, fixed<2,3>, col_basis, col_major > matrix23f_c;
typedef matrix< double, fixed<2,3>, col_basis, col_major > matrix23d_c;
typedef matrix< int, fixed<4,3>, row_basis, row_major > matrix43i_r;
typedef matrix< float, fixed<4,3>, row_basis, row_major > matrix43f_r;
typedef matrix< double, fixed<4,3>, row_basis, row_major > matrix43d_r;
typedef matrix< int, fixed<3,4>, col_basis, col_major > matrix34i_c;
typedef matrix< float, fixed<3,4>, col_basis, col_major > matrix34f_c;
typedef matrix< double, fixed<3,4>, col_basis, col_major > matrix34d_c;
/* quaternions */
typedef quaternion<float, fixed<>,vector_first,negative_cross>
quaternionf_n;
typedef quaternion<float, fixed<>,vector_first,positive_cross>
quaternionf_p;
typedef quaternion<double,fixed<>,vector_first,negative_cross>
quaterniond_n;
typedef quaternion<double,fixed<>,vector_first,positive_cross>
quaterniond_p;
typedef quaternion<float> quaternionf;
typedef quaternion<double> quaterniond;
/* dynamically resizable vectors */
typedef vector< int, dynamic<> > vectori;
typedef vector< float, dynamic<> > vectorf;
typedef vector< double, dynamic<> > vectord;
/* dynamically resizable matrices */
typedef matrix< int, dynamic<> > matrixi;
typedef matrix< float, dynamic<> > matrixf;
typedef matrix< double, dynamic<> > matrixd;
typedef matrix< int, dynamic<>, row_basis, row_major > matrixi_r;
typedef matrix< int, dynamic<>, col_basis, col_major > matrixi_c;
typedef matrix< float, dynamic<>, row_basis, row_major > matrixf_r;
typedef matrix< float, dynamic<>, col_basis, col_major > matrixf_c;
typedef matrix< double, dynamic<>, row_basis, row_major > matrixd_r;
typedef matrix< double, dynamic<>, col_basis, col_major > matrixd_c;
/* constants */
typedef constants<float> constantsf;
typedef constants<double> constantsd;
/* epsilon/tolerance values (placeholder) */
typedef epsilon<float> epsilonf;
typedef epsilon<double> epsilond;
} // namespace cml
#endif

69
Lib/Include/CML/mathlib/vector_angle.h Normal file
View File

@@ -0,0 +1,69 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef vector_angle_h
#define vector_angle_h
#include <cml/mathlib/checking.h>
/* Functions for finding the signed and unsigned angles between vectors in
* 3D and 2D.
*
* Note that the input vectors for these functions are not required to be
* unit length.
*
* @todo: Clean up promotions, conversions, and return types.
*/
namespace cml {
/** Signed angle between two 3D vectors. */
template< class VecT_1, class VecT_2, class VecT_3 >
typename detail::DotPromote<
typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector, VecT_3
>::promoted_scalar
signed_angle(const VecT_1& v1, const VecT_2& v2, const VecT_3& reference)
{
typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
vector_type;
typedef typename detail::DotPromote<vector_type,VecT_3>::promoted_scalar
value_type;
vector_type c = cross(v1,v2);
value_type angle = std::atan2(double(length(c)),double(dot(v1,v2)));
return dot(c,reference) < value_type(0) ? -angle : angle;
}
/** Unsigned angle between two 3D vectors. */
template< class VecT_1, class VecT_2 >
typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
unsigned_angle(const VecT_1& v1, const VecT_2& v2) {
return std::atan2(double(length(cross(v1,v2))),double(dot(v1,v2)));
}
/** Signed angle between two 2D vectors. */
template< class VecT_1, class VecT_2 >
typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
signed_angle_2D(const VecT_1& v1, const VecT_2& v2) {
return std::atan2(double(perp_dot(v1,v2)),double(dot(v1,v2)));
}
/** Unsigned angle between two 2D vectors. */
template< class VecT_1, class VecT_2 >
typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
unsigned_angle_2D(const VecT_1& v1, const VecT_2& v2) {
return std::fabs(signed_angle_2D(v1,v2));
}
} // namespace cml
#endif

304
Lib/Include/CML/mathlib/vector_misc.h Normal file
View File

@@ -0,0 +1,304 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef vector_misc_h
#define vector_misc_h
#include <cml/mathlib/coord_conversion.h>
/* Miscellaneous vector functions. */
namespace cml {
/* Function to project a vector v onto a hyperplane specified by a unit-length
* normal n.
*
* @todo: Clean up promotion code.
*/
template < class VecT_1, class VecT_2 >
typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
project_to_hplane(const VecT_1& v, const VecT_2& n)
{
typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
result_type;
result_type result;
et::detail::Resize(result, v.size());
result = v - dot(v,n) * n;
return result;
}
/* Return a vector perpendicular (CCW) to a 2D vector. */
template < class VecT > vector< typename VecT::value_type, fixed<2> >
perp(const VecT& v)
{
typedef vector< typename VecT::value_type, fixed<2> > temporary_type;
/* Checking */
detail::CheckVec2(v);
return temporary_type(-v[1],v[0]);
}
/* @todo: unit_cross() and cross_cardinal() should probably go in
* vector_products.h, but I'm trying to avoid modifying the existing codebase
* for now.
*/
/** Return normalized cross product of two vectors */
template< class LeftT, class RightT >
typename detail::CrossPromote<LeftT,RightT>::promoted_vector
unit_cross(const LeftT& left, const RightT& right) {
/* @todo: This will probably break with dynamic<> vectors */
return normalize(cross(left,right));
}
/** Return the cross product of v and the i'th cardinal basis vector */
template < class VecT > vector< typename VecT::value_type, fixed<3> >
cross_cardinal(const VecT& v, size_t i)
{
typedef vector< typename VecT::value_type, fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckIndex3(i);
size_t j, k;
cyclic_permutation(i, i, j, k);
vector_type result;
result[i] = value_type(0);
result[j] = v[k];
result[k] = -v[j];
return result;
}
/** Return the cross product of the i'th cardinal basis vector and v */
template < class VecT > vector< typename VecT::value_type, fixed<3> >
cross_cardinal(size_t i, const VecT& v)
{
typedef vector< typename VecT::value_type, fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckIndex3(i);
size_t j, k;
cyclic_permutation(i, i, j, k);
vector_type result;
result[i] = value_type(0);
result[j] = -v[k];
result[k] = v[j];
return result;
}
/** Rotate a 3D vector v about a unit-length vector n. */
template< class VecT_1, class VecT_2, typename Real >
vector<
typename et::ScalarPromote<
typename VecT_1::value_type,
typename VecT_2::value_type
>::type,
fixed<3>
>
rotate_vector(const VecT_1& v, const VecT_2& n, Real angle)
{
typedef vector<
typename et::ScalarPromote<
typename VecT_1::value_type,
typename VecT_2::value_type
>::type,
fixed<3>
> result_type;
/* Checking */
detail::CheckVec3(v);
detail::CheckVec3(n);
result_type parallel = dot(v,n)*n;
return (
std::cos(angle)*(v-parallel) + std::sin(angle)*cross(n,v) + parallel
);
}
/** Rotate a 2D vector v about a unit-length vector n. */
template< class VecT, typename Real >
vector< typename VecT::value_type, fixed<2> >
rotate_vector_2D(const VecT& v, Real angle)
{
typedef vector< typename VecT::value_type, fixed<2> > result_type;
typedef typename result_type::value_type value_type;
/* Checking */
detail::CheckVec2(v);
value_type s = std::sin(angle);
value_type c = std::cos(angle);
return result_type(c * v[0] - s * v[1], s * v[0] + c * v[1]);
}
/** Random unit 3D or 2D vector
*
* @todo: This is just placeholder code for what will be a more thorough
* 'random unit' implementation:
*
* - All dimensions will be handled uniformly if practical, perhaps through
* a normal distrubution PRNG.
*
* - Failing that (or perhaps even in this case), dimensions 2 and 3 will be
* dispatched to special-case code, most likely implementing the algorithms
* below.
*
* - Like the utility random functions, the option of using one's own PRGN
* will be made available.
*
* @todo: Once N-d random vectors are supported, add a 'random unit
* quaternion' function that wraps a call to random_unit() with a 4D vector as
* the argument.
*/
template < typename E, class A > void
random_unit(vector<E,A>& v)
{
typedef vector<E,A> vector_type;
typedef typename vector_type::value_type value_type;
switch (v.size()) {
case 3:
{
vector< E, fixed<3> > temp;
spherical_to_cartesian(
value_type(1),
value_type(random_unit() * constants<value_type>::two_pi()),
acos_safe(random_real(value_type(-1),value_type(1))),
2,
colatitude,
temp
);
v[0] = temp[0];
v[1] = temp[1];
v[2] = temp[2];
break;
}
case 2:
{
vector< E, fixed<2> > temp;
polar_to_cartesian(
value_type(1),
value_type(random_unit() * constants<value_type>::two_pi()),
temp
);
v[0] = temp[0];
v[1] = temp[1];
break;
}
default:
throw std::invalid_argument(
"random_unit() for N-d vectors not implemented yet");
break;
}
}
/* Random vector within a given angle of a unit-length axis, i.e. in a cone
* (3D) or wedge (2D).
*
* The same notes the appear above apply here too, more or less. One
* difference is that this is really only useful in 2D and 3D (presumably), so
* we'll probably just do a compile- or run-time dispatch as appropriate.
*
* Also, there may be a better algorithm for generating a random unit vector
* in a cone; need to look into that.
*
* All of this 'temp' stuff is because there's no compile-time dispatch for
* 3D and 2D vectors, but that'll be fixed soon.
*/
template < typename E, class A, class VecT > void
random_unit(vector<E,A>& v, const VecT& axis, E theta)
{
typedef vector<E,A> vector_type;
typedef typename vector_type::value_type value_type;
switch (v.size()) {
case 3:
{
vector< E, fixed<3> > temp, n, temp_axis;
temp_axis[0] = axis[0];
temp_axis[1] = axis[1];
temp_axis[2] = axis[2];
/* @todo: Function for finding 'any perpendicular vector'? */
n = axis_3D(cml::index_of_min_abs(axis[0],axis[1],axis[2]));
n = cross(n,temp_axis);
/* Rotate v 'away from' the axis by a random angle in the range
* [-theta,theta]
*/
temp = rotate_vector(temp_axis,n,random_real(-theta,theta));
/* Rotate v about the axis by a random angle in the range [-pi,pi]
*/
temp = rotate_vector(
temp,
temp_axis,
random_real(
-constants<value_type>::pi(),
constants<value_type>::pi()
)
);
v[0] = temp[0];
v[1] = temp[1];
v[2] = temp[2];
break;
}
case 2:
{
vector< E, fixed<2> > temp, temp_axis;
temp_axis[0] = axis[0];
temp_axis[1] = axis[1];
temp = rotate_vector_2D(temp_axis, random_real(-theta,theta));
v[0] = temp[0];
v[1] = temp[1];
break;
}
default:
throw std::invalid_argument(
"random_unit(v,axis,theta) only implemented for 2D and 3D");
break;
}
}
/* NEW: Manhattan distance */
template< class VecT_1, class VecT_2 >
typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
manhattan_distance(const VecT_1& v1, const VecT_2& v2) {
/* Check that a promotion exists */
typedef typename et::VectorPromote<
VecT_1,VecT_2>::temporary_type promoted_vector;
typedef typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar scalar_type;
scalar_type sum = scalar_type(0);
for (size_t i = 0; i < v1.size(); ++i) {
sum += std::fabs(v2[i]-v1[i]);
}
return sum;
}
} // namespace cml
#endif

338
Lib/Include/CML/mathlib/vector_ortho.h Normal file
View File

@@ -0,0 +1,338 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
*
* Functions for orthonormalizing a set of basis vectors in 3D or 2D, and for
* constructing an orthonormal basis given various input parameters.
*/
#ifndef vector_ortho_h
#define vector_ortho_h
#include <cml/mathlib/vector_misc.h>
#include <cml/mathlib/misc.h>
namespace cml {
//////////////////////////////////////////////////////////////////////////////
// Orthonormalization in 3D and 2D
//////////////////////////////////////////////////////////////////////////////
/** Orthonormalize 3 basis vectors in R3.
*
* Called with the default values, this function performs a single Gram-
* Schmidt step to orthonormalize the input vectors. By default, the direction
* of the 3rd basis vector is unchanged by this operation, but the unaffected
* axis can be specified via the 'stable_axis' parameter.
*
* The arguments 'num_iter' and 's' can be specified to an iterative Gram-
* Schmidt step. 'num_iter' is the number of iterations applied, and 's' is
* the fraction applied towards orthonormality each step.
*
* In most cases, the default arguments can be ignored, leaving only the three
* input vectors.
*/
template < typename E, class A > void
orthonormalize(vector<E,A>& v0, vector<E,A>& v1, vector<E,A>& v2,
size_t stable_axis = 2, size_t num_iter = 0, E s = E(1))
{
/* Checking */
detail::CheckVec3(v0);
detail::CheckVec3(v1);
detail::CheckVec3(v2);
detail::CheckIndex3(stable_axis);
typedef vector< E, fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
/* Iterative Gram-Schmidt; this step is skipped by default. */
for (size_t i = 0; i < num_iter; ++i) {
value_type dot01 = dot(v0,v1);
value_type dot12 = dot(v1,v2);
value_type dot20 = dot(v2,v0);
value_type inv_dot00 = value_type(1) / dot(v0,v0);
value_type inv_dot11 = value_type(1) / dot(v1,v1);
value_type inv_dot22 = value_type(1) / dot(v2,v2);
vector_type temp0 = v0 - s*dot01*inv_dot11*v1 - s*dot20*inv_dot22*v2;
vector_type temp1 = v1 - s*dot12*inv_dot22*v2 - s*dot01*inv_dot00*v0;
vector_type temp2 = v2 - s*dot20*inv_dot00*v0 - s*dot12*inv_dot11*v1;
v0 = temp0;
v1 = temp1;
v2 = temp2;
}
/* Final Gram-Schmidt step to ensure orthonormality. If no iterations
* have been requested (num_iter = 0), this is the only step. The step
* is performed such that the direction of the axis indexed by
* 'stable_axis' is unchanged.
*/
size_t i, j, k;
cyclic_permutation(stable_axis, i, j, k);
vector_type v[] = { v0, v1, v2 };
v[i].normalize();
v[j] = normalize(project_to_hplane(v[j],v[i]));
v[k] = normalize(project_to_hplane(project_to_hplane(v[k],v[i]),v[j]));
v0 = v[0];
v1 = v[1];
v2 = v[2];
}
/** Orthonormalize 2 basis vectors in R2 */
template < typename E, class A > void
orthonormalize(vector<E,A>& v0, vector<E,A>& v1,
size_t stable_axis = 0, size_t num_iter = 0, E s = E(1))
{
typedef vector< E, fixed<2> > vector_type;
typedef typename vector_type::value_type value_type;
/* Checking */
detail::CheckVec2(v0);
detail::CheckVec2(v1);
detail::CheckIndex2(stable_axis);
/* Iterative Gram-Schmidt; this step is skipped by default. */
for (size_t i = 0; i < num_iter; ++i) {
value_type dot01 = dot(v0,v1);
vector_type temp0 = v0 - (s * dot01 * v1) / dot(v1,v1);
vector_type temp1 = v1 - (s * dot01 * v0) / dot(v0,v0);
v0 = temp0;
v1 = temp1;
}
/* Final Gram-Schmidt step to ensure orthonormality. If no iterations
* have been requested (num_iter = 0), this is the only step. The step
* is performed such that the direction of the axis indexed by
* 'stable_axis' is unchanged.
*/
size_t i, j;
cyclic_permutation(stable_axis, i, j);
vector_type v[] = { v0, v1 };
v[i].normalize();
v[j] = normalize(project_to_hplane(v[j],v[i]));
v0 = v[0];
v1 = v[1];
}
//////////////////////////////////////////////////////////////////////////////
// Orthonormal basis construction in 3D and 2D
//////////////////////////////////////////////////////////////////////////////
/** This version of orthonormal_basis() ultimately does the work for all
* orthonormal_basis_*() functions. Given input vectors 'align' and
* 'reference', and an order 'axis_order_\<i\>\<j\>\<k\>', it constructs an
* orthonormal basis such that the i'th basis vector is aligned with (parallel
* to and pointing in the same direction as) 'align', and the j'th basis
* vector is maximally aligned with 'reference'. The k'th basis vector is
* chosen such that the basis has a determinant of +1.
*
* @note The algorithm fails when 'align' is nearly parallel to
* 'reference'; this should be checked for and handled externally if it's a
* case that may occur.
*
* @internal This is an example of the 'non-const argument modification
* invalidates expression' gotcha. If x, y or z were to be assigned to before
* we were 'done' with align and reference, and if one of them were the same
* object as align or reference, then the algorithm could fail. As is the
* basis vectors are assigned at the end of the function from a temporary
* array, so all is well.
*/
template < class VecT_1, class VecT_2, typename E, class A > void
orthonormal_basis(
const VecT_1& align,
const VecT_2& reference,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z,
bool normalize_align = true,
AxisOrder order = axis_order_zyx)
{
typedef vector< E,fixed<3> > vector_type;
typedef typename vector_type::value_type value_type;
/* Checking handled by cross() and assignment to fixed<3>. */
size_t i, j, k;
bool odd;
detail::unpack_axis_order(order, i, j, k, odd);
vector_type axis[3];
axis[i] = normalize_align ? normalize(align) : align;
axis[k] = unit_cross(axis[i],reference);
axis[j] = cross(axis[k],axis[i]);
if (odd) {
axis[k] = -axis[k];
}
x = axis[0];
y = axis[1];
z = axis[2];
}
/** This version of orthonormal_basis() constructs in arbitrary basis given a
* vector with which to align the i'th basis vector. To avoid the failure
* case, the reference vector is always chosen so as to not be parallel to
* 'align'. This means the algorithm will always generate a valid basis, which
* can be useful in some circumstances; however, it should be noted that the
* basis will likely 'pop' as the alignment vector changes, and so may not be
* suitable for billboarding or other similar applications.
*/
template < class VecT, typename E, class A >
void orthonormal_basis(
const VecT& align,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z,
bool normalize_align = true,
AxisOrder order = axis_order_zyx)
{
/* Checking (won't be necessary with index_of_min_abs() member function */
detail::CheckVec3(align);
/* @todo: vector member function index_of_min_abs() would clean this up */
orthonormal_basis(
align,
axis_3D(cml::index_of_min_abs(align[0],align[1],align[2])),
x, y, z, normalize_align, order
);
}
/** orthonormal_basis_axial() generates a basis in which the j'th basis vector
* is aligned with 'axis' and the i'th basis vector is maximally aligned (as
* 'aligned as possible') with 'align'. This can be used for e.g. axial
* billboarding for, say, trees or beam effects.
*
* Note that the implementation simply passes off to the 'reference' version
* of orthonormal_basis(), with the parameters adjusted so that the alignment
* is axial.
*
* @note With this algorithm the failure case is when 'align' and 'axis'
* are nearly parallel; if this is likely, it should be checked for and
* handled externally.
*/
template < class VecT_1, class VecT_2, typename E, class A >
void orthonormal_basis_axial(
const VecT_1& align,
const VecT_2& axis,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z,
bool normalize_align = true,
AxisOrder order = axis_order_zyx)
{
orthonormal_basis(
axis,
align,
x,
y,
z,
normalize_align,
detail::swap_axis_order(order));
}
/** orthonormal_basis_viewplane() builds a basis aligned with a viewplane, as
* extracted from the input view matrix. The function takes into account the
* handedness of the input view matrix and orients the basis accordingly.
*
* @note The generated basis will always be valid.
*/
template < class MatT, typename E, class A >
void orthonormal_basis_viewplane(
const MatT& view_matrix,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z,
Handedness handedness,
AxisOrder order = axis_order_zyx)
{
typedef MatT matrix_type;
typedef typename matrix_type::value_type value_type;
orthonormal_basis(
-(handedness == left_handed ? value_type(1) : value_type(-1)) *
matrix_get_transposed_z_basis_vector(view_matrix),
matrix_get_transposed_y_basis_vector(view_matrix),
x, y, z, false, order
);
}
/** Build a viewplane-oriented basis from a left-handedness view matrix. */
template < class MatT, typename E, class A >
void orthonormal_basis_viewplane_LH(
const MatT& view_matrix,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z,
AxisOrder order = axis_order_zyx)
{
orthonormal_basis_viewplane(
view_matrix,x,y,z,left_handed,order);
}
/** Build a viewplane-oriented basis from a right-handedness view matrix. */
template < class MatT, typename E, class A >
void orthonormal_basis_viewplane_RH(
const MatT& view_matrix,
vector<E,A>& x,
vector<E,A>& y,
vector<E,A>& z,
AxisOrder order = axis_order_zyx)
{
orthonormal_basis_viewplane(
view_matrix,x,y,z,right_handed,order);
}
/** Build a 2D orthonormal basis. */
template < class VecT, typename E, class A >
void orthonormal_basis_2D(
const VecT& align,
vector<E,A>& x,
vector<E,A>& y,
bool normalize_align = true,
AxisOrder2D order = axis_order_xy)
{
typedef vector< E,fixed<2> > vector_type;
/* Checking handled by perp() and assignment to fixed<2>. */
size_t i, j;
bool odd;
detail::unpack_axis_order_2D(order, i, j, odd);
vector_type axis[2];
axis[i] = normalize_align ? normalize(align) : align;
axis[j] = perp(axis[i]);
if (odd) {
axis[j] = -axis[j];
}
x = axis[0];
y = axis[1];
}
} // namespace cml
#endif

176
Lib/Include/CML/mathlib/vector_transform.h Normal file
View File

@@ -0,0 +1,176 @@
/* -*- C++ -*- ------------------------------------------------------------
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).
*-----------------------------------------------------------------------*/
/** @file
* @brief
*/
#ifndef vector_transform_h
#define vector_transform_h
#include <cml/mathlib/checking.h>
/* Functions for transforming a vector, representing a geometric point or
* or vector, by an affine transfom.
*
* Note: This functionality may be provisional, depending on what architecture
* we settle on for the higher-level math functions. If we do keep these
* functions, then this code may ending up being a placeholder for expression
* template code.
*/
namespace cml {
/** A fixed-size temporary 4D vector */
#define TEMP_VEC4 vector< \
typename et::ScalarPromote< \
typename MatT::value_type, \
typename VecT::value_type \
>::type, \
fixed<4> \
>
/** A fixed-size temporary 3D vector */
#define TEMP_VEC3 vector< \
typename et::ScalarPromote< \
typename MatT::value_type, \
typename VecT::value_type \
>::type, \
fixed<3> \
>
/** A fixed-size temporary 2D vector */
#define TEMP_VEC2 vector< \
typename et::ScalarPromote< \
typename MatT::value_type, \
typename VecT::value_type \
>::type, \
fixed<2> \
>
namespace detail {
template < class MatT, class VecT > TEMP_VEC4
transform_vector_4D(const MatT& m, const VecT& v, row_basis) {
return v*m;
}
template < class MatT, class VecT > TEMP_VEC4
transform_vector_4D(const MatT& m, const VecT& v, col_basis) {
return m*v;
}
} // namespace detail
/** Apply a 4x4 homogeneous transform matrix to a 4D vector */
template < class MatT, class VecT > TEMP_VEC4
transform_vector_4D(const MatT& m, const VecT& v) {
return detail::transform_vector_4D(m,v,typename MatT::basis_orient());
}
/** Apply a homogeneous (e.g. perspective) transform to a 3D point. */
template < class MatT, class VecT > TEMP_VEC3
transform_point_4D(const MatT& m, const VecT& v)
{
typedef TEMP_VEC3 vector_type;
typedef typename TEMP_VEC3::coordinate_type coordinate_type;
/* Checking */
detail::CheckMatHomogeneous3D(m);
detail::CheckVec3(v);
/* Compute the 4D point: */
TEMP_VEC4 v4 = transform_vector_4D(
m, TEMP_VEC4(v[0], v[1], v[2], coordinate_type(1)));
/* Return the projected point: */
coordinate_type w = v4[3];
return vector_type(v4[0]/w, v4[1]/w, v4[2]/w);
}
/** Apply a 3D affine transform to a 3D point */
template < class MatT, class VecT > TEMP_VEC3
transform_point(const MatT& m, const VecT& v)
{
typedef TEMP_VEC3 vector_type;
/* Checking */
detail::CheckMatAffine3D(m);
detail::CheckVec3(v);
return vector_type(
m.basis_element(0,0)*v[0]+m.basis_element(1,0)*v[1]+
m.basis_element(2,0)*v[2]+m.basis_element(3,0),
m.basis_element(0,1)*v[0]+m.basis_element(1,1)*v[1]+
m.basis_element(2,1)*v[2]+m.basis_element(3,1),
m.basis_element(0,2)*v[0]+m.basis_element(1,2)*v[1]+
m.basis_element(2,2)*v[2]+m.basis_element(3,2)
);
}
/** Apply a 3D affine transform to a 3D vector */
template < class MatT, class VecT > TEMP_VEC3
transform_vector(const MatT& m, const VecT& v)
{
typedef TEMP_VEC3 vector_type;
/* Checking */
detail::CheckMatLinear3D(m);
detail::CheckVec3(v);
return vector_type(
m.basis_element(0,0)*v[0]+m.basis_element(1,0)*v[1]+
m.basis_element(2,0)*v[2],
m.basis_element(0,1)*v[0]+m.basis_element(1,1)*v[1]+
m.basis_element(2,1)*v[2],
m.basis_element(0,2)*v[0]+m.basis_element(1,2)*v[1]+
m.basis_element(2,2)*v[2]
);
}
/** Apply a 2D affine transform to a 2D point */
template < class MatT, class VecT > TEMP_VEC2
transform_point_2D(const MatT& m, const VecT& v)
{
typedef TEMP_VEC2 vector_type;
/* Checking */
detail::CheckMatAffine2D(m);
detail::CheckVec2(v);
return vector_type(
m.basis_element(0,0)*v[0]+m.basis_element(1,0)*v[1]+
m.basis_element(2,0),
m.basis_element(0,1)*v[0]+m.basis_element(1,1)*v[1]+
m.basis_element(2,1)
);
}
/** Apply a 2D affine transform to a 2D vector */
template < class MatT, class VecT > TEMP_VEC2
transform_vector_2D(const MatT& m, const VecT& v)
{
typedef TEMP_VEC2 vector_type;
/* Checking */
detail::CheckMatLinear2D(m);
detail::CheckVec2(v);
return vector_type(
m.basis_element(0,0)*v[0] + m.basis_element(1,0)*v[1],
m.basis_element(0,1)*v[0] + m.basis_element(1,1)*v[1]
);
}
#undef TEMP_VEC4
#undef TEMP_VEC3
#undef TEMP_VEC2
} // namespace cml
#endif