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