Panda3D

collisionBox.cxx
Go to the documentation of this file.
1 /**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file collisionBox.cxx
10 * @author amith tudur
11 * @date 2009-07-31
12 */
13
14 #include "collisionBox.h"
15 #include "collisionLine.h"
16 #include "collisionRay.h"
17 #include "collisionSphere.h"
18 #include "collisionSegment.h"
19 #include "collisionCapsule.h"
20 #include "collisionHandler.h"
21 #include "collisionEntry.h"
22 #include "config_collide.h"
23 #include "boundingSphere.h"
24 #include "datagram.h"
25 #include "datagramIterator.h"
26 #include "bamReader.h"
27 #include "bamWriter.h"
28 #include "nearly_zero.h"
29 #include "cmath.h"
30 #include "mathNumbers.h"
31 #include "geom.h"
32 #include "geomLines.h"
33 #include "geomTriangles.h"
34 #include "geomVertexWriter.h"
35 #include "config_mathutil.h"
36 #include "dcast.h"
37
38 #include <math.h>
39
40 using std::max;
41 using std::min;
42
43 PStatCollector CollisionBox::_volume_pcollector("Collision Volumes:CollisionBox");
44 PStatCollector CollisionBox::_test_pcollector("Collision Tests:CollisionBox");
45 TypeHandle CollisionBox::_type_handle;
46
47 const int CollisionBox::plane_def[6][4] = {
48 {0, 4, 5, 1},
49 {4, 6, 7, 5},
50 {6, 2, 3, 7},
51 {2, 0, 1, 3},
52 {1, 5, 7, 3},
53 {2, 6, 4, 0},
54 };
55
56 /**
57 *
58 */
59 CollisionSolid *CollisionBox::
60 make_copy() {
61 return new CollisionBox(*this);
62 }
63
64 /**
65 * Compute parameters for each of the box's sides
66 */
67 void CollisionBox::
68 setup_box(){
69 for(int plane = 0; plane < 6; plane++) {
70 LPoint3 array[4];
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]);
75 setup_points(array, array+4, plane);
76 }
77 }
78
79 /**
80 * Computes the plane and 2d projection of points that make up this side.
81 */
82 void CollisionBox::
83 setup_points(const LPoint3 *begin, const LPoint3 *end, int plane) {
84 int num_points = end - begin;
85 nassertv(num_points >= 3);
86
87 _points[plane].clear();
88
89 // Construct a matrix that rotates the points from the (X,0,Z) plane into
90 // the 3-d plane.
91 LMatrix4 to_3d_mat;
92 calc_to_3d_mat(to_3d_mat, plane);
93
94 // And the inverse matrix rotates points from 3-d space into the 2-d plane.
95 _to_2d_mat[plane].invert_from(to_3d_mat);
96
97 // Now project all of the points onto the 2-d plane.
98
99 const LPoint3 *pi;
100 for (pi = begin; pi != end; ++pi) {
101 LPoint3 point = (*pi) * _to_2d_mat[plane];
102 _points[plane].push_back(PointDef(point[0], point[2]));
103 }
104
105 nassertv(_points[plane].size() >= 3);
106
107 #ifndef NDEBUG
108 /*
109 // Now make sure the points define a convex polygon.
110 if (is_concave()) {
111 collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
112 const LPoint3 *pi;
113 for (pi = begin; pi != end; ++pi) {
114 collide_cat.error(false) << " " << (*pi) << "\n";
115 }
116 collide_cat.error(false)
117 << " normal " << normal << " with length " << normal.length() << "\n";
118 _points.clear();
119 }
120 */
121 #endif
122
123 compute_vectors(_points[plane]);
124 }
125
126 /**
127 * First Dispatch point for box as a FROM object
128 */
129 PT(CollisionEntry) CollisionBox::
130 test_intersection(const CollisionEntry &entry) const {
131 return entry.get_into()->test_intersection_from_box(entry);
132 }
133
134 /**
135 * Transforms the solid by the indicated matrix.
136 */
137 void CollisionBox::
138 xform(const LMatrix4 &mat) {
139 _min = _min * mat;
140 _max = _max * mat;
141 _center = _center * mat;
142 for(int v = 0; v < 8; v++) {
143 _vertex[v] = _vertex[v] * mat;
144 }
145 for(int p = 0; p < 6 ; p++) {
146 _planes[p] = set_plane(p);
147 }
148 _x = _vertex[0].get_x() - _center.get_x();
149 _y = _vertex[0].get_y() - _center.get_y();
150 _z = _vertex[0].get_z() - _center.get_z();
151 _radius = sqrt(_x * _x + _y * _y + _z * _z);
152 setup_box();
153 mark_viz_stale();
154 mark_internal_bounds_stale();
155 }
156
157 /**
158 * Returns the point in space deemed to be the "origin" of the solid for
159 * collision purposes. The closest intersection point to this origin point is
160 * considered to be the most significant.
161 */
162 LPoint3 CollisionBox::
163 get_collision_origin() const {
164 return _center;
165 }
166
167 /**
168 * Returns a PStatCollector that is used to count the number of bounding
169 * volume tests made against a solid of this type in a given frame.
170 */
171 PStatCollector &CollisionBox::
172 get_volume_pcollector() {
173 return _volume_pcollector;
174 }
175
176 /**
177 * Returns a PStatCollector that is used to count the number of intersection
178 * tests made against a solid of this type in a given frame.
179 */
180 PStatCollector &CollisionBox::
181 get_test_pcollector() {
182 return _test_pcollector;
183 }
184
185 /**
186 *
187 */
188 void CollisionBox::
189 output(std::ostream &out) const {
190 out << "box, (" << get_min() << ") to (" << get_max() << ")";
191 }
192
193 /**
194 * Sphere is chosen as the Bounding Volume type for speed and efficiency
195 */
196 PT(BoundingVolume) CollisionBox::
197 compute_internal_bounds() const {
198 return new BoundingSphere(_center, _radius);
199 }
200
201 /**
202 * Double dispatch point for sphere as FROM object
203 */
204 PT(CollisionEntry) CollisionBox::
205 test_intersection_from_sphere(const CollisionEntry &entry) const {
206
207 const CollisionSphere *sphere;
208 DCAST_INTO_R(sphere, entry.get_from(), nullptr);
209
210 CPT(TransformState) wrt_space = entry.get_wrt_space();
211 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
212
213 const LMatrix4 &wrt_mat = wrt_space->get_mat();
214
215 LPoint3 orig_center = sphere->get_center() * wrt_mat;
216 LPoint3 from_center = orig_center;
217 bool moved_from_center = false;
218 PN_stdfloat t = 1.0f;
219 LPoint3 contact_point(from_center);
220 PN_stdfloat actual_t = 1.0f;
221
222 LVector3 from_radius_v =
223 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
224 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
225 PN_stdfloat from_radius = csqrt(from_radius_2);
226
227 int ip;
228 PN_stdfloat max_dist = 0.0;
229 PN_stdfloat dist = 0.0;
230 bool intersect;
231 LPlane plane;
232 LVector3 normal;
233
234 for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
235 plane = get_plane( ip );
236 if (_points[ip].size() < 3) {
237 continue;
238 }
239 if (wrt_prev_space != wrt_space) {
240 // If we have a delta between the previous position and the current
241 // position, we use that to determine some more properties of the
242 // collision.
243 LPoint3 b = from_center;
244 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
245 LVector3 delta = b - a;
246
247 // First, there is no collision if the "from" object is definitely
248 // moving in the same direction as the plane's normal.
249 PN_stdfloat dot = delta.dot(plane.get_normal());
250 if (dot > 0.1f) {
251 continue; // no intersection
252 }
253
254 if (IS_NEARLY_ZERO(dot)) {
255 // If we're moving parallel to the plane, the sphere is tested at its
256 // final point. Leave it as it is.
257
258 } else {
259 /*
260 * Otherwise, we're moving into the plane; the sphere is tested at the point
261 * along its path that is closest to intersecting the plane. This may be the
262 * actual intersection point, or it may be the starting point or the final
263 * point. dot is equal to the (negative) magnitude of 'delta' along the
264 * direction of the plane normal t = ratio of (distance from start pos to
265 * plane) to (distance from start pos to end pos), along axis of plane normal
266 */
267 PN_stdfloat dist_to_p = plane.dist_to_plane(a);
268 t = (dist_to_p / -dot);
269
270 // also compute the actual contact point and time of contact for
271 // handlers that need it
272 actual_t = ((dist_to_p - from_radius) / -dot);
273 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
274 contact_point = a + (actual_t * delta);
275
276 if (t >= 1.0f) {
277 // Leave it where it is.
278
279 } else if (t < 0.0f) {
280 from_center = a;
281 moved_from_center = true;
282 } else {
283 from_center = a + t * delta;
284 moved_from_center = true;
285 }
286 }
287 }
288
289 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
290
291 #ifndef NDEBUG
292 /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
293 std::cout
294 << "polygon within " << entry.get_into_node_path()
295 << " has normal " << normal << " of length " << normal.length()
296 << "\n";
297 normal.normalize();
298 }*/
299 #endif
300
301 // The nearest point within the plane to our center is the intersection of
302 // the line (center, center - normal) with the plane.
303
304 if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
305 // No intersection with plane? This means the plane's effective normal
306 // was within the plane itself. A useless polygon.
307 continue;
308 }
309
310 if (dist > from_radius || dist < -from_radius) {
311 // No intersection with the plane.
312 continue;
313 }
314
315 LPoint2 p = to_2d(from_center - dist * plane.get_normal(), ip);
316 PN_stdfloat edge_dist = 0.0f;
317
318 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
319 if (cpa != nullptr) {
320 // We have a clip plane; apply it.
321 Points new_points;
322 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
323 // All points are behind the clip plane; just do the default test.
324 edge_dist = dist_to_polygon(p, _points[ip]);
325 } else if (new_points.empty()) {
326 // The polygon is completely clipped.
327 continue;
328 } else {
329 // Test against the clipped polygon.
330 edge_dist = dist_to_polygon(p, new_points);
331 }
332 } else {
333 // No clip plane is in effect. Do the default test.
334 edge_dist = dist_to_polygon(p, _points[ip]);
335 }
336
337 max_dist = from_radius;
338
339 // Now we have edge_dist, which is the distance from the sphere center to
340 // the nearest edge of the polygon, within the polygon's plane.
341 // edge_dist<0 means the point is within the polygon.
342 if(edge_dist < 0) {
343 intersect = true;
344 continue;
345 }
346
347 if((edge_dist > 0) &&
348 ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
349 // No intersection; the circle is outside the polygon.
350 continue;
351 }
352
353 // The sphere appears to intersect the polygon. If the edge is less than
354 // from_radius away, the sphere may be resting on an edge of the polygon.
355 // Determine how far the center of the sphere must remain from the plane,
356 // based on its distance from the nearest edge.
357
358 if (edge_dist >= 0.0f) {
359 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
360 max_dist = csqrt(max_dist_2);
361 }
362
363 if (dist > max_dist) {
364 // There's no intersection: the sphere is hanging off the edge.
365 continue;
366 }
367 intersect = true;
368 }
369 if( !intersect )
370 return nullptr;
371
372 if (collide_cat.is_debug()) {
373 collide_cat.debug()
374 << "intersection detected from " << entry.get_from_node_path()
375 << " into " << entry.get_into_node_path() << "\n";
376 }
377
378 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
379
380 PN_stdfloat into_depth = max_dist - dist;
381 if (moved_from_center) {
382 // We have to base the depth of intersection on the sphere's final resting
383 // point, not the point from which we tested the intersection.
384 PN_stdfloat orig_dist;
385 plane.intersects_line(orig_dist, orig_center, -normal);
386 into_depth = max_dist - orig_dist;
387 }
388
389 // Clamp the surface point to the box bounds.
390 LPoint3 surface = from_center - normal * dist;
391 surface = surface.fmax(_min);
392 surface = surface.fmin(_max);
393
394 new_entry->set_surface_normal(normal);
395 new_entry->set_surface_point(surface);
396 new_entry->set_interior_point(surface - normal * into_depth);
397 new_entry->set_contact_pos(contact_point);
398 new_entry->set_contact_normal(plane.get_normal());
399 new_entry->set_t(actual_t);
400
401 return new_entry;
402 }
403
404 /**
405 *
406 */
407 PT(CollisionEntry) CollisionBox::
408 test_intersection_from_line(const CollisionEntry &entry) const {
409 const CollisionLine *line;
410 DCAST_INTO_R(line, entry.get_from(), nullptr);
411
412 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
413
414 LPoint3 from_origin = line->get_origin() * wrt_mat;
415 LVector3 from_direction = line->get_direction() * wrt_mat;
416
417 double t1, t2;
418 if (!intersects_line(t1, t2, from_origin, from_direction)) {
419 // No intersection.
420 return nullptr;
421 }
422
423 if (collide_cat.is_debug()) {
424 collide_cat.debug()
425 << "intersection detected from " << entry.get_from_node_path()
426 << " into " << entry.get_into_node_path() << "\n";
427 }
428 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
429
430 LPoint3 point = from_origin + t1 * from_direction;
431 new_entry->set_surface_point(point);
432
433 if (has_effective_normal() && line->get_respect_effective_normal()) {
434 new_entry->set_surface_normal(get_effective_normal());
435 } else {
436 LVector3 normal(
437 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
438 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
439 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
440 );
441 normal.normalize();
442 new_entry->set_surface_normal(normal);
443 }
444
445 return new_entry;
446 }
447
448 /**
449 * Double dispatch point for ray as a FROM object
450 */
451 PT(CollisionEntry) CollisionBox::
452 test_intersection_from_ray(const CollisionEntry &entry) const {
453 const CollisionRay *ray;
454 DCAST_INTO_R(ray, entry.get_from(), nullptr);
455 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
456
457 LPoint3 from_origin = ray->get_origin() * wrt_mat;
458 LVector3 from_direction = ray->get_direction() * wrt_mat;
459
460 double t1, t2;
461 if (!intersects_line(t1, t2, from_origin, from_direction) || (t1 < 0.0 && t2 < 0.0)) {
462 // No intersection.
463 return nullptr;
464 }
465
466 if (collide_cat.is_debug()) {
467 collide_cat.debug()
468 << "intersection detected from " << entry.get_from_node_path()
469 << " into " << entry.get_into_node_path() << "\n";
470 }
471 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
472
473 if (t1 < 0.0) {
474 // The origin is inside the box, so we take the exit as our surface point.
475 new_entry->set_interior_point(from_origin);
476 t1 = t2;
477 }
478
479 LPoint3 point = from_origin + t1 * from_direction;
480 new_entry->set_surface_point(point);
481
482 if (has_effective_normal() && ray->get_respect_effective_normal()) {
483 new_entry->set_surface_normal(get_effective_normal());
484 } else {
485 LVector3 normal(
486 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
487 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
488 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
489 );
490 normal.normalize();
491 new_entry->set_surface_normal(normal);
492 }
493
494 return new_entry;
495 }
496
497 /**
498 * Double dispatch point for segment as a FROM object
499 */
500 PT(CollisionEntry) CollisionBox::
501 test_intersection_from_segment(const CollisionEntry &entry) const {
502 const CollisionSegment *seg;
503 DCAST_INTO_R(seg, entry.get_from(), nullptr);
504 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
505
506 LPoint3 from_origin = seg->get_point_a() * wrt_mat;
507 LPoint3 from_extent = seg->get_point_b() * wrt_mat;
508 LVector3 from_direction = from_extent - from_origin;
509
510 double t1, t2;
511 if (!intersects_line(t1, t2, from_origin, from_direction) ||
512 (t1 < 0.0 && t2 < 0.0) || (t1 > 1.0 && t2 > 1.0)) {
513 // No intersection.
514 return nullptr;
515 }
516
517 if (collide_cat.is_debug()) {
518 collide_cat.debug()
519 << "intersection detected from " << entry.get_from_node_path()
520 << " into " << entry.get_into_node_path() << "\n";
521 }
522 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
523
524 // In case the segment is entirely inside the cube, we consider the point
525 // closest to the surface as our entry point.
526 if (t1 < (1.0 - t2)) {
527 std::swap(t1, t2);
528 }
529
530 // Our interior point is the closest point to t2 that is inside the segment.
531 new_entry->set_interior_point(from_origin + std::min(std::max(t2, 0.0), 1.0) * from_direction);
532
533 LPoint3 point = from_origin + t1 * from_direction;
534 new_entry->set_surface_point(point);
535
536 if (has_effective_normal() && seg->get_respect_effective_normal()) {
537 new_entry->set_surface_normal(get_effective_normal());
538 } else {
539 LVector3 normal(
540 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
541 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
542 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
543 );
544 normal.normalize();
545 new_entry->set_surface_normal(normal);
546 }
547
548 return new_entry;
549 }
550
551 /**
552 * Double dispatch point for capsule as a FROM object
553 */
554 PT(CollisionEntry) CollisionBox::
555 test_intersection_from_capsule(const CollisionEntry &entry) const {
556 const CollisionCapsule *capsule;
557 DCAST_INTO_R(capsule, entry.get_from(), nullptr);
558
559 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
560
561 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
562 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
563 LVector3 from_direction = from_b - from_a;
564 PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, capsule->get_radius())).length_squared();
565 PN_stdfloat radius = csqrt(radius_sq);
566
567 LPoint3 box_min = get_min();
568 LPoint3 box_max = get_max();
569 LVector3 dimensions = box_max - box_min;
570
571 // The method below is inspired by Christer Ericson's book Real-Time
572 // Collision Detection. Instead of testing a capsule against a box, we test
573 // a segment against an box that is oversized by the capsule radius.
574
575 // First, we test if the line segment intersects a box with its faces
576 // expanded outwards by the capsule radius. If not, there is no collision.
577 double t1, t2;
578 if (!intersects_line(t1, t2, from_a, from_direction, radius)) {
579 return nullptr;
580 }
581
582 if (t2 < 0.0 || t1 > 1.0) {
583 return nullptr;
584 }
585
586 t1 = std::min(1.0, std::max(0.0, (t1 + t2) * 0.5));
587 LPoint3 point = from_a + from_direction * t1;
588
589 // We now have a point of intersection between the line segment and the
590 // oversized box. Check on how many axes it lies outside the box. If it is
591 // less than two, we know that it does not lie in one of the rounded regions
592 // of the oversized rounded box, and it is a guaranteed hit. Otherwise, we
593 // will need to test against the edge regions.
594 if ((point[0] < box_min[0] || point[0] > box_max[0]) +
595 (point[1] < box_min[1] || point[1] > box_max[1]) +
596 (point[2] < box_min[2] || point[2] > box_max[2]) > 1) {
597 // Test the capsule against each edge of the box.
598 static const struct {
599 LPoint3 point;
600 int axis;
601 } edges[] = {
602 {{0, 0, 0}, 0},
603 {{0, 1, 0}, 0},
604 {{0, 0, 1}, 0},
605 {{0, 1, 1}, 0},
606 {{0, 0, 0}, 1},
607 {{0, 0, 1}, 1},
608 {{1, 0, 0}, 1},
609 {{1, 0, 1}, 1},
610 {{0, 0, 0}, 2},
611 {{0, 1, 0}, 2},
612 {{1, 0, 0}, 2},
613 {{1, 1, 0}, 2},
614 };
615
616 PN_stdfloat best_dist_sq = FLT_MAX;
617
618 for (int i = 0; i < 12; ++i) {
619 LPoint3 vertex = edges[i].point;
620 vertex.componentwise_mult(dimensions);
621 vertex += box_min;
622 LVector3 delta(0);
623 delta[edges[i].axis] = dimensions[edges[i].axis];
624 double u1, u2;
625 CollisionCapsule::calc_closest_segment_points(u1, u2, from_a, from_direction, vertex, delta);
626 PN_stdfloat dist_sq = ((from_a + from_direction * u1) - (vertex + delta * u2)).length_squared();
627 if (dist_sq < best_dist_sq) {
628 best_dist_sq = dist_sq;
629 }
630 }
631
632 if (best_dist_sq > radius_sq) {
633 // It is not actually touching any edge.
634 return nullptr;
635 }
636 }
637
638 if (collide_cat.is_debug()) {
639 collide_cat.debug()
640 << "intersection detected from " << entry.get_from_node_path()
641 << " into " << entry.get_into_node_path() << "\n";
642 }
643 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
644
645 // Which is the longest axis?
646 LVector3 diff = point - _center;
647 diff[0] /= dimensions[0];
648 diff[1] /= dimensions[1];
649 diff[2] /= dimensions[2];
650 int axis = 0;
651 if (cabs(diff[0]) > cabs(diff[1])) {
652 if (cabs(diff[0]) > cabs(diff[2])) {
653 axis = 0;
654 } else {
655 axis = 2;
656 }
657 } else {
658 if (cabs(diff[1]) > cabs(diff[2])) {
659 axis = 1;
660 } else {
661 axis = 2;
662 }
663 }
664 LVector3 normal(0);
665 normal[axis] = std::copysign(1, diff[axis]);
666
667 LPoint3 clamped = point.fmax(box_min).fmin(box_max);
668 LPoint3 surface_point = clamped;
669 surface_point[axis] = (diff[axis] >= 0.0f) ? box_max[axis] : box_min[axis];
670
671 // Is the point inside the box?
672 LVector3 interior_vec;
673 if (clamped != point) {
674 // No, it is outside. The interior point is in the direction of the
675 // surface point.
676 interior_vec = point - surface_point;
677 if (!interior_vec.normalize()) {
678 interior_vec = normal;
679 }
680 } else {
681 // It is inside. I think any point will work for this.
682 interior_vec = normal;
683 }
684 new_entry->set_interior_point(point - interior_vec * radius);
685 new_entry->set_surface_point(surface_point);
686
687 if (has_effective_normal() && capsule->get_respect_effective_normal()) {
688 new_entry->set_surface_normal(get_effective_normal());
689 } else {
690 new_entry->set_surface_normal(normal);
691 }
692
693 return new_entry;
694 }
695
696 /**
697 * Double dispatch point for box as a FROM object
698 */
699 PT(CollisionEntry) CollisionBox::
700 test_intersection_from_box(const CollisionEntry &entry) const {
701 const CollisionBox *box;
702 DCAST_INTO_R(box, entry.get_from(), nullptr);
703
704 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
705
706 LPoint3 diff = wrt_mat.xform_point_general(box->get_center()) - _center;
707 LVector3 from_extents = box->get_dimensions() * 0.5f;
708 LVector3 into_extents = get_dimensions() * 0.5f;
709
710 LVecBase3 box_x = wrt_mat.get_row3(0);
711 LVecBase3 box_y = wrt_mat.get_row3(1);
712 LVecBase3 box_z = wrt_mat.get_row3(2);
713
714 // To make the math simpler, normalize the box basis vectors, instead
715 // applying the scale to the box dimensions. Note that this doesn't work
716 // for a non-uniform scales applied after a rotation, since that has the
717 // possibility of making the box no longer a box.
718 PN_stdfloat l;
719 l = box_x.length();
720 from_extents[0] *= l;
721 box_x /= l;
722 l = box_y.length();
723 from_extents[1] *= l;
724 box_y /= l;
725 l = box_z.length();
726 from_extents[2] *= l;
727 box_z /= l;
728
729 PN_stdfloat r1, r2;
730 PN_stdfloat min_pen = 0;
731 PN_stdfloat pen;
732 int axis = 0;
733
734 // SAT test for the three axes of the into cube.
735 r1 = into_extents[0];
736 r2 = cabs(box_x[0] * from_extents[0]) +
737 cabs(box_y[0] * from_extents[1]) +
738 cabs(box_z[0] * from_extents[2]);
739 pen = r1 + r2 - cabs(diff[0]);
740 if (pen < 0) {
741 return nullptr;
742 }
743 min_pen = pen;
744
745 r1 = into_extents[1];
746 r2 = cabs(box_x[1] * from_extents[0]) +
747 cabs(box_y[1] * from_extents[1]) +
748 cabs(box_z[1] * from_extents[2]);
749 pen = r1 + r2 - cabs(diff[1]);
750 if (pen < 0) {
751 return nullptr;
752 }
753 if (pen < min_pen) {
754 min_pen = pen;
755 axis = 1;
756 }
757
758 r1 = into_extents[2];
759 r2 = cabs(box_x[2] * from_extents[0]) +
760 cabs(box_y[2] * from_extents[1]) +
761 cabs(box_z[2] * from_extents[2]);
762 pen = r1 + r2 - cabs(diff[2]);
763 if (pen < 0) {
764 return nullptr;
765 }
766 if (pen < min_pen) {
767 min_pen = pen;
768 axis = 2;
769 }
770
771 // SAT test for the three axes of the from cube.
772 r1 = cabs(box_x[0] * into_extents[0]) +
773 cabs(box_x[1] * into_extents[1]) +
774 cabs(box_x[2] * into_extents[2]);
775 r2 = from_extents[0];
776 pen = r1 + r2 - cabs(diff.dot(box_x));
777 if (pen < 0) {
778 return nullptr;
779 }
780 if (pen < min_pen) {
781 min_pen = pen;
782 }
783
784 r1 = cabs(box_y[0] * into_extents[0]) +
785 cabs(box_y[1] * into_extents[1]) +
786 cabs(box_y[2] * into_extents[2]);
787 r2 = from_extents[1];
788 pen = r1 + r2 - cabs(diff.dot(box_y));
789 if (pen < 0) {
790 return nullptr;
791 }
792 if (pen < min_pen) {
793 min_pen = pen;
794 }
795
796 r1 = cabs(box_z[0] * into_extents[0]) +
797 cabs(box_z[1] * into_extents[1]) +
798 cabs(box_z[2] * into_extents[2]);
799 r2 = from_extents[2];
800 pen = r1 + r2 - cabs(diff.dot(box_z));
801 if (pen < 0) {
802 return nullptr;
803 }
804 if (pen < min_pen) {
805 min_pen = pen;
806 }
807
808 // SAT test of the nine cross products.
809 r1 = into_extents[1] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[1]);
810 r2 = from_extents[1] * cabs(box_z[0]) + from_extents[2] * cabs(box_y[0]);
811 if (cabs(diff[2] * box_x[1] - diff[1] * box_x[2]) > r1 + r2) {
812 return nullptr;
813 }
814
815 r1 = into_extents[1] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[1]);
816 r2 = from_extents[0] * cabs(box_z[0]) + from_extents[2] * cabs(box_x[0]);
817 if (cabs(diff[2] * box_y[1] - diff[1] * box_y[2]) > r1 + r2) {
818 return nullptr;
819 }
820
821 r1 = into_extents[1] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[1]);
822 r2 = from_extents[0] * cabs(box_y[0]) + from_extents[1] * cabs(box_x[0]);
823 if (cabs(diff[2] * box_z[1] - diff[1] * box_z[2]) > r1 + r2) {
824 return nullptr;
825 }
826
827 r1 = into_extents[0] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[0]);
828 r2 = from_extents[1] * cabs(box_z[1]) + from_extents[2] * cabs(box_y[1]);
829 if (cabs(diff[0] * box_x[2] - diff[2] * box_x[0]) > r1 + r2) {
830 return nullptr;
831 }
832
833 r1 = into_extents[0] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[0]);
834 r2 = from_extents[0] * cabs(box_z[1]) + from_extents[2] * cabs(box_x[1]);
835 if (cabs(diff[0] * box_y[2] - diff[2] * box_y[0]) > r1 + r2) {
836 return nullptr;
837 }
838
839 r1 = into_extents[0] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[0]);
840 r2 = from_extents[0] * cabs(box_y[1]) + from_extents[1] * cabs(box_x[1]);
841 if (cabs(diff[0] * box_z[2] - diff[2] * box_z[0]) > r1 + r2) {
842 return nullptr;
843 }
844
845 r1 = into_extents[0] * cabs(box_x[1]) + into_extents[1] * cabs(box_x[0]);
846 r2 = from_extents[1] * cabs(box_z[2]) + from_extents[2] * cabs(box_y[2]);
847 if (cabs(diff[1] * box_x[0] - diff[0] * box_x[1]) > r1 + r2) {
848 return nullptr;
849 }
850
851 r1 = into_extents[0] * cabs(box_y[1]) + into_extents[1] * cabs(box_y[0]);
852 r2 = from_extents[0] * cabs(box_z[2]) + from_extents[2] * cabs(box_x[2]);
853 if (cabs(diff[1] * box_y[0] - diff[0] * box_y[1]) > r1 + r2) {
854 return nullptr;
855 }
856
857 r1 = into_extents[0] * cabs(box_z[1]) + into_extents[1] * cabs(box_z[0]);
858 r2 = from_extents[0] * cabs(box_y[2]) + from_extents[1] * cabs(box_x[2]);
859 if (cabs(diff[1] * box_z[0] - diff[0] * box_z[1]) > r1 + r2) {
860 return nullptr;
861 }
862
863 if (collide_cat.is_debug()) {
864 collide_cat.debug()
865 << "intersection detected from " << entry.get_from_node_path()
866 << " into " << entry.get_into_node_path() << "\n";
867 }
868 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
869
870 // This isn't always the correct surface point. However, it seems to be
871 // enough to let the pusher do the right thing.
872 LPoint3 surface(
873 min(max(diff[0], -into_extents[0]), into_extents[0]),
874 min(max(diff[1], -into_extents[1]), into_extents[1]),
875 min(max(diff[2], -into_extents[2]), into_extents[2]));
876
877 // Create the normal along the axis of least penetration.
878 LVector3 normal(0);
879 PN_stdfloat diff_axis = diff[axis];
880 int sign = (diff_axis >= 0) ? 1 : -1;
881 normal[axis] = sign;
882 surface[axis] = into_extents[axis] * sign;
883
884 new_entry->set_surface_point(surface + _center);
885
886 // Does not generate the correct depth. Needs fixing.
887 new_entry->set_interior_point(surface + _center + normal * -min_pen);
888
889 if (has_effective_normal() && box->get_respect_effective_normal()) {
890 new_entry->set_surface_normal(get_effective_normal());
891 } else {
892 new_entry->set_surface_normal(normal);
893 }
894
895 return new_entry;
896 }
897
898 /**
899 * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
900 * solid.
901 */
902 void CollisionBox::
903 fill_viz_geom() {
904 if (collide_cat.is_debug()) {
905 collide_cat.debug()
906 << "Recomputing viz for " << *this << "\n";
907 }
908
909 PT(GeomVertexData) vdata = new GeomVertexData
910 ("collision", GeomVertexFormat::get_v3(),
911 Geom::UH_static);
912
913 vdata->unclean_set_num_rows(8);
914
915 {
916 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
917 vertex.set_data3(_min[0], _min[1], _min[2]);
918 vertex.set_data3(_min[0], _max[1], _min[2]);
919 vertex.set_data3(_max[0], _max[1], _min[2]);
920 vertex.set_data3(_max[0], _min[1], _min[2]);
921
922 vertex.set_data3(_min[0], _min[1], _max[2]);
923 vertex.set_data3(_min[0], _max[1], _max[2]);
924 vertex.set_data3(_max[0], _max[1], _max[2]);
925 vertex.set_data3(_max[0], _min[1], _max[2]);
926 }
927
928 PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static);
929
930 // Bottom
931 tris->add_vertices(0, 1, 2);
932 tris->add_vertices(2, 3, 0);
933
934 // Top
935 tris->add_vertices(4, 7, 6);
936 tris->add_vertices(6, 5, 4);
937
938 // Sides
939 tris->add_vertices(0, 4, 1);
940 tris->add_vertices(1, 4, 5);
941
942 tris->add_vertices(1, 5, 2);
943 tris->add_vertices(2, 5, 6);
944
945 tris->add_vertices(2, 6, 3);
946 tris->add_vertices(3, 6, 7);
947
948 tris->add_vertices(3, 7, 0);
949 tris->add_vertices(0, 7, 4);
950
951 PT(GeomLines) lines = new GeomLines(Geom::UH_static);
952
953 // Bottom
954 lines->add_vertices(0, 1);
955 lines->add_vertices(1, 2);
956 lines->add_vertices(0, 3);
957 lines->add_vertices(2, 3);
958
959 // Top
960 lines->add_vertices(4, 5);
961 lines->add_vertices(5, 6);
962 lines->add_vertices(4, 7);
963 lines->add_vertices(6, 7);
964
965 // Sides
966 lines->add_vertices(0, 4);
967 lines->add_vertices(1, 5);
968 lines->add_vertices(2, 6);
969 lines->add_vertices(3, 7);
970
971 PT(Geom) geom1 = new Geom(vdata);
972 geom1->add_primitive(tris);
973
974 PT(Geom) geom2 = new Geom(vdata);
975 geom2->add_primitive(lines);
976
977 _viz_geom->add_geom(geom1, get_solid_viz_state());
978 _viz_geom->add_geom(geom2, get_wireframe_viz_state());
979
980 _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
981 _bounds_viz_geom->add_geom(geom2, get_wireframe_viz_state());
982 }
983
984 /**
985 * Determine the point(s) of intersection of a parametric line with the box.
986 * The line is infinite in both directions, and passes through "from" and
987 * from+delta. If the line does not intersect the box, the function returns
988 * false, and t1 and t2 are undefined. If it does intersect the box, it
989 * returns true, and t1 and t2 are set to the points along the equation
990 * from+t*delta that correspond to the two points of intersection.
991 */
992 bool CollisionBox::
993 intersects_line(double &t1, double &t2,
994 const LPoint3 &from, const LVector3 &delta,
995 PN_stdfloat inflate_size) const {
996
997 LPoint3 bmin = _min - LVector3(inflate_size);
998 LPoint3 bmax = _max + LVector3(inflate_size);
999
1000 double tmin = -DBL_MAX;
1001 double tmax = DBL_MAX;
1002
1003 for (int i = 0; i < 3; ++i) {
1004 PN_stdfloat d = delta[i];
1005 if (!IS_NEARLY_ZERO(d)) {
1006 double tmin2 = (bmin[i] - from[i]) / d;
1007 double tmax2 = (bmax[i] - from[i]) / d;
1008 if (tmin2 > tmax2) {
1009 std::swap(tmin2, tmax2);
1010 }
1011 tmin = std::max(tmin, tmin2);
1012 tmax = std::min(tmax, tmax2);
1013
1014 if (tmin > tmax) {
1015 return false;
1016 }
1017
1018 } else if (from[i] < bmin[i] || from[i] > bmax[i]) {
1019 // The line is entirely parallel in this dimension.
1020 return false;
1021 }
1022 }
1023
1024 t1 = tmin;
1025 t2 = tmax;
1026 return true;
1027 }
1028
1029 /**
1030 * Clips the polygon by all of the clip planes named in the clip plane
1031 * attribute and fills new_points up with the resulting points.
1032 *
1033 * The return value is true if the set of points is unmodified (all points are
1034 * behind all the clip planes), or false otherwise.
1035 */
1036 bool CollisionBox::
1037 apply_clip_plane(CollisionBox::Points &new_points,
1038 const ClipPlaneAttrib *cpa,
1039 const TransformState *net_transform, int plane_no) const {
1040 bool all_in = true;
1041
1042 int num_planes = cpa->get_num_on_planes();
1043 bool first_plane = true;
1044
1045 for (int i = 0; i < num_planes; i++) {
1046 NodePath plane_path = cpa->get_on_plane(i);
1047 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
1048 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
1049 CPT(TransformState) new_transform =
1050 net_transform->invert_compose(plane_path.get_net_transform());
1051
1052 LPlane plane = plane_node->get_plane() * new_transform->get_mat();
1053 if (first_plane) {
1054 first_plane = false;
1055 if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
1056 all_in = false;
1057 }
1058 } else {
1059 Points last_points;
1060 last_points.swap(new_points);
1061 if (!clip_polygon(new_points, last_points, plane, plane_no)) {
1062 all_in = false;
1063 }
1064 }
1065 }
1066 }
1067
1068 if (!all_in) {
1069 compute_vectors(new_points);
1070 }
1071
1072 return all_in;
1073 }
1074 /**
1075 * Clips the source_points of the polygon by the indicated clipping plane, and
1076 * modifies new_points to reflect the new set of clipped points (but does not
1077 * compute the vectors in new_points).
1078 *
1079 * The return value is true if the set of points is unmodified (all points are
1080 * behind the clip plane), or false otherwise.
1081 */
1082 bool CollisionBox::
1083 clip_polygon(CollisionBox::Points &new_points,
1084 const CollisionBox::Points &source_points,
1085 const LPlane &plane, int plane_no) const {
1086 new_points.clear();
1087 if (source_points.empty()) {
1088 return true;
1089 }
1090
1091 LPoint3 from3d;
1092 LVector3 delta3d;
1093 if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
1094 // The clipping plane is parallel to the polygon. The polygon is either
1095 // all in or all out.
1096 if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
1097 // A point within the polygon is behind the clipping plane: the polygon
1098 // is all in.
1099 new_points = source_points;
1100 return true;
1101 }
1102 return false;
1103 }
1104
1105 // Project the line of intersection into the 2-d plane. Now we have a 2-d
1106 // clipping line.
1107 LPoint2 from2d = to_2d(from3d,plane_no);
1108 LVector2 delta2d = to_2d(delta3d,plane_no);
1109
1110 PN_stdfloat a = -delta2d[1];
1111 PN_stdfloat b = delta2d[0];
1112 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
1113
1114 // Now walk through the points. Any point on the left of our line gets
1115 // removed, and the line segment clipped at the point of intersection.
1116
1117 // We might increase the number of vertices by as many as 1, if the plane
1118 // clips off exactly one corner. (We might also decrease the number of
1119 // vertices, or keep them the same number.)
1120 new_points.reserve(source_points.size() + 1);
1121
1122 LPoint2 last_point = source_points.back()._p;
1123 bool last_is_in = !is_right(last_point - from2d, delta2d);
1124 bool all_in = last_is_in;
1125 Points::const_iterator pi;
1126 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
1127 const LPoint2 &this_point = (*pi)._p;
1128 bool this_is_in = !is_right(this_point - from2d, delta2d);
1129
1130 // There appears to be a compiler bug in gcc 4.0: we need to extract this
1131 // comparison outside of the if statement.
1132 bool crossed_over = (this_is_in != last_is_in);
1133 if (crossed_over) {
1134 // We have just crossed over the clipping line. Find the point of
1135 // intersection.
1136 LVector2 d = this_point - last_point;
1137 PN_stdfloat denom = (a * d[0] + b * d[1]);
1138 if (denom != 0.0) {
1139 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
1140 LPoint2 p = last_point + t * d;
1141
1142 new_points.push_back(PointDef(p[0], p[1]));
1143 last_is_in = this_is_in;
1144 }
1145 }
1146
1147 if (this_is_in) {
1148 // We are behind the clipping line. Keep the point.
1149 new_points.push_back(PointDef(this_point[0], this_point[1]));
1150 } else {
1151 all_in = false;
1152 }
1153
1154 last_point = this_point;
1155 }
1156
1157 return all_in;
1158 }
1159
1160
1161 /**
1162 * Returns the linear distance from the 2-d point to the nearest part of the
1163 * polygon defined by the points vector. The result is negative if the point
1164 * is within the polygon.
1165 */
1166 PN_stdfloat CollisionBox::
1167 dist_to_polygon(const LPoint2 &p, const CollisionBox::Points &points) const {
1168
1169 // We know that that the polygon is convex and is defined with the points in
1170 // counterclockwise order. Therefore, we simply compare the signed distance
1171 // to each line segment; we ignore any negative values, and take the minimum
1172 // of all the positive values.
1173
1174 // If all values are negative, the point is within the polygon; we therefore
1175 // return an arbitrary negative result.
1176
1177 bool got_dist = false;
1178 PN_stdfloat best_dist = -1.0f;
1179
1180 size_t num_points = points.size();
1181 for (size_t i = 0; i < num_points - 1; ++i) {
1182 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
1183 points[i]._v);
1184 if (d >= 0.0f) {
1185 if (!got_dist || d < best_dist) {
1186 best_dist = d;
1187 got_dist = true;
1188 }
1189 }
1190 }
1191
1192 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
1193 points[num_points - 1]._v);
1194 if (d >= 0.0f) {
1195 if (!got_dist || d < best_dist) {
1196 best_dist = d;
1197 got_dist = true;
1198 }
1199 }
1200
1201 return best_dist;
1202 }
1203
1204 /**
1205 * Returns the linear distance of p to the line segment defined by f and t,
1206 * where v = (t - f).normalize(). The result is negative if p is left of the
1207 * line, positive if it is right of the line. If the result is positive, it
1208 * is constrained by endpoints of the line segment (i.e. the result might be
1209 * larger than it would be for a straight distance-to-line test). If the
1210 * result is negative, we don't bother.
1211 */
1212 PN_stdfloat CollisionBox::
1213 dist_to_line_segment(const LPoint2 &p,
1214 const LPoint2 &f, const LPoint2 &t,
1215 const LVector2 &v) {
1216 LVector2 v1 = (p - f);
1217 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
1218 if (d < 0.0f) {
1219 return d;
1220 }
1221
1222 // Compute the nearest point on the line.
1223 LPoint2 q = p + LVector2(-v[1], v[0]) * d;
1224
1225 // Now constrain that point to the line segment.
1226 if (v[0] > 0.0f) {
1227 // X+
1228 if (v[1] > 0.0f) {
1229 // Y+
1230 if (v[0] > v[1]) {
1231 // X-dominant.
1232 if (q[0] < f[0]) {
1233 return (p - f).length();
1234 } if (q[0] > t[0]) {
1235 return (p - t).length();
1236 } else {
1237 return d;
1238 }
1239 } else {
1240 // Y-dominant.
1241 if (q[1] < f[1]) {
1242 return (p - f).length();
1243 } if (q[1] > t[1]) {
1244 return (p - t).length();
1245 } else {
1246 return d;
1247 }
1248 }
1249 } else {
1250 // Y-
1251 if (v[0] > -v[1]) {
1252 // X-dominant.
1253 if (q[0] < f[0]) {
1254 return (p - f).length();
1255 } if (q[0] > t[0]) {
1256 return (p - t).length();
1257 } else {
1258 return d;
1259 }
1260 } else {
1261 // Y-dominant.
1262 if (q[1] > f[1]) {
1263 return (p - f).length();
1264 } if (q[1] < t[1]) {
1265 return (p - t).length();
1266 } else {
1267 return d;
1268 }
1269 }
1270 }
1271 } else {
1272 // X-
1273 if (v[1] > 0.0f) {
1274 // Y+
1275 if (-v[0] > v[1]) {
1276 // X-dominant.
1277 if (q[0] > f[0]) {
1278 return (p - f).length();
1279 } if (q[0] < t[0]) {
1280 return (p - t).length();
1281 } else {
1282 return d;
1283 }
1284 } else {
1285 // Y-dominant.
1286 if (q[1] < f[1]) {
1287 return (p - f).length();
1288 } if (q[1] > t[1]) {
1289 return (p - t).length();
1290 } else {
1291 return d;
1292 }
1293 }
1294 } else {
1295 // Y-
1296 if (-v[0] > -v[1]) {
1297 // X-dominant.
1298 if (q[0] > f[0]) {
1299 return (p - f).length();
1300 } if (q[0] < t[0]) {
1301 return (p - t).length();
1302 } else {
1303 return d;
1304 }
1305 } else {
1306 // Y-dominant.
1307 if (q[1] > f[1]) {
1308 return (p - f).length();
1309 } if (q[1] < t[1]) {
1310 return (p - t).length();
1311 } else {
1312 return d;
1313 }
1314 }
1315 }
1316 }
1317 }
1318
1319 /**
1320 * Returns true if the indicated point is within the polygon's 2-d space,
1321 * false otherwise.
1322 */
1323 bool CollisionBox::
1324 point_is_inside(const LPoint2 &p, const CollisionBox::Points &points) const {
1325 // We insist that the polygon be convex. This makes things a bit simpler.
1326 // In the case of a convex polygon, defined with points in counterclockwise
1327 // order, a point is interior to the polygon iff the point is not right of
1328 // each of the edges.
1329 for (int i = 0; i < (int)points.size() - 1; i++) {
1330 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1331 return false;
1332 }
1333 }
1334 if (is_right(p - points[points.size() - 1]._p,
1335 points[0]._p - points[points.size() - 1]._p)) {
1336 return false;
1337 }
1338
1339 return true;
1340 }
1341
1342 /**
1343 * Now that the _p members of the given points array have been computed, go
1344 * back and compute all of the _v members.
1345 */
1346 void CollisionBox::
1347 compute_vectors(Points &points) {
1348 size_t num_points = points.size();
1349 for (size_t i = 0; i < num_points; i++) {
1350 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1351 points[i]._v.normalize();
1352 }
1353 }
1354
1355 /**
1356 * Factory method to generate a CollisionBox object
1357 */
1358 void CollisionBox::
1359 register_with_read_factory() {
1360 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
1361 }
1362
1363 /**
1364 * Function to write the important information in the particular object to a
1365 * Datagram
1366 */
1367 void CollisionBox::
1368 write_datagram(BamWriter *manager, Datagram &me) {
1369 CollisionSolid::write_datagram(manager, me);
1370 _center.write_datagram(me);
1371 _min.write_datagram(me);
1372 _max.write_datagram(me);
1373 for(int i=0; i < 8; i++) {
1374 _vertex[i].write_datagram(me);
1375 }
1376 me.add_stdfloat(_radius);
1377 me.add_stdfloat(_x);
1378 me.add_stdfloat(_y);
1379 me.add_stdfloat(_z);
1380 for(int i=0; i < 6; i++) {
1381 _planes[i].write_datagram(me);
1382 }
1383 for(int i=0; i < 6; i++) {
1384 _to_2d_mat[i].write_datagram(me);
1385 }
1386 for(int i=0; i < 6; i++) {
1387 me.add_uint16(_points[i].size());
1388 for (size_t j = 0; j < _points[i].size(); j++) {
1389 _points[i][j]._p.write_datagram(me);
1390 _points[i][j]._v.write_datagram(me);
1391 }
1392 }
1393 }
1394
1395 /**
1396 * Factory method to generate a CollisionBox object
1397 */
1398 TypedWritable *CollisionBox::
1399 make_CollisionBox(const FactoryParams &params) {
1400 CollisionBox *me = new CollisionBox;
1401 DatagramIterator scan;
1402 BamReader *manager;
1403
1404 parse_params(params, scan, manager);
1405 me->fillin(scan, manager);
1406 return me;
1407 }
1408
1409 /**
1410 * Function that reads out of the datagram (or asks manager to read) all of
1411 * the data that is needed to re-create this object and stores it in the
1412 * appropiate place
1413 */
1414 void CollisionBox::
1415 fillin(DatagramIterator& scan, BamReader* manager) {
1416 CollisionSolid::fillin(scan, manager);
1417 _center.read_datagram(scan);
1418 _min.read_datagram(scan);
1419 _max.read_datagram(scan);
1420 for(int i=0; i < 8; i++) {
1421 _vertex[i].read_datagram(scan);
1422 }
1423 _radius = scan.get_stdfloat();
1424 _x = scan.get_stdfloat();
1425 _y = scan.get_stdfloat();
1426 _z = scan.get_stdfloat();
1427 for(int i=0; i < 6; i++) {
1428 _planes[i].read_datagram(scan);
1429 }
1430 for(int i=0; i < 6; i++) {
1431 _to_2d_mat[i].read_datagram(scan);
1432 }
1433 for(int i=0; i < 6; i++) {
1434 size_t size = scan.get_uint16();
1435 for (size_t j = 0; j < size; j++) {
1436 LPoint2 p;
1437 LVector2 v;
1438 p.read_datagram(scan);
1439 v.read_datagram(scan);
1440 _points[i].push_back(PointDef(p, v));
1441 }
1442 }
1443 }
bamReader.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
parse_params
void parse_params(const FactoryParams &params, DatagramIterator &scan, BamReader *&manager)
Takes in a FactoryParams, passed from a WritableFactory into any TypedWritable's make function,...
Definition: bamReader.I:275
bamWriter.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
boundingSphere.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
BamReader
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:110
BamReader::get_factory
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition: bamReader.I:177
BamWriter
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
Definition: bamWriter.h:63
BoundingSphere
This defines a bounding sphere, consisting of a center and a radius.
Definition: boundingSphere.h:25
BoundingVolume
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
Definition: boundingVolume.h:41
ClipPlaneAttrib
This functions similarly to a LightAttrib.
Definition: clipPlaneAttrib.h:31
ClipPlaneAttrib::get_on_plane
get_on_plane
Returns the nth plane enabled by the attribute, sorted in render order.
Definition: clipPlaneAttrib.h:74
ClipPlaneAttrib::get_num_on_planes
get_num_on_planes
Returns the number of planes that are enabled by the attribute.
Definition: clipPlaneAttrib.h:74
CollisionBox::PointDef
Definition: collisionBox.h:118
CollisionBox
A cuboid collision volume or object.
Definition: collisionBox.h:27
CollisionBox::register_with_read_factory
static void register_with_read_factory()
Factory method to generate a CollisionBox object.
Definition: collisionBox.cxx:1359
CollisionBox::dist_to_polygon
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...
Definition: collisionBox.cxx:1167
CollisionBox::setup_points
void setup_points(const LPoint3 *begin, const LPoint3 *end, int plane)
Computes the plane and 2d projection of points that make up this side.
Definition: collisionBox.cxx:83
CollisionBox::apply_clip_plane
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...
Definition: collisionBox.cxx:1037
CollisionBox::clip_polygon
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...
Definition: collisionBox.cxx:1083
CollisionBox::write_datagram
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
Definition: collisionBox.cxx:1368
CollisionBox::calc_to_3d_mat
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 ...
Definition: collisionBox.I:233
CollisionBox::compute_vectors
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 ...
Definition: collisionBox.cxx:1347
CollisionBox::setup_box
void setup_box()
Compute parameters for each of the box's sides.
Definition: collisionBox.cxx:68
CollisionBox::get_plane
LPlane get_plane(int n) const
Returns the nth face of the rectangular solid.
Definition: collisionBox.I:181
CollisionBox::point_is_inside
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.
Definition: collisionBox.cxx:1324
CollisionBox::get_volume_pcollector
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
Definition: collisionBox.cxx:172
CollisionBox::to_2d
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 ...
Definition: collisionBox.I:223
CollisionBox::get_point
LPoint3 get_point(int n) const
Returns the nth vertex of the OBB.
Definition: collisionBox.I:150
CollisionBox::get_collision_origin
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
Definition: collisionBox.cxx:163
CollisionBox::get_test_pcollector
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
Definition: collisionBox.cxx:181
CollisionBox::set_plane
LPlane set_plane(int n) const
Creates the nth face of the rectangular solid.
Definition: collisionBox.I:190
CollisionCapsule
This implements a solid consisting of a cylinder with hemispherical endcaps, also known as a capsule ...
Definition: collisionCapsule.h:27
CollisionEntry
Defines a single collision event.
Definition: collisionEntry.h:42
CollisionEntry::get_into
get_into
Returns the CollisionSolid pointer for the particular solid was collided into.
Definition: collisionEntry.h:98
CollisionEntry::get_from_node_path
get_from_node_path
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
Definition: collisionEntry.h:101
CollisionEntry::get_into_node_path
get_into_node_path
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
Definition: collisionEntry.h:102
CollisionEntry::get_from
get_from
Returns the CollisionSolid pointer for the particular solid that triggered this collision.
Definition: collisionEntry.h:97
CollisionLine
An infinite line, similar to a CollisionRay, except that it extends in both directions.
Definition: collisionLine.h:25
CollisionRay
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:27
CollisionSegment
A finite line segment, with two specific endpoints but no thickness.
Definition: collisionSegment.h:31
CollisionSolid
The abstract base class for all things that can collide with other things in the world,...
Definition: collisionSolid.h:45
CollisionSolid::write_datagram
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
Definition: collisionSolid.cxx:345
CollisionSolid::has_effective_normal
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
Definition: collisionSolid.I:71
CollisionSolid::get_effective_normal
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
Definition: collisionSolid.I:81
CollisionSolid::get_respect_effective_normal
get_respect_effective_normal
See set_respect_effective_normal().
Definition: collisionSolid.h:72
CollisionSphere
A spherical collision volume or object.
Definition: collisionSphere.h:25
DatagramIterator
A class to retrieve the individual data elements previously stored in a Datagram.
Definition: datagramIterator.h:27
DatagramIterator::get_stdfloat
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
Definition: datagramIterator.I:242
DatagramIterator::get_uint16
uint16_t get_uint16()
Extracts an unsigned 16-bit integer.
Definition: datagramIterator.I:145
Datagram
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition: datagram.h:38
Datagram::add_stdfloat
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
Definition: datagram.I:133
Datagram::add_uint16
void add_uint16(uint16_t value)
Adds an unsigned 16-bit integer to the datagram.
Definition: datagram.I:85
FactoryParams
An instance of this class is passed to the Factory when requesting it to do its business and construc...
Definition: factoryParams.h:36
Factory::register_factory
void register_factory(TypeHandle handle, CreateFunc *func, void *user_data=nullptr)
Registers a new kind of thing the Factory will be able to create.
Definition: factory.I:73
GeomLines
Defines a series of disconnected line segments.
Definition: geomLines.h:23
GeomTriangles
Defines a series of disconnected triangles.
Definition: geomTriangles.h:23
GeomVertexData
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
Definition: geomVertexData.h:68
GeomVertexFormat::get_v3
static const GeomVertexFormat * get_v3()
Returns a standard vertex format with just a 3-component vertex position.
Definition: geomVertexFormat.I:251
GeomVertexWriter
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
Definition: geomVertexWriter.h:55
Geom
A container for geometry primitives.
Definition: geom.h:54
NodePath
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:159
NodePath::node
PandaNode * node() const
Returns the referenced node of the path.
Definition: nodePath.I:227
PStatCollector
A lightweight class that represents a single element that may be timed and/or counted via stats.
Definition: pStatCollector.h:43
PlaneNode
A node that contains a plane.
Definition: planeNode.h:36
PlaneNode::get_plane
const LPlane & get_plane() const
Returns the plane represented by the PlaneNode.
Definition: planeNode.I:54
PlaneNode::get_clip_effect
int get_clip_effect() const
Returns the clip_effect bits for this clip plane.
Definition: planeNode.I:128
TransformState
Indicates a coordinate-system transform on vertices.
Definition: transformState.h:54
TypeHandle
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:81
TypedWritable
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:35
pvector< PointDef >
cmath.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PT
PT(CollisionEntry) CollisionBox
First Dispatch point for box as a FROM object.
Definition: collisionBox.cxx:129
collisionBox.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionCapsule.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionEntry.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionHandler.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionLine.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionRay.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionSegment.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
collisionSphere.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
config_collide.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
config_mathutil.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
datagramIterator.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
datagram.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
dcast.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
geomLines.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
geomTriangles.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
geomVertexWriter.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
geom.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
mathNumbers.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
nearly_zero.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.