00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "collisionSphere.h"
00017 #include "collisionLine.h"
00018 #include "collisionRay.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 "geomTristrips.h"
00033 #include "geomVertexWriter.h"
00034
00035 PStatCollector CollisionSphere::_volume_pcollector(
00036 "Collision Volumes:CollisionSphere");
00037 PStatCollector CollisionSphere::_test_pcollector(
00038 "Collision Tests:CollisionSphere");
00039 TypeHandle CollisionSphere::_type_handle;
00040
00041
00042
00043
00044
00045
00046 CollisionSolid *CollisionSphere::
00047 make_copy() {
00048 return new CollisionSphere(*this);
00049 }
00050
00051
00052
00053
00054
00055
00056 PT(CollisionEntry) CollisionSphere::
00057 test_intersection(const CollisionEntry &entry) const {
00058 return entry.get_into()->test_intersection_from_sphere(entry);
00059 }
00060
00061
00062
00063
00064
00065
00066 void CollisionSphere::
00067 xform(const LMatrix4 &mat) {
00068 _center = _center * mat;
00069
00070
00071
00072 LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
00073 _radius = length(radius_v);
00074 mark_viz_stale();
00075 mark_internal_bounds_stale();
00076 }
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 LPoint3 CollisionSphere::
00087 get_collision_origin() const {
00088 return get_center();
00089 }
00090
00091
00092
00093
00094
00095
00096
00097
00098 PStatCollector &CollisionSphere::
00099 get_volume_pcollector() {
00100 return _volume_pcollector;
00101 }
00102
00103
00104
00105
00106
00107
00108
00109
00110 PStatCollector &CollisionSphere::
00111 get_test_pcollector() {
00112 return _test_pcollector;
00113 }
00114
00115
00116
00117
00118
00119
00120 void CollisionSphere::
00121 output(ostream &out) const {
00122 out << "sphere, c (" << get_center() << "), r " << get_radius();
00123 }
00124
00125
00126
00127
00128
00129
00130 PT(BoundingVolume) CollisionSphere::
00131 compute_internal_bounds() const {
00132 return new BoundingSphere(_center, _radius);
00133 }
00134
00135
00136
00137
00138
00139
00140 PT(CollisionEntry) CollisionSphere::
00141 test_intersection_from_sphere(const CollisionEntry &entry) const {
00142 const CollisionSphere *sphere;
00143 DCAST_INTO_R(sphere, entry.get_from(), 0);
00144
00145 CPT(TransformState) wrt_space = entry.get_wrt_space();
00146
00147 const LMatrix4 &wrt_mat = wrt_space->get_mat();
00148
00149 LPoint3 from_b = sphere->get_center() * wrt_mat;
00150
00151 LPoint3 into_center(get_center());
00152 PN_stdfloat into_radius(get_radius());
00153
00154 LVector3 from_radius_v =
00155 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00156 PN_stdfloat from_radius = length(from_radius_v);
00157
00158 LPoint3 into_intersection_point(from_b);
00159 double t1, t2;
00160 LPoint3 contact_point(into_intersection_point);
00161 PN_stdfloat actual_t = 0.0f;
00162
00163 LVector3 vec = from_b - into_center;
00164 PN_stdfloat dist2 = dot(vec, vec);
00165 if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) {
00166
00167
00168 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00169 LPoint3 from_a = sphere->get_center() * wrt_prev_space->get_mat();
00170
00171 if (!from_a.almost_equal(from_b)) {
00172 LVector3 from_direction = from_b - from_a;
00173 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
00174
00175 return NULL;
00176 }
00177
00178 if (t2 < 0.0 || t1 > 1.0) {
00179
00180
00181 return NULL;
00182 }
00183
00184
00185 actual_t = min(1.0, max(0.0, t1));
00186 contact_point = from_a + actual_t * (from_b - from_a);
00187
00188 if (t1 < 0.0) {
00189
00190
00191 into_intersection_point = from_a;
00192 } else {
00193
00194
00195 into_intersection_point = from_a + t1 * from_direction;
00196 }
00197 } else {
00198
00199 return NULL;
00200 }
00201 }
00202
00203 if (collide_cat.is_debug()) {
00204 collide_cat.debug()
00205 << "intersection detected from " << entry.get_from_node_path()
00206 << " into " << entry.get_into_node_path() << "\n";
00207 }
00208 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00209
00210 LPoint3 from_center = sphere->get_center() * wrt_mat;
00211
00212 LVector3 surface_normal;
00213 LVector3 v(into_intersection_point - into_center);
00214 PN_stdfloat vec_length = v.length();
00215 if (IS_NEARLY_ZERO(vec_length)) {
00216
00217
00218
00219 surface_normal.set(1.0, 0.0, 0.0);
00220 } else {
00221 surface_normal = v / vec_length;
00222 }
00223
00224 LVector3 eff_normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
00225
00226 LVector3 contact_normal;
00227 LVector3 v2 = contact_point - into_center;
00228 PN_stdfloat v2_len = v2.length();
00229 if (IS_NEARLY_ZERO(v2_len)) {
00230
00231
00232
00233 contact_normal.set(1.0, 0.0, 0.0);
00234 } else {
00235 contact_normal = v2 / v2_len;
00236 }
00237
00238 new_entry->set_surface_normal(eff_normal);
00239 new_entry->set_surface_point(into_center + surface_normal * into_radius);
00240 new_entry->set_interior_point(from_center - surface_normal * from_radius);
00241 new_entry->set_contact_pos(contact_point);
00242 new_entry->set_contact_normal(contact_normal);
00243 new_entry->set_t(actual_t);
00244
00245 return new_entry;
00246 }
00247
00248
00249
00250
00251
00252
00253 PT(CollisionEntry) CollisionSphere::
00254 test_intersection_from_line(const CollisionEntry &entry) const {
00255 const CollisionLine *line;
00256 DCAST_INTO_R(line, entry.get_from(), 0);
00257
00258 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00259
00260 LPoint3 from_origin = line->get_origin() * wrt_mat;
00261 LVector3 from_direction = line->get_direction() * wrt_mat;
00262
00263 double t1, t2;
00264 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00265
00266 return NULL;
00267 }
00268
00269 if (collide_cat.is_debug()) {
00270 collide_cat.debug()
00271 << "intersection detected from " << entry.get_from_node_path()
00272 << " into " << entry.get_into_node_path() << "\n";
00273 }
00274 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00275
00276 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
00277 new_entry->set_surface_point(into_intersection_point);
00278
00279 if (has_effective_normal() && line->get_respect_effective_normal()) {
00280 new_entry->set_surface_normal(get_effective_normal());
00281 } else {
00282 LVector3 normal = into_intersection_point - get_center();
00283 normal.normalize();
00284 new_entry->set_surface_normal(normal);
00285 }
00286
00287 return new_entry;
00288 }
00289
00290
00291
00292
00293
00294
00295 PT(CollisionEntry) CollisionSphere::
00296 test_intersection_from_box(const CollisionEntry &entry) const {
00297 const CollisionBox *box;
00298 DCAST_INTO_R(box, entry.get_from(), 0);
00299
00300 CPT(TransformState) wrt_space = entry.get_wrt_space();
00301 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00302
00303 const LMatrix4 &wrt_mat = wrt_space->get_mat();
00304
00305 CollisionBox local_b( *box );
00306 local_b.xform( wrt_mat );
00307
00308 LPoint3 from_center = local_b.get_center();
00309
00310 LPoint3 orig_center = get_center();
00311 LPoint3 to_center = orig_center;
00312 LPoint3 contact_point(from_center);
00313 PN_stdfloat actual_t = 1.0f;
00314
00315 PN_stdfloat to_radius = get_radius();
00316 PN_stdfloat to_radius_2 = to_radius * to_radius;
00317
00318 int ip;
00319 PN_stdfloat max_dist = 0.0f;
00320 PN_stdfloat dist = 0.0f;
00321 bool intersect;
00322 LPlane plane;
00323 LVector3 normal;
00324
00325 for (ip = 0, intersect=false; ip < 6 && !intersect; ip++) {
00326 plane = local_b.get_plane( ip );
00327 if (local_b.get_plane_points(ip).size() < 3) {
00328 continue;
00329 }
00330 normal = (has_effective_normal() && box->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
00331
00332 #ifndef NDEBUG
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342 #endif
00343
00344
00345
00346
00347 if (!plane.intersects_line(dist, to_center, -(plane.get_normal()))) {
00348
00349
00350 continue;
00351 }
00352
00353 if (dist > to_radius || dist < -to_radius) {
00354
00355 continue;
00356 }
00357
00358 LPoint2 p = local_b.to_2d(to_center - dist * plane.get_normal(), ip);
00359 PN_stdfloat edge_dist = 0.0f;
00360
00361 edge_dist = local_b.dist_to_polygon(p, local_b.get_plane_points(ip));
00362
00363 if(edge_dist < 0) {
00364 intersect = true;
00365 continue;
00366 }
00367
00368 if((edge_dist > 0) &&
00369 ((edge_dist * edge_dist + dist * dist) > to_radius_2)) {
00370
00371 continue;
00372 }
00373
00374
00375
00376
00377
00378
00379
00380 max_dist = to_radius;
00381 if (edge_dist >= 0.0f) {
00382 PN_stdfloat max_dist_2 = max(to_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
00383 max_dist = csqrt(max_dist_2);
00384 }
00385
00386 if (dist > max_dist) {
00387
00388 continue;
00389 }
00390 intersect = true;
00391 }
00392 if( !intersect )
00393 return NULL;
00394
00395 if (collide_cat.is_debug()) {
00396 collide_cat.debug()
00397 << "intersection detected from " << entry.get_from_node_path()
00398 << " into " << entry.get_into_node_path() << "\n";
00399 }
00400
00401 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00402
00403 PN_stdfloat into_depth = max_dist - dist;
00404
00405 new_entry->set_surface_normal(normal);
00406 new_entry->set_surface_point(to_center - normal * dist);
00407 new_entry->set_interior_point(to_center - normal * (dist + into_depth));
00408 new_entry->set_contact_pos(contact_point);
00409 new_entry->set_contact_normal(plane.get_normal());
00410 new_entry->set_t(actual_t);
00411
00412 return new_entry;
00413 }
00414
00415
00416
00417
00418
00419
00420 PT(CollisionEntry) CollisionSphere::
00421 test_intersection_from_ray(const CollisionEntry &entry) const {
00422 const CollisionRay *ray;
00423 DCAST_INTO_R(ray, entry.get_from(), 0);
00424
00425 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00426
00427 LPoint3 from_origin = ray->get_origin() * wrt_mat;
00428 LVector3 from_direction = ray->get_direction() * wrt_mat;
00429
00430 double t1, t2;
00431 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00432
00433 return NULL;
00434 }
00435
00436 if (t2 < 0.0) {
00437
00438 return NULL;
00439 }
00440
00441 t1 = max(t1, 0.0);
00442
00443 if (collide_cat.is_debug()) {
00444 collide_cat.debug()
00445 << "intersection detected from " << entry.get_from_node_path()
00446 << " into " << entry.get_into_node_path() << "\n";
00447 }
00448 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00449
00450 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
00451 new_entry->set_surface_point(into_intersection_point);
00452
00453 if (has_effective_normal() && ray->get_respect_effective_normal()) {
00454 new_entry->set_surface_normal(get_effective_normal());
00455 } else {
00456 LVector3 normal = into_intersection_point - get_center();
00457 normal.normalize();
00458 new_entry->set_surface_normal(normal);
00459 }
00460
00461 return new_entry;
00462 }
00463
00464
00465
00466
00467
00468
00469 PT(CollisionEntry) CollisionSphere::
00470 test_intersection_from_segment(const CollisionEntry &entry) const {
00471 const CollisionSegment *segment;
00472 DCAST_INTO_R(segment, entry.get_from(), 0);
00473
00474 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00475
00476 LPoint3 from_a = segment->get_point_a() * wrt_mat;
00477 LPoint3 from_b = segment->get_point_b() * wrt_mat;
00478 LVector3 from_direction = from_b - from_a;
00479
00480 double t1, t2;
00481 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
00482
00483 return NULL;
00484 }
00485
00486 if (t2 < 0.0 || t1 > 1.0) {
00487
00488
00489 return NULL;
00490 }
00491
00492 t1 = max(t1, 0.0);
00493
00494 if (collide_cat.is_debug()) {
00495 collide_cat.debug()
00496 << "intersection detected from " << entry.get_from_node_path()
00497 << " into " << entry.get_into_node_path() << "\n";
00498 }
00499 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00500
00501 LPoint3 into_intersection_point = from_a + t1 * from_direction;
00502 new_entry->set_surface_point(into_intersection_point);
00503
00504 if (has_effective_normal() && segment->get_respect_effective_normal()) {
00505 new_entry->set_surface_normal(get_effective_normal());
00506 } else {
00507 LVector3 normal = into_intersection_point - get_center();
00508 normal.normalize();
00509 new_entry->set_surface_normal(normal);
00510 }
00511
00512 return new_entry;
00513 }
00514
00515
00516
00517
00518
00519
00520 PT(CollisionEntry) CollisionSphere::
00521 test_intersection_from_parabola(const CollisionEntry &entry) const {
00522 const CollisionParabola *parabola;
00523 DCAST_INTO_R(parabola, entry.get_from(), 0);
00524
00525 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00526
00527
00528 LParabola local_p(parabola->get_parabola());
00529 local_p.xform(wrt_mat);
00530
00531 double t;
00532 if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
00533 local_p.calc_point(parabola->get_t1()),
00534 local_p.calc_point(parabola->get_t2()))) {
00535
00536 return NULL;
00537 }
00538
00539 if (collide_cat.is_debug()) {
00540 collide_cat.debug()
00541 << "intersection detected from " << entry.get_from_node_path()
00542 << " into " << entry.get_into_node_path() << "\n";
00543 }
00544 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00545
00546 LPoint3 into_intersection_point = local_p.calc_point(t);
00547 new_entry->set_surface_point(into_intersection_point);
00548
00549 if (has_effective_normal() && parabola->get_respect_effective_normal()) {
00550 new_entry->set_surface_normal(get_effective_normal());
00551 } else {
00552 LVector3 normal = into_intersection_point - get_center();
00553 normal.normalize();
00554 new_entry->set_surface_normal(normal);
00555 }
00556
00557 return new_entry;
00558 }
00559
00560
00561
00562
00563
00564
00565
00566 void CollisionSphere::
00567 fill_viz_geom() {
00568 if (collide_cat.is_debug()) {
00569 collide_cat.debug()
00570 << "Recomputing viz for " << *this << "\n";
00571 }
00572
00573 static const int num_slices = 16;
00574 static const int num_stacks = 8;
00575
00576 PT(GeomVertexData) vdata = new GeomVertexData
00577 ("collision", GeomVertexFormat::get_v3(),
00578 Geom::UH_static);
00579 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00580
00581 PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
00582 for (int sl = 0; sl < num_slices; ++sl) {
00583 PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
00584 PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
00585 vertex.add_data3(compute_point(0.0, longitude0));
00586 for (int st = 1; st < num_stacks; ++st) {
00587 PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
00588 vertex.add_data3(compute_point(latitude, longitude0));
00589 vertex.add_data3(compute_point(latitude, longitude1));
00590 }
00591 vertex.add_data3(compute_point(1.0, longitude0));
00592
00593 strip->add_next_vertices(num_stacks * 2);
00594 strip->close_primitive();
00595 }
00596
00597 PT(Geom) geom = new Geom(vdata);
00598 geom->add_primitive(strip);
00599
00600 _viz_geom->add_geom(geom, get_solid_viz_state());
00601 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
00602 }
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617 bool CollisionSphere::
00618 intersects_line(double &t1, double &t2,
00619 const LPoint3 &from, const LVector3 &delta,
00620 PN_stdfloat inflate_radius) const {
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651 double A = dot(delta, delta);
00652
00653 nassertr(A != 0.0, false);
00654
00655 LVector3 fc = from - get_center();
00656 double B = 2.0f* dot(delta, fc);
00657 double fc_d2 = dot(fc, fc);
00658 double radius = get_radius() + inflate_radius;
00659 double C = fc_d2 - radius * radius;
00660
00661 double radical = B*B - 4.0*A*C;
00662
00663 if (IS_NEARLY_ZERO(radical)) {
00664
00665 t1 = t2 = -B /(2.0*A);
00666 return true;
00667
00668 } else if (radical < 0.0) {
00669
00670 return false;
00671 }
00672
00673 double reciprocal_2A = 1.0/(2.0*A);
00674 double sqrt_radical = sqrtf(radical);
00675 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00676 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00677
00678 return true;
00679 }
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694 bool CollisionSphere::
00695 intersects_parabola(double &t, const LParabola ¶bola,
00696 double t1, double t2,
00697 const LPoint3 &p1, const LPoint3 &p2) const {
00698 if (t1 == t2) {
00699
00700 if ((p1 - _center).length_squared() > _radius * _radius) {
00701
00702 return false;
00703 }
00704 t = t1;
00705 return true;
00706 }
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720 double tmid = (t1 + t2) * 0.5;
00721 if (tmid != t1 && tmid != t2) {
00722 LPoint3 pmid = parabola.calc_point(tmid);
00723 LPoint3 pmid2 = (p1 + p2) * 0.5f;
00724
00725 if ((pmid - pmid2).length_squared() > 0.001f) {
00726
00727 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
00728 return true;
00729 }
00730 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
00731 }
00732 }
00733
00734
00735 double t1a, t2a;
00736 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
00737 return false;
00738 }
00739
00740 if (t2a < 0.0 || t1a > 1.0) {
00741 return false;
00742 }
00743
00744 t = max(t1a, 0.0);
00745 return true;
00746 }
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756 LVertex CollisionSphere::
00757 compute_point(PN_stdfloat latitude, PN_stdfloat longitude) const {
00758 PN_stdfloat s1, c1;
00759 csincos(latitude * MathNumbers::pi, &s1, &c1);
00760
00761 PN_stdfloat s2, c2;
00762 csincos(longitude * 2.0f * MathNumbers::pi, &s2, &c2);
00763
00764 LVertex p(s1 * c2, s1 * s2, c1);
00765 return p * get_radius() + get_center();
00766 }
00767
00768
00769
00770
00771
00772
00773 void CollisionSphere::
00774 register_with_read_factory() {
00775 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionSphere);
00776 }
00777
00778
00779
00780
00781
00782
00783
00784 void CollisionSphere::
00785 write_datagram(BamWriter *manager, Datagram &me) {
00786 CollisionSolid::write_datagram(manager, me);
00787 _center.write_datagram(me);
00788 me.add_stdfloat(_radius);
00789 }
00790
00791
00792
00793
00794
00795
00796 TypedWritable *CollisionSphere::
00797 make_CollisionSphere(const FactoryParams ¶ms) {
00798 CollisionSphere *me = new CollisionSphere;
00799 DatagramIterator scan;
00800 BamReader *manager;
00801
00802 parse_params(params, scan, manager);
00803 me->fillin(scan, manager);
00804 return me;
00805 }
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815 void CollisionSphere::
00816 fillin(DatagramIterator& scan, BamReader* manager) {
00817 CollisionSolid::fillin(scan, manager);
00818 _center.read_datagram(scan);
00819 _radius = scan.get_stdfloat();
00820 }