#
root/OpenSceneGraph/trunk/src/osgShadow/ConvexPolyhedron.cpp
@
9475

Revision 9475, 60.9 kB (checked in by robert, 8 years ago) |
---|

Rev | Line | |
---|---|---|

[8984] | 1 | /* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield |

2 | * | |

3 | * This library is open source and may be redistributed and/or modified under | |

4 | * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or | |

5 | * (at your option) any later version. The full license is in LICENSE file | |

6 | * included with this distribution, and on the openscenegraph.org website. | |

7 | * | |

8 | * This library is distributed in the hope that it will be useful, | |

9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |

10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |

11 | * OpenSceneGraph Public License for more details. | |

12 | * | |

13 | * ViewDependentShadow codes Copyright (C) 2008 Wojciech Lewandowski | |

14 | * Thanks to to my company http://www.ai.com.pl for allowing me free this work. | |

15 | */ | |

16 | ||

17 | #include<osgShadow/ConvexPolyhedron> | |

18 | #include<osg/Group> | |

19 | #include<osg/Geode> | |

20 | #include<osgDB/WriteFile> | |

21 | ||

[9009] | 22 | #include<cassert> |

23 | #include<deque> | |

24 | #include<algorithm> | |

25 | ||

[9475] | 26 | #include <stdio.h> |

[9343] | 27 | #include <string.h> |

[9009] | 28 | |

[8984] | 29 | using namespace osgShadow; |

30 | ||

31 | ||

32 | #if defined( DEBUG ) || defined( _DEBUG ) || defined( _DEBUG_ ) | |

33 | ||

34 | // #define MAKE_CHECKS 0 | |

35 | ||

36 | #endif | |

37 | ||

38 | #if MAKE_CHECKS | |

39 | ||

40 | inline std::ostream & DEBUG_NOTIFY( void ) | |

41 | { | |

42 | return osg::notify( osg::WARN ); | |

43 | } | |

44 | #define WARN DEBUG_NOTIFY() | |

45 | ||

46 | #define CONVEX_POLYHEDRON_CHECK_COHERENCY 1 | |

47 | #define CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA 2 | |

48 | #define CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA 2 | |

49 | #define CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON 1 | |

50 | #define CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON 1 | |

51 | #define CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON 1 | |

52 | #define CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON 1 | |

53 | #define CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT 1 | |

54 | #define CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT 1 | |

55 | ||

56 | ||

57 | #else | |

58 | ||

59 | #define WARN osg::notify( osg::WARN ) | |

60 | ||

61 | #define CONVEX_POLYHEDRON_CHECK_COHERENCY 0 | |

62 | #define CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA 0 | |

63 | #define CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA 0 | |

64 | #define CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON 0 | |

65 | #define CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON 0 | |

66 | #define CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON 0 | |

67 | #define CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON 0 | |

68 | #define CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT 0 | |

69 | #define CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT 0 | |

70 | ||

71 | #endif | |

72 | ||

73 | const osg::Matrix & ConvexPolyhedron::defaultMatrix = *(osg::Matrix*)NULL; | |

74 | static const double epsi = pow( 2.0, -20.0 ); //circa 8 times float prec(~2^-23) | |

75 | static const double plane_hull_tolerance = 1.0e-5; | |

76 | static const double point_plane_tolerance = 0; | |

77 | static const double point_point_tolerance = 0; | |

78 | static const double point_point_equivalence = 0; | |

79 | ||

80 | // Tim Moore modifications for GCC 4.3 August 15, 2008 | |

81 | // they correspond to Adrian Egli tweaks for VS 7.3 on August 19, 2008 | |

82 | namespace | |

83 | { | |

84 | typedef std::vector< double > Distances; | |

85 | typedef std::vector< osg::Vec4d > Points; | |

86 | ||

87 | // Auxilliary params contined per face | |

88 | struct FaceDistances | |

89 | { | |

90 | ConvexPolyhedron::Faces::iterator itr; | |

91 | Points points; | |

92 | Distances distances; | |

93 | int below, above, on; | |

94 | }; | |

95 | } // namespace | |

96 | ||

97 | ConvexPolyhedron::ConvexPolyhedron( const osg::Matrix& matrix, | |

98 | const osg::Matrix& inverse, | |

99 | const osg::BoundingBox& bb ) | |

100 | { | |

101 | setToBoundingBox( bb ); | |

102 | ||

103 | if( &matrix != &defaultMatrix && &inverse != &defaultMatrix ) | |

104 | transform( matrix, inverse ); | |

105 | else if( &matrix != &defaultMatrix && &inverse == &defaultMatrix ) | |

106 | transform( matrix, osg::Matrix::inverse( matrix ) ); | |

107 | else if( &matrix == &defaultMatrix && &inverse != &defaultMatrix ) | |

108 | transform( osg::Matrix::inverse( inverse ), matrix ); | |

109 | } | |

110 | ||

111 | void ConvexPolyhedron::setToUnitFrustum(bool withNear, bool withFar) | |

112 | { | |

113 | const osg::Vec3d v000(-1.0,-1.0,-1.0); | |

114 | const osg::Vec3d v010(-1.0,1.0,-1.0); | |

115 | const osg::Vec3d v001(-1.0,-1.0,1.0); | |

116 | const osg::Vec3d v011(-1.0,1.0,1.0); | |

117 | const osg::Vec3d v100(1.0,-1.0,-1.0); | |

118 | const osg::Vec3d v110(1.0,1.0,-1.0); | |

119 | const osg::Vec3d v101(1.0,-1.0,1.0); | |

120 | const osg::Vec3d v111(1.0,1.0,1.0); | |

121 | ||

122 | _faces.clear(); | |

123 | ||

124 | { // left plane. | |

125 | Face& face = createFace(); | |

126 | face.name = "left"; | |

127 | face.plane.set(1.0,0.0,0.0,1.0); | |

128 | face.vertices.push_back(v000); | |

129 | face.vertices.push_back(v001); | |

130 | face.vertices.push_back(v011); | |

131 | face.vertices.push_back(v010); | |

132 | } | |

133 | ||

134 | { // right plane. | |

135 | Face& face = createFace(); | |

136 | face.name = "right"; | |

137 | face.plane.set(-1.0,0.0,0.0,1.0); | |

138 | face.vertices.push_back(v100); | |

139 | face.vertices.push_back(v110); | |

140 | face.vertices.push_back(v111); | |

141 | face.vertices.push_back(v101); | |

142 | } | |

143 | ||

144 | { // bottom plane. | |

145 | Face& face = createFace(); | |

146 | face.name = "bottom"; | |

147 | face.plane.set(0.0,1.0,0.0,1.0); | |

148 | face.vertices.push_back(v000); | |

149 | face.vertices.push_back(v100); | |

150 | face.vertices.push_back(v101); | |

151 | face.vertices.push_back(v001); | |

152 | } | |

153 | ||

154 | { // top plane. | |

155 | Face& face = createFace(); | |

156 | face.name = "top"; | |

157 | face.plane.set(0.0,-1.0,0.0,1.0); | |

158 | face.vertices.push_back(v010); | |

159 | face.vertices.push_back(v011); | |

160 | face.vertices.push_back(v111); | |

161 | face.vertices.push_back(v110); | |

162 | } | |

163 | ||

164 | if (withNear) | |

165 | { // near plane | |

166 | Face& face = createFace(); | |

167 | face.name = "near"; | |

168 | face.plane.set(0.0,0.0,1.0,1.0); | |

169 | face.vertices.push_back(v000); | |

170 | face.vertices.push_back(v010); | |

171 | face.vertices.push_back(v110); | |

172 | face.vertices.push_back(v100); | |

173 | } | |

174 | ||

175 | if (withFar) | |

176 | { // far plane | |

177 | Face& face = createFace(); | |

178 | face.name = "far"; | |

179 | face.plane.set(0.0,0.0,-1.0,1.0); | |

180 | face.vertices.push_back(v001); | |

181 | face.vertices.push_back(v101); | |

182 | face.vertices.push_back(v111); | |

183 | face.vertices.push_back(v011); | |

184 | } | |

185 | } | |

186 | ||

187 | void ConvexPolyhedron::setToBoundingBox(const osg::BoundingBox& bb) | |

188 | { | |

189 | _faces.clear(); | |

190 | ||

191 | // Ignore invalid and one dimensional boxes | |

192 | if( bb._min[0] >= bb._max[0] || | |

193 | bb._min[1] >= bb._max[1] || | |

194 | bb._min[2] >= bb._max[2] ) return; | |

195 | ||

196 | const osg::Vec3d v000(bb.xMin(),bb.yMin(), bb.zMin()); | |

197 | const osg::Vec3d v010(bb.xMin(),bb.yMax(), bb.zMin()); | |

198 | const osg::Vec3d v001(bb.xMin(),bb.yMin(), bb.zMax()); | |

199 | const osg::Vec3d v011(bb.xMin(),bb.yMax(), bb.zMax()); | |

200 | const osg::Vec3d v100(bb.xMax(),bb.yMin(), bb.zMin()); | |

201 | const osg::Vec3d v110(bb.xMax(),bb.yMax(), bb.zMin()); | |

202 | const osg::Vec3d v101(bb.xMax(),bb.yMin(), bb.zMax()); | |

203 | const osg::Vec3d v111(bb.xMax(),bb.yMax(), bb.zMax()); | |

204 | ||

205 | ||

206 | ||

207 | { // x min plane | |

208 | Face& face = createFace(); | |

209 | face.name = "xMin"; | |

210 | face.plane.set(1.0,0.0,0.0,-bb.xMin()); | |

211 | face.vertices.push_back(v000); | |

212 | face.vertices.push_back(v001); | |

213 | face.vertices.push_back(v011); | |

214 | face.vertices.push_back(v010); | |

215 | } | |

216 | ||

217 | { // x max plane. | |

218 | Face& face = createFace(); | |

219 | face.name = "xMax"; | |

220 | face.plane.set(-1.0,0.0,0.0,bb.xMax()); | |

221 | face.vertices.push_back(v100); | |

222 | face.vertices.push_back(v110); | |

223 | face.vertices.push_back(v111); | |

224 | face.vertices.push_back(v101); | |

225 | } | |

226 | ||

227 | { // y min plane. | |

228 | Face& face = createFace(); | |

229 | face.name = "yMin"; | |

230 | face.plane.set(0.0,1.0,0.0,-bb.yMin()); | |

231 | face.vertices.push_back(v000); | |

232 | face.vertices.push_back(v100); | |

233 | face.vertices.push_back(v101); | |

234 | face.vertices.push_back(v001); | |

235 | } | |

236 | ||

237 | { // y max plane. | |

238 | Face& face = createFace(); | |

239 | face.name = "yMax"; | |

240 | face.plane.set(0.0,-1.0,0.0,bb.yMax()); | |

241 | face.vertices.push_back(v010); | |

242 | face.vertices.push_back(v011); | |

243 | face.vertices.push_back(v111); | |

244 | face.vertices.push_back(v110); | |

245 | } | |

246 | { // z min plane | |

247 | Face& face = createFace(); | |

248 | face.name = "zMin"; | |

249 | face.plane.set(0.0,0.0,1.0,-bb.zMin()); | |

250 | face.vertices.push_back(v000); | |

251 | face.vertices.push_back(v010); | |

252 | face.vertices.push_back(v110); | |

253 | face.vertices.push_back(v100); | |

254 | } | |

255 | ||

256 | { // z max plane | |

257 | Face& face = createFace(); | |

258 | face.name = "zMax"; | |

259 | face.plane.set(0.0,0.0,-1.0,bb.zMax()); | |

260 | face.vertices.push_back(v001); | |

261 | face.vertices.push_back(v101); | |

262 | face.vertices.push_back(v111); | |

263 | face.vertices.push_back(v011); | |

264 | } | |

265 | } | |

