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

Revision 10764, 60.9 kB (checked in by robert, 5 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> | |

[10764] | 25 | #include<iterator> |

[9009] | 26 | |

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

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

[9009] | 29 | |

[8984] | 30 | using namespace osgShadow; |

31 | ||

32 | ||

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

34 | ||

35 | // #define MAKE_CHECKS 0 | |

36 | ||

37 | #endif | |

38 | ||

39 | #if MAKE_CHECKS | |

40 | ||

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

42 | { | |

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

44 | } | |

45 | #define WARN DEBUG_NOTIFY() | |

46 | ||

47 | #define CONVEX_POLYHEDRON_CHECK_COHERENCY 1 | |

48 | #define CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA 2 | |

49 | #define CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA 2 | |

50 | #define CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON 1 | |

51 | #define CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON 1 | |

52 | #define CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON 1 | |

53 | #define CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON 1 | |

54 | #define CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT 1 | |

55 | #define CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT 1 | |

56 | ||

57 | ||

58 | #else | |

59 | ||

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

61 | ||

62 | #define CONVEX_POLYHEDRON_CHECK_COHERENCY 0 | |

63 | #define CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA 0 | |

64 | #define CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA 0 | |

65 | #define CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON 0 | |

66 | #define CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON 0 | |

67 | #define CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON 0 | |

68 | #define CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON 0 | |

69 | #define CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT 0 | |

70 | #define CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT 0 | |

71 | ||

72 | #endif | |

73 | ||

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

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

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

77 | static const double point_plane_tolerance = 0; | |

78 | static const double point_point_tolerance = 0; | |

79 | static const double point_point_equivalence = 0; | |

80 | ||

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

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

83 | namespace | |

84 | { | |

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

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

87 | ||

88 | // Auxilliary params contined per face | |

89 | struct FaceDistances | |

90 | { | |

91 | ConvexPolyhedron::Faces::iterator itr; | |

92 | Points points; | |

93 | Distances distances; | |

94 | int below, above, on; | |

95 | }; | |

96 | } // namespace | |

97 | ||

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

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

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

101 | { | |

102 | setToBoundingBox( bb ); | |

103 | ||

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

105 | transform( matrix, inverse ); | |

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

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

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

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

110 | } | |

111 | ||

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

113 | { | |

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

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

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

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

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

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

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

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

122 | ||

123 | _faces.clear(); | |

124 | ||

125 | { // left plane. | |

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

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

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

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

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

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

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

133 | } | |

134 | ||

135 | { // right plane. | |

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

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

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

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

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

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

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

143 | } | |

144 | ||

145 | { // bottom plane. | |

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

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

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

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

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

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

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

153 | } | |

154 | ||

155 | { // top plane. | |

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

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

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

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

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

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

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

163 | } | |

164 | ||

165 | if (withNear) | |

166 | { // near plane | |

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

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

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

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

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

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

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

174 | } | |

175 | ||

176 | if (withFar) | |

177 | { // far plane | |

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

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

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

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

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

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

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

185 | } | |

186 | } | |

187 | ||

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

189 | { | |

190 | _faces.clear(); | |

191 | ||

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

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

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

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

196 | ||

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

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

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

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

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

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

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

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

205 | ||

206 | ||

207 | ||

208 | { // x min plane | |

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

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

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

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

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

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

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

216 | } | |

217 | ||

218 | { // x max plane. | |

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

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

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

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

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

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

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

226 | } | |

227 | ||

228 | { // y min plane. | |

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

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

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

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

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

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

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

236 | } | |

237 | ||

238 | { // y max plane. | |

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

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

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

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

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

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

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

246 | } | |

247 | { // z min plane | |

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

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

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

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

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

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

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

255 | } | |

256 | ||

257 | { // z max plane | |

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

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

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

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

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

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

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

265 | } | |

266 | } | |

267 | ||

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

269 | { | |

270 | bool requires_infinite_plane_clip = false; | |

271 | ||

272 | ConvexPolyhedron cp = *this; | |

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

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

275 | ++itr) | |

276 | { | |

277 | Face& face = *itr; | |

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

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

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

281 | ++vitr) | |

282 | { | |

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

284 | v = v * matrix; | |

285 | ||

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

287 | requires_infinite_plane_clip = true; | |

288 | break; | |

289 | } | |

290 | ||

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

292 | } | |

