void AIGraph::gatherExternalNodesNear(const glm::vec3& center,
const float radius,
std::vector<AIGraphNode*>& nodes,
NodeType type) {
// the bounds end up covering more than might fit
auto planecoords = glm::vec2(center);
auto minWorld = planecoords - glm::vec2(radius);
auto maxWorld = planecoords + glm::vec2(radius);
auto minGrid = worldToGrid(minWorld);
auto maxGrid = worldToGrid(maxWorld);
for (int x = minGrid.x; x <= maxGrid.x; ++x) {
for (int y = minGrid.y; y <= maxGrid.y; ++y) {
int i = (x * WORLD_GRID_WIDTH) + y;
if (i < 0 || i >= static_cast<int>(gridNodes.size())) {
continue;
}
auto& external = gridNodes[i];
copy_if(external.begin(), external.end(), back_inserter(nodes),
[¢er, &radius, &type](const auto node) {
return node->type == type &&
glm::distance2(center, node->position) <
radius * radius;
});
}
}
}
void StompCollisionSpace::getVoxelsInBody(const bodies::Body &body, std::vector<tf::Vector3> &voxels)
{
bodies::BoundingSphere bounding_sphere;
body.computeBoundingSphere(bounding_sphere);
int x,y,z,x_min,x_max,y_min,y_max,z_min,z_max;
double xw,yw,zw;
tf::Vector3 v;
worldToGrid(bounding_sphere.center,bounding_sphere.center.x()-bounding_sphere.radius,bounding_sphere.center.y()-bounding_sphere.radius,
bounding_sphere.center.z()-bounding_sphere.radius, x_min,y_min,z_min);
worldToGrid(bounding_sphere.center,bounding_sphere.center.x()+bounding_sphere.radius,bounding_sphere.center.y()+bounding_sphere.radius,
bounding_sphere.center.z()+bounding_sphere.radius, x_max,y_max,z_max);
for(x = x_min; x <= x_max; ++x)
{
for(y = y_min; y <= y_max; ++y)
{
for(z = z_min; z <= z_max; ++z)
{
gridToWorld(bounding_sphere.center,x,y,z,xw,yw,zw);
v.setX(xw);
v.setY(yw);
v.setZ(zw);
// compute all intersections
int count=0;
std::vector<tf::Vector3> pts;
body.intersectsRay(v, tf::Vector3(0, 0, 1), &pts, count);
// if we have an odd number of intersections, we are inside
if (pts.size() % 2 == 1)
voxels.push_back(v);
}
}
}
// ROS_INFO("number of occupied voxels in bounding sphere: %i", voxels.size());
}
void GameWorld::addToGrid(GameObject* object)
{
auto coord = worldToGrid(glm::vec2(object->getPosition()));
if( coord.x < 0 || coord.y < 0 || coord.x >= WORLD_GRID_WIDTH || coord.y >= WORLD_GRID_WIDTH )
{
return;
}
auto index = (coord.x * WORLD_GRID_WIDTH) + coord.y;
worldGrid[index].instances.insert(object);
if( object->model->resource )
{
float cellhalf = WORLD_CELL_SIZE/2.f;
auto world = glm::vec3(glm::vec2(coord) * glm::vec2(WORLD_CELL_SIZE) - glm::vec2(WORLD_GRID_SIZE/2.f) + glm::vec2(cellhalf), 0.f);
auto offset = world - object->getPosition();
float maxRadius = glm::length(offset) + object->model->resource->getBoundingRadius();
worldGrid[index].boundingRadius = std::max(worldGrid[index].boundingRadius, maxRadius);
}
}
void GameWorld::destroyObject(GameObject* object)
{
auto coord = worldToGrid(glm::vec2(object->getPosition()));
if( coord.x < 0 || coord.y < 0 || coord.x >= WORLD_GRID_WIDTH || coord.y >= WORLD_GRID_WIDTH )
{
return;
}
auto index = (coord.x * WORLD_GRID_WIDTH) + coord.y;
worldGrid[index].instances.erase(object);
auto& pool = getTypeObjectPool(object);
pool.remove(object);
auto it = std::find(allObjects.begin(), allObjects.end(), object);
RW_CHECK(it != allObjects.end(), "destroying object not in allObjects");
if (it != allObjects.end()) {
allObjects.erase(it);
}
delete object;
}
void OccupancyGrid::getOccupiedVoxels(double x_center, double y_center, double z_center, double radius, std::string text, std::vector<geometry_msgs::Point> &voxels)
{
int x_c, y_c, z_c, radius_c;
geometry_msgs::Point v;
worldToGrid(x_center, y_center, z_center, x_c, y_c, z_c);
radius_c = radius/getResolution() + 0.5;
for(int z = z_c-radius_c; z < z_c+radius_c; ++z)
{
for(int y = y_c-radius_c; y < y_c+radius_c; ++y)
{
for(int x = x_c-radius_c; x < x_c+radius_c; ++x)
{
if(getCell(x,y,z) == 0)
{
gridToWorld(x, y, z, v.x, v.y, v.z);
voxels.push_back(v);
}
}
}
}
}
void MapFilter::setEntityPose(const geo::Transform2& pose, const std::vector<std::vector<geo::Vec2> >& contours,
double obstacle_inflation)
{
std::cout << "MapFilter::setEntityPose" << std::endl;
if (!mask_.data)
return;
std::vector<std::vector<cv::Point> > cv_contours;
for(std::vector<std::vector<geo::Vec2> >::const_iterator it = contours.begin(); it != contours.end(); ++it)
{
const std::vector<geo::Vec2>& contour = *it;
std::vector<cv::Point> points(contour.size());
for(unsigned int i = 0; i < contour.size(); ++i)
{
geo::Vec2 p_MAP = pose * contour[i];
int mx, my;
worldToGrid(p_MAP.x, p_MAP.y, mx, my);
points[i] = cv::Point(mx, my);
}
cv_contours.push_back(points);
for(unsigned int i = 0; i < contour.size(); ++i)
{
int j = (i + 1) % contour.size();
cv::line(mask_, points[i], points[j], cv::Scalar(0), obstacle_inflation / res_ + 1);
}
}
cv::fillPoly(mask_, cv_contours, cv::Scalar(0));
send_update_ = true;
}
void Grid3dUtility::get7Neighbors(std::vector<Vec3i>& neighbors,const Vec3f& p) const
{
Vec3i gridCoord = worldToGrid(p);
get7Neighbors(neighbors, gridCoord);
}
void Grid3dUtility::get27Neighbors(std::vector<Vec3i>& neighbors,const Vec3f& p, const SReal radius)
{
Vec3i gridCoord = worldToGrid(p);
int iradius = std::floor(radius/h);
get27Neighbors(neighbors, gridCoord, iradius );
}
int Grid3dUtility::cellId(const Vec3f& v) const
{
Vec3i gridCoord = worldToGrid(v);
return cellId(gridCoord);
}
bool Grid3dUtility::isInside(const Vec3f& v) const
{
Vec3i gridCoord = worldToGrid(v);
return isInside(gridCoord);
}
void Grid2dUtility::get5Neighbors(std::vector<Vec2i>& neighbors,const Vec2r& p) const
{
Vec2i gridCoord = worldToGrid(p);
get5Neighbors(neighbors, gridCoord);
}
void Grid2dUtility::get9Neighbors(std::vector<int>& neighbors, const Vec2r& p, const SReal radius)
{
Vec2i gridCoord = worldToGrid(p);
int iradius = std::floor(radius/h);
get9Neighbors(neighbors, gridCoord, iradius );
}
int Grid2dUtility::cellId(const Vec2r& v) const
{
Vec2i gridCoord = worldToGrid(v);
return cellId(gridCoord);
}
void USObstacleGridProvider::addUsMeasurement(bool actuatorChanged, float m, SensorData::UsActuatorMode mappedActuatorMode, unsigned int timeStamp,
const FilteredSensorData& theFilteredSensorData, const FrameInfo& theFrameInfo,
const OdometryData& theOdometryData)
{
ASSERT(mappedActuatorMode >= 0);
ASSERT(mappedActuatorMode < 4);
bool isNew = timeStamp == theFilteredSensorData.usTimeStamp;
if(timeStamp > bufferedMeasurements[mappedActuatorMode].timeStamp)
{
if(actuatorChanged || m < bufferedMeasurements[mappedActuatorMode].value)
bufferedMeasurements[mappedActuatorMode].value = m;
bufferedMeasurements[mappedActuatorMode].timeStamp = timeStamp;
}
if(m < parameters.minValidUSDist)
return;
switch(mappedActuatorMode)
{
case SensorData::leftToLeft:
for(std::vector<UsMeasurement>::iterator it = postponedLeftToRightMeasurements.begin(); it != postponedLeftToRightMeasurements.end();)
{
if(timeStamp - it->timeStamp > 1000) // postponed measurement is to old
it = postponedLeftToRightMeasurements.erase(it);
else if(abs(m - it->value) < 200)
{
addUsMeasurement(false, it->value, SensorData::leftToRight, it->timeStamp, theFilteredSensorData,theFrameInfo,theOdometryData);
it = postponedLeftToRightMeasurements.erase(it);
}
else
++it;
}
break;
case SensorData::rightToRight:
for(std::vector<UsMeasurement>::iterator it = postponedRightToLeftMeasurements.begin(); it != postponedRightToLeftMeasurements.end();)
{
if(timeStamp - it->timeStamp > 1000) // postponed measurement is to old
it = postponedRightToLeftMeasurements.erase(it);
else if(abs(m - it->value) < 200)
{
addUsMeasurement(false, it->value, SensorData::rightToLeft, it->timeStamp, theFilteredSensorData,theFrameInfo,theOdometryData);
it = postponedRightToLeftMeasurements.erase(it);
}
else
++it;
}
break;
case SensorData::leftToRight:
if(isNew && (timeStamp - bufferedMeasurements[SensorData::leftToLeft].timeStamp > 1000 || abs(bufferedMeasurements[SensorData::leftToLeft].value - m) > 200))
{
postponedLeftToRightMeasurements.push_back(UsMeasurement(m, timeStamp));
return; // Don't fill cells, as leftToRight measurements are only valid if left Sensor measured something
}
break;
case SensorData::rightToLeft:
if(isNew && (timeStamp - bufferedMeasurements[SensorData::rightToRight].timeStamp > 1000 || abs(bufferedMeasurements[SensorData::rightToRight].value - m) > 200))
{
postponedRightToLeftMeasurements.push_back(UsMeasurement(m, timeStamp));
return; // Don't fill cells, as rightToLeft measurements are only valid if right Sensor measured something
}
break;
default:
ASSERT(false);
break;
}
// Compute and draw relevant area:
if(m > parameters.maxValidUSDist)
m = static_cast<float>(parameters.maxValidUSDist);
Vector2<> measurement(m, 0.0f);
Vector2<> base, leftOfCone(measurement), rightOfCone(measurement);
switch(mappedActuatorMode)
{
case SensorData::leftToLeft: // left transmitter, left sensor => left
base = parameters.usLeftPose.translation;
leftOfCone.rotate(parameters.usOuterOpeningAngle);
rightOfCone.rotate(parameters.usInnerOpeningAngle);
leftOfCone = parameters.usLeftPose * leftOfCone;
rightOfCone = parameters.usLeftPose * rightOfCone;
break;
case SensorData::rightToRight: // right transmitter, right sensor => right
base = parameters.usRightPose.translation;
leftOfCone.rotate(-parameters.usInnerOpeningAngle);
rightOfCone.rotate(-parameters.usOuterOpeningAngle);
leftOfCone = parameters.usRightPose * leftOfCone;
rightOfCone = parameters.usRightPose * rightOfCone;
break;
case SensorData::leftToRight: // left transmitter, right sensor => center left
base = parameters.usCenterPose.translation;
leftOfCone.rotate(parameters.usCenterOpeningAngle);
rightOfCone.rotate(parameters.usCenterOpeningAngle * 0.25f);
leftOfCone = parameters.usCenterPose * leftOfCone;
rightOfCone = parameters.usCenterPose * rightOfCone;
break;
case SensorData::rightToLeft: // right transmitter, left sensor => center right
base = parameters.usCenterPose.translation;
leftOfCone.rotate(-parameters.usCenterOpeningAngle * 0.25f);
rightOfCone.rotate(-parameters.usCenterOpeningAngle);
leftOfCone = parameters.usCenterPose * leftOfCone;
rightOfCone = parameters.usCenterPose * rightOfCone;
break;
default:
ASSERT(false);
break;
}
// Draw current (positive) measurement:
if(m < parameters.maxValidUSDist)
{
/*LINE("module:USObstacleGridProvider:us", base.x, base.y, leftOfCone.x, leftOfCone.y,
30, Drawings::ps_solid, ColorRGBA(255, 0, 0));
LINE("module:USObstacleGridProvider:us", base.x, base.y, rightOfCone.x, rightOfCone.y,
30, Drawings::ps_solid, ColorRGBA(255, 0, 0));
LINE("module:USObstacleGridProvider:us", rightOfCone.x, rightOfCone.y, leftOfCone.x, leftOfCone.y,
30, Drawings::ps_solid, ColorRGBA(255, 0, 0));*/
}
// Compute additional cones:
// *Free is used for clearing cells between the robot and the obstacle
// *Far is used to add a second obstacle line to the grid (should help in case of high walk speeds and imprecision)
Vector2<> leftOfConeFree(leftOfCone - base);
Vector2<> rightOfConeFree(rightOfCone - base);
Vector2<> leftOfConeFar(leftOfCone - base);
Vector2<> rightOfConeFar(rightOfCone - base);
float cellDiameter = sqrt(static_cast<float>(USObstacleGrid::CELL_SIZE * USObstacleGrid::CELL_SIZE + USObstacleGrid::CELL_SIZE * USObstacleGrid::CELL_SIZE));
leftOfConeFree.normalize(leftOfConeFree.abs() - cellDiameter);
rightOfConeFree.normalize(rightOfConeFree.abs() - cellDiameter);
leftOfConeFar.normalize(leftOfConeFar.abs() + cellDiameter);
rightOfConeFar.normalize(rightOfConeFar.abs() + cellDiameter);
leftOfConeFree += base;
rightOfConeFree += base;
leftOfConeFar += base;
rightOfConeFar += base;
// Transfer cones to grid coordinate system:
const Vector2<int> leftOfConeCells =
worldToGrid(Vector2<int>(static_cast<int>(leftOfCone.x), static_cast<int>(leftOfCone.y)), theOdometryData);
const Vector2<int> leftOfConeCellsFree =
worldToGrid(Vector2<int>(static_cast<int>(leftOfConeFree.x), static_cast<int>(leftOfConeFree.y)), theOdometryData);
const Vector2<int> leftOfConeCellsFar =
worldToGrid(Vector2<int>(static_cast<int>(leftOfConeFar.x), static_cast<int>(leftOfConeFar.y)), theOdometryData);
const Vector2<int> rightOfConeCells =
worldToGrid(Vector2<int>(static_cast<int>(rightOfCone.x), static_cast<int>(rightOfCone.y)), theOdometryData);
const Vector2<int> rightOfConeCellsFree =
worldToGrid(Vector2<int>(static_cast<int>(rightOfConeFree.x), static_cast<int>(rightOfConeFree.y)), theOdometryData);
const Vector2<int> rightOfConeCellsFar =
worldToGrid(Vector2<int>(static_cast<int>(rightOfConeFar.x), static_cast<int>(rightOfConeFar.y)), theOdometryData);
// Free empty space until obstacle:
polyPoints.clear();
// Origin (sensor position):
Vector2<int> p1(static_cast<int>(base.x), static_cast<int>(base.y));
p1 = worldToGrid(p1, theOdometryData);
polyPoints.push_back(p1);
// Left corner of "cone":
polyPoints.push_back(Point(leftOfConeCellsFree, Point::NO_OBSTACLE));
// Right corner of "cone":
const Vector2<> ll(rightOfConeFree);
float f1 = ll.abs(),
f2 = f1 ? leftOfConeFree.abs() / f1 : 0;
Vector2<> gridPoint(ll);
gridPoint *= f2;
const Vector2<int> gridPointInt(static_cast<int>(gridPoint.x), static_cast<int>(gridPoint.y));
polyPoints.push_back(Point(worldToGrid(gridPointInt, theOdometryData), polyPoints.back().flags));
polyPoints.push_back(Point(rightOfConeCellsFree, Point::NO_OBSTACLE));
// Sensor position again:
polyPoints.push_back(p1);
// Clip and fill:
for(int j = 0; j < (int) polyPoints.size(); ++j)
clipPointP2(p1, polyPoints[j]);
fillScanBoundary(theFrameInfo);
// Enter obstacle to grid:
// If the sensor measures a high value, cells are cleared but
// no obstacles are entered
if(m != parameters.maxValidUSDist)
{
line(leftOfConeCells, rightOfConeCells, theFrameInfo);
line(leftOfConeCellsFar, rightOfConeCellsFar, theFrameInfo);
}
}
pair<int, float> Grid::trace(const Ray &ray, float tmin, float tmax, int ignored_id, int flags) const {
float3 p1 = ray.at(tmin), p2 = ray.at(tmax);
int2 pos = worldToGrid((int2)p1.xz()), end = worldToGrid((int2)p2.xz());
//TODO: verify for rays going out of grid space
if(!isInsideGrid(pos) || !isInsideGrid(end))
return make_pair(-1, constant::inf);
// Algorithm idea from: RTCD by Christer Ericson
int dx = end.x > pos.x? 1 : end.x < pos.x? -1 : 0;
int dz = end.y > pos.y? 1 : end.y < pos.y? -1 : 0;
float cell_size = (float)node_size;
float inv_cell_size = 1.0f / cell_size;
float lenx = fabs(p2.x - p1.x);
float lenz = fabs(p2.z - p1.z);
float minx = float(node_size) * floorf(p1.x * inv_cell_size), maxx = minx + cell_size;
float minz = float(node_size) * floorf(p1.z * inv_cell_size), maxz = minz + cell_size;
float tx = (p1.x > p2.x? p1.x - minx : maxx - p1.x) / lenx;
float tz = (p1.z > p2.z? p1.z - minz : maxz - p1.z) / lenz;
float deltax = cell_size / lenx;
float deltaz = cell_size / lenz;
int out = -1;
float out_dist = tmax + constant::epsilon;
while(true) {
int node_id = nodeAt(pos);
const Node &node = m_nodes[node_id];
if(flagTest(node.obj_flags, flags) && intersection(ray, node.bbox) < out_dist) {
const Object *objects[node.size];
int count = extractObjects(node_id, objects, ignored_id, flags);
for(int n = 0; n < count; n++) {
float dist = intersection(ray, objects[n]->bbox);
if(dist < out_dist) {
out_dist = dist;
out = objects[n] - &m_objects[0];
}
}
if(node.is_dirty)
updateNode(node_id);
}
if(tx <= tz || dz == 0) {
if(pos.x == end.x)
break;
tx += deltax;
pos.x += dx;
}
else {
if(pos.y == end.y)
break;
tz += deltaz;
pos.y += dz;
}
float ray_pos = tmin + max((tx - deltax) * lenx, (tz - deltaz) * lenz);
if(ray_pos >= out_dist)
break;
}
return make_pair(out, out_dist);
}
void Grid::traceCoherent(const vector<Segment> &segments, vector<pair<int, float>> &out, int ignored_id, int flags) const {
out.resize(segments.size(), make_pair(-1, constant::inf));
if(segments.empty())
return;
int2 start, end; {
float3 pmin(constant::inf, constant::inf, constant::inf);
float3 pmax(-constant::inf, -constant::inf, -constant::inf);
for(int s = 0; s < (int)segments.size(); s++) {
const Segment &segment = segments[s];
float tmin = max(0.0f, intersection(segment, m_bounding_box));
float tmax = min(segment.length(), -intersection(-segment, m_bounding_box));
float3 p1 = segment.at(tmin), p2 = segment.at(tmax);
pmin = min(pmin, min(p1, p2));
pmax = max(pmax, max(p1, p2));
}
start = worldToGrid((int2)pmin.xz());
end = worldToGrid((int2)pmax.xz());
start = max(start, int2(0, 0));
end = min(end, int2(m_size.x - 1, m_size.y - 1));
}
float max_dist = -constant::inf;
Interval idir[3], origin[3]; {
const Segment &first = segments.front();
idir [0] = first.invDir().x; idir [1] = first.invDir().y; idir [2] = first.invDir().z;
origin[0] = first.origin().x; origin[1] = first.origin().y; origin[2] = first.origin().z;
for(int s = 1; s < (int)segments.size(); s++) {
const Segment &segment = segments[s];
float tidir[3] = { segment.invDir().x, segment.invDir().y, segment.invDir().z };
float torigin[3] = { segment.origin().x, segment.origin().y, segment.origin().z };
max_dist = max(max_dist, segment.length());
for(int i = 0; i < 3; i++) {
idir [i] = Interval(min(idir [i].min, tidir [i]), max(idir [i].max, tidir [i]));
origin[i] = Interval(min(origin[i].min, torigin[i]), max(origin[i].max, torigin[i]));
}
}
}
for(int x = start.x; x <= end.x; x++) for(int z = start.y; z <= end.y; z++) {
int node_id = nodeAt(int2(x, z));
const Node &node = m_nodes[node_id];
if(flagTest(node.obj_flags, flags) && intersection(idir, origin, node.bbox) < max_dist) {
const Object *objects[node.size];
int count = extractObjects(node_id, objects, ignored_id, flags);
for(int n = 0; n < count; n++) {
if(intersection(idir, origin, objects[n]->bbox) < max_dist) {
for(int s = 0; s < (int)segments.size(); s++) {
const Segment &segment = segments[s];
float dist = intersection(segment, objects[n]->bbox);
if(dist < out[s].second) {
out[s].second = dist;
out[s].first = objects[n] - &m_objects[0];
}
}
}
}
if(node.is_dirty)
updateNode(node_id);
}
}
}
void collision_detection::StaticDistanceField::initialize(
const bodies::Body& body,
double resolution,
double space_around_body,
bool save_points)
{
points_.clear();
inv_twice_resolution_ = 1.0 / (2.0 * resolution);
logInform(" create points at res=%f",resolution);
EigenSTL::vector_Vector3d points;
determineCollisionPoints(body, resolution, points);
if (points.empty())
{
logWarn(" StaticDistanceField::initialize: No points in body. Using origin.");
points.push_back(body.getPose().translation());
if (body.getType() == shapes::MESH)
{
const bodies::ConvexMesh& mesh = dynamic_cast<const bodies::ConvexMesh&>(body);
const EigenSTL::vector_Vector3d& verts = mesh.getVertices();
logWarn(" StaticDistanceField::initialize: also using %d vertices.", int(verts.size()));
EigenSTL::vector_Vector3d::const_iterator it = verts.begin();
EigenSTL::vector_Vector3d::const_iterator it_end = verts.end();
for ( ; it != it_end ; ++it)
{
points.push_back(*it);
}
}
}
logInform(" StaticDistanceField::initialize: Using %d points.", points.size());
AABB aabb;
aabb.add(points);
logInform(" space_around_body = %f",space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-space)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_ -= Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
aabb.max_ += Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_.x() = std::floor(aabb.min_.x() / resolution) * resolution;
aabb.min_.y() = std::floor(aabb.min_.y() / resolution) * resolution;
aabb.min_.z() = std::floor(aabb.min_.z() / resolution) * resolution;
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (post-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
Eigen::Vector3d size = aabb.max_ - aabb.min_;
double diagonal = size.norm();
logInform(" DF: sz=(%7.3f %7.3f %7.3f) cnt=(%d %d %d) diag=%f",
size.x(),
size.y(),
size.z(),
int(size.x()/resolution),
int(size.y()/resolution),
int(size.z()/resolution),
diagonal);
distance_field::PropagationDistanceField df(
size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
diagonal * 2.0,
true);
df.addPointsToField(points);
DistPosEntry default_entry;
default_entry.distance_ = diagonal * 2.0;
default_entry.cell_id_ = -1;
resize(size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
default_entry);
logInform(" copy %d points.",
getNumCells(distance_field::DIM_X) *
getNumCells(distance_field::DIM_Y) *
getNumCells(distance_field::DIM_Z));
int pdf_x,pdf_y,pdf_z;
int sdf_x,sdf_y,sdf_z;
Eigen::Vector3d pdf_p, sdf_p;
df.worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x,pdf_y,pdf_z);
worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(0,0,0, pdf_x,pdf_y,pdf_z);
worldToGrid(0,0,0, sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
0.0,
0.0,
0.0,
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(points[0].x(), points[0].y(), points[0].z(), pdf_x,pdf_y,pdf_z);
worldToGrid(points[0].x(), points[0].y(), points[0].z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
points[0].x(),
points[0].y(),
points[0].z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
for (int z = 0 ; z < df.getZNumCells() ; ++z)
{
for (int y = 0 ; y < df.getYNumCells() ; ++y)
{
for (int x = 0 ; x < df.getXNumCells() ; ++x)
{
DistPosEntry entry;
double dist = df.getDistance(x, y, z);
const distance_field::PropDistanceFieldVoxel& voxel = df.getCell(x,y,z);
if (dist < 0)
{
// propogation distance field has a bias of -1*resolution on points inside the object
if (dist <= -resolution)
{
dist += resolution;
}
else
{
logError("PropagationDistanceField returned distance=%f between 0 and -resolution=%f."
" Did someone fix it?"
" Need to remove workaround from static_distance_field.cpp",
dist,-resolution);
dist = 0.0;
}
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_negative_point_.x(),
voxel.closest_negative_point_.y(),
voxel.closest_negative_point_.z());
}
else
{
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_point_.x(),
voxel.closest_point_.y(),
voxel.closest_point_.z());
}
setCell(x, y, z, entry);
}
}
}
if (save_points)
std::swap(points, points_);
}
bool Grid2dUtility::isInside(const Vec2r& v) const
{
Vec2i gridCoord = worldToGrid(v);
return isInside(gridCoord);
}
void GridUtility::get27Neighbors(std::vector<Vec3i>& neighbors,const Vec3r& p, const HReal radius) const
{
Vec3i gridCoord = worldToGrid(p);
get27Neighbors(neighbors, gridCoord, std::floor(radius/h) );
}
