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-2020 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 <cstdio>
34 #include <wx/debug.h>
35 #include <wx/log.h>
36 
37 #include <cmath>
38 
39 //static unsigned int gs_next_rayID = 0;
40 
41 void RAY::Init( const SFVEC3F& o, const SFVEC3F& d )
42 {
43  m_Origin = o;
44  m_Dir = d;
45  m_InvDir = 1.0f / d;
46 
47  //rayID = gs_next_rayID;
48  //gs_next_rayID++;
49 
50  // An Efficient and Robust Ray–Box Intersection Algorithm
51  // Amy Williams Steve Barrus R. Keith Morley Peter Shirley
52  // University of Utah
53  // http://people.csail.mit.edu/amy/papers/box-jgt.pdf
54  m_dirIsNeg[0] = m_Dir.x < 0.0f;
55  m_dirIsNeg[1] = m_Dir.y < 0.0f;
56  m_dirIsNeg[2] = m_Dir.z < 0.0f;
57 
58 
59  // ray slope
60 
61  // "Fast Ray / Axis-Aligned Bounding Box Overlap Tests using Ray Slopes"
62  // by Martin Eisemann, Thorsten Grosch, Stefan Müller and Marcus Magnor
63  // Computer Graphics Lab, TU Braunschweig, Germany and
64  // University of Koblenz-Landau, Germany
65  // Licence: "This source code is public domain, but please mention us if you use it."
66  //
67  // https://github.com/rjw57/mcvoxel/tree/master/third-party/rayslope
68  // https://github.com/rjw57/mcvoxel/blob/master/third-party/rayslope/ray.cpp
69 
70  ibyj = m_Dir.x * m_InvDir.y;
71  jbyi = m_Dir.y * m_InvDir.x;
72  jbyk = m_Dir.y * m_InvDir.z;
73  kbyj = m_Dir.z * m_InvDir.y;
74  ibyk = m_Dir.x * m_InvDir.z;
75  kbyi = m_Dir.z * m_InvDir.x;
76  c_xy = m_Origin.y - jbyi * m_Origin.x;
77  c_xz = m_Origin.z - kbyi * m_Origin.x;
78  c_yx = m_Origin.x - ibyj * m_Origin.y;
79  c_yz = m_Origin.z - kbyj * m_Origin.y;
80  c_zx = m_Origin.x - ibyk * m_Origin.z;
81  c_zy = m_Origin.y - jbyk * m_Origin.z;
82 
83  // ray slope classification
84  if( m_Dir.x < 0 )
85  {
86  if( m_Dir.y < 0 )
87  {
88  if( m_Dir.z < 0 )
89  {
91  }
92  else if( m_Dir.z > 0 )
93  {
95  }
96  else//( m_Dir.z >= 0 )
97  {
99  }
100  }
101  else//( m_Dir.y >= 0 )
102  {
103  if( m_Dir.z < 0 )
104  {
106  if( m_Dir.y == 0 )
108  }
109  else//( m_Dir.z >= 0 )
110  {
111  if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
113  else if( m_Dir.z == 0 )
115  else if( m_Dir.y == 0 )
117  else
119  }
120  }
121  }
122  else//( m_Dir.x >= 0 )
123  {
124  if( m_Dir.y < 0 )
125  {
126  if( m_Dir.z < 0 )
127  {
129  if( m_Dir.x == 0 )
131  }
132  else//( m_Dir.z >= 0 )
133  {
134  if( ( m_Dir.x == 0 ) && ( m_Dir.z == 0 ) )
136  else if( m_Dir.z == 0 )
138  else if( m_Dir.x == 0 )
140  else
142  }
143  }
144  else//( m_Dir.y >= 0 )
145  {
146  if( m_Dir.z < 0 )
147  {
148  if( ( m_Dir.x == 0 ) && ( m_Dir.y == 0 ) )
150  else if( m_Dir.x == 0 )
152  else if( m_Dir.y == 0 )
154  else
156  }
157  else//( m_Dir.z > 0 )
158  {
159  if( m_Dir.x == 0 )
160  {
161  if( m_Dir.y == 0 )
163  else if( m_Dir.z == 0 )
165  else
167  }
168  else
169  {
170  if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
172  else if( m_Dir.y == 0 )
174  else if( m_Dir.z == 0 )
176  else
178  }
179  }
180  }
181  }
182 }
183 
184 
185 bool IntersectSegment( const SFVEC2F &aStartA, const SFVEC2F &aEnd_minus_startA,
186  const SFVEC2F &aStartB, const SFVEC2F &aEnd_minus_startB )
187 {
188  float rxs = aEnd_minus_startA.x *
189  aEnd_minus_startB.y - aEnd_minus_startA.y *
190  aEnd_minus_startB.x;
191 
192  if( std::abs( rxs ) > glm::epsilon<float>() )
193  {
194  float inv_rxs = 1.0f / rxs;
195 
196  SFVEC2F pq = aStartB - aStartA;
197 
198  float t = (pq.x * aEnd_minus_startB.y - pq.y * aEnd_minus_startB.x) * inv_rxs;
199 
200  if( (t < 0.0f) || (t > 1.0f) )
201  return false;
202 
203  float u = (pq.x * aEnd_minus_startA.y - pq.y * aEnd_minus_startA.x) * inv_rxs;
204 
205  if( (u < 0.0f) || (u > 1.0f) )
206  return false;
207 
208  return true;
209  }
210 
211  return false;
212 }
213 
214 
215 // !TODO: not tested
216 bool RAY::IntersectSphere( const SFVEC3F &aCenter, float aRadius, float &aOutT0, float &aOutT1 ) const
217 {
218 /*
219  // Ray-sphere intersection: algebraic
220 
221  SFVEC3F CO = m_Origin - aCenter;
222 
223  float a = glm::dot( m_Dir, m_Dir );
224  float b = 2.0f * glm::dot( CO, m_Dir );
225  float c = glm::dot( CO, CO ) - aRadius*aRadius;
226 
227  float discriminant = b * b - 4.0f * a * c;
228 
229  if( discriminant < 0.0f )
230  return false;
231 
232  aOutT0 = (-b - sqrtf(discriminant)) / (2.0f * a);
233  aOutT1 = (-b + sqrtf(discriminant)) / (2.0f * a);
234 
235  if( aOutT0 > aOutT1 )
236  {
237  float temp = aOutT0;
238  aOutT0 = aOutT1;
239  aOutT1 = temp;
240  }
241 
242  return true;
243 */
244 
245  // Ray-sphere intersection: geometric
246  SFVEC3F OC = aCenter - m_Origin;
247  float p_dot_d = glm::dot( OC, m_Dir );
248 
249  if( p_dot_d < 0.0f )
250  return 0.0f;
251 
252  float p_dot_p = glm::dot( OC, OC );
253  float discriminant = p_dot_p - p_dot_d * p_dot_d;
254 
255  if( discriminant > aRadius*aRadius )
256  return false;
257 
258  discriminant = sqrtf( aRadius*aRadius - discriminant );
259 
260  aOutT0 = p_dot_d - discriminant;
261  aOutT1 = p_dot_d + discriminant;
262 
263  if( aOutT0 > aOutT1 )
264  {
265  float temp = aOutT0;
266  aOutT0 = aOutT1;
267  aOutT1 = temp;
268  }
269 
270  return true;
271 }
272 
273 
274 RAYSEG2D::RAYSEG2D( const SFVEC2F& s, const SFVEC2F& e )
275 {
276  m_Start = s;
277  m_End = e;
278  m_End_minus_start = e - s;
279  m_Length = glm::length( m_End_minus_start );
280  m_Dir = glm::normalize( m_End_minus_start );
281  m_InvDir = (1.0f / m_Dir);
282 
283  if( fabs(m_Dir.x) < FLT_EPSILON )
284  m_InvDir.x = NextFloatDown(FLT_MAX);
285 
286  if( fabs(m_Dir.y) < FLT_EPSILON )
287  m_InvDir.y = NextFloatDown(FLT_MAX);
288 
290 }
291 
292 
294  const SFVEC2F &aEnd_minus_start,
295  float *aOutT ) const
296 {
297  float rxs = m_End_minus_start.x *
298  aEnd_minus_start.y - m_End_minus_start.y *
299  aEnd_minus_start.x;
300 
301  if( std::abs( rxs ) > glm::epsilon<float>() )
302  {
303  const float inv_rxs = 1.0f / rxs;
304 
305  const SFVEC2F pq = aStart - m_Start;
306 
307  const float t = (pq.x * aEnd_minus_start.y - pq.y * aEnd_minus_start.x) * inv_rxs;
308 
309  if( (t < 0.0f) || (t > 1.0f) )
310  return false;
311 
312  float u = (pq.x * m_End_minus_start.y - pq.y * m_End_minus_start.x) * inv_rxs;
313 
314  if( (u < 0.0f) || (u > 1.0f) )
315  return false;
316 
317  *aOutT = t;
318 
319  return true;
320  }
321 
322  return false;
323 }
324 
325 
326 // http://geomalgorithms.com/a02-_lines.html
327 float RAYSEG2D::DistanceToPointSquared( const SFVEC2F &aPoint ) const
328 {
329  SFVEC2F p = aPoint - m_Start;
330 
331  const float c1 = glm::dot( p, m_End_minus_start );
332 
333  if( c1 < FLT_EPSILON )
334  return glm::dot( p, p );
335 
336  if( m_DOT_End_minus_start <= c1 )
337  p = aPoint - m_End;
338  else
339  {
340  const float b = c1 / m_DOT_End_minus_start;
341  const SFVEC2F pb = m_Start + m_End_minus_start * b;
342 
343  p = aPoint - pb;
344  }
345 
346  return glm::dot( p, p );
347 }
348 
349 
350 bool RAYSEG2D::IntersectCircle( const SFVEC2F &aCenter,
351  float aRadius,
352  float *aOutT0,
353  float *aOutT1,
354  SFVEC2F *aOutNormalT0,
355  SFVEC2F *aOutNormalT1 ) const
356 {
357  // This code used directly from Steve Marschner's CS667 framework
358  // http://cs665pd.googlecode.com/svn/trunk/photon/sphere.cpp
359 
360  // Compute some factors used in computation
361  const float qx = m_Start.x - aCenter.x;
362  const float qy = m_Start.y - aCenter.y;
363 
364  const float qd = qx * m_Dir.x + qy * m_Dir.y;
365  const float qq = qx * qx + qy * qy;
366 
367  // solving the quadratic equation for t at the pts of intersection
368  // dd*t^2 + (2*qd)*t + (qq-r^2) = 0
369  const float discriminantsqr = (qd * qd - (qq - aRadius * aRadius));
370 
371  // If the discriminant is less than zero, there is no intersection
372  if( discriminantsqr < FLT_EPSILON )
373  return false;
374 
375 
376  // Otherwise check and make sure that the intersections occur on the ray (t
377  // > 0) and return the closer one
378  const float discriminant = std::sqrt( discriminantsqr );
379  const float t1 = (-qd - discriminant);
380  const float t2 = (-qd + discriminant);
381 
382  if( (( t1 < 0.0f ) || ( t1 > m_Length ) ) &&
383  (( t2 < 0.0f ) || ( t2 > m_Length ) ) )
384  return false;// Neither intersection was in the ray's half line.
385 
386  // Convert the intersection to a normalized
387  *aOutT0 = t1 / m_Length;
388  *aOutT1 = t2 / m_Length;
389 
390  SFVEC2F hitPointT1 = at( t1 );
391  SFVEC2F hitPointT2 = at( t2 );
392 
393  *aOutNormalT0 = (hitPointT1 - aCenter) / aRadius;
394  *aOutNormalT1 = (hitPointT2 - aCenter) / aRadius;
395 
396  return true;
397 }
398 
399 
400 void RAY::debug() const
401 {
402  wxLogDebug( "O(%f, %f, %f) D(%f, %f, %f)\n", m_Origin.x, m_Origin.y, m_Origin.z,
403  m_Dir.x, m_Dir.y, m_Dir.z );
404 }
float c_xy
Definition: ray.h:78
void Init(const SFVEC3F &o, const SFVEC3F &d)
Definition: ray.cpp:41
bool IntersectSphere(const SFVEC3F &aCenter, float aRadius, float &aOutT0, float &aOutT1) const
Definition: ray.cpp:216
bool IntersectCircle(const SFVEC2F &aCenter, float aRadius, float *aOutT0, float *aOutT1, SFVEC2F *aOutNormalT0, SFVEC2F *aOutNormalT1) const
Definition: ray.cpp:350
float c_zy
Definition: ray.h:78
SFVEC2F m_Dir
Definition: ray.h:115
float jbyk
Definition: ray.h:77
void debug() const
Definition: ray.cpp:400
float kbyi
Definition: ray.h:77
float c_yx
Definition: ray.h:78
SFVEC2F at(float t) const
Definition: ray.h:141
SFVEC3F m_InvDir
Definition: ray.h:75
float kbyj
Definition: ray.h:77
glm::vec2 SFVEC2F
Definition: xv3d_types.h:45
float NextFloatDown(float v)
Definition: 3d_fastmath.h:157
RAY_CLASSIFICATION m_Classification
Definition: ray.h:73
bool IntersectSegment(const SFVEC2F &aStart, const SFVEC2F &aEnd_minus_start, float *aOutT) const
Definition: ray.cpp:293
unsigned int m_dirIsNeg[3]
Definition: ray.h:80
float m_DOT_End_minus_start
dot( m_End_minus_start, m_End_minus_start)
Definition: ray.h:118
float m_Length
Definition: ray.h:117
SFVEC2F m_End
Definition: ray.h:113
SFVEC2F m_InvDir
Definition: ray.h:116
bool IntersectSegment(const SFVEC2F &aStartA, const SFVEC2F &aEnd_minus_startA, const SFVEC2F &aStartB, const SFVEC2F &aEnd_minus_startB)
Definition: ray.cpp:185
SFVEC3F m_Dir
Definition: ray.h:72
float ibyj
Definition: ray.h:77
float jbyi
Definition: ray.h:77
float c_yz
Definition: ray.h:78
SFVEC3F m_Origin
Definition: ray.h:69
SFVEC2F m_Start
Definition: ray.h:112
float c_zx
Definition: ray.h:78
RAYSEG2D(const SFVEC2F &s, const SFVEC2F &e)
Definition: ray.cpp:274
glm::vec3 SFVEC3F
Definition: xv3d_types.h:47
float DistanceToPointSquared(const SFVEC2F &aPoint) const
Definition: ray.cpp:327
float c_xz
Definition: ray.h:78
SFVEC2F m_End_minus_start
Definition: ray.h:114
float ibyk
Definition: ray.h:77