Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- function r = PointToLineSegment3D( S1, S2, Pnt )
- % r = PointToLineSegment3D( S1, S2, Pnt )
- vx = S1(1)-Pnt(1);
- vy = S1(2)-Pnt(2);
- % vz = S1(3)-Pnt(3);
- ux = S2(1)-S1(1);
- uy = S2(2)-S1(2);
- % uz = S2(3)-S1(3);
- lenSqr= (ux*ux+uy*uy); % +uz*uz
- detP= -vx*ux + -vy*uy;
- if( detP < 0 )
- r = norm(S1-Pnt,2);
- elseif( detP > lenSqr )
- r = norm(S2-Pnt,2);
- else
- r = abs(ux*vy-uy*vx)/sqrt(lenSqr);
- end
- end
- X = A+t*(B-A)
- 0 == (A+t*(B-A) - P)' * (B-A) == (A-P)'*(B-A)+t*norm(B-A)^2
- t = (A-P)'*(B-A) / norm(B-A)^2
- d = norm(X-P)
- d = norm(A+t*(B-A)-P)
- function r = PointToLineSegment3D( S1, S2, Pnt )
- % r = PointToLineSegment3D( S1, S2, Pnt )
- vx = S1(1)-Pnt(1);
- vy = S1(2)-Pnt(2);
- vz = S1(3)-Pnt(3);
- ux = S2(1)-S1(1);
- uy = S2(2)-S1(2);
- uz = S2(3)-S1(3);
- lenSqr= (ux*ux+uy*uy+uz*uz)
- detP= -vx*ux + -vy*uy + -vz*uz;
- if( detP < 0 )
- r = norm(S1-Pnt,2);
- elseif( detP > lenSqr )
- r = norm(S2-Pnt,2);
- else
- r =norm( abs(cross((S2-S1),(S1-Pnt)))/sqrt(lenSqr));
- end
- end
- Pnt=[1 1 1];
- S1=[0 0 0];
- S2=[0 3 3];
- vx = S1(1)-Pnt(1);
- vy = S1(2)-Pnt(2);
- vz = S1(3)-Pnt(3);
- ux = S2(1)-S1(1);
- uy = S2(2)-S1(2);
- uz = S2(3)-S1(3);
- lenSqr= (ux*ux+uy*uy+uz*uz)
- detP= -vx*ux + -vy*uy + -vz*uz
- i1 = uz*vy-uy*vz
- j1 = ux*vz-uz*vx
- k1 = uy*vx-ux*vy
- if( detP < 0 )
- r = norm(S1-Pnt,2)
- elseif( detP > lenSqr )
- r = norm(S2-Pnt,2)
- else
- r = sqrt(conv(i1,i1)+conv(j1,j1)+conv(k1,k1)) /sqrt(lenSqr)
- end
- #include<iostream>
- #include<math.h>
- #include<stdio.h>
- #include<vector>
- #include<iterator>
- #include <iomanip>
- using namespace std;
- int main()
- {
- double vx,vy,vz,ux,uy,uz,r=0,lenSqr,detP, c,tmp;
- int i;
- vector<double>copy;
- vector<double>Pnt;
- vector<double>S1;
- vector<double>S2;
- for(i=0; i<9; i++)
- {
- cin>>c;
- copy.push_back(c);
- }
- for(i=0; i<3; i++)
- {
- Pnt.insert(Pnt.begin(), copy[i]);
- // cout<<copy[i]<<endl;
- }
- copy.erase(copy.begin(),copy.begin()+3);
- copy.shrink_to_fit();
- for(i=0; i<3; i++)
- {
- S1.insert(S1.begin(), copy[i]);
- }
- copy.erase(copy.begin(),copy.begin()+3);
- copy.shrink_to_fit();
- for(i=0; i<3; i++)
- {
- S2.insert(S2.begin(), copy[i]);
- copy.erase(copy.begin());
- }
- copy.shrink_to_fit();
- /*
- vector<float>Pnt(3,1.0);
- //for(i=0; i<3; i++)
- //cout<<Pnt[i];
- vector<float>S1(3,0.0);
- //for(i=0; i<3; i++)
- //cout<<S1[i];
- vector<float>S2;
- S2.insert(S2.begin(), 3.0);
- S2.insert(S2.begin(), 3.0);
- S2.insert(S2.begin(), 0.0);
- //for(int i=0; i<3; i++)
- //cout<<S2[i];
- //cout<<endl;
- */
- vx = S1[0]-Pnt[0];
- vy = S1[1]-Pnt[1];
- vz = S1[2]-Pnt[2];
- //cout<<"V: "<<vx<<vy<<vz<<endl;
- ux = S2[0]-S1[0];
- uy = S2[1]-S1[1];
- uz = S2[2]-S1[2];
- //cout<<"U: "<<ux<<uy<<uz<<endl;
- lenSqr= (ux*ux+uy*uy+uz*uz);
- //cout<<"lenSqr "<<lenSqr<<endl;
- detP= (-vx*ux ) + (-vy*uy) + (-vz*uz);
- //cout<<"detP "<<detP<<endl;
- if( detP < 0 )
- {
- // r = norm(S1-Pnt,2)
- for(i=0; i<3; i++)
- {
- tmp=pow((S1[i]-Pnt[i]),2);
- r += tmp;
- // cout<<"r: "<<r;
- }
- r = sqrt(r);
- cout<<fixed<<r;
- }
- else if( detP > lenSqr )
- {
- // r = norm(S2-Pnt,2);
- for(i=0; i<3; i++)
- {
- tmp=pow((S2[i]-Pnt[i]),2);
- r += tmp;
- // cout<<"r: "<<r;
- }
- r = sqrt(r);
- cout<<fixed<<r;
- }
- //if(detP <= lenSqr || detP>=0)
- else
- {
- // r =norm( abs(cross((S2-S1),(S1-Pnt)))/sqrt(lenSqr));
- float i1,j1,k1;
- i1 = uz*vy-uy*vz;
- j1 = ux*vz-uz*vx;
- k1 = uy*vx-ux*vy;
- //cout<<"I J k: "<<i1<<j1<<k1<<endl;
- r=sqrt(pow(i1,2)+pow(j1,2)+pow(k1,2))/sqrt(lenSqr);
- cout<<fixed<<r;
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement