00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "collisionBox.h"
00016 #include "collisionLine.h"
00017 #include "collisionRay.h"
00018 #include "collisionSphere.h"
00019 #include "collisionSegment.h"
00020 #include "collisionHandler.h"
00021 #include "collisionEntry.h"
00022 #include "config_collide.h"
00023 #include "boundingSphere.h"
00024 #include "datagram.h"
00025 #include "datagramIterator.h"
00026 #include "bamReader.h"
00027 #include "bamWriter.h"
00028 #include "nearly_zero.h"
00029 #include "cmath.h"
00030 #include "mathNumbers.h"
00031 #include "geom.h"
00032 #include "geomTrifans.h"
00033 #include "geomVertexWriter.h"
00034 #include "config_mathutil.h"
00035 #include "dcast.h"
00036
00037 #include <math.h>
00038
00039 PStatCollector CollisionBox::_volume_pcollector("Collision Volumes:CollisionBox");
00040 PStatCollector CollisionBox::_test_pcollector("Collision Tests:CollisionBox");
00041 TypeHandle CollisionBox::_type_handle;
00042
00043 const int CollisionBox::plane_def[6][4] = {
00044 {0, 4, 5, 1},
00045 {4, 6, 7, 5},
00046 {6, 2, 3, 7},
00047 {2, 0, 1, 3},
00048 {1, 5, 7, 3},
00049 {2, 6, 4, 0},
00050 };
00051
00052
00053
00054
00055
00056
00057 CollisionSolid *CollisionBox::
00058 make_copy() {
00059 return new CollisionBox(*this);
00060 }
00061
00062
00063
00064
00065
00066
00067 void CollisionBox::
00068 setup_box(){
00069 for(int plane = 0; plane < 6; plane++) {
00070 LPoint3 array[4];
00071 array[0] = get_point(plane_def[plane][0]);
00072 array[1] = get_point(plane_def[plane][1]);
00073 array[2] = get_point(plane_def[plane][2]);
00074 array[3] = get_point(plane_def[plane][3]);
00075 setup_points(array, array+4, plane);
00076 }
00077 }
00078
00079
00080
00081
00082
00083
00084
00085 void CollisionBox::
00086 setup_points(const LPoint3 *begin, const LPoint3 *end, int plane) {
00087 int num_points = end - begin;
00088 nassertv(num_points >= 3);
00089
00090 _points[plane].clear();
00091
00092
00093
00094 LMatrix4 to_3d_mat;
00095 calc_to_3d_mat(to_3d_mat, plane);
00096
00097
00098
00099 _to_2d_mat[plane].invert_from(to_3d_mat);
00100
00101
00102
00103 const LPoint3 *pi;
00104 for (pi = begin; pi != end; ++pi) {
00105 LPoint3 point = (*pi) * _to_2d_mat[plane];
00106 _points[plane].push_back(PointDef(point[0], point[2]));
00107 }
00108
00109 nassertv(_points[plane].size() >= 3);
00110
00111 #ifndef NDEBUG
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 #endif
00126
00127 compute_vectors(_points[plane]);
00128 }
00129
00130
00131
00132
00133
00134
00135 PT(CollisionEntry) CollisionBox::
00136 test_intersection(const CollisionEntry &entry) const {
00137 return entry.get_into()->test_intersection_from_box(entry);
00138 }
00139
00140
00141
00142
00143
00144
00145 void CollisionBox::
00146 xform(const LMatrix4 &mat) {
00147 _center = _center * mat;
00148 for(int v = 0; v < 8; v++)
00149 _vertex[v] = _vertex[v] * mat;
00150 for(int p = 0; p < 6 ; p++)
00151 _planes[p] = set_plane(p);
00152 _x = _vertex[0].get_x()-_center.get_x();
00153 _y = _vertex[0].get_y()-_center.get_y();
00154 _z = _vertex[0].get_z()-_center.get_z();
00155 _radius = sqrt( _x*_x + _y*_y + _z*_z );
00156 setup_box();
00157 mark_viz_stale();
00158 mark_internal_bounds_stale();
00159 }
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169 LPoint3 CollisionBox::
00170 get_collision_origin() const {
00171 return _center;
00172 }
00173
00174
00175
00176
00177
00178
00179 LPoint3 CollisionBox::
00180 get_min() const {
00181 return _min;
00182 }
00183
00184
00185
00186
00187
00188
00189 LPoint3 CollisionBox::
00190 get_max() const {
00191 return _max;
00192 }
00193
00194
00195
00196
00197
00198
00199 LPoint3 CollisionBox::
00200 get_approx_center() const {
00201 return (_min + _max) * 0.5f;
00202 }
00203
00204
00205
00206
00207
00208
00209
00210
00211 PStatCollector &CollisionBox::
00212 get_volume_pcollector() {
00213 return _volume_pcollector;
00214 }
00215
00216
00217
00218
00219
00220
00221
00222
00223 PStatCollector &CollisionBox::
00224 get_test_pcollector() {
00225 return _test_pcollector;
00226 }
00227
00228
00229
00230
00231
00232
00233 void CollisionBox::
00234 output(ostream &out) const {
00235 }
00236
00237
00238
00239
00240
00241
00242
00243 PT(BoundingVolume) CollisionBox::
00244 compute_internal_bounds() const {
00245 return new BoundingSphere(_center, _radius);
00246 }
00247
00248
00249
00250
00251
00252
00253 PT(CollisionEntry) CollisionBox::
00254 test_intersection_from_sphere(const CollisionEntry &entry) const {
00255
00256 const CollisionSphere *sphere;
00257 DCAST_INTO_R(sphere, entry.get_from(), 0);
00258
00259 CPT(TransformState) wrt_space = entry.get_wrt_space();
00260 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00261
00262 const LMatrix4 &wrt_mat = wrt_space->get_mat();
00263
00264 LPoint3 orig_center = sphere->get_center() * wrt_mat;
00265 LPoint3 from_center = orig_center;
00266 bool moved_from_center = false;
00267 PN_stdfloat t = 1.0f;
00268 LPoint3 contact_point(from_center);
00269 PN_stdfloat actual_t = 1.0f;
00270
00271 LVector3 from_radius_v =
00272 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00273 PN_stdfloat from_radius_2 = from_radius_v.length_squared();
00274 PN_stdfloat from_radius = csqrt(from_radius_2);
00275
00276 int ip;
00277 PN_stdfloat max_dist = 0.0;
00278 PN_stdfloat dist = 0.0;
00279 bool intersect;
00280 LPlane plane;
00281 LVector3 normal;
00282
00283 for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
00284 plane = get_plane( ip );
00285 if (_points[ip].size() < 3) {
00286 continue;
00287 }
00288 if (wrt_prev_space != wrt_space) {
00289
00290
00291
00292 LPoint3 b = from_center;
00293 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat();
00294 LVector3 delta = b - a;
00295
00296
00297
00298 PN_stdfloat dot = delta.dot(plane.get_normal());
00299 if (dot > 0.1f) {
00300 continue;
00301 }
00302
00303 if (IS_NEARLY_ZERO(dot)) {
00304
00305
00306
00307 } else {
00308
00309
00310
00311
00312
00313
00314
00315
00316 PN_stdfloat dist_to_p = plane.dist_to_plane(a);
00317 t = (dist_to_p / -dot);
00318
00319
00320
00321 actual_t = ((dist_to_p - from_radius) / -dot);
00322 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t));
00323 contact_point = a + (actual_t * delta);
00324
00325 if (t >= 1.0f) {
00326
00327
00328 } else if (t < 0.0f) {
00329 from_center = a;
00330 moved_from_center = true;
00331 } else {
00332 from_center = a + t * delta;
00333 moved_from_center = true;
00334 }
00335 }
00336 }
00337
00338 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
00339
00340 #ifndef NDEBUG
00341
00342
00343
00344
00345
00346
00347
00348 #endif
00349
00350
00351
00352
00353 if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
00354
00355
00356 continue;
00357 }
00358
00359 if (dist > from_radius || dist < -from_radius) {
00360
00361 continue;
00362 }
00363
00364 LPoint2 p = to_2d(from_center - dist * plane.get_normal(), ip);
00365 PN_stdfloat edge_dist = 0.0f;
00366
00367 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00368 if (cpa != (ClipPlaneAttrib *)NULL) {
00369
00370 Points new_points;
00371 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
00372
00373
00374 edge_dist = dist_to_polygon(p, _points[ip]);
00375 } else if (new_points.empty()) {
00376
00377 continue;
00378 } else {
00379
00380 edge_dist = dist_to_polygon(p, new_points);
00381 }
00382 } else {
00383
00384 edge_dist = dist_to_polygon(p, _points[ip]);
00385 }
00386
00387 max_dist = from_radius;
00388
00389
00390
00391
00392 if(edge_dist < 0) {
00393 intersect = true;
00394 continue;
00395 }
00396
00397 if((edge_dist > 0) &&
00398 ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
00399
00400 continue;
00401 }
00402
00403
00404
00405
00406
00407
00408
00409 if (edge_dist >= 0.0f) {
00410 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
00411 max_dist = csqrt(max_dist_2);
00412 }
00413
00414 if (dist > max_dist) {
00415
00416 continue;
00417 }
00418 intersect = true;
00419 }
00420 if( !intersect )
00421 return NULL;
00422
00423 if (collide_cat.is_debug()) {
00424 collide_cat.debug()
00425 << "intersection detected from " << entry.get_from_node_path()
00426 << " into " << entry.get_into_node_path() << "\n";
00427 }
00428
00429 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00430
00431 PN_stdfloat into_depth = max_dist - dist;
00432 if (moved_from_center) {
00433
00434
00435
00436 PN_stdfloat orig_dist;
00437 plane.intersects_line(orig_dist, orig_center, -normal);
00438 into_depth = max_dist - orig_dist;
00439 }
00440
00441 new_entry->set_surface_normal(normal);
00442 new_entry->set_surface_point(from_center - normal * dist);
00443 new_entry->set_interior_point(from_center - normal * (dist + into_depth));
00444 new_entry->set_contact_pos(contact_point);
00445 new_entry->set_contact_normal(plane.get_normal());
00446 new_entry->set_t(actual_t);
00447
00448 return new_entry;
00449 }
00450
00451
00452
00453
00454
00455
00456
00457 PT(CollisionEntry) CollisionBox::
00458 test_intersection_from_ray(const CollisionEntry &entry) const {
00459 const CollisionRay *ray;
00460 DCAST_INTO_R(ray, entry.get_from(), 0);
00461 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00462
00463 LPoint3 from_origin = ray->get_origin() * wrt_mat;
00464 LVector3 from_direction = ray->get_direction() * wrt_mat;
00465
00466 int i, j;
00467 PN_stdfloat t;
00468 PN_stdfloat near_t = 0.0;
00469 bool intersect;
00470 LPlane plane;
00471 LPlane near_plane;
00472
00473
00474
00475 for (i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
00476 plane = get_plane(i);
00477
00478 if (!plane.intersects_line(t, from_origin, from_direction)) {
00479
00480 continue;
00481 }
00482
00483 if (t < 0.0f) {
00484
00485
00486 continue;
00487 }
00488 LPoint3 plane_point = from_origin + t * from_direction;
00489 LPoint2 p = to_2d(plane_point, i);
00490
00491 if (!point_is_inside(p, _points[i])){
00492 continue;
00493 }
00494 intersect = true;
00495 if (j) {
00496 if(t < near_t) {
00497 near_plane = plane;
00498 near_t = t;
00499 }
00500 }
00501 else {
00502 near_plane = plane;
00503 near_t = t;
00504 }
00505 ++j;
00506 }
00507
00508
00509 if(!intersect) {
00510
00511 return NULL;
00512 }
00513
00514 if (collide_cat.is_debug()) {
00515 collide_cat.debug()
00516 << "intersection detected from " << entry.get_from_node_path()
00517 << " into " << entry.get_into_node_path() << "\n";
00518 }
00519
00520 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00521
00522 LPoint3 into_intersection_point = from_origin + near_t * from_direction;
00523
00524 LVector3 normal =
00525 (has_effective_normal() && ray->get_respect_effective_normal())
00526 ? get_effective_normal() : near_plane.get_normal();
00527
00528 new_entry->set_surface_normal(normal);
00529 new_entry->set_surface_point(into_intersection_point);
00530
00531 return new_entry;
00532 }
00533
00534
00535
00536
00537
00538
00539
00540 PT(CollisionEntry) CollisionBox::
00541 test_intersection_from_segment(const CollisionEntry &entry) const {
00542 const CollisionSegment *seg;
00543 DCAST_INTO_R(seg, entry.get_from(), 0);
00544 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00545
00546 LPoint3 from_origin = seg->get_point_a() * wrt_mat;
00547 LPoint3 from_extent = seg->get_point_b() * wrt_mat;
00548 LVector3 from_direction = from_extent - from_origin;
00549
00550 int i, j;
00551 PN_stdfloat t;
00552 PN_stdfloat near_t = 0.0;
00553 bool intersect;
00554 LPlane plane;
00555 LPlane near_plane;
00556
00557
00558
00559 for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
00560 plane = get_plane(i);
00561
00562 if (!plane.intersects_line(t, from_origin, from_direction)) {
00563
00564 continue;
00565 }
00566
00567 if (t < 0.0f || t > 1.0f) {
00568
00569
00570
00571 continue;
00572 }
00573 LPoint3 plane_point = from_origin + t * from_direction;
00574 LPoint2 p = to_2d(plane_point, i);
00575
00576 if (!point_is_inside(p, _points[i])){
00577 continue;
00578 }
00579 intersect = true;
00580 if(j) {
00581 if(t < near_t) {
00582 near_plane = plane;
00583 near_t = t;
00584 }
00585 }
00586 else {
00587 near_plane = plane;
00588 near_t = t;
00589 }
00590 ++j;
00591 }
00592
00593
00594 if(!intersect) {
00595
00596 return NULL;
00597 }
00598
00599 if (collide_cat.is_debug()) {
00600 collide_cat.debug()
00601 << "intersection detected from " << entry.get_from_node_path()
00602 << " into " << entry.get_into_node_path() << "\n";
00603 }
00604
00605 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00606
00607 LPoint3 into_intersection_point = from_origin + near_t * from_direction;
00608
00609 LVector3 normal =
00610 (has_effective_normal() && seg->get_respect_effective_normal())
00611 ? get_effective_normal() : near_plane.get_normal();
00612
00613 new_entry->set_surface_normal(normal);
00614 new_entry->set_surface_point(into_intersection_point);
00615
00616 return new_entry;
00617 }
00618
00619
00620
00621
00622
00623
00624
00625
00626 PT(CollisionEntry) CollisionBox::
00627 test_intersection_from_box(const CollisionEntry &entry) const {
00628 return NULL;
00629 }
00630
00631
00632
00633
00634
00635
00636
00637
00638 void CollisionBox::
00639 fill_viz_geom() {
00640 if (collide_cat.is_debug()) {
00641 collide_cat.debug()
00642 << "Recomputing viz for " << *this << "\n";
00643 }
00644
00645 PT(GeomVertexData) vdata = new GeomVertexData
00646 ("collision", GeomVertexFormat::get_v3(),
00647 Geom::UH_static);
00648 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00649
00650 for(int i = 0; i < 6; i++) {
00651 for(int j = 0; j < 4; j++)
00652 vertex.add_data3(get_point(plane_def[i][j]));
00653
00654 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
00655 body->add_consecutive_vertices(i*4, 4);
00656 body->close_primitive();
00657
00658 PT(Geom) geom = new Geom(vdata);
00659 geom->add_primitive(body);
00660
00661 _viz_geom->add_geom(geom, get_solid_viz_state());
00662 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
00663 }
00664 }
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677 bool CollisionBox::
00678 apply_clip_plane(CollisionBox::Points &new_points,
00679 const ClipPlaneAttrib *cpa,
00680 const TransformState *net_transform, int plane_no) const {
00681 bool all_in = true;
00682
00683 int num_planes = cpa->get_num_on_planes();
00684 bool first_plane = true;
00685
00686 for (int i = 0; i < num_planes; i++) {
00687 NodePath plane_path = cpa->get_on_plane(i);
00688 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
00689 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
00690 CPT(TransformState) new_transform =
00691 net_transform->invert_compose(plane_path.get_net_transform());
00692
00693 LPlane plane = plane_node->get_plane() * new_transform->get_mat();
00694 if (first_plane) {
00695 first_plane = false;
00696 if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
00697 all_in = false;
00698 }
00699 } else {
00700 Points last_points;
00701 last_points.swap(new_points);
00702 if (!clip_polygon(new_points, last_points, plane, plane_no)) {
00703 all_in = false;
00704 }
00705 }
00706 }
00707 }
00708
00709 if (!all_in) {
00710 compute_vectors(new_points);
00711 }
00712
00713 return all_in;
00714 }
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727 bool CollisionBox::
00728 clip_polygon(CollisionBox::Points &new_points,
00729 const CollisionBox::Points &source_points,
00730 const LPlane &plane, int plane_no) const {
00731 new_points.clear();
00732 if (source_points.empty()) {
00733 return true;
00734 }
00735
00736 LPoint3 from3d;
00737 LVector3 delta3d;
00738 if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
00739
00740
00741 if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
00742
00743
00744 new_points = source_points;
00745 return true;
00746 }
00747 return false;
00748 }
00749
00750
00751
00752 LPoint2 from2d = to_2d(from3d,plane_no);
00753 LVector2 delta2d = to_2d(delta3d,plane_no);
00754
00755 PN_stdfloat a = -delta2d[1];
00756 PN_stdfloat b = delta2d[0];
00757 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
00758
00759
00760
00761
00762
00763
00764
00765
00766 new_points.reserve(source_points.size() + 1);
00767
00768 LPoint2 last_point = source_points.back()._p;
00769 bool last_is_in = !is_right(last_point - from2d, delta2d);
00770 bool all_in = last_is_in;
00771 Points::const_iterator pi;
00772 for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
00773 const LPoint2 &this_point = (*pi)._p;
00774 bool this_is_in = !is_right(this_point - from2d, delta2d);
00775
00776
00777
00778 bool crossed_over = (this_is_in != last_is_in);
00779 if (crossed_over) {
00780
00781
00782 LVector2 d = this_point - last_point;
00783 PN_stdfloat denom = (a * d[0] + b * d[1]);
00784 if (denom != 0.0) {
00785 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom;
00786 LPoint2 p = last_point + t * d;
00787
00788 new_points.push_back(PointDef(p[0], p[1]));
00789 last_is_in = this_is_in;
00790 }
00791 }
00792
00793 if (this_is_in) {
00794
00795 new_points.push_back(PointDef(this_point[0], this_point[1]));
00796 } else {
00797 all_in = false;
00798 }
00799
00800 last_point = this_point;
00801 }
00802
00803 return all_in;
00804 }
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815 PN_stdfloat CollisionBox::
00816 dist_to_polygon(const LPoint2 &p, const CollisionBox::Points &points) const {
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826 bool got_dist = false;
00827 PN_stdfloat best_dist = -1.0f;
00828
00829 size_t num_points = points.size();
00830 for (size_t i = 0; i < num_points - 1; ++i) {
00831 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
00832 points[i]._v);
00833 if (d >= 0.0f) {
00834 if (!got_dist || d < best_dist) {
00835 best_dist = d;
00836 got_dist = true;
00837 }
00838 }
00839 }
00840
00841 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
00842 points[num_points - 1]._v);
00843 if (d >= 0.0f) {
00844 if (!got_dist || d < best_dist) {
00845 best_dist = d;
00846 got_dist = true;
00847 }
00848 }
00849
00850 return best_dist;
00851 }
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865 PN_stdfloat CollisionBox::
00866 dist_to_line_segment(const LPoint2 &p,
00867 const LPoint2 &f, const LPoint2 &t,
00868 const LVector2 &v) {
00869 LVector2 v1 = (p - f);
00870 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]);
00871 if (d < 0.0f) {
00872 return d;
00873 }
00874
00875
00876 LPoint2 q = p + LVector2(-v[1], v[0]) * d;
00877
00878
00879 if (v[0] > 0.0f) {
00880
00881 if (v[1] > 0.0f) {
00882
00883 if (v[0] > v[1]) {
00884
00885 if (q[0] < f[0]) {
00886 return (p - f).length();
00887 } if (q[0] > t[0]) {
00888 return (p - t).length();
00889 } else {
00890 return d;
00891 }
00892 } else {
00893
00894 if (q[1] < f[1]) {
00895 return (p - f).length();
00896 } if (q[1] > t[1]) {
00897 return (p - t).length();
00898 } else {
00899 return d;
00900 }
00901 }
00902 } else {
00903
00904 if (v[0] > -v[1]) {
00905
00906 if (q[0] < f[0]) {
00907 return (p - f).length();
00908 } if (q[0] > t[0]) {
00909 return (p - t).length();
00910 } else {
00911 return d;
00912 }
00913 } else {
00914
00915 if (q[1] > f[1]) {
00916 return (p - f).length();
00917 } if (q[1] < t[1]) {
00918 return (p - t).length();
00919 } else {
00920 return d;
00921 }
00922 }
00923 }
00924 } else {
00925
00926 if (v[1] > 0.0f) {
00927
00928 if (-v[0] > v[1]) {
00929
00930 if (q[0] > f[0]) {
00931 return (p - f).length();
00932 } if (q[0] < t[0]) {
00933 return (p - t).length();
00934 } else {
00935 return d;
00936 }
00937 } else {
00938
00939 if (q[1] < f[1]) {
00940 return (p - f).length();
00941 } if (q[1] > t[1]) {
00942 return (p - t).length();
00943 } else {
00944 return d;
00945 }
00946 }
00947 } else {
00948
00949 if (-v[0] > -v[1]) {
00950
00951 if (q[0] > f[0]) {
00952 return (p - f).length();
00953 } if (q[0] < t[0]) {
00954 return (p - t).length();
00955 } else {
00956 return d;
00957 }
00958 } else {
00959
00960 if (q[1] > f[1]) {
00961 return (p - f).length();
00962 } if (q[1] < t[1]) {
00963 return (p - t).length();
00964 } else {
00965 return d;
00966 }
00967 }
00968 }
00969 }
00970 }
00971
00972
00973
00974
00975
00976
00977
00978 bool CollisionBox::
00979 point_is_inside(const LPoint2 &p, const CollisionBox::Points &points) const {
00980
00981
00982
00983
00984 for (int i = 0; i < (int)points.size() - 1; i++) {
00985 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
00986 return false;
00987 }
00988 }
00989 if (is_right(p - points[points.size() - 1]._p,
00990 points[0]._p - points[points.size() - 1]._p)) {
00991 return false;
00992 }
00993
00994 return true;
00995 }
00996
00997
00998
00999
01000
01001
01002
01003
01004 void CollisionBox::
01005 compute_vectors(Points &points) {
01006 size_t num_points = points.size();
01007 for (size_t i = 0; i < num_points; i++) {
01008 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
01009 points[i]._v.normalize();
01010 }
01011 }
01012
01013
01014
01015
01016
01017
01018 void CollisionBox::
01019 register_with_read_factory() {
01020 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
01021 }
01022
01023
01024
01025
01026
01027
01028
01029 void CollisionBox::
01030 write_datagram(BamWriter *manager, Datagram &me) {
01031 CollisionSolid::write_datagram(manager, me);
01032 _center.write_datagram(me);
01033 _min.write_datagram(me);
01034 _max.write_datagram(me);
01035 for(int i=0; i < 8; i++) {
01036 _vertex[i].write_datagram(me);
01037 }
01038 me.add_stdfloat(_radius);
01039 me.add_stdfloat(_x);
01040 me.add_stdfloat(_y);
01041 me.add_stdfloat(_z);
01042 for(int i=0; i < 6; i++) {
01043 _planes[i].write_datagram(me);
01044 }
01045 for(int i=0; i < 6; i++) {
01046 _to_2d_mat[i].write_datagram(me);
01047 }
01048 for(int i=0; i < 6; i++) {
01049 me.add_uint16(_points[i].size());
01050 for (size_t j = 0; j < _points[i].size(); j++) {
01051 _points[i][j]._p.write_datagram(me);
01052 _points[i][j]._v.write_datagram(me);
01053 }
01054 }
01055 }
01056
01057
01058
01059
01060
01061
01062 TypedWritable *CollisionBox::
01063 make_CollisionBox(const FactoryParams ¶ms) {
01064 CollisionBox *me = new CollisionBox;
01065 DatagramIterator scan;
01066 BamReader *manager;
01067
01068 parse_params(params, scan, manager);
01069 me->fillin(scan, manager);
01070 return me;
01071 }
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081 void CollisionBox::
01082 fillin(DatagramIterator& scan, BamReader* manager) {
01083 CollisionSolid::fillin(scan, manager);
01084 _center.read_datagram(scan);
01085 _min.read_datagram(scan);
01086 _max.read_datagram(scan);
01087 for(int i=0; i < 8; i++) {
01088 _vertex[i].read_datagram(scan);
01089 }
01090 _radius = scan.get_stdfloat();
01091 _x = scan.get_stdfloat();
01092 _y = scan.get_stdfloat();
01093 _z = scan.get_stdfloat();
01094 for(int i=0; i < 6; i++) {
01095 _planes[i].read_datagram(scan);
01096 }
01097 for(int i=0; i < 6; i++) {
01098 _to_2d_mat[i].read_datagram(scan);
01099 }
01100 for(int i=0; i < 6; i++) {
01101 size_t size = scan.get_uint16();
01102 for (size_t j = 0; j < size; j++) {
01103 LPoint2 p;
01104 LVector2 v;
01105 p.read_datagram(scan);
01106 v.read_datagram(scan);
01107 _points[i].push_back(PointDef(p, v));
01108 }
01109 }
01110 }