266 | ||

267 | void ConvexPolyhedron::transform(const osg::Matrix& matrix, const osg::Matrix& inverse) | |

268 | { | |

269 | bool requires_infinite_plane_clip = false; | |

270 | ||

271 | ConvexPolyhedron cp = *this; | |

272 | for(Faces::iterator itr = _faces.begin(); | |

273 | itr != _faces.end() && !requires_infinite_plane_clip; | |

274 | ++itr) | |

275 | { | |

276 | Face& face = *itr; | |

277 | face.plane.transformProvidingInverse(inverse); | |

278 | for(Vertices::iterator vitr = face.vertices.begin(); | |

279 | vitr != face.vertices.end(); | |

280 | ++vitr) | |

281 | { | |

282 | osg::Vec4d v( *vitr, 1.0 ); | |

283 | v = v * matrix; | |

284 | ||

285 | if( v[3] <= 0 ) { | |

286 | requires_infinite_plane_clip = true; | |

287 | break; | |

288 | } | |

289 | ||

290 | vitr->set( v[0]/v[3], v[1]/v[3], v[2]/v[3] ); | |

291 | } | |

292 | } | |

293 | ||

294 | if( requires_infinite_plane_clip ) { | |

295 | *this = cp; | |

296 | transformClip( matrix, inverse ); | |

297 | // cp.dumpGeometry(&cp._faces.back(),&cp._faces.back().plane,this); | |

298 | } | |

299 | ||

300 | // Perpective transforms and lack of precision | |

301 | // occasionaly cause removal of some points | |

302 | ||

303 | removeDuplicateVertices( ); | |

304 | ||

305 | checkCoherency( true, "ConvexPolyhedron::transform" ); | |

306 | } | |

307 | ||

308 | void ConvexPolyhedron::transformClip(const osg::Matrix& matrix, const osg::Matrix& inverse) | |

309 | { | |

310 | double tolerance = 0.00001; | |

311 | ||

312 | if( _faces.empty() ) return; | |

313 | ||

314 | ||

315 | ConvexPolyhedron cp( *this ); | |

316 | ||

317 | typedef std::vector< FaceDistances > FaceDistancesList; | |

318 | FaceDistancesList faceDistances; | |

319 | faceDistances.resize( _faces.size() ); | |

320 | ||

321 | double min = FLT_MAX, max = -FLT_MAX; //Hull max & min point distances | |

322 | ||

323 | FaceDistancesList::iterator fd = faceDistances.begin(); | |

324 | // First step compute each face points distances to cutting plane | |

325 | for( Faces::iterator itr = _faces.begin(); | |

326 | itr != _faces.end(); | |

327 | ++itr, ++fd ) | |

328 | { | |

329 | fd->itr = itr; | |

330 | fd->distances.reserve( itr->vertices.size() ); | |

331 | fd->on = 0; | |

332 | fd->above = 0; | |

333 | fd->below = 0; | |

334 | ||

335 | itr->plane.transformProvidingInverse(inverse); | |

336 | ||

337 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

338 | vitr != itr->vertices.end(); | |

339 | ++vitr) | |

340 | { | |

341 | osg::Vec4d p( *vitr, 1.0 ); | |

342 | p = p * matrix; | |

343 | vitr->set( p[0]/p[3], p[1]/p[3], p[2]/p[3] ); | |

344 | ||

345 | double d = p[3]-tolerance; | |

346 | ||

347 | fd->points.push_back( p ); | |

348 | fd->distances.push_back( d ); | |

349 | if ( d>point_plane_tolerance ) ++fd->above; | |

350 | else if ( d<-point_plane_tolerance ) ++fd->below; | |

351 | else ++fd->on; | |

352 | min = osg::minimum( min, d ); | |

353 | max = osg::maximum( max, d ); | |

354 | } | |

355 | } | |

356 | ||

357 | if( max <= plane_hull_tolerance ) { // All points on or below cutting plane | |

358 | _faces.clear(); | |

359 | return; | |

360 | } | |

361 | ||

362 | if( min >= -plane_hull_tolerance ) { // All points on or above cutting plane | |

363 | return; | |

364 | } | |

365 | ||

366 | typedef std::pair<osg::Vec3d, osg::Vec3d> Edge; | |

367 | typedef std::set< Edge > Edges; | |

368 | Edges edges; | |

369 | ||

370 | for( FaceDistancesList::iterator fd = faceDistances.begin(); | |

371 | fd != faceDistances.end(); | |

372 | ++fd ) | |

373 | { | |

374 | if ( fd->below == 0 ) | |

375 | { // skip face if all points on or above cutting plane ( below == 0 ) | |

376 | continue; | |

377 | } | |

378 | ||

379 | if ( /* fd->below > 0 && */ fd->above == 0 && fd->on == 0 ) | |

380 | { | |

381 | _faces.erase( fd->itr ); // remove face if points below or on | |

382 | continue; | |

383 | } | |

384 | ||

385 | // cut the face if some points above and below plane | |

386 | // assert( fd->below > 0 && fd->above > 0 ); | |

387 | ||

388 | Face& face = *(fd->itr); | |

389 | Points& vertices = fd->points; | |

390 | Distances& distances = fd->distances; | |

391 | Vertices newFaceVertices; | |

392 | Vertices newVertices; | |

393 | ||

394 | for(unsigned int i=0; i < vertices.size(); ++i) | |

395 | { | |

396 | osg::Vec4d &va = vertices[i]; | |

397 | osg::Vec4d &vb = vertices[(i+1)%vertices.size()]; | |

398 | double &distance_a = distances[i]; | |

399 | double &distance_b = distances[(i+1)%vertices.size()]; | |

400 | ||

401 | // Is first edge point above or on the plane? | |

402 | if ( -point_plane_tolerance <= distance_a ) { | |

403 | ||

404 | osg::Vec3d v( vertices[i][0], vertices[i][1], vertices[i][2] ); | |

405 | v /= vertices[i][3]; | |

406 | ||

407 | if( newVertices.empty() || v != newVertices.back() ) | |

408 | newVertices.push_back( v ); | |

409 | ||

410 | if ( distance_a <= point_plane_tolerance ) { | |

411 | if( newFaceVertices.empty() || v != newFaceVertices.back() ) | |

412 | newFaceVertices.push_back( v ); | |

413 | } | |

414 | } | |

415 | ||

416 | // Does edge intersect plane ? | |

417 | if ( ( distance_a < -point_plane_tolerance && distance_b > point_plane_tolerance ) || | |

418 | ( distance_b < -point_plane_tolerance && distance_a > point_plane_tolerance ) ) | |

419 | { | |

420 | osg::Vec4d intersection; // Inserting vertex | |

421 | double da = fabs( distance_a ), db = fabs( distance_b ); | |

422 | ||

423 | // tweaks to improve coherency of polytope after cut | |

424 | if( da <= point_point_equivalence && da <= db ) { | |

425 | intersection = va; | |

426 | } else if( db <= point_point_equivalence && db <= da ) { | |

427 | intersection = vb; | |

428 | } else { | |

429 | double dab4 = 0.25 * ( da + db ); | |

430 | if( dab4 < da && dab4 < db ) { | |

431 | intersection = (vb*distance_a - va*distance_b)/(distance_a-distance_b); | |

432 | } else { | |

433 | osg::Vec4d v = (vb - va)/(distance_a-distance_b); | |

434 | if( da < db ) | |

435 | intersection = va + v * distance_a; | |

436 | else | |

437 | intersection = vb + v * distance_b; | |

438 | } | |

439 | } | |

440 | ||

441 | ||

442 | osg::Vec3d v( intersection[0], intersection[1], intersection[2] ); | |

443 | v /= intersection[3]; | |

444 | ||

445 | if( newVertices.empty() || v != newVertices.back() ) | |

446 | newVertices.push_back( v ); | |

447 | ||

448 | if( newFaceVertices.empty() || v != newFaceVertices.back() ) | |

449 | newFaceVertices.push_back( v ); | |

450 | } | |

451 | } | |

452 | ||

453 | if( newVertices.size() && newVertices.front() == newVertices.back() ) | |

454 | newVertices.pop_back(); | |

455 | ||

456 | if( newFaceVertices.size() && newFaceVertices.front() == newFaceVertices.back() ) | |

457 | newFaceVertices.pop_back(); | |

458 | ||

459 | if( newFaceVertices.size() == 1 ) { // This is very rare but correct | |

460 | WARN | |

461 | << "ConvexPolyhedron::transformClip - Slicing face polygon returns " | |

462 | << newFaceVertices.size() | |

463 | << " points. Should be 2 (usually) or 1 (rarely)." | |

464 | << std::endl; | |

465 | ||

466 | } else if( newFaceVertices.size() == 2 ) { | |

467 | if( newFaceVertices[0] < newFaceVertices[1] ) { | |

468 | edges.insert( Edge( newFaceVertices[0], newFaceVertices[1] ) ); | |

469 | } else { | |

470 | edges.insert( Edge( newFaceVertices[1], newFaceVertices[0] ) ); | |

471 | } | |

472 | } else if( newFaceVertices.size() > 2 ) { | |

473 | ||

474 | #if CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT | |

475 | ||

476 | // This may happen if face polygon is not planar or convex. | |

477 | // It may happen when face was distorted by projection transform | |

478 | // or when some polygon vertices land in incorrect positions after | |

479 | // some transformation by badly conditioned matrix (weak precison) | |

480 | ||

481 | WARN | |

482 | << "ConvexPolyhedron::transformClip - Slicing face polygon returns " | |

483 | << newFaceVertices.size() | |

484 | << " points. Should be 2 (usually) or 1 (rarely)." | |

485 | << std::endl; | |

486 | #endif | |

487 | ||

488 | #if CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT | |

489 | dumpGeometry( &face, NULL, &cp ); | |

490 | #endif | |

491 | ||

492 | // Lets try to recover from this uneasy situation | |

493 | // by comparing current face polygon edges cut by new plane | |

494 | // with edges selected for new plane | |

495 | ||

496 | unsigned i0 = 0, i1 = newFaceVertices.size() - 1; | |

497 | unsigned j0 = 0, j1 = newVertices.size() - 1; | |

498 | ||

499 | for( ; i0 < newFaceVertices.size(); i1 = i0++, j1 = j0++ ) { | |

500 | while( newFaceVertices[i0] != newVertices[j0] ) j1 = j0++; | |

501 | if( newFaceVertices[i1] == newVertices[j1] ) | |

502 | { | |

503 | if( newFaceVertices[i0] < newFaceVertices[i1] ) { | |

504 | edges.insert( Edge( newFaceVertices[i0], newFaceVertices[i1] ) ); | |

505 | } else { | |

506 | edges.insert( Edge( newFaceVertices[i1], newFaceVertices[i0] ) ); | |

507 | } | |

508 | } | |

509 | } | |

510 | } | |

511 | ||

512 | if( newVertices.size() >= 3 ) { //Add faces with at least 3 points | |

513 | ||

514 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

515 | int convex = isFacePolygonConvex( face ); | |

516 | face.vertices.swap( newVertices ); | |

517 | if( convex && !isFacePolygonConvex( face ) ) { | |

518 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

519 | WARN << "ConvexPolyhedron::transformClip - polygon output non convex." | |

520 | << " This may lead to other issues in ConvexPolyhedron math" << std::endl; | |

521 | #endif | |

522 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

523 | dumpGeometry( &face, NULL, &cp ); | |

524 | #endif | |

525 | } | |

526 | #else | |

527 | face.vertices.swap( newVertices ); | |

528 | #endif | |

529 | } else //Remove face reduced to 0, 1, 2 points | |

530 | _faces.erase( fd->itr ); | |

531 | } | |

