-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
posed_half_space.h
72 lines (59 loc) · 2.7 KB
/
posed_half_space.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
#pragma once
#include <limits>
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_throw.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/proximity/mesh_traits.h"
#include "drake/geometry/proximity/plane.h"
namespace drake {
namespace geometry {
namespace internal {
/* The definition of a half space, posed in an arbitrary frame. It is
defined by its boundary plane such that a point's signed distance relative to
the half space is equal to its height relative to the boundary plane (see
Plane for details). The boundary plane's normal points outside the half space.
The %PosedHalfSpace class is distinct from the HalfSpace class in that the
HalfSpace class defines the half space in its own canonical frame. The posed
half space can be defined in an arbitrary frame.
The signed distance of a point Q can only be meaningfully evaluated if the
point is measured and expressed in the same frame as the half space:
```
const Vector3<T> nhat_F = ...;
const Vector3<T> p_FP = ...; // P is a point on the half space boundary.
const PosedHalfSpace<T> half_space_F(nhat_F, p_FP); // Half space in frame F.
const double distance_Q = half_space_F.CalcSignedDistance(p_FQ); // valid!
const double distance_R = half_space_F.CalcSignedDistance(p_GR); // invalid!
```
*/
template <typename T>
class PosedHalfSpace {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PosedHalfSpace)
/* Constructs a half space in frame F in terms of its boundary plane. The
parameters define the boundary plane (see Plane for details). The boundary
plane's normal points _outside_ the half space. */
PosedHalfSpace(const Vector3<T>& nhat_F, const Vector3<T>& p_FP,
bool already_normalized = false)
: plane_(nhat_F, p_FP, already_normalized) {}
/* Computes the signed distance to the point Q (measured and expressed in
Frame F). If Q's signed distance is positive, it lies outside the half space.
If it is negative, it lies inside. If it is zero, it lies on the boundary
plane of the half space.
The return type depends on both the half space's scalar type `T` and the
given query point's scalar type `U`. See
@ref drake::geometry::promoted_numerical "promoted_numerical_t" for details.
*/
template <typename U = T>
promoted_numerical_t<U, T> CalcSignedDistance(const Vector3<U>& p_FQ) const {
return plane_.CalcHeight(p_FQ);
}
/* Gets the normal expressed in Frame F. */
const Vector3<T>& normal() const { return plane_.normal(); }
/* Gets the boundary plane of `this` posed half space. */
const Plane<T>& boundary_plane() const { return plane_; }
private:
Plane<T> plane_;
};
} // namespace internal
} // namespace geometry
} // namespace drake