Panda3D
 All Classes Functions Variables Enumerations
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();
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 ////////////////////////////////////////////////////////////////////
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 ////////////////////////////////////////////////////////////////////
159 test_intersection_from_sphere(const CollisionEntry &entry) const {
160  const CollisionSphere *sphere;
161  DCAST_INTO_R(sphere, entry.get_from(), 0);
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 ////////////////////////////////////////////////////////////////////
240 test_intersection_from_line(const CollisionEntry &entry) const {
241  const CollisionLine *line;
242  DCAST_INTO_R(line, entry.get_from(), 0);
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 
265  if (has_effective_normal() && line->get_respect_effective_normal()) {
266  new_entry->set_surface_normal(get_effective_normal());
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 ////////////////////////////////////////////////////////////////////
290 test_intersection_from_ray(const CollisionEntry &entry) const {
291  const CollisionRay *ray;
292  DCAST_INTO_R(ray, entry.get_from(), 0);
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 
329  if (has_effective_normal() && ray->get_respect_effective_normal()) {
330  new_entry->set_surface_normal(get_effective_normal());
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 ////////////////////////////////////////////////////////////////////
354 test_intersection_from_segment(const CollisionEntry &entry) const {
355  const CollisionSegment *segment;
356  DCAST_INTO_R(segment, entry.get_from(), 0);
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 
395  if (has_effective_normal() && segment->get_respect_effective_normal()) {
396  new_entry->set_surface_normal(get_effective_normal());
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 ////////////////////////////////////////////////////////////////////
420 test_intersection_from_parabola(const CollisionEntry &entry) const {
421  const CollisionParabola *parabola;
422  DCAST_INTO_R(parabola, entry.get_from(), 0);
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()) {
449  new_entry->set_surface_normal(get_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 }
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
bool get_respect_effective_normal() const
See set_respect_effective_normal().
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
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:416
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
Definition: lvecBase3.h:1373
The abstract base class for all things that can collide with other things in the world, and all the things they can collide with (except geometry).
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
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
Defines a series of triangle strips.
Definition: geomTristrips.h:25
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 &quot;into&quot; object, which represents the depth to which the &quot;...
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
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...
float length() const
Returns the length of the vector, by the Pythagorean theorem.
Definition: lvecBase3.h:765
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
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
PN_stdfloat get_t2() const
Returns the ending point on the parabola.
const LParabola & get_parabola() const
Returns the parabola specified by this solid.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the &quot;origin&quot; of the solid for collision purposes.
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
Definition: factory.I:90
void add_next_vertices(int num_vertices)
Adds the next n vertices in sequence, beginning from the last vertex added to the primitive + 1...
An infinite line, similar to a CollisionRay, except that it extends in both directions.
Definition: collisionLine.h:28
This is a two-component vector offset.
Definition: lvector2.h:91
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition: bamReader.I:213
This defines a parabolic arc, or subset of an arc, similar to the path of a projectile or falling obj...
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
void set_row(int row, const LVecBase4f &v)
Replaces the indicated row of the matrix.
Definition: lmatrix.h:1187
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:85
This implements a solid roughly in cylindrical shape.
Definition: collisionTube.h:30
bool normalize()
Normalizes the vector in place.
Definition: lvecBase3.h:782
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
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 &quot;into&quot; 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 write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
Definition: lvecBase3.h:1355
void set_surface_normal(const LVector3 &normal)
Stores the surface normal of the &quot;into&quot; object at the point of the intersection.
bool extend_by(const GeometricBoundingVolume *vol)
Increases the size of the volume to include the given volume.