15 #include "collisionTube.h"
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"
27 #include "geometricBoundingVolume.h"
29 #include "datagramIterator.h"
30 #include "bamReader.h"
31 #include "bamWriter.h"
33 #include "transformState.h"
35 #include "geomTristrips.h"
36 #include "geomVertexWriter.h"
37 #include "boundingSphere.h"
39 PStatCollector CollisionTube::_volume_pcollector(
"Collision Volumes:CollisionTube");
40 PStatCollector CollisionTube::_test_pcollector(
"Collision Tests:CollisionTube");
66 _radius = length(radius_v);
94 return _volume_pcollector;
106 return _test_pcollector;
115 output(ostream &out)
const {
116 out <<
"tube, a (" << _a <<
"), b (" << _b <<
"), r " << _radius;
125 compute_internal_bounds()
const {
130 DCAST_INTO_R(gbound, bound, bound);
137 points[0] = _a - vec * _radius;
138 points[1] = _b + vec * _radius;
140 gbound->
around(points, points + 2);
159 test_intersection_from_sphere(const
CollisionEntry &entry)
const {
161 DCAST_INTO_R(sphere, entry.get_from(), 0);
163 CPT(TransformState) wrt_space = entry.get_wrt_space();
164 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
166 const
LMatrix4 &wrt_mat = wrt_space->get_mat();
168 LPoint3 from_a = sphere->get_center() * wrt_mat;
172 PN_stdfloat actual_t = 0.0f;
174 if (wrt_prev_space != wrt_space) {
177 from_a = sphere->get_center() * wrt_prev_space->get_mat();
180 LVector3 from_direction = from_b - from_a;
183 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
184 PN_stdfloat from_radius = length(from_radius_v);
187 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
192 if (t2 < 0.0 || t1 > 1.0) {
199 actual_t = min(1.0, max(0.0, t1));
200 contact_point = from_a + actual_t * (from_b - from_a);
202 if (collide_cat.is_debug()) {
204 <<
"intersection detected from " << entry.get_from_node_path() <<
" into "
205 << entry.get_into_node_path() <<
"\n";
209 LPoint3 into_intersection_point;
213 into_intersection_point = from_b;
217 into_intersection_point = from_a + t2 * from_direction;
219 set_intersection_point(new_entry, into_intersection_point, from_radius);
223 calculate_surface_point_and_normal(contact_point,
227 new_entry->set_contact_pos(contact_point);
228 new_entry->set_contact_normal(contact_normal);
229 new_entry->set_t(actual_t);
242 DCAST_INTO_R(line, entry.get_from(), 0);
244 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
246 LPoint3 from_origin = line->get_origin() * wrt_mat;
247 LVector3 from_direction = line->get_direction() * wrt_mat;
250 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
255 if (collide_cat.is_debug()) {
257 <<
"intersection detected from " << entry.get_from_node_path()
258 <<
" into " << entry.get_into_node_path() <<
"\n";
262 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
263 set_intersection_point(new_entry, into_intersection_point, 0.0);
265 if (has_effective_normal() && line->get_respect_effective_normal()) {
266 new_entry->set_surface_normal(get_effective_normal());
269 LVector3 normal = into_intersection_point * _inv_mat;
270 if (normal[1] > _length) {
272 normal[1] -= _length;
273 }
else if (normal[1] > 0.0f) {
277 normal = normalize(normal * _mat);
278 new_entry->set_surface_normal(normal);
292 DCAST_INTO_R(ray, entry.get_from(), 0);
294 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
296 LPoint3 from_origin = ray->get_origin() * wrt_mat;
297 LVector3 from_direction = ray->get_direction() * wrt_mat;
300 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
310 if (collide_cat.is_debug()) {
312 <<
"intersection detected from " << entry.get_from_node_path()
313 <<
" into " << entry.get_into_node_path() <<
"\n";
317 LPoint3 into_intersection_point;
321 into_intersection_point = from_origin;
325 into_intersection_point = from_origin + t1 * from_direction;
327 set_intersection_point(new_entry, into_intersection_point, 0.0);
330 new_entry->set_surface_normal(get_effective_normal());
333 LVector3 normal = into_intersection_point * _inv_mat;
334 if (normal[1] > _length) {
336 normal[1] -= _length;
337 }
else if (normal[1] > 0.0f) {
341 normal = normalize(normal * _mat);
342 new_entry->set_surface_normal(normal);
354 test_intersection_from_segment(const
CollisionEntry &entry)
const {
356 DCAST_INTO_R(segment, entry.get_from(), 0);
358 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
360 LPoint3 from_a = segment->get_point_a() * wrt_mat;
361 LPoint3 from_b = segment->get_point_b() * wrt_mat;
362 LVector3 from_direction = from_b - from_a;
365 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
370 if (t2 < 0.0 || t1 > 1.0) {
376 if (collide_cat.is_debug()) {
378 <<
"intersection detected from " << entry.get_from_node_path()
379 <<
" into " << entry.get_into_node_path() <<
"\n";
383 LPoint3 into_intersection_point;
387 into_intersection_point = from_a;
391 into_intersection_point = from_a + t1 * from_direction;
393 set_intersection_point(new_entry, into_intersection_point, 0.0);
396 new_entry->set_surface_normal(get_effective_normal());
399 LVector3 normal = into_intersection_point * _inv_mat;
400 if (normal[1] > _length) {
402 normal[1] -= _length;
403 }
else if (normal[1] > 0.0f) {
407 normal = normalize(normal * _mat);
408 new_entry->set_surface_normal(normal);
420 test_intersection_from_parabola(const
CollisionEntry &entry)
const {
422 DCAST_INTO_R(parabola, entry.get_from(), 0);
424 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
428 local_p.xform(wrt_mat);
431 if (!intersects_parabola(t, local_p, parabola->
get_t1(), parabola->
get_t2(),
432 local_p.calc_point(parabola->
get_t1()),
433 local_p.calc_point(parabola->
get_t2()))) {
438 if (collide_cat.is_debug()) {
440 <<
"intersection detected from " << entry.get_from_node_path()
441 <<
" into " << entry.get_into_node_path() <<
"\n";
445 LPoint3 into_intersection_point = local_p.calc_point(t);
446 set_intersection_point(new_entry, into_intersection_point, 0.0);
448 if (has_effective_normal() && parabola->get_respect_effective_normal()) {
449 new_entry->set_surface_normal(get_effective_normal());
452 LVector3 normal = into_intersection_point * _inv_mat;
453 if (normal[1] > _length) {
455 normal[1] -= _length;
456 }
else if (normal[1] > 0.0f) {
460 normal = normalize(normal * _mat);
461 new_entry->set_surface_normal(normal);
475 if (collide_cat.is_debug()) {
477 <<
"Recomputing viz for " << *
this <<
"\n";
484 PN_stdfloat length = direction.
length();
487 ("collision", GeomVertexFormat::get_v3(),
493 static const
int num_slices = 8;
494 static const
int num_rings = 4;
496 for (ri = 0; ri < num_rings; ri++) {
497 for (si = 0; si <= num_slices; si++) {
498 vertex.add_data3(calc_sphere1_vertex(ri, si, num_rings, num_slices));
499 vertex.add_data3(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
502 strip->close_primitive();
506 for (si = 0; si <= num_slices; si++) {
507 vertex.add_data3(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
508 vertex.add_data3(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
511 strip->add_next_vertices((num_slices + 1) * 2);
512 strip->close_primitive();
515 for (ri = num_rings - 1; ri >= 0; ri--) {
516 for (si = 0; si <= num_slices; si++) {
517 vertex.add_data3(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
518 vertex.add_data3(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
520 strip->add_next_vertices((num_slices + 1) * 2);
521 strip->close_primitive();
525 geom->add_primitive(strip);
529 look_at(mat, direction,
LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
531 geom->transform_vertices(mat);
533 _viz_geom->add_geom(geom, get_solid_viz_state());
534 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
547 _length = direction.
length();
549 look_at(_mat, direction,
LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
551 _inv_mat.invert_from(_mat);
554 mark_internal_bounds_stale();
564 LVertex CollisionTube::
565 calc_sphere1_vertex(
int ri,
int si,
int num_rings,
int num_slices) {
566 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
567 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
570 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
571 PN_stdfloat x_rim = ccos(theta);
572 PN_stdfloat z_rim = csin(theta);
575 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
576 PN_stdfloat to_pole = csin(phi);
578 PN_stdfloat x = _radius * x_rim * to_pole;
579 PN_stdfloat y = -_radius * ccos(phi);
580 PN_stdfloat z = _radius * z_rim * to_pole;
582 return LVertex(x, y, z);
592 LVertex CollisionTube::
593 calc_sphere2_vertex(
int ri,
int si,
int num_rings,
int num_slices,
594 PN_stdfloat length) {
595 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
596 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
599 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
600 PN_stdfloat x_rim = ccos(theta);
601 PN_stdfloat z_rim = csin(theta);
604 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
605 PN_stdfloat to_pole = csin(phi);
607 PN_stdfloat x = _radius * x_rim * to_pole;
608 PN_stdfloat y = length + _radius * ccos(phi);
609 PN_stdfloat z = _radius * z_rim * to_pole;
611 return LVertex(x, y, z);
628 intersects_line(
double &t1,
double &t2,
630 PN_stdfloat inflate_radius)
const {
633 LPoint3 from = from0 * _inv_mat;
636 PN_stdfloat radius = _radius + inflate_radius;
645 LVector2 delta2(delta[0], delta[2]);
647 double A = dot(delta2, delta2);
649 if (IS_NEARLY_ZERO(A)) {
653 if (from2.dot(from2) > radius * radius) {
659 if (IS_NEARLY_ZERO(delta[1])) {
664 if (from[1] < -radius || from[1] > _length + radius) {
668 if (from[1] < 0.0f) {
670 if (from.dot(from) > radius * radius) {
673 }
else if (from[1] > _length) {
676 if (from.dot(from) > radius * radius) {
688 t1 = (-radius - from[1]) / delta[1];
689 t2 = (_length + radius - from[1]) / delta[1];
696 double B = 2.0f * dot(delta2, from2);
697 double fc_d2 = dot(from2, from2);
698 double C = fc_d2 - radius * radius;
700 double radical = B*B - 4.0*A*C;
702 if (IS_NEARLY_ZERO(radical)) {
704 t1 = t2 = -B / (2.0*A);
706 }
else if (radical < 0.0) {
711 double reciprocal_2A = 1.0 / (2.0 * A);
712 double sqrt_radical = sqrtf(radical);
713 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
714 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
720 PN_stdfloat t1_y = from[1] + t1 * delta[1];
721 PN_stdfloat t2_y = from[1] + t2 * delta[1];
723 if (t1_y < -radius && t2_y < -radius) {
727 }
else if (t1_y > _length + radius && t2_y > _length + radius) {
736 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
743 }
else if (t1_y > _length) {
747 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
759 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
766 }
else if (t2_y > _length) {
770 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
790 sphere_intersects_line(
double &t1,
double &t2, PN_stdfloat center_y,
792 PN_stdfloat inflate_radius)
const {
795 PN_stdfloat radius = _radius + inflate_radius;
797 double A = dot(delta, delta);
799 nassertr(A != 0.0,
false);
803 double B = 2.0f* dot(delta, fc);
804 double fc_d2 = dot(fc, fc);
805 double C = fc_d2 - radius * radius;
807 double radical = B*B - 4.0*A*C;
809 if (IS_NEARLY_ZERO(radical)) {
811 t1 = t2 = -B / (2.0 * A);
814 }
else if (radical < 0.0) {
819 double reciprocal_2A = 1.0 / (2.0 * A);
820 double sqrt_radical = sqrtf(radical);
821 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
822 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
841 intersects_parabola(
double &t,
const LParabola ¶bola,
842 double t1,
double t2,
853 double tmid = (t1 + t2) * 0.5;
855 if (tmid != t1 && tmid != t2) {
856 LPoint3 pmid = parabola.calc_point(tmid);
857 LPoint3 pmid2 = (p1 + p2) * 0.5f;
859 if ((pmid - pmid2).length_squared() > 0.001f) {
861 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
864 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
870 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
874 if (t2a < 0.0 || t1a > 1.0) {
891 calculate_surface_point_and_normal(
const LPoint3 &surface_point,
896 LPoint3 point = surface_point * _inv_mat;
899 if (point[1] <= 0.0) {
903 normal.set(0.0, -1.0, 0.0);
905 point = normal * _radius;
907 }
else if (point[1] >= _length) {
909 normal.set(point[0], point[1] - _length, point[2]);
911 normal.set(0.0, 1.0, 0.0);
913 point = normal * _radius;
919 if (!normal2d.normalize()) {
920 normal2d.set(0.0, 1.0);
922 normal.set(normal2d[0], 0.0, normal2d[1]);
923 point.set(normal[0] * _radius, point[1], normal[2] * _radius);
927 result_point = point * _mat;
928 result_normal = normal * _mat;
941 const LPoint3 &into_intersection_point,
942 double extra_radius)
const {
946 calculate_surface_point_and_normal(into_intersection_point,
983 _a.write_datagram(dg);
1002 parse_params(params, scan, manager);
1003 node->fillin(scan, manager);
1015 void CollisionTube::
1017 CollisionSolid::fillin(scan, manager);
1018 _a.read_datagram(scan);
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.
bool get_respect_effective_normal() const
See set_respect_effective_normal().
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
This is a two-component vector offset.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
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_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
This defines a bounding sphere, consisting of a center and a radius.
Base class for objects that can be written to and read from Bam files.
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
Defines a series of triangle strips.
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
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.
void set_interior_point(const LPoint3 &point)
Stores the point, within the interior of the "into" object, which represents the depth to which the "...
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
float length() const
Returns the length of the vector, by the Pythagorean theorem.
static void register_with_read_factory()
Tells the BamReader how to create objects of type CollisionTube.
A lightweight class that represents a single element that may be timed and/or counted via stats...
This is another abstract class, for a general class of bounding volumes that actually enclose points ...
A finite line segment, with two specific endpoints but no thickness.
virtual void write_datagram(BamWriter *manager, Datagram &dg)
Writes the contents of this object to the datagram for shipping out to a Bam file.
This is a 4-by-4 transform matrix.
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.
An instance of this class is passed to the Factory when requesting it to do its business and construc...
PN_stdfloat get_t2() const
Returns the ending point on the parabola.
const LParabola & get_parabola() const
Returns the parabola specified by this solid.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
void add_next_vertices(int num_vertices)
Adds the next n vertices in sequence, beginning from the last vertex added to the primitive + 1...
An infinite line, similar to a CollisionRay, except that it extends in both directions.
This is a two-component vector offset.
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
This defines a parabolic arc, or subset of an arc, similar to the path of a projectile or falling obj...
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
void set_row(int row, const LVecBase4f &v)
Replaces the indicated row of the matrix.
A class to retrieve the individual data elements previously stored in a Datagram. ...
TypeHandle is the identifier used to differentiate C++ class types.
This implements a solid roughly in cylindrical shape.
bool normalize()
Normalizes the vector in place.
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
void set_surface_point(const LPoint3 &point)
Stores the point, on the surface of the "into" object, at which a collision is detected.
bool around(const GeometricBoundingVolume **first, const GeometricBoundingVolume **last)
Resets the volume to enclose only the volumes indicated.
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
void set_surface_normal(const LVector3 &normal)
Stores the surface normal of the "into" object at the point of the intersection.
bool extend_by(const GeometricBoundingVolume *vol)
Increases the size of the volume to include the given volume.