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 }

View Code