-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
contact_surface.cc
61 lines (50 loc) · 1.9 KB
/
contact_surface.cc
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
#include "drake/geometry/query_results/contact_surface.h"
#include "drake/common/default_scalars.h"
namespace drake {
namespace geometry {
using std::make_unique;
using std::unique_ptr;
using std::vector;
template <typename T>
const Vector3<T>& ContactSurface<T>::EvaluateGradE_M_W(int index) const {
if (grad_eM_W_ == nullptr) {
throw std::runtime_error(
"ContactSurface::EvaluateGradE_M_W() invalid; no gradient values "
"stored. Mesh M may be rigid, or the constituent gradients weren't "
"requested.");
}
return (*grad_eM_W_)[index];
}
template <typename T>
const Vector3<T>& ContactSurface<T>::EvaluateGradE_N_W(int index) const {
if (grad_eN_W_ == nullptr) {
throw std::runtime_error(
"ContactSurface::EvaluateGradE_N_W() invalid; no gradient values "
"stored. Mesh N may be rigid, or the constituent gradients weren't "
"requested.");
}
return (*grad_eN_W_)[index];
}
template <typename T>
bool ContactSurface<T>::Equal(const ContactSurface<T>& surface) const {
// Confirm we have the same representation. Technically, mesh and field
// representations are linked, but we'll test both to be safe.
if (this->mesh_W_.index() != surface.mesh_W_.index()) return false;
if (this->e_MN_.index() != surface.e_MN_.index()) return false;
if (is_triangle()) {
if (!this->tri_mesh_W().Equal(surface.tri_mesh_W())) return false;
if (!this->tri_e_MN().Equal(surface.tri_e_MN())) return false;
} else {
if (!this->poly_mesh_W().Equal(surface.poly_mesh_W())) return false;
if (!this->poly_e_MN().Equal(surface.poly_e_MN())) return false;
}
// TODO(SeanCurtis-TRI) This isn't testing the following quantities:
// 1. Geometry ids
// 2. Gradients.
// All checks passed.
return true;
}
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
class ContactSurface);
} // namespace geometry
} // namespace drake