Plane detectPlane(
SimulatorArgument *simArgs,
Plane *plane,
int planeIdx,
EventLog *eventD,
EventLog *eventR,
Vector *bestLocation,
Vector *bestLocationSimulated,
double *pMinError)
{
double minError = -1.0;
// meaningless plane
Plane bestPlane(Vector(0, 0, 0), Vector(0, 0, 1));
// in here, assume that order of location of each eventDirect is identical to eventReflected
Vector vRealListener = eventD->location;
Vector vListener = getSimulatedListener(eventD);
PlaneDetector planeDetector(vListener);
// not enough measurements. can not detect plane. continue
if (eventR->measurements.size() < 4)
{
fprintf(stderr, "invalid point!! planeIdx = %d\n", planeIdx);
exit(1);
}
//do plane detection
for (size_t j = 0; j < eventR->measurements.size(); j++)
{
int bid = eventR->measurements[j].userBid;
Vector vBeacon = (simArgs->beacons.findByUserBid(bid))->getLocation();
double distance = eventR->measurements[j].distance;
//insert reflected distance data into plane detector
planeDetector.addReflectedDistance(vBeacon, distance);
}
Plane detectedPlane = planeDetector.getPlane();
double error = planeDetector.getDistanceError(*plane);
if (minError < 0 || error < minError)
{
minError = error;
bestPlane = detectedPlane;
*bestLocation = vRealListener;
*bestLocationSimulated = vListener;
}
*pMinError = minError;
return bestPlane;
}
//Performs a plane sweep across the triangles to find the best splitting plane using the SAH.
//From "On building fast kd-Trees for Ray Tracing, and on doing that in O(N log N)" by I. Wald and V. Havran
pair<pair<SplitPlane, SplitSide>, double> KDNode::findPlane(vector<Triangle*>& triangles, AABB boundingBox) {
double bestCost = INFINITY;
SplitPlane bestPlane(0, 0);
SplitSide bestSide;
for (int dim = 0; dim < 3; dim++) {
vector<SweepEvent> events;
for (auto t : triangles) {
AABB b = t->getBoundingBox().intersection(boundingBox);
if (b.getSize()[dim] == 0.0) {
events.push_back(SweepEvent(t, b.getStartpoint()[dim], PLANAR));
}
else {
events.push_back(SweepEvent(t, b.getStartpoint()[dim], START));
events.push_back(SweepEvent(t, b.getEndpoint()[dim], END));
}
}
sort(events.begin(), events.end());
int left = 0;
int planar = 0;
int right = triangles.size();
for (int i = 0; i < events.size(); i++) {
double coordinate = events[i].coordinate;
SplitPlane plane(coordinate, dim);
int pAdd = 0; int pPlan = 0; int pRem = 0;
while (i < events.size() && events[i].coordinate == coordinate && events[i].type == END) {
pRem++; i++;
}
while (i < events.size() && events[i].coordinate == coordinate && events[i].type == PLANAR) {
pPlan++; i++;
}
while (i < events.size() && events[i].coordinate == coordinate && events[i].type == START) {
pAdd++; i++;
}
planar = pPlan; right -= pPlan; right -= pRem;
pair<double, SplitSide> result = SAH(plane, boundingBox, left, right, planar);
if (result.first < bestCost) {
bestCost = result.first;
bestPlane = plane;
bestSide = result.second;
}
left += pAdd; left += pPlan; planar = 0;
}
}
return make_pair(make_pair(bestPlane, bestSide), bestCost);
}
Plane detectPlane(
SimulatorArgument *simArgs,
Plane *plane,
int planeIdx,
EventLogList *eventsDirect,
Vector *bestLocation,
Vector *bestLocationSimulated,
double *pMinError)
{
EventGenerator reflectedEventGenerator;
// normal vector of each plane faces outside of cube
Vector vFacing = plane->vNormal;
printf("\nGenerating events for Plane %d...", planeIdx);
fflush(stdout);
reflectedEventGenerator.generateEventForPlaneDetection(
simArgs,
listenerInterval,
minMargin,
listenerZ,
vFacing);
EventLogList *eventsReflected = &reflectedEventGenerator.events;
printf("done\n");
double minError = -1.0;
// meaningless plane
Plane bestPlane(Vector(0, 0, 0), Vector(0, 0, 1));
char filename[255];
sprintf(filename, "pd_%02d.dat.%02d", planeIdx, simArgs->randomSeed);
FILE* fp = fopen(filename, "w");
double prev_x = 0.0;
// in here, assume that order of location of each eventDirect is identical to eventReflected
for (size_t i = 0; i < eventsDirect->size(); i++)
{
printf("Detecting Plane %d...%3d%% (%7d / %7d) \r",
planeIdx,
(int)((i + 1) / (double)eventsDirect->size() * 100),
(int)(i + 1),
(int) eventsDirect->size());
fflush(stdout);
EventLog eventD = eventsDirect->events[i];
EventLog eventR = eventsReflected->events[i];
Vector vRealListener = eventD.location;
Vector vListener = getSimulatedListener(simArgs, &eventD);
double estimationError = vListener.getDistance(vRealListener);
PlaneDetector planeDetector(vListener);
if (vRealListener.x != prev_x) fprintf(fp, "\n");
prev_x = vRealListener.x;
//log real listener location
fprintf(fp, "%f\t%f\t%f\t", vRealListener.x, vRealListener.y, vRealListener.z);
// not enough measurements. can not detect plane. continue
if (eventR.measurements.size() < 4)
{
fprintf(fp, "-100.0\t-100.0\t%f\n", estimationError);
continue;
}
//do plane detection
for (size_t j = 0; j < eventR.measurements.size(); j++)
{
int bid = eventR.measurements[j].userBid;
Vector vBeacon = (simArgs->beacons.findByUserBid(bid))->getLocation();
double distance = eventR.measurements[j].distance;
//insert reflected distance data into plane detector
planeDetector.addReflectedDistance(vBeacon, distance);
}
Plane detectedPlane = planeDetector.getPlane();
double error = planeDetector.getDistanceError(*plane);
double errorAngle = planeDetector.getAngleError(*plane);
if (minError < 0 || error < minError)
{
minError = error;
bestPlane = detectedPlane;
*bestLocation = vRealListener;
*bestLocationSimulated = vListener;
}
fprintf(fp, "%f\t%f\t%f\n", error, errorAngle, estimationError);
}
fclose(fp);
*pMinError = minError;
printf("\n");
return bestPlane;
}