double network::gibbsSampling(int pos, int value, int t) {
int size = nodes.size();
int sample[2][size];
srand((unsigned int) time(NULL));
//Forward Sampling
topologicalSort();
for (int i = 0; i < size; i++) {
int j = topOrder[i];
if (nodes.at(j).isObserved()) {
sample[0][j] = nodes.at(j).getValue();
} else {
sample[0][j] = (toss() <= (nodes.at(j).factor(1)));
nodes.at(j).setValue(sample[0][j]);
}
}
//GIBBS SAMPLING
double sumMix = 0;
double sum = 0;
int mixSize = t*0.02;
int mixStart = 0;
for (int i = 1; i <= t; i++) {
assign(sample[0]);
for (int j = 0; j < 7; j++) {
if ((*(nodeAt(j))).isObserved()) {
sample[1][j] = sample[0][j];
} else {
sample[1][j] = (toss() <= (pSamplingNode(j, 1)));
(*(nodeAt(j))).setValue(sample[1][j]);
}
}
//MIXING PROCESS
if (i <= mixStart + mixSize) {
sumMix += pSamplingNode(pos, value);
} else {
sum += pSamplingNode(pos, value);
}
if (i == mixStart + mixSize * 2) {
if (abs(sumMix / mixSize - sum / mixSize) < 0.001) {
sum += sumMix;
}else{
sumMix = sum;
sum = 0;
mixStart += mixSize;
}
}
//UPDATE OF PREVIOUS SAMPLE
for (int j = 0; j < 7; j++) {
sample[0][j] = sample[1][j];
}
}
return sum/(t-mixStart);
}
int Grid::pixelIntersect(const int2 &screen_pos, bool (*pixelTest)(const ObjectDef&, const int2&), int flags) const {
IRect grid_box(0, 0, m_size.x, m_size.y);
int best = -1;
FBox best_box;
for(int y = grid_box.min.y; y < grid_box.max.y; y++) {
const int2 &row_rect = m_row_rects[y];
if(row_rect.x >= screen_pos.y || row_rect.y <= screen_pos.y)
continue;
for(int x = grid_box.min.x; x < grid_box.max.x; x++) {
int node_id = nodeAt(int2(x, y));
const Node &node = m_nodes[node_id];
if(!flagTest(node.obj_flags, flags) || !node.rect.isInside(screen_pos))
continue;
if(node.is_dirty)
updateNode(node_id);
const Object *objects[node.size];
int count = extractObjects(node_id, objects, -1, flags);
for(int n = 0; n < count; n++)
if(objects[n]->rect().isInside(screen_pos) && pixelTest(*objects[n], screen_pos)) {
if(best == -1 || drawingOrder(objects[n]->bbox, best_box) == 1) {
best = objects[n] - &m_objects[0];
best_box = objects[n]->bbox;
}
}
}
}
return best;
}
void Grid::findAll(vector<int> &out, const FBox &box, int ignored_id, int flags) const {
IRect grid_box = nodeCoords(box);
for(int y = grid_box.min.y; y <= grid_box.max.y; y++)
for(int x = grid_box.min.x; x <= grid_box.max.x; x++) {
int node_id = nodeAt(int2(x, y));
const Node &node = m_nodes[node_id];
if(!flagTest(node.obj_flags, flags) || !areOverlapping(box, node.bbox))
continue;
bool anything_found = false;
const Object *objects[node.size];
int count = extractObjects(node_id, objects, ignored_id, flags);
for(int n = 0; n < count; n++)
if(areOverlapping(box, objects[n]->bbox)) {
if(objects[n]->node_id == -1)
disableOverlap(objects[n]);
out.push_back(objects[n] - &m_objects[0]);
anything_found = true;
}
if(!anything_found && node.is_dirty)
updateNode(node_id);
}
clearDisables();
}
void Grid::findAll(vector<int> &out, const IRect &view_rect, int flags) const {
IRect grid_box(0, 0, m_size.x, m_size.y);
for(int y = grid_box.min.y; y < grid_box.max.y; y++) {
const int2 &row_rect = m_row_rects[y];
if(row_rect.x >= view_rect.max.y || row_rect.y <= view_rect.min.y)
continue;
for(int x = grid_box.min.x; x < grid_box.max.x; x++) {
int node_id = nodeAt(int2(x, y));
const Node &node = m_nodes[node_id];
if(!flagTest(node.obj_flags, flags) || !areOverlapping(view_rect, node.rect))
continue;
bool anything_found = false;
const Object *objects[node.size];
int count = extractObjects(node_id, objects, -1, flags);
for(int n = 0; n < count; n++) {
if(areOverlapping(view_rect, objects[n]->rect())) {
if(objects[n]->node_id == -1)
disableOverlap(objects[n]);
out.push_back(objects[n] - &m_objects[0]);
anything_found = true;
}
}
if(!anything_found && node.is_dirty)
updateNode(node_id);
}
}
clearDisables();
}
/* Construct a binary tree node with given data and insert it into the heap */
priorityQueueEntry priorityQueueInsert(priorityQueue* queue, int key, void* data)
{
/* Construction */
priorityQueueEntry* entry = (priorityQueueEntry*) malloc(sizeof (priorityQueueEntry));
entry->key=key;
entry->data=data;
entry->parent=NULL;
entry->childl=NULL;
entry->childr=NULL;
/* Insertion */
queue->size++;
if(queue->size==1)
queue->root=entry;
else {
/* Find the variable that shall store the new entry */
priorityQueueEntry* parent=nodeAt(queue, (queue->size)/2);
priorityQueueEntry* *destination;
if(queue->size%2==0)
destination=&(parent->childl);
else
destination=&(parent->childr);
assert(*destination==NULL);
*destination=entry;
entry->parent=parent;
promote(entry);
}
return* entry;
}
void testNewHeap(void) {
printf("Running newHeap test\n");
Node nodes[10] = { newNode(1), newNode(2), newNode(3), newNode(4), newNode(5), newNode(6), newNode(7), newNode(8), newNode(9), newNode(10) };
Heap heap = newHeap(10, nodes);
Node *sortedNodes = getNodes(heap);
for(int i = 0; i<10; i++) {
int expected = i + 1;
if(nodeAt(sortedNodes, i)->data != expected) {
char message[50];
sprintf(message, "expected data=%d, got data=%d", expected, (*(sortedNodes + i))->data);
printFailed(message);
return;
}
}
printPassed();
}
/* Drop the root of the binary heap, but return its contents */
void* priorityQueueExtract(priorityQueue* queue)
{
void* data=queue->root->data;
priorityQueueEntry* lastentry=nodeAt(queue, queue->size);
swap(lastentry, queue->root);
if(lastentry->parent) {
if(lastentry->parent->childl==lastentry)
lastentry->parent->childl=NULL;
else
lastentry->parent->childr=NULL;
} else
queue->root=NULL;
free(lastentry);
demote(queue->root);
queue->size--;
return data;
}
int Grid::findAny(const FBox &box, int ignored_id, int flags) const {
IRect grid_box = nodeCoords(box);
for(int y = grid_box.min.y; y <= grid_box.max.y; y++)
for(int x = grid_box.min.x; x <= grid_box.max.x; x++) {
int node_id = nodeAt(int2(x, y));
const Node &node = m_nodes[node_id];
if(!node.size || !flagTest(node.obj_flags, flags) || !areOverlapping(box, node.bbox))
continue;
const Object *objects[node.size];
int count = extractObjects(node_id, objects, ignored_id, flags);
for(int n = 0; n < count; n++)
if(areOverlapping(box, objects[n]->bbox))
return objects[n] - &m_objects[0];
if(node.is_dirty)
updateNode(node_id);
}
return -1;
}
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);
}
}
}