293 | } | |

294 | ||

295 | if( requires_infinite_plane_clip ) { | |

296 | *this = cp; | |

297 | transformClip( matrix, inverse ); | |

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

299 | } | |

300 | ||

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

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

303 | ||

304 | removeDuplicateVertices( ); | |

305 | ||

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

307 | } | |

308 | ||

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

310 | { | |

311 | double tolerance = 0.00001; | |

312 | ||

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

314 | ||

315 | ||

316 | ConvexPolyhedron cp( *this ); | |

317 | ||

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

319 | FaceDistancesList faceDistances; | |

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

321 | ||

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

323 | ||

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

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

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

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

328 | ++itr, ++fd ) | |

329 | { | |

330 | fd->itr = itr; | |

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

332 | fd->on = 0; | |

333 | fd->above = 0; | |

334 | fd->below = 0; | |

335 | ||

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

337 | ||

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

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

340 | ++vitr) | |

341 | { | |

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

343 | p = p * matrix; | |

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

345 | ||

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

347 | ||

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

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

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

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

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

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

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

355 | } | |

356 | } | |

357 | ||

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

359 | _faces.clear(); | |

360 | return; | |

361 | } | |

362 | ||

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

364 | return; | |

365 | } | |

366 | ||

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

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

369 | Edges edges; | |

370 | ||

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

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

373 | ++fd ) | |

374 | { | |

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

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

377 | continue; | |

378 | } | |

379 | ||

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

381 | { | |

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

383 | continue; | |

384 | } | |

385 | ||

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

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

388 | ||

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

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

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

392 | Vertices newFaceVertices; | |

393 | Vertices newVertices; | |

394 | ||

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

396 | { | |

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

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

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

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

401 | ||

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

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

404 | ||

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

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

407 | ||

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

409 | newVertices.push_back( v ); | |

410 | ||

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

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

413 | newFaceVertices.push_back( v ); | |

414 | } | |

415 | } | |

416 | ||

417 | // Does edge intersect plane ? | |

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

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

420 | { | |

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

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

423 | ||

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

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

426 | intersection = va; | |

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

428 | intersection = vb; | |

429 | } else { | |

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

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

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

433 | } else { | |

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

435 | if( da < db ) | |

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

437 | else | |

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

439 | } | |

440 | } | |

441 | ||

442 | ||

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

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

445 | ||

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

447 | newVertices.push_back( v ); | |

448 | ||

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

450 | newFaceVertices.push_back( v ); | |

451 | } | |

452 | } | |

453 | ||

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

455 | newVertices.pop_back(); | |

456 | ||

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

458 | newFaceVertices.pop_back(); | |

459 | ||

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

461 | WARN | |

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

463 | << newFaceVertices.size() | |

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

465 | << std::endl; | |

466 | ||

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

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

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

470 | } else { | |

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

472 | } | |

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

474 | ||

475 | #if CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT | |

476 | ||

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

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

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

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

481 | ||

482 | WARN | |

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

484 | << newFaceVertices.size() | |

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

486 | << std::endl; | |

487 | #endif | |

488 | ||

489 | #if CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT | |

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

491 | #endif | |

492 | ||

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

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

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

496 | ||

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

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

499 | ||

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

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

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

503 | { | |

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

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

506 | } else { | |

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

508 | } | |

509 | } | |

510 | } | |

511 | } | |

512 | ||

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

514 | ||

515 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

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

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

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

519 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

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

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

522 | #endif | |

523 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

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

525 | #endif | |

526 | } | |

527 | #else | |

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

529 | #endif | |

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

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

532 | } | |

533 | ||

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

535 | unsigned count = 0; | |

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

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

538 | ++itr ) | |

539 | { | |

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

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

542 | ++vitr ) | |

543 | { | |

544 | center += *vitr; | |

545 | count ++; | |

546 | } | |

547 | } | |

548 | ||

549 | center /= count; | |

550 | ||

551 | ||

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

553 | { | |

554 | Face face; | |

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

556 | ||

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

558 | ||

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

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

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

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

563 | ||

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

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

566 | { | |

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

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

569 | { | |

570 | bool not_added = false; | |

571 | ||

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

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

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

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

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

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

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

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

580 | else | |

581 | not_added = true; | |

582 | ||

583 | if( not_added ) | |

584 | ++itr; | |

585 | else | |

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

587 | } | |

588 | } | |

589 | ||

590 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

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

592 | WARN | |

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

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

595 | << std::endl; | |

596 | } | |

