/* Find the closest point in list to point p, return NULL if list empty */
int point_list_closest(struct point_list *l, struct point *p)
{
assert(l != NULL);
assert(p != NULL);
int i;
int length = point_list_length(l);
double min_d, temp_d;
int min_i;
if (length <= 0)
return -1;
min_i = 0;
min_d = point_distance(&(l->plist[0]), p);
for (i = 1; i < length; i++) {
temp_d = point_distance(&(l->plist[i]), p);
if (temp_d < min_d) {
min_d = temp_d;
min_i = i;
}
}
return min_i;
}
double *dist_ps(Point *pt, LSEG *lseg)
{
double m; /* slope of perp. */
LINE *ln;
double *result, *tmpdist;
Point *ip;
/* construct a line that's perpendicular. See if the intersection of
the two lines is on the line segment. */
if (lseg->p[1].x == lseg->p[0].x)
m = 0;
else if (lseg->p[1].y == lseg->p[0].y) /* slope is infinite */
m = (double)DBL_MAX;
else m = (-1) * (lseg->p[1].y - lseg->p[0].y) /
(lseg->p[1].x - lseg->p[0].x);
ln = line_construct_pm(pt, m);
if ((ip = interpt_sl(lseg, ln)) != NULL)
result = point_distance(pt, ip);
else /* intersection is not on line segment, so distance is min
of distance from point to an endpoint */
{
result = point_distance(pt, &lseg->p[0]);
tmpdist = point_distance(pt, &lseg->p[1]);
if (*tmpdist < *result) *result = *tmpdist;
PFREE (tmpdist);
}
if (ip != NULL) PFREE(ip);
PFREE(ln);
return (result);
}
int main(int argc, char **argv)
{
Point p1;
Point *p2 = malloc(sizeof(Point));
assert(p2);
double distance;
point_set(&p1, 1.0, 1.0);
point_set(p2, 1.0, 1.0);
distance = point_distance(&p1, p2);
point_translate(&p1, 1.0, 0.0);
distance = point_distance(&p1, p2);
point_set(&p1, 0.0, 0.0);
point_set(p2, 3.0, 4.0);
distance = point_distance(&p1, p2);
printf("%f\n", distance);
free(p2);
p2 = NULL;
printf("OK\n");
return 0;
}
int main(int argc, char **argv)
{
struct point p1, *p2;
point_set(&p1, 1.0, 1.0);
p2 = malloc(sizeof(struct point));
assert(p2);
point_set(p2, 1.0, 1.0);
assert(point_distance(&p1, p2) == 0.0);
point_translate(&p1, 1.0, 0.0);
assert(point_distance(&p1, p2) == 1.0);
point_set(&p1, 0.0, 0.0);
point_set(p2, 3.0, 4.0);
assert(point_distance(&p1, p2) == 5.0);
free(p2);
p2 = NULL;
printf("OK\n");
return 0;
}
/*
* Allocate a new point and initialize it to x,y. Then
* add that point to the SortedPoints list. Return
* 1 on success and 0 on error (e.g., out of memory).
*/
int sp_addNewPoint(SortedPoints *sp, double x, double y)
{
Point ORIGIN;
ORIGIN.x = 0;
ORIGIN.y = 0;
// Local iterator node
Node *curr;
// Allocate memory for the new node
Node *n;
n = (Node *)malloc(sizeof(Node));
if (!n) return 0; // malloc() returns a NULL pointer if not enough memory.
// Initializing the new node 'n'
n->p.x = x;
n->p.y = y;
// Inserting the new node in the correct place in the linked list
// If the list is empty, add the point as the head.
if (sp->head == NULL) {
sp->head = n;
return 1;
}
curr = sp->head;
while (curr != NULL) {
if (point_distance(&n->p, &ORIGIN) < point_distance(&curr->p, &ORIGIN)) {
insertBefore(sp, n, curr);
return 1;
}
if(point_distance(&n->p, &ORIGIN) == point_distance(&curr->p, &ORIGIN)) {
if(n->p.x < curr->p.x) {
insertBefore(sp, n, curr);
return 1;
}
if(n->p.x == curr->p.x && n->p.y < curr->p.y) {
insertBefore(sp, n, curr);
return 1;
}
}
curr = curr->next;
}
// If we reach this point, n is not less than any of the other nodes and
// we add it to the end of the list
curr = sp->head;
while (curr->next != NULL) curr = curr->next;
insertAfter(n, curr);
return 1;
}
static void QuadTree_get_nearest_internal(QuadTree qt, real *x, real *y, real *min, int *imin, int tentative, int *flag){
/* get the narest point years to {x[0], ..., x[dim]} and store in y.*/
SingleLinkedList l;
real *coord, dist;
int dim, i, iq = -1;
real qmin;
real *point = x;
*flag = 0;
if (!qt) return;
dim = qt->dim;
l = qt->l;
if (l){
while (l){
coord = node_data_get_coord(SingleLinkedList_get_data(l));
dist = point_distance(point, coord, dim);
if(*min < 0 || dist < *min) {
*min = dist;
*imin = node_data_get_id(SingleLinkedList_get_data(l));
for (i = 0; i < dim; i++) y[i] = coord[i];
}
l = SingleLinkedList_get_next(l);
}
}
if (qt->qts){
dist = point_distance(qt->center, point, dim);
if (*min >= 0 && (dist - sqrt((real) dim) * qt->width > *min)){
return;
} else {
if (tentative){/* quick first approximation*/
qmin = -1;
for (i = 0; i < 1<<dim; i++){
if (qt->qts[i]){
dist = point_distance(qt->qts[i]->average, point, dim);
if (dist < qmin || qmin < 0){
qmin = dist; iq = i;
}
}
}
assert(iq >= 0);
QuadTree_get_nearest_internal(qt->qts[iq], x, y, min, imin, tentative, flag);
} else {
for (i = 0; i < 1<<dim; i++){
QuadTree_get_nearest_internal(qt->qts[i], x, y, min, imin, tentative, flag);
}
}
}
}
}
double triangle_area(triangle_3D triangle)
{
double a_length, b_length, gamma;
point_3D a_vector, b_vector;
a_length = point_distance(triangle.a,triangle.b);
b_length = point_distance(triangle.a,triangle.c);
substract_vectors(triangle.b,triangle.a,a_vector);
substract_vectors(triangle.c,triangle.a,b_vector);
gamma = vectors_angle(a_vector,b_vector);
return 1/2.0 * a_length * b_length * sin(gamma);
}
void mesh_3D::update_bounding_sphere()
{
unsigned int i;
double distance;
this->bounding_sphere_center.x = 0;
this->bounding_sphere_center.y = 0;
this->bounding_sphere_center.z = 0;
for (i = 0; i < this->vertices.size(); i++)
{
bounding_sphere_center.x += this->vertices[i].position.x;
bounding_sphere_center.y += this->vertices[i].position.y;
bounding_sphere_center.z += this->vertices[i].position.z;
}
bounding_sphere_center.x /= this->vertices.size();
bounding_sphere_center.y /= this->vertices.size();
bounding_sphere_center.z /= this->vertices.size();
this->bounding_sphere_radius = 0;
for (i = 0; i < this->vertices.size(); i++)
{
distance = point_distance(this->vertices[i].position,this->bounding_sphere_center);
if (distance > this->bounding_sphere_radius)
this->bounding_sphere_radius = distance;
}
}
/*
** Distance from a point to a path
*/
double *dist_ppth(Point *pt, PATH *path)
{
double *result;
double *tmp;
int i;
LSEG lseg;
switch (path->npts) {
case 0:
result = PALLOCTYPE(double);
*result = Abs((double) DBL_MAX); /* +infinity */
break;
case 1:
result = point_distance(pt, &path->p[0]);
break;
default:
/*
* the distance from a point to a path is the smallest distance
* from the point to any of its constituent segments.
*/
Assert(path->npts > 1);
result = PALLOCTYPE(double);
for (i = 0; i < path->npts - 1; ++i) {
statlseg_construct(&lseg, &path->p[i], &path->p[i+1]);
tmp = dist_ps(pt, &lseg);
if (i == 0 || *tmp < *result)
*result = *tmp;
PFREE(tmp);
}
break;
}
return(result);
}
void QuadTree_get_supernodes_internal(QuadTree qt, real bh, real *point, int nodeid, int *nsuper, int *nsupermax, real **center, real **supernode_wgts, real **distances, real *counts, int *flag){
SingleLinkedList l;
real *coord, dist;
int dim, i;
(*counts)++;
if (!qt) return;
dim = qt->dim;
l = qt->l;
if (l){
while (l){
check_or_realloc_arrays(dim, nsuper, nsupermax, center, supernode_wgts, distances);
if (node_data_get_id(SingleLinkedList_get_data(l)) != nodeid){
coord = node_data_get_coord(SingleLinkedList_get_data(l));
for (i = 0; i < dim; i++){
(*center)[dim*(*nsuper)+i] = coord[i];
}
(*supernode_wgts)[*nsuper] = node_data_get_weight(SingleLinkedList_get_data(l));
(*distances)[*nsuper] = point_distance(point, coord, dim);
(*nsuper)++;
}
l = SingleLinkedList_get_next(l);
}
}
if (qt->qts){
dist = point_distance(qt->center, point, dim);
if (qt->width < bh*dist){
check_or_realloc_arrays(dim, nsuper, nsupermax, center, supernode_wgts, distances);
for (i = 0; i < dim; i++){
(*center)[dim*(*nsuper)+i] = qt->average[i];
}
(*supernode_wgts)[*nsuper] = qt->total_weight;
(*distances)[*nsuper] = point_distance(qt->average, point, dim);
(*nsuper)++;
} else {
for (i = 0; i < 1<<dim; i++){
QuadTree_get_supernodes_internal(qt->qts[i], bh, point, nodeid, nsuper, nsupermax, center,
supernode_wgts, distances, counts, flag);
}
}
}
}
bool CartesianPlane::point_in_plane(Point3D pt) {
if (point_distance(pt) < 1e-12) {
return true; }
else {
return false;
};
};
double *dist_pb(Point *pt, BOX *box)
{
Point *tmp;
double *result;
tmp = close_pb(pt, box);
result = point_distance(tmp, pt);
PFREE(tmp);
return(result);
}
bool CHARACTER::is_hitting_door()
{
bool done = false;
for(int i = 0; i < 16 && !done; i++)
{
if(!game.controller->doors[i].valid)
continue;
if(!game.controller->get_door(i))
continue;
for(int j = 0; j < game.controller->doors[i].pos_count-1; j++)
{
if(point_distance(core.pos, game.controller->doors[i].pos[j], game.controller->doors[i].pos[j+1]) < 30.0f)
{
done = true;
break;
}
}
}
return done;
}
bool scene_3D::cast_shadow_ray(point_3D position, light_3D light, double threshold, double range)
{
unsigned int i, j;
triangle_3D triangle;
double a,b,c,t,distance;
point_3D intersection;
point_3D light_position;
light_position = light.get_position();
light_position.x += random_double() * range;
light_position.y += random_double() * range;
light_position.z += random_double() * range;
line_3D line(position,light_position);
for (i = 0; i < this->meshes.size(); i++)
{
if (!line.intersects_sphere(this->meshes[i]->bounding_sphere_center,this->meshes[i]->bounding_sphere_radius))
continue;
for (j = 0; j < this->meshes[i]->triangle_indices.size(); j += 3)
{
triangle.a = this->meshes[i]->vertices[this->meshes[i]->triangle_indices[j]].position;
triangle.b = this->meshes[i]->vertices[this->meshes[i]->triangle_indices[j + 1]].position;
triangle.c = this->meshes[i]->vertices[this->meshes[i]->triangle_indices[j + 2]].position;
if (line.intersects_triangle(triangle,a,b,c,t))
{
line.get_point(t,intersection);
distance = point_distance(position,intersection);
if (distance > threshold)
{
return false;
}
}
}
}
return true;
}
static void cluster_points(array_t *clusters, array_t *points)
{
int n = array_length(points);
int k = array_length(clusters);
for (int i=0; i<n; i++) {
float distance = FLT_MAX;
cluster_t *cl_nearest = NULL;
point_t *p = array_at(points, i);
for (int j=0; j<k; j++) {
cluster_t *cl = array_at(clusters, j);
float d = point_distance(p, &cl->centroid);
if (d < distance) {
distance = d;
cl_nearest = cl;
}
}
if (cl_nearest == NULL) {
__asm__ volatile ("BKPT");
}
/* copy point and add to cluster */
array_push_back(cl_nearest->points, p);
}
int sp_addNewPoint(SortedPoints *sp, double x, double y)
{
if (sp->size == 10)
{
printf("SortedPoint list full\n");
return 0;
}
else if (sp->size > 10)
{
printf("List above 10 for some reason, error\n");
return 0;
}
else
{
Point *originPointer = &originPoint;
Point *newPoint = (Point*)malloc(sizeof(Point));
point_set(newPoint, x, y);
int position;
int placePos;
Point tempPoint;
Point *tempPointer;
for (position = 0; position < 10; position++)
{
if (sp->used[position] == 'n')
{
sp->pointArray[position] = *newPoint;
sp->used[position] = 'y';
break;
}
else
{
tempPoint = sp->pointArray[position];
tempPointer = &tempPoint;
if (point_distance(originPointer, newPoint) == point_distance(originPointer, tempPointer))
{
if (point_getX(newPoint) == point_getX(tempPointer))
{
if (point_getY(newPoint) < point_getY(tempPointer))
{
placePos = position;
//shift everything else over
sp_shiftPoints(sp, position);
//this position holds newPoint
sp->pointArray[placePos] = *newPoint;
break;
}
}
else if (point_getX(newPoint) < point_getX(tempPointer))
{
placePos = position;
//shift everything else over
sp_shiftPoints(sp, position);
//this position holds newPoint
sp->pointArray[placePos] = *newPoint;
break;
}
}
else if (point_distance(originPointer, newPoint) < point_distance(originPointer, tempPointer))
{
placePos = position;
//shift everything else over
sp_shiftPoints(sp, position);
//this position holds newPoint
sp->pointArray[placePos] = *newPoint;
break;
}
}
}
free(newPoint);
//free(originPointer);
sp->used[sp->size] = 'y';
sp->size = sp->size + 1;
return 1;
}
}
neighbor_pairing_t* distance_based_neighbor_pairing_new(point_cloud_t* points,
real_t* R,
int* num_ghost_points)
{
ASSERT(R != NULL);
ASSERT(num_ghost_points != NULL);
#ifndef NDEBUG
for (int i = 0; i < points->num_points; ++i)
ASSERT(R[i] > 0.0);
#endif
// Stick all the points into a kd-tree so that we can pair them up.
kd_tree_t* tree = kd_tree_new(points->points, points->num_points);
// Find the maximum radius of interaction.
real_t R_max = -FLT_MAX;
for (int i = 0; i < points->num_points; ++i)
R_max = MAX(R_max, R[i]);
// Add ghost points to the kd-tree and fetch an exchanger. This may add
// too many ghost points, but hopefully that won't be an issue.
exchanger_t* ex = kd_tree_find_ghost_points(tree, points->comm, R_max);
// We'll toss neighbor pairs into this expandable array.
int_array_t* pair_array = int_array_new();
for (int i = 0; i < points->num_points; ++i)
{
// Find all the neighbors for this point. We only count those
// neighbors {j} for which j > i.
point_t* xi = &points->points[i];
int_array_t* neighbors = kd_tree_within_radius(tree, xi, R_max);
for (int k = 0; k < neighbors->size; ++k)
{
int j = neighbors->data[k];
if (j > i)
{
real_t D = point_distance(xi, &points->points[j]);
if (D < MAX(R[i], R[j]))
{
int_array_append(pair_array, i);
int_array_append(pair_array, j);
}
}
}
int_array_free(neighbors);
}
// Create a neighbor pairing.
int num_pairs = pair_array->size/2;
neighbor_pairing_t* neighbors =
unweighted_neighbor_pairing_new("Distance-based point pairs",
num_pairs, pair_array->data, ex);
// Set the number of ghost points referred to within the neighbor pairing.
*num_ghost_points = kd_tree_size(tree) - points->num_points;
// Clean up.
int_array_release_data_and_free(pair_array); // Release control of data.
kd_tree_free(tree);
return neighbors;
}
VLD::VLD(const ImageScale& series, T const& P1, T const& P2) : contrast(0.0) {
//============== initializing============//
principleAngle.fill(0);
descriptor.fill(0);
weight.fill(0);
begin_point[0]=P1.x;
begin_point[1]=P1.y;
end_point[0]=P2.x;
end_point[1]=P2.y;
float dy= float(end_point[1]- begin_point[1]), dx= float(end_point[0]- begin_point[0]);
distance=sqrt(dy*dy+dx*dx);
if (distance==0)
std::cerr<<"Two SIFT points have the same coordinate"<<std::endl;
float radius=std::max(distance/float(dimension+1), 2.0f);//at least 2
double mainAngle= get_orientation();//absolute angle
int image_index=series.getIndex(radius);
const Image<float> & ang = series.angles[image_index];
const Image<float> & m = series.magnitudes[image_index];
double ratio=series.ratios[image_index];
// std::cout<<std::endl<<"index of image "<<radius<<" "<<image_index<<" "<<ratio<<std::endl;
int w=m.Width() ,h=m.Height();
float r=float(radius/ratio);//series.radius_size;
float sigma2=r*r;
//======Computing the descriptors=====//
for (int i=0;i<dimension; i++){
double statistic[binNum];
std::fill_n(statistic, binNum, 0.0);
float xi= float(begin_point[0]+ float(i+1)/(dimension+1)*(dx));
float yi= float(begin_point[1]+ float(i+1)/(dimension+1)*(dy));
yi/=float(ratio);
xi/=float(ratio);
for (int y=int(yi-r);y<=int(yi+r+0.5);y++){
for (int x=int(xi-r);x<=int(xi+r+0.5);x++){
float d=point_distance(xi,yi,float(x),float(y));
if (d<=r && inside(w,h,x,y,1)){
//================angle and magnitude==========================//
double angle;
if (ang(y,x)>=0)
angle=ang(y,x)-mainAngle;//relative angle
else angle=0.0;
//cout<<angle<<endl;
while (angle<0)
angle +=2*PI;
while (angle>=2*PI)
angle -=2*PI;
//===============principle angle==============================//
int index=int(angle*binNum/(2*PI)+0.5);
double Gweight=exp(-d*d/4.5/sigma2)*(m(y,x));
// std::cout<<"in number "<<image_index<<" "<<x<<" "<<y<<" "<<m(y,x)<<std::endl;
if (index<binNum)
statistic[index]+=Gweight;
else // possible since the 0.5
statistic[0]+=Gweight;
//==============the descriptor===============================//
int index2=int(angle*subdirection/(2*PI)+0.5);
assert(index2>=0 && index2<=subdirection);
if (index2<subdirection)
descriptor[subdirection*i+index2]+=Gweight;
else descriptor[subdirection*i]+=Gweight;// possible since the 0.5
}
}
}
//=====================find the biggest angle of ith SIFT==================//
int index,second_index;
max(statistic,weight[i],binNum,index,second_index);
principleAngle[i]=index;
}
normalize_weight(descriptor);
contrast= std::accumulate(weight.begin(), weight.end(), 0.0);
contrast/=distance/ratio;
normalize_weight(weight);
}
float KVLD(const Image<float>& I1,const Image<float>& I2,
std::vector<keypoint>& F1, std::vector<keypoint>& F2,const std::vector<Pair>& matches,
std::vector<Pair>& matchesFiltered,std::vector<double>& score,libNumerics::matrix<float>& E,std::vector<bool>& valide, KvldParameters& kvldParameters){
matchesFiltered.clear();
score.clear();
ImageScale Chaine1(I1);
ImageScale Chaine2(I2);
std::cout<<"Image scale-space complete..."<<std::endl;
float range1=getRange(I1,std::min(F1.size(),matches.size()),kvldParameters.inlierRate);
float range2=getRange(I2,std::min(F2.size(),matches.size()),kvldParameters.inlierRate);
size_t size=matches.size();
////================distance map construction, for use of selecting neighbors===============//
//std::cout<<"computing distance maps"<<std::endl;
//libNumerics::matrix<float> dist1=libNumerics::matrix<float>::zeros(F1.size(), F1.size());
//libNumerics::matrix<float> dist2=libNumerics::matrix<float>::zeros(F2.size(), F2.size());
// for (int a1=0; a1<F1.size();++a1)
// for (int a2=0; a2<F1.size();++a2)
// dist1(a1,a2)=point_distance(F1[a1],F1[a2]);
// for (int b1=0; b1<F2.size();++b1)
// for (int b2=0; b2<F2.size();++b2)
// dist2(b1,b2)=point_distance(F2[b1],F2[b2]);
fill(valide.begin(),valide.end(), true);
std::vector<double> scoretable(size, 0);
std::vector<size_t> result(size, 0);
//============main iteration for match verification==========//
std::cout<<"main iteration";
bool change=true, initial=true;
while(change){
std::cout<<".";
change=false;
fill(scoretable.begin(), scoretable.end(), 0.0);
fill(result.begin(), result.end(), 0);
//========substep 1: search for each match its neighbors and verify if they are gvld-consistent ============//
for (int it1=0; it1<size-1;it1++){
if (valide[it1]){
size_t a1=matches[it1].first, b1=matches[it1].second;
for (int it2=it1+1; it2<size;it2++){
if (valide[it2]){
size_t a2=matches[it2].first, b2=matches[it2].second;
float dist1=point_distance(F1[a1],F1[a2]);
float dist2=point_distance(F2[b1],F2[b2]);
if ( dist1>min_dist && dist2>min_dist
&& (dist1<range1 || dist2<range2)){
if(E(it1,it2)==-1){//update E if unknow
E(it1,it2)=-2; E(it2,it1)=-2;
if(!kvldParameters.geometry || consistent(F1[a1],F1[a2],F2[b1],F2[b2])<distance_thres){
VLD vld1(Chaine1,F1[a1],F1[a2]);
VLD vld2(Chaine2,F2[b1],F2[b2]);
//vld1.test();
double error=vld1.difference(vld2);
//std::cout<<std::endl<<it1<<" "<<it2<<" "<<dist1(a1,a2)<<" "<< dist2(b1,b2)<<" "<<error<<std::endl;
if (error<juge){
E(it1,it2)=(float)error;
E(it2,it1)=(float)error;
//std::cout<<E(it2,it1)<<std::endl;
}
}
}
if(E(it1,it2)>=0) {
result[it1]+=1;
result[it2]+=1;
scoretable[it1]+=double(E(it1,it2));
scoretable[it2]+=double(E(it1,it2));
if (result[it1]>=max_connection)
break;
}
}
}
}
}
}
//========substep 2: remove false matches by K gvld-consistency criteria ============//
for (int it=0; it<size;it++){
if (valide[it] && result[it]<kvldParameters.K) {valide[it]=false;change=true;}
}
//========substep 3: remove multiple matches to a same point by keeping the one with the best average gvld-consistency score ============//
if(uniqueMatch){
for (int it1=0; it1<size-1;it1++){
if (valide[it1]){
size_t a1=matches[it1].first, b1=matches[it1].second;
for (int it2=it1+1; it2<size;it2++)
if (valide[it2]){
size_t a2=matches[it2].first, b2=matches[it2].second;
if(a1==a2||b1==b2
||(F1[a1].x==F1[a2].x && F1[a1].y==F1[a2].y && (F2[b1].x!=F2[b2].x || F2[b1].y!=F2[b2].y))
||((F1[a1].x!=F1[a2].x || F1[a1].y!=F1[a2].y ) && F2[b1].x==F2[b2].x && F2[b1].y==F2[b2].y)
){
//cardinal comparison
if(result[it1]>result[it2]){
valide[it2]=false;change=true;
}else if(result[it1]<result[it2]){
valide[it1]=false;change=true;
}else if(result[it1]==result[it2]){
//score comparison
if (scoretable[it1]>scoretable[it2]){
valide[it1]=false;change=true;
}else if (scoretable[it1]<scoretable[it2]){
valide[it2]=false;change=true;
}
}
}
}
}
}
}
//========substep 4: if geometric verification is set, re-score matches by geometric-consistency, and remove poorly scored ones ============================//
if (uniqueMatch && kvldParameters.geometry){
for (int i=0;i<size;i++) scoretable[i]=0;
std::vector<bool> switching;
for (int i=0;i<size;i++) switching.push_back(false);
for (int it1=0; it1<size;it1++){
if (valide[it1]) {
size_t a1=matches[it1].first, b1=matches[it1].second;
float index=0.0f;
int good_index=0;
for (int it2=0; it2<size;it2++){
if (it1!=it2 && valide[it2]){
size_t a2=matches[it2].first, b2=matches[it2].second;
float dist1=point_distance(F1[a1],F1[a2]);
float dist2=point_distance(F2[b1],F2[b2]);
if ((dist1<range1 || dist2<range2)
&& (dist1>min_dist && dist2>min_dist)
){
float d=consistent(F1[a1],F1[a2],F2[b1],F2[b2]);
scoretable[it1]+=d;
index+=1;
if (d<distance_thres)
good_index++;
}
}
}
scoretable[it1]/=index;
if (good_index<0.3f*float(index) && scoretable[it1]>1.2){switching[it1]=true;change=true;}
}
}
for (int it1=0; it1<size;it1++){
if (switching[it1])
valide[it1]=false;
}
}
}
std::cout<<std::endl;
//=============== generating output list ===================//
for (int it=0; it<size;it++){
if (valide[it]){
matchesFiltered.push_back(matches[it]);
score.push_back(scoretable[it]);
}
}
return float(matchesFiltered.size())/matches.size();
}
bool line_3D::intersects_triangle(triangle_3D triangle, double &a, double &b, double &c, double &t)
{
point_3D vector1,vector2,vector3,normal;
point_3D center;
double bounding_sphere_radius;
double distance_ca, distance_cb, distance_cc;
// compute the triangle bounding sphere:
center.x = (triangle.a.x + triangle.b.x + triangle.c.x) / 3.0;
center.y = (triangle.a.y + triangle.b.y + triangle.c.y) / 3.0;
center.z = (triangle.a.z + triangle.b.z + triangle.c.z) / 3.0;
distance_ca = point_distance(center,triangle.a);
distance_cb = point_distance(center,triangle.b);
distance_cc = point_distance(center,triangle.c);
bounding_sphere_radius = distance_ca;
if (distance_cb > bounding_sphere_radius)
bounding_sphere_radius = distance_cb;
if (distance_cc > bounding_sphere_radius)
bounding_sphere_radius = distance_cc;
a = 0.0;
b = 0.0;
c = 0.0;
substract_vectors(triangle.a,triangle.b,vector1);
substract_vectors(triangle.a,triangle.c,vector2);
cross_product(vector1,vector2,normal);
/*
Compute general plane equation in form
qa * x + qb * y + qc * z + d = 0:
*/
double qa = normal.x;
double qb = normal.y;
double qc = normal.z;
double d = -1 * (qa * triangle.a.x + qb * triangle.a.y + qc * triangle.a.z);
/* Solve for t: */
double denominator = (qa * this->q0 + qb * this->q1 + qc * this->q2);
if (denominator == 0)
return false;
t = (-qa * this->c0 - qb * this->c1 - qc * this->c2 - d) / denominator;
/* t now contains parameter value for the intersection */
if (t < 0.0)
return false;
point_3D intersection;
this->get_point(t,intersection); // intersection in 3D space
if (point_distance(intersection,center) > bounding_sphere_radius)
return false;
// vectors from the intersection to each triangle vertex:
substract_vectors(triangle.a,intersection,vector1);
substract_vectors(triangle.b,intersection,vector2);
substract_vectors(triangle.c,intersection,vector3);
point_3D normal1, normal2, normal3;
// now multiply the vectors to get their normals:
cross_product(vector1,vector2,normal1);
cross_product(vector2,vector3,normal2);
cross_product(vector3,vector1,normal3);
// if one of the vectors points in other direction than the others, the point is not inside the triangle:
if (dot_product(normal1,normal2) <= 0 || dot_product(normal2,normal3) <= 0)
return false;
// now compute the barycentric coordinates:
triangle_3D helper_triangle;
double total_area;
total_area = triangle_area(triangle);
helper_triangle.a = intersection;
helper_triangle.b = triangle.b;
helper_triangle.c = triangle.c;
a = triangle_area(helper_triangle) / total_area;
helper_triangle.a = triangle.a;
helper_triangle.b = intersection;
helper_triangle.c = triangle.c;
b = triangle_area(helper_triangle) / total_area;
helper_triangle.a = triangle.a;
helper_triangle.b = triangle.b;
helper_triangle.c = intersection;
c = triangle_area(helper_triangle) / total_area;
return true;
}
static void QuadTree_repulsive_force_interact(QuadTree qt1, QuadTree qt2, real *x, real *force, real bh, real p, real KP, real *counts){
/* calculate the all to all reopulsive force and accumulate on each node of the quadtree if an interaction is possible.
force[i*dim+j], j=1,...,dim is teh force on node i
*/
SingleLinkedList l1, l2;
real *x1, *x2, dist, wgt1, wgt2, f, *f1, *f2, w1, w2;
int dim, i, j, i1, i2, k;
QuadTree qt11, qt12;
if (!qt1 || !qt2) return;
assert(qt1->n > 0 && qt2->n > 0);
dim = qt1->dim;
l1 = qt1->l;
l2 = qt2->l;
/* far enough, calculate repulsive force */
dist = point_distance(qt1->average, qt2->average, dim);
if (qt1->width + qt2->width < bh*dist){
counts[0]++;
x1 = qt1->average;
w1 = qt1->total_weight;
f1 = get_or_alloc_force_qt(qt1, dim);
x2 = qt2->average;
w2 = qt2->total_weight;
f2 = get_or_alloc_force_qt(qt2, dim);
assert(dist > 0);
for (k = 0; k < dim; k++){
if (p == -1){
f = w1*w2*KP*(x1[k] - x2[k])/(dist*dist);
} else {
f = w1*w2*KP*(x1[k] - x2[k])/pow(dist, 1.- p);
}
f1[k] += f;
f2[k] -= f;
}
return;
}
/* both at leaves, calculate repulsive force */
if (l1 && l2){
while (l1){
x1 = node_data_get_coord(SingleLinkedList_get_data(l1));
wgt1 = node_data_get_weight(SingleLinkedList_get_data(l1));
i1 = node_data_get_id(SingleLinkedList_get_data(l1));
f1 = get_or_assign_node_force(force, i1, l1, dim);
l2 = qt2->l;
while (l2){
x2 = node_data_get_coord(SingleLinkedList_get_data(l2));
wgt2 = node_data_get_weight(SingleLinkedList_get_data(l2));
i2 = node_data_get_id(SingleLinkedList_get_data(l2));
f2 = get_or_assign_node_force(force, i2, l2, dim);
if ((qt1 == qt2 && i2 < i1) || i1 == i2) {
l2 = SingleLinkedList_get_next(l2);
continue;
}
counts[1]++;
dist = distance_cropped(x, dim, i1, i2);
for (k = 0; k < dim; k++){
if (p == -1){
f = wgt1*wgt2*KP*(x1[k] - x2[k])/(dist*dist);
} else {
f = wgt1*wgt2*KP*(x1[k] - x2[k])/pow(dist, 1.- p);
}
f1[k] += f;
f2[k] -= f;
}
l2 = SingleLinkedList_get_next(l2);
}
l1 = SingleLinkedList_get_next(l1);
}
return;
}
/* identical, split one */
if (qt1 == qt2){
for (i = 0; i < 1<<dim; i++){
qt11 = qt1->qts[i];
for (j = i; j < 1<<dim; j++){
qt12 = qt1->qts[j];
QuadTree_repulsive_force_interact(qt11, qt12, x, force, bh, p, KP, counts);
}
}
} else {
/* split the one with bigger box, or one not at the last level */
if (qt1->width > qt2->width && !l1){
for (i = 0; i < 1<<dim; i++){
qt11 = qt1->qts[i];
QuadTree_repulsive_force_interact(qt11, qt2, x, force, bh, p, KP, counts);
}
} else if (qt2->width > qt1->width && !l2){
for (i = 0; i < 1<<dim; i++){
qt11 = qt2->qts[i];
QuadTree_repulsive_force_interact(qt11, qt1, x, force, bh, p, KP, counts);
}
} else if (!l1){/* pick one that is not at the last level */
for (i = 0; i < 1<<dim; i++){
qt11 = qt1->qts[i];
QuadTree_repulsive_force_interact(qt11, qt2, x, force, bh, p, KP, counts);
}
} else if (!l2){
for (i = 0; i < 1<<dim; i++){
qt11 = qt2->qts[i];
QuadTree_repulsive_force_interact(qt11, qt1, x, force, bh, p, KP, counts);
}
} else {
assert(0); /* can be both at the leaf level since that should be catched at the beginning of this func. */
}
}
}
void create_boundary_generators(ptr_array_t* surface_points,
ptr_array_t* surface_normals,
ptr_array_t* surface_tags,
point_t** boundary_generators,
int* num_boundary_generators,
char*** tag_names,
int_array_t*** tags,
int* num_tags)
{
ASSERT(surface_points->size >= 4); // surface must be closed!
ASSERT(surface_points->size == surface_normals->size);
ASSERT(surface_points->size == surface_tags->size);
int num_surface_points = surface_points->size;
// Compute the minimum distance from each surface point to its neighbors.
real_t* h_min = polymec_malloc(sizeof(real_t) * num_surface_points);
{
// Dump the surface points into a kd-tree.
point_t* surf_points = polymec_malloc(sizeof(point_t) * num_surface_points);
for (int i = 0; i < num_surface_points; ++i)
surf_points[i] = *((point_t*)surface_points->data[i]);
kd_tree_t* tree = kd_tree_new(surf_points, num_surface_points);
int neighbors[2];
for (int i = 0; i < num_surface_points; ++i)
{
// Find the "nearest 2" points to the ith surface point--the first is
// the point itself, and the second is its nearest neighbor.
// FIXME: Serious memory error within here. We work around it for
// FIXME: the moment by freshing the tree pointer, which is
// FIXME: corrupted. ICK!
kd_tree_t* tree_p = tree;
kd_tree_nearest_n(tree, &surf_points[i], 2, neighbors);
tree = tree_p;
ASSERT(neighbors[0] == i);
ASSERT(neighbors[1] >= 0);
ASSERT(neighbors[1] < num_surface_points);
h_min[i] = point_distance(&surf_points[i], &surf_points[neighbors[1]]);
}
// Clean up.
kd_tree_free(tree);
polymec_free(surf_points);
}
// Generate boundary points for each surface point based on how many
// surfaces it belongs to.
ptr_array_t* boundary_points = ptr_array_new();
string_int_unordered_map_t* tag_indices = string_int_unordered_map_new();
ptr_array_t* boundary_tags = ptr_array_new();
for (int i = 0; i < num_surface_points; ++i)
{
// Add any tags from this point to the set of existing boundary tags.
string_slist_t* tags = surface_tags->data[i];
for (string_slist_node_t* t_iter = tags->front; t_iter != NULL; t_iter = t_iter->next)
string_int_unordered_map_insert(tag_indices, t_iter->value, tag_indices->size);
// Retrieve the surface point.
point_t* x_surf = surface_points->data[i];
// Retrieve the list of normal vectors for this surface point.
ptr_slist_t* normal_list = surface_normals->data[i];
int num_normals = normal_list->size;
vector_t normals[num_normals];
int n_offset = 0;
for (ptr_slist_node_t* n_iter = normal_list->front; n_iter != NULL; n_iter = n_iter->next)
normals[n_offset++] = *((vector_t*)n_iter->value);
// For now, let's keep things relatively simple.
if (num_normals > 2)
{
polymec_error("create_boundary_generators: Too many normal vectors (%d) for surface point %d at (%g, %g, %g)",
num_normals, i, x_surf->x, x_surf->y, x_surf->z);
}
// Create boundary points based on this list of normals.
if (num_normals == 1)
{
// This point only belongs to one surface, so we create boundary points
// on either side of it.
point_t* x_out = polymec_malloc(sizeof(point_t));
x_out->x = x_surf->x + h_min[i]*normals[0].x;
x_out->y = x_surf->y + h_min[i]*normals[0].y;
x_out->z = x_surf->z + h_min[i]*normals[0].z;
ptr_array_append_with_dtor(boundary_points, x_out, polymec_free);
point_t* x_in = polymec_malloc(sizeof(point_t));
x_in->x = x_surf->x - h_min[i]*normals[0].x;
x_in->y = x_surf->y - h_min[i]*normals[0].y;
x_in->z = x_surf->z - h_min[i]*normals[0].z;
ptr_array_append_with_dtor(boundary_points, x_in, polymec_free);
}
else if (num_normals == 2)
{
// This point appears at the interface between two surfaces.
// (Or so it seems.)
ASSERT(vector_dot(&normals[0], &normals[1]) < 0.0);
point_t* x1 = polymec_malloc(sizeof(point_t));
x1->x = x_surf->x + h_min[i]*normals[0].x;
x1->y = x_surf->y + h_min[i]*normals[0].y;
x1->z = x_surf->z + h_min[i]*normals[0].z;
ptr_array_append_with_dtor(boundary_points, x1, polymec_free);
point_t* x2 = polymec_malloc(sizeof(point_t));
x2->x = x_surf->x - h_min[i]*normals[1].x;
x2->y = x_surf->y - h_min[i]*normals[1].y;
x2->z = x_surf->z - h_min[i]*normals[1].z;
ptr_array_append_with_dtor(boundary_points, x2, polymec_free);
}
// Tag the boundary point appropriately.
ptr_array_append(boundary_tags, tags); // Borrowed ref to tags.
}
// Move the surface points into a contiguous array.
*boundary_generators = polymec_malloc(sizeof(point_t) * boundary_points->size);
*num_boundary_generators = boundary_points->size;
for (int i = 0; i < boundary_points->size; ++i)
{
(*boundary_generators)[i] = *((point_t*)boundary_points->data[i]);
}
// Transcribe the tags.
*tag_names = polymec_malloc(sizeof(char*) * tag_indices->size);
*tags = polymec_malloc(sizeof(int_array_t*) * tag_indices->size);
*num_tags = tag_indices->size;
char* tag_name;
int pos = 0, tag_index;
while (string_int_unordered_map_next(tag_indices, &pos, &tag_name, &tag_index))
{
(*tag_names)[tag_index] = string_dup(tag_name);
(*tags)[tag_index] = int_array_new();
for (int j = 0; j < *num_boundary_generators; ++j)
int_array_append((*tags)[tag_index], tag_index);
}
// Clean up.
string_int_unordered_map_free(tag_indices);
polymec_free(h_min);
}
color scene_3D::compute_lighting(point_3D position, material surface_material, point_3D surface_normal)
{
unsigned int i, j;
point_3D vector_to_light, vector_to_camera, reflection_vector;
color final_color, light_color;
double helper, intensity, distance_penalty;
int helper_color[3];
helper_color[0] = surface_material.ambient_intensity * surface_material.surface_color.red;
helper_color[1] = surface_material.ambient_intensity * surface_material.surface_color.green;
helper_color[2] = surface_material.ambient_intensity * surface_material.surface_color.blue;
vector_to_camera.x = -1 * position.x;
vector_to_camera.y = -1 * position.y;
vector_to_camera.z = -1 * position.z;
normalize(vector_to_camera);
for (i = 0; i < this->lights.size(); i++)
{
unsigned int sum;
sum = this->cast_shadow_ray(position,*this->lights[i],ERROR_OFFSET,0.0) ? 1 : 0; // main shadow ray
for (j = 1; j < this->shadow_rays; j++) // additional shadow rays
sum += this->cast_shadow_ray(position,*this->lights[i],ERROR_OFFSET,this->shadow_range) ? 1 : 0;
if (sum != 0) // at least one shadow ray hit the light
{
double shadow_ratio; // how much shadow the point is in, 0.0 = full shadow, 1.0 = no shadow
shadow_ratio = sum / ((double) this->shadow_rays);
distance_penalty = 1.0 - point_distance(position,this->lights[i]->get_position()) / this->lights[i]->distance_factor;
distance_penalty = distance_penalty < 0 ? 0 : pow(distance_penalty,0.25);
intensity = this->lights[i]->get_intensity() * distance_penalty * shadow_ratio;
substract_vectors(this->lights[i]->get_position(),position,vector_to_light);
normalize(vector_to_light);
helper = -1 * dot_product(vector_to_light,surface_normal);
helper = helper < 0 ? 0 : helper;
// add diffuse part:
helper_color[0] += intensity * surface_material.surface_color.red * surface_material.diffuse_intensity * helper;
helper_color[1] += intensity * surface_material.surface_color.green * surface_material.diffuse_intensity * helper;
helper_color[2] += intensity * surface_material.surface_color.blue * surface_material.diffuse_intensity * helper;
// add specular part:
reflection_vector = make_reflection_vector(surface_normal,vector_to_light);
helper = pow(dot_product(reflection_vector,vector_to_camera),surface_material.specular_exponent);
helper = helper < 0 ? 0 : helper;
light_color = this->lights[i]->get_color();
helper_color[0] += intensity * surface_material.specular_intensity * helper * light_color.red;
helper_color[1] += intensity * surface_material.specular_intensity * helper * light_color.green;
helper_color[2] += intensity * surface_material.specular_intensity * helper * light_color.blue;
}
}
final_color.red = saturate_int(helper_color[0],0,255);
final_color.green = saturate_int(helper_color[1],0,255);
final_color.blue = saturate_int(helper_color[2],0,255);
return final_color;
}
color scene_3D::cast_ray(line_3D line, double threshold, unsigned int recursion_depth)
{
unsigned int k, l, m;
triangle_3D triangle;
color final_color, helper_color, add_color;
double depth, t;
double *texture_coords_a, *texture_coords_b, *texture_coords_c;
double barycentric_a, barycentric_b, barycentric_c;
point_3D starting_point;
point_3D normal,normal_a,normal_b,normal_c;
point_3D reflection_vector, incoming_vector_reverse;
material mat;
int color_sum[3];
line.get_point(0,starting_point);
depth = 99999999;
final_color.red = this->background_color.red;
final_color.green = this->background_color.green;
final_color.blue = this->background_color.blue;
for (k = 0; k < this->meshes.size(); k++)
{
if (!line.intersects_sphere(this->meshes[k]->bounding_sphere_center,this->meshes[k]->bounding_sphere_radius))
continue;
for (l = 0; l < this->meshes[k]->triangle_indices.size(); l += 3)
{
triangle.a = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l]].position;
triangle.b = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l + 1]].position;
triangle.c = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l + 2]].position;
texture_coords_a = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l]].texture_coords;
texture_coords_b = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l + 1]].texture_coords;
texture_coords_c = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l + 2]].texture_coords;
if (line.intersects_triangle(triangle,barycentric_a,barycentric_b,barycentric_c,t))
{
point_3D intersection;
line.get_point(t,intersection);
double distance = point_distance(starting_point,intersection);
mat = this->meshes[k]->get_material();
if (distance < depth && distance > threshold) // depth test
{
depth = distance;
normal_a = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l]].normal;
normal_b = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l + 1]].normal;
normal_c = this->meshes[k]->vertices[this->meshes[k]->triangle_indices[l + 2]].normal;
normal.x = 0;
normal.y = 0;
normal.z = 0;
normal.x = barycentric_a * normal_a.x + barycentric_b * normal_b.x + barycentric_c * normal_c.x;
normal.y = barycentric_a * normal_a.y + barycentric_b * normal_b.y + barycentric_c * normal_c.y;
normal.z = barycentric_a * normal_a.z + barycentric_b * normal_b.z + barycentric_c * normal_c.z;
normalize(normal); // interpolation breaks normalization
if (!this->meshes[k]->use_3D_texture && this->meshes[k]->get_texture() != 0) // 2d texture
{
double u,v;
u = barycentric_a * texture_coords_a[0] + barycentric_b * texture_coords_b[0] + barycentric_c * texture_coords_c[0];
v = barycentric_a * texture_coords_a[1] + barycentric_b * texture_coords_b[1] + barycentric_c * texture_coords_c[1];
color_buffer_get_pixel(this->meshes[k]->get_texture(),u * this->meshes[k]->get_texture()->width,v * this->meshes[k]->get_texture()->height,&final_color.red,&final_color.green,&final_color.blue);
}
else if (this->meshes[k]->use_3D_texture && this->meshes[k]->get_texture_3D() != 0) // 3d texture
{
final_color = this->meshes[k]->get_texture_3D()->get_color(intersection.x,intersection.y,intersection.z);
}
else // mesh color
{
final_color.red = 255;
final_color.green = 255;
final_color.blue = 255;
}
helper_color = compute_lighting(intersection,mat,normal);
final_color = multiply_colors(helper_color,final_color);
if (recursion_depth != 0)
{
incoming_vector_reverse = line.get_vector_to_origin();
if (mat.reflection > 0) // reflection
{
color_sum[0] = 0;
color_sum[1] = 0;
color_sum[2] = 0;
for (m = 0; m < this->reflection_rays; m++)
{
point_3D helper_point;
reflection_vector = make_reflection_vector(normal,incoming_vector_reverse);
reflection_vector.x *= -1;
reflection_vector.y *= -1;
reflection_vector.z *= -1;
if (m > 0) // alter the ray slightly
alter_vector(reflection_vector,this->reflection_range);
helper_point.x = intersection.x + reflection_vector.x;
helper_point.y = intersection.y + reflection_vector.y;
helper_point.z = intersection.z + reflection_vector.z;
line_3D reflection_line(intersection,helper_point);
add_color = cast_ray(reflection_line,ERROR_OFFSET,recursion_depth - 1);
color_sum[0] += add_color.red;
color_sum[1] += add_color.green;
color_sum[2] += add_color.blue;
}
add_color.red = color_sum[0] / this->reflection_rays;
add_color.green = color_sum[1] / this->reflection_rays;
add_color.blue = color_sum[2] / this->reflection_rays;
final_color = interpolate_colors(final_color,add_color,mat.reflection);
}
if (mat.transparency > 0) // refraction
{
color_sum[0] = 0;
color_sum[1] = 0;
color_sum[2] = 0;
for (m = 0; m < this->refraction_rays; m++)
{
point_3D helper_point;
point_3D refraction_vector;
refraction_vector = make_refraction_vector(normal,incoming_vector_reverse,mat.refractive_index);
if (m > 0) // alter the ray slightly
alter_vector(refraction_vector,this->refraction_range);
helper_point.x = intersection.x + refraction_vector.x;
helper_point.y = intersection.y + refraction_vector.y;
helper_point.z = intersection.z + refraction_vector.z;
line_3D refraction_line(intersection,helper_point);
add_color = cast_ray(refraction_line,ERROR_OFFSET,recursion_depth - 1);
color_sum[0] += add_color.red;
color_sum[1] += add_color.green;
color_sum[2] += add_color.blue;
}
add_color.red = color_sum[0] / this->refraction_rays;
add_color.green = color_sum[1] / this->refraction_rays;
add_color.blue = color_sum[2] / this->refraction_rays;
final_color = interpolate_colors(final_color,add_color,mat.transparency);
}
}
}
}
}
}
return final_color;
}
int main(int argc, char *argv[]) {
if (argc == 1) {
puts("Use: level_edit <level_folder>");
return 1;
}
qw_screen(800, 600, 0, "Projekt Defense");
/* get background image from level folder */
char lvl_bg[128] = {0};
strcpy(lvl_bg, argv[1]);
strcat(lvl_bg, "/background.png");
qw_image background = qw_loadimage(lvl_bg);
int max_points = 128,
points_i = 0;
SDL_Point *points = malloc(sizeof(SDL_Point) * max_points);
while (qw_running()) {
qw_drawimage(background);
qw_color(200, 100, 120, 255);
qw_fillrect(qw_mousex - 2, qw_mousey - 2, 4, 4);
/* waypoint placement */
if (qw_mousedown(SDL_BUTTON_LEFT)) {
if (points_i == 0) {
points[points_i++] = point_new(qw_mousex, qw_mousey);
} else {
/* if not the first point placed: check if current point is to close to the last one */
SDL_Point np = point_new(qw_mousex, qw_mousey);
if (point_distance(points[points_i - 1], np) > 7.f)
points[points_i++] = np;
}
/* we need more points? */
if (points_i == max_points) {
max_points += 128;
points = realloc(points, sizeof(SDL_Point) * max_points);
}
}
if (qw_keydown(QW_KEY(E))) {
if (points_i > 1) {
export_points(argv[1], points, points_i);
puts("Exported!");
qw_quit();
}
}
qw_color(100, 120, 200, 255);
SDL_RenderDrawLines(qw_renderer, points, points_i);
qw_redraw();
if (qw_keydown(QW_KEY(ESCAPE))) {
qw_quit();
}
}
free(points);
qw_destroyimage(background);
return 0;
}
void octree_insert(octree_t* tree, point_t* point, int index)
{
if (tree->root == NULL) // Empty tree
{
octree_node_t* node = leaf_new(point, index);
tree->root = node;
++tree->num_points;
}
else if (tree->root->type == OCTREE_LEAF_NODE)
{
point_t center = {.x = 0.5 * (tree->bbox.x1 + tree->bbox.x2),
.y = 0.5 * (tree->bbox.y1 + tree->bbox.y2),
.z = 0.5 * (tree->bbox.z1 + tree->bbox.z2)};
// The tree consists of a single node.
octree_node_t* root = tree->root;
// Does the given point already exist here?
if (point_distance(&root->leaf_node.point, point) == 0.0)
return;
// We need to create a branch node here.
octree_node_t* node = root;
tree->root = branch_new();
int slot = find_slot(¢er, point);
tree->root->branch_node.children[slot] = node;
}
// Now we proceed with the normal logic, given that the root node
// is a branch node.
ASSERT(tree->root->type == OCTREE_BRANCH_NODE);
octree_node_t* node = tree->root;
point_t center = {.x = 0.5 * (tree->bbox.x1 + tree->bbox.x2),
.y = 0.5 * (tree->bbox.y1 + tree->bbox.y2),
.z = 0.5 * (tree->bbox.z1 + tree->bbox.z2)};
real_t lx = tree->bbox.x2 - tree->bbox.x1;
real_t ly = tree->bbox.y2 - tree->bbox.y1;
real_t lz = tree->bbox.z2 - tree->bbox.z1;
int slot = find_slot(¢er, point);
static real_t xf[] = {-0.25, -0.25, -0.25, -0.25, +0.25, +0.25, +0.25, +0.25};
static real_t yf[] = {-0.25, -0.25, +0.25, +0.25, -0.25, -0.25, +0.25, +0.25};
static real_t zf[] = {-0.25, +0.25, -0.25, +0.25, -0.25, +0.25, -0.25, +0.25};
while ((node->branch_node.children[slot] != NULL) &&
(node->branch_node.children[slot]->type == OCTREE_BRANCH_NODE))
{
node = node->branch_node.children[slot];
center.x += xf[slot]*lx;
lx *= 0.5;
center.y += yf[slot]*ly;
ly *= 0.5;
center.z += zf[slot]*lz;
lz *= 0.5;
slot = find_slot(¢er, point);
}
octree_node_t* leaf = node->branch_node.children[slot];
if (leaf == NULL)
{
// No leaf here, so we create a new one!
leaf = leaf_new(point, index);
node->branch_node.children[slot] = leaf;
++tree->num_points;
}
else
{
// Is the point already in this node?
if (point_distance(&leaf->leaf_node.point, point) == 0.0)
return;
else
{
// We have to make a new branch.
int old_slot, new_slot;
do
{
node->branch_node.children[slot] = branch_new();
node = node->branch_node.children[slot];
center.x += xf[slot]*lx;
lx *= 0.5;
center.y += yf[slot]*ly;
ly *= 0.5;
center.z += zf[slot]*lz;
lz *= 0.5;
new_slot = find_slot(¢er, point);
old_slot = find_slot(¢er, &leaf->leaf_node.point);
}
while (new_slot == old_slot);
node->branch_node.children[old_slot] = leaf;
octree_node_t* new_leaf = leaf_new(point, index);
node->branch_node.children[new_slot] = new_leaf;
++tree->num_points;
}
}
}
void octree_delete(octree_t* tree, point_t* point, int index)
{
// FIXME
}
int octree_size(octree_t* tree)
{
return tree->num_points;
}
static void node_clear(octree_node_t* node)
{
if (node == NULL)
{
return;
}
else if (node->type == OCTREE_LEAF_NODE)
{
polymec_free(node);
}
else
{
ASSERT(node->type == OCTREE_BRANCH_NODE);
for (int i = 0; i < 8; ++i)
node_clear(node->branch_node.children[i]);
}
}
void octree_clear(octree_t* tree)
{
node_clear(tree->root);
tree->root = NULL;
tree->num_points = 0;
}
