Panda3D
Loading...
Searching...
No Matches
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
32PStatCollector CollisionInvSphere::_volume_pcollector("Collision Volumes:CollisionInvSphere");
33PStatCollector CollisionInvSphere::_test_pcollector("Collision Tests:CollisionInvSphere");
34TypeHandle CollisionInvSphere::_type_handle;
35
36/**
37 *
38 */
39CollisionSolid *CollisionInvSphere::
40make_copy() {
41 return new CollisionInvSphere(*this);
42}
43
44/**
45 *
46 */
47PT(CollisionEntry) CollisionInvSphere::
48test_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 */
74void CollisionInvSphere::
75output(std::ostream &out) const {
76 out << "invsphere, c (" << get_center() << "), r " << get_radius();
77}
78
79/**
80 *
81 */
82PT(BoundingVolume) CollisionInvSphere::
83compute_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 */
92PT(CollisionEntry) CollisionInvSphere::
93test_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 */
144PT(CollisionEntry) CollisionInvSphere::
145test_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 */
185PT(CollisionEntry) CollisionInvSphere::
186test_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 */
228PT(CollisionEntry) CollisionInvSphere::
229test_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 */
294PT(CollisionEntry) CollisionInvSphere::
295test_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 */
349PT(CollisionEntry) CollisionInvSphere::
350test_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 */
408void CollisionInvSphere::
409fill_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 */
461
462/**
463 * Factory method to generate a CollisionInvSphere object
464 */
465TypedWritable *CollisionInvSphere::
466make_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 */
481void CollisionInvSphere::
482fillin(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.
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
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.
An infinite ray, with a specific origin and direction.
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...
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.
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.
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.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.