597 | #endif | |

598 | ||

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

[8984] | 600 | |

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

602 | ||

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

604 | ||

605 | _faces.push_back(face); | |

606 | ||

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

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

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

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

611 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

612 | WARN | |

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

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

615 | << std::endl; | |

616 | #endif | |

617 | #if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON | |

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

619 | #endif | |

620 | } | |

621 | ||

622 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

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

624 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

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

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

627 | #endif | |

628 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

629 | ConvexPolyhedron cp; | |

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

631 | cp.dumpGeometry( ); | |

632 | #endif | |

633 | } | |

634 | #endif | |

635 | } | |

636 | ||

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

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

639 | ||

640 | removeDuplicateVertices( ); | |

641 | ||

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

643 | } | |

644 | ||

645 | bool ConvexPolyhedron::mergeFaces | |

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

647 | { | |

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

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

650 | Edges edges; | |

651 | ||

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

653 | { | |

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

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

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

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

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

659 | } | |

660 | ||

661 | bool intersection = false; | |

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

663 | { | |

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

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

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

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

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

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

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

671 | edges.insert( edge ); | |

672 | else { | |

673 | edges.erase( itr ); | |

674 | intersection = true; | |

675 | } | |

676 | } | |

677 | ||

678 | if( intersection ) | |

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

680 | ||

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

682 | { | |

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

684 | ||

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

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

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

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

689 | ||

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

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

692 | { | |

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

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

695 | { | |

696 | bool not_added = false; | |

697 | ||

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

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

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

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

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

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

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

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

706 | else | |

707 | not_added = true; | |

708 | ||

709 | if( not_added ) | |

710 | ++itr; | |

711 | else | |

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

713 | } | |

714 | } | |

715 | ||

716 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

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

718 | WARN | |

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

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

721 | << std::endl; | |

722 | } | |

723 | #endif | |

724 | ||

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

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

727 | vertices.pop_back(); | |

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

729 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

730 | WARN | |

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

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

733 | << std::endl; | |

734 | #endif | |

735 | #if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON | |

736 | #endif | |

737 | } | |

738 | ||

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

740 | face.plane = face0.plane; | |

[9009] | 741 | |

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

743 | ||

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

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

746 | normal.normalize(); | |

747 | ||

748 | osg::Vec3d center; | |

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

750 | { | |

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

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

753 | } | |

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

755 | ||

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

757 | #endif | |

758 | ||

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

760 | ||

761 | // No testing for concave polys | |

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

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

764 | } | |

765 | return intersection; | |

766 | } | |

767 | ||

768 | void ConvexPolyhedron::mergeCoplanarFaces | |

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

770 | { | |

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

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

773 | ++itr0 ) | |

774 | { | |

775 | double tolerance = delta_tolerance; | |

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

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

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

779 | } | |

780 | ||

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

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

783 | ) | |

784 | { | |

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

786 | ++itr1; | |

787 | continue; | |

788 | } | |

789 | ||

790 | bool attempt_merge = true; | |

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

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

793 | { | |

794 | attempt_merge = false; | |

795 | break; | |

796 | } | |

797 | } | |

798 | ||

799 | if( !attempt_merge && | |

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

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

802 | attempt_merge = true; | |

803 | ||

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

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

806 | else | |

807 | ++itr1; | |

808 | } | |

809 | } | |

810 | } | |

811 | ||

812 | void ConvexPolyhedron::removeDuplicateVertices( void ) | |

813 | { | |

814 | #if 1 | |

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

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

817 | ||

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

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

820 | ||

821 | VertexSet vertexSet; | |

822 | Points points; | |

823 | ||

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

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

826 | ++itr ) | |

827 | { | |

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

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

830 | ++vitr ) | |

831 | { | |

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

833 | } | |

834 | } | |

835 | ||

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

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

838 | ++vitr ) | |

839 | { | |

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

841 | } | |

842 | ||

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

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

845 | ++itr ) | |

846 | { | |

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

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

849 | } | |

850 | ||

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

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

853 | ++itr ) | |

854 | { | |

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

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

857 | ++vitr ) | |

858 | { | |

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

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

861 | } | |

862 | } | |

863 | #endif | |

864 | ||

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

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

867 | ) | |

