hdu 4741 Save Labman No.004 计算几何
直接贴模版！！！
求异面直线的距离及坐标！！
代码如下：
1 #include<iostream> 2 #include<stdio.h> 3 #include<algorithm> 4 #include<iomanip> 5 #include<cmath> 6 #include<cstring> 7 #include<vector> 8 #define I(p) scanf("%lf%lf%lf",&p.x,&p.y,&p.z) 9 #define eps 1e-20 10 #define zero(x) (((x)>0?(x):-(x))<eps) 11 using namespace std; 12 struct point 13 { 14 double x,y,z; 15 point operator+(point a) 16 { 17 point c; 18 c.x=x+a.x; 19 c.y=y+a.y; 20 c.z=z+a.z; 21 return c; 22 } 23 }; 24 struct line{point a,b;}l1,l2; 25 struct plane 26 { 27 point a,b,c; 28 plane(point _a,point _b,point _c) 29 { 30 a=_a; 31 b=_b; 32 c=_c; 33 } 34 }; 35 point xmult(point u,point v) 36 { 37 point ret; 38 ret.x=u.y*v.z-v.y*u.z; 39 ret.y=u.z*v.x-u.x*v.z; 40 ret.z=u.x*v.y-u.y*v.x; 41 return ret; 42 } 43 double dmult(point u,point v) 44 { 45 return u.x*v.x+u.y*v.y+u.z*v.z; 46 } 47 point subt(point u,point v) 48 { 49 point ret; 50 ret.x=u.x-v.x; 51 ret.y=u.y-v.y; 52 ret.z=u.z-v.z; 53 return ret; 54 } 55 //取平面法向量 56 57 point pvec(plane s) 58 { 59 60 return xmult(subt(s.a,s.b),subt(s.b,s.c)); 61 62 } 63 64 point pvec(point s1,point s2,point s3) 65 { 66 return xmult(subt(s1,s2),subt(s2,s3)); 67 68 } 69 70 double vlen(point p) 71 { 72 return sqrt(p.x*p.x+p.y*p.y+p.z*p.z); 73 } 74 double distances(point p1,point p2) 75 { 76 return sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)+(p1.z-p2.z)*(p1.z-p2.z)); 77 } 78 79 //判两直线平行 80 81 int parallel(line u,line v) 82 { 83 84 return vlen(xmult(subt(u.a,u.b),subt(v.a,v.b)))<eps; 85 } 86 87 int parallel(point u1,point u2,point v1,point v2) 88 { 89 90 return vlen(xmult(subt(u1,u2),subt(v1,v2)))<eps; 91 92 } 93 94 //判两平面平行 95 96 int parallel(plane u,plane v) 97 { 98 99 return vlen(xmult(pvec(u),pvec(v)))<eps; 100 101 } 102 103 int parallel(point u1,point u2,point u3,point v1,point v2,point v3) 104 { 105 return vlen(xmult(pvec(u1,u2,u3),pvec(v1,v2,v3)))<eps; 106 107 } 108 109 //判直线与平面平行 110 111 int parallel(line l,plane s) 112 { 113 return zero(dmult(subt(l.a,l.b),pvec(s))); 114 115 } 116 117 int parallel(point l1,point l2,point s1,point s2,point s3) 118 { 119 120 return zero(dmult(subt(l1,l2),pvec(s1,s2,s3))); 121 122 } 123 124 //判两直线垂直 125 126 int perpendicular(line u,line v) 127 { 128 129 return zero(dmult(subt(u.a,u.b),subt(v.a,v.b))); 130 131 } 132 int perpendicular(point u1,point u2,point v1,point v2) 133 { 134 135 return zero(dmult(subt(u1,u2),subt(v1,v2))); 136 137 } 138 139 //判两平面垂直 140 141 int perpendicular(plane u,plane v) 142 { 143 144 return zero(dmult(pvec(u),pvec(v))); 145 146 } 147 148 int perpendicular(point u1,point u2,point u3,point v1,point v2,point v3) 149 { 150 return zero(dmult(pvec(u1,u2,u3),pvec(v1,v2,v3))); 151 152 } 153 154 //判直线与平面平行 155 156 int perpendicular(line l,plane s) 157 { 158 return vlen(xmult(subt(l.a,l.b),pvec(s)))<eps; 159 160 } 161 162 int perpendicular(point l1,point l2,point s1,point s2,point s3) 163 { 164 165 return vlen(xmult(subt(l1,l2),pvec(s1,s2,s3)))<eps; 166 167 } 168 169 //计算两直线交点,注意事先判断直线是否共面和平行! 170 //线段交点请另外判线段相交(同时还是要判断是否平行!) 171 point intersection(point u1,point u2,point v1,point v2) 172 { 173 174 point ret=u1; 175 176 double t=((u1.x-v1.x)*(v1.y-v2.y)-(u1.y-v1.y)*(v1.x-v2.x)) 177 178 /((u1.x-u2.x)*(v1.y-v2.y)-(u1.y-u2.y)*(v1.x-v2.x)); 179 180 ret.x+=(u2.x-u1.x)*t; 181 ret.y+=(u2.y-u1.y)*t; 182 183 ret.z+=(u2.z-u1.z)*t; 184 185 return ret; 186 187 } 188 point intersection(point l1,point l2,point s1,point s2,point s3) 189 { 190 191 point ret=pvec(s1,s2,s3); 192 193 double t=(ret.x*(s1.x-l1.x)+ret.y*(s1.y-l1.y)+ret.z*(s1.z-l1.z))/ 194 (ret.x*(l2.x-l1.x)+ret.y*(l2.y-l1.y)+ret.z*(l2.z-l1.z)); 195 196 ret.x=l1.x+(l2.x-l1.x)*t; 197 198 ret.y=l1.y+(l2.y-l1.y)*t; 199 200 ret.z=l1.z+(l2.z-l1.z)*t; 201 202 return ret; 203 } 204 //计算两平面交线,注意事先判断是否平行,并保证三点不共线! 205 line intersection(plane u,plane v) 206 { 207 208 line ret; 209 ret.a=parallel(v.a,v.b,u.a,u.b,u.c)?intersection(v.b,v.c,u.a,u.b,u.c):intersection(v.a,v.b,u.a,u.b,u.c); 210 211 ret.b=parallel(v.c,v.a,u.a,u.b,u.c)?intersection(v.b,v.c,u.a,u.b,u.c):intersection(v.c,v.a,u.a,u.b,u.c); 212 213 return ret; 214 } 215 line intersection(point u1,point u2,point u3,point v1,point v2,point v3) 216 { 217 218 line ret; 219 220 ret.a=parallel(v1,v2,u1,u2,u3)?intersection(v2,v3,u1,u2,u3):intersection(v1,v2,u1,u2,u3); 221 222 ret.b=parallel(v3,v1,u1,u2,u3)?intersection(v2,v3,u1,u2,u3):intersection(v3,v1,u1,u2,u3); 223 return ret; 224 225 } 226 //计算两直线交点,注意事先判断直线是否共面和平行! 227 //线段交点请另外判线段相交(同时还是要判断是否平行!) 228 point intersection(line u,line v) 229 { 230 231 point ret=u.a; 232 double x, y; 233 x= ((u.a.x-v.a.x)*(v.a.y-v.b.y)-(u.a.y-v.a.y)*(v.a.x-v.b.x)); 234 y= ((u.a.x-u.b.x)*(v.a.y-v.b.y)-(u.a.y-u.b.y)*(v.a.x-v.b.x)); 235 double t=((u.a.x-v.a.x)*(v.a.y-v.b.y)-(u.a.y-v.a.y)*(v.a.x-v.b.x)) 236 237 /((u.a.x-u.b.x)*(v.a.y-v.b.y)-(u.a.y-u.b.y)*(v.a.x-v.b.x)); 238 if(fabs(x)>eps){ 239 ret.x+=(u.b.x-u.a.x)*t; 240 241 ret.y+=(u.b.y-u.a.y)*t; 242 243 ret.z+=(u.b.z-u.a.z)*t; 244 } 245 return ret; 246 247 } 248 int main() 249 { 250 int t; 251 double dis,len1,len2; 252 scanf("%d",&t); 253 while(t--){ 254 I(l1.a); 255 I(l1.b); 256 I(l2.a); 257 I(l2.b); 258 point n=xmult(subt(l1.a,l1.b),subt(l2.a,l2.b)),a,b; 259 a=l1.a+n; 260 b=l2.a+n; 261 plane p1(l1.a,l1.b,a),p2(l2.a,l2.b,b); 262 line m=intersection(p1,p2); 263 point ans1=intersection(m,l1); 264 point ans2=intersection(m,l2); 265 printf("%.6lf\n",distances(ans1,ans2)); 266 printf("%.6lf %.6lf %.6lf %.6lf %.6lf %.6lf\n",ans1.x,ans1.y,ans1.z,ans2.x,ans2.y,ans2.z); 267 } 268 return 0; 269 }
