diff --git a/plugins/plantarchitecture/src/InputOutput.cpp b/plugins/plantarchitecture/src/InputOutput.cpp index b1c227527..28c3c8f28 100644 --- a/plugins/plantarchitecture/src/InputOutput.cpp +++ b/plugins/plantarchitecture/src/InputOutput.cpp @@ -3508,6 +3508,38 @@ vec3 worldToLocal(const vec3 &world_point, const vec3 &xform_position, float qw, return result; } +// Inverse of worldToLocal: transform a point in a link's local frame into world space. +vec3 localToWorld(const vec3 &local_point, const USDLink &link) { + const vec3 &v = local_point; + vec3 u(link.qx, link.qy, link.qz); + vec3 uv; + uv.x = u.y * v.z - u.z * v.y; + uv.y = u.z * v.x - u.x * v.z; + uv.z = u.x * v.y - u.y * v.x; + vec3 uuv; + uuv.x = u.y * uv.z - u.z * uv.y; + uuv.y = u.z * uv.x - u.x * uv.z; + uuv.z = u.x * uv.y - u.y * uv.x; + vec3 result; + result.x = v.x + 2.f * (link.qw * uv.x + uuv.x); + result.y = v.y + 2.f * (link.qw * uv.y + uuv.y); + result.z = v.z + 2.f * (link.qw * uv.z + uuv.z); + return result + link.position; +} + +// Joint anchor on the parent link at the point where the child actually starts. A child does not always begin at +// the parent's tip: a lateral shoot branches off at a node, and segments shorter than min_segment_length are dropped. +vec3 parentAnchorAt(const vec3 &world_point, const USDLink &parent) { + return worldToLocal(world_point, parent.position, parent.qw, parent.qx, parent.qy, parent.qz); +} + +// Joint anchor on a leaf/fruit/flower link at the parent's tip. Organ links are positioned at their mesh centroid, so +// the anchor has to be expressed relative to it rather than placed at the organ's origin. +vec3 organAnchorAtParentTip(const USDLink &parent, const USDLink &organ) { + vec3 tip = localToWorld(make_vec3(0, 0, parent.half_length), parent); + return worldToLocal(tip, organ.position, organ.qw, organ.qx, organ.qy, organ.qz); +} + // Build a visual mesh for a tube segment by extracting the primitives belonging to // that segment from the tube compound object. For a tube with N nodes and R radial // subdivisions, segment i (between node i and node i+1) has 2*R triangles. @@ -3798,7 +3830,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo } else { joint.name = "J_" + data.links[parent_idx].name + "_to_" + link.name; joint.parent_link_index = parent_idx; - joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length); + joint.local_pos_parent = parentAnchorAt(start, data.links[parent_idx]); joint.local_pos_child = vec3(0, 0, -half_len); float avg_radius = (data.links[parent_idx].radius + seg_radius) / 2.f; @@ -3904,7 +3936,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo joint.name = "J_" + data.links[parent_idx].name + "_to_" + link.name; joint.parent_link_index = parent_idx; joint.child_link_index = this_link_idx; - joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length); + joint.local_pos_parent = parentAnchorAt(start, data.links[parent_idx]); joint.local_pos_child = vec3(0, 0, -half_len); float avg_radius = (data.links[parent_idx].radius + seg_radius) / 2.f; joint.stiffness = computeJointStiffness(params.elastic_modulus, avg_radius, length); @@ -3970,7 +4002,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo joint.parent_link_index = parent_idx; joint.child_link_index = this_link_idx; joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length); - joint.local_pos_child = vec3(0, 0, 0); + joint.local_pos_child = organAnchorAtParentTip(data.links[parent_idx], data.links[this_link_idx]); joint.stiffness = params.organ_spring_stiffness; joint.damping = params.organ_spring_damping; joint.is_fixed = false; @@ -4059,7 +4091,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo joint.name = "J_" + data.links[parent_idx].name + "_to_" + link.name; joint.parent_link_index = parent_idx; joint.child_link_index = this_link_idx; - joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length); + joint.local_pos_parent = parentAnchorAt(start, data.links[parent_idx]); joint.local_pos_child = vec3(0, 0, -half_len); float avg_radius = (data.links[parent_idx].radius + seg_radius) / 2.f; joint.stiffness = computeJointStiffness(params.elastic_modulus, avg_radius, length); @@ -4101,7 +4133,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo uint organ_objID = fbud.inflorescence_objIDs[inf_idx]; USDLink link; - link.name = "S" + std::to_string(shoot->ID) + "_P" + std::to_string(phytomer_idx) + "_" + organ_label + std::to_string(inf_idx); + link.name = "S" + std::to_string(shoot->ID) + "_P" + std::to_string(phytomer_idx) + "_Ped" + std::to_string(petiole_idx) + "_B" + std::to_string(bud_idx) + "_" + organ_label + std::to_string(inf_idx); link.link_type = USD_LINK_MESH; link.qw = 1; link.qx = 0; link.qy = 0; link.qz = 0; link.radius = 0; @@ -4120,7 +4152,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo joint.parent_link_index = organ_attach; joint.child_link_index = this_link_idx; joint.local_pos_parent = vec3(0, 0, data.links[organ_attach].half_length); - joint.local_pos_child = vec3(0, 0, 0); + joint.local_pos_child = organAnchorAtParentTip(data.links[organ_attach], data.links[this_link_idx]); joint.stiffness = params.organ_spring_stiffness; joint.damping = params.organ_spring_damping; joint.is_fixed = false; diff --git a/plugins/plantarchitecture/tests/selfTest.cpp b/plugins/plantarchitecture/tests/selfTest.cpp index 98fb19ddb..ab10dcbdd 100644 --- a/plugins/plantarchitecture/tests/selfTest.cpp +++ b/plugins/plantarchitecture/tests/selfTest.cpp @@ -5111,6 +5111,161 @@ DOCTEST_TEST_CASE("USD export organs") { std::remove(filename.c_str()); } +DOCTEST_TEST_CASE("USD export joint anchors coincide") { + // Each joint's anchor on body0 (localPos0) and on body1 (localPos1) must map to the same world point. + // Otherwise PhysX pulls the bodies together when the simulation starts and the plant jumps out of its rest pose. + Context context; + PlantArchitecture plantarchitecture(&context); + plantarchitecture.disableMessages(); + plantarchitecture.loadPlantModelFromLibrary("bean"); + + uint plantID = plantarchitecture.buildPlantInstanceFromLibrary(make_vec3(0, 0, 0), 500); + + std::string filename = "test_usd_joint_anchors.usda"; + plantarchitecture.writePlantStructureUSD(plantID, filename); + + std::ifstream file(filename); + DOCTEST_REQUIRE(file.is_open()); + + auto parse_tuple = [](const std::string &line) { + std::vector values; + std::string inside = line.substr(line.find('(') + 1, line.find(')') - line.find('(') - 1); + std::stringstream ss(inside); + std::string item; + while (std::getline(ss, item, ',')) { + values.push_back(std::stof(item)); + } + return values; + }; + auto quoted_name = [](const std::string &line) { + size_t first = line.find('"'); + return line.substr(first + 1, line.find('"', first + 1) - first - 1); + }; + auto link_name = [](const std::string &line) { + size_t start = line.find("/Links/") + 7; + return line.substr(start, line.find('>') - start); + }; + // Rotate v by unit quaternion (w, x, y, z) + auto rotate = [](const std::vector &q, const vec3 &v) { + vec3 u(q[1], q[2], q[3]); + vec3 uv = cross(u, v); + vec3 uuv = cross(u, uv); + return v + 2.f * (q[0] * uv + uuv); + }; + + struct Pose { + vec3 position; + std::vector orient; + }; + struct JointAnchors { + std::string name, body0, body1; + vec3 pos0, pos1; + }; + std::map poses; + std::vector joints; + + std::string line, current_xform; + bool in_joint = false; + JointAnchors joint; + while (std::getline(file, line)) { + if (line.find("def Xform \"") != std::string::npos) { + current_xform = quoted_name(line); + } else if (line.find("float3 xformOp:translate = ") != std::string::npos) { + std::vector t = parse_tuple(line); + poses[current_xform].position = make_vec3(t[0], t[1], t[2]); + } else if (line.find("quatf xformOp:orient = ") != std::string::npos) { + poses[current_xform].orient = parse_tuple(line); + } else if (line.find("def PhysicsSphericalJoint \"") != std::string::npos) { + in_joint = true; + joint = JointAnchors(); + joint.name = quoted_name(line); + } else if (in_joint && line.find("rel physics:body0") != std::string::npos) { + joint.body0 = link_name(line); + } else if (in_joint && line.find("rel physics:body1") != std::string::npos) { + joint.body1 = link_name(line); + } else if (in_joint && line.find("physics:localPos0") != std::string::npos) { + std::vector p = parse_tuple(line); + joint.pos0 = make_vec3(p[0], p[1], p[2]); + } else if (in_joint && line.find("physics:localPos1") != std::string::npos) { + std::vector p = parse_tuple(line); + joint.pos1 = make_vec3(p[0], p[1], p[2]); + joints.push_back(joint); + in_joint = false; + } + } + file.close(); + std::remove(filename.c_str()); + + DOCTEST_REQUIRE(!joints.empty()); + + int leaf_joints = 0; + int fruit_joints = 0; + int misaligned = 0; + for (const auto &j : joints) { + DOCTEST_REQUIRE(poses.count(j.body0) == 1); + DOCTEST_REQUIRE(poses.count(j.body1) == 1); + const Pose &p0 = poses[j.body0]; + const Pose &p1 = poses[j.body1]; + vec3 anchor0 = p0.position + rotate(p0.orient, j.pos0); + vec3 anchor1 = p1.position + rotate(p1.orient, j.pos1); + if ((anchor0 - anchor1).magnitude() > 1e-3f) { + misaligned++; + } + if (j.body1.find("_Leaf") != std::string::npos) { + leaf_joints++; + } else if (j.body1.find("_Fruit") != std::string::npos) { + fruit_joints++; + } + } + + DOCTEST_CHECK(misaligned == 0); + // Make sure the organ joints are actually covered + DOCTEST_CHECK(leaf_joints > 0); + DOCTEST_CHECK(fruit_joints > 0); +} + +DOCTEST_TEST_CASE("USD export link names are unique") { + // A phytomer can carry more than one fruiting bud. Their fruit links must still get distinct names, or the + // .usda file fails to load with a "Duplicate prim" error. This seed grows such a phytomer on the apple model. + Context context; + context.seedRandomGenerator(9); + PlantArchitecture plantarchitecture(&context); + plantarchitecture.disableMessages(); + plantarchitecture.loadPlantModelFromLibrary("apple"); + + uint plantID = plantarchitecture.buildPlantInstanceFromLibrary(make_vec3(0, 0, 0), 560); + + std::string filename = "test_usd_unique_names.usda"; + plantarchitecture.writePlantStructureUSD(plantID, filename); + + std::ifstream file(filename); + DOCTEST_REQUIRE(file.is_open()); + + std::set names; + std::vector duplicates; + int fruit_links = 0; + std::string line; + while (std::getline(file, line)) { + size_t pos = line.find("def Xform \""); + if (pos == std::string::npos) { + continue; + } + size_t start = pos + 11; + std::string name = line.substr(start, line.find('"', start) - start); + if (!names.insert(name).second) { + duplicates.push_back(name); + } + if (name.find("Fruit") != std::string::npos) { + fruit_links++; + } + } + file.close(); + std::remove(filename.c_str()); + + DOCTEST_CHECK(fruit_links > 0); + DOCTEST_CHECK(duplicates.empty()); +} + DOCTEST_TEST_CASE("USD export minimum segment filtering") { Context context; PlantArchitecture plantarchitecture(&context);