Panda3D
 All Classes Functions Variables Enumerations
collisionSphere.cxx
1 // Filename: collisionSphere.cxx
2 // Created by: drose (24Apr00)
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 
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 "boundingSphere.h"
25 #include "datagram.h"
26 #include "datagramIterator.h"
27 #include "bamReader.h"
28 #include "bamWriter.h"
29 #include "nearly_zero.h"
30 #include "cmath.h"
31 #include "mathNumbers.h"
32 #include "geom.h"
33 #include "geomTristrips.h"
34 #include "geomVertexWriter.h"
35 
36 PStatCollector CollisionSphere::_volume_pcollector(
37  "Collision Volumes:CollisionSphere");
38 PStatCollector CollisionSphere::_test_pcollector(
39  "Collision Tests:CollisionSphere");
40 TypeHandle CollisionSphere::_type_handle;
41 
42 ////////////////////////////////////////////////////////////////////
43 // Function: CollisionSphere::make_copy
44 // Access: Public, Virtual
45 // Description:
46 ////////////////////////////////////////////////////////////////////
47 CollisionSolid *CollisionSphere::
48 make_copy() {
49  return new CollisionSphere(*this);
50 }
51 
52 ////////////////////////////////////////////////////////////////////
53 // Function: CollisionSphere::test_intersection
54 // Access: Public, Virtual
55 // Description:
56 ////////////////////////////////////////////////////////////////////
58 test_intersection(const CollisionEntry &entry) const {
59  return entry.get_into()->test_intersection_from_sphere(entry);
60 }
61 
62 ////////////////////////////////////////////////////////////////////
63 // Function: CollisionSphere::xform
64 // Access: Public, Virtual
65 // Description: Transforms the solid by the indicated matrix.
66 ////////////////////////////////////////////////////////////////////
68 xform(const LMatrix4 &mat) {
69  _center = _center * mat;
70 
71  // This is a little cheesy and fails miserably in the presence of a
72  // non-uniform scale.
73  LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
74  _radius = length(radius_v);
75  mark_viz_stale();
76  mark_internal_bounds_stale();
77 }
78 
79 ////////////////////////////////////////////////////////////////////
80 // Function: CollisionSphere::get_collision_origin
81 // Access: Public, Virtual
82 // Description: Returns the point in space deemed to be the "origin"
83 // of the solid for collision purposes. The closest
84 // intersection point to this origin point is considered
85 // to be the most significant.
86 ////////////////////////////////////////////////////////////////////
89  return get_center();
90 }
91 
92 ////////////////////////////////////////////////////////////////////
93 // Function: CollisionSphere::get_volume_pcollector
94 // Access: Public, Virtual
95 // Description: Returns a PStatCollector that is used to count the
96 // number of bounding volume tests made against a solid
97 // of this type in a given frame.
98 ////////////////////////////////////////////////////////////////////
101  return _volume_pcollector;
102 }
103 
104 ////////////////////////////////////////////////////////////////////
105 // Function: CollisionSphere::get_test_pcollector
106 // Access: Public, Virtual
107 // Description: Returns a PStatCollector that is used to count the
108 // number of intersection tests made against a solid
109 // of this type in a given frame.
110 ////////////////////////////////////////////////////////////////////
113  return _test_pcollector;
114 }
115 
116 ////////////////////////////////////////////////////////////////////
117 // Function: CollisionSphere::output
118 // Access: Public, Virtual
119 // Description:
120 ////////////////////////////////////////////////////////////////////
121 void CollisionSphere::
122 output(ostream &out) const {
123  out << "sphere, c (" << get_center() << "), r " << get_radius();
124 }
125 
126 ////////////////////////////////////////////////////////////////////
127 // Function: CollisionSphere::compute_internal_bounds
128 // Access: Protected, Virtual
129 // Description:
130 ////////////////////////////////////////////////////////////////////
132 compute_internal_bounds() const {
133  return new BoundingSphere(_center, _radius);
134 }
135 
136 ////////////////////////////////////////////////////////////////////
137 // Function: CollisionSphere::test_intersection_from_sphere
138 // Access: Public, Virtual
139 // Description:
140 ////////////////////////////////////////////////////////////////////
142 test_intersection_from_sphere(const CollisionEntry &entry) const {
143  const CollisionSphere *sphere;
144  DCAST_INTO_R(sphere, entry.get_from(), 0);
145 
146  CPT(TransformState) wrt_space = entry.get_wrt_space();
147 
148  const LMatrix4 &wrt_mat = wrt_space->get_mat();
149 
150  LPoint3 from_b = sphere->get_center() * wrt_mat;
151 
152  LPoint3 into_center(get_center());
153  PN_stdfloat into_radius(get_radius());
154 
155  LVector3 from_radius_v =
156  LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
157  PN_stdfloat from_radius = length(from_radius_v);
158 
159  LPoint3 into_intersection_point(from_b);
160  double t1, t2;
161  LPoint3 contact_point(into_intersection_point);
162  PN_stdfloat actual_t = 0.0f;
163 
164  LVector3 vec = from_b - into_center;
165  PN_stdfloat dist2 = dot(vec, vec);
166  if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) {
167  // No intersection with the current position. Check the delta
168  // from the previous frame.
169  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
170  LPoint3 from_a = sphere->get_center() * wrt_prev_space->get_mat();
171 
172  if (!from_a.almost_equal(from_b)) {
173  LVector3 from_direction = from_b - from_a;
174  if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
175  // No intersection.
176  return NULL;
177  }
178 
179  if (t2 < 0.0 || t1 > 1.0) {
180  // Both intersection points are before the start of the segment or
181  // after the end of the segment.
182  return NULL;
183  }
184 
185  // doubles, not floats, to satisfy min and max templates.
186  actual_t = min(1.0, max(0.0, t1));
187  contact_point = from_a + actual_t * (from_b - from_a);
188 
189  if (t1 < 0.0) {
190  // Point a is within the sphere. The first intersection point is
191  // point a itself.
192  into_intersection_point = from_a;
193  } else {
194  // Point a is outside the sphere, and point b is either inside the
195  // sphere or beyond it. The first intersection point is at t1.
196  into_intersection_point = from_a + t1 * from_direction;
197  }
198  } else {
199  // No delta, therefore no intersection.
200  return NULL;
201  }
202  }
203 
204  if (collide_cat.is_debug()) {
205  collide_cat.debug()
206  << "intersection detected from " << entry.get_from_node_path()
207  << " into " << entry.get_into_node_path() << "\n";
208  }
209  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
210 
211  LPoint3 from_center = sphere->get_center() * wrt_mat;
212 
213  LVector3 surface_normal;
214  LVector3 v(into_intersection_point - into_center);
215  PN_stdfloat vec_length = v.length();
216  if (IS_NEARLY_ZERO(vec_length)) {
217  // If we don't have a collision normal (e.g. the centers are
218  // exactly coincident), then make up an arbitrary normal--any one
219  // is as good as any other.
220  surface_normal.set(1.0, 0.0, 0.0);
221  } else {
222  surface_normal = v / vec_length;
223  }
224 
225  LVector3 eff_normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
226 
227  LVector3 contact_normal;
228  LVector3 v2 = contact_point - into_center;
229  PN_stdfloat v2_len = v2.length();
230  if (IS_NEARLY_ZERO(v2_len)) {
231  // If we don't have a collision normal (e.g. the centers are
232  // exactly coincident), then make up an arbitrary normal--any one
233  // is as good as any other.
234  contact_normal.set(1.0, 0.0, 0.0);
235  } else {
236  contact_normal = v2 / v2_len;
237  }
238 
239  new_entry->set_surface_normal(eff_normal);
240  new_entry->set_surface_point(into_center + surface_normal * into_radius);
241  new_entry->set_interior_point(from_center - surface_normal * from_radius);
242  new_entry->set_contact_pos(contact_point);
243  new_entry->set_contact_normal(contact_normal);
244  new_entry->set_t(actual_t);
245 
246  return new_entry;
247 }
248 
249 ////////////////////////////////////////////////////////////////////
250 // Function: CollisionSphere::test_intersection_from_line
251 // Access: Public, Virtual
252 // Description:
253 ////////////////////////////////////////////////////////////////////
255 test_intersection_from_line(const CollisionEntry &entry) const {
256  const CollisionLine *line;
257  DCAST_INTO_R(line, entry.get_from(), 0);
258 
259  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
260 
261  LPoint3 from_origin = line->get_origin() * wrt_mat;
262  LVector3 from_direction = line->get_direction() * wrt_mat;
263 
264  double t1, t2;
265  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
266  // No intersection.
267  return NULL;
268  }
269 
270  if (collide_cat.is_debug()) {
271  collide_cat.debug()
272  << "intersection detected from " << entry.get_from_node_path()
273  << " into " << entry.get_into_node_path() << "\n";
274  }
275  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
276 
277  LPoint3 into_intersection_point = from_origin + t1 * from_direction;
278  new_entry->set_surface_point(into_intersection_point);
279 
281  new_entry->set_surface_normal(get_effective_normal());
282  } else {
283  LVector3 normal = into_intersection_point - get_center();
284  normal.normalize();
285  new_entry->set_surface_normal(normal);
286  }
287 
288  return new_entry;
289 }
290 
291 ////////////////////////////////////////////////////////////////////
292 // Function: CollisionSphere::test_intersection_from_box
293 // Access: Public, Virtual
294 // Description: Double dispatch point for box as a FROM object
295 ////////////////////////////////////////////////////////////////////
297 test_intersection_from_box(const CollisionEntry &entry) const {
298  const CollisionBox *box;
299  DCAST_INTO_R(box, entry.get_from(), 0);
300 
301  CPT(TransformState) wrt_space = entry.get_wrt_space();
302  CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
303 
304  const LMatrix4 &wrt_mat = wrt_space->get_mat();
305 
306  CollisionBox local_b( *box );
307  local_b.xform( wrt_mat );
308 
309  LPoint3 from_center = local_b.get_center();
310 
311  LPoint3 orig_center = get_center();
312  LPoint3 to_center = orig_center;
313  LPoint3 contact_point(from_center);
314  PN_stdfloat actual_t = 1.0f;
315 
316  PN_stdfloat to_radius = get_radius();
317  PN_stdfloat to_radius_2 = to_radius * to_radius;
318 
319  int ip;
320  PN_stdfloat max_dist = 0.0f;
321  PN_stdfloat dist = 0.0f; // initial assignment to squelch silly compiler warning
322  bool intersect;
323  LPlane plane;
324  LVector3 normal;
325 
326  for (ip = 0, intersect=false; ip < 6 && !intersect; ip++) {
327  plane = local_b.get_plane( ip );
328  if (local_b.get_plane_points(ip).size() < 3) {
329  continue;
330  }
331  normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
332 
333  #ifndef NDEBUG
334  /*
335  if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
336  collide_cat.info()
337  << "polygon being collided with " << entry.get_into_node_path()
338  << " has normal " << normal << " of length " << normal.length()
339  << "\n";
340  normal.normalize();
341  }
342  */
343  #endif
344 
345  // The nearest point within the plane to our center is the
346  // intersection of the line (center, center - normal) with the plane.
347 
348  if (!plane.intersects_line(dist, to_center, -(plane.get_normal()))) {
349  // No intersection with plane? This means the plane's effective
350  // normal was within the plane itself. A useless polygon.
351  continue;
352  }
353 
354  if (dist > to_radius || dist < -to_radius) {
355  // No intersection with the plane.
356  continue;
357  }
358 
359  LPoint2 p = local_b.to_2d(to_center - dist * plane.get_normal(), ip);
360  PN_stdfloat edge_dist = 0.0f;
361 
362  edge_dist = local_b.dist_to_polygon(p, local_b.get_plane_points(ip));
363 
364  if(edge_dist < 0) {
365  intersect = true;
366  continue;
367  }
368 
369  if((edge_dist > 0) &&
370  ((edge_dist * edge_dist + dist * dist) > to_radius_2)) {
371  // No intersection; the circle is outside the polygon.
372  continue;
373  }
374 
375  // The sphere appears to intersect the polygon. If the edge is less
376  // than to_radius away, the sphere may be resting on an edge of
377  // the polygon. Determine how far the center of the sphere must
378  // remain from the plane, based on its distance from the nearest
379  // edge.
380 
381  max_dist = to_radius;
382  if (edge_dist >= 0.0f) {
383  PN_stdfloat max_dist_2 = max(to_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
384  max_dist = csqrt(max_dist_2);
385  }
386 
387  if (dist > max_dist) {
388  // There's no intersection: the sphere is hanging off the edge.
389  continue;
390  }
391  intersect = true;
392  }
393  if( !intersect )
394  return NULL;
395 
396  if (collide_cat.is_debug()) {
397  collide_cat.debug()
398  << "intersection detected from " << entry.get_from_node_path()
399  << " into " << entry.get_into_node_path() << "\n";
400  }
401 
402  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
403 
404  PN_stdfloat into_depth = max_dist - dist;
405 
406  new_entry->set_surface_normal(normal);
407  new_entry->set_surface_point(to_center - normal * dist);
408  new_entry->set_interior_point(to_center - normal * (dist + into_depth));
409  new_entry->set_contact_pos(contact_point);
410  new_entry->set_contact_normal(plane.get_normal());
411  new_entry->set_t(actual_t);
412 
413  return new_entry;
414 }
415 
416 ////////////////////////////////////////////////////////////////////
417 // Function: CollisionSphere::test_intersection_from_ray
418 // Access: Public, Virtual
419 // Description:
420 ////////////////////////////////////////////////////////////////////
422 test_intersection_from_ray(const CollisionEntry &entry) const {
423  const CollisionRay *ray;
424  DCAST_INTO_R(ray, entry.get_from(), 0);
425 
426  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
427 
428  LPoint3 from_origin = ray->get_origin() * wrt_mat;
429  LVector3 from_direction = ray->get_direction() * wrt_mat;
430 
431  double t1, t2;
432  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
433  // No intersection.
434  return NULL;
435  }
436 
437  if (t2 < 0.0) {
438  // Both intersection points are before the start of the ray.
439  return NULL;
440  }
441 
442  t1 = max(t1, 0.0);
443 
444  if (collide_cat.is_debug()) {
445  collide_cat.debug()
446  << "intersection detected from " << entry.get_from_node_path()
447  << " into " << entry.get_into_node_path() << "\n";
448  }
449  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
450 
451  LPoint3 into_intersection_point = from_origin + t1 * from_direction;
452  new_entry->set_surface_point(into_intersection_point);
453 
454  if (has_effective_normal() && ray->get_respect_effective_normal()) {
455  new_entry->set_surface_normal(get_effective_normal());
456  } else {
457  LVector3 normal = into_intersection_point - get_center();
458  normal.normalize();
459  new_entry->set_surface_normal(normal);
460  }
461 
462  return new_entry;
463 }
464 
465 ////////////////////////////////////////////////////////////////////
466 // Function: CollisionSphere::test_intersection_from_segment
467 // Access: Public, Virtual
468 // Description:
469 ////////////////////////////////////////////////////////////////////
471 test_intersection_from_segment(const CollisionEntry &entry) const {
472  const CollisionSegment *segment;
473  DCAST_INTO_R(segment, entry.get_from(), 0);
474 
475  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
476 
477  LPoint3 from_a = segment->get_point_a() * wrt_mat;
478  LPoint3 from_b = segment->get_point_b() * wrt_mat;
479  LVector3 from_direction = from_b - from_a;
480 
481  double t1, t2;
482  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
483  // No intersection.
484  return NULL;
485  }
486 
487  if (t2 < 0.0 || t1 > 1.0) {
488  // Both intersection points are before the start of the segment or
489  // after the end of the segment.
490  return NULL;
491  }
492 
493  t1 = max(t1, 0.0);
494 
495  if (collide_cat.is_debug()) {
496  collide_cat.debug()
497  << "intersection detected from " << entry.get_from_node_path()
498  << " into " << entry.get_into_node_path() << "\n";
499  }
500  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
501 
502  LPoint3 into_intersection_point = from_a + t1 * from_direction;
503  new_entry->set_surface_point(into_intersection_point);
504 
505  if (has_effective_normal() && segment->get_respect_effective_normal()) {
506  new_entry->set_surface_normal(get_effective_normal());
507  } else {
508  LVector3 normal = into_intersection_point - get_center();
509  normal.normalize();
510  new_entry->set_surface_normal(normal);
511  }
512 
513  return new_entry;
514 }
515 
516 ////////////////////////////////////////////////////////////////////
517 // Function: CollisionSphere::test_intersection_from_parabola
518 // Access: Public, Virtual
519 // Description:
520 ////////////////////////////////////////////////////////////////////
522 test_intersection_from_parabola(const CollisionEntry &entry) const {
523  const CollisionParabola *parabola;
524  DCAST_INTO_R(parabola, entry.get_from(), 0);
525 
526  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
527 
528  // Convert the parabola into local coordinate space.
529  LParabola local_p(parabola->get_parabola());
530  local_p.xform(wrt_mat);
531 
532  double t;
533  if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
534  local_p.calc_point(parabola->get_t1()),
535  local_p.calc_point(parabola->get_t2()))) {
536  // No intersection.
537  return NULL;
538  }
539 
540  if (collide_cat.is_debug()) {
541  collide_cat.debug()
542  << "intersection detected from " << entry.get_from_node_path()
543  << " into " << entry.get_into_node_path() << "\n";
544  }
545  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
546 
547  LPoint3 into_intersection_point = local_p.calc_point(t);
548  new_entry->set_surface_point(into_intersection_point);
549 
550  if (has_effective_normal() && parabola->get_respect_effective_normal()) {
551  new_entry->set_surface_normal(get_effective_normal());
552  } else {
553  LVector3 normal = into_intersection_point - get_center();
554  normal.normalize();
555  new_entry->set_surface_normal(normal);
556  }
557 
558  return new_entry;
559 }
560 
561 ////////////////////////////////////////////////////////////////////
562 // Function: CollisionSphere::fill_viz_geom
563 // Access: Protected, Virtual
564 // Description: Fills the _viz_geom GeomNode up with Geoms suitable
565 // for rendering this solid.
566 ////////////////////////////////////////////////////////////////////
567 void CollisionSphere::
568 fill_viz_geom() {
569  if (collide_cat.is_debug()) {
570  collide_cat.debug()
571  << "Recomputing viz for " << *this << "\n";
572  }
573 
574  static const int num_slices = 16;
575  static const int num_stacks = 8;
576 
577  PT(GeomVertexData) vdata = new GeomVertexData
578  ("collision", GeomVertexFormat::get_v3(),
579  Geom::UH_static);
580  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
581 
582  PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
583  for (int sl = 0; sl < num_slices; ++sl) {
584  PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
585  PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
586  vertex.add_data3(compute_point(0.0, longitude0));
587  for (int st = 1; st < num_stacks; ++st) {
588  PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
589  vertex.add_data3(compute_point(latitude, longitude0));
590  vertex.add_data3(compute_point(latitude, longitude1));
591  }
592  vertex.add_data3(compute_point(1.0, longitude0));
593 
594  strip->add_next_vertices(num_stacks * 2);
595  strip->close_primitive();
596  }
597 
598  PT(Geom) geom = new Geom(vdata);
599  geom->add_primitive(strip);
600 
601  _viz_geom->add_geom(geom, get_solid_viz_state());
602  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
603 }
604 
605 ////////////////////////////////////////////////////////////////////
606 // Function: CollisionSphere::intersects_line
607 // Access: Protected
608 // Description: Determine the point(s) of intersection of a parametric
609 // line with the sphere. The line is infinite in both
610 // directions, and passes through "from" and from+delta.
611 // If the line does not intersect the sphere, the
612 // function returns false, and t1 and t2 are undefined.
613 // If it does intersect the sphere, it returns true, and
614 // t1 and t2 are set to the points along the equation
615 // from+t*delta that correspond to the two points of
616 // intersection.
617 ////////////////////////////////////////////////////////////////////
618 bool CollisionSphere::
619 intersects_line(double &t1, double &t2,
620  const LPoint3 &from, const LVector3 &delta,
621  PN_stdfloat inflate_radius) const {
622  // Solve the equation for the intersection of a line with a sphere
623  // using the quadratic equation.
624 
625  // A line segment from f to f+d is defined as all P such that
626  // P = f + td for 0 <= t <= 1.
627 
628  // A sphere with radius r about point c is defined as all P such
629  // that r^2 = (P - c)^2.
630 
631  // Substituting P in the above we have:
632 
633  // r^2 = (f + td - c)^2 =
634  // (f^2 + ftd - fc + ftd + t^2d^2 - tdc - fc - tdc + c^2) =
635  // t^2(d^2) + t(fd + fd - dc - dc) + (f^2 - fc - fc + c^2) =
636  // t^2(d^2) + t(2d(f - c)) + (f - c)^2
637 
638  // Thus, the equation is quadratic in t, and we have
639  // at^2 + bt + c = 0
640 
641  // Where a = d^2
642  // b = 2d(f - c)
643  // c = (f - c)^2 - r^2
644 
645  // Solving for t using the quadratic equation gives us the point of
646  // intersection along the line segment. Actually, there are two
647  // solutions (since it is quadratic): one for the front of the
648  // sphere, and one for the back. In the case where the line is
649  // tangent to the sphere, there is only one solution (and the
650  // radical is zero).
651 
652  double A = dot(delta, delta);
653 
654  nassertr(A != 0.0, false);
655 
656  LVector3 fc = from - get_center();
657  double B = 2.0f* dot(delta, fc);
658  double fc_d2 = dot(fc, fc);
659  double radius = get_radius() + inflate_radius;
660  double C = fc_d2 - radius * radius;
661 
662  double radical = B*B - 4.0*A*C;
663 
664  if (IS_NEARLY_ZERO(radical)) {
665  // Tangent.
666  t1 = t2 = -B /(2.0*A);
667  return true;
668 
669  } else if (radical < 0.0) {
670  // No real roots: no intersection with the line.
671  return false;
672  }
673 
674  double reciprocal_2A = 1.0/(2.0*A);
675  double sqrt_radical = sqrtf(radical);
676  t1 = ( -B - sqrt_radical ) * reciprocal_2A;
677  t2 = ( -B + sqrt_radical ) * reciprocal_2A;
678 
679  return true;
680 }
681 
682 ////////////////////////////////////////////////////////////////////
683 // Function: CollisionSphere::intersects_parabola
684 // Access: Protected
685 // Description: Determine a point of intersection of a parametric
686 // parabola with the sphere.
687 //
688 // We only consider the segment of the parabola between
689 // t1 and t2, which has already been computed as
690 // corresponding to points p1 and p2. If there is an
691 // intersection, t is set to the parametric point of
692 // intersection, and true is returned; otherwise, false
693 // is returned.
694 ////////////////////////////////////////////////////////////////////
695 bool CollisionSphere::
696 intersects_parabola(double &t, const LParabola &parabola,
697  double t1, double t2,
698  const LPoint3 &p1, const LPoint3 &p2) const {
699  if (t1 == t2) {
700  // Special case: a single point.
701  if ((p1 - _center).length_squared() > _radius * _radius) {
702  // No intersection.
703  return false;
704  }
705  t = t1;
706  return true;
707  }
708 
709  // To directly test for intersection between a parabola (quadratic)
710  // and a sphere (also quadratic) requires solving a quartic
711  // equation. Doable, but hard, and I'm a programmer, not a
712  // mathematician. So I'll solve it the programmer's way instead, by
713  // approximating the parabola with a series of line segments.
714  // Hence, this function works by recursively subdividing the
715  // parabola as necessary.
716 
717  // First, see if the line segment (p1 - p2) comes sufficiently close
718  // to the parabola. Do this by computing the parametric intervening
719  // point and comparing its distance from the linear intervening
720  // point.
721  double tmid = (t1 + t2) * 0.5;
722  if (tmid != t1 && tmid != t2) {
723  LPoint3 pmid = parabola.calc_point(tmid);
724  LPoint3 pmid2 = (p1 + p2) * 0.5f;
725 
726  if ((pmid - pmid2).length_squared() > 0.001f) {
727  // Subdivide.
728  if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
729  return true;
730  }
731  return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
732  }
733  }
734 
735  // The line segment is sufficiently close; compare the segment itself.
736  double t1a, t2a;
737  if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
738  return false;
739  }
740 
741  if (t2a < 0.0 || t1a > 1.0) {
742  return false;
743  }
744 
745  t = max(t1a, 0.0);
746  return true;
747 }
748 
749 ////////////////////////////////////////////////////////////////////
750 // Function: CollisionSphere::compute_point
751 // Access: Protected
752 // Description: Returns a point on the surface of the sphere.
753 // latitude and longitude range from 0.0 to 1.0. This
754 // is used by fill_viz_geom() to create a visible
755 // representation of the sphere.
756 ////////////////////////////////////////////////////////////////////
757 LVertex CollisionSphere::
758 compute_point(PN_stdfloat latitude, PN_stdfloat longitude) const {
759  PN_stdfloat s1, c1;
760  csincos(latitude * MathNumbers::pi, &s1, &c1);
761 
762  PN_stdfloat s2, c2;
763  csincos(longitude * 2.0f * MathNumbers::pi, &s2, &c2);
764 
765  LVertex p(s1 * c2, s1 * s2, c1);
766  return p * get_radius() + get_center();
767 }
768 
769 ////////////////////////////////////////////////////////////////////
770 // Function: CollisionSphere::register_with_read_factory
771 // Access: Public, Static
772 // Description: Factory method to generate a CollisionSphere object
773 ////////////////////////////////////////////////////////////////////
776  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionSphere);
777 }
778 
779 ////////////////////////////////////////////////////////////////////
780 // Function: CollisionSphere::write_datagram
781 // Access: Public
782 // Description: Function to write the important information in
783 // the particular object to a Datagram
784 ////////////////////////////////////////////////////////////////////
787  CollisionSolid::write_datagram(manager, me);
788  _center.write_datagram(me);
789  me.add_stdfloat(_radius);
790 }
791 
792 ////////////////////////////////////////////////////////////////////
793 // Function: CollisionSphere::make_CollisionSphere
794 // Access: Protected
795 // Description: Factory method to generate a CollisionSphere object
796 ////////////////////////////////////////////////////////////////////
797 TypedWritable *CollisionSphere::
798 make_CollisionSphere(const FactoryParams &params) {
800  DatagramIterator scan;
801  BamReader *manager;
802 
803  parse_params(params, scan, manager);
804  me->fillin(scan, manager);
805  return me;
806 }
807 
808 ////////////////////////////////////////////////////////////////////
809 // Function: CollisionSphere::fillin
810 // Access: Protected
811 // Description: Function that reads out of the datagram (or asks
812 // manager to read) all of the data that is needed to
813 // re-create this object and stores it in the appropiate
814 // place
815 ////////////////////////////////////////////////////////////////////
816 void CollisionSphere::
817 fillin(DatagramIterator& scan, BamReader* manager) {
818  CollisionSolid::fillin(scan, manager);
819  _center.read_datagram(scan);
820  _radius = scan.get_stdfloat();
821 }
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
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_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:122
static void register_with_read_factory()
Factory method to generate a CollisionSphere object.
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_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
This defines a bounding sphere, consisting of a center and a radius.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the &quot;origin&quot; of the solid for collision purposes.
A cuboid collision volume or object.
Definition: collisionBox.h:30
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:37
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
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.
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
A lightweight class that represents a single element that may be timed and/or counted via stats...
A finite line segment, with two specific endpoints but no thickness.
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.
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
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...
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
A class to retrieve the individual data elements previously stored in a Datagram. ...
This is a two-component point in space.
Definition: lpoint2.h:92
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
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 write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
Definition: lvecBase3.h:1355