KiCad PCB EDA Suite
ray.cpp
Go to the documentation of this file.
1 /*
2  * This program source code file is part of KiCad, a free EDA CAD application.
3  *
4  * Copyright (C) 2015-2017 Mario Luzeiro <mrluzeiro@ua.pt>
5  * Copyright (C) 1992-2017 KiCad Developers, see AUTHORS.txt for contributors.
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, you may find one here:
19  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
20  * or you may search the http://www.gnu.org website for the version 2 license,
21  * or you may write to the Free Software Foundation, Inc.,
22  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
23  */
24 
31 #include "ray.h"
32 #include "../../3d_fastmath.h"
33 #include <stdio.h>
34 #include <wx/debug.h>
35 
36 //static unsigned int gs_next_rayID = 0;
37 
38 void RAY::Init( const SFVEC3F& o, const SFVEC3F& d )
39 {
40  m_Origin = o;
41  m_Dir = d;
42  m_InvDir = 1.0f / d;
43 
44  //rayID = gs_next_rayID;
45  //gs_next_rayID++;
46 
47  // An Efficient and Robust Ray–Box Intersection Algorithm
48  // Amy Williams Steve Barrus R. Keith Morley Peter Shirley
49  // University of Utah
50  // http://people.csail.mit.edu/amy/papers/box-jgt.pdf
51  m_dirIsNeg[0] = m_Dir.x <= 0.0f;
52  m_dirIsNeg[1] = m_Dir.y <= 0.0f;
53  m_dirIsNeg[2] = m_Dir.z <= 0.0f;
54 
55 
56  // ray slope
57 
58  // "Fast Ray / Axis-Aligned Bounding Box Overlap Tests using Ray Slopes"
59  // by Martin Eisemann, Thorsten Grosch, Stefan Müller and Marcus Magnor
60  // Computer Graphics Lab, TU Braunschweig, Germany and
61  // University of Koblenz-Landau, Germany
62  // Licence: "This source code is public domain, but please mention us if you use it."
63  //
64  // https://github.com/rjw57/mcvoxel/tree/master/third-party/rayslope
65  // https://github.com/rjw57/mcvoxel/blob/master/third-party/rayslope/ray.cpp
66 
67  ibyj = m_Dir.x * m_InvDir.y;
68  jbyi = m_Dir.y * m_InvDir.x;
69  jbyk = m_Dir.y * m_InvDir.z;
70  kbyj = m_Dir.z * m_InvDir.y;
71  ibyk = m_Dir.x * m_InvDir.z;
72  kbyi = m_Dir.z * m_InvDir.x;
73  c_xy = m_Origin.y - jbyi * m_Origin.x;
74  c_xz = m_Origin.z - kbyi * m_Origin.x;
75  c_yx = m_Origin.x - ibyj * m_Origin.y;
76  c_yz = m_Origin.z - kbyj * m_Origin.y;
77  c_zx = m_Origin.x - ibyk * m_Origin.z;
78  c_zy = m_Origin.y - jbyk * m_Origin.z;
79 
80  // ray slope classification
81  if( m_Dir.x < 0 )
82  {
83  if( m_Dir.y < 0 )
84  {
85  if( m_Dir.z < 0 )
86  {
88  }
89  else if( m_Dir.z > 0 ){
91  }
92  else//( m_Dir.z >= 0 )
93  {
95  }
96  }
97  else//( m_Dir.y >= 0 )
98  {
99  if( m_Dir.z < 0 )
100  {
102  if( m_Dir.y == 0 )
104  }
105  else//( m_Dir.z >= 0 )
106  {
107  if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
109  else if( m_Dir.z == 0 )
111  else if( m_Dir.y == 0 )
113  else
115  }
116  }
117  }
118  else//( m_Dir.x >= 0 )
119  {
120  if( m_Dir.y < 0 )
121  {
122  if( m_Dir.z < 0 )
123  {
125  if( m_Dir.x == 0 )
127  }
128  else//( m_Dir.z >= 0 )
129  {
130  if( ( m_Dir.x == 0 ) && ( m_Dir.z == 0 ) )
132  else if( m_Dir.z == 0 )
134  else if( m_Dir.x == 0 )
136  else
138  }
139  }
140  else//( m_Dir.y >= 0 )
141  {
142  if( m_Dir.z < 0 )
143  {
144  if( ( m_Dir.x == 0 ) && ( m_Dir.y == 0 ) )
146  else if( m_Dir.x == 0 )
148  else if( m_Dir.y == 0 )
150  else
152  }
153  else//( m_Dir.z > 0 )
154  {
155  if( m_Dir.x == 0 )
156  {
157  if( m_Dir.y == 0 )
159  else if( m_Dir.z == 0 )
161  else
163  }
164  else
165  {
166  if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
168  else if( m_Dir.y == 0 )
170  else if( m_Dir.z == 0 )
172  else
174  }
175  }
176  }
177  }
178 }
179 
180 
181 bool IntersectSegment( const SFVEC2F &aStartA, const SFVEC2F &aEnd_minus_startA,
182  const SFVEC2F &aStartB, const SFVEC2F &aEnd_minus_startB )
183 {
184  float rxs = aEnd_minus_startA.x *
185  aEnd_minus_startB.y - aEnd_minus_startA.y *
186  aEnd_minus_startB.x;
187 
188  if( fabs(rxs) > glm::epsilon<float>() )
189  {
190  float inv_rxs = 1.0f / rxs;
191 
192  SFVEC2F pq = aStartB - aStartA;
193 
194  float t = (pq.x * aEnd_minus_startB.y - pq.y * aEnd_minus_startB.x) * inv_rxs;
195 
196  if( (t < 0.0f) || (t > 1.0f) )
197  return false;
198 
199  float u = (pq.x * aEnd_minus_startA.y - pq.y * aEnd_minus_startA.x) * inv_rxs;
200 
201  if( (u < 0.0f) || (u > 1.0f) )
202  return false;
203 
204  return true;
205  }
206 
207  return false;
208 }
209 
210 
211 // !TODO: not tested
212 bool RAY::IntersectSphere( const SFVEC3F &aCenter, float aRadius, float &aOutT0, float &aOutT1 ) const
213 {
214 /*
215  // Ray-sphere intersection: algebraic
216 
217  SFVEC3F CO = m_Origin - aCenter;
218 
219  float a = glm::dot( m_Dir, m_Dir );
220  float b = 2.0f * glm::dot( CO, m_Dir );
221  float c = glm::dot( CO, CO ) - aRadius*aRadius;
222 
223  float discriminant = b * b - 4.0f * a * c;
224 
225  if( discriminant < 0.0f )
226  return false;
227 
228  aOutT0 = (-b - sqrtf(discriminant)) / (2.0f * a);
229  aOutT1 = (-b + sqrtf(discriminant)) / (2.0f * a);
230 
231  if( aOutT0 > aOutT1 )
232  {
233  float temp = aOutT0;
234  aOutT0 = aOutT1;
235  aOutT1 = temp;
236  }
237 
238  return true;
239 */
240 
241  // Ray-sphere intersection: geometric
242  SFVEC3F OC = aCenter - m_Origin;
243  float p_dot_d = glm::dot( OC, m_Dir );
244 
245  if( p_dot_d < 0.0f )
246  return 0.0f;
247 
248  float p_dot_p = glm::dot( OC, OC );
249  float discriminant = p_dot_p - p_dot_d * p_dot_d;
250 
251  if( discriminant > aRadius*aRadius )
252  return false;
253 
254  discriminant = sqrtf( aRadius*aRadius - discriminant );
255 
256  aOutT0 = p_dot_d - discriminant;
257  aOutT1 = p_dot_d + discriminant;
258 
259  if( aOutT0 > aOutT1 )
260  {
261  float temp = aOutT0;
262  aOutT0 = aOutT1;
263  aOutT1 = temp;
264  }
265 
266  return true;
267 }
268 
269 
270 RAYSEG2D::RAYSEG2D( const SFVEC2F& s, const SFVEC2F& e )
271 {
272  m_Start = s;
273  m_End = e;
274  m_End_minus_start = e - s;
275  m_Length = glm::length( m_End_minus_start );
276  m_Dir = glm::normalize( m_End_minus_start );
277  m_InvDir = (1.0f / m_Dir);
278 
279  if( fabs(m_Dir.x) < FLT_EPSILON )
280  m_InvDir.x = NextFloatDown(FLT_MAX);
281 
282  if( fabs(m_Dir.y) < FLT_EPSILON )
283  m_InvDir.y = NextFloatDown(FLT_MAX);
284 
286 }
287 
288 
290  const SFVEC2F &aEnd_minus_start,
291  float *aOutT ) const
292 {
293  float rxs = m_End_minus_start.x *
294  aEnd_minus_start.y - m_End_minus_start.y *
295  aEnd_minus_start.x;
296 
297  if( fabs( rxs ) > glm::epsilon<float>() )
298  {
299  const float inv_rxs = 1.0f / rxs;
300 
301  const SFVEC2F pq = aStart - m_Start;
302 
303  const float t = (pq.x * aEnd_minus_start.y - pq.y * aEnd_minus_start.x) * inv_rxs;
304 
305  if( (t < 0.0f) || (t > 1.0f) )
306  return false;
307 
308  float u = (pq.x * m_End_minus_start.y - pq.y * m_End_minus_start.x) * inv_rxs;
309 
310  if( (u < 0.0f) || (u > 1.0f) )
311  return false;
312 
313  *aOutT = t;
314 
315  return true;
316  }
317 
318  return false;
319 }
320 
321 
322 // http://geomalgorithms.com/a02-_lines.html
323 float RAYSEG2D::DistanceToPointSquared( const SFVEC2F &aPoint ) const
324 {
325  SFVEC2F p = aPoint - m_Start;
326 
327  const float c1 = glm::dot( p, m_End_minus_start );
328 
329  if( c1 < FLT_EPSILON )
330  return glm::dot( p, p );
331 
332  if( m_DOT_End_minus_start <= c1 )
333  p = aPoint - m_End;
334  else
335  {
336  const float b = c1 / m_DOT_End_minus_start;
337  const SFVEC2F pb = m_Start + m_End_minus_start * b;
338 
339  p = aPoint - pb;
340  }
341 
342  return glm::dot( p, p );
343 }
344 
345 
346 bool RAYSEG2D::IntersectCircle( const SFVEC2F &aCenter,
347  float aRadius,
348  float *aOutT0,
349  float *aOutT1,
350  SFVEC2F *aOutNormalT0,
351  SFVEC2F *aOutNormalT1 ) const
352 {
353  // This code used directly from Steve Marschner's CS667 framework
354  // http://cs665pd.googlecode.com/svn/trunk/photon/sphere.cpp
355 
356  // Compute some factors used in computation
357  const float qx = m_Start.x - aCenter.x;
358  const float qy = m_Start.y - aCenter.y;
359 
360  const float qd = qx * m_Dir.x + qy * m_Dir.y;
361  const float qq = qx * qx + qy * qy;
362 
363  // solving the quadratic equation for t at the pts of intersection
364  // dd*t^2 + (2*qd)*t + (qq-r^2) = 0
365  const float discriminantsqr = (qd * qd - (qq - aRadius * aRadius));
366 
367  // If the discriminant is less than zero, there is no intersection
368  if( discriminantsqr < FLT_EPSILON )
369  return false;
370 
371 
372  // Otherwise check and make sure that the intersections occur on the ray (t
373  // > 0) and return the closer one
374  const float discriminant = sqrt( discriminantsqr );
375  const float t1 = (-qd - discriminant);
376  const float t2 = (-qd + discriminant);
377 
378  if( (( t1 < 0.0f ) || ( t1 > m_Length ) ) &&
379  (( t2 < 0.0f ) || ( t2 > m_Length ) ) )
380  return false;// Neither intersection was in the ray's half line.
381 
382  // Convert the intersection to a normalized
383  *aOutT0 = t1 / m_Length;
384  *aOutT1 = t2 / m_Length;
385 
386  SFVEC2F hitPointT1 = at( t1 );
387  SFVEC2F hitPointT2 = at( t2 );
388 
389  *aOutNormalT0 = (hitPointT1 - aCenter) / aRadius;
390  *aOutNormalT1 = (hitPointT2 - aCenter) / aRadius;
391 
392  return true;
393 }
394 
395 
396 void RAY::debug() const
397 {
398  printf("O(%f, %f, %f) D(%f, %f, %f)\n", m_Origin.x, m_Origin.y, m_Origin.z,
399  m_Dir.x, m_Dir.y, m_Dir.z );
400 }
Definition: ray.h:38
Definition: ray.h:38
Definition: ray.h:38
float c_xy
Definition: ray.h:54
Definition: ray.h:39
Definition: ray.h:38
void Init(const SFVEC3F &o, const SFVEC3F &d)
Definition: ray.cpp:38
float DistanceToPointSquared(const SFVEC2F &aPoint) const
Definition: ray.cpp:323
Definition: ray.h:38
Definition: ray.h:39
Definition: ray.h:39
float c_zy
Definition: ray.h:54
SFVEC2F m_Dir
Definition: ray.h:91
SFVEC2F at(float t) const
Definition: ray.h:117
Definition: ray.h:39
float jbyk
Definition: ray.h:53
void debug() const
Definition: ray.cpp:396
Definition: ray.h:39
float kbyi
Definition: ray.h:53
float c_yx
Definition: ray.h:54
Definition: ray.h:38
Definition: ray.h:38
SFVEC3F m_InvDir
Definition: ray.h:51
bool IntersectSphere(const SFVEC3F &aCenter, float aRadius, float &aOutT0, float &aOutT1) const
Definition: ray.cpp:212
Definition: ray.h:39
Definition: ray.h:38
Definition: ray.h:39
float kbyj
Definition: ray.h:53
glm::vec2 SFVEC2F
Definition: xv3d_types.h:45
float NextFloatDown(float v)
Definition: 3d_fastmath.h:157
Definition: ray.h:38
Definition: ray.h:39
RAY_CLASSIFICATION m_Classification
Definition: ray.h:49
unsigned int m_dirIsNeg[3]
Definition: ray.h:56
float m_DOT_End_minus_start
dot( m_End_minus_start, m_End_minus_start)
Definition: ray.h:94
float m_Length
Definition: ray.h:93
Definition: ray.h:38
SFVEC2F m_End
Definition: ray.h:89
SFVEC2F m_InvDir
Definition: ray.h:92
Definition: ray.h:39
bool IntersectSegment(const SFVEC2F &aStartA, const SFVEC2F &aEnd_minus_startA, const SFVEC2F &aStartB, const SFVEC2F &aEnd_minus_startB)
Definition: ray.cpp:181
SFVEC3F m_Dir
Definition: ray.h:48
Definition: ray.h:39
float ibyj
Definition: ray.h:53
float jbyi
Definition: ray.h:53
float c_yz
Definition: ray.h:54
SFVEC3F m_Origin
Definition: ray.h:45
SFVEC2F m_Start
Definition: ray.h:88
Definition: ray.h:38
Definition: ray.h:38
float c_zx
Definition: ray.h:54
Definition: ray.h:38
Definition: ray.h:38
RAYSEG2D(const SFVEC2F &s, const SFVEC2F &e)
Definition: ray.cpp:270
glm::vec3 SFVEC3F
Definition: xv3d_types.h:47
float c_xz
Definition: ray.h:54
Definition: ray.h:39
Definition: ray.h:38
bool IntersectCircle(const SFVEC2F &aCenter, float aRadius, float *aOutT0, float *aOutT1, SFVEC2F *aOutNormalT0, SFVEC2F *aOutNormalT1) const
Definition: ray.cpp:346
bool IntersectSegment(const SFVEC2F &aStart, const SFVEC2F &aEnd_minus_start, float *aOutT) const
Definition: ray.cpp:289
SFVEC2F m_End_minus_start
Definition: ray.h:90
float ibyk
Definition: ray.h:53