532 | ||

533 | osg::Vec3d center( 0, 0, 0 ); | |

534 | unsigned count = 0; | |

535 | for( Faces::iterator itr = _faces.begin(); | |

536 | itr != _faces.end(); | |

537 | ++itr ) | |

538 | { | |

539 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

540 | vitr != itr->vertices.end(); | |

541 | ++vitr ) | |

542 | { | |

543 | center += *vitr; | |

544 | count ++; | |

545 | } | |

546 | } | |

547 | ||

548 | center /= count; | |

549 | ||

550 | ||

551 | if ( edges.size() > 1 ) //Ignore faces reduced to 0, 1, 2 points | |

552 | { | |

553 | Face face; | |

554 | face.name = "Almost infinite plane"; | |

555 | ||

556 | std::deque< osg::Vec3d > vertices; | |

557 | ||

558 | Edges::iterator itr = edges.begin(); | |

559 | vertices.push_back( itr->first ); | |

560 | vertices.push_back( itr->second ); | |

561 | edges.erase( itr++ ); | |

562 | ||

563 | for( unsigned int vertices_size = 0; | |

564 | vertices_size < vertices.size(); ) | |

565 | { | |

566 | vertices_size = vertices.size(); | |

567 | for( itr = edges.begin(); itr != edges.end(); ) | |

568 | { | |

569 | bool not_added = false; | |

570 | ||

571 | if( itr->first == vertices.back() ) | |

572 | vertices.push_back( itr->second ); | |

573 | else if ( itr->first == vertices.front() ) | |

574 | vertices.push_front( itr->second ); | |

575 | else if ( itr->second == vertices.back() ) | |

576 | vertices.push_back( itr->first ); | |

577 | else if ( itr->second == vertices.front() ) | |

578 | vertices.push_front( itr->first ); | |

579 | else | |

580 | not_added = true; | |

581 | ||

582 | if( not_added ) | |

583 | ++itr; | |

584 | else | |

585 | edges.erase( itr++ ); | |

586 | } | |

587 | } | |

588 | ||

589 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

590 | if( !edges.empty() ) { | |

591 | WARN | |

592 | << "ConvexPolyhedron::transformClip - Building new face polygon - " | |

593 | << "Found edges not matching former polygon ends" | |

594 | << std::endl; | |

595 | } | |

596 | #endif | |

597 | ||

[9009] | 598 | std::copy(vertices.begin(), vertices.end(), std::back_inserter(face.vertices)); |

[8984] | 599 | |

600 | face.plane.set( vertices[0], vertices[1], vertices[2] ); | |

601 | ||

602 | if( face.plane.distance( center ) < 0.0 ) face.plane.flip(); | |

603 | ||

604 | _faces.push_back(face); | |

605 | ||

606 | // Last vertex is duplicated - remove one instance | |

607 | if( face.vertices.front() == face.vertices.back() ) | |

608 | face.vertices.pop_back(); | |

609 | else {// If not duplicated then it may mean we have open polygon ;-( | |

610 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

611 | WARN | |

612 | << "ConvexPolyhedron::transformClip - Building new face polygon - " | |

613 | << " Polygon not properly closed." | |

614 | << std::endl; | |

615 | #endif | |

616 | #if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON | |

617 | dumpGeometry( &_faces.back(), NULL, &cp ); | |

618 | #endif | |

619 | } | |

620 | ||

621 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

622 | if( !isFacePolygonConvex( face ) ) { | |

623 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

624 | WARN << "ConvexPolyhedron::transformClip - new face polygon non convex." | |

625 | << " This may lead to other issues in ConvexPolyhedron math" << std::endl; | |

626 | #endif | |

627 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

628 | ConvexPolyhedron cp; | |

629 | cp.createFace() = face; | |

630 | cp.dumpGeometry( ); | |

631 | #endif | |

632 | } | |

633 | #endif | |

634 | } | |

635 | ||

636 | // Perpective transforms and lack of precision | |

637 | // occasionaly cause removal of some points | |

638 | ||

639 | removeDuplicateVertices( ); | |

640 | ||

641 | checkCoherency( true, "ConvexPolyhedron::transformClip" ); | |

642 | } | |

643 | ||

644 | bool ConvexPolyhedron::mergeFaces | |

645 | ( const Face & face0, const Face & face1, Face & face ) | |

646 | { | |

647 | typedef std::pair< osg::Vec3d, osg::Vec3d > Edge; | |

648 | typedef std::set<Edge> Edges; | |

649 | Edges edges; | |

650 | ||

651 | for(unsigned int i=0; i<face0.vertices.size(); ++i) | |

652 | { | |

653 | const osg::Vec3d* va = &face0.vertices[i]; | |

654 | const osg::Vec3d* vb = &face0.vertices[(i+1)%face0.vertices.size()]; | |

655 | if( *vb == *va ) continue; // ignore duplicated point edges | |

656 | if( *vb < *va ) std::swap( va, vb ); | |

657 | edges.insert( Edge( *va, *vb ) ); | |

658 | } | |

659 | ||

660 | bool intersection = false; | |

661 | for(unsigned int i=0; i<face1.vertices.size(); ++i) | |

662 | { | |

663 | const osg::Vec3d* va = &face1.vertices[i]; | |

664 | const osg::Vec3d* vb = &face1.vertices[(i+1)%face1.vertices.size()]; | |

665 | if( *vb == *va ) continue; // ignore duplicated point edges | |

666 | if( *vb < *va ) std::swap( va, vb ); | |

667 | Edge edge( *va, *vb ); | |

668 | Edges::iterator itr = edges.find( edge ); | |

669 | if( itr == edges.end() ) | |

670 | edges.insert( edge ); | |

671 | else { | |

672 | edges.erase( itr ); | |

673 | intersection = true; | |

674 | } | |

675 | } | |

676 | ||

677 | if( intersection ) | |

678 | face.vertices.clear(); | |

679 | ||

680 | if( intersection && edges.size() > 1 ) //Ignore faces reduced to 0, 1, 2 points | |

681 | { | |

682 | std::deque< osg::Vec3d > vertices; | |

683 | ||

684 | Edges::iterator itr = edges.begin(); | |

685 | vertices.push_back( itr->first ); | |

686 | vertices.push_back( itr->second ); | |

687 | edges.erase( itr++ ); | |

688 | ||

689 | for( unsigned int vertices_size = 0; | |

690 | vertices_size < vertices.size(); ) | |

691 | { | |

692 | vertices_size = vertices.size(); | |

693 | for( itr = edges.begin(); itr != edges.end(); ) | |

694 | { | |

695 | bool not_added = false; | |

696 | ||

697 | if( itr->first == vertices.back() ) | |

698 | vertices.push_back( itr->second ); | |

699 | else if ( itr->first == vertices.front() ) | |

700 | vertices.push_front( itr->second ); | |

701 | else if ( itr->second == vertices.back() ) | |

702 | vertices.push_back( itr->first ); | |

703 | else if ( itr->second == vertices.front() ) | |

704 | vertices.push_front( itr->first ); | |

705 | else | |

706 | not_added = true; | |

707 | ||

708 | if( not_added ) | |

709 | ++itr; | |

710 | else | |

711 | edges.erase( itr++ ); | |

712 | } | |

713 | } | |

714 | ||

715 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

716 | if( !edges.empty() ) { | |

717 | WARN | |

718 | << "ConvexPolyhedron::mergeFaces - Building new face polygon - " | |

719 | << "Found edges not matching former polygon ends." | |

720 | << std::endl; | |

721 | } | |

722 | #endif | |

723 | ||

724 | // Last vertex is duplicated - remove one instance | |

725 | if( vertices.front() == vertices.back() ) | |

726 | vertices.pop_back(); | |

727 | else {// If not duplicated then it may mean we have open polygon ;-( | |

728 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

729 | WARN | |

730 | << "ConvexPolyhedron::mergeFaces - Building new face polygon - " | |

731 | << " Polygon not properly closed." | |

732 | << std::endl; | |

733 | #endif | |

734 | #if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON | |

735 | #endif | |

736 | } | |

737 | ||

738 | #if 1 // Resulting plane will be the same as face0 | |

739 | face.plane = face0.plane; | |

[9009] | 740 | |

741 | std::copy(vertices.begin(), vertices.end(), std::back_inserter(face.vertices)); | |

742 | ||

[8984] | 743 | #else // Compute resulting plane as average of faces (not a good idea) |

744 | osg::Vec3d normal = face0.plane.getNormal() + face1.plane.getNormal(); | |

745 | normal.normalize(); | |

746 | ||

747 | osg::Vec3d center; | |

748 | for( unsigned int i = 0; i < vertices.size(); ++i ) | |

749 | { | |

750 | center += vertices[i]; | |

751 | face.vertices.push_back( vertices[i] ); | |

752 | } | |

753 | center /= vertices.size(); | |

754 | ||

755 | face.plane.set( normal, center ); | |

756 | #endif | |

757 | ||

758 | face.name = face0.name + " + " + face1.name; | |

759 | ||

760 | // No testing for concave polys | |

761 | // Its possible to build concave polygon from two merged faces | |

762 | // But after all coplanar faces are merged final result should be convex. | |

763 | } | |

764 | return intersection; | |

765 | } | |

