Panda3D
Loading...
Searching...
No Matches
collisionCapsule.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 collisionCapsule.cxx
10 * @author drose
11 * @date 2003-09-25
12 */
13
14#include "collisionCapsule.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 "collisionParabola.h"
22#include "config_collide.h"
23#include "look_at.h"
24#include "geom.h"
25#include "geomNode.h"
27#include "datagram.h"
28#include "datagramIterator.h"
29#include "bamReader.h"
30#include "bamWriter.h"
31#include "cmath.h"
32#include "transformState.h"
33#include "geom.h"
34#include "geomTristrips.h"
35#include "geomVertexWriter.h"
36#include "boundingSphere.h"
37
38PStatCollector CollisionCapsule::_volume_pcollector("Collision Volumes:CollisionCapsule");
39PStatCollector CollisionCapsule::_test_pcollector("Collision Tests:CollisionCapsule");
40TypeHandle CollisionCapsule::_type_handle;
41
42/**
43 *
44 */
45CollisionSolid *CollisionCapsule::
46make_copy() {
47 return new CollisionCapsule(*this);
48}
49
50/**
51 *
52 */
53PT(CollisionEntry) CollisionCapsule::
54test_intersection(const CollisionEntry &entry) const {
55 return entry.get_into()->test_intersection_from_capsule(entry);
56}
57
58/**
59 * Transforms the solid by the indicated matrix.
60 */
61void CollisionCapsule::
62xform(const LMatrix4 &mat) {
63 _a = _a * mat;
64 _b = _b * mat;
65
66 // This is a little cheesy and fails miserably in the presence of a non-
67 // uniform scale.
68 LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
69 _radius = length(radius_v);
70
71 recalc_internals();
72 CollisionSolid::xform(mat);
73}
74
75/**
76 * Returns the point in space deemed to be the "origin" of the solid for
77 * collision purposes. The closest intersection point to this origin point is
78 * considered to be the most significant.
79 */
82 return get_point_a();
83}
84
85/**
86 * Returns a PStatCollector that is used to count the number of bounding
87 * volume tests made against a solid of this type in a given frame.
88 */
91 return _volume_pcollector;
92}
93
94/**
95 * Returns a PStatCollector that is used to count the number of intersection
96 * tests made against a solid of this type in a given frame.
97 */
100 return _test_pcollector;
101}
102
103/**
104 *
105 */
106void CollisionCapsule::
107output(std::ostream &out) const {
108 out << "capsule, a (" << _a << "), b (" << _b << "), r " << _radius;
109}
110
111/**
112 *
113 */
114PT(BoundingVolume) CollisionCapsule::
115compute_internal_bounds() const {
116 PT(BoundingVolume) bound = CollisionSolid::compute_internal_bounds();
117
118 if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
120 DCAST_INTO_R(gbound, bound, bound);
121
122 LVector3 vec = (_b - _a);
123 if (vec.normalize()) {
124 // The bounding volume includes both endpoints, plus a little bit more
125 // to include the radius in both directions.
126 LPoint3 points[2];
127 points[0] = _a - vec * _radius;
128 points[1] = _b + vec * _radius;
129
130 gbound->around(points, points + 2);
131
132 } else {
133 // Both endpoints are coincident; therefore, the bounding volume is a
134 // sphere.
135 BoundingSphere sphere(_a, _radius);
136 gbound->extend_by(&sphere);
137 }
138 }
139
140 return bound;
141}
142
143/**
144 *
145 */
146PT(CollisionEntry) CollisionCapsule::
147test_intersection_from_sphere(const CollisionEntry &entry) const {
148 const CollisionSphere *sphere;
149 DCAST_INTO_R(sphere, entry.get_from(), nullptr);
150
151 CPT(TransformState) wrt_space = entry.get_wrt_space();
152 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
153
154 const LMatrix4 &wrt_mat = wrt_space->get_mat();
155
156 LPoint3 from_a = sphere->get_center() * wrt_mat;
157 LPoint3 from_b = from_a;
158
159 LPoint3 contact_point;
160 PN_stdfloat actual_t = 0.0f;
161
162 if (wrt_prev_space != wrt_space) {
163 // If the sphere is moving relative to the capsule, it becomes a capsule itself.
164 from_a = sphere->get_center() * wrt_prev_space->get_mat();
165 }
166
167 LVector3 from_direction = from_b - from_a;
168
169 LVector3 from_radius_v =
170 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
171 PN_stdfloat from_radius = length(from_radius_v);
172
173 double t1, t2;
174 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
175 // No intersection.
176 return nullptr;
177 }
178
179 if (t2 < 0.0 || t1 > 1.0) {
180 // Both intersection points are before the start of the segment or after
181 // the end of the segment.
182 return nullptr;
183 }
184
185 // doubles, not floats, to satisfy min and max templates.
186 actual_t = std::min(1.0, std::max(0.0, t1));
187 contact_point = from_a + actual_t * (from_b - from_a);
188
189 if (collide_cat.is_debug()) {
190 collide_cat.debug()
191 << "intersection detected from " << entry.get_from_node_path() << " into "
192 << entry.get_into_node_path() << "\n";
193 }
194 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
195
196 LPoint3 into_intersection_point;
197 if (t2 > 1.0) {
198 // Point b is within the capsule. The first intersection point is point b
199 // itself.
200 into_intersection_point = from_b;
201 } else {
202 // Point b is outside the capsule, and point a is either inside the capsule or
203 // beyond it. The first intersection point is at t2.
204 into_intersection_point = from_a + t2 * from_direction;
205 }
206 set_intersection_point(new_entry, into_intersection_point, from_radius);
207
208 LPoint3 fake_contact_point;
209 LVector3 contact_normal;
210 calculate_surface_point_and_normal(contact_point,
211 from_radius,
212 fake_contact_point,
213 contact_normal);
214 new_entry->set_contact_pos(contact_point);
215 new_entry->set_contact_normal(contact_normal);
216 new_entry->set_t(actual_t);
217
218 return new_entry;
219}
220
221/**
222 *
223 */
224PT(CollisionEntry) CollisionCapsule::
225test_intersection_from_line(const CollisionEntry &entry) const {
226 const CollisionLine *line;
227 DCAST_INTO_R(line, entry.get_from(), nullptr);
228
229 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
230
231 LPoint3 from_origin = line->get_origin() * wrt_mat;
232 LVector3 from_direction = line->get_direction() * wrt_mat;
233
234 double t1, t2;
235 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
236 // No intersection.
237 return nullptr;
238 }
239
240 if (collide_cat.is_debug()) {
241 collide_cat.debug()
242 << "intersection detected from " << entry.get_from_node_path()
243 << " into " << entry.get_into_node_path() << "\n";
244 }
245 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
246
247 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
248 set_intersection_point(new_entry, into_intersection_point, 0.0);
249
251 new_entry->set_surface_normal(get_effective_normal());
252
253 } else {
254 LVector3 normal = into_intersection_point * _inv_mat;
255 if (normal[1] > _length) {
256 // The point is within the top endcap.
257 normal[1] -= _length;
258 } else if (normal[1] > 0.0f) {
259 // The point is within the cylinder body.
260 normal[1] = 0;
261 }
262 normal = normalize(normal * _mat);
263 new_entry->set_surface_normal(normal);
264 }
265
266 return new_entry;
267}
268
269/**
270 *
271 */
272PT(CollisionEntry) CollisionCapsule::
273test_intersection_from_ray(const CollisionEntry &entry) const {
274 const CollisionRay *ray;
275 DCAST_INTO_R(ray, entry.get_from(), nullptr);
276
277 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
278
279 LPoint3 from_origin = ray->get_origin() * wrt_mat;
280 LVector3 from_direction = ray->get_direction() * wrt_mat;
281
282 double t1, t2;
283 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
284 // No intersection.
285 return nullptr;
286 }
287
288 if (t2 < 0.0) {
289 // Both intersection points are before the start of the ray.
290 return nullptr;
291 }
292
293 if (collide_cat.is_debug()) {
294 collide_cat.debug()
295 << "intersection detected from " << entry.get_from_node_path()
296 << " into " << entry.get_into_node_path() << "\n";
297 }
298 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
299
300 LPoint3 into_intersection_point;
301 if (t1 < 0.0) {
302 // Point a is within the capsule. The first intersection point is point a
303 // itself.
304 into_intersection_point = from_origin;
305 } else {
306 // Point a is outside the capsule. The first intersection point is at t1.
307 into_intersection_point = from_origin + t1 * from_direction;
308 }
309 set_intersection_point(new_entry, into_intersection_point, 0.0);
310
312 new_entry->set_surface_normal(get_effective_normal());
313
314 } else {
315 LVector3 normal = into_intersection_point * _inv_mat;
316 if (normal[1] > _length) {
317 // The point is within the top endcap.
318 normal[1] -= _length;
319 } else if (normal[1] > 0.0f) {
320 // The point is within the cylinder body.
321 normal[1] = 0;
322 }
323 normal = normalize(normal * _mat);
324 new_entry->set_surface_normal(normal);
325 }
326
327 return new_entry;
328}
329
330/**
331 *
332 */
333PT(CollisionEntry) CollisionCapsule::
334test_intersection_from_segment(const CollisionEntry &entry) const {
335 const CollisionSegment *segment;
336 DCAST_INTO_R(segment, entry.get_from(), nullptr);
337
338 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
339
340 LPoint3 from_a = segment->get_point_a() * wrt_mat;
341 LPoint3 from_b = segment->get_point_b() * wrt_mat;
342 LVector3 from_direction = from_b - from_a;
343
344 double t1, t2;
345 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
346 // No intersection.
347 return nullptr;
348 }
349
350 if (t2 < 0.0 || t1 > 1.0) {
351 // Both intersection points are before the start of the segment or after
352 // the end of the segment.
353 return nullptr;
354 }
355
356 if (collide_cat.is_debug()) {
357 collide_cat.debug()
358 << "intersection detected from " << entry.get_from_node_path()
359 << " into " << entry.get_into_node_path() << "\n";
360 }
361 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
362
363 LPoint3 into_intersection_point;
364 if (t1 < 0.0) {
365 // Point a is within the capsule. The first intersection point is point a
366 // itself.
367 into_intersection_point = from_a;
368 } else {
369 // Point a is outside the capsule, and point b is either inside the capsule or
370 // beyond it. The first intersection point is at t1.
371 into_intersection_point = from_a + t1 * from_direction;
372 }
373 set_intersection_point(new_entry, into_intersection_point, 0.0);
374
376 new_entry->set_surface_normal(get_effective_normal());
377
378 } else {
379 LVector3 normal = into_intersection_point * _inv_mat;
380 if (normal[1] > _length) {
381 // The point is within the top endcap.
382 normal[1] -= _length;
383 } else if (normal[1] > 0.0f) {
384 // The point is within the cylinder body.
385 normal[1] = 0;
386 }
387 normal = normalize(normal * _mat);
388 new_entry->set_surface_normal(normal);
389 }
390
391 return new_entry;
392}
393
394/**
395 *
396 */
397PT(CollisionEntry) CollisionCapsule::
398test_intersection_from_capsule(const CollisionEntry &entry) const {
399 const CollisionCapsule *capsule;
400 DCAST_INTO_R(capsule, entry.get_from(), nullptr);
401
402 LPoint3 into_a = _a;
403 LVector3 into_direction = _b - into_a;
404
405 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
406
407 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
408 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
409 LVector3 from_direction = from_b - from_a;
410
411 LVector3 from_radius_v =
412 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
413 PN_stdfloat from_radius = length(from_radius_v);
414
415 // Determine the points on each segment with the smallest distance between.
416 double into_t, from_t;
417 calc_closest_segment_points(into_t, from_t,
418 into_a, into_direction,
419 from_a, from_direction);
420 LPoint3 into_closest = into_a + into_direction * into_t;
421 LPoint3 from_closest = from_a + from_direction * from_t;
422
423 // If the distance is greater than the sum of capsule radii, the test fails.
424 LVector3 closest_vec = from_closest - into_closest;
425 PN_stdfloat distance = closest_vec.length();
426 if (distance > _radius + from_radius) {
427 return nullptr;
428 }
429
430 if (collide_cat.is_debug()) {
431 collide_cat.debug()
432 << "intersection detected from " << entry.get_from_node_path()
433 << " into " << entry.get_into_node_path() << "\n";
434 }
435 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
436
437 if (distance != 0) {
438 // This is the most common case, where the line segments don't touch
439 // exactly. We point the normal along the vector of the closest distance.
440 LVector3 surface_normal = closest_vec * (1.0 / distance);
441
442 new_entry->set_surface_point(into_closest + surface_normal * _radius);
443 new_entry->set_interior_point(from_closest - surface_normal * from_radius);
444
446 new_entry->set_surface_normal(get_effective_normal());
447 } else if (distance != 0) {
448 new_entry->set_surface_normal(surface_normal);
449 }
450 } else {
451 // The rare case of the line segments touching exactly.
452 set_intersection_point(new_entry, into_closest, 0);
453 }
454
455 return new_entry;
456}
457
458/**
459 *
460 */
461PT(CollisionEntry) CollisionCapsule::
462test_intersection_from_parabola(const CollisionEntry &entry) const {
463 const CollisionParabola *parabola;
464 DCAST_INTO_R(parabola, entry.get_from(), nullptr);
465
466 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
467
468 // Convert the parabola into local coordinate space.
469 LParabola local_p(parabola->get_parabola());
470 local_p.xform(wrt_mat);
471
472 double t;
473 if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
474 local_p.calc_point(parabola->get_t1()),
475 local_p.calc_point(parabola->get_t2()))) {
476 // No intersection.
477 return nullptr;
478 }
479
480 if (collide_cat.is_debug()) {
481 collide_cat.debug()
482 << "intersection detected from " << entry.get_from_node_path()
483 << " into " << entry.get_into_node_path() << "\n";
484 }
485 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
486
487 LPoint3 into_intersection_point = local_p.calc_point(t);
488 set_intersection_point(new_entry, into_intersection_point, 0.0);
489
491 new_entry->set_surface_normal(get_effective_normal());
492
493 } else {
494 LVector3 normal = into_intersection_point * _inv_mat;
495 if (normal[1] > _length) {
496 // The point is within the top endcap.
497 normal[1] -= _length;
498 } else if (normal[1] > 0.0f) {
499 // The point is within the cylinder body.
500 normal[1] = 0;
501 }
502 normal = normalize(normal * _mat);
503 new_entry->set_surface_normal(normal);
504 }
505
506 return new_entry;
507}
508
509/**
510 * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
511 * solid.
512 */
513void CollisionCapsule::
514fill_viz_geom() {
515 if (collide_cat.is_debug()) {
516 collide_cat.debug()
517 << "Recomputing viz for " << *this << "\n";
518 }
519
520 // Generate the vertices such that we draw a capsule with one endpoint at (0,
521 // 0, 0), and another at (0, length, 0). Then we'll rotate and translate it
522 // into place with the appropriate look_at matrix.
523 LVector3 direction = (_b - _a);
524 PN_stdfloat length = direction.length();
525
526 PT(GeomVertexData) vdata = new GeomVertexData
527 ("collision", GeomVertexFormat::get_v3(),
528 Geom::UH_static);
529 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
530
531 PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
532 // Generate the first endcap.
533 static const int num_slices = 8;
534 static const int num_rings = 4;
535 int ri, si;
536 for (ri = 0; ri < num_rings; ri++) {
537 for (si = 0; si <= num_slices; si++) {
538 vertex.add_data3(calc_sphere1_vertex(ri, si, num_rings, num_slices));
539 vertex.add_data3(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
540 }
541 strip->add_next_vertices((num_slices + 1) * 2);
542 strip->close_primitive();
543 }
544
545 // Now the cylinder sides.
546 for (si = 0; si <= num_slices; si++) {
547 vertex.add_data3(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
548 vertex.add_data3(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
549 length));
550 }
551 strip->add_next_vertices((num_slices + 1) * 2);
552 strip->close_primitive();
553
554 // And the second endcap.
555 for (ri = num_rings - 1; ri >= 0; ri--) {
556 for (si = 0; si <= num_slices; si++) {
557 vertex.add_data3(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
558 vertex.add_data3(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
559 }
560 strip->add_next_vertices((num_slices + 1) * 2);
561 strip->close_primitive();
562 }
563
564 PT(Geom) geom = new Geom(vdata);
565 geom->add_primitive(strip);
566
567 // Now transform the vertices to their actual location.
568 LMatrix4 mat;
569 look_at(mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
570 mat.set_row(3, _a);
571 geom->transform_vertices(mat);
572
573 _viz_geom->add_geom(geom, get_solid_viz_state());
574 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
575}
576
577/**
578 * Should be called internally to recompute the matrix and length when the
579 * properties of the capsule have changed.
580 */
581void CollisionCapsule::
582recalc_internals() {
583 LVector3 direction = (_b - _a);
584 _length = direction.length();
585
586 look_at(_mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
587 _mat.set_row(3, _a);
588 _inv_mat.invert_from(_mat);
589
590 mark_viz_stale();
591 mark_internal_bounds_stale();
592}
593
594/**
595 * Calculates a particular vertex on the surface of the first endcap
596 * hemisphere, for use in generating the viz geometry.
597 */
598LVertex CollisionCapsule::
599calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) {
600 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
601 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
602
603 // Find the point on the rim, based on the slice.
604 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
605 PN_stdfloat x_rim = ccos(theta);
606 PN_stdfloat z_rim = csin(theta);
607
608 // Now pull that point in towards the pole, based on the ring.
609 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
610 PN_stdfloat to_pole = csin(phi);
611
612 PN_stdfloat x = _radius * x_rim * to_pole;
613 PN_stdfloat y = -_radius * ccos(phi);
614 PN_stdfloat z = _radius * z_rim * to_pole;
615
616 return LVertex(x, y, z);
617}
618
619/**
620 * Calculates a particular vertex on the surface of the second endcap
621 * hemisphere, for use in generating the viz geometry.
622 */
623LVertex CollisionCapsule::
624calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
625 PN_stdfloat length) {
626 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
627 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
628
629 // Find the point on the rim, based on the slice.
630 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
631 PN_stdfloat x_rim = ccos(theta);
632 PN_stdfloat z_rim = csin(theta);
633
634 // Now pull that point in towards the pole, based on the ring.
635 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
636 PN_stdfloat to_pole = csin(phi);
637
638 PN_stdfloat x = _radius * x_rim * to_pole;
639 PN_stdfloat y = length + _radius * ccos(phi);
640 PN_stdfloat z = _radius * z_rim * to_pole;
641
642 return LVertex(x, y, z);
643}
644
645/**
646 * Given line segments s1 and s2 defined by two points each, computes the
647 * point on each segment with the closest distance between them.
648 */
649void CollisionCapsule::
650calc_closest_segment_points(double &t1, double &t2,
651 const LPoint3 &from1, const LVector3 &delta1,
652 const LPoint3 &from2, const LVector3 &delta2) {
653 // Copyright 2001 softSurfer, 2012 Dan Sunday
654 // This code may be freely used, distributed and modified for any purpose
655 // providing that this copyright notice is included with it.
656 // SoftSurfer makes no warranty for this code, and cannot be held
657 // liable for any real or imagined damage resulting from its use.
658 // Users of this code must verify correctness for their application.
659 LVector3 w = from1 - from2;
660 PN_stdfloat a = delta1.dot(delta1); // always >= 0
661 PN_stdfloat b = delta1.dot(delta2);
662 PN_stdfloat c = delta2.dot(delta2); // always >= 0
663 PN_stdfloat d = delta1.dot(w);
664 PN_stdfloat e = delta2.dot(w);
665 PN_stdfloat D = a * c - b * b; // always >= 0
666 PN_stdfloat sN, sD = D;
667 PN_stdfloat tN, tD = D;
668
669 // compute the line parameters of the two closest points
670 if (IS_NEARLY_ZERO(D)) { // the lines are almost parallel
671 sN = 0.0; // force using point P0 on segment S1
672 sD = 1.0; // to prevent possible division by 0.0 later
673 tN = e;
674 tD = c;
675 } else {
676 // get the closest points on the infinite lines
677 sN = (b*e - c*d);
678 tN = (a*e - b*d);
679 if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
680 sN = 0.0;
681 tN = e;
682 tD = c;
683 } else if (sN > sD) { // sc > 1 => the s=1 edge is visible
684 sN = sD;
685 tN = e + b;
686 tD = c;
687 }
688 }
689
690 if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
691 tN = 0.0;
692 // recompute sc for this edge
693 if (-d < 0.0) {
694 sN = 0.0;
695 } else if (-d > a) {
696 sN = sD;
697 } else {
698 sN = -d;
699 sD = a;
700 }
701 } else if (tN > tD) { // tc > 1 => the t=1 edge is visible
702 tN = tD;
703 // recompute sc for this edge
704 if ((-d + b) < 0.0) {
705 sN = 0;
706 } else if ((-d + b) > a) {
707 sN = sD;
708 } else {
709 sN = (-d + b);
710 sD = a;
711 }
712 }
713
714 // finally do the division to get sc and tc
715 t1 = (IS_NEARLY_ZERO(sN) ? 0.0 : sN / sD);
716 t2 = (IS_NEARLY_ZERO(tN) ? 0.0 : tN / tD);
717}
718
719/**
720 * Determine the point(s) of intersection of a parametric line with the capsule.
721 * The line is infinite in both directions, and passes through "from" and
722 * from+delta. If the line does not intersect the capsule, the function returns
723 * false, and t1 and t2 are undefined. If it does intersect the capsule, it
724 * returns true, and t1 and t2 are set to the points along the equation
725 * from+t*delta that correspond to the two points of intersection.
726 */
727bool CollisionCapsule::
728intersects_line(double &t1, double &t2,
729 const LPoint3 &from0, const LVector3 &delta0,
730 PN_stdfloat inflate_radius) const {
731 // Convert the line into our canonical coordinate space: the capsule is aligned
732 // with the y axis.
733 LPoint3 from = from0 * _inv_mat;
734 LVector3 delta = delta0 * _inv_mat;
735
736 PN_stdfloat radius = _radius + inflate_radius;
737
738 // Now project the line into the X-Z plane to test for intersection with a
739 // 2-d circle around the origin. The equation for this is very similar to
740 // the formula for the intersection of a line with a sphere; see
741 // CollisionSphere::intersects_line() for the complete derivation. It's a
742 // little bit simpler because the circle is centered on the origin.
743 LVector2 from2(from[0], from[2]);
744 LVector2 delta2(delta[0], delta[2]);
745
746 double A = dot(delta2, delta2);
747
748 if (IS_NEARLY_ZERO(A)) {
749 // If the delta2 is 0, the line is perpendicular to the X-Z plane. The
750 // whole line intersects with the infinite cylinder if the point is within
751 // the circle.
752 if (from2.dot(from2) > radius * radius) {
753 // Nope, the 2-d point is outside the circle, so no intersection.
754 return false;
755 }
756
757 if (IS_NEARLY_ZERO(delta[1])) {
758 // Actually, the whole delta vector is 0, so the line is just a point.
759 // In this case, (since we have already shown the point is within the
760 // infinite cylinder), we intersect if and only if the three-dimensional
761 // point is between the endcaps.
762 if (from[1] < -radius || from[1] > _length + radius) {
763 // Way out.
764 return false;
765 }
766 if (from[1] < 0.0f) {
767 // Possibly within the first endcap.
768 if (from.dot(from) > radius * radius) {
769 return false;
770 }
771 } else if (from[1] > _length) {
772 // Possibly within the second endcap.
773 from[1] -= _length;
774 if (from.dot(from) > radius * radius) {
775 return false;
776 }
777 }
778
779 // The point is within the capsule!
780 t1 = t2 = 0.0;
781 return true;
782 }
783
784 // The 2-d point is within the circle, so compute our intersection points
785 // to include the entire vertical slice of the cylinder.
786 t1 = (-radius - from[1]) / delta[1];
787 t2 = (_length + radius - from[1]) / delta[1];
788
789 } else {
790 // The line is not perpendicular to the X-Z plane, so its projection into
791 // the plane is 2-d line. Test that 2-d line for intersection with the
792 // circular projection of the cylinder.
793
794 double B = 2.0f * dot(delta2, from2);
795 double fc_d2 = dot(from2, from2);
796 double C = fc_d2 - radius * radius;
797
798 double radical = B*B - 4.0*A*C;
799
800 if (IS_NEARLY_ZERO(radical)) {
801 // Tangent.
802 t1 = t2 = -B / (2.0*A);
803
804 } else if (radical < 0.0) {
805 // No real roots: no intersection with the line.
806 return false;
807
808 } else {
809 double reciprocal_2A = 1.0 / (2.0 * A);
810 double sqrt_radical = sqrtf(radical);
811 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
812 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
813 }
814 }
815
816 // Now we need to verify that the intersection points fall within the length
817 // of the cylinder.
818 PN_stdfloat t1_y = from[1] + t1 * delta[1];
819 PN_stdfloat t2_y = from[1] + t2 * delta[1];
820
821 if (t1_y < -radius && t2_y < -radius) {
822 // Both points are way off the bottom of the capsule; no intersection.
823 return false;
824 } else if (t1_y > _length + radius && t2_y > _length + radius) {
825 // Both points are way off the top of the capsule; no intersection.
826 return false;
827 }
828
829 if (t1_y < 0.0f) {
830 // The starting point is off the bottom of the capsule. Test the line
831 // against the first endcap.
832 double t1a, t2a;
833 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, radius)) {
834 // If there's no intersection with the endcap, there can't be an
835 // intersection with the cylinder.
836 return false;
837 }
838 t1 = t1a;
839
840 } else if (t1_y > _length) {
841 // The starting point is off the top of the capsule. Test the line against
842 // the second endcap.
843 double t1b, t2b;
844 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, radius)) {
845 // If there's no intersection with the endcap, there can't be an
846 // intersection with the cylinder.
847 return false;
848 }
849 t1 = t1b;
850 }
851
852 if (t2_y < 0.0f) {
853 // The ending point is off the bottom of the capsule. Test the line against
854 // the first endcap.
855 double t1a, t2a;
856 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, radius)) {
857 // If there's no intersection with the endcap, there can't be an
858 // intersection with the cylinder.
859 return false;
860 }
861 t2 = t2a;
862
863 } else if (t2_y > _length) {
864 // The ending point is off the top of the capsule. Test the line against the
865 // second endcap.
866 double t1b, t2b;
867 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, radius)) {
868 // If there's no intersection with the endcap, there can't be an
869 // intersection with the cylinder.
870 return false;
871 }
872 t2 = t2b;
873 }
874
875 return true;
876}
877
878/**
879 * After confirming that the line intersects an infinite cylinder, test
880 * whether it intersects one or the other endcaps. The y parameter specifies
881 * the center of the sphere (and hence the particular endcap).
882 */
883bool CollisionCapsule::
884sphere_intersects_line(double &t1, double &t2, PN_stdfloat center_y,
885 const LPoint3 &from, const LVector3 &delta,
886 PN_stdfloat radius) {
887 // See CollisionSphere::intersects_line() for a derivation of the formula
888 // here.
889 double A = dot(delta, delta);
890
891 nassertr(A != 0.0, false);
892
893 LVector3 fc = from;
894 fc[1] -= center_y;
895 double B = 2.0f* dot(delta, fc);
896 double fc_d2 = dot(fc, fc);
897 double C = fc_d2 - radius * radius;
898
899 double radical = B*B - 4.0*A*C;
900
901 if (IS_NEARLY_ZERO(radical)) {
902 // Tangent.
903 t1 = t2 = -B / (2.0 * A);
904 return true;
905
906 } else if (radical < 0.0) {
907 // No real roots: no intersection with the line.
908 return false;
909 }
910
911 double reciprocal_2A = 1.0 / (2.0 * A);
912 double sqrt_radical = sqrtf(radical);
913 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
914 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
915
916 return true;
917}
918
919/**
920 * Determine a point of intersection of a parametric parabola with the capsule.
921 *
922 * We only consider the segment of the parabola between t1 and t2, which has
923 * already been computed as corresponding to points p1 and p2. If there is an
924 * intersection, t is set to the parametric point of intersection, and true is
925 * returned; otherwise, false is returned.
926 */
927bool CollisionCapsule::
928intersects_parabola(double &t, const LParabola &parabola,
929 double t1, double t2,
930 const LPoint3 &p1, const LPoint3 &p2) const {
931 // I don't even want to think about the math to do this calculation directly
932 // --it's even worse than sphere-parabola. So I'll use the recursive
933 // subdivision solution again, just like I did for sphere-parabola.
934
935 // First, see if the line segment (p1 - p2) comes sufficiently close to the
936 // parabola. Do this by computing the parametric intervening point and
937 // comparing its distance from the linear intervening point.
938 double tmid = (t1 + t2) * 0.5;
939
940 if (tmid != t1 && tmid != t2) {
941 LPoint3 pmid = parabola.calc_point(tmid);
942 LPoint3 pmid2 = (p1 + p2) * 0.5f;
943
944 if ((pmid - pmid2).length_squared() > 0.001f) {
945 // Subdivide.
946 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
947 return true;
948 }
949 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
950 }
951 }
952
953 // The line segment is sufficiently close; compare the segment itself.
954 double t1a, t2a;
955 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
956 return false;
957 }
958
959 if (t2a < 0.0 || t1a > 1.0) {
960 return false;
961 }
962
963 t = std::max(t1a, 0.0);
964 return true;
965}
966
967/**
968 * Calculates a point that is exactly on the surface of the capsule and its
969 * corresponding normal, given a point that is supposedly on the surface of
970 * the capsule.
971 */
972void CollisionCapsule::
973calculate_surface_point_and_normal(const LPoint3 &surface_point,
974 double extra_radius,
975 LPoint3 &result_point,
976 LVector3 &result_normal) const {
977 // Convert the point into our canonical space for analysis.
978 LPoint3 point = surface_point * _inv_mat;
979 LVector3 normal;
980
981 if (point[1] <= 0.0) {
982 // The point is on the first endcap.
983 normal = point;
984 if (!normal.normalize()) {
985 normal.set(0.0, -1.0, 0.0);
986 }
987 point = normal * _radius;
988
989 } else if (point[1] >= _length) {
990 // The point is on the second endcap.
991 normal.set(point[0], point[1] - _length, point[2]);
992 if (!normal.normalize()) {
993 normal.set(0.0, 1.0, 0.0);
994 }
995 point = normal * _radius;
996 point[1] += _length;
997
998 } else {
999 // The point is within the cylinder part.
1000 LVector2d normal2d(point[0], point[2]);
1001 if (!normal2d.normalize()) {
1002 normal2d.set(0.0, 1.0);
1003 }
1004 normal.set(normal2d[0], 0.0, normal2d[1]);
1005 point.set(normal[0] * _radius, point[1], normal[2] * _radius);
1006 }
1007
1008 // Now convert the point and normal back into real space.
1009 result_point = point * _mat;
1010 result_normal = normal * _mat;
1011}
1012
1013/**
1014 * After an intersection has been detected, record the computed intersection
1015 * point in the CollisionEntry, and also compute the relevant normal based on
1016 * that point.
1017 */
1018void CollisionCapsule::
1019set_intersection_point(CollisionEntry *new_entry,
1020 const LPoint3 &into_intersection_point,
1021 double extra_radius) const {
1022 LPoint3 point;
1023 LVector3 normal;
1024
1025 calculate_surface_point_and_normal(into_intersection_point,
1026 extra_radius,
1027 point,
1028 normal);
1029
1030 if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) {
1031 normal = get_effective_normal();
1032 }
1033
1034 new_entry->set_surface_normal(normal);
1035 new_entry->set_surface_point(point);
1036 // Also adjust the original point into the capsule by the amount of
1037 // extra_radius, which should put it on the surface of the capsule if our
1038 // collision was tangential.
1039 new_entry->set_interior_point(into_intersection_point - normal * extra_radius);
1040}
1041
1042/**
1043 * Tells the BamReader how to create objects of type CollisionCapsule.
1044 */
1047 BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
1048}
1049
1050/**
1051 * Writes the contents of this object to the datagram for shipping out to a
1052 * Bam file.
1053 */
1055write_datagram(BamWriter *manager, Datagram &dg) {
1056 CollisionSolid::write_datagram(manager, dg);
1057 _a.write_datagram(dg);
1058 _b.write_datagram(dg);
1059 dg.add_stdfloat(_radius);
1060}
1061
1062/**
1063 * This function is called by the BamReader's factory when a new object of
1064 * type CollisionCapsule is encountered in the Bam file. It should create the
1065 * CollisionCapsule and extract its information from the file.
1066 */
1067TypedWritable *CollisionCapsule::
1068make_from_bam(const FactoryParams &params) {
1069 CollisionCapsule *node = new CollisionCapsule();
1070 DatagramIterator scan;
1071 BamReader *manager;
1072
1073 parse_params(params, scan, manager);
1074 node->fillin(scan, manager);
1075
1076 return node;
1077}
1078
1079/**
1080 * This internal function is called by make_from_bam to read in all of the
1081 * relevant data from the BamFile for the new CollisionCapsule.
1082 */
1083void CollisionCapsule::
1084fillin(DatagramIterator &scan, BamReader *manager) {
1085 CollisionSolid::fillin(scan, manager);
1086 _a.read_datagram(scan);
1087 _b.read_datagram(scan);
1088 _radius = scan.get_stdfloat();
1089 recalc_internals();
1090}
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.
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 defines a bounding sphere, consisting of a center and a radius.
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
This implements a solid consisting of a cylinder with hemispherical endcaps, also known as a capsule ...
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 &dg)
Writes the contents of this object to the datagram for shipping out to a Bam file.
static void register_with_read_factory()
Tells the BamReader how to create objects of type CollisionCapsule.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
Defines a single collision event.
get_into
Returns the CollisionSolid pointer for the particular solid was collided into.
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...
void set_surface_normal(const LVector3 &normal)
Stores the surface normal of the "into" object at the point of the intersection.
void set_interior_point(const LPoint3 &point)
Stores the point, within the interior of the "into" object, which represents the depth to which the "...
get_from
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
void set_surface_point(const LPoint3 &point)
Stores the point, on the surface of the "into" object, at which a collision is detected.
An infinite line, similar to a CollisionRay, except that it extends in both directions.
This defines a parabolic arc, or subset of an arc, similar to the path of a projectile or falling obj...
get_t1
Returns the starting point on the parabola.
get_t2
Returns the ending point on the parabola.
get_parabola
Returns the parabola specified by this solid.
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,...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
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.
A class to retrieve the individual data elements previously stored in a Datagram.
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition datagram.h:38
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:133
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 another abstract class, for a general class of bounding volumes that actually enclose points ...
bool extend_by(const GeometricBoundingVolume *vol)
Increases the size of the volume to include the given volume.
bool around(const GeometricBoundingVolume **first, const GeometricBoundingVolume **last)
Resets the volume to enclose only the volumes indicated.
A lightweight class that represents a single element that may be timed and/or counted via stats.
Indicates a coordinate-system transform on vertices.
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.
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.