PageRenderTime 19ms CodeModel.GetById 1ms app.highlight 13ms RepoModel.GetById 2ms app.codeStats 0ms

/indra/llmath/llline.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 196 lines | 100 code | 29 blank | 67 comment | 4 complexity | d64fc11b1feb080c3ab92d0c99b67a48 MD5 | raw file
  1/** 
  2 * @file llline.cpp
  3 * @author Andrew Meadows
  4 * @brief Simple line class that can compute nearest approach between two lines
  5 *
  6 * $LicenseInfo:firstyear=2006&license=viewerlgpl$
  7 * Second Life Viewer Source Code
  8 * Copyright (C) 2010, Linden Research, Inc.
  9 * 
 10 * This library is free software; you can redistribute it and/or
 11 * modify it under the terms of the GNU Lesser General Public
 12 * License as published by the Free Software Foundation;
 13 * version 2.1 of the License only.
 14 * 
 15 * This library is distributed in the hope that it will be useful,
 16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 18 * Lesser General Public License for more details.
 19 * 
 20 * You should have received a copy of the GNU Lesser General Public
 21 * License along with this library; if not, write to the Free Software
 22 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 23 * 
 24 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 25 * $/LicenseInfo$
 26 */
 27
 28#include "linden_common.h"
 29
 30#include "llline.h"
 31#include "llrand.h"
 32
 33const F32 SOME_SMALL_NUMBER = 1.0e-5f;
 34const F32 SOME_VERY_SMALL_NUMBER = 1.0e-8f;
 35
 36LLLine::LLLine()
 37:	mPoint(0.f, 0.f, 0.f),
 38	mDirection(1.f, 0.f, 0.f)
 39{ }
 40
 41LLLine::LLLine( const LLVector3& first_point, const LLVector3& second_point )
 42{
 43	setPoints(first_point, second_point);
 44}
 45
 46void LLLine::setPoints( const LLVector3& first_point, const LLVector3& second_point )
 47{
 48	mPoint = first_point;
 49	mDirection = second_point - first_point;
 50	mDirection.normalize();
 51}
 52
 53void LLLine::setPointDirection( const LLVector3& first_point, const LLVector3& second_point )
 54{
 55	setPoints(first_point, first_point + second_point);
 56}
 57
 58bool LLLine::intersects( const LLVector3& point, F32 radius ) const
 59{
 60	LLVector3 other_direction = point - mPoint;
 61	LLVector3 nearest_point = mPoint + mDirection * (other_direction * mDirection);
 62	F32 nearest_approach = (nearest_point - point).length();
 63	return (nearest_approach <= radius);
 64}
 65
 66// returns the point on this line that is closest to some_point
 67LLVector3 LLLine::nearestApproach( const LLVector3& some_point ) const
 68{
 69	return (mPoint + mDirection * ((some_point - mPoint) * mDirection));
 70}
 71
 72// the accuracy of this method sucks when you give it two nearly
 73// parallel lines, so you should probably check for parallelism
 74// before you call this
 75// 
 76// returns the point on this line that is closest to other_line
 77LLVector3 LLLine::nearestApproach( const LLLine& other_line ) const
 78{
 79	LLVector3 between_points = other_line.mPoint - mPoint;
 80	F32 dir_dot_dir = mDirection * other_line.mDirection;
 81	F32 one_minus_dir_dot_dir = 1.0f - fabs(dir_dot_dir);
 82	if ( one_minus_dir_dot_dir < SOME_VERY_SMALL_NUMBER )
 83	{
 84#ifdef LL_DEBUG
 85		llwarns << "LLLine::nearestApproach() was given two very "
 86			<< "nearly parallel lines dir1 = " << mDirection 
 87			<< " dir2 = " << other_line.mDirection << " with 1-dot_product = " 
 88			<< one_minus_dir_dot_dir << llendl;
 89#endif
 90		// the lines are approximately parallel
 91		// We shouldn't fall in here because this check should have been made
 92		// BEFORE this function was called.  We dare not continue with the 
 93		// computations for fear of division by zero, but we have to return 
 94		// something so we return a bogus point -- caller beware.
 95		return 0.5f * (mPoint + other_line.mPoint);
 96	}
 97
 98	F32 odir_dot_bp = other_line.mDirection * between_points;
 99
100	F32 numerator = 0;
101	F32 denominator = 0;
102	for (S32 i=0; i<3; i++)
103	{
104		F32 factor = dir_dot_dir * other_line.mDirection.mV[i] - mDirection.mV[i];
105		numerator += ( between_points.mV[i] - odir_dot_bp * other_line.mDirection.mV[i] ) * factor;
106		denominator -= factor * factor;
107	}
108
109	F32 length_to_nearest_approach = numerator / denominator;
110
111	return mPoint + length_to_nearest_approach * mDirection;
112}
113
114std::ostream& operator<<( std::ostream& output_stream, const LLLine& line )
115{
116	output_stream << "{point=" << line.mPoint << "," << "dir=" << line.mDirection << "}";
117	return output_stream;
118}
119
120
121F32 ALMOST_PARALLEL = 0.99f;
122F32 TOO_SMALL_FOR_DIVISION = 0.0001f;
123
124// returns 'true' if this line intersects the plane
125// on success stores the intersection point in 'result'
126bool LLLine::intersectsPlane( LLVector3& result, const LLLine& plane ) const
127{
128	// p = P + l * d     equation for a line
129	// 
130	// N * p = D         equation for a point
131	//
132	// N * (P + l * d) = D
133	// N*P + l * (N*d) = D
134	// l * (N*d) = D - N*P
135	// l =  ( D - N*P ) / ( N*d )
136	//
137
138	F32 dot = plane.mDirection * mDirection;
139	if (fabs(dot) < TOO_SMALL_FOR_DIVISION)
140	{
141		return false;
142	}
143
144	F32 plane_dot = plane.mDirection * plane.mPoint;
145	F32 length = ( plane_dot - (plane.mDirection * mPoint) ) / dot;
146	result = mPoint + length * mDirection;
147	return true;
148}
149
150//static 
151// returns 'true' if planes intersect, and stores the result 
152// the second and third arguments are treated as planes
153// where mPoint is on the plane and mDirection is the normal
154// result.mPoint will be the intersection line's closest approach 
155// to first_plane.mPoint
156bool LLLine::getIntersectionBetweenTwoPlanes( LLLine& result, const LLLine& first_plane, const LLLine& second_plane )
157{
158	// TODO -- if we ever get some generic matrix solving code in our libs
159	// then we should just use that, since this problem is really just
160	// linear algebra.
161
162	F32 dot = fabs(first_plane.mDirection * second_plane.mDirection);
163	if (dot > ALMOST_PARALLEL)
164	{
165		// the planes are nearly parallel
166		return false;
167	}
168
169	LLVector3 direction = first_plane.mDirection % second_plane.mDirection;
170	direction.normalize();
171
172	LLVector3 first_intersection;
173	{
174		LLLine intersection_line(first_plane);
175		intersection_line.mDirection = direction % first_plane.mDirection;
176		intersection_line.mDirection.normalize();
177		intersection_line.intersectsPlane(first_intersection, second_plane);
178	}
179
180	/*
181	LLVector3 second_intersection;
182	{
183		LLLine intersection_line(second_plane);
184		intersection_line.mDirection = direction % second_plane.mDirection;
185		intersection_line.mDirection.normalize();
186		intersection_line.intersectsPlane(second_intersection, first_plane);
187	}
188	*/
189
190	result.mPoint = first_intersection;
191	result.mDirection = direction;
192
193	return true;
194}
195
196