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__ |