/** * @brief Sets up a new brush model * @sa EndModel */ void BeginModel (int entityNum) { dBspModel_t* mod; int start, end; int j; const entity_t* e; if (curTile->nummodels == MAX_MAP_MODELS) Sys_Error("MAX_MAP_MODELS (%i)", curTile->nummodels); mod = &curTile->models[curTile->nummodels]; mod->firstface = curTile->numfaces; firstmodeledge = curTile->numedges; /* bound the brushes */ e = &entities[entityNum]; start = e->firstbrush; end = start + e->numbrushes; AABB modBox; modBox.setNegativeVolume(); for (j = start; j < end; j++) { const mapbrush_t* b = &mapbrushes[j]; /* not a real brush (origin brush) - e.g. func_door */ if (!b->numsides) continue; modBox.add(b->mbBox); } VectorCopy(modBox.mins, mod->mins); VectorCopy(modBox.maxs, mod->maxs); }
void PhotonMap::init() { // build the underlying balanced kd-Tree if (size() > 1) { Photon **orig = (Photon **) malloc(sizeof(Photon*) * (size() + 1)); Photon **tree = (Photon **) malloc(sizeof(Photon*) * (size() + 1)); AABB aabb; // add all photons for (unsigned i = 1; i <= size(); ++i) { orig[i] = &m_photons[i]; m_photons[i].plane = 0; aabb.add(m_photons[i].position); tree[i] = NULL; } // recursively balance tree _balance(tree, orig, 1, 1, size(), aabb); free(orig); unsigned d, j = 1, temp = 1; Photon temp_photon = m_photons[j]; // update internal bookkeeping for (unsigned i = 1; i <= size(); ++i) { d = tree[j] - &m_photons[0]; tree[j] = NULL; if (d != temp) { m_photons[j] = m_photons[d]; } else { m_photons[j] = temp_photon; if (i < size()) { for (; temp <= size(); ++temp) if (tree[temp] != NULL) break; temp_photon = m_photons[temp]; j = temp; } continue; } j = d; } free(tree); } m_halfPhotons = m_size / 2 - 1; }
/** needs to be done here, on map brushes as worldMins and worldMaxs from levels.c * are only calculated on BSPing * @param[out] mapSize the returned size in map units */ static void Check_MapSize (vec3_t mapSize) { AABB mapBox; mapBox.setNegativeVolume(); for (int i = 0; i < nummapbrushes; i++) { const mapbrush_t* brush = &mapbrushes[i]; for (int bi = 0; bi < brush->numsides; bi++) { const winding_t* winding = brush->original_sides[bi].winding; for (int vi = 0; vi < winding->numpoints; vi++) mapBox.add(winding->p[vi]); } } mapBox.getDiagonal(mapSize); }
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_); }