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