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