868 | { | |

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

870 | ||

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

872 | ||

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

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

875 | ) | |

876 | { | |

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

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

879 | } else { | |

880 | prev = *vitr; | |

881 | ++vitr; | |

882 | } | |

883 | } | |

884 | ||

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

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

887 | else | |

888 | ++itr; | |

889 | } | |

890 | ||

891 | mergeCoplanarFaces(); | |

892 | ||

893 | #if 1 | |

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

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

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

897 | VertexCounter vertexCounter; | |

898 | ||

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

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

901 | ++itr) | |

902 | { | |

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

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

905 | ++vitr ) | |

906 | { | |

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

908 | } | |

909 | } | |

910 | ||

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

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

913 | ) | |

914 | { | |

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

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

917 | ) | |

918 | { | |

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

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

921 | ||

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

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

924 | vitr + 1 ); | |

925 | ||

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

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

928 | vitr - 1 ); | |

929 | ||

930 | ||

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

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

933 | ||

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

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

936 | ||

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

938 | { | |

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

940 | if( dot < limit ) { | |

941 | WARN << "ConvexPolyhedron::removeDuplicateVertices" | |

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

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

944 | << " Length1: " << da | |

945 | << " Length2: " << db | |

946 | << std::endl; | |

947 | } | |

948 | } else { | |

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

950 | WARN << "ConvexPolyhedron::removeDuplicateVertices" | |

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

952 | << std::endl; | |

953 | ||

954 | } | |

955 | #endif | |

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

957 | } else { | |

958 | ++vitr; | |

959 | } | |

960 | } | |

961 | ||

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

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

964 | else | |

965 | ++itr; | |

966 | } | |

967 | #endif | |

968 | ||

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

970 | } | |

971 | ||

972 | int ConvexPolyhedron::pointsColinear | |

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

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

975 | { | |

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

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

978 | ||

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

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

981 | ||

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

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

984 | ||

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

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

987 | ||

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

989 | } | |

990 | ||

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

992 | { | |

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

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

995 | { | |

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

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

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

999 | ||

1000 | ||

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

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

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

1004 | { | |

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

1006 | } | |

1007 | #endif | |

1008 | ||

1009 | // cast points on a plane | |

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

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

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

1013 | ||

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

1015 | colinear++; | |

1016 | } else { | |

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

1018 | ||

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

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

1021 | } | |

1022 | } | |

1023 | ||

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

1025 | return 0; | |

1026 | ||

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

1028 | return 0; | |

1029 | ||

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

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

1032 | ||

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

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

1035 | ||

1036 | return 0; | |

1037 | } | |

1038 | ||

1039 | bool ConvexPolyhedron::checkCoherency | |

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

1041 | { | |

1042 | bool result = true; | |

1043 | bool convex = true; | |

1044 | ||

1045 | #if CONVEX_POLYHEDRON_CHECK_COHERENCY | |

1046 | ||

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

1048 | ||

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

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

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

1052 | ||

1053 | EdgeCounter edgeCounter; | |

1054 | VertexCounter vertexCounter; | |

1055 | ||

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

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

1058 | ++itr) | |

1059 | { | |

1060 | Face& face = *itr; | |

1061 | ||

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

1063 | convex = false; | |

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

1065 | WARN << errorPrefix << | |

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

1067 | #endif | |

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

1069 | dumpGeometry( &face ); | |

1070 | #endif | |

1071 | } | |

1072 | ||

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

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

1075 | { | |

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

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

1078 | ++vertexCounter[ a ]; | |

1079 | if (a<b) | |

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

1081 | else | |

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

1083 | } | |

1084 | } | |

1085 | ||

1086 | ||

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

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

1089 | ++itr) | |

1090 | { | |

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

1092 | ||

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

1094 | result = false; | |

1095 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1096 | WARN << errorPrefix << | |

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

1098 | #endif | |

1099 | } | |

1100 | ||

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

1102 | result = false; | |

1103 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1104 | WARN << errorPrefix << | |

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

1106 | #endif | |

1107 | } | |

1108 | ||

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

1110 | result = false; | |

1111 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1112 | WARN << errorPrefix << | |

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

1114 | #endif | |

1115 | } | |

1116 | ||

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

1118 | result = false; | |

1119 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1120 | WARN << errorPrefix << | |

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

1122 | #endif | |

1123 | } | |

1124 | ||

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

