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 <[email protected]>
5  * Copyright (C) 2015-2021 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 
25 #include "ray.h"
26 #include "../../3d_fastmath.h"
27 #include <cstdio>
28 #include <wx/debug.h>
29 #include <wx/log.h>
30 
31 #include <cmath>
32 
33 //static unsigned int gs_next_rayID = 0;
34 
35 void RAY::Init( const SFVEC3F& o, const SFVEC3F& d )
36 {
37  m_Origin = o;
38  m_Dir = d;
39  m_InvDir = 1.0f / d;
40 
41  rayID = 0; // Not used, just set to 0
42  //rayID = gs_next_rayID;
43  //gs_next_rayID++;
44 
45  // An Efficient and Robust Ray–Box Intersection Algorithm
46  // Amy Williams Steve Barrus R. Keith Morley Peter Shirley
47  // University of Utah
48  // http://people.csail.mit.edu/amy/papers/box-jgt.pdf
49  m_dirIsNeg[0] = m_Dir.x < 0.0f;
50  m_dirIsNeg[1] = m_Dir.y < 0.0f;
51  m_dirIsNeg[2] = m_Dir.z < 0.0f;
52 
53  // ray slope
54 
55  // "Fast Ray / Axis-Aligned Bounding Box Overlap Tests using Ray Slopes"
56  // by Martin Eisemann, Thorsten Grosch, Stefan Müller and Marcus Magnor
57  // Computer Graphics Lab, TU Braunschweig, Germany and
58  // University of Koblenz-Landau, Germany
59  // Licence: "This source code is public domain, but please mention us if you use it."
60  //
61  // https://github.com/rjw57/mcvoxel/tree/master/third-party/rayslope
62  // https://github.com/rjw57/mcvoxel/blob/master/third-party/rayslope/ray.cpp
63 
64  ibyj = m_Dir.x * m_InvDir.y;
65  jbyi = m_Dir.y * m_InvDir.x;
66  jbyk = m_Dir.y * m_InvDir.z;
67  kbyj = m_Dir.z * m_InvDir.y;
68  ibyk = m_Dir.x * m_InvDir.z;
69  kbyi = m_Dir.z * m_InvDir.x;
70  c_xy = m_Origin.y - jbyi * m_Origin.x;
71  c_xz = m_Origin.z - kbyi * m_Origin.x;
72  c_yx = m_Origin.x - ibyj * m_Origin.y;
73  c_yz = m_Origin.z - kbyj * m_Origin.y;
74  c_zx = m_Origin.x - ibyk * m_Origin.z;
75  c_zy = m_Origin.y - jbyk * m_Origin.z;
76 
77  // ray slope classification
78  if( m_Dir.x < 0 )
79  {
80  if( m_Dir.y < 0 )
81  {
82  if( m_Dir.z < 0 )
83  {
85  }
86  else if( m_Dir.z > 0 )
87  {
89  }
90  else
91  {
93  }
94  }
95  else
96  {
97  if( m_Dir.z < 0 )
98  {
100 
101  if( m_Dir.y == 0 )
103  }
104  else
105  {
106  if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
108  else if( m_Dir.z == 0 )
110  else if( m_Dir.y == 0 )
112  else
114  }
115  }
116  }
117  else
118  {
119  if( m_Dir.y < 0 )
120  {
121  if( m_Dir.z < 0 )
122  {
124 
125  if( m_Dir.x == 0 )
127  }
128  else
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
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
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 * aEnd_minus_startB.y - aEnd_minus_startA.y *
185  aEnd_minus_startB.x;
186 
187  if( std::abs( rxs ) > glm::epsilon<float>() )
188  {
189  float inv_rxs = 1.0f / rxs;
190 
191  SFVEC2F pq = aStartB - aStartA;
192 
193  float t = ( pq.x * aEnd_minus_startB.y - pq.y * aEnd_minus_startB.x ) * inv_rxs;
194 
195  if( ( t < 0.0f ) || ( t > 1.0f ) )
196  return false;
197 
198  float u = ( pq.x * aEnd_minus_startA.y - pq.y * aEnd_minus_startA.x ) * inv_rxs;
199 
200  if( ( u < 0.0f ) || ( u > 1.0f ) )
201  return false;
202 
203  return true;
204  }
205 
206  return false;
207 }
208 
209 
211 bool RAY::IntersectSphere( const SFVEC3F &aCenter, float aRadius, float &aOutT0,
212  float &aOutT1 ) const
213 {
214  // Ray-sphere intersection: geometric
215  SFVEC3F OC = aCenter - m_Origin;
216  float p_dot_d = glm::dot( OC, m_Dir );
217 
218  if( p_dot_d < 0.0f )
219  return 0.0f;
220 
221  float p_dot_p = glm::dot( OC, OC );
222  float discriminant = p_dot_p - p_dot_d * p_dot_d;
223 
224  if( discriminant > aRadius*aRadius )
225  return false;
226 
227  discriminant = sqrtf( aRadius*aRadius - discriminant );
228 
229  aOutT0 = p_dot_d - discriminant;
230  aOutT1 = p_dot_d + discriminant;
231 
232  if( aOutT0 > aOutT1 )
233  {
234  float temp = aOutT0;
235  aOutT0 = aOutT1;
236  aOutT1 = temp;
237  }
238 
239  return true;
240 }
241 
242 
243 RAYSEG2D::RAYSEG2D( const SFVEC2F& s, const SFVEC2F& e )
244 {
245  m_Start = s;
246  m_End = e;
247  m_End_minus_start = e - s;
248  m_Length = glm::length( m_End_minus_start );
249  m_Dir = glm::normalize( m_End_minus_start );
250  m_InvDir = ( 1.0f / m_Dir );
251 
252  if( fabs( m_Dir.x ) < FLT_EPSILON )
253  m_InvDir.x = NextFloatDown( FLT_MAX );
254 
255  if( fabs( m_Dir.y ) < FLT_EPSILON )
256  m_InvDir.y = NextFloatDown( FLT_MAX );
257 
259 }
260 
261 
262 bool RAYSEG2D::IntersectSegment( const SFVEC2F &aStart, const SFVEC2F &aEnd_minus_start,
263  float *aOutT ) const
264 {
265  float rxs = m_End_minus_start.x * aEnd_minus_start.y - m_End_minus_start.y *
266  aEnd_minus_start.x;
267 
268  if( std::abs( rxs ) > glm::epsilon<float>() )
269  {
270  const float inv_rxs = 1.0f / rxs;
271 
272  const SFVEC2F pq = aStart - m_Start;
273 
274  const float t = ( pq.x * aEnd_minus_start.y - pq.y * aEnd_minus_start.x ) * inv_rxs;
275 
276  if( ( t < 0.0f ) || ( t > 1.0f ) )
277  return false;
278 
279  float u = ( pq.x * m_End_minus_start.y - pq.y * m_End_minus_start.x ) * inv_rxs;
280 
281  if( ( u < 0.0f ) || ( u > 1.0f ) )
282  return false;
283 
284  *aOutT = t;
285 
286  return true;
287  }
288 
289  return false;
290 }
291 
292 
293 // http://geomalgorithms.com/a02-_lines.html
294 float RAYSEG2D::DistanceToPointSquared( const SFVEC2F &aPoint ) const
295 {
296  SFVEC2F p = aPoint - m_Start;
297 
298  const float c1 = glm::dot( p, m_End_minus_start );
299 
300  if( c1 < FLT_EPSILON )
301  return glm::dot( p, p );
302 
303  if( m_DOT_End_minus_start <= c1 )
304  {
305  p = aPoint - m_End;
306  }
307  else
308  {
309  const float b = c1 / m_DOT_End_minus_start;
310  const SFVEC2F pb = m_Start + m_End_minus_start * b;
311 
312  p = aPoint - pb;
313  }
314 
315  return glm::dot( p, p );
316 }
317 
318 
319 bool RAYSEG2D::IntersectCircle( const SFVEC2F &aCenter, float aRadius, float *aOutT0,
320  float *aOutT1, SFVEC2F *aOutNormalT0, SFVEC2F *aOutNormalT1 ) const
321 {
322  // This code used directly from Steve Marschner's CS667 framework
323  // http://cs665pd.googlecode.com/svn/trunk/photon/sphere.cpp
324 
325  // Compute some factors used in computation
326  const float qx = m_Start.x - aCenter.x;
327  const float qy = m_Start.y - aCenter.y;
328 
329  const float qd = qx * m_Dir.x + qy * m_Dir.y;
330  const float qq = qx * qx + qy * qy;
331 
332  // solving the quadratic equation for t at the pts of intersection
333  // dd*t^2 + (2*qd)*t + (qq-r^2) = 0
334  const float discriminantsqr = (qd * qd - (qq - aRadius * aRadius));
335 
336  // If the discriminant is less than zero, there is no intersection
337  if( discriminantsqr < FLT_EPSILON )
338  return false;
339 
340  // Otherwise check and make sure that the intersections occur on the ray (t
341  // > 0) and return the closer one
342  const float discriminant = std::sqrt( discriminantsqr );
343  const float t1 = ( -qd - discriminant );
344  const float t2 = ( -qd + discriminant );
345 
346  if( ( ( t1 < 0.0f ) || ( t1 > m_Length ) ) && ( ( t2 < 0.0f ) || ( t2 > m_Length ) ) )
347  return false; // Neither intersection was in the ray's half line.
348 
349  // Convert the intersection to a normalized
350  *aOutT0 = t1 / m_Length;
351  *aOutT1 = t2 / m_Length;
352 
353  SFVEC2F hitPointT1 = at( t1 );
354  SFVEC2F hitPointT2 = at( t2 );
355 
356  *aOutNormalT0 = ( hitPointT1 - aCenter ) / aRadius;
357  *aOutNormalT1 = ( hitPointT2 - aCenter ) / aRadius;
358 
359  return true;
360 }
float c_xy
Definition: ray.h:73
void Init(const SFVEC3F &o, const SFVEC3F &d)
Definition: ray.cpp:35
bool IntersectSphere(const SFVEC3F &aCenter, float aRadius, float &aOutT0, float &aOutT1) const
Definition: ray.cpp:211
bool IntersectCircle(const SFVEC2F &aCenter, float aRadius, float *aOutT0, float *aOutT1, SFVEC2F *aOutNormalT0, SFVEC2F *aOutNormalT1) const
Definition: ray.cpp:319
float c_zy
Definition: ray.h:73
SFVEC2F m_Dir
Definition: ray.h:110
float jbyk
Definition: ray.h:72
float kbyi
Definition: ray.h:72
float c_yx
Definition: ray.h:73
SFVEC2F at(float t) const
Definition: ray.h:137
SFVEC3F m_InvDir
Definition: ray.h:70
float kbyj
Definition: ray.h:72
glm::vec2 SFVEC2F
Definition: xv3d_types.h:42
float NextFloatDown(float v)
Definition: 3d_fastmath.h:157
RAY_CLASSIFICATION m_Classification
Definition: ray.h:68
bool IntersectSegment(const SFVEC2F &aStart, const SFVEC2F &aEnd_minus_start, float *aOutT) const
Definition: ray.cpp:262
unsigned int m_dirIsNeg[3]
Definition: ray.h:75
float m_DOT_End_minus_start
dot( m_End_minus_start, m_End_minus_start)
Definition: ray.h:113
float m_Length
Definition: ray.h:112
SFVEC2F m_End
Definition: ray.h:108
unsigned int rayID
unique ray ID - not used - dummy
Definition: ray.h:65
SFVEC2F m_InvDir
Definition: ray.h:111
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:67
float ibyj
Definition: ray.h:72
float jbyi
Definition: ray.h:72
float c_yz
Definition: ray.h:73
SFVEC3F m_Origin
Definition: ray.h:64
SFVEC2F m_Start
Definition: ray.h:107
float c_zx
Definition: ray.h:73
RAYSEG2D(const SFVEC2F &s, const SFVEC2F &e)
Definition: ray.cpp:243
glm::vec3 SFVEC3F
Definition: xv3d_types.h:44
float DistanceToPointSquared(const SFVEC2F &aPoint) const
Definition: ray.cpp:294
float c_xz
Definition: ray.h:73
SFVEC2F m_End_minus_start
Definition: ray.h:109
float ibyk
Definition: ray.h:72