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);
69 CollisionSolid::xform(mat);
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 {
126 PT(
BoundingVolume) bound = CollisionSolid::compute_internal_bounds();
128 if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
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(), NULL);
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()) {
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,
229 new_entry->
set_t(actual_t);
242 DCAST_INTO_R(line, entry.
get_from(), NULL);
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()) {
262 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
263 set_intersection_point(new_entry, into_intersection_point, 0.0);
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);
292 DCAST_INTO_R(ray, entry.
get_from(), NULL);
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()) {
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);
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);
354 test_intersection_from_segment(
const CollisionEntry &entry)
const {
356 DCAST_INTO_R(segment, entry.
get_from(), NULL);
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()) {
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);
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);
420 test_intersection_from_parabola(
const CollisionEntry &entry)
const {
422 DCAST_INTO_R(parabola, entry.
get_from(), NULL);
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()) {
445 LPoint3 into_intersection_point = local_p.calc_point(t);
446 set_intersection_point(new_entry, into_intersection_point, 0.0);
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);
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));
501 strip->add_next_vertices((num_slices + 1) * 2);
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);
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;
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,
1002 parse_params(params, scan, manager);
1003 node->fillin(scan, manager);
1015 void CollisionTube::
1017 CollisionSolid::fillin(scan, manager);
const LParabola & get_parabola() const
Returns the parabola specified by this solid.
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
An infinite ray, with a specific origin and direction.
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
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...
NodePath get_into_node_path() const
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
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.
PN_stdfloat get_t2() const
Returns the ending point on the parabola.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
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.
void set_contact_normal(const LVector3 &normal)
Stores the surface normal of the "into" object at the contact pos.
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Defines a series of triangle strips.
void set_t(PN_stdfloat t)
Sets a time value for this collision relative to other CollisionEntries.
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 "...
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...
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.
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
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...
bool invert_from(const LMatrix4f &other)
Computes the inverse of the other matrix, and stores the result in this matrix.
void add_data3(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Sets the write row to a particular 3-component value, and advances the write row. ...
LVecBase4f xform(const LVecBase4f &v) const
4-component vector or point times matrix.
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
An infinite line, similar to a CollisionRay, except that it extends in both directions.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
This is a two-component vector offset.
void set_contact_pos(const LPoint3 &pos)
Stores the position of the "from" object at the instant at which the collision is first detected...
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...
bool normalize()
Normalizes the vector in place.
void set_row(int row, const LVecBase4f &v)
Replaces the indicated row of the matrix.
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
A class to retrieve the individual data elements previously stored in a Datagram. ...
NodePath get_from_node_path() const
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
TypeHandle is the identifier used to differentiate C++ class types.
This implements a solid roughly in cylindrical shape.
bool get_respect_effective_normal() const
See set_respect_effective_normal().
bool normalize()
Normalizes the vector in place.
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 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.
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.