OpenWalnut  1.4.0
WMath.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 "WMath.h"
00026 #include "WPlane.h"
00027 #include "linearAlgebra/WPosition.h"
00028 #include "linearAlgebra/WVectorFixed.h"
00029 #include "../WAssert.h"
00030 #include "../WLimits.h"
00031 
00032 bool testIntersectTriangle( const WPosition& p1, const WPosition& p2, const WPosition& p3, const WPlane& p )
00033 {
00034     const WVector3d& normal = p.getNormal();
00035     const WPosition& planePoint = p.getPosition();
00036 
00037     double r1 = dot( normal, p1 - planePoint );
00038     double r2 = dot( normal, p2 - planePoint );
00039     double r3 = dot( normal, p3 - planePoint );
00040 
00041     // TODO(math): use signum here!
00042     if( std::abs( ( ( r1 > 0 ) - ( r1 < 0 ) ) + ( ( r2 > 0) - ( r2 < 0 ) ) + ( ( r3 > 0 ) - ( r3 < 0 ) ) ) == 3 )
00043     {
00044         return false;
00045     }
00046     return true;
00047 }
00048 
00049 bool intersectPlaneSegment( const WPlane& p,
00050                                    const WPosition& p1,
00051                                    const WPosition& p2,
00052                                    boost::shared_ptr< WPosition > pointOfIntersection )
00053 {
00054     const WVector3d& normal = normalize( p.getNormal() );
00055     double const d = dot( normal, p.getPosition() );
00056     WAssert( pointOfIntersection.get(), "Place to store a point of intersection is not ready!" );
00057     *pointOfIntersection = p.getPosition();   // otherwise it would be undefined
00058 
00059     // at least one point is in plane (maybe the whole segment)
00060     if( std::abs( dot( normal, p1 - p.getPosition() ) ) <= 2 * wlimits::DBL_EPS )
00061     {
00062         *pointOfIntersection = p1;
00063         return true;
00064     }
00065     else if( std::abs( dot( normal, p2 - p.getPosition() ) ) <= 2 * wlimits::DBL_EPS )
00066     {
00067         *pointOfIntersection = p2;
00068         return true;
00069     }
00070 
00071     if( std::abs( dot( normal,  p1 - p2 ) ) < wlimits::DBL_EPS ) // plane and line are parallel
00072     {
00073         return false;
00074     }
00075 
00076     double const t = ( d - dot( normal, p2 ) ) / ( dot( normal, p1 - p2 ) );
00077 
00078     *pointOfIntersection = p2 + t * ( p1 - p2 );
00079 
00080     if( t >= -wlimits::DBL_EPS && t <= ( 1.0 + wlimits::DBL_EPS ) )
00081     {
00082         return true;
00083     }
00084     return false;
00085 }
00086 
00087 bool intersectPlaneLineNearCP( const WPlane& p, const WLine& l, boost::shared_ptr< WPosition > cutPoint )
00088 {
00089     bool result = false;
00090     double minDistance = wlimits::MAX_DOUBLE;
00091     WAssert( cutPoint.get(), "Place to store a point of intersection is not ready!" );
00092     *cutPoint = WPosition( 0, 0, 0 );
00093     for( size_t i = 1; i < l.size(); ++i ) // test each segment
00094     {
00095         boost::shared_ptr< WPosition > cP( new WPosition( 0, 0, 0 ) );
00096         if( intersectPlaneSegment( p, l[i-1], l[i], cP ) )
00097         {
00098             result = true;
00099             double dist = length2( *cP - p.getPosition() );
00100             if( dist < minDistance )
00101             {
00102                 minDistance = dist;
00103                 *cutPoint = *cP;
00104             }
00105         }
00106     }
00107     return result;
00108 }