-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
signed_distance_pair.h
90 lines (80 loc) · 3.72 KB
/
signed_distance_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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#pragma once
#include <utility>
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/eigen_types.h"
#include "drake/geometry/geometry_ids.h"
namespace drake {
namespace geometry {
/** The data for reporting the signed distance between two geometries, A and B.
It provides the id's of the two geometries, the witness points Ca and Cb on
the surfaces of A and B, the signed distance, and nhat_BA_W a direction of
fastest increasing distance (always unit length and always point outward
from B's surface).
- When A and B are separated, distance > 0.
- When A and B are touching or penetrating, distance <= 0.
- By definition, nhat_AB_W must be in the opposite direction of nhat_BA_W.
- (p_WCa - p_Wcb) = distance · nhat_BA_W.
@warning For two geometries that are just touching (i.e., distance = 0), the
underlying code can guarantee a correct value for nhat_BA_W only
when one geometry is a sphere, and the other geometry is a sphere, a
box, or a cylinder. Otherwise, the underlying code is not in place yet
to guarantee a correct value for nhat_BA_W when surfaces are just
touching, and the vector will be populated by NaN values.
@tparam T The underlying scalar type. Must be a valid Eigen scalar.
*/
template <typename T>
struct SignedDistancePair{
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SignedDistancePair)
SignedDistancePair() = default;
// TODO(DamrongGuoy): When we have a full implementation of computing
// nhat_BA_W in ComputeSignedDistancePairwiseClosestPoints, check a
// condition like this (within epsilon):
// DRAKE_DEMAND(nhat_BA_W.norm() == T(1.));
/** Constructor
@param a The id of the first geometry (A).
@param b The id of the second geometry (B).
@param p_ACa_in The witness point on geometry A's surface, in A's frame.
@param p_BCb_in The witness point on geometry B's surface, in B's frame.
@param dist The signed distance between p_A and p_B.
@param nhat_BA_W_in A direction of fastest increasing distance.
@pre nhat_BA_W_in is unit-length. */
SignedDistancePair(GeometryId a, GeometryId b, const Vector3<T>& p_ACa_in,
const Vector3<T>& p_BCb_in, const T& dist,
const Vector3<T>& nhat_BA_W_in)
: id_A(a),
id_B(b),
p_ACa(p_ACa_in),
p_BCb(p_BCb_in),
distance(dist),
nhat_BA_W(nhat_BA_W_in) {}
/** Swaps the interpretation of geometries A and B. */
void SwapAAndB() {
// Leave distance alone; swapping A and B doesn't change it.
std::swap(id_A, id_B);
std::swap(p_ACa, p_BCb);
nhat_BA_W = -nhat_BA_W;
}
/** The id of the first geometry in the pair. */
GeometryId id_A;
/** The id of the second geometry in the pair. */
GeometryId id_B;
// TODO(SeanCurtis-TRI): Determine if this is the *right* API. Should we
// really be returning the points in geometry frame and not the world frame?
// 1. Penetration as point pair returns the points in world frame.
// 2. FCL computes it in world frame.
// 3. Generally, MBP would want the points in *body* frame. MBP has access
// to X_WB but does *not* generally have access to X_BG (it would have to
// query QueryObject for that information). Although, such a query can
// easily be provided.
/** The witness point on geometry A's surface, expressed in A's frame. */
Vector3<T> p_ACa;
/** The witness point on geometry B's surface, expressed in B's frame. */
Vector3<T> p_BCb;
/** The signed distance between p_ACa and p_BCb. */
T distance{};
/** A direction of fastest increasing distance. */
Vector3<T> nhat_BA_W;
};
} // namespace geometry
} // namespace drake