WGridRegular3D.h 43.2 KB
Newer Older
wiebel's avatar
wiebel committed
1 2
//---------------------------------------------------------------------------
//
3
// Project: OpenWalnut ( http://www.openwalnut.org )
wiebel's avatar
wiebel committed
4
//
5 6
// Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
// For more information see http://www.openwalnut.org/copying
wiebel's avatar
wiebel committed
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
//
// This file is part of OpenWalnut.
//
// OpenWalnut is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenWalnut is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
//
//---------------------------------------------------------------------------

#ifndef WGRIDREGULAR3D_H
#define WGRIDREGULAR3D_H

28 29
#include <cmath>
#include <string>
30
#include <utility>
31
#include <vector>
32

33
#include <boost/array.hpp>
34
#include <boost/shared_ptr.hpp>
35

36
#include <osg/Matrix>
37
#include <osg/Vec3>
38

39 40 41
#include "../common/exceptions/WOutOfBounds.h"
#include "../common/exceptions/WPreconditionNotMet.h"
#include "../common/math/WLinearAlgebraFunctions.h"
42
#include "../common/math/WMatrix.h"
43
#include "../common/math/linearAlgebra/WLinearAlgebra.h"
44
#include "../common/WBoundingBox.h"
45
#include "../common/WCondition.h"
46
#include "../common/WDefines.h"
47
#include "../common/WProperties.h"
48

49
#include "WGrid.h"
50
#include "WGridTransformOrtho.h"
wiebel's avatar
wiebel committed
51 52

/**
cornimueller's avatar
cornimueller committed
53 54 55
 * A grid that has parallelepiped cells which all have the same proportion. I.e.
 * the samples along a single axis are equidistant. The distance of samples may
 * vary between axes.
56 57
 *
 * \warning Positions on the upper bounddaries in x, y and z are considered outside the grid.
wiebel's avatar
wiebel committed
58
 * \ingroup dataHandler
wiebel's avatar
wiebel committed
59
 */