766 | ||

767 | void ConvexPolyhedron::mergeCoplanarFaces | |

768 | ( const double & dot_tolerance, const double & delta_tolerance ) | |

769 | { | |

770 | for(Faces::iterator itr0 = _faces.begin(); | |

771 | itr0 != _faces.end(); | |

772 | ++itr0 ) | |

773 | { | |

774 | double tolerance = delta_tolerance; | |

775 | for( unsigned i = 0; i < itr0->vertices.size(); ++i ) { | |

776 | tolerance = osg::maximum( tolerance, | |

777 | fabs( itr0->plane.distance( itr0->vertices[i] ) ) ); | |

778 | } | |

779 | ||

780 | for(Faces::iterator itr1 = _faces.begin(); | |

781 | itr1 != _faces.end(); | |

782 | ) | |

783 | { | |

784 | if( itr1 == itr0 ) { | |

785 | ++itr1; | |

786 | continue; | |

787 | } | |

788 | ||

789 | bool attempt_merge = true; | |

790 | for( unsigned i = 0; i < itr1->vertices.size(); ++i ) { | |

791 | if( fabs( itr0->plane.distance( itr1->vertices[i] ) ) > tolerance ) | |

792 | { | |

793 | attempt_merge = false; | |

794 | break; | |

795 | } | |

796 | } | |

797 | ||

798 | if( !attempt_merge && | |

799 | 1.0 - itr0->plane.getNormal() * itr1->plane.getNormal() < dot_tolerance && | |

800 | fabs( itr0->plane.ptr()[3] - itr1->plane.ptr()[3] ) < delta_tolerance ) | |

801 | attempt_merge = true; | |

802 | ||

803 | if( attempt_merge && mergeFaces( *itr0, *itr1, *itr0 ) ) | |

804 | itr1 = _faces.erase( itr1 ); | |

805 | else | |

806 | ++itr1; | |

807 | } | |

808 | } | |

809 | } | |

810 | ||

811 | void ConvexPolyhedron::removeDuplicateVertices( void ) | |

812 | { | |

813 | #if 1 | |

814 | // Aggressive removal, find very close points and replace them | |

815 | // with their average. Second step wil do the rest. | |

816 | ||

817 | typedef std::map< osg::Vec3f, osg::Vec4d > Points; | |

818 | typedef std::set< osg::Vec3d > VertexSet; | |

819 | ||

820 | VertexSet vertexSet; | |

821 | Points points; | |

822 | ||

823 | for( Faces::iterator itr = _faces.begin(); | |

824 | itr != _faces.end(); | |

825 | ++itr ) | |

826 | { | |

827 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

828 | vitr != itr->vertices.end(); | |

829 | ++vitr ) | |

830 | { | |

831 | vertexSet.insert( *vitr ); | |

832 | } | |

833 | } | |

834 | ||

835 | for( VertexSet::iterator vitr = vertexSet.begin(); | |

836 | vitr != vertexSet.end(); | |

837 | ++vitr ) | |

838 | { | |

839 | points[ *vitr ] += osg::Vec4d( *vitr, 1.0 ); | |

840 | } | |

841 | ||

842 | for( Points::iterator itr = points.begin(); | |

843 | itr != points.end(); | |

844 | ++itr ) | |

845 | { | |

846 | if( itr->second[3] > 1.0 ) | |

847 | itr->second /= itr->second[3]; | |

848 | } | |

849 | ||

850 | for( Faces::iterator itr = _faces.begin(); | |

851 | itr != _faces.end(); | |

852 | ++itr ) | |

853 | { | |

854 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

855 | vitr != itr->vertices.end(); | |

856 | ++vitr ) | |

857 | { | |

858 | osg::Vec4d &v = points[ *vitr ]; | |

859 | *vitr = osg::Vec3d( v[0], v[1], v[2] ); | |

860 | } | |

861 | } | |

862 | #endif | |

863 | ||

864 | for( Faces::iterator itr = _faces.begin(); | |

865 | itr != _faces.end(); | |

866 | ) | |

867 | { | |

868 | assert( itr->vertices.size() > 0 ); | |

869 | ||

870 | osg::Vec3d prev = itr->vertices.back(); | |

871 | ||

872 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

873 | vitr != itr->vertices.end(); | |

874 | ) | |

875 | { | |

876 | if( *vitr == prev ) { | |

877 | vitr = itr->vertices.erase( vitr ); | |

878 | } else { | |

879 | prev = *vitr; | |

880 | ++vitr; | |

881 | } | |

882 | } | |

883 | ||

884 | if( itr->vertices.size() < 3 ) | |

885 | itr = _faces.erase( itr ); | |

886 | else | |

887 | ++itr; | |

888 | } | |

889 | ||

890 | mergeCoplanarFaces(); | |

891 | ||

892 | #if 1 | |

893 | // Experimentally remove vertices on colinear edge chains | |

894 | // effectivelyy replacing them with one edge | |

895 | typedef std::map<osg::Vec3d,int> VertexCounter; | |

896 | VertexCounter vertexCounter; | |

897 | ||

898 | for( Faces::iterator itr = _faces.begin(); | |

899 | itr != _faces.end(); | |

900 | ++itr) | |

901 | { | |

902 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

903 | vitr != itr->vertices.end(); | |

904 | ++vitr ) | |

905 | { | |

906 | ++vertexCounter[ *vitr ]; | |

907 | } | |

908 | } | |

909 | ||

910 | for( Faces::iterator itr = _faces.begin(); | |

911 | itr != _faces.end(); | |

912 | ) | |

913 | { | |

914 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

915 | vitr != itr->vertices.end(); | |

916 | ) | |

917 | { | |

918 | if( vertexCounter[ *vitr ] == 2 ) { | |

919 | #if 0 // Sanity check if we could remove this point without changing poly shape | |

920 | ||

921 | Vertices::iterator next = ( vitr + 1 == itr->vertices.end() ? | |

922 | itr->vertices.begin() : | |

923 | vitr + 1 ); | |

924 | ||

925 | Vertices::iterator prev = ( vitr == itr->vertices.begin() ? | |

926 | itr->vertices.end() - 1 : | |

927 | vitr - 1 ); | |

928 | ||

929 | ||

930 | osg::Vec3d va = *vitr - *prev; | |

931 | osg::Vec3d vb = *next - *vitr; | |

932 | ||

933 | double da = va.normalize(); | |

934 | double db = vb.normalize(); | |

935 | ||

936 | if( 0.001 < da && 0.001 < db ) | |

937 | { | |

938 | double dot = va * vb, limit = cos( 0.01 ); | |

939 | if( dot < limit ) { | |

940 | WARN << "ConvexPolyhedron::removeDuplicateVertices" | |

941 | << " - removed mid point connecting two non colinear edges." | |

942 | << " Angle(deg): " << osg::RadiansToDegrees( acos( dot ) ) | |

943 | << " Length1: " << da | |

944 | << " Length2: " << db | |

945 | << std::endl; | |

946 | } | |

947 | } else { | |

948 | if( da == 0.0 || db == 0.0 ) | |

949 | WARN << "ConvexPolyhedron::removeDuplicateVertices" | |

950 | << " - removed degenerated mid point connecting two edges" | |

951 | << std::endl; | |

952 | ||

953 | } | |

954 | #endif | |

955 | vitr = itr->vertices.erase( vitr ); | |

956 | } else { | |

957 | ++vitr; | |

958 | } | |

959 | } | |

960 | ||

961 | if( itr->vertices.size() < 3 ) | |

962 | itr = _faces.erase( itr ); | |

963 | else | |

964 | ++itr; | |

965 | } | |

966 | #endif | |

967 | ||

968 | checkCoherency( false, "Leave ConvexPolyhedron::removeDuplicateVertices" ); | |

969 | } | |

970 | ||

971 | int ConvexPolyhedron::pointsColinear | |

972 | ( const osg::Vec3d & a, const osg::Vec3d & b, const osg::Vec3d & c, | |

973 | const double & dot_tolerance, const double & delta_tolerance ) | |

974 | { | |

975 | osg::Vec3d va = b - a; | |

976 | osg::Vec3d vb = c - b; | |

977 | ||

978 | double da = va.normalize(); | |

979 | double db = vb.normalize(); | |

980 | ||

981 | if( delta_tolerance >= da || delta_tolerance >= db ) | |

982 | return -1; // assume collinearity if one of the edges is zero length | |

983 | ||

984 | if( 1.0 - fabs( va * vb ) <= dot_tolerance ) | |

985 | return 1; // edge normals match collinearity condition | |

986 | ||

987 | return 0; // nope. not collinear | |

988 | } | |

