1 //
2 // Copyright 2016 Pixar
3 //
4 // Licensed under the Apache License, Version 2.0 (the "Apache License")
5 // with the following modification; you may not use this file except in
6 // compliance with the Apache License and the following modification to it:
7 // Section 6. Trademarks. is deleted and replaced with:
8 //
9 // 6. Trademarks. This License does not grant permission to use the trade
10 // names, trademarks, service marks, or product names of the Licensor
11 // and its affiliates, except as required to comply with Section 4(c) of
12 // the License and to reproduce the content of the NOTICE file.
13 //
14 // You may obtain a copy of the Apache License at
15 //
16 // http://www.apache.org/licenses/LICENSE-2.0
17 //
18 // Unless required by applicable law or agreed to in writing, software
19 // distributed under the Apache License with the above modification is
20 // distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
21 // KIND, either express or implied. See the Apache License for the specific
22 // language governing permissions and limitations under the Apache License.
23 //
24
25 #include "pxr/pxr.h"
26 #include "pxr/base/gf/ray.h"
27 #include "pxr/base/gf/bbox3d.h"
28 #include "pxr/base/gf/line.h"
29 #include "pxr/base/gf/lineSeg.h"
30 #include "pxr/base/gf/math.h"
31 #include "pxr/base/gf/ostreamHelpers.h"
32 #include "pxr/base/gf/plane.h"
33 #include "pxr/base/gf/range3d.h"
34 #include "pxr/base/gf/rotation.h"
35 #include "pxr/base/gf/vec2d.h"
36 #include "pxr/base/tf/type.h"
37
38 #include <ostream>
39
40 PXR_NAMESPACE_OPEN_SCOPE
41
42 // Tolerance for GfIsClose
43 static const double tolerance = 1e-6;
44
45 // CODE_COVERAGE_OFF_GCOV_BUG - gcov is finicky
TF_REGISTRY_FUNCTION(TfType)46 TF_REGISTRY_FUNCTION(TfType) {
47 TfType::Define<GfRay>();
48 }
49 // CODE_COVERAGE_ON_GCOV_BUG
50
51 // Menv 2x uses SetDirection, but it looks like it is only in
52 // sgu/rayIntersectAction.cpp so I'm just going to change the name
53 // and let the compiler complain about any problems.
54 void
SetPointAndDirection(const GfVec3d & startPoint,const GfVec3d & direction)55 GfRay::SetPointAndDirection(const GfVec3d &startPoint,
56 const GfVec3d &direction) {
57 _startPoint = startPoint;
58 _direction = direction;
59 }
60
61 void
SetEnds(const GfVec3d & startPoint,const GfVec3d & endPoint)62 GfRay::SetEnds(const GfVec3d &startPoint, const GfVec3d &endPoint)
63 {
64 _startPoint = startPoint;
65 _direction = endPoint - startPoint;
66 }
67
68 GfRay &
Transform(const GfMatrix4d & matrix)69 GfRay::Transform(const GfMatrix4d &matrix)
70 {
71 _startPoint = matrix.Transform(_startPoint);
72 _direction = matrix.TransformDir(_direction);
73
74 return *this;
75 }
76
77 GfVec3d
FindClosestPoint(const GfVec3d & point,double * rayDistance) const78 GfRay::FindClosestPoint(const GfVec3d &point, double *rayDistance) const
79 {
80 GfLine l;
81 double len = l.Set(_startPoint, _direction);
82 double lrd;
83 (void) l.FindClosestPoint(point, &lrd);
84
85 if (lrd < 0.0) lrd = 0.0;
86
87 if (rayDistance)
88 *rayDistance = lrd / len;
89
90 return l.GetPoint(lrd);
91 }
92
93 bool
GfFindClosestPoints(const GfRay & ray,const GfLine & line,GfVec3d * rayPoint,GfVec3d * linePoint,double * rayDistance,double * lineDistance)94 GfFindClosestPoints(const GfRay &ray, const GfLine &line,
95 GfVec3d *rayPoint,
96 GfVec3d *linePoint,
97 double *rayDistance,
98 double *lineDistance)
99 {
100 GfLine l;
101 double len = l.Set(ray._startPoint, ray._direction);
102
103 GfVec3d rp, lp;
104 double rd,ld;
105
106 if (!GfFindClosestPoints(l, line, &rp, &lp, &rd, &ld))
107 return false;
108
109 if (rd < 0.0) rd = 0.0;
110
111 if (rayPoint)
112 *rayPoint = l.GetPoint(rd);
113
114 if (linePoint)
115 *linePoint = lp;
116
117 if (rayDistance)
118 *rayDistance = rd / len;
119
120 if (lineDistance)
121 *lineDistance = ld;
122
123 return true;
124 }
125
126 bool
GfFindClosestPoints(const GfRay & ray,const GfLineSeg & seg,GfVec3d * rayPoint,GfVec3d * segPoint,double * rayDistance,double * segDistance)127 GfFindClosestPoints(const GfRay &ray, const GfLineSeg &seg,
128 GfVec3d *rayPoint,
129 GfVec3d *segPoint,
130 double *rayDistance,
131 double *segDistance)
132 {
133 GfLine l;
134 double len = l.Set(ray._startPoint, ray._direction);
135
136 GfVec3d rp, sp;
137 double rd,sd;
138
139 if (!GfFindClosestPoints(l, seg, &rp, &sp, &rd, &sd))
140 return false;
141
142 if (rd < 0.0) rd = 0.0;
143
144 if (rayPoint)
145 *rayPoint = l.GetPoint(rd);
146
147 if (segPoint)
148 *segPoint = sp;
149
150 if (rayDistance)
151 *rayDistance = rd / len;
152
153 if (segDistance)
154 *segDistance = sd;
155
156 return true;
157 }
158
159 bool
Intersect(const GfVec3d & p0,const GfVec3d & p1,const GfVec3d & p2,double * distance,GfVec3d * barycentricCoords,bool * frontFacing,double maxDist) const160 GfRay::Intersect(const GfVec3d &p0, const GfVec3d &p1, const GfVec3d &p2,
161 double *distance,
162 GfVec3d *barycentricCoords, bool *frontFacing,
163 double maxDist) const
164 {
165 // Intersect the ray with the plane containing the three points.
166 GfPlane plane(p0, p1, p2);
167 double intersectionDist;
168 if (! Intersect(plane, &intersectionDist, frontFacing))
169 return false;
170
171 if (intersectionDist > maxDist)
172 return false;
173
174 // Find the largest component of the plane normal. The other two
175 // dimensions are the axes of the aligned plane we will use to
176 // project the triangle.
177 double xAbs = GfAbs(plane.GetNormal()[0]);
178 double yAbs = GfAbs(plane.GetNormal()[1]);
179 double zAbs = GfAbs(plane.GetNormal()[2]);
180 unsigned int axis0, axis1;
181 if (xAbs > yAbs && xAbs > zAbs) {
182 axis0 = 1;
183 axis1 = 2;
184 }
185 else if (yAbs > zAbs) {
186 axis0 = 2;
187 axis1 = 0;
188 }
189 else {
190 axis0 = 0;
191 axis1 = 1;
192 }
193
194 // Determine if the projected intersection (of the ray's line and
195 // the triangle's plane) lies within the projected triangle.
196 // Since we deal with only 2 components, we can avoid the third
197 // computation.
198 double inter0 = _startPoint[axis0] + intersectionDist * _direction[axis0];
199 double inter1 = _startPoint[axis1] + intersectionDist * _direction[axis1];
200 GfVec2d d0(inter0 - p0[axis0], inter1 - p0[axis1]);
201 GfVec2d d1(p1[axis0] - p0[axis0], p1[axis1] - p0[axis1]);
202 GfVec2d d2(p2[axis0] - p0[axis0], p2[axis1] - p0[axis1]);
203
204 // XXX This code can miss some intersections on very tiny tris.
205 double alpha;
206 double beta = ((d0[1] * d1[0] - d0[0] * d1[1]) /
207 (d2[1] * d1[0] - d2[0] * d1[1]));
208 // clamp beta to 0 if it is only very slightly less than 0
209 if (beta < 0 && beta > -GF_MIN_VECTOR_LENGTH) {
210 // CODE_COVERAGE_OFF_NO_REPORT - architecture dependent numerics
211 beta = 0;
212 // CODE_COVERAGE_ON_NO_REPORT
213 }
214 if (beta < 0.0 || beta > 1.0) {
215 return false;
216 }
217 alpha = -1.0;
218 if (d1[1] < -GF_MIN_VECTOR_LENGTH || d1[1] > GF_MIN_VECTOR_LENGTH)
219 alpha = (d0[1] - beta * d2[1]) / d1[1];
220 else
221 alpha = (d0[0] - beta * d2[0]) / d1[0];
222
223 // clamp alpha to 0 if it is only very slightly less than 0
224 if (alpha < 0 && alpha > -GF_MIN_VECTOR_LENGTH) {
225 // CODE_COVERAGE_OFF_NO_REPORT - architecture dependent numerics
226 alpha = 0;
227 // CODE_COVERAGE_ON_NO_REPORT
228 }
229
230 // clamp gamma to 0 if it is only very slightly less than 0
231 float gamma = 1.0 - (alpha + beta);
232 if (gamma < 0 && gamma > -GF_MIN_VECTOR_LENGTH) {
233 // CODE_COVERAGE_OFF_NO_REPORT - architecture dependent numerics
234 gamma = 0;
235 // CODE_COVERAGE_ON_NO_REPORT
236 }
237 if (alpha < 0.0 || gamma < 0.0)
238 return false;
239
240 if (distance)
241 *distance = intersectionDist;
242 if (barycentricCoords)
243 barycentricCoords->Set(gamma, alpha, beta);
244
245 return true;
246 }
247
248 bool
Intersect(const GfPlane & plane,double * distance,bool * frontFacing) const249 GfRay::Intersect(const GfPlane &plane,
250 double *distance, bool *frontFacing) const
251 {
252 // The dot product of the ray direction and the plane normal
253 // indicates the angle between them. Reject glancing
254 // intersections. Note: this also rejects ill-formed planes with
255 // zero normals.
256 double d = GfDot(_direction, plane.GetNormal());
257 if (d < GF_MIN_VECTOR_LENGTH && d > -GF_MIN_VECTOR_LENGTH)
258 return false;
259
260 // Get a point on the plane.
261 GfVec3d planePoint = plane.GetDistanceFromOrigin() * plane.GetNormal();
262
263 // Compute the parametric distance t to the plane. Reject
264 // intersections outside the ray bounds.
265 double t = GfDot(planePoint - _startPoint, plane.GetNormal()) / d;
266 if (t < 0.0)
267 return false;
268
269 if (distance)
270 *distance = t;
271 if (frontFacing)
272 *frontFacing = (d < 0.0);
273
274 return true;
275 }
276
277 bool
Intersect(const GfRange3d & box,double * enterDistance,double * exitDistance) const278 GfRay::Intersect(const GfRange3d &box,
279 double *enterDistance, double *exitDistance) const
280 {
281 if (box.IsEmpty())
282 return false;
283
284 // Compute the intersection distance of all 6 planes of the
285 // box. Save the largest near-plane intersection and the smallest
286 // far-plane intersection.
287 double maxNearest = -DBL_MAX, minFarthest = DBL_MAX;
288 for (size_t i = 0; i < 3; i++) {
289
290 // Skip dimensions almost parallel to the ray.
291 double d = GetDirection()[i];
292 if (GfAbs(d) < GF_MIN_VECTOR_LENGTH) {
293 // ray is parallel to this set of planes.
294 // If origin is not between them, no intersection.
295 if (GetStartPoint()[i] < box.GetMin()[i] ||
296 GetStartPoint()[i] > box.GetMax()[i]) {
297 return false;
298 } else {
299 continue;
300 }
301 }
302
303 d = 1.0 / d;
304 double t1 = d * (box.GetMin()[i] - GetStartPoint()[i]);
305 double t2 = d * (box.GetMax()[i] - GetStartPoint()[i]);
306
307 // Make sure t1 is the nearer one
308 if (t1 > t2) {
309 double tmp = t1;
310 t1 = t2;
311 t2 = tmp;
312 }
313
314 // Update the min and max
315 if (t1 > maxNearest)
316 maxNearest = t1;
317 if (t2 < minFarthest)
318 minFarthest = t2;
319 }
320
321 // If the largest near-plane intersection is after the smallest
322 // far-plane intersection, the ray's line misses the box. Also
323 // check if both intersections are completely outside the near/far
324 // bounds.
325 if (maxNearest > minFarthest ||
326 minFarthest < 0.0)
327 return false;
328
329 if (enterDistance)
330 *enterDistance = maxNearest;
331 if (exitDistance)
332 *exitDistance = minFarthest;
333 return true;
334 }
335
336 bool
Intersect(const GfBBox3d & box,double * enterDistance,double * exitDistance) const337 GfRay::Intersect(const GfBBox3d& box,
338 double* enterDistance, double* exitDistance ) const
339 {
340 // Transform the ray to the local space of the bbox.
341 GfRay localRay(*this);
342 localRay.Transform(box.GetInverseMatrix());
343
344 // We take advantage of the fact that the time of intersection is
345 // invariant before/after transformation to just return the results of
346 // the intersection in local space.
347 return localRay.Intersect(box.GetRange(), enterDistance, exitDistance);
348 }
349
350 bool
Intersect(const GfVec3d & center,double radius,double * enterDistance,double * exitDistance) const351 GfRay::Intersect(const GfVec3d& center, double radius,
352 double* enterDistance, double* exitDistance ) const
353 {
354
355 GfVec3d p1 = _startPoint;
356 GfVec3d p2 = p1 + _direction;
357
358 // Intersection pt: p = p1 + u (p2 -p1)
359 // we are solving for u.
360 // where p1 = [x1 y1 z1], p2 = [x2 y2 z2]
361 //
362 double A, B, C;
363 double x1, x2, x3, y1, y2, y3, z1, z2, z3;
364 x1 = p1[0]; y1 = p1[1]; z1 = p1[2];
365 x2 = p2[0]; y2 = p2[1]; z2 = p2[2];
366 x3 = center[0]; y3 = center[1]; z3 = center[2];
367
368 A = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1);
369 B = 2 * ((x2-x1)*(x1-x3) + (y2-y1)*(y1-y3) + (z2-z1)*(z1-z3));
370 C = x3*x3 + y3*y3 + z3*z3 + x1*x1 + y1*y1 + z1*z1
371 - 2*(x3*x1 +y3*y1 +z3*z1) - radius*radius;
372
373 return _SolveQuadratic(A, B, C, enterDistance, exitDistance);
374 }
375
376 bool
Intersect(const GfVec3d & origin,const GfVec3d & axis,const double radius,double * enterDistance,double * exitDistance) const377 GfRay::Intersect(const GfVec3d &origin,
378 const GfVec3d &axis,
379 const double radius,
380 double *enterDistance,
381 double *exitDistance) const
382 {
383 GfVec3d unitAxis = axis.GetNormalized();
384
385 GfVec3d delta = _startPoint - origin;
386 GfVec3d u = _direction - GfDot(_direction, unitAxis) * unitAxis;
387 GfVec3d v = delta - GfDot(delta, unitAxis) * unitAxis;
388
389 // Quadratic equation for implicit infinite cylinder
390 double a = GfDot(u, u);
391 double b = 2.0 * GfDot(u, v);
392 double c = GfDot(v, v) - GfSqr(radius);
393
394 return _SolveQuadratic(a, b, c, enterDistance, exitDistance);
395 }
396
397 bool
Intersect(const GfVec3d & origin,const GfVec3d & axis,const double radius,const double height,double * enterDistance,double * exitDistance) const398 GfRay::Intersect(const GfVec3d &origin,
399 const GfVec3d &axis,
400 const double radius,
401 const double height,
402 double *enterDistance,
403 double *exitDistance) const
404 {
405 GfVec3d unitAxis = axis.GetNormalized();
406
407 // Apex of cone
408 GfVec3d apex = origin + height * unitAxis;
409
410 GfVec3d delta = _startPoint - apex;
411 GfVec3d u =_direction - GfDot(_direction, unitAxis) * unitAxis;
412 GfVec3d v = delta - GfDot(delta, unitAxis) * unitAxis;
413
414 double p = GfDot(_direction, unitAxis);
415 double q = GfDot(delta, unitAxis);
416
417 double cos2 = GfSqr(height) / (GfSqr(height) + GfSqr(radius));
418 double sin2 = 1 - cos2;
419
420 double a = cos2 * GfDot(u, u) - sin2 * GfSqr(p);
421 double b = 2.0 * (cos2 * GfDot(u, v) - sin2 * p * q);
422 double c = cos2 * GfDot(v, v) - sin2 * GfSqr(q);
423
424 if (!_SolveQuadratic(a, b, c, enterDistance, exitDistance)) {
425 return false;
426 }
427
428 // Eliminate any solutions on the double cone
429 bool enterValid = GfDot(unitAxis, GetPoint(*enterDistance) - apex) <= 0.0;
430 bool exitValid = GfDot(unitAxis, GetPoint(*exitDistance) - apex) <= 0.0;
431
432 if ((!enterValid) && (!exitValid)) {
433
434 // Solutions lie only on double cone
435 return false;
436 }
437
438 if (!enterValid) {
439 *enterDistance = *exitDistance;
440 }
441 else if (!exitValid) {
442 *exitDistance = *enterDistance;
443 }
444
445 return true;
446 }
447
448 bool
_SolveQuadratic(const double a,const double b,const double c,double * enterDistance,double * exitDistance) const449 GfRay::_SolveQuadratic(const double a,
450 const double b,
451 const double c,
452 double *enterDistance,
453 double *exitDistance) const
454 {
455 if (GfIsClose(a, 0.0, tolerance)) {
456 if (GfIsClose(b, 0.0, tolerance)) {
457
458 // Degenerate solution
459 return false;
460 }
461
462 double t = -c / b;
463
464 if (t < 0.0) {
465 return false;
466 }
467
468 if (enterDistance) {
469 *enterDistance = t;
470 }
471
472 if (exitDistance) {
473 *exitDistance = t;
474 }
475
476 return true;
477 }
478
479 // Discriminant
480 double disc = GfSqr(b) - 4.0 * a * c;
481
482 if (GfIsClose(disc, 0.0, tolerance)) {
483
484 // Tangent
485 double t = -b / (2.0 * a);
486
487 if (t < 0.0) {
488 return false;
489 }
490
491 if (enterDistance) {
492 *enterDistance = t;
493 }
494
495 if (exitDistance) {
496 *exitDistance = t;
497 }
498
499 return true;
500 }
501
502 if (disc < 0.0) {
503
504 // No intersection
505 return false;
506 }
507
508 // Two intersection points
509 double q = -0.5 * (b + copysign(1.0, b) * GfSqrt(disc));
510 double t0 = q / a;
511 double t1 = c / q;
512
513 if (t0 > t1) {
514 std::swap(t0, t1);
515 }
516
517 if (t1 >= 0) {
518
519 if (enterDistance) {
520 *enterDistance = t0;
521 }
522
523 if (exitDistance) {
524 *exitDistance = t1;
525 }
526
527 return true;
528 }
529
530 return false;
531 }
532
533 std::ostream &
operator <<(std::ostream & out,const GfRay & r)534 operator<<(std::ostream& out, const GfRay& r)
535 {
536 return out << '[' << Gf_OstreamHelperP(r.GetStartPoint()) << " >> "
537 << Gf_OstreamHelperP(r.GetDirection()) << ']';
538 }
539
540 PXR_NAMESPACE_CLOSE_SCOPE
541