Panda3D
collisionTube.cxx
1 // Filename: collisionTube.cxx
2 // Created by: drose (25Sep03)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "collisionTube.h"
16 #include "collisionSphere.h"
17 #include "collisionLine.h"
18 #include "collisionRay.h"
19 #include "collisionSegment.h"
20 #include "collisionHandler.h"
21 #include "collisionEntry.h"
22 #include "collisionParabola.h"
23 #include "config_collide.h"
24 #include "look_at.h"
25 #include "geom.h"
26 #include "geomNode.h"
27 #include "geometricBoundingVolume.h"
28 #include "datagram.h"
29 #include "datagramIterator.h"
30 #include "bamReader.h"
31 #include "bamWriter.h"
32 #include "cmath.h"
33 #include "transformState.h"
34 #include "geom.h"
35 #include "geomTristrips.h"
36 #include "geomVertexWriter.h"
37 #include "boundingSphere.h"
38 
39 PStatCollector CollisionTube::_volume_pcollector("Collision Volumes:CollisionTube");
40 PStatCollector CollisionTube::_test_pcollector("Collision Tests:CollisionTube");
41 TypeHandle CollisionTube::_type_handle;
42 
43 ////////////////////////////////////////////////////////////////////
44 // Function: CollisionTube::make_copy
45 // Access: Public, Virtual
46 // Description:
47 ////////////////////////////////////////////////////////////////////
48 CollisionSolid *CollisionTube::
49 make_copy() {
50  return new CollisionTube(*this);
51 }
52 
53 ////////////////////////////////////////////////////////////////////
54 // Function: CollisionTube::xform
55 // Access: Public, Virtual
56 // Description: Transforms the solid by the indicated matrix.
57 ////////////////////////////////////////////////////////////////////
58 void CollisionTube::
59 xform(const LMatrix4 &mat) {
60  _a = _a * mat;
61  _b = _b * mat;
62 
63  // This is a little cheesy and fails miserably in the presence of a
64  // non-uniform scale.
65  LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
66  _radius = length(radius_v);
67 
68  recalc_internals();
69  CollisionSolid::xform(mat);
70 }
71 
72 ////////////////////////////////////////////////////////////////////
73 // Function: CollisionTube::get_collision_origin
74 // Access: Public, Virtual
75 // Description: Returns the point in space deemed to be the "origin"
76 // of the solid for collision purposes. The closest
77 // intersection point to this origin point is considered
78 // to be the most significant.
79 ////////////////////////////////////////////////////////////////////
82  return get_point_a();
83 }
84 
85 ////////////////////////////////////////////////////////////////////
86 // Function: CollisionTube::get_volume_pcollector
87 // Access: Public, Virtual
88 // Description: Returns a PStatCollector that is used to count the
89 // number of bounding volume tests made against a solid
90 // of this type in a given frame.
91 ////////////////////////////////////////////////////////////////////
94  return _volume_pcollector;
95 }
96 
97 ////////////////////////////////////////////////////////////////////
98 // Function: CollisionTube::get_test_pcollector
99 // Access: Public, Virtual
100 // Description: Returns a PStatCollector that is used to count the
101 // number of intersection tests made against a solid
102 // of this type in a given frame.
103 ////////////////////////////////////////////////////////////////////
106  return _test_pcollector;
107 }
108 
109 ////////////////////////////////////////////////////////////////////
110 // Function: CollisionTube::output
111 // Access: Public, Virtual
112 // Description:
113 ////////////////////////////////////////////////////////////////////
114 void CollisionTube::
115 output(ostream &out) const {
116  out << "tube, a (" << _a << "), b (" << _b << "), r " << _radius;
117 }
118 
119 ////////////////////////////////////////////////////////////////////
120 // Function: CollisionTube::compute_internal_bounds
121 // Access: Protected, Virtual
122 // Description:
123 ////////////////////////////////////////////////////////////////////
124 PT(BoundingVolume) CollisionTube::
125 compute_internal_bounds() const {
126  PT(BoundingVolume) bound = CollisionSolid::compute_internal_bounds();
127 
128  if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
129  GeometricBoundingVolume *gbound;
130  DCAST_INTO_R(gbound, bound, bound);
131 
132  LVector3 vec = (_b - _a);
133  if (vec.normalize()) {
134  // The bounding volume includes both endpoints, plus a little
135  // bit more to include the radius in both directions.
136  LPoint3 points[2];
137  points[0] = _a - vec * _radius;
138  points[1] = _b + vec * _radius;
139 
140  gbound->around(points, points + 2);
141 
142  } else {
143  // Both endpoints are coincident; therefore, the bounding volume
144  // is a sphere.
145  BoundingSphere sphere(_a, _radius);
146  gbound->extend_by(&sphere);
147  }
148  }
149 
150  return bound;
151 }
152 
153 ////////////////////////////////////////////////////////////////////
154 // Function: CollisionTube::test_intersection_from_sphere
155 // Access: Public, Virtual
156 // Description:
157 ////////////////////////////////////////////////////////////////////
158 PT(CollisionEntry) CollisionTube::
159 test_intersection_from_sphere(const CollisionEntry &entry) const {
160  const CollisionSphere *sphere;
161  DCAST_INTO_R(sphere, entry.get_from(), NULL);
162 
163  CPT(TransformState) wrt_space = entry.get_wrt_space();
164  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
165 
166  const LMatrix4 &wrt_mat = wrt_space->get_mat();
167 
168  LPoint3 from_a = sphere->get_center() * wrt_mat;
169  LPoint3 from_b = from_a;
170 
171  LPoint3 contact_point;
172  PN_stdfloat actual_t = 0.0f;
173 
174  if (wrt_prev_space != wrt_space) {
175  // If the sphere is moving relative to the tube, it becomes a tube
176  // itself.
177  from_a = sphere->get_center() * wrt_prev_space->get_mat();
178  }
179 
180  LVector3 from_direction = from_b - from_a;
181 
182  LVector3 from_radius_v =
183  LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
184  PN_stdfloat from_radius = length(from_radius_v);
185 
186  double t1, t2;
187  if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
188  // No intersection.
189  return NULL;
190  }
191 
192  if (t2 < 0.0 || t1 > 1.0) {
193  // Both intersection points are before the start of the segment or
194  // after the end of the segment.
195  return NULL;
196  }
197 
198  // doubles, not floats, to satisfy min and max templates.
199  actual_t = min(1.0, max(0.0, t1));
200  contact_point = from_a + actual_t * (from_b - from_a);
201 
202  if (collide_cat.is_debug()) {
203  collide_cat.debug()
204  << "intersection detected from " << entry.get_from_node_path() << " into "
205  << entry.get_into_node_path() << "\n";
206  }
207  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
208 
209  LPoint3 into_intersection_point;
210  if (t2 > 1.0) {
211  // Point b is within the tube. The first intersection point is
212  // point b itself.
213  into_intersection_point = from_b;
214  } else {
215  // Point b is outside the tube, and point a is either inside the
216  // tube or beyond it. The first intersection point is at t2.
217  into_intersection_point = from_a + t2 * from_direction;
218  }
219  set_intersection_point(new_entry, into_intersection_point, from_radius);
220 
221  LPoint3 fake_contact_point;
222  LVector3 contact_normal;
223  calculate_surface_point_and_normal(contact_point,
224  from_radius,
225  fake_contact_point,
226  contact_normal);
227  new_entry->set_contact_pos(contact_point);
228  new_entry->set_contact_normal(contact_normal);
229  new_entry->set_t(actual_t);
230 
231  return new_entry;
232 }
233 
234 ////////////////////////////////////////////////////////////////////
235 // Function: CollisionTube::test_intersection_from_line
236 // Access: Public, Virtual
237 // Description:
238 ////////////////////////////////////////////////////////////////////
239 PT(CollisionEntry) CollisionTube::
240 test_intersection_from_line(const CollisionEntry &entry) const {
241  const CollisionLine *line;
242  DCAST_INTO_R(line, entry.get_from(), NULL);
243 
244  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
245 
246  LPoint3 from_origin = line->get_origin() * wrt_mat;
247  LVector3 from_direction = line->get_direction() * wrt_mat;
248 
249  double t1, t2;
250  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
251  // No intersection.
252  return NULL;
253  }
254 
255  if (collide_cat.is_debug()) {
256  collide_cat.debug()
257  << "intersection detected from " << entry.get_from_node_path()
258  << " into " << entry.get_into_node_path() << "\n";
259  }
260  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
261 
262  LPoint3 into_intersection_point = from_origin + t1 * from_direction;
263  set_intersection_point(new_entry, into_intersection_point, 0.0);
264 
267 
268  } else {
269  LVector3 normal = into_intersection_point * _inv_mat;
270  if (normal[1] > _length) {
271  // The point is within the top endcap.
272  normal[1] -= _length;
273  } else if (normal[1] > 0.0f) {
274  // The point is within the cylinder body.
275  normal[1] = 0;
276  }
277  normal = normalize(normal * _mat);
278  new_entry->set_surface_normal(normal);
279  }
280 
281  return new_entry;
282 }
283 
284 ////////////////////////////////////////////////////////////////////
285 // Function: CollisionTube::test_intersection_from_ray
286 // Access: Public, Virtual
287 // Description:
288 ////////////////////////////////////////////////////////////////////
289 PT(CollisionEntry) CollisionTube::
290 test_intersection_from_ray(const CollisionEntry &entry) const {
291  const CollisionRay *ray;
292  DCAST_INTO_R(ray, entry.get_from(), NULL);
293 
294  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
295 
296  LPoint3 from_origin = ray->get_origin() * wrt_mat;
297  LVector3 from_direction = ray->get_direction() * wrt_mat;
298 
299  double t1, t2;
300  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
301  // No intersection.
302  return NULL;
303  }
304 
305  if (t2 < 0.0) {
306  // Both intersection points are before the start of the ray.
307  return NULL;
308  }
309 
310  if (collide_cat.is_debug()) {
311  collide_cat.debug()
312  << "intersection detected from " << entry.get_from_node_path()
313  << " into " << entry.get_into_node_path() << "\n";
314  }
315  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
316 
317  LPoint3 into_intersection_point;
318  if (t1 < 0.0) {
319  // Point a is within the tube. The first intersection point is
320  // point a itself.
321  into_intersection_point = from_origin;
322  } else {
323  // Point a is outside the tube. The first intersection point is
324  // at t1.
325  into_intersection_point = from_origin + t1 * from_direction;
326  }
327  set_intersection_point(new_entry, into_intersection_point, 0.0);
328 
331 
332  } else {
333  LVector3 normal = into_intersection_point * _inv_mat;
334  if (normal[1] > _length) {
335  // The point is within the top endcap.
336  normal[1] -= _length;
337  } else if (normal[1] > 0.0f) {
338  // The point is within the cylinder body.
339  normal[1] = 0;
340  }
341  normal = normalize(normal * _mat);
342  new_entry->set_surface_normal(normal);
343  }
344 
345  return new_entry;
346 }
347 
348 ////////////////////////////////////////////////////////////////////
349 // Function: CollisionTube::test_intersection_from_segment
350 // Access: Public, Virtual
351 // Description:
352 ////////////////////////////////////////////////////////////////////
353 PT(CollisionEntry) CollisionTube::
354 test_intersection_from_segment(const CollisionEntry &entry) const {
355  const CollisionSegment *segment;
356  DCAST_INTO_R(segment, entry.get_from(), NULL);
357 
358  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
359 
360  LPoint3 from_a = segment->get_point_a() * wrt_mat;
361  LPoint3 from_b = segment->get_point_b() * wrt_mat;
362  LVector3 from_direction = from_b - from_a;
363 
364  double t1, t2;
365  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
366  // No intersection.
367  return NULL;
368  }
369 
370  if (t2 < 0.0 || t1 > 1.0) {
371  // Both intersection points are before the start of the segment or
372  // after the end of the segment.
373  return NULL;
374  }
375 
376  if (collide_cat.is_debug()) {
377  collide_cat.debug()
378  << "intersection detected from " << entry.get_from_node_path()
379  << " into " << entry.get_into_node_path() << "\n";
380  }
381  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
382 
383  LPoint3 into_intersection_point;
384  if (t1 < 0.0) {
385  // Point a is within the tube. The first intersection point is
386  // point a itself.
387  into_intersection_point = from_a;
388  } else {
389  // Point a is outside the tube, and point b is either inside the
390  // tube or beyond it. The first intersection point is at t1.
391  into_intersection_point = from_a + t1 * from_direction;
392  }
393  set_intersection_point(new_entry, into_intersection_point, 0.0);
394 
397 
398  } else {
399  LVector3 normal = into_intersection_point * _inv_mat;
400  if (normal[1] > _length) {
401  // The point is within the top endcap.
402  normal[1] -= _length;
403  } else if (normal[1] > 0.0f) {
404  // The point is within the cylinder body.
405  normal[1] = 0;
406  }
407  normal = normalize(normal * _mat);
408  new_entry->set_surface_normal(normal);
409  }
410 
411  return new_entry;
412 }
413 
414 ////////////////////////////////////////////////////////////////////
415 // Function: CollisionTube::test_intersection_from_parabola
416 // Access: Public, Virtual
417 // Description:
418 ////////////////////////////////////////////////////////////////////
419 PT(CollisionEntry) CollisionTube::
420 test_intersection_from_parabola(const CollisionEntry &entry) const {
421  const CollisionParabola *parabola;
422  DCAST_INTO_R(parabola, entry.get_from(), NULL);
423 
424  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
425 
426  // Convert the parabola into local coordinate space.
427  LParabola local_p(parabola->get_parabola());
428  local_p.xform(wrt_mat);
429 
430  double t;
431  if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
432  local_p.calc_point(parabola->get_t1()),
433  local_p.calc_point(parabola->get_t2()))) {
434  // No intersection.
435  return NULL;
436  }
437 
438  if (collide_cat.is_debug()) {
439  collide_cat.debug()
440  << "intersection detected from " << entry.get_from_node_path()
441  << " into " << entry.get_into_node_path() << "\n";
442  }
443  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
444 
445  LPoint3 into_intersection_point = local_p.calc_point(t);
446  set_intersection_point(new_entry, into_intersection_point, 0.0);
447 
448  if (has_effective_normal() && parabola->get_respect_effective_normal()) {
450 
451  } else {
452  LVector3 normal = into_intersection_point * _inv_mat;
453  if (normal[1] > _length) {
454  // The point is within the top endcap.
455  normal[1] -= _length;
456  } else if (normal[1] > 0.0f) {
457  // The point is within the cylinder body.
458  normal[1] = 0;
459  }
460  normal = normalize(normal * _mat);
461  new_entry->set_surface_normal(normal);
462  }
463 
464  return new_entry;
465 }
466 
467 ////////////////////////////////////////////////////////////////////
468 // Function: CollisionTube::fill_viz_geom
469 // Access: Protected, Virtual
470 // Description: Fills the _viz_geom GeomNode up with Geoms suitable
471 // for rendering this solid.
472 ////////////////////////////////////////////////////////////////////
473 void CollisionTube::
474 fill_viz_geom() {
475  if (collide_cat.is_debug()) {
476  collide_cat.debug()
477  << "Recomputing viz for " << *this << "\n";
478  }
479 
480  // Generate the vertices such that we draw a tube with one endpoint
481  // at (0, 0, 0), and another at (0, length, 0). Then we'll rotate
482  // and translate it into place with the appropriate look_at matrix.
483  LVector3 direction = (_b - _a);
484  PN_stdfloat length = direction.length();
485 
486  PT(GeomVertexData) vdata = new GeomVertexData
487  ("collision", GeomVertexFormat::get_v3(),
488  Geom::UH_static);
489  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
490 
491  PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
492  // Generate the first endcap.
493  static const int num_slices = 8;
494  static const int num_rings = 4;
495  int ri, si;
496  for (ri = 0; ri < num_rings; ri++) {
497  for (si = 0; si <= num_slices; si++) {
498  vertex.add_data3(calc_sphere1_vertex(ri, si, num_rings, num_slices));
499  vertex.add_data3(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
500  }
501  strip->add_next_vertices((num_slices + 1) * 2);
502  strip->close_primitive();
503  }
504 
505  // Now the cylinder sides.
506  for (si = 0; si <= num_slices; si++) {
507  vertex.add_data3(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
508  vertex.add_data3(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
509  length));
510  }
511  strip->add_next_vertices((num_slices + 1) * 2);
512  strip->close_primitive();
513 
514  // And the second endcap.
515  for (ri = num_rings - 1; ri >= 0; ri--) {
516  for (si = 0; si <= num_slices; si++) {
517  vertex.add_data3(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
518  vertex.add_data3(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
519  }
520  strip->add_next_vertices((num_slices + 1) * 2);
521  strip->close_primitive();
522  }
523 
524  PT(Geom) geom = new Geom(vdata);
525  geom->add_primitive(strip);
526 
527  // Now transform the vertices to their actual location.
528  LMatrix4 mat;
529  look_at(mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
530  mat.set_row(3, _a);
531  geom->transform_vertices(mat);
532 
533  _viz_geom->add_geom(geom, get_solid_viz_state());
534  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
535 }
536 
537 ////////////////////////////////////////////////////////////////////
538 // Function: CollisionTube::recalc_internals
539 // Access: Private
540 // Description: Should be called internally to recompute the matrix
541 // and length when the properties of the tube have
542 // changed.
543 ////////////////////////////////////////////////////////////////////
544 void CollisionTube::
545 recalc_internals() {
546  LVector3 direction = (_b - _a);
547  _length = direction.length();
548 
549  look_at(_mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
550  _mat.set_row(3, _a);
551  _inv_mat.invert_from(_mat);
552 
553  mark_viz_stale();
554  mark_internal_bounds_stale();
555 }
556 
557 ////////////////////////////////////////////////////////////////////
558 // Function: CollisionTube::calc_sphere1_vertex
559 // Access: Private
560 // Description: Calculates a particular vertex on the surface of the
561 // first endcap hemisphere, for use in generating the
562 // viz geometry.
563 ////////////////////////////////////////////////////////////////////
564 LVertex CollisionTube::
565 calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) {
566  PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
567  PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
568 
569  // Find the point on the rim, based on the slice.
570  PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
571  PN_stdfloat x_rim = ccos(theta);
572  PN_stdfloat z_rim = csin(theta);
573 
574  // Now pull that point in towards the pole, based on the ring.
575  PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
576  PN_stdfloat to_pole = csin(phi);
577 
578  PN_stdfloat x = _radius * x_rim * to_pole;
579  PN_stdfloat y = -_radius * ccos(phi);
580  PN_stdfloat z = _radius * z_rim * to_pole;
581 
582  return LVertex(x, y, z);
583 }
584 
585 ////////////////////////////////////////////////////////////////////
586 // Function: CollisionTube::calc_sphere2_vertex
587 // Access: Private
588 // Description: Calculates a particular vertex on the surface of the
589 // second endcap hemisphere, for use in generating the
590 // viz geometry.
591 ////////////////////////////////////////////////////////////////////
592 LVertex CollisionTube::
593 calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
594  PN_stdfloat length) {
595  PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
596  PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
597 
598  // Find the point on the rim, based on the slice.
599  PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
600  PN_stdfloat x_rim = ccos(theta);
601  PN_stdfloat z_rim = csin(theta);
602 
603  // Now pull that point in towards the pole, based on the ring.
604  PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
605  PN_stdfloat to_pole = csin(phi);
606 
607  PN_stdfloat x = _radius * x_rim * to_pole;
608  PN_stdfloat y = length + _radius * ccos(phi);
609  PN_stdfloat z = _radius * z_rim * to_pole;
610 
611  return LVertex(x, y, z);
612 }
613 
614 ////////////////////////////////////////////////////////////////////
615 // Function: CollisionTube::intersects_line
616 // Access: Private
617 // Description: Determine the point(s) of intersection of a parametric
618 // line with the tube. The line is infinite in both
619 // directions, and passes through "from" and from+delta.
620 // If the line does not intersect the tube, the
621 // function returns false, and t1 and t2 are undefined.
622 // If it does intersect the tube, it returns true, and
623 // t1 and t2 are set to the points along the equation
624 // from+t*delta that correspond to the two points of
625 // intersection.
626 ////////////////////////////////////////////////////////////////////
627 bool CollisionTube::
628 intersects_line(double &t1, double &t2,
629  const LPoint3 &from0, const LVector3 &delta0,
630  PN_stdfloat inflate_radius) const {
631  // Convert the line into our canonical coordinate space: the tube is
632  // aligned with the y axis.
633  LPoint3 from = from0 * _inv_mat;
634  LVector3 delta = delta0 * _inv_mat;
635 
636  PN_stdfloat radius = _radius + inflate_radius;
637 
638  // Now project the line into the X-Z plane to test for intersection
639  // with a 2-d circle around the origin. The equation for this is
640  // very similar to the formula for the intersection of a line with a
641  // sphere; see CollisionSphere::intersects_line() for the complete
642  // derivation. It's a little bit simpler because the circle is
643  // centered on the origin.
644  LVector2 from2(from[0], from[2]);
645  LVector2 delta2(delta[0], delta[2]);
646 
647  double A = dot(delta2, delta2);
648 
649  if (IS_NEARLY_ZERO(A)) {
650  // If the delta2 is 0, the line is perpendicular to the X-Z plane.
651  // The whole line intersects with the infinite cylinder if the
652  // point is within the circle.
653  if (from2.dot(from2) > radius * radius) {
654  // Nope, the 2-d point is outside the circle, so no
655  // intersection.
656  return false;
657  }
658 
659  if (IS_NEARLY_ZERO(delta[1])) {
660  // Actually, the whole delta vector is 0, so the line is just a
661  // point. In this case, (since we have already shown the point
662  // is within the infinite cylinder), we intersect if and only if
663  // the three-dimensional point is between the endcaps.
664  if (from[1] < -radius || from[1] > _length + radius) {
665  // Way out.
666  return false;
667  }
668  if (from[1] < 0.0f) {
669  // Possibly within the first endcap.
670  if (from.dot(from) > radius * radius) {
671  return false;
672  }
673  } else if (from[1] > _length) {
674  // Possibly within the second endcap.
675  from[1] -= _length;
676  if (from.dot(from) > radius * radius) {
677  return false;
678  }
679  }
680 
681  // The point is within the tube!
682  t1 = t2 = 0.0;
683  return true;
684  }
685 
686  // The 2-d point is within the circle, so compute our intersection
687  // points to include the entire vertical slice of the cylinder.
688  t1 = (-radius - from[1]) / delta[1];
689  t2 = (_length + radius - from[1]) / delta[1];
690 
691  } else {
692  // The line is not perpendicular to the X-Z plane, so its
693  // projection into the plane is 2-d line. Test that 2-d line for
694  // intersection with the circular projection of the cylinder.
695 
696  double B = 2.0f * dot(delta2, from2);
697  double fc_d2 = dot(from2, from2);
698  double C = fc_d2 - radius * radius;
699 
700  double radical = B*B - 4.0*A*C;
701 
702  if (IS_NEARLY_ZERO(radical)) {
703  // Tangent.
704  t1 = t2 = -B / (2.0*A);
705 
706  } else if (radical < 0.0) {
707  // No real roots: no intersection with the line.
708  return false;
709 
710  } else {
711  double reciprocal_2A = 1.0 / (2.0 * A);
712  double sqrt_radical = sqrtf(radical);
713  t1 = ( -B - sqrt_radical ) * reciprocal_2A;
714  t2 = ( -B + sqrt_radical ) * reciprocal_2A;
715  }
716  }
717 
718  // Now we need to verify that the intersection points fall within
719  // the length of the cylinder.
720  PN_stdfloat t1_y = from[1] + t1 * delta[1];
721  PN_stdfloat t2_y = from[1] + t2 * delta[1];
722 
723  if (t1_y < -radius && t2_y < -radius) {
724  // Both points are way off the bottom of the tube; no
725  // intersection.
726  return false;
727  } else if (t1_y > _length + radius && t2_y > _length + radius) {
728  // Both points are way off the top of the tube; no intersection.
729  return false;
730  }
731 
732  if (t1_y < 0.0f) {
733  // The starting point is off the bottom of the tube. Test the
734  // line against the first endcap.
735  double t1a, t2a;
736  if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
737  // If there's no intersection with the endcap, there can't be an
738  // intersection with the cylinder.
739  return false;
740  }
741  t1 = t1a;
742 
743  } else if (t1_y > _length) {
744  // The starting point is off the top of the tube. Test the
745  // line against the second endcap.
746  double t1b, t2b;
747  if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
748  // If there's no intersection with the endcap, there can't be an
749  // intersection with the cylinder.
750  return false;
751  }
752  t1 = t1b;
753  }
754 
755  if (t2_y < 0.0f) {
756  // The ending point is off the bottom of the tube. Test the
757  // line against the first endcap.
758  double t1a, t2a;
759  if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
760  // If there's no intersection with the endcap, there can't be an
761  // intersection with the cylinder.
762  return false;
763  }
764  t2 = t2a;
765 
766  } else if (t2_y > _length) {
767  // The ending point is off the top of the tube. Test the
768  // line against the second endcap.
769  double t1b, t2b;
770  if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
771  // If there's no intersection with the endcap, there can't be an
772  // intersection with the cylinder.
773  return false;
774  }
775  t2 = t2b;
776  }
777 
778  return true;
779 }
780 
781 ////////////////////////////////////////////////////////////////////
782 // Function: CollisionTube::sphere_intersects_line
783 // Access: Private
784 // Description: After confirming that the line intersects an infinite
785 // cylinder, test whether it intersects one or the other
786 // endcaps. The y parameter specifies the center of the
787 // sphere (and hence the particular endcap.
788 ////////////////////////////////////////////////////////////////////
789 bool CollisionTube::
790 sphere_intersects_line(double &t1, double &t2, PN_stdfloat center_y,
791  const LPoint3 &from, const LVector3 &delta,
792  PN_stdfloat inflate_radius) const {
793  // See CollisionSphere::intersects_line() for a derivation of the
794  // formula here.
795  PN_stdfloat radius = _radius + inflate_radius;
796 
797  double A = dot(delta, delta);
798 
799  nassertr(A != 0.0, false);
800 
801  LVector3 fc = from;
802  fc[1] -= center_y;
803  double B = 2.0f* dot(delta, fc);
804  double fc_d2 = dot(fc, fc);
805  double C = fc_d2 - radius * radius;
806 
807  double radical = B*B - 4.0*A*C;
808 
809  if (IS_NEARLY_ZERO(radical)) {
810  // Tangent.
811  t1 = t2 = -B / (2.0 * A);
812  return true;
813 
814  } else if (radical < 0.0) {
815  // No real roots: no intersection with the line.
816  return false;
817  }
818 
819  double reciprocal_2A = 1.0 / (2.0 * A);
820  double sqrt_radical = sqrtf(radical);
821  t1 = ( -B - sqrt_radical ) * reciprocal_2A;
822  t2 = ( -B + sqrt_radical ) * reciprocal_2A;
823 
824  return true;
825 }
826 
827 ////////////////////////////////////////////////////////////////////
828 // Function: CollisionTube::intersects_parabola
829 // Access: Protected
830 // Description: Determine a point of intersection of a parametric
831 // parabola with the tube.
832 //
833 // We only consider the segment of the parabola between
834 // t1 and t2, which has already been computed as
835 // corresponding to points p1 and p2. If there is an
836 // intersection, t is set to the parametric point of
837 // intersection, and true is returned; otherwise, false
838 // is returned.
839 ////////////////////////////////////////////////////////////////////
840 bool CollisionTube::
841 intersects_parabola(double &t, const LParabola &parabola,
842  double t1, double t2,
843  const LPoint3 &p1, const LPoint3 &p2) const {
844  // I don't even want to think about the math to do this calculation
845  // directly--it's even worse than sphere-parabola. So I'll use the
846  // recursive subdivision solution again, just like I did for
847  // sphere-parabola.
848 
849  // First, see if the line segment (p1 - p2) comes sufficiently close
850  // to the parabola. Do this by computing the parametric intervening
851  // point and comparing its distance from the linear intervening
852  // point.
853  double tmid = (t1 + t2) * 0.5;
854 
855  if (tmid != t1 && tmid != t2) {
856  LPoint3 pmid = parabola.calc_point(tmid);
857  LPoint3 pmid2 = (p1 + p2) * 0.5f;
858 
859  if ((pmid - pmid2).length_squared() > 0.001f) {
860  // Subdivide.
861  if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
862  return true;
863  }
864  return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
865  }
866  }
867 
868  // The line segment is sufficiently close; compare the segment itself.
869  double t1a, t2a;
870  if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
871  return false;
872  }
873 
874  if (t2a < 0.0 || t1a > 1.0) {
875  return false;
876  }
877 
878  t = max(t1a, 0.0);
879  return true;
880 }
881 
882 ////////////////////////////////////////////////////////////////////
883 // Function: CollisionTube::calculate_surface_point_and_normal
884 // Access: Private
885 // Description: Calculates a point that is exactly on the surface
886 // of the tube and its corresponding normal, given
887 // a point that is supposedly on the surface of the
888 // tube.
889 ////////////////////////////////////////////////////////////////////
890 void CollisionTube::
891 calculate_surface_point_and_normal(const LPoint3 &surface_point,
892  double extra_radius,
893  LPoint3 &result_point,
894  LVector3 &result_normal) const {
895  // Convert the point into our canonical space for analysis.
896  LPoint3 point = surface_point * _inv_mat;
897  LVector3 normal;
898 
899  if (point[1] <= 0.0) {
900  // The point is on the first endcap.
901  normal = point;
902  if (!normal.normalize()) {
903  normal.set(0.0, -1.0, 0.0);
904  }
905  point = normal * _radius;
906 
907  } else if (point[1] >= _length) {
908  // The point is on the second endcap.
909  normal.set(point[0], point[1] - _length, point[2]);
910  if (!normal.normalize()) {
911  normal.set(0.0, 1.0, 0.0);
912  }
913  point = normal * _radius;
914  point[1] += _length;
915 
916  } else {
917  // The point is within the cylinder part.
918  LVector2d normal2d(point[0], point[2]);
919  if (!normal2d.normalize()) {
920  normal2d.set(0.0, 1.0);
921  }
922  normal.set(normal2d[0], 0.0, normal2d[1]);
923  point.set(normal[0] * _radius, point[1], normal[2] * _radius);
924  }
925 
926  // Now convert the point and normal back into real space.
927  result_point = point * _mat;
928  result_normal = normal * _mat;
929 }
930 
931 ////////////////////////////////////////////////////////////////////
932 // Function: CollisionTube::set_intersection_point
933 // Access: Private
934 // Description: After an intersection has been detected, record the
935 // computed intersection point in the CollisionEntry,
936 // and also compute the relevant normal based on that
937 // point.
938 ////////////////////////////////////////////////////////////////////
939 void CollisionTube::
940 set_intersection_point(CollisionEntry *new_entry,
941  const LPoint3 &into_intersection_point,
942  double extra_radius) const {
943  LPoint3 point;
944  LVector3 normal;
945 
946  calculate_surface_point_and_normal(into_intersection_point,
947  extra_radius,
948  point,
949  normal);
950 
952  normal = get_effective_normal();
953  }
954 
955  new_entry->set_surface_normal(normal);
956  new_entry->set_surface_point(point);
957  // Also adjust the original point into the tube by the amount of
958  // extra_radius, which should put it on the surface of the tube if
959  // our collision was tangential.
960  new_entry->set_interior_point(into_intersection_point - normal * extra_radius);
961 }
962 
963 ////////////////////////////////////////////////////////////////////
964 // Function: CollisionTube::register_with_read_factory
965 // Access: Public, Static
966 // Description: Tells the BamReader how to create objects of type
967 // CollisionTube.
968 ////////////////////////////////////////////////////////////////////
969 void CollisionTube::
971  BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
972 }
973 
974 ////////////////////////////////////////////////////////////////////
975 // Function: CollisionTube::write_datagram
976 // Access: Public, Virtual
977 // Description: Writes the contents of this object to the datagram
978 // for shipping out to a Bam file.
979 ////////////////////////////////////////////////////////////////////
980 void CollisionTube::
982  CollisionSolid::write_datagram(manager, dg);
983  _a.write_datagram(dg);
984  _b.write_datagram(dg);
985  dg.add_stdfloat(_radius);
986 }
987 
988 ////////////////////////////////////////////////////////////////////
989 // Function: CollisionTube::make_from_bam
990 // Access: Protected, Static
991 // Description: This function is called by the BamReader's factory
992 // when a new object of type CollisionTube is encountered
993 // in the Bam file. It should create the CollisionTube
994 // and extract its information from the file.
995 ////////////////////////////////////////////////////////////////////
996 TypedWritable *CollisionTube::
997 make_from_bam(const FactoryParams &params) {
998  CollisionTube *node = new CollisionTube();
999  DatagramIterator scan;
1000  BamReader *manager;
1001 
1002  parse_params(params, scan, manager);
1003  node->fillin(scan, manager);
1004 
1005  return node;
1006 }
1007 
1008 ////////////////////////////////////////////////////////////////////
1009 // Function: CollisionTube::fillin
1010 // Access: Protected
1011 // Description: This internal function is called by make_from_bam to
1012 // read in all of the relevant data from the BamFile for
1013 // the new CollisionTube.
1014 ////////////////////////////////////////////////////////////////////
1015 void CollisionTube::
1016 fillin(DatagramIterator &scan, BamReader *manager) {
1017  CollisionSolid::fillin(scan, manager);
1018  _a.read_datagram(scan);
1019  _b.read_datagram(scan);
1020  _radius = scan.get_stdfloat();
1021  recalc_internals();
1022 }
const LParabola & get_parabola() const
Returns the parabola specified by this solid.
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:31
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
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_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
NodePath get_into_node_path() const
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:122
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
This is a two-component vector offset.
Definition: lvector2.h:429
PN_stdfloat get_t2() const
Returns the ending point on the parabola.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
Definition: lvecBase3.h:1389
The abstract base class for all things that can collide with other things in the world, and all the things they can collide with (except geometry).
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
This defines a bounding sphere, consisting of a center and a radius.
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:37
void set_contact_normal(const LVector3 &normal)
Stores the surface normal of the "into" object at the contact pos.
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
Defines a series of triangle strips.
Definition: geomTristrips.h:25
void set_t(PN_stdfloat t)
Sets a time value for this collision relative to other CollisionEntries.
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
Definition: bamWriter.h:73
A spherical collision volume or object.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
void set_interior_point(const LPoint3 &point)
Stores the point, within the interior of the "into" object, which represents the depth to which the "...
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
Definition: datagram.I:240
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
static void register_with_read_factory()
Tells the BamReader how to create objects of type CollisionTube.
A lightweight class that represents a single element that may be timed and/or counted via stats...
This is another abstract class, for a general class of bounding volumes that actually enclose points ...
A finite line segment, with two specific endpoints but no thickness.
virtual void write_datagram(BamWriter *manager, Datagram &dg)
Writes the contents of this object to the datagram for shipping out to a Bam file.
This is a 4-by-4 transform matrix.
Definition: lmatrix.h:451
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
Definition: lvecBase3.h:1371
Defines a single collision event.
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
A container for geometry primitives.
Definition: geom.h:58
An instance of this class is passed to the Factory when requesting it to do its business and construc...
Definition: factoryParams.h:40
bool invert_from(const LMatrix4f &other)
Computes the inverse of the other matrix, and stores the result in this matrix.
Definition: lmatrix.h:2173
void add_data3(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Sets the write row to a particular 3-component value, and advances the write row. ...
LVecBase4f xform(const LVecBase4f &v) const
4-component vector or point times matrix.
Definition: lmatrix.h:1647
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
Definition: factory.I:90
An infinite line, similar to a CollisionRay, except that it extends in both directions.
Definition: collisionLine.h:28
float length() const
Returns the length of the vector, by the Pythagorean theorem.
Definition: lvecBase3.h:766
This is a two-component vector offset.
Definition: lvector2.h:91
void set_contact_pos(const LPoint3 &pos)
Stores the position of the "from" object at the instant at which the collision is first detected...
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition: bamReader.I:213
This defines a parabolic arc, or subset of an arc, similar to the path of a projectile or falling obj...
bool normalize()
Normalizes the vector in place.
Definition: lvecBase2.h:1833
void set_row(int row, const LVecBase4f &v)
Replaces the indicated row of the matrix.
Definition: lmatrix.h:1187
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
A class to retrieve the individual data elements previously stored in a Datagram. ...
NodePath get_from_node_path() const
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
This implements a solid roughly in cylindrical shape.
Definition: collisionTube.h:30
bool get_respect_effective_normal() const
See set_respect_effective_normal().
bool normalize()
Normalizes the vector in place.
Definition: lvecBase3.h:783
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition: datagram.h:43
void set_surface_point(const LPoint3 &point)
Stores the point, on the surface of the "into" object, at which a collision is detected.
bool around(const GeometricBoundingVolume **first, const GeometricBoundingVolume **last)
Resets the volume to enclose only the volumes indicated.
void set_surface_normal(const LVector3 &normal)
Stores the surface normal of the "into" object at the point of the intersection.
bool extend_by(const GeometricBoundingVolume *vol)
Increases the size of the volume to include the given volume.
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.