27 #include "linearAlgebra/WPosition.h"
28 #include "linearAlgebra/WVectorFixed.h"
29 #include "../WAssert.h"
30 #include "../WLimits.h"
35 const WPosition& planePoint = p.getPosition();
37 double r1 = dot( normal, p1 - planePoint );
38 double r2 = dot( normal, p2 - planePoint );
39 double r3 = dot( normal, p3 - planePoint );
42 if( std::abs( ( ( r1 > 0 ) - ( r1 < 0 ) ) + ( ( r2 > 0) - ( r2 < 0 ) ) + ( ( r3 > 0 ) - ( r3 < 0 ) ) ) == 3 )
49 bool intersectPlaneSegment(
const WPlane& p,
52 boost::shared_ptr< WPosition > pointOfIntersection )
54 const WVector3d& normal = normalize( p.getNormal() );
55 double const d = dot( normal, p.getPosition() );
56 WAssert( pointOfIntersection.get(),
"Place to store a point of intersection is not ready!" );
57 *pointOfIntersection = p.getPosition();
60 if( std::abs( dot( normal, p1 - p.getPosition() ) ) <= 2 *
wlimits::DBL_EPS )
62 *pointOfIntersection = p1;
65 else if( std::abs( dot( normal, p2 - p.getPosition() ) ) <= 2 *
wlimits::DBL_EPS )
67 *pointOfIntersection = p2;
76 double const t = ( d - dot( normal, p2 ) ) / ( dot( normal, p1 - p2 ) );
78 *pointOfIntersection = p2 + t * ( p1 - p2 );
87 bool intersectPlaneLineNearCP(
const WPlane& p,
const WLine& l, boost::shared_ptr< WPosition > cutPoint )
91 WAssert( cutPoint.get(),
"Place to store a point of intersection is not ready!" );
93 for(
size_t i = 1; i < l.
size(); ++i )
95 boost::shared_ptr< WPosition > cP(
new WPosition( 0, 0, 0 ) );
96 if( intersectPlaneSegment( p, l[i-1], l[i], cP ) )
99 double dist = length2( *cP - p.getPosition() );
100 if( dist < minDistance )
A line is an ordered sequence of WPositions.
This only is a 3d double vector.
const double DBL_EPS
Smallest double such: 1.0 + DBL_EPS == 1.0 is still true.
const double MAX_DOUBLE
Maximum double value.
size_type size() const
Wrapper around std::vector member function.