1126 | result = false; | |

1127 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1128 | WARN << errorPrefix << | |

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

1130 | #endif | |

1131 | } | |

1132 | ||

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

1134 | result = false; | |

1135 | #if ( 1 < CONVEX_POLYHEDRON_WARN_ON_INCOHERENT_DATA ) | |

1136 | WARN << errorPrefix << | |

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

1138 | #endif | |

1139 | } | |

1140 | } | |

1141 | ||

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

1143 | if( !convex ) | |

1144 | WARN << errorPrefix | |

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

1146 | ||

1147 | if ( !result ) | |

1148 | WARN << errorPrefix | |

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

1150 | #endif | |

1151 | ||

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

1153 | if( !result || !convex ) | |

1154 | dumpGeometry( ); | |

1155 | #endif | |

1156 | ||

1157 | #if ( 1 < CONVEX_POLYHEDRON_DUMP_ON_INCOHERENT_DATA ) | |

1158 | if( !result ) | |

1159 | dumpGeometry( ); | |

1160 | #endif | |

1161 | ||

1162 | ||

1163 | #endif // CONVEX_POLYHEDRON_CHECK_COHERENCY | |

1164 | return result && convex; | |

1165 | } | |

1166 | ||

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

1168 | { | |

1169 | osg::BoundingBox bb; | |

1170 | ||

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

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

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

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

1175 | ++vitr ) | |

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

1177 | } else { | |

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

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

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

1181 | ++vitr ) | |

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

1183 | } | |

1184 | ||

1185 | return bb; | |

1186 | } | |

1187 | ||

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

1189 | { | |

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

1191 | char ac[16]; | |

1192 | int i = 0; | |

1193 | ||

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

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

1196 | ++itr) | |

1197 | { | |

1198 | const char* arg; | |

1199 | if (i < 6) { | |

1200 | arg = apc[i]; | |

1201 | } else { | |

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

1203 | arg = ac; | |

1204 | } | |

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

1206 | ||

1207 | i++; | |

1208 | } | |

1209 | ||

1210 | removeDuplicateVertices(); | |

1211 | } | |

1212 | ||

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

1214 | { | |

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

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

1217 | ++itr) | |

1218 | { | |

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

1220 | } | |

1221 | ||

1222 | removeDuplicateVertices(); | |

1223 | } | |

1224 | ||

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

1226 | { | |

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

1228 | ||

1229 | ConvexPolyhedron cp( *this ); | |

1230 | ||

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

1232 | FaceDistancesList faceDistances; | |

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

1234 | ||

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

1236 | ||

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

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

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

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

1241 | ++itr, ++fd ) | |

1242 | { | |

1243 | fd->itr = itr; | |

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

1245 | fd->on = 0; | |

1246 | fd->above = 0; | |

1247 | fd->below = 0; | |

1248 | ||

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

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

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

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

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

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

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

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

1257 | #endif | |

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

1259 | return; | |

1260 | ||

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

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

1263 | ++vitr) | |

1264 | { | |

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

1266 | ||

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

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

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

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

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

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

1273 | } | |

1274 | } | |

1275 | ||

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

1277 | _faces.clear(); | |

1278 | return; | |

1279 | } | |

1280 | ||

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

1282 | return; | |

1283 | } | |

1284 | ||

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

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

1287 | Edges edges; | |

1288 | ||

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

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

1291 | ++fd ) | |

1292 | { | |

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

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

1295 | continue; | |

1296 | } | |

1297 | ||

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

1299 | { | |

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

1301 | continue; | |

1302 | } | |

1303 | ||

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

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

1306 | ||

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

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

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

1310 | Vertices newFaceVertices; | |

1311 | Vertices newVertices; | |

1312 | ||

1313 | ||

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

1315 | { | |

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

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

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

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

1320 | ||

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

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

1323 | ||

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

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

1326 | ||

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

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

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

1330 | } | |

1331 | } | |

1332 | ||

1333 | // Does edge intersect plane ? | |

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

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

1336 | { | |

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

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

1339 | ||

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

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

1342 | intersection = va; | |

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

1344 | intersection = vb; | |

1345 | } else { | |

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

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

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

1349 | } else { | |

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

1351 | if( da < db ) | |

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

1353 | else | |

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

1355 | } | |

1356 | } | |

1357 | ||

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

1359 | newVertices.push_back( intersection ); | |

