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(), 0);
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;
277 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
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++) {
288 plane = get_plane( 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;
375 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
378 edge_dist = dist_to_polygon(p, _points[ip]);
379 }
else if (new_points.empty()) {
384 edge_dist = dist_to_polygon(p, new_points);
388 edge_dist = dist_to_polygon(p, _points[ip]);
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()) {
429 <<
"intersection detected from " << entry.get_from_node_path()
430 <<
" into " << entry.get_into_node_path() <<
"\n";
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;
445 new_entry->set_surface_normal(normal);
446 new_entry->set_surface_point(from_center - normal * dist);
447 new_entry->set_interior_point(from_center - normal * (dist + into_depth));
448 new_entry->set_contact_pos(contact_point);
449 new_entry->set_contact_normal(plane.get_normal());
450 new_entry->set_t(actual_t);
464 DCAST_INTO_R(ray, entry.get_from(), 0);
465 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
467 LPoint3 from_origin = ray->get_origin() * wrt_mat;
468 LVector3 from_direction = ray->get_direction() * wrt_mat;
472 PN_stdfloat near_t = 0.0;
479 for (i = 0, intersect =
false, t = 0, j = 0; i < 6 && j < 2; i++) {
480 plane = get_plane(i);
482 if (!plane.intersects_line(t, from_origin, from_direction)) {
492 LPoint3 plane_point = from_origin + t * from_direction;
493 LPoint2 p = to_2d(plane_point, i);
495 if (!point_is_inside(p, _points[i])){
518 if (collide_cat.is_debug()) {
520 <<
"intersection detected from " << entry.get_from_node_path()
521 <<
" into " << entry.get_into_node_path() <<
"\n";
526 LPoint3 into_intersection_point = from_origin + near_t * from_direction;
529 (has_effective_normal() && ray->get_respect_effective_normal())
530 ? get_effective_normal() : near_plane.get_normal();
532 new_entry->set_surface_normal(normal);
533 new_entry->set_surface_point(into_intersection_point);
545 test_intersection_from_segment(const
CollisionEntry &entry)
const {
547 DCAST_INTO_R(seg, entry.get_from(), 0);
548 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
550 LPoint3 from_origin = seg->get_point_a() * wrt_mat;
551 LPoint3 from_extent = seg->get_point_b() * wrt_mat;
552 LVector3 from_direction = from_extent - from_origin;
556 PN_stdfloat near_t = 0.0;
563 for(i = 0, intersect =
false, t = 0, j = 0; i < 6 && j < 2; i++) {
564 plane = get_plane(i);
566 if (!plane.intersects_line(t, from_origin, from_direction)) {
571 if (t < 0.0f || t > 1.0f) {
577 LPoint3 plane_point = from_origin + t * from_direction;
578 LPoint2 p = to_2d(plane_point, i);
580 if (!point_is_inside(p, _points[i])){
602 if (collide_cat.is_debug()) {
604 <<
"intersection detected from " << entry.get_from_node_path()
605 <<
" into " << entry.get_into_node_path() <<
"\n";
610 LPoint3 into_intersection_point = from_origin + near_t * from_direction;
613 (has_effective_normal() && seg->get_respect_effective_normal())
614 ? get_effective_normal() : near_plane.get_normal();
616 new_entry->set_surface_normal(normal);
617 new_entry->set_surface_point(into_intersection_point);
643 if (collide_cat.is_debug()) {
645 <<
"Recomputing viz for " << *
this <<
"\n";
649 ("collision", GeomVertexFormat::get_v3(),
652 vdata->unclean_set_num_rows(8);
656 vertex.set_data3(_min[0], _min[1], _min[2]);
657 vertex.set_data3(_min[0], _max[1], _min[2]);
658 vertex.set_data3(_max[0], _max[1], _min[2]);
659 vertex.set_data3(_max[0], _min[1], _min[2]);
661 vertex.set_data3(_min[0], _min[1], _max[2]);
662 vertex.set_data3(_min[0], _max[1], _max[2]);
663 vertex.set_data3(_max[0], _max[1], _max[2]);
664 vertex.set_data3(_max[0], _min[1], _max[2]);
670 tris->add_vertices(0, 1, 2);
671 tris->add_vertices(2, 3, 0);
674 tris->add_vertices(4, 7, 6);
675 tris->add_vertices(6, 5, 4);
678 tris->add_vertices(0, 4, 1);
679 tris->add_vertices(1, 4, 5);
681 tris->add_vertices(1, 5, 2);
682 tris->add_vertices(2, 5, 6);
684 tris->add_vertices(2, 6, 3);
685 tris->add_vertices(3, 6, 7);
687 tris->add_vertices(3, 7, 0);
688 tris->add_vertices(0, 7, 4);
691 geom->add_primitive(tris);
693 _viz_geom->add_geom(geom, get_solid_viz_state());
694 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
711 const TransformState *net_transform,
int plane_no)
const {
714 int num_planes = cpa->get_num_on_planes();
715 bool first_plane =
true;
717 for (
int i = 0; i < num_planes; i++) {
718 NodePath plane_path = cpa->get_on_plane(i);
721 CPT(TransformState) new_transform =
722 net_transform->invert_compose(plane_path.get_net_transform());
724 LPlane plane = plane_node->
get_plane() * new_transform->get_mat();
727 if (!
clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
732 last_points.swap(new_points);
733 if (!
clip_polygon(new_points, last_points, plane, plane_no)) {
761 const LPlane &plane,
int plane_no)
const {
763 if (source_points.empty()) {
769 if (!plane.intersects_plane(from3d, delta3d,
get_plane(plane_no))) {
775 new_points = source_points;
786 PN_stdfloat a = -delta2d[1];
787 PN_stdfloat b = delta2d[0];
788 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
797 new_points.reserve(source_points.size() + 1);
799 LPoint2 last_point = source_points.back()._p;
800 bool last_is_in = !is_right(last_point - from2d, delta2d);
801 bool all_in = last_is_in;
802 Points::const_iterator pi;
803 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
804 const LPoint2 &this_point = (*pi)._p;
805 bool this_is_in = !is_right(this_point - from2d, delta2d);
809 bool crossed_over = (this_is_in != last_is_in);
813 LVector2 d = this_point - last_point;
814 PN_stdfloat denom = (a * d[0] + b * d[1]);
816 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
817 LPoint2 p = last_point + t * d;
819 new_points.push_back(
PointDef(p[0], p[1]));
820 last_is_in = this_is_in;
826 new_points.push_back(
PointDef(this_point[0], this_point[1]));
831 last_point = this_point;
857 bool got_dist =
false;
858 PN_stdfloat best_dist = -1.0f;
860 size_t num_points = points.size();
861 for (
size_t i = 0; i < num_points - 1; ++i) {
862 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
865 if (!got_dist || d < best_dist) {
872 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
873 points[num_points - 1]._v);
875 if (!got_dist || d < best_dist) {
896 PN_stdfloat CollisionBox::
897 dist_to_line_segment(
const LPoint2 &p,
901 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
919 return (p - t).length();
926 return (p - f).length();
928 return (p - t).length();
938 return (p - f).length();
940 return (p - t).length();
947 return (p - f).length();
949 return (p - t).length();
962 return (p - f).length();
964 return (p - t).length();
971 return (p - f).length();
973 return (p - t).length();
983 return (p - f).length();
985 return (p - t).length();
992 return (p - f).length();
994 return (p - t).length();
1015 for (
int i = 0; i < (int)points.size() - 1; i++) {
1016 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1020 if (is_right(p - points[points.size() - 1]._p,
1021 points[0]._p - points[points.size() - 1]._p)) {
1037 size_t num_points = points.size();
1038 for (
size_t i = 0; i < num_points; i++) {
1039 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1040 points[i]._v.normalize();
1066 for(
int i=0; i < 8; i++) {
1073 for(
int i=0; i < 6; i++) {
1074 _planes[i].write_datagram(me);
1076 for(
int i=0; i < 6; i++) {
1079 for(
int i=0; i < 6; i++) {
1081 for (
size_t j = 0; j < _points[i].size(); j++) {
1082 _points[i][j]._p.write_datagram(me);
1083 _points[i][j]._v.write_datagram(me);
1099 parse_params(params, scan, manager);
1100 me->fillin(scan, manager);
1114 CollisionSolid::fillin(scan, manager);
1118 for(
int i=0; i < 8; i++) {
1125 for(
int i=0; i < 6; i++) {
1126 _planes[i].read_datagram(scan);
1128 for(
int i=0; i < 6; i++) {
1131 for(
int i=0; i < 6; i++) {
1133 for (
size_t j = 0; j < size; j++) {
1138 _points[i].push_back(PointDef(p, v));
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
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.
bool get_respect_effective_normal() const
See set_respect_effective_normal().
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 ...
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
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).
This defines a bounding sphere, consisting of a center and a radius.
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...
A cuboid collision volume or object.
Base class for objects that can be written to and read from Bam files.
PandaNode * node() const
Returns the referenced node of the path.
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.
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.
LPlane get_plane(int n) const
Returns the nth face of the rectangular solid.
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...
void write_datagram(Datagram &destination) const
Writes the matrix to the Datagram using add_stdfloat().
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.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
int get_clip_effect() const
Returns the clip_effect bits for this clip plane.
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.
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.
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.
void read_datagram(DatagramIterator &source)
Reads the matrix from the Datagram using get_stdfloat().
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
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.
This is a two-component vector offset.
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
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...
Defines a series of disconnected triangles.
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 ...
LPlane set_plane(int n) const
Creates the nth face of the rectangular solid.
A class to retrieve the individual data elements previously stored in a Datagram. ...
This is a two-component point in space.
TypeHandle is the identifier used to differentiate C++ class types.
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
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 ...
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
A node that contains a plane.
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...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...