Advertisement
Guest User

Untitled

a guest
Feb 20th, 2019
91
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.43 KB | None | 0 0
  1. function r = PointToLineSegment3D( S1, S2, Pnt )
  2. % r = PointToLineSegment3D( S1, S2, Pnt )
  3.  
  4. vx = S1(1)-Pnt(1);
  5. vy = S1(2)-Pnt(2);
  6. % vz = S1(3)-Pnt(3);
  7.  
  8. ux = S2(1)-S1(1);
  9. uy = S2(2)-S1(2);
  10. % uz = S2(3)-S1(3);
  11.  
  12. lenSqr= (ux*ux+uy*uy); % +uz*uz
  13. detP= -vx*ux + -vy*uy;
  14.  
  15. if( detP < 0 )
  16. r = norm(S1-Pnt,2);
  17.  
  18. elseif( detP > lenSqr )
  19. r = norm(S2-Pnt,2);
  20.  
  21. else
  22. r = abs(ux*vy-uy*vx)/sqrt(lenSqr);
  23. end
  24. end
  25.  
  26. X = A+t*(B-A)
  27.  
  28. 0 == (A+t*(B-A) - P)' * (B-A) == (A-P)'*(B-A)+t*norm(B-A)^2
  29.  
  30. t = (A-P)'*(B-A) / norm(B-A)^2
  31.  
  32. d = norm(X-P)
  33.  
  34. d = norm(A+t*(B-A)-P)
  35.  
  36. function r = PointToLineSegment3D( S1, S2, Pnt )
  37. % r = PointToLineSegment3D( S1, S2, Pnt )
  38.  
  39. vx = S1(1)-Pnt(1);
  40. vy = S1(2)-Pnt(2);
  41. vz = S1(3)-Pnt(3);
  42.  
  43. ux = S2(1)-S1(1);
  44. uy = S2(2)-S1(2);
  45. uz = S2(3)-S1(3);
  46.  
  47. lenSqr= (ux*ux+uy*uy+uz*uz)
  48.  
  49.  
  50. detP= -vx*ux + -vy*uy + -vz*uz;
  51.  
  52. if( detP < 0 )
  53. r = norm(S1-Pnt,2);
  54.  
  55. elseif( detP > lenSqr )
  56. r = norm(S2-Pnt,2);
  57.  
  58. else
  59. r =norm( abs(cross((S2-S1),(S1-Pnt)))/sqrt(lenSqr));
  60. end
  61. end
  62.  
  63. Pnt=[1 1 1];
  64. S1=[0 0 0];
  65. S2=[0 3 3];
  66.  
  67. vx = S1(1)-Pnt(1);
  68. vy = S1(2)-Pnt(2);
  69. vz = S1(3)-Pnt(3);
  70.  
  71. ux = S2(1)-S1(1);
  72. uy = S2(2)-S1(2);
  73. uz = S2(3)-S1(3);
  74.  
  75. lenSqr= (ux*ux+uy*uy+uz*uz)
  76. detP= -vx*ux + -vy*uy + -vz*uz
  77.  
  78. i1 = uz*vy-uy*vz
  79. j1 = ux*vz-uz*vx
  80. k1 = uy*vx-ux*vy
  81.  
  82. if( detP < 0 )
  83. r = norm(S1-Pnt,2)
  84.  
  85. elseif( detP > lenSqr )
  86. r = norm(S2-Pnt,2)
  87.  
  88. else
  89. r = sqrt(conv(i1,i1)+conv(j1,j1)+conv(k1,k1)) /sqrt(lenSqr)
  90. end
  91.  
  92. #include<iostream>
  93. #include<math.h>
  94. #include<stdio.h>
  95. #include<vector>
  96. #include<iterator>
  97. #include <iomanip>
  98.  
  99. using namespace std;
  100. int main()
  101. {
  102.  
  103. double vx,vy,vz,ux,uy,uz,r=0,lenSqr,detP, c,tmp;
  104. int i;
  105. vector<double>copy;
  106. vector<double>Pnt;
  107. vector<double>S1;
  108. vector<double>S2;
  109.  
  110. for(i=0; i<9; i++)
  111. {
  112. cin>>c;
  113. copy.push_back(c);
  114. }
  115. for(i=0; i<3; i++)
  116. {
  117. Pnt.insert(Pnt.begin(), copy[i]);
  118. // cout<<copy[i]<<endl;
  119. }
  120. copy.erase(copy.begin(),copy.begin()+3);
  121. copy.shrink_to_fit();
  122. for(i=0; i<3; i++)
  123. {
  124. S1.insert(S1.begin(), copy[i]);
  125. }
  126. copy.erase(copy.begin(),copy.begin()+3);
  127. copy.shrink_to_fit();
  128. for(i=0; i<3; i++)
  129. {
  130. S2.insert(S2.begin(), copy[i]);
  131. copy.erase(copy.begin());
  132. }
  133. copy.shrink_to_fit();
  134.  
  135.  
  136.  
  137. /*
  138. vector<float>Pnt(3,1.0);
  139. //for(i=0; i<3; i++)
  140. //cout<<Pnt[i];
  141.  
  142. vector<float>S1(3,0.0);
  143. //for(i=0; i<3; i++)
  144. //cout<<S1[i];
  145.  
  146. vector<float>S2;
  147. S2.insert(S2.begin(), 3.0);
  148. S2.insert(S2.begin(), 3.0);
  149. S2.insert(S2.begin(), 0.0);
  150.  
  151. //for(int i=0; i<3; i++)
  152. //cout<<S2[i];
  153. //cout<<endl;
  154. */
  155.  
  156. vx = S1[0]-Pnt[0];
  157. vy = S1[1]-Pnt[1];
  158. vz = S1[2]-Pnt[2];
  159. //cout<<"V: "<<vx<<vy<<vz<<endl;
  160. ux = S2[0]-S1[0];
  161. uy = S2[1]-S1[1];
  162. uz = S2[2]-S1[2];
  163. //cout<<"U: "<<ux<<uy<<uz<<endl;
  164.  
  165.  
  166. lenSqr= (ux*ux+uy*uy+uz*uz);
  167. //cout<<"lenSqr "<<lenSqr<<endl;
  168. detP= (-vx*ux ) + (-vy*uy) + (-vz*uz);
  169. //cout<<"detP "<<detP<<endl;
  170.  
  171.  
  172.  
  173. if( detP < 0 )
  174. {
  175. // r = norm(S1-Pnt,2)
  176. for(i=0; i<3; i++)
  177. {
  178. tmp=pow((S1[i]-Pnt[i]),2);
  179. r += tmp;
  180. // cout<<"r: "<<r;
  181. }
  182. r = sqrt(r);
  183. cout<<fixed<<r;
  184. }
  185.  
  186. else if( detP > lenSqr )
  187. {
  188. // r = norm(S2-Pnt,2);
  189. for(i=0; i<3; i++)
  190. {
  191. tmp=pow((S2[i]-Pnt[i]),2);
  192. r += tmp;
  193. // cout<<"r: "<<r;
  194. }
  195. r = sqrt(r);
  196. cout<<fixed<<r;
  197. }
  198. //if(detP <= lenSqr || detP>=0)
  199. else
  200. {
  201. // r =norm( abs(cross((S2-S1),(S1-Pnt)))/sqrt(lenSqr));
  202. float i1,j1,k1;
  203.  
  204. i1 = uz*vy-uy*vz;
  205. j1 = ux*vz-uz*vx;
  206. k1 = uy*vx-ux*vy;
  207. //cout<<"I J k: "<<i1<<j1<<k1<<endl;
  208. r=sqrt(pow(i1,2)+pow(j1,2)+pow(k1,2))/sqrt(lenSqr);
  209. cout<<fixed<<r;
  210. }
  211.  
  212. return 0;
  213. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement