275 lines
9.3 KiB
C++
275 lines
9.3 KiB
C++
/*
|
|
* Copyright (C) 2015 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "test_config.h"
|
|
#include "gazebo/common/Mesh.hh"
|
|
#include "gazebo/common/MeshManager.hh"
|
|
#include "gazebo/gazebo_config.h"
|
|
#include "test/util.hh"
|
|
|
|
using namespace gazebo;
|
|
|
|
class MeshManager : public gazebo::testing::AutoLogFixture { };
|
|
|
|
#ifdef HAVE_GTS
|
|
/////////////////////////////////////////////////
|
|
TEST_F(MeshManager, CreateExtrudedPolyline)
|
|
{
|
|
// test extrusion of a path with two subpaths:
|
|
// a smaller square inside a bigger square.
|
|
// The smaller square should be treated as a hole inside the bigger square.
|
|
std::vector<std::vector<ignition::math::Vector2d> > path;
|
|
std::vector<ignition::math::Vector2d> subpath01;
|
|
subpath01.push_back(ignition::math::Vector2d(0, 0));
|
|
subpath01.push_back(ignition::math::Vector2d(1, 0));
|
|
subpath01.push_back(ignition::math::Vector2d(1, 1));
|
|
subpath01.push_back(ignition::math::Vector2d(0, 1));
|
|
subpath01.push_back(ignition::math::Vector2d(0, 0));
|
|
|
|
std::vector<ignition::math::Vector2d> subpath02;
|
|
subpath02.push_back(ignition::math::Vector2d(0.25, 0.25));
|
|
subpath02.push_back(ignition::math::Vector2d(0.25, 0.75));
|
|
subpath02.push_back(ignition::math::Vector2d(0.75, 0.75));
|
|
subpath02.push_back(ignition::math::Vector2d(0.75, 0.25));
|
|
subpath02.push_back(ignition::math::Vector2d(0.25, 0.25));
|
|
|
|
path.push_back(subpath01);
|
|
path.push_back(subpath02);
|
|
|
|
std::string meshName = "extruded_path";
|
|
double height = 10.0;
|
|
common::MeshManager::Instance()->CreateExtrudedPolyline(
|
|
meshName, path, height);
|
|
|
|
// check mesh
|
|
EXPECT_TRUE(common::MeshManager::Instance()->HasMesh(meshName));
|
|
const common::Mesh *mesh = common::MeshManager::Instance()->GetMesh(meshName);
|
|
EXPECT_TRUE(mesh != NULL);
|
|
|
|
unsigned int submeshCount = mesh->GetSubMeshCount();
|
|
EXPECT_EQ(submeshCount, 1u);
|
|
|
|
// check submesh bounds
|
|
const common::SubMesh *submesh = mesh->GetSubMesh(0);
|
|
EXPECT_TRUE(submesh != NULL);
|
|
EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), submesh->Min());
|
|
EXPECT_EQ(ignition::math::Vector3d(1.0, 1.0, 10.0), submesh->Max());
|
|
|
|
// check vertices
|
|
for (unsigned int i = 0; i < submesh->GetVertexCount(); ++i)
|
|
{
|
|
ignition::math::Vector3d v = submesh->Vertex(i);
|
|
|
|
// check no vertices are in the region of the hole
|
|
EXPECT_FALSE((v.X() > 0.25 && v.X() < 0.75));
|
|
EXPECT_FALSE((v.Y() > 0.25 && v.Y() < 0.75));
|
|
|
|
// check extruded height
|
|
EXPECT_TRUE((ignition::math::equal(v.Z(), 0.0) ||
|
|
ignition::math::equal(v.Z(), 10.0)));
|
|
}
|
|
|
|
// verify same number of normals and vertices
|
|
EXPECT_EQ(submesh->GetVertexCount(), submesh->GetNormalCount());
|
|
|
|
// check normals
|
|
for (unsigned int i = 0; i < submesh->GetNormalCount(); ++i)
|
|
{
|
|
ignition::math::Vector3d v = submesh->Vertex(i);
|
|
ignition::math::Vector3d n = submesh->Normal(i);
|
|
|
|
// vertex at 0 could be a bottom face or side face
|
|
if (ignition::math::equal(v.Z(), 0.0))
|
|
{
|
|
if (ignition::math::equal(n.Z(), 0.0))
|
|
{
|
|
// side face - check non-zero normal
|
|
EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
|
|
ignition::math::equal(n.Y(), 0.0)));
|
|
}
|
|
else
|
|
{
|
|
// bottom face - normal in -z direction
|
|
EXPECT_TRUE((n == -ignition::math::Vector3d::UnitZ) ||
|
|
(ignition::math::equal(n.Z(), 0.0)));
|
|
}
|
|
}
|
|
|
|
// vertex at height could be a top face or side face
|
|
if (ignition::math::equal(v.Z(), 10.0))
|
|
{
|
|
if (ignition::math::equal(n.Z(), 0.0))
|
|
{
|
|
// side face - check non-zero normal
|
|
EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
|
|
ignition::math::equal(n.Y(), 0.0)));
|
|
}
|
|
else
|
|
{
|
|
// top face - normal in +z direction
|
|
EXPECT_TRUE((n == ignition::math::Vector3d::UnitZ) ||
|
|
(ignition::math::equal(n.Z(), 0.0)));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
TEST_F(MeshManager, CreateExtrudedPolylineClosedPath)
|
|
{
|
|
// test extrusion of a path that has two closed subpaths, i.e.,
|
|
// first and last vertices are the same.
|
|
// The following two subpaths form the letter 'A'.
|
|
std::vector<std::vector<ignition::math::Vector2d> > path2;
|
|
std::vector<ignition::math::Vector2d> subpath03;
|
|
subpath03.push_back(ignition::math::Vector2d(2.27467, 1.0967));
|
|
subpath03.push_back(ignition::math::Vector2d(1.81094, 2.35418));
|
|
subpath03.push_back(ignition::math::Vector2d(2.74009, 2.35418));
|
|
|
|
std::vector<ignition::math::Vector2d> subpath04;
|
|
subpath04.push_back(ignition::math::Vector2d(2.08173, 0.7599));
|
|
subpath04.push_back(ignition::math::Vector2d(2.4693, 0.7599));
|
|
subpath04.push_back(ignition::math::Vector2d(3.4323, 3.28672));
|
|
subpath04.push_back(ignition::math::Vector2d(3.07689, 3.28672));
|
|
subpath04.push_back(ignition::math::Vector2d(2.84672, 2.63851));
|
|
subpath04.push_back(ignition::math::Vector2d(1.7077, 2.63851));
|
|
subpath04.push_back(ignition::math::Vector2d(1.47753, 3.28672));
|
|
subpath04.push_back(ignition::math::Vector2d(1.11704, 3.28672));
|
|
|
|
path2.push_back(subpath03);
|
|
path2.push_back(subpath04);
|
|
|
|
std::string meshName = "extruded_path_closed";
|
|
double height = 2.0;
|
|
common::MeshManager::Instance()->CreateExtrudedPolyline(
|
|
meshName, path2, height);
|
|
|
|
// check mesh
|
|
EXPECT_TRUE(common::MeshManager::Instance()->HasMesh(meshName));
|
|
const common::Mesh *mesh = common::MeshManager::Instance()->GetMesh(meshName);
|
|
EXPECT_TRUE(mesh != NULL);
|
|
|
|
unsigned int submeshCount = mesh->GetSubMeshCount();
|
|
EXPECT_EQ(submeshCount, 1u);
|
|
|
|
// check submesh bounds
|
|
const common::SubMesh *submesh = mesh->GetSubMesh(0);
|
|
EXPECT_TRUE(submesh != NULL);
|
|
EXPECT_EQ(submesh->Min(), ignition::math::Vector3d(1.11704, 0.7599, 0));
|
|
EXPECT_EQ(submesh->Max(), ignition::math::Vector3d(3.4323, 3.28672, 2.0));
|
|
|
|
for (unsigned int i = 0; i < submesh->GetVertexCount(); ++i)
|
|
{
|
|
ignition::math::Vector3d v = submesh->Vertex(i);
|
|
|
|
// check no vertices are in the region of the hole using a point-in-polygon
|
|
// algorithm
|
|
bool pointInPolygon = false;
|
|
for (unsigned int j = 0, k = subpath03.size()-1; j < subpath03.size();
|
|
k = ++j)
|
|
{
|
|
if (((subpath03[j].Y() > v.Y()) != (subpath03[k].Y() > v.Y())) &&
|
|
(v.X() < (subpath03[k].X()-subpath03[j].X()) *
|
|
(v.Y()-subpath03[j].Y()) /
|
|
(subpath03[k].Y()-subpath03[j].Y()) + subpath03[j].X()) )
|
|
{
|
|
pointInPolygon = !pointInPolygon;
|
|
}
|
|
}
|
|
EXPECT_FALSE(pointInPolygon);
|
|
|
|
// check extruded height
|
|
EXPECT_TRUE((ignition::math::equal(v.Z(), 0.0) ||
|
|
ignition::math::equal(v.Z(), 2.0)));
|
|
}
|
|
|
|
// verify same number of normals and vertices
|
|
EXPECT_EQ(submesh->GetVertexCount(), submesh->GetNormalCount());
|
|
|
|
// check normals
|
|
for (unsigned int i = 0; i < submesh->GetNormalCount(); ++i)
|
|
{
|
|
ignition::math::Vector3d v = submesh->Vertex(i);
|
|
ignition::math::Vector3d n = submesh->Normal(i);
|
|
|
|
// vertex at 0 could be a bottom face or side face
|
|
if (ignition::math::equal(v.Z(), 0.0))
|
|
{
|
|
if (ignition::math::equal(n.Z(), 0.0))
|
|
{
|
|
// side face - check non-zero normal
|
|
EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
|
|
ignition::math::equal(n.Y(), 0.0)));
|
|
}
|
|
else
|
|
{
|
|
// bottom face - normal in -z direction
|
|
EXPECT_TRUE((n == -ignition::math::Vector3d::UnitZ) ||
|
|
(ignition::math::equal(n.Z(), 0.0)));
|
|
}
|
|
}
|
|
|
|
// vertex at height could be a top face or side face
|
|
if (ignition::math::equal(v.Z(), 10.0))
|
|
{
|
|
if (ignition::math::equal(n.Z(), 0.0))
|
|
{
|
|
// side face - check non-zero normal
|
|
EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
|
|
ignition::math::equal(n.Y(), 0.0)));
|
|
}
|
|
else
|
|
{
|
|
// top face - normal in +z direction
|
|
EXPECT_TRUE((n == ignition::math::Vector3d::UnitZ) ||
|
|
(ignition::math::equal(n.Z(), 0.0)));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
/////////////////////////////////////////////////
|
|
TEST_F(MeshManager, CreateExtrudedPolylineInvalid)
|
|
{
|
|
// test extruding invalid polyline
|
|
std::vector<std::vector<ignition::math::Vector2d> > path;
|
|
std::vector<ignition::math::Vector2d> subpath01;
|
|
subpath01.push_back(ignition::math::Vector2d(0, 0));
|
|
subpath01.push_back(ignition::math::Vector2d(0, 1));
|
|
subpath01.push_back(ignition::math::Vector2d(0, 2));
|
|
|
|
path.push_back(subpath01);
|
|
|
|
std::string meshName = "extruded_path_invalid";
|
|
double height = 10.0;
|
|
common::MeshManager::Instance()->CreateExtrudedPolyline(
|
|
meshName, path, height);
|
|
|
|
// check mesh does not exist due to extrusion failure
|
|
EXPECT_TRUE(!common::MeshManager::Instance()->HasMesh(meshName));
|
|
}
|
|
|
|
/////////////////////////////////////////////////
|
|
int main(int argc, char **argv)
|
|
{
|
|
::testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|