Panda3D
collisionBox.cxx
Go to the documentation of this file.
1 /**
2  * PANDA 3D SOFTWARE
3  * Copyright (c) Carnegie Mellon University. All rights reserved.
4  *
5  * All use of this software is subject to the terms of the revised BSD
6  * license. You should have received a copy of this license along
7  * with this source code in a file named "LICENSE."
8  *
9  * @file collisionBox.cxx
10  * @author amith tudur
11  * @date 2009-07-31
12  */
13 
14 #include "collisionBox.h"
15 #include "collisionLine.h"
16 #include "collisionRay.h"
17 #include "collisionSphere.h"
18 #include "collisionSegment.h"
19 #include "collisionCapsule.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 "geomLines.h"
33 #include "geomTriangles.h"
34 #include "geomVertexWriter.h"
35 #include "config_mathutil.h"
36 #include "dcast.h"
37 
38 #include <math.h>
39 
40 using std::max;
41 using std::min;
42 
43 PStatCollector CollisionBox::_volume_pcollector("Collision Volumes:CollisionBox");
44 PStatCollector CollisionBox::_test_pcollector("Collision Tests:CollisionBox");
45 TypeHandle CollisionBox::_type_handle;
46 
47 const int CollisionBox::plane_def[6][4] = {
48  {0, 4, 5, 1},
49  {4, 6, 7, 5},
50  {6, 2, 3, 7},
51  {2, 0, 1, 3},
52  {1, 5, 7, 3},
53  {2, 6, 4, 0},
54 };
55 
56 /**
57  *
58  */
59 CollisionSolid *CollisionBox::
60 make_copy() {
61  return new CollisionBox(*this);
62 }
63 
64 /**
65  * Compute parameters for each of the box's sides
66  */
68 setup_box(){
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  * Computes the plane and 2d projection of points that make up this side.
81  */
83 setup_points(const LPoint3 *begin, const LPoint3 *end, int plane) {
84  int num_points = end - begin;
85  nassertv(num_points >= 3);
86 
87  _points[plane].clear();
88 
89  // Construct a matrix that rotates the points from the (X,0,Z) plane into
90  // the 3-d plane.
91  LMatrix4 to_3d_mat;
92  calc_to_3d_mat(to_3d_mat, plane);
93 
94  // And the inverse matrix rotates points from 3-d space into the 2-d plane.
95  _to_2d_mat[plane].invert_from(to_3d_mat);
96 
97  // Now project all of the points onto the 2-d plane.
98 
99  const LPoint3 *pi;
100  for (pi = begin; pi != end; ++pi) {
101  LPoint3 point = (*pi) * _to_2d_mat[plane];
102  _points[plane].push_back(PointDef(point[0], point[2]));
103  }
104 
105  nassertv(_points[plane].size() >= 3);
106 
107 #ifndef NDEBUG
108  /*
109  // Now make sure the points define a convex polygon.
110  if (is_concave()) {
111  collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
112  const LPoint3 *pi;
113  for (pi = begin; pi != end; ++pi) {
114  collide_cat.error(false) << " " << (*pi) << "\n";
115  }
116  collide_cat.error(false)
117  << " normal " << normal << " with length " << normal.length() << "\n";
118  _points.clear();
119  }
120  */
121 #endif
122 
123  compute_vectors(_points[plane]);
124 }
125 
126 /**
127  * First Dispatch point for box as a FROM object
128  */
129 PT(CollisionEntry) CollisionBox::
130 test_intersection(const CollisionEntry &entry) const {
131  return entry.get_into()->test_intersection_from_box(entry);
132 }
133 
134 /**
135  * Transforms the solid by the indicated matrix.
136  */
137 void CollisionBox::
138 xform(const LMatrix4 &mat) {
139  _min = _min * mat;
140  _max = _max * mat;
141  _center = _center * mat;
142  for(int v = 0; v < 8; v++) {
143  _vertex[v] = _vertex[v] * mat;
144  }
145  for(int p = 0; p < 6 ; p++) {
146  _planes[p] = set_plane(p);
147  }
148  _x = _vertex[0].get_x() - _center.get_x();
149  _y = _vertex[0].get_y() - _center.get_y();
150  _z = _vertex[0].get_z() - _center.get_z();
151  _radius = sqrt(_x * _x + _y * _y + _z * _z);
152  setup_box();
153  mark_viz_stale();
154  mark_internal_bounds_stale();
155 }
156 
157 /**
158  * Returns the point in space deemed to be the "origin" of the solid for
159  * collision purposes. The closest intersection point to this origin point is
160  * considered to be the most significant.
161  */
163 get_collision_origin() const {
164  return _center;
165 }
166 
167 /**
168  * Returns a PStatCollector that is used to count the number of bounding
169  * volume tests made against a solid of this type in a given frame.
170  */
173  return _volume_pcollector;
174 }
175 
176 /**
177  * Returns a PStatCollector that is used to count the number of intersection
178  * tests made against a solid of this type in a given frame.
179  */
182  return _test_pcollector;
183 }
184 
185 /**
186  *
187  */
188 void CollisionBox::
189 output(std::ostream &out) const {
190  out << "box, (" << get_min() << ") to (" << get_max() << ")";
191 }
192 
193 /**
194  * Sphere is chosen as the Bounding Volume type for speed and efficiency
195  */
196 PT(BoundingVolume) CollisionBox::
197 compute_internal_bounds() const {
198  return new BoundingSphere(_center, _radius);
199 }
200 
201 /**
202  * Double dispatch point for sphere as FROM object
203  */
204 PT(CollisionEntry) CollisionBox::
205 test_intersection_from_sphere(const CollisionEntry &entry) const {
206 
207  const CollisionSphere *sphere;
208  DCAST_INTO_R(sphere, entry.get_from(), nullptr);
209 
210  CPT(TransformState) wrt_space = entry.get_wrt_space();
211  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
212 
213  const LMatrix4 &wrt_mat = wrt_space->get_mat();
214 
215  LPoint3 orig_center = sphere->get_center() * wrt_mat;
216  LPoint3 from_center = orig_center;
217  bool moved_from_center = false;
218  PN_stdfloat t = 1.0f;
219  LPoint3 contact_point(from_center);
220  PN_stdfloat actual_t = 1.0f;
221 
222  LVector3 from_radius_v =
223  LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
224  PN_stdfloat from_radius_2 = from_radius_v.length_squared();
225  PN_stdfloat from_radius = csqrt(from_radius_2);
226 
227  int ip;
228  PN_stdfloat max_dist = 0.0;
229  PN_stdfloat dist = 0.0;
230  bool intersect;
231  LPlane plane;
232  LVector3 normal;
233 
234  for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
235  plane = get_plane( ip );
236  if (_points[ip].size() < 3) {
237  continue;
238  }
239  if (wrt_prev_space != wrt_space) {
240  // If we have a delta between the previous position and the current
241  // position, we use that to determine some more properties of the
242  // collision.
243  LPoint3 b = from_center;
244  LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
245  LVector3 delta = b - a;
246 
247  // First, there is no collision if the "from" object is definitely
248  // moving in the same direction as the plane's normal.
249  PN_stdfloat dot = delta.dot(plane.get_normal());
250  if (dot > 0.1f) {
251  continue; // no intersection
252  }
253 
254  if (IS_NEARLY_ZERO(dot)) {
255  // If we're moving parallel to the plane, the sphere is tested at its
256  // final point. Leave it as it is.
257 
258  } else {
259 /*
260  * Otherwise, we're moving into the plane; the sphere is tested at the point
261  * along its path that is closest to intersecting the plane. This may be the
262  * actual intersection point, or it may be the starting point or the final
263  * point. dot is equal to the (negative) magnitude of 'delta' along the
264  * direction of the plane normal t = ratio of (distance from start pos to
265  * plane) to (distance from start pos to end pos), along axis of plane normal
266  */
267  PN_stdfloat dist_to_p = plane.dist_to_plane(a);
268  t = (dist_to_p / -dot);
269 
270  // also compute the actual contact point and time of contact for
271  // handlers that need it
272  actual_t = ((dist_to_p - from_radius) / -dot);
273  actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
274  contact_point = a + (actual_t * delta);
275 
276  if (t >= 1.0f) {
277  // Leave it where it is.
278 
279  } else if (t < 0.0f) {
280  from_center = a;
281  moved_from_center = true;
282  } else {
283  from_center = a + t * delta;
284  moved_from_center = true;
285  }
286  }
287  }
288 
289  normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
290 
291 #ifndef NDEBUG
292  /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
293  std::cout
294  << "polygon within " << entry.get_into_node_path()
295  << " has normal " << normal << " of length " << normal.length()
296  << "\n";
297  normal.normalize();
298  }*/
299 #endif
300 
301  // The nearest point within the plane to our center is the intersection of
302  // the line (center, center - normal) with the plane.
303 
304  if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
305  // No intersection with plane? This means the plane's effective normal
306  // was within the plane itself. A useless polygon.
307  continue;
308  }
309 
310  if (dist > from_radius || dist < -from_radius) {
311  // No intersection with the plane.
312  continue;
313  }
314 
315  LPoint2 p = to_2d(from_center - dist * plane.get_normal(), ip);
316  PN_stdfloat edge_dist = 0.0f;
317 
318  const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
319  if (cpa != nullptr) {
320  // We have a clip plane; apply it.
321  Points new_points;
322  if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
323  // All points are behind the clip plane; just do the default test.
324  edge_dist = dist_to_polygon(p, _points[ip]);
325  } else if (new_points.empty()) {
326  // The polygon is completely clipped.
327  continue;
328  } else {
329  // Test against the clipped polygon.
330  edge_dist = dist_to_polygon(p, new_points);
331  }
332  } else {
333  // No clip plane is in effect. Do the default test.
334  edge_dist = dist_to_polygon(p, _points[ip]);
335  }
336 
337  max_dist = from_radius;
338 
339  // Now we have edge_dist, which is the distance from the sphere center to
340  // the nearest edge of the polygon, within the polygon's plane.
341  // edge_dist<0 means the point is within the polygon.
342  if(edge_dist < 0) {
343  intersect = true;
344  continue;
345  }
346 
347  if((edge_dist > 0) &&
348  ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
349  // No intersection; the circle is outside the polygon.
350  continue;
351  }
352 
353  // The sphere appears to intersect the polygon. If the edge is less than
354  // from_radius away, the sphere may be resting on an edge of the polygon.
355  // Determine how far the center of the sphere must remain from the plane,
356  // based on its distance from the nearest edge.
357 
358  if (edge_dist >= 0.0f) {
359  PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
360  max_dist = csqrt(max_dist_2);
361  }
362 
363  if (dist > max_dist) {
364  // There's no intersection: the sphere is hanging off the edge.
365  continue;
366  }
367  intersect = true;
368  }
369  if( !intersect )
370  return nullptr;
371 
372  if (collide_cat.is_debug()) {
373  collide_cat.debug()
374  << "intersection detected from " << entry.get_from_node_path()
375  << " into " << entry.get_into_node_path() << "\n";
376  }
377 
378  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
379 
380  PN_stdfloat into_depth = max_dist - dist;
381  if (moved_from_center) {
382  // We have to base the depth of intersection on the sphere's final resting
383  // point, not the point from which we tested the intersection.
384  PN_stdfloat orig_dist;
385  plane.intersects_line(orig_dist, orig_center, -normal);
386  into_depth = max_dist - orig_dist;
387  }
388 
389  // Clamp the surface point to the box bounds.
390  LPoint3 surface = from_center - normal * dist;
391  surface = surface.fmax(_min);
392  surface = surface.fmin(_max);
393 
394  new_entry->set_surface_normal(normal);
395  new_entry->set_surface_point(surface);
396  new_entry->set_interior_point(surface - normal * into_depth);
397  new_entry->set_contact_pos(contact_point);
398  new_entry->set_contact_normal(plane.get_normal());
399  new_entry->set_t(actual_t);
400 
401  return new_entry;
402 }
403 
404 /**
405  *
406  */
407 PT(CollisionEntry) CollisionBox::
408 test_intersection_from_line(const CollisionEntry &entry) const {
409  const CollisionLine *line;
410  DCAST_INTO_R(line, entry.get_from(), nullptr);
411 
412  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
413 
414  LPoint3 from_origin = line->get_origin() * wrt_mat;
415  LVector3 from_direction = line->get_direction() * wrt_mat;
416 
417  double t1, t2;
418  if (!intersects_line(t1, t2, from_origin, from_direction)) {
419  // No intersection.
420  return nullptr;
421  }
422 
423  if (collide_cat.is_debug()) {
424  collide_cat.debug()
425  << "intersection detected from " << entry.get_from_node_path()
426  << " into " << entry.get_into_node_path() << "\n";
427  }
428  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
429 
430  LPoint3 point = from_origin + t1 * from_direction;
431  new_entry->set_surface_point(point);
432 
434  new_entry->set_surface_normal(get_effective_normal());
435  } else {
436  LVector3 normal(
437  IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
438  IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
439  IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
440  );
441  normal.normalize();
442  new_entry->set_surface_normal(normal);
443  }
444 
445  return new_entry;
446 }
447 
448 /**
449  * Double dispatch point for ray as a FROM object
450  */
451 PT(CollisionEntry) CollisionBox::
452 test_intersection_from_ray(const CollisionEntry &entry) const {
453  const CollisionRay *ray;
454  DCAST_INTO_R(ray, entry.get_from(), nullptr);
455  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
456 
457  LPoint3 from_origin = ray->get_origin() * wrt_mat;
458  LVector3 from_direction = ray->get_direction() * wrt_mat;
459 
460  double t1, t2;
461  if (!intersects_line(t1, t2, from_origin, from_direction) || (t1 < 0.0 && t2 < 0.0)) {
462  // No intersection.
463  return nullptr;
464  }
465 
466  if (collide_cat.is_debug()) {
467  collide_cat.debug()
468  << "intersection detected from " << entry.get_from_node_path()
469  << " into " << entry.get_into_node_path() << "\n";
470  }
471  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
472 
473  if (t1 < 0.0) {
474  // The origin is inside the box, so we take the exit as our surface point.
475  new_entry->set_interior_point(from_origin);
476  t1 = t2;
477  }
478 
479  LPoint3 point = from_origin + t1 * from_direction;
480  new_entry->set_surface_point(point);
481 
483  new_entry->set_surface_normal(get_effective_normal());
484  } else {
485  LVector3 normal(
486  IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
487  IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
488  IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
489  );
490  normal.normalize();
491  new_entry->set_surface_normal(normal);
492  }
493 
494  return new_entry;
495 }
496 
497 /**
498  * Double dispatch point for segment as a FROM object
499  */
500 PT(CollisionEntry) CollisionBox::
501 test_intersection_from_segment(const CollisionEntry &entry) const {
502  const CollisionSegment *seg;
503  DCAST_INTO_R(seg, entry.get_from(), nullptr);
504  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
505 
506  LPoint3 from_origin = seg->get_point_a() * wrt_mat;
507  LPoint3 from_extent = seg->get_point_b() * wrt_mat;
508  LVector3 from_direction = from_extent - from_origin;
509 
510  double t1, t2;
511  if (!intersects_line(t1, t2, from_origin, from_direction) ||
512  (t1 < 0.0 && t2 < 0.0) || (t1 > 1.0 && t2 > 1.0)) {
513  // No intersection.
514  return nullptr;
515  }
516 
517  if (collide_cat.is_debug()) {
518  collide_cat.debug()
519  << "intersection detected from " << entry.get_from_node_path()
520  << " into " << entry.get_into_node_path() << "\n";
521  }
522  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
523 
524  // In case the segment is entirely inside the cube, we consider the point
525  // closest to the surface as our entry point.
526  if (t1 < (1.0 - t2)) {
527  std::swap(t1, t2);
528  }
529 
530  // Our interior point is the closest point to t2 that is inside the segment.
531  new_entry->set_interior_point(from_origin + std::min(std::max(t2, 0.0), 1.0) * from_direction);
532 
533  LPoint3 point = from_origin + t1 * from_direction;
534  new_entry->set_surface_point(point);
535 
537  new_entry->set_surface_normal(get_effective_normal());
538  } else {
539  LVector3 normal(
540  IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
541  IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
542  IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
543  );
544  normal.normalize();
545  new_entry->set_surface_normal(normal);
546  }
547 
548  return new_entry;
549 }
550 
551 /**
552  * Double dispatch point for capsule as a FROM object
553  */
554 PT(CollisionEntry) CollisionBox::
555 test_intersection_from_capsule(const CollisionEntry &entry) const {
556  const CollisionCapsule *capsule;
557  DCAST_INTO_R(capsule, entry.get_from(), nullptr);
558 
559  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
560 
561  LPoint3 from_a = capsule->get_point_a() * wrt_mat;
562  LPoint3 from_b = capsule->get_point_b() * wrt_mat;
563  LVector3 from_direction = from_b - from_a;
564  PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, capsule->get_radius())).length_squared();
565  PN_stdfloat radius = csqrt(radius_sq);
566 
567  LPoint3 box_min = get_min();
568  LPoint3 box_max = get_max();
569  LVector3 dimensions = box_max - box_min;
570 
571  // The method below is inspired by Christer Ericson's book Real-Time
572  // Collision Detection. Instead of testing a capsule against a box, we test
573  // a segment against an box that is oversized by the capsule radius.
574 
575  // First, we test if the line segment intersects a box with its faces
576  // expanded outwards by the capsule radius. If not, there is no collision.
577  double t1, t2;
578  if (!intersects_line(t1, t2, from_a, from_direction, radius)) {
579  return nullptr;
580  }
581 
582  if (t2 < 0.0 || t1 > 1.0) {
583  return nullptr;
584  }
585 
586  t1 = std::min(1.0, std::max(0.0, (t1 + t2) * 0.5));
587  LPoint3 point = from_a + from_direction * t1;
588 
589  // We now have a point of intersection between the line segment and the
590  // oversized box. Check on how many axes it lies outside the box. If it is
591  // less than two, we know that it does not lie in one of the rounded regions
592  // of the oversized rounded box, and it is a guaranteed hit. Otherwise, we
593  // will need to test against the edge regions.
594  if ((point[0] < box_min[0] || point[0] > box_max[0]) +
595  (point[1] < box_min[1] || point[1] > box_max[1]) +
596  (point[2] < box_min[2] || point[2] > box_max[2]) > 1) {
597  // Test the capsule against each edge of the box.
598  static const struct {
599  LPoint3 point;
600  int axis;
601  } edges[] = {
602  {{0, 0, 0}, 0},
603  {{0, 1, 0}, 0},
604  {{0, 0, 1}, 0},
605  {{0, 1, 1}, 0},
606  {{0, 0, 0}, 1},
607  {{0, 0, 1}, 1},
608  {{1, 0, 0}, 1},
609  {{1, 0, 1}, 1},
610  {{0, 0, 0}, 2},
611  {{0, 1, 0}, 2},
612  {{1, 0, 0}, 2},
613  {{1, 1, 0}, 2},
614  };
615 
616  PN_stdfloat best_dist_sq = FLT_MAX;
617 
618  for (int i = 0; i < 12; ++i) {
619  LPoint3 vertex = edges[i].point;
620  vertex.componentwise_mult(dimensions);
621  vertex += box_min;
622  LVector3 delta(0);
623  delta[edges[i].axis] = dimensions[edges[i].axis];
624  double u1, u2;
625  CollisionCapsule::calc_closest_segment_points(u1, u2, from_a, from_direction, vertex, delta);
626  PN_stdfloat dist_sq = ((from_a + from_direction * u1) - (vertex + delta * u2)).length_squared();
627  if (dist_sq < best_dist_sq) {
628  best_dist_sq = dist_sq;
629  }
630  }
631 
632  if (best_dist_sq > radius_sq) {
633  // It is not actually touching any edge.
634  return nullptr;
635  }
636  }
637 
638  if (collide_cat.is_debug()) {
639  collide_cat.debug()
640  << "intersection detected from " << entry.get_from_node_path()
641  << " into " << entry.get_into_node_path() << "\n";
642  }
643  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
644 
645  // Which is the longest axis?
646  LVector3 diff = point - _center;
647  diff[0] /= dimensions[0];
648  diff[1] /= dimensions[1];
649  diff[2] /= dimensions[2];
650  int axis = 0;
651  if (cabs(diff[0]) > cabs(diff[1])) {
652  if (cabs(diff[0]) > cabs(diff[2])) {
653  axis = 0;
654  } else {
655  axis = 2;
656  }
657  } else {
658  if (cabs(diff[1]) > cabs(diff[2])) {
659  axis = 1;
660  } else {
661  axis = 2;
662  }
663  }
664  LVector3 normal(0);
665  normal[axis] = std::copysign(1, diff[axis]);
666 
667  LPoint3 clamped = point.fmax(box_min).fmin(box_max);
668  LPoint3 surface_point = clamped;
669  surface_point[axis] = (diff[axis] >= 0.0f) ? box_max[axis] : box_min[axis];
670 
671  // Is the point inside the box?
672  LVector3 interior_vec;
673  if (clamped != point) {
674  // No, it is outside. The interior point is in the direction of the
675  // surface point.
676  interior_vec = point - surface_point;
677  if (!interior_vec.normalize()) {
678  interior_vec = normal;
679  }
680  } else {
681  // It is inside. I think any point will work for this.
682  interior_vec = normal;
683  }
684  new_entry->set_interior_point(point - interior_vec * radius);
685  new_entry->set_surface_point(surface_point);
686 
688  new_entry->set_surface_normal(get_effective_normal());
689  } else {
690  new_entry->set_surface_normal(normal);
691  }
692 
693  return new_entry;
694 }
695 
696 /**
697  * Double dispatch point for box as a FROM object
698  */
699 PT(CollisionEntry) CollisionBox::
700 test_intersection_from_box(const CollisionEntry &entry) const {
701  const CollisionBox *box;
702  DCAST_INTO_R(box, entry.get_from(), nullptr);
703 
704  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
705 
706  LPoint3 diff = wrt_mat.xform_point_general(box->get_center()) - _center;
707  LVector3 from_extents = box->get_dimensions() * 0.5f;
708  LVector3 into_extents = get_dimensions() * 0.5f;
709 
710  LVecBase3 box_x = wrt_mat.get_row3(0);
711  LVecBase3 box_y = wrt_mat.get_row3(1);
712  LVecBase3 box_z = wrt_mat.get_row3(2);
713 
714  // To make the math simpler, normalize the box basis vectors, instead
715  // applying the scale to the box dimensions. Note that this doesn't work
716  // for a non-uniform scales applied after a rotation, since that has the
717  // possibility of making the box no longer a box.
718  PN_stdfloat l;
719  l = box_x.length();
720  from_extents[0] *= l;
721  box_x /= l;
722  l = box_y.length();
723  from_extents[1] *= l;
724  box_y /= l;
725  l = box_z.length();
726  from_extents[2] *= l;
727  box_z /= l;
728 
729  PN_stdfloat r1, r2;
730  PN_stdfloat min_pen = 0;
731  PN_stdfloat pen;
732  int axis = 0;
733 
734  // SAT test for the three axes of the into cube.
735  r1 = into_extents[0];
736  r2 = cabs(box_x[0] * from_extents[0]) +
737  cabs(box_y[0] * from_extents[1]) +
738  cabs(box_z[0] * from_extents[2]);
739  pen = r1 + r2 - cabs(diff[0]);
740  if (pen < 0) {
741  return nullptr;
742  }
743  min_pen = pen;
744 
745  r1 = into_extents[1];
746  r2 = cabs(box_x[1] * from_extents[0]) +
747  cabs(box_y[1] * from_extents[1]) +
748  cabs(box_z[1] * from_extents[2]);
749  pen = r1 + r2 - cabs(diff[1]);
750  if (pen < 0) {
751  return nullptr;
752  }
753  if (pen < min_pen) {
754  min_pen = pen;
755  axis = 1;
756  }
757 
758  r1 = into_extents[2];
759  r2 = cabs(box_x[2] * from_extents[0]) +
760  cabs(box_y[2] * from_extents[1]) +
761  cabs(box_z[2] * from_extents[2]);
762  pen = r1 + r2 - cabs(diff[2]);
763  if (pen < 0) {
764  return nullptr;
765  }
766  if (pen < min_pen) {
767  min_pen = pen;
768  axis = 2;
769  }
770 
771  // SAT test for the three axes of the from cube.
772  r1 = cabs(box_x[0] * into_extents[0]) +
773  cabs(box_x[1] * into_extents[1]) +
774  cabs(box_x[2] * into_extents[2]);
775  r2 = from_extents[0];
776  pen = r1 + r2 - cabs(diff.dot(box_x));
777  if (pen < 0) {
778  return nullptr;
779  }
780  if (pen < min_pen) {
781  min_pen = pen;
782  }
783 
784  r1 = cabs(box_y[0] * into_extents[0]) +
785  cabs(box_y[1] * into_extents[1]) +
786  cabs(box_y[2] * into_extents[2]);
787  r2 = from_extents[1];
788  pen = r1 + r2 - cabs(diff.dot(box_y));
789  if (pen < 0) {
790  return nullptr;
791  }
792  if (pen < min_pen) {
793  min_pen = pen;
794  }
795 
796  r1 = cabs(box_z[0] * into_extents[0]) +
797  cabs(box_z[1] * into_extents[1]) +
798  cabs(box_z[2] * into_extents[2]);
799  r2 = from_extents[2];
800  pen = r1 + r2 - cabs(diff.dot(box_z));
801  if (pen < 0) {
802  return nullptr;
803  }
804  if (pen < min_pen) {
805  min_pen = pen;
806  }
807 
808  // SAT test of the nine cross products.
809  r1 = into_extents[1] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[1]);
810  r2 = from_extents[1] * cabs(box_z[0]) + from_extents[2] * cabs(box_y[0]);
811  if (cabs(diff[2] * box_x[1] - diff[1] * box_x[2]) > r1 + r2) {
812  return nullptr;
813  }
814 
815  r1 = into_extents[1] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[1]);
816  r2 = from_extents[0] * cabs(box_z[0]) + from_extents[2] * cabs(box_x[0]);
817  if (cabs(diff[2] * box_y[1] - diff[1] * box_y[2]) > r1 + r2) {
818  return nullptr;
819  }
820 
821  r1 = into_extents[1] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[1]);
822  r2 = from_extents[0] * cabs(box_y[0]) + from_extents[1] * cabs(box_x[0]);
823  if (cabs(diff[2] * box_z[1] - diff[1] * box_z[2]) > r1 + r2) {
824  return nullptr;
825  }
826 
827  r1 = into_extents[0] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[0]);
828  r2 = from_extents[1] * cabs(box_z[1]) + from_extents[2] * cabs(box_y[1]);
829  if (cabs(diff[0] * box_x[2] - diff[2] * box_x[0]) > r1 + r2) {
830  return nullptr;
831  }
832 
833  r1 = into_extents[0] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[0]);
834  r2 = from_extents[0] * cabs(box_z[1]) + from_extents[2] * cabs(box_x[1]);
835  if (cabs(diff[0] * box_y[2] - diff[2] * box_y[0]) > r1 + r2) {
836  return nullptr;
837  }
838 
839  r1 = into_extents[0] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[0]);
840  r2 = from_extents[0] * cabs(box_y[1]) + from_extents[1] * cabs(box_x[1]);
841  if (cabs(diff[0] * box_z[2] - diff[2] * box_z[0]) > r1 + r2) {
842  return nullptr;
843  }
844 
845  r1 = into_extents[0] * cabs(box_x[1]) + into_extents[1] * cabs(box_x[0]);
846  r2 = from_extents[1] * cabs(box_z[2]) + from_extents[2] * cabs(box_y[2]);
847  if (cabs(diff[1] * box_x[0] - diff[0] * box_x[1]) > r1 + r2) {
848  return nullptr;
849  }
850 
851  r1 = into_extents[0] * cabs(box_y[1]) + into_extents[1] * cabs(box_y[0]);
852  r2 = from_extents[0] * cabs(box_z[2]) + from_extents[2] * cabs(box_x[2]);
853  if (cabs(diff[1] * box_y[0] - diff[0] * box_y[1]) > r1 + r2) {
854  return nullptr;
855  }
856 
857  r1 = into_extents[0] * cabs(box_z[1]) + into_extents[1] * cabs(box_z[0]);
858  r2 = from_extents[0] * cabs(box_y[2]) + from_extents[1] * cabs(box_x[2]);
859  if (cabs(diff[1] * box_z[0] - diff[0] * box_z[1]) > r1 + r2) {
860  return nullptr;
861  }
862 
863  if (collide_cat.is_debug()) {
864  collide_cat.debug()
865  << "intersection detected from " << entry.get_from_node_path()
866  << " into " << entry.get_into_node_path() << "\n";
867  }
868  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
869 
870  // This isn't always the correct surface point. However, it seems to be
871  // enough to let the pusher do the right thing.
872  LPoint3 surface(
873  min(max(diff[0], -into_extents[0]), into_extents[0]),
874  min(max(diff[1], -into_extents[1]), into_extents[1]),
875  min(max(diff[2], -into_extents[2]), into_extents[2]));
876 
877  // Create the normal along the axis of least penetration.
878  LVector3 normal(0);
879  PN_stdfloat diff_axis = diff[axis];
880  int sign = (diff_axis >= 0) ? 1 : -1;
881  normal[axis] = sign;
882  surface[axis] = into_extents[axis] * sign;
883 
884  new_entry->set_surface_point(surface + _center);
885 
886  // Does not generate the correct depth. Needs fixing.
887  new_entry->set_interior_point(surface + _center + normal * -min_pen);
888 
890  new_entry->set_surface_normal(get_effective_normal());
891  } else {
892  new_entry->set_surface_normal(normal);
893  }
894 
895  return new_entry;
896 }
897 
898 /**
899  * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
900  * solid.
901  */
902 void CollisionBox::
903 fill_viz_geom() {
904  if (collide_cat.is_debug()) {
905  collide_cat.debug()
906  << "Recomputing viz for " << *this << "\n";
907  }
908 
909  PT(GeomVertexData) vdata = new GeomVertexData
910  ("collision", GeomVertexFormat::get_v3(),
911  Geom::UH_static);
912 
913  vdata->unclean_set_num_rows(8);
914 
915  {
916  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
917  vertex.set_data3(_min[0], _min[1], _min[2]);
918  vertex.set_data3(_min[0], _max[1], _min[2]);
919  vertex.set_data3(_max[0], _max[1], _min[2]);
920  vertex.set_data3(_max[0], _min[1], _min[2]);
921 
922  vertex.set_data3(_min[0], _min[1], _max[2]);
923  vertex.set_data3(_min[0], _max[1], _max[2]);
924  vertex.set_data3(_max[0], _max[1], _max[2]);
925  vertex.set_data3(_max[0], _min[1], _max[2]);
926  }
927 
928  PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static);
929 
930  // Bottom
931  tris->add_vertices(0, 1, 2);
932  tris->add_vertices(2, 3, 0);
933 
934  // Top
935  tris->add_vertices(4, 7, 6);
936  tris->add_vertices(6, 5, 4);
937 
938  // Sides
939  tris->add_vertices(0, 4, 1);
940  tris->add_vertices(1, 4, 5);
941 
942  tris->add_vertices(1, 5, 2);
943  tris->add_vertices(2, 5, 6);
944 
945  tris->add_vertices(2, 6, 3);
946  tris->add_vertices(3, 6, 7);
947 
948  tris->add_vertices(3, 7, 0);
949  tris->add_vertices(0, 7, 4);
950 
951  PT(GeomLines) lines = new GeomLines(Geom::UH_static);
952 
953  // Bottom
954  lines->add_vertices(0, 1);
955  lines->add_vertices(1, 2);
956  lines->add_vertices(0, 3);
957  lines->add_vertices(2, 3);
958 
959  // Top
960  lines->add_vertices(4, 5);
961  lines->add_vertices(5, 6);
962  lines->add_vertices(4, 7);
963  lines->add_vertices(6, 7);
964 
965  // Sides
966  lines->add_vertices(0, 4);
967  lines->add_vertices(1, 5);
968  lines->add_vertices(2, 6);
969  lines->add_vertices(3, 7);
970 
971  PT(Geom) geom1 = new Geom(vdata);
972  geom1->add_primitive(tris);
973 
974  PT(Geom) geom2 = new Geom(vdata);
975  geom2->add_primitive(lines);
976 
977  _viz_geom->add_geom(geom1, get_solid_viz_state());
978  _viz_geom->add_geom(geom2, get_wireframe_viz_state());
979 
980  _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
981  _bounds_viz_geom->add_geom(geom2, get_wireframe_viz_state());
982 }
983 
984 /**
985  * Determine the point(s) of intersection of a parametric line with the box.
986  * The line is infinite in both directions, and passes through "from" and
987  * from+delta. If the line does not intersect the box, the function returns
988  * false, and t1 and t2 are undefined. If it does intersect the box, it
989  * returns true, and t1 and t2 are set to the points along the equation
990  * from+t*delta that correspond to the two points of intersection.
991  */
992 bool CollisionBox::
993 intersects_line(double &t1, double &t2,
994  const LPoint3 &from, const LVector3 &delta,
995  PN_stdfloat inflate_size) const {
996 
997  LPoint3 bmin = _min - LVector3(inflate_size);
998  LPoint3 bmax = _max + LVector3(inflate_size);
999 
1000  double tmin = -DBL_MAX;
1001  double tmax = DBL_MAX;
1002 
1003  for (int i = 0; i < 3; ++i) {
1004  PN_stdfloat d = delta[i];
1005  if (!IS_NEARLY_ZERO(d)) {
1006  double tmin2 = (bmin[i] - from[i]) / d;
1007  double tmax2 = (bmax[i] - from[i]) / d;
1008  if (tmin2 > tmax2) {
1009  std::swap(tmin2, tmax2);
1010  }
1011  tmin = std::max(tmin, tmin2);
1012  tmax = std::min(tmax, tmax2);
1013 
1014  if (tmin > tmax) {
1015  return false;
1016  }
1017 
1018  } else if (from[i] < bmin[i] || from[i] > bmax[i]) {
1019  // The line is entirely parallel in this dimension.
1020  return false;
1021  }
1022  }
1023 
1024  t1 = tmin;
1025  t2 = tmax;
1026  return true;
1027 }
1028 
1029 /**
1030  * Clips the polygon by all of the clip planes named in the clip plane
1031  * attribute and fills new_points up with the resulting points.
1032  *
1033  * The return value is true if the set of points is unmodified (all points are
1034  * behind all the clip planes), or false otherwise.
1035  */
1038  const ClipPlaneAttrib *cpa,
1039  const TransformState *net_transform, int plane_no) const {
1040  bool all_in = true;
1041 
1042  int num_planes = cpa->get_num_on_planes();
1043  bool first_plane = true;
1044 
1045  for (int i = 0; i < num_planes; i++) {
1046  NodePath plane_path = cpa->get_on_plane(i);
1047  PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
1048  if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
1049  CPT(TransformState) new_transform =
1050  net_transform->invert_compose(plane_path.get_net_transform());
1051 
1052  LPlane plane = plane_node->get_plane() * new_transform->get_mat();
1053  if (first_plane) {
1054  first_plane = false;
1055  if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
1056  all_in = false;
1057  }
1058  } else {
1059  Points last_points;
1060  last_points.swap(new_points);
1061  if (!clip_polygon(new_points, last_points, plane, plane_no)) {
1062  all_in = false;
1063  }
1064  }
1065  }
1066  }
1067 
1068  if (!all_in) {
1069  compute_vectors(new_points);
1070  }
1071 
1072  return all_in;
1073 }
1074 /**
1075  * Clips the source_points of the polygon by the indicated clipping plane, and
1076  * modifies new_points to reflect the new set of clipped points (but does not
1077  * compute the vectors in new_points).
1078  *
1079  * The return value is true if the set of points is unmodified (all points are
1080  * behind the clip plane), or false otherwise.
1081  */
1084  const CollisionBox::Points &source_points,
1085  const LPlane &plane, int plane_no) const {
1086  new_points.clear();
1087  if (source_points.empty()) {
1088  return true;
1089  }
1090 
1091  LPoint3 from3d;
1092  LVector3 delta3d;
1093  if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
1094  // The clipping plane is parallel to the polygon. The polygon is either
1095  // all in or all out.
1096  if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
1097  // A point within the polygon is behind the clipping plane: the polygon
1098  // is all in.
1099  new_points = source_points;
1100  return true;
1101  }
1102  return false;
1103  }
1104 
1105  // Project the line of intersection into the 2-d plane. Now we have a 2-d
1106  // clipping line.
1107  LPoint2 from2d = to_2d(from3d,plane_no);
1108  LVector2 delta2d = to_2d(delta3d,plane_no);
1109 
1110  PN_stdfloat a = -delta2d[1];
1111  PN_stdfloat b = delta2d[0];
1112  PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
1113 
1114  // Now walk through the points. Any point on the left of our line gets
1115  // removed, and the line segment clipped at the point of intersection.
1116 
1117  // We might increase the number of vertices by as many as 1, if the plane
1118  // clips off exactly one corner. (We might also decrease the number of
1119  // vertices, or keep them the same number.)
1120  new_points.reserve(source_points.size() + 1);
1121 
1122  LPoint2 last_point = source_points.back()._p;
1123  bool last_is_in = !is_right(last_point - from2d, delta2d);
1124  bool all_in = last_is_in;
1125  Points::const_iterator pi;
1126  for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
1127  const LPoint2 &this_point = (*pi)._p;
1128  bool this_is_in = !is_right(this_point - from2d, delta2d);
1129 
1130  // There appears to be a compiler bug in gcc 4.0: we need to extract this
1131  // comparison outside of the if statement.
1132  bool crossed_over = (this_is_in != last_is_in);
1133  if (crossed_over) {
1134  // We have just crossed over the clipping line. Find the point of
1135  // intersection.
1136  LVector2 d = this_point - last_point;
1137  PN_stdfloat denom = (a * d[0] + b * d[1]);
1138  if (denom != 0.0) {
1139  PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
1140  LPoint2 p = last_point + t * d;
1141 
1142  new_points.push_back(PointDef(p[0], p[1]));
1143  last_is_in = this_is_in;
1144  }
1145  }
1146 
1147  if (this_is_in) {
1148  // We are behind the clipping line. Keep the point.
1149  new_points.push_back(PointDef(this_point[0], this_point[1]));
1150  } else {
1151  all_in = false;
1152  }
1153 
1154  last_point = this_point;
1155  }
1156 
1157  return all_in;
1158 }
1159 
1160 
1161 /**
1162  * Returns the linear distance from the 2-d point to the nearest part of the
1163  * polygon defined by the points vector. The result is negative if the point
1164  * is within the polygon.
1165  */
1166 PN_stdfloat CollisionBox::
1167 dist_to_polygon(const LPoint2 &p, const CollisionBox::Points &points) const {
1168 
1169  // We know that that the polygon is convex and is defined with the points in
1170  // counterclockwise order. Therefore, we simply compare the signed distance
1171  // to each line segment; we ignore any negative values, and take the minimum
1172  // of all the positive values.
1173 
1174  // If all values are negative, the point is within the polygon; we therefore
1175  // return an arbitrary negative result.
1176 
1177  bool got_dist = false;
1178  PN_stdfloat best_dist = -1.0f;
1179 
1180  size_t num_points = points.size();
1181  for (size_t i = 0; i < num_points - 1; ++i) {
1182  PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
1183  points[i]._v);
1184  if (d >= 0.0f) {
1185  if (!got_dist || d < best_dist) {
1186  best_dist = d;
1187  got_dist = true;
1188  }
1189  }
1190  }
1191 
1192  PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
1193  points[num_points - 1]._v);
1194  if (d >= 0.0f) {
1195  if (!got_dist || d < best_dist) {
1196  best_dist = d;
1197  got_dist = true;
1198  }
1199  }
1200 
1201  return best_dist;
1202 }
1203 
1204 /**
1205  * Returns the linear distance of p to the line segment defined by f and t,
1206  * where v = (t - f).normalize(). The result is negative if p is left of the
1207  * line, positive if it is right of the line. If the result is positive, it
1208  * is constrained by endpoints of the line segment (i.e. the result might be
1209  * larger than it would be for a straight distance-to-line test). If the
1210  * result is negative, we don't bother.
1211  */
1212 PN_stdfloat CollisionBox::
1213 dist_to_line_segment(const LPoint2 &p,
1214  const LPoint2 &f, const LPoint2 &t,
1215  const LVector2 &v) {
1216  LVector2 v1 = (p - f);
1217  PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
1218  if (d < 0.0f) {
1219  return d;
1220  }
1221 
1222  // Compute the nearest point on the line.
1223  LPoint2 q = p + LVector2(-v[1], v[0]) * d;
1224 
1225  // Now constrain that point to the line segment.
1226  if (v[0] > 0.0f) {
1227  // X+
1228  if (v[1] > 0.0f) {
1229  // Y+
1230  if (v[0] > v[1]) {
1231  // X-dominant.
1232  if (q[0] < f[0]) {
1233  return (p - f).length();
1234  } if (q[0] > t[0]) {
1235  return (p - t).length();
1236  } else {
1237  return d;
1238  }
1239  } else {
1240  // Y-dominant.
1241  if (q[1] < f[1]) {
1242  return (p - f).length();
1243  } if (q[1] > t[1]) {
1244  return (p - t).length();
1245  } else {
1246  return d;
1247  }
1248  }
1249  } else {
1250  // Y-
1251  if (v[0] > -v[1]) {
1252  // X-dominant.
1253  if (q[0] < f[0]) {
1254  return (p - f).length();
1255  } if (q[0] > t[0]) {
1256  return (p - t).length();
1257  } else {
1258  return d;
1259  }
1260  } else {
1261  // Y-dominant.
1262  if (q[1] > f[1]) {
1263  return (p - f).length();
1264  } if (q[1] < t[1]) {
1265  return (p - t).length();
1266  } else {
1267  return d;
1268  }
1269  }
1270  }
1271  } else {
1272  // X-
1273  if (v[1] > 0.0f) {
1274  // Y+
1275  if (-v[0] > v[1]) {
1276  // X-dominant.
1277  if (q[0] > f[0]) {
1278  return (p - f).length();
1279  } if (q[0] < t[0]) {
1280  return (p - t).length();
1281  } else {
1282  return d;
1283  }
1284  } else {
1285  // Y-dominant.
1286  if (q[1] < f[1]) {
1287  return (p - f).length();
1288  } if (q[1] > t[1]) {
1289  return (p - t).length();
1290  } else {
1291  return d;
1292  }
1293  }
1294  } else {
1295  // Y-
1296  if (-v[0] > -v[1]) {
1297  // X-dominant.
1298  if (q[0] > f[0]) {
1299  return (p - f).length();
1300  } if (q[0] < t[0]) {
1301  return (p - t).length();
1302  } else {
1303  return d;
1304  }
1305  } else {
1306  // Y-dominant.
1307  if (q[1] > f[1]) {
1308  return (p - f).length();
1309  } if (q[1] < t[1]) {
1310  return (p - t).length();
1311  } else {
1312  return d;
1313  }
1314  }
1315  }
1316  }
1317 }
1318 
1319 /**
1320  * Returns true if the indicated point is within the polygon's 2-d space,
1321  * false otherwise.
1322  */
1324 point_is_inside(const LPoint2 &p, const CollisionBox::Points &points) const {
1325  // We insist that the polygon be convex. This makes things a bit simpler.
1326  // In the case of a convex polygon, defined with points in counterclockwise
1327  // order, a point is interior to the polygon iff the point is not right of
1328  // each of the edges.
1329  for (int i = 0; i < (int)points.size() - 1; i++) {
1330  if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1331  return false;
1332  }
1333  }
1334  if (is_right(p - points[points.size() - 1]._p,
1335  points[0]._p - points[points.size() - 1]._p)) {
1336  return false;
1337  }
1338 
1339  return true;
1340 }
1341 
1342 /**
1343  * Now that the _p members of the given points array have been computed, go
1344  * back and compute all of the _v members.
1345  */
1347 compute_vectors(Points &points) {
1348  size_t num_points = points.size();
1349  for (size_t i = 0; i < num_points; i++) {
1350  points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1351  points[i]._v.normalize();
1352  }
1353 }
1354 
1355 /**
1356  * Factory method to generate a CollisionBox object
1357  */
1360  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
1361 }
1362 
1363 /**
1364  * Function to write the important information in the particular object to a
1365  * Datagram
1366  */
1368 write_datagram(BamWriter *manager, Datagram &me) {
1369  CollisionSolid::write_datagram(manager, me);
1370  _center.write_datagram(me);
1371  _min.write_datagram(me);
1372  _max.write_datagram(me);
1373  for(int i=0; i < 8; i++) {
1374  _vertex[i].write_datagram(me);
1375  }
1376  me.add_stdfloat(_radius);
1377  me.add_stdfloat(_x);
1378  me.add_stdfloat(_y);
1379  me.add_stdfloat(_z);
1380  for(int i=0; i < 6; i++) {
1381  _planes[i].write_datagram(me);
1382  }
1383  for(int i=0; i < 6; i++) {
1384  _to_2d_mat[i].write_datagram(me);
1385  }
1386  for(int i=0; i < 6; i++) {
1387  me.add_uint16(_points[i].size());
1388  for (size_t j = 0; j < _points[i].size(); j++) {
1389  _points[i][j]._p.write_datagram(me);
1390  _points[i][j]._v.write_datagram(me);
1391  }
1392  }
1393 }
1394 
1395 /**
1396  * Factory method to generate a CollisionBox object
1397  */
1398 TypedWritable *CollisionBox::
1399 make_CollisionBox(const FactoryParams &params) {
1400  CollisionBox *me = new CollisionBox;
1401  DatagramIterator scan;
1402  BamReader *manager;
1403 
1404  parse_params(params, scan, manager);
1405  me->fillin(scan, manager);
1406  return me;
1407 }
1408 
1409 /**
1410  * Function that reads out of the datagram (or asks manager to read) all of
1411  * the data that is needed to re-create this object and stores it in the
1412  * appropiate place
1413  */
1414 void CollisionBox::
1415 fillin(DatagramIterator& scan, BamReader* manager) {
1416  CollisionSolid::fillin(scan, manager);
1417  _center.read_datagram(scan);
1418  _min.read_datagram(scan);
1419  _max.read_datagram(scan);
1420  for(int i=0; i < 8; i++) {
1421  _vertex[i].read_datagram(scan);
1422  }
1423  _radius = scan.get_stdfloat();
1424  _x = scan.get_stdfloat();
1425  _y = scan.get_stdfloat();
1426  _z = scan.get_stdfloat();
1427  for(int i=0; i < 6; i++) {
1428  _planes[i].read_datagram(scan);
1429  }
1430  for(int i=0; i < 6; i++) {
1431  _to_2d_mat[i].read_datagram(scan);
1432  }
1433  for(int i=0; i < 6; i++) {
1434  size_t size = scan.get_uint16();
1435  for (size_t j = 0; j < size; j++) {
1436  LPoint2 p;
1437  LVector2 v;
1438  p.read_datagram(scan);
1439  v.read_datagram(scan);
1440  _points[i].push_back(PointDef(p, v));
1441  }
1442  }
1443 }
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
void parse_params(const FactoryParams &params, DatagramIterator &scan, BamReader *&manager)
Takes in a FactoryParams, passed from a WritableFactory into any TypedWritable's make function,...
Definition: bamReader.I:275
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:110
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition: bamReader.I:177
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
Definition: bamWriter.h:63
This defines a bounding sphere, consisting of a center and a radius.
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
This functions similarly to a LightAttrib.
get_on_plane
Returns the nth plane enabled by the attribute, sorted in render order.
get_num_on_planes
Returns the number of planes that are enabled by the attribute.
A cuboid collision volume or object.
Definition: collisionBox.h:27
static void register_with_read_factory()
Factory method to generate a CollisionBox object.
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...
void setup_points(const LPoint3 *begin, const LPoint3 *end, int plane)
Computes the plane and 2d projection of points that make up this side.
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...
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 void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
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:233
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 ...
void setup_box()
Compute parameters for each of the box's sides.
LPlane get_plane(int n) const
Returns the nth face of the rectangular solid.
Definition: collisionBox.I:181
bool point_is_inside(const LPoint2 &p, const Points &points) const
Returns true if the indicated point is within the polygon's 2-d space, false otherwise.
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
LPoint2 to_2d(const LVecBase3 &point3d, int plane) const
Assuming the indicated point in 3-d space lies within the polygon's plane, returns the corresponding ...
Definition: collisionBox.I:223
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
Definition: collisionBox.I:150
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
LPlane set_plane(int n) const
Creates the nth face of the rectangular solid.
Definition: collisionBox.I:190
This implements a solid consisting of a cylinder with hemispherical endcaps, also known as a capsule ...
Defines a single collision event.
get_into
Returns the CollisionSolid pointer for the particular solid was collided into.
get_from_node_path
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
get_into_node_path
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
get_from
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
An infinite line, similar to a CollisionRay, except that it extends in both directions.
Definition: collisionLine.h:25
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:27
A finite line segment, with two specific endpoints but no thickness.
The abstract base class for all things that can collide with other things in the world,...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
get_respect_effective_normal
See set_respect_effective_normal().
A spherical collision volume or object.
A class to retrieve the individual data elements previously stored in a Datagram.
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
uint16_t get_uint16()
Extracts an unsigned 16-bit integer.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition: datagram.h:38
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
Definition: datagram.I:133
void add_uint16(uint16_t value)
Adds an unsigned 16-bit integer to the datagram.
Definition: datagram.I:85
An instance of this class is passed to the Factory when requesting it to do its business and construc...
Definition: factoryParams.h:36
void register_factory(TypeHandle handle, CreateFunc *func, void *user_data=nullptr)
Registers a new kind of thing the Factory will be able to create.
Definition: factory.I:73
Defines a series of disconnected line segments.
Definition: geomLines.h:23
Defines a series of disconnected triangles.
Definition: geomTriangles.h:23
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
static const GeomVertexFormat * get_v3()
Returns a standard vertex format with just a 3-component vertex position.
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
A container for geometry primitives.
Definition: geom.h:54
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:159
PandaNode * node() const
Returns the referenced node of the path.
Definition: nodePath.I:227
A lightweight class that represents a single element that may be timed and/or counted via stats.
A node that contains a plane.
Definition: planeNode.h:36
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
Definition: planeNode.I:54
int get_clip_effect() const
Returns the clip_effect bits for this clip plane.
Definition: planeNode.I:128
Indicates a coordinate-system transform on vertices.
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:81
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:35
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PT(CollisionEntry) CollisionBox
First Dispatch point for box as a FROM object.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.