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_);
}
}
