OBB.h 4.89 KB
Newer Older
sachinc's avatar
sachinc committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
sachinc's avatar
sachinc committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

Joseph Mirabel's avatar
Joseph Mirabel committed
38
39
#ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H
sachinc's avatar
sachinc committed
40

41
#include <hpp/fcl/math/types.h>
sachinc's avatar
sachinc committed
42

43
44
namespace hpp
{
sachinc's avatar
sachinc committed
45
46
namespace fcl
{
47
  class CollisionRequest;
sachinc's avatar
sachinc committed
48

49
/// @brief Oriented bounding box class
sachinc's avatar
sachinc committed
50
51
52
class OBB
{
public:
53
54
  /// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box. 
  /// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Joseph Mirabel's avatar
Joseph Mirabel committed
55
  Matrix3f axes;
sachinc's avatar
sachinc committed
56

57
  /// @brief Center of OBB
sachinc's avatar
sachinc committed
58
59
  Vec3f To;
  
60
  /// @brief Half dimensions of OBB
sachinc's avatar
sachinc committed
61
62
  Vec3f extent;

63
64
  /// Check collision between two OBB
  /// \return true if collision happens. 
sachinc's avatar
sachinc committed
65
66
  bool overlap(const OBB& other) const;

67
68
69
70
  /// Check collision between two OBB
  /// \return true if collision happens. 
  /// \retval sqrDistLowerBound squared lower bound on distance between boxes if
  ///         they do not overlap.
71
72
  bool overlap(const OBB& other, const CollisionRequest& request,
               FCL_REAL& sqrDistLowerBound) const;
73

74
  /// @brief Check whether the OBB contains a point.
jpan's avatar
   
jpan committed
75
  bool contain(const Vec3f& p) const;
sachinc's avatar
sachinc committed
76

77
  /// @brief A simple way to merge the OBB and a point (the result is not compact).
sachinc's avatar
sachinc committed
78
79
  OBB& operator += (const Vec3f& p);

80
  /// @brief Merge the OBB and another OBB (the result is not compact).
sachinc's avatar
sachinc committed
81
82
83
84
85
86
  OBB& operator += (const OBB& other)
  {
     *this = *this + other;
     return *this;
  }

87
  /// @brief Return the merged OBB of current OBB and the other one (the result is not compact).
sachinc's avatar
sachinc committed
88
89
  OBB operator + (const OBB& other) const;

90
  /// @brief Width of the OBB.
91
  inline FCL_REAL width() const
sachinc's avatar
sachinc committed
92
93
94
95
  {
    return 2 * extent[0];
  }

96
  /// @brief Height of the OBB.
97
  inline FCL_REAL height() const
sachinc's avatar
sachinc committed
98
99
100
101
  {
    return 2 * extent[1];
  }

102
  /// @brief Depth of the OBB
103
  inline FCL_REAL depth() const
sachinc's avatar
sachinc committed
104
105
106
107
  {
    return 2 * extent[2];
  }

108
  /// @brief Volume of the OBB
109
  inline FCL_REAL volume() const
sachinc's avatar
sachinc committed
110
111
112
113
  {
    return width() * height() * depth();
  }

114
  /// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
115
  inline FCL_REAL size() const
sachinc's avatar
sachinc committed
116
  {
117
    return extent.squaredNorm();
sachinc's avatar
sachinc committed
118
119
  }

120
  /// @brief Center of the OBB
121
  inline const Vec3f& center() const
sachinc's avatar
sachinc committed
122
123
124
125
126
  {
    return To;
  }


127
128
  /// @brief Distance between two OBBs, not implemented.
  FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
sachinc's avatar
sachinc committed
129
130
};

jpan's avatar
jpan committed
131
132
133
134

/// @brief Translate the OBB bv
OBB translate(const OBB& bv, const Vec3f& t);

135
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
136
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
sachinc's avatar
sachinc committed
137

138
139
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
140
141
	     const OBB& b2, const CollisionRequest& request,
             FCL_REAL& sqrDistLowerBound);
142

143

144
145
146
147
148
/// Check collision between two boxes
/// \param B, T orientation and position of first box,
/// \param a half dimensions of first box,
/// \param b half dimensions of second box.
/// The second box is in identity configuration.
149
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
sachinc's avatar
sachinc committed
150
151
}

152
153
} // namespace hpp

sachinc's avatar
sachinc committed
154
#endif