void BVH::build() { Pane::info("BVH # Started building from mesh"); std::vector<BVHPrimitive> data; data.reserve(mesh->faces.size()); // TODO: reserve for primitives // Create data structures for BVH for(unsigned i = 0; i < mesh->faces.size(); i++) data.emplace_back(i, computeAABB(mesh, i)); Pane::info("BVH # Finished creating bounding volumes"); unsigned totalNodes = 0; // Build recursively BVHNode *root = buildRecursive(data, 0, mesh->faces.size(), primitives, totalNodes); Pane::info("BVH # Finished volume hierarchy build"); // Allocate linear nodes nodes.resize(totalNodes); unsigned offset = 0; flatten(root, offset); Pane::info("BVH # Finished flattening volume hierarchy"); }
planning_models::KinematicModel::JointModel* planning_models::KinematicModel::buildRecursive(LinkModel *parent, const urdf::Link *link, const std::vector<MultiDofConfig>& multi_dof_configs) { JointModel *joint = constructJointModel(link->parent_joint.get(), link, multi_dof_configs); if(joint == NULL) { return NULL; } joint_model_map_[joint->name_] = joint; for(JointModel::js_type::const_iterator it = joint->getJointStateEquivalents().begin(); it != joint->getJointStateEquivalents().end(); it++) { joint_model_map_[it->right] = joint; } joint_model_vector_.push_back(joint); joint->parent_link_model_ = parent; joint->child_link_model_ = constructLinkModel(link); if (parent == NULL) joint->child_link_model_->joint_origin_transform_.setIdentity(); link_model_map_[joint->child_link_model_->name_] = joint->child_link_model_; link_model_vector_.push_back(joint->child_link_model_); if(joint->child_link_model_->shape_ != NULL) { link_models_with_collision_geometry_vector_.push_back(joint->child_link_model_); } joint->child_link_model_->parent_joint_model_ = joint; for (unsigned int i = 0 ; i < link->child_links.size() ; ++i) { JointModel* jm = buildRecursive(joint->child_link_model_, link->child_links[i].get(), multi_dof_configs); if(jm != NULL) { joint->child_link_model_->child_joint_models_.push_back(jm); } } return joint; }
void KdTree::build(std::vector<Point>& points) { pointNodes.clear(); pointNodes.reserve(points.size()); root = buildRecursive(points, 0, points.size() - 1, 0); //printList(pointNodes); }
PointNode* KdTree::buildRecursive(std::vector<Point>& list, int left, int right, int axis) { pointNodes.push_back(PointNode{}); PointNode& pn = pointNodes.back(); if(left == right) { pn.p = &list[left]; pn.left = nullptr; pn.right = nullptr; } else { if ( axis == 0) { pn.p = &median(list, left, right, ByX); } else { pn.p = &median(list, left, right, ByY); } //printList(list); //2 elements left : first is median, second is the right leaf if(right - left == 1) { pn.left = nullptr; } else { pn.left = buildRecursive(list, left, left + (right - left) / 2 - 1, (axis + 1) % 2); } pn.right = buildRecursive(list, left + (right - left) / 2 + 1, right, (axis + 1) % 2); } return &pn; }
void KdTree::build() { agents_.reserve(simulator_->agents_.size()); for (std::size_t i = agents_.size(); i < simulator_->agents_.size(); ++i) { agents_.push_back(i); } nodes_.resize(2 * simulator_->agents_.size() - 1); if (!agents_.empty()) { buildRecursive(0, agents_.size(), 0); } }
/* ------------------------ KinematicModel ------------------------ */ planning_models::KinematicModel::KinematicModel(const urdf::Model &model, const std::vector<GroupConfig>& group_configs, const std::vector<MultiDofConfig>& multi_dof_configs) { model_name_ = model.getName(); if (model.getRoot()) { const urdf::Link *root = model.getRoot().get(); root_ = buildRecursive(NULL, root, multi_dof_configs); buildGroups(group_configs); } else { root_ = NULL; ROS_WARN("No root link found"); } }
BVHNode* BVH::buildRecursive(std::vector<BVHPrimitive> & data, unsigned start, unsigned end, std::vector<Face*> & orderedPrimitives, unsigned & totalNodes) { // Primitives in this branch const unsigned nPrimitives = end - start; // Allocate node (TODO: get from pool) BVHNode *node = new BVHNode; totalNodes++; // Calculate bounding box for all primitives given glm::aabb bounds; for(unsigned i = start; i < end; ++i) bounds = glm::aabb::combine(bounds, data[i].bounds); if(nPrimitives <= m_primitivesPerNode) // only n primitive left, create leaf node { const unsigned firstPrimitive = (unsigned)orderedPrimitives.size(); for(unsigned i = start; i < end; ++i) { const unsigned primitive = data[i].primitive; orderedPrimitives.push_back(&mesh->faces[primitive]); } node->createLeaf(firstPrimitive, nPrimitives, bounds); } else { // Choose split dimension based on aabb centroids glm::aabb centroidBounds; for(unsigned i = start; i < end; ++i) centroidBounds.expand(data[i].centroid); if(centroidBounds.empty()) { // All centroids the same, just create a leaf node const unsigned firstPrimitive = (unsigned)orderedPrimitives.size(); for(unsigned i = start; i < end; ++i) { const unsigned primitive = data[i].primitive; orderedPrimitives.push_back(&mesh->faces[primitive]); } node->createLeaf(firstPrimitive, nPrimitives, bounds); return node; } const int dim = centroidBounds.largest_axis(); // Partition primitives to two subtrees const unsigned mid = (start + end) / 2; std::nth_element(&data[start], &data[mid], &data[end-1]+1, [=] (const BVHPrimitive & a, const BVHPrimitive & b) { return a.centroid[dim] < b.centroid[dim]; }); node->createInterior(dim, buildRecursive(data, start, mid, orderedPrimitives, totalNodes), buildRecursive(data, mid, end, orderedPrimitives, totalNodes)); } return node; }
void KdTree::buildRecursive(std::size_t begin, std::size_t end, std::size_t node) { nodes_[node].begin_ = begin; nodes_[node].end_ = end; nodes_[node].minX_ = nodes_[node].maxX_ = simulator_->agents_[agents_[begin]]->position_.getX(); nodes_[node].minY_ = nodes_[node].maxY_ = simulator_->agents_[agents_[begin]]->position_.getY(); for (std::size_t i = begin + 1; i < end; ++i) { if (simulator_->agents_[agents_[i]]->position_.getX() > nodes_[node].maxX_) { nodes_[node].maxX_ = simulator_->agents_[agents_[i]]->position_.getX(); } else if (simulator_->agents_[agents_[i]]->position_.getX() < nodes_[node].minX_) { nodes_[node].minX_ = simulator_->agents_[agents_[i]]->position_.getX(); } if (simulator_->agents_[agents_[i]]->position_.getY() > nodes_[node].maxY_) { nodes_[node].maxY_ = simulator_->agents_[agents_[i]]->position_.getY(); } else if (simulator_->agents_[agents_[i]]->position_.getY() < nodes_[node].minY_) { nodes_[node].minY_ = simulator_->agents_[agents_[i]]->position_.getY(); } } if (end - begin > HRVO_MAX_LEAF_SIZE) { const bool vertical = nodes_[node].maxX_ - nodes_[node].minX_ > nodes_[node].maxY_ - nodes_[node].minY_; const float split = 0.5f * (vertical ? nodes_[node].maxX_ + nodes_[node].minX_ : nodes_[node].maxY_ + nodes_[node].minY_); std::size_t left = begin; std::size_t right = end - 1; while (true) { while (left <= right && (vertical ? simulator_->agents_[agents_[left]]->position_.getX() : simulator_->agents_[agents_[left]]->position_.getY()) < split) { ++left; } while (right >= left && (vertical ? simulator_->agents_[agents_[right]]->position_.getX() : simulator_->agents_[agents_[right]]->position_.getY()) >= split) { --right; } if (left > right) { break; } else { std::swap(agents_[left], agents_[right]); ++left; --right; } } if (left == begin) { ++left; ++right; } nodes_[node].left_ = node + 1; nodes_[node].right_ = 2 * (left - begin) + node; buildRecursive(begin, left, nodes_[node].left_); buildRecursive(left, end, nodes_[node].right_); } }