]>
Dogcows Code - chaz/yoink/blob - src/Moof/cml/mathlib/vector_ortho.h
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 *-----------------------------------------------------------------------*/
11 * Functions for orthonormalizing a set of basis vectors in 3D or 2D, and for
12 * constructing an orthonormal basis given various input parameters.
15 #ifndef vector_ortho_h
16 #define vector_ortho_h
18 #include <cml/mathlib/vector_misc.h>
19 #include <cml/mathlib/misc.h>
23 //////////////////////////////////////////////////////////////////////////////
24 // Orthonormalization in 3D and 2D
25 //////////////////////////////////////////////////////////////////////////////
28 /** Orthonormalize 3 basis vectors in R3.
30 * Called with the default values, this function performs a single Gram-
31 * Schmidt step to orthonormalize the input vectors. By default, the direction
32 * of the 3rd basis vector is unchanged by this operation, but the unaffected
33 * axis can be specified via the 'stable_axis' parameter.
35 * The arguments 'num_iter' and 's' can be specified to an iterative Gram-
36 * Schmidt step. 'num_iter' is the number of iterations applied, and 's' is
37 * the fraction applied towards orthonormality each step.
39 * In most cases, the default arguments can be ignored, leaving only the three
42 template < typename E
, class A
> void
43 orthonormalize(vector
<E
,A
>& v0
, vector
<E
,A
>& v1
, vector
<E
,A
>& v2
,
44 size_t stable_axis
= 2, size_t num_iter
= 0, E s
= E(1))
47 detail::CheckVec3(v0
);
48 detail::CheckVec3(v1
);
49 detail::CheckVec3(v2
);
50 detail::CheckIndex3(stable_axis
);
52 typedef vector
< E
, fixed
<3> > vector_type
;
53 typedef typename
vector_type::value_type value_type
;
55 /* Iterative Gram-Schmidt; this step is skipped by default. */
57 for (size_t i
= 0; i
< num_iter
; ++i
) {
58 value_type dot01
= dot(v0
,v1
);
59 value_type dot12
= dot(v1
,v2
);
60 value_type dot20
= dot(v2
,v0
);
61 value_type inv_dot00
= value_type(1) / dot(v0
,v0
);
62 value_type inv_dot11
= value_type(1) / dot(v1
,v1
);
63 value_type inv_dot22
= value_type(1) / dot(v2
,v2
);
65 vector_type temp0
= v0
- s
*dot01
*inv_dot11
*v1
- s
*dot20
*inv_dot22
*v2
;
66 vector_type temp1
= v1
- s
*dot12
*inv_dot22
*v2
- s
*dot01
*inv_dot00
*v0
;
67 vector_type temp2
= v2
- s
*dot20
*inv_dot00
*v0
- s
*dot12
*inv_dot11
*v1
;
74 /* Final Gram-Schmidt step to ensure orthonormality. If no iterations
75 * have been requested (num_iter = 0), this is the only step. The step
76 * is performed such that the direction of the axis indexed by
77 * 'stable_axis' is unchanged.
81 cyclic_permutation(stable_axis
, i
, j
, k
);
82 vector_type v
[] = { v0
, v1
, v2
};
85 v
[j
] = normalize(project_to_hplane(v
[j
],v
[i
]));
86 v
[k
] = normalize(project_to_hplane(project_to_hplane(v
[k
],v
[i
]),v
[j
]));
93 /** Orthonormalize 2 basis vectors in R2 */
94 template < typename E
, class A
> void
95 orthonormalize(vector
<E
,A
>& v0
, vector
<E
,A
>& v1
,
96 size_t stable_axis
= 0, size_t num_iter
= 0, E s
= E(1))
98 typedef vector
< E
, fixed
<2> > vector_type
;
99 typedef typename
vector_type::value_type value_type
;
102 detail::CheckVec2(v0
);
103 detail::CheckVec2(v1
);
104 detail::CheckIndex2(stable_axis
);
106 /* Iterative Gram-Schmidt; this step is skipped by default. */
108 for (size_t i
= 0; i
< num_iter
; ++i
) {
109 value_type dot01
= dot(v0
,v1
);
111 vector_type temp0
= v0
- (s
* dot01
* v1
) / dot(v1
,v1
);
112 vector_type temp1
= v1
- (s
* dot01
* v0
) / dot(v0
,v0
);
118 /* Final Gram-Schmidt step to ensure orthonormality. If no iterations
119 * have been requested (num_iter = 0), this is the only step. The step
120 * is performed such that the direction of the axis indexed by
121 * 'stable_axis' is unchanged.
125 cyclic_permutation(stable_axis
, i
, j
);
126 vector_type v
[] = { v0
, v1
};
129 v
[j
] = normalize(project_to_hplane(v
[j
],v
[i
]));
135 //////////////////////////////////////////////////////////////////////////////
136 // Orthonormal basis construction in 3D and 2D
137 //////////////////////////////////////////////////////////////////////////////
139 /** This version of orthonormal_basis() ultimately does the work for all
140 * orthonormal_basis_*() functions. Given input vectors 'align' and
141 * 'reference', and an order 'axis_order_\<i\>\<j\>\<k\>', it constructs an
142 * orthonormal basis such that the i'th basis vector is aligned with (parallel
143 * to and pointing in the same direction as) 'align', and the j'th basis
144 * vector is maximally aligned with 'reference'. The k'th basis vector is
145 * chosen such that the basis has a determinant of +1.
147 * @note The algorithm fails when 'align' is nearly parallel to
148 * 'reference'; this should be checked for and handled externally if it's a
149 * case that may occur.
151 * @internal This is an example of the 'non-const argument modification
152 * invalidates expression' gotcha. If x, y or z were to be assigned to before
153 * we were 'done' with align and reference, and if one of them were the same
154 * object as align or reference, then the algorithm could fail. As is the
155 * basis vectors are assigned at the end of the function from a temporary
156 * array, so all is well.
158 template < class VecT_1
, class VecT_2
, typename E
, class A
> void
161 const VecT_2
& reference
,
165 bool normalize_align
= true,
166 AxisOrder order
= axis_order_zyx
)
168 typedef vector
< E
,fixed
<3> > vector_type
;
169 typedef typename
vector_type::value_type value_type
;
171 /* Checking handled by cross() and assignment to fixed<3>. */
175 detail::unpack_axis_order(order
, i
, j
, k
, odd
);
179 axis
[i
] = normalize_align
? normalize(align
) : align
;
180 axis
[k
] = unit_cross(axis
[i
],reference
);
181 axis
[j
] = cross(axis
[k
],axis
[i
]);
192 /** This version of orthonormal_basis() constructs in arbitrary basis given a
193 * vector with which to align the i'th basis vector. To avoid the failure
194 * case, the reference vector is always chosen so as to not be parallel to
195 * 'align'. This means the algorithm will always generate a valid basis, which
196 * can be useful in some circumstances; however, it should be noted that the
197 * basis will likely 'pop' as the alignment vector changes, and so may not be
198 * suitable for billboarding or other similar applications.
200 template < class VecT
, typename E
, class A
>
201 void orthonormal_basis(
206 bool normalize_align
= true,
207 AxisOrder order
= axis_order_zyx
)
209 /* Checking (won't be necessary with index_of_min_abs() member function */
210 detail::CheckVec3(align
);
212 /* @todo: vector member function index_of_min_abs() would clean this up */
216 axis_3D(cml::index_of_min_abs(align
[0],align
[1],align
[2])),
217 x
, y
, z
, normalize_align
, order
221 /** orthonormal_basis_axial() generates a basis in which the j'th basis vector
222 * is aligned with 'axis' and the i'th basis vector is maximally aligned (as
223 * 'aligned as possible') with 'align'. This can be used for e.g. axial
224 * billboarding for, say, trees or beam effects.
226 * Note that the implementation simply passes off to the 'reference' version
227 * of orthonormal_basis(), with the parameters adjusted so that the alignment
230 * @note With this algorithm the failure case is when 'align' and 'axis'
231 * are nearly parallel; if this is likely, it should be checked for and
232 * handled externally.
234 template < class VecT_1
, class VecT_2
, typename E
, class A
>
235 void orthonormal_basis_axial(
241 bool normalize_align
= true,
242 AxisOrder order
= axis_order_zyx
)
251 detail::swap_axis_order(order
));
254 /** orthonormal_basis_viewplane() builds a basis aligned with a viewplane, as
255 * extracted from the input view matrix. The function takes into account the
256 * handedness of the input view matrix and orients the basis accordingly.
258 * @note The generated basis will always be valid.
260 template < class MatT
, typename E
, class A
>
261 void orthonormal_basis_viewplane(
262 const MatT
& view_matrix
,
266 Handedness handedness
,
267 AxisOrder order
= axis_order_zyx
)
269 typedef MatT matrix_type
;
270 typedef typename
matrix_type::value_type value_type
;
273 -(handedness
== left_handed
? value_type(1) : value_type(-1)) *
274 matrix_get_transposed_z_basis_vector(view_matrix
),
275 matrix_get_transposed_y_basis_vector(view_matrix
),
276 x
, y
, z
, false, order
280 /** Build a viewplane-oriented basis from a left-handedness view matrix. */
281 template < class MatT
, typename E
, class A
>
282 void orthonormal_basis_viewplane_LH(
283 const MatT
& view_matrix
,
287 AxisOrder order
= axis_order_zyx
)
289 orthonormal_basis_viewplane(
290 view_matrix
,x
,y
,z
,left_handed
,order
);
293 /** Build a viewplane-oriented basis from a right-handedness view matrix. */
294 template < class MatT
, typename E
, class A
>
295 void orthonormal_basis_viewplane_RH(
296 const MatT
& view_matrix
,
300 AxisOrder order
= axis_order_zyx
)
302 orthonormal_basis_viewplane(
303 view_matrix
,x
,y
,z
,right_handed
,order
);
306 /** Build a 2D orthonormal basis. */
307 template < class VecT
, typename E
, class A
>
308 void orthonormal_basis_2D(
312 bool normalize_align
= true,
313 AxisOrder2D order
= axis_order_xy
)
315 typedef vector
< E
,fixed
<2> > vector_type
;
317 /* Checking handled by perp() and assignment to fixed<2>. */
321 detail::unpack_axis_order_2D(order
, i
, j
, odd
);
325 axis
[i
] = normalize_align
? normalize(align
) : align
;
326 axis
[j
] = perp(axis
[i
]);
This page took 0.053493 seconds and 4 git commands to generate.