1360 | ||

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

1362 | newFaceVertices.push_back( intersection ); | |

1363 | } | |

1364 | } | |

1365 | ||

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

1367 | newVertices.pop_back(); | |

1368 | ||

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

1370 | newFaceVertices.pop_back(); | |

1371 | ||

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

1373 | WARN | |

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

1375 | << newFaceVertices.size() | |

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

1377 | << std::endl; | |

1378 | ||

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

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

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

1382 | } else { | |

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

1384 | } | |

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

1386 | ||

1387 | #if CONVEX_POLYHEDRON_WARN_ON_INCORRECT_FACE_CUT | |

1388 | ||

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

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

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

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

1393 | ||

1394 | WARN | |

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

1396 | << newFaceVertices.size() | |

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

1398 | << std::endl; | |

1399 | #endif | |

1400 | ||

1401 | #if CONVEX_POLYHEDRON_DUMP_ON_INCORRECT_FACE_CUT | |

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

1403 | #endif | |

1404 | ||

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

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

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

1408 | ||

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

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

1411 | ||

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

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

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

1415 | { | |

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

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

1418 | } else { | |

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

1420 | } | |

1421 | } | |

1422 | } | |

1423 | } | |

1424 | ||

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

1426 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

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

1428 | vertices.swap( newVertices ); | |

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

1430 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

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

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

1433 | #endif | |

1434 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

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

1436 | #endif | |

1437 | } | |

1438 | #else | |

1439 | vertices.swap( newVertices ); | |

1440 | #endif | |

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

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

1443 | } | |

1444 | ||

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

1446 | { | |

1447 | Face face; | |

1448 | face.name = name; | |

1449 | face.plane = plane; | |

1450 | ||

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

1452 | ||

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

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

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

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

1457 | ||

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

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

1460 | { | |

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

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

1463 | { | |

1464 | bool not_added = false; | |

1465 | ||

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

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

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

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

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

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

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

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

1474 | else | |

1475 | not_added = true; | |

1476 | ||

1477 | if( not_added ) | |

1478 | ++itr; | |

1479 | else | |

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

1481 | } | |

1482 | } | |

1483 | ||

1484 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

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

1486 | WARN | |

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

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

1489 | << std::endl; | |

1490 | } | |

1491 | #endif | |

1492 | ||

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

1494 | ||

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

1496 | ||

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

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

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

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

1501 | #if CONVEX_POLYHEDRON_WARN_ON_BAD_POLYGON | |

1502 | WARN | |

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

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

1505 | << std::endl; | |

1506 | #endif | |

1507 | #if CONVEX_POLYHEDRON_DUMP_ON_BAD_POLYGON | |

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

1509 | #endif | |

1510 | } | |

1511 | ||

1512 | #if ( CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON || CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON ) | |

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

1514 | #if CONVEX_POLYHEDRON_DUMP_ON_CONCAVE_POLYGON | |

1515 | ConvexPolyhedron cp; | |

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

1517 | cp.dumpGeometry( ); | |

1518 | #endif | |

1519 | #if CONVEX_POLYHEDRON_WARN_ON_CONCAVE_POLYGON | |

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

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

1522 | #endif | |

1523 | } | |

1524 | #endif | |

1525 | } | |

1526 | ||

1527 | // removeDuplicateVertices( ); | |

1528 | } | |

1529 | //////////////////////////////////////////////////////////////////////////// | |

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

1531 | { | |

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

1533 | ||

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

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

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

1537 | ||

1538 | EdgeMap edgeMap; | |

1539 | ||

1540 | // Build edge maps | |

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

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

1543 | ++itr) | |

1544 | { | |

1545 | Face& face = *itr; | |

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

1547 | { | |

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

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

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

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

1552 | } | |

1553 | } | |

1554 | ||

1555 | // Offset faces | |

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

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

1558 | ++itr) | |

1559 | { | |

1560 | Face& face = *itr; | |

1561 | ||

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

1563 | ||

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

1565 | ||

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

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

1568 | { | |

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

1570 | } | |

1571 | } | |

1572 | ||

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

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

1575 | SilhouetteFaces silhouetteFaces; | |

1576 | ||

1577 | int new_face_counter = 0; | |

1578 | ||

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

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

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

1582 | ++eitr) | |

