-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
penetration_as_point_pair.h
52 lines (43 loc) · 1.65 KB
/
penetration_as_point_pair.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
#pragma once
#include <utility>
#include "drake/common/eigen_types.h"
#include "drake/geometry/geometry_ids.h"
namespace drake {
namespace geometry {
/** A characterization of the intersection of two penetrating geometries. The
characterization consists of a pair of points and a normal. The points
represent a point on each geometry that most deeply penetrates the other
geometry (in the normal direction). For convenience, the penetration depth
is provided and is equal to:
depth = `(p_WCb - p_WCa) ⋅ nhat_BA_W`
(`depth` is strictly positive when there is penetration and otherwise not
defined.)
@tparam T The underlying scalar type. Must be a valid Eigen scalar. */
template <typename T>
struct PenetrationAsPointPair {
/** Swaps the interpretation of geometries A and B. */
void SwapAAndB() {
// Leave depth unchanged.
std::swap(id_A, id_B);
std::swap(p_WCa, p_WCb);
nhat_BA_W = -nhat_BA_W;
}
/** The id of the first geometry in the contact. */
GeometryId id_A;
/** The id of the second geometry in the contact. */
GeometryId id_B;
/** The point on A that most deeply penetrates B, measured and expressed in
the world frame. */
Vector3<T> p_WCa;
/** The point on B that most deeply penetrates A, measured and expressed in
the world frame. */
Vector3<T> p_WCb;
/** The unit-length normal which defines the penetration direction, pointing
from geometry B into geometry A, measured and expressed in the world frame.
It _approximates_ the normal to the plane on which the contact patch lies. */
Vector3<T> nhat_BA_W;
/** The penetration depth. */
T depth{-1.0};
};
} // namespace geometry
} // namespace drake