Blender V5.3
BLI_math_rotation.hh
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2023 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
5#pragma once
6
10
12
14#include "BLI_math_euler.hh"
15#include "BLI_math_matrix.hh"
17#include "BLI_math_vector.hh"
18
19namespace blender {
20
21namespace math {
22
23/* -------------------------------------------------------------------- */
26
33template<typename T, typename RotT>
34[[nodiscard]] QuaternionBase<T> rotate(const QuaternionBase<T> &a, const RotT &b);
35
42template<typename T, typename RotT, typename AngleT>
43[[nodiscard]] AxisAngleBase<T, AngleT> rotate(const AxisAngleBase<T, AngleT> &a, const RotT &b);
44
51template<typename T, typename RotT>
52[[nodiscard]] EulerXYZBase<T> rotate(const EulerXYZBase<T> &a, const RotT &b);
53
60template<typename T, typename RotT>
61[[nodiscard]] Euler3Base<T> rotate(const Euler3Base<T> &a, const RotT &b);
62
66template<typename T>
68 const QuaternionBase<T> &b);
69
75template<typename T>
76[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
77 const VecBase<T, 3> &v2,
78 const VecBase<T, 3> &v3,
79 const VecBase<T, 3> &normal);
80
84template<typename T>
85[[nodiscard]] QuaternionBase<T> from_triangle(const VecBase<T, 3> &v1,
86 const VecBase<T, 3> &v2,
87 const VecBase<T, 3> &v3);
88
95template<typename T>
96[[nodiscard]] QuaternionBase<T> from_vector(const VecBase<T, 3> &vector,
97 const AxisSigned track_flag,
98 const Axis up_flag);
99
104template<typename T>
105[[nodiscard]] QuaternionBase<T> from_tracking(AxisSigned forward_axis, Axis up_axis);
106
110template<typename T> [[nodiscard]] MatBase<T, 3, 3> to_gimbal_axis(const Euler3Base<T> &rotation);
111
113
114/* -------------------------------------------------------------------- */
117
125template<typename T> [[nodiscard]] AngleRadianBase<T> angle_of(const QuaternionBase<T> &q)
126{
128 return T(2) * math::safe_acos(q.w);
129}
130
139template<typename T> [[nodiscard]] AngleRadianBase<T> angle_of_signed(const QuaternionBase<T> &q)
140{
142 return T(2) * ((q.w >= T(0)) ? math::safe_acos(q.w) : -math::safe_acos(-q.w));
143}
144
151template<typename T>
153 const QuaternionBase<T> &b)
154{
155 return angle_of(rotation_between(a, b));
156}
157template<typename T>
159{
162 return math::safe_acos(dot(a, b));
163}
164template<typename T>
166{
167 if (a == b) {
169 }
170 if (abs(a) == abs(b)) {
171 return AngleFraction<T>::pi();
172 }
173 return AngleFraction<T>::pi() / 2;
174}
175
181template<typename T>
187
188inline float2 rotate_around_point_2d(const float2 p, const float2 pivot, const AngleRadian angle)
189{
190 return from_rotation<float2x2>(angle) * (p - pivot) + pivot;
191}
192
194
195/* -------------------------------------------------------------------- */
198
199template<typename T, typename RotT>
200[[nodiscard]] QuaternionBase<T> rotate(const QuaternionBase<T> &a, const RotT &b)
201{
202 return a * QuaternionBase<T>(b);
203}
204
205template<typename T, typename RotT, typename AngleT>
210
211template<typename T, typename RotT>
212[[nodiscard]] EulerXYZBase<T> rotate(const EulerXYZBase<T> &a, const RotT &b)
213{
215 return to_euler(tmp);
216}
217
218template<typename T, typename RotT>
219[[nodiscard]] Euler3Base<T> rotate(const Euler3Base<T> &a, const RotT &b)
220{
223 return to_euler(tmp, a.order());
224}
225
226template<typename T>
228 const QuaternionBase<T> &b)
229{
230 return invert(a) * b;
231}
232
233template<typename T>
235 const VecBase<T, 3> &v2,
236 const VecBase<T, 3> &v3,
237 const VecBase<T, 3> &normal)
238{
239 /* Force to used an unused var to avoid the same function signature as the version without
240 * `normal` argument. */
241 UNUSED_VARS(v3);
242
243 using Vec3T = VecBase<T, 3>;
244
245 /* Move z-axis to face-normal. */
246 const Vec3T z_axis = normal;
247 Vec3T nor = normalize(Vec3T(z_axis.y, -z_axis.x, T(0)));
248 if (is_zero(nor.xy())) {
249 nor.x = T(1);
250 }
251
252 T angle = T(-0.5) * math::safe_acos(z_axis.z);
253 T si = math::sin(angle);
254 QuaternionBase<T> q1(math::cos(angle), nor.x * si, nor.y * si, T(0));
255
256 /* Rotate back line v1-v2. */
257 Vec3T line = transform_point(conjugate(q1), (v2 - v1));
258 /* What angle has this line with x-axis? */
259 line = normalize(Vec3T(line.x, line.y, T(0)));
260
261 angle = T(0.5) * math::atan2(line.y, line.x);
263
264 return q1 * q2;
265}
266
267template<typename T>
269 const VecBase<T, 3> &v2,
270 const VecBase<T, 3> &v3)
271{
272 return from_triangle(v1, v2, v3, normal_tri(v1, v2, v3));
273}
274
275template<typename T>
276[[nodiscard]] QuaternionBase<T> from_vector(const VecBase<T, 3> &vector,
277 const AxisSigned track_flag,
278 const Axis up_flag)
279{
280 using Vec2T = VecBase<T, 2>;
281 using Vec3T = VecBase<T, 3>;
282 using Vec4T = VecBase<T, 4>;
283
284 const T vec_len = length(vector);
285
286 if (vec_len == 0.0f) [[unlikely]] {
288 }
289
290 const Axis axis = track_flag.axis();
291 const Vec3T vec = track_flag.is_negative() ? vector : -vector;
292
293 Vec3T rotation_axis;
294 constexpr T eps = T(1e-4);
295 T axis_len;
296 switch (axis) {
297 case Axis::X:
298 rotation_axis = normalize_and_get_length(Vec3T(T(0), -vec.z, vec.y), axis_len);
299 if (axis_len < eps) {
300 rotation_axis = Vec3T(0, 1, 0);
301 }
302 break;
303 case Axis::Y:
304 rotation_axis = normalize_and_get_length(Vec3T(vec.z, T(0), -vec.x), axis_len);
305 if (axis_len < eps) {
306 rotation_axis = Vec3T(0, 0, 1);
307 }
308 break;
309 default:
310 case Axis::Z:
311 rotation_axis = normalize_and_get_length(Vec3T(-vec.y, vec.x, T(0)), axis_len);
312 if (axis_len < eps) {
313 rotation_axis = Vec3T(1, 0, 0);
314 }
315 break;
316 }
317 /* TODO(fclem): Can optimize here by initializing AxisAngle using the cos an sin directly.
318 * Avoiding the need for safe_acos and deriving sin from cos. */
319 const T rotation_angle = math::safe_acos(vec[axis.as_int()] / vec_len);
320
322 AxisAngleBase<T, AngleRadianBase<T>>(rotation_axis, rotation_angle));
323
324 if (axis == up_flag) {
325 /* Nothing else to do. */
326 return q1;
327 }
328
329 /* Extract rotation between the up axis of the rotated space and the up axis. */
330 /* There might be an easier way to get this angle directly from the quaternion representation. */
331 const Vec3T rotated_up = transform_point(q1, Vec3T(0, 0, 1));
332
333 /* Project using axes index instead of arithmetic. It's much faster and more precise. */
334 const AxisSigned y_axis_signed = math::cross(AxisSigned(axis), AxisSigned(up_flag));
335 const Axis x_axis = up_flag;
336 const Axis y_axis = y_axis_signed.axis();
337
338 Vec2T projected = normalize(Vec2T(rotated_up[x_axis.as_int()], rotated_up[y_axis.as_int()]));
339 /* Flip sign for flipped axis. */
340 if (y_axis_signed.is_negative()) {
341 projected.y = -projected.y;
342 }
343 /* Not sure if this was a bug or not in the previous implementation.
344 * Carry over this weird behavior to avoid regressions. */
345 if (axis == Axis::Z) {
346 projected = -projected;
347 }
348
349 const AngleCartesianBase<T> angle(projected.x, projected.y);
350 const AngleCartesianBase<T> half_angle = angle / T(2);
351
352 const QuaternionBase<T> q2(Vec4T(half_angle.cos(), vec * (half_angle.sin() / vec_len)));
353
354 return q2 * q1;
355}
356
357template<typename T>
358[[nodiscard]] QuaternionBase<T> from_tracking(AxisSigned forward_axis, Axis up_axis)
359{
360 BLI_assert(forward_axis.axis() != up_axis);
361
362 /* Curve have Z forward, Y up, X left. */
363 return QuaternionBase<T>(
365 from_orthonormal_axes(forward_axis, AxisSigned(up_axis))));
366}
367
368template<typename T> [[nodiscard]] MatBase<T, 3, 3> to_gimbal_axis(const Euler3Base<T> &rotation)
369{
370 using Mat3T = MatBase<T, 3, 3>;
371 using Vec3T = VecBase<T, 3>;
372 const int i_index = rotation.i_index();
373 const int j_index = rotation.j_index();
374 const int k_index = rotation.k_index();
375
376 Mat3T result;
377 /* First axis is local. */
378 result[i_index] = from_rotation<Mat3T>(rotation)[i_index];
379 /* Second axis is local minus first rotation. */
380 Euler3Base<T> tmp_rot = rotation;
381 tmp_rot.i() = T(0);
382 result[j_index] = from_rotation<Mat3T>(tmp_rot)[j_index];
383 /* Last axis is global. */
384 result[k_index] = Vec3T(0);
385 result[k_index][k_index] = T(1);
386
387 return result;
388}
389
391
392/* -------------------------------------------------------------------- */
395
400template<typename T> QuaternionBase<T> to_quaternion(const CartesianBasis &rotation)
401{
407 constexpr auto map = [](AxisSigned x, AxisSigned y, AxisSigned z) {
408 return x.as_int() << 16 | y.as_int() << 8 | z.as_int();
409 };
410
411 switch (map(rotation.axes.x, rotation.axes.y, rotation.axes.z)) {
412 default:
415 return QuaternionBase<T>{T(0.5), T(-0.5), T(-0.5), T(-0.5)};
417 return QuaternionBase<T>{
418 T(rcp(std::numbers::sqrt2)), T(0), T(0), T(-rcp(std::numbers::sqrt2))};
420 return QuaternionBase<T>{T(0.5), T(0.5), T(0.5), T(-0.5)};
422 return QuaternionBase<T>{
423 T(0), T(rcp(std::numbers::sqrt2)), T(rcp(std::numbers::sqrt2)), T(0)};
425 return QuaternionBase<T>{
426 T(rcp(std::numbers::sqrt2)), T(0), T(rcp(std::numbers::sqrt2)), T(0)};
428 return QuaternionBase<T>{
429 T(rcp(std::numbers::sqrt2)), T(0), T(-rcp(std::numbers::sqrt2)), T(0)};
431 return QuaternionBase<T>{T(0), T(0), T(1), T(0)};
433 return QuaternionBase<T>{T(0.5), T(0.5), T(0.5), T(0.5)};
435 return QuaternionBase<T>{
436 T(0), T(0), T(rcp(std::numbers::sqrt2)), T(rcp(std::numbers::sqrt2))};
438 return QuaternionBase<T>{T(0.5), T(0.5), T(-0.5), T(-0.5)};
440 return QuaternionBase<T>{
441 T(rcp(std::numbers::sqrt2)), T(rcp(std::numbers::sqrt2)), T(0), T(0)};
443 return QuaternionBase<T>{T(0.5), T(-0.5), T(0.5), T(0.5)};
445 return QuaternionBase<T>{
446 T(rcp(std::numbers::sqrt2)), T(0), T(0), T(rcp(std::numbers::sqrt2))};
448 return QuaternionBase<T>{T(0.5), T(0.5), T(-0.5), T(0.5)};
450 return QuaternionBase<T>{
451 T(0), T(-rcp(std::numbers::sqrt2)), T(rcp(std::numbers::sqrt2)), T(0)};
453 return QuaternionBase<T>{
454 T(0), T(rcp(std::numbers::sqrt2)), T(0), T(rcp(std::numbers::sqrt2))};
456 return QuaternionBase<T>{T(0), T(0), T(0), T(1)};
458 return QuaternionBase<T>{
459 T(0), T(-rcp(std::numbers::sqrt2)), T(0), T(rcp(std::numbers::sqrt2))};
461 return QuaternionBase<T>{T(0), T(1), T(0), T(0)};
463 return QuaternionBase<T>{T(0.5), T(-0.5), T(0.5), T(-0.5)};
465 return QuaternionBase<T>{
466 T(rcp(std::numbers::sqrt2)), T(-rcp(std::numbers::sqrt2)), T(0), T(0)};
468 return QuaternionBase<T>{T(0.5), T(-0.5), T(-0.5), T(0.5)};
470 return QuaternionBase<T>{
471 T(0), T(0), T(-rcp(std::numbers::sqrt2)), T(rcp(std::numbers::sqrt2))};
472 }
473}
474
476
477} // namespace math
478
479namespace math {
480
481/* -------------------------------------------------------------------- */
484
485/* Using explicit template instantiations in order to reduce compilation time. */
486extern template EulerXYZ to_euler(const AxisAngle &);
487extern template EulerXYZ to_euler(const AxisAngleCartesian &);
488extern template EulerXYZ to_euler(const Quaternion &);
489extern template Euler3 to_euler(const AxisAngle &, EulerOrder);
491extern template Euler3 to_euler(const Quaternion &, EulerOrder);
492extern template Quaternion to_quaternion(const AxisAngle &);
494extern template Quaternion to_quaternion(const Euler3 &);
495extern template Quaternion to_quaternion(const EulerXYZ &);
496extern template AxisAngleCartesian to_axis_angle(const Euler3 &);
499extern template AxisAngle to_axis_angle(const Euler3 &);
500extern template AxisAngle to_axis_angle(const EulerXYZ &);
501extern template AxisAngle to_axis_angle(const Quaternion &);
502
504
505} // namespace math
506} // namespace blender
#define UNUSED_VARS(...)
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
Definition btQuadWord.h:117
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition btVector3.h:356
constexpr bool is_negative() const
constexpr int as_int() const
static constexpr Value Z
static constexpr Value X
static constexpr Value Y
#define T
QuaternionBase< T > from_triangle(const VecBase< T, 3 > &v1, const VecBase< T, 3 > &v2, const VecBase< T, 3 > &v3, const VecBase< T, 3 > &normal)
AngleRadianBase< float > AngleRadian
QuaternionBase< T > conjugate(const QuaternionBase< T > &a)
QuaternionBase< float > Quaternion
T cos(const AngleRadianBase< T > &a)
VecBase< T, 3 > normal_tri(const VecBase< T, 3 > &v1, const VecBase< T, 3 > &v2, const VecBase< T, 3 > &v3)
QuaternionBase< T > from_vector(const VecBase< T, 3 > &vector, const AxisSigned track_flag, const Axis up_flag)
QuaternionBase< T > to_quaternion(const AxisAngleBase< T, AngleT > &axis_angle)
float2 rotate_around_point_2d(const float2 p, const float2 pivot, const AngleRadian angle)
T length(const VecBase< T, Size > &a)
QuaternionBase< T > from_tracking(AxisSigned forward_axis, Axis up_axis)
CartesianBasis rotation_between(const CartesianBasis &a, const CartesianBasis &b)
AngleRadianBase< T > angle_between(const QuaternionBase< T > &a, const QuaternionBase< T > &b)
AxisAngleBase< float, AngleRadianBase< float > > AxisAngle
QuaternionBase< T > normalize_and_get_length(const QuaternionBase< T > &q, T &out_length)
bool is_unit_scale(const MatBase< T, NumCol, NumRow > &m)
AngleRadianBase< T > angle_between_signed(const QuaternionBase< T > &a, const QuaternionBase< T > &b)
EulerXYZBase< float > EulerXYZ
T dot(const QuaternionBase< T > &a, const QuaternionBase< T > &b)
MatBase< T, 3, 3 > to_gimbal_axis(const Euler3Base< T > &rotation)
bool is_zero(const T &a)
CartesianBasis invert(const CartesianBasis &basis)
AxisSigned cross(const AxisSigned a, const AxisSigned b)
AngleRadianBase< T > angle_of(const QuaternionBase< T > &q)
T atan2(const T &y, const T &x)
T rcp(const T &a)
T safe_acos(const T &a)
AxisAngleBase< T, AngleT > to_axis_angle(const EulerXYZBase< T > &euler)
MatBase< T, NumCol, NumRow > normalize(const MatBase< T, NumCol, NumRow > &a)
AxisAngleBase< float, AngleCartesianBase< float > > AxisAngleCartesian
T sin(const AngleRadianBase< T > &a)
Euler3Base< float > Euler3
T abs(const T &a)
AngleRadianBase< T > angle_of_signed(const QuaternionBase< T > &q)
MatBase< T, NumCol, NumRow > rotate(const MatBase< T, NumCol, NumRow > &mat, const RotationT &rotation)
CartesianBasis from_orthonormal_axes(const AxisSigned forward, const AxisSigned up)
MatT from_rotation(const RotationT &rotation)
Euler3Base< T > to_euler(const AxisAngleBase< T, AngleT > &axis_angle, EulerOrder order)
VecBase< T, 3 > transform_point(const CartesianBasis &basis, const VecBase< T, 3 > &v)
BMesh Iterators.
BLI_assert(slot->slot_subtype.map==BMO_OP_SLOT_SUBTYPE_MAP_FLT)
VecBase< float, 2 > float2
ATTR_WARN_UNUSED_RESULT const BMVert * v2
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
const btScalar eps
Definition poly34.cpp:11
const EulerOrder & order() const