bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

rename Point->Triangle, use heuristic sphere vertices generation, add const

This commit is contained in:
Kei Okada 2013-04-16 17:11:18 +09:00 committed by Ioan Sucan
parent d98c6d3532
commit e9ebddba55
1 changed files with 61 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
collada_urdf/src/collada_urdf.cpp
View File

@ -508,11 +508,11 @@ private:
mutable resource_retriever::Retriever retriever_; mutable resource_retriever::Retriever retriever_;
}; };
class Point class Triangle
{ {
public: public:
Point(urdf::Vector3 _p1, urdf::Vector3 _p2, urdf::Vector3 _p3) { this->p1 = _p1; this->p2 = _p2; this->p3 = _p3;}; Triangle(const urdf::Vector3 _p1, const urdf::Vector3 _p2, const urdf::Vector3 _p3) { this->p1 = _p1; this->p2 = _p2; this->p3 = _p3;};
Point() { this->clear(); }; Triangle() { this->clear(); };
urdf::Vector3 p1, p2, p3; urdf::Vector3 p1, p2, p3;
void clear() { p1.clear(); p2.clear(); p3.clear(); }; void clear() { p1.clear(); p2.clear(); p3.clear(); };
@ -1243,89 +1243,47 @@ protected:
case urdf::Geometry::SPHERE: { case urdf::Geometry::SPHERE: {
urdf::Sphere* urdf_sphere = (urdf::Sphere *) geometry.get(); urdf::Sphere* urdf_sphere = (urdf::Sphere *) geometry.get();
double r = urdf_sphere->radius; double r = urdf_sphere->radius;
double phi, phid;
int seg = 64, ring = 64;
phid = M_PI * 2 / seg;
phi = 0;
std::vector<Point> sphere_vertices; double theta, thetad;
thetad = M_PI / (ring + 1);
theta = 0;
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.309, r* 0.500, r* 0.809), urdf::Vector3(r* 0.809, r* 0.309, r* 0.500), urdf::Vector3(r* 0.500, r* 0.809, r* 0.309))); std::vector<Triangle> sphere_vertices;
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r* 0.526, r* 0.000), urdf::Vector3(r* 0.500, r* 0.809, r* 0.309), urdf::Vector3(r* 0.809, r* 0.309, r* 0.500))); std::vector<urdf::Vector3> points;
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r* 0.851), urdf::Vector3(r* 0.809, r* 0.309, r* 0.500), urdf::Vector3(r* 0.309, r* 0.500, r* 0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r* 0.526), urdf::Vector3(r* 0.309, r* 0.500, r* 0.809), urdf::Vector3(r* 0.500, r* 0.809, r* 0.309))); for ( unsigned int i = 0; i < ring; ++ i ) {
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.000, r* 1.000), urdf::Vector3(r*-0.309, r*-0.500, r* 0.809), urdf::Vector3(r* 0.309, r*-0.500, r* 0.809))); double theta_ = theta + thetad * ( i + 1 );
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r* 0.526), urdf::Vector3(r* 0.309, r*-0.500, r* 0.809), urdf::Vector3(r*-0.309, r*-0.500, r* 0.809))); for (unsigned int j = 0; j < seg; ++ j ) {
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r* 0.851), urdf::Vector3(r*-0.309, r*-0.500, r* 0.809), urdf::Vector3(r* 0.000, r* 0.000, r* 1.000))); points.push_back(urdf::Vector3(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r* 0.851), urdf::Vector3(r* 0.000, r* 0.000, r* 1.000), urdf::Vector3(r* 0.309, r*-0.500, r* 0.809))); }
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.500, r* 0.809, r* 0.309), urdf::Vector3(r* 0.000, r* 1.000, r* 0.000), urdf::Vector3(r*-0.500, r* 0.809, r*-0.309))); }
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r*-0.526), urdf::Vector3(r*-0.500, r* 0.809, r*-0.309), urdf::Vector3(r* 0.000, r* 1.000, r* 0.000))); points.push_back(urdf::Vector3(0, 0, r));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r* 0.526), urdf::Vector3(r* 0.000, r* 1.000, r* 0.000), urdf::Vector3(r*-0.500, r* 0.809, r* 0.309))); points.push_back(urdf::Vector3(0, 0, -r));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r* 0.526, r* 0.000), urdf::Vector3(r*-0.500, r* 0.809, r* 0.309), urdf::Vector3(r*-0.500, r* 0.809, r*-0.309))); for(unsigned int i = 0; i < ring - 1; ++i) {
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.309, r* 0.500, r* 0.809), urdf::Vector3(r* 0.000, r* 0.000, r* 1.000), urdf::Vector3(r* 0.309, r* 0.500, r* 0.809))); for(unsigned int j = 0; j < seg; ++j) {
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r* 0.851), urdf::Vector3(r* 0.309, r* 0.500, r* 0.809), urdf::Vector3(r* 0.000, r* 0.000, r* 1.000))); unsigned int a, b, c, d;
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r* 0.851), urdf::Vector3(r* 0.000, r* 0.000, r* 1.000), urdf::Vector3(r*-0.309, r* 0.500, r* 0.809))); a = i * seg + j;
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r* 0.526), urdf::Vector3(r*-0.309, r* 0.500, r* 0.809), urdf::Vector3(r* 0.309, r* 0.500, r* 0.809))); b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.809, r* 0.309, r* 0.500), urdf::Vector3(r*-0.309, r* 0.500, r* 0.809), urdf::Vector3(r*-0.500, r* 0.809, r* 0.309))); c = (i + 1) * seg + j;
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r* 0.526), urdf::Vector3(r*-0.500, r* 0.809, r* 0.309), urdf::Vector3(r*-0.309, r* 0.500, r* 0.809))); d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r* 0.851), urdf::Vector3(r*-0.309, r* 0.500, r* 0.809), urdf::Vector3(r*-0.809, r* 0.309, r* 0.500))); sphere_vertices.push_back(Triangle(points[a], points[c], points[b]));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r* 0.526, r* 0.000), urdf::Vector3(r*-0.809, r* 0.309, r* 0.500), urdf::Vector3(r*-0.500, r* 0.809, r* 0.309))); sphere_vertices.push_back(Triangle(points[b], points[c], points[d]));
sphere_vertices.push_back(Point(urdf::Vector3(r*-1.000, r* 0.000, r* 0.000), urdf::Vector3(r*-0.809, r*-0.309, r* 0.500), urdf::Vector3(r*-0.809, r* 0.309, r* 0.500))); }
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r* 0.851), urdf::Vector3(r*-0.809, r* 0.309, r* 0.500), urdf::Vector3(r*-0.809, r*-0.309, r* 0.500))); }
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r*-0.526, r* 0.000), urdf::Vector3(r*-0.809, r*-0.309, r* 0.500), urdf::Vector3(r*-1.000, r* 0.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r* 0.526, r* 0.000), urdf::Vector3(r*-1.000, r* 0.000, r* 0.000), urdf::Vector3(r*-0.809, r* 0.309, r* 0.500))); for(unsigned int j = 0; j < seg; ++j) {
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.809, r*-0.309, r*-0.500), urdf::Vector3(r*-1.000, r* 0.000, r* 0.000), urdf::Vector3(r*-0.809, r* 0.309, r*-0.500))); unsigned int a, b;
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r* 0.526, r* 0.000), urdf::Vector3(r*-0.809, r* 0.309, r*-0.500), urdf::Vector3(r*-1.000, r* 0.000, r* 0.000))); a = j;
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r*-0.526, r* 0.000), urdf::Vector3(r*-1.000, r* 0.000, r* 0.000), urdf::Vector3(r*-0.809, r*-0.309, r*-0.500))); b = (j == seg - 1) ? 0 : (j + 1);
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r*-0.851), urdf::Vector3(r*-0.809, r*-0.309, r*-0.500), urdf::Vector3(r*-0.809, r* 0.309, r*-0.500))); sphere_vertices.push_back(Triangle(points[ring*seg], points[a], points[b]));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.500, r*-0.809, r*-0.309), urdf::Vector3(r*-0.809, r*-0.309, r*-0.500), urdf::Vector3(r*-0.309, r*-0.500, r*-0.809))); a = (ring - 1) * seg + j;
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r*-0.851), urdf::Vector3(r*-0.309, r*-0.500, r*-0.809), urdf::Vector3(r*-0.809, r*-0.309, r*-0.500))); b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r*-0.526, r* 0.000), urdf::Vector3(r*-0.809, r*-0.309, r*-0.500), urdf::Vector3(r*-0.500, r*-0.809, r*-0.309))); sphere_vertices.push_back(Triangle(points[a], points[ring*seg+1], points[b]));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r*-0.526), urdf::Vector3(r*-0.500, r*-0.809, r*-0.309), urdf::Vector3(r*-0.309, r*-0.500, r*-0.809))); }
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.500, r*-0.809, r* 0.309), urdf::Vector3(r*-0.309, r*-0.500, r* 0.809), urdf::Vector3(r*-0.809, r*-0.309, r* 0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r* 0.851), urdf::Vector3(r*-0.809, r*-0.309, r* 0.500), urdf::Vector3(r*-0.309, r*-0.500, r* 0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r* 0.526), urdf::Vector3(r*-0.309, r*-0.500, r* 0.809), urdf::Vector3(r*-0.500, r*-0.809, r* 0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r*-0.526, r* 0.000), urdf::Vector3(r*-0.500, r*-0.809, r* 0.309), urdf::Vector3(r*-0.809, r*-0.309, r* 0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-1.000, r* 0.000), urdf::Vector3(r*-0.500, r*-0.809, r* 0.309), urdf::Vector3(r*-0.500, r*-0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r*-0.526, r* 0.000), urdf::Vector3(r*-0.500, r*-0.809, r*-0.309), urdf::Vector3(r*-0.500, r*-0.809, r* 0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r* 0.526), urdf::Vector3(r*-0.500, r*-0.809, r* 0.309), urdf::Vector3(r* 0.000, r*-1.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r*-0.526), urdf::Vector3(r* 0.000, r*-1.000, r* 0.000), urdf::Vector3(r*-0.500, r*-0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.500, r*-0.809, r* 0.309), urdf::Vector3(r* 0.809, r*-0.309, r* 0.500), urdf::Vector3(r* 0.309, r*-0.500, r* 0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r* 0.851), urdf::Vector3(r* 0.309, r*-0.500, r* 0.809), urdf::Vector3(r* 0.809, r*-0.309, r* 0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r*-0.526, r* 0.000), urdf::Vector3(r* 0.809, r*-0.309, r* 0.500), urdf::Vector3(r* 0.500, r*-0.809, r* 0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r* 0.526), urdf::Vector3(r* 0.500, r*-0.809, r* 0.309), urdf::Vector3(r* 0.309, r*-0.500, r* 0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.500, r*-0.809, r*-0.309), urdf::Vector3(r* 0.500, r*-0.809, r* 0.309), urdf::Vector3(r* 0.000, r*-1.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r* 0.526), urdf::Vector3(r* 0.000, r*-1.000, r* 0.000), urdf::Vector3(r* 0.500, r*-0.809, r* 0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r*-0.526, r* 0.000), urdf::Vector3(r* 0.500, r*-0.809, r* 0.309), urdf::Vector3(r* 0.500, r*-0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r*-0.526), urdf::Vector3(r* 0.500, r*-0.809, r*-0.309), urdf::Vector3(r* 0.000, r*-1.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.809, r*-0.309, r*-0.500), urdf::Vector3(r* 0.500, r*-0.809, r*-0.309), urdf::Vector3(r* 0.309, r*-0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r*-0.526), urdf::Vector3(r* 0.309, r*-0.500, r*-0.809), urdf::Vector3(r* 0.500, r*-0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r*-0.526, r* 0.000), urdf::Vector3(r* 0.500, r*-0.809, r*-0.309), urdf::Vector3(r* 0.809, r*-0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r*-0.851), urdf::Vector3(r* 0.809, r*-0.309, r*-0.500), urdf::Vector3(r* 0.309, r*-0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 1.000, r* 0.000, r* 0.000), urdf::Vector3(r* 0.809, r* 0.309, r* 0.500), urdf::Vector3(r* 0.809, r*-0.309, r* 0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r* 0.851), urdf::Vector3(r* 0.809, r*-0.309, r* 0.500), urdf::Vector3(r* 0.809, r* 0.309, r* 0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r* 0.526, r* 0.000), urdf::Vector3(r* 0.809, r* 0.309, r* 0.500), urdf::Vector3(r* 1.000, r* 0.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r*-0.526, r* 0.000), urdf::Vector3(r* 1.000, r* 0.000, r* 0.000), urdf::Vector3(r* 0.809, r*-0.309, r* 0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.809, r* 0.309, r*-0.500), urdf::Vector3(r* 1.000, r* 0.000, r* 0.000), urdf::Vector3(r* 0.809, r*-0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r*-0.526, r* 0.000), urdf::Vector3(r* 0.809, r*-0.309, r*-0.500), urdf::Vector3(r* 1.000, r* 0.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r* 0.526, r* 0.000), urdf::Vector3(r* 1.000, r* 0.000, r* 0.000), urdf::Vector3(r* 0.809, r* 0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r*-0.851), urdf::Vector3(r* 0.809, r* 0.309, r*-0.500), urdf::Vector3(r* 0.809, r*-0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.500, r* 0.809, r*-0.309), urdf::Vector3(r* 0.000, r* 1.000, r* 0.000), urdf::Vector3(r* 0.500, r* 0.809, r* 0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r* 0.526), urdf::Vector3(r* 0.500, r* 0.809, r* 0.309), urdf::Vector3(r* 0.000, r* 1.000, r* 0.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r*-0.526), urdf::Vector3(r* 0.000, r* 1.000, r* 0.000), urdf::Vector3(r* 0.500, r* 0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r* 0.526, r* 0.000), urdf::Vector3(r* 0.500, r* 0.809, r*-0.309), urdf::Vector3(r* 0.500, r* 0.809, r* 0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.309, r* 0.500, r*-0.809), urdf::Vector3(r* 0.500, r* 0.809, r*-0.309), urdf::Vector3(r* 0.809, r* 0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.851, r* 0.526, r* 0.000), urdf::Vector3(r* 0.809, r* 0.309, r*-0.500), urdf::Vector3(r* 0.500, r* 0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r*-0.526), urdf::Vector3(r* 0.500, r* 0.809, r*-0.309), urdf::Vector3(r* 0.309, r* 0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r*-0.851), urdf::Vector3(r* 0.309, r* 0.500, r*-0.809), urdf::Vector3(r* 0.809, r* 0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.309, r* 0.500, r*-0.809), urdf::Vector3(r*-0.809, r* 0.309, r*-0.500), urdf::Vector3(r*-0.500, r* 0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.851, r* 0.526, r* 0.000), urdf::Vector3(r*-0.500, r* 0.809, r*-0.309), urdf::Vector3(r*-0.809, r* 0.309, r*-0.500)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r*-0.851), urdf::Vector3(r*-0.809, r* 0.309, r*-0.500), urdf::Vector3(r*-0.309, r* 0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r*-0.526), urdf::Vector3(r*-0.309, r* 0.500, r*-0.809), urdf::Vector3(r*-0.500, r* 0.809, r*-0.309)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.000, r*-1.000), urdf::Vector3(r* 0.309, r*-0.500, r*-0.809), urdf::Vector3(r*-0.309, r*-0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r*-0.851, r*-0.526), urdf::Vector3(r*-0.309, r*-0.500, r*-0.809), urdf::Vector3(r* 0.309, r*-0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r*-0.851), urdf::Vector3(r* 0.309, r*-0.500, r*-0.809), urdf::Vector3(r* 0.000, r* 0.000, r*-1.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r*-0.851), urdf::Vector3(r* 0.000, r* 0.000, r*-1.000), urdf::Vector3(r*-0.309, r*-0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.000, r*-1.000), urdf::Vector3(r*-0.309, r* 0.500, r*-0.809), urdf::Vector3(r* 0.309, r* 0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.000, r* 0.851, r*-0.526), urdf::Vector3(r* 0.309, r* 0.500, r*-0.809), urdf::Vector3(r*-0.309, r* 0.500, r*-0.809)));
sphere_vertices.push_back(Point(urdf::Vector3(r*-0.526, r* 0.000, r*-0.851), urdf::Vector3(r*-0.309, r* 0.500, r*-0.809), urdf::Vector3(r* 0.000, r* 0.000, r*-1.000)));
sphere_vertices.push_back(Point(urdf::Vector3(r* 0.526, r* 0.000, r*-0.851), urdf::Vector3(r* 0.000, r* 0.000, r*-1.000), urdf::Vector3(r* 0.309, r* 0.500, r*-0.809)));
_loadVertices(sphere_vertices, cgeometry); _loadVertices(sphere_vertices, cgeometry);
break; break;
@ -1335,21 +1293,21 @@ protected:
double x = urdf_box->dim.x/2; double x = urdf_box->dim.x/2;
double y = urdf_box->dim.y/2; double y = urdf_box->dim.y/2;
double z = urdf_box->dim.z/2; double z = urdf_box->dim.z/2;
std::vector<Point> box_vertices; std::vector<Triangle> box_vertices;
box_vertices.push_back(Point(urdf::Vector3( x, y, -z), urdf::Vector3(-x, -y, -z), urdf::Vector3(-x, y, -z))); box_vertices.push_back(Triangle(urdf::Vector3( x, y, -z), urdf::Vector3(-x, -y, -z), urdf::Vector3(-x, y, -z)));
box_vertices.push_back(Point(urdf::Vector3(-x, -y, -z), urdf::Vector3( x, y, -z), urdf::Vector3( x, -y, -z))); box_vertices.push_back(Triangle(urdf::Vector3(-x, -y, -z), urdf::Vector3( x, y, -z), urdf::Vector3( x, -y, -z)));
box_vertices.push_back(Point(urdf::Vector3(-x, y, z), urdf::Vector3( x, -y, z), urdf::Vector3( x, y, z))); box_vertices.push_back(Triangle(urdf::Vector3(-x, y, z), urdf::Vector3( x, -y, z), urdf::Vector3( x, y, z)));
box_vertices.push_back(Point(urdf::Vector3( x, -y, z), urdf::Vector3(-x, y, z), urdf::Vector3(-x, -y, z))); box_vertices.push_back(Triangle(urdf::Vector3( x, -y, z), urdf::Vector3(-x, y, z), urdf::Vector3(-x, -y, z)));
box_vertices.push_back(Point(urdf::Vector3( x, y, -z), urdf::Vector3(-x, y, z), urdf::Vector3( x, y, z))); box_vertices.push_back(Triangle(urdf::Vector3( x, y, -z), urdf::Vector3(-x, y, z), urdf::Vector3( x, y, z)));
box_vertices.push_back(Point(urdf::Vector3(-x, y, z), urdf::Vector3( x, y, -z), urdf::Vector3(-x, y, -z))); box_vertices.push_back(Triangle(urdf::Vector3(-x, y, z), urdf::Vector3( x, y, -z), urdf::Vector3(-x, y, -z)));
box_vertices.push_back(Point(urdf::Vector3( x, -y, -z), urdf::Vector3( x, y, z), urdf::Vector3( x, -y, z))); box_vertices.push_back(Triangle(urdf::Vector3( x, -y, -z), urdf::Vector3( x, y, z), urdf::Vector3( x, -y, z)));
box_vertices.push_back(Point(urdf::Vector3( x, y, z), urdf::Vector3( x, -y, -z), urdf::Vector3( x, y, -z))); box_vertices.push_back(Triangle(urdf::Vector3( x, y, z), urdf::Vector3( x, -y, -z), urdf::Vector3( x, y, -z)));
box_vertices.push_back(Point(urdf::Vector3(-x, -y, -z), urdf::Vector3( x, -y, z), urdf::Vector3(-x, -y, z))); box_vertices.push_back(Triangle(urdf::Vector3(-x, -y, -z), urdf::Vector3( x, -y, z), urdf::Vector3(-x, -y, z)));
box_vertices.push_back(Point(urdf::Vector3( x, -y, z), urdf::Vector3(-x, -y, -z), urdf::Vector3( x, -y, -z))); box_vertices.push_back(Triangle(urdf::Vector3( x, -y, z), urdf::Vector3(-x, -y, -z), urdf::Vector3( x, -y, -z)));
box_vertices.push_back(Point(urdf::Vector3(-x, y, -z), urdf::Vector3(-x, -y, z), urdf::Vector3(-x, y, z))); box_vertices.push_back(Triangle(urdf::Vector3(-x, y, -z), urdf::Vector3(-x, -y, z), urdf::Vector3(-x, y, z)));
box_vertices.push_back(Point(urdf::Vector3(-x, -y, z), urdf::Vector3(-x, y, -z), urdf::Vector3(-x, -y, -z))); box_vertices.push_back(Triangle(urdf::Vector3(-x, -y, z), urdf::Vector3(-x, y, -z), urdf::Vector3(-x, -y, -z)));
_loadVertices(box_vertices, cgeometry); _loadVertices(box_vertices, cgeometry);
break; break;
@ -1358,16 +1316,16 @@ protected:
urdf::Cylinder* urdf_cylinder = (urdf::Cylinder*) geometry.get(); urdf::Cylinder* urdf_cylinder = (urdf::Cylinder*) geometry.get();
double l = urdf_cylinder->length; double l = urdf_cylinder->length;
double r = urdf_cylinder->radius; double r = urdf_cylinder->radius;
std::vector<Point> cylinder_vertices; std::vector<Triangle> cylinder_vertices;
for(int i = 0; i < 32; i++ ) { for(int i = 0; i < 32; i++ ) {
double s1 = sin(2*M_PI*i/32); double s1 = sin(2*M_PI*i/32);
double c1 = cos(2*M_PI*i/32); double c1 = cos(2*M_PI*i/32);
double s2 = sin(2*M_PI*(i+1)/32); double s2 = sin(2*M_PI*(i+1)/32);
double c2 = cos(2*M_PI*(i+1)/32); double c2 = cos(2*M_PI*(i+1)/32);
cylinder_vertices.push_back(Point(urdf::Vector3(0, 0,-l/2), urdf::Vector3(r*s2, r*c2,-l/2), urdf::Vector3(r*s1, r*c1,-l/2))); cylinder_vertices.push_back(Triangle(urdf::Vector3(0, 0,-l/2), urdf::Vector3(r*s2, r*c2,-l/2), urdf::Vector3(r*s1, r*c1,-l/2)));
cylinder_vertices.push_back(Point(urdf::Vector3(r*s1, r*c1,-l/2), urdf::Vector3(r*s2, r*c2,-l/2), urdf::Vector3(r*s2, r*c2, l/2))); cylinder_vertices.push_back(Triangle(urdf::Vector3(r*s1, r*c1,-l/2), urdf::Vector3(r*s2, r*c2,-l/2), urdf::Vector3(r*s2, r*c2, l/2)));
cylinder_vertices.push_back(Point(urdf::Vector3(r*s1, r*c1,-l/2), urdf::Vector3(r*s2, r*c2, l/2), urdf::Vector3(r*s1, r*c1, l/2))); cylinder_vertices.push_back(Triangle(urdf::Vector3(r*s1, r*c1,-l/2), urdf::Vector3(r*s2, r*c2, l/2), urdf::Vector3(r*s1, r*c1, l/2)));
cylinder_vertices.push_back(Point(urdf::Vector3(0, 0, l/2), urdf::Vector3(r*s1, r*c1, l/2), urdf::Vector3(r*s2, r*c2, l/2))); cylinder_vertices.push_back(Triangle(urdf::Vector3(0, 0, l/2), urdf::Vector3(r*s1, r*c1, l/2), urdf::Vector3(r*s2, r*c2, l/2)));
} }
_loadVertices(cylinder_vertices, cgeometry); _loadVertices(cylinder_vertices, cgeometry);
@ -1499,7 +1457,7 @@ protected:
return pmout; return pmout;
} }
void _loadVertices(std::vector<Point> vertices, domGeometryRef pdomgeom) { void _loadVertices(const std::vector<Triangle> vertices, domGeometryRef pdomgeom) {
aiScene* scene = new aiScene(); aiScene* scene = new aiScene();
scene->mRootNode = new aiNode(); scene->mRootNode = new aiNode();
scene->mRootNode->mNumMeshes = 1; scene->mRootNode->mNumMeshes = 1;