/**
* @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_);
}
