1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
|
/**
* @file llline.cpp
* @author Andrew Meadows
* @brief Simple line class that can compute nearest approach between two lines
*
* Copyright (c) 2001-$CurrentYear$, Linden Research, Inc.
* $License$
*/
#include "linden_common.h"
#include "llline.h"
#include "llrand.h"
const F32 SOME_SMALL_NUMBER = 1.0e-5f;
const F32 SOME_VERY_SMALL_NUMBER = 1.0e-8f;
LLLine::LLLine()
: mPoint(0.f, 0.f, 0.f),
mDirection(1.f, 0.f, 0.f)
{ }
LLLine::LLLine( const LLVector3& first_point, const LLVector3& second_point )
{
setPoints(first_point, second_point);
}
void LLLine::setPoints( const LLVector3& first_point, const LLVector3& second_point )
{
mPoint = first_point;
mDirection = second_point - first_point;
mDirection.normalize();
}
void LLLine::setPointDirection( const LLVector3& first_point, const LLVector3& second_point )
{
setPoints(first_point, first_point + second_point);
}
bool LLLine::intersects( const LLVector3& point, F32 radius ) const
{
LLVector3 other_direction = point - mPoint;
LLVector3 nearest_point = mPoint + mDirection * (other_direction * mDirection);
F32 nearest_approach = (nearest_point - point).length();
return (nearest_approach <= radius);
}
// returns the point on this line that is closest to some_point
LLVector3 LLLine::nearestApproach( const LLVector3& some_point ) const
{
return (mPoint + mDirection * ((some_point - mPoint) * mDirection));
}
// the accuracy of this method sucks when you give it two nearly
// parallel lines, so you should probably check for parallelism
// before you call this
//
// returns the point on this line that is closest to other_line
LLVector3 LLLine::nearestApproach( const LLLine& other_line ) const
{
LLVector3 between_points = other_line.mPoint - mPoint;
F32 dir_dot_dir = mDirection * other_line.mDirection;
F32 one_minus_dir_dot_dir = 1.0f - fabs(dir_dot_dir);
if ( one_minus_dir_dot_dir < SOME_VERY_SMALL_NUMBER )
{
#ifdef LL_DEBUG
llwarns << "LLLine::nearestApproach() was given two very "
<< "nearly parallel lines dir1 = " << mDirection
<< " dir2 = " << other_line.mDirection << " with 1-dot_product = "
<< one_minus_dir_dot_dir << llendl;
#endif
// the lines are approximately parallel
// We shouldn't fall in here because this check should have been made
// BEFORE this function was called. We dare not continue with the
// computations for fear of division by zero, but we have to return
// something so we return a bogus point -- caller beware.
return 0.5f * (mPoint + other_line.mPoint);
}
F32 odir_dot_bp = other_line.mDirection * between_points;
F32 numerator = 0;
F32 denominator = 0;
for (S32 i=0; i<3; i++)
{
F32 factor = dir_dot_dir * other_line.mDirection.mV[i] - mDirection.mV[i];
numerator += ( between_points.mV[i] - odir_dot_bp * other_line.mDirection.mV[i] ) * factor;
denominator -= factor * factor;
}
F32 length_to_nearest_approach = numerator / denominator;
return mPoint + length_to_nearest_approach * mDirection;
}
std::ostream& operator<<( std::ostream& output_stream, const LLLine& line )
{
output_stream << "{point=" << line.mPoint << "," << "dir=" << line.mDirection << "}";
return output_stream;
}
F32 ALMOST_PARALLEL = 0.99f;
F32 TOO_SMALL_FOR_DIVISION = 0.0001f;
// returns 'true' if this line intersects the plane
// on success stores the intersection point in 'result'
bool LLLine::intersectsPlane( LLVector3& result, const LLLine& plane ) const
{
// p = P + l * d equation for a line
//
// N * p = D equation for a point
//
// N * (P + l * d) = D
// N*P + l * (N*d) = D
// l * (N*d) = D - N*P
// l = ( D - N*P ) / ( N*d )
//
F32 dot = plane.mDirection * mDirection;
if (fabs(dot) < TOO_SMALL_FOR_DIVISION)
{
return false;
}
F32 plane_dot = plane.mDirection * plane.mPoint;
F32 length = ( plane_dot - (plane.mDirection * mPoint) ) / dot;
result = mPoint + length * mDirection;
return true;
}
//static
// returns 'true' if planes intersect, and stores the result
// the second and third arguments are treated as planes
// where mPoint is on the plane and mDirection is the normal
// result.mPoint will be the intersection line's closest approach
// to first_plane.mPoint
bool LLLine::getIntersectionBetweenTwoPlanes( LLLine& result, const LLLine& first_plane, const LLLine& second_plane )
{
// TODO -- if we ever get some generic matrix solving code in our libs
// then we should just use that, since this problem is really just
// linear algebra.
F32 dot = fabs(first_plane.mDirection * second_plane.mDirection);
if (dot > ALMOST_PARALLEL)
{
// the planes are nearly parallel
return false;
}
LLVector3 direction = first_plane.mDirection % second_plane.mDirection;
direction.normalize();
LLVector3 first_intersection;
{
LLLine intersection_line(first_plane);
intersection_line.mDirection = direction % first_plane.mDirection;
intersection_line.mDirection.normalize();
intersection_line.intersectsPlane(first_intersection, second_plane);
}
/*
LLVector3 second_intersection;
{
LLLine intersection_line(second_plane);
intersection_line.mDirection = direction % second_plane.mDirection;
intersection_line.mDirection.normalize();
intersection_line.intersectsPlane(second_intersection, first_plane);
}
*/
result.mPoint = first_intersection;
result.mDirection = direction;
return true;
}
|
