octave_packages/geometry-1.5.0/geom2d/intersectLines.m

   1 %% Copyright (c) 2011, INRA
   2 %% 2003-2011, David Legland <david.legland@grignon.inra.fr>
   3 %% 2011 Adapted to Octave by Juan Pablo Carbajal <carbajal@ifi.uzh.ch>
   4 %%
   5 %% All rights reserved.
   6 %% (simplified BSD License)
   7 %%
   8 %% Redistribution and use in source and binary forms, with or without
   9 %% modification, are permitted provided that the following conditions are met:
  10 %%
  11 %% 1. Redistributions of source code must retain the above copyright notice, this
  12 %%    list of conditions and the following disclaimer.
  13 %%
  14 %% 2. Redistributions in binary form must reproduce the above copyright notice,
  15 %%    this list of conditions and the following disclaimer in the documentation
  16 %%    and/or other materials provided with the distribution.
  17 %%
  18 %% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  19 %% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20 %% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21 %% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  22 %% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  23 %% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  24 %% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  25 %% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  26 %% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  27 %% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  28 %% POSSIBILITY OF SUCH DAMAGE.
  29 %%
  30 %% The views and conclusions contained in the software and documentation are
  31 %% those of the authors and should not be interpreted as representing official
  32 %% policies, either expressed or implied, of copyright holder.
  33
  34 %% -*- texinfo -*-
  35 %% @deftypefn {Function File} {@var{point} =} intersectLines (@var{line1}, @var{line2})
  36 %% @deftypefnx {Function File} {@var{point} =} intersectLines (@var{line1}, @var{line2},@var{eps})
  37 %% Return all intersection points of N lines in 2D.
  38 %%
  39 %% Returns the intersection point of lines @var{line1} and @var{line2}.
  40 %% @var{line1} and @var{line2} are [1*4]
  41 %%   arrays, containing parametric representation of each line (in the form
  42 %%   [x0 y0 dx dy], see @code{createLine} for details).
  43 %%
  44 %%   In case of colinear lines, returns [Inf Inf].
  45 %%   In case of parallel but not colinear lines, returns [NaN NaN].
  46 %%
  47 %%   If each input is [N*4] array, the result is a [N*2] array containing
  48 %%   intersections of each couple of lines.
  49 %%   If one of the input has N rows and the other 1 row, the result is a
  50 %%   [N*2] array.
  51 %%
  52 %% A third input argument specifies the tolerance for detecting parallel lines.
  53 %% Default is 1e-14.
  54 %%
  55 %% Example
  56 %%
  57 %% @example
  58 %%   line1 = createLine([0 0], [10 10]);
  59 %%   line2 = createLine([0 10], [10 0]);
  60 %%   point = intersectLines(line1, line2)
  61 %%   point =
  62 %%       5   5
  63 %% @end example
  64 %%
  65 %% @seealso{lines2d, edges2d, intersectEdges, intersectLineEdge, intersectLineCircle}
  66 %% @end deftypefn
  67
  68 function point = intersectLines(line1, line2, varargin)
  69
  70   % extreact tolerance
  71   tol = 1e-14;
  72   if !isempty(varargin)
  73       tol = varargin{1};
  74   end
  75
  76   x1 =  line1(:,1);
  77   y1 =  line1(:,2);
  78   dx1 = line1(:,3);
  79   dy1 = line1(:,4);
  80
  81   x2 =  line2(:,1);
  82   y2 =  line2(:,2);
  83   dx2 = line2(:,3);
  84   dy2 = line2(:,4);
  85
  86   N1 = length(x1);
  87   N2 = length(x2);
  88
  89   % indices of parallel lines
  90   par = abs(dx1.*dy2 - dx2.*dy1) < tol;
  91
  92   % indices of colinear lines
  93   col = abs((x2-x1) .* dy1 - (y2-y1) .* dx1) < tol & par ;
  94
  95   x0(col) = Inf;
  96   y0(col) = Inf;
  97   x0(par & !col) = NaN;
  98   y0(par & !col) = NaN;
  99
 100   i = !par;
 101
 102   % compute intersection points
 103   if N1==N2
 104     x0(i) = ((y2(i)-y1(i)).*dx1(i).*dx2(i) + x1(i).*dy1(i).*dx2(i) - x2(i).*dy2(i).*dx1(i)) ./ ...
 105           (dx2(i).*dy1(i)-dx1(i).*dy2(i)) ;
 106     y0(i) = ((x2(i)-x1(i)).*dy1(i).*dy2(i) + y1(i).*dx1(i).*dy2(i) - y2(i).*dx2(i).*dy1(i)) ./ ...
 107           (dx1(i).*dy2(i)-dx2(i).*dy1(i)) ;
 108
 109   elseif N1==1
 110     x0(i) = ((y2(i)-y1).*dx1.*dx2(i) + x1.*dy1.*dx2(i) - x2(i).*dy2(i).*dx1) ./ ...
 111           (dx2(i).*dy1-dx1.*dy2(i)) ;
 112     y0(i) = ((x2(i)-x1).*dy1.*dy2(i) + y1.*dx1.*dy2(i) - y2(i).*dx2(i).*dy1) ./ ...
 113           (dx1.*dy2(i)-dx2(i).*dy1) ;
 114
 115   elseif N2==1
 116        x0(i) = ((y2-y1(i)).*dx1(i).*dx2 + x1(i).*dy1(i).*dx2 - x2.*dy2.*dx1(i)) ./ ...
 117           (dx2.*dy1(i)-dx1(i).*dy2) ;
 118     y0(i) = ((x2-x1(i)).*dy1(i).*dy2 + y1(i).*dx1(i).*dy2 - y2.*dx2.*dy1(i)) ./ ...
 119           (dx1(i).*dy2-dx2.*dy1(i)) ;
 120
 121   else
 122       % formattage a rajouter
 123        x0(i) = ((y2(i)-y1(i)).*dx1(i).*dx2(i) + x1(i).*dy1(i).*dx2(i) - x2(i).*dy2(i).*dx1(i)) ./ ...
 124           (dx2(i).*dy1(i)-dx1(i).*dy2(i)) ;
 125     y0(i) = ((x2(i)-x1(i)).*dy1(i).*dy2(i) + y1(i).*dx1(i).*dy2(i) - y2(i).*dx2(i).*dy1(i)) ./ ...
 126           (dx1(i).*dy2(i)-dx2(i).*dy1(i)) ;
 127   end
 128
 129   % concatenate result
 130   point = [x0' y0'];
 131
 132 endfunction
 133
 134 %!test % basic test with two orthogonal lines
 135 %!  line1 = [3 1 0 1];
 136 %!  line2 = [1 4 1 0];
 137 %!  assert (intersectLines(line1, line2), [3 4], 1e-6);
 138
 139 %!test % orthognal diagonal lines
 140 %!  line1 = [0 0 3 2];
 141 %!  line2 = [5 -1 4 -6];
 142 %!  assert (intersectLines(line1, line2), [3 2], 1e-6);
 143
 144 %!test % one diagonal and one horizontal line
 145 %!  line1 = [10 2 25 0];
 146 %!  line2 = [5 -1 4 -6];
 147 %!  assert (intersectLines(line1, line2), [3 2], 1e-6);
 148
 149 %!test % check for dx and dy very big compared to other line
 150 %!  line1 = [3 1 0 1000];
 151 %!  line2 = [1 4 -14 0];
 152 %!  assert (intersectLines(line1, line2), [3 4], 1e-6);
 153
 154 %!test
 155 %!  line1 = [2 0 20000 30000];
 156 %!  line2 = [1 6 1 -1];
 157 %!  assert (intersectLines(line1, line2), [4 3], 1e-6);
 158
 159 %!test
 160 %!  line1 = [3 1 0 1];
 161 %!  line2 = repmat([1 4 1 0], 5, 1);
 162 %!  res = repmat([3 4], 5, 1);
 163 %!  inters = intersectLines(line1, line2);
 164 %!  assert (res, inters, 1e-6);
 165
 166 %!test
 167 %!  line1 = repmat([3 1 0 1], 5, 1);
 168 %!  line2 = [1 4 1 0];
 169 %!  res = repmat([3 4], 5, 1);
 170 %!  inters = intersectLines(line1, line2);
 171 %!  assert (res, inters, 1e-6);
 172
 173 %!test
 174 %!  line1 = repmat([3 1 0 1], 5, 1);
 175 %!  line2 = repmat([1 4 1 0], 5, 1);
 176 %!  res = repmat([3 4], 5, 1);
 177 %!  inters = intersectLines(line1, line2);
 178 %!  assert (res, inters, 1e-6);