]>
Dogcows Code - chaz/yoink/blob - vector_ortho.h
88506f5a0fbe05565c1fe741c202af27e9847f5d
1 /* -*- C++ -*- ------------------------------------------------------------
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
8 *-----------------------------------------------------------------------*/
13 #ifndef vector_ortho_h
14 #define vector_ortho_h
16 #include <cml/mathlib/vector_misc.h>
17 #include <cml/mathlib/misc.h>
19 /* Functions for orthonormalizing a set of basis vector in 3D or 2D, and for
20 * constructing an orthonormal basis given various input parameters.
25 /* Orthonormalize 3 basis vectors in R3.
27 * Called with the default values, this function performs a single Gram-
28 * Schmidt step to orthonormalize the input vectors. By default, the direction
29 * of the 3rd basis vector is unchanged by this operation, but the unaffected
30 * axis can be specified via the 'stable_axis' parameter.
32 * The arguments 'num_iter' and 's' can be specified to an iterative Gram-
33 * Schmidt step. 'num_iter' is the number of iterations applied, and 's' is
34 * the fraction applied towards orthonormality each step.
36 * In most cases, the default arguments can be ignored, leaving only the three
40 //////////////////////////////////////////////////////////////////////////////
41 // Orthonormalization in 3D and 2D
42 //////////////////////////////////////////////////////////////////////////////
44 template < typename E
, class A
> void
45 orthonormalize(vector
<E
,A
>& v0
, vector
<E
,A
>& v1
, vector
<E
,A
>& v2
,
46 size_t stable_axis
= 2, size_t num_iter
= 0, E s
= E(1))
49 detail::CheckVec3(v0
);
50 detail::CheckVec3(v1
);
51 detail::CheckVec3(v2
);
52 detail::CheckIndex3(stable_axis
);
54 typedef vector
< E
, fixed
<3> > vector_type
;
55 typedef typename
vector_type::value_type value_type
;
57 /* Iterative Gram-Schmidt; this step is skipped by default. */
59 for (size_t i
= 0; i
< num_iter
; ++i
) {
60 value_type dot01
= dot(v0
,v1
);
61 value_type dot12
= dot(v1
,v2
);
62 value_type dot20
= dot(v2
,v0
);
63 value_type inv_dot00
= value_type(1) / dot(v0
,v0
);
64 value_type inv_dot11
= value_type(1) / dot(v1
,v1
);
65 value_type inv_dot22
= value_type(1) / dot(v2
,v2
);
67 vector_type temp0
= v0
- s
*dot01
*inv_dot11
*v1
- s
*dot20
*inv_dot22
*v2
;
68 vector_type temp1
= v1
- s
*dot12
*inv_dot22
*v2
- s
*dot01
*inv_dot00
*v0
;
69 vector_type temp2
= v2
- s
*dot20
*inv_dot00
*v0
- s
*dot12
*inv_dot11
*v1
;
76 /* Final Gram-Schmidt step to ensure orthonormality. If no iterations
77 * have been requested (num_iter = 0), this is the only step. The step
78 * is performed such that the direction of the axis indexed by
79 * 'stable_axis' is unchanged.
83 cyclic_permutation(stable_axis
, i
, j
, k
);
84 vector_type v
[] = { v0
, v1
, v2
};
87 v
[j
] = normalize(project_to_hplane(v
[j
],v
[i
]));
88 v
[k
] = normalize(project_to_hplane(project_to_hplane(v
[k
],v
[i
]),v
[j
]));
95 /* Orthonormalize 2 basis vectors in R2 */
96 template < typename E
, class A
> void
97 orthonormalize(vector
<E
,A
>& v0
, vector
<E
,A
>& v1
,
98 size_t stable_axis
= 0, size_t num_iter
= 0, E s
= E(1))
100 typedef vector
< E
, fixed
<2> > vector_type
;
101 typedef typename
vector_type::value_type value_type
;
104 detail::CheckVec2(v0
);
105 detail::CheckVec2(v1
);
106 detail::CheckIndex2(stable_axis
);
108 /* Iterative Gram-Schmidt; this step is skipped by default. */
110 for (size_t i
= 0; i
< num_iter
; ++i
) {
111 value_type dot01
= dot(v0
,v1
);
113 vector_type temp0
= v0
- (s
* dot01
* v1
) / dot(v1
,v1
);
114 vector_type temp1
= v1
- (s
* dot01
* v0
) / dot(v0
,v0
);
120 /* Final Gram-Schmidt step to ensure orthonormality. If no iterations
121 * have been requested (num_iter = 0), this is the only step. The step
122 * is performed such that the direction of the axis indexed by
123 * 'stable_axis' is unchanged.
127 cyclic_permutation(stable_axis
, i
, j
);
128 vector_type v
[] = { v0
, v1
};
131 v
[j
] = normalize(project_to_hplane(v
[j
],v
[i
]));
137 //////////////////////////////////////////////////////////////////////////////
138 // Orthonormal basis construction in 3D and 2D
139 //////////////////////////////////////////////////////////////////////////////
141 /* This version of orthonormal_basis() ultimately does the work for all
142 * orthonormal_basis_*() functions. Given input vectors 'align' and
143 * 'reference', and an order 'axis_order_<i><j><k>', it constructs an
144 * orthonormal basis such that the i'th basis vector is aligned with (parallel
145 * to and pointing in the same direction as) 'align', and the j'th basis
146 * vector is maximally aligned with 'reference'. The k'th basis vector is
147 * chosen such that the basis has a determinant of +1.
149 * Note that the algorithm fails when 'align' is nearly parallel to
150 * 'reference'; this should be checked for and handled externally if it's a
151 * case that may occur.
154 /* Note: This is an example of the 'non-const argument modification
155 * invalidates expression' gotcha. If x, y or z were to be assigned to before
156 * we were 'done' with align and reference, and if one of them were the same
157 * object as align or reference, then the algorithm could fail. As is the
158 * basis vectors are assigned at the end of the function from a temporary
159 * array, so all is well.
162 template < class VecT_1
, class VecT_2
, typename E
, class A
> void
165 const VecT_2
& reference
,
169 bool normalize_align
= true,
170 AxisOrder order
= axis_order_zyx
)
172 typedef vector
< E
,fixed
<3> > vector_type
;
173 typedef typename
vector_type::value_type value_type
;
175 /* Checking handled by cross() and assignment to fixed<3>. */
179 detail::unpack_axis_order(order
, i
, j
, k
, odd
);
183 axis
[i
] = normalize_align
? normalize(align
) : align
;
184 axis
[k
] = unit_cross(axis
[i
],reference
);
185 axis
[j
] = cross(axis
[k
],axis
[i
]);
196 /* This version of orthonormal_basis() constructs in arbitrary basis given a
197 * vector with which to align the i'th basis vector. To avoid the failure
198 * case, the reference vector is always chosen so as to not be parallel to
199 * 'align'. This means the algorithm will always generate a valid basis, which
200 * can be useful in some circumstances; however, it should be noted that the
201 * basis will likely 'pop' as the alignment vector changes, and so may not be
202 * suitable for billboarding or other similar applications.
205 template < class VecT
, typename E
, class A
>
206 void orthonormal_basis(
211 bool normalize_align
= true,
212 AxisOrder order
= axis_order_zyx
)
214 /* Checking (won't be necessary with index_of_min_abs() member function */
215 detail::CheckVec3(align
);
217 /* @todo: vector member function index_of_min_abs() would clean this up */
221 axis_3D(cml::index_of_min_abs(align
[0],align
[1],align
[2])),
222 x
, y
, z
, normalize_align
, order
226 /* orthonormal_basis_axial() generates a basis in which the j'th basis vector
227 * is aligned with 'axis' and the i'th basis vector is maximally aligned (as
228 * 'aligned as possible') with 'align'. This can be used for e.g. axial
229 * billboarding for, say, trees or beam effects.
231 * Note that the implementation simply passes off to the 'reference' version
232 * of orthonormal_basis(), with the parameters adjusted so that the alignment
235 * With this algorithm the failure case is when 'align' and 'axis' are nearly
236 * parallel; if this is likely, it should be checked for and handled
240 template < class VecT_1
, class VecT_2
, typename E
, class A
>
241 void orthonormal_basis_axial(
247 bool normalize_align
= true,
248 AxisOrder order
= axis_order_zyx
)
257 detail::swap_axis_order(order
));
260 /* orthonormal_basis_viewplane() builds a basis aligned with a viewplane, as
261 * extracted from the input view matrix. The function takes into account the
262 * handedness of the input view matrix and orients the basis accordingly.
264 * The generated basis will always be valid.
266 template < class MatT
, typename E
, class A
>
267 void orthonormal_basis_viewplane(
268 const MatT
& view_matrix
,
272 Handedness handedness
,
273 AxisOrder order
= axis_order_zyx
)
275 typedef MatT matrix_type
;
276 typedef typename
matrix_type::value_type value_type
;
279 -(handedness
== left_handed
? value_type(1) : value_type(-1)) *
280 matrix_get_transposed_z_basis_vector(view_matrix
),
281 matrix_get_transposed_y_basis_vector(view_matrix
),
282 x
, y
, z
, false, order
286 /** Build a viewplane-oriented basis from a left-handedness view matrix. */
287 template < class MatT
, typename E
, class A
>
288 void orthonormal_basis_viewplane_LH(
289 const MatT
& view_matrix
,
293 AxisOrder order
= axis_order_zyx
)
295 orthonormal_basis_viewplane(
296 view_matrix
,x
,y
,z
,left_handed
,order
);
299 /** Build a viewplane-oriented basis from a right-handedness view matrix. */
300 template < class MatT
, typename E
, class A
>
301 void orthonormal_basis_viewplane_RH(
302 const MatT
& view_matrix
,
306 AxisOrder order
= axis_order_zyx
)
308 orthonormal_basis_viewplane(
309 view_matrix
,x
,y
,z
,right_handed
,order
);
312 /* Build a 2D orthonormal basis. */
313 template < class VecT
, typename E
, class A
>
314 void orthonormal_basis_2D(
318 bool normalize_align
= true,
319 AxisOrder2D order
= axis_order_xy
)
321 typedef vector
< E
,fixed
<2> > vector_type
;
323 /* Checking handled by perp() and assignment to fixed<2>. */
327 detail::unpack_axis_order_2D(order
, i
, j
, odd
);
331 axis
[i
] = normalize_align
? normalize(align
) : align
;
332 axis
[j
] = perp(axis
[i
]);
This page took 0.04675 seconds and 3 git commands to generate.