test-oriented-triangles.cc 2.73 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
// Copyright (C) 2016 LAAS-CNRS
// Author: Anna Seppala 
//
// This file is part of the hpp-affordance.
//
// hpp-affordance is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// hpp-affordance is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with hpp-affordance.  If not, see <http://www.gnu.org/licenses/>.

#include <hpp/affordance/affordance-extraction.hh>
#include <hpp/affordance/operations.hh>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>

Akseppal's avatar
Akseppal committed
24
#define BOOST_TEST_MODULE test-oriented-triangles
25
26
27
28
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE (test_affordance)

Akseppal's avatar
Akseppal committed
29
BOOST_AUTO_TEST_CASE (oriented_triangles)
30
31
32
33
34
35
36
37
38
39
40
41
42
{
	hpp::affordance::SupportOperationPtr_t support (new hpp::affordance::SupportOperation());
  hpp::affordance::LeanOperationPtr_t lean (new hpp::affordance::LeanOperation(0.1));

  std::vector <hpp::affordance::OperationBasePtr_t> operations;
  operations.push_back(support);
  operations.push_back(lean);

	std::vector<fcl::Vec3f> vertices;
  std::vector<fcl::Triangle> triangles;

  typedef fcl::BVHModel<fcl::OBBRSS> Model;

Akseppal's avatar
Akseppal committed
43
44
45
  fcl::Vec3f vert1 (-1,0,0);
  fcl::Vec3f vert2 (1,0,1);
  fcl::Vec3f vert3 (1,0,-1);
46
47
48
49
	vertices.push_back (vert1);
	vertices.push_back (vert2);
	vertices.push_back (vert3);

Akseppal's avatar
Akseppal committed
50
51
52
53
54
55
56
	int size_ = 7;
	int affFound = 0;
  int affNotFound = 0;
	fcl::Vec3f T (0,0,0);
	fcl::Vec3f axis (1,0,0);
	fcl::Matrix3f R;

Steve Tonneau's avatar
Steve Tonneau committed
57
    for (int i = 0; i < size_; ++i) {
Akseppal's avatar
Akseppal committed
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
	  boost::shared_ptr<Model> model (new Model ());
		fcl::Triangle tri (0,1,2);
		triangles.push_back (tri); 
	
		fcl::Quaternion3f quat;
		quat.fromAxisAngle(axis,3.1416*float(i)/180.0);
		quat.toRotation(R);
	
	  fcl::Transform3f pose (R, T);
	
		model->beginModel ();
		model->addSubModel (vertices, triangles);
		model->endModel ();
	
	  boost::shared_ptr <fcl::CollisionObject> obj (new fcl::CollisionObject(model, pose));
	
	  hpp::affordance::SemanticsDataPtr_t h = hpp::affordance::affordanceAnalysis (obj, operations);

		if (h->affordances_[1].size() == 1 && h->affordances_[0].size() == 0) {
			affFound += 1;
		} else {
			affNotFound += 1;
		}
}
	BOOST_CHECK_MESSAGE (affFound == 6 && affNotFound == 1, 
		"Strictly six affordances should have been found and one rejected. Now "<< affFound << 
		"were found and "<< affNotFound << " rejected.");
85
86
}
BOOST_AUTO_TEST_SUITE_END ()