47PStatCollector CollisionPolygon::_volume_pcollector(
"Collision Volumes:CollisionPolygon");
48PStatCollector CollisionPolygon::_test_pcollector(
"Collision Tests:CollisionPolygon");
57 _points(copy._points),
58 _to_2d_mat(copy._to_2d_mat)
81 int num_points = end - begin;
90 for (pi = begin; pi != end && all_ok; ++pi) {
96 for (pj = begin; pj != pi && all_ok; ++pj) {
108 bool got_normal =
false;
109 for (
int i = 2; i < num_points && !got_normal; i++) {
110 LPlane plane(begin[0], begin[1], begin[i]);
111 LVector3 normal = plane.get_normal();
112 PN_stdfloat normal_length = normal.length();
113 got_normal = IS_THRESHOLD_EQUAL(normal_length, 1.0f, 0.001f);
130 return (_points.size() >= 3);
139 if (_points.size() < 3) {
144 LPoint2 p0 = _points[0]._p;
145 LPoint2 p1 = _points[1]._p;
146 PN_stdfloat dx1 = p1[0] - p0[0];
147 PN_stdfloat dy1 = p1[1] - p0[1];
151 PN_stdfloat dx2 = p1[0] - p0[0];
152 PN_stdfloat dy2 = p1[1] - p0[1];
153 int asum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0);
155 for (
size_t i = 0; i < _points.size() - 1; i++) {
157 p1 = _points[(i+3) % _points.size()]._p;
163 int csum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0);
179xform(
const LMatrix4 &mat) {
184 if (collide_cat.is_spam()) {
186 <<
"CollisionPolygon transformed by:\n";
187 mat.write(collide_cat.spam(
false), 2);
188 if (_points.empty()) {
189 collide_cat.spam(
false)
194 if (!_points.empty()) {
196 rederive_to_3d_mat(to_3d_mat);
198 epvector<LPoint3> verts;
199 verts.reserve(_points.size());
200 Points::const_iterator pi;
201 for (pi = _points.begin(); pi != _points.end(); ++pi) {
202 verts.push_back(to_3d((*pi)._p, to_3d_mat) * mat);
205 const LPoint3 *verts_begin = &verts[0];
206 const LPoint3 *verts_end = verts_begin + verts.size();
207 setup_points(verts_begin, verts_end);
210 CollisionSolid::xform(mat);
221 rederive_to_3d_mat(to_3d_mat);
223 LPoint2 median = _points[0]._p;
224 for (
int n = 1; n < (int)_points.size(); n++) {
225 median += _points[n]._p;
227 median /= _points.size();
229 return to_3d(median, to_3d_mat);
239 bool bounds_only)
const {
241 if (cpa ==
nullptr) {
244 return CollisionSolid::get_viz(trav, data, bounds_only);
247 if (collide_cat.is_debug()) {
249 <<
"drawing polygon with clip plane " << *cpa <<
"\n";
258 if (apply_clip_plane(new_points, cpa, data.get_net_transform(trav))) {
260 return CollisionSolid::get_viz(trav, data, bounds_only);
263 if (new_points.empty()) {
271 draw_polygon(viz_geom_node, bounds_viz_geom_node, new_points);
274 return bounds_viz_geom_node;
276 return viz_geom_node;
286 return _volume_pcollector;
295 return _test_pcollector;
301void CollisionPolygon::
302output(std::ostream &out)
const {
303 out <<
"cpolygon, (" << get_plane()
304 <<
"), " << _points.size() <<
" vertices";
310void CollisionPolygon::
311write(std::ostream &out,
int indent_level)
const {
312 indent(out, indent_level) << (*this) <<
"\n";
313 Points::const_iterator pi;
314 for (pi = _points.begin(); pi != _points.end(); ++pi) {
315 indent(out, indent_level + 2) << (*pi)._p <<
"\n";
319 rederive_to_3d_mat(to_3d_mat);
320 out <<
"In 3-d space:\n";
321 for (pi = _points.begin(); pi != _points.end(); ++pi) {
322 LVertex vert = to_3d((*pi)._p, to_3d_mat);
323 indent(out, indent_level + 2) << vert <<
"\n";
331compute_internal_bounds()
const {
332 if (_points.empty()) {
337 rederive_to_3d_mat(to_3d_mat);
339 Points::const_iterator pi = _points.begin();
340 LPoint3 p = to_3d((*pi)._p, to_3d_mat);
345 for (++pi; pi != _points.end(); ++pi) {
346 p = to_3d((*pi)._p, to_3d_mat);
348 n.set(min(n[0], p[0]),
351 x.set(max(x[0], p[0]),
365 if (_points.size() < 3) {
370 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
375 const LMatrix4 &wrt_mat = wrt_space->get_mat();
377 LPoint3 orig_center = sphere->get_center() * wrt_mat;
378 LPoint3 from_center = orig_center;
379 bool moved_from_center =
false;
380 PN_stdfloat t = 1.0f;
381 LPoint3 contact_point(from_center);
382 PN_stdfloat actual_t = 1.0f;
384 LVector3 from_radius_v =
385 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
386 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
387 PN_stdfloat from_radius = csqrt(from_radius_2);
389 if (wrt_prev_space != wrt_space) {
393 LPoint3 b = from_center;
394 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
395 LVector3 delta = b - a;
399 PN_stdfloat dot = delta.dot(get_normal());
404 if (IS_NEARLY_ZERO(dot)) {
417 PN_stdfloat dist_to_p = dist_to_plane(a);
418 t = (dist_to_p / -dot);
422 actual_t = ((dist_to_p - from_radius) / -dot);
423 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
424 contact_point = a + (actual_t * delta);
429 }
else if (t < 0.0f) {
431 moved_from_center =
true;
434 from_center = a + t * delta;
435 moved_from_center =
true;
442 if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001)) {
443 if (collide_cat.is_info()) {
446 <<
" has normal " << normal <<
" of length " << normal.length()
456 if (!get_plane().intersects_line(dist, from_center, -get_normal())) {
462 if (dist > from_radius || dist < -from_radius) {
467 LPoint2 p = to_2d(from_center - dist * get_normal());
468 PN_stdfloat edge_dist = 0.0f;
471 if (cpa !=
nullptr) {
474 if (apply_clip_plane(new_points, cpa, entry.
get_into_node_path().get_net_transform())) {
476 edge_dist = dist_to_polygon(p, _points);
478 }
else if (new_points.empty()) {
484 edge_dist = dist_to_polygon(p, new_points);
489 edge_dist = dist_to_polygon(p, _points);
495 if (edge_dist > from_radius) {
505 PN_stdfloat max_dist = from_radius;
506 if (edge_dist >= 0.0f) {
507 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
508 max_dist = csqrt(max_dist_2);
511 if (dist > max_dist || -dist > max_dist) {
516 if (collide_cat.is_debug()) {
523 PN_stdfloat into_depth = max_dist - dist;
524 if (moved_from_center) {
527 PN_stdfloat orig_dist;
528 get_plane().intersects_line(orig_dist, orig_center, -normal);
529 into_depth = max_dist - orig_dist;
532 new_entry->set_surface_normal(normal);
533 new_entry->set_surface_point(from_center - normal * dist);
534 new_entry->set_interior_point(from_center - normal * (dist + into_depth));
535 new_entry->set_contact_pos(contact_point);
536 new_entry->set_contact_normal(get_normal());
537 new_entry->set_t(actual_t);
548 if (_points.size() < 3) {
553 DCAST_INTO_R(line, entry.
get_from(),
nullptr);
555 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
557 LPoint3 from_origin = line->get_origin() * wrt_mat;
558 LVector3 from_direction = line->get_direction() * wrt_mat;
561 if (!get_plane().intersects_line(t, from_origin, from_direction)) {
566 LPoint3 plane_point = from_origin + t * from_direction;
567 LPoint2 p = to_2d(plane_point);
570 if (cpa !=
nullptr) {
573 if (apply_clip_plane(new_points, cpa, entry.
get_into_node_path().get_net_transform())) {
575 if (!point_is_inside(p, _points)) {
580 if (new_points.size() < 3) {
583 if (!point_is_inside(p, new_points)) {
590 if (!point_is_inside(p, _points)) {
595 if (collide_cat.is_debug()) {
604 new_entry->set_surface_normal(normal);
605 new_entry->set_surface_point(plane_point);
616 if (_points.size() < 3) {
621 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
623 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
625 LPoint3 from_origin = ray->get_origin() * wrt_mat;
626 LVector3 from_direction = ray->get_direction() * wrt_mat;
629 if (!get_plane().intersects_line(t, from_origin, from_direction)) {
639 LPoint3 plane_point = from_origin + t * from_direction;
640 LPoint2 p = to_2d(plane_point);
643 if (cpa !=
nullptr) {
646 if (apply_clip_plane(new_points, cpa, entry.
get_into_node_path().get_net_transform())) {
648 if (!point_is_inside(p, _points)) {
653 if (new_points.size() < 3) {
656 if (!point_is_inside(p, new_points)) {
663 if (!point_is_inside(p, _points)) {
668 if (collide_cat.is_debug()) {
677 new_entry->set_surface_normal(normal);
678 new_entry->set_surface_point(plane_point);
688test_intersection_from_segment(
const CollisionEntry &entry)
const {
689 if (_points.size() < 3) {
694 DCAST_INTO_R(segment, entry.
get_from(),
nullptr);
696 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
698 LPoint3 from_a = segment->get_point_a() * wrt_mat;
699 LPoint3 from_b = segment->get_point_b() * wrt_mat;
700 LPoint3 from_direction = from_b - from_a;
703 if (!get_plane().intersects_line(t, from_a, from_direction)) {
708 if (t < 0.0f || t > 1.0f) {
714 LPoint3 plane_point = from_a + t * from_direction;
715 LPoint2 p = to_2d(plane_point);
718 if (cpa !=
nullptr) {
721 if (apply_clip_plane(new_points, cpa, entry.
get_into_node_path().get_net_transform())) {
723 if (!point_is_inside(p, _points)) {
728 if (new_points.size() < 3) {
731 if (!point_is_inside(p, new_points)) {
738 if (!point_is_inside(p, _points)) {
743 if (collide_cat.is_debug()) {
752 new_entry->set_surface_normal(normal);
753 new_entry->set_surface_point(plane_point);
762test_intersection_from_capsule(
const CollisionEntry &entry)
const {
764 DCAST_INTO_R(capsule, entry.
get_from(),
nullptr);
766 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
768 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
769 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
770 LVector3 from_radius_v =
771 LVector3(capsule->get_radius(), 0.0f, 0.0f) * wrt_mat;
772 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
773 PN_stdfloat from_radius = csqrt(from_radius_2);
775 PN_stdfloat dist_a = get_plane().dist_to_plane(from_a);
776 PN_stdfloat dist_b = get_plane().dist_to_plane(from_b);
779 if (dist_a >= from_radius && dist_b >= from_radius) {
784 if (dist_a <= -from_radius && dist_b <= -from_radius) {
790 rederive_to_3d_mat(to_3d_mat);
793 LPoint3 intersect_3d;
794 if (!get_plane().intersects_line(intersect_3d, from_a, from_b)) {
796 intersect_3d = (from_a + from_b) * 0.5f;
800 LPoint2 intersect_2d = to_2d(intersect_3d);
801 LPoint2 closest_p_2d = intersect_2d;
802 PN_stdfloat best_dist_2 = -1;
804 size_t num_points = _points.size();
805 for (
size_t i = 0; i < num_points; ++i) {
806 const LPoint2 &p1 = _points[i]._p;
807 const LPoint2 &p2 = _points[(i + 1) % num_points]._p;
810 LVector2 v = intersect_2d - p1;
811 LVector2 pv = p2 - p1;
812 if (is_right(v, pv)) {
813 PN_stdfloat t = v.dot(pv) / pv.length_squared();
814 t = max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
816 LPoint2 p = p1 + pv * t;
817 PN_stdfloat d = (p - intersect_2d).length_squared();
818 if (best_dist_2 < 0 || d < best_dist_2) {
825 LPoint3 closest_p_3d = to_3d(closest_p_2d, to_3d_mat);
828 LVector3 from_v = from_b - from_a;
830 PN_stdfloat t = (closest_p_3d - from_a).dot(from_v) / from_v.length_squared();
831 LPoint3 ref_point_3d = from_a + from_v * max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
838 if (!get_plane().intersects_line(dist, ref_point_3d, -get_normal())) {
844 if (dist > from_radius || dist < -from_radius) {
849 LPoint2 ref_point_2d = to_2d(ref_point_3d);
850 LPoint2 surface_point_2d = ref_point_2d;
851 PN_stdfloat edge_dist_2 = -1;
853 for (
size_t i = 0; i < num_points; ++i) {
854 const LPoint2 &p1 = _points[i]._p;
855 const LPoint2 &p2 = _points[(i + 1) % num_points]._p;
858 LVector2 v = ref_point_2d - p1;
859 LVector2 pv = p2 - p1;
860 if (is_right(v, pv)) {
861 PN_stdfloat t = v.dot(pv) / pv.length_squared();
862 t = max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
864 LPoint2 p = p1 + pv * t;
865 PN_stdfloat d = (p - ref_point_2d).length_squared();
866 if (edge_dist_2 < 0 || d < edge_dist_2) {
867 surface_point_2d = p;
877 if (edge_dist_2 > from_radius_2) {
887 PN_stdfloat max_dist = from_radius;
888 if (edge_dist_2 >= 0.0f) {
889 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist_2, (PN_stdfloat)0.0);
890 max_dist = csqrt(max_dist_2);
893 if (dist > max_dist || -dist > max_dist) {
898 if (collide_cat.is_debug()) {
906 new_entry->set_surface_normal(normal);
908 if ((dist_a < 0 || dist_b < 0) && !IS_NEARLY_EQUAL(dist_a, dist_b)) {
911 LPoint3 deepest_3d = (dist_a < dist_b) ? from_a : from_b;
912 LPoint2 deepest_2d = to_2d(deepest_3d);
913 surface_point_2d = deepest_2d;
914 PN_stdfloat best_dist_2 = -1;
916 for (
size_t i = 0; i < num_points; ++i) {
917 const LPoint2 &p1 = _points[i]._p;
918 const LPoint2 &p2 = _points[(i + 1) % num_points]._p;
921 LVector2 v = deepest_2d - p1;
922 LVector2 pv = p2 - p1;
923 if (is_right(v, pv)) {
924 PN_stdfloat t = v.dot(pv) / pv.length_squared();
925 t = max(min(t, (PN_stdfloat)1), (PN_stdfloat)0);
927 LPoint2 p = p1 + pv * t;
928 PN_stdfloat d = (p - deepest_2d).length_squared();
929 if (best_dist_2 < 0 || d < best_dist_2) {
930 surface_point_2d = p;
936 if (best_dist_2 < 0) {
938 new_entry->set_surface_point(deepest_3d - normal * min(dist_a, dist_b));
939 new_entry->set_interior_point(deepest_3d - normal * from_radius);
945 LPoint3 surface_point = to_3d(surface_point_2d, to_3d_mat);
946 LPoint3 interior_point = ref_point_3d - normal * max_dist;
947 new_entry->set_surface_point(surface_point);
948 new_entry->set_interior_point((interior_point - surface_point).project(normal) + surface_point);
957test_intersection_from_parabola(
const CollisionEntry &entry)
const {
958 if (_points.size() < 3) {
963 DCAST_INTO_R(parabola, entry.
get_from(),
nullptr);
965 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
969 local_p.xform(wrt_mat);
972 if (!get_plane().intersects_parabola(t1, t2, local_p)) {
978 if (t1 >= parabola->
get_t1() && t1 <= parabola->get_t2()) {
979 if (t2 >= parabola->
get_t1() && t2 <= parabola->get_t2()) {
988 }
else if (t2 >= parabola->
get_t1() && t2 <= parabola->get_t2()) {
997 LPoint3 plane_point = local_p.calc_point(t);
998 LPoint2 p = to_2d(plane_point);
1001 if (cpa !=
nullptr) {
1004 if (apply_clip_plane(new_points, cpa, entry.
get_into_node_path().get_net_transform())) {
1006 if (!point_is_inside(p, _points)) {
1011 if (new_points.size() < 3) {
1014 if (!point_is_inside(p, new_points)) {
1021 if (!point_is_inside(p, _points)) {
1026 if (collide_cat.is_debug()) {
1035 new_entry->set_surface_normal(normal);
1036 new_entry->set_surface_point(plane_point);
1048 DCAST_INTO_R(box, entry.
get_from(),
nullptr);
1052 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
1053 LMatrix4 plane_mat = wrt_mat * _to_2d_mat;
1055 LPoint3 from_center = box->get_center() * plane_mat;
1056 LVector3 from_extents = box->get_dimensions() * 0.5f;
1059 LVecBase3 box_x = plane_mat.get_row3(0) * from_extents[0];
1060 LVecBase3 box_y = plane_mat.get_row3(1) * from_extents[1];
1061 LVecBase3 box_z = plane_mat.get_row3(2) * from_extents[2];
1064 if (cabs(from_center[1]) > cabs(box_x[1]) + cabs(box_y[1]) + cabs(box_z[1])) {
1070 PN_stdfloat r1, center, r2;
1072 r1 = cabs(box_x.dot(box_x)) + cabs(box_y.dot(box_x)) + cabs(box_z.dot(box_x));
1073 project(box_x, center, r2);
1074 if (cabs(from_center.dot(box_x) - center) > r1 + r2) {
1078 r1 = cabs(box_x.dot(box_y)) + cabs(box_y.dot(box_y)) + cabs(box_z.dot(box_y));
1079 project(box_y, center, r2);
1080 if (cabs(from_center.dot(box_y) - center) > r1 + r2) {
1084 r1 = cabs(box_x.dot(box_z)) + cabs(box_y.dot(box_z)) + cabs(box_z.dot(box_z));
1085 project(box_z, center, r2);
1086 if (cabs(from_center.dot(box_z) - center) > r1 + r2) {
1092 Points::const_iterator pi;
1093 for (pi = _points.begin(); pi != _points.end(); ++pi) {
1094 const PointDef &pd = *pi;
1097 axis.set(-box_x[1] * pd._v[1],
1098 box_x[0] * pd._v[1] - box_x[2] * pd._v[0],
1099 box_x[1] * pd._v[0]);
1100 r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
1101 project(axis, center, r2);
1102 if (cabs(from_center.dot(axis) - center) > r1 + r2) {
1106 axis.set(-box_y[1] * pd._v[1],
1107 box_y[0] * pd._v[1] - box_y[2] * pd._v[0],
1108 box_y[1] * pd._v[0]);
1109 r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
1110 project(axis, center, r2);
1111 if (cabs(from_center.dot(axis) - center) > r1 + r2) {
1115 axis.set(-box_z[1] * pd._v[1],
1116 box_z[0] * pd._v[1] - box_z[2] * pd._v[0],
1117 box_z[1] * pd._v[0]);
1118 r1 = cabs(box_x.dot(axis)) + cabs(box_y.dot(axis)) + cabs(box_z.dot(axis));
1119 project(axis, center, r2);
1120 if (cabs(from_center.dot(axis) - center) > r1 + r2) {
1125 if (collide_cat.is_debug()) {
1133 new_entry->set_surface_normal(normal);
1138 LPoint3 interior_point = box->get_center() * wrt_mat +
1139 wrt_mat.get_row3(0) * from_extents[0] * ((box_x[1] > 0) - (box_x[1] < 0)) +
1140 wrt_mat.get_row3(1) * from_extents[1] * ((box_y[1] > 0) - (box_y[1] < 0)) +
1141 wrt_mat.get_row3(2) * from_extents[2] * ((box_z[1] > 0) - (box_z[1] < 0));
1144 new_entry->set_surface_point(get_plane().project(interior_point));
1145 new_entry->set_interior_point(interior_point);
1154void CollisionPolygon::
1156 if (collide_cat.is_debug()) {
1158 <<
"Recomputing viz for " << *
this <<
"\n";
1160 nassertv(_viz_geom !=
nullptr && _bounds_viz_geom !=
nullptr);
1161 draw_polygon(_viz_geom, _bounds_viz_geom, _points);
1172PN_stdfloat CollisionPolygon::
1173dist_to_line_segment(
const LPoint2 &p,
1174 const LPoint2 &f,
const LPoint2 &t,
1175 const LVector2 &v) {
1176 LVector2 v1 = (p - f);
1177 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
1183 LPoint2 q = p + LVector2(-v[1], v[0]) * d;
1193 return (p - f).length();
1194 }
if (q[0] > t[0]) {
1195 return (p - t).length();
1202 return (p - f).length();
1203 }
if (q[1] > t[1]) {
1204 return (p - t).length();
1214 return (p - f).length();
1215 }
if (q[0] > t[0]) {
1216 return (p - t).length();
1223 return (p - f).length();
1224 }
if (q[1] < t[1]) {
1225 return (p - t).length();
1238 return (p - f).length();
1239 }
if (q[0] < t[0]) {
1240 return (p - t).length();
1247 return (p - f).length();
1248 }
if (q[1] > t[1]) {
1249 return (p - t).length();
1256 if (-v[0] > -v[1]) {
1259 return (p - f).length();
1260 }
if (q[0] < t[0]) {
1261 return (p - t).length();
1268 return (p - f).length();
1269 }
if (q[1] < t[1]) {
1270 return (p - t).length();
1284void CollisionPolygon::
1285compute_vectors(Points &points) {
1286 size_t num_points = points.size();
1287 for (
size_t i = 0; i < num_points; i++) {
1288 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1289 points[i]._v.normalize();
1297void CollisionPolygon::
1300 if (points.size() < 3) {
1301 if (collide_cat.is_debug()) {
1303 <<
"(Degenerate poly, ignoring.)\n";
1309 rederive_to_3d_mat(to_3d_mat);
1316 Points::const_iterator pi;
1317 for (pi = points.begin(); pi != points.end(); ++pi) {
1318 vertex.add_data3(to_3d((*pi)._p, to_3d_mat));
1322 body->add_consecutive_vertices(0, points.size());
1323 body->close_primitive();
1326 border->add_consecutive_vertices(0, points.size());
1327 border->add_vertex(0);
1328 border->close_primitive();
1331 geom1->add_primitive(body);
1334 geom2->add_primitive(border);
1348bool CollisionPolygon::
1355 for (
int i = 0; i < (int)points.size() - 1; i++) {
1356 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1360 if (is_right(p - points[points.size() - 1]._p,
1361 points[0]._p - points[points.size() - 1]._p)) {
1373PN_stdfloat CollisionPolygon::
1384 bool got_dist =
false;
1385 PN_stdfloat best_dist = -1.0f;
1387 size_t num_points = points.size();
1388 for (
size_t i = 0; i < num_points - 1; ++i) {
1389 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
1392 if (!got_dist || d < best_dist) {
1399 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
1400 points[num_points - 1]._v);
1402 if (!got_dist || d < best_dist) {
1415void CollisionPolygon::
1416project(
const LVector3 &axis, PN_stdfloat ¢er, PN_stdfloat &extent)
const {
1417 PN_stdfloat begin, end;
1419 Points::const_iterator pi;
1420 pi = _points.begin();
1422 const LPoint2 &point = (*pi)._p;
1423 begin = point[0] * axis[0] + point[1] * axis[2];
1426 for (; pi != _points.end(); ++pi) {
1427 const LPoint2 &point = (*pi)._p;
1429 PN_stdfloat t = point[0] * axis[0] + point[1] * axis[2];
1430 begin = min(begin, t);
1434 center = (end + begin) * 0.5f;
1435 extent = cabs((end - begin) * 0.5f);
1441void CollisionPolygon::
1442setup_points(
const LPoint3 *begin,
const LPoint3 *end) {
1443 int num_points = end - begin;
1444 nassertv(num_points >= 3);
1450 LVector3 normal = LVector3::zero();
1456 for (
int i = 0; i < num_points; i++) {
1457 const LPoint3 &p0 = begin[i];
1458 const LPoint3 &p1 = begin[(i + 1) % num_points];
1459 normal[0] += p0[1] * p1[2] - p0[2] * p1[1];
1460 normal[1] += p0[2] * p1[0] - p0[0] * p1[2];
1461 normal[2] += p0[0] * p1[1] - p0[1] * p1[0];
1464 if (normal.length_squared() == 0.0f) {
1473 collide_cat.error() <<
"Invalid points in CollisionPolygon:\n";
1475 for (pi = begin; pi != end; ++pi) {
1476 collide_cat.error(
false) <<
" " << (*pi) <<
"\n";
1478 collide_cat.error(
false)
1479 <<
" normal " << normal <<
" with length " << normal.length() <<
"\n";
1485 if (collide_cat.is_spam()) {
1487 <<
"CollisionPolygon defined with " << num_points <<
" vertices:\n";
1489 for (pi = begin; pi != end; ++pi) {
1490 collide_cat.spam(
false) <<
" " << (*pi) <<
"\n";
1495 set_plane(LPlane(normal, begin[0]));
1500 calc_to_3d_mat(to_3d_mat);
1503 _to_2d_mat.invert_from(to_3d_mat);
1508 for (pi = begin; pi != end; ++pi) {
1509 LPoint3 point = (*pi) * _to_2d_mat;
1510 _points.push_back(PointDef(point[0], point[2]));
1513 nassertv(_points.size() >= 3);
1531 compute_vectors(_points);
1538LPoint3 CollisionPolygon::
1539legacy_to_3d(
const LVecBase2 &point2d,
int axis)
const {
1540 nassertr(!point2d.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
1542 LVector3 normal = get_normal();
1543 PN_stdfloat D = get_plane()[3];
1545 nassertr(!normal.is_nan(), LPoint3(0.0f, 0.0f, 0.0f));
1546 nassertr(!cnan(D), LPoint3(0.0f, 0.0f, 0.0f));
1550 return LPoint3(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], point2d[0], point2d[1]);
1553 return LPoint3(point2d[0],
1554 -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], point2d[1]);
1557 return LPoint3(point2d[0], point2d[1],
1558 -(normal[0]*point2d[0] + normal[1]*point2d[1] + D)/normal[2]);
1561 nassertr(
false, LPoint3(0.0f, 0.0f, 0.0f));
1562 return LPoint3(0.0f, 0.0f, 0.0f);
1573bool CollisionPolygon::
1576 const LPlane &plane)
const {
1578 if (source_points.empty()) {
1584 if (!plane.intersects_plane(from3d, delta3d, get_plane())) {
1587 if (plane.dist_to_plane(get_plane().
get_point()) < 0.0) {
1590 new_points = source_points;
1598 LPoint2 from2d = to_2d(from3d);
1599 LVector2 delta2d = to_2d(delta3d);
1601 PN_stdfloat a = -delta2d[1];
1602 PN_stdfloat b = delta2d[0];
1603 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
1611 new_points.reserve(source_points.size() + 1);
1613 LPoint2 last_point = source_points.back()._p;
1614 bool last_is_in = !is_right(last_point - from2d, delta2d);
1615 bool all_in = last_is_in;
1616 Points::const_iterator pi;
1617 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
1618 const LPoint2 &this_point = (*pi)._p;
1619 bool this_is_in = !is_right(this_point - from2d, delta2d);
1623 bool crossed_over = (this_is_in != last_is_in);
1627 LVector2 d = this_point - last_point;
1628 PN_stdfloat denom = (a * d[0] + b * d[1]);
1630 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
1631 LPoint2 p = last_point + t * d;
1633 new_points.push_back(PointDef(p[0], p[1]));
1634 last_is_in = this_is_in;
1640 new_points.push_back(PointDef(this_point[0], this_point[1]));
1645 last_point = this_point;
1658bool CollisionPolygon::
1665 bool first_plane =
true;
1667 for (
int i = 0; i < num_planes; i++) {
1672 net_transform->invert_compose(plane_path.get_net_transform());
1674 LPlane plane = plane_node->
get_plane() * new_transform->get_mat();
1676 first_plane =
false;
1677 if (!clip_polygon(new_points, _points, plane)) {
1682 last_points.swap(new_points);
1683 if (!clip_polygon(new_points, last_points, plane)) {
1691 compute_vectors(new_points);
1705 for (
size_t i = 0; i < _points.size(); i++) {
1706 _points[i]._p.write_datagram(me);
1707 _points[i]._v.write_datagram(me);
1709 _to_2d_mat.write_datagram(me);
1717void CollisionPolygon::
1719 CollisionPlane::fillin(scan, manager);
1722 for (
size_t i = 0; i < size; i++) {
1725 p.read_datagram(scan);
1726 v.read_datagram(scan);
1727 _points.push_back(PointDef(p, v));
1729 _to_2d_mat.read_datagram(scan);
1735 if (_points.size() >= 3) {
1737 rederive_to_3d_mat(to_3d_mat);
1739 epvector<LPoint3> verts;
1740 verts.reserve(_points.size());
1741 Points::const_iterator pi;
1742 for (pi = _points.begin(); pi != _points.end(); ++pi) {
1743 verts.push_back(to_3d((*pi)._p, to_3d_mat));
1746 const LPoint3 *verts_begin = &verts[0];
1747 const LPoint3 *verts_end = verts_begin + verts.size();
1748 setup_points(verts_begin, verts_end);
1764 me->fillin(scan, manager);
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
void parse_params(const FactoryParams ¶ms, DatagramIterator &scan, BamReader *&manager)
Takes in a FactoryParams, passed from a WritableFactory into any TypedWritable's make function,...
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...
int get_file_minor_ver() const
Returns the minor version number of the Bam file currently being read.
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
An axis-aligned bounding box; that is, a minimum and maximum coordinate triple.
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
This functions similarly to a LightAttrib.
get_on_plane
Returns the nth plane enabled by the attribute, sorted in render order.
get_num_on_planes
Returns the number of planes that are enabled by the attribute.
A cuboid collision volume or object.
This implements a solid consisting of a cylinder with hemispherical endcaps, also known as a capsule ...
Defines a single collision event.
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...
get_from
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
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_parabola
Returns the parabola specified by this solid.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
static bool verify_points(const LPoint3 *begin, const LPoint3 *end)
Verifies that the indicated set of points will define a valid CollisionPolygon: that is,...
get_point
Returns the nth vertex of the CollisionPolygon, expressed in 3-D space.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
is_concave
Returns true if the CollisionPolygon appears to be concave, or false if it is safely convex.
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
static void register_with_read_factory()
Factory method to generate a CollisionPolygon object.
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
static TypedWritable * make_CollisionPolygon(const FactoryParams ¶ms)
Factory method to generate a CollisionPolygon object.
is_valid
Returns true if the CollisionPolygon is valid (that is, it has at least three vertices),...
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
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,...
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.
This collects together the pieces of data that are accumulated for each node while walking the scene ...
This object performs a depth-first traversal of the scene graph, with optional view-frustum culling,...
A class to retrieve the individual data elements previously stored in a Datagram.
uint16_t get_uint16()
Extracts an unsigned 16-bit integer.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
void add_uint16(uint16_t value)
Adds an unsigned 16-bit integer to the datagram.
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.
Defines a series of line strips.
A node that holds Geom objects, renderable pieces of geometry.
void add_geom(Geom *geom, const RenderState *state=RenderState::make_empty())
Adds a new Geom to the node.
Defines a series of triangle fans.
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
A container for geometry primitives.
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
PandaNode * node() const
Returns the referenced node of the path.
A lightweight class that represents a single element that may be timed and/or counted via stats.
A basic node of the scene graph or data graph.
A node that contains a plane.
get_clip_effect
Returns the clip_effect bits for this clip plane.
get_plane
Returns the plane represented by the PlaneNode.
TypeHandle is the identifier used to differentiate C++ class types.
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.
std::ostream & indent(std::ostream &out, int indent_level)
A handy function for doing text formatting.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.