geometric_shape_to_BVH_model.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2011-2014, Willow Garage, Inc.
 * Copyright (c) 2014-2015, Open Source Robotics Foundation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * * Neither the name of Open Source Robotics Foundation nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */


#ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H
#define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H

#include "coal/shape/geometric_shapes.h"
#include "coal/BVH/BVH_model.h"
#include <boost/math/constants/constants.hpp>

namespace coal {

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Box& shape,
 const Transform3s& pose) {
 CoalScalar a = shape.halfSide[0];
 CoalScalar b = shape.halfSide[1];
 CoalScalar c = shape.halfSide[2];
 std::vector<Vec3s> points(8);
 std::vector<Triangle> tri_indices(12);
 points[0] = Vec3s(a, -b, c);
 points[1] = Vec3s(a, b, c);
 points[2] = Vec3s(-a, b, c);
 points[3] = Vec3s(-a, -b, c);
 points[4] = Vec3s(a, -b, -c);
 points[5] = Vec3s(a, b, -c);
 points[6] = Vec3s(-a, b, -c);
 points[7] = Vec3s(-a, -b, -c);

 tri_indices[0].set(0, 4, 1);
 tri_indices[1].set(1, 4, 5);
 tri_indices[2].set(2, 6, 3);
 tri_indices[3].set(3, 6, 7);
 tri_indices[4].set(3, 0, 2);
 tri_indices[5].set(2, 0, 1);
 tri_indices[6].set(6, 5, 7);
 tri_indices[7].set(7, 5, 4);
 tri_indices[8].set(1, 5, 2);
 tri_indices[9].set(2, 5, 6);
 tri_indices[10].set(3, 7, 0);
 tri_indices[11].set(0, 7, 4);

 for (unsigned int i = 0; i < points.size(); ++i) {
 points[i] = pose.transform(points[i]).eval();
 }

 model.beginModel();
 model.addSubModel(points, tri_indices);
 model.endModel();
 model.computeLocalAABB();
}

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
 const Transform3s& pose, unsigned int seg,
 unsigned int ring) {
 std::vector<Vec3s> points;
 std::vector<Triangle> tri_indices;

 CoalScalar r = shape.radius;
 CoalScalar phi, phid;
 const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 phid = pi * 2 / seg;
 phi = 0;

 CoalScalar theta, thetad;
 thetad = pi / (ring + 1);
 theta = 0;

 for (unsigned int i = 0; i < ring; ++i) {
 CoalScalar theta_ = theta + thetad * (i + 1);
 for (unsigned int j = 0; j < seg; ++j) {
 points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid),
 r * sin(theta_) * sin(phi + j * phid),
 r * cos(theta_)));
 }
 }
 points.push_back(Vec3s(0, 0, r));
 points.push_back(Vec3s(0, 0, -r));

 for (unsigned int i = 0; i < ring - 1; ++i) {
 for (unsigned int j = 0; j < seg; ++j) {
 unsigned int a, b, c, d;
 a = i * seg + j;
 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
 c = (i + 1) * seg + j;
 d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
 tri_indices.push_back(Triangle(a, c, b));
 tri_indices.push_back(Triangle(b, c, d));
 }
 }

 for (unsigned int j = 0; j < seg; ++j) {
 unsigned int a, b;
 a = j;
 b = (j == seg - 1) ? 0 : (j + 1);
 tri_indices.push_back(Triangle(ring * seg, a, b));

 a = (ring - 1) * seg + j;
 b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
 tri_indices.push_back(Triangle(a, ring * seg + 1, b));
 }

 for (unsigned int i = 0; i < points.size(); ++i) {
 points[i] = pose.transform(points[i]);
 }

 model.beginModel();
 model.addSubModel(points, tri_indices);
 model.endModel();
 model.computeLocalAABB();
}

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
 const Transform3s& pose,
 unsigned int n_faces_for_unit_sphere) {
 CoalScalar r = shape.radius;
 CoalScalar n_low_bound =
 std::sqrt((CoalScalar)n_faces_for_unit_sphere / CoalScalar(2.)) * r * r;
 unsigned int ring = (unsigned int)ceil(n_low_bound);
 unsigned int seg = (unsigned int)ceil(n_low_bound);

 generateBVHModel(model, shape, pose, seg, ring);
}

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
 const Transform3s& pose, unsigned int tot,
 unsigned int h_num) {
 std::vector<Vec3s> points;
 std::vector<Triangle> tri_indices;

 CoalScalar r = shape.radius;
 CoalScalar h = shape.halfLength;
 CoalScalar phi, phid;
 const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 phid = pi * 2 / tot;
 phi = 0;

 CoalScalar hd = 2 * h / h_num;

 for (unsigned int i = 0; i < tot; ++i)
 points.push_back(
 Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h));

 for (unsigned int i = 0; i < h_num - 1; ++i) {
 for (unsigned int j = 0; j < tot; ++j) {
 points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j),
 h - (i + 1) * hd));
 }
 }

 for (unsigned int i = 0; i < tot; ++i)
 points.push_back(
 Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));

 points.push_back(Vec3s(0, 0, h));
 points.push_back(Vec3s(0, 0, -h));

 for (unsigned int i = 0; i < tot; ++i) {
 Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
 tri_indices.push_back(tmp);
 }

 for (unsigned int i = 0; i < tot; ++i) {
 Triangle tmp((h_num + 1) * tot + 1,
 h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
 tri_indices.push_back(tmp);
 }

 for (unsigned int i = 0; i < h_num; ++i) {
 for (unsigned int j = 0; j < tot; ++j) {
 unsigned int a, b, c, d;
 a = j;
 b = (j == tot - 1) ? 0 : (j + 1);
 c = j + tot;
 d = (j == tot - 1) ? tot : (j + 1 + tot);

 unsigned int start = i * tot;
 tri_indices.push_back(Triangle(start + b, start + a, start + c));
 tri_indices.push_back(Triangle(start + b, start + c, start + d));
 }
 }

 for (unsigned int i = 0; i < points.size(); ++i) {
 points[i] = pose.transform(points[i]);
 }

 model.beginModel();
 model.addSubModel(points, tri_indices);
 model.endModel();
 model.computeLocalAABB();
}

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
 const Transform3s& pose,
 unsigned int tot_for_unit_cylinder) {
 CoalScalar r = shape.radius;
 CoalScalar h = 2 * shape.halfLength;

 const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r);
 CoalScalar phid = pi * 2 / tot;

 CoalScalar circle_edge = phid * r;
 unsigned int h_num = (unsigned int)ceil(h / circle_edge);

 generateBVHModel(model, shape, pose, tot, h_num);
}

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
 const Transform3s& pose, unsigned int tot,
 unsigned int h_num) {
 std::vector<Vec3s> points;
 std::vector<Triangle> tri_indices;

 CoalScalar r = shape.radius;
 CoalScalar h = shape.halfLength;

 CoalScalar phi, phid;
 const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 phid = pi * 2 / tot;
 phi = 0;

 CoalScalar hd = 2 * h / h_num;

 for (unsigned int i = 0; i < h_num - 1; ++i) {
 CoalScalar h_i = h - (i + 1) * hd;
 CoalScalar rh = r * (0.5 - h_i / h / 2);
 for (unsigned int j = 0; j < tot; ++j) {
 points.push_back(
 Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
 }
 }

 for (unsigned int i = 0; i < tot; ++i)
 points.push_back(
 Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));

 points.push_back(Vec3s(0, 0, h));
 points.push_back(Vec3s(0, 0, -h));

 for (unsigned int i = 0; i < tot; ++i) {
 Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
 tri_indices.push_back(tmp);
 }

 for (unsigned int i = 0; i < tot; ++i) {
 Triangle tmp(h_num * tot + 1,
 (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)),
 (h_num - 1) * tot + i);
 tri_indices.push_back(tmp);
 }

 for (unsigned int i = 0; i < h_num - 1; ++i) {
 for (unsigned int j = 0; j < tot; ++j) {
 unsigned int a, b, c, d;
 a = j;
 b = (j == tot - 1) ? 0 : (j + 1);
 c = j + tot;
 d = (j == tot - 1) ? tot : (j + 1 + tot);

 unsigned int start = i * tot;
 tri_indices.push_back(Triangle(start + b, start + a, start + c));
 tri_indices.push_back(Triangle(start + b, start + c, start + d));
 }
 }

 for (unsigned int i = 0; i < points.size(); ++i) {
 points[i] = pose.transform(points[i]);
 }

 model.beginModel();
 model.addSubModel(points, tri_indices);
 model.endModel();
 model.computeLocalAABB();
}

template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
 const Transform3s& pose, unsigned int tot_for_unit_cone) {
 CoalScalar r = shape.radius;
 CoalScalar h = 2 * shape.halfLength;

 const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
 unsigned int tot = (unsigned int)(tot_for_unit_cone * r);
 CoalScalar phid = pi * 2 / tot;

 CoalScalar circle_edge = phid * r;
 unsigned int h_num = (unsigned int)ceil(h / circle_edge);

 generateBVHModel(model, shape, pose, tot, h_num);
}

} // namespace coal

#endif