Initial commit
This commit is contained in:
338
Lib/Include/CML/mathlib/vector_ortho.h
Normal file
338
Lib/Include/CML/mathlib/vector_ortho.h
Normal 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
|
||||
Reference in New Issue
Block a user