00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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();
00057
00058
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 )
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 )
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 }