58 std::vector<Neighbors>& neighbors) {
59 typedef std::vector<boost::array<FCL_REAL, 6> > VectorArray6;
62 for (std::size_t i = 0; i < boxes.size(); ++i) {
63 const boost::array<FCL_REAL, 6>&
box(boxes[i]);
72 assert(s == fixedSize);
74 for (VectorArray6::const_iterator it = boxes.begin(); it != boxes.end();
76 const boost::array<FCL_REAL, 6>& otherBox = *it;
83 if ((fabs(
x - xo - s) < e) && (fabs(
y - yo) < e) && (fabs(z - zo) < e)) {
85 }
else if ((fabs(
x - xo + s) < e) && (fabs(
y - yo) < e) &&
88 }
else if ((fabs(
x - xo) < e) && (fabs(
y - yo - s) < e) &&
91 }
else if ((fabs(
x - xo) < e) && (fabs(
y - yo + s) < e) &&
94 }
else if ((fabs(
x - xo) < e) && (fabs(
y - yo) < e) &&
95 (fabs(z - zo - s) < e)) {
97 }
else if ((fabs(
x - xo) < e) && (fabs(
y - yo) < e) &&
98 (fabs(z - zo + s) < e)) {
108 std::vector<boost::array<FCL_REAL, 6> > boxes(this->
toBoxes());
109 std::vector<internal::Neighbors> neighbors(boxes.size());
113 typedef std::vector<Vec3f> VectorVec3f;
114 std::vector<Vec3f> vertices;
116 typedef boost::array<std::size_t, 4> Array4;
117 typedef std::vector<Array4> VectorArray4;
118 std::vector<Array4> faces;
120 for (std::size_t i = 0; i < boxes.size(); ++i) {
121 const boost::array<FCL_REAL, 6>&
box(boxes[i]);
129 vertices.push_back(
Vec3f(
x - .5 * size,
y - .5 * size, z - .5 * size));
130 vertices.push_back(
Vec3f(
x + .5 * size,
y - .5 * size, z - .5 * size));
131 vertices.push_back(
Vec3f(
x - .5 * size,
y + .5 * size, z - .5 * size));
132 vertices.push_back(
Vec3f(
x + .5 * size,
y + .5 * size, z - .5 * size));
133 vertices.push_back(
Vec3f(
x - .5 * size,
y - .5 * size, z + .5 * size));
134 vertices.push_back(
Vec3f(
x + .5 * size,
y - .5 * size, z + .5 * size));
135 vertices.push_back(
Vec3f(
x - .5 * size,
y + .5 * size, z + .5 * size));
136 vertices.push_back(
Vec3f(
x + .5 * size,
y + .5 * size, z + .5 * size));
140 Array4
a = {{8 * i + 1, 8 * i + 5, 8 * i + 7, 8 * i + 3}};
144 Array4
a = {{8 * i + 2, 8 * i + 4, 8 * i + 8, 8 * i + 6}};
148 Array4
a = {{8 * i + 1, 8 * i + 2, 8 * i + 6, 8 * i + 5}};
152 Array4
a = {{8 * i + 4, 8 * i + 3, 8 * i + 7, 8 * i + 8}};
156 Array4
a = {{8 * i + 1, 8 * i + 2, 8 * i + 4, 8 * i + 3}};
160 Array4
a = {{8 * i + 5, 8 * i + 6, 8 * i + 8, 8 * i + 7}};
168 throw std::runtime_error(std::string(
"failed to open file \"") +
filename +
171 os <<
"# list of vertices\n";
172 for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end();
175 os <<
"v " <<
v[0] <<
" " <<
v[1] <<
" " <<
v[2] <<
'\n';
177 os <<
"\n# list of faces\n";
178 for (VectorArray4::const_iterator it = faces.begin(); it != faces.end();
180 const Array4& f = *it;
181 os <<
"f " << f[0] <<
" " << f[1] <<
" " << f[2] <<
" " << f[3] <<
'\n';
186 const Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
188 typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> InputType;
189 typedef InputType::ConstRowXpr RowType;
191 shared_ptr<octomap::OcTree>
octree(
new octomap::OcTree(resolution));
192 for (Eigen::DenseIndex row_id = 0; row_id < point_cloud.rows(); ++row_id) {
193 RowType row = point_cloud.row(row_id);
194 octomap::point3d p(
static_cast<float>(row[0]),
static_cast<float>(row[1]),
195 static_cast<float>(row[2]));
196 octree->updateNode(p,
true);
198 octree->updateInnerOccupancy();