static int dot_contains(carmen_dot_filter_p f, double x, double y, double stdev) {
double theta;
double dx, dy, ux, uy, vx, vy, vxy;
if (f->type == CARMEN_DOT_PERSON) {
ux = f->person_filter.x;
uy = f->person_filter.y;
vx = f->person_filter.px;
vy = f->person_filter.py;
vxy = f->person_filter.pxy;
}
else if (f->type == CARMEN_DOT_TRASH) {
ux = f->trash_filter.x;
uy = f->trash_filter.y;
vx = f->trash_filter.px;
vy = f->trash_filter.py;
vxy = f->trash_filter.pxy;
}
else { // door
ux = f->door_filter.x;
uy = f->door_filter.y;
vx = f->door_filter.px;
vy = f->door_filter.py;
vxy = f->door_filter.pxy;
}
dx = x - ux;
dy = y - uy;
theta = bnorm_theta(vx, vy, vxy);
rotate2d(&dx, &dy, -theta);
if (fabs(dx) <= stdev*sqrt(vx/fabs(cos(theta))) &&
fabs(dy) <= stdev*sqrt(vy/fabs(sin(theta))))
return 1;
return 0;
}
wxThread::ExitCode HaloRenderingThread::Entry()
{
wxCommandEvent evt(wxEVT_HALO_RENDERING_THREAD_NOTIFY, wxID_ANY);
setupStruct.resultValid = false;
setupStruct.imageBuffer = 0;
setupStruct.rayPaths = new map<RayPathId, RayPathDescriptor>;
if (setupStruct.crystals.size() == 0)
{
evt.SetString(wxT("No crystal population set up."));
evt.SetInt(1);
setupStruct.notifee->AddPendingEvent(evt);
return 0;
}
// Cast rays on crystals and refracted rays will form a pixel.
int crystalTypeIndex = 0;
CrystalDescriptor *currentDescriptor = &setupStruct.crystals[0];
const int N_CRYSTAL_TYPES = setupStruct.crystals.size();
int crystalsRemaining = currentDescriptor->populationWeight;
Mesh currentMesh;
size_t percentStep = setupStruct.crystalCount / 100;
if (percentStep < 1) percentStep = 1;
Vector3 sunPos(0, 0, -100000);
rotate2d(sunPos.y, sunPos.z, setupStruct.solarAltitude * 3.14159265636 / 180.0);
// Two prependicular vector
Vector3 prepX = sunPos % Vector3(0, 1, 0); // FIXME: Sun on the zenith
Vector3 prepY = sunPos % prepX;
prepX /= ~prepX;
prepY /= ~prepY;
setupStruct.deleteBuffer = freeBuffer;
setupStruct.deleteMap = freeMap;
size_t size = setupStruct.imageSize;
setupStruct.imageBuffer = new uint32_t[size * size * 6];
memset(setupStruct.imageBuffer, 0, size * size * 6 * sizeof(uint32_t));
uint32_t *leftPlane = setupStruct.imageBuffer; // 0th plane
uint32_t *rightPlane = setupStruct.imageBuffer + size * size; // 1st plane
uint32_t *topPlane = setupStruct.imageBuffer + 2 * size * size; // 2nd plane
uint32_t *bottomPlane = setupStruct.imageBuffer + 3 * size * size; // 3rd plane
uint32_t *backPlane = setupStruct.imageBuffer + 4 * size * size; // 4th plane
uint32_t *frontPlane = setupStruct.imageBuffer + 5 * size * size; // 5th plane
double halfImageSize = size * 0.5;
int maxRays = (setupStruct.maxRayCastInfoSize * 1000000) / (sizeof(RayPathDescriptor) + sizeof(RayPathId));
if (maxRays == 0) maxRays = 1;
int recordRayModulus = setupStruct.crystalCount / maxRays;
if (!recordRayModulus) recordRayModulus = 1;
struct RefractionColor
{
int rgb;
double refractionIndex;
};
RefractionColor colors[] =
{
{ 0x0000FF, 1.3072 },
{ 0x0080FF, 1.3094 },
{ 0x00FFFF, 1.31 },
{ 0x00FF80, 1.3107 },
{ 0x00FF00, 1.3114 },
{ 0x80FF00, 1.3125 },
{ 0xFFFF00, 1.3136 },
{ 0xFF8000, 1.3147 },
{ 0xFF0000, 1.3158 },
{ 0xFF0040, 1.3172 },
{ 0xFF0080, 1.32 },
};
const int N_COLORS = sizeof(colors) / sizeof(colors[0]);
// Cast many rays.
for (size_t i = 0; i < setupStruct.crystalCount; i++)
{
if (setupStruct.cancelled)
{
// If the user shut the rendering down...
delete[] setupStruct.imageBuffer;
setupStruct.imageBuffer = 0;
return 0;
}
if (!crystalsRemaining)
{
// We iterate through the crystals based on their population weights.
crystalTypeIndex++;
if (crystalTypeIndex >= N_CRYSTAL_TYPES) crystalTypeIndex = 0;
currentDescriptor = &setupStruct.crystals[crystalTypeIndex];
crystalsRemaining = currentDescriptor->populationWeight;
}
// Get the raw mesh
currentMesh = currentDescriptor->mesh;
// Rotate it according to the orientation.
Matrix orientationMatrix = getOrientationMatrix(currentDescriptor->orientation);
// Apply wobbliness
Vector3 wobbleRotationAxis(1, 0, 0);
rotate2d(wobbleRotationAxis.x, wobbleRotationAxis.z, randFloat(0, 3.1415));
double wobblinessLimit = currentDescriptor->wobbliness * 3.1415 / 180.0;
Matrix wobbleMatrix = createRotationMatrix(
wobbleRotationAxis,
randFloatNormal(
0,
wobblinessLimit
)
);
Matrix transformation = orientationMatrix % wobbleMatrix;
transformMesh(currentMesh, transformation);
// Crystal created, now cast a ray on it.
Vector3 offset = prepX * randFloat(-1, 1) + prepY * randFloat(-1, 1);
vector<RayPath> rayPaths;
// Compute color here
double colorCode = randFloat(0, N_COLORS - 1);
RefractionColor prev = colors[(int)(floor(colorCode))];
RefractionColor next = colors[(int)(ceil(colorCode))];
double kColor = colorCode - floor(colorCode);
RefractionColor currentColor;
currentColor.rgb = prev.rgb;
currentColor.refractionIndex = (1 - kColor) * prev.refractionIndex + kColor * next.refractionIndex;
// Compute real poisition of the ray (Sun is a disk)
Vector3 realSunPos = sunPos;
Vector3 rayRotVector;
double rayRotVectorLength;
do
{
rayRotVector = realSunPos % getRandomVector();
rayRotVectorLength = ~rayRotVector;
}
while (rayRotVectorLength == 0);
rayRotVector /= rayRotVectorLength;
realSunPos = transformVector(
createRotationMatrix(
rayRotVector,
randFloat(
0,
setupStruct.solarDiskRadius * 3.1415926536 / 180.0
)
),
realSunPos
);
computeRayPathInGlassMesh(currentMesh, currentColor.refractionIndex, realSunPos + offset, -realSunPos, 0.01, rayPaths);
/* Project rays on the six planes. */
for (size_t j = 0; j < rayPaths.size(); j++)
{
RayPath ¤t = rayPaths[j];
size_t pathLength = current.size();
Vector3 exitDir = current[pathLength - 1].first - current[pathLength - 2].first;
Vector3 projectionDir = -exitDir;
double xPos, yPos;
// select the plane to project on.
uint32_t *plane;
double xm = 1, ym = 1;
int planeId;
if ((fabs(projectionDir.x) > fabs(projectionDir.y)) && (fabs(projectionDir.x) > fabs(projectionDir.z)))
{
if (projectionDir.x < 0)
{
plane = leftPlane;
planeId = 0;
xm = -1;
}
else
{
plane = rightPlane;
planeId = 1;
}
xPos = xm * projectionDir.z / fabs(projectionDir.x) * halfImageSize + halfImageSize;
yPos = -ym * projectionDir.y / fabs(projectionDir.x) * halfImageSize + halfImageSize;
}
if ((fabs(projectionDir.y) > fabs(projectionDir.x)) && (fabs(projectionDir.y) > fabs(projectionDir.z)))
{
if (projectionDir.y < 0)
{
plane = bottomPlane;
planeId = 3;
ym = -1;
}
else
{
plane = topPlane;
planeId = 2;
}
xPos = xm * projectionDir.x / fabs(projectionDir.y) * halfImageSize + halfImageSize;
yPos = -ym * projectionDir.z / fabs(projectionDir.y) * halfImageSize + halfImageSize;
}
if ((fabs(projectionDir.z) > fabs(projectionDir.x)) && (fabs(projectionDir.z) > fabs(projectionDir.y)))
{
if (projectionDir.z < 0)
{
plane = frontPlane;
planeId = 5;
}
else
{
plane = backPlane;
planeId = 4;
xm = -1;
}
xPos = xm * projectionDir.x / fabs(projectionDir.z) * halfImageSize + halfImageSize;
yPos = -ym * projectionDir.y / fabs(projectionDir.z) * halfImageSize + halfImageSize;
}
// Calculate the new intensity of the pixel.
xPos = clampInInt(xPos, 0, size - 1);
yPos = clampInInt(yPos, 0, size - 1);
int prevPixel = plane[(int)(yPos) * size + (int)(xPos)];
int nextPixel = 0;
double intensity = current[pathLength - 2].second * 20;
for (int j = 0; j < 3; j++)
{
int currentSaturation = (prevPixel >> (8 * j)) & 0xFF;
int toAdd = (int)(((currentColor.rgb >> (8 * j)) & 0xFF) * intensity) >> 8;
int nextSaturation;
if (currentSaturation + toAdd > 255) nextSaturation = 255;
else nextSaturation = currentSaturation + toAdd;
nextPixel += (1 << (8 * j)) * nextSaturation * setupStruct.pixelIntensity;
}
plane[(int)(yPos) * size + (int)(xPos)] = nextPixel;
// Record the ray if needed
if (!(i % recordRayModulus))
{
RayPathId pixelId(planeId, (int)xPos, (int)yPos);
RayPathDescriptor theDescriptor(
¤tDescriptor->mesh,
transformation,
realSunPos + offset,
-realSunPos,
intensity
);
map<RayPathId, RayPathDescriptor>::iterator it = setupStruct.rayPaths->find(pixelId);
if (it == setupStruct.rayPaths->end())
{
// If not found, insert it as new
setupStruct.rayPaths->insert(
make_pair(
pixelId,
theDescriptor
)
);
}
else if (it->second.intensity < intensity)
{
// If found, update it if the current ray is more intense
it->second = theDescriptor;
}
}
}
if (i % percentStep == 0)
{
sendNotifyString(wxString::Format(wxT("Casting rays: %d%%"), i / percentStep));
}
crystalsRemaining--;
}
evt.SetString(wxT("Completed."));
evt.SetInt(1); //< 1 means the operation is finished.
setupStruct.resultValid = true;
setupStruct.notifee->AddPendingEvent(evt);
return 0;
}
static int dot_filter(double x, double y) {
int i, imax;
double pmax, p;
double ux, uy, dx, dy;
double vx, vy, vxy;
double theta;
imax = -1;
pmax = 0.0;
if (map_prob(x, y) >= map_occupied_threshold)
return 1;
for (i = 0; i < num_filters; i++) {
ux = filters[i].person_filter.x;
uy = filters[i].person_filter.y;
vx = filters[i].person_filter.px;
vy = filters[i].person_filter.py;
vxy = filters[i].person_filter.pxy;
dx = x - ux;
dy = y - uy;
// check if (x,y) is roughly within 3 stdev's of (E[x],E[y])
theta = bnorm_theta(vx, vy, vxy);
rotate2d(&dx, &dy, -theta);
if (fabs(dx) < 4.0*sqrt(vx/fabs(cos(theta))) &&
fabs(dy) < 4.0*sqrt(vy/fabs(sin(theta)))) {
p = bnorm_f(x, y, ux, uy, vx, vy, vxy);
if (p > pmax) {
pmax = p;
imax = i;
}
}
//printf("3 person std dev's = (%.2f, %.2f)\n",
// 3.0*sqrt(vx/fabs(cos(theta))), 3.0*sqrt(vx/fabs(cos(theta))));
ux = filters[i].trash_filter.x;
uy = filters[i].trash_filter.y;
vx = filters[i].trash_filter.px;
vy = filters[i].trash_filter.py;
vxy = filters[i].trash_filter.pxy;
dx = x - ux;
dy = y - uy;
// check if (x,y) is roughly within 3 stdev's of (E[x],E[y])
theta = bnorm_theta(vx, vy, vxy);
rotate2d(&dx, &dy, -theta);
if (fabs(dx) < 4.0*sqrt(vx/fabs(cos(theta))) &&
fabs(dy) < 4.0*sqrt(vy/fabs(sin(theta)))) {
p = bnorm_f(x, y, ux, uy, vx, vy, vxy);
if (p > pmax) {
pmax = p;
imax = i;
}
}
//printf("3 trash std dev's = (%.2f, %.2f)\n",
// 3.0*sqrt(vx/fabs(cos(theta))), 3.0*sqrt(vx/fabs(cos(theta))));
ux = filters[i].door_filter.x;
uy = filters[i].door_filter.y;
vx = filters[i].door_filter.px;
vy = filters[i].door_filter.py;
vxy = filters[i].door_filter.pxy;
dx = x - ux;
dy = y - uy;
// check if (x,y) is roughly within 3 stdev's of (E[x],E[y])
theta = bnorm_theta(vx, vy, vxy);
rotate2d(&dx, &dy, -theta);
if (fabs(dx) < 4.0*sqrt(vx/fabs(cos(theta))) &&
fabs(dy) < 4.0*sqrt(vy/fabs(sin(theta)))) {
p = bnorm_f(x, y, ux, uy, vx, vy, vxy);
if (p > pmax) {
pmax = p;
imax = i;
}
}
//printf("3 door std dev's = (%.2f, %.2f)\n",
// 3.0*sqrt(vx/fabs(cos(theta))), 3.0*sqrt(vx/fabs(cos(theta))));
}
/*
if (imax >= 0)
printf("pmax = %.4f, map_prob = %.4f at filter %d (x=%.2f, y = %.2f)\n",
pmax, map_prob(x, y), imax, x, y);
*/
if (imax >= 0 && pmax > map_prob(x, y)) {
//printf("pmax = %.4f, map_prob = %.4f at filter %d (x=%.2f, y = %.2f)\n",
// pmax, map_prob(x, y), imax, x, y);
filters[imax].person_filter.hidden_cnt = 0;
person_filter_sensor_update(&filters[imax].person_filter, x, y);
if (filters[imax].do_motion_update) {
filters[imax].do_motion_update = 0;
trash_filter_motion_update(&filters[imax].trash_filter);
door_filter_motion_update(&filters[imax].door_filter);
//filters[imax].updated = 1;
}
if (do_sensor_update || filters[imax].sensor_update_cnt < sensor_update_cnt) {
filters[imax].sensor_update_cnt++;
trash_filter_sensor_update(&filters[imax].trash_filter, x, y);
door_filter_sensor_update(&filters[imax].door_filter, x, y);
//filters[imax].updated = 1;
}
filters[imax].type = dot_classify(&filters[imax]);
//if (filters[imax].type == CARMEN_DOT_PERSON)
filters[imax].updated = 1;
return 1;
}
return 0;
}
void HexagonalPrismGeneratorDialog::OngeneratePrismClick(wxCommandEvent& event)
{
const double SIN60 = 0.866025404;
double basicPoints[6][2] =
{
{1, 0},
{0.5, SIN60},
{-0.5, SIN60},
{-1, 0},
{-0.5, -SIN60},
{0.5, -SIN60}
}; //< Points of a hexagon.
double prismHeight;
prismHeightTextCtrl->GetValue().ToDouble(&prismHeight);
vector<int> face;
if (kernPrismRadioButton->GetValue())
{
// Create Kern basic points.
// Kern prism's basal face is triangular.
double sideFaceRatio;
sideFaceRatioTextBox->GetValue().ToDouble(&sideFaceRatio);
const double ANG120 = 3.1415926526 * 2.0/3.0;
double angle1 = ANG120 * (sideFaceRatio) / (1 + sideFaceRatio);
for (int i = 0; i < 3; i++)
{
double x = 1;
double y = 0;
rotate2d(x, y, i * ANG120);
basicPoints[2 * i][0] = x;
basicPoints[2 * i][1] = y;
x = 1;
y = 0;
rotate2d(x, y, i * ANG120 + angle1);
basicPoints[2 * i + 1][0] = x;
basicPoints[2 * i + 1][1] = y;
}
}
else if (parryPlateRadioButton->GetValue())
{
// generate oblate prism.
double height;
parryPlateHeightTextBox->GetValue().ToDouble(&height);
for (int i = 0; i < 6; i++)
{
double tmpx = basicPoints[i][0];
double tmpy = basicPoints[i][1];
if (tmpx < 0)
{
basicPoints[i][0] = tmpx * height + (1 - height)*-1;
basicPoints[i][1] = tmpy * height;
}
else
{
basicPoints[i][0] = tmpx * height + (1 - height);
basicPoints[i][1] = tmpy * height;
}
}
}
initMesh(mesh);
// Set vertices
// First 6 vertex is for the front hexagonal face.
for (int i = 0; i < 6; i++)
{
mesh.vertices.push_back(Vector3(basicPoints[i][0], basicPoints[i][1], prismHeight));
}
// Next 6 vertex is for the back hexagonal face
for (int i = 0; i < 6; i++)
{
mesh.vertices.push_back(Vector3(basicPoints[i][0], basicPoints[i][1], -prismHeight));
}
// Make front face
face.clear();
for (int i = 0; i < 6; i++)
{
face.push_back(i);
}
mesh.faces.push_back(face);
// Make back face
face.clear();
for (int i = 0; i < 6; i++)
{
face.push_back(i + 6);
}
mesh.faces.push_back(face);
// Make side faces
for (int i = 0; i < 6; i++)
{
int j = i + 1 == 6 ? 0 : i + 1;
face.clear();
// Add vertices on the front face
face.push_back(i);
face.push_back(j);
// Add vertices on the back face
face.push_back(j + 6);
face.push_back(i + 6);
mesh.faces.push_back(face);
}
// Scale the prism to fit in a sphere which has unit radius.
double vertexDistance = ~mesh.vertices[0]; // All vertices are the same distance from the center.
for (size_t i = 0; i < mesh.vertices.size(); i++)
{
mesh.vertices[i] /= vertexDistance;
}
EndModal(ID_BUTTON_GENERATEPRISM);
}
