203 lines
6.4 KiB
C++
203 lines
6.4 KiB
C++
/**
|
|
* @file llline.cpp
|
|
* @author Andrew Meadows
|
|
* @brief Simple line class that can compute nearest approach between two lines
|
|
*
|
|
* $LicenseInfo:firstyear=2006&license=viewergpl$
|
|
*
|
|
* Copyright (c) 2006-2009, Linden Research, Inc.
|
|
*
|
|
* Second Life Viewer Source Code
|
|
* The source code in this file ("Source Code") is provided by Linden Lab
|
|
* to you under the terms of the GNU General Public License, version 2.0
|
|
* ("GPL"), unless you have obtained a separate licensing agreement
|
|
* ("Other License"), formally executed by you and Linden Lab. Terms of
|
|
* the GPL can be found in doc/GPL-license.txt in this distribution, or
|
|
* online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
|
|
*
|
|
* There are special exceptions to the terms and conditions of the GPL as
|
|
* it is applied to this Source Code. View the full text of the exception
|
|
* in the file doc/FLOSS-exception.txt in this software distribution, or
|
|
* online at
|
|
* http://secondlifegrid.net/programs/open_source/licensing/flossexception
|
|
*
|
|
* By copying, modifying or distributing this software, you acknowledge
|
|
* that you have read and understood your obligations described above,
|
|
* and agree to abide by those obligations.
|
|
*
|
|
* ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
|
|
* WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
|
|
* COMPLETENESS OR PERFORMANCE.
|
|
* $/LicenseInfo$
|
|
*/
|
|
|
|
#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;
|
|
}
|
|
|
|
|