00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "collisionTube.h"
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 "look_at.h"
00024 #include "geom.h"
00025 #include "geomNode.h"
00026 #include "geometricBoundingVolume.h"
00027 #include "datagram.h"
00028 #include "datagramIterator.h"
00029 #include "bamReader.h"
00030 #include "bamWriter.h"
00031 #include "cmath.h"
00032 #include "transformState.h"
00033 #include "geom.h"
00034 #include "geomTristrips.h"
00035 #include "geomVertexWriter.h"
00036 #include "boundingSphere.h"
00037
00038 PStatCollector CollisionTube::_volume_pcollector("Collision Volumes:CollisionTube");
00039 PStatCollector CollisionTube::_test_pcollector("Collision Tests:CollisionTube");
00040 TypeHandle CollisionTube::_type_handle;
00041
00042
00043
00044
00045
00046
00047 CollisionSolid *CollisionTube::
00048 make_copy() {
00049 return new CollisionTube(*this);
00050 }
00051
00052
00053
00054
00055
00056
00057 void CollisionTube::
00058 xform(const LMatrix4 &mat) {
00059 _a = _a * mat;
00060 _b = _b * mat;
00061
00062
00063
00064 LVector3 radius_v = LVector3(_radius, 0.0f, 0.0f) * mat;
00065 _radius = length(radius_v);
00066
00067 recalc_internals();
00068 CollisionSolid::xform(mat);
00069 }
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079 LPoint3 CollisionTube::
00080 get_collision_origin() const {
00081 return get_point_a();
00082 }
00083
00084
00085
00086
00087
00088
00089
00090
00091 PStatCollector &CollisionTube::
00092 get_volume_pcollector() {
00093 return _volume_pcollector;
00094 }
00095
00096
00097
00098
00099
00100
00101
00102
00103 PStatCollector &CollisionTube::
00104 get_test_pcollector() {
00105 return _test_pcollector;
00106 }
00107
00108
00109
00110
00111
00112
00113 void CollisionTube::
00114 output(ostream &out) const {
00115 out << "tube, a (" << _a << "), b (" << _b << "), r " << _radius;
00116 }
00117
00118
00119
00120
00121
00122
00123 PT(BoundingVolume) CollisionTube::
00124 compute_internal_bounds() const {
00125 PT(BoundingVolume) bound = CollisionSolid::compute_internal_bounds();
00126
00127 if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) {
00128 GeometricBoundingVolume *gbound;
00129 DCAST_INTO_R(gbound, bound, bound);
00130
00131 LVector3 vec = (_b - _a);
00132 if (vec.normalize()) {
00133
00134
00135 LPoint3 points[2];
00136 points[0] = _a - vec * _radius;
00137 points[1] = _b + vec * _radius;
00138
00139 gbound->around(points, points + 2);
00140
00141 } else {
00142
00143
00144 BoundingSphere sphere(_a, _radius);
00145 gbound->extend_by(&sphere);
00146 }
00147 }
00148
00149 return bound;
00150 }
00151
00152
00153
00154
00155
00156
00157 PT(CollisionEntry) CollisionTube::
00158 test_intersection_from_sphere(const CollisionEntry &entry) const {
00159 const CollisionSphere *sphere;
00160 DCAST_INTO_R(sphere, entry.get_from(), 0);
00161
00162 CPT(TransformState) wrt_space = entry.get_wrt_space();
00163 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00164
00165 const LMatrix4 &wrt_mat = wrt_space->get_mat();
00166
00167 LPoint3 from_a = sphere->get_center() * wrt_mat;
00168 LPoint3 from_b = from_a;
00169
00170 LPoint3 contact_point;
00171 PN_stdfloat actual_t = 0.0f;
00172
00173 if (wrt_prev_space != wrt_space) {
00174
00175
00176 from_a = sphere->get_center() * wrt_prev_space->get_mat();
00177 }
00178
00179 LVector3 from_direction = from_b - from_a;
00180
00181 LVector3 from_radius_v =
00182 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00183 PN_stdfloat from_radius = length(from_radius_v);
00184
00185 double t1, t2;
00186 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
00187
00188 return NULL;
00189 }
00190
00191 if (t2 < 0.0 || t1 > 1.0) {
00192
00193
00194 return NULL;
00195 }
00196
00197
00198 actual_t = min(1.0, max(0.0, t1));
00199 contact_point = from_a + actual_t * (from_b - from_a);
00200
00201 if (collide_cat.is_debug()) {
00202 collide_cat.debug()
00203 << "intersection detected from " << entry.get_from_node_path() << " into "
00204 << entry.get_into_node_path() << "\n";
00205 }
00206 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00207
00208 LPoint3 into_intersection_point;
00209 if (t2 > 1.0) {
00210
00211
00212 into_intersection_point = from_b;
00213 } else {
00214
00215
00216 into_intersection_point = from_a + t2 * from_direction;
00217 }
00218 set_intersection_point(new_entry, into_intersection_point, from_radius);
00219
00220 LPoint3 fake_contact_point;
00221 LVector3 contact_normal;
00222 calculate_surface_point_and_normal(contact_point,
00223 from_radius,
00224 fake_contact_point,
00225 contact_normal);
00226 new_entry->set_contact_pos(contact_point);
00227 new_entry->set_contact_normal(contact_normal);
00228 new_entry->set_t(actual_t);
00229
00230 return new_entry;
00231 }
00232
00233
00234
00235
00236
00237
00238 PT(CollisionEntry) CollisionTube::
00239 test_intersection_from_line(const CollisionEntry &entry) const {
00240 const CollisionLine *line;
00241 DCAST_INTO_R(line, entry.get_from(), 0);
00242
00243 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00244
00245 LPoint3 from_origin = line->get_origin() * wrt_mat;
00246 LVector3 from_direction = line->get_direction() * wrt_mat;
00247
00248 double t1, t2;
00249 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00250
00251 return NULL;
00252 }
00253
00254 if (collide_cat.is_debug()) {
00255 collide_cat.debug()
00256 << "intersection detected from " << entry.get_from_node_path()
00257 << " into " << entry.get_into_node_path() << "\n";
00258 }
00259 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00260
00261 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
00262 set_intersection_point(new_entry, into_intersection_point, 0.0);
00263
00264 if (has_effective_normal() && line->get_respect_effective_normal()) {
00265 new_entry->set_surface_normal(get_effective_normal());
00266
00267 } else {
00268 LVector3 normal = into_intersection_point * _inv_mat;
00269 if (normal[1] > _length) {
00270
00271 normal[1] -= _length;
00272 } else if (normal[1] > 0.0f) {
00273
00274 normal[1] = 0;
00275 }
00276 normal = normalize(normal * _mat);
00277 new_entry->set_surface_normal(normal);
00278 }
00279
00280 return new_entry;
00281 }
00282
00283
00284
00285
00286
00287
00288 PT(CollisionEntry) CollisionTube::
00289 test_intersection_from_ray(const CollisionEntry &entry) const {
00290 const CollisionRay *ray;
00291 DCAST_INTO_R(ray, entry.get_from(), 0);
00292
00293 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00294
00295 LPoint3 from_origin = ray->get_origin() * wrt_mat;
00296 LVector3 from_direction = ray->get_direction() * wrt_mat;
00297
00298 double t1, t2;
00299 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00300
00301 return NULL;
00302 }
00303
00304 if (t2 < 0.0) {
00305
00306 return NULL;
00307 }
00308
00309 if (collide_cat.is_debug()) {
00310 collide_cat.debug()
00311 << "intersection detected from " << entry.get_from_node_path()
00312 << " into " << entry.get_into_node_path() << "\n";
00313 }
00314 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00315
00316 LPoint3 into_intersection_point;
00317 if (t1 < 0.0) {
00318
00319
00320 into_intersection_point = from_origin;
00321 } else {
00322
00323
00324 into_intersection_point = from_origin + t1 * from_direction;
00325 }
00326 set_intersection_point(new_entry, into_intersection_point, 0.0);
00327
00328 if (has_effective_normal() && ray->get_respect_effective_normal()) {
00329 new_entry->set_surface_normal(get_effective_normal());
00330
00331 } else {
00332 LVector3 normal = into_intersection_point * _inv_mat;
00333 if (normal[1] > _length) {
00334
00335 normal[1] -= _length;
00336 } else if (normal[1] > 0.0f) {
00337
00338 normal[1] = 0;
00339 }
00340 normal = normalize(normal * _mat);
00341 new_entry->set_surface_normal(normal);
00342 }
00343
00344 return new_entry;
00345 }
00346
00347
00348
00349
00350
00351
00352 PT(CollisionEntry) CollisionTube::
00353 test_intersection_from_segment(const CollisionEntry &entry) const {
00354 const CollisionSegment *segment;
00355 DCAST_INTO_R(segment, entry.get_from(), 0);
00356
00357 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00358
00359 LPoint3 from_a = segment->get_point_a() * wrt_mat;
00360 LPoint3 from_b = segment->get_point_b() * wrt_mat;
00361 LVector3 from_direction = from_b - from_a;
00362
00363 double t1, t2;
00364 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
00365
00366 return NULL;
00367 }
00368
00369 if (t2 < 0.0 || t1 > 1.0) {
00370
00371
00372 return NULL;
00373 }
00374
00375 if (collide_cat.is_debug()) {
00376 collide_cat.debug()
00377 << "intersection detected from " << entry.get_from_node_path()
00378 << " into " << entry.get_into_node_path() << "\n";
00379 }
00380 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00381
00382 LPoint3 into_intersection_point;
00383 if (t1 < 0.0) {
00384
00385
00386 into_intersection_point = from_a;
00387 } else {
00388
00389
00390 into_intersection_point = from_a + t1 * from_direction;
00391 }
00392 set_intersection_point(new_entry, into_intersection_point, 0.0);
00393
00394 if (has_effective_normal() && segment->get_respect_effective_normal()) {
00395 new_entry->set_surface_normal(get_effective_normal());
00396
00397 } else {
00398 LVector3 normal = into_intersection_point * _inv_mat;
00399 if (normal[1] > _length) {
00400
00401 normal[1] -= _length;
00402 } else if (normal[1] > 0.0f) {
00403
00404 normal[1] = 0;
00405 }
00406 normal = normalize(normal * _mat);
00407 new_entry->set_surface_normal(normal);
00408 }
00409
00410 return new_entry;
00411 }
00412
00413
00414
00415
00416
00417
00418 PT(CollisionEntry) CollisionTube::
00419 test_intersection_from_parabola(const CollisionEntry &entry) const {
00420 const CollisionParabola *parabola;
00421 DCAST_INTO_R(parabola, entry.get_from(), 0);
00422
00423 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00424
00425
00426 LParabola local_p(parabola->get_parabola());
00427 local_p.xform(wrt_mat);
00428
00429 double t;
00430 if (!intersects_parabola(t, local_p, parabola->get_t1(), parabola->get_t2(),
00431 local_p.calc_point(parabola->get_t1()),
00432 local_p.calc_point(parabola->get_t2()))) {
00433
00434 return NULL;
00435 }
00436
00437 if (collide_cat.is_debug()) {
00438 collide_cat.debug()
00439 << "intersection detected from " << entry.get_from_node_path()
00440 << " into " << entry.get_into_node_path() << "\n";
00441 }
00442 PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00443
00444 LPoint3 into_intersection_point = local_p.calc_point(t);
00445 set_intersection_point(new_entry, into_intersection_point, 0.0);
00446
00447 if (has_effective_normal() && parabola->get_respect_effective_normal()) {
00448 new_entry->set_surface_normal(get_effective_normal());
00449
00450 } else {
00451 LVector3 normal = into_intersection_point * _inv_mat;
00452 if (normal[1] > _length) {
00453
00454 normal[1] -= _length;
00455 } else if (normal[1] > 0.0f) {
00456
00457 normal[1] = 0;
00458 }
00459 normal = normalize(normal * _mat);
00460 new_entry->set_surface_normal(normal);
00461 }
00462
00463 return new_entry;
00464 }
00465
00466
00467
00468
00469
00470
00471
00472 void CollisionTube::
00473 fill_viz_geom() {
00474 if (collide_cat.is_debug()) {
00475 collide_cat.debug()
00476 << "Recomputing viz for " << *this << "\n";
00477 }
00478
00479
00480
00481
00482 LVector3 direction = (_b - _a);
00483 PN_stdfloat length = direction.length();
00484
00485 PT(GeomVertexData) vdata = new GeomVertexData
00486 ("collision", GeomVertexFormat::get_v3(),
00487 Geom::UH_static);
00488 GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00489
00490 PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
00491
00492 static const int num_slices = 8;
00493 static const int num_rings = 4;
00494 int ri, si;
00495 for (ri = 0; ri < num_rings; ri++) {
00496 for (si = 0; si <= num_slices; si++) {
00497 vertex.add_data3(calc_sphere1_vertex(ri, si, num_rings, num_slices));
00498 vertex.add_data3(calc_sphere1_vertex(ri + 1, si, num_rings, num_slices));
00499 }
00500 strip->add_next_vertices((num_slices + 1) * 2);
00501 strip->close_primitive();
00502 }
00503
00504
00505 for (si = 0; si <= num_slices; si++) {
00506 vertex.add_data3(calc_sphere1_vertex(num_rings, si, num_rings, num_slices));
00507 vertex.add_data3(calc_sphere2_vertex(num_rings, si, num_rings, num_slices,
00508 length));
00509 }
00510 strip->add_next_vertices((num_slices + 1) * 2);
00511 strip->close_primitive();
00512
00513
00514 for (ri = num_rings - 1; ri >= 0; ri--) {
00515 for (si = 0; si <= num_slices; si++) {
00516 vertex.add_data3(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length));
00517 vertex.add_data3(calc_sphere2_vertex(ri, si, num_rings, num_slices, length));
00518 }
00519 strip->add_next_vertices((num_slices + 1) * 2);
00520 strip->close_primitive();
00521 }
00522
00523 PT(Geom) geom = new Geom(vdata);
00524 geom->add_primitive(strip);
00525
00526
00527 LMatrix4 mat;
00528 look_at(mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
00529 mat.set_row(3, _a);
00530 geom->transform_vertices(mat);
00531
00532 _viz_geom->add_geom(geom, get_solid_viz_state());
00533 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
00534 }
00535
00536
00537
00538
00539
00540
00541
00542
00543 void CollisionTube::
00544 recalc_internals() {
00545 LVector3 direction = (_b - _a);
00546 _length = direction.length();
00547
00548 look_at(_mat, direction, LVector3(0.0f, 0.0f, 1.0f), CS_zup_right);
00549 _mat.set_row(3, _a);
00550 _inv_mat.invert_from(_mat);
00551
00552 mark_viz_stale();
00553 mark_internal_bounds_stale();
00554 }
00555
00556
00557
00558
00559
00560
00561
00562
00563 LVertex CollisionTube::
00564 calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) {
00565 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
00566 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
00567
00568
00569 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
00570 PN_stdfloat x_rim = ccos(theta);
00571 PN_stdfloat z_rim = csin(theta);
00572
00573
00574 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
00575 PN_stdfloat to_pole = csin(phi);
00576
00577 PN_stdfloat x = _radius * x_rim * to_pole;
00578 PN_stdfloat y = -_radius * ccos(phi);
00579 PN_stdfloat z = _radius * z_rim * to_pole;
00580
00581 return LVertex(x, y, z);
00582 }
00583
00584
00585
00586
00587
00588
00589
00590
00591 LVertex CollisionTube::
00592 calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices,
00593 PN_stdfloat length) {
00594 PN_stdfloat r = (PN_stdfloat)ri / (PN_stdfloat)num_rings;
00595 PN_stdfloat s = (PN_stdfloat)si / (PN_stdfloat)num_slices;
00596
00597
00598 PN_stdfloat theta = s * 2.0f * MathNumbers::pi;
00599 PN_stdfloat x_rim = ccos(theta);
00600 PN_stdfloat z_rim = csin(theta);
00601
00602
00603 PN_stdfloat phi = r * 0.5f * MathNumbers::pi;
00604 PN_stdfloat to_pole = csin(phi);
00605
00606 PN_stdfloat x = _radius * x_rim * to_pole;
00607 PN_stdfloat y = length + _radius * ccos(phi);
00608 PN_stdfloat z = _radius * z_rim * to_pole;
00609
00610 return LVertex(x, y, z);
00611 }
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626 bool CollisionTube::
00627 intersects_line(double &t1, double &t2,
00628 const LPoint3 &from0, const LVector3 &delta0,
00629 PN_stdfloat inflate_radius) const {
00630
00631
00632 LPoint3 from = from0 * _inv_mat;
00633 LVector3 delta = delta0 * _inv_mat;
00634
00635 PN_stdfloat radius = _radius + inflate_radius;
00636
00637
00638
00639
00640
00641
00642
00643 LVector2 from2(from[0], from[2]);
00644 LVector2 delta2(delta[0], delta[2]);
00645
00646 double A = dot(delta2, delta2);
00647
00648 if (IS_NEARLY_ZERO(A)) {
00649
00650
00651
00652 if (from2.dot(from2) > radius * radius) {
00653
00654
00655 return false;
00656 }
00657
00658 if (IS_NEARLY_ZERO(delta[1])) {
00659
00660
00661
00662
00663 if (from[1] < -radius || from[1] > _length + radius) {
00664
00665 return false;
00666 }
00667 if (from[1] < 0.0f) {
00668
00669 if (from.dot(from) > radius * radius) {
00670 return false;
00671 }
00672 } else if (from[1] > _length) {
00673
00674 from[1] -= _length;
00675 if (from.dot(from) > radius * radius) {
00676 return false;
00677 }
00678 }
00679
00680
00681 t1 = t2 = 0.0;
00682 return true;
00683 }
00684
00685
00686
00687 t1 = (-radius - from[1]) / delta[1];
00688 t2 = (_length + radius - from[1]) / delta[1];
00689
00690 } else {
00691
00692
00693
00694
00695 double B = 2.0f * dot(delta2, from2);
00696 double fc_d2 = dot(from2, from2);
00697 double C = fc_d2 - radius * radius;
00698
00699 double radical = B*B - 4.0*A*C;
00700
00701 if (IS_NEARLY_ZERO(radical)) {
00702
00703 t1 = t2 = -B / (2.0*A);
00704
00705 } else if (radical < 0.0) {
00706
00707 return false;
00708
00709 } else {
00710 double reciprocal_2A = 1.0 / (2.0 * A);
00711 double sqrt_radical = sqrtf(radical);
00712 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00713 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00714 }
00715 }
00716
00717
00718
00719 PN_stdfloat t1_y = from[1] + t1 * delta[1];
00720 PN_stdfloat t2_y = from[1] + t2 * delta[1];
00721
00722 if (t1_y < -radius && t2_y < -radius) {
00723
00724
00725 return false;
00726 } else if (t1_y > _length + radius && t2_y > _length + radius) {
00727
00728 return false;
00729 }
00730
00731 if (t1_y < 0.0f) {
00732
00733
00734 double t1a, t2a;
00735 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
00736
00737
00738 return false;
00739 }
00740 t1 = t1a;
00741
00742 } else if (t1_y > _length) {
00743
00744
00745 double t1b, t2b;
00746 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
00747
00748
00749 return false;
00750 }
00751 t1 = t1b;
00752 }
00753
00754 if (t2_y < 0.0f) {
00755
00756
00757 double t1a, t2a;
00758 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) {
00759
00760
00761 return false;
00762 }
00763 t2 = t2a;
00764
00765 } else if (t2_y > _length) {
00766
00767
00768 double t1b, t2b;
00769 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) {
00770
00771
00772 return false;
00773 }
00774 t2 = t2b;
00775 }
00776
00777 return true;
00778 }
00779
00780
00781
00782
00783
00784
00785
00786
00787
00788 bool CollisionTube::
00789 sphere_intersects_line(double &t1, double &t2, PN_stdfloat center_y,
00790 const LPoint3 &from, const LVector3 &delta,
00791 PN_stdfloat inflate_radius) const {
00792
00793
00794 PN_stdfloat radius = _radius + inflate_radius;
00795
00796 double A = dot(delta, delta);
00797
00798 nassertr(A != 0.0, false);
00799
00800 LVector3 fc = from;
00801 fc[1] -= center_y;
00802 double B = 2.0f* dot(delta, fc);
00803 double fc_d2 = dot(fc, fc);
00804 double C = fc_d2 - radius * radius;
00805
00806 double radical = B*B - 4.0*A*C;
00807
00808 if (IS_NEARLY_ZERO(radical)) {
00809
00810 t1 = t2 = -B / (2.0 * A);
00811 return true;
00812
00813 } else if (radical < 0.0) {
00814
00815 return false;
00816 }
00817
00818 double reciprocal_2A = 1.0 / (2.0 * A);
00819 double sqrt_radical = sqrtf(radical);
00820 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00821 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00822
00823 return true;
00824 }
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839 bool CollisionTube::
00840 intersects_parabola(double &t, const LParabola ¶bola,
00841 double t1, double t2,
00842 const LPoint3 &p1, const LPoint3 &p2) const {
00843
00844
00845
00846
00847
00848
00849
00850
00851
00852 double tmid = (t1 + t2) * 0.5;
00853
00854 if (tmid != t1 && tmid != t2) {
00855 LPoint3 pmid = parabola.calc_point(tmid);
00856 LPoint3 pmid2 = (p1 + p2) * 0.5f;
00857
00858 if ((pmid - pmid2).length_squared() > 0.001f) {
00859
00860 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
00861 return true;
00862 }
00863 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
00864 }
00865 }
00866
00867
00868 double t1a, t2a;
00869 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
00870 return false;
00871 }
00872
00873 if (t2a < 0.0 || t1a > 1.0) {
00874 return false;
00875 }
00876
00877 t = max(t1a, 0.0);
00878 return true;
00879 }
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889 void CollisionTube::
00890 calculate_surface_point_and_normal(const LPoint3 &surface_point,
00891 double extra_radius,
00892 LPoint3 &result_point,
00893 LVector3 &result_normal) const {
00894
00895 LPoint3 point = surface_point * _inv_mat;
00896 LVector3 normal;
00897
00898 if (point[1] <= 0.0) {
00899
00900 normal = point;
00901 if (!normal.normalize()) {
00902 normal.set(0.0, -1.0, 0.0);
00903 }
00904 point = normal * _radius;
00905
00906 } else if (point[1] >= _length) {
00907
00908 normal.set(point[0], point[1] - _length, point[2]);
00909 if (!normal.normalize()) {
00910 normal.set(0.0, 1.0, 0.0);
00911 }
00912 point = normal * _radius;
00913 point[1] += _length;
00914
00915 } else {
00916
00917 LVector2d normal2d(point[0], point[2]);
00918 if (!normal2d.normalize()) {
00919 normal2d.set(0.0, 1.0);
00920 }
00921 normal.set(normal2d[0], 0.0, normal2d[1]);
00922 point.set(normal[0] * _radius, point[1], normal[2] * _radius);
00923 }
00924
00925
00926 result_point = point * _mat;
00927 result_normal = normal * _mat;
00928 }
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938 void CollisionTube::
00939 set_intersection_point(CollisionEntry *new_entry,
00940 const LPoint3 &into_intersection_point,
00941 double extra_radius) const {
00942 LPoint3 point;
00943 LVector3 normal;
00944
00945 calculate_surface_point_and_normal(into_intersection_point,
00946 extra_radius,
00947 point,
00948 normal);
00949
00950 if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) {
00951 normal = get_effective_normal();
00952 }
00953
00954 new_entry->set_surface_normal(normal);
00955 new_entry->set_surface_point(point);
00956
00957
00958
00959 new_entry->set_interior_point(into_intersection_point - normal * extra_radius);
00960 }
00961
00962
00963
00964
00965
00966
00967
00968 void CollisionTube::
00969 register_with_read_factory() {
00970 BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
00971 }
00972
00973
00974
00975
00976
00977
00978
00979 void CollisionTube::
00980 write_datagram(BamWriter *manager, Datagram &dg) {
00981 CollisionSolid::write_datagram(manager, dg);
00982 _a.write_datagram(dg);
00983 _b.write_datagram(dg);
00984 dg.add_stdfloat(_radius);
00985 }
00986
00987
00988
00989
00990
00991
00992
00993
00994
00995 TypedWritable *CollisionTube::
00996 make_from_bam(const FactoryParams ¶ms) {
00997 CollisionTube *node = new CollisionTube();
00998 DatagramIterator scan;
00999 BamReader *manager;
01000
01001 parse_params(params, scan, manager);
01002 node->fillin(scan, manager);
01003
01004 return node;
01005 }
01006
01007
01008
01009
01010
01011
01012
01013
01014 void CollisionTube::
01015 fillin(DatagramIterator &scan, BamReader *manager) {
01016 CollisionSolid::fillin(scan, manager);
01017 _a.read_datagram(scan);
01018 _b.read_datagram(scan);
01019 _radius = scan.get_stdfloat();
01020 recalc_internals();
01021 }