bool shape::intersects(shape const& shape) const {
vector direction(shape.centroid() - centroid());
std::vector<vector> simplex;
simplex.push_back(vertices_[support(direction)] - shape.vertices()[shape.support(-direction)]);
direction = -direction;
for(;;) {
vector const a(vertices_[support(direction)] - shape.vertices()[shape.support(-direction)]);
if(a.dot(direction) <= 0.0f)
return false;
simplex.push_back(a);
vector const ao(-a);
if(simplex.size() == 3) {
vector const b(simplex[1]);
vector const c(simplex[0]);
vector const ab(b - a);
vector const ac(c - a);
vector const ab_triple(vector::triple_product_left(ac, ab, ab));
if(ab_triple.dot(ao) >= 0.0f) {
simplex.erase(simplex.begin());
direction = ab_triple;
} else {
vector const ac_triple(vector::triple_product_left(ab, ac, ac));
if(ac_triple.dot(ao) >= 0.0f) {
simplex.erase(simplex.begin() + 1);
direction = ac_triple;
} else
return true;
}
} else {
vector const b(simplex[0]);
vector const ab(b - a);
direction = vector::triple_product_left(ab, ao, ab);
if(!direction)
direction = ab.left();
}
}
}
std::tuple<bool, vector, float, vector, vector> shape::distance(shape const& shape) const {
vector direction(shape.centroid() - centroid());
vector a1(vertices_[support(direction)]);
vector a2(shape.vertices()[shape.support(-direction)]);
vector a(a1 - a2);
vector b1(vertices_[support(-direction)]);
vector b2(shape.vertices()[shape.support(direction)]);
vector b(b1 - b2);
vector c1, c2, c;
direction = segment(b, a).closest(vector());
for(int unsigned iterations(0); iterations < 10; ++iterations) {
direction = -direction.normalize();
if(!direction)
return std::make_tuple(false, vector(), 0.0f, vector(), vector());
c1 = vertices_[support(direction)];
c2 = shape.vertices()[shape.support(-direction)];
c = c1 - c2;
if(a.cross(b) * b.cross(c) > 0.0f && a.cross(b) * c.cross(a) > 0.0f)
return std::make_tuple(false, vector(), 0.0f, vector(), vector());
float const projection(c.dot(direction));
if(projection - a.dot(direction) < std::sqrt(std::numeric_limits<float>::epsilon())) {
std::tuple<vector, vector> const closest_points(get_closest_points(a1, a2, a, b1, b2, b));
return std::make_tuple(true, direction, -projection, std::get<0>(closest_points), std::get<1>(closest_points));
}
vector const point1(segment(a, c).closest(vector()));
vector const point2(segment(c, b).closest(vector()));
float const point1_length(point1.length());
float const point2_length(point2.length());
if(point1_length <= std::numeric_limits<float>::epsilon()) {
std::tuple<vector, vector> const closest_points(get_closest_points(a1, a2, a, c1, c2, c));
return std::make_tuple(true, direction, point1_length, std::get<0>(closest_points), std::get<1>(closest_points));
} else if(point2_length <= std::numeric_limits<float>::epsilon()) {
std::tuple<vector, vector> const closest_points(get_closest_points(c1, c2, c, b1, b2, b));
return std::make_tuple(true, direction, point2_length, std::get<0>(closest_points), std::get<1>(closest_points));
}
if(point1_length < point2_length) {
b1 = c1;
b2 = c2;
b = c;
direction = point1;
} else {
a1 = c1;
a2 = c2;
a = c;
direction = point2;
}
}
std::tuple<vector, vector> const closest_points(get_closest_points(a1, a2, a, b1, b2, b));
return std::make_tuple(true, direction, -c.dot(direction), std::get<0>(closest_points), std::get<1>(closest_points));
}