Panda3D
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 
44 using std::max;
45 using std::min;
46 
47 PStatCollector CollisionPolygon::_volume_pcollector("Collision Volumes:CollisionPolygon");
48 PStatCollector CollisionPolygon::_test_pcollector("Collision Tests:CollisionPolygon");
49 TypeHandle CollisionPolygon::_type_handle;
50 
51 /**
52  *
53  */
54 CollisionPolygon::
55 CollisionPolygon(const CollisionPolygon &copy) :
56  CollisionPlane(copy),
57  _points(copy._points),
58  _to_2d_mat(copy._to_2d_mat)
59 {
60 }
61 
62 /**
63  *
64  */
65 CollisionSolid *CollisionPolygon::
66 make_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  */
80 verify_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  */
129 is_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  */
138 is_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  */
179 xform(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  */
219 get_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  */
237 PT(PandaNode) CollisionPolygon::
238 get_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  */
301 void CollisionPolygon::
302 output(std::ostream &out) const {
303  out << "cpolygon, (" << get_plane()
304  << "), " << _points.size() << " vertices";
305 }
306 
307 /**
308  *
309  */
310 void CollisionPolygon::
311 write(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  */
330 PT(BoundingVolume) CollisionPolygon::
331 compute_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  */
363 PT(CollisionEntry) CollisionPolygon::
364 test_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  */
546 PT(CollisionEntry) CollisionPolygon::
547 test_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  */
614 PT(CollisionEntry) CollisionPolygon::
615 test_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  */
687 PT(CollisionEntry) CollisionPolygon::
688 test_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  * This is part of the double-dispatch implementation of test_intersection().
760  * It is called when the "from" object is a parabola.
761  */
762 PT(CollisionEntry) CollisionPolygon::
763 test_intersection_from_parabola(const CollisionEntry &entry) const {
764  if (_points.size() < 3) {
765  return nullptr;
766  }
767 
768  const CollisionParabola *parabola;
769  DCAST_INTO_R(parabola, entry.get_from(), nullptr);
770 
771  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
772 
773  // Convert the parabola into local coordinate space.
774  LParabola local_p(parabola->get_parabola());
775  local_p.xform(wrt_mat);
776 
777  PN_stdfloat t1, t2;
778  if (!get_plane().intersects_parabola(t1, t2, local_p)) {
779  // No intersection.
780  return nullptr;
781  }
782 
783  PN_stdfloat t;
784  if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) {
785  if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
786  // Both intersection points are within our segment of the parabola.
787  // Choose the first of the two.
788  t = min(t1, t2);
789  } else {
790  // Only t1 is within our segment.
791  t = t1;
792  }
793 
794  } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
795  // Only t2 is within our segment.
796  t = t2;
797 
798  } else {
799  // Neither intersection point is within our segment.
800  return nullptr;
801  }
802 
803  LPoint3 plane_point = local_p.calc_point(t);
804  LPoint2 p = to_2d(plane_point);
805 
806  const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
807  if (cpa != nullptr) {
808  // We have a clip plane; apply it.
809  Points new_points;
810  if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) {
811  // All points are behind the clip plane.
812  if (!point_is_inside(p, _points)) {
813  return nullptr;
814  }
815 
816  } else {
817  if (new_points.size() < 3) {
818  return nullptr;
819  }
820  if (!point_is_inside(p, new_points)) {
821  return nullptr;
822  }
823  }
824 
825  } else {
826  // No clip plane is in effect. Do the default test.
827  if (!point_is_inside(p, _points)) {
828  return nullptr;
829  }
830  }
831 
832  if (collide_cat.is_debug()) {
833  collide_cat.debug()
834  << "intersection detected from " << entry.get_from_node_path()
835  << " into " << entry.get_into_node_path() << "\n";
836  }
837  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
838 
839  LVector3 normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
840 
841  new_entry->set_surface_normal(normal);
842  new_entry->set_surface_point(plane_point);
843 
844  return new_entry;
845 }
846 
847 /**
848  * This is part of the double-dispatch implementation of test_intersection().
849  * It is called when the "from" object is a box.
850  */
851 PT(CollisionEntry) CollisionPolygon::
852 test_intersection_from_box(const CollisionEntry &entry) const {
853  const CollisionBox *box;
854  DCAST_INTO_R(box, entry.get_from(), nullptr);
855 
856  // To make things easier, transform the box into the coordinate space of the
857  // plane.
858  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
859  LMatrix4 plane_mat = wrt_mat * _to_2d_mat;
860 
861  LPoint3 from_center = box->get_center() * plane_mat;
862  LVector3 from_extents = box->get_dimensions() * 0.5f;
863 
864  // Determine the basis vectors describing the box.
865  LVecBase3 box_x = plane_mat.get_row3(0) * from_extents[0];
866  LVecBase3 box_y = plane_mat.get_row3(1) * from_extents[1];
867  LVecBase3 box_z = plane_mat.get_row3(2) * from_extents[2];
868 
869  // Is there a separating axis between the plane and the box?
870  if (cabs(from_center[1]) > cabs(box_x[1]) + cabs(box_y[1]) + cabs(box_z[1])) {
871  // There is one. No collision.
872  return nullptr;
873  }
874 
875  // Now do the same for each of the box' primary axes.
876  PN_stdfloat r1, center, r2;
877 
878  r1 = cabs(box_x.dot(box_x)) + cabs(box_y.dot(box_x)) + cabs(box_z.dot(box_x));
879  project(box_x, center, r2);
880  if (cabs(from_center.dot(box_x) - center) > r1 + r2) {
881  return nullptr;
882  }
883 
884  r1 = cabs(box_x.dot(box_y)) + cabs(box_y.dot(box_y)) + cabs(box_z.dot(box_y));
885  project(box_y, center, r2);
886  if (cabs(from_center.dot(box_y) - center) > r1 + r2) {
887  return nullptr;
888  }
889 
890  r1 = cabs(box_x.dot(box_z)) + cabs(box_y.dot(box_z)) + cabs(box_z.dot(box_z));
891  project(box_z, center, r2);
892  if (cabs(from_center.dot(box_z) - center) > r1 + r2) {
893  return nullptr;
894  }
895 
896  // Now do the same check for the cross products between the box axes and the
897  // polygon edges.
898  Points::const_iterator pi;
899  for (pi = _points.begin(); pi != _points.end(); ++pi) {
900  const PointDef &pd = *pi;
901  LVector3 axis;
902 
903  axis.set(-box_x[1] * pd._v[1],
904  box_x[0] * pd._v[1] - box_x[2] * pd._v[0],
905  box_x[1] * pd._v[0]);
906  r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
907  project(axis, center, r2);
908  if (cabs(from_center.dot(axis) - center) > r1 + r2) {
909  return nullptr;
910  }
911 
912  axis.set(-box_y[1] * pd._v[1],
913  box_y[0] * pd._v[1] - box_y[2] * pd._v[0],
914  box_y[1] * pd._v[0]);
915  r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
916  project(axis, center, r2);
917  if (cabs(from_center.dot(axis) - center) > r1 + r2) {
918  return nullptr;
919  }
920 
921  axis.set(-box_z[1] * pd._v[1],
922  box_z[0] * pd._v[1] - box_z[2] * pd._v[0],
923  box_z[1] * pd._v[0]);
924  r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
925  project(axis, center, r2);
926  if (cabs(from_center.dot(axis) - center) > r1 + r2) {
927  return nullptr;
928  }
929  }
930 
931  if (collide_cat.is_debug()) {
932  collide_cat.debug()
933  << "intersection detected from " << entry.get_from_node_path()
934  << " into " << entry.get_into_node_path() << "\n";
935  }
936  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
937 
938  LVector3 normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
939  new_entry->set_surface_normal(normal);
940 
941  // Determine which point on the cube will be the interior point. This is
942  // the calculation that is also used for the plane, which is not perfectly
943  // applicable, but I suppose it's better than nothing.
944  LPoint3 interior_point = box->get_center() * wrt_mat +
945  wrt_mat.get_row3(0) * from_extents[0] * ((box_x[1] > 0) - (box_x[1] < 0)) +
946  wrt_mat.get_row3(1) * from_extents[1] * ((box_y[1] > 0) - (box_y[1] < 0)) +
947  wrt_mat.get_row3(2) * from_extents[2] * ((box_z[1] > 0) - (box_z[1] < 0));
948 
949  // The surface point is the interior point projected onto the plane.
950  new_entry->set_surface_point(get_plane().project(interior_point));
951  new_entry->set_interior_point(interior_point);
952 
953  return new_entry;
954 }
955 
956 /**
957  * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
958  * solid.
959  */
960 void CollisionPolygon::
961 fill_viz_geom() {
962  if (collide_cat.is_debug()) {
963  collide_cat.debug()
964  << "Recomputing viz for " << *this << "\n";
965  }
966  nassertv(_viz_geom != nullptr && _bounds_viz_geom != nullptr);
967  draw_polygon(_viz_geom, _bounds_viz_geom, _points);
968 }
969 
970 /**
971  * Returns the linear distance of p to the line segment defined by f and t,
972  * where v = (t - f).normalize(). The result is negative if p is left of the
973  * line, positive if it is right of the line. If the result is positive, it
974  * is constrained by endpoints of the line segment (i.e. the result might be
975  * larger than it would be for a straight distance-to-line test). If the
976  * result is negative, we don't bother.
977  */
978 PN_stdfloat CollisionPolygon::
979 dist_to_line_segment(const LPoint2 &p,
980  const LPoint2 &f, const LPoint2 &t,
981  const LVector2 &v) {
982  LVector2 v1 = (p - f);
983  PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
984  if (d < 0.0f) {
985  return d;
986  }
987 
988  // Compute the nearest point on the line.
989  LPoint2 q = p + LVector2(-v[1], v[0]) * d;
990 
991  // Now constrain that point to the line segment.
992  if (v[0] > 0.0f) {
993  // X+
994  if (v[1] > 0.0f) {
995  // Y+
996  if (v[0] > v[1]) {
997  // X-dominant.
998  if (q[0] < f[0]) {
999  return (p - f).length();
1000  } if (q[0] > t[0]) {
1001  return (p - t).length();
1002  } else {
1003  return d;
1004  }
1005  } else {
1006  // Y-dominant.
1007  if (q[1] < f[1]) {
1008  return (p - f).length();
1009  } if (q[1] > t[1]) {
1010  return (p - t).length();
1011  } else {
1012  return d;
1013  }
1014  }
1015  } else {
1016  // Y-
1017  if (v[0] > -v[1]) {
1018  // X-dominant.
1019  if (q[0] < f[0]) {
1020  return (p - f).length();
1021  } if (q[0] > t[0]) {
1022  return (p - t).length();
1023  } else {
1024  return d;
1025  }
1026  } else {
1027  // Y-dominant.
1028  if (q[1] > f[1]) {
1029  return (p - f).length();
1030  } if (q[1] < t[1]) {
1031  return (p - t).length();
1032  } else {
1033  return d;
1034  }
1035  }
1036  }
1037  } else {
1038  // X-
1039  if (v[1] > 0.0f) {
1040  // Y+
1041  if (-v[0] > v[1]) {
1042  // X-dominant.
1043  if (q[0] > f[0]) {
1044  return (p - f).length();
1045  } if (q[0] < t[0]) {
1046  return (p - t).length();
1047  } else {
1048  return d;
1049  }
1050  } else {
1051  // Y-dominant.
1052  if (q[1] < f[1]) {
1053  return (p - f).length();
1054  } if (q[1] > t[1]) {
1055  return (p - t).length();
1056  } else {
1057  return d;
1058  }
1059  }
1060  } else {
1061  // Y-
1062  if (-v[0] > -v[1]) {
1063  // X-dominant.
1064  if (q[0] > f[0]) {
1065  return (p - f).length();
1066  } if (q[0] < t[0]) {
1067  return (p - t).length();
1068  } else {
1069  return d;
1070  }
1071  } else {
1072  // Y-dominant.
1073  if (q[1] > f[1]) {
1074  return (p - f).length();
1075  } if (q[1] < t[1]) {
1076  return (p - t).length();
1077  } else {
1078  return d;
1079  }
1080  }
1081  }
1082  }
1083 }
1084 
1085 
1086 /**
1087  * Now that the _p members of the given points array have been computed, go
1088  * back and compute all of the _v members.
1089  */
1090 void CollisionPolygon::
1091 compute_vectors(Points &points) {
1092  size_t num_points = points.size();
1093  for (size_t i = 0; i < num_points; i++) {
1094  points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1095  points[i]._v.normalize();
1096  }
1097 }
1098 
1099 /**
1100  * Fills up the indicated GeomNode with the Geoms to draw the polygon
1101  * indicated with the given set of 2-d points.
1102  */
1103 void CollisionPolygon::
1104 draw_polygon(GeomNode *viz_geom_node, GeomNode *bounds_viz_geom_node,
1105  const CollisionPolygon::Points &points) const {
1106  if (points.size() < 3) {
1107  if (collide_cat.is_debug()) {
1108  collide_cat.debug()
1109  << "(Degenerate poly, ignoring.)\n";
1110  }
1111  return;
1112  }
1113 
1114  LMatrix4 to_3d_mat;
1115  rederive_to_3d_mat(to_3d_mat);
1116 
1117  PT(GeomVertexData) vdata = new GeomVertexData
1118  ("collision", GeomVertexFormat::get_v3(),
1119  Geom::UH_static);
1120  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
1121 
1122  Points::const_iterator pi;
1123  for (pi = points.begin(); pi != points.end(); ++pi) {
1124  vertex.add_data3(to_3d((*pi)._p, to_3d_mat));
1125  }
1126 
1127  PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
1128  body->add_consecutive_vertices(0, points.size());
1129  body->close_primitive();
1130 
1131  PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static);
1132  border->add_consecutive_vertices(0, points.size());
1133  border->add_vertex(0);
1134  border->close_primitive();
1135 
1136  PT(Geom) geom1 = new Geom(vdata);
1137  geom1->add_primitive(body);
1138 
1139  PT(Geom) geom2 = new Geom(vdata);
1140  geom2->add_primitive(border);
1141 
1142  viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_viz_state());
1143  viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_viz_state());
1144 
1145  bounds_viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_bounds_viz_state());
1146  bounds_viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_bounds_viz_state());
1147 }
1148 
1149 
1150 /**
1151  * Returns true if the indicated point is within the polygon's 2-d space,
1152  * false otherwise.
1153  */
1154 bool CollisionPolygon::
1155 point_is_inside(const LPoint2 &p, const CollisionPolygon::Points &points) const {
1156  // We insist that the polygon be convex. This makes things a bit simpler.
1157 
1158  // In the case of a convex polygon, defined with points in counterclockwise
1159  // order, a point is interior to the polygon iff the point is not right of
1160  // each of the edges.
1161  for (int i = 0; i < (int)points.size() - 1; i++) {
1162  if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1163  return false;
1164  }
1165  }
1166  if (is_right(p - points[points.size() - 1]._p,
1167  points[0]._p - points[points.size() - 1]._p)) {
1168  return false;
1169  }
1170 
1171  return true;
1172 }
1173 
1174 /**
1175  * Returns the linear distance from the 2-d point to the nearest part of the
1176  * polygon defined by the points vector. The result is negative if the point
1177  * is within the polygon.
1178  */
1179 PN_stdfloat CollisionPolygon::
1180 dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const {
1181 
1182  // We know that that the polygon is convex and is defined with the points in
1183  // counterclockwise order. Therefore, we simply compare the signed distance
1184  // to each line segment; we ignore any negative values, and take the minimum
1185  // of all the positive values.
1186 
1187  // If all values are negative, the point is within the polygon; we therefore
1188  // return an arbitrary negative result.
1189 
1190  bool got_dist = false;
1191  PN_stdfloat best_dist = -1.0f;
1192 
1193  size_t num_points = points.size();
1194  for (size_t i = 0; i < num_points - 1; ++i) {
1195  PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
1196  points[i]._v);
1197  if (d >= 0.0f) {
1198  if (!got_dist || d < best_dist) {
1199  best_dist = d;
1200  got_dist = true;
1201  }
1202  }
1203  }
1204 
1205  PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
1206  points[num_points - 1]._v);
1207  if (d >= 0.0f) {
1208  if (!got_dist || d < best_dist) {
1209  best_dist = d;
1210  got_dist = true;
1211  }
1212  }
1213 
1214  return best_dist;
1215 }
1216 
1217 /**
1218  * Projects the polygon onto the given axis, returning the center on the line
1219  * and the half extent.
1220  */
1221 void CollisionPolygon::
1222 project(const LVector3 &axis, PN_stdfloat &center, PN_stdfloat &extent) const {
1223  PN_stdfloat begin, end;
1224 
1225  Points::const_iterator pi;
1226  pi = _points.begin();
1227 
1228  const LPoint2 &point = (*pi)._p;
1229  begin = point[0] * axis[0] + point[1] * axis[2];
1230  end = begin;
1231 
1232  for (; pi != _points.end(); ++pi) {
1233  const LPoint2 &point = (*pi)._p;
1234 
1235  PN_stdfloat t = point[0] * axis[0] + point[1] * axis[2];
1236  begin = min(begin, t);
1237  end = max(end, t);
1238  }
1239 
1240  center = (end + begin) * 0.5f;
1241  extent = cabs((end - begin) * 0.5f);
1242 }
1243 
1244 /**
1245  *
1246  */
1247 void CollisionPolygon::
1248 setup_points(const LPoint3 *begin, const LPoint3 *end) {
1249  int num_points = end - begin;
1250  nassertv(num_points >= 3);
1251 
1252  _points.clear();
1253 
1254  // Tell the base CollisionPlane class what its plane will be. To do this,
1255  // we must first compute the polygon normal.
1256  LVector3 normal = LVector3::zero();
1257 
1258  // Project the polygon into each of the three major planes and calculate the
1259  // area of each 2-d projection. This becomes the polygon normal. This
1260  // works because the ratio between these different areas corresponds to the
1261  // angle at which the polygon is tilted toward each plane.
1262  for (int i = 0; i < num_points; i++) {
1263  const LPoint3 &p0 = begin[i];
1264  const LPoint3 &p1 = begin[(i + 1) % num_points];
1265  normal[0] += p0[1] * p1[2] - p0[2] * p1[1];
1266  normal[1] += p0[2] * p1[0] - p0[0] * p1[2];
1267  normal[2] += p0[0] * p1[1] - p0[1] * p1[0];
1268  }
1269 
1270  if (normal.length_squared() == 0.0f) {
1271  // The polygon has no area.
1272  return;
1273  }
1274 
1275 #ifndef NDEBUG
1276  // Make sure all the source points are good.
1277  {
1278  if (!verify_points(begin, end)) {
1279  collide_cat.error() << "Invalid points in CollisionPolygon:\n";
1280  const LPoint3 *pi;
1281  for (pi = begin; pi != end; ++pi) {
1282  collide_cat.error(false) << " " << (*pi) << "\n";
1283  }
1284  collide_cat.error(false)
1285  << " normal " << normal << " with length " << normal.length() << "\n";
1286 
1287  return;
1288  }
1289  }
1290 
1291  if (collide_cat.is_spam()) {
1292  collide_cat.spam()
1293  << "CollisionPolygon defined with " << num_points << " vertices:\n";
1294  const LPoint3 *pi;
1295  for (pi = begin; pi != end; ++pi) {
1296  collide_cat.spam(false) << " " << (*pi) << "\n";
1297  }
1298  }
1299 #endif
1300 
1301  set_plane(LPlane(normal, begin[0]));
1302 
1303  // Construct a matrix that rotates the points from the (X,0,Z) plane into
1304  // the 3-d plane.
1305  LMatrix4 to_3d_mat;
1306  calc_to_3d_mat(to_3d_mat);
1307 
1308  // And the inverse matrix rotates points from 3-d space into the 2-d plane.
1309  _to_2d_mat.invert_from(to_3d_mat);
1310 
1311  // Now project all of the points onto the 2-d plane.
1312 
1313  const LPoint3 *pi;
1314  for (pi = begin; pi != end; ++pi) {
1315  LPoint3 point = (*pi) * _to_2d_mat;
1316  _points.push_back(PointDef(point[0], point[2]));
1317  }
1318 
1319  nassertv(_points.size() >= 3);
1320 
1321 #ifndef NDEBUG
1322  /*
1323  // Now make sure the points define a convex polygon.
1324  if (is_concave()) {
1325  collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
1326  const LPoint3 *pi;
1327  for (pi = begin; pi != end; ++pi) {
1328  collide_cat.error(false) << " " << (*pi) << "\n";
1329  }
1330  collide_cat.error(false)
1331  << " normal " << normal << " with length " << normal.length() << "\n";
1332  _points.clear();
1333  }
1334  */
1335 #endif
1336 
1337  compute_vectors(_points);
1338 }
1339 
1340 /**
1341  * Converts the indicated point to 3-d space according to the way
1342  * CollisionPolygons used to be stored in bam files prior to 4.9.
1343  */
1344 LPoint3 CollisionPolygon::
1345 legacy_to_3d(const LVecBase2 &point2d, int axis) const {
1346  nassertr(!point2d.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
1347 
1348  LVector3 normal = get_normal();
1349  PN_stdfloat D = get_plane()[3];
1350 
1351  nassertr(!normal.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
1352  nassertr(!cnan(D), LPoint3(0.0f, 0.0f, 0.0f));
1353 
1354  switch (axis) {
1355  case 0: // AT_x:
1356  return LPoint3(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], point2d[0], point2d[1]);
1357 
1358  case 1: // AT_y:
1359  return LPoint3(point2d[0],
1360  -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], point2d[1]);
1361 
1362  case 2: // AT_z:
1363  return LPoint3(point2d[0], point2d[1],
1364  -(normal[0]*point2d[0] + normal[1]*point2d[1] + D)/normal[2]);
1365  }
1366 
1367  nassertr(false, LPoint3(0.0f, 0.0f, 0.0f));
1368  return LPoint3(0.0f, 0.0f, 0.0f);
1369 }
1370 
1371 /**
1372  * Clips the source_points of the polygon by the indicated clipping plane, and
1373  * modifies new_points to reflect the new set of clipped points (but does not
1374  * compute the vectors in new_points).
1375  *
1376  * The return value is true if the set of points is unmodified (all points are
1377  * behind the clip plane), or false otherwise.
1378  */
1379 bool CollisionPolygon::
1380 clip_polygon(CollisionPolygon::Points &new_points,
1381  const CollisionPolygon::Points &source_points,
1382  const LPlane &plane) const {
1383  new_points.clear();
1384  if (source_points.empty()) {
1385  return true;
1386  }
1387 
1388  LPoint3 from3d;
1389  LVector3 delta3d;
1390  if (!plane.intersects_plane(from3d, delta3d, get_plane())) {
1391  // The clipping plane is parallel to the polygon. The polygon is either
1392  // all in or all out.
1393  if (plane.dist_to_plane(get_plane().get_point()) < 0.0) {
1394  // A point within the polygon is behind the clipping plane: the polygon
1395  // is all in.
1396  new_points = source_points;
1397  return true;
1398  }
1399  return false;
1400  }
1401 
1402  // Project the line of intersection into the 2-d plane. Now we have a 2-d
1403  // clipping line.
1404  LPoint2 from2d = to_2d(from3d);
1405  LVector2 delta2d = to_2d(delta3d);
1406 
1407  PN_stdfloat a = -delta2d[1];
1408  PN_stdfloat b = delta2d[0];
1409  PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
1410 
1411  // Now walk through the points. Any point on the left of our line gets
1412  // removed, and the line segment clipped at the point of intersection.
1413 
1414  // We might increase the number of vertices by as many as 1, if the plane
1415  // clips off exactly one corner. (We might also decrease the number of
1416  // vertices, or keep them the same number.)
1417  new_points.reserve(source_points.size() + 1);
1418 
1419  LPoint2 last_point = source_points.back()._p;
1420  bool last_is_in = !is_right(last_point - from2d, delta2d);
1421  bool all_in = last_is_in;
1422  Points::const_iterator pi;
1423  for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
1424  const LPoint2 &this_point = (*pi)._p;
1425  bool this_is_in = !is_right(this_point - from2d, delta2d);
1426 
1427  // There appears to be a compiler bug in gcc 4.0: we need to extract this
1428  // comparison outside of the if statement.
1429  bool crossed_over = (this_is_in != last_is_in);
1430  if (crossed_over) {
1431  // We have just crossed over the clipping line. Find the point of
1432  // intersection.
1433  LVector2 d = this_point - last_point;
1434  PN_stdfloat denom = (a * d[0] + b * d[1]);
1435  if (denom != 0.0) {
1436  PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
1437  LPoint2 p = last_point + t * d;
1438 
1439  new_points.push_back(PointDef(p[0], p[1]));
1440  last_is_in = this_is_in;
1441  }
1442  }
1443 
1444  if (this_is_in) {
1445  // We are behind the clipping line. Keep the point.
1446  new_points.push_back(PointDef(this_point[0], this_point[1]));
1447  } else {
1448  all_in = false;
1449  }
1450 
1451  last_point = this_point;
1452  }
1453 
1454  return all_in;
1455 }
1456 
1457 /**
1458  * Clips the polygon by all of the clip planes named in the clip plane
1459  * attribute and fills new_points up with the resulting points.
1460  *
1461  * The return value is true if the set of points is unmodified (all points are
1462  * behind all the clip planes), or false otherwise.
1463  */
1464 bool CollisionPolygon::
1465 apply_clip_plane(CollisionPolygon::Points &new_points,
1466  const ClipPlaneAttrib *cpa,
1467  const TransformState *net_transform) const {
1468  bool all_in = true;
1469 
1470  int num_planes = cpa->get_num_on_planes();
1471  bool first_plane = true;
1472 
1473  for (int i = 0; i < num_planes; i++) {
1474  NodePath plane_path = cpa->get_on_plane(i);
1475  PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
1476  if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
1477  CPT(TransformState) new_transform =
1478  net_transform->invert_compose(plane_path.get_net_transform());
1479 
1480  LPlane plane = plane_node->get_plane() * new_transform->get_mat();
1481  if (first_plane) {
1482  first_plane = false;
1483  if (!clip_polygon(new_points, _points, plane)) {
1484  all_in = false;
1485  }
1486  } else {
1487  Points last_points;
1488  last_points.swap(new_points);
1489  if (!clip_polygon(new_points, last_points, plane)) {
1490  all_in = false;
1491  }
1492  }
1493  }
1494  }
1495 
1496  if (!all_in) {
1497  compute_vectors(new_points);
1498  }
1499 
1500  return all_in;
1501 }
1502 
1503 /**
1504  * Function to write the important information in the particular object to a
1505  * Datagram
1506  */
1508 write_datagram(BamWriter *manager, Datagram &me) {
1509  CollisionPlane::write_datagram(manager, me);
1510  me.add_uint16(_points.size());
1511  for (size_t i = 0; i < _points.size(); i++) {
1512  _points[i]._p.write_datagram(me);
1513  _points[i]._v.write_datagram(me);
1514  }
1515  _to_2d_mat.write_datagram(me);
1516 }
1517 
1518 /**
1519  * Function that reads out of the datagram (or asks manager to read) all of
1520  * the data that is needed to re-create this object and stores it in the
1521  * appropiate place
1522  */
1523 void CollisionPolygon::
1524 fillin(DatagramIterator &scan, BamReader *manager) {
1525  CollisionPlane::fillin(scan, manager);
1526 
1527  size_t size = scan.get_uint16();
1528  for (size_t i = 0; i < size; i++) {
1529  LPoint2 p;
1530  LVector2 v;
1531  p.read_datagram(scan);
1532  v.read_datagram(scan);
1533  _points.push_back(PointDef(p, v));
1534  }
1535  _to_2d_mat.read_datagram(scan);
1536 
1537  if (manager->get_file_minor_ver() < 13) {
1538  // Before bam version 6.13, we were inadvertently storing CollisionPolygon
1539  // vertices clockwise, instead of counter-clockwise. Correct that by re-
1540  // projecting.
1541  if (_points.size() >= 3) {
1542  LMatrix4 to_3d_mat;
1543  rederive_to_3d_mat(to_3d_mat);
1544 
1545  epvector<LPoint3> verts;
1546  verts.reserve(_points.size());
1547  Points::const_iterator pi;
1548  for (pi = _points.begin(); pi != _points.end(); ++pi) {
1549  verts.push_back(to_3d((*pi)._p, to_3d_mat));
1550  }
1551 
1552  const LPoint3 *verts_begin = &verts[0];
1553  const LPoint3 *verts_end = verts_begin + verts.size();
1554  setup_points(verts_begin, verts_end);
1555  }
1556  }
1557 }
1558 
1559 
1560 /**
1561  * Factory method to generate a CollisionPolygon object
1562  */
1564 make_CollisionPolygon(const FactoryParams &params) {
1566  DatagramIterator scan;
1567  BamReader *manager;
1568 
1569  parse_params(params, scan, manager);
1570  me->fillin(scan, manager);
1571  return me;
1572 }
1573 
1574 /**
1575  * Factory method to generate a CollisionPolygon object
1576  */
1580 }
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.
Definition: collisionBox.h:27
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.
Definition: collisionLine.h:25
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.
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
is_valid
Returns true if the CollisionPolygon is valid (that is, it has at least three vertices),...
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:27
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,...
Definition: cullTraverser.h:45
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...
Definition: factoryParams.h:36
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:586
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
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
Definition: planeNode.I:54
int get_clip_effect() const
Returns the clip_effect bits for this clip plane.
Definition: planeNode.I:128
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.
Definition: typedWritable.h:35
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.
PT(PandaNode) CollisionPolygon
Returns a GeomNode that may be rendered to visualize the CollisionSolid.
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.