void collision_detection::StaticDistanceField::determineCollisionPoints(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}
void distance_field::findInternalPointsConvex(
const bodies::Body& body,
double resolution,
EigenSTL::vector_Vector3d& points)
{
bodies::BoundingSphere sphere;
body.computeBoundingSphere(sphere);
double xval_s = std::floor((sphere.center.x() - sphere.radius - resolution) / resolution) * resolution;
double yval_s = std::floor((sphere.center.y() - sphere.radius - resolution) / resolution) * resolution;
double zval_s = std::floor((sphere.center.z() - sphere.radius - resolution) / resolution) * resolution;
double xval_e = sphere.center.x() + sphere.radius + resolution;
double yval_e = sphere.center.y() + sphere.radius + resolution;
double zval_e = sphere.center.z() + sphere.radius + resolution;
Eigen::Vector3d pt;
for(pt.x() = xval_s; pt.x() <= xval_e; pt.x() += resolution) {
for(pt.y() = yval_s; pt.y() <= yval_e; pt.y() += resolution) {
for(pt.z() = zval_s; pt.z() <= zval_e; pt.z() += resolution) {
if(body.containsPoint(pt)) {
points.push_back(pt);
}
}
}
}
}