OpenWalnut  1.4.0
WMath.cpp
1 //---------------------------------------------------------------------------
2 //
3 // Project: OpenWalnut ( http://www.openwalnut.org )
4 //
5 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
6 // For more information see http://www.openwalnut.org/copying
7 //
8 // This file is part of OpenWalnut.
9 //
10 // OpenWalnut is free software: you can redistribute it and/or modify
11 // it under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 3 of the License, or
13 // (at your option) any later version.
14 //
15 // OpenWalnut is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
22 //
23 //---------------------------------------------------------------------------
24 
25 #include "WMath.h"
26 #include "WPlane.h"
27 #include "linearAlgebra/WPosition.h"
28 #include "linearAlgebra/WVectorFixed.h"
29 #include "../WAssert.h"
30 #include "../WLimits.h"
31 
32 bool testIntersectTriangle( const WPosition& p1, const WPosition& p2, const WPosition& p3, const WPlane& p )
33 {
34  const WVector3d& normal = p.getNormal();
35  const WPosition& planePoint = p.getPosition();
36 
37  double r1 = dot( normal, p1 - planePoint );
38  double r2 = dot( normal, p2 - planePoint );
39  double r3 = dot( normal, p3 - planePoint );
40 
41  // TODO(math): use signum here!
42  if( std::abs( ( ( r1 > 0 ) - ( r1 < 0 ) ) + ( ( r2 > 0) - ( r2 < 0 ) ) + ( ( r3 > 0 ) - ( r3 < 0 ) ) ) == 3 )
43  {
44  return false;
45  }
46  return true;
47 }
48 
49 bool intersectPlaneSegment( const WPlane& p,
50  const WPosition& p1,
51  const WPosition& p2,
52  boost::shared_ptr< WPosition > pointOfIntersection )
53 {
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(); // otherwise it would be undefined
58 
59  // at least one point is in plane (maybe the whole segment)
60  if( std::abs( dot( normal, p1 - p.getPosition() ) ) <= 2 * wlimits::DBL_EPS )
61  {
62  *pointOfIntersection = p1;
63  return true;
64  }
65  else if( std::abs( dot( normal, p2 - p.getPosition() ) ) <= 2 * wlimits::DBL_EPS )
66  {
67  *pointOfIntersection = p2;
68  return true;
69  }
70 
71  if( std::abs( dot( normal, p1 - p2 ) ) < wlimits::DBL_EPS ) // plane and line are parallel
72  {
73  return false;
74  }
75 
76  double const t = ( d - dot( normal, p2 ) ) / ( dot( normal, p1 - p2 ) );
77 
78  *pointOfIntersection = p2 + t * ( p1 - p2 );
79 
80  if( t >= -wlimits::DBL_EPS && t <= ( 1.0 + wlimits::DBL_EPS ) )
81  {
82  return true;
83  }
84  return false;
85 }
86 
87 bool intersectPlaneLineNearCP( const WPlane& p, const WLine& l, boost::shared_ptr< WPosition > cutPoint )
88 {
89  bool result = false;
90  double minDistance = wlimits::MAX_DOUBLE;
91  WAssert( cutPoint.get(), "Place to store a point of intersection is not ready!" );
92  *cutPoint = WPosition( 0, 0, 0 );
93  for( size_t i = 1; i < l.size(); ++i ) // test each segment
94  {
95  boost::shared_ptr< WPosition > cP( new WPosition( 0, 0, 0 ) );
96  if( intersectPlaneSegment( p, l[i-1], l[i], cP ) )
97  {
98  result = true;
99  double dist = length2( *cP - p.getPosition() );
100  if( dist < minDistance )
101  {
102  minDistance = dist;
103  *cutPoint = *cP;
104  }
105  }
106  }
107  return result;
108 }
A line is an ordered sequence of WPositions.
Definition: WLine.h:41
This only is a 3d double vector.
Represents a plane with a normal vector and a position in space.
Definition: WPlane.h:39
const WPosition & getPosition() const
Returns a point in that plane.
Definition: WPlane.h:181
A fixed size matrix class.
Definition: WMatrixFixed.h:149
const double DBL_EPS
Smallest double such: 1.0 + DBL_EPS == 1.0 is still true.
Definition: WLimits.cpp:36
const WVector3d & getNormal() const
Returns the normal of the plane.
Definition: WPlane.h:186
const double MAX_DOUBLE
Maximum double value.
Definition: WLimits.cpp:31
size_type size() const
Wrapper around std::vector member function.
Definition: WMixinVector.h:267