00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "collisionPolygon.h"
00016 #include "collisionHandler.h"
00017 #include "collisionEntry.h"
00018 #include "collisionSphere.h"
00019 #include "collisionLine.h"
00020 #include "collisionRay.h"
00021 #include "collisionSegment.h"
00022 #include "config_collide.h"
00023 #include "cullTraverserData.h"
00024 #include "boundingBox.h"
00025 #include "pointerToArray.h"
00026 #include "geomNode.h"
00027 #include "geom.h"
00028 #include "datagram.h"
00029 #include "datagramIterator.h"
00030 #include "bamReader.h"
00031 #include "bamWriter.h"
00032 #include "transformState.h"
00033 #include "clipPlaneAttrib.h"
00034 #include "nearly_zero.h"
00035 #include "geom.h"
00036 #include "geomTrifans.h"
00037 #include "geomLinestrips.h"
00038 #include "geomVertexWriter.h"
00039 #include "renderState.h"
00040
00041 #include <algorithm>
00042
00043 PStatCollector CollisionPolygon::_volume_pcollector("Collision Volumes:CollisionPolygon");
00044 PStatCollector CollisionPolygon::_test_pcollector("Collision Tests:CollisionPolygon");
00045 TypeHandle CollisionPolygon::_type_handle;
00046
00047
00048
00049
00050
00051
00052 CollisionPolygon::
00053 CollisionPolygon(const CollisionPolygon ©) :
00054 CollisionPlane(copy),
00055 _points(copy._points),
00056 _to_2d_mat(copy._to_2d_mat)
00057 {
00058 }
00059
00060
00061
00062
00063
00064
00065 CollisionSolid *CollisionPolygon::
00066 make_copy() {
00067 return new CollisionPolygon(*this);
00068 }
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 bool CollisionPolygon::
00083 verify_points(const LPoint3 *begin, const LPoint3 *end) {
00084 int num_points = end - begin;
00085 if (num_points < 3) {
00086 return false;
00087 }
00088
00089 bool all_ok = true;
00090
00091
00092 const LPoint3 *pi;
00093 for (pi = begin; pi != end && all_ok; ++pi) {
00094 if ((*pi).is_nan()) {
00095 all_ok = false;
00096 } else {
00097
00098 const LPoint3 *pj;
00099 for (pj = begin; pj != pi && all_ok; ++pj) {
00100 if ((*pj) == (*pi)) {
00101 all_ok = false;
00102 }
00103 }
00104 }
00105 }
00106
00107 if (all_ok) {
00108
00109
00110
00111 bool got_normal = false;
00112 for (int i = 2; i < num_points && !got_normal; i++) {
00113 LPlane plane(begin[0], begin[1], begin[i]);
00114 LVector3 normal = plane.get_normal();
00115 PN_stdfloat normal_length = normal.length();
00116 got_normal = IS_THRESHOLD_EQUAL(normal_length, 1.0f, 0.001f);
00117 }
00118
00119 if (!got_normal) {
00120 all_ok = false;
00121 }
00122 }
00123
00124 return all_ok;
00125 }
00126
00127
00128
00129
00130
00131
00132
00133
00134 bool CollisionPolygon::
00135 is_valid() const {
00136 return (_points.size() >= 3);
00137 }
00138
00139
00140
00141
00142
00143
00144
00145 bool CollisionPolygon::
00146 is_concave() const {
00147 if (_points.size() < 3) {
00148
00149 return true;
00150 }
00151
00152 LPoint2 p0 = _points[0]._p;
00153 LPoint2 p1 = _points[1]._p;
00154 PN_stdfloat dx1 = p1[0] - p0[0];
00155 PN_stdfloat dy1 = p1[1] - p0[1];
00156 p0 = p1;
00157 p1 = _points[2]._p;
00158
00159 PN_stdfloat dx2 = p1[0] - p0[0];
00160 PN_stdfloat dy2 = p1[1] - p0[1];
00161 int asum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0);
00162
00163 for (size_t i = 0; i < _points.size() - 1; i++) {
00164 p0 = p1;
00165 p1 = _points[(i+3) % _points.size()]._p;
00166
00167 dx1 = dx2;
00168 dy1 = dy2;
00169 dx2 = p1[0] - p0[0];
00170 dy2 = p1[1] - p0[1];
00171 int csum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0);
00172
00173 if (csum ^ asum) {
00174
00175 return true;
00176 }
00177 }
00178
00179
00180 return false;
00181 }
00182
00183
00184
00185
00186
00187
00188 void CollisionPolygon::
00189 xform(const LMatrix4 &mat) {
00190
00191
00192
00193
00194 if (collide_cat.is_spam()) {
00195 collide_cat.spam()
00196 << "CollisionPolygon transformed by:\n";
00197 mat.write(collide_cat.spam(false), 2);
00198 if (_points.empty()) {
00199 collide_cat.spam(false)
00200 << " (no points)\n";
00201 }
00202 }
00203
00204 if (!_points.empty()) {
00205 LMatrix4 to_3d_mat;
00206 rederive_to_3d_mat(to_3d_mat);
00207
00208 epvector<LPoint3> verts;
00209 verts.reserve(_points.size());
00210 Points::const_iterator pi;
00211 for (pi = _points.begin(); pi != _points.end(); ++pi) {
00212 verts.push_back(to_3d((*pi)._p, to_3d_mat) * mat);
00213 }
00214
00215 const LPoint3 *verts_begin = &verts[0];
00216 const LPoint3 *verts_end = verts_begin + verts.size();
00217 setup_points(verts_begin, verts_end);
00218 }
00219
00220 CollisionSolid::xform(mat);
00221 }
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231 LPoint3 CollisionPolygon::
00232 get_collision_origin() const {
00233 LMatrix4 to_3d_mat;
00234 rederive_to_3d_mat(to_3d_mat);
00235
00236 LPoint2 median = _points[0]._p;
00237 for (int n = 1; n < (int)_points.size(); n++) {
00238 median += _points[n]._p;
00239 }
00240 median /= _points.size();
00241
00242 return to_3d(median, to_3d_mat);
00243 }
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253 PT(PandaNode) CollisionPolygon::
00254 get_viz(const CullTraverser *trav, const CullTraverserData &data,
00255 bool bounds_only) const {
00256 const ClipPlaneAttrib *cpa = DCAST(ClipPlaneAttrib, data._state->get_attrib(ClipPlaneAttrib::get_class_slot()));
00257 if (cpa == (const ClipPlaneAttrib *)NULL) {
00258
00259
00260 return CollisionSolid::get_viz(trav, data, bounds_only);
00261 }
00262
00263 if (collide_cat.is_debug()) {
00264 collide_cat.debug()
00265 << "drawing polygon with clip plane " << *cpa << "\n";
00266 }
00267
00268
00269
00270
00271
00272
00273
00274 Points new_points;
00275 if (apply_clip_plane(new_points, cpa, data.get_net_transform(trav))) {
00276
00277
00278 return CollisionSolid::get_viz(trav, data, bounds_only);
00279 }
00280
00281 if (new_points.empty()) {
00282
00283 return NULL;
00284 }
00285
00286
00287 PT(GeomNode) viz_geom_node = new GeomNode("viz");
00288 PT(GeomNode) bounds_viz_geom_node = new GeomNode("bounds_viz");
00289 draw_polygon(viz_geom_node, bounds_viz_geom_node, new_points);
00290
00291 if (bounds_only) {
00292 return bounds_viz_geom_node.p();
00293 } else {
00294 return viz_geom_node.p();
00295 }
00296 }
00297
00298
00299
00300
00301
00302
00303
00304
00305 PStatCollector &CollisionPolygon::
00306 get_volume_pcollector() {
00307 return _volume_pcollector;
00308 }
00309
00310
00311
00312
00313
00314
00315
00316
00317 PStatCollector &CollisionPolygon::
00318 get_test_pcollector() {
00319 return _test_pcollector;
00320 }
00321
00322
00323
00324
00325
00326
00327 void CollisionPolygon::
00328 output(ostream &out) const {
00329 out << "cpolygon, (" << get_plane()
00330 << "), " << _points.size() << " vertices";
00331 }
00332
00333
00334
00335
00336
00337
00338 void CollisionPolygon::
00339 write(ostream &out, int indent_level) const {
00340 indent(out, indent_level) << (*this) << "\n";
00341 Points::const_iterator pi;
00342 for (pi = _points.begin(); pi != _points.end(); ++pi) {
00343 indent(out, indent_level + 2) << (*pi)._p << "\n";
00344 }
00345
00346 LMatrix4 to_3d_mat;
00347 rederive_to_3d_mat(to_3d_mat);
00348 out << "In 3-d space:\n";
00349 for (pi = _points.begin(); pi != _points.end(); ++pi) {
00350 LVertex vert = to_3d((*pi)._p, to_3d_mat);
00351 indent(out, indent_level + 2) << vert << "\n";
00352 }
00353 }
00354
00355
00356
00357
00358
00359
00360 PT(BoundingVolume) CollisionPolygon::
00361 compute_internal_bounds() const {
00362 if (_points.empty()) {
00363 return new BoundingBox;
00364 }
00365
00366 LMatrix4 to_3d_mat;
00367 rederive_to_3d_mat(to_3d_mat);
00368
00369 Points::const_iterator pi = _points.begin();
00370 LPoint3 p = to_3d((*pi)._p, to_3d_mat);
00371
00372 LPoint3 x = p;
00373 LPoint3 n = p;
00374
00375 for (++pi; pi != _points.end(); ++pi) {
00376 p = to_3d((*pi)._p, to_3d_mat);
00377
00378 n.set(min(n[0], p[0]),
00379 min(n[1], p[1]),
00380 min(n[2], p[2]));
00381 x.set(max(x[0], p[0]),
00382 max(x[1], p[1]),
00383 max(x[2], p[2]));
00384 }
00385
00386 return new BoundingBox(n, x);
00387 }
00388
00389
00390
00391
00392
00393
00394
00395
00396 PT(CollisionEntry) CollisionPolygon::
00397 test_intersection_from_sphere(const CollisionEntry &entry) const {
00398 if (_points.size() < 3) {
00399 return NULL;
00400 }
00401
00402 const CollisionSphere *sphere;
00403 DCAST_INTO_R(sphere, entry.get_from(), 0);
00404
00405 CPT(TransformState) wrt_space = entry.get_wrt_space();
00406 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00407
00408 const LMatrix4 &wrt_mat = wrt_space->get_mat();
00409
00410 LPoint3 orig_center = sphere->get_center() * wrt_mat;
00411 LPoint3 from_center = orig_center;
00412 bool moved_from_center = false;
00413 PN_stdfloat t = 1.0f;
00414 LPoint3 contact_point(from_center);
00415 PN_stdfloat actual_t = 1.0f;
00416
00417 LVector3 from_radius_v =
00418 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00419 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
00420 PN_stdfloat from_radius = csqrt(from_radius_2);
00421
00422 if (wrt_prev_space != wrt_space) {
00423
00424
00425
00426 LPoint3 b = from_center;
00427 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
00428 LVector3 delta = b - a;
00429
00430
00431
00432 PN_stdfloat dot = delta.dot(get_normal());
00433 if (dot > 0.1f) {
00434 return NULL;
00435 }
00436
00437 if (IS_NEARLY_ZERO(dot)) {
00438
00439
00440
00441 } else {
00442
00443
00444
00445
00446
00447
00448
00449
00450 PN_stdfloat dist_to_p = dist_to_plane(a);
00451 t = (dist_to_p / -dot);
00452
00453
00454
00455 actual_t = ((dist_to_p - from_radius) / -dot);
00456 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
00457 contact_point = a + (actual_t * delta);
00458
00459 if (t >= 1.0f) {
00460
00461
00462 } else if (t < 0.0f) {
00463 from_center = a;
00464 moved_from_center = true;
00465
00466 } else {
00467 from_center = a + t * delta;
00468 moved_from_center = true;
00469 }
00470 }
00471 }
00472
00473 LVector3 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00474 #ifndef NDEBUG
00475 if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
00476 collide_cat.info()
00477 << "polygon within " << entry.get_into_node_path()
00478 << " has normal " << normal << " of length " << normal.length()
00479 << "\n";
00480 normal.normalize();
00481 }
00482 #endif
00483
00484
00485
00486 PN_stdfloat dist;
00487 if (!get_plane().intersects_line(dist, from_center, -get_normal())) {
00488
00489
00490 return NULL;
00491 }
00492
00493 if (dist > from_radius || dist < -from_radius) {
00494
00495 return NULL;
00496 }
00497
00498 LPoint2 p = to_2d(from_center - dist * get_normal());
00499 PN_stdfloat edge_dist = 0.0f;
00500
00501 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00502 if (cpa != (ClipPlaneAttrib *)NULL) {
00503
00504 Points new_points;
00505 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
00506
00507
00508 edge_dist = dist_to_polygon(p, _points);
00509
00510 } else if (new_points.empty()) {
00511
00512 return NULL;
00513
00514 } else {
00515
00516 edge_dist = dist_to_polygon(p, new_points);
00517 }
00518
00519 } else {
00520
00521 edge_dist = dist_to_polygon(p, _points);
00522 }
00523
00524
00525
00526
00527
00528 if (edge_dist > from_radius) {
00529
00530 return NULL;
00531 }
00532
00533
00534
00535
00536
00537
00538
00539 PN_stdfloat max_dist = from_radius;
00540 if (edge_dist >= 0.0f) {
00541 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
00542 max_dist = csqrt(max_dist_2);
00543 }
00544
00545 if (dist > max_dist) {
00546
00547 return NULL;
00548 }
00549
00550 if (collide_cat.is_debug()) {
00551 collide_cat.debug()
00552 << "intersection detected from " << entry.get_from_node_path()
00553 << " into " << entry.get_into_node_path() << "\n";
00554 }
00555 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00556
00557 PN_stdfloat into_depth = max_dist - dist;
00558 if (moved_from_center) {
00559
00560
00561
00562 PN_stdfloat orig_dist;
00563 get_plane().intersects_line(orig_dist, orig_center, -normal);
00564 into_depth = max_dist - orig_dist;
00565 }
00566
00567 new_entry->set_surface_normal(normal);
00568 new_entry->set_surface_point(from_center - normal * dist);
00569 new_entry->set_interior_point(from_center - normal * (dist + into_depth));
00570 new_entry->set_contact_pos(contact_point);
00571 new_entry->set_contact_normal(get_normal());
00572 new_entry->set_t(actual_t);
00573
00574 return new_entry;
00575 }
00576
00577
00578
00579
00580
00581
00582
00583
00584 PT(CollisionEntry) CollisionPolygon::
00585 test_intersection_from_line(const CollisionEntry &entry) const {
00586 if (_points.size() < 3) {
00587 return NULL;
00588 }
00589
00590 const CollisionLine *line;
00591 DCAST_INTO_R(line, entry.get_from(), 0);
00592
00593 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00594
00595 LPoint3 from_origin = line->get_origin() * wrt_mat;
00596 LVector3 from_direction = line->get_direction() * wrt_mat;
00597
00598 PN_stdfloat t;
00599 if (!get_plane().intersects_line(t, from_origin, from_direction)) {
00600
00601 return NULL;
00602 }
00603
00604 LPoint3 plane_point = from_origin + t * from_direction;
00605 LPoint2 p = to_2d(plane_point);
00606
00607 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00608 if (cpa != (ClipPlaneAttrib *)NULL) {
00609
00610 Points new_points;
00611 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
00612
00613 if (!point_is_inside(p, _points)) {
00614 return NULL;
00615 }
00616
00617 } else {
00618 if (new_points.size() < 3) {
00619 return NULL;
00620 }
00621 if (!point_is_inside(p, new_points)) {
00622 return NULL;
00623 }
00624 }
00625
00626 } else {
00627
00628 if (!point_is_inside(p, _points)) {
00629 return NULL;
00630 }
00631 }
00632
00633 if (collide_cat.is_debug()) {
00634 collide_cat.debug()
00635 << "intersection detected from " << entry.get_from_node_path()
00636 << " into " << entry.get_into_node_path() << "\n";
00637 }
00638 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00639
00640 LVector3 normal = (has_effective_normal() && line->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00641
00642 new_entry->set_surface_normal(normal);
00643 new_entry->set_surface_point(plane_point);
00644
00645 return new_entry;
00646 }
00647
00648
00649
00650
00651
00652
00653
00654
00655 PT(CollisionEntry) CollisionPolygon::
00656 test_intersection_from_ray(const CollisionEntry &entry) const {
00657 if (_points.size() < 3) {
00658 return NULL;
00659 }
00660
00661 const CollisionRay *ray;
00662 DCAST_INTO_R(ray, entry.get_from(), 0);
00663
00664 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00665
00666 LPoint3 from_origin = ray->get_origin() * wrt_mat;
00667 LVector3 from_direction = ray->get_direction() * wrt_mat;
00668
00669 PN_stdfloat t;
00670 if (!get_plane().intersects_line(t, from_origin, from_direction)) {
00671
00672 return NULL;
00673 }
00674
00675 if (t < 0.0f) {
00676
00677 return NULL;
00678 }
00679
00680 LPoint3 plane_point = from_origin + t * from_direction;
00681 LPoint2 p = to_2d(plane_point);
00682
00683 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00684 if (cpa != (ClipPlaneAttrib *)NULL) {
00685
00686 Points new_points;
00687 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
00688
00689 if (!point_is_inside(p, _points)) {
00690 return NULL;
00691 }
00692
00693 } else {
00694 if (new_points.size() < 3) {
00695 return NULL;
00696 }
00697 if (!point_is_inside(p, new_points)) {
00698 return NULL;
00699 }
00700 }
00701
00702 } else {
00703
00704 if (!point_is_inside(p, _points)) {
00705 return NULL;
00706 }
00707 }
00708
00709 if (collide_cat.is_debug()) {
00710 collide_cat.debug()
00711 << "intersection detected from " << entry.get_from_node_path()
00712 << " into " << entry.get_into_node_path() << "\n";
00713 }
00714 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00715
00716 LVector3 normal = (has_effective_normal() && ray->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00717
00718 new_entry->set_surface_normal(normal);
00719 new_entry->set_surface_point(plane_point);
00720
00721 return new_entry;
00722 }
00723
00724
00725
00726
00727
00728
00729
00730
00731 PT(CollisionEntry) CollisionPolygon::
00732 test_intersection_from_segment(const CollisionEntry &entry) const {
00733 if (_points.size() < 3) {
00734 return NULL;
00735 }
00736
00737 const CollisionSegment *segment;
00738 DCAST_INTO_R(segment, entry.get_from(), 0);
00739
00740 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00741
00742 LPoint3 from_a = segment->get_point_a() * wrt_mat;
00743 LPoint3 from_b = segment->get_point_b() * wrt_mat;
00744 LPoint3 from_direction = from_b - from_a;
00745
00746 PN_stdfloat t;
00747 if (!get_plane().intersects_line(t, from_a, from_direction)) {
00748
00749 return NULL;
00750 }
00751
00752 if (t < 0.0f || t > 1.0f) {
00753
00754
00755 return NULL;
00756 }
00757
00758 LPoint3 plane_point = from_a + t * from_direction;
00759 LPoint2 p = to_2d(plane_point);
00760
00761 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00762 if (cpa != (ClipPlaneAttrib *)NULL) {
00763
00764 Points new_points;
00765 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
00766
00767 if (!point_is_inside(p, _points)) {
00768 return NULL;
00769 }
00770
00771 } else {
00772 if (new_points.size() < 3) {
00773 return NULL;
00774 }
00775 if (!point_is_inside(p, new_points)) {
00776 return NULL;
00777 }
00778 }
00779
00780 } else {
00781
00782 if (!point_is_inside(p, _points)) {
00783 return NULL;
00784 }
00785 }
00786
00787 if (collide_cat.is_debug()) {
00788 collide_cat.debug()
00789 << "intersection detected from " << entry.get_from_node_path()
00790 << " into " << entry.get_into_node_path() << "\n";
00791 }
00792 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00793
00794 LVector3 normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00795
00796 new_entry->set_surface_normal(normal);
00797 new_entry->set_surface_point(plane_point);
00798
00799 return new_entry;
00800 }
00801
00802
00803
00804
00805
00806
00807
00808
00809 PT(CollisionEntry) CollisionPolygon::
00810 test_intersection_from_parabola(const CollisionEntry &entry) const {
00811 if (_points.size() < 3) {
00812 return NULL;
00813 }
00814
00815 const CollisionParabola *parabola;
00816 DCAST_INTO_R(parabola, entry.get_from(), 0);
00817
00818 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00819
00820
00821 LParabola local_p(parabola->get_parabola());
00822 local_p.xform(wrt_mat);
00823
00824 PN_stdfloat t1, t2;
00825 if (!get_plane().intersects_parabola(t1, t2, local_p)) {
00826
00827 return NULL;
00828 }
00829
00830 PN_stdfloat t;
00831 if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) {
00832 if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00833
00834
00835 t = min(t1, t2);
00836 } else {
00837
00838 t = t1;
00839 }
00840
00841 } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00842
00843 t = t2;
00844
00845 } else {
00846
00847 return NULL;
00848 }
00849
00850 LPoint3 plane_point = local_p.calc_point(t);
00851 LPoint2 p = to_2d(plane_point);
00852
00853 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00854 if (cpa != (ClipPlaneAttrib *)NULL) {
00855
00856 Points new_points;
00857 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
00858
00859 if (!point_is_inside(p, _points)) {
00860 return NULL;
00861 }
00862
00863 } else {
00864 if (new_points.size() < 3) {
00865 return NULL;
00866 }
00867 if (!point_is_inside(p, new_points)) {
00868 return NULL;
00869 }
00870 }
00871
00872 } else {
00873
00874 if (!point_is_inside(p, _points)) {
00875 return NULL;
00876 }
00877 }
00878
00879 if (collide_cat.is_debug()) {
00880 collide_cat.debug()
00881 << "intersection detected from " << entry.get_from_node_path()
00882 << " into " << entry.get_into_node_path() << "\n";
00883 }
00884 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00885
00886 LVector3 normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00887
00888 new_entry->set_surface_normal(normal);
00889 new_entry->set_surface_point(plane_point);
00890
00891 return new_entry;
00892 }
00893
00894
00895
00896
00897
00898
00899
00900 void CollisionPolygon::
00901 fill_viz_geom() {
00902 if (collide_cat.is_debug()) {
00903 collide_cat.debug()
00904 << "Recomputing viz for " << *this << "\n";
00905 }
00906 nassertv(_viz_geom != (GeomNode *)NULL && _bounds_viz_geom != (GeomNode *)NULL);
00907 draw_polygon(_viz_geom, _bounds_viz_geom, _points);
00908 }
00909
00910
00911
00912
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922 PN_stdfloat CollisionPolygon::
00923 dist_to_line_segment(const LPoint2 &p,
00924 const LPoint2 &f, const LPoint2 &t,
00925 const LVector2 &v) {
00926 LVector2 v1 = (p - f);
00927 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
00928 if (d < 0.0f) {
00929 return d;
00930 }
00931
00932
00933 LPoint2 q = p + LVector2(-v[1], v[0]) * d;
00934
00935
00936 if (v[0] > 0.0f) {
00937
00938 if (v[1] > 0.0f) {
00939
00940 if (v[0] > v[1]) {
00941
00942 if (q[0] < f[0]) {
00943 return (p - f).length();
00944 } if (q[0] > t[0]) {
00945 return (p - t).length();
00946 } else {
00947 return d;
00948 }
00949 } else {
00950
00951 if (q[1] < f[1]) {
00952 return (p - f).length();
00953 } if (q[1] > t[1]) {
00954 return (p - t).length();
00955 } else {
00956 return d;
00957 }
00958 }
00959 } else {
00960
00961 if (v[0] > -v[1]) {
00962
00963 if (q[0] < f[0]) {
00964 return (p - f).length();
00965 } if (q[0] > t[0]) {
00966 return (p - t).length();
00967 } else {
00968 return d;
00969 }
00970 } else {
00971
00972 if (q[1] > f[1]) {
00973 return (p - f).length();
00974 } if (q[1] < t[1]) {
00975 return (p - t).length();
00976 } else {
00977 return d;
00978 }
00979 }
00980 }
00981 } else {
00982
00983 if (v[1] > 0.0f) {
00984
00985 if (-v[0] > v[1]) {
00986
00987 if (q[0] > f[0]) {
00988 return (p - f).length();
00989 } if (q[0] < t[0]) {
00990 return (p - t).length();
00991 } else {
00992 return d;
00993 }
00994 } else {
00995
00996 if (q[1] < f[1]) {
00997 return (p - f).length();
00998 } if (q[1] > t[1]) {
00999 return (p - t).length();
01000 } else {
01001 return d;
01002 }
01003 }
01004 } else {
01005
01006 if (-v[0] > -v[1]) {
01007
01008 if (q[0] > f[0]) {
01009 return (p - f).length();
01010 } if (q[0] < t[0]) {
01011 return (p - t).length();
01012 } else {
01013 return d;
01014 }
01015 } else {
01016
01017 if (q[1] > f[1]) {
01018 return (p - f).length();
01019 } if (q[1] < t[1]) {
01020 return (p - t).length();
01021 } else {
01022 return d;
01023 }
01024 }
01025 }
01026 }
01027 }
01028
01029
01030
01031
01032
01033
01034
01035
01036
01037 void CollisionPolygon::
01038 compute_vectors(Points &points) {
01039 size_t num_points = points.size();
01040 for (size_t i = 0; i < num_points; i++) {
01041 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
01042 points[i]._v.normalize();
01043 }
01044 }
01045
01046
01047
01048
01049
01050
01051
01052
01053 void CollisionPolygon::
01054 draw_polygon(GeomNode *viz_geom_node, GeomNode *bounds_viz_geom_node,
01055 const CollisionPolygon::Points &points) const {
01056 if (points.size() < 3) {
01057 if (collide_cat.is_debug()) {
01058 collide_cat.debug()
01059 << "(Degenerate poly, ignoring.)\n";
01060 }
01061 return;
01062 }
01063
01064 LMatrix4 to_3d_mat;
01065 rederive_to_3d_mat(to_3d_mat);
01066
01067 PT(GeomVertexData) vdata = new GeomVertexData
01068 ("collision", GeomVertexFormat::get_v3(),
01069 Geom::UH_static);
01070 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
01071
01072 Points::const_iterator pi;
01073 for (pi = points.begin(); pi != points.end(); ++pi) {
01074 vertex.add_data3(to_3d((*pi)._p, to_3d_mat));
01075 }
01076
01077 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
01078 body->add_consecutive_vertices(0, points.size());
01079 body->close_primitive();
01080
01081 PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static);
01082 border->add_consecutive_vertices(0, points.size());
01083 border->add_vertex(0);
01084 border->close_primitive();
01085
01086 PT(Geom) geom1 = new Geom(vdata);
01087 geom1->add_primitive(body);
01088
01089 PT(Geom) geom2 = new Geom(vdata);
01090 geom2->add_primitive(border);
01091
01092 viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_viz_state());
01093 viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_viz_state());
01094
01095 bounds_viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_bounds_viz_state());
01096 bounds_viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_bounds_viz_state());
01097 }
01098
01099
01100
01101
01102
01103
01104
01105
01106 bool CollisionPolygon::
01107 point_is_inside(const LPoint2 &p, const CollisionPolygon::Points &points) const {
01108
01109
01110
01111
01112
01113 for (int i = 0; i < (int)points.size() - 1; i++) {
01114 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
01115 return false;
01116 }
01117 }
01118 if (is_right(p - points[points.size() - 1]._p,
01119 points[0]._p - points[points.size() - 1]._p)) {
01120 return false;
01121 }
01122
01123 return true;
01124 }
01125
01126
01127
01128
01129
01130
01131
01132
01133
01134 PN_stdfloat CollisionPolygon::
01135 dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const {
01136
01137
01138
01139
01140
01141
01142
01143
01144
01145 bool got_dist = false;
01146 PN_stdfloat best_dist = -1.0f;
01147
01148 size_t num_points = points.size();
01149 for (size_t i = 0; i < num_points - 1; ++i) {
01150 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
01151 points[i]._v);
01152 if (d >= 0.0f) {
01153 if (!got_dist || d < best_dist) {
01154 best_dist = d;
01155 got_dist = true;
01156 }
01157 }
01158 }
01159
01160 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
01161 points[num_points - 1]._v);
01162 if (d >= 0.0f) {
01163 if (!got_dist || d < best_dist) {
01164 best_dist = d;
01165 got_dist = true;
01166 }
01167 }
01168
01169 return best_dist;
01170 }
01171
01172
01173
01174
01175
01176
01177 void CollisionPolygon::
01178 setup_points(const LPoint3 *begin, const LPoint3 *end) {
01179 int num_points = end - begin;
01180 nassertv(num_points >= 3);
01181
01182 _points.clear();
01183
01184
01185
01186 LVector3 normal = LVector3::zero();
01187
01188
01189
01190
01191
01192
01193 for (int i = 0; i < num_points; i++) {
01194 const LPoint3 &p0 = begin[i];
01195 const LPoint3 &p1 = begin[(i + 1) % num_points];
01196 normal[0] += p0[1] * p1[2] - p0[2] * p1[1];
01197 normal[1] += p0[2] * p1[0] - p0[0] * p1[2];
01198 normal[2] += p0[0] * p1[1] - p0[1] * p1[0];
01199 }
01200
01201 if (normal.length_squared() == 0.0f) {
01202
01203 return;
01204 }
01205
01206 #ifndef NDEBUG
01207
01208 {
01209 if (!verify_points(begin, end)) {
01210 collide_cat.error() << "Invalid points in CollisionPolygon:\n";
01211 const LPoint3 *pi;
01212 for (pi = begin; pi != end; ++pi) {
01213 collide_cat.error(false) << " " << (*pi) << "\n";
01214 }
01215 collide_cat.error(false)
01216 << " normal " << normal << " with length " << normal.length() << "\n";
01217
01218 return;
01219 }
01220 }
01221
01222 if (collide_cat.is_spam()) {
01223 collide_cat.spam()
01224 << "CollisionPolygon defined with " << num_points << " vertices:\n";
01225 const LPoint3 *pi;
01226 for (pi = begin; pi != end; ++pi) {
01227 collide_cat.spam(false) << " " << (*pi) << "\n";
01228 }
01229 }
01230 #endif
01231
01232 set_plane(LPlane(normal, begin[0]));
01233
01234
01235
01236 LMatrix4 to_3d_mat;
01237 calc_to_3d_mat(to_3d_mat);
01238
01239
01240
01241 _to_2d_mat.invert_from(to_3d_mat);
01242
01243
01244
01245 const LPoint3 *pi;
01246 for (pi = begin; pi != end; ++pi) {
01247 LPoint3 point = (*pi) * _to_2d_mat;
01248 _points.push_back(PointDef(point[0], point[2]));
01249 }
01250
01251 nassertv(_points.size() >= 3);
01252
01253 #ifndef NDEBUG
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267 #endif
01268
01269 compute_vectors(_points);
01270 }
01271
01272
01273
01274
01275
01276
01277
01278
01279 LPoint3 CollisionPolygon::
01280 legacy_to_3d(const LVecBase2 &point2d, int axis) const {
01281 nassertr(!point2d.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
01282
01283 LVector3 normal = get_normal();
01284 PN_stdfloat D = get_plane()[3];
01285
01286 nassertr(!normal.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
01287 nassertr(!cnan(D), LPoint3(0.0f, 0.0f, 0.0f));
01288
01289 switch (axis) {
01290 case 0:
01291 return LPoint3(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], point2d[0], point2d[1]);
01292
01293 case 1:
01294 return LPoint3(point2d[0],
01295 -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], point2d[1]);
01296
01297 case 2:
01298 return LPoint3(point2d[0], point2d[1],
01299 -(normal[0]*point2d[0] + normal[1]*point2d[1] + D)/normal[2]);
01300 }
01301
01302 nassertr(false, LPoint3(0.0f, 0.0f, 0.0f));
01303 return LPoint3(0.0f, 0.0f, 0.0f);
01304 }
01305
01306
01307
01308
01309
01310
01311
01312
01313
01314
01315
01316
01317
01318 bool CollisionPolygon::
01319 clip_polygon(CollisionPolygon::Points &new_points,
01320 const CollisionPolygon::Points &source_points,
01321 const LPlane &plane) const {
01322 new_points.clear();
01323 if (source_points.empty()) {
01324 return true;
01325 }
01326
01327 LPoint3 from3d;
01328 LVector3 delta3d;
01329 if (!plane.intersects_plane(from3d, delta3d, get_plane())) {
01330
01331
01332 if (plane.dist_to_plane(get_plane().get_point()) < 0.0) {
01333
01334
01335 new_points = source_points;
01336 return true;
01337 }
01338 return false;
01339 }
01340
01341
01342
01343 LPoint2 from2d = to_2d(from3d);
01344 LVector2 delta2d = to_2d(delta3d);
01345
01346 PN_stdfloat a = -delta2d[1];
01347 PN_stdfloat b = delta2d[0];
01348 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
01349
01350
01351
01352
01353
01354
01355
01356
01357 new_points.reserve(source_points.size() + 1);
01358
01359 LPoint2 last_point = source_points.back()._p;
01360 bool last_is_in = !is_right(last_point - from2d, delta2d);
01361 bool all_in = last_is_in;
01362 Points::const_iterator pi;
01363 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
01364 const LPoint2 &this_point = (*pi)._p;
01365 bool this_is_in = !is_right(this_point - from2d, delta2d);
01366
01367
01368
01369 bool crossed_over = (this_is_in != last_is_in);
01370 if (crossed_over) {
01371
01372
01373 LVector2 d = this_point - last_point;
01374 PN_stdfloat denom = (a * d[0] + b * d[1]);
01375 if (denom != 0.0) {
01376 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
01377 LPoint2 p = last_point + t * d;
01378
01379 new_points.push_back(PointDef(p[0], p[1]));
01380 last_is_in = this_is_in;
01381 }
01382 }
01383
01384 if (this_is_in) {
01385
01386 new_points.push_back(PointDef(this_point[0], this_point[1]));
01387 } else {
01388 all_in = false;
01389 }
01390
01391 last_point = this_point;
01392 }
01393
01394 return all_in;
01395 }
01396
01397
01398
01399
01400
01401
01402
01403
01404
01405
01406
01407
01408 bool CollisionPolygon::
01409 apply_clip_plane(CollisionPolygon::Points &new_points,
01410 const ClipPlaneAttrib *cpa,
01411 const TransformState *net_transform) const {
01412 bool all_in = true;
01413
01414 int num_planes = cpa->get_num_on_planes();
01415 bool first_plane = true;
01416
01417 for (int i = 0; i < num_planes; i++) {
01418 NodePath plane_path = cpa->get_on_plane(i);
01419 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
01420 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
01421 CPT(TransformState) new_transform =
01422 net_transform->invert_compose(plane_path.get_net_transform());
01423
01424 LPlane plane = plane_node->get_plane() * new_transform->get_mat();
01425 if (first_plane) {
01426 first_plane = false;
01427 if (!clip_polygon(new_points, _points, plane)) {
01428 all_in = false;
01429 }
01430 } else {
01431 Points last_points;
01432 last_points.swap(new_points);
01433 if (!clip_polygon(new_points, last_points, plane)) {
01434 all_in = false;
01435 }
01436 }
01437 }
01438 }
01439
01440 if (!all_in) {
01441 compute_vectors(new_points);
01442 }
01443
01444 return all_in;
01445 }
01446
01447
01448
01449
01450
01451
01452
01453 void CollisionPolygon::
01454 write_datagram(BamWriter *manager, Datagram &me) {
01455 CollisionPlane::write_datagram(manager, me);
01456 me.add_uint16(_points.size());
01457 for (size_t i = 0; i < _points.size(); i++) {
01458 _points[i]._p.write_datagram(me);
01459 _points[i]._v.write_datagram(me);
01460 }
01461 _to_2d_mat.write_datagram(me);
01462 }
01463
01464
01465
01466
01467
01468
01469
01470
01471
01472 void CollisionPolygon::
01473 fillin(DatagramIterator &scan, BamReader *manager) {
01474 CollisionPlane::fillin(scan, manager);
01475
01476 size_t size = scan.get_uint16();
01477 for (size_t i = 0; i < size; i++) {
01478 LPoint2 p;
01479 LVector2 v;
01480 p.read_datagram(scan);
01481 v.read_datagram(scan);
01482 _points.push_back(PointDef(p, v));
01483 }
01484 _to_2d_mat.read_datagram(scan);
01485
01486 if (manager->get_file_minor_ver() < 13) {
01487
01488
01489
01490 if (_points.size() >= 3) {
01491 LMatrix4 to_3d_mat;
01492 rederive_to_3d_mat(to_3d_mat);
01493
01494 epvector<LPoint3> verts;
01495 verts.reserve(_points.size());
01496 Points::const_iterator pi;
01497 for (pi = _points.begin(); pi != _points.end(); ++pi) {
01498 verts.push_back(to_3d((*pi)._p, to_3d_mat));
01499 }
01500
01501 const LPoint3 *verts_begin = &verts[0];
01502 const LPoint3 *verts_end = verts_begin + verts.size();
01503 setup_points(verts_begin, verts_end);
01504 }
01505 }
01506 }
01507
01508
01509
01510
01511
01512
01513
01514 TypedWritable* CollisionPolygon::
01515 make_CollisionPolygon(const FactoryParams ¶ms) {
01516 CollisionPolygon *me = new CollisionPolygon;
01517 DatagramIterator scan;
01518 BamReader *manager;
01519
01520 parse_params(params, scan, manager);
01521 me->fillin(scan, manager);
01522 return me;
01523 }
01524
01525
01526
01527
01528
01529
01530 void CollisionPolygon::
01531 register_with_read_factory() {
01532 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPolygon);
01533 }
01534
01535