Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
D
dynamic
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Requirements
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Locked files
Build
Pipelines
Jobs
Pipeline schedules
Test cases
Artifacts
Deploy
Releases
Package registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Code review analytics
Issue analytics
Insights
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
MostafaMollaali
dynamic
Commits
f1899545
Commit
f1899545
authored
12 years ago
by
Tom Fischer
Browse files
Options
Downloads
Patches
Plain Diff
forgot to add the files into the previous commit
parent
c68d88c1
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
GeoLib/Grid.h
+465
-0
465 additions, 0 deletions
GeoLib/Grid.h
GeoLib/OctTree.h
+241
-0
241 additions, 0 deletions
GeoLib/OctTree.h
with
706 additions
and
0 deletions
GeoLib/Grid.h
0 → 100644
+
465
−
0
View file @
f1899545
/*
* Grid.h
*
* Created on: Feb 2, 2012
* Author: TF
*/
#ifndef GRID_H_
#define GRID_H_
#include
<vector>
// GeoLib
#include
"AxisAlignedBoundingBox.h"
#ifndef NDEBUG
// BaseLib
#include
"StringTools.h"
#endif
namespace
GeoLib
{
template
<
typename
POINT
>
class
Grid
:
public
GeoLib
::
AABB
{
public:
/**
* @brief The constructor of the grid object takes a vector of points. Furthermore the
* use can specify the maximum number of points per grid cell (in the average!).
*
* The number of grid cells are computed with the following formula
* \f$\frac{n_{points}}{n_{cells}} \le n_{max\_per\_cell}\f$
*
* In order to limit the memory wasting the maximum number of points per grid cell
* (in the average) should be a power of two.
*
* @param pnts (input) the points that are managed with the Grid
* @param max_num_per_grid_cell (input) max number per grid cell in the average (default 512)
*/
Grid
(
std
::
vector
<
POINT
*>
const
&
pnts
,
size_t
max_num_per_grid_cell
=
512
);
/**
* This is the destructor of the class. It deletes the internal data structures *not*
* including the pointers to the points.
*/
virtual
~
Grid
()
{
delete
[]
_grid_quad_to_node_map
;
}
/**
* The method calculates the grid cell within the grid to given point is belonging to, i.e.,
* the (internal) coordinates of the grid are computed. The method searches the actual
* grid cell and all its possible neighbors for the POINT object which has the smallest
* distance. A pointer to this object is returned.
*
* If there is not such a point, i.e., all the searched grid cells do not contain any
* POINT object a NULL pointer is returned.
*
* @param pnt a field that holds the coordinates of the point
* @return a point with the smallest distance within the grid cells that are outlined above or NULL
*/
POINT
const
*
getNearestPoint
(
double
const
*
const
pnt
)
const
;
/**
* Method fetchs all points that are located within grid cells that intersects
* the axis aligned cube defined by its center and half edge length.
*
* @param pnt (input) the center point of the axis aligned cube
* @param half_len (input) half of the edge length of the axis aligned cube
* @param pnts (output) all points within grid cells that intersects
* the axis aligned cube
*/
void
getPointsWithinCube
(
double
const
*
const
pnt
,
double
half_len
,
std
::
vector
<
POINT
*>&
pnts
)
const
;
#ifndef NDEBUG
/**
* Method creates a geometry for every mesh grid box. Additionally it
* creates one geometry containing all the box geometries.
* @param geo_obj
*/
void
createGridGeometry
(
GeoLib
::
GEOObjects
*
geo_obj
)
const
;
#endif
private
:
/**
* Method calculates the grid cell coordinates for the given point pnt. If
* the point is located outside of the bounding box the coordinates of the
* grid cell on the border that is nearest to given point will be returned.
* @param pnt (input) the coordinates of the point
* @param coords (output) the coordinates of the grid cell
*/
inline
void
getGridCoords
(
double
const
*
const
pnt
,
size_t
*
coords
)
const
;
/**
*
* point numbering of the grid cell is as follow
* 7 -------- 6
* /: /|
* / : / |
* / : / |
* / : / |
* 4 -------- 5 |
* | 3 ....|... 2
* | . | /
* | . | /
* | . | /
* |. |/
* 0 -------- 1
* the face numbering is as follow:
* face nodes
* 0 0,3,2,1 bottom
* 1 0,1,5,4 front
* 2 1,2,6,5 right
* 3 2,3,7,6 back
* 4 3,0,4,7 left
* 5 4,5,6,7 top
* @param pnt (input) coordinates of the point
* @param sqr_dists (output) squared distances of the point to the faces
* ordered in the same sequence as above described
* @param coords coordinates of the grid cell
*/
void
getPointCellBorderDistances
(
double
const
*
const
pnt
,
double
dists
[
6
],
size_t
const
*
const
coords
)
const
;
bool
calcNearestPointInGridCell
(
double
const
*
const
pnt
,
size_t
const
*
const
coords
,
double
&
sqr_min_dist
,
POINT
*
&
nearest_pnt
)
const
;
double
_step_sizes
[
3
];
double
_inverse_step_sizes
[
3
];
size_t
_n_steps
[
3
];
std
::
vector
<
POINT
*>*
_grid_quad_to_node_map
;
};
template
<
typename
POINT
>
Grid
<
POINT
>::
Grid
(
std
::
vector
<
POINT
*>
const
&
pnts
,
size_t
max_num_per_grid_cell
)
:
GeoLib
::
AABB
(),
_grid_quad_to_node_map
(
NULL
)
{
// compute axis aligned bounding box
const
size_t
n_pnts
(
pnts
.
size
());
for
(
size_t
k
(
0
);
k
<
n_pnts
;
k
++
)
{
this
->
update
(
pnts
[
k
]
->
getCoords
());
}
double
delta
[
3
]
=
{
0.0
,
0.0
,
0.0
};
for
(
size_t
k
(
0
);
k
<
3
;
k
++
)
{
// make the bounding box a little bit bigger,
// such that the node with maximal coordinates fits into the grid
_max_pnt
[
k
]
*=
(
1.0
+
1e-6
);
if
(
fabs
(
_max_pnt
[
k
])
<
std
::
numeric_limits
<
double
>::
epsilon
())
{
_max_pnt
[
k
]
=
(
_max_pnt
[
k
]
-
_min_pnt
[
k
])
*
(
1.0
+
1e-6
);
}
delta
[
k
]
=
_max_pnt
[
k
]
-
_min_pnt
[
k
];
}
// *** condition: n_pnts / (_n_steps[0] * _n_steps[1] * _n_steps[2]) < max_num_per_grid_cell
// *** with _n_steps[1] = _n_steps[0] * delta[1]/delta[0], _n_steps[2] = _n_steps[0] * delta[2]/delta[0]
if
(
fabs
(
delta
[
1
])
<
std
::
numeric_limits
<
double
>::
epsilon
()
||
fabs
(
delta
[
2
])
<
std
::
numeric_limits
<
double
>::
epsilon
())
{
// 1d case y = z = 0
if
(
fabs
(
delta
[
1
])
<
std
::
numeric_limits
<
double
>::
epsilon
()
&&
fabs
(
delta
[
2
])
<
std
::
numeric_limits
<
double
>::
epsilon
())
{
_n_steps
[
0
]
=
static_cast
<
size_t
>
(
ceil
(
n_pnts
/
(
double
)
max_num_per_grid_cell
));
_n_steps
[
1
]
=
1
;
_n_steps
[
2
]
=
1
;
}
else
{
// 1d case x = z = 0
if
(
fabs
(
delta
[
0
])
<
std
::
numeric_limits
<
double
>::
epsilon
()
&&
fabs
(
delta
[
2
])
<
std
::
numeric_limits
<
double
>::
epsilon
())
{
_n_steps
[
0
]
=
1
;
_n_steps
[
1
]
=
static_cast
<
size_t
>
(
ceil
(
n_pnts
/
(
double
)
max_num_per_grid_cell
));
_n_steps
[
2
]
=
1
;
}
else
{
// 1d case x = y = 0
if
(
fabs
(
delta
[
0
])
<
std
::
numeric_limits
<
double
>::
epsilon
()
&&
fabs
(
delta
[
1
])
<
std
::
numeric_limits
<
double
>::
epsilon
())
{
_n_steps
[
0
]
=
1
;
_n_steps
[
1
]
=
1
;
_n_steps
[
2
]
=
static_cast
<
size_t
>
(
ceil
(
n_pnts
/
(
double
)
max_num_per_grid_cell
));
}
else
{
// 2d case
if
(
fabs
(
delta
[
1
])
<
std
::
numeric_limits
<
double
>::
epsilon
())
{
_n_steps
[
0
]
=
static_cast
<
size_t
>
(
ceil
(
sqrt
(
n_pnts
*
delta
[
0
]
/
(
max_num_per_grid_cell
*
delta
[
2
]))));
_n_steps
[
1
]
=
1
;
_n_steps
[
2
]
=
static_cast
<
size_t
>
(
ceil
(
_n_steps
[
0
]
*
delta
[
2
]
/
delta
[
0
]));
}
else
{
_n_steps
[
0
]
=
static_cast
<
size_t
>
(
ceil
(
sqrt
(
n_pnts
*
delta
[
0
]
/
(
max_num_per_grid_cell
*
delta
[
1
]))));
_n_steps
[
1
]
=
static_cast
<
size_t
>
(
ceil
(
_n_steps
[
0
]
*
delta
[
1
]
/
delta
[
0
]));
_n_steps
[
2
]
=
1
;
}
}
}
}
}
else
{
// 3d case
_n_steps
[
0
]
=
static_cast
<
size_t
>
(
ceil
(
pow
(
n_pnts
*
delta
[
0
]
*
delta
[
0
]
/
(
max_num_per_grid_cell
*
delta
[
1
]
*
delta
[
2
]),
1.
/
3.
)));
_n_steps
[
1
]
=
static_cast
<
size_t
>
(
ceil
(
_n_steps
[
0
]
*
delta
[
1
]
/
delta
[
0
]));
_n_steps
[
2
]
=
static_cast
<
size_t
>
(
ceil
(
_n_steps
[
0
]
*
delta
[
2
]
/
delta
[
0
]));
}
const
size_t
n_plane
(
_n_steps
[
0
]
*
_n_steps
[
1
]);
_grid_quad_to_node_map
=
new
std
::
vector
<
POINT
*>
[
n_plane
*
_n_steps
[
2
]];
// some frequently used expressions to fill the grid vectors
for
(
size_t
k
(
0
);
k
<
3
;
k
++
)
{
_step_sizes
[
k
]
=
delta
[
k
]
/
_n_steps
[
k
];
_inverse_step_sizes
[
k
]
=
1.0
/
_step_sizes
[
k
];
}
// fill the grid vectors
for
(
size_t
l
(
0
);
l
<
n_pnts
;
l
++
)
{
double
const
*
const
pnt
(
pnts
[
l
]
->
getCoords
());
const
size_t
i
(
static_cast
<
size_t
>
((
pnt
[
0
]
-
_min_pnt
[
0
])
*
_inverse_step_sizes
[
0
]));
const
size_t
j
(
static_cast
<
size_t
>
((
pnt
[
1
]
-
_min_pnt
[
1
])
*
_inverse_step_sizes
[
1
]));
const
size_t
k
(
static_cast
<
size_t
>
((
pnt
[
2
]
-
_min_pnt
[
2
])
*
_inverse_step_sizes
[
2
]));
if
(
i
>=
_n_steps
[
0
]
||
j
>=
_n_steps
[
1
]
||
k
>=
_n_steps
[
2
])
{
std
::
cout
<<
"error computing indices "
<<
std
::
endl
;
}
_grid_quad_to_node_map
[
i
+
j
*
_n_steps
[
0
]
+
k
*
n_plane
].
push_back
(
pnts
[
l
]);
}
#ifndef NDEBUG
size_t
pnts_cnt
(
0
);
for
(
size_t
k
(
0
);
k
<
n_plane
*
_n_steps
[
2
];
k
++
)
pnts_cnt
+=
_grid_quad_to_node_map
[
k
].
size
();
assert
(
n_pnts
==
pnts_cnt
);
#endif
}
template
<
typename
POINT
>
POINT
const
*
Grid
<
POINT
>::
getNearestPoint
(
double
const
*
const
pnt
)
const
{
size_t
coords
[
3
];
getGridCoords
(
pnt
,
coords
);
double
sqr_min_dist
(
MathLib
::
sqrDist
(
&
_min_pnt
,
&
_max_pnt
));
POINT
*
nearest_pnt
(
NULL
);
double
dists
[
6
];
getPointCellBorderDistances
(
pnt
,
dists
,
coords
);
if
(
calcNearestPointInGridCell
(
pnt
,
coords
,
sqr_min_dist
,
nearest_pnt
))
{
double
min_dist
(
sqrt
(
sqr_min_dist
));
if
(
dists
[
0
]
>=
min_dist
&&
dists
[
1
]
>=
min_dist
&&
dists
[
2
]
>=
min_dist
&&
dists
[
3
]
>=
min_dist
&&
dists
[
4
]
>=
min_dist
&&
dists
[
5
]
>=
min_dist
)
{
return
nearest_pnt
;
}
}
else
{
// search in all border cells for at least one neighbor
double
sqr_min_dist_tmp
;
POINT
*
nearest_pnt_tmp
(
NULL
);
size_t
offset
(
1
);
while
(
nearest_pnt
==
NULL
)
{
size_t
tmp_coords
[
3
];
for
(
tmp_coords
[
0
]
=
coords
[
0
]
-
offset
;
tmp_coords
[
0
]
<
coords
[
0
]
+
offset
;
tmp_coords
[
0
]
++
)
{
for
(
tmp_coords
[
1
]
=
coords
[
1
]
-
offset
;
tmp_coords
[
1
]
<
coords
[
1
]
+
offset
;
tmp_coords
[
1
]
++
)
{
for
(
tmp_coords
[
2
]
=
coords
[
2
]
-
offset
;
tmp_coords
[
2
]
<
coords
[
2
]
+
offset
;
tmp_coords
[
2
]
++
)
{
// do not check the origin grid cell twice
if
(
!
(
tmp_coords
[
0
]
==
coords
[
0
]
&&
tmp_coords
[
1
]
==
coords
[
1
]
&&
tmp_coords
[
2
]
==
coords
[
2
]))
{
// check if temporary grid cell coordinates are valid
if
(
tmp_coords
[
0
]
<
_n_steps
[
0
]
&&
tmp_coords
[
1
]
<
_n_steps
[
1
]
&&
tmp_coords
[
2
]
<
_n_steps
[
2
])
{
if
(
calcNearestPointInGridCell
(
pnt
,
tmp_coords
,
sqr_min_dist_tmp
,
nearest_pnt_tmp
))
{
if
(
sqr_min_dist_tmp
<
sqr_min_dist
)
{
sqr_min_dist
=
sqr_min_dist_tmp
;
nearest_pnt
=
nearest_pnt_tmp
;
}
}
}
// valid grid cell coordinates
}
// same element
}
// end k
}
// end j
}
// end i
offset
++
;
}
// end while
}
// end else
double
len
(
sqrt
(
MathLib
::
sqrDist
(
pnt
,
nearest_pnt
->
getCoords
())));
// search all other grid cells within the cube with the edge nodes
std
::
vector
<
POINT
*>
pnts
;
getPointsWithinCube
(
pnt
,
len
,
pnts
);
const
size_t
n_pnts
(
pnts
.
size
());
for
(
size_t
k
(
0
);
k
<
n_pnts
;
k
++
)
{
const
double
sqr_dist
(
MathLib
::
sqrDist
(
pnt
,
pnts
[
k
]
->
getCoords
()));
if
(
sqr_dist
<
sqr_min_dist
)
{
sqr_min_dist
=
sqr_dist
;
nearest_pnt
=
pnts
[
k
];
}
}
return
nearest_pnt
;
}
template
<
typename
POINT
>
void
Grid
<
POINT
>::
getPointsWithinCube
(
double
const
*
const
pnt
,
double
half_len
,
std
::
vector
<
POINT
*>&
pnts
)
const
{
double
tmp_pnt
[
3
]
=
{
pnt
[
0
]
-
half_len
,
pnt
[
1
]
-
half_len
,
pnt
[
2
]
-
half_len
};
// min
size_t
min_coords
[
3
];
getGridCoords
(
tmp_pnt
,
min_coords
);
tmp_pnt
[
0
]
=
pnt
[
0
]
+
half_len
;
tmp_pnt
[
1
]
=
pnt
[
1
]
+
half_len
;
tmp_pnt
[
2
]
=
pnt
[
2
]
+
half_len
;
size_t
max_coords
[
3
];
getGridCoords
(
tmp_pnt
,
max_coords
);
size_t
coords
[
3
],
steps0_x_steps1
(
_n_steps
[
0
]
*
_n_steps
[
1
]);
for
(
coords
[
0
]
=
min_coords
[
0
];
coords
[
0
]
<
max_coords
[
0
]
+
1
;
coords
[
0
]
++
)
{
for
(
coords
[
1
]
=
min_coords
[
1
];
coords
[
1
]
<
max_coords
[
1
]
+
1
;
coords
[
1
]
++
)
{
const
size_t
coords0_p_coords1_x_steps0
(
coords
[
0
]
+
coords
[
1
]
*
_n_steps
[
0
]);
for
(
coords
[
2
]
=
min_coords
[
2
];
coords
[
2
]
<
max_coords
[
2
]
+
1
;
coords
[
2
]
++
)
{
// copy pnts
const
size_t
n_pnts
(
_grid_quad_to_node_map
[
coords0_p_coords1_x_steps0
+
coords
[
2
]
*
steps0_x_steps1
].
size
());
for
(
size_t
k
(
0
);
k
<
n_pnts
;
k
++
)
{
pnts
.
push_back
(
_grid_quad_to_node_map
[
coords0_p_coords1_x_steps0
+
coords
[
2
]
*
steps0_x_steps1
][
k
]);
}
}
}
}
}
#ifndef NDEBUG
template
<
typename
POINT
>
void
Grid
<
POINT
>::
createGridGeometry
(
GeoLib
::
GEOObjects
*
geo_obj
)
const
{
std
::
vector
<
std
::
string
>
grid_names
;
GeoLib
::
Point
const
&
llf
(
getMinPoint
());
GeoLib
::
Point
const
&
urb
(
getMaxPoint
());
const
double
dx
((
urb
[
0
]
-
llf
[
0
])
/
_n_steps
[
0
]);
const
double
dy
((
urb
[
1
]
-
llf
[
1
])
/
_n_steps
[
1
]);
const
double
dz
((
urb
[
2
]
-
llf
[
2
])
/
_n_steps
[
2
]);
// create grid names and grid boxes as geometry
for
(
size_t
i
(
0
);
i
<
_n_steps
[
0
];
i
++
)
{
for
(
size_t
j
(
0
);
j
<
_n_steps
[
1
];
j
++
)
{
for
(
size_t
k
(
0
);
k
<
_n_steps
[
2
];
k
++
)
{
std
::
string
name
(
"Grid-"
);
name
+=
number2str
<
size_t
>
(
i
);
name
+=
"-"
;
name
+=
number2str
<
size_t
>
(
j
);
name
+=
"-"
;
name
+=
number2str
<
size_t
>
(
k
);
grid_names
.
push_back
(
name
);
std
::
vector
<
GeoLib
::
Point
*>*
points
(
new
std
::
vector
<
GeoLib
::
Point
*>
);
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
i
*
dx
,
llf
[
1
]
+
j
*
dy
,
llf
[
2
]
+
k
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
i
*
dx
,
llf
[
1
]
+
(
j
+
1
)
*
dy
,
llf
[
2
]
+
k
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
(
i
+
1
)
*
dx
,
llf
[
1
]
+
(
j
+
1
)
*
dy
,
llf
[
2
]
+
k
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
(
i
+
1
)
*
dx
,
llf
[
1
]
+
j
*
dy
,
llf
[
2
]
+
k
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
i
*
dx
,
llf
[
1
]
+
j
*
dy
,
llf
[
2
]
+
(
k
+
1
)
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
i
*
dx
,
llf
[
1
]
+
(
j
+
1
)
*
dy
,
llf
[
2
]
+
(
k
+
1
)
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
(
i
+
1
)
*
dx
,
llf
[
1
]
+
(
j
+
1
)
*
dy
,
llf
[
2
]
+
(
k
+
1
)
*
dz
));
points
->
push_back
(
new
GeoLib
::
Point
(
llf
[
0
]
+
(
i
+
1
)
*
dx
,
llf
[
1
]
+
j
*
dy
,
llf
[
2
]
+
(
k
+
1
)
*
dz
));
geo_obj
->
addPointVec
(
points
,
grid_names
[
grid_names
.
size
()
-
1
],
NULL
);
std
::
vector
<
GeoLib
::
Polyline
*>*
plys
(
new
std
::
vector
<
GeoLib
::
Polyline
*>
);
GeoLib
::
Polyline
*
ply0
(
new
GeoLib
::
Polyline
(
*
points
));
for
(
size_t
l
(
0
);
l
<
4
;
l
++
)
{
ply0
->
addPoint
(
l
);
}
ply0
->
addPoint
(
0
);
plys
->
push_back
(
ply0
);
GeoLib
::
Polyline
*
ply1
(
new
GeoLib
::
Polyline
(
*
points
));
for
(
size_t
l
(
4
);
l
<
8
;
l
++
)
{
ply1
->
addPoint
(
l
);
}
ply1
->
addPoint
(
4
);
plys
->
push_back
(
ply1
);
GeoLib
::
Polyline
*
ply2
(
new
GeoLib
::
Polyline
(
*
points
));
ply2
->
addPoint
(
0
);
ply2
->
addPoint
(
4
);
plys
->
push_back
(
ply2
);
GeoLib
::
Polyline
*
ply3
(
new
GeoLib
::
Polyline
(
*
points
));
ply3
->
addPoint
(
1
);
ply3
->
addPoint
(
5
);
plys
->
push_back
(
ply3
);
GeoLib
::
Polyline
*
ply4
(
new
GeoLib
::
Polyline
(
*
points
));
ply4
->
addPoint
(
2
);
ply4
->
addPoint
(
6
);
plys
->
push_back
(
ply4
);
GeoLib
::
Polyline
*
ply5
(
new
GeoLib
::
Polyline
(
*
points
));
ply5
->
addPoint
(
3
);
ply5
->
addPoint
(
7
);
plys
->
push_back
(
ply5
);
geo_obj
->
addPolylineVec
(
plys
,
grid_names
[
grid_names
.
size
()
-
1
],
NULL
);
}
}
}
std
::
string
merged_geo_name
(
"Grid"
);
geo_obj
->
mergeGeometries
(
grid_names
,
merged_geo_name
);
}
#endif
template
<
typename
POINT
>
void
Grid
<
POINT
>::
getGridCoords
(
double
const
*
const
pnt
,
size_t
*
coords
)
const
{
for
(
size_t
k
(
0
);
k
<
3
;
k
++
)
{
if
(
pnt
[
k
]
<
_min_pnt
[
k
])
{
coords
[
k
]
=
0
;
}
else
{
if
(
pnt
[
k
]
>
_max_pnt
[
k
])
{
coords
[
k
]
=
_n_steps
[
k
]
-
1
;
}
else
{
coords
[
k
]
=
static_cast
<
size_t
>
((
pnt
[
k
]
-
_min_pnt
[
k
])
*
_inverse_step_sizes
[
k
]);
}
}
}
}
template
<
typename
POINT
>
bool
Grid
<
POINT
>::
calcNearestPointInGridCell
(
double
const
*
const
pnt
,
size_t
const
*
const
coords
,
double
&
sqr_min_dist
,
POINT
*&
nearest_pnt
)
const
{
const
size_t
grid_idx
(
coords
[
0
]
+
coords
[
1
]
*
_n_steps
[
0
]
+
coords
[
2
]
*
_n_steps
[
0
]
*
_n_steps
[
1
]);
std
::
vector
<
POINT
*>
const
&
pnts
(
_grid_quad_to_node_map
[
grid_idx
]);
if
(
pnts
.
empty
())
return
false
;
const
size_t
n_pnts
(
pnts
.
size
());
sqr_min_dist
=
MathLib
::
sqrDist
(
pnts
[
0
]
->
getCoords
(),
pnt
);
nearest_pnt
=
pnts
[
0
];
for
(
size_t
i
(
1
);
i
<
n_pnts
;
i
++
)
{
const
double
sqr_dist
(
MathLib
::
sqrDist
(
pnts
[
i
]
->
getCoords
(),
pnt
));
if
(
sqr_dist
<
sqr_min_dist
)
{
sqr_min_dist
=
sqr_dist
;
nearest_pnt
=
pnts
[
i
];
}
}
return
true
;
}
template
<
typename
POINT
>
void
Grid
<
POINT
>::
getPointCellBorderDistances
(
double
const
*
const
pnt
,
double
dists
[
6
],
size_t
const
*
const
coords
)
const
{
dists
[
0
]
=
(
pnt
[
2
]
-
_min_pnt
[
2
]
+
coords
[
2
]
*
_step_sizes
[
2
]);
// bottom
dists
[
5
]
=
(
_step_sizes
[
2
]
-
dists
[
0
]);
// top
dists
[
1
]
=
(
pnt
[
1
]
-
_min_pnt
[
1
]
+
coords
[
1
]
*
_step_sizes
[
1
]);
// front
dists
[
3
]
=
(
_step_sizes
[
1
]
-
dists
[
1
]);
// back
dists
[
4
]
=
(
pnt
[
0
]
-
_min_pnt
[
0
]
+
coords
[
0
]
*
_step_sizes
[
0
]);
// left
dists
[
2
]
=
(
_step_sizes
[
0
]
-
dists
[
4
]);
// right
}
}
// end namespace GeoLib
#endif
/* MESHGRID_H_ */
This diff is collapsed.
Click to expand it.
GeoLib/OctTree.h
0 → 100644
+
241
−
0
View file @
f1899545
/*
* @file OctTree.h
*
* Created on: Feb 27, 2012
* Author: TF
*/
#ifndef OCTTREE_H_
#define OCTTREE_H_
namespace
GeoLib
{
template
<
typename
POINT
>
class
OctTree
{
public:
static
OctTree
<
POINT
>*
createOctTree
(
POINT
&
ll
,
POINT
&
ur
,
size_t
max_points_per_node
)
{
const
double
dx
(
ur
[
0
]
-
ll
[
0
]);
const
double
dy
(
ur
[
1
]
-
ll
[
1
]);
const
double
dz
(
ur
[
2
]
-
ll
[
2
]);
if
(
dx
>=
dy
&&
dx
>=
dz
)
{
ll
[
1
]
-=
(
dx
-
dy
)
/
2.0
;
ur
[
1
]
+=
(
dx
-
dy
)
/
2.0
;
ll
[
2
]
-=
(
dx
-
dz
)
/
2.0
;
ur
[
2
]
+=
(
dx
-
dz
)
/
2.0
;
}
else
{
if
(
dy
>=
dx
&&
dy
>=
dz
)
{
ll
[
0
]
-=
(
dy
-
dx
)
/
2.0
;
ur
[
0
]
+=
(
dy
-
dx
)
/
2.0
;
ll
[
2
]
-=
(
dy
-
dz
)
/
2.0
;
ur
[
2
]
+=
(
dy
-
dz
)
/
2.0
;
}
else
{
ll
[
0
]
-=
(
dz
-
dx
)
/
2.0
;
ur
[
0
]
+=
(
dz
-
dx
)
/
2.0
;
ll
[
1
]
-=
(
dz
-
dy
)
/
2.0
;
ur
[
1
]
+=
(
dz
-
dy
)
/
2.0
;
}
}
OctTree
<
POINT
>::
_max_points_per_node
=
max_points_per_node
;
return
new
OctTree
<
POINT
>
(
ll
,
ur
);
}
virtual
~
OctTree
()
{
for
(
size_t
k
(
0
);
k
<
8
;
k
++
)
delete
_childs
[
k
];
}
/**
* This method adds the given point to the OctTree. If necessary,
* the OctTree will be extended.
* @param pnt the point
* @return If the point can be inserted the method returns true, else false.
*/
bool
addPoint
(
POINT
*
pnt
)
{
if
((
*
pnt
)[
0
]
<
_ll
[
0
])
return
false
;
if
((
*
pnt
)[
0
]
>
_ur
[
0
])
return
false
;
if
((
*
pnt
)[
1
]
<
_ll
[
1
])
return
false
;
if
((
*
pnt
)[
1
]
>
_ur
[
1
])
return
false
;
if
((
*
pnt
)[
2
]
<
_ll
[
2
])
return
false
;
if
((
*
pnt
)[
2
]
>
_ur
[
2
])
return
false
;
if
(
!
_is_leaf
)
{
for
(
size_t
k
(
0
);
k
<
8
;
k
++
)
{
if
(
_childs
[
k
]
->
addPoint
(
pnt
))
{
return
true
;
}
}
}
// check if point is already in OctTree
bool
pnt_in_tree
(
false
);
for
(
size_t
k
(
0
);
k
<
_pnts
.
size
()
&&
!
pnt_in_tree
;
k
++
)
{
const
double
sqr_dist
(
MathLib
::
sqrDist
(
(
_pnts
[
k
])
->
getCoords
(),
pnt
->
getCoords
()
));
if
(
sqr_dist
<
std
::
numeric_limits
<
double
>::
epsilon
())
pnt_in_tree
=
true
;
}
if
(
!
pnt_in_tree
)
_pnts
.
push_back
(
pnt
);
else
return
false
;
if
(
_pnts
.
size
()
>
OctTree
<
POINT
>::
_max_points_per_node
)
splitNode
();
return
true
;
}
/**
* range query - returns all points inside the range (min[0], max[0]) x (min[1], max[1]) x (min[2], max[2])
* @param min
* @param max
* @param pnts
*/
void
getPointsInRange
(
POINT
const
&
min
,
POINT
const
&
max
,
std
::
vector
<
POINT
*>
&
pnts
)
const
{
if
(
_ur
[
0
]
<
min
[
0
])
return
;
if
(
_ur
[
1
]
<
min
[
1
])
return
;
if
(
_ur
[
2
]
<
min
[
2
])
return
;
if
(
max
[
0
]
<
_ll
[
0
])
return
;
if
(
max
[
1
]
<
_ll
[
1
])
return
;
if
(
max
[
2
]
<
_ll
[
2
])
return
;
if
(
_is_leaf
)
{
typename
std
::
vector
<
POINT
*>::
const_iterator
it
;
for
(
it
=
(
_pnts
.
begin
());
it
!=
_pnts
.
end
();
it
++
)
{
pnts
.
push_back
(
*
it
);
}
}
else
{
for
(
size_t
k
(
0
);
k
<
8
;
k
++
)
{
_childs
[
k
]
->
getPointsInRange
(
min
,
max
,
pnts
);
}
}
}
private
:
enum
OctTreeQuadrant
{
NEL
=
0
,
//!< north east lower
NWL
,
//!< north west lower
SWL
,
//!< south west lower
SEL
,
//!< south east lower
NEU
,
//!< south west upper
NWU
,
//!< south west upper
SWU
,
//!< south west upper
SEU
//!< south east upper
};
/**
* private constructor
* @param ll lower left point
* @param ur upper right point
* @return
*/
OctTree
(
POINT
const
&
ll
,
POINT
const
&
ur
)
:
_ll
(
ll
),
_ur
(
ur
),
_is_leaf
(
true
)
{
// init childs
for
(
size_t
k
(
0
);
k
<
8
;
k
++
)
_childs
[
k
]
=
NULL
;
}
void
splitNode
()
{
const
double
x_mid
((
_ur
[
0
]
+
_ll
[
0
])
/
2.0
);
const
double
y_mid
((
_ur
[
1
]
+
_ll
[
1
])
/
2.0
);
const
double
z_mid
((
_ur
[
2
]
+
_ll
[
2
])
/
2.0
);
POINT
p0
(
x_mid
,
y_mid
,
_ll
[
2
]),
p1
(
_ur
[
0
],
_ur
[
1
],
z_mid
);
// create child NEL
_childs
[
NEL
]
=
new
OctTree
<
POINT
>
(
p0
,
p1
);
// create child NWL
p0
[
0
]
=
_ll
[
0
];
p1
[
0
]
=
x_mid
;
_childs
[
NWL
]
=
new
OctTree
<
POINT
>
(
p0
,
p1
);
// create child SWL
p0
[
1
]
=
_ll
[
1
];
p1
[
1
]
=
y_mid
;
_childs
[
SWL
]
=
new
OctTree
<
POINT
>
(
_ll
,
p1
);
// create child NEU
_childs
[
NEU
]
=
new
OctTree
<
POINT
>
(
p1
,
_ur
);
// create child SEL
p0
[
0
]
=
x_mid
;
p1
[
0
]
=
_ur
[
0
];
_childs
[
SEL
]
=
new
OctTree
<
POINT
>
(
p0
,
p1
);
// create child NWU
p0
[
0
]
=
_ll
[
0
];
p0
[
1
]
=
y_mid
;
p0
[
2
]
=
z_mid
;
p1
[
0
]
=
x_mid
;
p1
[
1
]
=
_ur
[
1
];
p1
[
2
]
=
_ur
[
2
];
_childs
[
NWU
]
=
new
OctTree
<
POINT
>
(
p0
,
p1
);
// create child SWU
p0
[
1
]
=
_ll
[
1
];
p1
[
1
]
=
y_mid
;
_childs
[
SWU
]
=
new
OctTree
<
POINT
>
(
p0
,
p1
);
// create child SEU
p0
[
0
]
=
x_mid
;
p1
[
0
]
=
_ur
[
0
];
p1
[
1
]
=
y_mid
;
p1
[
2
]
=
_ur
[
2
];
_childs
[
SEU
]
=
new
OctTree
<
POINT
>
(
p0
,
p1
);
// distribute points to sub quadtrees
const
size_t
n_pnts
(
_pnts
.
size
());
for
(
size_t
j
(
0
);
j
<
n_pnts
;
j
++
)
{
bool
nfound
(
true
);
for
(
size_t
k
(
0
);
k
<
8
&&
nfound
;
k
++
)
{
if
(
_childs
[
k
]
->
addPoint
(
_pnts
[
j
]))
{
nfound
=
false
;
}
}
}
_pnts
.
clear
();
_is_leaf
=
false
;
}
/**
* childs are sorted:
* _childs[0] is north east lower child
* _childs[1] is north west lower child
* _childs[2] is south west lower child
* _childs[3] is south east lower child
* _childs[4] is north east upper child
* _childs[5] is north west upper child
* _childs[6] is south west upper child
* _childs[7] is south east upper child
*/
OctTree
<
POINT
>*
_childs
[
8
];
/**
* lower left front face point of the cube
*/
POINT
const
_ll
;
/**
* upper right back face point of the cube
*/
POINT
const
_ur
;
std
::
vector
<
POINT
*>
_pnts
;
bool
_is_leaf
;
/**
* maximum number of points per leaf
*/
static
size_t
_max_points_per_node
;
};
template
<
typename
POINT
>
size_t
OctTree
<
POINT
>::
_max_points_per_node
=
0
;
}
// end namespace GeoLib
#endif
/* OCTTREE_H_ */
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment