Revision: 8347
http://playerstage.svn.sourceforge.net/playerstage/?rev=8347&view=rev
Author: natepak
Date: 2009-10-30 21:45:34 +0000 (Fri, 30 Oct 2009)
Log Message:
-----------
Improved effeciency of stl reader
Modified Paths:
--------------
code/gazebo/trunk/server/Mesh.cc
code/gazebo/trunk/server/Mesh.hh
code/gazebo/trunk/server/STLLoader.cc
code/gazebo/trunk/worlds/test.world
Modified: code/gazebo/trunk/server/Mesh.cc
===================================================================
--- code/gazebo/trunk/server/Mesh.cc 2009-10-30 20:54:10 UTC (rev 8346)
+++ code/gazebo/trunk/server/Mesh.cc 2009-10-30 21:45:34 UTC (rev 8347)
@@ -466,6 +466,32 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Return true if this submesh has the vertex
+bool SubMesh::HasVertex( const Vector3 &v ) const
+{
+ std::vector< Vector3 >::const_iterator iter;
+
+ for (iter = this->vertices.begin(); iter != this->vertices.end(); iter++)
+ if (v == (*iter))
+ return true;
+
+ return false;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the index of the vertex
+unsigned int SubMesh::GetVertexIndex(const Vector3 &v) const
+{
+ std::vector< Vector3 >::const_iterator iter;
+
+ for (iter = this->vertices.begin(); iter != this->vertices.end(); iter++)
+ if (v == (*iter))
+ return iter - this->vertices.begin();
+
+ return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Put all the data into flat arrays
void SubMesh::FillArrays(float **vertArr, unsigned int **indArr) const
{
Modified: code/gazebo/trunk/server/Mesh.hh
===================================================================
--- code/gazebo/trunk/server/Mesh.hh 2009-10-30 20:54:10 UTC (rev 8346)
+++ code/gazebo/trunk/server/Mesh.hh 2009-10-30 21:45:34 UTC (rev 8347)
@@ -143,6 +143,12 @@
/// \brief Get the material index
public: unsigned int GetMaterialIndex() const;
+ /// \brief Return true if this submesh has the vertex
+ public: bool HasVertex( const Vector3 &v ) const;
+
+ /// \brief Get the index of the vertex
+ public: unsigned int GetVertexIndex(const Vector3 &v) const;
+
/// \brief Put all the data into flat arrays
public: void FillArrays(float **vertArr, unsigned int **indArr) const;
Modified: code/gazebo/trunk/server/STLLoader.cc
===================================================================
--- code/gazebo/trunk/server/STLLoader.cc 2009-10-30 20:54:10 UTC (rev
8346)
+++ code/gazebo/trunk/server/STLLoader.cc 2009-10-30 21:45:34 UTC (rev
8347)
@@ -32,11 +32,18 @@
FILE *file = fopen(filename.c_str(), "r");
- if (extension == "stl" || extension == "stla")
+ std::cout << "SLT Load\n";
+
+ /*if (extension == "stl" || extension == "stla")
this->ReadAscii(file, mesh);
else if (extension == "stlb")
this->ReadBinary(file, mesh);
+ */
+ this->ReadBinary(file, mesh);
+
+ fclose(file);
+
return mesh;
}
@@ -45,34 +52,22 @@
void STLLoader::ReadAscii( FILE *filein, Mesh *mesh )
{
int count;
- int i;
- int icor3;
int ivert;
char *next;
float r1;
float r2;
float r3;
float r4;
- float temp[3];
char token[LINE_MAX_LEN];
int width;
- float face_normal[3][FACE_MAX];
- int face[ORDER_MAX][FACE_MAX];
- int face_order[FACE_MAX];
- int vertex_material[ORDER_MAX][FACE_MAX];
- float vertex_normal[3][ORDER_MAX][FACE_MAX];
- int text_num;
- int face_num;
- int cor3_num;
- int object_num;
- int dup_num;
char input[LINE_MAX_LEN];
- float cor3[3][COR3_MAX];
+ SubMesh *subMesh = new SubMesh();
+ mesh->AddSubMesh(subMesh);
+
// Read the next line of the file into INPUT.
while ( fgets ( input, LINE_MAX_LEN, filein ) != NULL )
{
- text_num = text_num + 1;
// Advance to the first nonspace character in INPUT.
for ( next = input; *next != '\0' && isspace(*next); next++ );
@@ -90,80 +85,54 @@
//FACET
if (this->Leqi(token, (char*)"facet"))
{
+ Vector3 normal;
+
//Get the XYZ coordinates of the normal vector to the face.
sscanf ( next, "%*s %e %e %e", &r1, &r2, &r3 );
- if ( face_num < FACE_MAX )
- {
- face_normal[0][face_num] = r1;
- face_normal[1][face_num] = r2;
- face_normal[2][face_num] = r3;
- }
+ normal.x = r1;
+ normal.y = r2;
+ normal.z = r3;
fgets ( input, LINE_MAX_LEN, filein );
- text_num = text_num + 1;
ivert = 0;
for ( ;; )
{
+ Vector3 vertex;
fgets ( input, LINE_MAX_LEN, filein );
- text_num = text_num + 1;
count = sscanf ( input, "%*s %e %e %e", &r1, &r2, &r3 );
if ( count != 3 )
break;
- temp[0] = r1;
- temp[1] = r2;
- temp[2] = r3;
+ vertex.x = r1;
+ vertex.y = r2;
+ vertex.z = r3;
- if ( cor3_num < 1000 )
- icor3 = this->RcolFind(cor3, 3, cor3_num, temp);
- else
- icor3 = -1;
-
- if ( icor3 == -1 )
+ if (!subMesh->HasVertex(vertex))
{
- icor3 = cor3_num;
-
- if ( cor3_num < COR3_MAX )
- {
- for ( i = 0; i < 3; i++ )
- cor3[i][cor3_num] = temp[i];
- }
- cor3_num = cor3_num + 1;
+ subMesh->AddVertex(vertex);
+ subMesh->AddNormal(normal);
}
- else
- dup_num = dup_num + 1;
+ subMesh->AddIndex( subMesh->GetVertexIndex(vertex) );
- if ( ivert < ORDER_MAX && face_num < FACE_MAX )
- {
- face[ivert][face_num] = icor3;
- vertex_material[ivert][face_num] = 0;
- for ( i = 0; i < 3; i++ )
- vertex_normal[i][ivert][face_num] = face_normal[i][face_num];
- }
-
ivert = ivert + 1;
}
fgets ( input, LINE_MAX_LEN, filein );
- text_num = text_num + 1;
-
- if ( face_num < FACE_MAX )
- face_order[face_num] = ivert;
-
- face_num = face_num + 1;
-
}
// COLOR
else if ( this->Leqi ( token, (char*)"color" ))
+ {
sscanf ( next, "%*s %f %f %f %f", &r1, &r2, &r3, &r4 );
+ }
// SOLID
else if ( this->Leqi ( token, (char*)"solid" ))
- object_num = object_num + 1;
+ {
+ }
// ENDSOLID
else if ( this->Leqi ( token, (char*)"endsolid" ))
{
@@ -172,20 +141,11 @@
else
{
printf ( "\n" );
- printf ( "STLA_READ - Fatal error!\n" );
+ printf ( "stl - Fatal error!\n" );
printf ( " Unrecognized first word on line.\n" );
}
}
- SubMesh *subMesh = new SubMesh();
- mesh->AddSubMesh(subMesh);
-
- for (int i=0; i < face_num; i++)
- subMesh->AddVertex( cor3[0][i], cor3[1][i], cor3[2][i] );
-
- for (int i=0; i < face_num; i++)
- for (int j=0; j < face_order[i]; j++)
- subMesh->AddIndex(face[j][i]);
}
////////////////////////////////////////////////////////////////////////////////
@@ -194,31 +154,19 @@
{
short int attribute = 0;
char c;
- float cvec[3];
- int icor3;
int i;
int iface;
- int ivert;
int face_num;
- int face_order[FACE_MAX];
- int face_material[FACE_MAX];
- int bytes_num;
- int cor3_num;
- int dup_num;
- float cor3[3][COR3_MAX];
- float face_normal[3][FACE_MAX];
- int face[3][FACE_MAX];
+ SubMesh *subMesh = new SubMesh();
+ mesh->AddSubMesh(subMesh);
+
//80 byte Header.
for ( i = 0; i < 80; i++ )
- {
c = (char)fgetc(filein);
- bytes_num = bytes_num + 1;
- }
//Number of faces.
face_num = this->LongIntRead(filein);
- bytes_num = bytes_num + 4;
//For each (triangular) face,
// components of normal vector,
@@ -226,59 +174,32 @@
// 2 byte "attribute".
for (iface = 0; iface < face_num; iface++)
{
- face_order[iface] = 3;
- face_material[iface] = 0;
+ Vector3 normal;
- for ( i = 0; i < 3; i++ )
- {
- face_normal[i][iface] = this->FloatRead(filein);
- bytes_num = bytes_num + 4;
- }
+ // Get the normal for the face
+ normal.x = this->FloatRead(filein);
+ normal.y = this->FloatRead(filein);
+ normal.z = this->FloatRead(filein);
- for (ivert = 0; ivert < face_order[iface]; ivert++)
+ // Get three vertices
+ for (i = 0; i < 3; i++)
{
- for ( i = 0; i < 3; i++ )
- {
- cvec[i] = this->FloatRead(filein);
- bytes_num = bytes_num + 4;
- }
+ Vector3 vertex;
- if ( cor3_num < 1000 )
- icor3 = this->RcolFind(cor3, 3, cor3_num, cvec);
- else
- icor3 = -1;
+ vertex.x = this->FloatRead(filein);
+ vertex.y = this->FloatRead(filein);
+ vertex.z = this->FloatRead(filein);
- if (icor3 == -1)
+ if (!subMesh->HasVertex(vertex))
{
- icor3 = cor3_num;
- if (cor3_num < COR3_MAX)
- {
- cor3[0][cor3_num] = cvec[0];
- cor3[1][cor3_num] = cvec[1];
- cor3[2][cor3_num] = cvec[2];
- }
- cor3_num = cor3_num + 1;
+ subMesh->AddVertex(vertex);
+ subMesh->AddNormal(normal);
}
- else
- dup_num = dup_num + 1;
-
- face[ivert][iface] = icor3;
+ subMesh->AddIndex( subMesh->GetVertexIndex(vertex) );
}
attribute = ShortIntRead ( filein );
-
- bytes_num = bytes_num + 2;
}
-
- SubMesh *subMesh = new SubMesh();
- mesh->AddSubMesh(subMesh);
-
- for (int i=0; i < face_num; i++)
- subMesh->AddVertex( cor3[0][i], cor3[1][i], cor3[2][i] );
-
- for (int i=0; i < face_num; i++)
- for (int j=0; j < face_order[i]; j++)
- subMesh->AddIndex(face[j][i]);
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/worlds/test.world
===================================================================
--- code/gazebo/trunk/worlds/test.world 2009-10-30 20:54:10 UTC (rev 8346)
+++ code/gazebo/trunk/worlds/test.world 2009-10-30 21:45:34 UTC (rev 8347)
@@ -30,26 +30,12 @@
<size>800 600</size>
<pos>100 100</pos>
<frames>
- <row height="50%">
- <camera width="50%">
+ <row height="100%">
+ <camera width="100%">
<xyz>-1 0 4</xyz>
<rpy>0 34 0</rpy>
</camera>
- <camera width="50%">
- <xyz>0 0 10</xyz>
- <rpy>0 90 0</rpy>
- </camera>
</row>
- <row height="50%">
- <camera width="50%">
- <xyz>0 0 0</xyz>
- <rpy>0 0 0</rpy>
- </camera>
- <camera width="50%">
- <xyz>10 0 1</xyz>
- <rpy>0 0 90</rpy>
- </camera>
- </row>
</frames>
</rendering:gui>
@@ -93,53 +79,13 @@
<visual>
<scale>0.1 0.1 0.1</scale>
- <mesh>unit_sphere</mesh>
+ <mesh>test.stlb</mesh>
<material>Gazebo/Rocky</material>
</visual>
</geom:sphere>
</body:sphere>
</model:physical>
- <!--
- Include the complete model described in the .model file
- This assumes the root node is a <model:...>
- -->
- <!-- <include embedded="false">
- <xi:include href="pioneer2dx.model" />
- </include>
- -->
-
- <model:physical name="pioneer2dx_model1">
- <xyz>0 0 0.145</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <controller:differential_position2d name="controller1">
- <leftJoint>left_wheel_hinge</leftJoint>
- <rightJoint>right_wheel_hinge</rightJoint>
- <wheelSeparation>0.39</wheelSeparation>
- <wheelDiameter>0.15</wheelDiameter>
- <torque>5</torque>
- <interface:position name="position_iface_0"/>
- </controller:differential_position2d>
-
- <model:physical name="laser">
- <xyz>0.15 0 0.18</xyz>
-
- <attach>
- <parentBody>chassis_body</parentBody>
- <myBody>laser_body</myBody>
- </attach>
-
- <include embedded="true">
- <xi:include href="models/sicklms200.model" />
- </include>
- </model:physical>
-
- <include embedded="true">
- <xi:include href="models/pioneer2dx.model" />
- </include>
- </model:physical>
-
<!-- White Directional light -->
<model:renderable name="directional_white">
<enableGravity>false</enableGravity>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry(R) Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9 - 12, 2009. Register now!
http://p.sf.net/sfu/devconference
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