60 61
template< typename T >
class WGridRegular3DTemplate : public WGrid // NOLINT
wiebel's avatar
wiebel committed
62
{
63 64 65
    // this (friend) is necessary to allow casting
    template <class U>
    friend class WGridRegular3DTemplate;
66 67 68 69
    /**
     * Only test are allowed as friends.
     */
    friend class WGridRegular3DTest;
wiebel's avatar
wiebel committed
70
public:
71 72 73
    /**
     * Convenience typedef for 3d vectors of the appropriate numerical type.
     */
74
    typedef WMatrixFixed< T, 3, 1 > Vector3Type;
75

wiebel's avatar
wiebel committed
76
    /**
77
     * Convenience typedef for a boost::shared_ptr< WGridRegular3DTemplate >.
78
     */
79
    typedef boost::shared_ptr< WGridRegular3DTemplate > SPtr;
80

cornimueller's avatar
cornimueller committed
81
    /**
82
     * Convenience typedef for a boost::shared_ptr< const WGridRegular3DTemplate >.
83
     */
84
    typedef boost::shared_ptr< const WGridRegular3DTemplate > ConstSPtr;
85

86 87 88 89 90
    /**
     * Convenience typedef for a boost::array< size_t, 8 >. Return type of getCellVertexIds.
     */
    typedef boost::array< size_t, 8 > CellVertexArray;

91 92 93
    /**
     * Copy constructor.
     * Copies the data from an WGridRegular3DTemplate object with arbitary numerical type.
94 95
     *
     * \param rhs A WGridRegular3DTemplate object, which mustn't have the same numerical type.
96 97 98 99
     */
    template< typename InputType >
    WGridRegular3DTemplate( WGridRegular3DTemplate< InputType > const& rhs ); // NOLINT -- no explicit, this allows casts

wiebel's avatar
wiebel committed
100
    /**
101
     * Defines the number of samples in each coordinate direction as ints,
102
     * and the transformation of the grid via a grid transform.
103
     *
Alexander Wiebel's avatar
Alexander Wiebel committed
104 105 106
     * \param nbPosX number of positions along first axis
     * \param nbPosY number of positions along second axis
     * \param nbPosZ number of positions along third axis
107
     * \param transform a grid transformation
wiebel's avatar
wiebel committed
108
     */
109 110
    WGridRegular3DTemplate( unsigned int nbPosX, unsigned int nbPosY, unsigned int nbPosZ,
                            WGridTransformOrthoTemplate< T > const transform = WGridTransformOrthoTemplate< T >() );
111 112 113

    /**
     * Returns the number of samples in x direction.
114
     * \return The number of samples in x direction.
115
     */
116 117
    unsigned int getNbCoordsX() const;

118 119
    /**
     * Returns the number of samples in y direction.
120
     * \return The number of samples in y direction.
121
     */
122 123
    unsigned int getNbCoordsY() const;

124 125
    /**
     * Returns the number of samples in z direction.
126
     * \return The number of samples in z direction.
127
     */
128
    unsigned int getNbCoordsZ() const;
129 130 131

    /**
     * Returns the distance between samples in x direction.
132
     * \return The distance between samples in x direction.
133
     */
134
    T getOffsetX() const;
135

136 137
    /**
     * Returns the distance between samples in y direction.
138
     * \return The distance between samples in y direction.
139
     */
140
    T getOffsetY() const;
141

142 143
    /**
     * Returns the distance between samples in z direction.
144
     * \return The distance between samples in z direction.
145
     */
146
    T getOffsetZ() const;
147

148 149
    /**
     * Returns the vector determining the direction of samples in x direction.
150 151
     * Adding this vector to a grid position in world coordinates yields the position of the next sample
     * along the grids (world coordinate) x-axis.
152
     * \return The vector determining the direction of samples in x direction.
153
     */
154
    Vector3Type getDirectionX() const;
155

156 157
    /**
     * Returns the vector determining the direction of samples in y direction.
158 159
     * Adding this vector to a grid position in world coordinates yields the position of the next sample
     * along the grids (world coordinate) y-axis.
160
     * \return The vector determining the direction of samples in y direction.
161
     */
162
    Vector3Type getDirectionY() const;
163

164 165
    /**
     * Returns the vector determining the direction of samples in z direction.
166 167
     * Adding this vector to a grid position in world coordinates yields the position of the next sample
     * along the grids (world coordinate) z-axis.
168
     * \return The vector determining the direction of samples in z direction.
169
     */
170
    Vector3Type getDirectionZ() const;
171

172 173
    /**
     * Returns the vector determining the unit (normalized) direction of samples in x direction.
174
     * \return The vector determining the unit (normalized) direction of samples in x direction.
175
     */
176
    Vector3Type getUnitDirectionX() const;
177 178 179

    /**
     * Returns the vector determining the unit (normalized) direction of samples in y direction.
180
     * \return The vector determining the unit (normalized) direction of samples in y direction.
181
     */
182
    Vector3Type getUnitDirectionY() const;
183 184 185

    /**
     * Returns the vector determining the unit (normalized) direction of samples in z direction.
186
     * \return The vector determining the unit (normalized) direction of samples in z direction.
187
     */
188
    Vector3Type getUnitDirectionZ() const;
189

190 191
    /**
     * Returns the position of the origin of the grid.
192
     * \return The position of the origin of the grid.
193
     */
194
    Vector3Type getOrigin() const;
195

196
    /**
197 198
     * Returns a 4x4 matrix that represents the grid's transformation.
     * \return The grid's transformation.
199
     */
200
    WMatrix< T > getTransformationMatrix() const;
201

202
    /**
203 204
     * \copybrief WGrid::getBoundingBox()
     * \return \copybrief WGrid::getBoundingBox()
205
     */
206
    WBoundingBox getBoundingBox() const;
207

wiebel's avatar
wiebel committed
208 209
    /**
     * Returns the i-th position on the grid.
Alexander Wiebel's avatar
Alexander Wiebel committed
210
     * \param i id of position to be obtained
211
     * \return i-th position of the grid.
wiebel's avatar
wiebel committed
212
     */
213
    Vector3Type getPosition( unsigned int i ) const;
wiebel's avatar
wiebel committed
214 215 216 217

    /**
     * Returns the position that is the iX-th in x direction, the iY-th in
     * y direction and the iZ-th in z direction.
Alexander Wiebel's avatar
Alexander Wiebel committed
218 219 220
     * \param iX id along first axis of position to be obtained
     * \param iY id along second axis of position to be obtained
     * \param iZ id along third axis of position to be obtained
221
     * \return Position (iX,iY,iZ)
wiebel's avatar
wiebel committed
222
     */
223
    Vector3Type getPosition( unsigned int iX, unsigned int iY, unsigned int iZ ) const;
wiebel's avatar
wiebel committed
224

225 226
    /**
     * Transforms world coordinates to texture coordinates.
227
     * \param point The point with these coordinates will be transformed.
228
     * \return point transformed into texture coordinate system
229
     */
230
    Vector3Type worldCoordToTexCoord( Vector3Type point );
231

232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247
    /**
     * Returns the i'th voxel where the given position belongs too.
     *
     * A voxel is a cuboid which surrounds a point on the grid.
     *
     * \verbatim
      Voxel:
                     ______________ ____ (0.5, 0.5, 0.5)
                    /:            /|
                   / :           / |
                  /  :          /  |
                 /   :         /   |
               _/____:_ ___ __/    |
                |    :        |    |
                |    :    *<--|--------- grid point (0, 0, 0)
                |    :........|....|__
248 249 250 251
         dz == 1|   /         |   /
                |  /          |  / dy == 1
                | /           | /
               _|/____________|/__
252 253 254 255 256
                |<- dx == 1 ->|
         -0.5,-0.5,-0.5
       \endverbatim
     *
     * Please note the first voxel has only 1/8 of the size a normal voxel
257
     * would have since all positions outside the grid do not belong
258 259 260 261 262 263 264 265
     * to any voxel. Note: a cell is different to a voxel in terms of position.
     * A voxel has a grid point as center whereas a cell has grid points as
     * corners.
     * \param pos Position for which we want to have the voxel number.
     *
     * \return Voxel number or -1 if the position refers to a point outside of
     * the grid.
     */
266
    int getVoxelNum( const Vector3Type& pos ) const;
267

schurade's avatar
schurade committed
268 269 270 271 272 273 274 275 276 277 278
    /**
     * returns the voxel index for a given discrete position in the grid
     *
     * \param x Position for which we want to have the voxel number.
     * \param y Position for which we want to have the voxel number.
     * \param z Position for which we want to have the voxel number.
     *
     * \return Voxel number or -1 if the position refers to a point outside of
     * the grid.
     */
    int getVoxelNum( const size_t x, const size_t y, const size_t z ) const;
279 280 281 282 283 284 285 286 287 288
    /**
     * Computes the X coordinate of that voxel that contains the
     * position pos.
     *
     * \param pos The position which selects the voxel for which the X
     * coordinate is computed.
     *
     * \return The X coordinate or -1 if pos refers to point outside of the
     * grid.
     */
289
    int getXVoxelCoord( const Vector3Type& pos ) const;
290 291 292 293 294 295 296 297 298 299 300

    /**
     * Computes the Y coordinate of that voxel that contains the
     * position pos.
     *
     * \param pos The position which selects the voxel for which the Y
     * coordinate is computed.
     *
     * \return The Y coordinate or -1 if pos refers to point outside of the
     * grid.
     */
301
    int getYVoxelCoord( const Vector3Type& pos ) const;
302 303 304 305 306 307 308 309 310 311 312

    /**
     * Computes the Z coordinate of that voxel that contains the
     * position pos.
     *
     * \param pos The position which selects the voxel for which the Z
     * coordinate is computed.
     *
     * \return The Z coordinate or -1 if pos refers to point outside of the
     * grid.
     */
313
    int getZVoxelCoord( const Vector3Type& pos ) const;
314 315 316 317 318 319 320 321 322 323 324 325

    /**
     * Computes the voxel coordinates of that voxel which contains
     * the position pos.
     *
     * \param pos The position selecting the voxel.
     *
     * \return A vector of ints where the first component is the X voxel
     * coordinate, the second the Y component voxel coordinate and the last the
     * Z component of the voxel coordinate. If the selecting position is
     * outside of the grid then -1 -1 -1 is returned.
     */
326
    WVector3i getVoxelCoord( const Vector3Type& pos ) const;
327

328
    /**
329 330
     * Computes the id of the cell containing the position pos. Note that the upper
     * bound of the grid does not belong to any cell
331 332
     *
     * \param pos The position selecting the cell.
333
     * \param success True if the position pos is inside the grid.
334 335
     *
     * \return id of the containing the position.
336
     */
337
    size_t getCellId( const Vector3Type& pos, bool* success ) const;
338 339 340 341 342

    /**
     * Computes the ids of the vertices of a cell given by its id.
     *
     * \param cellId The id of the cell we want to know ther vertices of.
343 344
     *
     * \return Ids of vertices belonging to cell with given cellId.
345 346 347 348 349 350 351 352 353 354 355 356 357 358

     * \verbatim
        z-axis  y-axis
        |      /
        | 6___/_7
        |/:    /|
        4_:___5 |
        | :...|.|
        |.2   | 3
        |_____|/ ____x-axis
       0      1
       \endverbatim
     *
     */
359
    CellVertexArray getCellVertexIds( size_t cellId ) const;
360

361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379
    /**
     * Computes the vertices for a voxel cuboid around the given point:
     *
     * \verbatim
        z-axis  y-axis
        |      /
        | h___/_g
        |/:    /|
        d_:___c |
        | :...|.|
        |.e   | f
        |_____|/ ____x-axis
       a      b
       \endverbatim
     *
     * As you can see the order of the points is: a, b, c, d, e, f, g, h.
     *
     * \param point Center of the cuboid which must not necesarrily be a point
     * of the grid.
380
     * \param margin If you need to shrink the Voxel put here the delta > 0.
381 382 383 384 385 386
     *
     * \return Reference to a list of vertices which are the corner points of
     * the cube. Note this must not be a voxel, but has the same size of the an
     * voxel. If you need voxels at grid positions fill this function with
     * voxel center positions aka grid points.
     */
387 388
    boost::shared_ptr< std::vector< Vector3Type > > getVoxelVertices( const Vector3Type& point,
                                                                      const T margin = 0.0 ) const;
389

Mathias Goldau's avatar
[STYLE]  
Mathias Goldau committed
390 391 392 393 394 395 396 397 398
    /**
     * Return the list of neighbour voxels.
     *
     * \throw WOutOfBounds If the voxel id is outside of the grid.
     *
     * \param id Number of the voxel for which the neighbours should be computed
     *
     * \return Vector of voxel ids which are all neighboured
     */
399 400
    std::vector< size_t > getNeighbours( size_t id ) const;

401 402 403 404 405 406 407 408 409 410 411
    /**
     * Return the list of all neighbour voxels.
     *
     * \throw WOutOfBounds If the voxel id is outside of the grid.
     *
     * \param id Number of the voxel for which the neighbours should be computed
     *
     * \return Vector of voxel ids which are all neighboured
     */
    std::vector< size_t > getNeighbours27( size_t id ) const;

412 413 414 415 416 417 418 419 420 421 422 423 424
    /**
     * Return the list of all neighbour voxels.
     *
     * \throw WOutOfBounds If the voxel id is outside of the grid.
     *
     * \param id Number of the voxel for which the neighbours should be computed
     *
     * \param range neighborhood range selected. It specifies the distance to count as neighbour in each direction.
     *
     * \return Vector of voxel ids which are all neighboured
     */
    std::vector< size_t > getNeighboursRange( size_t id, size_t range ) const;

schurade's avatar
schurade committed
425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456
    /**
     * Return the list of all neighbour voxels.
     *
     * \throw WOutOfBounds If the voxel id is outside of the grid.
     *
     * \param id Number of the voxel for which the neighbours should be computed
     *
     * \return Vector of voxel ids which are all neighboured along the XY plane
     */
    std::vector< size_t > getNeighbours9XY( size_t id ) const;

    /**
     * Return the list of all neighbour voxels.
     *
     * \throw WOutOfBounds If the voxel id is outside of the grid.
     *
     * \param id Number of the voxel for which the neighbours should be computed
     *
     * \return Vector of voxel ids which are all neighboured along the YZ plane
     */
    std::vector< size_t > getNeighbours9YZ( size_t id ) const;

    /**
     * Return the list of all neighbour voxels.
     *
     * \throw WOutOfBounds If the voxel id is outside of the grid.
     *
     * \param id Number of the voxel for which the neighbours should be computed
     *
     * \return Vector of voxel ids which are all neighboured along the XZ plane
     */
    std::vector< size_t > getNeighbours9XZ( size_t id ) const;
457

458 459 460 461 462 463 464
    /**
     * Decides whether a certain position is inside this grid or not.
     *
     * \param pos Position to test
     *
     * \return True if and only if the given point is inside or on boundary of this grid, otherwise false.
     */
465
    bool encloses( const Vector3Type& pos ) const;
466

467 468
    /**
     * Return whether the transformations of the grid are only translation and/or scaling
469
     * \return Transformation does not contain rotation?
470
     */
471
    bool isNotRotated() const;
472 473

    /**
474 475
     * Returns the transformation used by this grid.
     * \return The transformation.
476
     */
477
    WGridTransformOrthoTemplate< T > const getTransform() const;
478

wiebel's avatar
wiebel committed
479 480
protected:
private:
481 482 483 484 485 486 487 488 489 490
    /**
     * Computes for the n'th component of the voxel coordinate where the voxel
     * contains the position pos.
     *
     * \param pos The position for which the n'th component of the voxel
     * coordinates should be computed.
     * \param axis The number of the component. (0 == x-axis, 1 == y-axis, ...)
     *
     * \return The n'th component of the voxel coordinate
     */
491
    int getNVoxelCoord( const Vector3Type& pos, size_t axis ) const;
492

493 494 495 496
    /**
     * Adds the specific information of this grid type to the
     * informational properties.
     */
497 498
    void initInformationProperties();

499 500 501
    unsigned int m_nbPosX; //!< Number of positions in x direction
    unsigned int m_nbPosY; //!< Number of positions in y direction
    unsigned int m_nbPosZ; //!< Number of positions in z direction
502

503
    //! The grid's transformation.
504
    WGridTransformOrthoTemplate< T > const m_transform;
wiebel's avatar
wiebel committed
505 506
};

