-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
shape_specification.cc
100 lines (83 loc) · 3.19 KB
/
shape_specification.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
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
91
92
93
94
95
96
97
98
99
100
#include "drake/geometry/shape_specification.h"
#include <fmt/format.h>
namespace drake {
namespace geometry {
using math::RigidTransform;
Shape::~Shape() {}
void Shape::Reify(ShapeReifier* reifier, void* user_data) const {
reifier_(*this, reifier, user_data); }
std::unique_ptr<Shape> Shape::Clone() const { return cloner_(*this); }
Sphere::Sphere(double radius)
: Shape(ShapeTag<Sphere>()), radius_(radius) {
if (radius < 0) {
throw std::logic_error(
fmt::format("Sphere radius should be >= 0 (was {}).", radius));
}
}
Cylinder::Cylinder(double radius, double length)
: Shape(ShapeTag<Cylinder>()),
radius_(radius),
length_(length) {
if (radius <= 0 || length <= 0) {
throw std::logic_error(
fmt::format("Cylinder radius and length should both be > 0 (were {} "
"and {}, respectively).",
radius, length));
}
}
HalfSpace::HalfSpace() : Shape(ShapeTag<HalfSpace>()) {}
RigidTransform<double> HalfSpace::MakePose(const Vector3<double>& Hz_dir_F,
const Vector3<double>& p_FB) {
const double norm = Hz_dir_F.norm();
// Note: this value of epsilon is somewhat arbitrary. It's merely a minor
// fence over which ridiculous vectors will trip.
if (norm < 1e-10) {
throw std::logic_error("Can't make pose from a zero vector.");
}
// First create basis.
// Projects the normal into the first quadrant in order to identify the
// *smallest* component of the normal.
const Vector3<double> u(Hz_dir_F.cwiseAbs());
int min_axis;
u.minCoeff(&min_axis);
// The axis corresponding to the smallest component of the normal will be
// *most* perpendicular.
Vector3<double> perp_axis{0, 0, 0};
perp_axis(min_axis) = 1;
// Now define x-, y-, and z-axes. The z-axis lies in the given direction.
Vector3<double> Hz_F = Hz_dir_F / norm;
Vector3<double> Hx_F = Hz_F.cross(perp_axis).normalized();
Vector3<double> Hy_F = Hz_F.cross(Hx_F);
// Transformation from canonical frame C to target frame F.
const auto R_FH =
math::RotationMatrixd::MakeFromOrthonormalColumns(Hx_F, Hy_F, Hz_F);
const Vector3<double> p_FH = Hz_F.dot(p_FB) * Hz_F;
return RigidTransform<double>(R_FH, p_FH);
}
Box::Box(double width, double depth, double height)
: Shape(ShapeTag<Box>()),
size_(width, depth, height) {
if (width <= 0 || depth <= 0 || height <= 0) {
throw std::logic_error(
fmt::format("Box width, depth, and height should all be > 0 (were {}, "
"{}, and {}, respectively).",
width, depth, height));
}
}
Box Box::MakeCube(double edge_size) {
return Box(edge_size, edge_size, edge_size);
}
Mesh::Mesh(const std::string& absolute_filename, double scale)
: Shape(ShapeTag<Mesh>()), filename_(absolute_filename), scale_(scale) {
if (std::abs(scale) < 1e-8) {
throw std::logic_error("Mesh |scale| cannot be < 1e-8.");
}
}
Convex::Convex(const std::string& absolute_filename, double scale)
: Shape(ShapeTag<Convex>()), filename_(absolute_filename), scale_(scale) {
if (std::abs(scale) < 1e-8) {
throw std::logic_error("Convex |scale| cannot be < 1e-8.");
}
}
} // namespace geometry
} // namespace drake