Panda3D
 All Classes Functions Variables Enumerations
collisionBox.cxx
1 // Filename: collisionBox.cxx
2 // Created by: amith tudur (31Jul09)
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 "collisionBox.h"
16 #include "collisionLine.h"
17 #include "collisionRay.h"
18 #include "collisionSphere.h"
19 #include "collisionSegment.h"
20 #include "collisionHandler.h"
21 #include "collisionEntry.h"
22 #include "config_collide.h"
23 #include "boundingSphere.h"
24 #include "datagram.h"
25 #include "datagramIterator.h"
26 #include "bamReader.h"
27 #include "bamWriter.h"
28 #include "nearly_zero.h"
29 #include "cmath.h"
30 #include "mathNumbers.h"
31 #include "geom.h"
32 #include "geomTriangles.h"
33 #include "geomVertexWriter.h"
34 #include "config_mathutil.h"
35 #include "dcast.h"
36 
37 #include <math.h>
38 
39 PStatCollector CollisionBox::_volume_pcollector("Collision Volumes:CollisionBox");
40 PStatCollector CollisionBox::_test_pcollector("Collision Tests:CollisionBox");
41 TypeHandle CollisionBox::_type_handle;
42 
43 const int CollisionBox::plane_def[6][4] = {
44  {0, 4, 5, 1},
45  {4, 6, 7, 5},
46  {6, 2, 3, 7},
47  {2, 0, 1, 3},
48  {1, 5, 7, 3},
49  {2, 6, 4, 0},
50 };
51 
52 ////////////////////////////////////////////////////////////////////
53 // Function: CollisionBox::make_copy
54 // Access: Public, Virtual
55 // Description:
56 ////////////////////////////////////////////////////////////////////
57 CollisionSolid *CollisionBox::
58 make_copy() {
59  return new CollisionBox(*this);
60 }
61 
62 ////////////////////////////////////////////////////////////////////
63 // Function: CollisionBox::setup_box
64 // Access: Public, Virtual
65 // Description: Compute parameters for each of the box's sides
66 ////////////////////////////////////////////////////////////////////
67 void CollisionBox::
69  for(int plane = 0; plane < 6; plane++) {
70  LPoint3 array[4];
71  array[0] = get_point(plane_def[plane][0]);
72  array[1] = get_point(plane_def[plane][1]);
73  array[2] = get_point(plane_def[plane][2]);
74  array[3] = get_point(plane_def[plane][3]);
75  setup_points(array, array+4, plane);
76  }
77 }
78 
79 ////////////////////////////////////////////////////////////////////
80 // Function: CollisionBox::setup_points
81 // Access: Private
82 // Description: Computes the plane and 2d projection of points that
83 // make up this side.
84 ////////////////////////////////////////////////////////////////////
85 void CollisionBox::
86 setup_points(const LPoint3 *begin, const LPoint3 *end, int plane) {
87  int num_points = end - begin;
88  nassertv(num_points >= 3);
89 
90  _points[plane].clear();
91 
92  // Construct a matrix that rotates the points from the (X,0,Z) plane
93  // into the 3-d plane.
94  LMatrix4 to_3d_mat;
95  calc_to_3d_mat(to_3d_mat, plane);
96 
97  // And the inverse matrix rotates points from 3-d space into the 2-d
98  // plane.
99  _to_2d_mat[plane].invert_from(to_3d_mat);
100 
101  // Now project all of the points onto the 2-d plane.
102 
103  const LPoint3 *pi;
104  for (pi = begin; pi != end; ++pi) {
105  LPoint3 point = (*pi) * _to_2d_mat[plane];
106  _points[plane].push_back(PointDef(point[0], point[2]));
107  }
108 
109  nassertv(_points[plane].size() >= 3);
110 
111 #ifndef NDEBUG
112  /*
113  // Now make sure the points define a convex polygon.
114  if (is_concave()) {
115  collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
116  const LPoint3 *pi;
117  for (pi = begin; pi != end; ++pi) {
118  collide_cat.error(false) << " " << (*pi) << "\n";
119  }
120  collide_cat.error(false)
121  << " normal " << normal << " with length " << normal.length() << "\n";
122  _points.clear();
123  }
124  */
125 #endif
126 
127  compute_vectors(_points[plane]);
128 }
129 
130 ////////////////////////////////////////////////////////////////////
131 // Function: CollisionBox::test_intersection
132 // Access: Public, Virtual
133 // Description: First Dispatch point for box as a FROM object
134 ////////////////////////////////////////////////////////////////////
136 test_intersection(const CollisionEntry &entry) const {
137  return entry.get_into()->test_intersection_from_box(entry);
138 }
139 
140 ////////////////////////////////////////////////////////////////////
141 // Function: CollisionBox::xform
142 // Access: Public, Virtual
143 // Description: Transforms the solid by the indicated matrix.
144 ////////////////////////////////////////////////////////////////////
145 void CollisionBox::
146 xform(const LMatrix4 &mat) {
147  _min = _min * mat;
148  _max = _max * mat;
149  _center = _center * mat;
150  for(int v = 0; v < 8; v++) {
151  _vertex[v] = _vertex[v] * mat;
152  }
153  for(int p = 0; p < 6 ; p++) {
154  _planes[p] = set_plane(p);
155  }
156  _x = _vertex[0].get_x() - _center.get_x();
157  _y = _vertex[0].get_y() - _center.get_y();
158  _z = _vertex[0].get_z() - _center.get_z();
159  _radius = sqrt(_x * _x + _y * _y + _z * _z);
160  setup_box();
161  mark_viz_stale();
162  mark_internal_bounds_stale();
163 }
164 
165 ////////////////////////////////////////////////////////////////////
166 // Function: CollisionBox::get_collision_origin
167 // Access: Public, Virtual
168 // Description: Returns the point in space deemed to be the "origin"
169 // of the solid for collision purposes. The closest
170 // intersection point to this origin point is considered
171 // to be the most significant.
172 ////////////////////////////////////////////////////////////////////
175  return _center;
176 }
177 
178 ////////////////////////////////////////////////////////////////////
179 // Function: CollisionBox::get_min
180 // Access: Public, Virtual
181 // Description:
182 ////////////////////////////////////////////////////////////////////
183 LPoint3 CollisionBox::
184 get_min() const {
185  return _min;
186 }
187 
188 ////////////////////////////////////////////////////////////////////
189 // Function: CollisionBox::get_max
190 // Access: Public, Virtual
191 // Description:
192 ////////////////////////////////////////////////////////////////////
193 LPoint3 CollisionBox::
194 get_max() const {
195  return _max;
196 }
197 
198 ////////////////////////////////////////////////////////////////////
199 // Function: CollisionBox::get_approx_center
200 // Access: Public, Virtual
201 // Description:
202 ////////////////////////////////////////////////////////////////////
203 LPoint3 CollisionBox::
204 get_approx_center() const {
205  return (_min + _max) * 0.5f;
206 }
207 
208 ////////////////////////////////////////////////////////////////////
209 // Function: CollisionBox::get_volume_pcollector
210 // Access: Public, Virtual
211 // Description: Returns a PStatCollector that is used to count the
212 // number of bounding volume tests made against a solid
213 // of this type in a given frame.
214 ////////////////////////////////////////////////////////////////////
217  return _volume_pcollector;
218 }
219 
220 ////////////////////////////////////////////////////////////////////
221 // Function: CollisionBox::get_test_pcollector
222 // Access: Public, Virtual
223 // Description: Returns a PStatCollector that is used to count the
224 // number of intersection tests made against a solid
225 // of this type in a given frame.
226 ////////////////////////////////////////////////////////////////////
229  return _test_pcollector;
230 }
231 
232 ////////////////////////////////////////////////////////////////////
233 // Function: CollisionBox::output
234 // Access: Public, Virtual
235 // Description:
236 ////////////////////////////////////////////////////////////////////
237 void CollisionBox::
238 output(ostream &out) const {
239 }
240 
241 ////////////////////////////////////////////////////////////////////
242 // Function: CollisionBox::compute_internal_bounds
243 // Access: Protected, Virtual
244 // Description: Sphere is chosen as the Bounding Volume type for
245 // speed and efficiency
246 ////////////////////////////////////////////////////////////////////
248 compute_internal_bounds() const {
249  return new BoundingSphere(_center, _radius);
250 }
251 
252 ////////////////////////////////////////////////////////////////////
253 // Function: CollisionBox::test_intersection_from_sphere
254 // Access: Public, Virtual
255 // Description: Double dispatch point for sphere as FROM object
256 ////////////////////////////////////////////////////////////////////
258 test_intersection_from_sphere(const CollisionEntry &entry) const {
259 
260  const CollisionSphere *sphere;
261  DCAST_INTO_R(sphere, entry.get_from(), 0);
262 
263  CPT(TransformState) wrt_space = entry.get_wrt_space();
264  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
265 
266  const LMatrix4 &wrt_mat = wrt_space->get_mat();
267 
268  LPoint3 orig_center = sphere->get_center() * wrt_mat;
269  LPoint3 from_center = orig_center;
270  bool moved_from_center = false;
271  PN_stdfloat t = 1.0f;
272  LPoint3 contact_point(from_center);
273  PN_stdfloat actual_t = 1.0f;
274 
275  LVector3 from_radius_v =
276  LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
277  PN_stdfloat from_radius_2 = from_radius_v.length_squared();
278  PN_stdfloat from_radius = csqrt(from_radius_2);
279 
280  int ip;
281  PN_stdfloat max_dist = 0.0;
282  PN_stdfloat dist = 0.0;
283  bool intersect;
284  LPlane plane;
285  LVector3 normal;
286 
287  for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
288  plane = get_plane( ip );
289  if (_points[ip].size() < 3) {
290  continue;
291  }
292  if (wrt_prev_space != wrt_space) {
293  // If we have a delta between the previous position and the
294  // current position, we use that to determine some more properties
295  // of the collision.
296  LPoint3 b = from_center;
297  LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
298  LVector3 delta = b - a;
299 
300  // First, there is no collision if the "from" object is definitely
301  // moving in the same direction as the plane's normal.
302  PN_stdfloat dot = delta.dot(plane.get_normal());
303  if (dot > 0.1f) {
304  continue; // no intersection
305  }
306 
307  if (IS_NEARLY_ZERO(dot)) {
308  // If we're moving parallel to the plane, the sphere is tested
309  // at its final point. Leave it as it is.
310 
311  } else {
312  // Otherwise, we're moving into the plane; the sphere is tested
313  // at the point along its path that is closest to intersecting
314  // the plane. This may be the actual intersection point, or it
315  // may be the starting point or the final point.
316  // dot is equal to the (negative) magnitude of 'delta' along the
317  // direction of the plane normal
318  // t = ratio of (distance from start pos to plane) to (distance
319  // from start pos to end pos), along axis of plane normal
320  PN_stdfloat dist_to_p = plane.dist_to_plane(a);
321  t = (dist_to_p / -dot);
322 
323  // also compute the actual contact point and time of contact
324  // for handlers that need it
325  actual_t = ((dist_to_p - from_radius) / -dot);
326  actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
327  contact_point = a + (actual_t * delta);
328 
329  if (t >= 1.0f) {
330  // Leave it where it is.
331 
332  } else if (t < 0.0f) {
333  from_center = a;
334  moved_from_center = true;
335  } else {
336  from_center = a + t * delta;
337  moved_from_center = true;
338  }
339  }
340  }
341 
342  normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
343 
344 #ifndef NDEBUG
345  /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
346  std::cout
347  << "polygon within " << entry.get_into_node_path()
348  << " has normal " << normal << " of length " << normal.length()
349  << "\n";
350  normal.normalize();
351  }*/
352 #endif
353 
354  // The nearest point within the plane to our center is the
355  // intersection of the line (center, center - normal) with the plane.
356 
357  if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
358  // No intersection with plane? This means the plane's effective
359  // normal was within the plane itself. A useless polygon.
360  continue;
361  }
362 
363  if (dist > from_radius || dist < -from_radius) {
364  // No intersection with the plane.
365  continue;
366  }
367 
368  LPoint2 p = to_2d(from_center - dist * plane.get_normal(), ip);
369  PN_stdfloat edge_dist = 0.0f;
370 
371  const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
372  if (cpa != (ClipPlaneAttrib *)NULL) {
373  // We have a clip plane; apply it.
374  Points new_points;
375  if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
376  // All points are behind the clip plane; just do the default
377  // test.
378  edge_dist = dist_to_polygon(p, _points[ip]);
379  } else if (new_points.empty()) {
380  // The polygon is completely clipped.
381  continue;
382  } else {
383  // Test against the clipped polygon.
384  edge_dist = dist_to_polygon(p, new_points);
385  }
386  } else {
387  // No clip plane is in effect. Do the default test.
388  edge_dist = dist_to_polygon(p, _points[ip]);
389  }
390 
391  max_dist = from_radius;
392 
393  // Now we have edge_dist, which is the distance from the sphere
394  // center to the nearest edge of the polygon, within the polygon's
395  // plane. edge_dist<0 means the point is within the polygon.
396  if(edge_dist < 0) {
397  intersect = true;
398  continue;
399  }
400 
401  if((edge_dist > 0) &&
402  ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
403  // No intersection; the circle is outside the polygon.
404  continue;
405  }
406 
407  // The sphere appears to intersect the polygon. If the edge is less
408  // than from_radius away, the sphere may be resting on an edge of
409  // the polygon. Determine how far the center of the sphere must
410  // remain from the plane, based on its distance from the nearest
411  // edge.
412 
413  if (edge_dist >= 0.0f) {
414  PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
415  max_dist = csqrt(max_dist_2);
416  }
417 
418  if (dist > max_dist) {
419  // There's no intersection: the sphere is hanging off the edge.
420  continue;
421  }
422  intersect = true;
423  }
424  if( !intersect )
425  return NULL;
426 
427  if (collide_cat.is_debug()) {
428  collide_cat.debug()
429  << "intersection detected from " << entry.get_from_node_path()
430  << " into " << entry.get_into_node_path() << "\n";
431  }
432 
433  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
434 
435  PN_stdfloat into_depth = max_dist - dist;
436  if (moved_from_center) {
437  // We have to base the depth of intersection on the sphere's final
438  // resting point, not the point from which we tested the
439  // intersection.
440  PN_stdfloat orig_dist;
441  plane.intersects_line(orig_dist, orig_center, -normal);
442  into_depth = max_dist - orig_dist;
443  }
444 
445  new_entry->set_surface_normal(normal);
446  new_entry->set_surface_point(from_center - normal * dist);
447  new_entry->set_interior_point(from_center - normal * (dist + into_depth));
448  new_entry->set_contact_pos(contact_point);
449  new_entry->set_contact_normal(plane.get_normal());
450  new_entry->set_t(actual_t);
451 
452  return new_entry;
453 }
454 
455 
456 ////////////////////////////////////////////////////////////////////
457 // Function: CollisionBox::test_intersection_from_ray
458 // Access: Public, Virtual
459 // Description: Double dispatch point for ray as a FROM object
460 ////////////////////////////////////////////////////////////////////
462 test_intersection_from_ray(const CollisionEntry &entry) const {
463  const CollisionRay *ray;
464  DCAST_INTO_R(ray, entry.get_from(), 0);
465  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
466 
467  LPoint3 from_origin = ray->get_origin() * wrt_mat;
468  LVector3 from_direction = ray->get_direction() * wrt_mat;
469 
470  int i, j;
471  PN_stdfloat t;
472  PN_stdfloat near_t = 0.0;
473  bool intersect;
474  LPlane plane;
475  LPlane near_plane;
476 
477  //Returns the details about the first plane of the box that the ray
478  //intersects.
479  for (i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
480  plane = get_plane(i);
481 
482  if (!plane.intersects_line(t, from_origin, from_direction)) {
483  // No intersection. The ray is parallel to the plane.
484  continue;
485  }
486 
487  if (t < 0.0f) {
488  // The intersection point is before the start of the ray, and so
489  // the ray is entirely in front of the plane.
490  continue;
491  }
492  LPoint3 plane_point = from_origin + t * from_direction;
493  LPoint2 p = to_2d(plane_point, i);
494 
495  if (!point_is_inside(p, _points[i])){
496  continue;
497  }
498  intersect = true;
499  if (j) {
500  if(t < near_t) {
501  near_plane = plane;
502  near_t = t;
503  }
504  }
505  else {
506  near_plane = plane;
507  near_t = t;
508  }
509  ++j;
510  }
511 
512 
513  if(!intersect) {
514  //No intersection with ANY of the box's planes has been detected
515  return NULL;
516  }
517 
518  if (collide_cat.is_debug()) {
519  collide_cat.debug()
520  << "intersection detected from " << entry.get_from_node_path()
521  << " into " << entry.get_into_node_path() << "\n";
522  }
523 
524  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
525 
526  LPoint3 into_intersection_point = from_origin + near_t * from_direction;
527 
528  LVector3 normal =
529  (has_effective_normal() && ray->get_respect_effective_normal())
530  ? get_effective_normal() : near_plane.get_normal();
531 
532  new_entry->set_surface_normal(normal);
533  new_entry->set_surface_point(into_intersection_point);
534 
535  return new_entry;
536 }
537 
538 
539 ////////////////////////////////////////////////////////////////////
540 // Function: CollisionBox::test_intersection_from_segment
541 // Access: Public, Virtual
542 // Description: Double dispatch point for segment as a FROM object
543 ////////////////////////////////////////////////////////////////////
545 test_intersection_from_segment(const CollisionEntry &entry) const {
546  const CollisionSegment *seg;
547  DCAST_INTO_R(seg, entry.get_from(), 0);
548  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
549 
550  LPoint3 from_origin = seg->get_point_a() * wrt_mat;
551  LPoint3 from_extent = seg->get_point_b() * wrt_mat;
552  LVector3 from_direction = from_extent - from_origin;
553 
554  int i, j;
555  PN_stdfloat t;
556  PN_stdfloat near_t = 0.0;
557  bool intersect;
558  LPlane plane;
559  LPlane near_plane;
560 
561  //Returns the details about the first plane of the box that the
562  //segment intersects.
563  for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
564  plane = get_plane(i);
565 
566  if (!plane.intersects_line(t, from_origin, from_direction)) {
567  // No intersection. The segment is parallel to the plane.
568  continue;
569  }
570 
571  if (t < 0.0f || t > 1.0f) {
572  // The intersection point is before the start of the segment,
573  // or after the end of the segment, so the segment is either
574  // entirely in front of or behind the plane.
575  continue;
576  }
577  LPoint3 plane_point = from_origin + t * from_direction;
578  LPoint2 p = to_2d(plane_point, i);
579 
580  if (!point_is_inside(p, _points[i])){
581  continue;
582  }
583  intersect = true;
584  if(j) {
585  if(t < near_t) {
586  near_plane = plane;
587  near_t = t;
588  }
589  }
590  else {
591  near_plane = plane;
592  near_t = t;
593  }
594  ++j;
595  }
596 
597  if(!intersect) {
598  //No intersection with ANY of the box's planes has been detected
599  return NULL;
600  }
601 
602  if (collide_cat.is_debug()) {
603  collide_cat.debug()
604  << "intersection detected from " << entry.get_from_node_path()
605  << " into " << entry.get_into_node_path() << "\n";
606  }
607 
608  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
609 
610  LPoint3 into_intersection_point = from_origin + near_t * from_direction;
611 
612  LVector3 normal =
613  (has_effective_normal() && seg->get_respect_effective_normal())
614  ? get_effective_normal() : near_plane.get_normal();
615 
616  new_entry->set_surface_normal(normal);
617  new_entry->set_surface_point(into_intersection_point);
618 
619  return new_entry;
620 }
621 
622 
623 ////////////////////////////////////////////////////////////////////
624 // Function: CollisionBox::test_intersection_from_box
625 // Access: Public, Virtual
626 // Description: Double dispatch point for box as a FROM object
627 // NOT Implemented.
628 ////////////////////////////////////////////////////////////////////
630 test_intersection_from_box(const CollisionEntry &entry) const {
631  return NULL;
632 }
633 
634 
635 ////////////////////////////////////////////////////////////////////
636 // Function: CollisionBox::fill_viz_geom
637 // Access: Protected, Virtual
638 // Description: Fills the _viz_geom GeomNode up with Geoms suitable
639 // for rendering this solid.
640 ////////////////////////////////////////////////////////////////////
641 void CollisionBox::
642 fill_viz_geom() {
643  if (collide_cat.is_debug()) {
644  collide_cat.debug()
645  << "Recomputing viz for " << *this << "\n";
646  }
647 
648  PT(GeomVertexData) vdata = new GeomVertexData
649  ("collision", GeomVertexFormat::get_v3(),
650  Geom::UH_static);
651 
652  vdata->unclean_set_num_rows(8);
653 
654  {
655  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
656  vertex.set_data3(_min[0], _min[1], _min[2]);
657  vertex.set_data3(_min[0], _max[1], _min[2]);
658  vertex.set_data3(_max[0], _max[1], _min[2]);
659  vertex.set_data3(_max[0], _min[1], _min[2]);
660 
661  vertex.set_data3(_min[0], _min[1], _max[2]);
662  vertex.set_data3(_min[0], _max[1], _max[2]);
663  vertex.set_data3(_max[0], _max[1], _max[2]);
664  vertex.set_data3(_max[0], _min[1], _max[2]);
665  }
666 
667  PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static);
668 
669  // Bottom
670  tris->add_vertices(0, 1, 2);
671  tris->add_vertices(2, 3, 0);
672 
673  // Top
674  tris->add_vertices(4, 7, 6);
675  tris->add_vertices(6, 5, 4);
676 
677  // Sides
678  tris->add_vertices(0, 4, 1);
679  tris->add_vertices(1, 4, 5);
680 
681  tris->add_vertices(1, 5, 2);
682  tris->add_vertices(2, 5, 6);
683 
684  tris->add_vertices(2, 6, 3);
685  tris->add_vertices(3, 6, 7);
686 
687  tris->add_vertices(3, 7, 0);
688  tris->add_vertices(0, 7, 4);
689 
690  PT(Geom) geom = new Geom(vdata);
691  geom->add_primitive(tris);
692 
693  _viz_geom->add_geom(geom, get_solid_viz_state());
694  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
695 }
696 
697 ////////////////////////////////////////////////////////////////////
698 // Function: CollisionBox::apply_clip_plane
699 // Access: Private
700 // Description: Clips the polygon by all of the clip planes named in
701 // the clip plane attribute and fills new_points up with
702 // the resulting points.
703 //
704 // The return value is true if the set of points is
705 // unmodified (all points are behind all the clip
706 // planes), or false otherwise.
707 ////////////////////////////////////////////////////////////////////
708 bool CollisionBox::
710  const ClipPlaneAttrib *cpa,
711  const TransformState *net_transform, int plane_no) const {
712  bool all_in = true;
713 
714  int num_planes = cpa->get_num_on_planes();
715  bool first_plane = true;
716 
717  for (int i = 0; i < num_planes; i++) {
718  NodePath plane_path = cpa->get_on_plane(i);
719  PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
720  if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
721  CPT(TransformState) new_transform =
722  net_transform->invert_compose(plane_path.get_net_transform());
723 
724  LPlane plane = plane_node->get_plane() * new_transform->get_mat();
725  if (first_plane) {
726  first_plane = false;
727  if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
728  all_in = false;
729  }
730  } else {
731  Points last_points;
732  last_points.swap(new_points);
733  if (!clip_polygon(new_points, last_points, plane, plane_no)) {
734  all_in = false;
735  }
736  }
737  }
738  }
739 
740  if (!all_in) {
741  compute_vectors(new_points);
742  }
743 
744  return all_in;
745 }
746 ////////////////////////////////////////////////////////////////////
747 // Function: CollisionBox::clip_polygon
748 // Access: Private
749 // Description: Clips the source_points of the polygon by the
750 // indicated clipping plane, and modifies new_points to
751 // reflect the new set of clipped points (but does not
752 // compute the vectors in new_points).
753 //
754 // The return value is true if the set of points is
755 // unmodified (all points are behind the clip plane), or
756 // false otherwise.
757 ////////////////////////////////////////////////////////////////////
758 bool CollisionBox::
760  const CollisionBox::Points &source_points,
761  const LPlane &plane, int plane_no) const {
762  new_points.clear();
763  if (source_points.empty()) {
764  return true;
765  }
766 
767  LPoint3 from3d;
768  LVector3 delta3d;
769  if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
770  // The clipping plane is parallel to the polygon. The polygon is
771  // either all in or all out.
772  if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
773  // A point within the polygon is behind the clipping plane: the
774  // polygon is all in.
775  new_points = source_points;
776  return true;
777  }
778  return false;
779  }
780 
781  // Project the line of intersection into the 2-d plane. Now we have
782  // a 2-d clipping line.
783  LPoint2 from2d = to_2d(from3d,plane_no);
784  LVector2 delta2d = to_2d(delta3d,plane_no);
785 
786  PN_stdfloat a = -delta2d[1];
787  PN_stdfloat b = delta2d[0];
788  PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
789 
790  // Now walk through the points. Any point on the left of our line
791  // gets removed, and the line segment clipped at the point of
792  // intersection.
793 
794  // We might increase the number of vertices by as many as 1, if the
795  // plane clips off exactly one corner. (We might also decrease the
796  // number of vertices, or keep them the same number.)
797  new_points.reserve(source_points.size() + 1);
798 
799  LPoint2 last_point = source_points.back()._p;
800  bool last_is_in = !is_right(last_point - from2d, delta2d);
801  bool all_in = last_is_in;
802  Points::const_iterator pi;
803  for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
804  const LPoint2 &this_point = (*pi)._p;
805  bool this_is_in = !is_right(this_point - from2d, delta2d);
806 
807  // There appears to be a compiler bug in gcc 4.0: we need to
808  // extract this comparison outside of the if statement.
809  bool crossed_over = (this_is_in != last_is_in);
810  if (crossed_over) {
811  // We have just crossed over the clipping line. Find the point
812  // of intersection.
813  LVector2 d = this_point - last_point;
814  PN_stdfloat denom = (a * d[0] + b * d[1]);
815  if (denom != 0.0) {
816  PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
817  LPoint2 p = last_point + t * d;
818 
819  new_points.push_back(PointDef(p[0], p[1]));
820  last_is_in = this_is_in;
821  }
822  }
823 
824  if (this_is_in) {
825  // We are behind the clipping line. Keep the point.
826  new_points.push_back(PointDef(this_point[0], this_point[1]));
827  } else {
828  all_in = false;
829  }
830 
831  last_point = this_point;
832  }
833 
834  return all_in;
835 }
836 
837 
838 ////////////////////////////////////////////////////////////////////
839 // Function: CollisionBox::
840 // Access: Private
841 // Description: Returns the linear distance from the 2-d point to the
842 // nearest part of the polygon defined by the points
843 // vector. The result is negative if the point is
844 // within the polygon.
845 ////////////////////////////////////////////////////////////////////
846 PN_stdfloat CollisionBox::
847 dist_to_polygon(const LPoint2 &p, const CollisionBox::Points &points) const {
848 
849  // We know that that the polygon is convex and is defined with the
850  // points in counterclockwise order. Therefore, we simply compare
851  // the signed distance to each line segment; we ignore any negative
852  // values, and take the minimum of all the positive values.
853 
854  // If all values are negative, the point is within the polygon; we
855  // therefore return an arbitrary negative result.
856 
857  bool got_dist = false;
858  PN_stdfloat best_dist = -1.0f;
859 
860  size_t num_points = points.size();
861  for (size_t i = 0; i < num_points - 1; ++i) {
862  PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
863  points[i]._v);
864  if (d >= 0.0f) {
865  if (!got_dist || d < best_dist) {
866  best_dist = d;
867  got_dist = true;
868  }
869  }
870  }
871 
872  PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
873  points[num_points - 1]._v);
874  if (d >= 0.0f) {
875  if (!got_dist || d < best_dist) {
876  best_dist = d;
877  got_dist = true;
878  }
879  }
880 
881  return best_dist;
882 }
883 
884 ////////////////////////////////////////////////////////////////////
885 // Function: CollisionBox::dist_to_line_segment
886 // Access: Private, Static
887 // Description: Returns the linear distance of p to the line segment
888 // defined by f and t, where v = (t - f).normalize().
889 // The result is negative if p is left of the line,
890 // positive if it is right of the line. If the result
891 // is positive, it is constrained by endpoints of the
892 // line segment (i.e. the result might be larger than it
893 // would be for a straight distance-to-line test). If
894 // the result is negative, we don't bother.
895 ////////////////////////////////////////////////////////////////////
896 PN_stdfloat CollisionBox::
897 dist_to_line_segment(const LPoint2 &p,
898  const LPoint2 &f, const LPoint2 &t,
899  const LVector2 &v) {
900  LVector2 v1 = (p - f);
901  PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
902  if (d < 0.0f) {
903  return d;
904  }
905 
906  // Compute the nearest point on the line.
907  LPoint2 q = p + LVector2(-v[1], v[0]) * d;
908 
909  // Now constrain that point to the line segment.
910  if (v[0] > 0.0f) {
911  // X+
912  if (v[1] > 0.0f) {
913  // Y+
914  if (v[0] > v[1]) {
915  // X-dominant.
916  if (q[0] < f[0]) {
917  return (p - f).length();
918  } if (q[0] > t[0]) {
919  return (p - t).length();
920  } else {
921  return d;
922  }
923  } else {
924  // Y-dominant.
925  if (q[1] < f[1]) {
926  return (p - f).length();
927  } if (q[1] > t[1]) {
928  return (p - t).length();
929  } else {
930  return d;
931  }
932  }
933  } else {
934  // Y-
935  if (v[0] > -v[1]) {
936  // X-dominant.
937  if (q[0] < f[0]) {
938  return (p - f).length();
939  } if (q[0] > t[0]) {
940  return (p - t).length();
941  } else {
942  return d;
943  }
944  } else {
945  // Y-dominant.
946  if (q[1] > f[1]) {
947  return (p - f).length();
948  } if (q[1] < t[1]) {
949  return (p - t).length();
950  } else {
951  return d;
952  }
953  }
954  }
955  } else {
956  // X-
957  if (v[1] > 0.0f) {
958  // Y+
959  if (-v[0] > v[1]) {
960  // X-dominant.
961  if (q[0] > f[0]) {
962  return (p - f).length();
963  } if (q[0] < t[0]) {
964  return (p - t).length();
965  } else {
966  return d;
967  }
968  } else {
969  // Y-dominant.
970  if (q[1] < f[1]) {
971  return (p - f).length();
972  } if (q[1] > t[1]) {
973  return (p - t).length();
974  } else {
975  return d;
976  }
977  }
978  } else {
979  // Y-
980  if (-v[0] > -v[1]) {
981  // X-dominant.
982  if (q[0] > f[0]) {
983  return (p - f).length();
984  } if (q[0] < t[0]) {
985  return (p - t).length();
986  } else {
987  return d;
988  }
989  } else {
990  // Y-dominant.
991  if (q[1] > f[1]) {
992  return (p - f).length();
993  } if (q[1] < t[1]) {
994  return (p - t).length();
995  } else {
996  return d;
997  }
998  }
999  }
1000  }
1001 }
1002 
1003 ////////////////////////////////////////////////////////////////////
1004 // Function: CollisionBox::point_is_inside
1005 // Access: Private
1006 // Description: Returns true if the indicated point is within the
1007 // polygon's 2-d space, false otherwise.
1008 ////////////////////////////////////////////////////////////////////
1009 bool CollisionBox::
1010 point_is_inside(const LPoint2 &p, const CollisionBox::Points &points) const {
1011  // We insist that the polygon be convex. This makes things a bit simpler.
1012  // In the case of a convex polygon, defined with points in counterclockwise
1013  // order, a point is interior to the polygon iff the point is not right of
1014  // each of the edges.
1015  for (int i = 0; i < (int)points.size() - 1; i++) {
1016  if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1017  return false;
1018  }
1019  }
1020  if (is_right(p - points[points.size() - 1]._p,
1021  points[0]._p - points[points.size() - 1]._p)) {
1022  return false;
1023  }
1024 
1025  return true;
1026 }
1027 
1028 ////////////////////////////////////////////////////////////////////
1029 // Function: CollisionBox::compute_vectors
1030 // Access: Private, Static
1031 // Description: Now that the _p members of the given points array
1032 // have been computed, go back and compute all of the _v
1033 // members.
1034 ////////////////////////////////////////////////////////////////////
1035 void CollisionBox::
1037  size_t num_points = points.size();
1038  for (size_t i = 0; i < num_points; i++) {
1039  points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1040  points[i]._v.normalize();
1041  }
1042 }
1043 
1044 ////////////////////////////////////////////////////////////////////
1045 // Function: CollisionBox::register_with_read_factory
1046 // Access: Public, Static
1047 // Description: Factory method to generate a CollisionBox object
1048 ////////////////////////////////////////////////////////////////////
1049 void CollisionBox::
1051  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
1052 }
1053 
1054 ////////////////////////////////////////////////////////////////////
1055 // Function: CollisionBox::write_datagram
1056 // Access: Public
1057 // Description: Function to write the important information in
1058 // the particular object to a Datagram
1059 ////////////////////////////////////////////////////////////////////
1060 void CollisionBox::
1062  CollisionSolid::write_datagram(manager, me);
1063  _center.write_datagram(me);
1064  _min.write_datagram(me);
1065  _max.write_datagram(me);
1066  for(int i=0; i < 8; i++) {
1067  _vertex[i].write_datagram(me);
1068  }
1069  me.add_stdfloat(_radius);
1070  me.add_stdfloat(_x);
1071  me.add_stdfloat(_y);
1072  me.add_stdfloat(_z);
1073  for(int i=0; i < 6; i++) {
1074  _planes[i].write_datagram(me);
1075  }
1076  for(int i=0; i < 6; i++) {
1077  _to_2d_mat[i].write_datagram(me);
1078  }
1079  for(int i=0; i < 6; i++) {
1080  me.add_uint16(_points[i].size());
1081  for (size_t j = 0; j < _points[i].size(); j++) {
1082  _points[i][j]._p.write_datagram(me);
1083  _points[i][j]._v.write_datagram(me);
1084  }
1085  }
1086 }
1087 
1088 ////////////////////////////////////////////////////////////////////
1089 // Function: CollisionBox::make_CollisionBox
1090 // Access: Protected
1091 // Description: Factory method to generate a CollisionBox object
1092 ////////////////////////////////////////////////////////////////////
1093 TypedWritable *CollisionBox::
1094 make_CollisionBox(const FactoryParams &params) {
1095  CollisionBox *me = new CollisionBox;
1096  DatagramIterator scan;
1097  BamReader *manager;
1098 
1099  parse_params(params, scan, manager);
1100  me->fillin(scan, manager);
1101  return me;
1102 }
1103 
1104 ////////////////////////////////////////////////////////////////////
1105 // Function: CollisionBox::fillin
1106 // Access: Protected
1107 // Description: Function that reads out of the datagram (or asks
1108 // manager to read) all of the data that is needed to
1109 // re-create this object and stores it in the appropiate
1110 // place
1111 ////////////////////////////////////////////////////////////////////
1112 void CollisionBox::
1113 fillin(DatagramIterator& scan, BamReader* manager) {
1114  CollisionSolid::fillin(scan, manager);
1115  _center.read_datagram(scan);
1116  _min.read_datagram(scan);
1117  _max.read_datagram(scan);
1118  for(int i=0; i < 8; i++) {
1119  _vertex[i].read_datagram(scan);
1120  }
1121  _radius = scan.get_stdfloat();
1122  _x = scan.get_stdfloat();
1123  _y = scan.get_stdfloat();
1124  _z = scan.get_stdfloat();
1125  for(int i=0; i < 6; i++) {
1126  _planes[i].read_datagram(scan);
1127  }
1128  for(int i=0; i < 6; i++) {
1129  _to_2d_mat[i].read_datagram(scan);
1130  }
1131  for(int i=0; i < 6; i++) {
1132  size_t size = scan.get_uint16();
1133  for (size_t j = 0; j < size; j++) {
1134  LPoint2 p;
1135  LVector2 v;
1136  p.read_datagram(scan);
1137  v.read_datagram(scan);
1138  _points[i].push_back(PointDef(p, v));
1139  }
1140  }
1141 }
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
void setup_box()
Compute parameters for each of the box&#39;s sides.
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:31
bool get_respect_effective_normal() const
See set_respect_effective_normal().
LPoint2 to_2d(const LVecBase3 &point3d, int plane) const
Assuming the indicated point in 3-d space lies within the polygon&#39;s plane, returns the corresponding ...
Definition: collisionBox.I:248
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:122
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the &quot;origin&quot; of the solid for collision purposes.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
Definition: lvecBase3.h:1373
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).
This defines a bounding sphere, consisting of a center and a radius.
bool clip_polygon(Points &new_points, const Points &source_points, const LPlane &plane, int plane_no) const
Clips the source_points of the polygon by the indicated clipping plane, and modifies new_points to re...
A cuboid collision volume or object.
Definition: collisionBox.h:30
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:37
PandaNode * node() const
Returns the referenced node of the path.
Definition: nodePath.I:284
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.
static void compute_vectors(Points &points)
Now that the _p members of the given points array have been computed, go back and compute all of the ...
PN_uint16 get_uint16()
Extracts an unsigned 16-bit integer.
LPlane get_plane(int n) const
Returns the nth face of the rectangular solid.
Definition: collisionBox.I:196
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
Definition: datagram.I:240
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
void write_datagram(Datagram &destination) const
Writes the matrix to the Datagram using add_stdfloat().
Definition: lmatrix.cxx:1148
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.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
Definition: lvecBase2.h:663
int get_clip_effect() const
Returns the clip_effect bits for this clip plane.
Definition: planeNode.I:157
bool point_is_inside(const LPoint2 &p, const Points &points) const
Returns true if the indicated point is within the polygon&#39;s 2-d space, false otherwise.
static void register_with_read_factory()
Factory method to generate a CollisionBox object.
void setup_points(const LPoint3 *begin, const LPoint3 *end, int plane)
Computes the plane and 2d projection of points that make up this side.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
Definition: lvecBase2.h:1161
This is a 4-by-4 transform matrix.
Definition: lmatrix.h:451
Defines a single collision event.
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
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 read_datagram(DatagramIterator &source)
Reads the matrix from the Datagram using get_stdfloat().
Definition: lmatrix.cxx:1162
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
Definition: planeNode.I:65
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
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
bool apply_clip_plane(Points &new_points, const ClipPlaneAttrib *cpa, const TransformState *net_transform, int plane_no) const
Clips the polygon by all of the clip planes named in the clip plane attribute and fills new_points up...
Defines a series of disconnected triangles.
Definition: geomTriangles.h:25
void calc_to_3d_mat(LMatrix4 &to_3d_mat, int plane) const
Fills the indicated matrix with the appropriate rotation transform to move points from the 2-d plane ...
Definition: collisionBox.I:261
LPlane set_plane(int n) const
Creates the nth face of the rectangular solid.
Definition: collisionBox.I:207
A class to retrieve the individual data elements previously stored in a Datagram. ...
This is a two-component point in space.
Definition: lpoint2.h:92
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
Definition: collisionBox.I:159
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
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
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
Definition: lvecBase3.h:1355
A node that contains a plane.
Definition: planeNode.h:39
PN_stdfloat dist_to_polygon(const LPoint2 &p, const Points &points) const
Returns the linear distance from the 2-d point to the nearest part of the polygon defined by the poin...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...