OpenWalnut 1.3.1
|
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 }