1583 | { | |

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

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

1586 | ||

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

1588 | { | |

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

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

1591 | assert( 0 ); | |

1592 | } | |

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

1594 | { | |

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

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

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

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

1599 | #else | |

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

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

1602 | #endif | |

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

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

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

[8984] | 1606 | { |

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

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

1609 | } | |

1610 | ||

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

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

[8984] | 1613 | { |

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

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

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

1617 | face.name = ac; | |

1618 | ||

1619 | // Compute face plane | |

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

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

1622 | ||

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

1624 | n.normalize(); | |

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

1626 | n.normalize(); | |

1627 | ||

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

1629 | { | |

1630 | n = -n; | |

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

1632 | } | |

1633 | ||

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

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

1636 | ||

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

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

1639 | } | |

1640 | } | |

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

1642 | { | |

1643 | assert( 0 ); | |

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

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

1646 | } | |

1647 | } | |

1648 | ||

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

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

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

1652 | ++itr) | |

1653 | { | |

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

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

1656 | Vertices newVertices; | |

1657 | ||

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

1659 | { | |

1660 | osg::Vec3d | |

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

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

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

1664 | ||

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

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

1667 | ||

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

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

1670 | ||

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

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

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

1674 | newVertices.push_back( vb ); | |

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

1676 | newVertices.push_back( vb ); | |

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

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

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

1680 | newVertices.push_back( vb ); | |

1681 | } | |

1682 | } | |

1683 | ||

1684 | vertices.swap( newVertices ); | |

1685 | } | |

1686 | ||

1687 | removeDuplicateVertices( ); | |

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

1689 | } | |

1690 | //////////////////////////////////////////////////////////////////////////// | |

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

1692 | { | |

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

1694 | { | |

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

1696 | ||

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

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

1699 | ++vitr ) | |

1700 | { | |

1701 | *vitr += offset; | |

1702 | } | |

1703 | } | |

1704 | } | |

1705 | //////////////////////////////////////////////////////////////////////////// | |

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

1707 | { | |

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

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

1710 | ++itr) | |

1711 | { | |

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

1713 | } | |

1714 | } | |

1715 | //////////////////////////////////////////////////////////////////////////// | |

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

1717 | { | |

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

1719 | VerticesSet verticesSet; | |

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

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

1722 | ++itr) | |

1723 | { | |

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

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

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

1727 | ++vitr) | |

1728 | { | |

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

1730 | } | |

1731 | } | |

1732 | ||

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

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

1735 | ++sitr) | |

1736 | { | |

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

1738 | } | |

1739 | } | |

1740 | //////////////////////////////////////////////////////////////////////////// | |

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

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

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

1744 | { | |

1745 | if( !geometry ) { | |

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

1747 | } else { | |

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

1749 | } | |

1750 | ||

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

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

1753 | ||

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

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

1756 | geometry->setColorBinding(osg::Geometry::BIND_PER_PRIMITIVE); | |

1757 | ||

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

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

1760 | ++itr) | |

1761 | { | |

1762 | if( colorInside[3] > 0 ) { | |

1763 | geometry->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, | |

1764 | vertices->size(), itr->vertices.size() ) ); | |

1765 | ||

1766 | colors->push_back( colorInside ); | |

1767 | } | |

1768 | ||

1769 | if( colorOutline[3] > 0 ) { | |

1770 | geometry->addPrimitiveSet( new osg::DrawArrays( GL_LINE_LOOP, | |

1771 | vertices->size(), itr->vertices.size() ) ); | |

1772 | ||

1773 | colors->push_back( colorOutline ); | |

1774 | } | |

1775 | ||

1776 | vertices->insert | |

1777 | ( vertices->end(), itr->vertices.begin(), itr->vertices.end() ); | |

1778 | } | |

1779 | ||

1780 | osg::StateSet* stateset = geometry->getOrCreateStateSet(); | |

1781 | stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); | |

1782 | stateset->setTextureMode(0, GL_TEXTURE_2D, osg::StateAttribute::OFF); | |

1783 | stateset->setTextureMode(1, GL_TEXTURE_2D, osg::StateAttribute::OFF); | |

1784 | ||

1785 | return geometry; | |

1786 | } | |

1787 | //////////////////////////////////////////////////////////////////////////// | |

1788 | bool ConvexPolyhedron::dumpGeometry | |

