void face_scan(const char *line, struct face *f,
struct vertex *v, int vertex_count)
{
int n, i;
int index[3];
/* TODO face format is more complex than this */
n = sscanf(line, "f %d %d %d", &index[0], &index[1], &index[2]);
if (n == 3) {
for (i = 0; i < 3; i++) {
if ((index[i] > vertex_count) || (index[i] < 1)) {
fprintf(stderr, "bad face line "
"(vertex index %d out of bounds):"
"\n%s\n", index[i], line);
} else {
f->v[i].x = v[index[i]-1].x;
f->v[i].y = v[index[i]-1].y;
f->v[i].z = v[index[i]-1].z;
f->v[i].w = 1.0;
}
}
compute_normal(f);
for (i = 0; i < 3; i++)
compute_centroid(f);
} else {
fprintf(stderr, "bad face line (needs 3 vertices):\n%s\n",
line);
}
}
polygon_t* polygon_star(point_t* x0, point_t* points, int num_points)
{
ASSERT(num_points > 2);
ASSERT(all_points_are_coplanar(points, num_points));
// Find the plane of the polygon.
vector_t normal;
compute_normal(points, num_points, &normal);
point_t xp;
compute_centroid(points, num_points, &xp);
sp_func_t* plane = plane_new(&normal, &xp);
// Find the angles of the points within the plane, and sort the points
// by this angle.
point2_t pts[num_points];
for (int i = 0; i < num_points; ++i)
plane_project(plane, &points[i], &pts[i]);
point2_t xc;
plane_project(plane, x0, &xc);
polygon2_t* poly2 = polygon2_star(&xc, pts, num_points);
// Read off the vertex ordering from the planar polygon.
ASSERT(polygon2_num_vertices(poly2) == num_points);
int* ordering = polygon2_ordering(poly2);
// Create our polygon with the given ordering.
polygon_t* poly = polygon_new_with_ordering(points, ordering, num_points);
// Clean up.
poly2 = NULL;
plane = NULL;
return poly;
}
Triangle::Triangle (const Point3D& a, const Point3D& b, const Point3D& c)
: GeometricObject(),
v0(a),
v1(b),
v2(c)
{
compute_normal();
}
void get_normals(struct rt_bot_internal *bot, float *dest)
{
size_t i;
for (i = 0; i < bot->num_faces; i++) {
compute_normal(curr->bot, bot->faces[3*i], bot->faces[3*i+1],
bot->faces[3*i+2], dest);
}
}
PlaneStatus hull_poly_test(uint *ref, PlaneStatus *status, uint ref_length, egreal *vertex, uint *polygon)
{
egreal *origo, normal[3], *v1, *v2, *v3;
uint test, i;
PlaneStatus output = PS_FULL;
for(i = 0; status[i] != PS_FULL; i++);
compute_normal(normal, vertex, polygon);
origo = &vertex[polygon[0] * 3];
v1 = &vertex[ref[i * 3] * 3];
v2 = &vertex[ref[i * 3 + 1] * 3];
v3 = &vertex[ref[i * 3 + 2] * 3];
if(-0.001 > (v1[0] - origo[0]) * normal[0] + (v1[1] - origo[1]) * normal[1] + (v1[2] - origo[2]) * normal[2])
return PS_OUTSIDE;
if(-0.001 > (v2[0] - origo[0]) * normal[0] + (v2[1] - origo[1]) * normal[1] + (v2[2] - origo[2]) * normal[2])
return PS_OUTSIDE;
if(-0.001 > (v3[0] - origo[0]) * normal[0] + (v3[1] - origo[1]) * normal[1] + (v3[2] - origo[2]) * normal[2])
return PS_OUTSIDE;
v1 = &vertex[polygon[0] * 3];
v2 = &vertex[polygon[1] * 3];
v3 = &vertex[polygon[2] * 3];
for(i = 0; i < ref_length; i++)
{
if(status[i] != PS_OUTSIDE && polygon != &ref[i * 3])
{
compute_normal(normal, vertex, &ref[i * 3]);
origo = &vertex[ref[i * 3] * 3];
test = 0;
if(-0.01 < (v1[0] - origo[0]) * normal[0] + (v1[1] - origo[1]) * normal[1] + (v1[2] - origo[2]) * normal[2])
test++;
if(-0.01 < (v2[0] - origo[0]) * normal[0] + (v2[1] - origo[1]) * normal[1] + (v2[2] - origo[2]) * normal[2])
test++;
if(-0.01 < (v3[0] - origo[0]) * normal[0] + (v3[1] - origo[1]) * normal[1] + (v3[2] - origo[2]) * normal[2])
test++;
if(test == 0)
return PS_OUTSIDE;
if(test < 3)
output = PS_PARSIAL;
}
}
return output;
}
uint *create_polygon_map(uint *ref, egreal *vertex, uint ref_length, uint vertex_length)
{
uint found = 0, *poly_own, *vertex_own, i = 0, j, group = 0;
egreal normal[3];
poly_own = malloc((sizeof *poly_own) * ref_length * 2);
for(i = 0; i < ref_length; i++)
poly_own[i] = -1;
vertex_own = malloc((sizeof *vertex_own) * vertex_length);
i = 0;
while(i < ref_length)
{
for(j = 0; j < vertex_length; j++)
vertex_own[j] = -1;
poly_own[i] = group;
vertex_own[ref[i * 3 + 0]] = group;
vertex_own[ref[i * 3 + 1]] = group;
vertex_own[ref[i * 3 + 2]] = group;
found = 1;
printf("group %i\n", group);
while(found != 0)
{
found = 0;
for(i = 0; i < ref_length; i++)
{
if(poly_own[i] == -1)
{
if(vertex_own[ref[i * 3 + 0]] != -1 && vertex_own[ref[i * 3 + 1]] != -1 && vertex_own[ref[i * 3 + 2]] != -1)
poly_own[i] = group;
else
{
printf("a\n");
for(j = 0; j < 3; j++)
{
if(vertex_own[ref[i * 3 + j]] == -1 && vertex_own[ref[i * 3 + ((j + 1) % 3)]] != -1 && vertex_own[ref[i * 3 + ((j + 2) % 3)]] != -1)
{
printf("b\n");
compute_normal(normal, vertex, &ref[i * 3]);
if(hull_test(vertex_own, vertex_length, vertex, normal, &vertex[ref[i * 3 + j] * 3]))
{
vertex_own[ref[i * 3 + j]] = group;
poly_own[i] = group;
found = 1;
}
}
}
}
}
}
}
group++;
for(i = 0; i < ref_length && poly_own[i] != -1; i++);
}
free(vertex_own);
return poly_own;
}
bool
Torus::hit(const Ray& ray, double& tmin, ShadeRec& sr) const {
if (!bbox.hit(ray))
return (false);
double x1 = ray.o.x; double y1 = ray.o.y; double z1 = ray.o.z;
double d1 = ray.d.x; double d2 = ray.d.y; double d3 = ray.d.z;
double coeffs[5]; // coefficient array for the quartic equation
double roots[4]; // solution array for the quartic equation
// define the coefficients of the quartic equation
double sum_d_sqrd = d1 * d1 + d2 * d2 + d3 * d3;
double e = x1 * x1 + y1 * y1 + z1 * z1 - a * a - b * b;
double f = x1 * d1 + y1 * d2 + z1 * d3;
double four_a_sqrd = 4.0 * a * a;
coeffs[0] = e * e - four_a_sqrd * (b * b - y1 * y1); // constant term
coeffs[1] = 4.0 * f * e + 2.0 * four_a_sqrd * y1 * d2;
coeffs[2] = 2.0 * sum_d_sqrd * e + 4.0 * f * f + four_a_sqrd * d2 * d2;
coeffs[3] = 4.0 * sum_d_sqrd * f;
coeffs[4] = sum_d_sqrd * sum_d_sqrd; // coefficient of t^4
// find roots of the quartic equation
int num_real_roots = SolveQuartic(coeffs, roots);
bool intersected = false;
double t = kHugeValue;
if (num_real_roots == 0) // ray misses the torus
return(false);
// find the smallest root greater than kEpsilon, if any
// the roots array is not sorted
for (int j = 0; j < num_real_roots; j++)
if (roots[j] > kEpsilon) {
intersected = true;
if (roots[j] < t)
t = roots[j];
}
if(!intersected)
return (false);
tmin = t;
sr.local_hit_point = ray.o + t * ray.d;
sr.normal = compute_normal(sr.local_hit_point);
return (true);
}
uint *c_triangelize(uint *output_length, uint *ref, uint poly_count, egreal *vertex, uint vertex_count)
{
egreal normal[3];
uint i, j = 0, *tri;
for(i = 0; i < poly_count * 4; i += 4)
{
if(ref[i] < vertex_count && ref[i + 1] < vertex_count && ref[i + 2] < vertex_count && vertex[ref[i] * 3] != E_REAL_MAX && vertex[ref[i + 1] * 3] != E_REAL_MAX && vertex[ref[i + 2] * 3] != E_REAL_MAX)
{
j++;
if(ref[i + 3] < vertex_count && vertex[ref[i + 3] * 3] != E_REAL_MAX)
j++;
}
}
tri = malloc((sizeof *tri) * j * 6);
j = 0;
for(i = 0; i < poly_count * 4; i += 4)
{
if(ref[i] < vertex_count && ref[i + 1] < vertex_count && ref[i + 2] < vertex_count && vertex[ref[i] * 3] != E_REAL_MAX && vertex[ref[i + 1] * 3] != E_REAL_MAX && vertex[ref[i + 2] * 3] != E_REAL_MAX)
{
if(ref[i + 3] < vertex_count && vertex[ref[i + 3] * 3] != E_REAL_MAX)
{
compute_normal(normal, vertex, &ref[i]);
if(0 < normal[0] * (vertex[ref[i + 3] * 3 + 0] - vertex[ref[i] * 3 + 0]) +
normal[1] * (vertex[ref[i + 3] * 3 + 1] - vertex[ref[i] * 3 + 1]) +
normal[2] * (vertex[ref[i + 3] * 3 + 2] - vertex[ref[i] * 3 + 2]))
{
tri[j++] = ref[i];
tri[j++] = ref[i + 1];
tri[j++] = ref[i + 2];
tri[j++] = ref[i];
tri[j++] = ref[i + 2];
tri[j++] = ref[i + 3];
}else
{
tri[j++] = ref[i];
tri[j++] = ref[i + 1];
tri[j++] = ref[i + 3];
tri[j++] = ref[i + 1];
tri[j++] = ref[i + 2];
tri[j++] = ref[i + 3];
}
}else
{
tri[j++] = ref[i];
tri[j++] = ref[i + 1];
tri[j++] = ref[i + 2];
}
}
}
*output_length = j / 3;
return tri;
}
polygon_t* polygon_giftwrap(point_t* points, int num_points)
{
ASSERT(num_points > 2);
ASSERT(all_points_are_coplanar(points, num_points));
// Find the plane of the polygon.
vector_t normal;
compute_normal(points, num_points, &normal);
point_t x0;
compute_centroid(points, num_points, &x0);
sp_func_t* plane = plane_new(&normal, &x0);
// Do the gift-wrapping in 2D.
point2_t pts[num_points];
for (int i = 0; i < num_points; ++i)
plane_project(plane, &points[i], &pts[i]);
polygon2_t* poly2 = polygon2_giftwrap(pts, num_points);
#if 0
// Re-embed the resulting vertices in 3D.
int num_vertices = polygon2_num_vertices(poly2);
point_t vertices[num_vertices];
int pos = 0, offset = 0;
point2_t* vtx;
while (polygon2_next_vertex(poly2, &pos, &vtx))
plane_embed(plane, vtx, &vertices[offset++]);
#endif
// Read off the vertex ordering from the planar polygon. Note that
// not all of the vertices will be used in general.
int num_p2_points = polygon2_num_vertices(poly2);
int* ordering = polygon2_ordering(poly2);
// Create our polygon with the given ordering.
polygon_t* poly = NULL;
if (num_p2_points < num_points)
{
point_t p2_points[num_p2_points];
for (int i = 0; i < num_p2_points; ++i)
point_copy(&p2_points[i], &points[ordering[i]]);
poly = polygon_new(p2_points, num_p2_points);
}
else
poly = polygon_new_with_ordering(points, ordering, num_points);
// Clean up.
poly2 = NULL;
plane = NULL;
return poly;
}
int Model::face_cb(p_ply_argument argument) {
long length, value_index;
int x,y,z;
ply_get_argument_property(argument, NULL, &length, &value_index);
switch (value_index) {
case 0:
triangles[count_t].x = (int)ply_get_argument_value(argument);
break;
case 1:
triangles[count_t].y = (int)ply_get_argument_value(argument);
break;
case 2:
triangles[count_t].z = (int)ply_get_argument_value(argument);
compute_normal(count_t); // Compute normal after all vertices of traingle are known
count_t++;
break;
default:
break;
}
return 1;
}
void cut_convex_edges(uint *n, uint *ref, egreal *vertex, uint ref_length)
{
uint i, j, edge, v;
egreal vec[3], f;
for(i = 0; i < ref_length; i++)
{
compute_normal(vec, vertex, &ref[i * 3]);
for(j = 0; j < 3; j++)
{
edge = n[i * 3 + j];
v = ((edge / 3) * 3 + ((edge + 2) % 3)) * 3;
f = vec[0] * (vertex[v] - vertex[ref[i * 3] * 3]) +
vec[1] * (vertex[v + 1] - vertex[ref[i * 3] * 3 + 1]) +
vec[2] * (vertex[v + 2] - vertex[ref[i * 3] * 3 + 2]);
if(f < -0.0001)
{
n[n[i * 3 + j]] = -1;
n[i * 3 + j] = -1;
}
}
}
}
/*
* Function which does the job, to be called as the
* "start routine" parameter of pthread_create subroutine.
* Uses the compute_xxx subroutines above.
*
* input parameters ("arg" parameter of pthread_create subroutine):
* pointer to a TH_DATA structure.
*
*/
void *thread_code(void *arg)
{
TH_DATA *th_data = (TH_DATA *) arg;
size_t fsize, fsize2, fsize3;
double *din, *dex, *dex2 = NULL;
int imax, index;
fsize = read_file(th_data->th_func.din_fname, (void **)&din);
if (fsize == (size_t) 0) {
sprintf(th_data->detail_data,
"FAIL: %s: reading %s, %s\n",
th_data->th_func.fident,
th_data->th_func.din_fname, strerror(errno));
th_data->th_result = 1;
SAFE_FREE(din);
pthread_exit((void *)1);
}
fsize2 = read_file(th_data->th_func.dex_fname, (void **)&dex);
if (fsize2 == (size_t) 0) {
sprintf(th_data->detail_data,
"FAIL: %s: reading %s, %s\n",
th_data->th_func.fident,
th_data->th_func.dex_fname, strerror(errno));
th_data->th_result = 1;
SAFE_FREE(din);
SAFE_FREE(dex);
pthread_exit((void *)1);
}
fsize3 = (size_t) 0;
switch (th_data->th_func.code_funct) {
case FUNC_MODF:
case FUNC_FMOD:
case FUNC_POW:
case FUNC_FREXP:
case FUNC_LDEXP:
case FUNC_GAM:
fsize3 = read_file(th_data->th_func.dex2_fname, (void **)&dex2);
if (fsize3 == (size_t) 0) {
sprintf(th_data->detail_data,
"FAIL: %s: reading %s, %s\n",
th_data->th_func.fident,
th_data->th_func.dex2_fname, strerror(errno));
th_data->th_result = 1;
SAFE_FREE(din);
SAFE_FREE(dex);
pthread_exit((void *)1);
}
}
switch (th_data->th_func.code_funct) {
case FUNC_NORMAL:
case FUNC_ATAN2:
case FUNC_HYPOT:
if (fsize2 != fsize)
goto file_size_error;
break;
case FUNC_MODF:
case FUNC_FMOD:
case FUNC_POW:
if (fsize2 != fsize || fsize3 != fsize)
goto file_size_error;
break;
case FUNC_FREXP:
case FUNC_LDEXP:
case FUNC_GAM:
if (fsize2 != fsize ||
(sizeof(double) / sizeof(int)) * fsize3 != fsize)
goto file_size_error;
break;
default:
file_size_error:
sprintf(th_data->detail_data,
"FAIL: %s: file sizes don't match\n",
th_data->th_func.fident);
th_data->th_result = 1;
SAFE_FREE(din);
SAFE_FREE(dex);
if (fsize3)
SAFE_FREE(dex2);
pthread_exit((void *)1);
}
imax = fsize / sizeof(double);
while (th_data->th_nloop <= num_loops) {
/* loop stopped by pthread_cancel */
for (index = th_data->th_num; index < imax; index += num_threads) { /* computation loop */
switch (th_data->th_func.code_funct) {
case FUNC_NORMAL:
compute_normal(th_data, din, dex, index);
break;
case FUNC_ATAN2:
case FUNC_HYPOT:
compute_atan2_hypot(th_data, din, dex, index);
break;
case FUNC_MODF:
compute_modf(th_data, din, dex, dex2, index);
break;
case FUNC_FMOD:
case FUNC_POW:
compute_fmod_pow(th_data,
din, dex, dex2, index);
break;
case FUNC_FREXP:
case FUNC_GAM:
compute_frexp_lgamma(th_data,
din, dex, (int *)dex2,
index);
break;
case FUNC_LDEXP:
compute_ldexp(th_data,
din, dex, (int *)dex2, index);
break;
default:
sprintf(th_data->detail_data,
"FAIL: %s: unexpected function type\n",
th_data->th_func.fident);
th_data->th_result = 1;
SAFE_FREE(din);
SAFE_FREE(dex);
if (fsize3)
SAFE_FREE(dex2);
pthread_exit((void *)1);
}
pthread_testcancel();
} /* end of computation loop */
++th_data->th_nloop;
} /* end of loop */
SAFE_FREE(din);
SAFE_FREE(dex);
if (fsize3)
SAFE_FREE(dex2);
pthread_exit(NULL);
}
glv_object_p glv_object_read(char *filename) {
carmen_FILE *fp;
glv_color_t current_color;
char buffer[10000];
long int nread, log_bytes = 0;
int buffer_pos, buffer_length, offset = 0, n;
glv_object_p obj;
float *fmessage;
int done_reading = 0;
int i, line_count = 0;
float min_x, max_x, min_y, max_y, min_z, max_z;
if(strncmp(filename + strlen(filename) - 4, ".glv", 4) &&
strncmp(filename + strlen(filename) - 7, ".glv.gz", 7) &&
strncmp(filename + strlen(filename) - 5, ".pmap", 5) &&
strncmp(filename + strlen(filename) - 8, ".pmap.gz", 8))
carmen_die("Error: file name must end in .glv, .glv.gz, .pmap, or .pmap.gz\n");
fp = carmen_fopen(filename, "r");
/* compute total number of bytes in logfile */
do {
nread = carmen_fread(buffer, 1, 10000, fp);
log_bytes += nread;
} while(nread > 0);
carmen_fseek(fp, 0L, SEEK_SET);
current_color.r = 255;
current_color.g = 255;
current_color.b = 255;
obj = glv_object_init();
buffer_pos = 0;
buffer_length = carmen_fread(buffer, 1, 10000, fp);
while(!done_reading || buffer_length > buffer_pos) {
line_count++;
if(line_count % 100000 == 0)
fprintf(stderr, "\rReading glv file... (%.0f%%) ",
(offset + buffer_pos) / (float)log_bytes * 100.0);
if(buffer_length - buffer_pos < 50 && !done_reading) {
memmove(buffer, buffer + buffer_pos, buffer_length - buffer_pos);
buffer_length -= buffer_pos;
offset += buffer_pos;
buffer_pos = 0;
n = carmen_fread(buffer + buffer_length,
1, 10000 - buffer_length - 1, fp);
if(n == 0)
done_reading = 1;
else
buffer_length += n;
}
else {
if(buffer[buffer_pos] == MESSAGE_ID_COLOR) {
current_color.r = (unsigned char)buffer[buffer_pos + 1];
current_color.g = (unsigned char)buffer[buffer_pos + 2];
current_color.b = (unsigned char)buffer[buffer_pos + 3];
buffer_pos += 4;
}
else if(buffer[buffer_pos] == MESSAGE_ID_POINT) {
if(obj->num_points == obj->max_points) {
obj->max_points += 100000;
obj->point = (glv_point_p)realloc(obj->point, obj->max_points *
sizeof(glv_point_t));
carmen_test_alloc(obj->point);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->point[obj->num_points].x = fmessage[0];
obj->point[obj->num_points].y = fmessage[1];
obj->point[obj->num_points].z = fmessage[2];
obj->point[obj->num_points].c = current_color;
obj->num_points++;
buffer_pos += 13;
}
else if(buffer[buffer_pos] == MESSAGE_ID_LINE) {
if(obj->num_lines == obj->max_lines) {
obj->max_lines += 100000;
obj->line = (glv_line_p)realloc(obj->line, obj->max_lines *
sizeof(glv_line_t));
carmen_test_alloc(obj->line);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->line[obj->num_lines].p1.x = fmessage[0];
obj->line[obj->num_lines].p1.y = fmessage[1];
obj->line[obj->num_lines].p1.z = fmessage[2];
obj->line[obj->num_lines].p2.x = fmessage[3];
obj->line[obj->num_lines].p2.y = fmessage[4];
obj->line[obj->num_lines].p2.z = fmessage[5];
obj->line[obj->num_lines].c = current_color;
obj->num_lines++;
buffer_pos += 25;
}
else if(buffer[buffer_pos] == MESSAGE_ID_FACE) {
if(obj->num_faces == obj->max_faces) {
obj->max_faces += 100000;
obj->face = (glv_face_p)realloc(obj->face, obj->max_faces *
sizeof(glv_face_t));
carmen_test_alloc(obj->face);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->face[obj->num_faces].p1.x = fmessage[0];
obj->face[obj->num_faces].p1.y = fmessage[1];
obj->face[obj->num_faces].p1.z = fmessage[2];
obj->face[obj->num_faces].p2.x = fmessage[3];
obj->face[obj->num_faces].p2.y = fmessage[4];
obj->face[obj->num_faces].p2.z = fmessage[5];
obj->face[obj->num_faces].p3.x = fmessage[6];
obj->face[obj->num_faces].p3.y = fmessage[7];
obj->face[obj->num_faces].p3.z = fmessage[8];
obj->face[obj->num_faces].c = current_color;
compute_normal(&obj->face[obj->num_faces]);
obj->num_faces++;
buffer_pos += 37;
}
}
}
min_x = 1e10;
max_x = -1e10;
min_y = 1e10;
max_y = -1e10;
min_z = 1e10;
max_z = -1e10;
for(i = 0; i < obj->num_points; i++)
adjust_extrema(obj->point[i], &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
for(i = 0; i < obj->num_lines; i++) {
adjust_extrema(obj->line[i].p1, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->line[i].p2, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
}
for(i = 0; i < obj->num_faces; i++) {
adjust_extrema(obj->face[i].p1, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->face[i].p2, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->face[i].p3, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
}
obj->centroid.x = (min_x + max_x) / 2;
obj->centroid.y = (min_y + max_y) / 2;
obj->centroid.z = (min_z + max_z) / 2;
obj->min.x = min_x;
obj->min.y = min_y;
obj->min.z = min_z;
obj->max.x = max_x;
obj->max.y = max_y;
obj->max.z = max_z;
carmen_fclose(fp);
fprintf(stderr, "\rReading glv file... (100%%) \n");
fprintf(stderr, "%d POINTS - %d LINES - %d FACES\n", obj->num_points,
obj->num_lines, obj->num_faces);
return obj;
}
void Surface::stop_photon(Photon& p)
{
if (p.is_valid()==false)
return;
if(isinf(_dCurvature))
{
p.valid=false;
return;
}
// flat case (intersect with z=0)
if (p.dz==0.)
{
// TODO
p.valid=false;
return;
}
double tmin=-p.z/p.dz;
if(_bIsFlat && (tmin<0.) )
{
// in the past of the photon
p.valid=false;
return;
}
// TODO optimise in case of spherical
// stop the photon in z=0
p.x+=tmin*p.dx;
p.y+=tmin*p.dy;
p.z=0.;
if(_bIsFlat || _bIsPerfect)
{
p.valid=update_auto_diameter(p.x,p.y);
return;
}
//compute the coef of the 2nd degree eq in t:
//the equation is t^2A+tB+C=0
// use p.z=0.
double dA=_dCurvature*(sqr(p.dx)+sqr(p.dy)+(_dConic+1.)*sqr(p.dz));
double dB=2.*(_dCurvature*(p.x*p.dx+p.y*p.dy)-p.dz);
double dC=_dCurvature*(sqr(p.x)+sqr(p.y));
double tfinal;
if (dA==0.)
{
if (dB==0.)
{
p.valid=false;
return;
}
//the equation is now : t*dB+dC=0 so:
tfinal=-dC/dB;
}
else //dA!=0
{
//solve the equation
double dB2=dB*dB;
double delta=dB2-4.*dC*dA;
if (delta<0.)
{
p.valid=false;
return;
}
double t1,t2;
if (delta!=dB2) // TODO enhance test
{
double sqrtDelta=sqrt(delta);
t1=(2.*dC)/(+sqrtDelta-dB);
t2=(2.*dC)/(-sqrtDelta-dB);
}
else
{
// delta~=dB2
// use approximate solution:
if (dB!=0.)
{
t1=-dC/dB;
t2=10.*t1; // to choose t1
}
else
{
p.valid=false; // bug if we are here
return;
}
}
// select t that gives the lowest abs(z)
//t1ok=abs(z+t1.*dz)<abs(z+t2.*dz); //oct ver
if (t1*t1<t2*t2) // todo optimize
tfinal=t1;
else
tfinal=t2;
}
// check if intersection is in the futur of the photon
if ( tmin+tfinal<0 )
{
p.valid=false;
return;
}
p.x+=p.dx*tfinal;
p.y+=p.dy*tfinal;
p.z+=p.dz*tfinal;
assert(p.is_valid());
p.valid=update_auto_diameter(p.x,p.y);
if(!_bIsAspheric)
return;
//aspheric mode
//p.x,p.y,p.z is already a good approximation of the surface (as a conic), but make some newton step
double x=p.x;
double y=p.y;
// double z=p.z;
double dOldT=0;
for(int iLoop=0;iLoop<NB_ITER_STOP_NEWTON;iLoop++)
{
//reproject z on aspheric curve
double zproj;
compute_z(x,y,zproj);
//compute normal on aspheric surface
double nx,ny,nz;
compute_normal(x,y,zproj,nx,ny,nz);
//compute the d value to have the plane x*nx+y*ny+z*nz+d=0
double d=-(x*nx+y*ny+zproj*nz);
//compute the intersect of this plane and the line defined by p (one newton step)
double t=(-d-p.x*nx-p.y*ny-p.z*nz)/(p.dx*nx+p.dy*ny+p.dz*nz); //TODO tester !=0
double distSQ=sqr(t-dOldT)*(sqr(p.dx)+sqr(p.dy)+sqr(p.dz));
x=p.x+t*p.dx;
y=p.y+t*p.dy;
double z=p.z+t*p.dz;
if(distSQ<sqr(RESOLUTION_STOP_NEWTON))
{
p.x=x;
p.y=y;
p.z=z;
p.valid=update_auto_diameter(p.x,p.y);
return;
}
dOldT=t;
}
//too many iterations
p.valid=false;
return;
}
void Surface::reflect_photon(Photon &p)
{
if (!p.is_valid())
return;
stop_photon(p);
if (!p.is_valid())
return;
if(_bIsPerfect)
{
//TODO optimise and merge tests
if(p.dz==0.) //no intersection
{
p.valid=false;
return;
}
//compute AP
if( (_dCurvature<CURVATURE_FLAT) && (_dCurvature>-CURVATURE_FLAT))
{
p.dz=-p.dz;
return; //flat mirror
}
double dFocal=0.5/_dCurvature;
double t=dFocal/p.dz;
double ax=p.dx*t;
double ay=p.dy*t;
double az=p.dz*t;
//TODO optimise tests with sign(t)
if(_dCurvature>0.)
{
if(p.dz>0.)
{
p.dx=+ax+p.x;
p.dy=+ay+p.y;
p.dz=-az-p.z;
}
else
{
p.dx=-ax-p.x;
p.dy=-ay-p.y;
p.dz=+az-p.z;
}
}
else
{
if(p.dz>0.)
{
p.dx=-ax-p.x;
p.dy=-ay-p.y;
p.dz=+az-p.z;
}
else
{
p.dx=+ax+p.x;
p.dy=+ay+p.y;
p.dz=-az-p.z;
}
}
return;
}
double nx,ny,nz;
compute_normal(p.x,p.y,p.z,nx,ny,nz);
// compute with the normal (nx,ny,nz)
double dRayonSq=sqr(nx)+sqr(ny)+sqr(nz);
assert(dRayonSq>0.);
double u=2.*(nx*p.dx+ny*p.dy+nz*p.dz)/dRayonSq;
p.dx-=nx*u;
p.dy-=ny*u;
p.dz-=nz*u;
}
void Surface::transmit_photon(Photon& p)
{
if (!p.is_valid())
return;
stop_photon(p);
if (!p.is_valid())
return;
if(_bIsPerfect)
{
//TODO optimise and merge tests
if(p.dz==0.) //no intersection
{
p.valid=false;
return;
}
//compute AP
if( (_dCurvature<CURVATURE_FLAT) && (_dCurvature>-CURVATURE_FLAT))
return; //flat lens
double dFocal=0.5/_dCurvature;
double t=dFocal/p.dz;
if(p.dz<0.)
t=-t;
double ax=p.dx*t;
double ay=p.dy*t;
double az=p.dz*t;
p.dx=ax-p.x;
p.dy=ay-p.y;
p.dz=az-p.z;
if(dFocal<0.)
{
p.dx=-p.dx;
p.dy=-p.dy;
p.dz=-p.dz;
}
return;
}
assert(_pMaterialNext!=0);
assert(_pMaterialPrev!=0);
double nx,ny,nz;
//compute the normal in p (center distance : ro)
double dRadiusSq=sqr(p.x)+sqr(p.y);
if (dRadiusSq!=0.)
{
compute_normal(p.x,p.y,p.z,nx,ny,nz);
Vector3D::normalize(nx,ny,nz);
}
else // at surface center or flat surface
{
nx=0.;
ny=0.;
nz=1.;
}
double u=nx*p.dx+ny*p.dy+nz*p.dz;
// compute with the normal (dx,dy,dz)
// u is the projection of I on the normal
//calcule de C1=u/I
double dC12=u*u/(sqr(p.dx)+sqr(p.dy)+sqr(p.dz));
double dRatioN=_pMaterialNext->index(p.lambda())/_pMaterialPrev->index(p.lambda()); //TODO store index and reuse
double denom=sqr(dRatioN)+dC12-1.;
if (denom<=0.)
{ // total reflexion
p.valid=false;
return;
}
double dT2T1=sqrt(dC12/denom);
double k=(1.-dT2T1)*u;
p.dx=nx*k+dT2T1*p.dx;
p.dy=ny*k+dT2T1*p.dy;
p.dz=nz*k+dT2T1*p.dz;
}
/**
* @brief get_clip_points
* @param ray_
* @param ipoints
* @param t
* @return the number of clipping position on the ray (0 to 2)
*/
int get_clip_points(Ray& ray_, Diff_Geom ipoints[2], float t[2]) const{
int numintersection = 0;
Vector3D E = ray_.ori() - m_vertex;
float AdD = m_axis.dot(ray_.dir());
float cosSqr = m_costheta*m_costheta;
float AdE = m_axis.dot(E);
float DdE = ray_.dir().dot(E);
float EdE = E.dot(E);
float c2 = AdD*AdD-cosSqr;
float c1 = AdD*AdE - cosSqr*DdE;
float c0 = AdE*AdE - cosSqr*EdE;
float dot;
if (std::fabs(c2)>0) {
float discr = c1*c1 - c0*c2;
float invC2 = 1.f/c2;
if (discr < 0) {
// No intersection
return 0;
} else if (discr > 0) {
// two distinct intersections
float root = std::sqrt(discr);
// entry point
t[numintersection] = (-c1 + root) * invC2;
ipoints[numintersection].set_pos(ray_.at(t[numintersection]));
E = ipoints[numintersection].pos() - m_vertex;
dot = E.dot(m_axis);
if ( (dot > 0) && (dot <= m_height))
{
Vector3D normal=compute_normal(ipoints[numintersection].pos());
ipoints[numintersection].set_normal(normal);
ipoints[numintersection].set_t(t[numintersection]);
++numintersection;
}
// exit point
t[numintersection] = (-c1 - root) * invC2;
ipoints[numintersection].set_pos(ray_.at(t[numintersection]));
E = ipoints[numintersection].pos() - m_vertex;
dot = E.dot(m_axis);
if ( (dot > 0) && (dot <= m_height))
{
Vector3D normal=compute_normal(ipoints[numintersection].pos());
ipoints[numintersection].set_normal(normal);
ipoints[numintersection].set_t(t[numintersection]);
++numintersection;
}
return numintersection;
} else {
// one reapeated intersection : ray is tangent to the cone
// may be return 0 instead of an intersection ?
return 0;
t[numintersection] = -c1 * invC2;
ipoints[numintersection].set_pos(ray_.at(t[numintersection]));
E = ipoints[numintersection].pos() - m_vertex;
dot = E.dot(m_axis);
if ( (dot > 0) && (dot <= m_height)) {
Vector3D normal=compute_normal(ipoints[numintersection].pos());
ipoints[numintersection].set_normal(normal);
ipoints[numintersection].set_t(t[numintersection]);
++numintersection;
}
return numintersection;
}
} else if (std::fabs(c1) > 0) {
// the ray is on the boundary of the cone
// we consider no intersection
// TODO : check this for CSG
return 0;
} else {
//return false;
// Cone contains ray V+tD
// The ray intersect the cone exactly at its vertex :(
ipoints[numintersection].set_pos(m_vertex);
ipoints[numintersection].set_normal(-m_axis);
E = ray_.ori() - m_vertex;
t[numintersection] = ( (E.dot(ray_.dir())<0) ? std::sqrt(EdE) : -std::sqrt(EdE) ) ;
ipoints[numintersection].set_t(t[numintersection]);
++numintersection;
// TODO compute cap plane intersection
// check with cap plane
Plane cap(m_vertex + m_axis * m_height, m_axis);
IntervalSet capset;
if (cap.clip(ray_, capset)) {
if (capset.bounds()[0].t < t[numintersection-1]) {
t[numintersection] = t[numintersection-1];
ipoints[numintersection] = ipoints[numintersection-1];
--numintersection;
}
ipoints[numintersection] = *(capset.bounds()[0].data);
delete capset.bounds()[0].data;
delete capset.bounds()[1].data;
return 2;
} else {
// must never reach this point !
assert(false);
return 0;
}
}
}
bool ConcavePartTorus::hit(const Ray& ray, double& tmin, ShadeRec& sr) const {
if (!bbox.hit(ray))
return false;
double x1 = ray.o.x; double y1 = ray.o.y; double z1 = ray.o.z;
double d1 = ray.d.x; double d2 = ray.d.y; double d3 = ray.d.z;
double coeffs[5]; // coefficient array for the quartic equation
double roots[4]; // solution array for the quartic equation
// define the coefficients of the quartic equation
double sum_d_sqrd = d1 * d1 + d2 * d2 + d3 * d3;
double e = x1 * x1 + y1 * y1 + z1 * z1 - a * a - b * b;
double f = x1 * d1 + y1 * d2 + z1 * d3;
double four_a_sqrd = 4.0 * a * a;
coeffs[0] = e * e - four_a_sqrd * (b * b - y1 * y1); // constant term
coeffs[1] = 4.0 * f * e + 2.0 * four_a_sqrd * y1 * d2;
coeffs[2] = 2.0 * sum_d_sqrd * e + 4.0 * f * f + four_a_sqrd * d2 * d2;
coeffs[3] = 4.0 * sum_d_sqrd * f;
coeffs[4] = sum_d_sqrd * sum_d_sqrd; // coefficient of t^4
// find roots of the quartic equation
int num_real_roots = SolveQuartic(coeffs, roots);
bool intersected = false;
double t = kHugeValue;
if (num_real_roots == 0) // ray misses the torus
return false;
// find the smallest root greater than kEpsilon, if any
// the roots array is not sorted
for (int j = 0; j < num_real_roots; j++) {
if (roots[j] > kEpsilon) {
if (roots[j] < t) {
Vector3D hit = ray.o + roots[j] * ray.d;
double phi = atan2(hit.x, hit.z);
if (phi < 0.0)
phi += TWO_PI;
double theta = atan2(hit.y, sqrt(hit.x * hit.x + hit.z * hit.z) - a);
if (theta < 0.0)
theta += TWO_PI;
bool good_theta;
if (theta_max < theta_min) {
good_theta = ((theta_min <= theta && theta <= 360)
|| (0 <= theta && theta <= theta_max));
} else {
good_theta = (theta_min <= theta && theta <= theta_max);
}
if (phi_min <= phi && phi <= phi_max && good_theta) {
intersected = true;
t = roots[j];
}
}
}
}
if(!intersected)
return false;
tmin = t;
sr.local_hit_point = ray.o + t * ray.d;
sr.normal = compute_normal(sr.local_hit_point);
return true;
}
static void polygon_compute_plane(polygon_t* poly)
{
compute_normal(poly->vertices, poly->num_vertices, &poly->normal);
compute_centroid(poly->vertices, poly->num_vertices, &poly->x0);
poly->plane = plane_new(&poly->normal, &poly->x0);
}
static void read_face (FILE* file,
char clockwise,
vector* vertices,
vector* normals,
vector* triangles)
{
vector idxs;
vector texCoords;
vector_new (&idxs, sizeof(int), 4);
vector_new (&texCoords, sizeof(int), 4);
unsigned int index;
unsigned int normal;
unsigned int texc;
fscanf (file, "f");
while (!feof(file) && fscanf(file, "%d", &index) > 0)
{
vector_append (&idxs, &index);
if (fgetc (file) == '/')
{
fscanf (file, "%d", &texc);
vector_append (&texCoords, &texc);
}
else fseek (file, -1, SEEK_CUR);
if (fgetc (file) == '/')
{
fscanf (file, "%d", &normal);
}
else fseek (file, -1, SEEK_CUR);
}
// Triangulate the face and add its triangles to the triangles vector.
triangle tri;
tri.vertexIndices[0] = *((int*) vector_ith (&idxs, 0)) - 1;
tri.textureIndices[0] = *((int*) vector_ith (&texCoords, 0)) - 1;
int i;
for (i = 1; i < vector_size(&idxs)-1; i++)
{
tri.vertexIndices[1] = *((int*) vector_ith (&idxs, i)) - 1;
tri.textureIndices[1] = *((int*) vector_ith (&texCoords, i)) - 1;
tri.vertexIndices[2] = *((int*) vector_ith (&idxs, i+1)) - 1;
tri.textureIndices[2] = *((int*) vector_ith (&texCoords, i+1)) - 1;
vector_append (triangles, &tri);
}
// Compute face normal and add contribution to each of the face's vertices.
unsigned int i0 = tri.vertexIndices[0];
unsigned int i1 = tri.vertexIndices[1];
unsigned int i2 = tri.vertexIndices[2];
vec3 n;
vec3 v0 = *((vec3*) vector_ith (vertices, i0));
vec3 v1 = *((vec3*) vector_ith (vertices, i1));
vec3 v2 = *((vec3*) vector_ith (vertices, i2));
compute_normal (clockwise, v0, v1, v2, &n);
for (i = 0; i < vector_size (&idxs); i++)
{
int j = *((int*) vector_ith (&idxs, i)) - 1;
vec3* normal = (vec3*) vector_ith (normals, j);
vec3_add (n, normal);
}
vector_free (&idxs);
vector_free (&texCoords);
}
float TorusObject::hit_test(const Ray &ray, Vector &normal, const Point *max_pos, bool *inside)
{
Ray inv_ray;
inv_ray.origin = mul_point(inv_trans, ray.origin);
inv_ray.direction = mul_vec(inv_trans, ray.direction);
inv_ray.direction.normalize();
double x1 = inv_ray.origin.x; double y1 = inv_ray.origin.y; double z1 = inv_ray.origin.z;
double d1 = inv_ray.direction.x; double d2 = inv_ray.direction.y; double d3 = inv_ray.direction.z;
double coeffs[5]; // coefficient array
double roots[4]; // solution array
//define the coefficients
double sum_d_sqrd = d1*d1 + d2*d2 + d3*d3;
double e = x1*x1 + y1*y1 + z1*z1 - radius*radius - thickness*thickness;
double f = x1*d1 + y1*d2 + z1*d3;
double four_a_sqrd = 4.0 * radius*radius;
coeffs[0] = e*e - four_a_sqrd * (thickness*thickness-y1*y1); // constante term
coeffs[1] = 4.0 * f * e + 2.0 * four_a_sqrd * y1 *d2;
coeffs[2] = 2.0 * sum_d_sqrd * e + 4.0 * f * f + four_a_sqrd * d2 * d2;
coeffs[3] = 4.0 * sum_d_sqrd * f;
coeffs[4] = sum_d_sqrd * sum_d_sqrd; // coefficient of t^4
//fin the roots
int num_real_roots = solveQuartic(coeffs, roots);
bool intersected = false;
double t = FLT_MAX;
if ( num_real_roots == 0) // ray misses the torus
return -1.0;
//find the smallest root greater than FLT_EPSILON, if any
for( int j = 0; j < num_real_roots; j++)
{
if(roots[j] > FLT_EPSILON)
{
intersected = true;
if(roots[j] < t)
t = roots[j];
}
}
double max_t;
if(max_pos)
max_t = (max_pos->x - ray.origin.x) / ray.direction.x;
else
max_t = FLT_MAX;
if( t > max_t || !intersected)
return -1.0;
Point hit = inv_ray.origin + t*inv_ray.direction;
normal = compute_normal(hit);
normal = mul_vec(inv_trans, normal);
normal.normalize();
return t;
}
//---------------------------------------------------------------
void testApp::update() {
ofBackground(100, 100, 100);
NUI_IMAGE_FRAME pImageFrame = {0};
NUI_LOCKED_RECT locked_rect = {0};
if(bKinectInitSucessful)
{
HRESULT hr=m_nui->NuiImageStreamGetNextFrame(depthStreamHandle,0,&pImageFrame);
if (SUCCEEDED(hr))
{
hr=pImageFrame.pFrameTexture->LockRect(0,&locked_rect,NULL,0);
if(locked_rect.Pitch!=0) {
// 将数据存入Mat中
cv::Mat depthimage(KINECT_HEIGHT,KINECT_WIDTH,CV_8UC1);
//cv::Mat depthimage2(KINECT_HEIGHT,KINECT_WIDTH,CV_8UC1);
if(test)
{
for (int i=0; i<depthimage.rows; i++)
{
uchar *ptr = depthimage.ptr<uchar>(i); //第i行的指针
//uchar *ptr2 = depthimage2.ptr<uchar>(i);
uchar *pBufferRun = (uchar*)(locked_rect.pBits) + i * locked_rect.Pitch;
USHORT *pBuffer = (USHORT*) pBufferRun;
for (int j=0; j<depthimage.cols; j++)
{
pBuffer[j]=pBuffer[j]>>3;
ptr[j]=(uchar)(256*pBuffer[j]/0x0fff); //直接将数据归一化处理
//ptr2[j]=(uchar)(256*depth_float[i*depthimage.cols+j]/0x0fff);
}
}
//test=false;
}
cv::imshow("orginal",depthimage);
//cv::imshow("sss",depthimage2);
cv::Mat depthimage_filter = depthimage.clone();
// 读取照片
#if 0
const int photonumMax = 1;
if(photonum < photonumMax)
{
char namebuffer[20];
sprintf(namebuffer,"%d.jpg",photonum);
string name = namebuffer;
depthimage_filter = cv::imread(name,CV_LOAD_IMAGE_GRAYSCALE);
++photonum;
}
#endif
// 滤波 双边滤波效果没出现（？）
//cv::medianBlur(depthimage,depthimage_filter,5);
//clock_t t1=clock();
cv::bilateralFilter(depthimage,depthimage_filter,11,20,20);
//clock_t t2=clock();
//double ti=(double)(t2-t1)/CLOCKS_PER_SEC;
//cout<<ti<<endl;
//cv::GaussianBlur(depthimage,depthimage_filter,cv::Size(5,5),0,0);
//cv::imshow("filter",depthimage_filter);
// 保存照片
#if 0
if(photonum < photonumMax)
{
char namebuffer[20];
sprintf(namebuffer,"%d.jpg",photonum);
string name = namebuffer;
cv::imwrite(name,depthimage);
++photonum;
}
#endif
// pyramid creat 高斯降采 论文中提到避免边界被平滑未处理（？）
//cv::Mat downmap1;
//cv::pyrDown(depthimage_filter,downmap1,cv::Size(depthimage_filter.cols/2,depthimage_filter.rows/2));
//cv::imshow("downonce",downmap1);
//cv::Mat downmap2;
//cv::pyrDown(downmap1,downmap2,cv::Size(downmap1.cols/2,downmap1.rows/2));
//cv::imshow("downtwice",downmap2);
// 滤波后深度数据提取与单位转换
//ofstream out;
//if(photonum < photonumMax&&test3)
//{
// char namebuffer[10];
// sprintf(namebuffer,"%d.txt",photonum);
// out.open(namebuffer,ios::trunc);
//}
for (int i=0; i<depthimage_filter.rows; i++)
{
uchar *ptr = depthimage_filter.ptr<uchar>(i); //第i行的指针
for (int j=0; j<depthimage_filter.cols; j++)
{
float s = (float)(ptr[j]*0x0fff)/256;
//if(s>2500&&s<2600)
//{
// cout<<s<<endl;
//}
depth_float[i*depthimage_filter.cols+j] = s/1000;//显示点云用，计算时要除1000，化为米单位
//if(test3)
// out<<s<<" ";
}
}
//out.close();
test3 = false;
// 试验区
ofMatrix4x4 tk=ofMatrix4x4(1,0,0,0,0,-1,0,0,0,0,1,0,0,0,0,1);
ofMatrix4x4 tk_next = tk;
ofVec4f camp = ofVec4f(0,0,0,1);
int size_g = TSDF_SIZE;
const int maxcountnum = 3;
if(countnum < maxcountnum)
{
//compute_tsdf(tk,depthimage,size_g,camp);
++ countnum;
if(countnum == maxcountnum)
test2 = true;
//}
//if(test2)
//{
//cv::imwrite("original.jpg",depthimage_filter);
//cout<<tk<<endl;
clock_t t1=clock();
compute_points(depthimage_filter,depthimage_filter.rows,depthimage_filter.cols,pointsmap_orignal);
compute_tsdf(tk_next,depthimage,size_g,camp);
compute_raycast(tk_next);
compute_normal(pointsmap_orignal,depthimage_filter.rows,depthimage_filter.cols,normalmap_orignal);
//compute_normal(pointsmap_final,depthimage_filter.rows,depthimage_filter.cols,normalmap_final);
for(int i = 0;i < 10; ++ i)
{
compute_pda(tk,i,tk_next);
}
camp = tk_next * camp;
tk = tk_next;
//changePosition(tk_next,pointsmap_orignal,normalmap_orignal);
clock_t t2=clock();
double ti=(double)(t2-t1)/CLOCKS_PER_SEC;
std::cout<<ti<<endl;
//test2=false;
}
// 转换坐标系为摄像机坐标系与法向
//changeType(depthimage_filter,depthimage_filter.rows,depthimage_filter.cols);//无用了
//compute_points(depthimage_filter,depthimage_filter.rows,depthimage_filter.cols,pointsmap_orignal);
//compute_points(downmap1,downmap1.rows,downmap1.cols,pointsmap_downonce);
//compute_points(downmap2,downmap2.rows,downmap2.cols,pointsmap_downtwice);
//
//compute_normal(pointsmap_orignal,depthimage_filter.rows,depthimage_filter.cols,normalmap_orignal);
//compute_normal(pointsmap_downonce,downmap1.rows,downmap1.cols,normalmap_downonce);
//compute_normal(pointsmap_downtwice,downmap2.rows,downmap2.cols,normalmap_downtwice);
if(bThreshWithOpenCV) {
} else {
}
}
m_nui->NuiImageStreamReleaseFrame(depthStreamHandle,&pImageFrame);
}
}