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