1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "Math/BsLineSegment3.h"
4#include "Math/BsRay.h"
5
6namespace bs
7{
8 LineSegment3::LineSegment3(const Vector3& start, const Vector3& end)
9 : start(start), end(end)
10 {
11
12 }
13
14 std::pair<std::array<Vector3, 2>, float> LineSegment3::getNearestPoint(const Ray& ray) const
15 {
16 const Vector3& org = ray.getOrigin();
17 const Vector3& dir = ray.getDirection();
18
19 Vector3 segDir = end - start;
20 float segExtent = segDir.normalize() * 0.5f;
21 Vector3 segCenter = start + segDir * segExtent;
22
23 Vector3 diff = org - segCenter;
24 float a01 = -dir.dot(segDir);
25 float b0 = diff.dot(dir);
26 float c = diff.dot(diff);
27 float det = fabs(1.0f - a01*a01);
28
29 float s0, s1;
30 float sqrDistance;
31 if (det > 0.0f) // Not parallel
32 {
33
34 float b1 = -diff.dot(segDir);
35 s1 = a01 * b0 - b1;
36 float extDet = segExtent * det;
37
38 if (s1 >= -extDet)
39 {
40 if (s1 <= extDet) // Interior of the segment and interior of the ray are closest
41 {
42 float invDet = 1.0f / det;
43 s0 = (a01*b1 - b0)*invDet;
44 s1 *= invDet;
45
46 sqrDistance = s0*(s0 + a01*s1 + 2.0f*b0) +
47 s1*(a01*s0 + s1 + 2.0f*b1) + c;
48 }
49 else // Segment end and interior of the ray are closest
50 {
51 s1 = segExtent;
52 s0 = -(a01*s1 + b0);
53 sqrDistance = -s0*s0 + s1*(s1 + (2.0f)*b1) + c;
54 }
55 }
56 else // Segment start and interior of the ray are closest
57 {
58 s1 = -segExtent;
59 s0 = -(a01*s1 + b0);
60 sqrDistance = -s0*s0 + s1*(s1 + (2.0f)*b1) + c;
61 }
62 }
63 else // Parallel
64 {
65 s1 = 0;
66 s0 = -b0;
67 sqrDistance = b0*s0 + c;
68 }
69
70 if (sqrDistance < 0.0f)
71 sqrDistance = 0.0f;
72
73 float distance = std::sqrt(sqrDistance);
74
75 std::array<Vector3, 2> nearestPoints;
76 nearestPoints[0] = org + s0 * dir;
77 nearestPoints[1] = segCenter + s1 * segDir;
78
79 return std::make_pair(nearestPoints, distance);
80 }
81}
82