GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/fcl.hxx Lines: 27 37 73.0 %
Date: 2024-04-26 13:14:21 Branches: 22 40 55.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2021 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_multibody_fcl_hxx__
6
#define __pinocchio_multibody_fcl_hxx__
7
8
#include <ostream>
9
#include <limits>
10
11
namespace pinocchio
12
{
13
14
  inline CollisionPair::CollisionPair()
15
  : Base((std::numeric_limits<GeomIndex>::max)(),(std::numeric_limits<GeomIndex>::max)())
16
  {}
17
18
4562
  inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
19
4562
  : Base(co1,co2)
20
  {
21

4562
    PINOCCHIO_CHECK_INPUT_ARGUMENT(co1 != co2,"The index of collision objects must not be equal.");
22
4562
  }
23
24
1275746
  inline bool CollisionPair::operator==(const CollisionPair& rhs) const
25
  {
26
46723
    return (first == rhs.first  && second == rhs.second)
27

1322469
      ||   (first == rhs.second && second == rhs.first );
28
  }
29
30
  inline bool CollisionPair::operator!=(const CollisionPair& other) const
31
  {
32
    return !(*this == other);
33
  }
34
35
  inline void CollisionPair::disp(std::ostream & os) const
36
  { os << "collision pair (" << first << "," << second << ")\n"; }
37
38
  inline std::ostream & operator << (std::ostream & os, const CollisionPair & X)
39
  {
40
    X.disp(os); return os;
41
  }
42
43
44
#ifdef PINOCCHIO_WITH_HPP_FCL
45
46
  inline bool operator==(const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
47
  {
48
    return lhs.collisionGeometry() == rhs.collisionGeometry()
49
            && lhs.getAABB().min_ == rhs.getAABB().min_
50
            && lhs.getAABB().max_ == rhs.getAABB().max_;
51
  }
52
53
#endif // PINOCCHIO_WITH_HPP_FCL
54
55
148
  inline bool operator==(const GeometryObject & lhs, const GeometryObject & rhs)
56
  {
57
148
    return (   lhs.name                 == rhs.name
58
148
            && lhs.parentFrame          == rhs.parentFrame
59
148
            && lhs.parentJoint          == rhs.parentJoint
60
148
            && lhs.geometry             == rhs.geometry
61
148
            && lhs.placement            == rhs.placement
62
148
            && lhs.meshPath             == rhs.meshPath
63
148
            && lhs.meshScale            == rhs.meshScale
64

296
            && lhs.disableCollision    == rhs.disableCollision
65
148
            );
66
  }
67
68
  inline bool operator!=(const GeometryObject & lhs, const GeometryObject & rhs)
69
  {
70
    return !(lhs == rhs);
71
  }
72
73
3
  inline std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object)
74
  {
75
3
    os  << "Name: \t \n" << geom_object.name << "\n"
76
3
        << "Parent frame ID: \t \n" << geom_object.parentFrame << "\n"
77
3
        << "Parent joint ID: \t \n" << geom_object.parentJoint << "\n"
78
3
        << "Position in parent frame: \t \n" << geom_object.placement << "\n"
79
3
        << "Absolute path to mesh file: \t \n" << geom_object.meshPath << "\n"
80
3
        << "Scale for transformation of the mesh: \t \n" << geom_object.meshScale.transpose() << "\n"
81


3
        << "Disable collision: \t \n" << geom_object.disableCollision << "\n"
82
3
        << std::endl;
83
3
    return os;
84
  }
85
86
87
} // namespace pinocchio
88
89
90
#endif // ifndef __pinocchio_multibody_fcl_hxx__