求空間中2條線段的最短距離(用osg+C++寫的)
阿新 • • 發佈:2019-01-29
2條線段的最短距離
float DistanceLineToLine( const osg::Vec3d& p1,const osg::Vec3d& p2,const osg::Vec3d& p3,const osg::Vec3d& p4 ) { float distance; float x1 = p1.x(); //A點座標(x1,y1,z1) float y1 = p1.y(); float z1 = p1.z(); float x2 = p2.x(); //B點座標(x2,y2,z2) float y2 = p2.y(); float z2 = p2.z(); float x3 = p3.x(); //C點座標(x3,y3,z3) float y3 = p3.y(); float z3 = p3.z(); float x4 = p4.x(); //D點座標(x4,y4,z4) float y4 = p4.y(); float z4 = p4.z(); float a = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1); float b = -((x2-x1)*(x4-x3)+(y2-y1)*(y4-y3)+(z2-z1)*(z4-z3)); float c = -((x1-x2)*(x1-x3)+(y1-y2)*(y1-y3)+(z1-z2)*(z1-z3)); float d = -((x2-x1)*(x4-x3)+(y2-y1)*(y4-y3)+(z2-z1)*(z4-z3)); float e = (x4-x3)*(x4-x3)+(y4-y3)*(y4-y3)+(z4-z3)*(z4-z3); float f = -((x1-x3)*(x4-x3)+(y1-y3)*(y4-y3)+(z1-z3)*(z4-z3)); if ((a*e-b*d)==0&&(b*d-a*e)==0) //平行 { float d1 = (p1-p3).length(); float d2 = (p1-p4).length(); distance = (d1<d2)?d1:d2; return distance; } float s = (b*f-e*c)/(a*e-b*d); float t = (a*f-d*c)/(b*d-a*e); if(0<=s&&s<=1&&0<=t&&t<=1) //說明P點落線上段AB上,Q點落線上段CD上 { //2條線段的公垂線段PQ; //P點座標 float X = x1+s*(x2-x1); float Y = y1+s*(y2-y1); float Z = z1+s*(z2-z1); //Q點座標 float U = x3+t*(x4-x3); float V = y3+t*(y4-y3); float W = z3+t*(z4-z3); osg::Vec3d P(X,Y,Z); osg::Vec3d Q(U,V,W); distance = (P-Q).length(); } else { float d1 = DistancePointToLine(p3,p4,p1); float d2 = DistancePointToLine(p3,p4,p2); float d3 = DistancePointToLine(p1,p2,p3); float d4 = DistancePointToLine(p1,p2,p4); distance = (d1<d2)?d1:d2; distance = (distance<d3)?distance:d3; distance = (distance<d4)?distance:d4; } return distance; }
空間中一個點到空間中一條線段的最短距離
float DistancePointToLine( const osg::Vec3d& star, const osg::Vec3d& end,const osg::Vec3d& center ) { float distance; float x0 = center.x();//P點座標(x0,y0,z0) float y0 = center.y(); float z0 = center.z(); float x1 = star.x(); //A點座標(x1,y1,z1) float y1 = star.y(); float z1 = star.z(); float x2 = end.x();//B點座標(x2,y2,z2) float y2 = end.y(); float z2 = end.z(); float t = ((x1-x0)*(x1-x2)+(y1-y0)*(y1-y2)+(z1-z0)*(z1-z2)) /((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2)); if (0<=t&&t<=1)//垂足Q點落線上段AB上 { float X = x1+t*(x2-x1); float Y = y1+t*(y2-y1); float Z = z1+t*(z2-z1); osg::Vec3d Q(X,Y,Z); distance = (Q-center).length(); } if (t<0) //垂足Q點不落線上段AB上,而是落在BA的延長線上 { distance = (star-center).length(); } if (t>1) //垂足Q點不落線上段AB上,而是落在AB的延長線上 { distance = (end-center).length(); } return distance; }
數學公式,我直接上傳了!地址:http://download.csdn.net/detail/liying426/5058179