1789 | ( const Face * face, | |

1790 | const osg::Plane * plane, | |

1791 | ConvexPolyhedron * base, | |

1792 | const char * filename, | |

1793 | const osg::Vec4d& colorOutline, | |

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

1795 | const osg::Vec4d& faceColorOutline, | |

1796 | const osg::Vec4d& faceColorInside, | |

1797 | const osg::Vec4d& planeColorOutline, | |

1798 | const osg::Vec4d& planeColorInside, | |

1799 | const osg::Vec4d& baseColorOutline, | |

1800 | const osg::Vec4d& baseColorInside ) const | |

1801 | { | |

1802 | osg::Group * group = new osg::Group(); | |

1803 | osg::Geode * geode = new osg::Geode(); | |

1804 | geode->getOrCreateStateSet()->setMode( GL_BLEND, osg::StateAttribute::ON ); | |

1805 | geode->getOrCreateStateSet()->setMode( GL_CULL_FACE, osg::StateAttribute::OFF ); | |

1806 | geode->getOrCreateStateSet()->setMode( GL_DEPTH_TEST, osg::StateAttribute::OFF ); | |

1807 | ||

1808 | group->addChild( geode ); | |

1809 | ||

1810 | Vertices vertices; | |

1811 | getPoints( vertices ); | |

1812 | ||

1813 | osg::BoundingBox bb; | |

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

1815 | bb.expandBy( vertices[i] ); | |

1816 | ||

1817 | ConvexPolyhedron cp( *this ), cpFace; | |

1818 | ||

1819 | for( Faces::iterator itr = cp._faces.begin(); itr != cp._faces.end(); ) | |

1820 | { | |

1821 | bool found = ( face && | |

1822 | itr->name == face->name && | |

1823 | itr->plane == face->plane && | |

1824 | itr->vertices == face->vertices ); | |

1825 | #if 1 | |

1826 | if( cp.isFacePolygonConvex( *itr ) < 0 ) | |

1827 | std::reverse( itr->vertices.begin(), itr->vertices.end() ); | |

1828 | #endif | |

1829 | ||

1830 | if( found ) { | |

1831 | cpFace.createFace() = *face; | |

1832 | itr = cp._faces.erase( itr ); | |

1833 | } else { | |

1834 | ++itr; | |

1835 | } | |

1836 | } | |

1837 | ||

1838 | osg::Geometry * geometry = cp.buildGeometry( colorOutline, colorInside ); | |

1839 | geometry->getOrCreateStateSet()->setMode( GL_CULL_FACE, osg::StateAttribute::ON ); | |

1840 | ||

1841 | geode->addDrawable( geometry ); | |

1842 | ||

1843 | if( face ) | |

1844 | geode->addDrawable( cpFace.buildGeometry( faceColorOutline, faceColorInside ) ); | |

1845 | ||

1846 | if( plane ) | |

1847 | { | |

1848 | ConvexPolyhedron cp; | |

1849 | Face & face = cp.createFace(); | |

1850 | face.plane = *plane; | |

1851 | ||

1852 | osg::Vec3d normal = face.plane.getNormal(); | |

1853 | osg::Vec3d side = fabs(normal.x()) < fabs(normal.y()) ? | |

1854 | osg::Vec3d(1.0, 0.0, 0.0) : | |

1855 | osg::Vec3d(0.0, 1.0, 0.0); | |

1856 | ||

1857 | osg::Vec3d v = normal ^ side; | |

1858 | v.normalize(); | |

1859 | v *= bb.radius(); | |

1860 | ||

1861 | osg::Vec3d u = v ^ normal; | |

1862 | u.normalize(); | |

1863 | u *= bb.radius(); | |

1864 | ||

1865 | osg::Vec3d c = bb.center(); | |

1866 | c -= face.plane.getNormal() * face.plane.distance( c ); | |

1867 | ||

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 | face.vertices.push_back( c + u - v ); | |

1872 | ||

1873 | geode->addDrawable( cp.buildGeometry( planeColorOutline, planeColorInside ) ); | |

1874 | } | |

1875 | ||

1876 | if( base ) | |

1877 | geode->addDrawable( base->buildGeometry( baseColorOutline, baseColorInside ) ); | |

1878 | ||

1879 | return osgDB::writeNodeFile( *group, std::string( filename ) ); | |

1880 | } | |

1881 | //////////////////////////////////////////////////////////////////////////// |

**Note:**See TracBrowser for help on using the browser.