function [distance, nearestPointOnSeg ]= point2segmentDistance(point, segmentPoint1, segmentPoint2)
segmentPoint1 = segmentPoint1(:);
segmentPoint2 = segmentPoint2(:);
V = segmentPoint2 - segmentPoint1;
V1 = point - segmentPoint1;
V2 = point - segmentPoint2;
theta1 = acos(V'*V1/(L*L1));
theta2 = acos(-V'*V2/(L*L2));
distance = L1; nearestPointOnSeg = segmentPoint1;
distance = L2; nearestPointOnSeg = segmentPoint2;
distance = L1*sin(theta1);
nearestPointOnSeg = segmentPoint1 + V/L * L1*cos(theta1);
[disA2Seg, A_nearestPseg ] = point2segmentDistance(PA, P1, P2)
[disB2Seg, B_nearestPseg ] = point2segmentDistance(PB, P1, P2)
plot3([P1(1),P2(1)], [P1(2),P2(2)],[P1(3),P2(3)],'r-o','markerfacecolor','r','linewidth', 2)
text(P1(1), P1(2), P1(3), 'P_1');
text(P2(1), P2(2), P2(3), 'P_2');
text(PA(1), PA(2), PA(3), 'P_A');
text(PB(1), PB(2), PB(3), 'P_B');
text(A_nearestPseg(1), A_nearestPseg(2), A_nearestPseg(3), '\_\_\_P(Nearest to A)');
text(B_nearestPseg(1), B_nearestPseg(2), B_nearestPseg(3)+eps, '\_\_\_P(Nearest to B)');
plot3([PA(1),A_nearestPseg(1)], [PA(2),A_nearestPseg(2)],[PA(3),A_nearestPseg(3)],...
'g-h','markerfacecolor','g','linewidth', 2)
plot3([PB(1),B_nearestPseg(1)], [PB(2),B_nearestPseg(2)],[PB(3),B_nearestPseg(3)],...
'b-s','markerfacecolor','b','linewidth', 2)