Blender V5.3
GEO_xpbd_constraint_align_rotations.hh
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2026 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
5#pragma once
6
8#include "BLI_math_vector.hh"
9
10namespace blender::xpbd {
11
18
20 const math::Quaternion &r0,
21 const math::Quaternion &r1,
22 const float3 &inertia0,
23 const float3 &inertia1,
24 const math::Quaternion &rest_rotation,
25 const float compliance_term,
26 const float4 &lambda_prev)
27{
28 const float inv_lumped_inertia0 = math::safe_rcp(0.5f * (inertia0.x + inertia0.y + inertia0.z));
29 const float inv_lumped_inertia1 = math::safe_rcp(0.5f * (inertia1.x + inertia1.y + inertia1.z));
30 if (inv_lumped_inertia0 == 0.0f && inv_lumped_inertia1 == 0.0f) {
31 /* Everything is pinned, so the constraint can't do anything. */
32 return {};
33 }
34
35 const float4 rest_rot_f = float4(rest_rotation);
36
37 /* Note In "Position and Orientation Based Cosserat Rods" (Kugelstadt, Schoemer) the W
38 * component of the Darboux vector is ignored. In "Sag-Free Initialization for Strand-Based
39 * Hybrid Hair Simulation" (Hsu et al.) it is included to improve stability in cases where the
40 * hair is bent at nearly 180 degrees. */
41 const math::Quaternion &rot_diff = math::invert_normalized(r0) * r1;
42 const float4 rot_diff_f = float4(rot_diff);
43
44 const float4 residual_neg = rot_diff_f - rest_rot_f;
45 const float4 residual_pos = rot_diff_f + rest_rot_f;
46 const float4 residual = math::length_squared(residual_neg) < math::length_squared(residual_pos) ?
47 residual_neg :
48 residual_pos;
49
50 const float error_squared = math::length_squared(residual + compliance_term * lambda_prev);
51
52 const float4 delta_lambda = (-residual - compliance_term * lambda_prev) /
53 (inv_lumped_inertia0 + inv_lumped_inertia1 + compliance_term);
54
55 const math::Quaternion offset0 = r1 * math::conjugate(
56 math::Quaternion(delta_lambda * inv_lumped_inertia0));
57 const math::Quaternion offset1 = r0 * math::Quaternion(delta_lambda * inv_lumped_inertia1);
58
59 return {delta_lambda, offset0, offset1, error_squared};
60}
61
62} // namespace blender::xpbd
QuaternionBase< T > conjugate(const QuaternionBase< T > &a)
QuaternionBase< float > Quaternion
T length_squared(const VecBase< T, Size > &a)
T safe_rcp(const T &a)
QuaternionBase< T > invert_normalized(const QuaternionBase< T > &q)
AlignRotationsConstraintResult evaluate_align_rotations_constraint(const math::Quaternion &r0, const math::Quaternion &r1, const float3 &inertia0, const float3 &inertia1, const math::Quaternion &rest_rotation, const float compliance_term, const float4 &lambda_prev)
VecBase< float, 4 > float4
VecBase< float, 3 > float3