Branch data Line data Source code
1 : : #include <math.h>
2 : : #include "GridSearchTree.hpp"
3 : :
4 : 308 : double dist (CubitPoint * a, CubitPoint * b){
5 : 308 : return sqrt((a->x() - b->x())*(a->x() - b->x()) + (a->y() - b->y())*(a->y() - b->y()) + (a->z() - b->z())*(a->z() - b->z()) );
6 : : }
7 : :
8 : 396 : CubitPoint * GridSearchTree::fix(CubitPoint * data) {
9 : :
10 : : long i, j, k;
11 : : double x, y, z;
12 : :
13 [ + - ]: 396 : x=data->x();
14 [ + - ]: 396 : y=data->y();
15 [ + - ]: 396 : z=data->z();
16 : :
17 : : // get the coordinates of the box containing the point
18 : 396 : i=(long)(x/(2*epsilon));
19 : 396 : j=(long)(y/(2*epsilon));
20 : 396 : k=(long)(z/(2*epsilon));
21 : :
22 : :
23 [ - + ]: 396 : if (x<0) i--;
24 [ - + ]: 396 : if (y<0) j--;
25 [ - + ]: 396 : if (z<0) k--;
26 : :
27 : :
28 : : int ofi;
29 : : int ofj;
30 : : int ofk;
31 : : int ii;
32 : :
33 : : // calculate the i, j, k offset for the seven neighboring boxes to be searched
34 [ + - ]: 396 : if (fabs(x-i*2*epsilon) < epsilon) ofi = -1; else ofi = 1;
35 [ + - ]: 396 : if (fabs(y-j*2*epsilon) < epsilon) ofj = -1; else ofj = 1;
36 [ + - ]: 396 : if (fabs(z-k*2*epsilon) < epsilon) ofk = -1; else ofk = 1;
37 : :
38 : : // mindist holds the distance to the current closest point
39 : 396 : double mindist=2*epsilon;
40 : : // curdist holds the distance to the current point
41 : : double curdist;
42 : : // closest is the current closest point
43 : 396 : CubitPoint * closest = NULL;
44 : : // curpoint is the current point
45 : : CubitPoint * curpoint;
46 : :
47 : : // construct grid cell ( box )
48 [ + - ][ + - ]: 396 : GridSearchTreeNode * curnode = new GridSearchTreeNode(i+ofi,j+ofj,k+ofk);
49 : : // attempt to find it in the tree
50 [ + - ]: 396 : pos = nodemap.find(curnode);
51 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
52 : : {
53 : : // if cell is in the tree search its list for close points
54 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
55 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
56 : : {
57 [ # # ]: 0 : curpoint = curlist.get_and_step();
58 [ # # ]: 0 : curdist = dist(data, curpoint);
59 [ # # ]: 0 : if (curdist<mindist)
60 : : {
61 : 0 : closest = curpoint;
62 : 0 : mindist=curdist;
63 : : }
64 [ # # ]: 0 : }
65 : : }
66 [ + - ][ + - ]: 396 : delete curnode;
67 : :
68 : : // construct grid cell ( box )
69 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i+ofi,j+ofj,k);
70 : : // attempt to find it in the tree
71 [ + - ]: 396 : pos = nodemap.find(curnode);
72 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
73 : : {
74 : : // if cell is in the tree search its list for close points
75 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
76 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
77 : : {
78 [ # # ]: 0 : curpoint = curlist.get_and_step();
79 [ # # ]: 0 : curdist = dist(data, curpoint);
80 [ # # ]: 0 : if (curdist<mindist)
81 : : {
82 : 0 : closest = curpoint;
83 : 0 : mindist=curdist;
84 : : }
85 [ # # ]: 0 : }
86 : : }
87 [ + - ][ + - ]: 396 : delete curnode;
88 : :
89 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i+ofi,j,k+ofk);
90 [ + - ]: 396 : pos = nodemap.find(curnode);
91 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
92 : : {
93 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
94 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
95 : : {
96 [ # # ]: 0 : curpoint = curlist.get_and_step();
97 [ # # ]: 0 : curdist = dist(data, curpoint);
98 [ # # ]: 0 : if (curdist<mindist)
99 : : {
100 : 0 : closest = curpoint;
101 : 0 : mindist=curdist;
102 : : }
103 [ # # ]: 0 : }
104 : : }
105 [ + - ][ + - ]: 396 : delete curnode;
106 : :
107 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i+ofi,j,k);
108 [ + - ]: 396 : pos = nodemap.find(curnode);
109 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
110 : : {
111 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
112 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
113 : : {
114 [ # # ]: 0 : curpoint = curlist.get_and_step();
115 [ # # ]: 0 : curdist = dist(data, curpoint);
116 [ # # ]: 0 : if (curdist<mindist)
117 : : {
118 : 0 : closest = curpoint;
119 : 0 : mindist=curdist;
120 : : }
121 [ # # ]: 0 : }
122 : : }
123 [ + - ][ + - ]: 396 : delete curnode;
124 : :
125 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i,j+ofj,k+ofk);
126 [ + - ]: 396 : pos = nodemap.find(curnode);
127 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
128 : : {
129 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
130 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
131 : : {
132 [ # # ]: 0 : curpoint = curlist.get_and_step();
133 [ # # ]: 0 : curdist = dist(data, curpoint);
134 [ # # ]: 0 : if (curdist<mindist)
135 : : {
136 : 0 : closest = curpoint;
137 : 0 : mindist=curdist;
138 : : }
139 [ # # ]: 0 : }
140 : : }
141 [ + - ][ + - ]: 396 : delete curnode;
142 : :
143 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i,j+ofj,k);
144 [ + - ]: 396 : pos = nodemap.find(curnode);
145 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
146 : : {
147 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
148 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
149 : : {
150 [ # # ]: 0 : curpoint = curlist.get_and_step();
151 [ # # ]: 0 : curdist = dist(data, curpoint);
152 [ # # ]: 0 : if (curdist<mindist)
153 : : {
154 : 0 : closest = curpoint;
155 : 0 : mindist=curdist;
156 : : }
157 [ # # ]: 0 : }
158 : : }
159 [ + - ][ + - ]: 396 : delete curnode;
160 : :
161 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i,j,k+ofk);
162 [ + - ]: 396 : pos = nodemap.find(curnode);
163 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ - + ]
164 : : {
165 [ # # ][ # # ]: 0 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
166 [ # # ][ # # ]: 0 : for (ii= curlist.size(); ii>0; ii--)
167 : : {
168 [ # # ]: 0 : curpoint = curlist.get_and_step();
169 [ # # ]: 0 : curdist = dist(data, curpoint);
170 [ # # ]: 0 : if (curdist<mindist)
171 : : {
172 : 0 : closest = curpoint;
173 : 0 : mindist=curdist;
174 : : }
175 [ # # ]: 0 : }
176 : : }
177 [ + - ][ + - ]: 396 : delete curnode;
178 : :
179 : :
180 [ + - ][ + - ]: 396 : curnode = new GridSearchTreeNode(i,j,k);
181 [ + - ]: 396 : pos = nodemap.find(curnode);
182 [ + - ][ + - ]: 396 : if (pos!=nodemap.end())
[ + + ]
183 : : {
184 [ + - ][ + - ]: 308 : DLIList<CubitPoint*> curlist = (*pos).first->get_list();
185 [ + - ][ + + ]: 616 : for (ii= curlist.size(); ii>0; ii--)
186 : : {
187 [ + - ]: 308 : curpoint = curlist.get_and_step();
188 [ + - ]: 308 : curdist = dist(data, curpoint);
189 [ + - ]: 308 : if (curdist<mindist)
190 : : {
191 : 308 : closest = curpoint;
192 : 308 : mindist=curdist;
193 : : }
194 [ + - ]: 308 : }
195 : : }
196 : :
197 : : // if closest point is within epsilon distance return the closest point
198 [ + + ]: 396 : if (mindist<=epsilon)
199 : : {
200 [ + - ][ + - ]: 308 : delete curnode;
201 : 308 : return closest;
202 : : }
203 : : else
204 : : {
205 : : // add current point and cell to tree
206 [ + - ][ + - ]: 88 : if (pos==nodemap.end())
[ + - ]
207 : : {
208 [ + - ]: 88 : curnode->add(data);
209 [ + - ][ + - ]: 88 : nodemap.insert(gmap::value_type(curnode, 1));
210 : : }
211 : : else
212 : : {
213 [ # # ][ # # ]: 0 : (*pos).first->add(data);
214 [ # # ][ # # ]: 0 : delete curnode;
215 : : }
216 : :
217 : 396 : return data;
218 : : }
219 : : }
220 : :
|
