Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[12041] pkg/trunk/motion_planning/ planning_mode

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 12041
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12041&view=rev
Author:   isucan
Date:     2009-03-03 20:39:23 +0000 (Tue, 03 Mar 2009)

Log Message:
-----------
functionality for defining meshes

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/CMakeLists.txt
    pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h
    pkg/trunk/motion_planning/planning_models/src/kinematic.cpp

Added Paths:
-----------
    pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp

Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================

--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt	2009-03-03 19:58:37 UTC (rev 12040)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt	2009-03-03 20:39:23 UTC (rev 12041)
@@ -5,7 +5,8 @@
 
 set(CMAKE_BUILD_TYPE Release)
 
-rospack_add_library(planning_models src/kinematic.cpp)
+rospack_add_library(planning_models src/kinematic.cpp
+                                    src/load_mesh.cpp)
 
 # Unit tests
 rospack_add_gtest(test_kinematic test/test_kinematic.cpp)

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h	2009-03-03 19:58:37 UTC (rev 12040)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h	2009-03-03 20:39:23 UTC (rev 12041)
@@ -37,6 +37,8 @@
 #ifndef PLANNING_MODELS_SHAPES_
 #define PLANNING_MODELS_SHAPES_
 
+#include <cstdlib>
+
 namespace planning_models
 {
     
@@ -56,7 +58,7 @@
 	    {
 	    }
 	    
-	    enum { UNKNOWN, SPHERE, CYLINDER, BOX } 
+	    enum { UNKNOWN, SPHERE, CYLINDER, BOX, MESH } 
 		type;
 	    
 	};
@@ -122,6 +124,52 @@
 	    double size[3]; 
 	};
 	
+	/** Definition of a mesh */
+	class Mesh : public Shape
+	{
+	public:
+	    Mesh(void) : Shape()
+	    {
+		type = MESH;
+		vertexCount = 0;
+		vertices = NULL;
+		triangleCount = 0;
+		triangles = NULL;
+		normals = NULL;
+	    }
+	    
+	    Mesh(unsigned int vCount, unsigned int tCount) : Shape()
+	    {
+		type = MESH;
+		vertexCount = vCount;
+		vertices = new double[vCount * 3];
+		triangleCount = tCount;
+		triangles = new unsigned int[tCount * 3];
+		normals = new double[tCount * 3];
+	    }
+	    
+	    virtual ~Mesh(void)
+	    {
+		if (vertices)
+		    delete[] vertices;
+		if (triangles)
+		    delete[] triangles;
+		if (normals)
+		    delete[] normals;
+	    }
+	    
+	    unsigned int  vertexCount;       // the number of available vertices
+	    double       *vertices;          // the position for each vertex 
+	                                     // vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
+	    
+	    unsigned int  triangleCount;     // the number of triangles formed with the vertices
+	    unsigned int *triangles;         // the vertex indices for each triangle
+	                                     // triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1, v2, v3)
+	    
+	    double       *normals;           // the normal to each triangle
+	                                     // unit vector represented as (x,y,z) 
+	};
+	
     }
 }
 

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-03-03 19:58:37 UTC (rev 12040)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-03-03 20:39:23 UTC (rev 12041)
@@ -38,16 +38,23 @@
 #include <algorithm>
 #include <cmath>
 
-/** Operator for sorting objects by name */
-template<typename T>
-struct SortByName
+namespace planning_models
 {
-    bool operator()(const T *a, const T *b) const
+    
+    /** Operator for sorting objects by name */
+    template<typename T>
+    struct SortByName
     {
-	return a->name < b->name;
-    }
-};
+	bool operator()(const T *a, const T *b) const
+	{
+	    return a->name < b->name;
+	}
+    };
     