989 | ||

990 | int ConvexPolyhedron::isFacePolygonConvex( Face & face, bool ignoreColinearVertices ) | |

991 | { | |

992 | int positive = 0, negative = 0, colinear = 0; | |

993 | for( unsigned int i = 0; i < face.vertices.size(); ++i ) | |

994 | { | |

995 | osg::Vec3d va = face.vertices[i]; | |

996 | osg::Vec3d vb = face.vertices[(i+1)%face.vertices.size()]; | |

997 | osg::Vec3d vc = face.vertices[(i+2)%face.vertices.size()]; | |

998 | ||

999 | ||

1000 | #if ( CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA > 1 || CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON ) | |

[9376] | 1001 | double dist = fabs( face.plane.distance( va ) ); |

[8984] | 1002 | if( dist > 0.0001 ) |

1003 | { | |

1004 | WARN << "ConvexPolyhedron::isFacePolygonConvex - plane point too far from plane (" << dist <<")" << std::endl; | |

1005 | } | |

1006 | #endif | |

1007 | ||

1008 | // cast points on a plane | |

1009 | va -= face.plane.getNormal() * face.plane.distance( va ); | |

1010 | vb -= face.plane.getNormal() * face.plane.distance( vb ); | |

1011 | vc -= face.plane.getNormal() * face.plane.distance( vc ); | |

1012 | ||

1013 | if( pointsColinear( va, vb, vc ) ) { | |

1014 | colinear++; | |

1015 | } else { | |

1016 | double side =( ( vc - vb ) ^ ( vb - va ) ) * face.plane.getNormal(); | |

1017 | ||

1018 | if( side < 0 ) negative++; | |

1019 | if( side > 0 ) positive++; | |

1020 | } | |

1021 | } | |

1022 | ||

1023 | if( !ignoreColinearVertices && colinear > 0 ) | |

1024 | return 0; | |

1025 | ||

1026 | if( !negative && !positive ) | |

1027 | return 0; | |

1028 | ||

[9376] | 1029 | if( (negative + colinear) == static_cast<int>(face.vertices.size()) ) |

[8984] | 1030 | return -( negative + colinear ); |

1031 | ||

[9376] | 1032 | if( (positive + colinear) == static_cast<int>(face.vertices.size()) ) |

[8984] | 1033 | return +( positive + colinear ); |

1034 | ||

1035 | return 0; | |

1036 | } | |

1037 | ||

1038 | bool ConvexPolyhedron::checkCoherency | |

1039 | ( bool checkForNonConvexPolys, const char * errorPrefix ) | |

1040 | { | |

1041 | bool result = true; | |

1042 | bool convex = true; | |

1043 | ||

1044 | #if CONVEX_POLYHEDRON_CHECK_COHERENCY | |

1045 | ||

1046 | if( !errorPrefix ) errorPrefix = "ConvexPolyhedron"; | |

1047 | ||

1048 | typedef std::pair<osg::Vec3d, osg::Vec3d> Edge; | |

1049 | typedef std::map<Edge,int> EdgeCounter; | |

1050 | typedef std::map<osg::Vec3d,int> VertexCounter; | |

1051 | ||

1052 | EdgeCounter edgeCounter; | |

1053 | VertexCounter vertexCounter; | |

1054 | ||

1055 | for( Faces::iterator itr = _faces.begin(); | |

1056 | itr != _faces.end(); | |

1057 | ++itr) | |

1058 | { | |

1059 | Face& face = *itr; | |

1060 | ||

1061 | if( checkForNonConvexPolys && !isFacePolygonConvex( face ) ) { | |

1062 | convex = false; | |

1063 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA || CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON ) | |

1064 | WARN << errorPrefix << | |

1065 | " - coherency fail - face polygon concave" << std::endl; | |

1066 | #endif | |

1067 | #if ( 1 < CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

1068 | dumpGeometry( &face ); | |

1069 | #endif | |

1070 | } | |

1071 | ||

1072 | Vertices& vertices = face.vertices; | |

1073 | for(unsigned int i=0; i<vertices.size(); ++i) | |

1074 | { | |

1075 | osg::Vec3d& a = vertices[ i ]; | |

1076 | osg::Vec3d& b = vertices[ (i+1) % vertices.size()]; | |

1077 | ++vertexCounter[ a ]; | |

1078 | if (a<b) | |

1079 | ++edgeCounter[Edge(a,b)]; | |

1080 | else | |

1081 | ++edgeCounter[Edge(b,a)]; | |

1082 | } | |

1083 | } | |

1084 | ||

1085 | ||

1086 | for( EdgeCounter::iterator itr = edgeCounter.begin(); | |

1087 | itr != edgeCounter.end(); | |

1088 | ++itr) | |

1089 | { | |

1090 | const Edge &e = itr->first; | |

1091 | ||

1092 | if( e.first.isNaN() ) { | |

1093 | result = false; | |

1094 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1095 | WARN << errorPrefix << | |

1096 | " - coherency fail - Vertex is NaN." << std::endl; | |

1097 | #endif | |

1098 | } | |

1099 | ||

1100 | if( e.second.isNaN() ) { | |

1101 | result = false; | |

1102 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1103 | WARN << errorPrefix << | |

1104 | " - coherency fail - Vertex is NaN." << std::endl; | |

1105 | #endif | |

1106 | } | |

1107 | ||

1108 | if( e.first == e.second ) { | |

1109 | result = false; | |

1110 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1111 | WARN << errorPrefix << | |

1112 | " - coherency fail - Edge with identical vertices." << std::endl; | |

1113 | #endif | |

1114 | } | |

1115 | ||

1116 | if( vertexCounter[ e.first ] < 3 ) { | |

1117 | result = false; | |

1118 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1119 | WARN << errorPrefix << | |

1120 | " - coherency fail - Vertex present " << vertexCounter[ e.first ] << " times" << std::endl; | |

1121 | #endif | |

1122 | } | |

1123 | ||

1124 | if( vertexCounter[ e.second ] < 3 ) { | |

1125 | result = false; | |

1126 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1127 | WARN << errorPrefix << | |

1128 | " - coherency fail - Vertex present " << vertexCounter[ e.second ] << " times" << std::endl; | |

1129 | #endif | |

1130 | } | |

1131 | ||

1132 | if( itr->second != 2 ) { | |

1133 | result = false; | |

1134 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1135 | WARN << errorPrefix << | |

1136 | " - coherency fail - Edge present " << itr->second << " times" << std::endl; | |

1137 | #endif | |

1138 | } | |

1139 | } | |

1140 | ||

1141 | #if ( 1 == CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1142 | if( !convex ) | |

1143 | WARN << errorPrefix | |

1144 | << " - coherency fail - non convex output" << std::endl; | |

1145 | ||

1146 | if ( !result ) | |

1147 | WARN << errorPrefix | |

1148 | << " - coherency fail - incoherent output" << std::endl; | |

1149 | #endif | |

1150 | ||

1151 | #if ( 1 == CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA ) | |

1152 | if( !result || !convex ) | |

1153 | dumpGeometry( ); | |

1154 | #endif | |

1155 | ||

1156 | #if ( 1 < CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA ) | |

1157 | if( !result ) | |

1158 | dumpGeometry( ); | |

1159 | #endif | |

1160 | ||

1161 | ||

1162 | #endif // CONVEX_POLYHEDRON_CHECK_COHERENCY | |

1163 | return result && convex; | |

1164 | } | |

1165 | ||

1166 | osg::BoundingBox ConvexPolyhedron::computeBoundingBox( const osg::Matrix & m ) const | |

1167 | { | |

1168 | osg::BoundingBox bb; | |

1169 | ||

1170 | if( &m != &defaultMatrix ) { | |

1171 | for( Faces::const_iterator itr = _faces.begin(); itr != _faces.end(); ++itr ) | |

1172 | for( Vertices::const_iterator vitr = itr->vertices.begin(); | |

1173 | vitr != itr->vertices.end(); | |

1174 | ++vitr ) | |

1175 | bb.expandBy( *vitr * m ); | |

1176 | } else { | |

1177 | for( Faces::const_iterator itr = _faces.begin(); itr != _faces.end(); ++itr ) | |

1178 | for( Vertices::const_iterator vitr = itr->vertices.begin(); | |

1179 | vitr != itr->vertices.end(); | |

1180 | ++vitr ) | |

1181 | bb.expandBy( *vitr ); | |

1182 | } | |

1183 | ||

1184 | return bb; | |

1185 | } | |

1186 | ||

1187 | void ConvexPolyhedron::cut(const osg::Polytope & polytope) | |

1188 | { | |

1189 | const char * apc[6] = { "left", "right", "bottom", "top", "near", "far" }; | |

1190 | char ac[16]; | |

1191 | int i = 0; | |

1192 | ||

1193 | for(osg::Polytope::PlaneList::const_iterator itr = polytope.getPlaneList().begin(); | |

1194 | itr != polytope.getPlaneList().end(); | |

1195 | ++itr) | |

1196 | { | |

1197 | const char* arg; | |

1198 | if (i < 6) { | |

1199 | arg = apc[i]; | |

1200 | } else { | |

1201 | sprintf(ac, "%d", i); | |

1202 | arg = ac; | |

1203 | } | |

1204 | cut(*itr, std::string( arg ) ); | |

1205 | ||

1206 | i++; | |

1207 | } | |

1208 | ||

1209 | removeDuplicateVertices(); | |

1210 | } | |

1211 | ||

1212 | void ConvexPolyhedron::cut(const ConvexPolyhedron& polytope) | |

1213 | { | |

1214 | for(Faces::const_iterator itr = polytope._faces.begin(); | |

1215 | itr != polytope._faces.end(); | |

1216 | ++itr) | |

1217 | { | |

1218 | cut(itr->plane, itr->name); | |

1219 | } | |

1220 | ||

1221 | removeDuplicateVertices(); | |

1222 | } | |

1223 | ||

1224 | void ConvexPolyhedron::cut(const osg::Plane& plane, const std::string& name) | |

