OpenWalnut  1.4.0
WPlane.cpp
00001 //---------------------------------------------------------------------------
00002 //
00003 // Project: OpenWalnut ( http://www.openwalnut.org )
00004 //
00005 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
00006 // For more information see http://www.openwalnut.org/copying
00007 //
00008 // This file is part of OpenWalnut.
00009 //
00010 // OpenWalnut is free software: you can redistribute it and/or modify
00011 // it under the terms of the GNU Lesser General Public License as published by
00012 // the Free Software Foundation, either version 3 of the License, or
00013 // (at your option) any later version.
00014 //
00015 // OpenWalnut is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 // GNU Lesser General Public License for more details.
00019 //
00020 // You should have received a copy of the GNU Lesser General Public License
00021 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
00022 //
00023 //---------------------------------------------------------------------------
00024 
00025 #include <set>
00026 
00027 #include <boost/shared_ptr.hpp>
00028 
00029 #include "../../common/math/WLinearAlgebraFunctions.h"
00030 #include "../../common/math/linearAlgebra/WVectorFixed.h"
00031 #include "../../common/WAssert.h"
00032 #include "../../common/WLimits.h"
00033 
00034 #include "WPlane.h"
00035 
00036 WPlane::WPlane( const WVector3d& normal, const WPosition& pos )
00037     : m_normal( normal ),
00038       m_pos( pos )
00039 {
00040     setNormal( normal );
00041 }
00042 
00043 WPlane::WPlane( const WVector3d& normal, const WPosition& pos, const WVector3d& first, const WVector3d& second )
00044     : m_normal( normal ),
00045       m_pos( pos )
00046 {
00047     setPlaneVectors( first, second );
00048     m_first = normalize( m_first );
00049     m_second = normalize( m_second );
00050 }
00051 
00052 WPlane::~WPlane()
00053 {
00054 }
00055 
00056 bool WPlane::isInPlane( WPosition /* point */ ) const
00057 {
00058     // TODO(math): implement this: pos + first*m + second*n == point ??
00059     return false;
00060 }
00061 
00062 void WPlane::resetPosition( WPosition newPos )
00063 {
00064     // TODO(math): check if pos is in plane first!
00065     m_pos = newPos;
00066 }
00067 
00068 
00069 boost::shared_ptr< std::set< WPosition > > WPlane::samplePoints( double stepWidth, size_t numX, size_t numY ) const
00070 {
00071     // idea: start from m_pos in m_first direction until boundary reached, increment in m_second direction from m_pos and start again
00072     boost::shared_ptr< std::set< WPosition > > result( new std::set< WPosition >() );
00073 
00074     // the plane has two directions m_first and m_second
00075     const WVector3d ycrement = stepWidth * m_second;
00076     const WVector3d xcrement = stepWidth * m_first;
00077     result->insert( m_pos );
00078     for( size_t i = 0; i < numY; ++i )
00079     {
00080         for( size_t j = 0; j < numX; ++j )
00081         {
00082             // NOTE: on some machines with older compilers, the size_t is not mapped to uint32_t or uint64_t. This
00083             // breaks our WMatrixFixed type promotion. So we use doubles directly.
00084             double id = i;
00085             double jd = j;
00086             result->insert( m_pos - id * ycrement - jd * xcrement );
00087             result->insert( m_pos + id * ycrement - jd * xcrement );
00088             result->insert( m_pos - id * ycrement + jd * xcrement );
00089             result->insert( m_pos + id * ycrement + jd * xcrement );
00090         }
00091     }
00092     return result;
00093 }
00094 
00095 // boost::shared_ptr< std::set< WPosition > > WPlane::samplePoints( const WGridRegular3D& grid, double stepWidth )
00096 // {
00097 //     // idea: start from m_pos in m_first direction until boundary reached, increment in m_second direction from m_pos and start again
00098 //     boost::shared_ptr< std::set< WPosition > > result( new std::set< WPosition >() );
00099 //
00100 //     // the plane has two directions m_first and m_second
00101 //     const WVector3d ycrement = stepWidth * m_second;
00102 //     const WVector3d xcrement = stepWidth * m_first;
00103 //     WPosition y_offset_up = m_pos;
00104 //     WPosition y_offset_down = m_pos;
00105 //     WPosition x_offset_right = m_pos;
00106 //     WPosition x_offset_left = m_pos;
00107 //     // TODO(math): assert( grid.encloses( m_pos ) );
00108 //     while( grid.encloses( y_offset_up ) || grid.encloses( y_offset_down ) )
00109 //     {
00110 //         if( grid.encloses( y_offset_up ) ) // walk up
00111 //         {
00112 //             x_offset_right = y_offset_up;
00113 //             x_offset_left = y_offset_up;
00114 //             while( grid.encloses( x_offset_right ) || grid.encloses( x_offset_left ) )
00115 //             {
00116 //                 if( grid.encloses( x_offset_right ) )
00117 //                 {
00118 //                     result->insert( x_offset_right );
00119 //                     x_offset_right += xcrement;
00120 //                 }
00121 //                 if( grid.encloses( x_offset_left ) )
00122 //                 {
00123 //                     result->insert( x_offset_left );
00124 //                     x_offset_left -= xcrement;
00125 //                 }
00126 //             }
00127 //             y_offset_up += ycrement;
00128 //         }
00129 //         if( grid.encloses( y_offset_down ) ) // walk down
00130 //         {
00131 //             x_offset_right = y_offset_down;
00132 //             x_offset_left = y_offset_down;
00133 //             while( grid.encloses( x_offset_right ) || grid.encloses( x_offset_left ) )
00134 //             {
00135 //                 if( grid.encloses( x_offset_right ) )
00136 //                 {
00137 //                     result->insert( x_offset_right );
00138 //                     x_offset_right += xcrement;
00139 //                 }
00140 //                 if( grid.encloses( x_offset_left ) )
00141 //                 {
00142 //                     result->insert( x_offset_left );
00143 //                     x_offset_left -= xcrement;
00144 //                 }
00145 //             }
00146 //             y_offset_down -= ycrement;
00147 //         }
00148 //     }
00149 //
00150 //     return result;
00151 // }
00152 
00153 WPosition WPlane::getPointInPlane( double x, double y ) const
00154 {
00155     WVector3d sd= m_pos +
00156                     x * m_first
00157                     +
00158                     y * m_second;
00159     return sd;
00160 }
00161 
00162 void WPlane::setPlaneVectors( const WVector3d& first, const WVector3d& second )
00163 {
00164     std::stringstream msg;
00165     msg << "The give two vectors are not perpendicular to plane. First: " << first << " second: " << second << " for plane normal: " << m_normal;
00166     WAssert( std::abs( dot( first, m_normal ) ) < wlimits::FLT_EPS && std::abs( dot( second, m_normal ) ) < wlimits::FLT_EPS, msg.str() );
00167 
00168     std::stringstream msg2;
00169     msg2 << "The given two vectors are not linear independent!: " << first << " and " << second;
00170     WAssert( linearIndependent( first, second ), msg2.str() );
00171 
00172     m_first = first;
00173     m_second = second;
00174 }