Get orthogonal points on other spline

Hi There,

my problem is, that I have 2 splines. The first one is devided in 20 equally sized parts. Now I have to get the 20 points on the other Spline, that are orthogonal on der blue spline. I tried it by just deviding the red one as well into 20 parts, but I realised, that the points are not orthogonal then.

I hope somebody can help me with my problem.

Thanks in advance.

(You got to have the "interparc" function by John D'Errico)

n = 3;
n1 = n-1;
a = 20;             % Breite des Krummers entlang x
b = 10;             % Höhe des Krümmers entlang y
P = [0 b;0 0;a 0];
x = P(:,1);         % x-Werte für scatter
y = P(:,2);         % y-Werte für scatter
T = 20;             % Anazhl an Teilpunkten für den Plot
H = 5;
R = 1;

t = linspace(0,1);
B = bernsteinMatrix(n1,t);
bezierCurve = B*P;
px = bezierCurve(:,1);
py = bezierCurve(:,2);
pt = interparc(T,px,py,'spline');  % x,y-Werte!!
plot(px,py,pt(:,1),pt(:,2),'b-o')
axis equal
axis tight
grid on
hold on

syms t
B = bernsteinMatrix(n1,t);
bezierCurve = B*P;
normalToCurve = diff(bezierCurve)*[0 1;-1 0]; 
normalToCurve = normalToCurve/norm(normalToCurve); 
newNormalToCurve = [double(subs(normalToCurve(1),t,linspace(0,1)))' double(subs(normalToCurve(2),t,linspace(0,1)))'];
f = linspace(0,1);
B = bernsteinMatrix(n1,f);
bezierCurve = B*P;
px = bezierCurve(:,1);
py = bezierCurve(:,2);

assume(t>=0 & t<=1)
B = bernsteinMatrix(n1,t);
bezierCurve = simplify(B*P);  
s(t)=int(norm(diff(bezierCurve)),0,t);

for i = 1:100
    S = double(s((i-1)/99));
    d = R*(1-S/double(s(1)))+(H/2)*S/double(s(1));
    delta(i) = d;
end
delta = delta';
pxnew1 = px+newNormalToCurve(:,1).*delta;
pynew1 = py+newNormalToCurve(:,2).*delta;
plot(pxnew1, pynew1,'r')

That's how I tried it, but did not work as I wanted

ptnew1 = interparc(T,pxnew1,pynew1,'spline');
plot(pxnew1,pynew1,ptnew1(:,1),ptnew1(:,2),'r-o')

function [pt,dudt,fofthandle] = interparc(t,px,py,varargin)
% interparc: interpolate points along a curve in 2 or more dimensions
% usage: pt = interparc(t,px,py)    % a 2-d curve
% usage: pt = interparc(t,px,py,pz) % a 3-d curve
% usage: pt = interparc(t,px,py,pz,pw,...) % a 4-d or higher dimensional curve
% usage: pt = interparc(t,px,py,method) % a 2-d curve, method is specified
% usage: [pt,dudt,fofthandle] = interparc(t,px,py,...) % also returns derivatives, and a function handle
%
% Interpolates new points at any fractional point along
% the curve defined by a list of points in 2 or more
% dimensions. The curve may be defined by any sequence
% of non-replicated points.
%
% arguments: (input)
%  t   - vector of numbers, 0 <= t <= 1, that define
%        the fractional distance along the curve to
%        interpolate the curve at. t = 0 will generate
%        the very first point in the point list, and
%        t = 1 yields the last point in that list.
%        Similarly, t = 0.5 will yield the mid-point
%        on the curve in terms of arc length as the
%        curve is interpolated by a parametric spline.
%
%        If t is a scalar integer, at least 2, then
%        it specifies the number of equally spaced
%        points in arclength to be generated along
%        the curve.
%
%  px, py, pz, ... - vectors of length n, defining
%        points along the curve. n must be at least 2.
%        Exact Replicate points should not be present
%        in the curve, although there is no constraint
%        that the curve has replicate independent
%        variables.
%
%  method - (OPTIONAL) string flag - denotes the method
%        used to compute the points along the curve.
%
%        method may be any of 'linear', 'spline', or 'pchip',
%        or any simple contraction thereof, such as 'lin',
%        'sp', or even 'p'.
%        
%        method == 'linear' --> Uses a linear chordal
%               approximation to interpolate the curve.
%               This method is the most efficient.
%
%        method == 'pchip' --> Uses a parametric pchip
%               approximation for the interpolation
%               in arc length.
%
%        method == 'spline' --> Uses a parametric spline
%               approximation for the interpolation in
%               arc length. Generally for a smooth curve,
%               this method may be most accurate.
%
%        method = 'csape' --> if available, this tool will
%               allow a periodic spline fit for closed curves.
%               ONLY use this method if your points should
%               represent a closed curve.
%               
%               If the last point is NOT the same as the
%               first point on the curve, then the curve
%               will be forced to be periodic by this option.
%               That is, the first point will be replicated
%               onto the end.
%
%               If csape is not present in your matlab release,
%               then an error will result.
%
%        DEFAULT: 'spline'
%
%
% arguments: (output)
%  pt - Interpolated points at the specified fractional
%        distance (in arc length) along the curve.
%
%  dudt - when a second return argument is required,
%       interparc will return the parametric derivatives
%       (dx/dt, dy/dt, dz/dt, ...) as an array.
%
%  fofthandle - a function handle, taking numbers in the interval [0,1]
%       and evaluating the function at those points.
%
%       Extrapolation will not be permitted by this call.
%       Any values of t that lie outside of the interval [0,1]
%       will be clipped to the endpoints of the curve.
%
% Example:
% % Interpolate a set of unequally spaced points around
% % the perimeter of a unit circle, generating equally
% % spaced points around the perimeter.
% theta = sort(rand(15,1))*2*pi;
% theta(end+1) = theta(1);
% px = cos(theta);
% py = sin(theta);
%
% % interpolate using parametric splines
% pt = interparc(100,px,py,'spline');
%
% % Plot the result
% plot(px,py,'r*',pt(:,1),pt(:,2),'b-o')
% axis([-1.1 1.1 -1.1 1.1])
% axis equal
% grid on
% xlabel X
% ylabel Y
% title 'Points in blue are uniform in arclength around the circle'
%
%
% Example:
% % For the previous set of points, generate exactly 6
% % points around the parametric splines, verifying
% % the uniformity of the arc length interpolant.
% pt = interparc(6,px,py,'spline');
%
% % Convert back to polar form. See that the radius
% % is indeed 1, quite accurately.
% [TH,R] = cart2pol(pt(:,1),pt(:,2))
% % TH =
% %       0.86005
% %        2.1141
% %       -2.9117
% %        -1.654
% %      -0.39649
% %       0.86005
% % R =
% %             1
% %        0.9997
% %        0.9998
% %       0.99999
% %        1.0001
% %             1
%
% % Unwrap the polar angles, and difference them.
% diff(unwrap(TH))
% % ans =
% %        1.2541
% %        1.2573
% %        1.2577
% %        1.2575
% %        1.2565
%
% % Six points around the circle should be separated by
% % 2*pi/5 radians, if they were perfectly uniform. The
% % slight differences are due to the imperfect accuracy
% % of the parametric splines.
% 2*pi/5
% % ans =
% %        1.2566
%
%
% See also: arclength, spline, pchip, interp1
%
% Author: John D'Errico
% e-mail: [e-mail]
% Release: 1.0
% Release date: 3/15/2010

% unpack the arguments and check for errors
if nargin < 3
  error('ARCLENGTH:insufficientarguments', ...
    'at least t, px, and py must be supplied')
end

t = t(:);
if (numel(t) == 1) && (t > 1) && (rem(t,1) == 0)
  % t specifies the number of points to be generated
  % equally spaced in arclength
  t = linspace(0,1,t)';
elseif any(t < 0) || any(t > 1)
  error('ARCLENGTH:impropert', ...
    'All elements of t must be 0 <= t <= 1')
end

% how many points will be interpolated?
nt = numel(t);

% the number of points on the curve itself
px = px(:);
py = py(:);
n = numel(px);

% are px and py both vectors of the same length?
if ~isvector(px) || ~isvector(py) || (length(py) ~= n)
  error('ARCLENGTH:improperpxorpy', ...
    'px and py must be vectors of the same length')
elseif n < 2
  error('ARCLENGTH:improperpxorpy', ...
    'px and py must be vectors of length at least 2')
end

% compose px and py into a single array. this way,
% if more dimensions are provided, the extension
% is trivial.
pxy = [px,py];
ndim = 2;

% the default method is 'linear'
method = 'spline';

% are there any other arguments?
if nargin > 3
  % there are. check the last argument. Is it a string?
  if ischar(varargin{end})
    method = varargin{end};
    varargin(end) = [];
    
    % method may be any of {'linear', 'pchip', 'spline', 'csape'.}
    % any any simple contraction thereof.
    valid = {'linear', 'pchip', 'spline', 'csape'};
    [method,errstr] = validstring(method,valid);
    if ~isempty(errstr)
      error('INTERPARC:incorrectmethod',errstr)
    end
  end
  
  % anything that remains in varargin must add
  % an additional dimension on the curve/polygon
  for i = 1:numel(varargin)
    pz = varargin{i};
    pz = pz(:);
    if numel(pz) ~= n
      error('ARCLENGTH:improperpxorpy', ...
        'pz must be of the same size as px and py')
    end
    pxy = [pxy,pz]; %#ok
  end
  
  % the final number of dimensions provided
  ndim = size(pxy,2);
end

% if csape, then make sure the first point is replicated at the end.
% also test to see if csape is available
if method(1) == 'c'
  if exist('csape','file') == 0
    error('CSAPE was requested, but you lack the necessary toolbox.')
  end
  
  p1 = pxy(1,:);
  pend = pxy(end,:);
  
  % get a tolerance on whether the first point is replicated.
  if norm(p1 - pend) > 10*eps(norm(max(abs(pxy),[],1)))
    % the two end points were not identical, so wrap the curve
    pxy(end+1,:) = p1;
    nt = nt + 1;
  end
end

% preallocate the result, pt
pt = NaN(nt,ndim);

% Compute the chordal (linear) arclength
% of each segment. This will be needed for
% any of the methods.
chordlen = sqrt(sum(diff(pxy,[],1).^2,2));

% Normalize the arclengths to a unit total
chordlen = chordlen/sum(chordlen);

% cumulative arclength
cumarc = [0;cumsum(chordlen)];

% The linear interpolant is trivial. do it as a special case
if method(1) == 'l'
  % The linear method.
  
  % which interval did each point fall in, in
  % terms of t?
  [junk,tbins] = histc(t,cumarc); %#ok
  
  % catch any problems at the ends
  tbins((tbins <= 0) | (t <= 0)) = 1;
  tbins((tbins >= n) | (t >= 1)) = n - 1;
  
  % interpolate
  s = (t - cumarc(tbins))./chordlen(tbins);
  % be nice, and allow the code to work on older releases
  % that don't have bsxfun
  pt = pxy(tbins,:) + (pxy(tbins+1,:) - pxy(tbins,:)).*repmat(s,1,ndim);
  
  % do we need to compute derivatives here?
  if nargout > 1
    dudt = (pxy(tbins+1,:) - pxy(tbins,:))./repmat(chordlen(tbins),1,ndim);
  end
  
  % do we need to create the spline as a piecewise linear function?
  if nargout > 2
    spl = cell(1,ndim);
    for i = 1:ndim
      coefs = [diff(pxy(:,i))./diff(cumarc),pxy(1:(end-1),i)];
      spl{i} = mkpp(cumarc.',coefs);
    end
    
    %create a function handle for evaluation, passing in the splines
    fofthandle = @(t) foft(t,spl);
  end
  
  % we are done at this point
  return
end

% If we drop down to here, we have either a spline
% or csape or pchip interpolant to work with.

% compute parametric splines
spl = cell(1,ndim);
spld = spl;
diffarray = [3 0 0;0 2 0;0 0 1;0 0 0];
for i = 1:ndim
  switch method
    case 'pchip'
      spl{i} = pchip(cumarc,pxy(:,i));
    case 'spline'
      spl{i} = spline(cumarc,pxy(:,i));
      nc = numel(spl{i}.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl{i}.order = 4;
      end
    case 'csape'
      % csape was specified, so the curve is presumed closed,
      % therefore periodic
      spl{i} = csape(cumarc,pxy(:,i),'periodic');
      nc = numel(spl{i}.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl{i}.order = 4;
      end
  end
  
  % and now differentiate them
  xp = spl{i};
  xp.coefs = xp.coefs*diffarray;
  xp.order = 3;
  spld{i} = xp;
end

% catch the case where there were exactly three points
% in the curve, and spline was used to generate the
% interpolant. In this case, spline creates a curve with
% only one piece, not two.
if (numel(cumarc) == 3) && (method(1) == 's')
  cumarc = spl{1}.breaks;
  n = numel(cumarc);
  chordlen = sum(chordlen);
end

% Generate the total arclength along the curve
% by integrating each segment and summing the
% results. The integration scheme does its job
% using an ode solver.

% polyarray here contains the derivative polynomials
% for each spline in a given segment
polyarray = zeros(ndim,3);
seglen = zeros(n-1,1);

% options for ode45
opts = odeset('reltol',1.e-9);
for i = 1:spl{1}.pieces
  % extract polynomials for the derivatives
  for j = 1:ndim
    polyarray(j,:) = spld{j}.coefs(i,:);
  end
  
  % integrate the arclength for the i'th segment
  % using ode45 for the integral. I could have
  % done this part with quad too, but then it
  % would not have been perfectly (numerically)
  % consistent with the next operation in this tool.
  [tout,yout] = ode45(@(t,y) segkernel(t,y),[0,chordlen(i)],0,opts); %#ok
  seglen(i) = yout(end);
end

% and normalize the segments to have unit total length
totalsplinelength = sum(seglen);
cumseglen = [0;cumsum(seglen)];

% which interval did each point fall into, in
% terms of t, but relative to the cumulative
% arc lengths along the parametric spline?
[junk,tbins] = histc(t*totalsplinelength,cumseglen); %#ok

% catch any problems at the ends
tbins((tbins <= 0) | (t <= 0)) = 1;
tbins((tbins >= n) | (t >= 1)) = n - 1;

% Do the fractional integration within each segment
% for the interpolated points. t is the parameter
% used to define the splines. It is defined in terms
% of a linear chordal arclength. This works nicely when
% a linear piecewise interpolant was used. However,
% what is asked for is an arclength interpolation
% in terms of arclength of the spline itself. Call s
% the arclength traveled along the spline.
s = totalsplinelength*t;

% the ode45 options will now include an events property
% so we can catch zero crossings.
opts = odeset('reltol',1.e-9,'events',@ode_events);

ti = t;
for i = 1:nt
  % si is the piece of arc length that we will look
  % for in this spline segment.
  si = s(i) - cumseglen(tbins(i));
  
  % extract polynomials for the derivatives
  % in the interval the point lies in
  for j = 1:ndim
    polyarray(j,:) = spld{j}.coefs(tbins(i),:);
  end
  
  % we need to integrate in t, until the integral
  % crosses the specified value of si. Because we
  % have defined totalsplinelength, the lengths will
  % be normalized at this point to a unit length.
  %
  % Start the ode solver at -si, so we will just
  % look for an event where y crosses zero.
  [tout,yout,te,ye] = ode45(@(t,y) segkernel(t,y),[0,chordlen(tbins(i))],-si,opts); %#ok
  
  % we only need that point where a zero crossing occurred
  % if no crossing was found, then we can look at each end.
  if ~isempty(te)
    ti(i) = te(1) + cumarc(tbins(i));
  else
    % a crossing must have happened at the very
    % beginning or the end, and the ode solver
    % missed it, not trapping that event.
    if abs(yout(1)) < abs(yout(end))
      % the event must have been at the start.
      ti(i) = tout(1) + cumarc(tbins(i));
    else
      % the event must have been at the end.
      ti(i) = tout(end) + cumarc(tbins(i));
    end
  end
end

% Interpolate the parametric splines at ti to get
% our interpolated value.
for L = 1:ndim
  pt(:,L) = ppval(spl{L},ti);
end

% do we need to compute first derivatives here at each point?
if nargout > 1
  dudt = zeros(nt,ndim);
  for L = 1:ndim
    dudt(:,L) = ppval(spld{L},ti);
  end
end

% create a function handle for evaluation, passing in the splines
if nargout > 2
  fofthandle = @(t) foft(t,spl);
end

% ===============================================
%  nested function for the integration kernel
% ===============================================
  function val = segkernel(t,y) %#ok
    % sqrt((dx/dt)^2 + (dy/dt)^2 + ...)
    val = zeros(size(t));
    for k = 1:ndim
      val = val + polyval(polyarray(k,:),t).^2;
    end
    val = sqrt(val);
    
  end % function segkernel

% ===============================================
%  nested function for ode45 integration events
% ===============================================
  function [value,isterminal,direction] = ode_events(t,y) %#ok
    % ode event trap, looking for zero crossings of y.
    value = y;
    isterminal = ones(size(y));
    direction = ones(size(y));
  end % function ode_events

end % mainline - interparc


% ===============================================
%       end mainline - interparc
% ===============================================
%       begin subfunctions
% ===============================================

% ===============================================
%  subfunction for evaluation at any point externally
% ===============================================
function f_t = foft(t,spl)
% tool allowing the user to evaluate the interpolant at any given point for any values t in [0,1]
pdim = numel(spl);
f_t = zeros(numel(t),pdim);

% convert t to a column vector, clipping it to [0,1] as we do.
t = max(0,min(1,t(:)));

% just loop over the splines in the cell array of splines
for i = 1:pdim
  f_t(:,i) = ppval(spl{i},t);
end
end % function foft


function [str,errorclass] = validstring(arg,valid)
% validstring: compares a string against a set of valid options
% usage: [str,errorclass] = validstring(arg,valid)
%
% If a direct hit, or any unambiguous shortening is found, that
% string is returned. Capitalization is ignored.
%
% arguments: (input)
%  arg - character string, to be tested against a list
%        of valid choices. Capitalization is ignored.
%
%  valid - cellstring array of alternative choices
%
% Arguments: (output)
%  str - string - resulting choice resolved from the
%        list of valid arguments. If no unambiguous
%        choice can be resolved, then str will be empty.
%
%  errorclass - string - A string argument that explains
%        the error. It will be one of the following
%        possibilities:
%
%        ''  --> No error. An unambiguous match for arg
%                was found among the choices.
%
%        'No match found' --> No match was found among 
%                the choices provided in valid.
%
%        'Ambiguous argument' --> At least two ambiguous
%                matches were found among those provided
%                in valid.
%        
%
% Example:
%  valid = {'off' 'on' 'The sky is falling'}
%  
%
% See also: parse_pv_pairs, strmatch, strcmpi
%
% Author: John D'Errico
% e-mail: [e-mail]
% Release: 1.0
% Release date: 3/25/2010

ind = find(strncmpi(lower(arg),valid,numel(arg)));
if isempty(ind)
  % No hit found
  errorclass = 'No match found';
  str = '';
elseif (length(ind) > 1)
  % Ambiguous arg, hitting more than one of the valid options
  errorclass = 'Ambiguous argument';
  str = '';
  return
else
  errorclass = '';
  str = valid{ind};
end

end % function validstring

[John D'Errico]

EVERYBODY should have that tool. Well, they SHOULD. I don't make up the rules though. (Anyway, IF I did make up the rules, I'm damn well going to start with world peace as rule #1. And then I'd probably be too tired to worry about rule #2 anyway.)

Are you asking to find the closest point on curve 2, from any given point on curve 1? Or are you asking to find, give a normal vector from curve 1, the intersection of that line with curve 2?

It almost sounds like you expect a normal vector from curve 1 to also just happen to be a vector that is normal to curve 2. That won't in general happen. The only time when it would be is for pairs of points that are at a locally minimal distance between the two curves. (Excluding the end points of the curves, if the curves are not closed.)

The point being, given a closed curve 1 floating in the plane, you cannot just arbitrarily pick 20 points on that curve, and then expect the normal to that curve to then intersect some other curve at a point that is ALSO normal to curve 2.

[Tim Schaller]

I need a matrix of 20 points on curve 2, that are cloesest to the 20 points on curve 1.

I think in this context the closest points also mean to be orthogonal to the curve 1.

You don't think, that it is possible to solve this problem?

[Bruno Luong]

"I need a matrix of 20 points on curve 2, that are cloesest to the 20 points on curve 1.

I think in this context the closest points also mean to be orthogonal to the curve 1"

If I read you correctly this is wrong.

The line that link 2 points that are the closest points (P) on a curve C2 from a given point (Q, on C1) is orthogonal to C2.

So in your case not necessary orthogonal to C1.

The way you describe the problem is so ambigeous that I don't know what you try to do.

Answer 1

[John D'Errico]

I did NOT say it was impossible to solve. I did say that what you are asking does NOT mean the closest poiints on curve 2 will happen to lie on lines that are orthogonal to BOTH curves. That is provably wrong, in fact.

What you are asking is in fact pretty easy, since I've already posted a tool that does exactly what you asked on the file exchange. (That is not to say the solution was trivial. Not difficult though.) The function distance2curve does exaclty what you are asking to do. That is, given a curve C, and a set of points in space, find the closest point on C to each point in the list. As long as that closest point does not lie at an end point of the curve, it will be on a line that is orthogonal to the curve. So if the curve is a smooth and closed curve, the minimal distance point will always lie on a normal vector.

Find distance2curve here:

https://www.mathworks.com/matlabcentral/fileexchange/34869-distance2curve?s_tid=srchtitle

However...

As you will see using the curve on a test example, suppose you have two curves. I'll call them C1 and C2. Now, choose an arbitrary set of points on C1. Find the closest point on C2 to each of them. Next, start with that same set of points on C2, and find the closest points on C1. You will NOT in general recover the first set of points for two completely general curves! (It is easy to give a counter-example here that shows my claim to be true. Think about it!)

Again, points of MUTUAL orthogonality on both curves will only lie at points that are at a locally minimal distance between the two curves. There will only be a finite set of such points, and you cannot choose them arbitrarily, unless the curves are quite simple. For example, two parallel lines or two concentric circles would be the simple cases where you have an infinite number of points that have closest points that also lie always on joint normal lines. But those a special cases, and fairly rare.

Anyway, distance2curve solves your problem as asked. I wrote it not long after I wrote interparc. I did consider writing a code that would compute the points of minimal overall distance between two space curves (it would be a little similar in nature), but then I got busy trying to solve world peace, and that just wore me out.

[Torsten]

As you will see using the curve on a test example, suppose you have two curves. I'll call them C1 and C2. Now, choose an arbitrary set of points on C1. Find the closest point on C2 to each of them. Next, start with that same set of points on C2, and find the closest points on C1. You will NOT in general recover the first set of points for two completely general curves! (It is easy to give a counter-example here that shows my claim to be true. Think about it!)

You mean there might be points on C1 that have the same distance to the points on C2 as the original points on C1 ? Another case does not come to my mind.

[Bruno Luong]

@Torsten simple example

Take

C1 = { (x,y): x <= 1 and y = 1 } U { (x,y): y <= 1 and x = 1 }

C2 = { (x,y): x <= 0 and y = 0 } U { (x,y): y <= 0 and x = 0 }

Select P1 := (x,y) = (1,1) on C1

The closest point on C2 to P1 is P2 = (0;0)

The closest point on C1 to P2 is P3 = (1,0) or P4=(0,1), both are different than P1.

You can smooth the corners a little bit; but this still happens in such similar configuration and it is not rare.

[Torsten]

Thank you, Bruno.

[John D'Errico]

I was just coming back in to add an example, since I knew someone would question my claim. Thanks Bruno.

[Tim Schaller]

I have one more Problem.

First I thought it perfectly worked, but I chose to get a third curce und try to do it.

Basically now the points of any point should be in an straight line, but if you watch clocely that does not happen. Any ideas what I have been doing wrong?

n = 3;
n1 = n-1;
a = 20;             
b = 10;            
P = [0 b;0 0;a 0];
x = P(:,1);        
y = P(:,2);        
T = 20;            
H = 5;
R = 1;

t = linspace(0,1);
B = bernsteinMatrix(n1,t);
bezierCurve = B*P;
px = bezierCurve(:,1);
py = bezierCurve(:,2);
pt = interparc(T,px,py,'spline');  
plot(px,py,pt(:,1),pt(:,2),'b-o',"MarkerSize",4)
axis equal
axis tight
grid on
hold on

syms t
B = bernsteinMatrix(n1,t);
bezierCurve = B*P;
normalToCurve = diff(bezierCurve)*[0 1;-1 0];
normalToCurve = normalToCurve/norm(normalToCurve);
newNormalToCurve = [double(subs(normalToCurve(1),t,linspace(0,1)))' double(subs(normalToCurve(2),t,linspace(0,1)))'];
f = linspace(0,1);
B = bernsteinMatrix(n1,f);
bezierCurve = B*P;
px = bezierCurve(:,1);
py = bezierCurve(:,2);
%%%%%%%%%% Variable s entlang der Mittellinie definiert
assume(t>=0 & t<=1)
B = bernsteinMatrix(n1,t);
bezierCurve = simplify(B*P);  
s(t)=int(norm(diff(bezierCurve)),0,t);
%%%%%%%%
%Obere Linie
for i = 1:100
    S = double(s((i-1)/99));
    d = R*(1-S/double(s(1)))+(H/2)*S/double(s(1));
    delta(i) = d;
end
delta = delta';
pxnew1 = px+newNormalToCurve(:,1).*delta;
pynew1 = py+newNormalToCurve(:,2).*delta;
plot(pxnew1, pynew1,'r') 
d2c = distance2curve([pxnew1 pynew1],pt,'linear');
plot(d2c(:,1),d2c(:,2),"r-o","MarkerSize",4)

%Untere Linie 
for i = 1:100
    S = double(s((i-1)/99));
    d = R*(1-S/double(s(1)))+(H/2)*S/double(s(1));
    delta(i) = - d;
end
pxnew2 = px+newNormalToCurve(:,1).*delta;
pynew2 = py+newNormalToCurve(:,2).*delta;
plot(pxnew2, pynew2,'r') 
d2c2 = distance2curve([pxnew2 pynew2],pt,'linear');
plot(d2c2(:,1),d2c2(:,2),"r-o","MarkerSize",4)
hold off

function [pt,dudt,fofthandle] = interparc(t,px,py,varargin)
% unpack the arguments and check for errors
if nargin < 3
  error('ARCLENGTH:insufficientarguments', ...
    'at least t, px, and py must be supplied')
end
t = t(:);
if (numel(t) == 1) && (t > 1) && (rem(t,1) == 0)
  % t specifies the number of points to be generated
  % equally spaced in arclength
  t = linspace(0,1,t)';
elseif any(t < 0) || any(t > 1)
  error('ARCLENGTH:impropert', ...
    'All elements of t must be 0 <= t <= 1')
end
% how many points will be interpolated?
nt = numel(t);
% the number of points on the curve itself
px = px(:);
py = py(:);
n = numel(px);
% are px and py both vectors of the same length?
if ~isvector(px) || ~isvector(py) || (length(py) ~= n)
  error('ARCLENGTH:improperpxorpy', ...
    'px and py must be vectors of the same length')
elseif n < 2
  error('ARCLENGTH:improperpxorpy', ...
    'px and py must be vectors of length at least 2')
end
% compose px and py into a single array. this way,
% if more dimensions are provided, the extension
% is trivial.
pxy = [px,py];
ndim = 2;
% the default method is 'linear'
method = 'spline';
% are there any other arguments?
if nargin > 3
  % there are. check the last argument. Is it a string?
  if ischar(varargin{end})
    method = varargin{end};
    varargin(end) = [];
    
    % method may be any of {'linear', 'pchip', 'spline', 'csape'.}
    % any any simple contraction thereof.
    valid = {'linear', 'pchip', 'spline', 'csape'};
    [method,errstr] = validstring(method,valid);
    if ~isempty(errstr)
      error('INTERPARC:incorrectmethod',errstr)
    end
  end
  
  % anything that remains in varargin must add
  % an additional dimension on the curve/polygon
  for i = 1:numel(varargin)
    pz = varargin{i};
    pz = pz(:);
    if numel(pz) ~= n
      error('ARCLENGTH:improperpxorpy', ...
        'pz must be of the same size as px and py')
    end
    pxy = [pxy,pz]; %#ok
  end
  
  % the final number of dimensions provided
  ndim = size(pxy,2);
end
% if csape, then make sure the first point is replicated at the end.
% also test to see if csape is available
if method(1) == 'c'
  if exist('csape','file') == 0
    error('CSAPE was requested, but you lack the necessary toolbox.')
  end
  
  p1 = pxy(1,:);
  pend = pxy(end,:);
  
  % get a tolerance on whether the first point is replicated.
  if norm(p1 - pend) > 10*eps(norm(max(abs(pxy),[],1)))
    % the two end points were not identical, so wrap the curve
    pxy(end+1,:) = p1;
    nt = nt + 1;
  end
end
% preallocate the result, pt
pt = NaN(nt,ndim);
% Compute the chordal (linear) arclength
% of each segment. This will be needed for
% any of the methods.
chordlen = sqrt(sum(diff(pxy,[],1).^2,2));
% Normalize the arclengths to a unit total
chordlen = chordlen/sum(chordlen);
% cumulative arclength
cumarc = [0;cumsum(chordlen)];
% The linear interpolant is trivial. do it as a special case
if method(1) == 'l'
  % The linear method.
  
  % which interval did each point fall in, in
  % terms of t?
  [junk,tbins] = histc(t,cumarc); %#ok
  
  % catch any problems at the ends
  tbins((tbins <= 0) | (t <= 0)) = 1;
  tbins((tbins >= n) | (t >= 1)) = n - 1;
  
  % interpolate
  s = (t - cumarc(tbins))./chordlen(tbins);
  % be nice, and allow the code to work on older releases
  % that don't have bsxfun
  pt = pxy(tbins,:) + (pxy(tbins+1,:) - pxy(tbins,:)).*repmat(s,1,ndim);
  
  % do we need to compute derivatives here?
  if nargout > 1
    dudt = (pxy(tbins+1,:) - pxy(tbins,:))./repmat(chordlen(tbins),1,ndim);
  end
  
  % do we need to create the spline as a piecewise linear function?
  if nargout > 2
    spl = cell(1,ndim);
    for i = 1:ndim
      coefs = [diff(pxy(:,i))./diff(cumarc),pxy(1:(end-1),i)];
      spl{i} = mkpp(cumarc.',coefs);
    end
    
    %create a function handle for evaluation, passing in the splines
    fofthandle = @(t) foft(t,spl);
  end
  
  % we are done at this point
  return
end
% compute parametric splines
spl = cell(1,ndim);
spld = spl;
diffarray = [3 0 0;0 2 0;0 0 1;0 0 0];
for i = 1:ndim
  switch method
    case 'pchip'
      spl{i} = pchip(cumarc,pxy(:,i));
    case 'spline'
      spl{i} = spline(cumarc,pxy(:,i));
      nc = numel(spl{i}.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl{i}.order = 4;
      end
    case 'csape'
      % csape was specified, so the curve is presumed closed,
      % therefore periodic
      spl{i} = csape(cumarc,pxy(:,i),'periodic');
      nc = numel(spl{i}.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl{i}.order = 4;
      end
  end
  
  % and now differentiate them
  xp = spl{i};
  xp.coefs = xp.coefs*diffarray;
  xp.order = 3;
  spld{i} = xp;
end
if (numel(cumarc) == 3) && (method(1) == 's')
  cumarc = spl{1}.breaks;
  n = numel(cumarc);
  chordlen = sum(chordlen);
end
polyarray = zeros(ndim,3);
seglen = zeros(n-1,1);
% options for ode45
opts = odeset('reltol',1.e-9);
for i = 1:spl{1}.pieces
  % extract polynomials for the derivatives
  for j = 1:ndim
    polyarray(j,:) = spld{j}.coefs(i,:);
  end
  
  [tout,yout] = ode45(@(t,y) segkernel(t,y),[0,chordlen(i)],0,opts); %#ok
  seglen(i) = yout(end);
end
% and normalize the segments to have unit total length
totalsplinelength = sum(seglen);
cumseglen = [0;cumsum(seglen)];
% which interval did each point fall into, in
% terms of t, but relative to the cumulative
% arc lengths along the parametric spline?
[junk,tbins] = histc(t*totalsplinelength,cumseglen); %#ok
% catch any problems at the ends
tbins((tbins <= 0) | (t <= 0)) = 1;
tbins((tbins >= n) | (t >= 1)) = n - 1;
s = totalsplinelength*t;
opts = odeset('reltol',1.e-9,'events',@ode_events);
ti = t;
for i = 1:nt
  % si is the piece of arc length that we will look
  % for in this spline segment.
  si = s(i) - cumseglen(tbins(i));
  
  % extract polynomials for the derivatives
  % in the interval the point lies in
  for j = 1:ndim
    polyarray(j,:) = spld{j}.coefs(tbins(i),:);
  end
  
  [tout,yout,te,ye] = ode45(@(t,y) segkernel(t,y),[0,chordlen(tbins(i))],-si,opts); %#ok
  
  % we only need that point where a zero crossing occurred
  % if no crossing was found, then we can look at each end.
  if ~isempty(te)
    ti(i) = te(1) + cumarc(tbins(i));
  else
    if abs(yout(1)) < abs(yout(end))
      % the event must have been at the start.
      ti(i) = tout(1) + cumarc(tbins(i));
    else
      % the event must have been at the end.
      ti(i) = tout(end) + cumarc(tbins(i));
    end
  end
end
% Interpolate the parametric splines at ti to get
% our interpolated value.
for L = 1:ndim
  pt(:,L) = ppval(spl{L},ti);
end
% do we need to compute first derivatives here at each point?
if nargout > 1
  dudt = zeros(nt,ndim);
  for L = 1:ndim
    dudt(:,L) = ppval(spld{L},ti);
  end
end
% create a function handle for evaluation, passing in the splines
if nargout > 2
  fofthandle = @(t) foft(t,spl);
end
% ===============================================
%  nested function for the integration kernel
% ===============================================
  function val = segkernel(t,y) %#ok
    % sqrt((dx/dt)^2 + (dy/dt)^2 + ...)
    val = zeros(size(t));
    for k = 1:ndim
      val = val + polyval(polyarray(k,:),t).^2;
    end
    val = sqrt(val);
    
  end % function segkernel
% ===============================================
%  nested function for ode45 integration events
% ===============================================
  function [value,isterminal,direction] = ode_events(t,y) %#ok
    % ode event trap, looking for zero crossings of y.
    value = y;
    isterminal = ones(size(y));
    direction = ones(size(y));
  end % function ode_events
end % mainline - interparc
function f_t = foft(t,spl)
% tool allowing the user to evaluate the interpolant at any given point for any values t in [0,1]
pdim = numel(spl);
f_t = zeros(numel(t),pdim);
% convert t to a column vector, clipping it to [0,1] as we do.
t = max(0,min(1,t(:)));
% just loop over the splines in the cell array of splines
for i = 1:pdim
  f_t(:,i) = ppval(spl{i},t);
end
end % function foft
function [str,errorclass] = validstring(arg,valid)
ind = find(strncmpi(lower(arg),valid,numel(arg)));
if isempty(ind)
  % No hit found
  errorclass = 'No match found';
  str = '';
elseif (length(ind) > 1)
  % Ambiguous arg, hitting more than one of the valid options
  errorclass = 'Ambiguous argument';
  str = '';
  return
else
  errorclass = '';
  str = valid{ind};
end
end % function validstring
function [xy,distance,t_a] = distance2curve(curvexy,mapxy,interpmethod)
% check for errors, defaults, etc...
if (nargin < 2)
  error('DISTANCE2CURVE:insufficientarguments', ...
    'at least curvexy and mapxy must be supplied')
elseif nargin > 3
  error('DISTANCE2CURVE:abundantarguments', ...
    'Too many arguments were supplied')
end
% get the dimension of the space our points live in
[n,p] = size(curvexy);
if isempty(curvexy) || isempty(mapxy)
  % empty begets empty. you might say this was a pointless exercise.
  xy = zeros(0,p);
  distance = zeros(0,p);
  t_a = zeros(0,p);
  return
end
% do curvexy and mapxy live in the same space?
if size(mapxy,2) ~= p
  error('DISTANCE2CURVE:improperpxorpy', ...
    'curvexy and mapxy do not appear to live in the same dimension spaces')
end
% do the points live in at least 2 dimensions?
if p < 2
  error('DISTANCE2CURVE:improperpxorpy', ...
    'The points MUST live in at least 2 dimensions for any curve to be defined.')
end
% how many points to be mapped to the curve?
m = size(mapxy,1);
% make sure that curvexy and mapxy are doubles, as uint8, etc
% would cause problems down the line.
curvexy = double(curvexy);
mapxy = double(mapxy);
% test for complex inputs
if ~isreal(curvexy) || ~isreal(mapxy)
  error('DISTANCE2CURVE:complexinputs','curvexy and mapxy may not be complex')
end
% default for interpmethod
if (nargin < 3) || isempty(interpmethod)
  interpmethod = 'linear';
elseif ~ischar(interpmethod)
  error('DISTANCE2CURVE:invalidinterpmethod', ...
    'Invalid method indicated. Only ''linear'',''pchip'',''spline'' allowed')
else
  validmethods = {'linear' 'pchip' 'spline'};
  ind = strmatch(lower(interpmethod),validmethods);
  if isempty(ind) || (length(ind) > 1)
    error('DISTANCE2CURVE:invalidinterpmethod', ...
      'Invalid method indicated. Only ''linear'',''pchip'',''spline'' allowed')
  end
  interpmethod = validmethods{ind};
end
% if the curve is a single point, stop here
if n == 1
  % return the appropriate parameters
  xy = repmat(curvexy,m,1);
  t_a = zeros(m,1);
  % 2 norm distance, or sqrt of sum of squares of differences
  distance = sqrt(sum(bsxfun(@minus,curvexy,mapxy).^2,2));
  % we can drop out here
  return
end
% compute the chordal linear arclengths, and scale to [0,1].
seglen = sqrt(sum(diff(curvexy,[],1).^2,2));
t0 = [0;cumsum(seglen)/sum(seglen)];
% We need to build some parametric splines.
% compute the splines, storing the polynomials in one 3-d array
ppsegs = cell(1,p);
% the breaks for the splines will be t0, unless spline got fancy
% on us here.
breaks = t0;
for i = 1:p
  switch interpmethod
    case 'linear'
      dt = diff(t0);
      ind = 1:(n-1);
      ppsegs{i} = [(curvexy(ind + 1,i) - curvexy(ind,i))./dt,curvexy(ind,i)];
    case 'pchip'
      spl = pchip(t0,curvexy(:,i));
      ppsegs{i} = spl.coefs;
    case 'spline'
      spl = spline(t0,curvexy(:,i));
      breaks = spl.breaks';
      nc = numel(spl.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl.order = 4;
      end
      ppsegs{i} = spl.coefs;
  end
end
nbr = numel(breaks);
% for each point in mapxy, find the closest point to those
% in curvexy. This part we can do in a vectorized form.
pointdistances = ipdm(mapxy,curvexy,'metric',2, ...
  'result','structure','subset','nearestneighbor');
% initialize the return variables, using the closest point
% found in the set curvexy.
xy = curvexy(pointdistances.columnindex,:);
distance = pointdistances.distance;
t = t0(pointdistances.columnindex);
% we must now do at least some looping, still vectorized where possible.
% the piecewise linear case is simpler though, so do it separately.
if strcmp(interpmethod,'linear');
  % loop over the individual points, vectorizing in the number of
  % segments, when there are many segments, but not many points to map.
  if n >= (5*m)
    % many segments, so loop over the points in mapxy
    for i = 1:m
      % the i'th point in mapxy
      xyi = mapxy(i,:);
      % Compute the location (in t) of the minimal distance
      % point to xyi, for all lines.
      tnum = zeros(nbr - 1,1);
      tden = tnum;
      for j = 1:p
        ppj = ppsegs{j};
        tden = tden + ppj(:,1).^2;
        tnum = tnum + ppj(:,1).*(xyi(j) - ppj(:,2));
      end
      tmin = tnum./tden;
      % toss out any element of tmin that is less than or equal to
      % zero, or or is greater than dt for that segment.
      tmin((tmin <= 0) | (tmin >= diff(t0))) = NaN;
      % for any segments with a valid minimum distance inside the
      % segment itself, compute that distance.
      dmin = zeros(nbr - 1,1);
      for j = 1:p
        ppi = ppsegs{j};
        dmin = dmin + (ppi(:,1).*tmin + ppi(:,2) - xyi(j)).^2;
      end
      dmin = sqrt(dmin);
      % what is the minimum distance among these segments?
      [mindist,minind] = min(dmin);
      if ~isnan(mindist) && (distance(i) > mindist)
        % there is a best segment, better than the
        % closest point from curvexy.
        distance(i) = mindist;
        t(i) = tmin(minind) + t0(minind);
        for j = 1:p
          ppj = ppsegs{j};
          xy(i,j) = ppj(minind,1).*tmin(minind) + ppj(minind,2);
        end
      end
    end
  else
    for i = 1:(n-1)
      % the i'th segment of the curve
      t1 = t0(i);
      t2 = t0(i+1);
      % Compute the location (in t) of the minimal distance
      % point to mapxy, for all points.
      tnum = zeros(m,1);
      tden = 0;
      for j = 1:p
        ppj = ppsegs{j};
        tden = tden + ppj(i,1).^2;
        tnum = tnum + ppj(i,1).*(mapxy(:,j) - ppj(i,2));
      end
      tmin = tnum./tden;
      % We only care about those points for this segment where there
      % is a minimal distance to the segment that is internal to the
      % segment.
      k = find((tmin > 0) & (tmin < (t2-t1)));
      nk = numel(k);
      if nk > 0
        % for any points with a valid minimum distance inside the
        % segment itself, compute that distance.
        dmin = zeros(nk,1);
        xymin = zeros(nk,p);
        for j = 1:p
          ppj = ppsegs{j};
          xymin(:,j) = ppj(i,1).*tmin(k) + ppj(i,2);
          dmin = dmin + (xymin(:,j) - mapxy(k,j)).^2;
        end
        dmin = sqrt(dmin);
        L = dmin < distance(k);
        % this segment has a closer point
        % closest point from curvexy.
        if any(L)
          distance(k(L)) = dmin(L);
          t(k(L)) = tmin(k(L)) + t0(i);
          xy(k(L),:) = xymin(L,:);
        end
      end
    end
  end
  % for the linear case, t is identical to the fractional arc length
  % along the curve.
  t_a = t;
else
  % cubic segments. here it is simplest to loop over the
  % distinct curve segments. We need not test the endpoints
  % of the segments, since the call to ipdm did that part.
  xytrans = zeros(1,p);
  polydiff = @(dp) dp(1:6).*[6 5 4 3 2 1];
  for j = 1:(n-1)
    % the j'th curve segment
    t1 = t0(j);
    t2 = t0(j+1);
    
    distpoly0 = zeros(1,7);
    for i = 1:p
      ppi = ppsegs{i};
      % this will allow us to translate each poly to pass through
      % (0,0) (i.e., at t = 0)
      xytrans(i) = ppi(j,4);
      distpoly0(1:2) = distpoly0(1:2) + ppi(j,1).*[ppi(j,1),2*ppi(j,2)];
      distpoly0(3) = distpoly0(3) + 2.*ppi(j,1).*ppi(j,3) + ppi(j,2).^2;
      distpoly0(4) = distpoly0(4) + 2.*ppi(j,2).*ppi(j,3);
      distpoly0(5) = distpoly0(5) + ppi(j,3).^2;
    end
    for i = 1:m
    
      xyi = mapxy(i,:) - xytrans;
      % update the poly for this particular point
      % (-2*a1*x0)*t^3
      % (-2*a2*x0)*t^2
      % (-2*a3*x0)*t
      % x0^2
      distpoly = distpoly0;
      for k = 1:p
        ppk = ppsegs{k};
        distpoly(4:6) = distpoly(4:6) - 2.*ppk(j,1:3).*xyi(k);
        distpoly(7) = distpoly(7) + xyi(k).^2;
      end
     
      diffpoly = polydiff(distpoly);
      tstationary = roots(diffpoly);
      % discard any with an imaginary part, those that are less
      % than 0, or greater than t2-t1.
      k = (imag(tstationary) ~= 0) | ...
        (real(tstationary) <= 0) | ...
        (real(tstationary) >= (t2 - t1));
      tstationary(k) = [];
      % for any solutions that remain, compute the distance.
      if ~isempty(tstationary)
        mindist = zeros(size(tstationary));
        xyij = zeros(numel(tstationary),p);
        for k = 1:p
          xyij(:,k) = polyval(ppsegs{k}(j,:),tstationary);
          mindist = mindist + (mapxy(i,k) - xyij(:,k)).^2;
        end
        mindist = sqrt(mindist);
        % just in case there is more than one stationary point
        [mindist,ind] = min(mindist);
        if mindist < distance(i)
          % we found a point on this segment that is better
          % than the endpoint values for that segment.
          distance(i) = mindist;
          xy(i,:) = xyij(ind,:);
          t(i) = tstationary(ind) + t0(j);
        end
      end % if ~isempty(tstationary)
    end % for i = 1:n
  end % for j = 1:(n-1)
  
  if nargout >= 2
    % build new piecewise polynomials for each segment that
    % represent (dx/dt)^2 + (dy/dt)^2 + ...
    %
    % Since each poly must be cubic at this point, the result will be
    % a 4th degree piecewise polynomial.
    kernelcoefs = zeros(nbr-1,5);
    for i = 1:p
      ppi = ppsegs{i};
      kernelcoefs = kernelcoefs + [9*ppi(:,1).^2, ...
        12*ppi(:,1).*ppi(:,2), ...
        4*ppi(:,2).^2 + 6*ppi(:,1).*ppi(:,3), ...
        4*ppi(:,2).*ppi(:,3), ppi(:,3).^2];
    end
    % get the arc length for each segment. quadgk will suffice here
    % since we need to integrate the sqrt of each poly
    arclengths = zeros(nbr-1,1);
    for i = 1:(nbr - 1)
      lengthfun = @(t) sqrt(polyval(kernelcoefs(i,:),t));
      arclengths(i) = quadgk(lengthfun,0,t0(i+1) - t0(i));
    end
    % get the cumulative arclengths, then scale by the sum
    % this gives us fractional arc lengths.
    arclengths = cumsum(arclengths);
    totallength = arclengths(end);
    arclengths = [0;arclengths/totallength];
    % where does each point fall in terms of fractional cumulative
    % chordal arclength? (i.e., t0?)
    [tbin,tbin] = histc(t,t0);
    tbin(tbin < 1) = 1; % being careful at the bottom end
    tbin(tbin >= nbr) = nbr - 1; % if the point fell at the very top...
    % the total length below the segment in question
    t_a = arclengths(tbin);
    % now get the piece in the tbin segment
    for i = 1:m
      lengthfun = @(t) sqrt(polyval(kernelcoefs(tbin(i),:),t));
      t_a(i) = t_a(i) + quadgk(lengthfun,0,t(i) - t0(tbin(i)))/totallength;
    end
  end
end % if strcmp(interpmethod,'linear');
end
% ==========================================================
function d = ipdm(data1,varargin)
% Default property values
params.Metric = 2;
params.Result = 'array';
params.Subset = 'all';
params.Limit = [];
params.ChunkSize = 2^25;
% untangle the arguments
if nargin<1
  % if called with no arguments, then the user probably
  % needs help. Give it to them.
  help ipdm
  return
end
% were two sets of data provided?
pvpairs = {};
if nargin==1
  % only 1 set of data provided
  dataflag = 1;
  data2 = [];
else
  if ischar(varargin{1})
    dataflag = 1;
    data2 = [];
    pvpairs = varargin;
  else
    dataflag = 2;
    data2 = varargin{1};
    if nargin>2
      pvpairs = varargin(2:end);
    end
  end
end
% get data sizes for later
[n1,dim] = size(data1);
if dataflag == 2
  n2 = size(data2,1);
end
% Test the class of the input variables
if ~(isa(data1,'double') || isa(data1,'single')) || ...
    ((dataflag == 2) && ~(isa(data2,'double') || isa(data2,'single')))
  error('data points must be either single or double precision variables.')
end
% do we need to process any property/value pairs?
if nargin>2
  params = parse_pv_pairs(params,pvpairs);
  % check for problems in the properties
  % was a legal Subset provided?
  if ~isempty(params.Subset) && ~ischar(params.Subset)
    error('If provided, ''Subset'' must be character')
  elseif isempty(params.Subset)
    params.Subset = 'all';
  end
  valid = {'all','maximum','minimum','largestfew','smallestfew', ...
    'nearestneighbor','farthestneighbor'};
  ind = find(strncmpi(params.Subset,valid,length(params.Subset)));
  if (length(ind)==1)
    params.Subset = valid{ind};
  else
    error(['Invalid Subset: ',params.Subset])
  end
  % was a limit provided?
  if ~ismember(params.Subset,{'all','nearestneighbor','farthestneighbor'}) && ...
      isempty(params.Limit)
    error('No limit provided, but a Subset that requires a limit value was specified')
  end
  % check the limit values for validity
  if length(params.Limit)>1
    error('Limit must be scalar or empty')
  end
  switch params.Subset
    case {'largestfew', 'smallestfew'}
      % must be at least 1, and an integer
      if (params.Limit<1) || (round(params.Limit)~=params.Limit)
        error('Limit must be a positive integer for LargestFew or NearestFew')
      end
  end
  % was a legal Result provided?
  if isempty(params.Result)
    params.result = 'Array';
  elseif ~ischar(params.Result)
    error('If provided, ''Result'' must be character or empty')
  end
  valid = {'array','structure'};
  ind = find(strncmpi(params.Result,valid,length(params.Result)));
  if (length(ind)==1)
    params.Result = valid{ind};
  else
    error(['Invalid Result: ',params.Subset])
  end
  % check for the metric
  if isempty(params.Metric)
    params.Metric = 2;
  elseif (length(params.Metric)~=1) || ~ismember(params.Metric,[0 1 2 inf])
    error('If supplied, ''Metric'' must be a scalar, and one of [0 1 2 inf]')
  end
end % if nargin>2
if (dim == 1) && (params.Metric == 2)
  params.Metric = 1;
end
params.usebsxfun = (5==exist('bsxfun','builtin'));
% check for dimension mismatch if 2 sets
if (dataflag==2) && (size(data2,2)~=dim)
  error('If 2 point sets provided, then both must have the same number of columns')
end
% Total number of distances to compute, in case I must do it in batches
if dataflag==1
  n2 = n1;
end
ntotal = n1*n2;
% FINALLY!!! Compute inter-point distances
switch params.Subset
  case 'all'
    % One set or two?
    if dataflag == 1
      d = distcomp(data1,data1,params);
    else
        d = distcomp(data1,data2,params);
    end
    % Must we return it as a struct?
    if params.Result(1) == 's'
      [rind,cind] = ndgrid(1:size(d,1),1:size(d,2));
      ds.rowindex = rind(:);
      ds.columnindex = cind(:);
      ds.distance = d(:);
      d = ds;
    end
  case {'minimum' 'maximum'}
   
    if ((ntotal*8)<=params.ChunkSize) || (params.Result(1) == 'a')
      % its small enough to do it all at once
      % One set or two?
      if dataflag == 1
        d = distcomp(data1,data1,params);
      else
          d = distcomp(data1,data2,params);
      end
      % Must we return it as a struct?
      if params.Result(1) == 'a'
        % its an array, fill the unwanted distances with +/- inf
        if params.Subset(2) == 'i'
          % minimum
          d(d<=params.Limit) = -inf;
        else
          % maximum
          d(d>=params.Limit) = +inf;
        end
      else
        % a struct will be returned
        if params.Subset(2) == 'i'
          % minimum
          [dist.rowindex,dist.columnindex] = find(d>=params.Limit);
        else
          % maximum
          [dist.rowindex,dist.columnindex] = find(d<=params.Limit);
        end
        dist.distance = d(dist.rowindex + n1*(dist.columnindex-1));
        d = dist;
      end
    else
      % we need to break this into chunks. This branch
      % will always return a struct.
      % this is the number of rows of data1 that we will
      % process at a time.
      bs = floor(params.ChunkSize/(8*n2));
      bs = min(n1,max(1,bs));
      accum = cell(0,1);
      % now loop over the chunks
      batch = 1:bs;
      while ~isempty(batch)
        % One set or two?
        if dataflag == 1
          dist = distcomp(data1(batch,:),data1,params);
        else
          dist = distcomp(data1(batch,:),data2,params);
        end
        % big or small as requested
        if ('i'==params.Subset(2))
          % minimum value specified
          [I,J,V] = find(dist>=params.Limit);
        else
          % maximum limit
          [I,J] = find(dist<=params.Limit);
          I = I(:);
          J = J(:);
          V = dist(I + (J-1)*length(batch));
          I = I + (batch(1)-1);
        end
        % and stuff them into the cell structure
        if ~isempty(V)
          accum{end+1,1} = [I,J,V(:)]; %#ok
        end
        % increment the batch
        batch = batch + bs;
        if batch(end)>n1
          batch(batch>n1) = [];
        end
      end
      % convert the cells into one flat array
      accum = cell2mat(accum);
      if isempty(accum)
        d.rowindex = [];
        d.columnindex = [];
        d.distance = [];
      else
        % we found something
        % sort on the second column, to put them in a reasonable order
        accum = sortrows(accum,[2 1]);
        d.rowindex = accum(:,1);
        d.columnindex = accum(:,2);
        d.distance = accum(:,3);
      end
    end
  case {'smallestfew' 'largestfew'}
    % find the k smallest/largest distances. k is
    % given by params.Limit
    % if only 1 set, params.Limit must be less than n*(n-1)/2
    if dataflag == 1
      params.Limit = min(params.Limit,n1*(n1-1)/2);
    end
    % is this a large problem?
    if ((ntotal*8) <= params.ChunkSize)
      % small potatoes
      % One set or two?
      if dataflag == 1
        dist = distcomp(data1,data1,params);
        % if only one data set, set the diagonal and
        % below that to +/- inf so we don't find it.
        temp = find(tril(ones(n1,n1),0));
        if params.Subset(1) == 's'
          dist(temp) = inf;
        else
          dist(temp) = -inf;
        end
      else
        dist = distcomp(data1,data2,params);
      end
      % sort the distances to find those we need
      if ('s'==params.Subset(1))
        % smallestfew
        [val,tags] = sort(dist(:),'ascend');
      else
        % largestfew
        [val,tags] = sort(dist(:),'descend');
      end
      val = val(1:params.Limit);
      tags = tags(1:params.Limit);
      % recover the row and column index from the linear
      % index returned by sort in tags.
      [d.rowindex,d.columnindex] = ind2sub([n1,size(dist,2)],tags);
      % create the matrix as a sparse one or a struct?
      if params.Result(1)=='a'
        % its an array, so make the array sparse.
        d = sparse(d.rowindex,d.columnindex,val,n1,size(dist,2));
      else
        % a structure
        d.distance = val;
      end
    else
      % chunks
      % this is the number of rows of data1 that we will
      % process at a time.
      bs = floor(params.ChunkSize/(8*n2));
      bs = min(n1,max(1,bs));
      
      if (8*params.Limit*n1/bs) <= params.ChunkSize
        
        accum = cell(0,1);
        % now loop over the chunks
        batch = (1:bs)';
        while ~isempty(batch)
          % One set or two?
          if dataflag == 1
            dist = distcomp(data1(batch,:),data1,params);
            k = find(tril(ones(length(batch),n2),batch(1)-1));
            if ('s'==params.Subset(1))
              dist(k) = inf;
            else
              dist(k) = -inf;
            end
          else
            dist = distcomp(data1(batch,:),data2,params);
          end
          % big or small as requested, keeping only the best
          % params.Limit number of elements
          if ('s'==params.Subset(1))
            % minimum value specified
            [tags,tags] = sort(dist(:),1,'ascend'); %#ok
            tags = tags(1:bs);
            [I,J] = ndgrid(batch,1:n2);
            ijv = [I(tags),J(tags),dist(tags)];
          else
            % maximum limit
            [tags,tags] = sort(dist(:),1,'descend'); %#ok
            tags = tags(1:bs);
            [I,J] = ndgrid(batch,1:n2);
            ijv = [I(tags),J(tags),dist(tags)];
          end
          % and stuff them into the cell structure
          accum{end+1,1} = ijv; %#ok
          % increment the batch
          batch = batch + bs;
          if batch(end)>n1
            batch(batch>n1) = [];
          end
        end
        % convert the cells into one flat array
        accum = cell2mat(accum);
        % keep only the params.Limit best of those singled out
        accum = sortrows(accum,3);
        if ('s'==params.Subset(1))
          % minimum value specified
          accum = accum(1:params.Limit,:);
        else
          % minimum value specified
          accum = accum(end + 1 - (1:params.Limit),:);
        end
        d.rowindex = accum(:,1);
        d.columnindex = accum(:,2);
        d.distance = accum(:,3);
        % create the matrix as a sparse one or a struct?
        if params.Result(1)=='a'
          % its an array, so make the array sparse.
          d = sparse(d.rowindex,d.columnindex,d.distance,n1,size(dist,2));
        end
      else
        
        ijv = zeros(0,3);
        % loop over the chunks
        batch = (1:bs)';
        while ~isempty(batch)
          % One set or two?
          if dataflag == 1
            dist = distcomp(data1(batch,:),data1,params);
            k = find(tril(ones(length(batch),n2),batch(1)-1));
            if ('s'==params.Subset(1))
              dist(k) = inf;
            else
              dist(k) = -inf;
            end
          else
            dist = distcomp(data1(batch,:),data2,params);
          end
          [I,J] = ndgrid(batch,1:n2);
          ijv = [ijv;[I(:),J(:),dist(:)]]; %#ok
          % big or small as requested, keeping only the best
          % params.Limit number of elements
          if size(ijv,1) > params.Limit
            if ('s'==params.Subset(1))
              % minimum value specified
              [tags,tags] = sort(ijv(:,3),1,'ascend'); %#ok
            else
              [tags,tags] = sort(ijv(:,3),1,'ascend'); %#ok
            end
            ijv = ijv(tags(1:params.Limit),:);
          end
          % increment the batch
          batch = batch + bs;
          if batch(end)>n1
            batch(batch>n1) = [];
          end
        end
        % They are fully trimmed down. stuff a structure
        d.rowindex = ijv(:,1);
        d.columnindex = ijv(:,2);
        d.distance = ijv(:,3);
        % create the matrix as a sparse one or a struct?
        if params.Result(1)=='a'
          % its an array, so make the array sparse.
          d = sparse(d.rowindex,d.columnindex,d.distance,n1,size(dist,2));
        end
      end
    end
  case {'nearestneighbor' 'farthestneighbor'}
    % find the closest/farthest neighbor for every point
    % is this a large problem? Or a 1-d problem?
    if dim == 1
      % first split it into the farthest versus nearest cases.
      if params.Subset(1) == 'f'
        % farthest away
        % One set or two?
        if dataflag == 1
          [d2min,minind] = min(data1);
          [d2max,maxind] = max(data1);
        else
          [d2min,minind] = min(data2);
          [d2max,maxind] = max(data2);
        end
        d.rowindex = (1:n1)';
        d.columnindex = repmat(maxind,n1,1);
        d.distance = repmat(d2max,n1,1);
        % which endpoint was further away?
        k = abs((data1 - d2min)) >= abs((data1 - d2max));
        if any(k)
          d.columnindex(k) = minind;
          d.distance(k) = d2min;
        end
      else
        % nearest. this is mainly a sort and some fussing around.
        d.rowindex = (1:n1)';
        d.columnindex = ones(n1,1);
        d.distance = zeros(n1,1);
        % One set or two?
        if dataflag == 1
          % if only one data point, then we are done
          if n1 == 2
            % if exactly two data points, its trivial
            d.columnindex = [2 1];
            d.distance = repmat(abs(diff(data1)),2,1);
          elseif n1>2
            % at least three points. do a sort.
            [sorted_data,tags] = sort(data1);
            % handle the first and last points separately
            d.columnindex(tags(1)) = tags(2);
            d.distance(tags(1)) = sorted_data(2) - sorted_data(1);
            d.columnindex(tags(end)) = tags(end-1);
            d.distance(tags(end)) = sorted_data(end) - sorted_data(end-1);
            ind = (2:(n1-1))';
            d1 = sorted_data(ind) - sorted_data(ind-1);
            d2 = sorted_data(ind+1) - sorted_data(ind);
            k = d1 < d2;
            d.distance(tags(ind(k))) = d1(k);
            d.columnindex(tags(ind(k))) = tags(ind(k)-1);
            k = ~k;
            d.distance(tags(ind(k))) = d2(k);
            d.columnindex(tags(ind(k))) = tags(ind(k)+1);
          end % if n1 == 2
        else
          % Two sets of data. still really a sort and some fuss.
          if n2 == 1
            % there is only one point in data2
            d.distance = abs(data1 - data2);
            % d.columnindex is already set correctly
          else
            ind12 = [1:n1,1:n2]';
            bool12 = [zeros(n1,1);ones(n2,1)];
            [sorted_data,tags] = sort([data1;data2]);
            ind12 = ind12(tags);
            bool12 = bool12(tags);
            % where did each point end up after the sort?
            loc1 = find(~bool12);
            loc2 = find(bool12);
            % for each point in data1, what is the (sorted) data2
            % element which appears most nearly to the left of it?
            cs = cumsum(bool12);
            leftelement = cs(loc1);
            % any points which fell below the minimum element in data2
            % will have a zero for the index of the element on their
            % left. fix this.
            leftelement = max(1,leftelement);
            % likewise, any point greater than the max in data2 will
            % have an n2 in left element. this too will be a problem
            % later, so fix it.
            leftelement = min(n2-1,leftelement);
            % distance to the left hand element
            dleft = abs(sorted_data(loc1) - sorted_data(loc2(leftelement)));
            dright = abs(sorted_data(loc1) - sorted_data(loc2(leftelement+1)));
            % find the points which are closer to the left element in data2
            k = (dleft < dright);
            d.distance(ind12(loc1(k))) = dleft(k);
            d.columnindex(ind12(loc1(k))) = ind12(loc2(leftelement(k)));
            k = ~k;
            d.distance(ind12(loc1(k))) = dright(k);
            d.columnindex(ind12(loc1(k))) = ind12(loc2(leftelement(k)+1));
          end % if n2 == 1
        end % if dataflag == 1
      end % if params.Subset(1) == 'f'
      % create the matrix as a sparse one or a struct?
      if params.Result(1)=='a'
        % its an array, so make the array sparse.
        d = sparse(d.rowindex,d.columnindex,d.distance,n1,n2);
      end
    elseif (ntotal>1000) && (((params.Metric == 0) && (params.Subset(1) == 'n')) || ...
        ((params.Metric == inf) && (params.Subset(1) == 'f')))
      % do the first dimension
      if dataflag == 1
        d = ipdm(data1(:,1),data1(:,1),'subset',params.Subset,'metric',params.Metric,'result','struct');
      else
        d = ipdm(data1(:,1),data2(:,1),'subset',params.Subset,'metric',params.Metric,'result','struct');
      end
      % its slightly different for nearest versus farthest here
      % now, loop over dimensions
      for i = 2:dim
        if dataflag == 1
          di = ipdm(data1(:,i),data1(:,i),'subset',params.Subset,'metric',params.Metric,'result','struct');
        else
          di = ipdm(data1(:,i),data2(:,i),'subset',params.Subset,'metric',params.Metric,'result','struct');
        end
        % did any of the distances change?
        if params.Metric == 0
          % the 0 norm, with nearest neighbour, so take the
          % smallest distance in any dimension.
          k = d.distance > di.distance;
        else
          % inf norm. so take the largest distance across dimensions
          k = d.distance < di.distance;
        end
        if any(k)
          d.distance(k) = di.distance(k);
          d.columnindex(k) = di.columnindex(k);
        end
      end
      % create the matrix as a sparse one or a struct?
      if params.Result(1)=='a'
        % its an array, so make the array sparse.
        d = sparse(d.rowindex,d.columnindex,d.distance,n1,n2);
      end
    elseif ((ntotal*8) <= params.ChunkSize)
      % None of the other special cases apply, so do it using brute
      % force for the small potatoes problem.
      % One set or two?
      if dataflag == 1
        dist = distcomp(data1,data1,params);
      else
        dist = distcomp(data1,data2,params);
      end
      % if only one data set and if a nearest neighbor
      % problem, set the diagonal to +inf so we don't find it.
      if (dataflag==1) && (n1>1) && ('n'==params.Subset(1))
        diagind = (1:n1) + (0:n1:(n1^2-1));
        dist(diagind) = +inf;
      end
      if ('n'==params.Subset(1))
        % nearest
        [val,j] = min(dist,[],2);
      else
        % farthest
        [val,j] = max(dist,[],2);
      end
      % create the matrix as a sparse one or a struct?
      if params.Result(1)=='a'
        % its an array, so make the array sparse.
        d = sparse((1:n1)',j,val,n1,size(dist,2));
      else
        % a structure
        d.rowindex = (1:n1)';
        d.columnindex = j;
        d.distance = val;
      end
    else
      % break it into chunks
      bs = floor(params.ChunkSize/(8*n2));
      bs = min(n1,max(1,bs));
      % pre-allocate the result
      d.rowindex = (1:n1)';
      d.columnindex = zeros(n1,1);
      d.distance = zeros(n1,1);
      % now loop over the chunks
      batch = 1:bs;
      while ~isempty(batch)
        % One set or two?
        if dataflag == 1
          dist = distcomp(data1(batch,:),data1,params);
        else
          dist = distcomp(data1(batch,:),data2,params);
        end
        % if only one data set and if a nearest neighbor
        % problem, set the diagonal to +inf so we don't find it.
        if (dataflag==1) && (n1>1) && ('n'==params.Subset(1))
          diagind = 1:length(batch);
          diagind = diagind + (diagind-2+batch(1))*length(batch);
          dist(diagind) = +inf;
        end
        % big or small as requested
        if ('n'==params.Subset(1))
          % nearest
          [val,j] = min(dist,[],2);
        else
          % farthest
          [val,j] = max(dist,[],2);
        end
        % and stuff them into the result structure
        d.columnindex(batch) = j;
        d.distance(batch) = val;
        % increment the batch
        batch = batch + bs;
        if batch(end)>n1
          batch(batch>n1) = [];
        end
      end
      % did we need to return a struct or an array?
      if params.Result(1) == 'a'
        % an array. make it a sparse one
        d = sparse(d.rowindex,d.columnindex,d.distance,n1,n2);
      end
    end % if dim == 1
end  % switch params.Subset
end
% End of mainline
% ======================================================
% begin subfunctions
% ======================================================
function d = distcomp(set1,set2,params)
% Subfunction to compute all distances between two sets of points
dim = size(set1,2);
if params.usebsxfun
  % its a recent enough version of matlab that we can
  % use bsxfun at all.
  n1 = size(set1,1);
  n2 = size(set2,1);
  if (dim>1) && ((n1*n2*dim)<=params.ChunkSize)
    % its a small enough problem that we might gain by full
    % use of bsxfun
    switch params.Metric
      case 2
        d = sum(bsxfun(@minus,reshape(set1,[n1,1,dim]),reshape(set2,[1,n2,dim])).^2,3);
      case 1
        d = sum(abs(bsxfun(@minus,reshape(set1,[n1,1,dim]),reshape(set2,[1,n2,dim]))),3);
      case inf
        d = max(abs(bsxfun(@minus,reshape(set1,[n1,1,dim]),reshape(set2,[1,n2,dim]))),[],3);
      case 0
        d = min(abs(bsxfun(@minus,reshape(set1,[n1,1,dim]),reshape(set2,[1,n2,dim]))),[],3);
    end
  else
    % too big, so that the ChunkSize will have been exceeded, or just 1-d
    if params.Metric == 2
      d = bsxfun(@minus,set1(:,1),set2(:,1)').^2;
    else
      d = abs(bsxfun(@minus,set1(:,1),set2(:,1)'));
    end
    for i=2:dim
      switch params.Metric
        case 2
          d = d + bsxfun(@minus,set1(:,i),set2(:,i)').^2;
        case 1
          d = d + abs(bsxfun(@minus,set1(:,i),set2(:,i)'));
        case inf
          d = max(d,abs(bsxfun(@minus,set1(:,i),set2(:,i)')));
        case 0
          d = min(d,abs(bsxfun(@minus,set1(:,i),set2(:,i)')));
      end
    end
  end
else
  % Cannot use bsxfun. Sigh. Do things the hard (and slower) way.
  n1 = size(set1,1);
  n2 = size(set2,1);
  if params.Metric == 2
    d = (repmat(set1(:,1),1,n2) - repmat(set2(:,1)',n1,1)).^2;
  else
    d = abs(repmat(set1(:,1),1,n2) - repmat(set2(:,1)',n1,1));
  end
  for i=2:dim
    switch params.Metric
      case 2
        d = d + (repmat(set1(:,i),1,n2) - repmat(set2(:,i)',n1,1)).^2;
      case 1
        d = d + abs(repmat(set1(:,i),1,n2) - repmat(set2(:,i)',n1,1));
      case inf
        d = max(d,abs(repmat(set1(:,i),1,n2) - repmat(set2(:,i)',n1,1)));
      case 0
        d = min(d,abs(repmat(set1(:,i),1,n2) - repmat(set2(:,i)',n1,1)));
    end
  end
end
% if 2 norm, then we must sqrt at the end
if params.Metric==2
  d = sqrt(d);
end
end
% ==============================================================
%    end main ipdm
%    begin included function - parse_pv_pairs
% ==============================================================
function params=parse_pv_pairs(params,pv_pairs)
npv = length(pv_pairs);
n = npv/2;
if n~=floor(n)
  error 'Property/value pairs must come in PAIRS.'
end
if n<=0
  % just return the defaults
  return
end
if ~isstruct(params)
  error 'No structure for defaults was supplied'
end
% there was at least one pv pair. process any supplied
propnames = fieldnames(params);
lpropnames = lower(propnames);
for i=1:n
  p_i = lower(pv_pairs{2*i-1});
  v_i = pv_pairs{2*i};
  ind = strmatch(p_i,lpropnames,'exact');
  if isempty(ind)
    ind = find(strncmp(p_i,lpropnames,length(p_i)));
    if isempty(ind)
      error(['No matching property found for: ',pv_pairs{2*i-1}])
    elseif length(ind)>1
      error(['Ambiguous property name: ',pv_pairs{2*i-1}])
    end
  end
  p_i = propnames{ind};
  % override the corresponding default in params.
  % Use setfield for comptability issues with older releases.
  params = setfield(params,p_i,v_i); %#ok
end
end

[Torsten]

If the three curves were parts of concentric circles, you could expect the three points to lie on a straight line. But why should they in the above constellation ? You are still in the illusion that the lines connecting the three points are orthogonal to all three curves, aren't you ?

[Tim Schaller]

Hi Torsten,

I don't think, that the line are parts of concentric circles, I have a different problem.

I try to tell you what I am thinking now.

The blue line is the middle line in my example. So if i get a point on the upper red line, that belongs to another point to the blue line and also is orthogonal to the blue line and then I do the same procedure to the other direction, then "lines that connect the red points with the blue point should be colinear.

What i can think of as a mistake, is that maybe the it is like reverse then I am thinking.

So that the orthognal axis is not "measured" at the blue line, but on the red line.

If that is the case, then I completely understand, why the points not lined up, but I also need to look for what to change in order to get the solution I want to.