572 TLS() : scope(1024), allocator(scope.
allocator()) {}
581 const float sub_delta_time_;
582 float substep_compliance_factor_;
584 const float interpolation_begin_;
585 const float interpolation_end_;
587 const float4x4 world_to_simulation_;
589 std::string geometry_tag_filter_;
591 int constraint_iterations_;
593 Geometries geometries_;
594 ConstraintsInfo constraints_;
598 Mutex warnings_mutex_;
605 const float total_delta_time,
607 const int constraint_iterations,
608 const float interpolation_begin,
609 const float interpolation_end,
611 const float4x4 &simulation_to_world)
614 sub_delta_time_(total_delta_time / substeps_),
615 interpolation_begin_(interpolation_begin),
616 interpolation_end_(interpolation_end),
617 world_to_simulation_(
math::
invert(simulation_to_world)),
618 geometry_tag_filter_(geometry_tag_filter),
619 constraint_iterations_(constraint_iterations)
626 this->gather_nested_bundle_paths();
627 this->gather_from_world__geometries();
628 this->prepare_geometry_chunks();
629 this->prepare_inverse_masses();
630 this->prepare_inverse_moments_of_inertia();
632 this->gather_from_world__mesh_colliders();
633 this->gather_from_world__stretch_shear_constraints();
634 this->gather_from_world__bend_twist_constraints();
635 this->gather_from_world__edge_length_constraints();
636 this->gather_from_world__cross_edge_length_constraints();
637 this->gather_from_world__damping();
638 this->gather_from_world__pin_positions();
639 this->gather_from_world__pin_rotations();
640 this->gather_from_world__collision_contacts();
642 this->parallel_for_each_chunk(32, [&](
const int chunk_i) {
643 TLS &tls = tls_.local();
644 this->create_chunk_constraints__rod_stretch_shear(tls, chunk_i);
645 this->create_chunk_constraints__rod_bend_twist(tls, chunk_i);
646 this->create_chunk_constraints__damping(tls, chunk_i);
647 this->create_chunk_constraints__pin_positions(tls, chunk_i);
648 this->create_chunk_constraints__pin_rotations(tls, chunk_i);
651 result_ = this->do_simulation();
653 this->write_back__pin_positions();
654 this->write_back__rod_stretch_shear();
656 this->finish_common_attribute_writers();
657 this->write_back_geometries_to_world();
659 this->write_back__contacts();
673 static float error_scale_from_threshold(
const float error_threshold)
675 return 1.0f / std::max(
math::square(error_threshold), 1e-12f);
678 void gather_nested_bundle_paths()
683 if (!bundle_ptr || !*bundle_ptr) {
686 const Bundle &bundle = **bundle_ptr;
687 const std::optional<StringRef> type = bundle.
type();
688 if (!type.has_value()) {
694 for (MutableSpan<std::string> paths : nested_bundle_paths_.values()) {
695 std::ranges::sort(paths);
699 void gather_from_world__geometries()
701 TLS &tls = tls_.local();
705 std::ranges::sort(paths);
706 for (
const StringRef path : paths) {
707 GeometrySet &geometry = *world_.lookup_path_for_write_ptr<GeometrySet>(path);
708 Set<std::string> tags;
709 if (geometry.has_bundle()) {
710 const Bundle &bundle_in_geo = *geometry.bundle();
711 if (
const std::optional<GListPtr> tags_list_ptr = bundle_in_geo.lookup_path<GListPtr>(
714 if (*tags_list_ptr) {
715 const GList &tags_list = **tags_list_ptr;
716 if (tags_list.cpp_type().is<std::string>()) {
717 tags_list.typed<std::string>().
foreach(
718 [&](
const std::string &tag) { tags.add(tag); });
727 GeometrySetData geo_set_data;
728 geo_set_data.path = path;
729 geo_set_data.geometry = std::move(geometry);
730 geo_set_data.tags = std::move(tags);
731 geometries_.geometry_sets.append(std::move(geo_set_data));
736 for (
const int geo_bundle_i : geometries_.geometry_sets.index_range()) {
737 GeometrySetData &geo_set_data = geometries_.geometry_sets[geo_bundle_i];
738 GeometrySet &geometry = geo_set_data.geometry;
744 if (!geometry.has(type)) {
747 bke::GeometryComponent &component = geometry.get_component_for_write(type);
748 geometries_.data_keys.add_new({geo_bundle_i, type, std::nullopt});
749 GeometryData &geo_data = tls.scope.construct<GeometryData>(
750 *component.attributes_for_write());
752 &geometry.get_curves_for_write()->geometry.wrap() :
756 geometries_.data.append(&geo_data);
758 if (geometry.has_grease_pencil()) {
759 using namespace blender::bke::greasepencil;
760 GreasePencil &grease_pencil = *geometry.get_grease_pencil_for_write();
761 for (
const int layer_i : grease_pencil.layers().index_range()) {
763 Drawing *drawing = grease_pencil.get_eval_drawing(
layer);
768 geometries_.data_keys.add_new(
770 GeometryData &geo_data = tls.scope.construct<GeometryData>(
772 geo_data.domain = AttrDomain::Point;
773 geo_data.curves = &curves;
774 geometries_.data.append(&geo_data);
778 geometries_.total_points_num = 0;
779 for (
const int data_key_i : geometries_.data_keys.index_range()) {
780 GeometryData &geo_data = *geometries_.data[data_key_i];
782 geo_data.size = geo_data.attributes.domain_size(domain);
783 geometries_.total_points_num += geo_data.size;
784 geo_data.temp_positions.reinitialize(geo_data.size);
785 geo_data.temp_rotations.reinitialize(geo_data.size);
786 geo_data.position_attr = this->ensure_attribute<float3>(
788 geo_data.velocity_attr = this->ensure_attribute<float3>(
790 geo_data.external_force_attr = this->lookup_attribute_default<float3>(
792 geo_data.masses = this->lookup_attribute_default<float>(
795 geo_data.is_hard_pinned.reinitialize(geo_data.size);
796 geo_data.is_hard_pinned.fill(
false);
798 geo_data.uses_rotation =
true;
799 geo_data.rotation_attr = this->ensure_attribute<math::Quaternion>(
801 geo_data.angular_velocity_attr = this->ensure_attribute<float3>(
803 geo_data.external_torque_attr = this->lookup_attribute_default<float3>(
805 geo_data.moments_of_inertia = this->lookup_attribute_default<float3>(
811 void ensure_friction_loaded(
const int data_key_i)
813 GeometryData &geo_data = *geometries_.data[data_key_i];
814 geo_data.load_friction_mutex.ensure([&]() {
815 geo_data.static_frictions = this->lookup_attribute_default<float>(
817 geo_data.dynamic_frictions = this->lookup_attribute_default<float>(
822 void ensure_radius_loaded(
const int data_key_i)
824 GeometryData &geo_data = *geometries_.data[data_key_i];
825 geo_data.load_radius_mutex.ensure([&]() {
826 geo_data.radii = this->lookup_attribute_default<float>(
828 geo_data.prev_radii = geo_data.radii;
832 void gather_from_world__mesh_colliders()
835 for (
const StringRef path : paths) {
837 if (!bundle_ptr || !*bundle_ptr) {
840 const Bundle &bundle = **bundle_ptr;
841 const Bundle *previous_bundle = this->get_previous_bundle(bundle);
842 const bke::GeometrySet *geometry = bundle.
lookup_ptr<bke::GeometrySet>(
846 const float compliance =
848 const bool deforming =
850 const bool use_edge_contacts =
852 const bool is_boundary =
854 const float error_threshold =
856 const bke::GeometrySet *prev_geometry = previous_bundle ?
857 previous_bundle->
lookup_ptr<bke::GeometrySet>(
863 Vector<int> affected_data;
864 for (
const int data_key_i : geometries_.data_keys.index_range()) {
865 if (this->effector_applies_to_geometry(path, bundle, data_key_i)) {
866 affected_data.
append(data_key_i);
867 this->ensure_friction_loaded(data_key_i);
868 this->ensure_radius_loaded(data_key_i);
871 Vector<int> instance_id_stack;
872 this->gather_colliders_in_geometry(path,
873 world_to_simulation_,
874 world_to_simulation_,
889 void gather_contacts__mesh_colliders(
const int chunk_i,
890 const float max_distance,
891 const int solver_refs_i,
892 const SubstepInterval &substep,
893 const ExternalFaceContacts &prev_face_contacts,
894 const ExternalEdgeContacts &prev_edge_contacts,
895 ExternalFaceContacts &r_face_contacts,
896 ExternalEdgeContacts &r_edge_contacts)
898 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
899 const GeometryData &geo_data = *geometries_.data[chunk.data_key_i];
900 for (
const MeshColliderUsage &collider_usage : geo_data.mesh_colliders) {
901 const MeshCollider &collider = constraints_.mesh_colliders[collider_usage.constraint_i];
903 collider.begin_transform, collider.end_transform, substep.interpolate_end);
905 collider.begin_transform, collider.end_transform, substep.interpolate_begin);
908 if (std::holds_alternative<StaticMeshInfo>(collider.mesh)) {
909 this->gather_contacts__mesh_collider(chunk_i,
924 else if (std::holds_alternative<DeformingMeshInfo>(collider.mesh)) {
925 this->gather_contacts__mesh_collider(chunk_i,
943 void gather_contacts__mesh_collider(
const int chunk_i,
944 const float max_distance,
945 const int solver_refs_i,
946 const bool is_deforming,
947 const SubstepInterval &substep,
948 const MeshCollider &collider,
949 const MeshColliderUsage &collider_usage,
953 const ExternalFaceContacts &prev_face_contacts,
954 const ExternalEdgeContacts &prev_edge_contacts,
955 ExternalFaceContacts &r_face_contacts,
956 ExternalEdgeContacts &r_edge_contacts)
959 const bke::bvh::Tree *corner_tris_bvh;
960 const bke::BVHTreeFromMesh *edges_bvh;
961 Span<float3> prev_vert_positions;
963 BLI_assert(std::holds_alternative<DeformingMeshInfo>(collider.mesh));
964 const auto &deforming_mesh = std::get<DeformingMeshInfo>(collider.mesh);
965 mesh = deforming_mesh.substep_meshes[substep.current_i + 1];
966 corner_tris_bvh = deforming_mesh.substep_corner_tris_bvh_trees[substep.current_i];
967 edges_bvh = &deforming_mesh.substep_edges_bvh_trees[substep.current_i];
968 prev_vert_positions = deforming_mesh.substep_meshes[substep.current_i]->vert_positions();
971 BLI_assert(std::holds_alternative<StaticMeshInfo>(collider.mesh));
972 const auto &static_mesh = std::get<StaticMeshInfo>(collider.mesh);
973 mesh = static_mesh.mesh;
974 corner_tris_bvh = static_mesh.corner_tris_bvh;
975 edges_bvh = &static_mesh.edges_bvh;
977 const Span<float3> vert_positions = mesh->vert_positions();
978 const Span<int2> edge_verts = mesh->edges();
979 const Span<float3> vert_normals = mesh->vert_normals();
980 const Span<int> corner_verts = mesh->corner_verts();
981 const Span<int3> corner_tris = mesh->corner_tris();
983 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
984 const GeometryData &geo_data = *geometries_.data[chunk.data_key_i];
985 const Span<float3> positions =
986 geometries_.solver_refs[solver_refs_i][chunk.data_key_i].positions;
991 const float local_to_mesh_radius_factor = std::max({mesh_to_local_radius_scale.
x,
992 mesh_to_local_radius_scale.
y,
993 mesh_to_local_radius_scale.
z});
994 const float mesh_to_local_radius_factor =
math::safe_rcp(local_to_mesh_radius_factor);
995 const float margin_local = mesh_to_local_radius_factor * collider.margin;
997 for (
const int point_i : chunk.points_range) {
998 if (geo_data.is_hard_pinned[point_i]) {
1001 const float3 &pos_local = positions[point_i];
1004 geo_data.prev_radii[point_i], geo_data.radii[point_i], substep.interpolate_end);
1005 const float radius_mesh = local_to_mesh_radius_factor * radius_local;
1006 const std::optional<ClosestMeshFaceContact> contact = this->get_closest_mesh_face_contact(
1012 max_distance + radius_mesh);
1018 float3 prev_contact_pos_mesh;
1020 const int3 &tri = corner_tris[contact->tri_i];
1022 contact->bary_coords,
1023 prev_vert_positions[corner_verts[tri[0]]],
1024 prev_vert_positions[corner_verts[tri[1]]],
1025 prev_vert_positions[corner_verts[tri[2]]]);
1028 prev_contact_pos_mesh = contact->nearest_pos;
1031 prev_contact_pos_mesh);
1034 const float3 collision_axis = (pos_local - contact_pos_local) *
1035 (contact->is_inside ? -1.0f : 1.0f) *
1036 (collider.is_boundary ? -1.0f : 1.0f);
1040 (collider.is_boundary ? -1.0f : 1.0f) :
1044 geo_data.static_frictions[point_i], collider.friction);
1046 geo_data.dynamic_frictions[point_i], collider.friction);
1048 const int contact_i = r_face_contacts.points.append_and_get_index(point_i);
1049 r_face_contacts.point_radii.append(radius_local);
1050 r_face_contacts.positions_on_face.append(contact_pos_local);
1051 r_face_contacts.collider_motion.append(contact_pos_local - prev_contact_pos_local);
1052 r_face_contacts.face_normals.append(valid_axis);
1053 r_face_contacts.face_margins.append(margin_local);
1054 r_face_contacts.static_frictions.append(static_friction);
1055 r_face_contacts.dynamic_frictions.append(dynamic_friction);
1056 r_face_contacts.compliance_terms.append(
1057 std::max(0.0f, substep_compliance_factor_ * collider.compliance));
1058 r_face_contacts.error_scales.append(error_scale_from_threshold(collider.error_threshold));
1060 const MeshContactId contact_id{collider_usage.constraint_i, point_i};
1061 r_face_contacts.mesh_contact_indices.add(contact_id, contact_i);
1062 r_face_contacts.init_or_preserve_state(
1063 prev_face_contacts, prev_face_contacts.mesh_contact_indices.lookup_try(contact_id));
1066 if (collider.use_edge_contacts) {
1067 auto handle_edge = [&](
const int geo_contact_id,
const int point0,
const int point1) {
1068 if (geo_data.is_hard_pinned[point0] && geo_data.is_hard_pinned[point1]) {
1071 const float3 &pos_local0 = positions[point0];
1072 const float3 &pos_local1 = positions[point1];
1076 geo_data.prev_radii[point0], geo_data.radii[point0], substep.interpolate_end);
1078 geo_data.prev_radii[point1], geo_data.radii[point1], substep.interpolate_end);
1080 const float max_radius_mesh = local_to_mesh_radius_factor *
1081 std::max(radius_local0, radius_local1);
1082 const std::optional<ClosestMeshEdgeContact> contact = this->get_closest_mesh_edge_contact(
1088 max_distance + max_radius_mesh);
1094 const int2 &edge = edge_verts[contact->edge_i];
1097 contact->nearest_pos);
1098 float3 prev_contact_pos_mesh;
1101 contact->edge_factor, prev_vert_positions[edge[0]], prev_vert_positions[edge[1]]);
1104 prev_contact_pos_mesh = contact->nearest_pos;
1107 prev_contact_pos_mesh);
1110 vert_positions[edge[0]]);
1121 vert_normals[edge[1]]);
1126 math::cross(edge_direction_local, averaged_vert_normal_local), edge_direction_local));
1131 math::square(this->compute_contact_friction(geo_data.static_frictions[point0],
1132 collider.friction)) +
1133 math::square(this->compute_contact_friction(geo_data.static_frictions[point1],
1134 collider.friction)));
1136 math::square(this->compute_contact_friction(geo_data.dynamic_frictions[point0],
1137 collider.friction)) +
1138 math::square(this->compute_contact_friction(geo_data.dynamic_frictions[point1],
1139 collider.friction)));
1141 const int contact_i = r_edge_contacts.point_pairs.append_and_get_index({point0, point1});
1142 r_edge_contacts.point_radii.append({radius_local0, radius_local1});
1143 r_edge_contacts.positions_on_edge.append(contact_pos_local);
1144 r_edge_contacts.collider_motion.append(contact_pos_local - prev_contact_pos_local);
1145 r_edge_contacts.edge_directions.append(edge_direction_local);
1146 r_edge_contacts.edge_normals.append(edge_normal_local *
1147 (collider.is_boundary ? -1.0f : 1.0f));
1148 r_edge_contacts.edge_margins.append(margin_local);
1149 r_edge_contacts.static_frictions.append(static_friction);
1150 r_edge_contacts.dynamic_frictions.append(dynamic_friction);
1151 r_edge_contacts.compliance_terms.append(
1152 std::max(0.0f, substep_compliance_factor_ * collider.compliance));
1153 r_edge_contacts.error_scales.append(error_scale_from_threshold(collider.error_threshold));
1155 const MeshContactId contact_id{collider_usage.constraint_i, geo_contact_id};
1156 r_edge_contacts.mesh_contact_indices.add(contact_id, contact_i);
1157 r_edge_contacts.init_or_preserve_state(
1158 prev_edge_contacts, prev_edge_contacts.mesh_contact_indices.lookup_try(contact_id));
1161 if (chunk.curves_range) {
1162 const OffsetIndices points_by_curve = geo_data.curves->points_by_curve();
1163 for (
const int curve : *chunk.curves_range) {
1164 for (
const int point0 : points_by_curve[curve].drop_back(1)) {
1165 const int geo_contact_id = point0;
1166 handle_edge(geo_contact_id, point0, point0 + 1);
1173 void gather_from_world__collision_contacts()
1176 for (
const StringRef path : paths) {
1178 if (!bundle_ptr || !*bundle_ptr) {
1182 constraints_.collision_contacts.append({path});
1186 struct ClosestMeshFaceContact {
1196 struct ClosestMeshEdgeContact {
1203 std::optional<ClosestMeshFaceContact> get_closest_mesh_face_contact(
1204 const float3 &sample_pos,
1205 const bke::bvh::Tree &corner_tris_bvh,
1206 const Span<int3> corner_tris,
1207 const Span<int> corner_verts,
1208 const Span<float3> vert_positions,
1209 const float max_distance)
const
1211 const std::optional<bke::bvh::ClosestPointResult> nearest = corner_tris_bvh.
closest_point(
1212 sample_pos, max_distance);
1214 return std::nullopt;
1216 const int tri_i = nearest->index;
1217 const int3 &tri = corner_tris[tri_i];
1220 vert_positions[corner_verts[corner_tris[tri_i][0]]],
1221 vert_positions[corner_verts[corner_tris[tri_i][1]]],
1222 vert_positions[corner_verts[corner_tris[tri_i][2]]]);
1224 vert_positions, corner_verts, tri, contact_pos);
1225 const float3 direction_to_mesh = contact_pos - sample_pos;
1227 if (this->is_bary_coord_close_to_edge(bary_coords)) {
1229 is_inside = this->is_inside_ray_using_rays(sample_pos, corner_tris_bvh, direction_to_mesh);
1232 is_inside =
math::dot(direction_to_mesh, contact_nor) > 0.0f;
1234 return ClosestMeshFaceContact{contact_pos, contact_nor, bary_coords, is_inside, tri_i};
1241 std::optional<ClosestMeshEdgeContact> get_closest_mesh_edge_contact(
1242 const float3 &sample_pos0,
1243 const float3 &sample_pos1,
1244 const bke::BVHTreeFromMesh &edges_bvh,
1245 const Span<int2> edges,
1246 const Span<float3> vert_positions,
1247 const float max_distance)
const
1251 Span<float3> vert_positions;
1256 const auto closest_edge_cb =
1257 [](
void *user_data,
int index,
const BVHTreeRay * , BVHTreeRayHit *hit) {
1258 const auto &
data = *
static_cast<const UserData *
>(user_data);
1261 const float3 &v0 =
data.vert_positions[edge[0]];
1262 const float3 &v1 =
data.vert_positions[edge[1]];
1264 float3 closest_on_ray, closest_on_edge;
1266 data.sample_pos0,
data.sample_pos1, v0, v1, closest_on_ray, closest_on_edge);
1278 UserData user_data = {edges, vert_positions, sample_pos0, sample_pos1};
1284 edges_bvh.
tree, sample_pos0, ray_dir, max_distance, &hit, closest_edge_cb, &user_data, 0);
1285 if (hit.
index == -1) {
1286 return std::nullopt;
1289 const int edge_i = hit.
index;
1290 const int2 &edge = edges[edge_i];
1292 const float3 v0 = vert_positions[edge[0]];
1293 const float3 v1 = vert_positions[edge[1]];
1296 return ClosestMeshEdgeContact{contact_pos, edge_factor, edge_i};
1299 bool is_bary_coord_close_to_edge(
const float3 &bary_coords)
const
1301 constexpr float epsilon = 1e-3f;
1306 bool is_inside_ray_using_rays(
const float3 &pos,
1307 const bke::bvh::Tree &bvh,
1308 const float3 &approx_ray_direction)
const
1313 const std::array<float3, 3> dirs = {
1317 int inside_count = 0;
1318 for (
const float3 &dir : dirs) {
1319 const bke::bvh::Ray ray(pos, dir);
1320 const std::optional<bke::bvh::RayHit> hit = bvh.
ray_intersect(ray);
1322 const float3 dir = hit->position(ray) - pos;
1324 inside_count += is_inside ? 1 : -1;
1325 if (std::abs(inside_count) >= 2) {
1333 return inside_count > 0;
1336 void gather_colliders_in_geometry(
const StringRef path,
1339 const GeometrySet &collider_geo,
1340 const GeometrySet *prev_collider_geo,
1342 const float friction,
1343 const float compliance,
1344 const bool deforming,
1345 const bool use_edge_contacts,
1346 const bool is_boundary,
1347 const float error_threshold,
1348 const Span<int> affected_data,
1349 Vector<int> &instance_id_stack)
1351 if (
const Mesh *mesh = collider_geo.
get_mesh()) {
1352 if (mesh->faces_num > 0) {
1353 MeshCollider mesh_collider;
1354 mesh_collider.path = path;
1355 mesh_collider.instance_ids = instance_id_stack;
1356 mesh_collider.margin = margin;
1357 mesh_collider.friction = friction;
1358 mesh_collider.compliance = compliance;
1359 mesh_collider.use_edge_contacts = use_edge_contacts;
1360 mesh_collider.is_boundary = is_boundary;
1361 mesh_collider.error_threshold = error_threshold;
1362 mesh_collider.begin_transform = prev_transform;
1363 mesh_collider.end_transform =
transform;
1364 const Mesh *prev_mesh = prev_collider_geo ? prev_collider_geo->
get_mesh() :
nullptr;
1365 mesh_collider.mesh = this->make_collider_mesh_info(*mesh, prev_mesh, deforming);
1367 const int collider_i = constraints_.mesh_colliders.append_and_get_index(
1368 std::move(mesh_collider));
1369 for (
const int data_key_i : affected_data) {
1370 geometries_.data[data_key_i]->mesh_colliders.append({collider_i});
1374 if (
const bke::Instances *instances = collider_geo.
get_instances()) {
1375 struct PrevInstanceItem {
1377 const bke::InstanceReference *reference;
1379 Map<int, PrevInstanceItem> prev_instance_by_id;
1380 if (prev_collider_geo) {
1381 const bke::Instances *prev_instances = prev_collider_geo->
get_instances();
1382 if (prev_instances) {
1383 const Span<float4x4> prev_instance_transforms = prev_instances->
transforms();
1384 const Span<int> prev_instance_ids = prev_instances->
unique_ids();
1386 const Span<bke::InstanceReference> prev_references = prev_instances->
references();
1387 for (
const int i : prev_instance_transforms.
index_range()) {
1388 const int prev_instance_id = prev_instance_ids[i];
1389 const float4x4 &prev_instance_transform = prev_instance_transforms[i];
1390 const bke::InstanceReference &prev_reference = prev_references[prev_handles[i]];
1391 prev_instance_by_id.
add(prev_instance_id, {&prev_instance_transform, &prev_reference});
1396 const Span<float4x4> instance_transforms = instances->transforms();
1397 const Span<bke::InstanceReference> references = instances->references();
1398 const Span<int> handles = instances->reference_handles();
1399 const Span<int> instance_ids = instances->unique_ids();
1400 for (
const int instance_i : instance_transforms.
index_range()) {
1401 const int handle = handles[instance_i];
1406 const float4x4 instance_transform = instance_transforms[instance_i];
1408 const bke::InstanceReference &reference = references[handle];
1409 GeometrySet reference_geo;
1411 GeometrySet prev_reference_geo;
1412 float4x4 prev_instance_transform = instance_transform;
1413 if (prev_instance_item) {
1414 prev_instance_item->reference->to_geometry_set(prev_reference_geo);
1415 prev_instance_transform = *prev_instance_item->transform;
1420 this->gather_colliders_in_geometry(path,
1421 transform * instance_transform,
1422 prev_transform * prev_instance_transform,
1424 prev_instance_item ? &prev_reference_geo :
nullptr,
1438 std::variant<StaticMeshInfo, DeformingMeshInfo> make_collider_mesh_info(
const Mesh &mesh,
1439 const Mesh *prev_mesh,
1440 const bool deforming)
1442 if (!deforming || !prev_mesh) {
1443 return StaticMeshInfo{&mesh, &mesh.bvh_tris(), mesh.bvh_edges_legacy()};
1445 if (mesh.verts_num != prev_mesh->verts_num) {
1446 return StaticMeshInfo{&mesh, &mesh.bvh_tris(), mesh.bvh_edges_legacy()};
1448 const int verts_num = mesh.verts_num;
1449 DeformingMeshInfo
result;
1450 result.substep_meshes.resize(substeps_ + 1);
1451 result.substep_corner_tris_bvh_trees.resize(substeps_);
1452 result.substep_edges_bvh_trees.resize(substeps_);
1454 const Span<float3> begin_positions = prev_mesh->vert_positions();
1455 const Span<float3> end_positions = mesh.vert_positions();
1458 IndexRange(substeps_ + 1), std::max(1024 / verts_num, 1), [&](
const IndexRange range) {
1459 TLS &tls = tls_.local();
1460 for (
const int mesh_i : range) {
1461 const float mix_factor = (mesh_i - 1) /
float(substeps_) *
1462 (interpolation_end_ - interpolation_begin_) +
1463 interpolation_begin_;
1464 const Mesh *substep_mesh =
nullptr;
1465 if (mix_factor == 0.0f && mesh_i == 0) {
1466 substep_mesh = prev_mesh;
1468 else if (mix_factor == 1.0f) {
1469 substep_mesh = &mesh;
1473 tls.scope.add_destruct_call(
1474 [interpolated_mesh]() {
BKE_id_free(
nullptr, interpolated_mesh); });
1475 MutableSpan<float3> substep_positions =
1476 interpolated_mesh->vert_positions_for_write();
1479 begin_positions[i], end_positions[i], mix_factor);
1481 interpolated_mesh->tag_positions_changed();
1482 substep_mesh = interpolated_mesh;
1484 result.substep_meshes[mesh_i] = substep_mesh;
1487 result.substep_corner_tris_bvh_trees[mesh_i - 1] = &substep_mesh->bvh_tris();
1488 result.substep_edges_bvh_trees[mesh_i - 1] = substep_mesh->bvh_edges_legacy();
1495 void prepare_geometry_chunks()
1497 constexpr int approx_points_per_chunk = 256;
1498 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1499 GeometryData &geo_data = *geometries_.data[data_key_i];
1500 if (geo_data.size == 0) {
1503 const int old_chunks_num = geometries_.chunks.size();
1505 if (geo_data.curves) {
1507 const bke::CurvesGeometry &curves = *geo_data.curves;
1509 Stack<IndexRange> curve_ranges_to_check;
1511 while (!curve_ranges_to_check.
is_empty()) {
1512 const IndexRange curves_range = curve_ranges_to_check.
pop();
1513 const IndexRange points_range = points_by_curve[curves_range];
1517 if (curves_range.
size() == 1 || points_range.
size() <= approx_points_per_chunk) {
1518 geometries_.chunks.append({data_key_i, points_range, curves_range});
1521 const int split_pos = curves_range.
size() / 2;
1522 const IndexRange curves_range_left = curves_range.
take_front(split_pos);
1523 const IndexRange curves_range_right = curves_range.
drop_front(split_pos);
1525 curve_ranges_to_check.
push(curves_range_right);
1526 curve_ranges_to_check.
push(curves_range_left);
1530 const IndexRange points_range =
IndexRange(geo_data.size);
1531 Stack<IndexRange> ranges_to_check;
1532 ranges_to_check.
push(points_range);
1533 while (!ranges_to_check.
is_empty()) {
1534 const IndexRange points_range = ranges_to_check.
pop();
1538 if (points_range.
size() <= approx_points_per_chunk) {
1539 geometries_.chunks.append({data_key_i, points_range});
1542 const int split_pos = points_range.
size() / 2;
1543 const IndexRange points_range_left = points_range.
take_front(split_pos);
1544 const IndexRange points_range_right = points_range.
drop_front(split_pos);
1546 ranges_to_check.
push(points_range_right);
1547 ranges_to_check.
push(points_range_left);
1553 geo_data.point_to_chunk.reinitialize(geo_data.size);
1554 for (
const int chunk_i : geo_data.chunks) {
1555 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
1556 geo_data.point_to_chunk.as_mutable_span().slice(chunk.points_range).fill(chunk_i);
1559 chunks_data_.reinitialize(geometries_.chunks.size());
1560 for (
const int chunk_i : geometries_.chunks.index_range()) {
1561 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
1562 geometries_.max_chunk_size = std::max<int>(geometries_.max_chunk_size,
1563 chunk.points_range.size());
1567 void prepare_inverse_masses()
1569 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1570 GeometryData &geo_data = *geometries_.data[data_key_i];
1571 geo_data.inv_masses.reinitialize(geo_data.size);
1572 MutableSpan<float> inv_masses = geo_data.inv_masses;
1574 if (
const std::optional<float> mass = geo_data.masses.get_if_single()) {
1578 const VArraySpan<float> masses = geo_data.masses;
1580 for (const int i : range) {
1581 const float mass = masses[i];
1582 inv_masses[i] = math::safe_rcp(mass);
1589 void prepare_inverse_moments_of_inertia()
1591 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1592 GeometryData &geo_data = *geometries_.data[data_key_i];
1593 if (!geo_data.uses_rotation) {
1597 MutableSpan<float3> inv_moments_of_inertia = geo_data.inv_moments_of_inertia;
1600 for (const int i : range) {
1601 const float3 &moment_of_inertia = geo_data.moments_of_inertia[i];
1602 if (math::is_zero(moment_of_inertia)) {
1603 inv_moments_of_inertia[i] = float3(0.0f);
1606 inv_moments_of_inertia[i] = math::safe_rcp(moment_of_inertia);
1613 void gather_from_world__stretch_shear_constraints()
1615 TLS &tls = tls_.
local();
1616 const Span<std::string> paths = nested_bundle_paths_.
lookup(RodStretchShearBundle::name);
1617 for (
const StringRef path : paths) {
1619 RodStretchShearConstraint constraint;
1620 constraint.path = path;
1621 constraint.error_threshold =
1623 constraint.lambda_pos_attr = bundle
1627 constraint.lambda_rot_attr = bundle
1632 const int constraint_i = constraints_.rod_stretch_shear_constraints.append_and_get_index(
1633 std::move(constraint));
1635 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1636 GeometryData &geo_data = *geometries_.data[data_key_i];
1637 if (!geo_data.curves) {
1640 if (!geo_data.uses_rotation) {
1642 TIP_(
"Rod stretch/shear constraint requires \"rotation\" attribute"));
1645 if (!this->effector_applies_to_geometry(path, bundle, data_key_i)) {
1648 geo_data.rod_stretch_shear_constraints.append({constraint_i});
1651 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1652 GeometryData &geo_data = *geometries_.data[data_key_i];
1653 for (RodStretchShearConstraintUsage &constraint_usage :
1654 geo_data.rod_stretch_shear_constraints)
1656 const RodStretchShearConstraint &constraint =
1657 constraints_.rod_stretch_shear_constraints[constraint_usage.constraint_i];
1658 VArray<float> rest_lengths = this->lookup_attribute_required<float>(
1659 data_key_i, this->prop_attr_name(constraint.path,
"rest_length"), geo_data.domain);
1660 if (!rest_lengths) {
1664 constraint_usage.compliances = this->make_range_spans(
1666 this->lookup_attribute_default<float>(
1668 this->prop_attr_name(constraint.path,
"compliance"),
1671 constraint_usage.rest_lengths = std::move(rest_lengths);
1672 constraint_usage.is_valid =
true;
1674 constraint_usage.lambdas_pos = tls.allocator.allocate_array<
float3>(geo_data.size);
1675 constraint_usage.lambdas_rot = tls.allocator.allocate_array<
float3>(geo_data.size);
1680 void create_chunk_constraints__rod_stretch_shear(TLS &tls,
const int chunk_i)
1682 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
1683 ChunkData &chunk_data = chunks_data_[chunk_i];
1684 const GeometryData &geo_data = *geometries_.
data[chunk.data_key_i];
1685 if (geo_data.rod_stretch_shear_constraints.is_empty()) {
1688 const bke::CurvesGeometry &curves = *geo_data.curves;
1690 for (
const RodStretchShearConstraintUsage &constraint_usage :
1691 geo_data.rod_stretch_shear_constraints)
1693 if (!constraint_usage.is_valid) {
1696 const RodStretchShearConstraint &constraint =
1697 constraints_.rod_stretch_shear_constraints[constraint_usage.constraint_i];
1698 chunk_data.static_constraints.append(
1699 &tls.scope.construct<xpbd::RodStretchAndShearConstraintSet>(
1701 *chunk.curves_range,
1703 constraint_usage.rest_lengths,
1704 constraint_usage.compliances.get_span_for_range(chunk.points_range),
1705 error_scale_from_threshold(constraint.error_threshold),
1706 constraint_usage.lambdas_pos,
1707 constraint_usage.lambdas_rot));
1711 void write_back__rod_stretch_shear()
1713 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1714 GeometryData &geo_data = *geometries_.data[data_key_i];
1715 for (
const RodStretchShearConstraintUsage &constraint_usage :
1716 geo_data.rod_stretch_shear_constraints)
1718 if (!constraint_usage.is_valid) {
1721 const RodStretchShearConstraint &constraint =
1722 constraints_.rod_stretch_shear_constraints[constraint_usage.constraint_i];
1723 geo_data.attributes.remove(constraint.lambda_pos_attr);
1724 geo_data.attributes.remove(constraint.lambda_rot_attr);
1725 if (bke::SpanAttributeWriter<float3> lambda_pos_attr =
1726 this->get_output_attribute_writer<float3>(
1727 data_key_i, constraint.lambda_pos_attr, geo_data.domain))
1729 lambda_pos_attr.span.copy_from(constraint_usage.lambdas_pos);
1730 lambda_pos_attr.finish();
1732 if (bke::SpanAttributeWriter<float3> lambda_rot_attr =
1733 this->get_output_attribute_writer<float3>(
1734 data_key_i, constraint.lambda_rot_attr, geo_data.domain))
1736 lambda_rot_attr.span.copy_from(constraint_usage.lambdas_rot);
1737 lambda_rot_attr.finish();
1743 void gather_from_world__bend_twist_constraints()
1745 TLS &tls = tls_.
local();
1746 const Span<std::string> paths = nested_bundle_paths_.
lookup(RodBendTwistBundle::name);
1747 for (
const StringRef path : paths) {
1750 RodBendTwistConstraint constraint;
1751 constraint.path = path;
1752 constraint.error_threshold =
1755 const int constraint_i = constraints_.rod_bend_twist_constraints.append_and_get_index(
1756 std::move(constraint));
1758 for (
const int data_key_i : geometries_.data.index_range()) {
1759 GeometryData &geo_data = *geometries_.data[data_key_i];
1760 if (!geo_data.curves) {
1763 if (!geo_data.uses_rotation) {
1765 TIP_(
"Rod stretch/shear constraint requires \"rotation\" attribute"));
1768 if (!this->effector_applies_to_geometry(path, bundle, data_key_i)) {
1771 geo_data.rod_bend_twist_constraints.append({constraint_i});
1774 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1775 GeometryData &geo_data = *geometries_.data[data_key_i];
1776 for (RodBendTwistConstraintUsage &constraint_usage : geo_data.rod_bend_twist_constraints) {
1777 const RodBendTwistConstraint &constraint =
1778 constraints_.rod_bend_twist_constraints[constraint_usage.constraint_i];
1779 constraint_usage.lambdas = tls.allocator.allocate_array<
float4>(geo_data.size);
1780 constraint_usage.compliances = this->make_range_spans(
1782 this->lookup_attribute_default<float>(
1784 this->prop_attr_name(constraint.path,
"compliance"),
1787 constraint_usage.rest_bend_rotations = this->lookup_attribute_default<math::Quaternion>(
1789 this->prop_attr_name(constraint.path,
"rest_bend_rotation"),
1796 void create_chunk_constraints__rod_bend_twist(TLS &tls,
const int chunk_i)
1798 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
1799 ChunkData &chunk_data = chunks_data_[chunk_i];
1800 const GeometryData &geo_data = *geometries_.
data[chunk.data_key_i];
1801 if (geo_data.rod_bend_twist_constraints.is_empty()) {
1804 const bke::CurvesGeometry &curves = *geo_data.curves;
1806 for (
const RodBendTwistConstraintUsage &constraint_usage : geo_data.rod_bend_twist_constraints)
1808 const RodBendTwistConstraint &constraint =
1809 constraints_.rod_bend_twist_constraints[constraint_usage.constraint_i];
1810 chunk_data.static_constraints.append(
1811 &tls.scope.construct<xpbd::RodBendAndTwistConstraintSet>(
1813 *chunk.curves_range,
1815 constraint_usage.rest_bend_rotations,
1816 constraint_usage.compliances.get_span_for_range(chunk.points_range),
1817 error_scale_from_threshold(constraint.error_threshold),
1818 constraint_usage.lambdas));
1822 void gather_from_world__edge_length_constraints()
1824 TLS &tls = tls_.
local();
1825 const Span<std::string> paths = nested_bundle_paths_.
lookup(EdgeLengthConstraintBundle::name);
1826 for (
const StringRef path : paths) {
1829 const float error_threshold =
1831 const int constraint_i = constraints_.edge_length_constraints.append_and_get_index(
1832 {path, error_threshold});
1834 for (
const int data_key_i : geometries_.data.index_range()) {
1835 const DataKey &data_key = geometries_.data_keys[data_key_i];
1836 GeometryData &geo_data = *geometries_.data[data_key_i];
1840 if (this->effector_applies_to_geometry(path, bundle, data_key_i)) {
1841 EdgeLengthConstraintUsage constraint_usage;
1842 constraint_usage.constraint_i = constraint_i;
1843 geo_data.edge_length_constraints.append(std::move(constraint_usage));
1848 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1849 const DataKey &data_key = geometries_.data_keys[data_key_i];
1850 GeometryData &geo_data = *geometries_.data[data_key_i];
1851 for (EdgeLengthConstraintUsage &constraint_usage : geo_data.edge_length_constraints) {
1852 const EdgeLengthConstraint &constraint =
1853 constraints_.edge_length_constraints[constraint_usage.constraint_i];
1854 const Mesh &mesh = *geometries_.geometry_sets[data_key.geo_bundle_i].geometry.get_mesh();
1855 const Span<int2> edges = mesh.edges();
1856 const int edge_num = edges.
size();
1858 VArray<float> rest_lengths = this->lookup_attribute_required<float>(
1859 data_key_i, this->prop_attr_name(constraint.path,
"rest_length"), AttrDomain::Edge);
1860 if (!rest_lengths) {
1864 constraint_usage.lambdas = tls.allocator.allocate_array<
float>(edge_num);
1865 constraint_usage.rest_lengths = rest_lengths;
1866 constraint_usage.compliances = this->lookup_attribute_default<float>(
1868 this->prop_attr_name(constraint.path,
"compliance"),
1871 constraint_usage.is_valid =
true;
1873 auto &constraint_set = tls.scope.construct<xpbd::DistanceConstraintSet>(
1876 constraint_usage.rest_lengths,
1877 constraint_usage.compliances,
1878 error_scale_from_threshold(constraint.error_threshold),
1879 constraint_usage.lambdas);
1880 xpbd::ConstraintColoring coloring = constraint_set.color_constraints(tls.allocator);
1881 geo_data.static_constraints.append({&constraint_set, std::move(coloring)});
1886 void gather_from_world__cross_edge_length_constraints()
1888 TLS &tls = tls_.
local();
1889 const Span<std::string> paths = nested_bundle_paths_.
lookup_as(
1890 CrossEdgeLengthConstraintBundle::name);
1891 for (
const StringRef path : paths) {
1893 const float error_threshold =
1895 const int constraint_i = constraints_.cross_edge_length_constraints.append_and_get_index(
1896 {path, error_threshold});
1897 for (
const int data_key_i : geometries_.data.index_range()) {
1898 const DataKey &data_key = geometries_.data_keys[data_key_i];
1899 GeometryData &geo_data = *geometries_.data[data_key_i];
1903 if (this->effector_applies_to_geometry(path, bundle, data_key_i)) {
1904 CrossEdgeLengthConstraintUsage constraint_usage;
1905 constraint_usage.constraint_i = constraint_i;
1906 geo_data.cross_edge_length_constraints.append(std::move(constraint_usage));
1911 for (
const int data_key_i : geometries_.data_keys.index_range()) {
1912 const DataKey &data_key = geometries_.data_keys[data_key_i];
1913 GeometryData &geo_data = *geometries_.data[data_key_i];
1914 for (CrossEdgeLengthConstraintUsage &constraint_usage :
1915 geo_data.cross_edge_length_constraints)
1917 const CrossEdgeLengthConstraint &constraint =
1918 constraints_.cross_edge_length_constraints[constraint_usage.constraint_i];
1919 const Mesh &mesh = *geometries_.geometry_sets[data_key.geo_bundle_i].geometry.get_mesh();
1920 const VArraySpan<float3> rest_positions = this->lookup_attribute_required<float3>(
1921 data_key_i, this->prop_attr_name(constraint.path,
"rest_position"), AttrDomain::Point);
1925 const VArray<float> edge_compliances = this->lookup_attribute_default<float>(
1927 this->prop_attr_name(constraint.path,
"compliance"),
1931 Vector<int2> &cross_edges = constraint_usage.cross_edge_points;
1932 Vector<float> &cross_edge_rest_lengths = constraint_usage.distances;
1933 Vector<float> &cross_edge_compliances = constraint_usage.compliances;
1935 this->foreach_cross_edge(mesh, [&](
const int edge_i,
const int v0,
const int v1) {
1936 const float3 &rest_position0 = rest_positions[v0];
1937 const float3 &rest_position1 = rest_positions[v1];
1938 const float rest_length =
math::distance(rest_position0, rest_position1);
1939 const float compliance = edge_compliances[edge_i];
1940 cross_edges.
append({v0, v1});
1941 cross_edge_rest_lengths.
append(rest_length);
1942 cross_edge_compliances.
append(compliance);
1945 constraint_usage.lambdas = tls.allocator.allocate_array<
float>(cross_edges.
size());
1947 auto &constraint_set = tls.scope.construct<xpbd::DistanceConstraintSet>(
1950 cross_edge_rest_lengths,
1951 cross_edge_compliances,
1952 error_scale_from_threshold(constraint.error_threshold),
1953 constraint_usage.lambdas);
1954 xpbd::ConstraintColoring coloring = constraint_set.color_constraints(tls.allocator);
1955 geo_data.static_constraints.append({&constraint_set, std::move(coloring)});
1960 void foreach_cross_edge(
const Mesh &mesh,
1961 std::invocable<int /*edge_i*/, int /*v0*/, int /*v1*/>
auto &&fn)
1965 const Span<int2> edges = mesh.edges();
1966 const Span<int> corner_verts = mesh.corner_verts();
1967 const OffsetIndices<int> faces = mesh.faces();
1972 faces, mesh.corner_edges(), mesh.edges_num, offsets, indices);
1975 const int2 &edge = edges[edge_i];
1976 const int edge_v0 = edge[0];
1977 const int edge_v1 = edge[1];
1978 const Span<int> incident_faces = edge_to_faces[edge_i];
1979 if (incident_faces.
size() != 2) {
1982 int face0 = incident_faces[0];
1983 int face1 = incident_faces[1];
1984 if (faces[face0].
size() > faces[face1].
size()) {
1985 std::swap(face0, face1);
1987 const IndexRange face0_corners = faces[face0];
1988 const IndexRange face1_corners = faces[face1];
1989 const int face0_size = face0_corners.
size();
1990 const int face1_size = face1_corners.
size();
1991 if (face0_size == 3 && face1_size == 3) {
1992 const int face0_v0 = corner_verts[face0_corners[0]];
1993 const int face0_v1 = corner_verts[face0_corners[1]];
1994 const int face0_v2 = corner_verts[face0_corners[2]];
1996 const int face1_v0 = corner_verts[face1_corners[0]];
1997 const int face1_v1 = corner_verts[face1_corners[1]];
1998 const int face1_v2 = corner_verts[face1_corners[2]];
2001 const int cross_edge_v0 = edge_v0 ^ edge_v1 ^ face0_v0 ^ face0_v1 ^ face0_v2;
2002 const int cross_edge_v1 = edge_v0 ^ edge_v1 ^ face1_v0 ^ face1_v1 ^ face1_v2;
2003 fn(edge_i, cross_edge_v0, cross_edge_v1);
2005 else if (face0_size == 3 && face1_size == 4) {
2006 const int face0_v0 = corner_verts[face0_corners[0]];
2007 const int face0_v1 = corner_verts[face0_corners[1]];
2008 const int face0_v2 = corner_verts[face0_corners[2]];
2011 const int cross_edge_v0 = edge_v0 ^ edge_v1 ^ face0_v0 ^ face0_v1 ^ face0_v2;
2012 for (
const int face1_corner : face1_corners) {
2013 const int face1_v = corner_verts[face1_corner];
2014 if (!
ELEM(face1_v, edge_v0, edge_v1)) {
2015 fn(edge_i, cross_edge_v0, face1_v);
2021 const Span<int> face0_verts = corner_verts.
slice(face0_corners);
2022 const Span<int> face1_verts = corner_verts.
slice(face1_corners);
2023 const int face0_i0 = face0_verts.
first_index(edge_v0);
2024 const int face0_dir = face0_verts[(face0_i0 + 1) % face0_size] == edge_v1 ? 1 : -1;
2025 const int face1_i1 = face1_verts.
first_index(edge_v1);
2026 const int face1_dir = face1_verts[(face1_i1 + 1) % face1_size] == edge_v0 ? 1 : -1;
2029 const int cross_edge_num = std::max(face0_size, face1_size) - 2;
2030 for (
const int i :
IndexRange(cross_edge_num)) {
2031 const int cross_edge_v0 = face0_verts[
mod_i(face0_i0 - face0_dir * (i + 1), face0_size)];
2032 const int cross_edge_v1 = face1_verts[
mod_i(face1_i1 - face1_dir * (i + 1), face1_size)];
2033 fn(edge_i, cross_edge_v0, cross_edge_v1);
2039 void gather_from_world__damping()
2041 TLS &tls = tls_.
local();
2042 const Span<std::string> paths = nested_bundle_paths_.
lookup(DampingBundle::name);
2043 for (
const StringRef path : paths) {
2045 if (!bundle_ptr || !*bundle_ptr) {
2048 const Bundle &bundle = **bundle_ptr;
2050 const int constraint_i = constraints_.damping_constraints.append_and_get_index({path});
2052 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2053 GeometryData &geo_data = *geometries_.data[data_key_i];
2054 if (this->effector_applies_to_geometry(path, bundle, data_key_i)) {
2055 geo_data.damping_constraints.append({constraint_i});
2060 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2061 GeometryData &geo_data = *geometries_.data[data_key_i];
2062 for (DampingConstraintUsage &constraint_usage : geo_data.damping_constraints) {
2063 const DampingConstraint &constraint =
2064 constraints_.damping_constraints[constraint_usage.constraint_i];
2065 constraint_usage.linear_dampings = this->make_range_spans(
2067 this->lookup_attribute_default<float>(data_key_i,
2068 this->prop_attr_name(constraint.path,
"linear"),
2071 constraint_usage.angular_dampings = this->make_range_spans(
2073 this->lookup_attribute_default<float>(data_key_i,
2074 this->prop_attr_name(constraint.path,
"angular"),
2078 constraint_usage.linear_damping_lambdas = tls.allocator.allocate_array<
float>(
2080 constraint_usage.angular_damping_lambdas = tls.allocator.allocate_array<
float>(
2086 void create_chunk_constraints__damping(TLS &tls,
const int chunk_i)
2088 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2089 ChunkData &chunk_data = chunks_data_[chunk_i];
2090 const GeometryData &geo_data = *geometries_.
data[chunk.data_key_i];
2092 for (
const DampingConstraintUsage &constraint_usage : geo_data.damping_constraints) {
2093 chunk_data.static_velocity_constraints.append(
2094 &tls.scope.construct<xpbd::LinearDampingConstraintSet>(
2097 constraint_usage.linear_dampings.get_span_for_range(chunk.points_range),
2098 constraint_usage.linear_damping_lambdas.slice(chunk.points_range)));
2099 if (geo_data.uses_rotation) {
2100 chunk_data.static_velocity_constraints.append(
2101 &tls.scope.construct<xpbd::AngularDampingConstraintSet>(
2104 constraint_usage.angular_dampings.get_span_for_range(chunk.points_range),
2105 constraint_usage.angular_damping_lambdas.slice(chunk.points_range)));
2110 void gather_from_world__pin_positions()
2112 TLS &tls = tls_.
local();
2113 const Span<std::string> paths = nested_bundle_paths_.
lookup(PinPositionBundle::name);
2114 for (
const StringRef path : paths) {
2116 PinPositionConstraint constraint;
2117 constraint.path = path;
2118 constraint.error_threshold =
2120 constraint.lambda_attr =
2122 const int constraint_i = constraints_.pin_position_constraints.append_and_get_index(
2123 std::move(constraint));
2125 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2126 GeometryData &geo_data = *geometries_.data[data_key_i];
2127 if (this->effector_applies_to_geometry(path, bundle, data_key_i)) {
2128 geo_data.pin_position_constraints.append({constraint_i});
2132 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2133 GeometryData &geo_data = *geometries_.data[data_key_i];
2134 for (PinPositionConstraintUsage &constraint_usage : geo_data.pin_position_constraints) {
2135 const PinPositionConstraint &constraint =
2136 constraints_.pin_position_constraints[constraint_usage.constraint_i];
2138 const VArray<bool> selection_attr = this->lookup_attribute_required<bool>(
2139 data_key_i, this->prop_attr_name(constraint.path,
"selection"), geo_data.domain);
2140 const VArray<float3> positions_attr = this->lookup_attribute_required<float3>(
2141 data_key_i, this->prop_attr_name(constraint.path,
"position"), geo_data.domain);
2142 if (!positions_attr || !selection_attr) {
2149 const VArray<float> compliances_attr = this->lookup_attribute_default<float>(
2151 this->prop_attr_name(constraint.path,
"compliance"),
2154 VArray<bool> prev_selection_attr;
2155 VArray<float3> prev_positions_attr;
2156 if (sub_delta_time_ > 0.0f) {
2157 prev_selection_attr = this->lookup_attribute_optional<bool>(
2159 this->prev_prop_attr_name(constraint.path,
"selection"),
2161 prev_positions_attr = this->lookup_attribute_optional<float3>(
2162 data_key_i, this->prev_prop_attr_name(constraint.path,
"position"), geo_data.domain);
2165 const int pin_num = pin_selection.
size();
2166 MutableSpan<int> points = tls.allocator.allocate_array<
int>(pin_num);
2169 MutableSpan<float3> end_positions = tls.allocator.allocate_array<
float3>(pin_num);
2170 MutableSpan<float> compliances = tls.allocator.allocate_array<
float>(pin_num);
2175 constraint_usage.points = points;
2176 constraint_usage.end_positions = end_positions;
2177 constraint_usage.compliances = compliances;
2179 constraint_usage.lambdas = tls.allocator.allocate_array<
float>(pin_num);
2180 constraint_usage.current_positions = tls.allocator.allocate_array<
float3>(pin_num);
2183 if (prev_selection_attr && prev_positions_attr) {
2184 MutableSpan<float3> begin_positions = tls.allocator.allocate_array<
float3>(pin_num);
2186 for (const int pin_i : range) {
2187 const int point_i = points[pin_i];
2188 const bool was_pinned = prev_selection_attr[point_i];
2190 begin_positions[pin_i] = prev_positions_attr[point_i];
2193 begin_positions[pin_i] = geo_data.position_attr.span[point_i];
2197 constraint_usage.begin_positions = begin_positions;
2200 constraint_usage.begin_positions = end_positions;
2206 void create_chunk_constraints__pin_positions(TLS &tls,
const int chunk_i)
2208 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2209 ChunkData &chunk_data = chunks_data_[chunk_i];
2210 GeometryData &geo_data = *geometries_.
data[chunk.data_key_i];
2211 for (
const int constraint_usage_i : geo_data.pin_position_constraints.index_range()) {
2212 const PinPositionConstraintUsage &constraint_usage =
2213 geo_data.pin_position_constraints[constraint_usage_i];
2214 const PinPositionConstraint &constraint =
2215 constraints_.pin_position_constraints[constraint_usage.constraint_i];
2218 for (
const int pin_i : constraint_usage.points.index_range()) {
2219 const int point_i = constraint_usage.points[pin_i];
2220 const float compliance = constraint_usage.compliances[pin_i];
2221 if (compliance <= 0.0f) {
2222 geo_data.is_hard_pinned[point_i] =
true;
2227 constraint_usage.points, chunk.points_range);
2231 chunk_data.pin_position_constraints.append({constraint_usage_i, pin_range});
2232 chunk_data.static_constraints.append(&tls.scope.construct<xpbd::PinPositionConstraintSet>(
2234 constraint_usage.points.slice(pin_range),
2235 constraint_usage.current_positions.slice(pin_range),
2236 constraint_usage.compliances.slice(pin_range),
2237 error_scale_from_threshold(constraint.error_threshold),
2238 constraint_usage.lambdas.slice(pin_range)));
2242 void write_back__pin_positions()
2244 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2245 GeometryData &geo_data = *geometries_.data[data_key_i];
2246 for (
const PinPositionConstraintUsage &constraint_usage : geo_data.pin_position_constraints)
2248 const PinPositionConstraint &constraint =
2249 constraints_.pin_position_constraints[constraint_usage.constraint_i];
2250 geo_data.attributes.remove(constraint.lambda_attr);
2252 for (
const PinPositionConstraintUsage &constraint_usage : geo_data.pin_position_constraints)
2254 if (constraint_usage.points.is_empty()) {
2258 const PinPositionConstraint &constraint =
2259 constraints_.pin_position_constraints[constraint_usage.constraint_i];
2260 if (bke::SpanAttributeWriter<float> lambda_attr = this->get_output_attribute_writer<float>(
2261 data_key_i, constraint.lambda_attr, geo_data.domain))
2263 for (
const int pin_i : constraint_usage.points.index_range()) {
2264 const int point_i = constraint_usage.points[pin_i];
2265 lambda_attr.span[point_i] = constraint_usage.lambdas[pin_i];
2267 lambda_attr.finish();
2273 void gather_from_world__pin_rotations()
2275 TLS &tls = tls_.
local();
2276 const Span<std::string> paths = nested_bundle_paths_.
lookup(PinRotationBundle::name);
2277 for (
const StringRef path : paths) {
2280 PinRotationConstraint constraint;
2281 constraint.path = path;
2282 constraint.error_threshold =
2284 const int constraint_i = constraints_.pin_rotation_constraints.append_and_get_index(
2285 std::move(constraint));
2287 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2288 GeometryData &geo_data = *geometries_.data[data_key_i];
2289 if (!geo_data.uses_rotation) {
2292 if (this->effector_applies_to_geometry(path, bundle, data_key_i)) {
2293 geo_data.pin_rotation_constraints.append({constraint_i});
2297 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2298 GeometryData &geo_data = *geometries_.data[data_key_i];
2299 for (PinRotationConstraintUsage &constraint_usage : geo_data.pin_rotation_constraints) {
2300 const PinRotationConstraint &constraint =
2301 constraints_.pin_rotation_constraints[constraint_usage.constraint_i];
2303 const VArray<bool> selection_attr = this->lookup_attribute_required<bool>(
2304 data_key_i, this->prop_attr_name(constraint.path,
"selection"), geo_data.domain);
2305 const VArray<math::Quaternion> rotation_attr =
2306 this->lookup_attribute_required<math::Quaternion>(
2307 data_key_i, this->prop_attr_name(constraint.path,
"rotation"), geo_data.domain);
2308 if (!selection_attr || !rotation_attr) {
2315 const VArray<float> compliances_attr = this->lookup_attribute_default<float>(
2317 this->prop_attr_name(constraint.path,
"compliance"),
2320 VArray<bool> prev_selection_attr;
2321 VArray<math::Quaternion> prev_rotations_attr;
2322 if (sub_delta_time_ > 0.0f) {
2323 prev_selection_attr = this->lookup_attribute_optional<bool>(
2325 this->prev_prop_attr_name(constraint.path,
"selection"),
2327 prev_rotations_attr = this->lookup_attribute_optional<math::Quaternion>(
2328 data_key_i, this->prev_prop_attr_name(constraint.path,
"rotation"), geo_data.domain);
2331 const int pin_num = pin_selection.
size();
2332 MutableSpan<int> points = tls.allocator.allocate_array<
int>(pin_num);
2335 MutableSpan<math::Quaternion> end_rotations =
2337 MutableSpan<float> compliances = tls.allocator.allocate_array<
float>(pin_num);
2342 constraint_usage.points = points;
2343 constraint_usage.end_rotations = end_rotations;
2344 constraint_usage.compliances = compliances;
2346 constraint_usage.lambdas = tls.allocator.allocate_array<
float4>(pin_num);
2347 constraint_usage.current_rotations = tls.allocator.allocate_array<
math::Quaternion>(
2351 if (prev_selection_attr && prev_rotations_attr) {
2352 MutableSpan<math::Quaternion> begin_rotations =
2355 for (const int pin_i : range) {
2356 const int point_i = points[pin_i];
2357 const bool was_pinned = prev_selection_attr[point_i];
2359 begin_rotations[pin_i] = prev_rotations_attr[point_i];
2362 begin_rotations[pin_i] = geo_data.rotation_attr.span[point_i];
2365 constraint_usage.begin_rotations = begin_rotations;
2368 constraint_usage.begin_rotations = end_rotations;
2374 void create_chunk_constraints__pin_rotations(TLS &tls,
const int chunk_i)
2376 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2377 ChunkData &chunk_data = chunks_data_[chunk_i];
2378 const GeometryData &geo_data = *geometries_.
data[chunk.data_key_i];
2379 for (
const int constraint_usage_i : geo_data.pin_rotation_constraints.index_range()) {
2380 const PinRotationConstraintUsage &constraint_usage =
2381 geo_data.pin_rotation_constraints[constraint_usage_i];
2382 const PinRotationConstraint &constraint =
2383 constraints_.pin_rotation_constraints[constraint_usage.constraint_i];
2386 constraint_usage.points, chunk.points_range);
2390 chunk_data.pin_rotation_constraints.append({constraint_usage_i, pin_range});
2391 chunk_data.static_constraints.append(&tls.scope.construct<xpbd::PinRotationConstraintSet>(
2393 constraint_usage.points.slice(pin_range),
2394 constraint_usage.current_rotations.slice(pin_range),
2395 constraint_usage.compliances.slice(pin_range),
2396 error_scale_from_threshold(constraint.error_threshold),
2397 constraint_usage.lambdas.slice(pin_range)));
2401 struct ResidualErrorTLS {
2402 float total_error_squared = 0.0f;
2403 int total_error_count = 0;
2406 Result do_simulation()
2408 this->prepare_solver_geometry_refs();
2409 float average_error_squared = 0.0f;
2411 if (this->support_chunk_simulation()) {
2412 threading::EnumerableThreadSpecific<ResidualErrorTLS> error_tls;
2414 this->parallel_for_each_chunk(1, [&](
const int chunk_i) {
2415 float chunk_error_squared;
2416 int chunk_error_count;
2417 int solver_refs_i = 0;
2418 for (
const int substep_i :
IndexRange(substeps_)) {
2419 const SubstepInterval substep(
2420 substeps_, substep_i, interpolation_begin_, interpolation_end_);
2421 this->simulate__update_pins__chunk(chunk_i, substep);
2422 this->simulate__inertial_update__chunk(chunk_i, solver_refs_i);
2423 this->simulate__gather_dynamic_constraints__chunk(substep, chunk_i, solver_refs_i);
2424 this->simulate__reset_forces__chunk(chunk_i);
2426 const Span<xpbd::GeometryRef> solver_refs = geometries_.solver_refs[solver_refs_i];
2427 xpbd::ConstraintSetParams solve_params{solver_refs, sub_delta_time_};
2428 for ([[maybe_unused]]
const int iter_i :
IndexRange(constraint_iterations_)) {
2429 xpbd::GaussSeidelUpdater updater{solver_refs};
2430 this->simulate__position_solve__single_iteration__chunk(
2431 chunk_i, solve_params, updater);
2436 this->simulate__update_velocities__chunk(chunk_i, solver_refs_i);
2437 this->simulate__velocity_solve__chunk(chunk_i, solver_refs_i);
2438 solver_refs_i = 1 - solver_refs_i;
2440 this->simulate__ensure_final_data_in_outputs__chunk(chunk_i);
2442 if (chunk_error_count) {
2443 error_tls.
local().total_error_squared += chunk_error_squared;
2444 error_tls.
local().total_error_count += chunk_error_count;
2448 float total_error_squared = 0.0f;
2449 int total_error_count = 0;
2450 for (
const ResidualErrorTLS &error : error_tls) {
2451 total_error_squared += error.total_error_squared;
2452 total_error_count += error.total_error_count;
2454 if (total_error_count) {
2455 average_error_squared = total_error_squared / total_error_count;
2459 int solver_refs_i = 0;
2460 for (
const int substep_i :
IndexRange(substeps_)) {
2461 const SubstepInterval substep(
2462 substeps_, substep_i, interpolation_begin_, interpolation_end_);
2463 this->parallel_for_each_chunk(16, [&](
const int chunk_i) {
2464 this->simulate__update_pins__chunk(chunk_i, substep);
2465 this->simulate__inertial_update__chunk(chunk_i, solver_refs_i);
2467 this->simulate__gather_dynamic_constraints(substep, solver_refs_i);
2468 this->simulate__reset_forces();
2469 for ([[maybe_unused]]
const int iter_i :
IndexRange(constraint_iterations_)) {
2470 this->simulate__position_solve__single_iteration(solver_refs_i, average_error_squared);
2472 this->parallel_for_each_chunk(16, [&](
const int chunk_i) {
2473 this->simulate__update_velocities__chunk(chunk_i, solver_refs_i);
2475 this->simulate__velocity_solve(solver_refs_i);
2476 this->parallel_for_each_chunk(16, [&](
const int chunk_i) {
2477 this->simulate__ensure_final_data_in_outputs__chunk(chunk_i);
2479 solver_refs_i = 1 - solver_refs_i;
2483 return Result{
math::sqrt(average_error_squared)};
2486 bool support_chunk_simulation()
const
2488 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2489 const GeometryData &geo_data = *geometries_.data[data_key_i];
2490 if (!geo_data.static_constraints.is_empty()) {
2497 void prepare_solver_geometry_refs()
2500 Array<xpbd::GeometryRef> &refs = geometries_.solver_refs[direction];
2502 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2503 GeometryData &geo_data = *geometries_.data[data_key_i];
2504 xpbd::GeometryRef &ref = refs[data_key_i];
2506 if (direction == 0) {
2507 ref.
positions = geo_data.temp_positions;
2509 if (geo_data.uses_rotation) {
2510 ref.
rotations = geo_data.temp_rotations;
2515 ref.
positions = geo_data.position_attr.span;
2517 if (geo_data.uses_rotation) {
2518 ref.
rotations = geo_data.rotation_attr.span;
2522 ref.
velocities = geo_data.velocity_attr.span;
2524 if (geo_data.uses_rotation) {
2533 void simulate__update_pins__chunk(
const int chunk_i,
const SubstepInterval &substep)
2535 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2536 const int data_key_i = chunk.data_key_i;
2537 GeometryData &geo_data = *geometries_.data[data_key_i];
2538 ChunkData &chunk_data = chunks_data_[chunk_i];
2541 for (
const PinPositionConstraintChunkUsage &constraint_chunk_usage :
2542 chunk_data.pin_position_constraints)
2544 const PinPositionConstraintUsage &constraint_usage =
2545 geo_data.pin_position_constraints[constraint_chunk_usage.constraint_usage_i];
2546 for (
const int pin_i : constraint_chunk_usage.pin_range) {
2547 const float3 &begin_pos = constraint_usage.begin_positions[pin_i];
2548 const float3 &end_pos = constraint_usage.end_positions[pin_i];
2550 constraint_usage.current_positions[pin_i] = pin_pos;
2553 if (geo_data.uses_rotation) {
2554 for (
const PinRotationConstraintChunkUsage &constraint_chunk_usage :
2555 chunk_data.pin_rotation_constraints)
2557 const PinRotationConstraintUsage &constraint_usage =
2558 geo_data.pin_rotation_constraints[constraint_chunk_usage.constraint_usage_i];
2559 for (
const int pin_i : constraint_chunk_usage.pin_range) {
2560 const math::Quaternion &begin_rot = constraint_usage.begin_rotations[pin_i];
2563 begin_rot, end_rot, substep.interpolate_end);
2564 constraint_usage.current_rotations[pin_i] = pin_rot;
2570 void simulate__inertial_update__chunk(
const int chunk_i,
const int solver_refs_i)
2572 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2573 const IndexRange points_range = chunk.points_range;
2574 const int data_key_i = chunk.data_key_i;
2575 GeometryData &geo_data = *geometries_.data[data_key_i];
2576 xpbd::GeometryRef &ref = geometries_.solver_refs[solver_refs_i][data_key_i];
2578 this->integrate_linear_velocities(sub_delta_time_,
2581 geo_data.velocity_attr.span.slice(points_range),
2582 geo_data.inv_masses.as_span().slice(points_range),
2583 geo_data.external_force_attr.slice(points_range));
2584 if (geo_data.uses_rotation) {
2585 this->integrate_angular_velocities(
2589 geo_data.angular_velocity_attr.span.slice(points_range),
2590 geo_data.inv_moments_of_inertia.as_span().slice(points_range),
2591 geo_data.external_torque_attr.slice(points_range));
2595 void simulate__gather_dynamic_constraints(
const SubstepInterval &substep,
2596 const int solver_refs_i)
2598 this->parallel_for_each_chunk(1, [&](
const int chunk_i) {
2599 this->simulate__gather_dynamic_constraints__chunk(substep, chunk_i, solver_refs_i);
2603 void simulate__gather_dynamic_constraints__chunk(
const SubstepInterval &substep,
2605 const int solver_refs_i)
2607 const float max_distance = this->get_max_search_distance(sub_delta_time_);
2608 ChunkData &chunk_data = chunks_data_[chunk_i];
2609 const ExternalFaceContacts &prev_face_contacts = chunk_data.external_face_contacts;
2610 const ExternalEdgeContacts &prev_edge_contacts = chunk_data.external_edge_contacts;
2611 ExternalFaceContacts new_face_contacts;
2612 ExternalEdgeContacts new_edge_contacts;
2613 this->gather_contacts__mesh_colliders(chunk_i,
2622 for (
const int i :
IndexRange(new_face_contacts.points.size())) {
2623 new_face_contacts.collider_velocities.append(
2626 for (
const int i :
IndexRange(new_edge_contacts.point_pairs.size())) {
2627 new_edge_contacts.collider_velocities.append(
2630 chunk_data.external_face_contacts = std::move(new_face_contacts);
2631 chunk_data.external_edge_contacts = std::move(new_edge_contacts);
2634 void simulate__reset_forces()
2636 this->parallel_for_each_chunk(
2637 16, [&](
const int chunk_i) { this->simulate__reset_forces__chunk(chunk_i); });
2638 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2639 GeometryData &geo_data = *geometries_.data[data_key_i];
2640 for (ConstraintWithColoring &constraint : geo_data.static_constraints) {
2641 constraint.constraint->reset_forces();
2646 void simulate__reset_forces__chunk(
const int chunk_i)
2648 ChunkData &chunk_data = chunks_data_[chunk_i];
2649 chunk_data.external_face_contacts.lambdas_normal.
fill(0.0f);
2650 chunk_data.external_face_contacts.lambdas_friction.fill(0.0f);
2651 for (xpbd::ConstraintSet *constraint : chunk_data.static_constraints) {
2654 for (xpbd::VelocityConstraintSet *constraint : chunk_data.static_velocity_constraints) {
2659 void simulate__position_solve__single_iteration(
const int solver_refs_i,
2660 float &r_average_error_squared)
2662 const Span<xpbd::GeometryRef> solver_refs = geometries_.solver_refs[solver_refs_i];
2663 xpbd::ConstraintSetParams solve_params{solver_refs, sub_delta_time_};
2664 threading::EnumerableThreadSpecific<ResidualErrorTLS> error_tls;
2666 this->parallel_for_each_chunk(1, [&](
const int chunk_i) {
2667 xpbd::GaussSeidelUpdater updater{solver_refs};
2668 this->simulate__position_solve__single_iteration__chunk(chunk_i, solve_params, updater);
2673 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2674 const GeometryData &geo_data = *geometries_.data[data_key_i];
2675 for (
const ConstraintWithColoring &constraint : geo_data.static_constraints) {
2676 for (
const int color_i : constraint.coloring.colors.index_range()) {
2677 const IndexMask &
mask = constraint.coloring.colors[color_i];
2679 const IndexMask sliced_mask = mask.slice(range);
2680 xpbd::GaussSeidelUpdater updater{solver_refs};
2681 constraint.constraint->solve_sequential(solve_params, updater, sliced_mask);
2689 float total_error_squared = 0.0f;
2690 int total_error_count = 0;
2691 for (
const ResidualErrorTLS &error : error_tls) {
2692 total_error_squared += error.total_error_squared;
2693 total_error_count += error.total_error_count;
2695 if (total_error_count) {
2696 r_average_error_squared = total_error_squared / total_error_count;
2700 void simulate__position_solve__single_iteration__chunk(
const int chunk_i,
2701 xpbd::ConstraintSetParams solve_params,
2702 xpbd::GaussSeidelUpdater &updater)
2704 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2705 ChunkData &chunk_data = chunks_data_[chunk_i];
2707 for (xpbd::ConstraintSet *constraint : chunk_data.static_constraints) {
2711 if (!chunk_data.external_face_contacts.points.is_empty()) {
2712 ExternalFaceContacts &contacts = chunk_data.external_face_contacts;
2713 xpbd::CollisionFaceConstraintSet constraint(chunk.data_key_i,
2715 contacts.point_radii,
2716 contacts.positions_on_face,
2717 contacts.collider_motion,
2718 contacts.face_normals,
2719 contacts.face_margins,
2720 contacts.compliance_terms,
2721 contacts.static_frictions,
2722 contacts.dynamic_frictions,
2723 contacts.error_scales,
2724 contacts.active_states,
2725 contacts.lambdas_normal);
2728 if (!chunk_data.external_edge_contacts.point_pairs.is_empty()) {
2729 ExternalEdgeContacts &contacts = chunk_data.external_edge_contacts;
2730 xpbd::CollisionEdgeConstraintSet constraint(chunk.data_key_i,
2731 contacts.point_pairs,
2732 contacts.point_radii,
2733 contacts.positions_on_edge,
2734 contacts.collider_motion,
2735 contacts.edge_directions,
2736 contacts.edge_normals,
2737 contacts.edge_margins,
2738 contacts.compliance_terms,
2739 contacts.static_frictions,
2740 contacts.dynamic_frictions,
2741 contacts.error_scales,
2742 contacts.active_states,
2743 contacts.point_mix_factors,
2744 contacts.lambdas_normal);
2749 void simulate__update_velocities__chunk(
const int chunk_i,
const int solver_refs_i)
2751 if (sub_delta_time_ <= 0.0f) {
2756 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2757 const IndexRange points_range = chunk.points_range;
2758 const int data_key_i = chunk.data_key_i;
2759 GeometryData &geo_data = *geometries_.data[data_key_i];
2760 xpbd::GeometryRef &ref = geometries_.solver_refs[solver_refs_i][data_key_i];
2761 this->update_linear_velocities(sub_delta_time_,
2764 geo_data.velocity_attr.span.slice(points_range));
2765 if (geo_data.uses_rotation) {
2766 this->update_angular_velocities(sub_delta_time_,
2769 geo_data.angular_velocity_attr.span.slice(points_range));
2773 void simulate__velocity_solve(
const int solver_refs_i)
2775 this->parallel_for_each_chunk(8, [&](
const int chunk_i) {
2776 this->simulate__velocity_solve__chunk(chunk_i, solver_refs_i);
2780 void simulate__velocity_solve__chunk(
const int chunk_i,
const int solver_refs_i)
2782 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2783 ChunkData &chunk_data = chunks_data_[chunk_i];
2785 const Span<xpbd::GeometryRef> solver_refs = geometries_.solver_refs[solver_refs_i];
2786 xpbd::ConstraintSetParams params{solver_refs, sub_delta_time_};
2787 xpbd::VelocityUpdater velocity_updater{solver_refs};
2789 for (xpbd::VelocityConstraintSet *constraint : chunk_data.static_velocity_constraints) {
2793 if (!chunk_data.external_face_contacts.points.is_empty()) {
2794 ExternalFaceContacts &contacts = chunk_data.external_face_contacts;
2795 xpbd::FrictionFaceConstraintSet constraint(chunk.data_key_i,
2797 contacts.face_normals,
2798 contacts.collider_velocities,
2799 contacts.dynamic_frictions,
2800 contacts.lambdas_normal,
2801 contacts.lambdas_friction);
2804 if (!chunk_data.external_edge_contacts.point_pairs.is_empty()) {
2805 ExternalEdgeContacts &contacts = chunk_data.external_edge_contacts;
2806 xpbd::FrictionEdgeConstraintSet constraint(chunk.data_key_i,
2807 contacts.point_pairs,
2808 contacts.edge_normals,
2809 contacts.collider_velocities,
2810 contacts.dynamic_frictions,
2811 contacts.lambdas_normal,
2812 contacts.point_mix_factors,
2813 contacts.lambdas_friction);
2818 void simulate__ensure_final_data_in_outputs__chunk(
const int chunk_i)
2820 if (substeps_ % 2 == 0) {
2824 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
2825 GeometryData &geo_data = *geometries_.
data[chunk.data_key_i];
2826 const IndexRange points_range = chunk.points_range;
2827 geo_data.position_attr.span.
slice(points_range)
2828 .copy_from(geo_data.temp_positions.as_span().slice(points_range));
2829 if (geo_data.uses_rotation) {
2830 geo_data.rotation_attr.span.slice(points_range)
2831 .copy_from(geo_data.temp_rotations.as_span().slice(points_range));
2835 void finish_common_attribute_writers()
2837 for (
const int data_key_i : geometries_.data_keys.index_range()) {
2838 GeometryData &geo_data = *geometries_.data[data_key_i];
2840 geo_data.position_attr.finish();
2841 geo_data.velocity_attr.finish();
2842 geo_data.rotation_attr.finish();
2843 geo_data.angular_velocity_attr.finish();
2847 void integrate_linear_velocities(
const float delta_time,
2848 const Span<float3> old_positions,
2849 MutableSpan<float3> new_positions,
2850 MutableSpan<float3> velocities,
2851 const Span<float> inv_masses,
2852 const Span<float3> forces)
2856 const float inv_mass = inv_masses[i];
2857 const float3 &old_pos = old_positions[i];
2859 const float3 new_velocity = velocities[i] + acceleration * delta_time;
2860 velocities[i] = new_velocity;
2861 new_positions[i] = old_pos + new_velocity * delta_time;
2865 void integrate_angular_velocities(
const float delta_time,
2866 const Span<math::Quaternion> old_rotations,
2867 MutableSpan<math::Quaternion> new_rotations,
2868 MutableSpan<float3> angular_velocities,
2869 const Span<float3> inv_inertias,
2870 const Span<float3> torques)
2875 const float3 &inv_inertia = inv_inertias[i];
2877 new_rotations[i] = old_rotation;
2890 void update_linear_velocities(
const float delta_time,
2891 const Span<float3> prev_positions,
2892 const Span<float3> new_positions,
2893 MutableSpan<float3> r_velocities)
2897 const float3 &old_pos = prev_positions[i];
2898 const float3 &new_pos = new_positions[i];
2905 void update_angular_velocities(
const float delta_time,
2906 const Span<math::Quaternion> prev_rotations,
2907 const Span<math::Quaternion> new_rotations,
2908 MutableSpan<float3> r_angular_velocities)
2911 for (
const int i : r_angular_velocities.
index_range()) {
2919 const float3 new_angular_velocity = 2.0f *
diff * inv_delta_time;
2920 r_angular_velocities[i] = new_angular_velocity;
2924 float compute_contact_friction(
const float point_friction,
const float collider_friction)
const
2926 return math::sqrt(point_friction * collider_friction);
2929 float get_max_search_distance(
const float delta_time)
2932 constexpr float max_velocity = 60.0f;
2933 const float max_search_distance = 2.0f * max_velocity * delta_time;
2934 return max_search_distance;
2937 template<
typename T>
2938 bke::SpanAttributeWriter<T> ensure_attribute(bke::MutableAttributeAccessor attributes,
2939 const StringRef name,
2942 const std::optional<bke::AttributeMetaData> meta_data = attributes.
lookup_meta_data(name);
2944 if (meta_data->domain != domain ||
2953 template<
typename T>
2954 bke::SpanAttributeWriter<T> get_output_attribute_writer(
const int geo_data_i,
2955 const StringRef name,
2961 GeometryData &geo_data = *geometries_.data[geo_data_i];
2962 return geo_data.attributes.lookup_or_add_for_write_span<
T>(name, domain);
2965 const Bundle *get_previous_bundle(
const Bundle &bundle)
const
2969 if (!previous_bundle_ptr || !*previous_bundle_ptr) {
2972 return &**previous_bundle_ptr;
2975 bool effector_applies_to_geometry([[maybe_unused]]
const StringRef effector_path,
2977 const int data_key_i)
const
2979 const DataKey &data_key = geometries_.data_keys[data_key_i];
2980 const GeometrySetData &geo_set_data = geometries_.geometry_sets[data_key.geo_bundle_i];
2982 const std::string
filter =
2988 void parallel_for_each_chunk(
const int grain_size,
const FunctionRef<
void(
int chunk_i)> fn)
2991 geometries_.chunks.index_range(), grain_size, [&](
const IndexRange chunk_range) {
2992 for (const int chunk_i : chunk_range) {
2998 void write_back_geometries_to_world()
3000 for (
const int geo_bundle_i : geometries_.geometry_sets.index_range()) {
3001 GeometrySetData &geo_set_data = geometries_.geometry_sets[geo_bundle_i];
3006 PointCloud *write_back__plane_contacts(
const IndexRange mesh_colliders_range,
3007 const OffsetIndices<int> points_by_chunk)
3012 bke::SpanAttributeWriter<int> geometries_writer =
3014 bke::SpanAttributeWriter<int> colliders_writer =
3016 bke::SpanAttributeWriter<int> geometry_points0_writer =
3018 MutableSpan<float3> positions = pointcloud->positions_for_write();
3019 bke::SpanAttributeWriter<float3> collider_velocities_writer =
3022 bke::SpanAttributeWriter<float3> normals_writer =
3024 bke::SpanAttributeWriter<float> static_frictions_writer =
3026 bke::SpanAttributeWriter<float> dynamic_frictions_writer =
3028 bke::SpanAttributeWriter<bool> active_states_writer =
3030 bke::SpanAttributeWriter<float> lambdas_normal_writer =
3032 bke::SpanAttributeWriter<float> lambdas_friction_writer =
3035 this->parallel_for_each_chunk(16, [&](
const int chunk_i) {
3036 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
3037 const ChunkData &chunk_data = chunks_data_[chunk_i];
3038 const ExternalFaceContacts &contacts = chunk_data.external_face_contacts;
3040 const IndexRange points = points_by_chunk[chunk_i];
3048 MutableSpan<int> geometries = geometries_writer.
span.slice(points);
3049 MutableSpan<int> colliders = colliders_writer.span.slice(points);
3050 MutableSpan<int> geometry_points0 = geometry_points0_writer.span.slice(points);
3052 geometries.
fill(chunk.data_key_i);
3053 for (
const auto &item : contacts.mesh_contact_indices.items()) {
3054 colliders[item.value] = mesh_colliders_range[item.key.mesh_collider_i];
3055 geometry_points0[item.value] = contacts.points[item.value];
3058 array_utils::copy(contacts.positions_on_face.as_span(), positions.slice(points));
3060 collider_velocities_writer.
span.slice(points));
3061 array_utils::copy(contacts.face_normals.as_span(), normals_writer.span.slice(points));
3063 static_frictions_writer.span.slice(points));
3065 dynamic_frictions_writer.span.slice(points));
3066 array_utils::copy(contacts.active_states.as_span(), active_states_writer.span.slice(points));
3068 lambdas_normal_writer.span.slice(points));
3070 lambdas_friction_writer.span.slice(points));
3072 pointcloud->tag_positions_changed();
3073 geometries_writer.
finish();
3074 colliders_writer.finish();
3075 geometry_points0_writer.finish();
3076 collider_velocities_writer.
finish();
3077 normals_writer.finish();
3078 static_frictions_writer.finish();
3079 dynamic_frictions_writer.finish();
3080 active_states_writer.finish();
3081 lambdas_normal_writer.finish();
3082 lambdas_friction_writer.finish();
3087 PointCloud *write_back__edge_contacts(
const IndexRange mesh_colliders_range,
3088 const OffsetIndices<int> points_by_chunk)
3093 bke::SpanAttributeWriter<int> geometries_writer =
3095 bke::SpanAttributeWriter<int> colliders_writer =
3097 bke::SpanAttributeWriter<int> geometry_points0_writer =
3099 bke::SpanAttributeWriter<int> geometry_points1_writer =
3101 MutableSpan<float3> positions = pointcloud->positions_for_write();
3102 bke::SpanAttributeWriter<float3> collider_velocities_writer =
3105 bke::SpanAttributeWriter<float3> normals_writer =
3107 bke::SpanAttributeWriter<float> static_frictions_writer =
3109 bke::SpanAttributeWriter<float> dynamic_frictions_writer =
3111 bke::SpanAttributeWriter<bool> active_states_writer =
3113 bke::SpanAttributeWriter<float> lambdas_normal_writer =
3115 bke::SpanAttributeWriter<float> lambdas_friction_writer =
3118 this->parallel_for_each_chunk(16, [&](
const int chunk_i) {
3119 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
3120 const ChunkData &chunk_data = chunks_data_[chunk_i];
3121 const ExternalEdgeContacts &contacts = chunk_data.external_edge_contacts;
3123 const IndexRange points = points_by_chunk[chunk_i];
3131 MutableSpan<int> geometries = geometries_writer.
span.slice(points);
3132 MutableSpan<int> colliders = colliders_writer.span.
slice(points);
3133 MutableSpan<int> geometry_points0 = geometry_points0_writer.span.
slice(points);
3134 MutableSpan<int> geometry_points1 = geometry_points1_writer.span.
slice(points);
3135 geometries.
fill(chunk.data_key_i);
3136 for (
const auto &item : contacts.mesh_contact_indices.items()) {
3137 colliders[item.value] = mesh_colliders_range[item.key.mesh_collider_i];
3138 geometry_points0[item.value] = contacts.point_pairs[item.value][0];
3139 geometry_points1[item.value] = contacts.point_pairs[item.value][1];
3142 array_utils::copy(contacts.positions_on_edge.as_span(), positions.slice(points));
3144 collider_velocities_writer.
span.slice(points));
3145 array_utils::copy(contacts.edge_normals.as_span(), normals_writer.span.slice(points));
3147 static_frictions_writer.span.slice(points));
3149 dynamic_frictions_writer.span.slice(points));
3150 array_utils::copy(contacts.active_states.as_span(), active_states_writer.span.slice(points));
3152 lambdas_normal_writer.span.slice(points));
3154 lambdas_friction_writer.span.slice(points));
3156 pointcloud->tag_positions_changed();
3157 geometries_writer.
finish();
3158 colliders_writer.finish();
3159 geometry_points0_writer.finish();
3160 geometry_points1_writer.finish();
3161 collider_velocities_writer.
finish();
3162 normals_writer.finish();
3163 static_frictions_writer.finish();
3164 dynamic_frictions_writer.finish();
3165 active_states_writer.finish();
3166 lambdas_normal_writer.finish();
3167 lambdas_friction_writer.finish();
3172 void write_back__contacts()
3174 for (CollisionContacts &contacts : this->constraints_.collision_contacts) {
3177 {contacts.path,
"plane_contacts"});
3182 const IndexRange geometries_range = geometries_.geometry_sets.index_range();
3183 const IndexRange mesh_colliders_range = geometries_range.
after(
3184 constraints_.mesh_colliders.size());
3185 Vector<std::string> collider_paths;
3186 collider_paths.
reserve(geometries_range.size() + mesh_colliders_range.
size());
3187 for (
const GeometrySetData &geometry_set_data : geometries_.geometry_sets) {
3190 for (
const MeshCollider &collider : constraints_.mesh_colliders) {
3194 Array<int> plane_contacts_offsets(geometries_.chunks.size() + 1);
3195 Array<int> edge_contacts_offsets(geometries_.chunks.size() + 1);
3196 this->parallel_for_each_chunk(16, [&](
const int chunk_i) {
3197 const GeometryDataChunk &chunk = geometries_.chunks[chunk_i];
3198 const ChunkData &chunk_data = chunks_data_[chunk_i];
3200 if (!effector_applies_to_geometry(contacts.path, bundle, chunk.data_key_i)) {
3201 plane_contacts_offsets[chunk_i] = 0;
3202 edge_contacts_offsets[chunk_i] = 0;
3206 plane_contacts_offsets[chunk_i] = chunk_data.external_face_contacts.points.
size();
3207 edge_contacts_offsets[chunk_i] = chunk_data.external_edge_contacts.point_pairs.size();
3210 plane_contacts_offsets);
3212 edge_contacts_offsets);
3216 if (plane_contacts_by_chunk.total_size() > 0) {
3218 write_back__plane_contacts(mesh_colliders_range, plane_contacts_by_chunk));
3220 if (edge_contacts_by_chunk.
total_size() > 0) {
3222 write_back__edge_contacts(mesh_colliders_range, edge_contacts_by_chunk));
3225 world_.
add_path_override(plane_points_path, std::move(plane_contacts_geometry));
3238 template<
typename T> VArrayRangeSpans<T> make_range_spans(TLS &tls,
const VArray<T> &varray)
3243 template<
typename T>
3244 VArray<T> lookup_attribute_default(
const int data_key_i,
3245 const StringRef attribute_name,
3247 const T &default_value)
3249 GeometryData &geo_data = *geometries_.data[data_key_i];
3250 VArray<T> varray = *geo_data.attributes.lookup<
T>(attribute_name, domain);
3255 "Attribute \"{}\" not found in \"{}\", using fallback value",
3257 geometries_.geometry_sets[geometries_.data_keys[data_key_i].geo_bundle_i].path));
3263 template<
typename T>
3264 VArray<T> lookup_attribute_required(
const int data_key_i,
3265 const StringRef attribute_name,
3268 GeometryData &geo_data = *geometries_.data[data_key_i];
3269 VArray<T> varray = *geo_data.attributes.lookup<
T>(attribute_name, domain);
3274 "Attribute \"{}\" not found on \"{}\", some functionality is disabled",
3276 geometries_.geometry_sets[geometries_.data_keys[data_key_i].geo_bundle_i].path));
3281 template<
typename T>
3282 VArray<T> lookup_attribute_optional(
const int data_key_i,
3283 const StringRef attribute_name,
3286 GeometryData &geo_data = *geometries_.data[data_key_i];
3287 VArray<T> varray = *geo_data.attributes.lookup<
T>(attribute_name, domain);
3292 "Optional attribute \"{}\" not found on \"{}\"",
3294 geometries_.geometry_sets[geometries_.data_keys[data_key_i].geo_bundle_i].path));
3299 std::string prop_attr_name(
const StringRef effector_path,
const StringRef prop_name)
const
3301 return fmt::format(
"sim:prop:{}:{}", effector_path, prop_name);
3304 std::string prev_prop_attr_name(
const StringRef effector_path,
const StringRef prop_name)
const
3306 return fmt::format(
"sim:prop_prev:{}:{}", effector_path, prop_name);
3311 std::lock_guard<Mutex>
lock(warnings_mutex_);
3312 warnings_.
add({type, std::move(warning)});