507 508 509 510 511 512 513 514 515 516 517 518 519
// Convenience typedefs
typedef WGridRegular3DTemplate< double > WGridRegular3D;
typedef WGridRegular3DTemplate< double > WGridRegular3DDouble;
typedef WGridRegular3DTemplate< float > WGridRegular3DFloat;

template< typename T >
template< typename InputType >
WGridRegular3DTemplate< T >::WGridRegular3DTemplate( WGridRegular3DTemplate< InputType > const& rhs ) :
    WGrid( rhs.m_nbPosX * rhs.m_nbPosY * rhs.m_nbPosZ ),
    m_nbPosX( rhs.m_nbPosX ),
    m_nbPosY( rhs.m_nbPosY ),
    m_nbPosZ( rhs.m_nbPosZ ),
    m_transform( rhs.m_transform )
520
{
521
    initInformationProperties();
522 523
}

524 525 526 527 528 529 530 531
template< typename T >
WGridRegular3DTemplate< T >::WGridRegular3DTemplate( unsigned int nbPosX, unsigned int nbPosY, unsigned int nbPosZ,
                                     WGridTransformOrthoTemplate< T > const transform )
    : WGrid( nbPosX * nbPosY * nbPosZ ),
      m_nbPosX( nbPosX ),
      m_nbPosY( nbPosY ),
      m_nbPosZ( nbPosZ ),
      m_transform( transform )