1225 | { | |

1226 | if( _faces.empty() ) return; | |

1227 | ||

1228 | ConvexPolyhedron cp( *this ); | |

1229 | ||

1230 | typedef std::vector< FaceDistances > FaceDistancesList; | |

1231 | FaceDistancesList faceDistances; | |

1232 | faceDistances.resize( _faces.size() ); | |

1233 | ||

1234 | double min = FLT_MAX, max = -FLT_MAX; //Hull max & min point distances | |

1235 | ||

1236 | FaceDistancesList::iterator fd = faceDistances.begin(); | |

1237 | // First step compute each face points distances to cutting plane | |

1238 | for( Faces::iterator itr = _faces.begin(); | |

1239 | itr != _faces.end(); | |

1240 | ++itr, ++fd ) | |

1241 | { | |

1242 | fd->itr = itr; | |

1243 | fd->distances.reserve( itr->vertices.size() ); | |

1244 | fd->on = 0; | |

1245 | fd->above = 0; | |

1246 | fd->below = 0; | |

1247 | ||

1248 | #if 0 // Skip if cutting plane the same as one of faces | |

1249 | if( plane.ptr()[0] ) == itr->plane.ptr()[0] && | |

1250 | plane.ptr()[1] ) == itr->plane.ptr()[1] && | |

1251 | plane.ptr()[2] ) == itr->plane.ptr()[2] && | |

[8986] | 1252 | #else // check plane using less precise float values |

[8984] | 1253 | if( float( plane.ptr()[0] ) == float( itr->plane.ptr()[0] ) && |

1254 | float( plane.ptr()[1] ) == float( itr->plane.ptr()[1] ) && | |

1255 | float( plane.ptr()[2] ) == float( itr->plane.ptr()[2] ) && | |

1256 | #endif | |

1257 | plane_hull_tolerance >= fabs( float( plane.ptr()[3] )- float( itr->plane.ptr()[3] ) ) ) | |

1258 | return; | |

1259 | ||

1260 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

1261 | vitr != itr->vertices.end(); | |

1262 | ++vitr) | |

1263 | { | |

1264 | double d = plane.distance( *vitr ); | |

1265 | ||

1266 | fd->distances.push_back( d ); | |

1267 | if ( d>point_plane_tolerance ) ++fd->above; | |

1268 | else if ( d<-point_plane_tolerance ) ++fd->below; | |

1269 | else ++fd->on; | |

1270 | min = osg::minimum( min, d ); | |

1271 | max = osg::maximum( max, d ); | |

1272 | } | |

1273 | } | |

1274 | ||

1275 | if( max <= plane_hull_tolerance ) { // All points on or below cutting plane | |

1276 | _faces.clear(); | |

1277 | return; | |

1278 | } | |

1279 | ||

1280 | if( min >= -plane_hull_tolerance ) { // All points on or above cutting plane | |

1281 | return; | |

1282 | } | |

1283 | ||

1284 | typedef std::pair<osg::Vec3d, osg::Vec3d> Edge; | |

1285 | typedef std::set< Edge > Edges; | |

1286 | Edges edges; | |

1287 | ||

1288 | for( FaceDistancesList::iterator fd = faceDistances.begin(); | |

1289 | fd != faceDistances.end(); | |

1290 | ++fd ) | |

1291 | { | |

1292 | if ( fd->below == 0 ) | |

1293 | { // skip face if all points on or above cutting plane ( below == 0 ) | |

1294 | continue; | |

1295 | } | |

1296 | ||

1297 | if ( /* fd->below > 0 && */ fd->above == 0 && fd->on == 0 ) | |

1298 | { | |

1299 | _faces.erase( fd->itr ); // remove face if points below or on | |

1300 | continue; | |

1301 | } | |

1302 | ||

1303 | // cut the face if some points above and below plane | |

1304 | // assert( fd->below > 0 && fd->above > 0 ); | |

1305 | ||

1306 | Face& face = *(fd->itr); | |

1307 | Vertices& vertices = face.vertices; | |

1308 | Distances& distances = fd->distances; | |

1309 | Vertices newFaceVertices; | |

1310 | Vertices newVertices; | |

1311 | ||

1312 | ||

1313 | for(unsigned int i=0; i < vertices.size(); ++i) | |

1314 | { | |

1315 | osg::Vec3d &va = vertices[i]; | |

1316 | osg::Vec3d &vb = vertices[(i+1)%vertices.size()]; | |

1317 | double &distance_a = distances[i]; | |

1318 | double &distance_b = distances[(i+1)%vertices.size()]; | |

1319 | ||

1320 | // Is first edge point above or on the plane? | |

1321 | if ( -point_plane_tolerance <= distance_a ) { | |

1322 | ||

1323 | if( newVertices.empty() || vertices[i] != newVertices.back() ) | |

1324 | newVertices.push_back( vertices[i] ); | |

1325 | ||

1326 | if ( distance_a <= point_plane_tolerance ) { | |

1327 | if( newFaceVertices.empty() || vertices[i] != newFaceVertices.back() ) | |

1328 | newFaceVertices.push_back( vertices[i] ); | |

1329 | } | |

1330 | } | |

1331 | ||

1332 | // Does edge intersect plane ? | |

1333 | if ( ( distance_a < -point_plane_tolerance && distance_b > point_plane_tolerance ) || | |

1334 | ( distance_b < -point_plane_tolerance && distance_a > point_plane_tolerance ) ) | |

1335 | { | |

1336 | osg::Vec3d intersection; // Inserting vertex | |

1337 | double da = fabs( distance_a ), db = fabs( distance_b ); | |

1338 | ||

1339 | // tweaks to improve coherency of polytope after cut | |

1340 | if( da <= point_point_equivalence && da <= db ) { | |

1341 | intersection = va; | |

1342 | } else if( db <= point_point_equivalence && db <= da ) { | |

1343 | intersection = vb; | |

1344 | } else { | |

1345 | double dab4 = 0.25 * ( da + db ); | |

1346 | if( dab4 < da && dab4 < db ) { | |

1347 | intersection = (vb*distance_a - va*distance_b)/(distance_a-distance_b); | |

1348 | } else { | |

1349 | osg::Vec3d v = (vb - va)/(distance_a-distance_b); | |

1350 | if( da < db ) | |

1351 | intersection = va + v * distance_a; | |

1352 | else | |

1353 | intersection = vb + v * distance_b; | |

1354 | } | |

1355 | } | |

1356 | ||

1357 | if( newVertices.empty() || intersection != newVertices.back() ) | |

1358 | newVertices.push_back( intersection ); | |

1359 | ||

1360 | if( newFaceVertices.empty() || intersection != newFaceVertices.back() ) | |

1361 | newFaceVertices.push_back( intersection ); | |

1362 | } | |

1363 | } | |

1364 | ||

1365 | if( newVertices.size() && newVertices.front() == newVertices.back() ) | |

1366 | newVertices.pop_back(); | |

1367 | ||

1368 | if( newFaceVertices.size() && newFaceVertices.front() == newFaceVertices.back() ) | |

1369 | newFaceVertices.pop_back(); | |

1370 | ||

1371 | if( newFaceVertices.size() == 1 ) { // This is very rare but correct | |

1372 | WARN | |

1373 | << "ConvexPolyhedron::cut - Slicing face polygon returns " | |

1374 | << newFaceVertices.size() | |

1375 | << " points. Should be 2 (usually) or 1 (rarely)." | |

1376 | << std::endl; | |

1377 | ||

1378 | } else if( newFaceVertices.size() == 2 ) { | |

1379 | if( newFaceVertices[0] < newFaceVertices[1] ) { | |

1380 | edges.insert( Edge( newFaceVertices[0], newFaceVertices[1] ) ); | |

1381 | } else { | |

1382 | edges.insert( Edge( newFaceVertices[1], newFaceVertices[0] ) ); | |

1383 | } | |

1384 | } else if( newFaceVertices.size() > 2 ) { | |

1385 | ||

1386 | #if CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT | |

1387 | ||

1388 | // This may happen if face polygon is not planar or convex. | |

1389 | // It may happen when face was distorted by projection transform | |

1390 | // or when some polygon vertices land in incorrect positions after | |

1391 | // some transformation by badly conditioned matrix (weak precison) | |

1392 | ||

1393 | WARN | |

1394 | << "ConvexPolyhedron::cut - Slicing face polygon returns " | |

1395 | << newFaceVertices.size() | |

1396 | << " points. Should be 2 (usually) or 1 (rarely)." | |

1397 | << std::endl; | |

1398 | #endif | |

1399 | ||

1400 | #if CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT | |

1401 | dumpGeometry( &face, &plane ); | |

1402 | #endif | |

1403 | ||

1404 | // Let try to recover from this uneasy situation | |

1405 | // by comparing current face polygon edges cut by new plane | |

1406 | // with edges selected for new plane | |

1407 | ||

1408 | unsigned i0 = 0, i1 = newFaceVertices.size() - 1; | |

1409 | unsigned j0 = 0, j1 = newVertices.size() - 1; | |

1410 | ||

1411 | for( ; i0 < newFaceVertices.size(); i1 = i0++, j1 = j0++ ) { | |

1412 | while( newFaceVertices[i0] != newVertices[j0] ) j1 = j0++; | |

1413 | if( newFaceVertices[i1] == newVertices[j1] ) | |

1414 | { | |

1415 | if( newFaceVertices[i0] < newFaceVertices[i1] ) { | |

1416 | edges.insert( Edge( newFaceVertices[i0], newFaceVertices[i1] ) ); | |

1417 | } else { | |

1418 | edges.insert( Edge( newFaceVertices[i1], newFaceVertices[i0] ) ); | |

1419 | } | |

1420 | } | |

1421 | } | |

1422 | } | |

1423 | ||

1424 | if( newVertices.size() >= 3 ) { //Add faces with at least 3 points | |

1425 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

1426 | int convex = isFacePolygonConvex( face ); | |

1427 | vertices.swap( newVertices ); | |

1428 | if( convex && !isFacePolygonConvex( face ) ) { | |

1429 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

1430 | WARN << "ConvexPolyhedron::cut - polygon output non convex." | |

1431 | << " This may lead to other issues in ConvexPolyhedron math" << std::endl; | |

1432 | #endif | |

1433 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

1434 | dumpGeometry( &face, &plane, &cp ); | |

1435 | #endif | |

1436 | } | |

1437 | #else | |

1438 | vertices.swap( newVertices ); | |

1439 | #endif | |

1440 | } else //Remove face reduced to 0, 1, 2 points | |

1441 | _faces.erase( fd->itr ); | |

1442 | } | |

