KiCad PCB EDA Suite
Loading...
Searching...
No Matches
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
35void 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 {
84 m_Classification = RAY_CLASSIFICATION::MMM;
85 }
86 else if( m_Dir.z > 0 )
87 {
88 m_Classification = RAY_CLASSIFICATION::MMP;
89 }
90 else
91 {
92 m_Classification = RAY_CLASSIFICATION::MMO;
93 }
94 }
95 else
96 {
97 if( m_Dir.z < 0 )
98 {
99 m_Classification = RAY_CLASSIFICATION::MPM;
100
101 if( m_Dir.y == 0 )
102 m_Classification = RAY_CLASSIFICATION::MOM;
103 }
104 else
105 {
106 if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
107 m_Classification = RAY_CLASSIFICATION::MOO;
108 else if( m_Dir.z == 0 )
109 m_Classification = RAY_CLASSIFICATION::MPO;
110 else if( m_Dir.y == 0 )
111 m_Classification = RAY_CLASSIFICATION::MOP;
112 else
113 m_Classification = RAY_CLASSIFICATION::MPP;
114 }
115 }
116 }
117 else
118 {
119 if( m_Dir.y < 0 )
120 {
121 if( m_Dir.z < 0 )
122 {
123 m_Classification = RAY_CLASSIFICATION::PMM;
124
125 if( m_Dir.x == 0 )
126 m_Classification = RAY_CLASSIFICATION::OMM;
127 }
128 else
129 {
130 if( ( m_Dir.x == 0 ) && ( m_Dir.z == 0 ) )
131 m_Classification = RAY_CLASSIFICATION::OMO;
132 else if( m_Dir.z == 0 )
133 m_Classification = RAY_CLASSIFICATION::PMO;
134 else if( m_Dir.x == 0 )
135 m_Classification = RAY_CLASSIFICATION::OMP;
136 else
137 m_Classification = RAY_CLASSIFICATION::PMP;
138 }
139 }
140 else
141 {
142 if( m_Dir.z < 0 )
143 {
144 if( ( m_Dir.x == 0 ) && ( m_Dir.y == 0 ) )
145 m_Classification = RAY_CLASSIFICATION::OOM;
146 else if( m_Dir.x == 0 )
147 m_Classification = RAY_CLASSIFICATION::OPM;
148 else if( m_Dir.y == 0 )
149 m_Classification = RAY_CLASSIFICATION::POM;
150 else
151 m_Classification = RAY_CLASSIFICATION::PPM;
152 }
153 else
154 {
155 if( m_Dir.x == 0 )
156 {
157 if( m_Dir.y == 0 )
158 m_Classification = RAY_CLASSIFICATION::OOP;
159 else if( m_Dir.z == 0 )
160 m_Classification = RAY_CLASSIFICATION::OPO;
161 else
162 m_Classification = RAY_CLASSIFICATION::OPP;
163 }
164 else
165 {
166 if( ( m_Dir.y == 0 ) && ( m_Dir.z == 0 ) )
167 m_Classification = RAY_CLASSIFICATION::POO;
168 else if( m_Dir.y == 0 )
169 m_Classification = RAY_CLASSIFICATION::POP;
170 else if( m_Dir.z == 0 )
171 m_Classification = RAY_CLASSIFICATION::PPO;
172 else
173 m_Classification = RAY_CLASSIFICATION::PPP;
174 }
175 }
176 }
177 }
178}
179
180
181bool 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
211bool 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
243RAYSEG2D::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
262bool 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
294float 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
319bool 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 NextFloatDown(float v)
Definition: 3d_fastmath.h:157
EDA_ANGLE abs(const EDA_ANGLE &aAngle)
Definition: eda_angle.h:390
bool IntersectSegment(const SFVEC2F &aStartA, const SFVEC2F &aEnd_minus_startA, const SFVEC2F &aStartB, const SFVEC2F &aEnd_minus_startB)
Definition: ray.cpp:181
bool IntersectCircle(const SFVEC2F &aCenter, float aRadius, float *aOutT0, float *aOutT1, SFVEC2F *aOutNormalT0, SFVEC2F *aOutNormalT1) const
Definition: ray.cpp:319
bool IntersectSegment(const SFVEC2F &aStart, const SFVEC2F &aEnd_minus_start, float *aOutT) const
Definition: ray.cpp:262
float m_DOT_End_minus_start
dot( m_End_minus_start, m_End_minus_start)
Definition: ray.h:113
float DistanceToPointSquared(const SFVEC2F &aPoint) const
Definition: ray.cpp:294
float m_Length
Definition: ray.h:112
SFVEC2F m_End_minus_start
Definition: ray.h:109
SFVEC2F m_Dir
Definition: ray.h:110
SFVEC2F m_InvDir
Definition: ray.h:111
SFVEC2F at(float t) const
Definition: ray.h:137
SFVEC2F m_Start
Definition: ray.h:107
SFVEC2F m_End
Definition: ray.h:108
RAYSEG2D(const SFVEC2F &s, const SFVEC2F &e)
Definition: ray.cpp:243
float ibyj
Definition: ray.h:72
SFVEC3F m_Dir
Definition: ray.h:67
void Init(const SFVEC3F &o, const SFVEC3F &d)
Definition: ray.cpp:35
unsigned int m_dirIsNeg[3]
Definition: ray.h:75
float c_zx
Definition: ray.h:73
float jbyk
Definition: ray.h:72
RAY_CLASSIFICATION m_Classification
Definition: ray.h:68
float jbyi
Definition: ray.h:72
float c_xz
Definition: ray.h:73
float c_xy
Definition: ray.h:73
float c_yz
Definition: ray.h:73
bool IntersectSphere(const SFVEC3F &aCenter, float aRadius, float &aOutT0, float &aOutT1) const
Definition: ray.cpp:211
float kbyi
Definition: ray.h:72
SFVEC3F m_InvDir
Definition: ray.h:70
SFVEC3F m_Origin
Definition: ray.h:64
float ibyk
Definition: ray.h:72
float c_yx
Definition: ray.h:73
float c_zy
Definition: ray.h:73
unsigned int rayID
unique ray ID - not used - dummy
Definition: ray.h:65
float kbyj
Definition: ray.h:72
glm::vec2 SFVEC2F
Definition: xv3d_types.h:42
glm::vec3 SFVEC3F
Definition: xv3d_types.h:44