Panda3D
collisionInvSphere.cxx
Go to the documentation of this file.
1 /**
2  * PANDA 3D SOFTWARE
3  * Copyright (c) Carnegie Mellon University. All rights reserved.
4  *
5  * All use of this software is subject to the terms of the revised BSD
6  * license. You should have received a copy of this license along
7  * with this source code in a file named "LICENSE."
8  *
9  * @file collisionInvSphere.cxx
10  * @author drose
11  * @date 2005-01-05
12  */
13 
14 #include "collisionInvSphere.h"
15 #include "collisionSphere.h"
16 #include "collisionLine.h"
17 #include "collisionRay.h"
18 #include "collisionSegment.h"
19 #include "collisionHandler.h"
20 #include "collisionEntry.h"
21 #include "config_collide.h"
22 #include "omniBoundingVolume.h"
23 #include "datagram.h"
24 #include "datagramIterator.h"
25 #include "bamReader.h"
26 #include "bamWriter.h"
27 #include "nearly_zero.h"
28 #include "geom.h"
29 #include "geomTristrips.h"
30 #include "geomVertexWriter.h"
31 
32 PStatCollector CollisionInvSphere::_volume_pcollector("Collision Volumes:CollisionInvSphere");
33 PStatCollector CollisionInvSphere::_test_pcollector("Collision Tests:CollisionInvSphere");
34 TypeHandle CollisionInvSphere::_type_handle;
35 
36 /**
37  *
38  */
39 CollisionSolid *CollisionInvSphere::
40 make_copy() {
41  return new CollisionInvSphere(*this);
42 }
43 
44 /**
45  *
46  */
47 PT(CollisionEntry) CollisionInvSphere::
48 test_intersection(const CollisionEntry &) const {
49  report_undefined_from_intersection(get_type());
50  return nullptr;
51 }
52 
53 /**
54  * Returns a PStatCollector that is used to count the number of bounding
55  * volume tests made against a solid of this type in a given frame.
56  */
59  return _volume_pcollector;
60 }
61 
62 /**
63  * Returns a PStatCollector that is used to count the number of intersection
64  * tests made against a solid of this type in a given frame.
65  */
68  return _test_pcollector;
69 }
70 
71 /**
72  *
73  */
74 void CollisionInvSphere::
75 output(std::ostream &out) const {
76  out << "invsphere, c (" << get_center() << "), r " << get_radius();
77 }
78 
79 /**
80  *
81  */
82 PT(BoundingVolume) CollisionInvSphere::
83 compute_internal_bounds() const {
84  // An inverse sphere always has an infinite bounding volume, since
85  // everything outside the sphere is solid matter.
86  return new OmniBoundingVolume();
87 }
88 
89 /**
90  *
91  */
92 PT(CollisionEntry) CollisionInvSphere::
93 test_intersection_from_sphere(const CollisionEntry &entry) const {
94  const CollisionSphere *sphere;
95  DCAST_INTO_R(sphere, entry.get_from(), nullptr);
96 
97  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
98 
99  LPoint3 from_center = sphere->get_center() * wrt_mat;
100  LVector3 from_radius_v =
101  LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
102  PN_stdfloat from_radius = length(from_radius_v);
103 
104  LPoint3 into_center = get_center();
105  PN_stdfloat into_radius = get_radius();
106 
107  LVector3 vec = from_center - into_center;
108  PN_stdfloat dist2 = dot(vec, vec);
109  if (dist2 < (into_radius - from_radius) * (into_radius - from_radius)) {
110  // No intersection--the sphere is within the hollow.
111  return nullptr;
112  }
113 
114  if (collide_cat.is_debug()) {
115  collide_cat.debug()
116  << "intersection detected from " << entry.get_from_node_path()
117  << " into " << entry.get_into_node_path() << "\n";
118  }
119  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
120 
121  LVector3 surface_normal;
122  PN_stdfloat vec_length = vec.length();
123  if (IS_NEARLY_ZERO(vec_length)) {
124  // If we don't have a collision normal (e.g. the centers are exactly
125  // coincident), then make up an arbitrary normal--any one is as good as
126  // any other.
127  surface_normal.set(1.0, 0.0, 0.0);
128  } else {
129  surface_normal = vec / -vec_length;
130  }
131 
132  LVector3 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
133 
134  new_entry->set_surface_normal(normal);
135  new_entry->set_surface_point(into_center - surface_normal * into_radius);
136  new_entry->set_interior_point(from_center - surface_normal * from_radius);
137 
138  return new_entry;
139 }
140 
141 /**
142  *
143  */
144 PT(CollisionEntry) CollisionInvSphere::
145 test_intersection_from_line(const CollisionEntry &entry) const {
146  const CollisionLine *line;
147  DCAST_INTO_R(line, entry.get_from(), nullptr);
148 
149  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
150 
151  LPoint3 from_origin = line->get_origin() * wrt_mat;
152  LVector3 from_direction = line->get_direction() * wrt_mat;
153 
154  double t1, t2;
155  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
156  // The line is in the middle of space, and therefore intersects the
157  // sphere.
158  t1 = t2 = 0.0;
159  }
160 
161  if (collide_cat.is_debug()) {
162  collide_cat.debug()
163  << "intersection detected from " << entry.get_from_node_path()
164  << " into " << entry.get_into_node_path() << "\n";
165  }
166  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
167 
168  LPoint3 into_intersection_point = from_origin + t2 * from_direction;
169  new_entry->set_surface_point(into_intersection_point);
170 
172  new_entry->set_surface_normal(get_effective_normal());
173  } else {
174  LVector3 normal = into_intersection_point - get_center();
175  normal.normalize();
176  new_entry->set_surface_normal(-normal);
177  }
178 
179  return new_entry;
180 }
181 
182 /**
183  *
184  */
185 PT(CollisionEntry) CollisionInvSphere::
186 test_intersection_from_ray(const CollisionEntry &entry) const {
187  const CollisionRay *ray;
188  DCAST_INTO_R(ray, entry.get_from(), nullptr);
189 
190  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
191 
192  LPoint3 from_origin = ray->get_origin() * wrt_mat;
193  LVector3 from_direction = ray->get_direction() * wrt_mat;
194 
195  double t1, t2;
196  if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
197  // The ray is in the middle of space, and therefore intersects the sphere.
198  t1 = t2 = 0.0;
199  }
200 
201  t2 = std::max(t2, 0.0);
202 
203  if (collide_cat.is_debug()) {
204  collide_cat.debug()
205  << "intersection detected from " << entry.get_from_node_path()
206  << " into " << entry.get_into_node_path() << "\n";
207  }
208  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
209 
210  LPoint3 into_intersection_point;
211  into_intersection_point = from_origin + t2 * from_direction;
212  new_entry->set_surface_point(into_intersection_point);
213 
215  new_entry->set_surface_normal(get_effective_normal());
216  } else {
217  LVector3 normal = into_intersection_point - get_center();
218  normal.normalize();
219  new_entry->set_surface_normal(-normal);
220  }
221 
222  return new_entry;
223 }
224 
225 /**
226  *
227  */
228 PT(CollisionEntry) CollisionInvSphere::
229 test_intersection_from_segment(const CollisionEntry &entry) const {
230  const CollisionSegment *segment;
231  DCAST_INTO_R(segment, entry.get_from(), nullptr);
232 
233  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
234 
235  LPoint3 from_a = segment->get_point_a() * wrt_mat;
236  LPoint3 from_b = segment->get_point_b() * wrt_mat;
237  LVector3 from_direction = from_b - from_a;
238 
239  double t1, t2;
240  if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
241  // The segment is in the middle of space, and therefore intersects the
242  // sphere.
243  t1 = t2 = 0.0;
244  }
245 
246  double t;
247  if (t2 <= 0.0) {
248  // The segment is completely below the shell.
249  t = 0.0;
250 
251  } else if (t1 >= 1.0) {
252  // The segment is completely above the shell.
253  t = 1.0;
254 
255  } else if (t2 <= 1.0) {
256  // The bottom edge of the segment intersects the shell.
257  t = std::min(t2, 1.0);
258 
259  } else if (t1 >= 0.0) {
260  // The top edge of the segment intersects the shell.
261  t = std::max(t1, 0.0);
262 
263  } else {
264  // Neither edge of the segment intersects the shell. It follows that both
265  // intersection points are within the hollow center of the sphere;
266  // therefore, there is no intersection.
267  return nullptr;
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_a + t * 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  *
293  */
294 PT(CollisionEntry) CollisionInvSphere::
295 test_intersection_from_capsule(const CollisionEntry &entry) const {
296  const CollisionCapsule *capsule;
297  DCAST_INTO_R(capsule, entry.get_from(), nullptr);
298 
299  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
300 
301  LPoint3 from_a = capsule->get_point_a() * wrt_mat;
302  LPoint3 from_b = capsule->get_point_b() * wrt_mat;
303 
304  LVector3 from_radius_v =
305  LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
306  PN_stdfloat from_radius = from_radius_v.length();
307 
308  LPoint3 center = get_center();
309  PN_stdfloat radius = get_radius();
310 
311  // Check which one of the points lies furthest inside the sphere.
312  PN_stdfloat dist_a = (from_a - center).length();
313  PN_stdfloat dist_b = (from_b - center).length();
314  if (dist_b > dist_a) {
315  // Store the furthest point into from_a/dist_a.
316  dist_a = dist_b;
317  from_a = from_b;
318  }
319 
320  // from_a now contains the furthest point. Is it inside?
321  if (dist_a < radius - from_radius) {
322  return nullptr;
323  }
324 
325  if (collide_cat.is_debug()) {
326  collide_cat.debug()
327  << "intersection detected from " << entry.get_from_node_path()
328  << " into " << entry.get_into_node_path() << "\n";
329  }
330  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
331 
332  LVector3 normal = center - from_a;
333  normal.normalize();
334  new_entry->set_surface_point(get_center() - normal * radius);
335  new_entry->set_interior_point(from_a - normal * from_radius);
336 
338  new_entry->set_surface_normal(get_effective_normal());
339  } else {
340  new_entry->set_surface_normal(normal);
341  }
342 
343  return new_entry;
344 }
345 
346 /**
347  * Double dispatch point for box as a FROM object
348  */
349 PT(CollisionEntry) CollisionInvSphere::
350 test_intersection_from_box(const CollisionEntry &entry) const {
351  const CollisionBox *box;
352  DCAST_INTO_R(box, entry.get_from(), nullptr);
353 
354  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
355 
356  LPoint3 center = get_center();
357  PN_stdfloat radius_sq = get_radius();
358  radius_sq *= radius_sq;
359 
360  // Just figure out which box point is furthest from the center. If it
361  // exceeds the radius, the furthest point wins.
362 
363  PN_stdfloat max_dist_sq = -1.0;
364  LPoint3 deepest_vertex;
365 
366  for (int i = 0; i < 8; ++i) {
367  LPoint3 point = wrt_mat.xform_point(box->get_point(i));
368 
369  PN_stdfloat dist_sq = (point - center).length_squared();
370  if (dist_sq > max_dist_sq) {
371  deepest_vertex = point;
372  max_dist_sq = dist_sq;
373  }
374  }
375 
376  if (max_dist_sq < radius_sq) {
377  // The point furthest away from the center is still inside the sphere.
378  // Therefore, no collision.
379  return nullptr;
380  }
381 
382  if (collide_cat.is_debug()) {
383  collide_cat.debug()
384  << "intersection detected from " << entry.get_from_node_path()
385  << " into " << entry.get_into_node_path() << "\n";
386  }
387 
388  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
389 
390  // The interior point is just the deepest cube vertex.
391  new_entry->set_interior_point(deepest_vertex);
392 
393  // Now extrapolate the surface point and normal from that.
394  LVector3 normal = center - deepest_vertex;
395  normal.normalize();
396  new_entry->set_surface_point(center - normal * get_radius());
397  new_entry->set_surface_normal(
399  ? get_effective_normal() : normal);
400 
401  return new_entry;
402 }
403 
404 /**
405  * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
406  * solid.
407  */
408 void CollisionInvSphere::
409 fill_viz_geom() {
410  if (collide_cat.is_debug()) {
411  collide_cat.debug()
412  << "Recomputing viz for " << *this << "\n";
413  }
414 
415  static const int num_slices = 16;
416  static const int num_stacks = 8;
417 
418  PT(GeomVertexData) vdata = new GeomVertexData
419  ("collision", GeomVertexFormat::get_v3(),
420  Geom::UH_static);
421  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
422 
423  PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
424  for (int sl = 0; sl < num_slices; ++sl) {
425  PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
426  PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
427  vertex.add_data3(compute_point(0.0, longitude0));
428  for (int st = 1; st < num_stacks; ++st) {
429  PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
430  vertex.add_data3(compute_point(latitude, longitude1));
431  vertex.add_data3(compute_point(latitude, longitude0));
432  }
433  vertex.add_data3(compute_point(1.0, longitude0));
434 
435  strip->add_next_vertices(num_stacks * 2);
436  strip->close_primitive();
437  }
438 
439  PT(Geom) geom = new Geom(vdata);
440  geom->add_primitive(strip);
441 
442  _viz_geom->add_geom(geom, get_solid_viz_state());
443 }
444 
445 /**
446  * Factory method to generate a CollisionInvSphere object
447  */
450  BamReader::get_factory()->register_factory(get_class_type(), make_CollisionInvSphere);
451 }
452 
453 /**
454  * Function to write the important information in the particular object to a
455  * Datagram
456  */
458 write_datagram(BamWriter *manager, Datagram &me) {
459  CollisionSphere::write_datagram(manager, me);
460 }
461 
462 /**
463  * Factory method to generate a CollisionInvSphere object
464  */
465 TypedWritable *CollisionInvSphere::
466 make_CollisionInvSphere(const FactoryParams &params) {
468  DatagramIterator scan;
469  BamReader *manager;
470 
471  parse_params(params, scan, manager);
472  me->fillin(scan, manager);
473  return me;
474 }
475 
476 /**
477  * Function that reads out of the datagram (or asks manager to read) all of
478  * the data that is needed to re-create this object and stores it in the
479  * appropiate place
480  */
481 void CollisionInvSphere::
482 fillin(DatagramIterator& scan, BamReader* manager) {
483  CollisionSphere::fillin(scan, manager);
484 }
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
void parse_params(const FactoryParams &params, DatagramIterator &scan, BamReader *&manager)
Takes in a FactoryParams, passed from a WritableFactory into any TypedWritable's make function,...
Definition: bamReader.I:275
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:110
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition: bamReader.I:177
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
Definition: bamWriter.h:63
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
A cuboid collision volume or object.
Definition: collisionBox.h:27
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
Definition: collisionBox.I:150
This implements a solid consisting of a cylinder with hemispherical endcaps, also known as a capsule ...
Defines a single collision event.
get_from_node_path
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
get_into_node_path
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
get_from
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
An inverted sphere: this is a sphere whose collision surface is the inside surface of the sphere.
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
static void register_with_read_factory()
Factory method to generate a CollisionInvSphere object.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
An infinite line, similar to a CollisionRay, except that it extends in both directions.
Definition: collisionLine.h:25
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:27
A finite line segment, with two specific endpoints but no thickness.
The abstract base class for all things that can collide with other things in the world,...
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
get_respect_effective_normal
See set_respect_effective_normal().
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.
A class to retrieve the individual data elements previously stored in a Datagram.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition: datagram.h:38
An instance of this class is passed to the Factory when requesting it to do its business and construc...
Definition: factoryParams.h:36
void register_factory(TypeHandle handle, CreateFunc *func, void *user_data=nullptr)
Registers a new kind of thing the Factory will be able to create.
Definition: factory.I:73
Defines a series of triangle strips.
Definition: geomTristrips.h:23
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
static const GeomVertexFormat * get_v3()
Returns a standard vertex format with just a 3-component vertex position.
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
A container for geometry primitives.
Definition: geom.h:54
This is a special kind of GeometricBoundingVolume that fills all of space.
A lightweight class that represents a single element that may be timed and/or counted via stats.
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:81
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:35
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PT(CollisionEntry) CollisionInvSphere
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.