15 #include "collisionBox.h" 16 #include "collisionLine.h" 17 #include "collisionRay.h" 18 #include "collisionSphere.h" 19 #include "collisionSegment.h" 20 #include "collisionHandler.h" 21 #include "collisionEntry.h" 22 #include "config_collide.h" 23 #include "boundingSphere.h" 25 #include "datagramIterator.h" 26 #include "bamReader.h" 27 #include "bamWriter.h" 28 #include "nearly_zero.h" 30 #include "mathNumbers.h" 32 #include "geomTriangles.h" 33 #include "geomVertexWriter.h" 34 #include "config_mathutil.h" 39 PStatCollector CollisionBox::_volume_pcollector(
"Collision Volumes:CollisionBox");
40 PStatCollector CollisionBox::_test_pcollector(
"Collision Tests:CollisionBox");
43 const int CollisionBox::plane_def[6][4] = {
69 for(
int plane = 0; plane < 6; plane++) {
71 array[0] =
get_point(plane_def[plane][0]);
72 array[1] =
get_point(plane_def[plane][1]);
73 array[2] =
get_point(plane_def[plane][2]);
74 array[3] =
get_point(plane_def[plane][3]);
87 int num_points = end - begin;
88 nassertv(num_points >= 3);
90 _points[plane].clear();
104 for (pi = begin; pi != end; ++pi) {
105 LPoint3 point = (*pi) * _to_2d_mat[plane];
106 _points[plane].push_back(
PointDef(point[0], point[2]));
109 nassertv(_points[plane].size() >= 3);
137 return entry.
get_into()->test_intersection_from_box(entry);
149 _center = _center * mat;
150 for(
int v = 0; v < 8; v++) {
151 _vertex[v] = _vertex[v] * mat;
153 for(
int p = 0; p < 6 ; p++) {
156 _x = _vertex[0].get_x() - _center.get_x();
157 _y = _vertex[0].get_y() - _center.get_y();
158 _z = _vertex[0].get_z() - _center.get_z();
159 _radius = sqrt(_x * _x + _y * _y + _z * _z);
162 mark_internal_bounds_stale();
204 get_approx_center()
const {
205 return (_min + _max) * 0.5f;
217 return _volume_pcollector;
229 return _test_pcollector;
238 output(ostream &out)
const {
248 compute_internal_bounds()
const {
258 test_intersection_from_sphere(
const CollisionEntry &entry)
const {
261 DCAST_INTO_R(sphere, entry.
get_from(), NULL);
263 CPT(TransformState) wrt_space = entry.get_wrt_space();
264 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
266 const LMatrix4 &wrt_mat = wrt_space->get_mat();
268 LPoint3 orig_center = sphere->get_center() * wrt_mat;
269 LPoint3 from_center = orig_center;
270 bool moved_from_center =
false;
271 PN_stdfloat t = 1.0f;
272 LPoint3 contact_point(from_center);
273 PN_stdfloat actual_t = 1.0f;
276 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
278 PN_stdfloat from_radius = csqrt(from_radius_2);
281 PN_stdfloat max_dist = 0.0;
282 PN_stdfloat dist = 0.0;
287 for(ip = 0, intersect =
false; ip < 6 && !intersect; ip++) {
289 if (_points[ip].size() < 3) {
292 if (wrt_prev_space != wrt_space) {
297 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
302 PN_stdfloat dot = delta.dot(plane.get_normal());
307 if (IS_NEARLY_ZERO(dot)) {
320 PN_stdfloat dist_to_p = plane.dist_to_plane(a);
321 t = (dist_to_p / -dot);
325 actual_t = ((dist_to_p - from_radius) / -dot);
326 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
327 contact_point = a + (actual_t * delta);
332 }
else if (t < 0.0f) {
334 moved_from_center =
true;
336 from_center = a + t * delta;
337 moved_from_center =
true;
357 if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
363 if (dist > from_radius || dist < -from_radius) {
368 LPoint2 p =
to_2d(from_center - dist * plane.get_normal(), ip);
369 PN_stdfloat edge_dist = 0.0f;
379 }
else if (new_points.empty()) {
391 max_dist = from_radius;
401 if((edge_dist > 0) &&
402 ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
413 if (edge_dist >= 0.0f) {
414 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
415 max_dist = csqrt(max_dist_2);
418 if (dist > max_dist) {
427 if (collide_cat.is_debug()) {
435 PN_stdfloat into_depth = max_dist - dist;
436 if (moved_from_center) {
440 PN_stdfloat orig_dist;
441 plane.intersects_line(orig_dist, orig_center, -normal);
442 into_depth = max_dist - orig_dist;
446 LPoint3 surface = from_center - normal * dist;
447 surface = surface.fmax(_min);
448 surface = surface.fmin(_max);
450 new_entry->set_surface_normal(normal);
451 new_entry->set_surface_point(surface);
452 new_entry->set_interior_point(surface - normal * into_depth);
453 new_entry->set_contact_pos(contact_point);
454 new_entry->set_contact_normal(plane.get_normal());
455 new_entry->set_t(actual_t);
469 DCAST_INTO_R(ray, entry.
get_from(), NULL);
470 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
472 LPoint3 from_origin = ray->get_origin() * wrt_mat;
473 LVector3 from_direction = ray->get_direction() * wrt_mat;
477 PN_stdfloat near_t = 0.0;
484 for (i = 0, intersect =
false, t = 0, j = 0; i < 6 && j < 2; i++) {
487 if (!plane.intersects_line(t, from_origin, from_direction)) {
497 LPoint3 plane_point = from_origin + t * from_direction;
523 if (collide_cat.is_debug()) {
531 LPoint3 into_intersection_point = from_origin + near_t * from_direction;
537 new_entry->set_surface_normal(normal);
538 new_entry->set_surface_point(into_intersection_point);
550 test_intersection_from_segment(
const CollisionEntry &entry)
const {
552 DCAST_INTO_R(seg, entry.
get_from(), NULL);
553 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
555 LPoint3 from_origin = seg->get_point_a() * wrt_mat;
556 LPoint3 from_extent = seg->get_point_b() * wrt_mat;
557 LVector3 from_direction = from_extent - from_origin;
561 PN_stdfloat near_t = 0.0;
568 for(i = 0, intersect =
false, t = 0, j = 0; i < 6 && j < 2; i++) {
571 if (!plane.intersects_line(t, from_origin, from_direction)) {
576 if (t < 0.0f || t > 1.0f) {
582 LPoint3 plane_point = from_origin + t * from_direction;
607 if (collide_cat.is_debug()) {
615 LPoint3 into_intersection_point = from_origin + near_t * from_direction;
621 new_entry->set_surface_normal(normal);
622 new_entry->set_surface_point(into_intersection_point);
648 if (collide_cat.is_debug()) {
650 <<
"Recomputing viz for " << *
this <<
"\n";
654 (
"collision", GeomVertexFormat::get_v3(),
657 vdata->unclean_set_num_rows(8);
661 vertex.
set_data3(_min[0], _min[1], _min[2]);
662 vertex.
set_data3(_min[0], _max[1], _min[2]);
663 vertex.
set_data3(_max[0], _max[1], _min[2]);
664 vertex.
set_data3(_max[0], _min[1], _min[2]);
666 vertex.
set_data3(_min[0], _min[1], _max[2]);
667 vertex.
set_data3(_min[0], _max[1], _max[2]);
668 vertex.
set_data3(_max[0], _max[1], _max[2]);
669 vertex.
set_data3(_max[0], _min[1], _max[2]);
675 tris->add_vertices(0, 1, 2);
676 tris->add_vertices(2, 3, 0);
679 tris->add_vertices(4, 7, 6);
680 tris->add_vertices(6, 5, 4);
683 tris->add_vertices(0, 4, 1);
684 tris->add_vertices(1, 4, 5);
686 tris->add_vertices(1, 5, 2);
687 tris->add_vertices(2, 5, 6);
689 tris->add_vertices(2, 6, 3);
690 tris->add_vertices(3, 6, 7);
692 tris->add_vertices(3, 7, 0);
693 tris->add_vertices(0, 7, 4);
696 geom->add_primitive(tris);
698 _viz_geom->add_geom(geom, get_solid_viz_state());
699 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
716 const TransformState *net_transform,
int plane_no)
const {
720 bool first_plane =
true;
722 for (
int i = 0; i < num_planes; i++) {
726 CPT(TransformState) new_transform =
727 net_transform->invert_compose(plane_path.get_net_transform());
729 LPlane plane = plane_node->
get_plane() * new_transform->get_mat();
732 if (!
clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
737 last_points.swap(new_points);
738 if (!
clip_polygon(new_points, last_points, plane, plane_no)) {
766 const LPlane &plane,
int plane_no)
const {
768 if (source_points.empty()) {
774 if (!plane.intersects_plane(from3d, delta3d,
get_plane(plane_no))) {
780 new_points = source_points;
791 PN_stdfloat a = -delta2d[1];
792 PN_stdfloat b = delta2d[0];
793 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
802 new_points.reserve(source_points.size() + 1);
804 LPoint2 last_point = source_points.back()._p;
805 bool last_is_in = !is_right(last_point - from2d, delta2d);
806 bool all_in = last_is_in;
807 Points::const_iterator pi;
808 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
809 const LPoint2 &this_point = (*pi)._p;
810 bool this_is_in = !is_right(this_point - from2d, delta2d);
814 bool crossed_over = (this_is_in != last_is_in);
818 LVector2 d = this_point - last_point;
819 PN_stdfloat denom = (a * d[0] + b * d[1]);
821 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
822 LPoint2 p = last_point + t * d;
824 new_points.push_back(
PointDef(p[0], p[1]));
825 last_is_in = this_is_in;
831 new_points.push_back(
PointDef(this_point[0], this_point[1]));
836 last_point = this_point;
862 bool got_dist =
false;
863 PN_stdfloat best_dist = -1.0f;
865 size_t num_points = points.size();
866 for (
size_t i = 0; i < num_points - 1; ++i) {
867 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
870 if (!got_dist || d < best_dist) {
877 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
878 points[num_points - 1]._v);
880 if (!got_dist || d < best_dist) {
901 PN_stdfloat CollisionBox::
902 dist_to_line_segment(
const LPoint2 &p,
906 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
924 return (p - t).length();
931 return (p - f).length();
933 return (p - t).length();
943 return (p - f).length();
945 return (p - t).length();
952 return (p - f).length();
954 return (p - t).length();
967 return (p - f).length();
969 return (p - t).length();
976 return (p - f).length();
978 return (p - t).length();
988 return (p - f).length();
990 return (p - t).length();
997 return (p - f).length();
999 return (p - t).length();
1020 for (
int i = 0; i < (int)points.size() - 1; i++) {
1021 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1025 if (is_right(p - points[points.size() - 1]._p,
1026 points[0]._p - points[points.size() - 1]._p)) {
1042 size_t num_points = points.size();
1043 for (
size_t i = 0; i < num_points; i++) {
1044 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1045 points[i]._v.normalize();
1071 for(
int i=0; i < 8; i++) {
1078 for(
int i=0; i < 6; i++) {
1079 _planes[i].write_datagram(me);
1081 for(
int i=0; i < 6; i++) {
1084 for(
int i=0; i < 6; i++) {
1086 for (
size_t j = 0; j < _points[i].size(); j++) {
1087 _points[i][j]._p.write_datagram(me);
1088 _points[i][j]._v.write_datagram(me);
1104 parse_params(params, scan, manager);
1105 me->fillin(scan, manager);
1119 CollisionSolid::fillin(scan, manager);
1123 for(
int i=0; i < 8; i++) {
1130 for(
int i=0; i < 6; i++) {
1131 _planes[i].read_datagram(scan);
1133 for(
int i=0; i < 6; i++) {
1136 for(
int i=0; i < 6; i++) {
1138 for (
size_t j = 0; j < size; j++) {
1143 _points[i].push_back(
PointDef(p, v));
void calc_to_3d_mat(LMatrix4 &to_3d_mat, int plane) const
Fills the indicated matrix with the appropriate rotation transform to move points from the 2-d plane ...
float length_squared() const
Returns the square of the vector's length, cheap and easy.
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...
void setup_box()
Compute parameters for each of the box's sides.
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...
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...
void set_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. ...
NodePath get_on_plane(int n) const
Returns the nth plane enabled by the attribute, sorted in render order.
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).
bool point_is_inside(const LPoint2 &p, const Points &points) const
Returns true if the indicated point is within the polygon's 2-d space, false otherwise.
This defines a bounding sphere, consisting of a center and a radius.
A cuboid collision volume or object.
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 ...
This functions similarly to a LightAttrib.
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.
LPoint2 to_2d(const LVecBase3 &point3d, int plane) const
Assuming the indicated point in 3-d space lies within the polygon's plane, returns the corresponding ...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
static void compute_vectors(Points &points)
Now that the _p members of the given points array have been computed, go back and compute all of the ...
PN_uint16 get_uint16()
Extracts an unsigned 16-bit integer.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
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...
A lightweight class that represents a single element that may be timed and/or counted via stats...
A finite line segment, with two specific endpoints but no thickness.
int get_num_on_planes() const
Returns the number of planes that are enabled by the attribute.
void write_datagram(Datagram &destination) const
Writes the matrix to the Datagram using add_stdfloat().
const CollisionSolid * get_into() const
Returns the CollisionSolid pointer for the particular solid was collided into.
static void register_with_read_factory()
Factory method to generate a CollisionBox object.
void setup_points(const LPoint3 *begin, const LPoint3 *end, int plane)
Computes the plane and 2d projection of points that make up this side.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
This is a 4-by-4 transform matrix.
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
Defines a single collision event.
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
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.
LPlane set_plane(int n) const
Creates the nth face of the rectangular solid.
void read_datagram(DatagramIterator &source)
Reads the matrix from the Datagram using get_stdfloat().
LPlane get_plane(int n) const
Returns the nth face of the rectangular solid.
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
void add_uint16(PN_uint16 value)
Adds an unsigned 16-bit integer to the datagram.
PN_stdfloat dist_to_polygon(const LPoint2 &p, const Points &points) const
Returns the linear distance from the 2-d point to the nearest part of the polygon defined by the poin...
This is a two-component vector offset.
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
PandaNode * node() const
Returns the referenced node of the path.
Defines a series of disconnected triangles.
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
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...
This is a two-component point in space.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
TypeHandle is the identifier used to differentiate C++ class types.
bool get_respect_effective_normal() const
See set_respect_effective_normal().
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
bool apply_clip_plane(Points &new_points, const ClipPlaneAttrib *cpa, const TransformState *net_transform, int plane_no) const
Clips the polygon by all of the clip planes named in the clip plane attribute and fills new_points up...
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
A node that contains a plane.
bool clip_polygon(Points &new_points, const Points &source_points, const LPlane &plane, int plane_no) const
Clips the source_points of the polygon by the indicated clipping plane, and modifies new_points to re...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
int get_clip_effect() const
Returns the clip_effect bits for this clip plane.