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