From 1da5452a097b3054a5ccf58df52cbf4dcb60fd07 Mon Sep 17 00:00:00 2001 From: Bosheng Li Date: Tue, 15 Oct 2024 23:08:45 -0400 Subject: [PATCH] Checkpoint. --- .../Constraints/StiffRodInitConstraints.comp | 93 +- .../StiffRodProjectConstraints.comp | 365 +++++++- .../StiffRodUpdateConstraints.comp | 18 + .../Operators/ExternalForce.comp | 1 - .../Includes/DynamicStrandsPhysics.glsl | 384 +------- .../include/StrandModel/BrokenBranch.hpp | 10 +- .../include/StrandModel/DynamicStrands.hpp | 27 +- .../StrandModel/DynamicStrandsPhysics.hpp | 83 +- .../EcoSysLab/src/BrokenBranch.cpp | 68 +- .../EcoSysLab/src/DynamicStrands.cpp | 11 +- .../EcoSysLab/src/DynamicStrandsPhysics.cpp | 873 +++++++++++++++--- .../EcoSysLab/src/DynamicStrandsRender.cpp | 1 + .../Shaders/Includes/Math.glsl | 13 +- 13 files changed, 1356 insertions(+), 591 deletions(-) diff --git a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodInitConstraints.comp b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodInitConstraints.comp index e04fbbf..a4a4de8 100644 --- a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodInitConstraints.comp +++ b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodInitConstraints.comp @@ -22,52 +22,77 @@ memoryBarrierShared(); \ barrier(); layout(push_constant) uniform PUSH_CONSTANTS { - uint constraint_size; - float time_step; - float inv_time_step; + uint constraint_size; + float time_step; }; #define DYNAMIC_STRANDS_PHYSICS_SET 1 #include "DynamicStrandsPhysics.glsl" + +void init_stretch_bending_twisting_constraint(in vec3 stiffness_coefficient_k, + out vec3 stretch_compliance, + out vec3 bending_and_torsion_compliance, out vec6 lambda_sum) { + float time_step_quadratic = time_step * time_step; + + stretch_compliance = vec3(time_step_quadratic / stiffness_coefficient_k[0], + time_step_quadratic / stiffness_coefficient_k[0], + time_step_quadratic / stiffness_coefficient_k[0]); + + bending_and_torsion_compliance = vec3(time_step_quadratic / stiffness_coefficient_k[1], + time_step_quadratic / stiffness_coefficient_k[1], + time_step_quadratic / stiffness_coefficient_k[2]); + + lambda_sum.v[0] = 0.0; + lambda_sum.v[1] = 0.0; + lambda_sum.v[2] = 0.0; + lambda_sum.v[3] = 0.0; + lambda_sum.v[4] = 0.0; + lambda_sum.v[5] = 0.0; +} + + + + void init_constraint(in uint rod_constraint_handle); + + + //Line 52 void main(){ - [[unroll]] - for (uint i = 0; i < ITERATIONS_PER_TASK; i++) - { - uint local_index = laneID + i * WORKGROUP_SIZE; - uint constraint_index = baseID + local_index; + [[unroll]] + for (uint i = 0; i < ITERATIONS_PER_TASK; i++) + { + uint local_index = laneID + i * WORKGROUP_SIZE; + uint constraint_index = baseID + local_index; - if(constraint_index >= constraint_size) break; - init_constraint(constraint_index); - } + if(constraint_index >= constraint_size) break; + init_constraint(constraint_index); + } } void init_constraint(in uint rod_constraint_handle){ - RodConstraint rod_constraint = rod_constraints[rod_constraint_handle]; - - vec3 stretch_compliance; - vec3 bending_and_torsion_compliance; - vec6 lambda_sum; - - init_stretch_bending_twisting_constraint( - rod_constraint.stiffness_coefficient_k_segment0_index.xyz, - inv_time_step, - rod_constraint.stretch_compliance_average_segment_length.w, - stretch_compliance, - bending_and_torsion_compliance, - lambda_sum); - - rod_constraints[rod_constraint_handle].stretch_compliance_average_segment_length.xyz = stretch_compliance; - rod_constraints[rod_constraint_handle].bending_and_torsion_compliance_next_constraint_handle.xyz = bending_and_torsion_compliance; - - rod_constraints[rod_constraint_handle].lambda_sum0 = lambda_sum.v[0]; - rod_constraints[rod_constraint_handle].lambda_sum1 = lambda_sum.v[1]; - rod_constraints[rod_constraint_handle].lambda_sum2 = lambda_sum.v[2]; - rod_constraints[rod_constraint_handle].lambda_sum3 = lambda_sum.v[3]; - rod_constraints[rod_constraint_handle].lambda_sum4 = lambda_sum.v[4]; - rod_constraints[rod_constraint_handle].lambda_sum5 = lambda_sum.v[5]; + RodConstraint rod_constraint = rod_constraints[rod_constraint_handle]; + + vec3 stretch_compliance; + vec3 bending_and_torsion_compliance; + vec6 lambda_sum; + + init_stretch_bending_twisting_constraint( + rod_constraint.stiffness_coefficient_k_segment0_index.xyz, + stretch_compliance, + bending_and_torsion_compliance, + lambda_sum); + + rod_constraints[rod_constraint_handle].stretch_compliance_average_segment_length.xyz = stretch_compliance; + rod_constraints[rod_constraint_handle].bending_and_torsion_compliance_next_constraint_handle.xyz = bending_and_torsion_compliance; + + rod_constraints[rod_constraint_handle].lambda_sum0 = lambda_sum.v[0]; + rod_constraints[rod_constraint_handle].lambda_sum1 = lambda_sum.v[1]; + rod_constraints[rod_constraint_handle].lambda_sum2 = lambda_sum.v[2]; + rod_constraints[rod_constraint_handle].lambda_sum3 = lambda_sum.v[3]; + rod_constraints[rod_constraint_handle].lambda_sum4 = lambda_sum.v[4]; + rod_constraints[rod_constraint_handle].lambda_sum5 = lambda_sum.v[5]; } \ No newline at end of file diff --git a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodProjectConstraints.comp b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodProjectConstraints.comp index a04c6f5..35457ad 100644 --- a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodProjectConstraints.comp +++ b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodProjectConstraints.comp @@ -27,6 +27,366 @@ layout(push_constant) uniform PUSH_CONSTANTS { #define DYNAMIC_STRANDS_PHYSICS_SET 1 #include "DynamicStrandsPhysics.glsl" + +void compute_bending_and_torsion_jacobians(in vec4 q0, in vec4 q1, float segment_length, out mat4 j_omega0, + out mat4 j_omega1) { + j_omega0 = mat4( + -q1[3], -q1[2], q1[1], q1[0], + q1[2], -q1[3], -q1[0], q1[1], + -q1[1], q1[0], -q1[3], q1[2], + 0.0, 0.0, 0.0, 0.0); + j_omega1 = mat4( + q0[3], q0[2], -q0[1], -q0[0], + -q0[2], q0[3], q0[0], -q0[1], + q0[1], -q0[0], q0[3], -q0[2], + 0.0, 0.0, 0.0, 0.0); + j_omega0 *= 2.0 / segment_length; + j_omega1 *= 2.0 / segment_length; +} + +void compute_matrix_g(in vec4 q, out mat4 g) { + g = mat4( + q[3], q[2], -q[1], 0.0, + -q[2], q[3], q[0], 0.0, + q[1], -q[0], q[3], 0.0, + -q[0], -q[1], -q[2], 0.0) * 0.5; +} + +void compute_matrix_k(in vec3 connector, in float inv_mass, in vec3 x, in mat3 inverse_inertia, out mat3 k) { + if (inv_mass != 0.0) { + vec3 v = connector - x; + float a = v[0]; + float b = v[1]; + float c = v[2]; + + float j11 = inverse_inertia[0][0]; + float j12 = inverse_inertia[0][1]; + float j13 = inverse_inertia[0][2]; + float j22 = inverse_inertia[1][1]; + float j23 = inverse_inertia[1][2]; + float j33 = inverse_inertia[2][2]; + + k[0][0] = c * c * j22 - b * c * (j23 + j23) + b * b * j33 + inv_mass; + k[0][1] = -(c * c * j12) + a * c * j23 + b * c * j13 - a * b * j33; + k[0][2] = b * c * j12 - a * c * j22 - b * b * j13 + a * b * j23; + k[1][0] = k[0][1]; + k[1][1] = c * c * j11 - a * c * (j13 + j13) + a * a * j33 + inv_mass; + k[1][2] = -(b * c * j11) + a * c * j12 + a * b * j13 - a * a * j23; + k[2][0] = k[0][2]; + k[2][1] = k[1][2]; + k[2][2] = b * b * j11 - a * b * (j12 + j12) + a * a * j22 + inv_mass; + } else { + k = mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); + } +} + +mat3 cross_product_matrix(in vec3 v) { + return mat3(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0); +} + +void solve_ldlt(in mat6 A, out vec6 x, out vec6 y, out vec6 z, in vec6 b) { + mat6 L; + L.v[0].v[0] = 1.0; + L.v[0].v[1] = 0.0; + L.v[0].v[2] = 0.0; + L.v[0].v[3] = 0.0; + L.v[0].v[4] = 0.0; + L.v[0].v[5] = 0.0; + + L.v[1].v[0] = 0.0; + L.v[1].v[1] = 1.0; + L.v[1].v[2] = 0.0; + L.v[1].v[3] = 0.0; + L.v[1].v[4] = 0.0; + L.v[1].v[5] = 0.0; + + L.v[2].v[0] = 0.0; + L.v[2].v[1] = 0.0; + L.v[2].v[2] = 1.0; + L.v[2].v[3] = 0.0; + L.v[2].v[4] = 0.0; + L.v[2].v[5] = 0.0; + + L.v[3].v[0] = 0.0; + L.v[3].v[1] = 0.0; + L.v[3].v[2] = 0.0; + L.v[3].v[3] = 1.0; + L.v[3].v[4] = 0.0; + L.v[3].v[5] = 0.0; + + L.v[4].v[0] = 0.0; + L.v[4].v[1] = 0.0; + L.v[4].v[2] = 0.0; + L.v[4].v[3] = 0.0; + L.v[4].v[4] = 1.0; + L.v[4].v[5] = 0.0; + + L.v[5].v[0] = 0.0; + L.v[5].v[1] = 0.0; + L.v[5].v[2] = 0.0; + L.v[5].v[3] = 0.0; + L.v[5].v[4] = 0.0; + L.v[5].v[5] = 1.0; + + vec6 D; + D.v[0] = 0.0; + D.v[1] = 0.0; + D.v[2] = 0.0; + D.v[3] = 0.0; + D.v[4] = 0.0; + D.v[5] = 0.0; + + + // Decomposition + for (int i = 0; i < 6; i++) { + // Compute diagonal elements of D + D.v[i] = A.v[i].v[i]; + for (int k = 0; k < i; k++) { + D.v[i] -= L.v[i].v[k] * L.v[i].v[k] * D.v[k]; + } + // Compute elements of L (below the diagonal) + for (int j = i + 1; j < 6; j++) { + L.v[j].v[i] = A.v[j].v[i]; + for (int k = 0; k < i; k++) { + L.v[j].v[i] -= L.v[j].v[k] * L.v[i].v[k] * D.v[k]; + } + L.v[j].v[i] /= D.v[i]; + } + } + + // Forward substitution + for (int i = 0; i < 6; i++) { + y.v[i] = b.v[i]; + for (int j = 0; j < i; j++) { + y.v[i] -= L.v[i].v[j] * y.v[j]; + } + } + + // Diagonal solve + for (int i = 0; i < 6; i++) { + z.v[i] = y.v[i] / D.v[i]; + } + + // Backward substitution + for (int i = 5; i >= 0; i--) { + x.v[i] = z.v[i]; + for (int j = i + 1; j < 6; j++) { + x.v[i] -= L.v[j].v[i] * x.v[j]; + } + } +} + +void solve_stretch_bending_twisting_constraints(in int constraint_handle, in float inv_mass0, in vec3 x0, + in mat3 inverse_inertia0, in vec4 q0, in float inv_mass1, in vec3 x1, + in mat3 inverse_inertia1, in vec4 q1, in vec3 rest_darboux_vector, + in float average_segment_length, in vec3 stretch_compliance, + in vec3 bending_and_torsion_compliance, in mat4 constraint_info, + out vec3 x0_correction, out vec4 q0_correction, out vec3 x1_correction, + out vec4 q1_correction, inout vec6 lambda_sum) { + + + vec3 omega = compute_darboux_vector(q0, q1, average_segment_length); + mat4 j_omega0, j_omega1; + compute_bending_and_torsion_jacobians(q0, q1, average_segment_length, j_omega0, j_omega1); + mat4 g0, g1; + compute_matrix_g(q0, g0); + compute_matrix_g(q1, g1); + + mat3 j_omega_g0 = mat3(j_omega0 * g0); + mat3 j_omega_g1 = mat3(j_omega1 * g1); + + vec3 connector0 = constraint_info[2].xyz; + vec3 connector1 = constraint_info[3].xyz; + + vec3 stretch_violation = connector0 - connector1; + vec3 bending_and_torsion_violation = omega - rest_darboux_vector; + + + stretch_violation = stretch_violation + stretch_compliance * vec3(lambda_sum.v[0], lambda_sum.v[1], lambda_sum.v[2]); + bending_and_torsion_violation = + bending_and_torsion_violation + + bending_and_torsion_compliance * vec3(lambda_sum.v[3], lambda_sum.v[4], lambda_sum.v[5]); + + vec6 rhs; + rhs.v[0] = -stretch_violation.x; + rhs.v[1] = -stretch_violation.y; + rhs.v[2] = -stretch_violation.z; + rhs.v[3] = -bending_and_torsion_violation.x; + rhs.v[4] = -bending_and_torsion_violation.y; + rhs.v[5] = -bending_and_torsion_violation.z; + + mat3 k0, k1; + compute_matrix_k(connector0, inv_mass0, x0, inverse_inertia0, k0); + compute_matrix_k(connector1, inv_mass1, x1, inverse_inertia1, k1); + mat3 k = k0 + k1; + mat6 jmjt; + jmjt.v[0].v[0] = k[0][0]; + jmjt.v[0].v[1] = k[0][1]; + jmjt.v[0].v[2] = k[0][2]; + jmjt.v[1].v[0] = k[1][0]; + jmjt.v[1].v[1] = k[1][1]; + jmjt.v[1].v[2] = k[1][2]; + jmjt.v[2].v[0] = k[2][0]; + jmjt.v[2].v[1] = k[2][1]; + jmjt.v[2].v[2] = k[2][2]; + + vec3 r0 = connector0 - x0; + vec3 r1 = connector1 - x1; + + mat3 r0_cross_t = cross_product_matrix(-r0); + mat3 r1_cross_t = cross_product_matrix(-r1); + + mat3 off_diag = mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); + if (inv_mass0 != 0.0) { + off_diag = j_omega_g0 * inverse_inertia0 * r0_cross_t * -1.0; + } + if (inv_mass1 != 0.0) { + off_diag += j_omega_g1 * inverse_inertia1 * r1_cross_t; + } + jmjt.v[3].v[0] = off_diag[0][0]; + jmjt.v[3].v[1] = off_diag[0][1]; + jmjt.v[3].v[2] = off_diag[0][2]; + jmjt.v[4].v[0] = off_diag[1][0]; + jmjt.v[4].v[1] = off_diag[1][1]; + jmjt.v[4].v[2] = off_diag[1][2]; + jmjt.v[5].v[0] = off_diag[2][0]; + jmjt.v[5].v[1] = off_diag[2][1]; + jmjt.v[5].v[2] = off_diag[2][2]; + + mat3 off_diag_t = transpose(off_diag); + jmjt.v[0].v[3] = off_diag_t[0][0]; + jmjt.v[0].v[4] = off_diag_t[0][1]; + jmjt.v[0].v[5] = off_diag_t[0][2]; + jmjt.v[1].v[3] = off_diag_t[1][0]; + jmjt.v[1].v[4] = off_diag_t[1][1]; + jmjt.v[1].v[5] = off_diag_t[1][2]; + jmjt.v[2].v[3] = off_diag_t[2][0]; + jmjt.v[2].v[4] = off_diag_t[2][1]; + jmjt.v[2].v[5] = off_diag_t[2][2]; + + mat3 m_inv_jt0 = inverse_inertia0 * transpose(j_omega_g0); + mat3 m_inv_jt1 = inverse_inertia1 * transpose(j_omega_g1); + + mat3 jmjt_omega = mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); + if (inv_mass0 != 0.0) { + jmjt_omega = j_omega_g0 * m_inv_jt0; + } + if (inv_mass1 != 0.0) { + jmjt_omega += j_omega_g1 * m_inv_jt1; + } + + jmjt.v[3].v[3] = jmjt_omega[0][0]; + jmjt.v[3].v[4] = jmjt_omega[0][1]; + jmjt.v[3].v[5] = jmjt_omega[0][2]; + jmjt.v[4].v[3] = jmjt_omega[1][0]; + jmjt.v[4].v[4] = jmjt_omega[1][1]; + jmjt.v[4].v[5] = jmjt_omega[1][2]; + jmjt.v[5].v[3] = jmjt_omega[2][0]; + jmjt.v[5].v[4] = jmjt_omega[2][1]; + jmjt.v[5].v[5] = jmjt_omega[2][2]; + + jmjt.v[0].v[0] += stretch_compliance[0]; + jmjt.v[1].v[1] += stretch_compliance[1]; + jmjt.v[2].v[2] += stretch_compliance[2]; + jmjt.v[3].v[3] += bending_and_torsion_compliance[0]; + jmjt.v[4].v[4] += bending_and_torsion_compliance[1]; + jmjt.v[5].v[5] += bending_and_torsion_compliance[2]; + + vec6 delta_lambda, y, z; + + solve_ldlt(jmjt, delta_lambda, y, z, rhs); + + lambda_sum.v[0] += delta_lambda.v[0]; + lambda_sum.v[1] += delta_lambda.v[1]; + lambda_sum.v[2] += delta_lambda.v[2]; + lambda_sum.v[3] += delta_lambda.v[3]; + lambda_sum.v[4] += delta_lambda.v[4]; + lambda_sum.v[5] += delta_lambda.v[5]; + + vec3 delta_lambda_stretch = vec3(delta_lambda.v[0], delta_lambda.v[1], delta_lambda.v[2]); + vec3 delta_lambda_bending_and_torsion = vec3(delta_lambda.v[3], delta_lambda.v[4], delta_lambda.v[5]); + + x0_correction = vec3(0, 0, 0); + q0_correction = vec4(0, 0, 0, 0); + x1_correction = vec3(0, 0, 0); + q1_correction = vec4(0, 0, 0, 0); + + if (inv_mass0 != 0.0) { + x0_correction += inv_mass0 * delta_lambda_stretch; + vec3 v = + inverse_inertia0 * r0_cross_t * (-1.0 * delta_lambda_stretch) + m_inv_jt0 * delta_lambda_bending_and_torsion; + q0_correction[0] = g0[0][0] * v[0] + g0[0][1] * v[1] + g0[0][2] * v[2]; + q0_correction[1] = g0[1][0] * v[0] + g0[1][1] * v[1] + g0[1][2] * v[2]; + q0_correction[2] = g0[2][0] * v[0] + g0[2][1] * v[1] + g0[2][2] * v[2]; + q0_correction[3] = g0[3][0] * v[0] + g0[3][1] * v[1] + g0[3][2] * v[2]; + } + + if (inv_mass1 != 0.0) { + x1_correction -= inv_mass1 * delta_lambda_stretch; + vec3 v = inverse_inertia1 * r1_cross_t * delta_lambda_stretch + m_inv_jt1 * delta_lambda_bending_and_torsion; + q1_correction[0] = g1[0][0] * v[0] + g1[0][1] * v[1] + g1[0][2] * v[2]; + q1_correction[1] = g1[1][0] * v[0] + g1[1][1] * v[1] + g1[1][2] * v[2]; + q1_correction[2] = g1[2][0] * v[0] + g1[2][1] * v[1] + g1[2][2] * v[2]; + q1_correction[3] = g1[3][0] * v[0] + g1[3][1] * v[1] + g1[3][2] * v[2]; + } + + + RodConstraint rod_constraint = rod_constraints[constraint_handle]; + rod_constraint.omega.xyz = omega; + rod_constraint.j_omega0 = j_omega0; + rod_constraint.j_omega1 = j_omega1; + rod_constraint.g0 = g0; + rod_constraint.g1 = g1; + rod_constraint.j_omega_g0 = mat4_cast(j_omega_g0); + rod_constraint.j_omega_g1 = mat4_cast(j_omega_g1); + + rod_constraint.connector0.xyz = connector0; + rod_constraint.connector1.xyz = connector1; + + rod_constraint.stretch_violation.xyz = stretch_violation; + rod_constraint.bending_and_torsion_violation.xyz = bending_and_torsion_violation; + + rod_constraint.delta_lambda_stretch.xyz = delta_lambda_stretch; + rod_constraint.delta_lambda_bending_and_torsion.xyz = delta_lambda_bending_and_torsion; + + rod_constraint.x0_correction.xyz = x0_correction; + rod_constraint.x1_correction.xyz = x1_correction; + + rod_constraint.q0_correction = q0_correction; + rod_constraint.q1_correction = q1_correction; + + rod_constraint.x[0] = delta_lambda.v[0]; + rod_constraint.x[1] = delta_lambda.v[1]; + rod_constraint.x[2] = delta_lambda.v[2]; + rod_constraint.x[3] = delta_lambda.v[3]; + rod_constraint.x[4] = delta_lambda.v[4]; + rod_constraint.x[5] = delta_lambda.v[5]; + + rod_constraint.y[0] = y.v[0]; + rod_constraint.y[1] = y.v[1]; + rod_constraint.y[2] = y.v[2]; + rod_constraint.y[3] = y.v[3]; + rod_constraint.y[4] = y.v[4]; + rod_constraint.y[5] = y.v[5]; + + rod_constraint.z[0] = z.v[0]; + rod_constraint.z[1] = z.v[1]; + rod_constraint.z[2] = z.v[2]; + rod_constraint.z[3] = z.v[3]; + rod_constraint.z[4] = z.v[4]; + rod_constraint.z[5] = z.v[5]; + + rod_constraint.rhs[0] = rhs.v[0]; + rod_constraint.rhs[1] = rhs.v[1]; + rod_constraint.rhs[2] = rhs.v[2]; + rod_constraint.rhs[3] = rhs.v[3]; + rod_constraint.rhs[4] = rhs.v[4]; + rod_constraint.rhs[5] = rhs.v[5]; + + rod_constraints[constraint_handle] = rod_constraint; +} + + void project_constraint(in int rod_constraint_handle); void main(){ @@ -41,11 +401,10 @@ void main(){ PerStrandData strand_data = per_strand_data_list[strand_index]; int rod_constraint_handle = strand_data.begin_rod_constraint_handle; - while(rod_constraint_handle != strand_data.end_rod_constraint_handle){ + while(rod_constraint_handle != -1){ project_constraint(rod_constraint_handle); rod_constraint_handle = floatBitsToInt(rod_constraints[rod_constraint_handle].bending_and_torsion_compliance_next_constraint_handle.w); } - project_constraint(rod_constraint_handle); } } @@ -71,7 +430,7 @@ void project_constraint(in int rod_constraint_handle){ inverse_inertia_tensor_w0 = mat3(segment0.inv_inertia_w); inverse_inertia_tensor_w1 = mat3(segment1.inv_inertia_w); - solve_stretch_bending_twisting_constraints( + solve_stretch_bending_twisting_constraints(rod_constraint_handle, segment0.inv_inertia_tensor_inv_mass.w, segment0.x.xyz, inverse_inertia_tensor_w0, segment0.q, segment1.inv_inertia_tensor_inv_mass.w, segment1.x.xyz, inverse_inertia_tensor_w1, segment1.q, rod_constraint.rest_darboux_vector_segment1_index.xyz, diff --git a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodUpdateConstraints.comp b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodUpdateConstraints.comp index d4cb359..855e9ba 100644 --- a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodUpdateConstraints.comp +++ b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Constraints/StiffRodUpdateConstraints.comp @@ -28,6 +28,24 @@ layout(push_constant) uniform PUSH_CONSTANTS { #define DYNAMIC_STRANDS_PHYSICS_SET 1 #include "DynamicStrandsPhysics.glsl" +void update_stretch_bending_twisting_constraint(in vec3 x0, in vec4 q0, in vec3 x1, in vec4 q1, + inout mat4 constraint_info) { + // constraintInfo contains + // 0: connector in segment 0 (local) + // 1: connector in segment 1 (local) + // 2: connector in segment 0 (global) + // 3: connector in segment 1 (global) + + // compute world space positions of connectors + mat3 rot0 = mat3_cast(q0); + mat3 rot1 = mat3_cast(q1); + + constraint_info[2].xyz = rot0 * constraint_info[0].xyz + x0; + constraint_info[3].xyz = rot1 * constraint_info[1].xyz + x1; +} + + + void update_constraint(in uint rod_constraint_handle); void main(){ diff --git a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Operators/ExternalForce.comp b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Operators/ExternalForce.comp index bcc60d5..f2efceb 100644 --- a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Operators/ExternalForce.comp +++ b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Compute/DynamicStrands/Operators/ExternalForce.comp @@ -32,7 +32,6 @@ layout(std430, set = 1, binding = 0) readonly buffer EXTERNAL_FORCE_BLOCK { layout(push_constant) uniform PUSH_CONSTANTS { uint command_size; - float delta_time; }; void main(){ diff --git a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Includes/DynamicStrandsPhysics.glsl b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Includes/DynamicStrandsPhysics.glsl index 731afd8..058f3b1 100644 --- a/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Includes/DynamicStrandsPhysics.glsl +++ b/EvoEngine_Plugins/EcoSysLab/Internals/EcoSysLabResources/Shaders/Includes/DynamicStrandsPhysics.glsl @@ -51,6 +51,37 @@ struct RodConstraint { float lambda_sum5; float padding0; float padding1; + + vec4 omega; + + mat4 j_omega0; + mat4 j_omega1; + + mat4 g0; + mat4 g1; + + mat4 j_omega_g0; + mat4 j_omega_g1; + + vec4 connector0; + vec4 connector1; + + vec4 stretch_violation; + vec4 bending_and_torsion_violation; + + vec4 delta_lambda_stretch; + vec4 delta_lambda_bending_and_torsion; + + vec4 x0_correction; + vec4 x1_correction; + + vec4 q0_correction; + vec4 q1_correction; + + float x[8]; + float y[8]; + float z[8]; + float rhs[8]; }; layout(std430, set = DYNAMIC_STRANDS_PHYSICS_SET, binding = 2) buffer ROD_CONSTRAINT_BLOCK { @@ -73,360 +104,11 @@ struct mat6 { vec6 v[6]; }; -void compute_bending_and_torsion_jacobians(in vec4 q0, in vec4 q1, float segment_length, out mat4 j_omega_0, - out mat4 j_omega_1); -void compute_matrix_g(in vec4 q, out mat4 g); -void compute_matrix_k(in vec3 connector, in float inv_mass, in vec3 x, in mat3 inverse_inertia, out mat3 k); -mat3 cross_product_matrix(in vec3 v); -void solve_ldlt(in mat6 a, out vec6 x, in vec6 b); - -mat3 compute_inverse_inertia_tensor_w(in StrandSegment strand_segment); - -void init_stretch_bending_twisting_constraint(in vec3 stiffness_coefficient_k, in float inverse_time_step_size, - in float average_segment_length, out vec3 stretch_compliance, - out vec3 bending_and_torsion_compliance, out vec6 lambda_sum); - -void update_stretch_bending_twisting_constraint(in vec3 x0, in vec4 q0, in vec3 x1, in vec4 q1, - inout mat4 constraint_info); - -void solve_stretch_bending_twisting_constraints(in float inv_mass0, in vec3 x0, in mat3 inverse_inertia0, in vec4 q0, - in float inv_mass1, in vec3 x1, in mat3 inverse_inertia1, in vec4 q1, - in vec3 rest_darboux_vector, in float average_segment_length, - in vec3 stretch_compliance, in vec3 bending_and_torsion_compliance, - in mat4 constraint_info, out vec3 x0_correction, out vec4 q0_correction, - out vec3 x1_correction, out vec4 q1_correction, inout vec6 lambda_sum); - -//------------------- -//| Implementations | -//------------------- - -void compute_bending_and_torsion_jacobians(in vec4 q0, in vec4 q1, float segment_length, out mat4 j_omega_0, - out mat4 j_omega_1) { - j_omega_0 = mat4(-q1[3], -q1[2], q1[1], q1[0], q1[2], -q1[3], -q1[0], q1[1], -q1[1], q1[0], -q1[3], q1[2], 0.0, 0.0, - 0.0, 0.0); - j_omega_1 = mat4(q0[3], q0[2], -q0[1], -q0[0], -q0[2], q0[3], q0[0], -q0[1], q0[1], -q0[0], q0[3], -q0[2], 0.0, 0.0, - 0.0, 0.0); - j_omega_0 = j_omega_0 * 2.0 / segment_length; - j_omega_1 = j_omega_1 * 2.0 / segment_length; -} - -void compute_matrix_g(in vec4 q, out mat4 g) { - g = mat4(0.5 * q[3], 0.5 * q[2], -0.5 * q[1], 0.0, -0.5 * q[2], 0.5 * q[3], 0.5 * q[0], 0.0, 0.5 * q[1], -0.5 * q[0], - 0.5 * q[3], 0.0, -0.5 * q[0], -0.5 * q[1], -0.5 * q[2], 0.0); -} - -void compute_matrix_k(in vec3 connector, in float inv_mass, in vec3 x, in mat3 inverse_inertia, out mat3 k) { - if (inv_mass != 0.0) { - vec3 v = connector - x; - float a = v[0]; - float b = v[1]; - float c = v[2]; - - float j11 = inverse_inertia[0][0]; - float j12 = inverse_inertia[0][1]; - float j13 = inverse_inertia[0][2]; - float j22 = inverse_inertia[1][1]; - float j23 = inverse_inertia[1][2]; - float j33 = inverse_inertia[2][2]; - - k[0][0] = c * c * j22 - b * c * (j23 + j23) + b * b * j33 + inv_mass; - k[0][1] = -(c * c * j12) + a * c * j23 + b * c * j13 - a * b * j33; - k[0][2] = b * c * j12 - a * c * j22 - b * b * j13 + a * b * j23; - k[1][0] = k[0][1]; - k[1][1] = c * c * j11 - a * c * (j13 + j13) + a * a * j33 + inv_mass; - k[1][2] = -(b * c * j11) + a * c * j12 + a * b * j13 - a * a * j23; - k[2][0] = k[0][2]; - k[2][1] = k[1][2]; - k[2][2] = b * b * j11 - a * b * (j12 + j12) + a * a * j22 + inv_mass; - } else { - k = mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); - } -} - -mat3 cross_product_matrix(in vec3 v) { - return mat3(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0); -} - -void solve_ldlt(in mat6 A, out vec6 x, in vec6 b) { - mat6 L; - L.v[0].v[0] = 1.0; - L.v[0].v[1] = 1.0; - L.v[0].v[2] = 1.0; - L.v[0].v[3] = 1.0; - L.v[0].v[4] = 1.0; - L.v[0].v[5] = 1.0; - - L.v[1].v[0] = 1.0; - L.v[1].v[1] = 1.0; - L.v[1].v[2] = 1.0; - L.v[1].v[3] = 1.0; - L.v[1].v[4] = 1.0; - L.v[1].v[5] = 1.0; - - L.v[2].v[0] = 1.0; - L.v[2].v[1] = 1.0; - L.v[2].v[2] = 1.0; - L.v[2].v[3] = 1.0; - L.v[2].v[4] = 1.0; - L.v[2].v[5] = 1.0; - - L.v[3].v[0] = 1.0; - L.v[3].v[1] = 1.0; - L.v[3].v[2] = 1.0; - L.v[3].v[3] = 1.0; - L.v[3].v[4] = 1.0; - L.v[3].v[5] = 1.0; - - L.v[4].v[0] = 1.0; - L.v[4].v[1] = 1.0; - L.v[4].v[2] = 1.0; - L.v[4].v[3] = 1.0; - L.v[4].v[4] = 1.0; - L.v[4].v[5] = 1.0; - - L.v[5].v[0] = 1.0; - L.v[5].v[1] = 1.0; - L.v[5].v[2] = 1.0; - L.v[5].v[3] = 1.0; - L.v[5].v[4] = 1.0; - L.v[5].v[5] = 1.0; - - vec6 D; - D.v[0] = 0.0; - D.v[1] = 0.0; - D.v[2] = 0.0; - D.v[3] = 0.0; - D.v[4] = 0.0; - D.v[5] = 0.0; - - // Decomposition - for (int j = 0; j < 6; j++) { - // Compute diagonal elements of D - D.v[j] = A.v[j].v[j]; - for (int k = 0; k < j; k++) { - D.v[j] -= L.v[j].v[k] * L.v[j].v[k] * D.v[k]; - } - // Compute elements of L (below the diagonal) - for (int i = j + 1; i < 6; i++) { - L.v[i].v[j] = A.v[i].v[j]; - for (int k = 0; k < j; k++) { - L.v[i].v[j] -= L.v[i].v[k] * L.v[j].v[k] * D.v[k]; - } - L.v[i].v[j] /= D.v[j]; - } - } - - // Forward substitution - vec6 y; - for (int i = 0; i < 6; i++) { - y.v[i] = b.v[i]; - for (int j = 0; j < i; j++) { - y.v[i] -= L.v[i].v[j] * y.v[j]; - } - } - - // Diagonal solve - vec6 z; - for (int i = 0; i < 6; i++) { - z.v[i] = y.v[i] / D.v[i]; - } - - // Backward substitution - for (int i = 5; i >= 0; i--) { - x.v[i] = z.v[i]; - for (int j = i + 1; j < 6; j++) { - x.v[i] -= L.v[j].v[i] * x.v[j]; - } - } -} mat3 compute_inverse_inertia_tensor_w(in StrandSegment strand_segment) { mat3 rot = mat3_cast(strand_segment.q); mat3 diag = - mat3( - strand_segment.inv_inertia_tensor_inv_mass.x, 0.0, 0.0, - 0.0, strand_segment.inv_inertia_tensor_inv_mass.y, 0.0, - 0.0, 0.0, strand_segment.inv_inertia_tensor_inv_mass.z); + mat3(strand_segment.inv_inertia_tensor_inv_mass.x, 0.0, 0.0, 0.0, strand_segment.inv_inertia_tensor_inv_mass.y, + 0.0, 0.0, 0.0, strand_segment.inv_inertia_tensor_inv_mass.z); return rot * diag * transpose(rot); -} - -void init_stretch_bending_twisting_constraint(in vec3 stiffness_coefficient_k, in float inverse_time_step_size, - in float average_segment_length, out vec3 stretch_compliance, - out vec3 bending_and_torsion_compliance, out vec6 lambda_sum) { - float inverse_time_step_quadratic = inverse_time_step_size * inverse_time_step_size; - float stretch_regularization_parameter = 0.1; - - stretch_compliance = vec3(stretch_regularization_parameter * inverse_time_step_quadratic, - stretch_regularization_parameter * inverse_time_step_quadratic, - stretch_regularization_parameter * inverse_time_step_quadratic); - - bending_and_torsion_compliance = vec3(inverse_time_step_quadratic / stiffness_coefficient_k[0], - inverse_time_step_quadratic / stiffness_coefficient_k[1], - inverse_time_step_quadratic / stiffness_coefficient_k[2]); - - lambda_sum.v[0] = 0.0; - lambda_sum.v[1] = 0.0; - lambda_sum.v[2] = 0.0; - lambda_sum.v[3] = 0.0; - lambda_sum.v[4] = 0.0; - lambda_sum.v[5] = 0.0; -} - -void update_stretch_bending_twisting_constraint(in vec3 x0, in vec4 q0, in vec3 x1, in vec4 q1, - inout mat4 constraint_info) { - // constraintInfo contains - // 0: connector in segment 0 (local) - // 1: connector in segment 1 (local) - // 2: connector in segment 0 (global) - // 3: connector in segment 1 (global) - - // compute world space positions of connectors - mat3 rot0 = mat3_cast(q0); - mat3 rot1 = mat3_cast(q1); - - constraint_info[2].xyz = rot0 * constraint_info[0].xyz + x0; - constraint_info[3].xyz = rot1 * constraint_info[1].xyz + x1; -} - -void solve_stretch_bending_twisting_constraints(in float inv_mass0, in vec3 x0, in mat3 inverse_inertia0, in vec4 q0, - in float inv_mass1, in vec3 x1, in mat3 inverse_inertia1, in vec4 q1, - in vec3 rest_darboux_vector, in float average_segment_length, - in vec3 stretch_compliance, in vec3 bending_and_torsion_compliance, - in mat4 constraint_info, out vec3 x0_correction, out vec4 q0_correction, - out vec3 x1_correction, out vec4 q1_correction, inout vec6 lambda_sum) { - vec3 omega = compute_darboux_vector(q0, q1, average_segment_length); - mat4 j_omega0, j_omega1; - compute_bending_and_torsion_jacobians(q0, q1, average_segment_length, j_omega0, j_omega1); - mat4 g0, g1; - compute_matrix_g(q0, g0); - compute_matrix_g(q1, g1); - - mat3 j_omega_g0 = mat3(j_omega0 * g0); - mat3 j_omega_g1 = mat3(j_omega1 * g1); - - vec3 connector0 = constraint_info[2].xyz; - vec3 connector1 = constraint_info[3].xyz; - - vec3 stretch_violation = connector0 - connector1; - vec3 bending_and_torsion_violation = omega - rest_darboux_vector; - - stretch_violation = stretch_violation + stretch_compliance * vec3(lambda_sum.v[0], lambda_sum.v[1], lambda_sum.v[2]); - bending_and_torsion_violation = - bending_and_torsion_violation + - bending_and_torsion_compliance * vec3(lambda_sum.v[3], lambda_sum.v[4], lambda_sum.v[5]); - - vec6 rhs; - rhs.v[0] = -stretch_violation.x; - rhs.v[1] = -stretch_violation.y; - rhs.v[2] = -stretch_violation.z; - rhs.v[3] = -bending_and_torsion_violation.x; - rhs.v[4] = -bending_and_torsion_violation.y; - rhs.v[5] = -bending_and_torsion_violation.z; - - mat3 k0, k1; - compute_matrix_k(connector0, inv_mass0, x0, inverse_inertia0, k0); - compute_matrix_k(connector1, inv_mass1, x1, inverse_inertia1, k1); - mat3 k = k0 + k1; - mat6 jmjt; - jmjt.v[0].v[0] = k[0][0]; - jmjt.v[0].v[1] = k[0][1]; - jmjt.v[0].v[2] = k[0][2]; - jmjt.v[1].v[0] = k[1][0]; - jmjt.v[1].v[1] = k[1][1]; - jmjt.v[1].v[2] = k[1][2]; - jmjt.v[2].v[0] = k[2][0]; - jmjt.v[2].v[1] = k[2][1]; - jmjt.v[2].v[2] = k[2][2]; - - vec3 r0 = connector0 - x0; - vec3 r1 = connector1 - x1; - - mat3 r0_cross_t = cross_product_matrix(-r0); - mat3 r1_cross_t = cross_product_matrix(-r1); - - mat3 off_diag = mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); - if (inv_mass0 != 0.0) { - off_diag = j_omega_g0 * inverse_inertia0 * r0_cross_t * -1.0; - } - if (inv_mass1 != 0.0) { - off_diag = off_diag + j_omega_g1 * inverse_inertia1 * r1_cross_t * -1.0; - } - jmjt.v[3].v[0] = off_diag[0][0]; - jmjt.v[3].v[1] = off_diag[0][1]; - jmjt.v[3].v[2] = off_diag[0][2]; - jmjt.v[4].v[0] = off_diag[1][0]; - jmjt.v[4].v[1] = off_diag[1][1]; - jmjt.v[4].v[2] = off_diag[1][2]; - jmjt.v[5].v[0] = off_diag[2][0]; - jmjt.v[5].v[1] = off_diag[2][1]; - jmjt.v[5].v[2] = off_diag[2][2]; - - mat3 off_diag_t = transpose(off_diag); - jmjt.v[0].v[3] = off_diag_t[0][0]; - jmjt.v[0].v[4] = off_diag_t[0][1]; - jmjt.v[0].v[5] = off_diag_t[0][2]; - jmjt.v[1].v[3] = off_diag_t[1][0]; - jmjt.v[1].v[4] = off_diag_t[1][1]; - jmjt.v[1].v[5] = off_diag_t[1][2]; - jmjt.v[2].v[3] = off_diag_t[2][0]; - jmjt.v[2].v[4] = off_diag_t[2][1]; - jmjt.v[2].v[5] = off_diag_t[2][2]; - - mat3 m_inv_jt0 = inverse_inertia0 * transpose(j_omega_g0); - mat3 m_inv_jt1 = inverse_inertia1 * transpose(j_omega_g1); - - mat3 jmjt_omega; - if (inv_mass0 != 0.0) { - jmjt_omega = j_omega_g0 * m_inv_jt0; - } - if (inv_mass1 != 0.0) { - jmjt_omega = jmjt_omega + j_omega_g1 * m_inv_jt1; - } - - jmjt.v[3].v[3] = jmjt_omega[0][0]; - jmjt.v[3].v[4] = jmjt_omega[0][1]; - jmjt.v[3].v[5] = jmjt_omega[0][2]; - jmjt.v[4].v[3] = jmjt_omega[1][0]; - jmjt.v[4].v[4] = jmjt_omega[1][1]; - jmjt.v[4].v[5] = jmjt_omega[1][2]; - jmjt.v[5].v[3] = jmjt_omega[2][0]; - jmjt.v[5].v[4] = jmjt_omega[2][1]; - jmjt.v[5].v[5] = jmjt_omega[2][2]; - - vec6 delta_lambda; - solve_ldlt(jmjt, delta_lambda, rhs); - - lambda_sum.v[0] += delta_lambda.v[0]; - lambda_sum.v[1] += delta_lambda.v[1]; - lambda_sum.v[2] += delta_lambda.v[2]; - lambda_sum.v[3] += delta_lambda.v[3]; - lambda_sum.v[4] += delta_lambda.v[4]; - lambda_sum.v[5] += delta_lambda.v[5]; - - vec3 delta_lambda_stretch = vec3(delta_lambda.v[0], delta_lambda.v[1], delta_lambda.v[2]); - vec3 delta_lambda_bending_and_torsion = vec3(delta_lambda.v[3], delta_lambda.v[4], delta_lambda.v[5]); - - x0_correction = vec3(0, 0, 0); - q0_correction = vec4(0, 0, 0, 0); - x1_correction = vec3(0, 0, 0); - q1_correction = vec4(0, 0, 0, 0); - - if (inv_mass0 != 0.0) { - x0_correction = inv_mass0 * delta_lambda_stretch; - vec3 v = - inverse_inertia0 * r0_cross_t * (-1.0 * delta_lambda_stretch) + m_inv_jt0 * delta_lambda_bending_and_torsion; - q0_correction[0] = g0[0][0] * v[0] + g0[0][1] * v[1] + g0[0][2] * v[2]; - q0_correction[1] = g0[1][0] * v[0] + g0[1][1] * v[1] + g0[1][2] * v[2]; - q0_correction[2] = g0[2][0] * v[0] + g0[2][1] * v[1] + g0[2][2] * v[2]; - q0_correction[3] = g0[3][0] * v[0] + g0[3][1] * v[1] + g0[3][2] * v[2]; - } - - if (inv_mass1 != 0.0) { - x1_correction = -inv_mass1 * delta_lambda_stretch; - vec3 v = inverse_inertia1 * r1_cross_t * delta_lambda_stretch + m_inv_jt1 * delta_lambda_bending_and_torsion; - q1_correction[0] = g1[0][0] * v[0] + g1[0][1] * v[1] + g1[0][2] * v[2]; - q1_correction[1] = g1[1][0] * v[0] + g1[1][1] * v[1] + g1[1][2] * v[2]; - q1_correction[2] = g1[2][0] * v[0] + g1[2][1] * v[1] + g1[2][2] * v[2]; - q1_correction[3] = g1[3][0] * v[0] + g1[3][1] * v[1] + g1[3][2] * v[2]; - } } \ No newline at end of file diff --git a/EvoEngine_Plugins/EcoSysLab/include/StrandModel/BrokenBranch.hpp b/EvoEngine_Plugins/EcoSysLab/include/StrandModel/BrokenBranch.hpp index 0df2758..2d28925 100644 --- a/EvoEngine_Plugins/EcoSysLab/include/StrandModel/BrokenBranch.hpp +++ b/EvoEngine_Plugins/EcoSysLab/include/StrandModel/BrokenBranch.hpp @@ -4,17 +4,17 @@ using namespace evo_engine; namespace eco_sys_lab_plugin { +class DsPositionUpdate; +class DsExternalForce; class BrokenBranch : public IPrivateComponent { public: - - bool enable_simulation = true; - PrivateComponentRef tree_ref{}; StrandModelStrandGroup strand_group{}; StrandModelStrandGroup subdivided_strand_group{}; - DynamicStrands::InitializeParameters initialize_parameters{}; DynamicStrands::StepParameters step_parameters{}; + std::shared_ptr external_force; + std::shared_ptr position_update; DynamicStrands dynamic_strands{}; void UpdateDynamicStrands(); void Serialize(YAML::Emitter& out) const override; @@ -29,7 +29,7 @@ class BrokenBranch : public IPrivateComponent { void Subdivide(float segment_length, const StrandModelStrandGroup& src); void InitializeStrandParticles(const StrandModelStrandGroup& strand_group) const; void ClearStrandParticles() const; - + void Step(); void InitializeStrandConcaveMesh(const StrandModelStrandGroup& strand_group, float max_edge_length) const; void ClearStrandConcaveMesh() const; }; diff --git a/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrands.hpp b/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrands.hpp index 78c29ed..0982184 100644 --- a/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrands.hpp +++ b/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrands.hpp @@ -16,8 +16,9 @@ class DynamicStrands { struct InitializeParameters { bool static_root = true; float wood_density = 1.f; - float youngs_modulus = 1.f; - float torsion_modulus = 1.f; + + float youngs_modulus = 0.01f; //In GPa + float torsion_modulus = 0.01f; //In GPa GlobalTransform root_transform{}; }; template @@ -26,9 +27,10 @@ class DynamicStrands { #pragma endregion #pragma region Step struct PhysicsParameters { + bool gpu = false; float time_step = 0.01f; uint32_t sub_step = 1; - uint32_t max_iteration; + uint32_t max_iteration = 10; }; struct RenderParameters { @@ -83,13 +85,18 @@ class DynamicStrands { glm::vec3 x0; float length; // Current position - glm::vec4 x; + glm::vec3 x; + float padding1; + // Last frame position - glm::vec4 last_x; - glm::vec4 old_x; + glm::vec3 last_x; + float padding2; + + glm::vec3 old_x; + float padding3; glm::vec3 acceleration = glm::vec3(0.f); - float padding1; + float padding4; glm::vec3 inertia_tensor; float mass; @@ -98,7 +105,7 @@ class DynamicStrands { float inv_mass; glm::mat4 inertia_w; - glm::mat4 inverse_inertia_w; + glm::mat4 inv_inertia_w; int neighbors[8]; }; @@ -123,8 +130,8 @@ class DynamicStrands { static glm::vec3 ComputeInertiaTensorBox(float mass, float width, float height, float depth); static glm::vec3 ComputeInertiaTensorRod(float mass, float radius, float length); - void InitializeOperators(const InitializeParameters& initialize_parameters) const; - void InitializeConstraints(const InitializeParameters& initialize_parameters) const; + void InitializeOperators(const InitializeParameters& initialize_parameters); + void InitializeConstraints(const InitializeParameters& initialize_parameters); void Render(const RenderParameters& render_parameters) const; void Physics(const PhysicsParameters& physics_parameters, const std::vector>& target_operators, diff --git a/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrandsPhysics.hpp b/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrandsPhysics.hpp index 940fed9..c900aa6 100644 --- a/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrandsPhysics.hpp +++ b/EvoEngine_Plugins/EcoSysLab/include/StrandModel/DynamicStrandsPhysics.hpp @@ -15,7 +15,7 @@ class DynamicStrandsPreStep { inline static std::shared_ptr pre_step_pipeline; void Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands); + DynamicStrands& target_dynamic_strands); }; class DynamicStrandsPostStep { @@ -28,28 +28,32 @@ class DynamicStrandsPostStep { inline static std::shared_ptr post_step_pipeline; void Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands); + DynamicStrands& target_dynamic_strands); }; class IDynamicStrandsOperator { public: virtual ~IDynamicStrandsOperator() = default; virtual void InitializeData(const DynamicStrands::InitializeParameters& initialize_parameters, - const DynamicStrands& target_dynamic_strands) { + DynamicStrands& target_dynamic_strands) { } virtual void Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) = 0; + DynamicStrands& target_dynamic_strands) = 0; virtual void DownloadData() { } + virtual void UploadData() { + } }; class IDynamicStrandsConstraint { public: virtual void InitializeData(const DynamicStrands::InitializeParameters& initialize_parameters, - const DynamicStrands& target_dynamic_strands){}; + DynamicStrands& target_dynamic_strands){}; virtual void Project(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) = 0; + DynamicStrands& target_dynamic_strands) = 0; virtual void DownloadData() { } + virtual void UploadData() { + } }; #pragma region Operators @@ -75,7 +79,7 @@ class DsPositionUpdate final : public IDynamicStrandsOperator { std::vector> commands_descriptor_sets; void Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) override; + DynamicStrands& target_dynamic_strands) override; }; class DsExternalForce final : public IDynamicStrandsOperator { public: @@ -99,7 +103,7 @@ class DsExternalForce final : public IDynamicStrandsOperator { std::vector> commands_descriptor_sets{}; void Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) override; + DynamicStrands& target_dynamic_strands) override; }; #pragma endregion @@ -121,7 +125,12 @@ class DsStiffRod final : public IDynamicStrandsConstraint { float padding1; float padding2; }; - + struct vec6 { + float v[6]; + }; + struct mat6 { + vec6 v[6]; + }; struct GpuRodConstraint { glm::vec3 stiffness_coefficient_k; int segment0_index; @@ -133,7 +142,7 @@ class DsStiffRod final : public IDynamicStrandsConstraint { float average_segment_length; glm::vec3 bending_and_torsion_compliance; - int next_handle = -1; + int next_constraint_handle = -1; /** * \brief @@ -145,7 +154,53 @@ class DsStiffRod final : public IDynamicStrandsConstraint { */ glm::mat4 constraint_info; - float lambda_sum[8]; + vec6 lambda_sum; + float lambda_sum_padding[2]; + + //--------- + //| Debug | + //--------- + + glm::vec3 omega; + float padding_omega; + + glm::mat4 j_omega0; + glm::mat4 j_omega1; + glm::mat4 g0; + glm::mat4 g1; + + glm::mat4 j_omega_g0; + glm::mat4 j_omega_g1; + + glm::vec3 connector0; + float padding0; + glm::vec3 connector1; + float padding1; + + glm::vec3 stretch_violation; + float padding2; + glm::vec3 bending_and_torsion_violation; + float padding3; + + glm::vec3 delta_lambda_stretch; + float padding4; + glm::vec3 delta_lambda_bending_and_torsion; + float padding5; + + glm::vec3 x0_correction; + float padding6; + + glm::vec3 x1_correction; + float padding7; + + glm::quat q0_correction; + glm::quat q1_correction; + + float x[8]; + float y[8]; + float z[8]; + float rhs[8]; + }; struct PerStrandData { @@ -168,7 +223,6 @@ class DsStiffRod final : public IDynamicStrandsConstraint { struct StiffRodInitConstraintConstant { uint32_t constraint_size = 0; float time_step; - float inv_time_step; }; struct StiffRodUpdateConstraintConstant { @@ -186,11 +240,12 @@ class DsStiffRod final : public IDynamicStrandsConstraint { std::vector> strands_physics_descriptor_sets{}; void InitializeData(const DynamicStrands::InitializeParameters& initialize_parameters, - const DynamicStrands& target_dynamic_strands) override; + DynamicStrands& target_dynamic_strands) override; void Project(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) override; + DynamicStrands& target_dynamic_strands) override; void DownloadData() override; + void UploadData() override; static glm::vec3 ComputeDarbouxVector(const glm::quat& q0, const glm::quat& q1, float average_segment_length); }; #pragma endregion diff --git a/EvoEngine_Plugins/EcoSysLab/src/BrokenBranch.cpp b/EvoEngine_Plugins/EcoSysLab/src/BrokenBranch.cpp index ae52732..1b570d5 100644 --- a/EvoEngine_Plugins/EcoSysLab/src/BrokenBranch.cpp +++ b/EvoEngine_Plugins/EcoSysLab/src/BrokenBranch.cpp @@ -17,13 +17,15 @@ void BrokenBranch::UpdateDynamicStrands() { dynamic_strands.InitializeStrandsGroup(initialize_parameters, subdivided_strand_group); - const auto& external_force = std::dynamic_pointer_cast(dynamic_strands.operators.front()); + + external_force = std::make_shared(); external_force->commands.resize(dynamic_strands.strand_segments.size()); Jobs::RunParallelFor(external_force->commands.size(), [&](const size_t i) { external_force->commands[i].strand_segment_index = static_cast(i); - external_force->commands[i].force = glm::vec3(0, -1, 0) * dynamic_strands.strand_segments[i].mass; + external_force->commands[i].force = glm::vec3(0, 0, 0) * dynamic_strands.strand_segments[i].mass; }); - const auto& position_update = std::dynamic_pointer_cast(dynamic_strands.operators.back()); + position_update = std::make_shared(); + position_update->commands.resize(dynamic_strands.strands.size()); Jobs::RunParallelFor(position_update->commands.size(), [&](const size_t i) { position_update->commands[i].strand_segment_index = dynamic_strands.strands[i].begin_segment_handle; @@ -59,6 +61,7 @@ bool BrokenBranch::OnInspect(const std::shared_ptr& editor_layer) { if (ImGui::Button("Experiment")) { ExperimentSetup(); } + if (ImGui::TreeNode("Stats")) { ImGui::Text((std::string("Original strand count: ") + std::to_string(strand_group.PeekStrands().size())).c_str()); ImGui::Text( @@ -134,41 +137,31 @@ bool BrokenBranch::OnInspect(const std::shared_ptr& editor_layer) { step_parameters.physics = true; const bool resume_render = step_parameters.render; step_parameters.render = false; - dynamic_strands.Step(step_parameters); + Step(); step_parameters.physics = false; step_parameters.render = resume_render; } } + if (!step_parameters.physics && !step_parameters.render) { if (ImGui::Button("Download strands")) { dynamic_strands.Download(); EVOENGINE_LOG("Downloaded data from GPU") } + ImGui::SameLine(); + if (ImGui::Button("Upload strands")) { + dynamic_strands.Upload(); + EVOENGINE_LOG("Uploaded data from GPU") + } } return false; } + void BrokenBranch::LateUpdate() { - if (enable_simulation && !dynamic_strands.strand_segments.empty()) { - const auto owner = GetOwner(); - const auto scene = GetScene(); - const auto current_root_transform = scene->GetDataComponent(owner); - GlobalTransform original_inverse_root_transform; - original_inverse_root_transform.value = glm::inverse(initialize_parameters.root_transform.value); - const auto& position_update = std::dynamic_pointer_cast(dynamic_strands.operators.back()); - Jobs::RunParallelFor(position_update->commands.size(), [&](const size_t i) { - position_update->commands[i].strand_segment_index = dynamic_strands.strands[i].begin_segment_handle; - position_update->commands[i] - .new_position = current_root_transform.TransformPoint(original_inverse_root_transform.TransformPoint( - dynamic_strands.strand_segments[position_update->commands[i].strand_segment_index].x0)); - }); - const auto editor_layer = Application::GetLayer(); - step_parameters.render_parameters.target_camera = editor_layer->GetSceneCamera(); - dynamic_strands.Step(step_parameters); - } + Step(); } void BrokenBranch::FixedUpdate() { - } void BrokenBranch::OnCreate() { @@ -182,6 +175,8 @@ void BrokenBranch::OnCreate() { dynamic_strands.constraints.emplace_back(std::make_shared()); dynamic_strands.post_step = std::make_shared(); + + step_parameters.physics_parameters.gpu = true; } void BrokenBranch::OnDestroy() { @@ -195,11 +190,13 @@ void BrokenBranch::ExperimentSetup() { const auto strand1_handle = strand_group.AllocateStrand(); auto& segment1 = strand_group.RefStrandSegment(strand_group.Extend(strand1_handle)); auto& strand1 = strand_group.RefStrand(strand1_handle); - segment1.end_position = glm::vec3(0, 0.5f, 0.); + segment1.end_position = glm::vec3(0, 5.0f, 0.); strand1.start_color = segment1.end_color = glm::vec4(1, 0, 0, 0); - strand1.start_thickness = segment1.end_thickness = 0.01f; + strand1.start_thickness = segment1.end_thickness = 0.1f; strand_group.CalculateRotations(); + + Subdivide(2, strand_group); } void BrokenBranch::Subdivide(const float segment_length, const StrandModelStrandGroup& src) { @@ -245,6 +242,29 @@ void BrokenBranch::ClearStrandParticles() const { } } +void BrokenBranch::Step() { + if (!dynamic_strands.strand_segments.empty()) { + dynamic_strands.operators.clear(); + if (step_parameters.physics) { + const auto owner = GetOwner(); + const auto scene = GetScene(); + const auto current_root_transform = scene->GetDataComponent(owner); + GlobalTransform original_inverse_root_transform; + original_inverse_root_transform.value = glm::inverse(initialize_parameters.root_transform.value); + Jobs::RunParallelFor(position_update->commands.size(), [&](const size_t i) { + position_update->commands[i].new_position = + current_root_transform.TransformPoint(original_inverse_root_transform.TransformPoint( + dynamic_strands.strand_segments[position_update->commands[i].strand_segment_index].x0)); + }); + dynamic_strands.operators.emplace_back(position_update); + + } + const auto editor_layer = Application::GetLayer(); + step_parameters.render_parameters.target_camera = editor_layer->GetSceneCamera(); + dynamic_strands.Step(step_parameters); + } +} + void BrokenBranch::InitializeStrandConcaveMesh(const StrandModelStrandGroup& strand_group, const float max_edge_length) const { const auto scene = GetScene(); diff --git a/EvoEngine_Plugins/EcoSysLab/src/DynamicStrands.cpp b/EvoEngine_Plugins/EcoSysLab/src/DynamicStrands.cpp index fb8ed5e..a29ab63 100644 --- a/EvoEngine_Plugins/EcoSysLab/src/DynamicStrands.cpp +++ b/EvoEngine_Plugins/EcoSysLab/src/DynamicStrands.cpp @@ -31,6 +31,7 @@ DynamicStrands::DynamicStrands() { for (auto& i : strands_descriptor_sets) { i = std::make_shared(strands_layout); } + } void DynamicStrands::Step(const StepParameters& target_step_parameters) { @@ -54,6 +55,12 @@ void DynamicStrands::Step(const StepParameters& target_step_parameters) { void DynamicStrands::Upload() const { device_strands_buffer->UploadVector(strands); device_strand_segments_buffer->UploadVector(strand_segments); + for (const auto& op : operators) { + op->UploadData(); + } + for (const auto& c : constraints) { + c->UploadData(); + } } void DynamicStrands::Download() { @@ -88,12 +95,12 @@ glm::vec3 DynamicStrands::ComputeInertiaTensorRod(const float mass, const float }; } -void DynamicStrands::InitializeOperators(const InitializeParameters& initialize_parameters) const { +void DynamicStrands::InitializeOperators(const InitializeParameters& initialize_parameters) { for (const auto& i : operators) i->InitializeData(initialize_parameters, *this); } -void DynamicStrands::InitializeConstraints(const InitializeParameters& initialize_parameters) const { +void DynamicStrands::InitializeConstraints(const InitializeParameters& initialize_parameters) { for (const auto& i : constraints) i->InitializeData(initialize_parameters, *this); } diff --git a/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsPhysics.cpp b/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsPhysics.cpp index 8c5bfe1..6bca5eb 100644 --- a/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsPhysics.cpp +++ b/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsPhysics.cpp @@ -1,4 +1,5 @@ #include "DynamicStrandsPhysics.hpp" +#include "Eigen" #include "Shader.hpp" using namespace eco_sys_lab_plugin; @@ -23,9 +24,8 @@ DynamicStrandsPreStep::DynamicStrandsPreStep() { if (!pre_step_pipeline) { static std::shared_ptr shader{}; shader = std::make_shared(); - shader->Set( - ShaderType::Compute, Platform::Constants::shader_global_defines, - std::filesystem::path("./EcoSysLabResources") / "Shaders/Compute/DynamicStrands/PreStep.comp"); + shader->Set(ShaderType::Compute, Platform::Constants::shader_global_defines, + std::filesystem::path("./EcoSysLabResources") / "Shaders/Compute/DynamicStrands/PreStep.comp"); pre_step_pipeline = std::make_shared(); pre_step_pipeline->compute_shader = shader; @@ -41,26 +41,86 @@ DynamicStrandsPreStep::DynamicStrandsPreStep() { } void DynamicStrandsPreStep::Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) { - const auto current_frame_index = Platform::GetCurrentFrameIndex(); - + DynamicStrands& target_dynamic_strands) { PreStepPushConstant push_constant; push_constant.segment_size = target_dynamic_strands.strand_segments.size(); push_constant.time_step = physics_parameters.time_step; - push_constant.inv_time_step = 1.f / push_constant.time_step; - const uint32_t task_work_group_invocations = - Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; - - Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { - pre_step_pipeline->Bind(vk_command_buffer); - pre_step_pipeline->BindDescriptorSet( - vk_command_buffer, 0, - target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - - pre_step_pipeline->PushConstant(vk_command_buffer, 0, push_constant); - vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.segment_size, task_work_group_invocations), 1, 1); - Platform::EverythingBarrier(vk_command_buffer); - }); + push_constant.inv_time_step = 1.f / push_constant.time_step; + if (physics_parameters.gpu) { + const auto current_frame_index = Platform::GetCurrentFrameIndex(); + const uint32_t task_work_group_invocations = + Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; + + Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { + pre_step_pipeline->Bind(vk_command_buffer); + pre_step_pipeline->BindDescriptorSet( + vk_command_buffer, 0, + target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + + pre_step_pipeline->PushConstant(vk_command_buffer, 0, push_constant); + vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.segment_size, task_work_group_invocations), 1, 1); + Platform::EverythingBarrier(vk_command_buffer); + }); + } else { + const auto update_inertia_w = [](DynamicStrands::GpuStrandSegment& segment) { + // Update w + glm::mat3 rot = glm::mat3_cast(segment.q); + glm::mat3 inertia_tensor_diag = glm::mat3(segment.inertia_tensor.x, 0.0, 0.0, 0.0, segment.inertia_tensor.y, 0.0, + 0.0, 0.0, segment.inertia_tensor.z); + glm::mat3 inertia_w = rot * inertia_tensor_diag * transpose(rot); + + glm::mat3 inverse_inertia_tensor_diag = + glm::mat3(segment.inv_inertia_tensor.x, 0.0, 0.0, 0.0, segment.inv_inertia_tensor.y, 0.0, 0.0, 0.0, + segment.inv_inertia_tensor.z); + glm::mat3 inverse_inertia_w = rot * inverse_inertia_tensor_diag * transpose(rot); + + segment.inertia_w = + glm::mat4(inertia_w[0][0], inertia_w[0][1], inertia_w[0][2], 0.0, inertia_w[1][0], inertia_w[1][1], + inertia_w[1][2], 0.0, inertia_w[2][0], inertia_w[2][1], inertia_w[2][2], 0.0, 0.0, 0.0, 0.0, 0.0); + segment.inv_inertia_w = + glm::mat4(inverse_inertia_w[0][0], inverse_inertia_w[0][1], inverse_inertia_w[0][2], 0.0, + inverse_inertia_w[1][0], inverse_inertia_w[1][1], inverse_inertia_w[1][2], 0.0, + inverse_inertia_w[2][0], inverse_inertia_w[2][1], inverse_inertia_w[2][2], 0.0, 0.0, 0.0, 0.0, 0.0); + }; + + Jobs::RunParallelFor(target_dynamic_strands.strand_segments.size(), [&](const size_t segment_index) { + auto& segment = target_dynamic_strands.strand_segments[segment_index]; + update_inertia_w(segment); + + // Calculate velocity and apply acceleration. + glm::vec3 velocity = glm::vec3(0, 0, 0); + if (segment.inv_mass != 0.0) { + velocity = push_constant.inv_time_step * (1.5f * segment.x - 2.0f * segment.old_x + 0.5f * segment.last_x); + velocity += segment.acceleration * push_constant.time_step; + } + // Calculate angular velocity and apply torque. + glm::vec3 angular_velocity = glm::vec3(0, 0, 0); + if (segment.inv_mass != 0.0) { + glm::quat rot = segment.q * glm::conjugate(segment.old_q); + angular_velocity = glm::vec3(rot.x, rot.y, rot.z) * 2.0f * push_constant.inv_time_step; + angular_velocity += push_constant.time_step * glm::mat3(segment.inv_inertia_w) * + (segment.torque - cross(angular_velocity, glm::mat3(segment.inertia_w) * angular_velocity)); + } + + // Shift position values + segment.last_x = segment.old_x; + segment.old_x = segment.x; + // Apply velocity + segment.x += push_constant.time_step * velocity; + + // Shift rotation values + segment.last_q = segment.old_q; + segment.old_q = segment.q; + // Apply angular velocity + glm::quat angular_velocity_q = glm::quat(0.0f, angular_velocity.x, angular_velocity.y, angular_velocity.z); + + segment.q += 0.5f * push_constant.time_step * (angular_velocity_q * segment.q); + segment.q = normalize(segment.q); + + // Update w + update_inertia_w(segment); + }); + } } DynamicStrandsPostStep::DynamicStrandsPostStep() { @@ -84,24 +144,27 @@ DynamicStrandsPostStep::DynamicStrandsPostStep() { } void DynamicStrandsPostStep::Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) { - const auto current_frame_index = Platform::GetCurrentFrameIndex(); + DynamicStrands& target_dynamic_strands) { + if (physics_parameters.gpu) { + const auto current_frame_index = Platform::GetCurrentFrameIndex(); + + PostStepPushConstant push_constant; + push_constant.segment_size = target_dynamic_strands.strand_segments.size(); + const uint32_t task_work_group_invocations = + Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; + + Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { + post_step_pipeline->Bind(vk_command_buffer); + post_step_pipeline->BindDescriptorSet( + vk_command_buffer, 0, + target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - PostStepPushConstant push_constant; - push_constant.segment_size = target_dynamic_strands.strand_segments.size(); - const uint32_t task_work_group_invocations = - Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; - - Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { - post_step_pipeline->Bind(vk_command_buffer); - post_step_pipeline->BindDescriptorSet( - vk_command_buffer, 0, - target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - - post_step_pipeline->PushConstant(vk_command_buffer, 0, push_constant); - vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.segment_size, task_work_group_invocations), 1, 1); - Platform::EverythingBarrier(vk_command_buffer); - }); + post_step_pipeline->PushConstant(vk_command_buffer, 0, push_constant); + vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.segment_size, task_work_group_invocations), 1, 1); + Platform::EverythingBarrier(vk_command_buffer); + }); + } else { + } } DsPositionUpdate::DsPositionUpdate() { @@ -153,32 +216,46 @@ DsPositionUpdate::DsPositionUpdate() { } void DsPositionUpdate::Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) { + DynamicStrands& target_dynamic_strands) { if (commands.empty()) return; + if (physics_parameters.gpu) { + const auto current_frame_index = Platform::GetCurrentFrameIndex(); + commands_buffer[current_frame_index]->UploadVector(commands); + commands_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(0, + commands_buffer[current_frame_index]); + + PositionUpdatePushConstant push_constant; + push_constant.commands_size = commands.size(); + const uint32_t task_work_group_invocations = + Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; + + Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { + external_force_pipeline->Bind(vk_command_buffer); + external_force_pipeline->BindDescriptorSet( + vk_command_buffer, 0, + target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + external_force_pipeline->BindDescriptorSet(vk_command_buffer, 1, + commands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - const auto current_frame_index = Platform::GetCurrentFrameIndex(); - commands_buffer[current_frame_index]->UploadVector(commands); - commands_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(0, commands_buffer[current_frame_index]); - - PositionUpdatePushConstant push_constant; - push_constant.commands_size = commands.size(); - const uint32_t task_work_group_invocations = - Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; - - Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { - external_force_pipeline->Bind(vk_command_buffer); - external_force_pipeline->BindDescriptorSet( - vk_command_buffer, 0, - target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - external_force_pipeline->BindDescriptorSet(vk_command_buffer, 1, - commands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - - external_force_pipeline->PushConstant(vk_command_buffer, 0, push_constant); + external_force_pipeline->PushConstant(vk_command_buffer, 0, push_constant); - vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.commands_size, task_work_group_invocations), 1, 1); - Platform::EverythingBarrier(vk_command_buffer); - }); + vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.commands_size, task_work_group_invocations), 1, 1); + Platform::EverythingBarrier(vk_command_buffer); + }); + } else { + Jobs::RunParallelFor(commands.size(), [&](const size_t command_index) { + const auto& position_update = commands[command_index]; + const uint32_t segment_index = position_update.strand_segment_index; + auto& segment = target_dynamic_strands.strand_segments[segment_index]; + const glm::vec3 new_position = position_update.new_position; + const glm::vec3 old_position = segment.x; + const glm::vec3 delta_position = new_position - old_position; + + segment.x = new_position; + segment.last_x = segment.last_x + delta_position; + }); + } } DsExternalForce::DsExternalForce() { @@ -230,33 +307,36 @@ DsExternalForce::DsExternalForce() { } void DsExternalForce::Execute(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) { + DynamicStrands& target_dynamic_strands) { if (commands.empty()) return; + if (physics_parameters.gpu) { + const auto current_frame_index = Platform::GetCurrentFrameIndex(); + commands_buffer[current_frame_index]->UploadVector(commands); - const auto current_frame_index = Platform::GetCurrentFrameIndex(); - commands_buffer[current_frame_index]->UploadVector(commands); + commands_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(0, + commands_buffer[current_frame_index]); - commands_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(0, commands_buffer[current_frame_index]); + ExternalForcePushConstant push_constant; + push_constant.commands_size = commands.size(); + const uint32_t task_work_group_invocations = + Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; - ExternalForcePushConstant push_constant; - push_constant.commands_size = commands.size(); - const uint32_t task_work_group_invocations = - Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; - - Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { - external_force_pipeline->Bind(vk_command_buffer); - external_force_pipeline->BindDescriptorSet( - vk_command_buffer, 0, - target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - external_force_pipeline->BindDescriptorSet(vk_command_buffer, 1, - commands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { + external_force_pipeline->Bind(vk_command_buffer); + external_force_pipeline->BindDescriptorSet( + vk_command_buffer, 0, + target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + external_force_pipeline->BindDescriptorSet(vk_command_buffer, 1, + commands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - external_force_pipeline->PushConstant(vk_command_buffer, 0, push_constant); + external_force_pipeline->PushConstant(vk_command_buffer, 0, push_constant); - vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.commands_size, task_work_group_invocations), 1, 1); - Platform::EverythingBarrier(vk_command_buffer); - }); + vkCmdDispatch(vk_command_buffer, Platform::DivUp(push_constant.commands_size, task_work_group_invocations), 1, 1); + Platform::EverythingBarrier(vk_command_buffer); + }); + } else { + } } DsStiffRod::DsStiffRod() { @@ -292,7 +372,7 @@ DsStiffRod::DsStiffRod() { per_strand_data_list_buffer = std::make_shared(storage_buffer_create_info, buffer_vma_allocation_create_info); rod_constraints_buffer = std::make_shared(storage_buffer_create_info, buffer_vma_allocation_create_info); - + if (!init_constraint_pipeline) { static std::shared_ptr init_constraint_shader{}; init_constraint_shader = std::make_shared(); @@ -361,7 +441,7 @@ DsStiffRod::DsStiffRod() { } void DsStiffRod::InitializeData(const DynamicStrands::InitializeParameters& initialize_parameters, - const DynamicStrands& target_dynamic_strands) { + DynamicStrands& target_dynamic_strands) { per_strand_data_list.resize(target_dynamic_strands.strands.size()); rod_constraints.clear(); rod_constraints.reserve(glm::max(0, static_cast(target_dynamic_strands.strand_segments.size()) - @@ -385,7 +465,7 @@ void DsStiffRod::InitializeData(const DynamicStrands::InitializeParameters& init while (next_segment_handle != -1) { rod_constraints.emplace_back(); auto& rod_constraint = rod_constraints.back(); - rod_constraint.next_handle = rod_constraints.size(); + rod_constraint.next_constraint_handle = rod_constraints.size(); rod_constraint.segment0_index = segment_handle; rod_constraint.segment1_index = next_segment_handle; const auto& segment0 = target_dynamic_strands.strand_segments[segment_handle]; @@ -409,20 +489,22 @@ void DsStiffRod::InitializeData(const DynamicStrands::InitializeParameters& init glm::mat4(glm::vec4(rotation0_transpose * (constraint_position - p0), 0.f), glm::vec4(rotation1_transpose * (constraint_position - p1), 0.f), glm::vec4(constraint_position, 0.f), glm::vec4(constraint_position, 0.f)); + const auto radius = (segment0.radius + segment1.radius) * .5f; + const float youngs_modulus = initialize_parameters.youngs_modulus * 1000000000.f; + const float shear_modulus = initialize_parameters.torsion_modulus * 1000000000.f; + const auto second_moment_of_area = glm::pi() * std::pow(radius * .5f, 4.f); + const auto polar_moment_of_inertia = glm::pi() * std::pow(radius, 4.f) / 2.f; + + const auto stretching_stiffness = youngs_modulus * glm::pi() * radius * radius / segment_length; + const auto bending_stiffness = youngs_modulus * second_moment_of_area / glm::pow(segment_length, 3.f); + const auto torsional_stiffness = shear_modulus * polar_moment_of_inertia / segment_length; - // compute bending and torsion stiffness of the K matrix diagonal; assumption: the rod axis follows the y-axis of - // the local frame as with Blender's armatures -#define M_PI_4 0.785398163397448309616 // pi/4 - const auto second_moment_of_area(static_cast(M_PI_4) * - std::pow((segment0.radius + segment1.radius) * .5f, 4.f)); - const auto bending_stiffness(initialize_parameters.youngs_modulus * second_moment_of_area); - const auto torsion_stiffness(2.f * initialize_parameters.torsion_modulus * second_moment_of_area); - rod_constraint.stiffness_coefficient_k = glm::vec3(bending_stiffness, torsion_stiffness, bending_stiffness); + rod_constraint.stiffness_coefficient_k = glm::vec3(stretching_stiffness, bending_stiffness, torsional_stiffness); segment_handle = next_segment_handle; next_segment_handle = target_dynamic_strands.strand_segments[segment_handle].next_handle; } - rod_constraints.back().next_handle = -1; + rod_constraints.back().next_constraint_handle = -1; stiff_rod_per_strand_data.end_rod_constraint_handle = static_cast(rod_constraints.size()) - 1; } @@ -431,75 +513,579 @@ void DsStiffRod::InitializeData(const DynamicStrands::InitializeParameters& init } void DsStiffRod::Project(const DynamicStrands::PhysicsParameters& physics_parameters, - const DynamicStrands& target_dynamic_strands) { - const auto current_frame_index = Platform::GetCurrentFrameIndex(); - rod_properties_buffer[current_frame_index]->Upload(rod_properties); - - strands_physics_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding( - 0, rod_properties_buffer[current_frame_index]); - strands_physics_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(1, per_strand_data_list_buffer); - strands_physics_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(2, rod_constraints_buffer); - - int num_sub_step = 1; - int num_iteration = 1; - + DynamicStrands& target_dynamic_strands) { StiffRodInitConstraintConstant init_push_constant; init_push_constant.constraint_size = rod_constraints.size(); - init_push_constant.time_step = physics_parameters.time_step / num_sub_step; - init_push_constant.inv_time_step = 1.f / init_push_constant.time_step; - + init_push_constant.time_step = physics_parameters.time_step / physics_parameters.sub_step; StiffRodUpdateConstraintConstant update_constraint_constant; update_constraint_constant.constraint_size = rod_constraints.size(); - + StiffRodProjectConstraintConstant project_constraint_constant; project_constraint_constant.strand_size = per_strand_data_list.size(); - + if (physics_parameters.gpu) { + const auto current_frame_index = Platform::GetCurrentFrameIndex(); + rod_properties_buffer[current_frame_index]->Upload(rod_properties); - const uint32_t task_work_group_invocations = - Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; + strands_physics_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding( + 0, rod_properties_buffer[current_frame_index]); + strands_physics_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(1, per_strand_data_list_buffer); + strands_physics_descriptor_sets[current_frame_index]->UpdateBufferDescriptorBinding(2, rod_constraints_buffer); - Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { - Platform::EverythingBarrier(vk_command_buffer); - for (int sub_step_i = 0; sub_step_i < num_sub_step; sub_step_i++) { - init_constraint_pipeline->Bind(vk_command_buffer); - init_constraint_pipeline->BindDescriptorSet( - vk_command_buffer, 0, - target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - init_constraint_pipeline->BindDescriptorSet( - vk_command_buffer, 1, strands_physics_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + const uint32_t task_work_group_invocations = + Platform::GetSelectedPhysicalDevice()->mesh_shader_properties_ext.maxPreferredTaskWorkGroupInvocations; - init_constraint_pipeline->PushConstant(vk_command_buffer, 0, init_push_constant); - vkCmdDispatch(vk_command_buffer, Platform::DivUp(init_push_constant.constraint_size, task_work_group_invocations), - 1, 1); + Platform::RecordCommandsMainQueue([&](const VkCommandBuffer vk_command_buffer) { Platform::EverythingBarrier(vk_command_buffer); - for (int iteration_i = 0; iteration_i < num_iteration; iteration_i++) { - update_constraint_pipeline->Bind(vk_command_buffer); - update_constraint_pipeline->BindDescriptorSet( - vk_command_buffer, 0, - target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - update_constraint_pipeline->BindDescriptorSet( - vk_command_buffer, 1, strands_physics_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - - update_constraint_pipeline->PushConstant(vk_command_buffer, 0, update_constraint_constant); - vkCmdDispatch(vk_command_buffer, - Platform::DivUp(update_constraint_constant.constraint_size, task_work_group_invocations), 1, 1); - Platform::EverythingBarrier(vk_command_buffer); - - project_constraint_pipeline->Bind(vk_command_buffer); - project_constraint_pipeline->BindDescriptorSet( + for (int sub_step_i = 0; sub_step_i < physics_parameters.sub_step; sub_step_i++) { + init_constraint_pipeline->Bind(vk_command_buffer); + init_constraint_pipeline->BindDescriptorSet( vk_command_buffer, 0, target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - project_constraint_pipeline->BindDescriptorSet( + init_constraint_pipeline->BindDescriptorSet( vk_command_buffer, 1, strands_physics_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); - project_constraint_pipeline->PushConstant(vk_command_buffer, 0, project_constraint_constant); + init_constraint_pipeline->PushConstant(vk_command_buffer, 0, init_push_constant); vkCmdDispatch(vk_command_buffer, - Platform::DivUp(project_constraint_constant.strand_size, task_work_group_invocations), 1, 1); + Platform::DivUp(init_push_constant.constraint_size, task_work_group_invocations), 1, 1); Platform::EverythingBarrier(vk_command_buffer); + for (int iteration_i = 0; iteration_i < physics_parameters.max_iteration; iteration_i++) { + update_constraint_pipeline->Bind(vk_command_buffer); + update_constraint_pipeline->BindDescriptorSet( + vk_command_buffer, 0, + target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + update_constraint_pipeline->BindDescriptorSet( + vk_command_buffer, 1, strands_physics_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + + update_constraint_pipeline->PushConstant(vk_command_buffer, 0, update_constraint_constant); + vkCmdDispatch(vk_command_buffer, + Platform::DivUp(update_constraint_constant.constraint_size, task_work_group_invocations), 1, 1); + Platform::EverythingBarrier(vk_command_buffer); + + project_constraint_pipeline->Bind(vk_command_buffer); + project_constraint_pipeline->BindDescriptorSet( + vk_command_buffer, 0, + target_dynamic_strands.strands_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + project_constraint_pipeline->BindDescriptorSet( + vk_command_buffer, 1, strands_physics_descriptor_sets[current_frame_index]->GetVkDescriptorSet()); + + project_constraint_pipeline->PushConstant(vk_command_buffer, 0, project_constraint_constant); + vkCmdDispatch(vk_command_buffer, + Platform::DivUp(project_constraint_constant.strand_size, task_work_group_invocations), 1, 1); + Platform::EverythingBarrier(vk_command_buffer); + } + } + }); + } else { + const auto init_constraints = [&]() { + const auto init_stretch_bending_twisting_constraint = + [&](const glm::vec3& stiffness_coefficient_k, glm::vec3& stretch_compliance, + glm::vec3& bending_and_torsion_compliance, vec6& lambda_sum) { + const float time_step_quadratic = init_push_constant.time_step * init_push_constant.time_step; + + stretch_compliance = glm::vec3(time_step_quadratic / stiffness_coefficient_k[0], + time_step_quadratic / stiffness_coefficient_k[0], + time_step_quadratic / stiffness_coefficient_k[0]); + + bending_and_torsion_compliance = glm::vec3(time_step_quadratic / stiffness_coefficient_k[1], + time_step_quadratic / stiffness_coefficient_k[1], + time_step_quadratic / stiffness_coefficient_k[2]); + + lambda_sum.v[0] = 0.0; + lambda_sum.v[1] = 0.0; + lambda_sum.v[2] = 0.0; + lambda_sum.v[3] = 0.0; + lambda_sum.v[4] = 0.0; + lambda_sum.v[5] = 0.0; + }; + + Jobs::RunParallelFor(rod_constraints.size(), [&](const size_t constraint_index) { + auto& rod_constraint = rod_constraints[constraint_index]; + init_stretch_bending_twisting_constraint( + rod_constraint.stiffness_coefficient_k, rod_constraint.stretch_compliance, + rod_constraint.bending_and_torsion_compliance, rod_constraint.lambda_sum); + }); + }; + const auto update_constraints = [&]() { + const auto update_stretch_bending_twisting_constraint = [](const glm::vec3& x0, const glm::quat& q0, + const glm::vec3& x1, const glm::quat& q1, + glm::mat4& constraint_info) { + // constraintInfo contains + // 0: connector in segment 0 (local) + // 1: connector in segment 1 (local) + // 2: connector in segment 0 (global) + // 3: connector in segment 1 (global) + + // compute world space positions of connectors + const glm::mat3 rot0 = glm::mat3_cast(q0); + const glm::mat3 rot1 = glm::mat3_cast(q1); + + constraint_info[2] = glm::vec4(rot0 * glm::vec3(constraint_info[0]) + x0, 0.0f); + constraint_info[3] = glm::vec4(rot1 * glm::vec3(constraint_info[1]) + x1, 0.0f); + }; + + Jobs::RunParallelFor(rod_constraints.size(), [&](const size_t constraint_index) { + auto& rod_constraint = rod_constraints[constraint_index]; + const auto& strand_segment0 = target_dynamic_strands.strand_segments[rod_constraint.segment0_index]; + const auto& strand_segment1 = target_dynamic_strands.strand_segments[rod_constraint.segment1_index]; + update_stretch_bending_twisting_constraint(strand_segment0.x, strand_segment0.q, strand_segment1.x, + strand_segment1.q, rod_constraint.constraint_info); + }); + }; + const auto solve_constraints = [&]() { + const auto solve_stretch_bending_twisting_constraints = + [&](const int constraint_handle, const float inv_mass0, const glm::vec3& x0, + const glm::mat3& inverse_inertia0, const glm::quat& q0, const float inv_mass1, const glm::vec3& x1, + const glm::mat3& inverse_inertia1, const glm::quat& q1, const glm::vec3& rest_darboux_vector, + const float average_segment_length, const glm::vec3& stretch_compliance, + const glm::vec3& bending_and_torsion_compliance, const glm::mat4& constraint_info, + glm::vec3& x0_correction, glm::quat& q0_correction, glm::vec3& x1_correction, glm::quat& q1_correction, + vec6& lambda_sum) { + const auto compute_bending_and_torsion_jacobians = [](const glm::quat& q0, const glm::quat& q1, + float segment_length, glm::mat4& j_omega0, + glm::mat4& j_omega1) { + j_omega0 = glm::mat4(-q1[3], -q1[2], q1[1], q1[0], q1[2], -q1[3], -q1[0], q1[1], -q1[1], q1[0], -q1[3], + q1[2], 0.0, 0.0, 0.0, 0.0); + j_omega1 = glm::mat4(q0[3], q0[2], -q0[1], -q0[0], -q0[2], q0[3], q0[0], -q0[1], q0[1], -q0[0], q0[3], + -q0[2], 0.0, 0.0, 0.0, 0.0); + j_omega0 *= 2.0 / segment_length; + j_omega1 *= 2.0 / segment_length; + }; + + const auto compute_matrix_g = [](const glm::quat& q, glm::mat4& g) { + g = glm::mat4(q[3], q[2], -q[1], 0.0, -q[2], q[3], q[0], 0.0, q[1], -q[0], q[3], 0.0, -q[0], -q[1], -q[2], + 0.0) * + 0.5f; + }; + + const auto compute_matrix_k = [](const glm::vec3& connector, const float inv_mass, const glm::vec3& x, + const glm::mat3& inverse_inertia, glm::mat3& k) { + if (inv_mass != 0.0) { + glm::vec3 v = connector - x; + const float a = v[0]; + const float b = v[1]; + const float c = v[2]; + + const float j11 = inverse_inertia[0][0]; + const float j12 = inverse_inertia[0][1]; + const float j13 = inverse_inertia[0][2]; + const float j22 = inverse_inertia[1][1]; + const float j23 = inverse_inertia[1][2]; + const float j33 = inverse_inertia[2][2]; + + k[0][0] = c * c * j22 - b * c * (j23 + j23) + b * b * j33 + inv_mass; + k[0][1] = -(c * c * j12) + a * c * j23 + b * c * j13 - a * b * j33; + k[0][2] = b * c * j12 - a * c * j22 - b * b * j13 + a * b * j23; + k[1][0] = k[0][1]; + k[1][1] = c * c * j11 - a * c * (j13 + j13) + a * a * j33 + inv_mass; + k[1][2] = -(b * c * j11) + a * c * j12 + a * b * j13 - a * a * j23; + k[2][0] = k[0][2]; + k[2][1] = k[1][2]; + k[2][2] = b * b * j11 - a * b * (j12 + j12) + a * a * j22 + inv_mass; + } else { + k = glm::mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); + } + }; + + const auto cross_product_matrix = [](const glm::vec3& v) { + return glm::mat3(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0); + }; + const auto solve_ldlt = [](const mat6& A, vec6& x, const vec6& b) { + mat6 L; + L.v[0].v[0] = 1.0; + L.v[0].v[1] = 0.0; + L.v[0].v[2] = 0.0; + L.v[0].v[3] = 0.0; + L.v[0].v[4] = 0.0; + L.v[0].v[5] = 0.0; + + L.v[1].v[0] = 0.0; + L.v[1].v[1] = 1.0; + L.v[1].v[2] = 0.0; + L.v[1].v[3] = 0.0; + L.v[1].v[4] = 0.0; + L.v[1].v[5] = 0.0; + + L.v[2].v[0] = 0.0; + L.v[2].v[1] = 0.0; + L.v[2].v[2] = 1.0; + L.v[2].v[3] = 0.0; + L.v[2].v[4] = 0.0; + L.v[2].v[5] = 0.0; + + L.v[3].v[0] = 0.0; + L.v[3].v[1] = 0.0; + L.v[3].v[2] = 0.0; + L.v[3].v[3] = 1.0; + L.v[3].v[4] = 0.0; + L.v[3].v[5] = 0.0; + + L.v[4].v[0] = 0.0; + L.v[4].v[1] = 0.0; + L.v[4].v[2] = 0.0; + L.v[4].v[3] = 0.0; + L.v[4].v[4] = 1.0; + L.v[4].v[5] = 0.0; + + L.v[5].v[0] = 0.0; + L.v[5].v[1] = 0.0; + L.v[5].v[2] = 0.0; + L.v[5].v[3] = 0.0; + L.v[5].v[4] = 0.0; + L.v[5].v[5] = 1.0; + + vec6 D; + D.v[0] = 0.0; + D.v[1] = 0.0; + D.v[2] = 0.0; + D.v[3] = 0.0; + D.v[4] = 0.0; + D.v[5] = 0.0; + + // Decomposition + for (int i = 0; i < 6; i++) { + // Compute diagonal elements of D + D.v[i] = A.v[i].v[i]; + for (int k = 0; k < i; k++) { + D.v[i] -= L.v[i].v[k] * L.v[i].v[k] * D.v[k]; + } + // Compute elements of L (below the diagonal) + for (int j = i + 1; j < 6; j++) { + L.v[j].v[i] = A.v[j].v[i]; + for (int k = 0; k < i; k++) { + L.v[j].v[i] -= L.v[j].v[k] * L.v[i].v[k] * D.v[k]; + } + L.v[j].v[i] /= D.v[i]; + } + } + vec6 y, z; + // Forward substitution + for (int i = 0; i < 6; i++) { + y.v[i] = b.v[i]; + for (int j = 0; j < i; j++) { + y.v[i] -= L.v[i].v[j] * y.v[j]; + } + } + + // Diagonal solve + for (int i = 0; i < 6; i++) { + z.v[i] = y.v[i] / D.v[i]; + } + + // Backward substitution + for (int i = 5; i >= 0; i--) { + x.v[i] = z.v[i]; + for (int j = i + 1; j < 6; j++) { + x.v[i] -= L.v[j].v[i] * x.v[j]; + } + } + }; + + glm::vec3 omega = ComputeDarbouxVector(q0, q1, average_segment_length); + glm::mat4 j_omega0, j_omega1; + compute_bending_and_torsion_jacobians(q0, q1, average_segment_length, j_omega0, j_omega1); + glm::mat4 g0, g1; + compute_matrix_g(q0, g0); + compute_matrix_g(q1, g1); + + glm::mat3 j_omega_g0 = glm::mat3(j_omega0 * g0); + glm::mat3 j_omega_g1 = glm::mat3(j_omega1 * g1); + + glm::vec3 connector0 = constraint_info[2]; + glm::vec3 connector1 = constraint_info[3]; + + glm::vec3 stretch_violation = connector0 - connector1; + glm::vec3 bending_and_torsion_violation = omega - rest_darboux_vector; + + stretch_violation += stretch_compliance * glm::vec3(lambda_sum.v[0], lambda_sum.v[1], lambda_sum.v[2]); + bending_and_torsion_violation += + bending_and_torsion_compliance * glm::vec3(lambda_sum.v[3], lambda_sum.v[4], lambda_sum.v[5]); + + vec6 rhs; + rhs.v[0] = -stretch_violation.x; + rhs.v[1] = -stretch_violation.y; + rhs.v[2] = -stretch_violation.z; + rhs.v[3] = -bending_and_torsion_violation.x; + rhs.v[4] = -bending_and_torsion_violation.y; + rhs.v[5] = -bending_and_torsion_violation.z; + + glm::mat3 k0, k1; + compute_matrix_k(connector0, inv_mass0, x0, inverse_inertia0, k0); + compute_matrix_k(connector1, inv_mass1, x1, inverse_inertia1, k1); + glm::mat3 k = k0 + k1; + + vec6 delta_lambda; + + glm::vec3 r0 = connector0 - x0; + glm::vec3 r1 = connector1 - x1; + + glm::mat3 r0_cross_t = cross_product_matrix(-r0); + glm::mat3 r1_cross_t = cross_product_matrix(-r1); + + glm::mat3 off_diag = glm::mat3(0, 0, 0, 0, 0, 0, 0, 0, 0); + if (inv_mass0 != 0.0) { + off_diag = j_omega_g0 * inverse_inertia0 * r0_cross_t * -1.0f; + } + if (inv_mass1 != 0.0) { + off_diag += j_omega_g1 * inverse_inertia1 * r1_cross_t; + } + glm::mat3 off_diag_t = transpose(off_diag); + + glm::mat3 m_inv_jt0 = inverse_inertia0 * transpose(j_omega_g0); + glm::mat3 m_inv_jt1 = inverse_inertia1 * transpose(j_omega_g1); + + glm::mat3 jmjt_omega = glm::mat3(0); + if (inv_mass0 != 0.0) { + jmjt_omega = j_omega_g0 * m_inv_jt0; + } + if (inv_mass1 != 0.0) { + jmjt_omega += j_omega_g1 * m_inv_jt1; + } + + constexpr bool use_eigen = true; + if (use_eigen) { + using Vector6r = Eigen::Matrix; + using Matrix6r = Eigen::Matrix; + using Vector6r = Eigen::Matrix; + Matrix6r jmjt(Matrix6r::Zero()); + + jmjt(0, 0) = k[0][0]; + jmjt(0, 1) = k[0][1]; + jmjt(0, 2) = k[0][2]; + jmjt(1, 0) = k[1][0]; + jmjt(1, 1) = k[1][1]; + jmjt(1, 2) = k[1][2]; + jmjt(2, 0) = k[2][0]; + jmjt(2, 1) = k[2][1]; + jmjt(2, 2) = k[2][2]; + + jmjt(3, 0) = off_diag[0][0]; + jmjt(3, 1) = off_diag[0][1]; + jmjt(3, 2) = off_diag[0][2]; + jmjt(4, 0) = off_diag[1][0]; + jmjt(4, 1) = off_diag[1][1]; + jmjt(4, 2) = off_diag[1][2]; + jmjt(5, 0) = off_diag[2][0]; + jmjt(5, 1) = off_diag[2][1]; + jmjt(5, 2) = off_diag[2][2]; + + jmjt(0, 3) = off_diag_t[0][0]; + jmjt(0, 4) = off_diag_t[0][1]; + jmjt(0, 5) = off_diag_t[0][2]; + jmjt(1, 3) = off_diag_t[1][0]; + jmjt(1, 4) = off_diag_t[1][1]; + jmjt(1, 5) = off_diag_t[1][2]; + jmjt(2, 3) = off_diag_t[2][0]; + jmjt(2, 4) = off_diag_t[2][1]; + jmjt(2, 5) = off_diag_t[2][2]; + + jmjt(3, 3) = jmjt_omega[0][0]; + jmjt(3, 4) = jmjt_omega[0][1]; + jmjt(3, 5) = jmjt_omega[0][2]; + jmjt(4, 3) = jmjt_omega[1][0]; + jmjt(4, 4) = jmjt_omega[1][1]; + jmjt(4, 5) = jmjt_omega[1][2]; + jmjt(5, 3) = jmjt_omega[2][0]; + jmjt(5, 4) = jmjt_omega[2][1]; + jmjt(5, 5) = jmjt_omega[2][2]; + + // JMJT.block<3, 3>(3, 3) = JMJTOmega; + + // add compliance + jmjt(0, 0) += stretch_compliance[0]; + jmjt(1, 1) += stretch_compliance[1]; + jmjt(2, 2) += stretch_compliance[2]; + jmjt(3, 3) += bending_and_torsion_compliance[0]; + jmjt(4, 4) += bending_and_torsion_compliance[1]; + jmjt(5, 5) += bending_and_torsion_compliance[2]; + + // solve linear equation system (Equation 19) + auto decomposition(jmjt.ldlt()); + + Vector6r rhs_e; + rhs_e(0) = rhs.v[0]; + rhs_e(1) = rhs.v[1]; + rhs_e(2) = rhs.v[2]; + rhs_e(3) = rhs.v[3]; + rhs_e(4) = rhs.v[4]; + rhs_e(5) = rhs.v[5]; + Vector6r delta_lambda_e(decomposition.solve(rhs_e)); + delta_lambda.v[0] = delta_lambda_e(0); + delta_lambda.v[1] = delta_lambda_e(1); + delta_lambda.v[2] = delta_lambda_e(2); + delta_lambda.v[3] = delta_lambda_e(3); + delta_lambda.v[4] = delta_lambda_e(4); + delta_lambda.v[5] = delta_lambda_e(5); + + } else { + mat6 jmjt; + jmjt.v[0].v[0] = k[0][0]; + jmjt.v[0].v[1] = k[0][1]; + jmjt.v[0].v[2] = k[0][2]; + jmjt.v[1].v[0] = k[1][0]; + jmjt.v[1].v[1] = k[1][1]; + jmjt.v[1].v[2] = k[1][2]; + jmjt.v[2].v[0] = k[2][0]; + jmjt.v[2].v[1] = k[2][1]; + jmjt.v[2].v[2] = k[2][2]; + + jmjt.v[3].v[0] = off_diag[0][0]; + jmjt.v[3].v[1] = off_diag[0][1]; + jmjt.v[3].v[2] = off_diag[0][2]; + jmjt.v[4].v[0] = off_diag[1][0]; + jmjt.v[4].v[1] = off_diag[1][1]; + jmjt.v[4].v[2] = off_diag[1][2]; + jmjt.v[5].v[0] = off_diag[2][0]; + jmjt.v[5].v[1] = off_diag[2][1]; + jmjt.v[5].v[2] = off_diag[2][2]; + + jmjt.v[0].v[3] = off_diag_t[0][0]; + jmjt.v[0].v[4] = off_diag_t[0][1]; + jmjt.v[0].v[5] = off_diag_t[0][2]; + jmjt.v[1].v[3] = off_diag_t[1][0]; + jmjt.v[1].v[4] = off_diag_t[1][1]; + jmjt.v[1].v[5] = off_diag_t[1][2]; + jmjt.v[2].v[3] = off_diag_t[2][0]; + jmjt.v[2].v[4] = off_diag_t[2][1]; + jmjt.v[2].v[5] = off_diag_t[2][2]; + + jmjt.v[3].v[3] = jmjt_omega[0][0]; + jmjt.v[3].v[4] = jmjt_omega[0][1]; + jmjt.v[3].v[5] = jmjt_omega[0][2]; + jmjt.v[4].v[3] = jmjt_omega[1][0]; + jmjt.v[4].v[4] = jmjt_omega[1][1]; + jmjt.v[4].v[5] = jmjt_omega[1][2]; + jmjt.v[5].v[3] = jmjt_omega[2][0]; + jmjt.v[5].v[4] = jmjt_omega[2][1]; + jmjt.v[5].v[5] = jmjt_omega[2][2]; + + jmjt.v[0].v[0] += stretch_compliance.x; + jmjt.v[1].v[1] += stretch_compliance.y; + jmjt.v[2].v[2] += stretch_compliance.z; + jmjt.v[3].v[3] += bending_and_torsion_compliance.x; + jmjt.v[4].v[4] += bending_and_torsion_compliance.y; + jmjt.v[5].v[5] += bending_and_torsion_compliance.z; + + solve_ldlt(jmjt, delta_lambda, rhs); + } + lambda_sum.v[0] += delta_lambda.v[0]; + lambda_sum.v[1] += delta_lambda.v[1]; + lambda_sum.v[2] += delta_lambda.v[2]; + lambda_sum.v[3] += delta_lambda.v[3]; + lambda_sum.v[4] += delta_lambda.v[4]; + lambda_sum.v[5] += delta_lambda.v[5]; + + glm::vec3 delta_lambda_stretch = glm::vec3(delta_lambda.v[0], delta_lambda.v[1], delta_lambda.v[2]); + glm::vec3 delta_lambda_bending_and_torsion = + glm::vec3(delta_lambda.v[3], delta_lambda.v[4], delta_lambda.v[5]); + + x0_correction = glm::vec3(0, 0, 0); + q0_correction = glm::quat(0, 0, 0, 0); + x1_correction = glm::vec3(0, 0, 0); + q1_correction = glm::quat(0, 0, 0, 0); + + if (inv_mass0 != 0.0) { + x0_correction += inv_mass0 * delta_lambda_stretch; + glm::vec3 v = inverse_inertia0 * r0_cross_t * (-1.0f * delta_lambda_stretch) + + m_inv_jt0 * delta_lambda_bending_and_torsion; + q0_correction.x = g0[0][0] * v[0] + g0[0][1] * v[1] + g0[0][2] * v[2]; + q0_correction.y = g0[1][0] * v[0] + g0[1][1] * v[1] + g0[1][2] * v[2]; + q0_correction.z = g0[2][0] * v[0] + g0[2][1] * v[1] + g0[2][2] * v[2]; + q0_correction.w = g0[3][0] * v[0] + g0[3][1] * v[1] + g0[3][2] * v[2]; + } + + if (inv_mass1 != 0.0) { + x1_correction -= inv_mass1 * delta_lambda_stretch; + glm::vec3 v = + inverse_inertia1 * r1_cross_t * delta_lambda_stretch + m_inv_jt1 * delta_lambda_bending_and_torsion; + q1_correction.x = g1[0][0] * v[0] + g1[0][1] * v[1] + g1[0][2] * v[2]; + q1_correction.y = g1[1][0] * v[0] + g1[1][1] * v[1] + g1[1][2] * v[2]; + q1_correction.z = g1[2][0] * v[0] + g1[2][1] * v[1] + g1[2][2] * v[2]; + q1_correction.w = g1[3][0] * v[0] + g1[3][1] * v[1] + g1[3][2] * v[2]; + } + + GpuRodConstraint rod_constraint = rod_constraints[constraint_handle]; + rod_constraint.omega = omega; + rod_constraint.j_omega0 = j_omega0; + rod_constraint.j_omega1 = j_omega1; + rod_constraint.g0 = g0; + rod_constraint.g1 = g1; + rod_constraint.j_omega_g0 = glm::mat4(j_omega_g0); + rod_constraint.j_omega_g1 = glm::mat4(j_omega_g1); + + rod_constraint.connector0 = connector0; + rod_constraint.connector1 = connector1; + + rod_constraint.stretch_violation = stretch_violation; + rod_constraint.bending_and_torsion_violation = bending_and_torsion_violation; + + rod_constraint.delta_lambda_stretch = delta_lambda_stretch; + rod_constraint.delta_lambda_bending_and_torsion = delta_lambda_bending_and_torsion; + + rod_constraint.x0_correction = x0_correction; + rod_constraint.x1_correction = x1_correction; + + rod_constraint.q0_correction = q0_correction; + rod_constraint.q1_correction = q1_correction; + + rod_constraint.x[0] = delta_lambda.v[0]; + rod_constraint.x[1] = delta_lambda.v[1]; + rod_constraint.x[2] = delta_lambda.v[2]; + rod_constraint.x[3] = delta_lambda.v[3]; + rod_constraint.x[4] = delta_lambda.v[4]; + rod_constraint.x[5] = delta_lambda.v[5]; + + rod_constraint.rhs[0] = rhs.v[0]; + rod_constraint.rhs[1] = rhs.v[1]; + rod_constraint.rhs[2] = rhs.v[2]; + rod_constraint.rhs[3] = rhs.v[3]; + rod_constraint.rhs[4] = rhs.v[4]; + rod_constraint.rhs[5] = rhs.v[5]; + + rod_constraints[constraint_handle] = rod_constraint; + }; + + const auto project_constraint = [&](const int rod_constraint_handle) { + GpuRodConstraint& rod_constraint = rod_constraints[rod_constraint_handle]; + glm::vec3 x0_correction, x1_correction; + glm::quat q0_correction, q1_correction; + glm::vec3 fake_compliance = glm::vec3(0); + auto& segment0 = target_dynamic_strands.strand_segments[rod_constraint.segment0_index]; + auto& segment1 = target_dynamic_strands.strand_segments[rod_constraint.segment1_index]; + solve_stretch_bending_twisting_constraints( + rod_constraint_handle, segment0.inv_mass, segment0.x, segment0.inv_inertia_w, segment0.q, segment1.inv_mass, + segment1.x, segment1.inv_inertia_w, segment1.q, rod_constraint.rest_darboux_vector, + rod_constraint.average_segment_length, fake_compliance, fake_compliance, rod_constraint.constraint_info, + x0_correction, q0_correction, x1_correction, q1_correction, rod_constraint.lambda_sum); + + if (segment0.inv_mass != 0.0) { + segment0.x = segment0.x + x0_correction; + segment0.q = normalize(segment0.q + q0_correction); + } + + if (segment1.inv_mass != 0.0) { + segment1.x = segment1.x + x1_correction; + segment1.q = normalize(segment1.q + q0_correction); + } + }; + + Jobs::RunParallelFor(per_strand_data_list.size(), [&](const size_t strand_index) { + const auto& per_strand_data = per_strand_data_list[strand_index]; + int rod_constraint_handle = per_strand_data.begin_rod_constraint_handle; + while (rod_constraint_handle != -1) { + project_constraint(rod_constraint_handle); + rod_constraint_handle = rod_constraints[rod_constraint_handle].next_constraint_handle; + } + }); + }; + for (int sub_step_i = 0; sub_step_i < physics_parameters.sub_step; sub_step_i++) { + init_constraints(); + for (int iteration_i = 0; iteration_i < physics_parameters.max_iteration; iteration_i++) { + update_constraints(); + solve_constraints(); } } - }); + } } void DsStiffRod::DownloadData() { @@ -507,6 +1093,11 @@ void DsStiffRod::DownloadData() { per_strand_data_list_buffer->DownloadVector(per_strand_data_list, per_strand_data_list.size()); } +void DsStiffRod::UploadData() { + rod_constraints_buffer->UploadVector(rod_constraints); + per_strand_data_list_buffer->UploadVector(per_strand_data_list); +} + glm::vec3 DsStiffRod::ComputeDarbouxVector(const glm::quat& q0, const glm::quat& q1, const float average_segment_length) { const auto relative_rotation = glm::conjugate(q0) * q1; diff --git a/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsRender.cpp b/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsRender.cpp index e96efbe..5fbb34e 100644 --- a/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsRender.cpp +++ b/EvoEngine_Plugins/EcoSysLab/src/DynamicStrandsRender.cpp @@ -21,6 +21,7 @@ void DynamicStrands::Render(const RenderParameters& render_parameters) const { uint32_t strand_segment_size = 0; }; + if (!render_pipeline) { static std::shared_ptr task_shader{}; static std::shared_ptr mesh_shader{}; diff --git a/EvoEngine_SDK/Internals/DefaultResources/Shaders/Includes/Math.glsl b/EvoEngine_SDK/Internals/DefaultResources/Shaders/Includes/Math.glsl index affbb9c..9513cbb 100644 --- a/EvoEngine_SDK/Internals/DefaultResources/Shaders/Includes/Math.glsl +++ b/EvoEngine_SDK/Internals/DefaultResources/Shaders/Includes/Math.glsl @@ -34,6 +34,10 @@ mat4 mat4_cast(in vec4 q) { return ret_val; } +mat4 mat4_cast(in mat3 m) { + return mat4(m[0][0], m[0][1], m[0][2], 0, m[1][0], m[1][1], m[1][2], 0, m[2][0], m[2][1], m[2][2], 0, 0, 0, 0, 0); +} + mat3 mat3_cast(in vec4 q) { float qxx = q.x * q.x; float qyy = q.y * q.y; @@ -114,13 +118,10 @@ vec4 conjugate(in vec4 q) { } vec4 quat_mul(in vec4 a, in vec4 b) { - return vec4(a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, - a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z, - a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x, - a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z); + return vec4(a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z, + a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x, a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z); } vec3 compute_darboux_vector(in vec4 q0, in vec4 q1, float length) { - return (quat_mul(conjugate(q0), q1) * 2.0 / length).xyz; // Eq. 27.5 + return 2.0 / length * quat_mul(conjugate(q0), q1).xyz; // Eq. 27.5 } -