1443 | ||

1444 | if ( edges.size() > 1 ) //Ignore faces reduced to 0, 1, 2 points | |

1445 | { | |

1446 | Face face; | |

1447 | face.name = name; | |

1448 | face.plane = plane; | |

1449 | ||

1450 | std::deque< osg::Vec3d > vertices; | |

1451 | ||

1452 | Edges::iterator itr = edges.begin(); | |

1453 | vertices.push_back( itr->first ); | |

1454 | vertices.push_back( itr->second ); | |

1455 | edges.erase( itr++ ); | |

1456 | ||

1457 | for( unsigned int vertices_size = 0; | |

1458 | vertices_size < vertices.size(); ) | |

1459 | { | |

1460 | vertices_size = vertices.size(); | |

1461 | for( itr = edges.begin(); itr != edges.end(); ) | |

1462 | { | |

1463 | bool not_added = false; | |

1464 | ||

1465 | if( itr->first == vertices.back() ) | |

1466 | vertices.push_back( itr->second ); | |

1467 | else if ( itr->first == vertices.front() ) | |

1468 | vertices.push_front( itr->second ); | |

1469 | else if ( itr->second == vertices.back() ) | |

1470 | vertices.push_back( itr->first ); | |

1471 | else if ( itr->second == vertices.front() ) | |

1472 | vertices.push_front( itr->first ); | |

1473 | else | |

1474 | not_added = true; | |

1475 | ||

1476 | if( not_added ) | |

1477 | ++itr; | |

1478 | else | |

1479 | edges.erase( itr++ ); | |

1480 | } | |

1481 | } | |

1482 | ||

1483 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

1484 | if( !edges.empty() ) { | |

1485 | WARN | |

1486 | << "ConvexPolyhedron::cut - Building new face polygon - " | |

1487 | << "Found edges not matching former polygon ends" | |

1488 | << std::endl; | |

1489 | } | |

1490 | #endif | |

1491 | ||

[9009] | 1492 | std::copy(vertices.begin(), vertices.end(), std::back_inserter(face.vertices)); |

1493 | ||

[8984] | 1494 | _faces.push_back(face); |

1495 | ||

1496 | // Last vertex is duplicated - remove one instance | |

1497 | if( face.vertices.front() == face.vertices.back() ) | |

1498 | face.vertices.pop_back(); | |

1499 | else {// If not duplicated then it may mean we have open polygon ;-( | |

1500 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

1501 | WARN | |

1502 | << "ConvexPolyhedron::cut - Building new face polygon - " | |

1503 | << " Polygon not properly closed." | |

1504 | << std::endl; | |

1505 | #endif | |

1506 | #if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON | |

1507 | dumpGeometry( &_faces.back(), &plane, &cp ); | |

1508 | #endif | |

1509 | } | |

1510 | ||

1511 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

1512 | if( !isFacePolygonConvex( face ) ) { | |

1513 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

1514 | ConvexPolyhedron cp; | |

1515 | cp.createFace() = face; | |

1516 | cp.dumpGeometry( ); | |

1517 | #endif | |

1518 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

1519 | WARN << "ConvexPolyhedron::cut - new face polygon non convex." | |

1520 | << " This may lead to other issues in ConvexPolyhedron math" << std::endl; | |

1521 | #endif | |

1522 | } | |

1523 | #endif | |

1524 | } | |

1525 | ||

1526 | // removeDuplicateVertices( ); | |

1527 | } | |

1528 | //////////////////////////////////////////////////////////////////////////// | |

1529 | void ConvexPolyhedron::extrude( const osg::Vec3d & offset ) | |

1530 | { | |

1531 | if( offset.length2() == 0 ) return; | |

1532 | ||

1533 | typedef std::pair<osg::Vec3d, osg::Vec3d> Edge; | |

1534 | typedef std::vector<Face*> EdgeFaces; | |

1535 | typedef std::map<Edge, EdgeFaces> EdgeMap; | |

1536 | ||

1537 | EdgeMap edgeMap; | |

1538 | ||

1539 | // Build edge maps | |

1540 | for(Faces::iterator itr = _faces.begin(); | |

1541 | itr != _faces.end(); | |

1542 | ++itr) | |

1543 | { | |

1544 | Face& face = *itr; | |

1545 | for(unsigned int i=0; i<face.vertices.size(); ++i) | |

1546 | { | |

1547 | osg::Vec3d& va = face.vertices[i]; | |

1548 | osg::Vec3d& vb = face.vertices[(i+1)%face.vertices.size()]; | |

1549 | if (va < vb) edgeMap[Edge(va,vb)].push_back(&face); | |

1550 | else edgeMap[Edge(vb,va)].push_back(&face); | |

1551 | } | |

1552 | } | |

1553 | ||

1554 | // Offset faces | |

1555 | for(Faces::iterator itr = _faces.begin(); | |

1556 | itr != _faces.end(); | |

1557 | ++itr) | |

1558 | { | |

1559 | Face& face = *itr; | |

1560 | ||

1561 | double dotOffset = face.plane.dotProductNormal( offset ); | |

1562 | ||

1563 | if( dotOffset >= 0 ) continue; | |

1564 | ||

1565 | face.plane.ptr()[3] -= dotOffset; | |

1566 | for(unsigned int i=0; i<face.vertices.size(); ++i) | |

1567 | { | |

1568 | face.vertices[i] += offset; | |

1569 | } | |

1570 | } | |

1571 | ||

1572 | typedef std::set< Edge > SilhouetteEdges; | |

1573 | typedef std::map< Face*, SilhouetteEdges > SilhouetteFaces; | |

1574 | SilhouetteFaces silhouetteFaces; | |

1575 | ||

1576 | int new_face_counter = 0; | |

1577 | ||

1578 | // Now add new faces from slhouette edges extrusion | |

1579 | for(EdgeMap::iterator eitr = edgeMap.begin(); | |

1580 | eitr != edgeMap.end(); | |

1581 | ++eitr) | |

1582 | { | |

1583 | const Edge& edge = eitr->first; | |

1584 | const EdgeFaces& edgeFaces = eitr->second; | |

1585 | ||

1586 | if ( edgeFaces.size()==1 ) | |

1587 | { | |

1588 | // WL: Execution should not reach this line. | |

1589 | // If you got here let me know: lewandowski@ai.com.pl | |

1590 | assert( 0 ); | |

1591 | } | |

1592 | else if ( edgeFaces.size()==2 ) | |

1593 | { | |

1594 | #if 0 // Use float normal computations | |

1595 | osg::Vec3f vf( offset ); | |

1596 | double dotOffset0 = osg::Vec3f( edgeFaces[0]->plane.getNormal() ) * vf; | |

1597 | double dotOffset1 = osg::Vec3f( edgeFaces[1]->plane.getNormal() ) * vf; | |

1598 | #else | |

1599 | double dotOffset0 = edgeFaces[0]->plane.getNormal() * offset; | |

1600 | double dotOffset1 = edgeFaces[1]->plane.getNormal() * offset; | |

1601 | #endif | |

1602 | //Select orthogonal faces and vertices appropriate for offseting | |

[9376] | 1603 | if( (dotOffset0 == 0.0 && dotOffset1 < 0.0) || |

1604 | (dotOffset1 == 0.0 && dotOffset0 < 0.0) ) | |

[8984] | 1605 | { |

1606 | Face * face = ( dotOffset0 == 0 ? edgeFaces[0] : edgeFaces[1] ); | |

1607 | silhouetteFaces[ face ].insert( edge ); | |

1608 | } | |

1609 | ||

[9376] | 1610 | if( (dotOffset0 < 0.0 && dotOffset1 > 0.0) || |

1611 | (dotOffset1 < 0.0 && dotOffset0 > 0.0) ) | |

[8984] | 1612 | { |

1613 | Face & face = createFace(); | |

1614 | char ac[40] = "Side plane from edge extrude "; | |

1615 | sprintf(ac + strlen(ac), "%d", new_face_counter++); | |

1616 | face.name = ac; | |

1617 | ||

1618 | // Compute face plane | |

1619 | face.vertices.push_back( edge.first ); | |

1620 | face.vertices.push_back( edge.second ); | |

1621 | ||

1622 | osg::Vec3d n = face.vertices[0] - face.vertices[1]; | |

1623 | n.normalize(); | |

1624 | n = ( n ^ offset ); | |

1625 | n.normalize(); | |

1626 | ||

1627 | if( n * ( edgeFaces[1]->plane.getNormal() + edgeFaces[0]->plane.getNormal() ) < 0 ) | |

1628 | { | |

1629 | n = -n; | |

1630 | std::swap( face.vertices[1], face.vertices[0] ); | |

1631 | } | |

1632 | ||

1633 | face.vertices.push_back( face.vertices[1] + offset ); | |

1634 | face.vertices.push_back( face.vertices[0] + offset ); | |

1635 | ||

1636 | face.plane.set( n,(face.vertices[0] + face.vertices[1] + | |

1637 | face.vertices[2] + face.vertices[3]) * .25 ); | |

1638 | } | |

1639 | } | |

1640 | else if( edgeFaces.size() > 2 ) | |

1641 | { | |

1642 | assert( 0 ); | |

1643 | // WL: Execution should not reach this line. | |

1644 | // If you got here let me know: lewandowski@ai.com.pl | |

1645 | } | |

1646 | } | |

1647 | ||

1648 | // Finally update faces which are orthogonal to our normal | |

1649 | for(SilhouetteFaces::iterator itr = silhouetteFaces.begin(); | |

1650 | itr != silhouetteFaces.end(); | |

1651 | ++itr) | |

1652 | { | |

1653 | SilhouetteEdges & edges = itr->second; | |

1654 | Vertices & vertices = itr->first->vertices; | |

1655 | Vertices newVertices; | |

1656 | ||

1657 | for( unsigned int i = 0; i < vertices.size(); i++ ) | |

1658 | { | |

1659 | osg::Vec3d | |

1660 | &va = vertices[ ( i + vertices.size() - 1 ) % vertices.size() ], | |

1661 | &vb = vertices[ i ], | |

1662 | &vc = vertices[ ( i + 1 ) % vertices.size() ]; | |

1663 | ||

1664 | Edge eab = va < vb ? Edge( va, vb ) : Edge( vb, va ); | |

1665 | Edge ebc = vb < vc ? Edge( vb, vc ) : Edge( vc, vb ); | |

1666 | ||

1667 | bool abFound = edges.find( eab ) != edges.end(); | |

1668 | bool bcFound = edges.find( ebc ) != edges.end(); | |

1669 | ||

1670 | if( abFound && bcFound ) { | |

1671 | newVertices.push_back( vb + offset ); | |

1672 | } else if( !abFound && !bcFound ) { | |

1673 | newVertices.push_back( vb ); | |

1674 | } else if( !abFound && bcFound ) { | |

1675 | newVertices.push_back( vb ); | |

1676 | newVertices.push_back( vb + offset ); | |

1677 | } else if( abFound && !bcFound ) { | |

1678 | newVertices.push_back( vb + offset ); | |

1679 | newVertices.push_back( vb ); | |

1680 | } | |

1681 | } | |

1682 | ||

1683 | vertices.swap( newVertices ); | |

1684 | } | |

1685 | ||

1686 | removeDuplicateVertices( ); | |

1687 | checkCoherency( true, "ConvexPolyhedron::extrude" ); | |

1688 | } | |

