Panda3D
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 ////////////////////////////////////////////////////////////////////
57 PT(CollisionEntry) CollisionSphere::
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 ////////////////////////////////////////////////////////////////////
67 void CollisionSphere::
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 ////////////////////////////////////////////////////////////////////
131 PT(BoundingVolume) CollisionSphere::
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 ////////////////////////////////////////////////////////////////////
141 PT(CollisionEntry) CollisionSphere::
142 test_intersection_from_sphere(const CollisionEntry &entry) const {
143  const CollisionSphere *sphere;
144  DCAST_INTO_R(sphere, entry.get_from(), NULL);
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 ////////////////////////////////////////////////////////////////////
254 PT(CollisionEntry) CollisionSphere::
255 test_intersection_from_line(const CollisionEntry &entry) const {
256  const CollisionLine *line;
257  DCAST_INTO_R(line, entry.get_from(), NULL);
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 ////////////////////////////////////////////////////////////////////
296 PT(CollisionEntry) CollisionSphere::
297 test_intersection_from_box(const CollisionEntry &entry) const {
298  const CollisionBox *box;
299  DCAST_INTO_R(box, entry.get_from(), NULL);
300 
301  // Instead of transforming the box into the sphere's coordinate space,
302  // we do it the other way around. It's easier that way.
303  const LMatrix4 &wrt_mat = entry.get_inv_wrt_mat();
304 
305  LPoint3 center = wrt_mat.xform_point(_center);
306  PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, _radius)).length_squared();
307 
308  LPoint3 box_min = box->get_min();
309  LPoint3 box_max = box->get_max();
310 
311  // Arvo's algorithm.
312  PN_stdfloat d = 0;
313  PN_stdfloat s;
314 
315  if (center[0] < box_min[0]) {
316  s = center[0] - box_min[0];
317  d += s * s;
318 
319  } else if (center[0] > box_max[0]) {
320  s = center[0] - box_max[0];
321  d += s * s;
322  }
323 
324  if (center[1] < box_min[1]) {
325  s = center[1] - box_min[1];
326  d += s * s;
327 
328  } else if (center[1] > box_max[1]) {
329  s = center[1] - box_max[1];
330  d += s * s;
331  }
332 
333  if (center[2] < box_min[2]) {
334  s = center[2] - box_min[2];
335  d += s * s;
336 
337  } else if (center[2] > box_max[2]) {
338  s = center[2] - box_max[2];
339  d += s * s;
340  }
341 
342  if (d > radius_sq) {
343  return NULL;
344  }
345 
346  if (collide_cat.is_debug()) {
347  collide_cat.debug()
348  << "intersection detected from " << entry.get_from_node_path()
349  << " into " << entry.get_into_node_path() << "\n";
350  }
351 
352  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
353 
354  // To get the interior point, clamp the sphere center to the AABB.
355  LPoint3 interior = entry.get_wrt_mat().xform_point(center.fmax(box_min).fmin(box_max));
356  new_entry->set_interior_point(interior);
357 
358  // Now extrapolate the surface point and normal from that.
359  LVector3 normal = interior - _center;
360  normal.normalize();
361  new_entry->set_surface_point(_center + normal * _radius);
362  new_entry->set_surface_normal(
364  ? get_effective_normal() : normal);
365 
366  return new_entry;
367 }
368 
369 ////////////////////////////////////////////////////////////////////
370 // Function: CollisionSphere::test_intersection_from_ray
371 // Access: Public, Virtual
372 // Description:
373 ////////////////////////////////////////////////////////////////////
374 PT(CollisionEntry) CollisionSphere::
375 test_intersection_from_ray(const CollisionEntry &entry) const {
376  const CollisionRay *ray;
377  DCAST_INTO_R(ray, entry.get_from(), NULL);
378 
379  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
380 
381  LPoint3 from_origin = ray->get_origin() * wrt_mat;
382  LVector3 from_direction = ray->get_direction() * wrt_mat;
383 
384  double t1, t2;
385  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
386  // No intersection.
387  return NULL;
388  }
389 
390  if (t2 < 0.0) {
391  // Both intersection points are before the start of the ray.
392  return NULL;
393  }
394 
395  t1 = max(t1, 0.0);
396 
397  if (collide_cat.is_debug()) {
398  collide_cat.debug()
399  << "intersection detected from " << entry.get_from_node_path()
400  << " into " << entry.get_into_node_path() << "\n";
401  }
402  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
403 
404  LPoint3 into_intersection_point = from_origin + t1 * from_direction;
405  new_entry->set_surface_point(into_intersection_point);
406 
408  new_entry->set_surface_normal(get_effective_normal());
409  } else {
410  LVector3 normal = into_intersection_point - get_center();
411  normal.normalize();
412  new_entry->set_surface_normal(normal);
413  }
414 
415  return new_entry;
416 }
417 
418 ////////////////////////////////////////////////////////////////////
419 // Function: CollisionSphere::test_intersection_from_segment
420 // Access: Public, Virtual
421 // Description:
422 ////////////////////////////////////////////////////////////////////
423 PT(CollisionEntry) CollisionSphere::
424 test_intersection_from_segment(const CollisionEntry &entry) const {
425  const CollisionSegment *segment;
426  DCAST_INTO_R(segment, entry.get_from(), NULL);
427 
428  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
429 
430  LPoint3 from_a = segment->get_point_a() * wrt_mat;
431  LPoint3 from_b = segment->get_point_b() * wrt_mat;
432  LVector3 from_direction = from_b - from_a;
433 
434  double t1, t2;
435  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
436  // No intersection.
437  return NULL;
438  }
439 
440  if (t2 < 0.0 || t1 > 1.0) {
441  // Both intersection points are before the start of the segment or
442  // after the end of the segment.
443  return NULL;
444  }
445 
446  t1 = max(t1, 0.0);
447 
448  if (collide_cat.is_debug()) {
449  collide_cat.debug()
450  << "intersection detected from " << entry.get_from_node_path()
451  << " into " << entry.get_into_node_path() << "\n";
452  }
453  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
454 
455  LPoint3 into_intersection_point = from_a + t1 * from_direction;
456  new_entry->set_surface_point(into_intersection_point);
457 
459  new_entry->set_surface_normal(get_effective_normal());
460  } else {
461  LVector3 normal = into_intersection_point - get_center();
462  normal.normalize();
463  new_entry->set_surface_normal(normal);
464  }
465 
466  return new_entry;
467 }
468 
469 ////////////////////////////////////////////////////////////////////
470 // Function: CollisionSphere::test_intersection_from_parabola
471 // Access: Public, Virtual
472 // Description:
473 ////////////////////////////////////////////////////////////////////
474 PT(CollisionEntry) CollisionSphere::
475 test_intersection_from_parabola(const CollisionEntry &entry) const {
476  const CollisionParabola *parabola;
477  DCAST_INTO_R(parabola, entry.get_from(), NULL);
478 
479  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
480 
481  // Convert the parabola into local coordinate space.
482  LParabola local_p(parabola->get_parabola());
483  local_p.xform(wrt_mat);
484 
485  double t;
486  if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
487  local_p.calc_point(parabola->get_t1()),
488  local_p.calc_point(parabola->get_t2()))) {
489  // No intersection.
490  return NULL;
491  }
492 
493  if (collide_cat.is_debug()) {
494  collide_cat.debug()
495  << "intersection detected from " << entry.get_from_node_path()
496  << " into " << entry.get_into_node_path() << "\n";
497  }
498  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
499 
500  LPoint3 into_intersection_point = local_p.calc_point(t);
501  new_entry->set_surface_point(into_intersection_point);
502 
503  if (has_effective_normal() && parabola->get_respect_effective_normal()) {
504  new_entry->set_surface_normal(get_effective_normal());
505  } else {
506  LVector3 normal = into_intersection_point - get_center();
507  normal.normalize();
508  new_entry->set_surface_normal(normal);
509  }
510 
511  return new_entry;
512 }
513 
514 ////////////////////////////////////////////////////////////////////
515 // Function: CollisionSphere::fill_viz_geom
516 // Access: Protected, Virtual
517 // Description: Fills the _viz_geom GeomNode up with Geoms suitable
518 // for rendering this solid.
519 ////////////////////////////////////////////////////////////////////
520 void CollisionSphere::
521 fill_viz_geom() {
522  if (collide_cat.is_debug()) {
523  collide_cat.debug()
524  << "Recomputing viz for " << *this << "\n";
525  }
526 
527  static const int num_slices = 16;
528  static const int num_stacks = 8;
529 
530  PT(GeomVertexData) vdata = new GeomVertexData
531  ("collision", GeomVertexFormat::get_v3(),
532  Geom::UH_static);
533  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
534 
535  PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
536  for (int sl = 0; sl < num_slices; ++sl) {
537  PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
538  PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
539  vertex.add_data3(compute_point(0.0, longitude0));
540  for (int st = 1; st < num_stacks; ++st) {
541  PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
542  vertex.add_data3(compute_point(latitude, longitude0));
543  vertex.add_data3(compute_point(latitude, longitude1));
544  }
545  vertex.add_data3(compute_point(1.0, longitude0));
546 
547  strip->add_next_vertices(num_stacks * 2);
548  strip->close_primitive();
549  }
550 
551  PT(Geom) geom = new Geom(vdata);
552  geom->add_primitive(strip);
553 
554  _viz_geom->add_geom(geom, get_solid_viz_state());
555  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
556 }
557 
558 ////////////////////////////////////////////////////////////////////
559 // Function: CollisionSphere::intersects_line
560 // Access: Protected
561 // Description: Determine the point(s) of intersection of a parametric
562 // line with the sphere. The line is infinite in both
563 // directions, and passes through "from" and from+delta.
564 // If the line does not intersect the sphere, the
565 // function returns false, and t1 and t2 are undefined.
566 // If it does intersect the sphere, it returns true, and
567 // t1 and t2 are set to the points along the equation
568 // from+t*delta that correspond to the two points of
569 // intersection.
570 ////////////////////////////////////////////////////////////////////
571 bool CollisionSphere::
572 intersects_line(double &t1, double &t2,
573  const LPoint3 &from, const LVector3 &delta,
574  PN_stdfloat inflate_radius) const {
575  // Solve the equation for the intersection of a line with a sphere
576  // using the quadratic equation.
577 
578  // A line segment from f to f+d is defined as all P such that
579  // P = f + td for 0 <= t <= 1.
580 
581  // A sphere with radius r about point c is defined as all P such
582  // that r^2 = (P - c)^2.
583 
584  // Substituting P in the above we have:
585 
586  // r^2 = (f + td - c)^2 =
587  // (f^2 + ftd - fc + ftd + t^2d^2 - tdc - fc - tdc + c^2) =
588  // t^2(d^2) + t(fd + fd - dc - dc) + (f^2 - fc - fc + c^2) =
589  // t^2(d^2) + t(2d(f - c)) + (f - c)^2
590 
591  // Thus, the equation is quadratic in t, and we have
592  // at^2 + bt + c = 0
593 
594  // Where a = d^2
595  // b = 2d(f - c)
596  // c = (f - c)^2 - r^2
597 
598  // Solving for t using the quadratic equation gives us the point of
599  // intersection along the line segment. Actually, there are two
600  // solutions (since it is quadratic): one for the front of the
601  // sphere, and one for the back. In the case where the line is
602  // tangent to the sphere, there is only one solution (and the
603  // radical is zero).
604 
605  double A = dot(delta, delta);
606 
607  nassertr(A != 0.0, false);
608 
609  LVector3 fc = from - get_center();
610  double B = 2.0f* dot(delta, fc);
611  double fc_d2 = dot(fc, fc);
612  double radius = get_radius() + inflate_radius;
613  double C = fc_d2 - radius * radius;
614 
615  double radical = B*B - 4.0*A*C;
616 
617  if (IS_NEARLY_ZERO(radical)) {
618  // Tangent.
619  t1 = t2 = -B /(2.0*A);
620  return true;
621 
622  } else if (radical < 0.0) {
623  // No real roots: no intersection with the line.
624  return false;
625  }
626 
627  double reciprocal_2A = 1.0/(2.0*A);
628  double sqrt_radical = sqrtf(radical);
629  t1 = ( -B - sqrt_radical ) * reciprocal_2A;
630  t2 = ( -B + sqrt_radical ) * reciprocal_2A;
631 
632  return true;
633 }
634 
635 ////////////////////////////////////////////////////////////////////
636 // Function: CollisionSphere::intersects_parabola
637 // Access: Protected
638 // Description: Determine a point of intersection of a parametric
639 // parabola with the sphere.
640 //
641 // We only consider the segment of the parabola between
642 // t1 and t2, which has already been computed as
643 // corresponding to points p1 and p2. If there is an
644 // intersection, t is set to the parametric point of
645 // intersection, and true is returned; otherwise, false
646 // is returned.
647 ////////////////////////////////////////////////////////////////////
648 bool CollisionSphere::
649 intersects_parabola(double &t, const LParabola &parabola,
650  double t1, double t2,
651  const LPoint3 &p1, const LPoint3 &p2) const {
652  if (t1 == t2) {
653  // Special case: a single point.
654  if ((p1 - _center).length_squared() > _radius * _radius) {
655  // No intersection.
656  return false;
657  }
658  t = t1;
659  return true;
660  }
661 
662  // To directly test for intersection between a parabola (quadratic)
663  // and a sphere (also quadratic) requires solving a quartic
664  // equation. Doable, but hard, and I'm a programmer, not a
665  // mathematician. So I'll solve it the programmer's way instead, by
666  // approximating the parabola with a series of line segments.
667  // Hence, this function works by recursively subdividing the
668  // parabola as necessary.
669 
670  // First, see if the line segment (p1 - p2) comes sufficiently close
671  // to the parabola. Do this by computing the parametric intervening
672  // point and comparing its distance from the linear intervening
673  // point.
674  double tmid = (t1 + t2) * 0.5;
675  if (tmid != t1 && tmid != t2) {
676  LPoint3 pmid = parabola.calc_point(tmid);
677  LPoint3 pmid2 = (p1 + p2) * 0.5f;
678 
679  if ((pmid - pmid2).length_squared() > 0.001f) {
680  // Subdivide.
681  if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
682  return true;
683  }
684  return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
685  }
686  }
687 
688  // The line segment is sufficiently close; compare the segment itself.
689  double t1a, t2a;
690  if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
691  return false;
692  }
693 
694  if (t2a < 0.0 || t1a > 1.0) {
695  return false;
696  }
697 
698  t = max(t1a, 0.0);
699  return true;
700 }
701 
702 ////////////////////////////////////////////////////////////////////
703 // Function: CollisionSphere::compute_point
704 // Access: Protected
705 // Description: Returns a point on the surface of the sphere.
706 // latitude and longitude range from 0.0 to 1.0. This
707 // is used by fill_viz_geom() to create a visible
708 // representation of the sphere.
709 ////////////////////////////////////////////////////////////////////
710 LVertex CollisionSphere::
711 compute_point(PN_stdfloat latitude, PN_stdfloat longitude) const {
712  PN_stdfloat s1, c1;
713  csincos(latitude * MathNumbers::pi, &s1, &c1);
714 
715  PN_stdfloat s2, c2;
716  csincos(longitude * 2.0f * MathNumbers::pi, &s2, &c2);
717 
718  LVertex p(s1 * c2, s1 * s2, c1);
719  return p * get_radius() + get_center();
720 }
721 
722 ////////////////////////////////////////////////////////////////////
723 // Function: CollisionSphere::register_with_read_factory
724 // Access: Public, Static
725 // Description: Factory method to generate a CollisionSphere object
726 ////////////////////////////////////////////////////////////////////
729  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionSphere);
730 }
731 
732 ////////////////////////////////////////////////////////////////////
733 // Function: CollisionSphere::write_datagram
734 // Access: Public
735 // Description: Function to write the important information in
736 // the particular object to a Datagram
737 ////////////////////////////////////////////////////////////////////
740  CollisionSolid::write_datagram(manager, me);
741  _center.write_datagram(me);
742  me.add_stdfloat(_radius);
743 }
744 
745 ////////////////////////////////////////////////////////////////////
746 // Function: CollisionSphere::make_CollisionSphere
747 // Access: Protected
748 // Description: Factory method to generate a CollisionSphere object
749 ////////////////////////////////////////////////////////////////////
750 TypedWritable *CollisionSphere::
751 make_CollisionSphere(const FactoryParams &params) {
753  DatagramIterator scan;
754  BamReader *manager;
755 
756  parse_params(params, scan, manager);
757  me->fillin(scan, manager);
758  return me;
759 }
760 
761 ////////////////////////////////////////////////////////////////////
762 // Function: CollisionSphere::fillin
763 // Access: Protected
764 // Description: Function that reads out of the datagram (or asks
765 // manager to read) all of the data that is needed to
766 // re-create this object and stores it in the appropiate
767 // place
768 ////////////////////////////////////////////////////////////////////
769 void CollisionSphere::
770 fillin(DatagramIterator& scan, BamReader* manager) {
771  CollisionSolid::fillin(scan, manager);
772  _center.read_datagram(scan);
773  _radius = scan.get_stdfloat();
774 }
float length_squared() const
Returns the square of the vector&#39;s length, cheap and easy.
Definition: lvecBase3.h:749
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 PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
NodePath get_into_node_path() const
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
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.
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_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.
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.
bool almost_equal(const LVecBase3f &other, float threshold) const
Returns true if two vectors are memberwise equal within a specified tolerance.
Definition: lvecBase3.h:1280
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...
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.
const CollisionSolid * get_into() const
Returns the CollisionSolid pointer for the particular solid was collided into.
LVecBase3f xform_point(const LVecBase3f &v) const
The matrix transforms a 3-component point (including translation component) and returns the result...
Definition: lmatrix.h:1667
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
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
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 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
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
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.