- const u32 near_wanted_count = 2;
- const u32 nearest_wanted_count = 2;
- // Last is near
- core::list<DFloat> near;
-
- for(core::list<PointWithAttr>::Iterator
- i = m_points.begin();
- i != m_points.end(); i++)
- {
- PointWithAttr &pwa = *i;
- u32 d = pwa.p.getDistanceFrom(p);
-
- DFloat df;
- df.v = pwa.attr.getFloat();
- df.d = d;
-
- // If near list is empty, add directly and continue
- if(near_list.size() == 0)
- {
- near_list.push_back(df);
- continue;
- }
-
- // Get distance of farthest in near list
- u32 near_d = 100000;
- if(near_list.size() > 0)
- {
- core::list<DFloat>::Iterator i = near_list.begin();
- near_d = i->d;
- }
-
- /*
- If point is closer than the farthest in the near list or
- there are not yet enough points on the list
- */
- if(d < near_d || near_list.size() < near_wanted_count)
- {
- // Find the right place in the near list and put it there
-
- // Go from farthest to near in the near list
- core::list<DFloat>::Iterator i = near_list.begin();
- for(; i != near_list.end(); i++)
- {
- // Stop when i is at the first nearer node
- if(i->d < d)
- break;
- }
- // Add df to before i
- if(i == near_list.end())
- near_list.push_back(df);
- else
- near_list.insert_before(i, df);
-
- // Keep near list at right size
- if(near_list.size() > near_wanted_count)
- {
- core::list<DFloat>::Iterator j = near_list.begin();
- near_list.erase(j);
- }
- }
- }
-
- // Return if no values found
- if(near_list.size() == 0)
- return 0.0;