532
{
533
    initInformationProperties();
534 535
}

536 537
template< typename T >
inline unsigned int WGridRegular3DTemplate< T >::getNbCoordsX() const
538 539 540 541
{
    return m_nbPosX;
}

542 543
template< typename T >
inline unsigned int WGridRegular3DTemplate< T >::getNbCoordsY() const
544 545 546 547
{
    return m_nbPosY;
}

548 549
template< typename T >
inline unsigned int WGridRegular3DTemplate< T >::getNbCoordsZ() const
550 551 552 553
{
    return m_nbPosZ;
}

554 555
template< typename T >
inline T WGridRegular3DTemplate< T >::getOffsetX() const
556
{
557
    return m_transform.getOffsetX();
558 559
}

560 561
template< typename T >
inline T WGridRegular3DTemplate< T >::getOffsetY() const
562
{
563
    return m_transform.getOffsetY();
564 565
}

566 567
template< typename T >
inline T WGridRegular3DTemplate< T >::getOffsetZ() const
568
{
569
    return m_transform.getOffsetZ();
570 571
}

572 573
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getDirectionX() const
574 575 576 577
{
    return m_transform.getDirectionX();
}

578 579
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getDirectionY() const
580 581 582 583
{
    return m_transform.getDirectionY();
}

584 585
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getDirectionZ() const
586 587 588 589
{
    return m_transform.getDirectionZ();
}

