% Contact ythomas@csail.mit.edu or msabuncu@csail.mit.edu for bugs or questions
%
%=========================================================================
%
% Copyright (c) 2008 Thomas Yeo and Mert Sabuncu
% All rights reserved.
%
%Redistribution and use in source and binary forms, with or without
%modification, are permitted provided that the following conditions are met:
%
% * Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% * Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% * Neither the names of the copyright holders nor the names of future
% contributors may be used to endorse or promote products derived from this
% software without specific prior written permission.
%
%THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
%ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
%WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
%DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
%ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
%(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
%LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
%ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
%(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
%SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%=========================================================================
function new_vec = ParallelTransport(old_vec, old_pt, new_pt, radius)

% new_vector = ParallelTransport(old_vector, old_pt, new_pt)
%
% Assumes everything is 3 x N

if(nargin < 4)
radius = 100;
end

if(max(abs(dot(old_vec, old_pt, 1)./radius./sqrt(sum(old_vec.^2, 1)))) > 1e-4)
keyboard;
error(['ParallelTransport: old_direction should be perpendicular to old_pt: ' num2str(max(abs(dot(old_vec, old_pt, 1)./radius./sqrt(sum(old_vec.^2, 1)))))]);
end

% First compute great circle direction to new pt;
oldv = MARS_findTangentVecPt1toPt2(old_pt, new_pt, radius);
if(abs(sum(oldv)) == 0)
new_vec = old_vec;
return;
end

oldv = oldv ./ repmat( sqrt(sum(oldv.^2, 1)), 3, 1);

oldw = cross(old_pt, oldv, 1);
oldw = oldw ./ repmat( sqrt(sum(oldw.^2, 1)), 3, 1);

% note that oldw = neww
newv = cross(oldw, new_pt, 1);
newv = newv ./ repmat( sqrt(sum(newv.^2, 1)), 3, 1);

proj_on_newv = dot(old_vec, oldv, 1);
proj_on_neww = dot(old_vec, oldw, 1);

new_vec = repmat(proj_on_newv, 3, 1) .* newv + repmat(proj_on_neww, 3, 1) .* oldw;

% %assumes everything is 3 x 1
% This old implementation is outdated
%
% if( abs(dot(old_direction,old_pt)) > 1e-10)
% error('ParallelTransport: old_direction should be perpendicular to old_pt');
% end
%
% if(abs(norm(old_direction)-1) > 1e-10)
% error('ParallelTransport: old_direction should be unit length');
% end
%
% new_direction = cross(cross(old_pt, old_direction), new_pt);
% new_direction = new_direction/norm(new_direction);