1689 | //////////////////////////////////////////////////////////////////////////// | |

1690 | void ConvexPolyhedron::translate( const osg::Vec3d & offset ) | |

1691 | { | |

1692 | for( Faces::iterator itr = _faces.begin(); itr != _faces.end(); ++itr ) | |

1693 | { | |

1694 | itr->plane.ptr()[3] -= itr->plane.dotProductNormal( offset ); | |

1695 | ||

1696 | for( Vertices::iterator vitr = itr->vertices.begin(); | |

1697 | vitr != itr->vertices.end(); | |

1698 | ++vitr ) | |

1699 | { | |

1700 | *vitr += offset; | |

1701 | } | |

1702 | } | |

1703 | } | |

1704 | //////////////////////////////////////////////////////////////////////////// | |

1705 | void ConvexPolyhedron::getPolytope(osg::Polytope& polytope) const | |

1706 | { | |

1707 | for(Faces::const_iterator itr = _faces.begin(); | |

1708 | itr != _faces.end(); | |

1709 | ++itr) | |

1710 | { | |

1711 | polytope.add(itr->plane); | |

1712 | } | |

1713 | } | |

1714 | //////////////////////////////////////////////////////////////////////////// | |

1715 | void ConvexPolyhedron::getPoints(Vertices& vertices) const | |

1716 | { | |

1717 | typedef std::set<osg::Vec3d> VerticesSet; | |

1718 | VerticesSet verticesSet; | |

1719 | for(Faces::const_iterator itr = _faces.begin(); | |

1720 | itr != _faces.end(); | |

1721 | ++itr) | |

1722 | { | |

1723 | const Face& face = *itr; | |

1724 | for(Vertices::const_iterator vitr = face.vertices.begin(); | |

1725 | vitr != face.vertices.end(); | |

1726 | ++vitr) | |

1727 | { | |

1728 | verticesSet.insert(*vitr); | |

1729 | } | |

1730 | } | |

1731 | ||

1732 | for(VerticesSet::iterator sitr = verticesSet.begin(); | |

1733 | sitr != verticesSet.end(); | |

1734 | ++sitr) | |

1735 | { | |

1736 | vertices.push_back(*sitr); | |

1737 | } | |

1738 | } | |

1739 | //////////////////////////////////////////////////////////////////////////// | |

1740 | osg::Geometry* ConvexPolyhedron::buildGeometry( const osg::Vec4d& colorOutline, | |

1741 | const osg::Vec4d& colorInside, | |

1742 | osg::Geometry* geometry ) const | |

1743 | { | |

1744 | if( !geometry ) { | |

1745 | geometry = new osg::Geometry; | |

1746 | } else { | |

1747 | geometry->getPrimitiveSetList( ).clear(); | |

1748 | } | |

1749 | ||

1750 | osg::Vec3dArray* vertices = new osg::Vec3dArray; | |

1751 | geometry->setVertexArray(vertices); | |

1752 | ||

1753 | osg::Vec4Array* colors = new osg::Vec4Array; | |

1754 | geometry->setColorArray(colors); | |

1755 | geometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE); | |

1756 | ||

1757 | for(Faces::const_iterator itr = _faces.begin(); | |

1758 | itr != _faces.end(); | |

1759 | ++itr) | |

1760 | { | |

1761 | if( colorInside[3] > 0 ) { | |

1762 | geometry->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, | |

1763 | vertices->size(), itr->vertices.size() ) ); | |

1764 | ||

1765 | colors->push_back( colorInside ); | |

1766 | } | |

1767 | ||

1768 | if( colorOutline[3] > 0 ) { | |

1769 | geometry->addPrimitiveSet( new osg::DrawArrays( GL_LINE_LOOP, | |

1770 | vertices->size(), itr->vertices.size() ) ); | |

1771 | ||

1772 | colors->push_back( colorOutline ); | |

1773 | } | |

1774 | ||

1775 | vertices->insert | |

1776 | ( vertices->end(), itr->vertices.begin(), itr->vertices.end() ); | |

1777 | } | |

1778 | ||

1779 | osg::StateSet* stateset = geometry->getOrCreateStateSet(); | |

1780 | stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); | |

1781 | stateset->setTextureMode(0, GL_TEXTURE_2D, osg::StateAttribute::OFF); | |

1782 | stateset->setTextureMode(1, GL_TEXTURE_2D, osg::StateAttribute::OFF); | |

1783 | ||

1784 | return geometry; | |

1785 | } | |

1786 | //////////////////////////////////////////////////////////////////////////// | |

1787 | bool ConvexPolyhedron::dumpGeometry | |

1788 | ( const Face * face, | |

1789 | const osg::Plane * plane, | |

1790 | ConvexPolyhedron * base, | |

1791 | const char * filename, | |

1792 | const osg::Vec4d& colorOutline, | |

1793 | const osg::Vec4d& colorInside, | |

1794 | const osg::Vec4d& faceColorOutline, | |

1795 | const osg::Vec4d& faceColorInside, | |

1796 | const osg::Vec4d& planeColorOutline, | |

1797 | const osg::Vec4d& planeColorInside, | |

1798 | const osg::Vec4d& baseColorOutline, | |

1799 | const osg::Vec4d& baseColorInside ) const | |

1800 | { | |

1801 | osg::Group * group = new osg::Group(); | |

1802 | osg::Geode * geode = new osg::Geode(); | |

1803 | geode->getOrCreateStateSet()->setMode( GL_BLEND, osg::StateAttribute::ON ); | |

1804 | geode->getOrCreateStateSet()->setMode( GL_CULL_FACE, osg::StateAttribute::OFF ); | |

1805 | geode->getOrCreateStateSet()->setMode( GL_DEPTH_TEST, osg::StateAttribute::OFF ); | |

1806 | ||

1807 | group->addChild( geode ); | |

1808 | ||

1809 | Vertices vertices; | |

1810 | getPoints( vertices ); | |

1811 | ||

1812 | osg::BoundingBox bb; | |

1813 | for( unsigned int i = 0; i < vertices.size(); i++ ) | |

1814 | bb.expandBy( vertices[i] ); | |

1815 | ||

1816 | ConvexPolyhedron cp( *this ), cpFace; | |

1817 | ||

1818 | for( Faces::iterator itr = cp._faces.begin(); itr != cp._faces.end(); ) | |

1819 | { | |

1820 | bool found = ( face && | |

1821 | itr->name == face->name && | |

1822 | itr->plane == face->plane && | |

1823 | itr->vertices == face->vertices ); | |

1824 | #if 1 | |

1825 | if( cp.isFacePolygonConvex( *itr ) < 0 ) | |

1826 | std::reverse( itr->vertices.begin(), itr->vertices.end() ); | |

1827 | #endif | |

1828 | ||

1829 | if( found ) { | |

1830 | cpFace.createFace() = *face; | |

1831 | itr = cp._faces.erase( itr ); | |

1832 | } else { | |

1833 | ++itr; | |

1834 | } | |

1835 | } | |

1836 | ||

1837 | osg::Geometry * geometry = cp.buildGeometry( colorOutline, colorInside ); | |

1838 | geometry->getOrCreateStateSet()->setMode( GL_CULL_FACE, osg::StateAttribute::ON ); | |

1839 | ||

1840 | geode->addDrawable( geometry ); | |

1841 | ||

1842 | if( face ) | |

1843 | geode->addDrawable( cpFace.buildGeometry( faceColorOutline, faceColorInside ) ); | |

1844 | ||

1845 | if( plane ) | |

1846 | { | |

1847 | ConvexPolyhedron cp; | |

1848 | Face & face = cp.createFace(); | |

1849 | face.plane = *plane; | |

1850 | ||

1851 | osg::Vec3d normal = face.plane.getNormal(); | |

1852 | osg::Vec3d side = fabs(normal.x()) < fabs(normal.y()) ? | |

1853 | osg::Vec3d(1.0, 0.0, 0.0) : | |

1854 | osg::Vec3d(0.0, 1.0, 0.0); | |

1855 | ||

1856 | osg::Vec3d v = normal ^ side; | |

1857 | v.normalize(); | |

1858 | v *= bb.radius(); | |

1859 | ||

1860 | osg::Vec3d u = v ^ normal; | |

1861 | u.normalize(); | |

1862 | u *= bb.radius(); | |

1863 | ||

1864 | osg::Vec3d c = bb.center(); | |

1865 | c -= face.plane.getNormal() * face.plane.distance( c ); | |

1866 | ||

1867 | face.vertices.push_back( c - u - v ); | |

1868 | face.vertices.push_back( c - u + v ); | |

1869 | face.vertices.push_back( c + u + v ); | |

1870 | face.vertices.push_back( c + u - v ); | |

1871 | ||

1872 | geode->addDrawable( cp.buildGeometry( planeColorOutline, planeColorInside ) ); | |

1873 | } | |

1874 | ||

1875 | if( base ) | |

1876 | geode->addDrawable( base->buildGeometry( baseColorOutline, baseColorInside ) ); | |

1877 | ||

1878 | return osgDB::writeNodeFile( *group, std::string( filename ) ); | |

1879 | } | |

1880 | //////////////////////////////////////////////////////////////////////////// |

**Note:**See TracBrowser for help on using the browser.