Panda3D
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 ////////////////////////////////////////////////////////////////////
135 PT(CollisionEntry) CollisionBox::
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 ////////////////////////////////////////////////////////////////////
247 PT(BoundingVolume) CollisionBox::
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 ////////////////////////////////////////////////////////////////////
257 PT(CollisionEntry) CollisionBox::
258 test_intersection_from_sphere(const CollisionEntry &entry) const {
259 
260  const CollisionSphere *sphere;
261  DCAST_INTO_R(sphere, entry.get_from(), NULL);
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  // Clamp the surface point to the box bounds.
446  LPoint3 surface = from_center - normal * dist;
447  surface = surface.fmax(_min);
448  surface = surface.fmin(_max);
449 
450  new_entry->set_surface_normal(normal);
451  new_entry->set_surface_point(surface);
452  new_entry->set_interior_point(surface - normal * into_depth);
453  new_entry->set_contact_pos(contact_point);
454  new_entry->set_contact_normal(plane.get_normal());
455  new_entry->set_t(actual_t);
456 
457  return new_entry;
458 }
459 
460 
461 ////////////////////////////////////////////////////////////////////
462 // Function: CollisionBox::test_intersection_from_ray
463 // Access: Public, Virtual
464 // Description: Double dispatch point for ray as a FROM object
465 ////////////////////////////////////////////////////////////////////
466 PT(CollisionEntry) CollisionBox::
467 test_intersection_from_ray(const CollisionEntry &entry) const {
468  const CollisionRay *ray;
469  DCAST_INTO_R(ray, entry.get_from(), NULL);
470  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
471 
472  LPoint3 from_origin = ray->get_origin() * wrt_mat;
473  LVector3 from_direction = ray->get_direction() * wrt_mat;
474 
475  int i, j;
476  PN_stdfloat t;
477  PN_stdfloat near_t = 0.0;
478  bool intersect;
479  LPlane plane;
480  LPlane near_plane;
481 
482  //Returns the details about the first plane of the box that the ray
483  //intersects.
484  for (i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
485  plane = get_plane(i);
486 
487  if (!plane.intersects_line(t, from_origin, from_direction)) {
488  // No intersection. The ray is parallel to the plane.
489  continue;
490  }
491 
492  if (t < 0.0f) {
493  // The intersection point is before the start of the ray, and so
494  // the ray is entirely in front of the plane.
495  continue;
496  }
497  LPoint3 plane_point = from_origin + t * from_direction;
498  LPoint2 p = to_2d(plane_point, i);
499 
500  if (!point_is_inside(p, _points[i])){
501  continue;
502  }
503  intersect = true;
504  if (j) {
505  if(t < near_t) {
506  near_plane = plane;
507  near_t = t;
508  }
509  }
510  else {
511  near_plane = plane;
512  near_t = t;
513  }
514  ++j;
515  }
516 
517 
518  if(!intersect) {
519  //No intersection with ANY of the box's planes has been detected
520  return NULL;
521  }
522 
523  if (collide_cat.is_debug()) {
524  collide_cat.debug()
525  << "intersection detected from " << entry.get_from_node_path()
526  << " into " << entry.get_into_node_path() << "\n";
527  }
528 
529  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
530 
531  LPoint3 into_intersection_point = from_origin + near_t * from_direction;
532 
533  LVector3 normal =
535  ? get_effective_normal() : near_plane.get_normal();
536 
537  new_entry->set_surface_normal(normal);
538  new_entry->set_surface_point(into_intersection_point);
539 
540  return new_entry;
541 }
542 
543 
544 ////////////////////////////////////////////////////////////////////
545 // Function: CollisionBox::test_intersection_from_segment
546 // Access: Public, Virtual
547 // Description: Double dispatch point for segment as a FROM object
548 ////////////////////////////////////////////////////////////////////
549 PT(CollisionEntry) CollisionBox::
550 test_intersection_from_segment(const CollisionEntry &entry) const {
551  const CollisionSegment *seg;
552  DCAST_INTO_R(seg, entry.get_from(), NULL);
553  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
554 
555  LPoint3 from_origin = seg->get_point_a() * wrt_mat;
556  LPoint3 from_extent = seg->get_point_b() * wrt_mat;
557  LVector3 from_direction = from_extent - from_origin;
558 
559  int i, j;
560  PN_stdfloat t;
561  PN_stdfloat near_t = 0.0;
562  bool intersect;
563  LPlane plane;
564  LPlane near_plane;
565 
566  //Returns the details about the first plane of the box that the
567  //segment intersects.
568  for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
569  plane = get_plane(i);
570 
571  if (!plane.intersects_line(t, from_origin, from_direction)) {
572  // No intersection. The segment is parallel to the plane.
573  continue;
574  }
575 
576  if (t < 0.0f || t > 1.0f) {
577  // The intersection point is before the start of the segment,
578  // or after the end of the segment, so the segment is either
579  // entirely in front of or behind the plane.
580  continue;
581  }
582  LPoint3 plane_point = from_origin + t * from_direction;
583  LPoint2 p = to_2d(plane_point, i);
584 
585  if (!point_is_inside(p, _points[i])){
586  continue;
587  }
588  intersect = true;
589  if(j) {
590  if(t < near_t) {
591  near_plane = plane;
592  near_t = t;
593  }
594  }
595  else {
596  near_plane = plane;
597  near_t = t;
598  }
599  ++j;
600  }
601 
602  if(!intersect) {
603  //No intersection with ANY of the box's planes has been detected
604  return NULL;
605  }
606 
607  if (collide_cat.is_debug()) {
608  collide_cat.debug()
609  << "intersection detected from " << entry.get_from_node_path()
610  << " into " << entry.get_into_node_path() << "\n";
611  }
612 
613  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
614 
615  LPoint3 into_intersection_point = from_origin + near_t * from_direction;
616 
617  LVector3 normal =
619  ? get_effective_normal() : near_plane.get_normal();
620 
621  new_entry->set_surface_normal(normal);
622  new_entry->set_surface_point(into_intersection_point);
623 
624  return new_entry;
625 }
626 
627 
628 ////////////////////////////////////////////////////////////////////
629 // Function: CollisionBox::test_intersection_from_box
630 // Access: Public, Virtual
631 // Description: Double dispatch point for box as a FROM object
632 // NOT Implemented.
633 ////////////////////////////////////////////////////////////////////
634 PT(CollisionEntry) CollisionBox::
635 test_intersection_from_box(const CollisionEntry &entry) const {
636  return NULL;
637 }
638 
639 
640 ////////////////////////////////////////////////////////////////////
641 // Function: CollisionBox::fill_viz_geom
642 // Access: Protected, Virtual
643 // Description: Fills the _viz_geom GeomNode up with Geoms suitable
644 // for rendering this solid.
645 ////////////////////////////////////////////////////////////////////
646 void CollisionBox::
647 fill_viz_geom() {
648  if (collide_cat.is_debug()) {
649  collide_cat.debug()
650  << "Recomputing viz for " << *this << "\n";
651  }
652 
653  PT(GeomVertexData) vdata = new GeomVertexData
654  ("collision", GeomVertexFormat::get_v3(),
655  Geom::UH_static);
656 
657  vdata->unclean_set_num_rows(8);
658 
659  {
660  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
661  vertex.set_data3(_min[0], _min[1], _min[2]);
662  vertex.set_data3(_min[0], _max[1], _min[2]);
663  vertex.set_data3(_max[0], _max[1], _min[2]);
664  vertex.set_data3(_max[0], _min[1], _min[2]);
665 
666  vertex.set_data3(_min[0], _min[1], _max[2]);
667  vertex.set_data3(_min[0], _max[1], _max[2]);
668  vertex.set_data3(_max[0], _max[1], _max[2]);
669  vertex.set_data3(_max[0], _min[1], _max[2]);
670  }
671 
672  PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static);
673 
674  // Bottom
675  tris->add_vertices(0, 1, 2);
676  tris->add_vertices(2, 3, 0);
677 
678  // Top
679  tris->add_vertices(4, 7, 6);
680  tris->add_vertices(6, 5, 4);
681 
682  // Sides
683  tris->add_vertices(0, 4, 1);
684  tris->add_vertices(1, 4, 5);
685 
686  tris->add_vertices(1, 5, 2);
687  tris->add_vertices(2, 5, 6);
688 
689  tris->add_vertices(2, 6, 3);
690  tris->add_vertices(3, 6, 7);
691 
692  tris->add_vertices(3, 7, 0);
693  tris->add_vertices(0, 7, 4);
694 
695  PT(Geom) geom = new Geom(vdata);
696  geom->add_primitive(tris);
697 
698  _viz_geom->add_geom(geom, get_solid_viz_state());
699  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
700 }
701 
702 ////////////////////////////////////////////////////////////////////
703 // Function: CollisionBox::apply_clip_plane
704 // Access: Private
705 // Description: Clips the polygon by all of the clip planes named in
706 // the clip plane attribute and fills new_points up with
707 // the resulting points.
708 //
709 // The return value is true if the set of points is
710 // unmodified (all points are behind all the clip
711 // planes), or false otherwise.
712 ////////////////////////////////////////////////////////////////////
713 bool CollisionBox::
715  const ClipPlaneAttrib *cpa,
716  const TransformState *net_transform, int plane_no) const {
717  bool all_in = true;
718 
719  int num_planes = cpa->get_num_on_planes();
720  bool first_plane = true;
721 
722  for (int i = 0; i < num_planes; i++) {
723  NodePath plane_path = cpa->get_on_plane(i);
724  PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
725  if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
726  CPT(TransformState) new_transform =
727  net_transform->invert_compose(plane_path.get_net_transform());
728 
729  LPlane plane = plane_node->get_plane() * new_transform->get_mat();
730  if (first_plane) {
731  first_plane = false;
732  if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
733  all_in = false;
734  }
735  } else {
736  Points last_points;
737  last_points.swap(new_points);
738  if (!clip_polygon(new_points, last_points, plane, plane_no)) {
739  all_in = false;
740  }
741  }
742  }
743  }
744 
745  if (!all_in) {
746  compute_vectors(new_points);
747  }
748 
749  return all_in;
750 }
751 ////////////////////////////////////////////////////////////////////
752 // Function: CollisionBox::clip_polygon
753 // Access: Private
754 // Description: Clips the source_points of the polygon by the
755 // indicated clipping plane, and modifies new_points to
756 // reflect the new set of clipped points (but does not
757 // compute the vectors in new_points).
758 //
759 // The return value is true if the set of points is
760 // unmodified (all points are behind the clip plane), or
761 // false otherwise.
762 ////////////////////////////////////////////////////////////////////
763 bool CollisionBox::
765  const CollisionBox::Points &source_points,
766  const LPlane &plane, int plane_no) const {
767  new_points.clear();
768  if (source_points.empty()) {
769  return true;
770  }
771 
772  LPoint3 from3d;
773  LVector3 delta3d;
774  if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
775  // The clipping plane is parallel to the polygon. The polygon is
776  // either all in or all out.
777  if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
778  // A point within the polygon is behind the clipping plane: the
779  // polygon is all in.
780  new_points = source_points;
781  return true;
782  }
783  return false;
784  }
785 
786  // Project the line of intersection into the 2-d plane. Now we have
787  // a 2-d clipping line.
788  LPoint2 from2d = to_2d(from3d,plane_no);
789  LVector2 delta2d = to_2d(delta3d,plane_no);
790 
791  PN_stdfloat a = -delta2d[1];
792  PN_stdfloat b = delta2d[0];
793  PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
794 
795  // Now walk through the points. Any point on the left of our line
796  // gets removed, and the line segment clipped at the point of
797  // intersection.
798 
799  // We might increase the number of vertices by as many as 1, if the
800  // plane clips off exactly one corner. (We might also decrease the
801  // number of vertices, or keep them the same number.)
802  new_points.reserve(source_points.size() + 1);
803 
804  LPoint2 last_point = source_points.back()._p;
805  bool last_is_in = !is_right(last_point - from2d, delta2d);
806  bool all_in = last_is_in;
807  Points::const_iterator pi;
808  for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
809  const LPoint2 &this_point = (*pi)._p;
810  bool this_is_in = !is_right(this_point - from2d, delta2d);
811 
812  // There appears to be a compiler bug in gcc 4.0: we need to
813  // extract this comparison outside of the if statement.
814  bool crossed_over = (this_is_in != last_is_in);
815  if (crossed_over) {
816  // We have just crossed over the clipping line. Find the point
817  // of intersection.
818  LVector2 d = this_point - last_point;
819  PN_stdfloat denom = (a * d[0] + b * d[1]);
820  if (denom != 0.0) {
821  PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
822  LPoint2 p = last_point + t * d;
823 
824  new_points.push_back(PointDef(p[0], p[1]));
825  last_is_in = this_is_in;
826  }
827  }
828 
829  if (this_is_in) {
830  // We are behind the clipping line. Keep the point.
831  new_points.push_back(PointDef(this_point[0], this_point[1]));
832  } else {
833  all_in = false;
834  }
835 
836  last_point = this_point;
837  }
838 
839  return all_in;
840 }
841 
842 
843 ////////////////////////////////////////////////////////////////////
844 // Function: CollisionBox::
845 // Access: Private
846 // Description: Returns the linear distance from the 2-d point to the
847 // nearest part of the polygon defined by the points
848 // vector. The result is negative if the point is
849 // within the polygon.
850 ////////////////////////////////////////////////////////////////////
851 PN_stdfloat CollisionBox::
852 dist_to_polygon(const LPoint2 &p, const CollisionBox::Points &points) const {
853 
854  // We know that that the polygon is convex and is defined with the
855  // points in counterclockwise order. Therefore, we simply compare
856  // the signed distance to each line segment; we ignore any negative
857  // values, and take the minimum of all the positive values.
858 
859  // If all values are negative, the point is within the polygon; we
860  // therefore return an arbitrary negative result.
861 
862  bool got_dist = false;
863  PN_stdfloat best_dist = -1.0f;
864 
865  size_t num_points = points.size();
866  for (size_t i = 0; i < num_points - 1; ++i) {
867  PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
868  points[i]._v);
869  if (d >= 0.0f) {
870  if (!got_dist || d < best_dist) {
871  best_dist = d;
872  got_dist = true;
873  }
874  }
875  }
876 
877  PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
878  points[num_points - 1]._v);
879  if (d >= 0.0f) {
880  if (!got_dist || d < best_dist) {
881  best_dist = d;
882  got_dist = true;
883  }
884  }
885 
886  return best_dist;
887 }
888 
889 ////////////////////////////////////////////////////////////////////
890 // Function: CollisionBox::dist_to_line_segment
891 // Access: Private, Static
892 // Description: Returns the linear distance of p to the line segment
893 // defined by f and t, where v = (t - f).normalize().
894 // The result is negative if p is left of the line,
895 // positive if it is right of the line. If the result
896 // is positive, it is constrained by endpoints of the
897 // line segment (i.e. the result might be larger than it
898 // would be for a straight distance-to-line test). If
899 // the result is negative, we don't bother.
900 ////////////////////////////////////////////////////////////////////
901 PN_stdfloat CollisionBox::
902 dist_to_line_segment(const LPoint2 &p,
903  const LPoint2 &f, const LPoint2 &t,
904  const LVector2 &v) {
905  LVector2 v1 = (p - f);
906  PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
907  if (d < 0.0f) {
908  return d;
909  }
910 
911  // Compute the nearest point on the line.
912  LPoint2 q = p + LVector2(-v[1], v[0]) * d;
913 
914  // Now constrain that point to the line segment.
915  if (v[0] > 0.0f) {
916  // X+
917  if (v[1] > 0.0f) {
918  // Y+
919  if (v[0] > v[1]) {
920  // X-dominant.
921  if (q[0] < f[0]) {
922  return (p - f).length();
923  } if (q[0] > t[0]) {
924  return (p - t).length();
925  } else {
926  return d;
927  }
928  } else {
929  // Y-dominant.
930  if (q[1] < f[1]) {
931  return (p - f).length();
932  } if (q[1] > t[1]) {
933  return (p - t).length();
934  } else {
935  return d;
936  }
937  }
938  } else {
939  // Y-
940  if (v[0] > -v[1]) {
941  // X-dominant.
942  if (q[0] < f[0]) {
943  return (p - f).length();
944  } if (q[0] > t[0]) {
945  return (p - t).length();
946  } else {
947  return d;
948  }
949  } else {
950  // Y-dominant.
951  if (q[1] > f[1]) {
952  return (p - f).length();
953  } if (q[1] < t[1]) {
954  return (p - t).length();
955  } else {
956  return d;
957  }
958  }
959  }
960  } else {
961  // X-
962  if (v[1] > 0.0f) {
963  // Y+
964  if (-v[0] > v[1]) {
965  // X-dominant.
966  if (q[0] > f[0]) {
967  return (p - f).length();
968  } if (q[0] < t[0]) {
969  return (p - t).length();
970  } else {
971  return d;
972  }
973  } else {
974  // Y-dominant.
975  if (q[1] < f[1]) {
976  return (p - f).length();
977  } if (q[1] > t[1]) {
978  return (p - t).length();
979  } else {
980  return d;
981  }
982  }
983  } else {
984  // Y-
985  if (-v[0] > -v[1]) {
986  // X-dominant.
987  if (q[0] > f[0]) {
988  return (p - f).length();
989  } if (q[0] < t[0]) {
990  return (p - t).length();
991  } else {
992  return d;
993  }
994  } else {
995  // Y-dominant.
996  if (q[1] > f[1]) {
997  return (p - f).length();
998  } if (q[1] < t[1]) {
999  return (p - t).length();
1000  } else {
1001  return d;
1002  }
1003  }
1004  }
1005  }
1006 }
1007 
1008 ////////////////////////////////////////////////////////////////////
1009 // Function: CollisionBox::point_is_inside
1010 // Access: Private
1011 // Description: Returns true if the indicated point is within the
1012 // polygon's 2-d space, false otherwise.
1013 ////////////////////////////////////////////////////////////////////
1014 bool CollisionBox::
1015 point_is_inside(const LPoint2 &p, const CollisionBox::Points &points) const {
1016  // We insist that the polygon be convex. This makes things a bit simpler.
1017  // In the case of a convex polygon, defined with points in counterclockwise
1018  // order, a point is interior to the polygon iff the point is not right of
1019  // each of the edges.
1020  for (int i = 0; i < (int)points.size() - 1; i++) {
1021  if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1022  return false;
1023  }
1024  }
1025  if (is_right(p - points[points.size() - 1]._p,
1026  points[0]._p - points[points.size() - 1]._p)) {
1027  return false;
1028  }
1029 
1030  return true;
1031 }
1032 
1033 ////////////////////////////////////////////////////////////////////
1034 // Function: CollisionBox::compute_vectors
1035 // Access: Private, Static
1036 // Description: Now that the _p members of the given points array
1037 // have been computed, go back and compute all of the _v
1038 // members.
1039 ////////////////////////////////////////////////////////////////////
1040 void CollisionBox::
1042  size_t num_points = points.size();
1043  for (size_t i = 0; i < num_points; i++) {
1044  points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1045  points[i]._v.normalize();
1046  }
1047 }
1048 
1049 ////////////////////////////////////////////////////////////////////
1050 // Function: CollisionBox::register_with_read_factory
1051 // Access: Public, Static
1052 // Description: Factory method to generate a CollisionBox object
1053 ////////////////////////////////////////////////////////////////////
1054 void CollisionBox::
1056  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
1057 }
1058 
1059 ////////////////////////////////////////////////////////////////////
1060 // Function: CollisionBox::write_datagram
1061 // Access: Public
1062 // Description: Function to write the important information in
1063 // the particular object to a Datagram
1064 ////////////////////////////////////////////////////////////////////
1065 void CollisionBox::
1067  CollisionSolid::write_datagram(manager, me);
1068  _center.write_datagram(me);
1069  _min.write_datagram(me);
1070  _max.write_datagram(me);
1071  for(int i=0; i < 8; i++) {
1072  _vertex[i].write_datagram(me);
1073  }
1074  me.add_stdfloat(_radius);
1075  me.add_stdfloat(_x);
1076  me.add_stdfloat(_y);
1077  me.add_stdfloat(_z);
1078  for(int i=0; i < 6; i++) {
1079  _planes[i].write_datagram(me);
1080  }
1081  for(int i=0; i < 6; i++) {
1082  _to_2d_mat[i].write_datagram(me);
1083  }
1084  for(int i=0; i < 6; i++) {
1085  me.add_uint16(_points[i].size());
1086  for (size_t j = 0; j < _points[i].size(); j++) {
1087  _points[i][j]._p.write_datagram(me);
1088  _points[i][j]._v.write_datagram(me);
1089  }
1090  }
1091 }
1092 
1093 ////////////////////////////////////////////////////////////////////
1094 // Function: CollisionBox::make_CollisionBox
1095 // Access: Protected
1096 // Description: Factory method to generate a CollisionBox object
1097 ////////////////////////////////////////////////////////////////////
1098 TypedWritable *CollisionBox::
1099 make_CollisionBox(const FactoryParams &params) {
1100  CollisionBox *me = new CollisionBox;
1101  DatagramIterator scan;
1102  BamReader *manager;
1103 
1104  parse_params(params, scan, manager);
1105  me->fillin(scan, manager);
1106  return me;
1107 }
1108 
1109 ////////////////////////////////////////////////////////////////////
1110 // Function: CollisionBox::fillin
1111 // Access: Protected
1112 // Description: Function that reads out of the datagram (or asks
1113 // manager to read) all of the data that is needed to
1114 // re-create this object and stores it in the appropiate
1115 // place
1116 ////////////////////////////////////////////////////////////////////
1117 void CollisionBox::
1118 fillin(DatagramIterator& scan, BamReader* manager) {
1119  CollisionSolid::fillin(scan, manager);
1120  _center.read_datagram(scan);
1121  _min.read_datagram(scan);
1122  _max.read_datagram(scan);
1123  for(int i=0; i < 8; i++) {
1124  _vertex[i].read_datagram(scan);
1125  }
1126  _radius = scan.get_stdfloat();
1127  _x = scan.get_stdfloat();
1128  _y = scan.get_stdfloat();
1129  _z = scan.get_stdfloat();
1130  for(int i=0; i < 6; i++) {
1131  _planes[i].read_datagram(scan);
1132  }
1133  for(int i=0; i < 6; i++) {
1134  _to_2d_mat[i].read_datagram(scan);
1135  }
1136  for(int i=0; i < 6; i++) {
1137  size_t size = scan.get_uint16();
1138  for (size_t j = 0; j < size; j++) {
1139  LPoint2 p;
1140  LVector2 v;
1141  p.read_datagram(scan);
1142  v.read_datagram(scan);
1143  _points[i].push_back(PointDef(p, v));
1144  }
1145  }
1146 }
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
float length_squared() const
Returns the square of the vector&#39;s length, cheap and easy.
Definition: lvecBase3.h:749
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...
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
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
NodePath get_into_node_path() const
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:122
void set_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. ...
NodePath get_on_plane(int n) const
Returns the nth plane enabled by the attribute, sorted in render order.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
Definition: lvecBase3.h:1389
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).
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.
This defines a bounding sphere, consisting of a center and a radius.
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
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.
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
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.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
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...
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
const CollisionSolid * get_into() const
Returns the CollisionSolid pointer for the particular solid was collided into.
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:1177
This is a 4-by-4 transform matrix.
Definition: lmatrix.h:451
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
Definition: lvecBase3.h:1371
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
Definition: planeNode.I:65
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
LPlane set_plane(int n) const
Creates the nth face of the rectangular solid.
Definition: collisionBox.I:207
void read_datagram(DatagramIterator &source)
Reads the matrix from the Datagram using get_stdfloat().
Definition: lmatrix.cxx:1162
LPlane get_plane(int n) const
Returns the nth face of the rectangular solid.
Definition: collisionBox.I:196
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
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...
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
Defines a series of disconnected triangles.
Definition: geomTriangles.h:25
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
Definition: collisionBox.I:159
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
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().
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
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...
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:165
A node that contains a plane.
Definition: planeNode.h:39
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...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
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