+    // load a mesh
+    shapes::Mesh* load_binary_stl(const char *filename);
+}
+
 const std::string& planning_models::KinematicModel::getModelName(void) const
 {
     return m_name;

Added: pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp	                        (rev 0)
+++ pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp	2009-03-03 20:39:23 UTC (rev 12041)
@@ -0,0 +1,225 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  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 the Willow Garage 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.
+*********************************************************************/
+
+#include <cstdio>
+#include <cmath>
+#include <cassert>
+#include <LinearMath/btVector3.h>
+#include <algorithm>
+#include <vector>
+#include <set>
+#include "planning_models/shapes.h"
+
+// \author Ioan Sucan ;  based on stl_to_mesh 
+
+namespace planning_models
+{
+
+    struct myVertex
+    {
+	btVector3    point;
+	unsigned int index;
+    };
+    
+    struct ltVertexValue
+    {
+	bool operator()(const myVertex &p1, const myVertex &p2) const
+	{
+	    const btVector3 &v1 = p1.point;
+	    const btVector3 &v2 = p2.point;
+	    if (v1.getX() < v2.getX())
+		return true;
+	    if (v1.getX() > v2.getX())
+		return false;
+	    if (v1.getY() < v2.getY())
+		return true;
+	    if (v1.getY() > v2.getY())
+		return false;
+	    if (v1.getZ() < v2.getZ())
+		return true;
+	    return false;
+	}
+    };
+
+    struct ltVertexIndex
+    {
+	bool operator()(const myVertex &p1, const myVertex &p2) const
+	{
+	    return p1.index < p2.index;
+	}
+    };
+    
+    shapes::Mesh* load_binary_stl(const char *filename)
+    {
+
+	FILE* input = fopen(filename, "r");
+	if (!input)
+	    return NULL;
+	
+	fseek(input, 0, SEEK_END);
+	long fileSize = ftell(input);
+	fseek(input, 0, SEEK_SET);
+	
+	char* buffer = new char[fileSize];
+	size_t rd = fread(buffer, fileSize, 1, input);
+	
+	fclose(input);
+	
+	if (rd == 1)
+	{
+	    char* pos = buffer;
+	    pos += 80; // skip the 80 byte header
+	    
+	    unsigned int numTriangles = *(unsigned int*)pos;
+	    pos += 4;
+	    
+	    // make sure we have read enough data
+	    if (50 * numTriangles + 84 <= fileSize)
+	    {
+		
+		std::set<myVertex, ltVertexValue> vertices;
+		std::vector<unsigned int>         triangles;
+		
+		for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
+		{
+		    // skip the normal
+		    pos += 12;
+		    
+		    // read vertices 
+		    btVector3 v1(0,0,0);
+		    btVector3 v2(0,0,0);
+		    btVector3 v3(0,0,0);
+		    
+		    v1.setX(*(float*)pos);
+		    pos += 4;
+		    v1.setY(*(float*)pos);
+		    pos += 4;
+		    v1.setZ(*(float*)pos);
+		    pos += 4;
+		    
+		    v2.setX(*(float*)pos);
+		    pos += 4;
+		    v2.setY(*(float*)pos);
+		    pos += 4;
+		    v2.setZ(*(float*)pos);
+		    pos += 4;
+		    
+		    v3.setX(*(float*)pos);
+		    pos += 4;
+		    v3.setY(*(float*)pos);
+		    pos += 4;
+		    v3.setZ(*(float*)pos);
+		    pos += 4;
+		    
+		    // skip attribute
+		    pos += 2;
+		    
+		    // check if we have new vertices
+		    myVertex vt;
+		    
+		    vt.point = v1;
+		    std::set<myVertex, ltVertexValue>::iterator p1 = vertices.find(vt);
+		    if (p1 == vertices.end())
+		    {
+			vt.index = vertices.size();
+			vertices.insert(vt);		    
+		    }
+		    else
+			vt.index = p1->index;
+		    triangles.push_back(vt.index);		
+		    
+		    vt.point = v2;
+		    std::set<myVertex, ltVertexValue>::iterator p2 = vertices.find(vt);
+		    if (p2 == vertices.end())
+		    {
+			vt.index = vertices.size();
+			vertices.insert(vt);		    
+		    }
+		    else
+			vt.index = p2->index;
+		    triangles.push_back(vt.index);		
+		    
+		    vt.point = v3;
+		    std::set<myVertex, ltVertexValue>::iterator p3 = vertices.find(vt);
+		    if (p3 == vertices.end())
+		    {
+			vt.index = vertices.size();
+			vertices.insert(vt);		    
+		    }
+		    else
+			vt.index = p3->index;
+		    triangles.push_back(vt.index);		
+		}
+		
+		delete[] buffer;
+		
+		// sort our vertices
+		std::vector<myVertex> vt;
+		vt.insert(vt.begin(), vertices.begin(), vertices.end());
+		std::sort(vt.begin(), vt.end(), ltVertexIndex());
+		
+		
+		// copy the data to a mesh structure 
+		unsigned int nt = triangles.size() / 3;
+		
+		shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
+		for (unsigned int i = 0 ; i < vt.size() ; ++i)
+		{
+		    mesh->vertices[3 * i    ] = vt[i].point.getX();
+		    mesh->vertices[3 * i + 1] = vt[i].point.getY();
+		    mesh->vertices[3 * i + 2] = vt[i].point.getZ();
+		}
+		
+		std::copy(triangles.begin(), triangles.end(), mesh->triangles);
+		
+		// compute normals 
+		for (unsigned int i = 0 ; i < nt ; ++i)
+		{
+		    btVector3 s1 = vt[triangles[i * 3    ]].point - vt[triangles[i * 3 + 1]].point;
+		    btVector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
+		    btVector3 normal = s1.cross(s2);
+		    normal.normalize();
+		    mesh->normals[3 * i    ] = normal.getX();
+		    mesh->normals[3 * i + 1] = normal.getY();
+		    mesh->normals[3 * i + 2] = normal.getZ();
+		}
+		
+		return mesh;
+	    }
+	}
+	
+	return NULL;	
+    }
+    
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[12041] pkg/trunk/motion_planning/ planning_mode

personalrobots-commit