16 const float3 row_p =
float3(matrix[0][p], matrix[1][p], matrix[2][p]);
17 const float3 row_q =
float3(matrix[0][q], matrix[1][q], matrix[2][q]);
19 const float3 new_row_p = rotation[0][0] * row_p + rotation[1][0] * row_q;
20 const float3 new_row_q = rotation[0][1] * row_p + rotation[1][1] * row_q;
22 result[0][p] = new_row_p[0];
23 result[1][p] = new_row_p[1];
24 result[2][p] = new_row_p[2];
25 result[0][q] = new_row_q[0];
26 result[1][q] = new_row_q[1];
27 result[2][q] = new_row_q[2];
36 const float3 col_p = matrix[p];
37 const float3 col_q = matrix[q];
39 result[p] = rotation[0][0] * col_p + rotation[0][1] * col_q;
40 result[q] = rotation[1][0] * col_p + rotation[1][1] * col_q;
48 const float off_diagonal = 2.0f *
abs(
y);
53 const float tau = (
x -
z) / off_diagonal;
54 const float w =
sqrt(tau * tau + 1.0f);
55 const float t = 1.0f / (tau > 0.0f ? (tau +
w) : (tau -
w));
57 const float sign_t = t > 0.0f ? 1.0f : -1.0f;
58 const float c = 1.0f /
sqrt(t * t + 1.0f);
59 const float s = -sign_t *
sign(
y) *
abs(t) * c;
68 block[0] =
float2(matrix[p][p], matrix[p][q]);
69 block[1] =
float2(matrix[q][p], matrix[q][q]);
71 const float t = block[0][0] + block[1][1];
72 const float d = block[0][1] - block[1][0];
78 const float u = t / d;
79 const float tmp =
sqrt(1.0f + u * u);
80 rotation =
float2x2(u / tmp, -(1.0f / tmp), 1.0f / tmp, u / tmp);
83 block = rotation * block;
90 constexpr float convergence_epsilon = 2.0f *
FLT_EPSILON;
91 const float zero_threshold =
FLT_MIN;
94 constexpr int max_sweeps = 80;
109 for (
int i = 0; i < max_sweeps; i++) {
110 bool finished =
true;
112 for (
int p = 1; p < 3; p++) {
113 for (
int q = 0; q < p; q++) {
114 const float threshold =
max(zero_threshold, convergence_epsilon * max_diag_entry);
117 if (
abs(
A[q][p]) > threshold ||
abs(
A[p][q]) > threshold) {
144 for (
int i = 0; i < 3; i++) {
145 const float a =
A[i][i];
156 for (
int i = 0; i < 3; i++) {
157 float max_val = 0.0f;
159 for (
int j = i; j < 3; j++) {
160 const float val = S[j];
166 if (max_val == 0.0f) {
170 const float tmp_s = S[i];
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
MatBase< R, C > transpose(MatBase< C, R >) RET
float3x3 jacobi_rotate_left_3x3(float3x3 matrix, int p, int q, float2x2 rotation)
void jacobi_svd_3x3(float3x3 A, float3x3 &U, float3 &S, float3x3 &V)
float3x3 jacobi_rotate_right_3x3(float3x3 matrix, int p, int q, float2x2 rotation)
float2x2 construct_jacobi_rotation(float x, float y, float z)
void matrix_svd(float4x4 matrix, float4x4 &U, float3 &S, float4x4 &V)
void jacobi_svd_2x2(float3x3 matrix, int p, int q, float2x2 &j_left, float2x2 &j_right)
float3x3 scale(float3x3 mat, float2 scale)
float3x3 mat3x3_identity()
float reduce_max(float2 a)
MatBase< float, 2, 2 > float2x2
MatBase< float, 4, 4 > float4x4
CCL_NAMESPACE_BEGIN struct Window V