Panda3D

Loading...
Searching...
No Matches
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::
60make_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 */
129PT(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 */
162LPoint3 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::
189output(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 */
196PT(BoundingVolume) CollisionBox::
197compute_internal_bounds() const {
198 return new BoundingSphere(_center, _radius);
199}
200
201/**
202 * Double dispatch point for sphere as FROM object
203 */
204PT(CollisionEntry) CollisionBox::
205test_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 bool fully_inside = true;
234
235 for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
236 plane = get_plane( ip );
237 if (_points[ip].size() < 3) {
238 fully_inside = false;
239 continue;
240 }
241 if (wrt_prev_space != wrt_space) {
242 // If we have a delta between the previous position and the current
243 // position, we use that to determine some more properties of the
244 // collision.
245 LPoint3 b = from_center;
246 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
247 LVector3 delta = b - a;
248
249 // First, there is no collision if the "from" object is definitely
250 // moving in the same direction as the plane's normal.
251 PN_stdfloat dot = delta.dot(plane.get_normal());
252 if (dot > 0.1f) {
253 fully_inside = false;
254 continue; // no intersection
255 }
256
257 if (IS_NEARLY_ZERO(dot)) {
258 // If we're moving parallel to the plane, the sphere is tested at its
259 // final point. Leave it as it is.
260
261 } else {
262/*
263 * Otherwise, we're moving into the plane; the sphere is tested at the point
264 * along its path that is closest to intersecting the plane. This may be the
265 * actual intersection point, or it may be the starting point or the final
266 * point. dot is equal to the (negative) magnitude of 'delta' along the
267 * direction of the plane normal t = ratio of (distance from start pos to
268 * plane) to (distance from start pos to end pos), along axis of plane normal
269 */
270 PN_stdfloat dist_to_p = plane.dist_to_plane(a);
271 t = (dist_to_p / -dot);
272
273 // also compute the actual contact point and time of contact for
274 // handlers that need it
275 actual_t = ((dist_to_p - from_radius) / -dot);
276 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
277 contact_point = a + (actual_t * delta);
278
279 if (t >= 1.0f) {
280 // Leave it where it is.
281
282 } else if (t < 0.0f) {
283 from_center = a;
284 moved_from_center = true;
285 } else {
286 from_center = a + t * delta;
287 moved_from_center = true;
288 }
289 }
290 }
291
292 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
293
294#ifndef NDEBUG
295 /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
296 std::cout
297 << "polygon within " << entry.get_into_node_path()
298 << " has normal " << normal << " of length " << normal.length()
299 << "\n";
300 normal.normalize();
301 }*/
302#endif
303
304 // The nearest point within the plane to our center is the intersection of
305 // the line (center, center - normal) with the plane.
306
307 if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
308 // No intersection with plane? This means the plane's effective normal
309 // was within the plane itself. A useless polygon.
310 fully_inside = false;
311 continue;
312 }
313
314 if (dist > from_radius) {
315 // Fully outside this plane, there can not be an intersection.
316 return nullptr;
317 }
318 if (dist < -from_radius) {
319 // Fully inside this plane.
320 continue;
321 }
322 fully_inside = false;
323
324 LPoint2 p = to_2d(from_center - dist * plane.get_normal(), ip);
325 PN_stdfloat edge_dist = 0.0f;
326
327 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
328 if (cpa != nullptr) {
329 // We have a clip plane; apply it.
330 Points new_points;
331 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
332 // All points are behind the clip plane; just do the default test.
333 edge_dist = dist_to_polygon(p, _points[ip]);
334 } else if (new_points.empty()) {
335 // The polygon is completely clipped.
336 continue;
337 } else {
338 // Test against the clipped polygon.
339 edge_dist = dist_to_polygon(p, new_points);
340 }
341 } else {
342 // No clip plane is in effect. Do the default test.
343 edge_dist = dist_to_polygon(p, _points[ip]);
344 }
345
346 max_dist = from_radius;
347
348 // Now we have edge_dist, which is the distance from the sphere center to
349 // the nearest edge of the polygon, within the polygon's plane.
350 // edge_dist<0 means the point is within the polygon.
351 if(edge_dist < 0) {
352 intersect = true;
353 continue;
354 }
355
356 if((edge_dist > 0) &&
357 ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
358 // No intersection; the circle is outside the polygon.
359 continue;
360 }
361
362 // The sphere appears to intersect the polygon. If the edge is less than
363 // from_radius away, the sphere may be resting on an edge of the polygon.
364 // Determine how far the center of the sphere must remain from the plane,
365 // based on its distance from the nearest edge.
366
367 if (edge_dist >= 0.0f) {
368 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
369 max_dist = csqrt(max_dist_2);
370 }
371
372 if (dist > max_dist) {
373 // There's no intersection: the sphere is hanging off the edge.
374 continue;
375 }
376 intersect = true;
377 }
378 if (!fully_inside && !intersect) {
379 return nullptr;
380 }
381
382 if (collide_cat.is_debug()) {
383 collide_cat.debug()
384 << "intersection detected from " << entry.get_from_node_path()
385 << " into " << entry.get_into_node_path() << "\n";
386 }
387
388 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
389
390 PN_stdfloat into_depth = max_dist - dist;
391 if (moved_from_center) {
392 // We have to base the depth of intersection on the sphere's final resting
393 // point, not the point from which we tested the intersection.
394 PN_stdfloat orig_dist;
395 plane.intersects_line(orig_dist, orig_center, -normal);
396 into_depth = max_dist - orig_dist;
397 }
398
399 // Clamp the surface point to the box bounds.
400 LPoint3 surface = from_center - normal * dist;
401 surface = surface.fmax(_min);
402 surface = surface.fmin(_max);
403
404 new_entry->set_surface_normal(normal);
405 new_entry->set_surface_point(surface);
406 new_entry->set_interior_point(surface - normal * into_depth);
407 new_entry->set_contact_pos(contact_point);
408 new_entry->set_contact_normal(plane.get_normal());
409 new_entry->set_t(actual_t);
410
411 return new_entry;
412}
413
414/**
415 *
416 */
417PT(CollisionEntry) CollisionBox::
418test_intersection_from_line(const CollisionEntry &entry) const {
419 const CollisionLine *line;
420 DCAST_INTO_R(line, entry.get_from(), nullptr);
421
422 CPT(TransformState) wrt_space = entry.get_wrt_space();
423 const LMatrix4 &wrt_mat = wrt_space->get_mat();
424
425 LPoint3 from_origin = line->get_origin() * wrt_mat;
426 LVector3 from_direction = line->get_direction() * wrt_mat;
427
428 double t1, t2;
429 if (!intersects_line(t1, t2, from_origin, from_direction)) {
430 // No intersection.
431 return nullptr;
432 }
433
434 if (collide_cat.is_debug()) {
435 collide_cat.debug()
436 << "intersection detected from " << entry.get_from_node_path()
437 << " into " << entry.get_into_node_path() << "\n";
438 }
439 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
440
441 LPoint3 point = from_origin + t1 * from_direction;
442 new_entry->set_surface_point(point);
443
444 if (has_effective_normal() && line->get_respect_effective_normal()) {
445 new_entry->set_surface_normal(get_effective_normal());
446 } else {
447 LVector3 normal(
448 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
449 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
450 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
451 );
452 normal.normalize();
453 new_entry->set_surface_normal(normal);
454 }
455
456 return new_entry;
457}
458
459/**
460 * Double dispatch point for ray as a FROM object
461 */
462PT(CollisionEntry) CollisionBox::
463test_intersection_from_ray(const CollisionEntry &entry) const {
464 const CollisionRay *ray;
465 DCAST_INTO_R(ray, entry.get_from(), nullptr);
466 CPT(TransformState) wrt_space = entry.get_wrt_space();
467 const LMatrix4 &wrt_mat = wrt_space->get_mat();
468
469 LPoint3 from_origin = ray->get_origin() * wrt_mat;
470 LVector3 from_direction = ray->get_direction() * wrt_mat;
471
472 double t1, t2;
473 if (!intersects_line(t1, t2, from_origin, from_direction) || (t1 < 0.0 && t2 < 0.0)) {
474 // No intersection.
475 return nullptr;
476 }
477
478 if (collide_cat.is_debug()) {
479 collide_cat.debug()
480 << "intersection detected from " << entry.get_from_node_path()
481 << " into " << entry.get_into_node_path() << "\n";
482 }
483 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
484
485 if (t1 < 0.0) {
486 // The origin is inside the box, so we take the exit as our surface point.
487 new_entry->set_interior_point(from_origin);
488 t1 = t2;
489 }
490
491 LPoint3 point = from_origin + t1 * from_direction;
492 new_entry->set_surface_point(point);
493
494 if (has_effective_normal() && ray->get_respect_effective_normal()) {
495 new_entry->set_surface_normal(get_effective_normal());
496 } else {
497 LVector3 normal(
498 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
499 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
500 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
501 );
502 normal.normalize();
503 new_entry->set_surface_normal(normal);
504 }
505
506 return new_entry;
507}
508
509/**
510 * Double dispatch point for segment as a FROM object
511 */
512PT(CollisionEntry) CollisionBox::
513test_intersection_from_segment(const CollisionEntry &entry) const {
514 const CollisionSegment *seg;
515 DCAST_INTO_R(seg, entry.get_from(), nullptr);
516 CPT(TransformState) wrt_space = entry.get_wrt_space();
517 const LMatrix4 &wrt_mat = wrt_space->get_mat();
518
519 LPoint3 from_origin = seg->get_point_a() * wrt_mat;
520 LPoint3 from_extent = seg->get_point_b() * wrt_mat;
521 LVector3 from_direction = from_extent - from_origin;
522
523 double t1, t2;
524 if (!intersects_line(t1, t2, from_origin, from_direction) ||
525 (t1 < 0.0 && t2 < 0.0) || (t1 > 1.0 && t2 > 1.0)) {
526 // No intersection.
527 return nullptr;
528 }
529
530 if (collide_cat.is_debug()) {
531 collide_cat.debug()
532 << "intersection detected from " << entry.get_from_node_path()
533 << " into " << entry.get_into_node_path() << "\n";
534 }
535 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
536
537 // In case the segment is entirely inside the cube, we consider the point
538 // closest to the surface as our entry point.
539 if (t1 < (1.0 - t2)) {
540 std::swap(t1, t2);
541 }
542
543 // Our interior point is the closest point to t2 that is inside the segment.
544 new_entry->set_interior_point(from_origin + std::min(std::max(t2, 0.0), 1.0) * from_direction);
545
546 LPoint3 point = from_origin + t1 * from_direction;
547 new_entry->set_surface_point(point);
548
549 if (has_effective_normal() && seg->get_respect_effective_normal()) {
550 new_entry->set_surface_normal(get_effective_normal());
551 } else {
552 LVector3 normal(
553 IS_NEARLY_EQUAL(point[0], _max[0]) - IS_NEARLY_EQUAL(point[0], _min[0]),
554 IS_NEARLY_EQUAL(point[1], _max[1]) - IS_NEARLY_EQUAL(point[1], _min[1]),
555 IS_NEARLY_EQUAL(point[2], _max[2]) - IS_NEARLY_EQUAL(point[2], _min[2])
556 );
557 normal.normalize();
558 new_entry->set_surface_normal(normal);
559 }
560
561 return new_entry;
562}
563
564/**
565 * Double dispatch point for capsule as a FROM object
566 */
567PT(CollisionEntry) CollisionBox::
568test_intersection_from_capsule(const CollisionEntry &entry) const {
569 const CollisionCapsule *capsule;
570 DCAST_INTO_R(capsule, entry.get_from(), nullptr);
571
572 CPT(TransformState) wrt_space = entry.get_wrt_space();
573 const LMatrix4 &wrt_mat = wrt_space->get_mat();
574
575 LPoint3 from_a = capsule->get_point_a() * wrt_mat;
576 LPoint3 from_b = capsule->get_point_b() * wrt_mat;
577 LVector3 from_direction = from_b - from_a;
578 PN_stdfloat radius_sq = wrt_mat.xform_vec(LVector3(0, 0, capsule->get_radius())).length_squared();
579 PN_stdfloat radius = csqrt(radius_sq);
580
581 LPoint3 box_min = get_min();
582 LPoint3 box_max = get_max();
583 LVector3 dimensions = box_max - box_min;
584
585 // The method below is inspired by Christer Ericson's book Real-Time
586 // Collision Detection. Instead of testing a capsule against a box, we test
587 // a segment against an box that is oversized by the capsule radius.
588
589 // First, we test if the line segment intersects a box with its faces
590 // expanded outwards by the capsule radius. If not, there is no collision.
591 double t1, t2;
592 if (!intersects_line(t1, t2, from_a, from_direction, radius)) {
593 return nullptr;
594 }
595
596 if (t2 < 0.0 || t1 > 1.0) {
597 return nullptr;
598 }
599
600 t1 = std::min(1.0, std::max(0.0, (t1 + t2) * 0.5));
601 LPoint3 point = from_a + from_direction * t1;
602
603 // We now have a point of intersection between the line segment and the
604 // oversized box. Check on how many axes it lies outside the box. If it is
605 // less than two, we know that it does not lie in one of the rounded regions
606 // of the oversized rounded box, and it is a guaranteed hit. Otherwise, we
607 // will need to test against the edge regions.
608 if ((point[0] < box_min[0] || point[0] > box_max[0]) +
609 (point[1] < box_min[1] || point[1] > box_max[1]) +
610 (point[2] < box_min[2] || point[2] > box_max[2]) > 1) {
611 // Test the capsule against each edge of the box.
612 static const struct {
613 LPoint3 point;
614 int axis;
615 } edges[] = {
616 {{0, 0, 0}, 0},
617 {{0, 1, 0}, 0},
618 {{0, 0, 1}, 0},
619 {{0, 1, 1}, 0},
620 {{0, 0, 0}, 1},
621 {{0, 0, 1}, 1},
622 {{1, 0, 0}, 1},
623 {{1, 0, 1}, 1},
624 {{0, 0, 0}, 2},
625 {{0, 1, 0}, 2},
626 {{1, 0, 0}, 2},
627 {{1, 1, 0}, 2},
628 };
629
630 PN_stdfloat best_dist_sq = FLT_MAX;
631
632 for (int i = 0; i < 12; ++i) {
633 LPoint3 vertex = edges[i].point;
634 vertex.componentwise_mult(dimensions);
635 vertex += box_min;
636 LVector3 delta(0);
637 delta[edges[i].axis] = dimensions[edges[i].axis];
638 double u1, u2;
639 CollisionCapsule::calc_closest_segment_points(u1, u2, from_a, from_direction, vertex, delta);
640 PN_stdfloat dist_sq = ((from_a + from_direction * u1) - (vertex + delta * u2)).length_squared();
641 if (dist_sq < best_dist_sq) {
642 best_dist_sq = dist_sq;
643 }
644 }
645
646 if (best_dist_sq > radius_sq) {
647 // It is not actually touching any edge.
648 return nullptr;
649 }
650 }
651
652 if (collide_cat.is_debug()) {
653 collide_cat.debug()
654 << "intersection detected from " << entry.get_from_node_path()
655 << " into " << entry.get_into_node_path() << "\n";
656 }
657 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
658
659 // Which is the longest axis?
660 LVector3 diff = point - _center;
661 diff[0] /= dimensions[0];
662 diff[1] /= dimensions[1];
663 diff[2] /= dimensions[2];
664 int axis = 0;
665 if (cabs(diff[0]) > cabs(diff[1])) {
666 if (cabs(diff[0]) > cabs(diff[2])) {
667 axis = 0;
668 } else {
669 axis = 2;
670 }
671 } else {
672 if (cabs(diff[1]) > cabs(diff[2])) {
673 axis = 1;
674 } else {
675 axis = 2;
676 }
677 }
678 LVector3 normal(0);
679 normal[axis] = std::copysign(1, diff[axis]);
680
681 LPoint3 clamped = point.fmax(box_min).fmin(box_max);
682 LPoint3 surface_point = clamped;
683 surface_point[axis] = (diff[axis] >= 0.0f) ? box_max[axis] : box_min[axis];
684
685 // Is the point inside the box?
686 LVector3 interior_vec;
687 if (clamped != point) {
688 // No, it is outside. The interior point is in the direction of the
689 // surface point.
690 interior_vec = point - surface_point;
691 if (!interior_vec.normalize()) {
692 interior_vec = normal;
693 }
694 } else {
695 // It is inside. I think any point will work for this.
696 interior_vec = normal;
697 }
698 new_entry->set_interior_point(point - interior_vec * radius);
699 new_entry->set_surface_point(surface_point);
700
701 if (has_effective_normal() && capsule->get_respect_effective_normal()) {
702 new_entry->set_surface_normal(get_effective_normal());
703 } else {
704 new_entry->set_surface_normal(normal);
705 }
706
707 return new_entry;
708}
709
710/**
711 * Double dispatch point for box as a FROM object
712 */
713PT(CollisionEntry) CollisionBox::
714test_intersection_from_box(const CollisionEntry &entry) const {
715 const CollisionBox *box;
716 DCAST_INTO_R(box, entry.get_from(), nullptr);
717
718 CPT(TransformState) wrt_space = entry.get_wrt_space();
719 const LMatrix4 &wrt_mat = wrt_space->get_mat();
720
721 LPoint3 diff = wrt_mat.xform_point_general(box->get_center()) - _center;
722 LVector3 from_extents = box->get_dimensions() * 0.5f;
723 LVector3 into_extents = get_dimensions() * 0.5f;
724
725 LVecBase3 box_x = wrt_mat.get_row3(0);
726 LVecBase3 box_y = wrt_mat.get_row3(1);
727 LVecBase3 box_z = wrt_mat.get_row3(2);
728
729 // To make the math simpler, normalize the box basis vectors, instead
730 // applying the scale to the box dimensions. Note that this doesn't work
731 // for a non-uniform scales applied after a rotation, since that has the
732 // possibility of making the box no longer a box.
733 PN_stdfloat l;
734 l = box_x.length();
735 from_extents[0] *= l;
736 box_x /= l;
737 l = box_y.length();
738 from_extents[1] *= l;
739 box_y /= l;
740 l = box_z.length();
741 from_extents[2] *= l;
742 box_z /= l;
743
744 PN_stdfloat r1, r2;
745 PN_stdfloat min_pen = 0;
746 PN_stdfloat pen;
747 int axis = 0;
748
749 // SAT test for the three axes of the into cube.
750 r1 = into_extents[0];
751 r2 = cabs(box_x[0] * from_extents[0]) +
752 cabs(box_y[0] * from_extents[1]) +
753 cabs(box_z[0] * from_extents[2]);
754 pen = r1 + r2 - cabs(diff[0]);
755 if (pen < 0) {
756 return nullptr;
757 }
758 min_pen = pen;
759
760 r1 = into_extents[1];
761 r2 = cabs(box_x[1] * from_extents[0]) +
762 cabs(box_y[1] * from_extents[1]) +
763 cabs(box_z[1] * from_extents[2]);
764 pen = r1 + r2 - cabs(diff[1]);
765 if (pen < 0) {
766 return nullptr;
767 }
768 if (pen < min_pen) {
769 min_pen = pen;
770 axis = 1;
771 }
772
773 r1 = into_extents[2];
774 r2 = cabs(box_x[2] * from_extents[0]) +
775 cabs(box_y[2] * from_extents[1]) +
776 cabs(box_z[2] * from_extents[2]);
777 pen = r1 + r2 - cabs(diff[2]);
778 if (pen < 0) {
779 return nullptr;
780 }
781 if (pen < min_pen) {
782 min_pen = pen;
783 axis = 2;
784 }
785
786 // SAT test for the three axes of the from cube.
787 r1 = cabs(box_x[0] * into_extents[0]) +
788 cabs(box_x[1] * into_extents[1]) +
789 cabs(box_x[2] * into_extents[2]);
790 r2 = from_extents[0];
791 pen = r1 + r2 - cabs(diff.dot(box_x));
792 if (pen < 0) {
793 return nullptr;
794 }
795 if (pen < min_pen) {
796 min_pen = pen;
797 }
798
799 r1 = cabs(box_y[0] * into_extents[0]) +
800 cabs(box_y[1] * into_extents[1]) +
801 cabs(box_y[2] * into_extents[2]);
802 r2 = from_extents[1];
803 pen = r1 + r2 - cabs(diff.dot(box_y));
804 if (pen < 0) {
805 return nullptr;
806 }
807 if (pen < min_pen) {
808 min_pen = pen;
809 }
810
811 r1 = cabs(box_z[0] * into_extents[0]) +
812 cabs(box_z[1] * into_extents[1]) +
813 cabs(box_z[2] * into_extents[2]);
814 r2 = from_extents[2];
815 pen = r1 + r2 - cabs(diff.dot(box_z));
816 if (pen < 0) {
817 return nullptr;
818 }
819 if (pen < min_pen) {
820 min_pen = pen;
821 }
822
823 // SAT test of the nine cross products.
824 r1 = into_extents[1] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[1]);
825 r2 = from_extents[1] * cabs(box_z[0]) + from_extents[2] * cabs(box_y[0]);
826 if (cabs(diff[2] * box_x[1] - diff[1] * box_x[2]) > r1 + r2) {
827 return nullptr;
828 }
829
830 r1 = into_extents[1] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[1]);
831 r2 = from_extents[0] * cabs(box_z[0]) + from_extents[2] * cabs(box_x[0]);
832 if (cabs(diff[2] * box_y[1] - diff[1] * box_y[2]) > r1 + r2) {
833 return nullptr;
834 }
835
836 r1 = into_extents[1] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[1]);
837 r2 = from_extents[0] * cabs(box_y[0]) + from_extents[1] * cabs(box_x[0]);
838 if (cabs(diff[2] * box_z[1] - diff[1] * box_z[2]) > r1 + r2) {
839 return nullptr;
840 }
841
842 r1 = into_extents[0] * cabs(box_x[2]) + into_extents[2] * cabs(box_x[0]);
843 r2 = from_extents[1] * cabs(box_z[1]) + from_extents[2] * cabs(box_y[1]);
844 if (cabs(diff[0] * box_x[2] - diff[2] * box_x[0]) > r1 + r2) {
845 return nullptr;
846 }
847
848 r1 = into_extents[0] * cabs(box_y[2]) + into_extents[2] * cabs(box_y[0]);
849 r2 = from_extents[0] * cabs(box_z[1]) + from_extents[2] * cabs(box_x[1]);
850 if (cabs(diff[0] * box_y[2] - diff[2] * box_y[0]) > r1 + r2) {
851 return nullptr;
852 }
853
854 r1 = into_extents[0] * cabs(box_z[2]) + into_extents[2] * cabs(box_z[0]);
855 r2 = from_extents[0] * cabs(box_y[1]) + from_extents[1] * cabs(box_x[1]);
856 if (cabs(diff[0] * box_z[2] - diff[2] * box_z[0]) > r1 + r2) {
857 return nullptr;
858 }
859
860 r1 = into_extents[0] * cabs(box_x[1]) + into_extents[1] * cabs(box_x[0]);
861 r2 = from_extents[1] * cabs(box_z[2]) + from_extents[2] * cabs(box_y[2]);
862 if (cabs(diff[1] * box_x[0] - diff[0] * box_x[1]) > r1 + r2) {
863 return nullptr;
864 }
865
866 r1 = into_extents[0] * cabs(box_y[1]) + into_extents[1] * cabs(box_y[0]);
867 r2 = from_extents[0] * cabs(box_z[2]) + from_extents[2] * cabs(box_x[2]);
868 if (cabs(diff[1] * box_y[0] - diff[0] * box_y[1]) > r1 + r2) {
869 return nullptr;
870 }
871
872 r1 = into_extents[0] * cabs(box_z[1]) + into_extents[1] * cabs(box_z[0]);
873 r2 = from_extents[0] * cabs(box_y[2]) + from_extents[1] * cabs(box_x[2]);
874 if (cabs(diff[1] * box_z[0] - diff[0] * box_z[1]) > r1 + r2) {
875 return nullptr;
876 }
877
878 if (collide_cat.is_debug()) {
879 collide_cat.debug()
880 << "intersection detected from " << entry.get_from_node_path()
881 << " into " << entry.get_into_node_path() << "\n";
882 }
883 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
884
885 // This isn't always the correct surface point. However, it seems to be
886 // enough to let the pusher do the right thing.
887 LPoint3 surface(
888 min(max(diff[0], -into_extents[0]), into_extents[0]),
889 min(max(diff[1], -into_extents[1]), into_extents[1]),
890 min(max(diff[2], -into_extents[2]), into_extents[2]));
891
892 // Create the normal along the axis of least penetration.
893 LVector3 normal(0);
894 PN_stdfloat diff_axis = diff[axis];
895 int sign = (diff_axis >= 0) ? 1 : -1;
896 normal[axis] = sign;
897 surface[axis] = into_extents[axis] * sign;
898
899 new_entry->set_surface_point(surface + _center);
900
901 // Does not generate the correct depth. Needs fixing.
902 new_entry->set_interior_point(surface + _center + normal * -min_pen);
903
904 if (has_effective_normal() && box->get_respect_effective_normal()) {
905 new_entry->set_surface_normal(get_effective_normal());
906 } else {
907 new_entry->set_surface_normal(normal);
908 }
909
910 return new_entry;
911}
912
913/**
914 * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
915 * solid.
916 */
917 void CollisionBox::
918fill_viz_geom() {
919 if (collide_cat.is_debug()) {
920 collide_cat.debug()
921 << "Recomputing viz for " << *this << "\n";
922 }
923
924 PT(GeomVertexData) vdata = new GeomVertexData
925 ("collision", GeomVertexFormat::get_v3(),
926 Geom::UH_static);
927
928 vdata->unclean_set_num_rows(8);
929
930 {
931 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
932 vertex.set_data3(_min[0], _min[1], _min[2]);
933 vertex.set_data3(_min[0], _max[1], _min[2]);
934 vertex.set_data3(_max[0], _max[1], _min[2]);
935 vertex.set_data3(_max[0], _min[1], _min[2]);
936
937 vertex.set_data3(_min[0], _min[1], _max[2]);
938 vertex.set_data3(_min[0], _max[1], _max[2]);
939 vertex.set_data3(_max[0], _max[1], _max[2]);
940 vertex.set_data3(_max[0], _min[1], _max[2]);
941 }
942
943 PT(GeomTriangles) tris = new GeomTriangles(Geom::UH_static);
944
945 // Bottom
946 tris->add_vertices(0, 1, 2);
947 tris->add_vertices(2, 3, 0);
948
949 // Top
950 tris->add_vertices(4, 7, 6);
951 tris->add_vertices(6, 5, 4);
952
953 // Sides
954 tris->add_vertices(0, 4, 1);
955 tris->add_vertices(1, 4, 5);
956
957 tris->add_vertices(1, 5, 2);
958 tris->add_vertices(2, 5, 6);
959
960 tris->add_vertices(2, 6, 3);
961 tris->add_vertices(3, 6, 7);
962
963 tris->add_vertices(3, 7, 0);
964 tris->add_vertices(0, 7, 4);
965
966 PT(GeomLines) lines = new GeomLines(Geom::UH_static);
967
968 // Bottom
969 lines->add_vertices(0, 1);
970 lines->add_vertices(1, 2);
971 lines->add_vertices(0, 3);
972 lines->add_vertices(2, 3);
973
974 // Top
975 lines->add_vertices(4, 5);
976 lines->add_vertices(5, 6);
977 lines->add_vertices(4, 7);
978 lines->add_vertices(6, 7);
979
980 // Sides
981 lines->add_vertices(0, 4);
982 lines->add_vertices(1, 5);
983 lines->add_vertices(2, 6);
984 lines->add_vertices(3, 7);
985
986 PT(Geom) geom1 = new Geom(vdata);
987 geom1->add_primitive(tris);
988
989 PT(Geom) geom2 = new Geom(vdata);
990 geom2->add_primitive(lines);
991
992 _viz_geom->add_geom(geom1, get_solid_viz_state());
993 _viz_geom->add_geom(geom2, get_wireframe_viz_state());
994
995 _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
996 _bounds_viz_geom->add_geom(geom2, get_wireframe_viz_state());
997}
998
999/**
1000 * Determine the point(s) of intersection of a parametric line with the box.
1001 * The line is infinite in both directions, and passes through "from" and
1002 * from+delta. If the line does not intersect the box, the function returns
1003 * false, and t1 and t2 are undefined. If it does intersect the box, it
1004 * returns true, and t1 and t2 are set to the points along the equation
1005 * from+t*delta that correspond to the two points of intersection.
1006 */
1007 bool CollisionBox::
1008intersects_line(double &t1, double &t2,
1009 const LPoint3 &from, const LVector3 &delta,
1010 PN_stdfloat inflate_size) const {
1011
1012 LPoint3 bmin = _min - LVector3(inflate_size);
1013 LPoint3 bmax = _max + LVector3(inflate_size);
1014
1015 double tmin = -DBL_MAX;
1016 double tmax = DBL_MAX;
1017
1018 for (int i = 0; i < 3; ++i) {
1019 PN_stdfloat d = delta[i];
1020 if (!IS_NEARLY_ZERO(d)) {
1021 double tmin2 = (bmin[i] - from[i]) / d;
1022 double tmax2 = (bmax[i] - from[i]) / d;
1023 if (tmin2 > tmax2) {
1024 std::swap(tmin2, tmax2);
1025 }
1026 tmin = std::max(tmin, tmin2);
1027 tmax = std::min(tmax, tmax2);
1028
1029 if (tmin > tmax) {
1030 return false;
1031 }
1032
1033 } else if (from[i] < bmin[i] || from[i] > bmax[i]) {
1034 // The line is entirely parallel in this dimension.
1035 return false;
1036 }
1037 }
1038
1039 t1 = tmin;
1040 t2 = tmax;
1041 return true;
1042}
1043
1044/**
1045 * Clips the polygon by all of the clip planes named in the clip plane
1046 * attribute and fills new_points up with the resulting points.
1047 *
1048 * The return value is true if the set of points is unmodified (all points are
1049 * behind all the clip planes), or false otherwise.
1050 */
1051 bool CollisionBox::
1052 apply_clip_plane(CollisionBox::Points &new_points,
1053 const ClipPlaneAttrib *cpa,
1054 const TransformState *net_transform, int plane_no) const {
1055 bool all_in = true;
1056
1057 int num_planes = cpa->get_num_on_planes();
1058 bool first_plane = true;
1059
1060 for (int i = 0; i < num_planes; i++) {
1061 NodePath plane_path = cpa->get_on_plane(i);
1062 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
1063 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
1064 CPT(TransformState) new_transform =
1065 net_transform->invert_compose(plane_path.get_net_transform());
1066
1067 LPlane plane = plane_node->get_plane() * new_transform->get_mat();
1068 if (first_plane) {
1069 first_plane = false;
1070 if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
1071 all_in = false;
1072 }
1073 } else {
1074 Points last_points;
1075 last_points.swap(new_points);
1076 if (!clip_polygon(new_points, last_points, plane, plane_no)) {
1077 all_in = false;
1078 }
1079 }
1080 }
1081 }
1082
1083 if (!all_in) {
1084 compute_vectors(new_points);
1085 }
1086
1087 return all_in;
1088}
1089/**
1090 * Clips the source_points of the polygon by the indicated clipping plane, and
1091 * modifies new_points to reflect the new set of clipped points (but does not
1092 * compute the vectors in new_points).
1093 *
1094 * The return value is true if the set of points is unmodified (all points are
1095 * behind the clip plane), or false otherwise.
1096 */
1097 bool CollisionBox::
1098 clip_polygon(CollisionBox::Points &new_points,
1099 const CollisionBox::Points &source_points,
1100 const LPlane &plane, int plane_no) const {
1101 new_points.clear();
1102 if (source_points.empty()) {
1103 return true;
1104 }
1105
1106 LPoint3 from3d;
1107 LVector3 delta3d;
1108 if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
1109 // The clipping plane is parallel to the polygon. The polygon is either
1110 // all in or all out.
1111 if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
1112 // A point within the polygon is behind the clipping plane: the polygon
1113 // is all in.
1114 new_points = source_points;
1115 return true;
1116 }
1117 return false;
1118 }
1119
1120 // Project the line of intersection into the 2-d plane. Now we have a 2-d
1121 // clipping line.
1122 LPoint2 from2d = to_2d(from3d,plane_no);
1123 LVector2 delta2d = to_2d(delta3d,plane_no);
1124
1125 PN_stdfloat a = -delta2d[1];
1126 PN_stdfloat b = delta2d[0];
1127 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
1128
1129 // Now walk through the points. Any point on the left of our line gets
1130 // removed, and the line segment clipped at the point of intersection.
1131
1132 // We might increase the number of vertices by as many as 1, if the plane
1133 // clips off exactly one corner. (We might also decrease the number of
1134 // vertices, or keep them the same number.)
1135 new_points.reserve(source_points.size() + 1);
1136
1137 LPoint2 last_point = source_points.back()._p;
1138 bool last_is_in = !is_right(last_point - from2d, delta2d);
1139 bool all_in = last_is_in;
1140 Points::const_iterator pi;
1141 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
1142 const LPoint2 &this_point = (*pi)._p;
1143 bool this_is_in = !is_right(this_point - from2d, delta2d);
1144
1145 // There appears to be a compiler bug in gcc 4.0: we need to extract this
1146 // comparison outside of the if statement.
1147 bool crossed_over = (this_is_in != last_is_in);
1148 if (crossed_over) {
1149 // We have just crossed over the clipping line. Find the point of
1150 // intersection.
1151 LVector2 d = this_point - last_point;
1152 PN_stdfloat denom = (a * d[0] + b * d[1]);
1153 if (denom != 0.0) {
1154 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
1155 LPoint2 p = last_point + t * d;
1156
1157 new_points.push_back(PointDef(p[0], p[1]));
1158 last_is_in = this_is_in;
1159 }
1160 }
1161
1162 if (this_is_in) {
1163 // We are behind the clipping line. Keep the point.
1164 new_points.push_back(PointDef(this_point[0], this_point[1]));
1165 } else {
1166 all_in = false;
1167 }
1168
1169 last_point = this_point;
1170 }
1171
1172 return all_in;
1173}
1174
1175
1176/**
1177 * Returns the linear distance from the 2-d point to the nearest part of the
1178 * polygon defined by the points vector. The result is negative if the point
1179 * is within the polygon.
1180 */
1181PN_stdfloat CollisionBox::
1182 dist_to_polygon(const LPoint2 &p, const CollisionBox::Points &points) const {
1183
1184 // We know that that the polygon is convex and is defined with the points in
1185 // counterclockwise order. Therefore, we simply compare the signed distance
1186 // to each line segment; we ignore any negative values, and take the minimum
1187 // of all the positive values.
1188
1189 // If all values are negative, the point is within the polygon; we therefore
1190 // return an arbitrary negative result.
1191
1192 bool got_dist = false;
1193 PN_stdfloat best_dist = -1.0f;
1194
1195 size_t num_points = points.size();
1196 for (size_t i = 0; i < num_points - 1; ++i) {
1197 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
1198 points[i]._v);
1199 if (d >= 0.0f) {
1200 if (!got_dist || d < best_dist) {
1201 best_dist = d;
1202 got_dist = true;
1203 }
1204 }
1205 }
1206
1207 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
1208 points[num_points - 1]._v);
1209 if (d >= 0.0f) {
1210 if (!got_dist || d < best_dist) {
1211 best_dist = d;
1212 got_dist = true;
1213 }
1214 }
1215
1216 return best_dist;
1217}
1218
1219/**
1220 * Returns the linear distance of p to the line segment defined by f and t,
1221 * where v = (t - f).normalize(). The result is negative if p is left of the
1222 * line, positive if it is right of the line. If the result is positive, it
1223 * is constrained by endpoints of the line segment (i.e. the result might be
1224 * larger than it would be for a straight distance-to-line test). If the
1225 * result is negative, we don't bother.
1226 */
1227PN_stdfloat CollisionBox::
1228dist_to_line_segment(const LPoint2 &p,
1229 const LPoint2 &f, const LPoint2 &t,
1230 const LVector2 &v) {
1231 LVector2 v1 = (p - f);
1232 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
1233 if (d < 0.0f) {
1234 return d;
1235 }
1236
1237 // Compute the nearest point on the line.
1238 LPoint2 q = p + LVector2(-v[1], v[0]) * d;
1239
1240 // Now constrain that point to the line segment.
1241 if (v[0] > 0.0f) {
1242 // X+
1243 if (v[1] > 0.0f) {
1244 // Y+
1245 if (v[0] > v[1]) {
1246 // X-dominant.
1247 if (q[0] < f[0]) {
1248 return (p - f).length();
1249 } if (q[0] > t[0]) {
1250 return (p - t).length();
1251 } else {
1252 return d;
1253 }
1254 } else {
1255 // Y-dominant.
1256 if (q[1] < f[1]) {
1257 return (p - f).length();
1258 } if (q[1] > t[1]) {
1259 return (p - t).length();
1260 } else {
1261 return d;
1262 }
1263 }
1264 } else {
1265 // Y-
1266 if (v[0] > -v[1]) {
1267 // X-dominant.
1268 if (q[0] < f[0]) {
1269 return (p - f).length();
1270 } if (q[0] > t[0]) {
1271 return (p - t).length();
1272 } else {
1273 return d;
1274 }
1275 } else {
1276 // Y-dominant.
1277 if (q[1] > f[1]) {
1278 return (p - f).length();
1279 } if (q[1] < t[1]) {
1280 return (p - t).length();
1281 } else {
1282 return d;
1283 }
1284 }
1285 }
1286 } else {
1287 // X-
1288 if (v[1] > 0.0f) {
1289 // Y+
1290 if (-v[0] > v[1]) {
1291 // X-dominant.
1292 if (q[0] > f[0]) {
1293 return (p - f).length();
1294 } if (q[0] < t[0]) {
1295 return (p - t).length();
1296 } else {
1297 return d;
1298 }
1299 } else {
1300 // Y-dominant.
1301 if (q[1] < f[1]) {
1302 return (p - f).length();
1303 } if (q[1] > t[1]) {
1304 return (p - t).length();
1305 } else {
1306 return d;
1307 }
1308 }
1309 } else {
1310 // Y-
1311 if (-v[0] > -v[1]) {
1312 // X-dominant.
1313 if (q[0] > f[0]) {
1314 return (p - f).length();
1315 } if (q[0] < t[0]) {
1316 return (p - t).length();
1317 } else {
1318 return d;
1319 }
1320 } else {
1321 // Y-dominant.
1322 if (q[1] > f[1]) {
1323 return (p - f).length();
1324 } if (q[1] < t[1]) {
1325 return (p - t).length();
1326 } else {
1327 return d;
1328 }
1329 }
1330 }
1331 }
1332}
1333
1334/**
1335 * Returns true if the indicated point is within the polygon's 2-d space,
1336 * false otherwise.
1337 */
1338 bool CollisionBox::
1339 point_is_inside(const LPoint2 &p, const CollisionBox::Points &points) const {
1340 // We insist that the polygon be convex. This makes things a bit simpler.
1341 // In the case of a convex polygon, defined with points in counterclockwise
1342 // order, a point is interior to the polygon iff the point is not right of
1343 // each of the edges.
1344 for (int i = 0; i < (int)points.size() - 1; i++) {
1345 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
1346 return false;
1347 }
1348 }
1349 if (is_right(p - points[points.size() - 1]._p,
1350 points[0]._p - points[points.size() - 1]._p)) {
1351 return false;
1352 }
1353
1354 return true;
1355}
1356
1357/**
1358 * Now that the _p members of the given points array have been computed, go
1359 * back and compute all of the _v members.
1360 */
1361 void CollisionBox::
1362 compute_vectors(Points &points) {
1363 size_t num_points = points.size();
1364 for (size_t i = 0; i < num_points; i++) {
1365 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
1366 points[i]._v.normalize();
1367 }
1368}
1369
1370/**
1371 * Factory method to generate a CollisionBox object
1372 */
1373 void CollisionBox::
1374 register_with_read_factory() {
1375 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
1376}
1377
1378/**
1379 * Function to write the important information in the particular object to a
1380 * Datagram
1381 */
1382 void CollisionBox::
1383 write_datagram(BamWriter *manager, Datagram &me) {
1384 CollisionSolid::write_datagram(manager, me);
1385 _center.write_datagram(me);
1386 _min.write_datagram(me);
1387 _max.write_datagram(me);
1388 for(int i=0; i < 8; i++) {
1389 _vertex[i].write_datagram(me);
1390 }
1391 me.add_stdfloat(_radius);
1392 me.add_stdfloat(_x);
1393 me.add_stdfloat(_y);
1394 me.add_stdfloat(_z);
1395 for(int i=0; i < 6; i++) {
1396 _planes[i].write_datagram(me);
1397 }
1398 for(int i=0; i < 6; i++) {
1399 _to_2d_mat[i].write_datagram(me);
1400 }
1401 for(int i=0; i < 6; i++) {
1402 me.add_uint16(_points[i].size());
1403 for (size_t j = 0; j < _points[i].size(); j++) {
1404 _points[i][j]._p.write_datagram(me);
1405 _points[i][j]._v.write_datagram(me);
1406 }
1407 }
1408}
1409
1410/**
1411 * Factory method to generate a CollisionBox object
1412 */
1413 TypedWritable *CollisionBox::
1414make_CollisionBox(const FactoryParams &params) {
1415 CollisionBox *me = new CollisionBox;
1416 DatagramIterator scan;
1417 BamReader *manager;
1418
1419 parse_params(params, scan, manager);
1420 me->fillin(scan, manager);
1421 return me;
1422}
1423
1424/**
1425 * Function that reads out of the datagram (or asks manager to read) all of
1426 * the data that is needed to re-create this object and stores it in the
1427 * appropiate place
1428 */
1429 void CollisionBox::
1430fillin(DatagramIterator& scan, BamReader* manager) {
1431 CollisionSolid::fillin(scan, manager);
1432 _center.read_datagram(scan);
1433 _min.read_datagram(scan);
1434 _max.read_datagram(scan);
1435 for(int i=0; i < 8; i++) {
1436 _vertex[i].read_datagram(scan);
1437 }
1438 _radius = scan.get_stdfloat();
1439 _x = scan.get_stdfloat();
1440 _y = scan.get_stdfloat();
1441 _z = scan.get_stdfloat();
1442 for(int i=0; i < 6; i++) {
1443 _planes[i].read_datagram(scan);
1444 }
1445 for(int i=0; i < 6; i++) {
1446 _to_2d_mat[i].read_datagram(scan);
1447 }
1448 for(int i=0; i < 6; i++) {
1449 size_t size = scan.get_uint16();
1450 for (size_t j = 0; j < size; j++) {
1451 LPoint2 p;
1452 LVector2 v;
1453 p.read_datagram(scan);
1454 v.read_datagram(scan);
1455 _points[i].push_back(PointDef(p, v));
1456 }
1457 if (size < 4) {
1458 LPoint3 array[4];
1459 array[0] = _vertex[plane_def[i][0]];
1460 array[1] = _vertex[plane_def[i][1]];
1461 array[2] = _vertex[plane_def[i][2]];
1462 array[3] = _vertex[plane_def[i][3]];
1463 setup_points(array, array+4, i);
1464 }
1465 }
1466}
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
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:1374
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:1182
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:1052
CollisionBox::CollisionBox
CollisionBox(const LPoint3 &center, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Create the Box by giving a Center and distances of each of the sides of box from the Center.
Definition collisionBox.I:19
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:1098
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:1383
CollisionBox::test_intersection
virtual PointerTo< CollisionEntry > test_intersection(const CollisionEntry &entry) const
First Dispatch point for box as a FROM object.
Definition collisionBox.cxx:130
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:1362
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::xform
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
Definition collisionBox.cxx:138
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:1339
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
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
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
DatagramIterator
A class to retrieve the individual data elements previously stored in a Datagram.
Definition datagramIterator.h:30
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
GeomVertexFormat::get_v3
static const GeomVertexFormat * get_v3()
Returns a standard vertex format with just a 3-component vertex position.
Definition geomVertexFormat.I:251
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_clip_effect
get_clip_effect
Returns the clip_effect bits for this clip plane.
Definition planeNode.h:71
PlaneNode::get_plane
get_plane
Returns the plane represented by the PlaneNode.
Definition planeNode.h:68
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
array
STL class.
cmath.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
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.
TransformState
STTransform::operator CPT TransformState() const
This is used internally to convert an STTransform into a TransformState pointer.
Definition stTransform.I:98