Panda3D
Loading...
Searching...
No Matches
collisionPolygon.cxx
Go to the documentation of this file.
1/**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file collisionPolygon.cxx
10 * @author drose
11 * @date 2000-04-25
12 */
13
14#include "collisionPolygon.h"
15#include "collisionHandler.h"
16#include "collisionEntry.h"
17#include "collisionSphere.h"
18#include "collisionLine.h"
19#include "collisionRay.h"
20#include "collisionSegment.h"
21#include "collisionParabola.h"
22#include "config_collide.h"
23#include "cullTraverserData.h"
24#include "boundingBox.h"
25#include "pointerToArray.h"
26#include "geomNode.h"
27#include "geom.h"
28#include "datagram.h"
29#include "datagramIterator.h"
30#include "bamReader.h"
31#include "bamWriter.h"
32#include "transformState.h"
33#include "clipPlaneAttrib.h"
34#include "nearly_zero.h"
35#include "geom.h"
36#include "geomTrifans.h"
37#include "geomLinestrips.h"
38#include "geomVertexWriter.h"
39#include "renderState.h"
40#include "epvector.h"
41
42#include <algorithm>
43
44using std::max;
45using std::min;
46
47PStatCollector CollisionPolygon::_volume_pcollector("Collision Volumes:CollisionPolygon");
48PStatCollector CollisionPolygon::_test_pcollector("Collision Tests:CollisionPolygon");
49TypeHandle CollisionPolygon::_type_handle;
50
51/**
52 *
53 */
54CollisionPolygon::
55CollisionPolygon(const CollisionPolygon &copy) :
56 CollisionPlane(copy),
57 _points(copy._points),
58 _to_2d_mat(copy._to_2d_mat)
59{
60}
61
62/**
63 *
64 */
65CollisionSolid *CollisionPolygon::
66make_copy() {
67 return new CollisionPolygon(*this);
68}
69
70/**
71 * Verifies that the indicated set of points will define a valid
72 * CollisionPolygon: that is, at least three non-collinear points, with no
73 * points repeated.
74 *
75 * This does not check that the polygon defined is convex; that check is made
76 * later, once we have projected the points to 2-d space where the decision is
77 * easier.
78 */
80verify_points(const LPoint3 *begin, const LPoint3 *end) {
81 int num_points = end - begin;
82 if (num_points < 3) {
83 return false;
84 }
85
86 bool all_ok = true;
87
88 // First, check for repeated or invalid points.
89 const LPoint3 *pi;
90 for (pi = begin; pi != end && all_ok; ++pi) {
91 if ((*pi).is_nan()) {
92 all_ok = false;
93 } else {
94 // Make sure no points are repeated.
95 const LPoint3 *pj;
96 for (pj = begin; pj != pi && all_ok; ++pj) {
97 if ((*pj) == (*pi)) {
98 all_ok = false;
99 }
100 }
101 }
102 }
103
104 if (all_ok) {
105 // Create a plane to determine the planarity of the first three points (or
106 // the first two points and the nth point thereafter, in case the first
107 // three points happen to be collinear).
108 bool got_normal = false;
109 for (int i = 2; i < num_points && !got_normal; i++) {
110 LPlane plane(begin[0], begin[1], begin[i]);
111 LVector3 normal = plane.get_normal();
112 PN_stdfloat normal_length = normal.length();
113 got_normal = IS_THRESHOLD_EQUAL(normal_length, 1.0f, 0.001f);
114 }
115
116 if (!got_normal) {
117 all_ok = false;
118 }
119 }
120
121 return all_ok;
122}
123
124/**
125 * Returns true if the CollisionPolygon is valid (that is, it has at least
126 * three vertices), or false otherwise.
127 */
129is_valid() const {
130 return (_points.size() >= 3);
131}
132
133/**
134 * Returns true if the CollisionPolygon appears to be concave, or false if it
135 * is safely convex.
136 */
138is_concave() const {
139 if (_points.size() < 3) {
140 // It's not even a valid polygon.
141 return true;
142 }
143
144 LPoint2 p0 = _points[0]._p;
145 LPoint2 p1 = _points[1]._p;
146 PN_stdfloat dx1 = p1[0] - p0[0];
147 PN_stdfloat dy1 = p1[1] - p0[1];
148 p0 = p1;
149 p1 = _points[2]._p;
150
151 PN_stdfloat dx2 = p1[0] - p0[0];
152 PN_stdfloat dy2 = p1[1] - p0[1];
153 int asum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0);
154
155 for (size_t i = 0; i < _points.size() - 1; i++) {
156 p0 = p1;
157 p1 = _points[(i+3) % _points.size()]._p;
158
159 dx1 = dx2;
160 dy1 = dy2;
161 dx2 = p1[0] - p0[0];
162 dy2 = p1[1] - p0[1];
163 int csum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0);
164
165 if (csum ^ asum) {
166 // Oops, the polygon is concave.
167 return true;
168 }
169 }
170
171 // The polygon is safely convex.
172 return false;
173}
174
175/**
176 * Transforms the solid by the indicated matrix.
177 */
179xform(const LMatrix4 &mat) {
180 // We need to convert all the vertices to 3-d for this operation, and then
181 // convert them back. Hopefully we won't lose too much precision during all
182 // of this.
183
184 if (collide_cat.is_spam()) {
185 collide_cat.spam()
186 << "CollisionPolygon transformed by:\n";
187 mat.write(collide_cat.spam(false), 2);
188 if (_points.empty()) {
189 collide_cat.spam(false)
190 << " (no points)\n";
191 }
192 }
193
194 if (!_points.empty()) {
195 LMatrix4 to_3d_mat;
196 rederive_to_3d_mat(to_3d_mat);
197
198 epvector<LPoint3> verts;
199 verts.reserve(_points.size());
200 Points::const_iterator pi;
201 for (pi = _points.begin(); pi != _points.end(); ++pi) {
202 verts.push_back(to_3d((*pi)._p, to_3d_mat) * mat);
203 }
204
205 const LPoint3 *verts_begin = &verts[0];
206 const LPoint3 *verts_end = verts_begin + verts.size();
207 setup_points(verts_begin, verts_end);
208 }
209
210 CollisionSolid::xform(mat);
211}
212
213/**
214 * Returns the point in space deemed to be the "origin" of the solid for
215 * collision purposes. The closest intersection point to this origin point is
216 * considered to be the most significant.
217 */
219get_collision_origin() const {
220 LMatrix4 to_3d_mat;
221 rederive_to_3d_mat(to_3d_mat);
222
223 LPoint2 median = _points[0]._p;
224 for (int n = 1; n < (int)_points.size(); n++) {
225 median += _points[n]._p;
226 }
227 median /= _points.size();
228
229 return to_3d(median, to_3d_mat);
230}
231
232/**
233 * Returns a GeomNode that may be rendered to visualize the CollisionSolid.
234 * This is used during the cull traversal to render the CollisionNodes that
235 * have been made visible.
236 */
237PT(PandaNode) CollisionPolygon::
238get_viz(const CullTraverser *trav, const CullTraverserData &data,
239 bool bounds_only) const {
240 const ClipPlaneAttrib *cpa = DCAST(ClipPlaneAttrib, data._state->get_attrib(ClipPlaneAttrib::get_class_slot()));
241 if (cpa == nullptr) {
242 // Fortunately, the polygon is not clipped. This is the normal, easy
243 // case.
244 return CollisionSolid::get_viz(trav, data, bounds_only);
245 }
246
247 if (collide_cat.is_debug()) {
248 collide_cat.debug()
249 << "drawing polygon with clip plane " << *cpa << "\n";
250 }
251
252 // The polygon is clipped. We need to render it clipped. We could just
253 // turn on the ClipPlaneAttrib state and render the full polygon, letting
254 // the hardware do the clipping, but we get fancy and clip it by hand
255 // instead, just to prove that our clipping algorithm works properly. This
256 // does require some more dynamic work.
257 Points new_points;
258 if (apply_clip_plane(new_points, cpa, data.get_net_transform(trav))) {
259 // All points are behind the clip plane; just draw the original polygon.
260 return CollisionSolid::get_viz(trav, data, bounds_only);
261 }
262
263 if (new_points.empty()) {
264 // All points are in front of the clip plane; draw nothing.
265 return nullptr;
266 }
267
268 // Draw the clipped polygon.
269 PT(GeomNode) viz_geom_node = new GeomNode("viz");
270 PT(GeomNode) bounds_viz_geom_node = new GeomNode("bounds_viz");
271 draw_polygon(viz_geom_node, bounds_viz_geom_node, new_points);
272
273 if (bounds_only) {
274 return bounds_viz_geom_node;
275 } else {
276 return viz_geom_node;
277 }
278}
279
280/**
281 * Returns a PStatCollector that is used to count the number of bounding
282 * volume tests made against a solid of this type in a given frame.
283 */
286 return _volume_pcollector;
287}
288
289/**
290 * Returns a PStatCollector that is used to count the number of intersection
291 * tests made against a solid of this type in a given frame.
292 */
295 return _test_pcollector;
296}
297
298/**
299 *
300 */
301void CollisionPolygon::
302output(std::ostream &out) const {
303 out << "cpolygon, (" << get_plane()
304 << "), " << _points.size() << " vertices";
305}
306
307/**
308 *
309 */
310void CollisionPolygon::
311write(std::ostream &out, int indent_level) const {
312 indent(out, indent_level) << (*this) << "\n";
313 Points::const_iterator pi;
314 for (pi = _points.begin(); pi != _points.end(); ++pi) {
315 indent(out, indent_level + 2) << (*pi)._p << "\n";
316 }
317
318 LMatrix4 to_3d_mat;
319 rederive_to_3d_mat(to_3d_mat);
320 out << "In 3-d space:\n";
321 for (pi = _points.begin(); pi != _points.end(); ++pi) {
322 LVertex vert = to_3d((*pi)._p, to_3d_mat);
323 indent(out, indent_level + 2) << vert << "\n";
324 }
325}
326
327/**
328 *
329 */
330PT(BoundingVolume) CollisionPolygon::
331compute_internal_bounds() const {
332 if (_points.empty()) {
333 return new BoundingBox;
334 }
335
336 LMatrix4 to_3d_mat;
337 rederive_to_3d_mat(to_3d_mat);
338
339 Points::const_iterator pi = _points.begin();
340 LPoint3 p = to_3d((*pi)._p, to_3d_mat);
341
342 LPoint3 x = p;
343 LPoint3 n = p;
344
345 for (++pi; pi != _points.end(); ++pi) {
346 p = to_3d((*pi)._p, to_3d_mat);
347
348 n.set(min(n[0], p[0]),
349 min(n[1], p[1]),
350 min(n[2], p[2]));
351 x.set(max(x[0], p[0]),
352 max(x[1], p[1]),
353 max(x[2], p[2]));
354 }
355
356 return new BoundingBox(n, x);
357}
358
359/**
360 * This is part of the double-dispatch implementation of test_intersection().
361 * It is called when the "from" object is a sphere.
362 */
363PT(CollisionEntry) CollisionPolygon::
364test_intersection_from_sphere(const CollisionEntry &entry) const {
365 if (_points.size() < 3) {
366 return nullptr;
367 }
368
369 const CollisionSphere *sphere;
370 DCAST_INTO_R(sphere, entry.get_from(), nullptr);
371
372 CPT(TransformState) wrt_space = entry.get_wrt_space();
373 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
374
375 const LMatrix4 &wrt_mat = wrt_space->get_mat();
376
377 LPoint3 orig_center = sphere->get_center() * wrt_mat;
378 LPoint3 from_center = orig_center;
379 bool moved_from_center = false;
380 PN_stdfloat t = 1.0f;
381 LPoint3 contact_point(from_center);
382 PN_stdfloat actual_t = 1.0f;
383
384 LVector3 from_radius_v =
385 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
386 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
387 PN_stdfloat from_radius = csqrt(from_radius_2);
388
389 if (wrt_prev_space != wrt_space) {
390 // If we have a delta between the previous position and the current
391 // position, we use that to determine some more properties of the
392 // collision.
393 LPoint3 b = from_center;
394 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
395 LVector3 delta = b - a;
396
397 // First, there is no collision if the "from" object is definitely moving
398 // in the same direction as the plane's normal.
399 PN_stdfloat dot = delta.dot(get_normal());
400 if (dot > 0.1f) {
401 return nullptr;
402 }
403
404 if (IS_NEARLY_ZERO(dot)) {
405 // If we're moving parallel to the plane, the sphere is tested at its
406 // final point. Leave it as it is.
407
408 } else {
409/*
410 * Otherwise, we're moving into the plane; the sphere is tested at the point
411 * along its path that is closest to intersecting the plane. This may be the
412 * actual intersection point, or it may be the starting point or the final
413 * point. dot is equal to the (negative) magnitude of 'delta' along the
414 * direction of the plane normal t = ratio of (distance from start pos to
415 * plane) to (distance from start pos to end pos), along axis of plane normal
416 */
417 PN_stdfloat dist_to_p = dist_to_plane(a);
418 t = (dist_to_p / -dot);
419
420 // also compute the actual contact point and time of contact for
421 // handlers that need it
422 actual_t = ((dist_to_p - from_radius) / -dot);
423 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
424 contact_point = a + (actual_t * delta);
425
426 if (t >= 1.0f) {
427 // Leave it where it is.
428
429 } else if (t < 0.0f) {
430 from_center = a;
431 moved_from_center = true;
432
433 } else {
434 from_center = a + t * delta;
435 moved_from_center = true;
436 }
437 }
438 }
439
440 LVector3 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
441#ifndef NDEBUG
442 if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001)) {
443 if (collide_cat.is_info()) {
444 collide_cat.info()
445 << "polygon within " << entry.get_into_node_path()
446 << " has normal " << normal << " of length " << normal.length()
447 << "\n";
448 }
449 normal.normalize();
450 }
451#endif
452
453 // The nearest point within the plane to our center is the intersection of
454 // the line (center, center - normal) with the plane.
455 PN_stdfloat dist;
456 if (!get_plane().intersects_line(dist, from_center, -get_normal())) {
457 // No intersection with plane? This means the plane's effective normal
458 // was within the plane itself. A useless polygon.
459 return nullptr;
460 }
461
462 if (dist > from_radius || dist < -from_radius) {
463 // No intersection with the plane.
464 return nullptr;
465 }
466
467 LPoint2 p = to_2d(from_center - dist * get_normal());
468 PN_stdfloat edge_dist = 0.0f;
469
470 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
471 if (cpa != nullptr) {
472 // We have a clip plane; apply it.
473 Points new_points;
474 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
475 // All points are behind the clip plane; just do the default test.
476 edge_dist = dist_to_polygon(p, _points);
477
478 } else if (new_points.empty()) {
479 // The polygon is completely clipped.
480 return nullptr;
481
482 } else {
483 // Test against the clipped polygon.
484 edge_dist = dist_to_polygon(p, new_points);
485 }
486
487 } else {
488 // No clip plane is in effect. Do the default test.
489 edge_dist = dist_to_polygon(p, _points);
490 }
491
492 // Now we have edge_dist, which is the distance from the sphere center to
493 // the nearest edge of the polygon, within the polygon's plane.
494
495 if (edge_dist > from_radius) {
496 // No intersection; the circle is outside the polygon.
497 return nullptr;
498 }
499
500 // The sphere appears to intersect the polygon. If the edge is less than
501 // from_radius away, the sphere may be resting on an edge of the polygon.
502 // Determine how far the center of the sphere must remain from the plane,
503 // based on its distance from the nearest edge.
504
505 PN_stdfloat max_dist = from_radius;
506 if (edge_dist >= 0.0f) {
507 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
508 max_dist = csqrt(max_dist_2);
509 }
510
511 if (dist > max_dist || -dist > max_dist) {
512 // There's no intersection: the sphere is hanging above or under the edge.
513 return nullptr;
514 }
515
516 if (collide_cat.is_debug()) {
517 collide_cat.debug()
518 << "intersection detected from " << entry.get_from_node_path()
519 << " into " << entry.get_into_node_path() << "\n";
520 }
521 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
522
523 PN_stdfloat into_depth = max_dist - dist;
524 if (moved_from_center) {
525 // We have to base the depth of intersection on the sphere's final resting
526 // point, not the point from which we tested the intersection.
527 PN_stdfloat orig_dist;
528 get_plane().intersects_line(orig_dist, orig_center, -normal);
529 into_depth = max_dist - orig_dist;
530 }
531
532 new_entry->set_surface_normal(normal);
533 new_entry->set_surface_point(from_center - normal * dist);
534 new_entry->set_interior_point(from_center - normal * (dist + into_depth));
535 new_entry->set_contact_pos(contact_point);
536 new_entry->set_contact_normal(get_normal());
537 new_entry->set_t(actual_t);
538
539 return new_entry;
540}
541
542/**
543 * This is part of the double-dispatch implementation of test_intersection().
544 * It is called when the "from" object is a line.
545 */
546PT(CollisionEntry) CollisionPolygon::
547test_intersection_from_line(const CollisionEntry &entry) const {
548 if (_points.size() < 3) {
549 return nullptr;
550 }
551
552 const CollisionLine *line;
553 DCAST_INTO_R(line, entry.get_from(), nullptr);
554
555 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
556
557 LPoint3 from_origin = line->get_origin() * wrt_mat;
558 LVector3 from_direction = line->get_direction() * wrt_mat;
559
560 PN_stdfloat t;
561 if (!get_plane().intersects_line(t, from_origin, from_direction)) {
562 // No intersection.
563 return nullptr;
564 }
565
566 LPoint3 plane_point = from_origin + t * from_direction;
567 LPoint2 p = to_2d(plane_point);
568
569 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
570 if (cpa != nullptr) {
571 // We have a clip plane; apply it.
572 Points new_points;
573 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
574 // All points are behind the clip plane.
575 if (!point_is_inside(p, _points)) {
576 return nullptr;
577 }
578
579 } else {
580 if (new_points.size() < 3) {
581 return nullptr;
582 }
583 if (!point_is_inside(p, new_points)) {
584 return nullptr;
585 }
586 }
587
588 } else {
589 // No clip plane is in effect. Do the default test.
590 if (!point_is_inside(p, _points)) {
591 return nullptr;
592 }
593 }
594
595 if (collide_cat.is_debug()) {
596 collide_cat.debug()
597 << "intersection detected from " << entry.get_from_node_path()
598 << " into " << entry.get_into_node_path() << "\n";
599 }
600 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
601
602 LVector3 normal = (has_effective_normal() && line->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
603
604 new_entry->set_surface_normal(normal);
605 new_entry->set_surface_point(plane_point);
606
607 return new_entry;
608}
609
610/**
611 * This is part of the double-dispatch implementation of test_intersection().
612 * It is called when the "from" object is a ray.
613 */
614PT(CollisionEntry) CollisionPolygon::
615test_intersection_from_ray(const CollisionEntry &entry) const {
616 if (_points.size() < 3) {
617 return nullptr;
618 }
619
620 const CollisionRay *ray;
621 DCAST_INTO_R(ray, entry.get_from(), nullptr);
622
623 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
624
625 LPoint3 from_origin = ray->get_origin() * wrt_mat;
626 LVector3 from_direction = ray->get_direction() * wrt_mat;
627
628 PN_stdfloat t;
629 if (!get_plane().intersects_line(t, from_origin, from_direction)) {
630 // No intersection.
631 return nullptr;
632 }
633
634 if (t < 0.0f) {
635 // The intersection point is before the start of the ray.
636 return nullptr;
637 }
638
639 LPoint3 plane_point = from_origin + t * from_direction;
640 LPoint2 p = to_2d(plane_point);
641
642 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
643 if (cpa != nullptr) {
644 // We have a clip plane; apply it.
645 Points new_points;
646 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
647 // All points are behind the clip plane.
648 if (!point_is_inside(p, _points)) {
649 return nullptr;
650 }
651
652 } else {
653 if (new_points.size() < 3) {
654 return nullptr;
655 }
656 if (!point_is_inside(p, new_points)) {
657 return nullptr;
658 }
659 }
660
661 } else {
662 // No clip plane is in effect. Do the default test.
663 if (!point_is_inside(p, _points)) {
664 return nullptr;
665 }
666 }
667
668 if (collide_cat.is_debug()) {
669 collide_cat.debug()
670 << "intersection detected from " << entry.get_from_node_path()
671 << " into " << entry.get_into_node_path() << "\n";
672 }
673 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
674
675 LVector3 normal = (has_effective_normal() && ray->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
676
677 new_entry->set_surface_normal(normal);
678 new_entry->set_surface_point(plane_point);
679
680 return new_entry;
681}
682
683/**
684 * This is part of the double-dispatch implementation of test_intersection().
685 * It is called when the "from" object is a segment.
686 */
687PT(CollisionEntry) CollisionPolygon::
688test_intersection_from_segment(const CollisionEntry &entry) const {
689 if (_points.size() < 3) {
690 return nullptr;
691 }
692
693 const CollisionSegment *segment;
694 DCAST_INTO_R(segment, entry.get_from(), nullptr);
695
696 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
697
698 LPoint3 from_a = segment->get_point_a() * wrt_mat;
699 LPoint3 from_b = segment->get_point_b() * wrt_mat;
700 LPoint3 from_direction = from_b - from_a;
701
702 PN_stdfloat t;
703 if (!get_plane().intersects_line(t, from_a, from_direction)) {
704 // No intersection.
705 return nullptr;
706 }
707
708 if (t < 0.0f || t > 1.0f) {
709 // The intersection point is before the start of the segment or after the
710 // end of the segment.
711 return nullptr;
712 }
713
714 LPoint3 plane_point = from_a + t * from_direction;
715 LPoint2 p = to_2d(plane_point);
716
717 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
718 if (cpa != nullptr) {
719 // We have a clip plane; apply it.
720 Points new_points;
721 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
722 // All points are behind the clip plane.
723 if (!point_is_inside(p, _points)) {
724 return nullptr;
725 }
726
727 } else {
728 if (new_points.size() < 3) {
729 return nullptr;
730 }
731 if (!point_is_inside(p, new_points)) {
732 return nullptr;
733 }
734 }
735
736 } else {
737 // No clip plane is in effect. Do the default test.
738 if (!point_is_inside(p, _points)) {
739 return nullptr;
740 }
741 }
742
743 if (collide_cat.is_debug()) {
744 collide_cat.debug()
745 << "intersection detected from " << entry.get_from_node_path()
746 << " into " << entry.get_into_node_path() << "\n";
747 }
748 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
749
750 LVector3 normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
751
752 new_entry->set_surface_normal(normal);
753 new_entry->set_surface_point(plane_point);
754
755 return new_entry;
756}
757
758/**
759 *
760 */
761PT(CollisionEntry) CollisionPolygon::
762test_intersection_from_capsule(const CollisionEntry &entry) const {
763 const CollisionCapsule *capsule;
764 DCAST_INTO_R(capsule, entry.get_from(), nullptr);
765
766 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
767
768 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
769 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
770 LVector3 from_radius_v =
771 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
772 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
773 PN_stdfloat from_radius = csqrt(from_radius_2);
774
775 PN_stdfloat dist_a = get_plane().dist_to_plane(from_a);
776 PN_stdfloat dist_b = get_plane().dist_to_plane(from_b);
777
778 // Some early-outs (optional)
779 if (dist_a >= from_radius && dist_b >= from_radius) {
780 // Entirely in front of the plane means no intersection.
781 return nullptr;
782 }
783
784 if (dist_a <= -from_radius && dist_b <= -from_radius) {
785 // Entirely behind the plane also means no intersection.
786 return nullptr;
787 }
788
789 LMatrix4 to_3d_mat;
790 rederive_to_3d_mat(to_3d_mat);
791
792 // Find the intersection point between the capsule's axis and the plane.
793 LPoint3 intersect_3d;
794 if (!get_plane().intersects_line(intersect_3d, from_a, from_b)) {
795 // Completely parallel. Take an arbitrary point along the capsule axis.
796 intersect_3d = (from_a + from_b) * 0.5f;
797 }
798
799 // Find the closest point on the polygon to this intersection point.
800 LPoint2 intersect_2d = to_2d(intersect_3d);
801 LPoint2 closest_p_2d = intersect_2d;
802 PN_stdfloat best_dist_2 = -1;
803
804 size_t num_points = _points.size();
805 for (size_t i = 0; i < num_points; ++i) {
806 const LPoint2 &p1 = _points[i]._p;
807 const LPoint2 &p2 = _points[(i + 1) % num_points]._p;
808
809 // Is the intersection outside the polygon?
810 LVector2 v = intersect_2d - p1;
811 LVector2 pv = p2 - p1;
812 if (is_right(v, pv)) {
813 PN_stdfloat t = v.dot(pv) / pv.length_squared();
814 t = max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
815
816 LPoint2 p = p1 + pv * t;
817 PN_stdfloat d = (p - intersect_2d).length_squared();
818 if (best_dist_2 < 0 || d < best_dist_2) {
819 closest_p_2d = p;
820 best_dist_2 = d;
821 }
822 }
823 }
824
825 LPoint3 closest_p_3d = to_3d(closest_p_2d, to_3d_mat);
826
827 // Now find the closest point on the capsule axis to this point.
828 LVector3 from_v = from_b - from_a;
829
830 PN_stdfloat t = (closest_p_3d - from_a).dot(from_v) / from_v.length_squared();
831 LPoint3 ref_point_3d = from_a + from_v * max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
832
833 // Okay, now we have a point to apply the sphere test on.
834
835 // The nearest point within the plane to our reference is the intersection of
836 // the line (reference, reference - normal) with the plane.
837 PN_stdfloat dist;
838 if (!get_plane().intersects_line(dist, ref_point_3d, -get_normal())) {
839 // No intersection with plane? This means the plane's effective normal
840 // was within the plane itself. A useless polygon.
841 return nullptr;
842 }
843
844 if (dist > from_radius || dist < -from_radius) {
845 // No intersection with the plane.
846 return nullptr;
847 }
848
849 LPoint2 ref_point_2d = to_2d(ref_point_3d);
850 LPoint2 surface_point_2d = ref_point_2d;
851 PN_stdfloat edge_dist_2 = -1;
852
853 for (size_t i = 0; i < num_points; ++i) {
854 const LPoint2 &p1 = _points[i]._p;
855 const LPoint2 &p2 = _points[(i + 1) % num_points]._p;
856
857 // Is the intersection outside the polygon?
858 LVector2 v = ref_point_2d - p1;
859 LVector2 pv = p2 - p1;
860 if (is_right(v, pv)) {
861 PN_stdfloat t = v.dot(pv) / pv.length_squared();
862 t = max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
863
864 LPoint2 p = p1 + pv * t;
865 PN_stdfloat d = (p - ref_point_2d).length_squared();
866 if (edge_dist_2 < 0 || d < edge_dist_2) {
867 surface_point_2d = p;
868 edge_dist_2 = d;
869 }
870 }
871 }
872
873 // Now we have edge_dist_2, which is the square of the distance from the
874 // reference point to the nearest edge of the polygon, within the polygon's
875 // plane.
876
877 if (edge_dist_2 > from_radius_2) {
878 // No intersection; the circle is outside the polygon.
879 return nullptr;
880 }
881
882 // The sphere appears to intersect the polygon. If the edge is less than
883 // from_radius away, the sphere may be resting on an edge of the polygon.
884 // Determine how far the center of the sphere must remain from the plane,
885 // based on its distance from the nearest edge.
886
887 PN_stdfloat max_dist = from_radius;
888 if (edge_dist_2 >= 0.0f) {
889 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist_2, (PN_stdfloat)0.0);
890 max_dist = csqrt(max_dist_2);
891 }
892
893 if (dist > max_dist || -dist > max_dist) {
894 // There's no intersection: the sphere is hanging above or under the edge.
895 return nullptr;
896 }
897
898 if (collide_cat.is_debug()) {
899 collide_cat.debug()
900 << "intersection detected from " << entry.get_from_node_path()
901 << " into " << entry.get_into_node_path() << "\n";
902 }
903 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
904
905 LVector3 normal = (has_effective_normal() && capsule->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
906 new_entry->set_surface_normal(normal);
907
908 if ((dist_a < 0 || dist_b < 0) && !IS_NEARLY_EQUAL(dist_a, dist_b)) {
909 // We need to report the deepest point below the polygon as the interior
910 // point so that the pusher will completely push it out.
911 LPoint3 deepest_3d = (dist_a < dist_b) ? from_a : from_b;
912 LPoint2 deepest_2d = to_2d(deepest_3d);
913 surface_point_2d = deepest_2d;
914 PN_stdfloat best_dist_2 = -1;
915
916 for (size_t i = 0; i < num_points; ++i) {
917 const LPoint2 &p1 = _points[i]._p;
918 const LPoint2 &p2 = _points[(i + 1) % num_points]._p;
919
920 // Is the deepest point outside the polygon?
921 LVector2 v = deepest_2d - p1;
922 LVector2 pv = p2 - p1;
923 if (is_right(v, pv)) {
924 PN_stdfloat t = v.dot(pv) / pv.length_squared();
925 t = max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
926
927 LPoint2 p = p1 + pv * t;
928 PN_stdfloat d = (p - deepest_2d).length_squared();
929 if (best_dist_2 < 0 || d < best_dist_2) {
930 surface_point_2d = p;
931 best_dist_2 = d;
932 }
933 }
934 }
935
936 if (best_dist_2 < 0) {
937 // Deepest point is completely within the polygon, easy case.
938 new_entry->set_surface_point(deepest_3d - normal * min(dist_a, dist_b));
939 new_entry->set_interior_point(deepest_3d - normal * from_radius);
940 return new_entry;
941 }
942 }
943
944 // Colliding with an edge, use the sphere test results.
945 LPoint3 surface_point = to_3d(surface_point_2d, to_3d_mat);
946 LPoint3 interior_point = ref_point_3d - normal * max_dist;
947 new_entry->set_surface_point(surface_point);
948 new_entry->set_interior_point((interior_point - surface_point).project(normal) + surface_point);
949 return new_entry;
950}
951
952/**
953 * This is part of the double-dispatch implementation of test_intersection().
954 * It is called when the "from" object is a parabola.
955 */
956PT(CollisionEntry) CollisionPolygon::
957test_intersection_from_parabola(const CollisionEntry &entry) const {
958 if (_points.size() < 3) {
959 return nullptr;
960 }
961
962 const CollisionParabola *parabola;
963 DCAST_INTO_R(parabola, entry.get_from(), nullptr);
964
965 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
966
967 // Convert the parabola into local coordinate space.
968 LParabola local_p(parabola->get_parabola());
969 local_p.xform(wrt_mat);
970
971 PN_stdfloat t1, t2;
972 if (!get_plane().intersects_parabola(t1, t2, local_p)) {
973 // No intersection.
974 return nullptr;
975 }
976
977 PN_stdfloat t;
978 if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) {
979 if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
980 // Both intersection points are within our segment of the parabola.
981 // Choose the first of the two.
982 t = min(t1, t2);
983 } else {
984 // Only t1 is within our segment.
985 t = t1;
986 }
987
988 } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
989 // Only t2 is within our segment.
990 t = t2;
991
992 } else {
993 // Neither intersection point is within our segment.
994 return nullptr;
995 }
996
997 LPoint3 plane_point = local_p.calc_point(t);
998 LPoint2 p = to_2d(plane_point);
999
1000 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
1001 if (cpa != nullptr) {
1002 // We have a clip plane; apply it.
1003 Points new_points;
1004 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
1005 // All points are behind the clip plane.
1006 if (!point_is_inside(p, _points)) {
1007 return nullptr;
1008 }
1009
1010 } else {
1011 if (new_points.size() < 3) {
1012 return nullptr;
1013 }
1014 if (!point_is_inside(p, new_points)) {
1015 return nullptr;
1016 }
1017 }
1018
1019 } else {
1020 // No clip plane is in effect. Do the default test.
1021 if (!point_is_inside(p, _points)) {
1022 return nullptr;
1023 }
1024 }
1025
1026 if (collide_cat.is_debug()) {
1027 collide_cat.debug()
1028 << "intersection detected from " << entry.get_from_node_path()
1029 << " into " << entry.get_into_node_path() << "\n";
1030 }
1031 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
1032
1033 LVector3 normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
1034
1035 new_entry->set_surface_normal(normal);
1036 new_entry->set_surface_point(plane_point);
1037
1038 return new_entry;
1039}
1040
1041/**
1042 * This is part of the double-dispatch implementation of test_intersection().
1043 * It is called when the "from" object is a box.
1044 */
1045PT(CollisionEntry) CollisionPolygon::
1046test_intersection_from_box(const CollisionEntry &entry) const {
1047 const CollisionBox *box;
1048 DCAST_INTO_R(box, entry.get_from(), nullptr);
1049
1050 // To make things easier, transform the box into the coordinate space of the
1051 // plane.
1052 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
1053 LMatrix4 plane_mat = wrt_mat * _to_2d_mat;
1054
1055 LPoint3 from_center = box->get_center() * plane_mat;
1056 LVector3 from_extents = box->get_dimensions() * 0.5f;
1057
1058 // Determine the basis vectors describing the box.
1059 LVecBase3 box_x = plane_mat.get_row3(0) * from_extents[0];
1060 LVecBase3 box_y = plane_mat.get_row3(1) * from_extents[1];
1061 LVecBase3 box_z = plane_mat.get_row3(2) * from_extents[2];
1062
1063 // Is there a separating axis between the plane and the box?
1064 if (cabs(from_center[1]) > cabs(box_x[1]) + cabs(box_y[1]) + cabs(box_z[1])) {
1065 // There is one. No collision.
1066 return nullptr;
1067 }
1068
1069 // Now do the same for each of the box' primary axes.
1070 PN_stdfloat r1, center, r2;
1071
1072 r1 = cabs(box_x.dot(box_x)) + cabs(box_y.dot(box_x)) + cabs(box_z.dot(box_x));
1073 project(box_x, center, r2);
1074 if (cabs(from_center.dot(box_x) - center) > r1 + r2) {
1075 return nullptr;
1076 }
1077
1078 r1 = cabs(box_x.dot(box_y)) + cabs(box_y.dot(box_y)) + cabs(box_z.dot(box_y));
1079 project(box_y, center, r2);
1080 if (cabs(from_center.dot(box_y) - center) > r1 + r2) {
1081 return nullptr;
1082 }
1083
1084 r1 = cabs(box_x.dot(box_z)) + cabs(box_y.dot(box_z)) + cabs(box_z.dot(box_z));
1085 project(box_z, center, r2);
1086 if (cabs(from_center.dot(box_z) - center) > r1 + r2) {
1087 return nullptr;
1088 }
1089
1090 // Now do the same check for the cross products between the box axes and the
1091 // polygon edges.
1092 Points::const_iterator pi;
1093 for (pi = _points.begin(); pi != _points.end(); ++pi) {
1094 const PointDef &pd = *pi;
1095 LVector3 axis;
1096
1097 axis.set(-box_x[1] * pd._v[1],
1098 box_x[0] * pd._v[1] - box_x[2] * pd._v[0],
1099 box_x[1] * pd._v[0]);
1100 r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
1101 project(axis, center, r2);
1102 if (cabs(from_center.dot(axis) - center) > r1 + r2) {
1103 return nullptr;
1104 }
1105
1106 axis.set(-box_y[1] * pd._v[1],
1107 box_y[0] * pd._v[1] - box_y[2] * pd._v[0],
1108 box_y[1] * pd._v[0]);
1109 r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
1110 project(axis, center, r2);
1111 if (cabs(from_center.dot(axis) - center) > r1 + r2) {
1112 return nullptr;
1113 }
1114
1115 axis.set(-box_z[1] * pd._v[1],
1116 box_z[0] * pd._v[1] - box_z[2] * pd._v[0],
1117 box_z[1] * pd._v[0]);
1118 r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
1119 project(axis, center, r2);
1120 if (cabs(from_center.dot(axis) - center) > r1 + r2) {
1121 return nullptr;
1122 }
1123 }
1124
1125 if (collide_cat.is_debug()) {
1126 collide_cat.debug()
1127 << "intersection detected from " << entry.get_from_node_path()
1128 << " into " << entry.get_into_node_path() << "\n";
1129 }
1130 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
1131
1132 LVector3 normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
1133 new_entry->set_surface_normal(normal);
1134
1135 // Determine which point on the cube will be the interior point. This is
1136 // the calculation that is also used for the plane, which is not perfectly
1137 // applicable, but I suppose it's better than nothing.
1138 LPoint3 interior_point = box->get_center() * wrt_mat +
1139 wrt_mat.get_row3(0) * from_extents[0] * ((box_x[1] > 0) - (box_x[1] < 0)) +
1140 wrt_mat.get_row3(1) * from_extents[1] * ((box_y[1] > 0) - (box_y[1] < 0)) +
1141 wrt_mat.get_row3(2) * from_extents[2] * ((box_z[1] > 0) - (box_z[1] < 0));
1142
1143 // The surface point is the interior point projected onto the plane.
1144 new_entry->set_surface_point(get_plane().project(interior_point));
1145 new_entry->set_interior_point(interior_point);
1146
1147 return new_entry;
1148}
1149
1150/**
1151 * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
1152 * solid.
1153 */
1154void CollisionPolygon::
1155fill_viz_geom() {
1156 if (collide_cat.is_debug()) {
1157 collide_cat.debug()
1158 << "Recomputing viz for " << *this << "\n";
1159 }
1160 nassertv(_viz_geom != nullptr && _bounds_viz_geom != nullptr);
1161 draw_polygon(_viz_geom, _bounds_viz_geom, _points);
1162}
1163
1164/**
1165 * Returns the linear distance of p to the line segment defined by f and t,
1166 * where v = (t - f).normalize(). The result is negative if p is left of the
1167 * line, positive if it is right of the line. If the result is positive, it
1168 * is constrained by endpoints of the line segment (i.e. the result might be
1169 * larger than it would be for a straight distance-to-line test). If the
1170 * result is negative, we don't bother.
1171 */
1172PN_stdfloat CollisionPolygon::
1173dist_to_line_segment(const LPoint2 &p,
1174 const LPoint2 &f, const LPoint2 &t,
1175 const LVector2 &v) {
1176 LVector2 v1 = (p - f);
1177 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
1178 if (d < 0.0f) {
1179 return d;
1180 }
1181
1182 // Compute the nearest point on the line.
1183 LPoint2 q = p + LVector2(-v[1], v[0]) * d;
1184
1185 // Now constrain that point to the line segment.
1186 if (v[0] > 0.0f) {
1187 // X+
1188 if (v[1] > 0.0f) {
1189 // Y+
1190 if (v[0] > v[1]) {
1191 // X-dominant.
1192 if (q[0] < f[0]) {
1193 return (p - f).length();
1194 } if (q[0] > t[0]) {
1195 return (p - t).length();
1196 } else {
1197 return d;
1198 }
1199 } else {
1200 // Y-dominant.
1201 if (q[1] < f[1]) {
1202 return (p - f).length();
1203 } if (q[1] > t[1]) {
1204 return (p - t).length();
1205 } else {
1206 return d;
1207 }
1208 }
1209 } else {
1210 // Y-
1211 if (v[0] > -v[1]) {
1212 // X-dominant.
1213 if (q[0] < f[0]) {
1214 return (p - f).length();
1215 } if (q[0] > t[0]) {
1216 return (p - t).length();
1217 } else {
1218 return d;
1219 }
1220 } else {
1221 // Y-dominant.
1222 if (q[1] > f[1]) {
1223 return (p - f).length();
1224 } if (q[1] < t[1]) {
1225 return (p - t).length();
1226 } else {
1227 return d;
1228 }
1229 }
1230 }
1231 } else {
1232 // X-
1233 if (v[1] > 0.0f) {
1234 // Y+
1235 if (-v[0] > v[1]) {
1236 // X-dominant.
1237 if (q[0] > f[0]) {
1238 return (p - f).length();
1239 } if (q[0] < t[0]) {
1240 return (p - t).length();
1241 } else {
1242 return d;
1243 }
1244 } else {
1245 // Y-dominant.
1246 if (q[1] < f[1]) {
1247 return (p - f).length();
1248 } if (q[1] > t[1]) {
1249 return (p - t).length();
1250 } else {
1251 return d;
1252 }
1253 }
1254 } else {
1255 // Y-
1256 if (-v[0] > -v[1]) {
1257 // X-dominant.
1258 if (q[0] > f[0]) {
1259 return (p - f).length();
1260 } if (q[0] < t[0]) {
1261 return (p - t).length();
1262 } else {
1263 return d;
1264 }
1265 } else {
1266 // Y-dominant.
1267 if (q[1] > f[1]) {
1268 return (p - f).length();
1269 } if (q[1] < t[1]) {
1270 return (p - t).length();
1271 } else {
1272 return d;
1273 }
1274 }
1275 }
1276 }
1277}
1278
1279
1280/**
1281 * Now that the _p members of the given points array have been computed, go
1282 * back and compute all of the _v members.
1283 */
1284void CollisionPolygon::
1285compute_vectors(Points &points) {
1286 size_t num_points = points.size();
1287 for (size_t i = 0; i < num_points; i++) {
1288 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1289 points[i]._v.normalize();
1290 }
1291}
1292
1293/**
1294 * Fills up the indicated GeomNode with the Geoms to draw the polygon
1295 * indicated with the given set of 2-d points.
1296 */
1297void CollisionPolygon::
1298draw_polygon(GeomNode *viz_geom_node, GeomNode *bounds_viz_geom_node,
1299 const CollisionPolygon::Points &points) const {
1300 if (points.size() < 3) {
1301 if (collide_cat.is_debug()) {
1302 collide_cat.debug()
1303 << "(Degenerate poly, ignoring.)\n";
1304 }
1305 return;
1306 }
1307
1308 LMatrix4 to_3d_mat;
1309 rederive_to_3d_mat(to_3d_mat);
1310
1311 PT(GeomVertexData) vdata = new GeomVertexData
1312 ("collision", GeomVertexFormat::get_v3(),
1313 Geom::UH_static);
1314 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
1315
1316 Points::const_iterator pi;
1317 for (pi = points.begin(); pi != points.end(); ++pi) {
1318 vertex.add_data3(to_3d((*pi)._p, to_3d_mat));
1319 }
1320
1321 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
1322 body->add_consecutive_vertices(0, points.size());
1323 body->close_primitive();
1324
1325 PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static);
1326 border->add_consecutive_vertices(0, points.size());
1327 border->add_vertex(0);
1328 border->close_primitive();
1329
1330 PT(Geom) geom1 = new Geom(vdata);
1331 geom1->add_primitive(body);
1332
1333 PT(Geom) geom2 = new Geom(vdata);
1334 geom2->add_primitive(border);
1335
1336 viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_viz_state());
1337 viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_viz_state());
1338
1339 bounds_viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_bounds_viz_state());
1340 bounds_viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_bounds_viz_state());
1341}
1342
1343
1344/**
1345 * Returns true if the indicated point is within the polygon's 2-d space,
1346 * false otherwise.
1347 */
1348bool CollisionPolygon::
1349point_is_inside(const LPoint2 &p, const CollisionPolygon::Points &points) const {
1350 // We insist that the polygon be convex. This makes things a bit simpler.
1351
1352 // In the case of a convex polygon, defined with points in counterclockwise
1353 // order, a point is interior to the polygon iff the point is not right of
1354 // each of the edges.
1355 for (int i = 0; i < (int)points.size() - 1; i++) {
1356 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1357 return false;
1358 }
1359 }
1360 if (is_right(p - points[points.size() - 1]._p,
1361 points[0]._p - points[points.size() - 1]._p)) {
1362 return false;
1363 }
1364
1365 return true;
1366}
1367
1368/**
1369 * Returns the linear distance from the 2-d point to the nearest part of the
1370 * polygon defined by the points vector. The result is negative if the point
1371 * is within the polygon.
1372 */
1373PN_stdfloat CollisionPolygon::
1374dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const {
1375
1376 // We know that that the polygon is convex and is defined with the points in
1377 // counterclockwise order. Therefore, we simply compare the signed distance
1378 // to each line segment; we ignore any negative values, and take the minimum
1379 // of all the positive values.
1380
1381 // If all values are negative, the point is within the polygon; we therefore
1382 // return an arbitrary negative result.
1383
1384 bool got_dist = false;
1385 PN_stdfloat best_dist = -1.0f;
1386
1387 size_t num_points = points.size();
1388 for (size_t i = 0; i < num_points - 1; ++i) {
1389 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
1390 points[i]._v);
1391 if (d >= 0.0f) {
1392 if (!got_dist || d < best_dist) {
1393 best_dist = d;
1394 got_dist = true;
1395 }
1396 }
1397 }
1398
1399 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
1400 points[num_points - 1]._v);
1401 if (d >= 0.0f) {
1402 if (!got_dist || d < best_dist) {
1403 best_dist = d;
1404 got_dist = true;
1405 }
1406 }
1407
1408 return best_dist;
1409}
1410
1411/**
1412 * Projects the polygon onto the given axis, returning the center on the line
1413 * and the half extent.
1414 */
1415void CollisionPolygon::
1416project(const LVector3 &axis, PN_stdfloat &center, PN_stdfloat &extent) const {
1417 PN_stdfloat begin, end;
1418
1419 Points::const_iterator pi;
1420 pi = _points.begin();
1421
1422 const LPoint2 &point = (*pi)._p;
1423 begin = point[0] * axis[0] + point[1] * axis[2];
1424 end = begin;
1425
1426 for (; pi != _points.end(); ++pi) {
1427 const LPoint2 &point = (*pi)._p;
1428
1429 PN_stdfloat t = point[0] * axis[0] + point[1] * axis[2];
1430 begin = min(begin, t);
1431 end = max(end, t);
1432 }
1433
1434 center = (end + begin) * 0.5f;
1435 extent = cabs((end - begin) * 0.5f);
1436}
1437
1438/**
1439 *
1440 */
1441void CollisionPolygon::
1442setup_points(const LPoint3 *begin, const LPoint3 *end) {
1443 int num_points = end - begin;
1444 nassertv(num_points >= 3);
1445
1446 _points.clear();
1447
1448 // Tell the base CollisionPlane class what its plane will be. To do this,
1449 // we must first compute the polygon normal.
1450 LVector3 normal = LVector3::zero();
1451
1452 // Project the polygon into each of the three major planes and calculate the
1453 // area of each 2-d projection. This becomes the polygon normal. This
1454 // works because the ratio between these different areas corresponds to the
1455 // angle at which the polygon is tilted toward each plane.
1456 for (int i = 0; i < num_points; i++) {
1457 const LPoint3 &p0 = begin[i];
1458 const LPoint3 &p1 = begin[(i + 1) % num_points];
1459 normal[0] += p0[1] * p1[2] - p0[2] * p1[1];
1460 normal[1] += p0[2] * p1[0] - p0[0] * p1[2];
1461 normal[2] += p0[0] * p1[1] - p0[1] * p1[0];
1462 }
1463
1464 if (normal.length_squared() == 0.0f) {
1465 // The polygon has no area.
1466 return;
1467 }
1468
1469#ifndef NDEBUG
1470 // Make sure all the source points are good.
1471 {
1472 if (!verify_points(begin, end)) {
1473 collide_cat.error() << "Invalid points in CollisionPolygon:\n";
1474 const LPoint3 *pi;
1475 for (pi = begin; pi != end; ++pi) {
1476 collide_cat.error(false) << " " << (*pi) << "\n";
1477 }
1478 collide_cat.error(false)
1479 << " normal " << normal << " with length " << normal.length() << "\n";
1480
1481 return;
1482 }
1483 }
1484
1485 if (collide_cat.is_spam()) {
1486 collide_cat.spam()
1487 << "CollisionPolygon defined with " << num_points << " vertices:\n";
1488 const LPoint3 *pi;
1489 for (pi = begin; pi != end; ++pi) {
1490 collide_cat.spam(false) << " " << (*pi) << "\n";
1491 }
1492 }
1493#endif
1494
1495 set_plane(LPlane(normal, begin[0]));
1496
1497 // Construct a matrix that rotates the points from the (X,0,Z) plane into
1498 // the 3-d plane.
1499 LMatrix4 to_3d_mat;
1500 calc_to_3d_mat(to_3d_mat);
1501
1502 // And the inverse matrix rotates points from 3-d space into the 2-d plane.
1503 _to_2d_mat.invert_from(to_3d_mat);
1504
1505 // Now project all of the points onto the 2-d plane.
1506
1507 const LPoint3 *pi;
1508 for (pi = begin; pi != end; ++pi) {
1509 LPoint3 point = (*pi) * _to_2d_mat;
1510 _points.push_back(PointDef(point[0], point[2]));
1511 }
1512
1513 nassertv(_points.size() >= 3);
1514
1515#ifndef NDEBUG
1516 /*
1517 // Now make sure the points define a convex polygon.
1518 if (is_concave()) {
1519 collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
1520 const LPoint3 *pi;
1521 for (pi = begin; pi != end; ++pi) {
1522 collide_cat.error(false) << " " << (*pi) << "\n";
1523 }
1524 collide_cat.error(false)
1525 << " normal " << normal << " with length " << normal.length() << "\n";
1526 _points.clear();
1527 }
1528 */
1529#endif
1530
1531 compute_vectors(_points);
1532}
1533
1534/**
1535 * Converts the indicated point to 3-d space according to the way
1536 * CollisionPolygons used to be stored in bam files prior to 4.9.
1537 */
1538LPoint3 CollisionPolygon::
1539legacy_to_3d(const LVecBase2 &point2d, int axis) const {
1540 nassertr(!point2d.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
1541
1542 LVector3 normal = get_normal();
1543 PN_stdfloat D = get_plane()[3];
1544
1545 nassertr(!normal.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
1546 nassertr(!cnan(D), LPoint3(0.0f, 0.0f, 0.0f));
1547
1548 switch (axis) {
1549 case 0: // AT_x:
1550 return LPoint3(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], point2d[0], point2d[1]);
1551
1552 case 1: // AT_y:
1553 return LPoint3(point2d[0],
1554 -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], point2d[1]);
1555
1556 case 2: // AT_z:
1557 return LPoint3(point2d[0], point2d[1],
1558 -(normal[0]*point2d[0] + normal[1]*point2d[1] + D)/normal[2]);
1559 }
1560
1561 nassertr(false, LPoint3(0.0f, 0.0f, 0.0f));
1562 return LPoint3(0.0f, 0.0f, 0.0f);
1563}
1564
1565/**
1566 * Clips the source_points of the polygon by the indicated clipping plane, and
1567 * modifies new_points to reflect the new set of clipped points (but does not
1568 * compute the vectors in new_points).
1569 *
1570 * The return value is true if the set of points is unmodified (all points are
1571 * behind the clip plane), or false otherwise.
1572 */
1573bool CollisionPolygon::
1574clip_polygon(CollisionPolygon::Points &new_points,
1575 const CollisionPolygon::Points &source_points,
1576 const LPlane &plane) const {
1577 new_points.clear();
1578 if (source_points.empty()) {
1579 return true;
1580 }
1581
1582 LPoint3 from3d;
1583 LVector3 delta3d;
1584 if (!plane.intersects_plane(from3d, delta3d, get_plane())) {
1585 // The clipping plane is parallel to the polygon. The polygon is either
1586 // all in or all out.
1587 if (plane.dist_to_plane(get_plane().get_point()) < 0.0) {
1588 // A point within the polygon is behind the clipping plane: the polygon
1589 // is all in.
1590 new_points = source_points;
1591 return true;
1592 }
1593 return false;
1594 }
1595
1596 // Project the line of intersection into the 2-d plane. Now we have a 2-d
1597 // clipping line.
1598 LPoint2 from2d = to_2d(from3d);
1599 LVector2 delta2d = to_2d(delta3d);
1600
1601 PN_stdfloat a = -delta2d[1];
1602 PN_stdfloat b = delta2d[0];
1603 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
1604
1605 // Now walk through the points. Any point on the left of our line gets
1606 // removed, and the line segment clipped at the point of intersection.
1607
1608 // We might increase the number of vertices by as many as 1, if the plane
1609 // clips off exactly one corner. (We might also decrease the number of
1610 // vertices, or keep them the same number.)
1611 new_points.reserve(source_points.size() + 1);
1612
1613 LPoint2 last_point = source_points.back()._p;
1614 bool last_is_in = !is_right(last_point - from2d, delta2d);
1615 bool all_in = last_is_in;
1616 Points::const_iterator pi;
1617 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
1618 const LPoint2 &this_point = (*pi)._p;
1619 bool this_is_in = !is_right(this_point - from2d, delta2d);
1620
1621 // There appears to be a compiler bug in gcc 4.0: we need to extract this
1622 // comparison outside of the if statement.
1623 bool crossed_over = (this_is_in != last_is_in);
1624 if (crossed_over) {
1625 // We have just crossed over the clipping line. Find the point of
1626 // intersection.
1627 LVector2 d = this_point - last_point;
1628 PN_stdfloat denom = (a * d[0] + b * d[1]);
1629 if (denom != 0.0) {
1630 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
1631 LPoint2 p = last_point + t * d;
1632
1633 new_points.push_back(PointDef(p[0], p[1]));
1634 last_is_in = this_is_in;
1635 }
1636 }
1637
1638 if (this_is_in) {
1639 // We are behind the clipping line. Keep the point.
1640 new_points.push_back(PointDef(this_point[0], this_point[1]));
1641 } else {
1642 all_in = false;
1643 }
1644
1645 last_point = this_point;
1646 }
1647
1648 return all_in;
1649}
1650
1651/**
1652 * Clips the polygon by all of the clip planes named in the clip plane
1653 * attribute and fills new_points up with the resulting points.
1654 *
1655 * The return value is true if the set of points is unmodified (all points are
1656 * behind all the clip planes), or false otherwise.
1657 */
1658bool CollisionPolygon::
1659apply_clip_plane(CollisionPolygon::Points &new_points,
1660 const ClipPlaneAttrib *cpa,
1661 const TransformState *net_transform) const {
1662 bool all_in = true;
1663
1664 int num_planes = cpa->get_num_on_planes();
1665 bool first_plane = true;
1666
1667 for (int i = 0; i < num_planes; i++) {
1668 NodePath plane_path = cpa->get_on_plane(i);
1669 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
1670 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
1671 CPT(TransformState) new_transform =
1672 net_transform->invert_compose(plane_path.get_net_transform());
1673
1674 LPlane plane = plane_node->get_plane() * new_transform->get_mat();
1675 if (first_plane) {
1676 first_plane = false;
1677 if (!clip_polygon(new_points, _points, plane)) {
1678 all_in = false;
1679 }
1680 } else {
1681 Points last_points;
1682 last_points.swap(new_points);
1683 if (!clip_polygon(new_points, last_points, plane)) {
1684 all_in = false;
1685 }
1686 }
1687 }
1688 }
1689
1690 if (!all_in) {
1691 compute_vectors(new_points);
1692 }
1693
1694 return all_in;
1695}
1696
1697/**
1698 * Function to write the important information in the particular object to a
1699 * Datagram
1700 */
1702write_datagram(BamWriter *manager, Datagram &me) {
1703 CollisionPlane::write_datagram(manager, me);
1704 me.add_uint16(_points.size());
1705 for (size_t i = 0; i < _points.size(); i++) {
1706 _points[i]._p.write_datagram(me);
1707 _points[i]._v.write_datagram(me);
1708 }
1709 _to_2d_mat.write_datagram(me);
1710}
1711
1712/**
1713 * Function that reads out of the datagram (or asks manager to read) all of
1714 * the data that is needed to re-create this object and stores it in the
1715 * appropiate place
1716 */
1717void CollisionPolygon::
1718fillin(DatagramIterator &scan, BamReader *manager) {
1719 CollisionPlane::fillin(scan, manager);
1720
1721 size_t size = scan.get_uint16();
1722 for (size_t i = 0; i < size; i++) {
1723 LPoint2 p;
1724 LVector2 v;
1725 p.read_datagram(scan);
1726 v.read_datagram(scan);
1727 _points.push_back(PointDef(p, v));
1728 }
1729 _to_2d_mat.read_datagram(scan);
1730
1731 if (manager->get_file_minor_ver() < 13) {
1732 // Before bam version 6.13, we were inadvertently storing CollisionPolygon
1733 // vertices clockwise, instead of counter-clockwise. Correct that by re-
1734 // projecting.
1735 if (_points.size() >= 3) {
1736 LMatrix4 to_3d_mat;
1737 rederive_to_3d_mat(to_3d_mat);
1738
1739 epvector<LPoint3> verts;
1740 verts.reserve(_points.size());
1741 Points::const_iterator pi;
1742 for (pi = _points.begin(); pi != _points.end(); ++pi) {
1743 verts.push_back(to_3d((*pi)._p, to_3d_mat));
1744 }
1745
1746 const LPoint3 *verts_begin = &verts[0];
1747 const LPoint3 *verts_end = verts_begin + verts.size();
1748 setup_points(verts_begin, verts_end);
1749 }
1750 }
1751}
1752
1753
1754/**
1755 * Factory method to generate a CollisionPolygon object
1756 */
1758make_CollisionPolygon(const FactoryParams &params) {
1760 DatagramIterator scan;
1761 BamReader *manager;
1762
1763 parse_params(params, scan, manager);
1764 me->fillin(scan, manager);
1765 return me;
1766}
1767
1768/**
1769 * Factory method to generate a CollisionPolygon object
1770 */
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
void parse_params(const FactoryParams &params, DatagramIterator &scan, BamReader *&manager)
Takes in a FactoryParams, passed from a WritableFactory into any TypedWritable's make function,...
Definition bamReader.I:275
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition bamReader.h:110
int get_file_minor_ver() const
Returns the minor version number of the Bam file currently being read.
Definition bamReader.I:83
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition bamReader.I:177
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
Definition bamWriter.h:63
An axis-aligned bounding box; that is, a minimum and maximum coordinate triple.
Definition boundingBox.h:29
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
This functions similarly to a LightAttrib.
get_on_plane
Returns the nth plane enabled by the attribute, sorted in render order.
get_num_on_planes
Returns the number of planes that are enabled by the attribute.
A cuboid collision volume or object.
This implements a solid consisting of a cylinder with hemispherical endcaps, also known as a capsule ...
Defines a single collision event.
get_from_node_path
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
get_into_node_path
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
get_from
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
An infinite line, similar to a CollisionRay, except that it extends in both directions.
This defines a parabolic arc, or subset of an arc, similar to the path of a projectile or falling obj...
get_t1
Returns the starting point on the parabola.
get_parabola
Returns the parabola specified by this solid.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
static bool verify_points(const LPoint3 *begin, const LPoint3 *end)
Verifies that the indicated set of points will define a valid CollisionPolygon: that is,...
get_point
Returns the nth vertex of the CollisionPolygon, expressed in 3-D space.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
is_concave
Returns true if the CollisionPolygon appears to be concave, or false if it is safely convex.
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
static void register_with_read_factory()
Factory method to generate a CollisionPolygon object.
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
static TypedWritable * make_CollisionPolygon(const FactoryParams &params)
Factory method to generate a CollisionPolygon object.
is_valid
Returns true if the CollisionPolygon is valid (that is, it has at least three vertices),...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
An infinite ray, with a specific origin and direction.
A finite line segment, with two specific endpoints but no thickness.
The abstract base class for all things that can collide with other things in the world,...
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
get_respect_effective_normal
See set_respect_effective_normal().
A spherical collision volume or object.
This collects together the pieces of data that are accumulated for each node while walking the scene ...
This object performs a depth-first traversal of the scene graph, with optional view-frustum culling,...
A class to retrieve the individual data elements previously stored in a Datagram.
uint16_t get_uint16()
Extracts an unsigned 16-bit integer.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition datagram.h:38
void add_uint16(uint16_t value)
Adds an unsigned 16-bit integer to the datagram.
Definition datagram.I:85
An instance of this class is passed to the Factory when requesting it to do its business and construc...
void register_factory(TypeHandle handle, CreateFunc *func, void *user_data=nullptr)
Registers a new kind of thing the Factory will be able to create.
Definition factory.I:73
Defines a series of line strips.
A node that holds Geom objects, renderable pieces of geometry.
Definition geomNode.h:34
void add_geom(Geom *geom, const RenderState *state=RenderState::make_empty())
Adds a new Geom to the node.
Definition geomNode.cxx:612
Defines a series of triangle fans.
Definition geomTrifans.h:23
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
static const GeomVertexFormat * get_v3()
Returns a standard vertex format with just a 3-component vertex position.
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
A container for geometry primitives.
Definition geom.h:54
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition nodePath.h:159
PandaNode * node() const
Returns the referenced node of the path.
Definition nodePath.I:227
A lightweight class that represents a single element that may be timed and/or counted via stats.
A basic node of the scene graph or data graph.
Definition pandaNode.h:65
A node that contains a plane.
Definition planeNode.h:36
get_clip_effect
Returns the clip_effect bits for this clip plane.
Definition planeNode.h:71
get_plane
Returns the plane represented by the PlaneNode.
Definition planeNode.h:68
Indicates a coordinate-system transform on vertices.
TypeHandle is the identifier used to differentiate C++ class types.
Definition typeHandle.h:81
Base class for objects that can be written to and read from Bam files.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
std::ostream & indent(std::ostream &out, int indent_level)
A handy function for doing text formatting.
Definition indent.cxx:20
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.