00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <cmath>
00026 #include <string>
00027 #include <vector>
00028
00029 #include "../common/exceptions/WOutOfBounds.h"
00030 #include "../common/exceptions/WPreconditionNotMet.h"
00031 #include "../common/math/WLinearAlgebraFunctions.h"
00032 #include "../common/WBoundingBox.h"
00033 #include "../common/WProperties.h"
00034 #include "../common/math/linearAlgebra/WLinearAlgebra.h"
00035
00036 #include "WGridRegular3D.h"
00037
00038 WGridRegular3D::WGridRegular3D( unsigned int nbPosX, unsigned int nbPosY, unsigned int nbPosZ,
00039 WGridTransformOrtho const transform )
00040 : WGrid( nbPosX * nbPosY * nbPosZ ),
00041 m_nbPosX( nbPosX ),
00042 m_nbPosY( nbPosY ),
00043 m_nbPosZ( nbPosZ ),
00044 m_transform( transform )
00045 {
00046 initInformationProperties();
00047 }
00048
00049 WPosition WGridRegular3D::getPosition( unsigned int i ) const
00050 {
00051 return getPosition( i % m_nbPosX, ( i / m_nbPosX ) % m_nbPosY, i / ( m_nbPosX * m_nbPosY ) );
00052 }
00053
00054 WPosition WGridRegular3D::getPosition( unsigned int iX, unsigned int iY, unsigned int iZ ) const
00055 {
00056 WVector3d i( iX, iY, iZ );
00057 return m_transform.positionToWorldSpace( i );
00058 }
00059
00060 WVector3d WGridRegular3D::worldCoordToTexCoord( WPosition point )
00061 {
00062 WVector3d r( m_transform.positionToGridSpace( point ) );
00063
00064
00065 r[0] = r[0] / m_nbPosX;
00066 r[1] = r[1] / m_nbPosY;
00067 r[2] = r[2] / m_nbPosZ;
00068
00069
00070 r[0] += 0.5 / m_nbPosX;
00071 r[1] += 0.5 / m_nbPosY;
00072 r[2] += 0.5 / m_nbPosZ;
00073
00074 return r;
00075 }
00076
00077 int WGridRegular3D::getVoxelNum( const WPosition& pos ) const
00078 {
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090 int xVoxelCoord = getXVoxelCoord( pos );
00091 int yVoxelCoord = getYVoxelCoord( pos );
00092 int zVoxelCoord = getZVoxelCoord( pos );
00093 if( xVoxelCoord == -1 || yVoxelCoord == -1 || zVoxelCoord == -1 )
00094 {
00095 return -1;
00096 }
00097 return xVoxelCoord
00098 + yVoxelCoord * ( m_nbPosX )
00099 + zVoxelCoord * ( m_nbPosX ) * ( m_nbPosY );
00100 }
00101
00102 int WGridRegular3D::getVoxelNum( const size_t x, const size_t y, const size_t z ) const
00103 {
00104
00105 if( x > m_nbPosX || y > m_nbPosY || z > m_nbPosZ )
00106 {
00107 return -1;
00108 }
00109 return x + y * ( m_nbPosX ) + z * ( m_nbPosX ) * ( m_nbPosY );
00110 }
00111
00112
00113 void WGridRegular3D::initInformationProperties()
00114 {
00115 WPropInt xDim = m_infoProperties->addProperty( "X dimension: ", "The x dimension of the grid.", static_cast<int>( getNbCoordsX() ) );
00116 WPropInt yDim = m_infoProperties->addProperty( "Y dimension: ", "The y dimension of the grid.", static_cast<int>( getNbCoordsY() ) );
00117 WPropInt zDim = m_infoProperties->addProperty( "Z dimension: ", "The z dimension of the grid.", static_cast<int>( getNbCoordsZ() ) );
00118 WPropDouble xOffset = m_infoProperties->addProperty( "X offset: ", "The distance between samples in x direction", getOffsetX() );
00119 WPropDouble yOffset = m_infoProperties->addProperty( "Y offset: ", "The distance between samples in y direction", getOffsetY() );
00120 WPropDouble zOffset = m_infoProperties->addProperty( "Z offset: ", "The distance between samples in z direction", getOffsetZ() );
00121 WPropMatrix4X4 transformation = m_infoProperties->addProperty( "Transformation", "The transformation of this grid.",
00122 static_cast< WMatrix4d >( getTransform() ) );
00123 }
00124
00125 int WGridRegular3D::getXVoxelCoord( const WPosition& pos ) const
00126 {
00127
00128 WPosition v = m_transform.positionToGridSpace( pos );
00129
00130
00131 double d;
00132 v[ 2 ] = modf( v[ 0 ] + 0.5, &d );
00133 int i = static_cast< int >( v[ 0 ] >= 0.0 && v[ 0 ] < m_nbPosX - 1.0 );
00134 return -1 + i * static_cast< int >( 1.0 + d );
00135 }
00136
00137 int WGridRegular3D::getYVoxelCoord( const WPosition& pos ) const
00138 {
00139 WPosition v = m_transform.positionToGridSpace( pos );
00140
00141 double d;
00142 v[ 0 ] = modf( v[ 1 ] + 0.5, &d );
00143 int i = static_cast< int >( v[ 1 ] >= 0.0 && v[ 1 ] < m_nbPosY - 1.0 );
00144 return -1 + i * static_cast< int >( 1.0 + d );
00145 }
00146
00147 int WGridRegular3D::getZVoxelCoord( const WPosition& pos ) const
00148 {
00149 WPosition v = m_transform.positionToGridSpace( pos );
00150
00151 double d;
00152 v[ 0 ] = modf( v[ 2 ] + 0.5, &d );
00153 int i = static_cast< int >( v[ 2 ] >= 0.0 && v[ 2 ] < m_nbPosZ - 1.0 );
00154 return -1 + i * static_cast< int >( 1.0 + d );
00155 }
00156
00157 WValue< int > WGridRegular3D::getVoxelCoord( const WPosition& pos ) const
00158 {
00159 WValue< int > result( 3 );
00160 result[0] = getXVoxelCoord( pos );
00161 result[1] = getYVoxelCoord( pos );
00162 result[2] = getZVoxelCoord( pos );
00163 return result;
00164 }
00165
00166 bool WGridRegular3D::isNotRotated() const
00167 {
00168 return m_transform.isNotRotated();
00169 }
00170
00171 size_t WGridRegular3D::getCellId( const WPosition& pos, bool* success ) const
00172 {
00173 WAssert( isNotRotated(), "Only feasible for grids that are only translated or scaled so far." );
00174
00175 WPosition posRelativeToOrigin = pos - getOrigin();
00176
00177 double xCellId = floor( posRelativeToOrigin[0] / getOffsetX() );
00178 double yCellId = floor( posRelativeToOrigin[1] / getOffsetY() );
00179 double zCellId = floor( posRelativeToOrigin[2] / getOffsetZ() );
00180
00181 *success = xCellId >= 0 && yCellId >=0 && zCellId >= 0 && xCellId < m_nbPosX - 1 && yCellId < m_nbPosY -1 && zCellId < m_nbPosZ -1;
00182
00183 return xCellId + yCellId * ( m_nbPosX - 1 ) + zCellId * ( m_nbPosX - 1 ) * ( m_nbPosY - 1 );
00184 }
00185
00186 std::vector< size_t > WGridRegular3D::getCellVertexIds( size_t cellId ) const
00187 {
00188 std::vector< size_t > vertices( 8 );
00189 size_t minVertexIdZ = cellId / ( ( m_nbPosX - 1 ) * ( m_nbPosY - 1 ) );
00190 size_t remainderXY = cellId - minVertexIdZ * ( ( m_nbPosX - 1 ) * ( m_nbPosY - 1 ) );
00191 size_t minVertexIdY = remainderXY / ( m_nbPosX - 1 );
00192 size_t minVertexIdX = remainderXY % ( m_nbPosX - 1 );
00193
00194 size_t minVertexId = minVertexIdX + minVertexIdY * m_nbPosX + minVertexIdZ * m_nbPosX * m_nbPosY;
00195
00196 vertices[0] = minVertexId;
00197 vertices[1] = minVertexId + 1;
00198 vertices[2] = minVertexId + m_nbPosX;
00199 vertices[3] = minVertexId + m_nbPosX +1;
00200 vertices[4] = minVertexId + m_nbPosX * m_nbPosY;
00201 vertices[5] = minVertexId + m_nbPosX * m_nbPosY + 1;
00202 vertices[6] = minVertexId + m_nbPosX * m_nbPosY + m_nbPosX;
00203 vertices[7] = minVertexId + m_nbPosX * m_nbPosY + m_nbPosX +1;
00204 return vertices;
00205 }
00206
00207 boost::shared_ptr< std::vector< WPosition > > WGridRegular3D::getVoxelVertices( const WPosition& point, const double margin ) const
00208 {
00209 typedef boost::shared_ptr< std::vector< WPosition > > ReturnType;
00210 ReturnType result = ReturnType( new std::vector< WPosition > );
00211 result->reserve( 8 );
00212 double halfMarginX = getOffsetX() / 2.0 - std::abs( margin );
00213 double halfMarginY = getOffsetY() / 2.0 - std::abs( margin );
00214 double halfMarginZ = getOffsetZ() / 2.0 - std::abs( margin );
00215 result->push_back( WPosition( point[0] - halfMarginX, point[1] - halfMarginY, point[2] - halfMarginZ ) );
00216 result->push_back( WPosition( point[0] + halfMarginX, point[1] - halfMarginY, point[2] - halfMarginZ ) );
00217 result->push_back( WPosition( point[0] + halfMarginX, point[1] - halfMarginY, point[2] + halfMarginZ ) );
00218 result->push_back( WPosition( point[0] - halfMarginX, point[1] - halfMarginY, point[2] + halfMarginZ ) );
00219 result->push_back( WPosition( point[0] - halfMarginX, point[1] + halfMarginY, point[2] - halfMarginZ ) );
00220 result->push_back( WPosition( point[0] + halfMarginX, point[1] + halfMarginY, point[2] - halfMarginZ ) );
00221 result->push_back( WPosition( point[0] + halfMarginX, point[1] + halfMarginY, point[2] + halfMarginZ ) );
00222 result->push_back( WPosition( point[0] - halfMarginX, point[1] + halfMarginY, point[2] + halfMarginZ ) );
00223 return result;
00224 }
00225
00226 std::vector< size_t > WGridRegular3D::getNeighbours( size_t id ) const
00227 {
00228 std::vector< size_t > neighbours;
00229 size_t x = id % m_nbPosX;
00230 size_t y = ( id / m_nbPosX ) % m_nbPosY;
00231 size_t z = id / ( m_nbPosX * m_nbPosY );
00232
00233 if( x >= m_nbPosX || y >= m_nbPosY || z >= m_nbPosZ )
00234 {
00235 std::stringstream ss;
00236 ss << "This point: " << id << " is not part of this grid: ";
00237 ss << " nbPosX: " << m_nbPosX;
00238 ss << " nbPosY: " << m_nbPosY;
00239 ss << " nbPosZ: " << m_nbPosZ;
00240 throw WOutOfBounds( ss.str() );
00241 }
00242
00243 if( x > 0 )
00244 {
00245 neighbours.push_back( id - 1 );
00246 }
00247 if( x < m_nbPosX - 1 )
00248 {
00249 neighbours.push_back( id + 1 );
00250 }
00251 if( y > 0 )
00252 {
00253 neighbours.push_back( id - m_nbPosX );
00254 }
00255 if( y < m_nbPosY - 1 )
00256 {
00257 neighbours.push_back( id + m_nbPosX );
00258 }
00259 if( z > 0 )
00260 {
00261 neighbours.push_back( id - ( m_nbPosX * m_nbPosY ) );
00262 }
00263 if( z < m_nbPosZ - 1 )
00264 {
00265 neighbours.push_back( id + ( m_nbPosX * m_nbPosY ) );
00266 }
00267 return neighbours;
00268 }
00269
00270 std::vector< size_t > WGridRegular3D::getNeighbours27( size_t id ) const
00271 {
00272 std::vector< size_t > neighbours;
00273 size_t x = id % m_nbPosX;
00274 size_t y = ( id / m_nbPosX ) % m_nbPosY;
00275 size_t z = id / ( m_nbPosX * m_nbPosY );
00276
00277 if( x >= m_nbPosX || y >= m_nbPosY || z >= m_nbPosZ )
00278 {
00279 std::stringstream ss;
00280 ss << "This point: " << id << " is not part of this grid: ";
00281 ss << " nbPosX: " << m_nbPosX;
00282 ss << " nbPosY: " << m_nbPosY;
00283 ss << " nbPosZ: " << m_nbPosZ;
00284 throw WOutOfBounds( ss.str() );
00285 }
00286
00287 std::vector< int >tempResult;
00288
00289 tempResult.push_back( getVoxelNum( x , y , z ) );
00290 tempResult.push_back( getVoxelNum( x , y - 1, z ) );
00291 tempResult.push_back( getVoxelNum( x , y + 1, z ) );
00292 tempResult.push_back( getVoxelNum( x - 1, y , z ) );
00293 tempResult.push_back( getVoxelNum( x - 1, y - 1, z ) );
00294 tempResult.push_back( getVoxelNum( x - 1, y + 1, z ) );
00295 tempResult.push_back( getVoxelNum( x + 1, y , z ) );
00296 tempResult.push_back( getVoxelNum( x + 1, y - 1, z ) );
00297 tempResult.push_back( getVoxelNum( x + 1, y + 1, z ) );
00298
00299 tempResult.push_back( getVoxelNum( x , y , z - 1 ) );
00300 tempResult.push_back( getVoxelNum( x , y - 1, z - 1 ) );
00301 tempResult.push_back( getVoxelNum( x , y + 1, z - 1 ) );
00302 tempResult.push_back( getVoxelNum( x - 1, y , z - 1 ) );
00303 tempResult.push_back( getVoxelNum( x - 1, y - 1, z - 1 ) );
00304 tempResult.push_back( getVoxelNum( x - 1, y + 1, z - 1 ) );
00305 tempResult.push_back( getVoxelNum( x + 1, y , z - 1 ) );
00306 tempResult.push_back( getVoxelNum( x + 1, y - 1, z - 1 ) );
00307 tempResult.push_back( getVoxelNum( x + 1, y + 1, z - 1 ) );
00308
00309 tempResult.push_back( getVoxelNum( x , y , z + 1 ) );
00310 tempResult.push_back( getVoxelNum( x , y - 1, z + 1 ) );
00311 tempResult.push_back( getVoxelNum( x , y + 1, z + 1 ) );
00312 tempResult.push_back( getVoxelNum( x - 1, y , z + 1 ) );
00313 tempResult.push_back( getVoxelNum( x - 1, y - 1, z + 1 ) );
00314 tempResult.push_back( getVoxelNum( x - 1, y + 1, z + 1 ) );
00315 tempResult.push_back( getVoxelNum( x + 1, y , z + 1 ) );
00316 tempResult.push_back( getVoxelNum( x + 1, y - 1, z + 1 ) );
00317 tempResult.push_back( getVoxelNum( x + 1, y + 1, z + 1 ) );
00318
00319 for( size_t k = 0; k < tempResult.size(); ++k )
00320 {
00321 if( tempResult[k] != -1 )
00322 {
00323 neighbours.push_back( tempResult[k] );
00324 }
00325 }
00326 return neighbours;
00327 }
00328
00329 std::vector< size_t > WGridRegular3D::getNeighbours9XY( size_t id ) const
00330 {
00331 std::vector< size_t > neighbours;
00332 size_t x = id % m_nbPosX;
00333 size_t y = ( id / m_nbPosX ) % m_nbPosY;
00334 size_t z = id / ( m_nbPosX * m_nbPosY );
00335
00336 if( x >= m_nbPosX || y >= m_nbPosY || z >= m_nbPosZ )
00337 {
00338 std::stringstream ss;
00339 ss << "This point: " << id << " is not part of this grid: ";
00340 ss << " nbPosX: " << m_nbPosX;
00341 ss << " nbPosY: " << m_nbPosY;
00342 ss << " nbPosZ: " << m_nbPosZ;
00343 throw WOutOfBounds( ss.str() );
00344 }
00345
00346 int vNum;
00347
00348 vNum = getVoxelNum( x - 1, y, z );
00349 if( vNum != -1 )
00350 {
00351 neighbours.push_back( vNum );
00352 }
00353 vNum = getVoxelNum( x - 1, y - 1, z );
00354 if( vNum != -1 )
00355 {
00356 neighbours.push_back( vNum );
00357 }
00358 vNum = getVoxelNum( x, y - 1, z );
00359 if( vNum != -1 )
00360 {
00361 neighbours.push_back( vNum );
00362 }
00363 vNum = getVoxelNum( x + 1, y - 1, z );
00364 if( vNum != -1 )
00365 {
00366 neighbours.push_back( vNum );
00367 }
00368 vNum = getVoxelNum( x + 1, y, z );
00369 if( vNum != -1 )
00370 {
00371 neighbours.push_back( vNum );
00372 }
00373 vNum = getVoxelNum( x + 1, y + 1, z );
00374 if( vNum != -1 )
00375 {
00376 neighbours.push_back( vNum );
00377 }
00378 vNum = getVoxelNum( x, y + 1, z );
00379 if( vNum != -1 )
00380 {
00381 neighbours.push_back( vNum );
00382 }
00383 vNum = getVoxelNum( x - 1, y + 1, z );
00384 if( vNum != -1 )
00385 {
00386 neighbours.push_back( vNum );
00387 }
00388 return neighbours;
00389 }
00390
00391 std::vector< size_t > WGridRegular3D::getNeighbours9YZ( size_t id ) const
00392 {
00393 std::vector< size_t > neighbours;
00394 size_t x = id % m_nbPosX;
00395 size_t y = ( id / m_nbPosX ) % m_nbPosY;
00396 size_t z = id / ( m_nbPosX * m_nbPosY );
00397
00398 if( x >= m_nbPosX || y >= m_nbPosY || z >= m_nbPosZ )
00399 {
00400 std::stringstream ss;
00401 ss << "This point: " << id << " is not part of this grid: ";
00402 ss << " nbPosX: " << m_nbPosX;
00403 ss << " nbPosY: " << m_nbPosY;
00404 ss << " nbPosZ: " << m_nbPosZ;
00405 throw WOutOfBounds( ss.str() );
00406 }
00407
00408 int vNum;
00409
00410 vNum = getVoxelNum( x, y, z - 1 );
00411 if( vNum != -1 )
00412 {
00413 neighbours.push_back( vNum );
00414 }
00415 vNum = getVoxelNum( x, y - 1, z - 1 );
00416 if( vNum != -1 )
00417 {
00418 neighbours.push_back( vNum );
00419 }
00420 vNum = getVoxelNum( x, y - 1, z );
00421 if( vNum != -1 )
00422 {
00423 neighbours.push_back( vNum );
00424 }
00425 vNum = getVoxelNum( x, y - 1, z + 1 );
00426 if( vNum != -1 )
00427 {
00428 neighbours.push_back( vNum );
00429 }
00430 vNum = getVoxelNum( x, y, z + 1 );
00431 if( vNum != -1 )
00432 {
00433 neighbours.push_back( vNum );
00434 }
00435 vNum = getVoxelNum( x, y + 1, z + 1 );
00436 if( vNum != -1 )
00437 {
00438 neighbours.push_back( vNum );
00439 }
00440 vNum = getVoxelNum( x, y + 1, z );
00441 if( vNum != -1 )
00442 {
00443 neighbours.push_back( vNum );
00444 }
00445 vNum = getVoxelNum( x, y + 1, z - 1 );
00446 if( vNum != -1 )
00447 {
00448 neighbours.push_back( vNum );
00449 }
00450
00451 return neighbours;
00452 }
00453
00454 std::vector< size_t > WGridRegular3D::getNeighbours9XZ( size_t id ) const
00455 {
00456 std::vector< size_t > neighbours;
00457 size_t x = id % m_nbPosX;
00458 size_t y = ( id / m_nbPosX ) % m_nbPosY;
00459 size_t z = id / ( m_nbPosX * m_nbPosY );
00460
00461 if( x >= m_nbPosX || y >= m_nbPosY || z >= m_nbPosZ )
00462 {
00463 std::stringstream ss;
00464 ss << "This point: " << id << " is not part of this grid: ";
00465 ss << " nbPosX: " << m_nbPosX;
00466 ss << " nbPosY: " << m_nbPosY;
00467 ss << " nbPosZ: " << m_nbPosZ;
00468 throw WOutOfBounds( ss.str() );
00469 }
00470
00471 int vNum;
00472
00473 vNum = getVoxelNum( x, y, z - 1 );
00474 if( vNum != -1 )
00475 {
00476 neighbours.push_back( vNum );
00477 }
00478 vNum = getVoxelNum( x - 1, y, z - 1 );
00479 if( vNum != -1 )
00480 {
00481 neighbours.push_back( vNum );
00482 }
00483 vNum = getVoxelNum( x - 1, y, z );
00484 if( vNum != -1 )
00485 {
00486 neighbours.push_back( vNum );
00487 }
00488 vNum = getVoxelNum( x - 1, y, z + 1 );
00489 if( vNum != -1 )
00490 {
00491 neighbours.push_back( vNum );
00492 }
00493 vNum = getVoxelNum( x, y, z + 1 );
00494 if( vNum != -1 )
00495 {
00496 neighbours.push_back( vNum );
00497 }
00498 vNum = getVoxelNum( x + 1, y, z + 1 );
00499 if( vNum != -1 )
00500 {
00501 neighbours.push_back( vNum );
00502 }
00503 vNum = getVoxelNum( x + 1, y, z );
00504 if( vNum != -1 )
00505 {
00506 neighbours.push_back( vNum );
00507 }
00508 vNum = getVoxelNum( x + 1, y, z - 1 );
00509 if( vNum != -1 )
00510 {
00511 neighbours.push_back( vNum );
00512 }
00513
00514 return neighbours;
00515 }
00516
00517 bool WGridRegular3D::encloses( WPosition const& pos ) const
00518 {
00519 WVector3d v = m_transform.positionToGridSpace( pos );
00520
00521 if( v[ 0 ] < 0.0 || v[ 0 ] >= m_nbPosX - 1 )
00522 {
00523 return false;
00524 }
00525 if( v[ 1 ] < 0.0 || v[ 1 ] >= m_nbPosY - 1 )
00526 {
00527 return false;
00528 }
00529 if( v[ 2 ] < 0.0 || v[ 2 ] >= m_nbPosZ - 1 )
00530 {
00531 return false;
00532 }
00533 return true;
00534 }
00535
00536 WBoundingBox WGridRegular3D::getBoundingBox() const
00537 {
00538 WBoundingBox result;
00539 result.expandBy( m_transform.positionToWorldSpace( WPosition( 0.0, 0.0, 0.0 ) ) );
00540 result.expandBy( m_transform.positionToWorldSpace( WPosition( getNbCoordsX() - 1, 0.0, 0.0 ) ) );
00541 result.expandBy( m_transform.positionToWorldSpace( WPosition( 0.0, getNbCoordsY() - 1, 0.0 ) ) );
00542 result.expandBy( m_transform.positionToWorldSpace( WPosition( getNbCoordsX() - 1, getNbCoordsY() - 1, 0.0 ) ) );
00543 result.expandBy( m_transform.positionToWorldSpace( WPosition( 0.0, 0.0, getNbCoordsZ() - 1 ) ) );
00544 result.expandBy( m_transform.positionToWorldSpace( WPosition( getNbCoordsX() - 1, 0.0, getNbCoordsZ() - 1 ) ) );
00545 result.expandBy( m_transform.positionToWorldSpace( WPosition( 0.0, getNbCoordsY() - 1, getNbCoordsZ() - 1 ) ) );
00546 result.expandBy( m_transform.positionToWorldSpace( WPosition( getNbCoordsX() - 1, getNbCoordsY() - 1, getNbCoordsZ() - 1 ) ) );
00547 return result;
00548 }