TooN Algorithm Library - tag  0.2
intersection.h
Go to the documentation of this file.
1 #ifndef TAG_INTERSECTION_H_
2 #define TAG_INTERSECTION_H_
3 
4 #include <algorithm>
5 
6 #include <TooN/helpers.h>
7 
8 namespace tag {
9 
13 
20 template<typename A, typename B, typename C, typename D, typename ABase, typename BBase, typename CBase, typename DBase>
21 inline bool intersect_plane_line( const TooN::Vector<3,A, ABase> & normal, const double d, const TooN::Vector<3,B, BBase> & p1, const TooN::Vector<3,C, CBase> & p2, TooN::Vector<3,D, DBase> & i){
22  const double EPSILON = 0.000001;
23 
24  TooN::Vector<3> dir = p2 - p1;
25  double c = (normal * dir);
26  if( fabs(c) < EPSILON )
27  return false;
28  double t = (d - normal * p1) / c;
29  i = p1 + t * dir;
30  return true;
31 }
32 
35 inline bool intersect_triangle(const TooN::Vector<3> & orig, const TooN::Vector<3> & dir,
36  const TooN::Vector<3> & vert0, const TooN::Vector<3> & vert1, const TooN::Vector<3> & vert2,
37  double & t, double & u, double & v)
38 {
39  const double EPSILON = 0.000001;
40 
41  double det,inv_det;
42 
43  // find vectors for two edges sharing vert0
44  TooN::Vector<3> edge1 = vert1 - vert0;
45  TooN::Vector<3> edge2 = vert2 - vert0;
46 
47  // begin calculating determinant - also used to calculate U parameter
48  TooN::Vector<3> pvec = dir ^ edge2;
49 
50  // if determinant is near zero, ray lies in plane of triangle
51  det = edge1 * pvec;
52 
53  if (det > -EPSILON && det < EPSILON)
54  return false;
55  inv_det = 1.0 / det;
56 
57  // calculate distance from vert0 to ray origin
58  TooN::Vector<3> tvec = orig - vert0;
59 
60  // calculate U parameter and test bounds
61  u = (tvec * pvec) * inv_det;
62  if (u < 0.0 || u > 1.0)
63  return false;
64 
65  // prepare to test V parameter
66  TooN::Vector<3> qvec = tvec ^ edge1;
67 
68  // calculate V parameter and test bounds
69  v = (dir * qvec) * inv_det;
70  if (v < 0.0 || u + v > 1.0)
71  return false;
72 
73  // calculate t, ray intersects triangle
74  t = (edge2 * qvec) * inv_det;
75  return true;
76 }
77 
80 inline bool intersect_culled_triangle(const TooN::Vector<3> & orig, const TooN::Vector<3> & dir,
81  const TooN::Vector<3> & vert0, const TooN::Vector<3> & vert1, const TooN::Vector<3> & vert2,
82  double & t, double & u, double & v)
83 {
84  const double EPSILON = 0.000001;
85 
86  double det,inv_det;
87 
88  // find vectors for two edges sharing vert0
89  TooN::Vector<3> edge1 = vert1 - vert0;
90  TooN::Vector<3> edge2 = vert2 - vert0;
91 
92  // begin calculating determinant - also used to calculate U parameter
93  TooN::Vector<3> pvec = dir ^ edge2;
94 
95  // if determinant is near zero, ray lies in plane of triangle
96  det = edge1 * pvec;
97 
98  if (det < EPSILON)
99  return false;
100 
101  // calculate distance from vert0 to ray origin
102  TooN::Vector<3> tvec = orig - vert0;
103 
104  // calculate U parameter and test bounds
105  u = tvec * pvec;
106  if (u < 0.0 || u > det)
107  return false;
108 
109  // prepare to test V parameter
110  TooN::Vector<3> qvec = tvec ^ edge1;
111 
112  // calculate V parameter and test bounds
113  v = dir * qvec;
114  if (v < 0.0 || u + v > det)
115  return false;
116 
117  // calculate t, scale parameters, ray intersects triangle
118  t = edge2 * qvec;
119  inv_det = 1.0 / det;
120  t *= inv_det;
121  u *= inv_det;
122  v *= inv_det;
123  return true;
124 }
125 
128 inline bool intersect_triangles( const TooN::Vector<3> & v1, const TooN::Vector<3> & v2, const TooN::Vector<3> & v3,
129  const TooN::Vector<3> & w1, const TooN::Vector<3> & w2, const TooN::Vector<3> & w3,
130  TooN::Vector<3> & p1, TooN::Vector<3> & p2 ){
131  const double EPSILON = 0.000001;
132 
133  const TooN::Vector<3> * tv[3]; // = {&v1, &v2, &v3};
134  tv[0] = &v1; tv[1] = &v2; tv[2] = &v3;
135  const TooN::Vector<3> * tw[3]; //= {&w1, &w2, &w3};
136  tw[0] = &w1; tw[1] = &w2; tw[2] = &w3;
137 
138  // normal vector of plane of triangle v
139  TooN::Vector<3> nv = (v2 - v1) ^ ( v3 - v1 );
140  TooN::normalize(nv);
141  double dv = v1 * nv;
142  double t1w = nv * w1 - dv;
143  double t2w = nv * w2 - dv;
144  double t3w = nv * w3 - dv;
145  // all of triangle w on one side of plane of v ?
146  if( (t1w < -EPSILON && t2w < -EPSILON && t3w < -EPSILON) ||
147  (t1w > EPSILON && t2w > EPSILON && t3w > EPSILON) ) {
148  return false;
149  }
150 
151  // normal vector of plane of triangle w
152  TooN::Vector<3> nw = (w2 - w1) ^ ( w3 - w1 );
153  TooN::normalize(nw);
154  double dw = w1 * nw;
155  double t1v = nw * v1 - dw;
156  double t2v = nw * v2 - dw;
157  double t3v = nw * v3 - dw;
158  // all of triangle v on one side of plane of w ?
159  if( (t1v < -EPSILON && t2v < -EPSILON && t3v < -EPSILON) ||
160  (t1v > EPSILON && t2v > EPSILON && t3v > EPSILON) ) {
161  return false;
162  }
163 
164  // direction of line of intersection of planes
165  TooN::Vector<3> d = nv ^ nw;
166  // are the supporting planes almost parallel ? -> not dealing with this case
167  if( d*d < EPSILON ){
168  return false;
169  }
170 
171  // find out which corner is alone on one side of the
172  int iv, iw;
173  if( t1v * t2v > 0 )
174  iv = 2;
175  else if( t1v * t3v > 0 )
176  iv = 1;
177  else
178  iv = 0;
179  if( t1w * t2w > 0 )
180  iw = 2;
181  else if( t1w * t3w > 0 )
182  iw = 1;
183  else
184  iw = 0;
185 
186  // compute interval points by intersecting with respective planes
187  // we know that they intersect, therefore no test for failure
188  TooN::Matrix<4,3> intersections;
189  intersect_plane_line( nw, dw, *tv[iv], *tv[(iv+1)%3], intersections[0].ref() );
190  intersect_plane_line( nw, dw, *tv[iv], *tv[(iv+2)%3], intersections[1].ref() );
191  intersect_plane_line( nv, dv, *tw[iw], *tw[(iw+1)%3], intersections[2].ref() );
192  intersect_plane_line( nv, dv, *tw[iw], *tw[(iw+2)%3], intersections[3].ref() );
193 
194  // project onto line
195  TooN::Vector<4> proj = intersections * d;
196 
197  // calculate interval extensions
198  int minIndex, maxIndex;
199  if( proj[0] < proj[1] ){ // min1 = proj[0], max1 = proj[1]
200  if( proj[2] < proj[3] ){ // min2 = proj[2], max2 = proj[3]
201  if( proj[0] > proj[2]){
202  minIndex = 0;
203  } else {
204  minIndex = 2;
205  }
206  if( proj[1] < proj[3] ){
207  maxIndex = 1;
208  } else {
209  maxIndex = 3;
210  }
211  } else {
212  if( proj[0] > proj[3]){ // min2 = proj[3], max2 = proj[2]
213  minIndex = 0;
214  } else {
215  minIndex = 3;
216  }
217  if( proj[1] < proj[2] ){
218  maxIndex = 1;
219  } else {
220  maxIndex = 2;
221  }
222  }
223  } else { // min1 = proj[1], max1 = proj[0]
224  if( proj[2] < proj[3] ){ // min2 = proj[2], max2 = proj[3]
225  if( proj[1] > proj[2]){
226  minIndex = 1;
227  } else {
228  minIndex = 2;
229  }
230  if( proj[0] < proj[3] ){
231  maxIndex = 0;
232  } else {
233  maxIndex = 3;
234  }
235  } else {
236  if( proj[1] > proj[3]){ // min2 = proj[3], max2 = proj[2]
237  minIndex = 1;
238  } else {
239  minIndex = 3;
240  }
241  if( proj[0] < proj[2] ){
242  maxIndex = 0;
243  } else {
244  maxIndex = 2;
245  }
246  }
247  }
248  // no intersection
249  if( proj[minIndex] > proj[maxIndex] )
250  return false;
251 
252  p1 = intersections[minIndex];
253  p2 = intersections[maxIndex];
254  return true;
255 }
256 
257 }
258 
259 #endif
Generated on Tue Aug 13 2013 14:11:40 for TooN Algorithm Library - tag by   doxygen 1.8.3.1