590 591
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getUnitDirectionX() const
592
{
593
    return m_transform.getUnitDirectionX();
594 595
}

596 597
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getUnitDirectionY() const
598
{
599
    return m_transform.getUnitDirectionY();
600 601
}

602 603
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getUnitDirectionZ() const
604
{
605
    return m_transform.getUnitDirectionZ();
606 607
}

608 609
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getOrigin() const
610
{
611
    return m_transform.getOrigin();
612 613
}

614 615
template< typename T >
inline WMatrix< T > WGridRegular3DTemplate< T >::getTransformationMatrix() const
616
{
617
    return m_transform.getTransformationMatrix();
618 619
}

620 621
template< typename T >
inline WBoundingBox WGridRegular3DTemplate< T >::getBoundingBox() const
622
{
623
    WBoundingBox result;
624 625 626 627 628 629 630 631
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( 0.0,                0.0,                0.0            ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( getNbCoordsX() - 1, 0.0,                0.0            ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( 0.0,                getNbCoordsY() - 1, 0.0            ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( getNbCoordsX() - 1, getNbCoordsY() - 1, 0.0            ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( 0.0,                0.0,                getNbCoordsZ() - 1 ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( getNbCoordsX() - 1, 0.0,                getNbCoordsZ() - 1 ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( 0.0,                getNbCoordsY() - 1, getNbCoordsZ() - 1 ) ) );
    result.expandBy( m_transform.positionToWorldSpace( Vector3Type( getNbCoordsX() - 1, getNbCoordsY() - 1, getNbCoordsZ() - 1 ) ) );
632
    return result;
633 634
}

635 636
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getPosition( unsigned int i ) const
637
{
638
    return getPosition( i % m_nbPosX, ( i / m_nbPosX ) % m_nbPosY, i / ( m_nbPosX * m_nbPosY ) );
639 640
}

641 642 643 644
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::getPosition( unsigned int iX,
                                                                                                   unsigned int iY,
                                                                                                   unsigned int iZ ) const
645
{
646
    Vector3Type i( iX, iY, iZ );
647
    return m_transform.positionToWorldSpace( i );
648 649
}

650 651
template< typename T >
inline typename WGridRegular3DTemplate< T >::Vector3Type WGridRegular3DTemplate< T >::worldCoordToTexCoord( WGridRegular3DTemplate< T >::Vector3Type point ) // NOLINT -- too long line
652
{
653
    Vector3Type r( m_transform.positionToGridSpace( point ) );
654 655 656 657 658 659 660 661 662 663 664 665 666 667

    // Scale to [0,1]
    r[0] = r[0] / m_nbPosX;
    r[1] = r[1] / m_nbPosY;
    r[2] = r[2] / m_nbPosZ;

    // Correct the coordinates to have the position at the center of the texture voxel.
    r[0] += 0.5 / m_nbPosX;
    r[1] += 0.5 / m_nbPosY;
    r[2] += 0.5 / m_nbPosZ;

    return r;
}

668
template< typename T >
669
inline int WGridRegular3DTemplate< T >::getVoxelNum( const Vector3Type& pos ) const
670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693
{
    // Note: the reason for the +1 is that the first and last Voxel in a x-axis
    // row are cut.
    //
    //  y-axis
    //  _|_______     ___ this is the 3rd Voxel
    // 1 |   |   |   v
    //   |...............
    //  _|_:_|_:_|_:_|_:____ x-axis
    //   | : | : | : | :
    //   |.:...:...:...:.
    //   0   1   2
    int xVoxelCoord = getXVoxelCoord( pos );
    int yVoxelCoord = getYVoxelCoord( pos );
    int zVoxelCoord = getZVoxelCoord( pos );
    if( xVoxelCoord == -1 || yVoxelCoord == -1 || zVoxelCoord == -1 )
    {
        return -1;
    }
    return xVoxelCoord
         + yVoxelCoord * ( m_nbPosX )
         + zVoxelCoord * ( m_nbPosX ) * ( m_nbPosY );
}

694 695
template< typename T >
inline int WGridRegular3DTemplate< T >::getVoxelNum( const size_t x, const size_t y, const size_t z ) const
696 697 698 699 700 701 702 703 704
{
    // since we use size_t here only a check for the upper bounds is needed
    if( x > m_nbPosX || y > m_nbPosY || z > m_nbPosZ )
    {
        return -1;
    }
    return x + y * ( m_nbPosX ) + z * ( m_nbPosX ) * ( m_nbPosY );
}

705 706
template< typename T >
inline int WGridRegular3DTemplate< T >::getXVoxelCoord( const WGridRegular3DTemplate< T >::Vector3Type& pos ) const
707 708
{
    // the current get*Voxel stuff is too complicated anyway
709
    Vector3Type v = m_transform.positionToGridSpace( pos );
710 711

    // this part could be refactored into an inline function
712 713 714 715
    T d;
    v[ 2 ] = std::modf( v[ 0 ] + T( 0.5 ), &d );
    int i = static_cast< int >( v[ 0 ] >= T( 0.0 ) && v[ 0 ] < m_nbPosX - T( 1.0 ) );
    return -1 + i * static_cast< int >( T( 1.0 ) + d );
716 717
}

718 719
template< typename T >
inline int WGridRegular3DTemplate< T >::getYVoxelCoord( const WGridRegular3DTemplate< T >::Vector3Type& pos ) const
720
{
721
    Vector3Type v = m_transform.positionToGridSpace( pos );
722

723 724 725 726
    T d;
    v[ 0 ] = std::modf( v[ 1 ] + T( 0.5 ), &d );
    int i = static_cast< int >( v[ 1 ] >= T( 0.0 ) && v[ 1 ] < m_nbPosY - T( 1.0 ) );
    return -1 + i * static_cast< int >( T( 1.0 ) + d );
727 728
}

729 730
template< typename T >
inline int WGridRegular3DTemplate< T >::getZVoxelCoord( const WGridRegular3DTemplate< T >::Vector3Type& pos ) const
731
{
732
    Vector3Type v = m_transform.positionToGridSpace( pos );
733

734 735 736 737
    T d;
    v[ 0 ] = std::modf( v[ 2 ] + T( 0.5 ), &d );
    int i = static_cast< int >( v[ 2 ] >= T( 0.0 ) && v[ 2 ] < m_nbPosZ - T( 1.0 ) );
    return -1 + i * static_cast< int >( T( 1.0 ) + d );
738 739
}

740 741
template< typename T >
inline WVector3i WGridRegular3DTemplate< T >::getVoxelCoord( const WGridRegular3DTemplate< T >::Vector3Type& pos ) const
742 743 744 745 746 747 748 749
{
    WVector3i result;
    result[0] = getXVoxelCoord( pos );
    result[1] = getYVoxelCoord( pos );
    result[2] = getZVoxelCoord( pos );
    return result;
}

750 751
template< typename T >
inline size_t WGridRegular3DTemplate< T >::getCellId( const WGridRegular3DTemplate< T >::Vector3Type& pos, bool* success ) const
752
{
753
    Vector3Type v = m_transform.positionToGridSpace( pos );
754

755 756 757
    T xCellId = floor( v[0] );
    T yCellId = floor( v[1] );
    T zCellId = floor( v[2] );
758 759 760 761 762 763

    *success = xCellId >= 0 && yCellId >=0 && zCellId >= 0 && xCellId < m_nbPosX - 1 && yCellId < m_nbPosY -1 && zCellId < m_nbPosZ -1;

    return xCellId + yCellId * ( m_nbPosX - 1 ) + zCellId * ( m_nbPosX - 1 ) * ( m_nbPosY - 1 );
}

764 765
template< typename T >
inline typename WGridRegular3DTemplate< T >::CellVertexArray WGridRegular3DTemplate< T >::getCellVertexIds( size_t cellId ) const
766
{
767
    typename WGridRegular3DTemplate< T >::CellVertexArray vertices;
768 769 770 771 772 773 774 775
    size_t minVertexIdZ =  cellId / ( ( m_nbPosX - 1 ) * ( m_nbPosY - 1 ) );
    size_t remainderXY = cellId - minVertexIdZ * ( ( m_nbPosX - 1 ) * ( m_nbPosY - 1 ) );
    size_t minVertexIdY = remainderXY  / ( m_nbPosX - 1 );
    size_t minVertexIdX = remainderXY % ( m_nbPosX - 1 );

    size_t minVertexId = minVertexIdX + minVertexIdY * m_nbPosX + minVertexIdZ * m_nbPosX * m_nbPosY;

    vertices[0] = minVertexId;
776
    vertices[1] = vertices[0] + 1;
777
    vertices[2] = minVertexId + m_nbPosX;
778
    vertices[3] = vertices[2] + 1;
779
    vertices[4] = minVertexId + m_nbPosX * m_nbPosY;
780 781 782
    vertices[5] = vertices[4] + 1;
    vertices[6] = vertices[4] + m_nbPosX;
    vertices[7] = vertices[6] + 1;
783 784 785
    return vertices;
}

786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144
template< typename T >
boost::shared_ptr< std::vector< typename WGridRegular3DTemplate< T >::Vector3Type > > WGridRegular3DTemplate< T >::getVoxelVertices( const WGridRegular3DTemplate< T >::Vector3Type& point, const T margin ) const // NOLINT -- too long line
{
    typedef boost::shared_ptr< std::vector< Vector3Type > > ReturnType;
    ReturnType result = ReturnType( new std::vector< Vector3Type > );
    result->reserve( 8 );
    T halfMarginX = getOffsetX() / 2.0 - std::abs( margin );
    T halfMarginY = getOffsetY() / 2.0 - std::abs( margin );
    T halfMarginZ = getOffsetZ() / 2.0 - std::abs( margin );
    result->push_back( Vector3Type( point[0] - halfMarginX, point[1] - halfMarginY, point[2] - halfMarginZ ) ); // a
    result->push_back( Vector3Type( point[0] + halfMarginX, point[1] - halfMarginY, point[2] - halfMarginZ ) ); // b
    result->push_back( Vector3Type( point[0] + halfMarginX, point[1] - halfMarginY, point[2] + halfMarginZ ) ); // c
    result->push_back( Vector3Type( point[0] - halfMarginX, point[1] - halfMarginY, point[2] + halfMarginZ ) ); // d
    result->push_back( Vector3Type( point[0] - halfMarginX, point[1] + halfMarginY, point[2] - halfMarginZ ) ); // e
    result->push_back( Vector3Type( point[0] + halfMarginX, point[1] + halfMarginY, point[2] - halfMarginZ ) ); // f
    result->push_back( Vector3Type( point[0] + halfMarginX, point[1] + halfMarginY, point[2] + halfMarginZ ) ); // g
    result->push_back( Vector3Type( point[0] - halfMarginX, point[1] + halfMarginY, point[2] + halfMarginZ ) ); // h
    return result;
}

template< typename T >
std::vector< size_t > WGridRegular3DTemplate< T >::getNeighbours( size_t id ) const
{
    std::vector< size_t > neighbours;
    size_t x = id % m_nbPosX;
    size_t y = ( id / m_nbPosX ) % m_nbPosY;
    size_t z = id / ( m_nbPosX * m_nbPosY );

    if( x >= m_nbPosX || y >= m_nbPosY || z >= m_nbPosZ )
    {
        std::stringstream ss;
        ss << "This point: " << id << " is not part of this grid: ";
        ss << " nbPosX: " << m_nbPosX;
        ss << " nbPosY: " << m_nbPosY;
        ss << " nbPosZ: " << m_nbPosZ;
        throw WOutOfBounds( ss.str() );
    }
    // for every neighbour we must check if its not on the boundary, it will be skipped otherwise
    if( x > 0 )
    {
        neighbours.push_back( id - 1 );
    }
    if( x < m_nbPosX - 1 )
    {
        neighbours.push_back( id + 1 );
    }
    if( y > 0 )
    {
        neighbours.push_back( id - m_nbPosX );
    }
    if( y < m_nbPosY - 1 )
    {
        neighbours.push_back( id + m_nbPosX );
    }
    if( z > 0 )
    {
        neighbours.push_back( id - ( m_nbPosX * m_nbPosY ) );
    }
    if( z < m_nbPosZ - 1 )
    {
         neighbours.push_back( id + ( m_nbPosX * m_nbPosY ) );
    }
    return neighbours;
}

template< typename T >
std::vector< size_t > WGridRegular3DTemplate< T >::getNeighbours27( size_t id ) const
{