Panda3D
|
00001 // Filename: collisionTube.cxx 00002 // Created by: drose (25Sep03) 00003 // 00004 //////////////////////////////////////////////////////////////////// 00005 // 00006 // PANDA 3D SOFTWARE 00007 // Copyright (c) Carnegie Mellon University. All rights reserved. 00008 // 00009 // All use of this software is subject to the terms of the revised BSD 00010 // license. You should have received a copy of this license along 00011 // with this source code in a file named "LICENSE." 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 // Function: CollisionTube::make_copy 00044 // Access: Public, Virtual 00045 // Description: 00046 //////////////////////////////////////////////////////////////////// 00047 CollisionSolid *CollisionTube:: 00048 make_copy() { 00049 return new CollisionTube(*this); 00050 } 00051 00052 //////////////////////////////////////////////////////////////////// 00053 // Function: CollisionTube::xform 00054 // Access: Public, Virtual 00055 // Description: Transforms the solid by the indicated matrix. 00056 //////////////////////////////////////////////////////////////////// 00057 void CollisionTube:: 00058 xform(const LMatrix4 &mat) { 00059 _a = _a * mat; 00060 _b = _b * mat; 00061 00062 // This is a little cheesy and fails miserably in the presence of a 00063 // non-uniform scale. 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 // Function: CollisionTube::get_collision_origin 00073 // Access: Public, Virtual 00074 // Description: Returns the point in space deemed to be the "origin" 00075 // of the solid for collision purposes. The closest 00076 // intersection point to this origin point is considered 00077 // to be the most significant. 00078 //////////////////////////////////////////////////////////////////// 00079 LPoint3 CollisionTube:: 00080 get_collision_origin() const { 00081 return get_point_a(); 00082 } 00083 00084 //////////////////////////////////////////////////////////////////// 00085 // Function: CollisionTube::get_volume_pcollector 00086 // Access: Public, Virtual 00087 // Description: Returns a PStatCollector that is used to count the 00088 // number of bounding volume tests made against a solid 00089 // of this type in a given frame. 00090 //////////////////////////////////////////////////////////////////// 00091 PStatCollector &CollisionTube:: 00092 get_volume_pcollector() { 00093 return _volume_pcollector; 00094 } 00095 00096 //////////////////////////////////////////////////////////////////// 00097 // Function: CollisionTube::get_test_pcollector 00098 // Access: Public, Virtual 00099 // Description: Returns a PStatCollector that is used to count the 00100 // number of intersection tests made against a solid 00101 // of this type in a given frame. 00102 //////////////////////////////////////////////////////////////////// 00103 PStatCollector &CollisionTube:: 00104 get_test_pcollector() { 00105 return _test_pcollector; 00106 } 00107 00108 //////////////////////////////////////////////////////////////////// 00109 // Function: CollisionTube::output 00110 // Access: Public, Virtual 00111 // Description: 00112 //////////////////////////////////////////////////////////////////// 00113 void CollisionTube:: 00114 output(ostream &out) const { 00115 out << "tube, a (" << _a << "), b (" << _b << "), r " << _radius; 00116 } 00117 00118 //////////////////////////////////////////////////////////////////// 00119 // Function: CollisionTube::compute_internal_bounds 00120 // Access: Protected, Virtual 00121 // Description: 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 // The bounding volume includes both endpoints, plus a little 00134 // bit more to include the radius in both directions. 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 // Both endpoints are coincident; therefore, the bounding volume 00143 // is a sphere. 00144 BoundingSphere sphere(_a, _radius); 00145 gbound->extend_by(&sphere); 00146 } 00147 } 00148 00149 return bound; 00150 } 00151 00152 //////////////////////////////////////////////////////////////////// 00153 // Function: CollisionTube::test_intersection_from_sphere 00154 // Access: Public, Virtual 00155 // Description: 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 // If the sphere is moving relative to the tube, it becomes a tube 00175 // itself. 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 // No intersection. 00188 return NULL; 00189 } 00190 00191 if (t2 < 0.0 || t1 > 1.0) { 00192 // Both intersection points are before the start of the segment or 00193 // after the end of the segment. 00194 return NULL; 00195 } 00196 00197 // doubles, not floats, to satisfy min and max templates. 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 // Point b is within the tube. The first intersection point is 00211 // point b itself. 00212 into_intersection_point = from_b; 00213 } else { 00214 // Point b is outside the tube, and point a is either inside the 00215 // tube or beyond it. The first intersection point is at t2. 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 // Function: CollisionTube::test_intersection_from_line 00235 // Access: Public, Virtual 00236 // Description: 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 // No intersection. 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 // The point is within the top endcap. 00271 normal[1] -= _length; 00272 } else if (normal[1] > 0.0f) { 00273 // The point is within the cylinder body. 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 // Function: CollisionTube::test_intersection_from_ray 00285 // Access: Public, Virtual 00286 // Description: 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 // No intersection. 00301 return NULL; 00302 } 00303 00304 if (t2 < 0.0) { 00305 // Both intersection points are before the start of the ray. 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 // Point a is within the tube. The first intersection point is 00319 // point a itself. 00320 into_intersection_point = from_origin; 00321 } else { 00322 // Point a is outside the tube. The first intersection point is 00323 // at t1. 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 // The point is within the top endcap. 00335 normal[1] -= _length; 00336 } else if (normal[1] > 0.0f) { 00337 // The point is within the cylinder body. 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 // Function: CollisionTube::test_intersection_from_segment 00349 // Access: Public, Virtual 00350 // Description: 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 // No intersection. 00366 return NULL; 00367 } 00368 00369 if (t2 < 0.0 || t1 > 1.0) { 00370 // Both intersection points are before the start of the segment or 00371 // after the end of the segment. 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 // Point a is within the tube. The first intersection point is 00385 // point a itself. 00386 into_intersection_point = from_a; 00387 } else { 00388 // Point a is outside the tube, and point b is either inside the 00389 // tube or beyond it. The first intersection point is at t1. 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 // The point is within the top endcap. 00401 normal[1] -= _length; 00402 } else if (normal[1] > 0.0f) { 00403 // The point is within the cylinder body. 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 // Function: CollisionTube::test_intersection_from_parabola 00415 // Access: Public, Virtual 00416 // Description: 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 // Convert the parabola into local coordinate space. 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 // No intersection. 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 // The point is within the top endcap. 00454 normal[1] -= _length; 00455 } else if (normal[1] > 0.0f) { 00456 // The point is within the cylinder body. 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 // Function: CollisionTube::fill_viz_geom 00468 // Access: Protected, Virtual 00469 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00470 // for rendering this solid. 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 // Generate the vertices such that we draw a tube with one endpoint 00480 // at (0, 0, 0), and another at (0, length, 0). Then we'll rotate 00481 // and translate it into place with the appropriate look_at matrix. 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 // Generate the first endcap. 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 // Now the cylinder sides. 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 // And the second endcap. 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 // Now transform the vertices to their actual location. 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 // Function: CollisionTube::recalc_internals 00538 // Access: Private 00539 // Description: Should be called internally to recompute the matrix 00540 // and length when the properties of the tube have 00541 // changed. 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 // Function: CollisionTube::calc_sphere1_vertex 00558 // Access: Private 00559 // Description: Calculates a particular vertex on the surface of the 00560 // first endcap hemisphere, for use in generating the 00561 // viz geometry. 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 // Find the point on the rim, based on the slice. 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 // Now pull that point in towards the pole, based on the ring. 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 // Function: CollisionTube::calc_sphere2_vertex 00586 // Access: Private 00587 // Description: Calculates a particular vertex on the surface of the 00588 // second endcap hemisphere, for use in generating the 00589 // viz geometry. 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 // Find the point on the rim, based on the slice. 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 // Now pull that point in towards the pole, based on the ring. 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 // Function: CollisionTube::intersects_line 00615 // Access: Private 00616 // Description: Determine the point(s) of intersection of a parametric 00617 // line with the tube. The line is infinite in both 00618 // directions, and passes through "from" and from+delta. 00619 // If the line does not intersect the tube, the 00620 // function returns false, and t1 and t2 are undefined. 00621 // If it does intersect the tube, it returns true, and 00622 // t1 and t2 are set to the points along the equation 00623 // from+t*delta that correspond to the two points of 00624 // intersection. 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 // Convert the line into our canonical coordinate space: the tube is 00631 // aligned with the y axis. 00632 LPoint3 from = from0 * _inv_mat; 00633 LVector3 delta = delta0 * _inv_mat; 00634 00635 PN_stdfloat radius = _radius + inflate_radius; 00636 00637 // Now project the line into the X-Z plane to test for intersection 00638 // with a 2-d circle around the origin. The equation for this is 00639 // very similar to the formula for the intersection of a line with a 00640 // sphere; see CollisionSphere::intersects_line() for the complete 00641 // derivation. It's a little bit simpler because the circle is 00642 // centered on the origin. 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 // If the delta2 is 0, the line is perpendicular to the X-Z plane. 00650 // The whole line intersects with the infinite cylinder if the 00651 // point is within the circle. 00652 if (from2.dot(from2) > radius * radius) { 00653 // Nope, the 2-d point is outside the circle, so no 00654 // intersection. 00655 return false; 00656 } 00657 00658 if (IS_NEARLY_ZERO(delta[1])) { 00659 // Actually, the whole delta vector is 0, so the line is just a 00660 // point. In this case, (since we have already shown the point 00661 // is within the infinite cylinder), we intersect if and only if 00662 // the three-dimensional point is between the endcaps. 00663 if (from[1] < -radius || from[1] > _length + radius) { 00664 // Way out. 00665 return false; 00666 } 00667 if (from[1] < 0.0f) { 00668 // Possibly within the first endcap. 00669 if (from.dot(from) > radius * radius) { 00670 return false; 00671 } 00672 } else if (from[1] > _length) { 00673 // Possibly within the second endcap. 00674 from[1] -= _length; 00675 if (from.dot(from) > radius * radius) { 00676 return false; 00677 } 00678 } 00679 00680 // The point is within the tube! 00681 t1 = t2 = 0.0; 00682 return true; 00683 } 00684 00685 // The 2-d point is within the circle, so compute our intersection 00686 // points to include the entire vertical slice of the cylinder. 00687 t1 = (-radius - from[1]) / delta[1]; 00688 t2 = (_length + radius - from[1]) / delta[1]; 00689 00690 } else { 00691 // The line is not perpendicular to the X-Z plane, so its 00692 // projection into the plane is 2-d line. Test that 2-d line for 00693 // intersection with the circular projection of the cylinder. 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 // Tangent. 00703 t1 = t2 = -B / (2.0*A); 00704 00705 } else if (radical < 0.0) { 00706 // No real roots: no intersection with the line. 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 // Now we need to verify that the intersection points fall within 00718 // the length of the cylinder. 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 // Both points are way off the bottom of the tube; no 00724 // intersection. 00725 return false; 00726 } else if (t1_y > _length + radius && t2_y > _length + radius) { 00727 // Both points are way off the top of the tube; no intersection. 00728 return false; 00729 } 00730 00731 if (t1_y < 0.0f) { 00732 // The starting point is off the bottom of the tube. Test the 00733 // line against the first endcap. 00734 double t1a, t2a; 00735 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) { 00736 // If there's no intersection with the endcap, there can't be an 00737 // intersection with the cylinder. 00738 return false; 00739 } 00740 t1 = t1a; 00741 00742 } else if (t1_y > _length) { 00743 // The starting point is off the top of the tube. Test the 00744 // line against the second endcap. 00745 double t1b, t2b; 00746 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) { 00747 // If there's no intersection with the endcap, there can't be an 00748 // intersection with the cylinder. 00749 return false; 00750 } 00751 t1 = t1b; 00752 } 00753 00754 if (t2_y < 0.0f) { 00755 // The ending point is off the bottom of the tube. Test the 00756 // line against the first endcap. 00757 double t1a, t2a; 00758 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) { 00759 // If there's no intersection with the endcap, there can't be an 00760 // intersection with the cylinder. 00761 return false; 00762 } 00763 t2 = t2a; 00764 00765 } else if (t2_y > _length) { 00766 // The ending point is off the top of the tube. Test the 00767 // line against the second endcap. 00768 double t1b, t2b; 00769 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) { 00770 // If there's no intersection with the endcap, there can't be an 00771 // intersection with the cylinder. 00772 return false; 00773 } 00774 t2 = t2b; 00775 } 00776 00777 return true; 00778 } 00779 00780 //////////////////////////////////////////////////////////////////// 00781 // Function: CollisionTube::sphere_intersects_line 00782 // Access: Private 00783 // Description: After confirming that the line intersects an infinite 00784 // cylinder, test whether it intersects one or the other 00785 // endcaps. The y parameter specifies the center of the 00786 // sphere (and hence the particular endcap. 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 // See CollisionSphere::intersects_line() for a derivation of the 00793 // formula here. 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 // Tangent. 00810 t1 = t2 = -B / (2.0 * A); 00811 return true; 00812 00813 } else if (radical < 0.0) { 00814 // No real roots: no intersection with the line. 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 // Function: CollisionTube::intersects_parabola 00828 // Access: Protected 00829 // Description: Determine a point of intersection of a parametric 00830 // parabola with the tube. 00831 // 00832 // We only consider the segment of the parabola between 00833 // t1 and t2, which has already been computed as 00834 // corresponding to points p1 and p2. If there is an 00835 // intersection, t is set to the parametric point of 00836 // intersection, and true is returned; otherwise, false 00837 // is returned. 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 // I don't even want to think about the math to do this calculation 00844 // directly--it's even worse than sphere-parabola. So I'll use the 00845 // recursive subdivision solution again, just like I did for 00846 // sphere-parabola. 00847 00848 // First, see if the line segment (p1 - p2) comes sufficiently close 00849 // to the parabola. Do this by computing the parametric intervening 00850 // point and comparing its distance from the linear intervening 00851 // point. 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 // Subdivide. 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 // The line segment is sufficiently close; compare the segment itself. 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 // Function: CollisionTube::calculate_surface_point_and_normal 00883 // Access: Private 00884 // Description: Calculates a point that is exactly on the surface 00885 // of the tube and its corresponding normal, given 00886 // a point that is supposedly on the surface of the 00887 // tube. 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 // Convert the point into our canonical space for analysis. 00895 LPoint3 point = surface_point * _inv_mat; 00896 LVector3 normal; 00897 00898 if (point[1] <= 0.0) { 00899 // The point is on the first endcap. 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 // The point is on the second endcap. 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 // The point is within the cylinder part. 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 // Now convert the point and normal back into real space. 00926 result_point = point * _mat; 00927 result_normal = normal * _mat; 00928 } 00929 00930 //////////////////////////////////////////////////////////////////// 00931 // Function: CollisionTube::set_intersection_point 00932 // Access: Private 00933 // Description: After an intersection has been detected, record the 00934 // computed intersection point in the CollisionEntry, 00935 // and also compute the relevant normal based on that 00936 // point. 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 // Also adjust the original point into the tube by the amount of 00957 // extra_radius, which should put it on the surface of the tube if 00958 // our collision was tangential. 00959 new_entry->set_interior_point(into_intersection_point - normal * extra_radius); 00960 } 00961 00962 //////////////////////////////////////////////////////////////////// 00963 // Function: CollisionTube::register_with_read_factory 00964 // Access: Public, Static 00965 // Description: Tells the BamReader how to create objects of type 00966 // CollisionTube. 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 // Function: CollisionTube::write_datagram 00975 // Access: Public, Virtual 00976 // Description: Writes the contents of this object to the datagram 00977 // for shipping out to a Bam file. 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 // Function: CollisionTube::make_from_bam 00989 // Access: Protected, Static 00990 // Description: This function is called by the BamReader's factory 00991 // when a new object of type CollisionTube is encountered 00992 // in the Bam file. It should create the CollisionTube 00993 // and extract its information from the file. 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 // Function: CollisionTube::fillin 01009 // Access: Protected 01010 // Description: This internal function is called by make_from_bam to 01011 // read in all of the relevant data from the BamFile for 01012 // the new CollisionTube. 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 }