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 LMatrix4f &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 LVector3f radius_v = LVector3f(_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 LPoint3f 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 LVector3f 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 LPoint3f 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 LMatrix4f &wrt_mat = wrt_space->get_mat(); 00166 00167 LPoint3f from_a = sphere->get_center() * wrt_mat; 00168 LPoint3f from_b = from_a; 00169 00170 LPoint3f contact_point; 00171 float 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 LVector3f from_direction = from_b - from_a; 00180 00181 LVector3f from_radius_v = 00182 LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; 00183 float 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 LPoint3f 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 LPoint3f fake_contact_point; 00221 LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00244 00245 LPoint3f from_origin = line->get_origin() * wrt_mat; 00246 LVector3f 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 LPoint3f 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 LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00294 00295 LPoint3f from_origin = ray->get_origin() * wrt_mat; 00296 LVector3f 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 LPoint3f 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 LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00358 00359 LPoint3f from_a = segment->get_point_a() * wrt_mat; 00360 LPoint3f from_b = segment->get_point_b() * wrt_mat; 00361 LVector3f 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 LPoint3f 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 LVector3f 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 LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00424 00425 // Convert the parabola into local coordinate space. 00426 Parabolaf 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 LPoint3f 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 LVector3f 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 LVector3f direction = (_b - _a); 00483 float 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_data3f(calc_sphere1_vertex(ri, si, num_rings, num_slices)); 00498 vertex.add_data3f(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_data3f(calc_sphere1_vertex(num_rings, si, num_rings, num_slices)); 00507 vertex.add_data3f(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_data3f(calc_sphere2_vertex(ri + 1, si, num_rings, num_slices, length)); 00517 vertex.add_data3f(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 LMatrix4f mat; 00528 look_at(mat, direction, LVector3f(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 LVector3f direction = (_b - _a); 00546 _length = direction.length(); 00547 00548 look_at(_mat, direction, LVector3f(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 Vertexf CollisionTube:: 00564 calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices) { 00565 float r = (float)ri / (float)num_rings; 00566 float s = (float)si / (float)num_slices; 00567 00568 // Find the point on the rim, based on the slice. 00569 float theta = s * 2.0f * MathNumbers::pi_f; 00570 float x_rim = ccos(theta); 00571 float z_rim = csin(theta); 00572 00573 // Now pull that point in towards the pole, based on the ring. 00574 float phi = r * 0.5f * MathNumbers::pi_f; 00575 float to_pole = csin(phi); 00576 00577 float x = _radius * x_rim * to_pole; 00578 float y = -_radius * ccos(phi); 00579 float z = _radius * z_rim * to_pole; 00580 00581 return Vertexf(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 Vertexf CollisionTube:: 00592 calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices, 00593 float length) { 00594 float r = (float)ri / (float)num_rings; 00595 float s = (float)si / (float)num_slices; 00596 00597 // Find the point on the rim, based on the slice. 00598 float theta = s * 2.0f * MathNumbers::pi_f; 00599 float x_rim = ccos(theta); 00600 float z_rim = csin(theta); 00601 00602 // Now pull that point in towards the pole, based on the ring. 00603 float phi = r * 0.5f * MathNumbers::pi_f; 00604 float to_pole = csin(phi); 00605 00606 float x = _radius * x_rim * to_pole; 00607 float y = length + _radius * ccos(phi); 00608 float z = _radius * z_rim * to_pole; 00609 00610 return Vertexf(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 LPoint3f from, LVector3f delta, float inflate_radius) const { 00629 // Convert the line into our canonical coordinate space: the tube is 00630 // aligned with the y axis. 00631 from = from * _inv_mat; 00632 delta = delta * _inv_mat; 00633 00634 float radius = _radius + inflate_radius; 00635 00636 // Now project the line into the X-Z plane to test for intersection 00637 // with a 2-d circle around the origin. The equation for this is 00638 // very similar to the formula for the intersection of a line with a 00639 // sphere; see CollisionSphere::intersects_line() for the complete 00640 // derivation. It's a little bit simpler because the circle is 00641 // centered on the origin. 00642 LVector2f from2(from[0], from[2]); 00643 LVector2f delta2(delta[0], delta[2]); 00644 00645 double A = dot(delta2, delta2); 00646 00647 if (IS_NEARLY_ZERO(A)) { 00648 // If the delta2 is 0, the line is perpendicular to the X-Z plane. 00649 // The whole line intersects with the infinite cylinder if the 00650 // point is within the circle. 00651 if (from2.dot(from2) > radius * radius) { 00652 // Nope, the 2-d point is outside the circle, so no 00653 // intersection. 00654 return false; 00655 } 00656 00657 if (IS_NEARLY_ZERO(delta[1])) { 00658 // Actually, the whole delta vector is 0, so the line is just a 00659 // point. In this case, (since we have already shown the point 00660 // is within the infinite cylinder), we intersect if and only if 00661 // the three-dimensional point is between the endcaps. 00662 if (from[1] < -radius || from[1] > _length + radius) { 00663 // Way out. 00664 return false; 00665 } 00666 if (from[1] < 0.0f) { 00667 // Possibly within the first endcap. 00668 if (from.dot(from) > radius * radius) { 00669 return false; 00670 } 00671 } else if (from[1] > _length) { 00672 // Possibly within the second endcap. 00673 from[1] -= _length; 00674 if (from.dot(from) > radius * radius) { 00675 return false; 00676 } 00677 } 00678 00679 // The point is within the tube! 00680 t1 = t2 = 0.0; 00681 return true; 00682 } 00683 00684 // The 2-d point is within the circle, so compute our intersection 00685 // points to include the entire vertical slice of the cylinder. 00686 t1 = (-radius - from[1]) / delta[1]; 00687 t2 = (_length + radius - from[1]) / delta[1]; 00688 00689 } else { 00690 // The line is not perpendicular to the X-Z plane, so its 00691 // projection into the plane is 2-d line. Test that 2-d line for 00692 // intersection with the circular projection of the cylinder. 00693 00694 double B = 2.0f * dot(delta2, from2); 00695 double fc_d2 = dot(from2, from2); 00696 double C = fc_d2 - radius * radius; 00697 00698 double radical = B*B - 4.0*A*C; 00699 00700 if (IS_NEARLY_ZERO(radical)) { 00701 // Tangent. 00702 t1 = t2 = -B / (2.0*A); 00703 00704 } else if (radical < 0.0) { 00705 // No real roots: no intersection with the line. 00706 return false; 00707 00708 } else { 00709 double reciprocal_2A = 1.0 / (2.0 * A); 00710 double sqrt_radical = sqrtf(radical); 00711 t1 = ( -B - sqrt_radical ) * reciprocal_2A; 00712 t2 = ( -B + sqrt_radical ) * reciprocal_2A; 00713 } 00714 } 00715 00716 // Now we need to verify that the intersection points fall within 00717 // the length of the cylinder. 00718 float t1_y = from[1] + t1 * delta[1]; 00719 float t2_y = from[1] + t2 * delta[1]; 00720 00721 if (t1_y < -radius && t2_y < -radius) { 00722 // Both points are way off the bottom of the tube; no 00723 // intersection. 00724 return false; 00725 } else if (t1_y > _length + radius && t2_y > _length + radius) { 00726 // Both points are way off the top of the tube; no intersection. 00727 return false; 00728 } 00729 00730 if (t1_y < 0.0f) { 00731 // The starting point is off the bottom of the tube. Test the 00732 // line against the first endcap. 00733 double t1a, t2a; 00734 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) { 00735 // If there's no intersection with the endcap, there can't be an 00736 // intersection with the cylinder. 00737 return false; 00738 } 00739 t1 = t1a; 00740 00741 } else if (t1_y > _length) { 00742 // The starting point is off the top of the tube. Test the 00743 // line against the second endcap. 00744 double t1b, t2b; 00745 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) { 00746 // If there's no intersection with the endcap, there can't be an 00747 // intersection with the cylinder. 00748 return false; 00749 } 00750 t1 = t1b; 00751 } 00752 00753 if (t2_y < 0.0f) { 00754 // The ending point is off the bottom of the tube. Test the 00755 // line against the first endcap. 00756 double t1a, t2a; 00757 if (!sphere_intersects_line(t1a, t2a, 0.0f, from, delta, inflate_radius)) { 00758 // If there's no intersection with the endcap, there can't be an 00759 // intersection with the cylinder. 00760 return false; 00761 } 00762 t2 = t2a; 00763 00764 } else if (t2_y > _length) { 00765 // The ending point is off the top of the tube. Test the 00766 // line against the second endcap. 00767 double t1b, t2b; 00768 if (!sphere_intersects_line(t1b, t2b, _length, from, delta, inflate_radius)) { 00769 // If there's no intersection with the endcap, there can't be an 00770 // intersection with the cylinder. 00771 return false; 00772 } 00773 t2 = t2b; 00774 } 00775 00776 return true; 00777 } 00778 00779 //////////////////////////////////////////////////////////////////// 00780 // Function: CollisionTube::sphere_intersects_line 00781 // Access: Private 00782 // Description: After confirming that the line intersects an infinite 00783 // cylinder, test whether it intersects one or the other 00784 // endcaps. The y parameter specifies the center of the 00785 // sphere (and hence the particular endcap. 00786 //////////////////////////////////////////////////////////////////// 00787 bool CollisionTube:: 00788 sphere_intersects_line(double &t1, double &t2, float center_y, 00789 const LPoint3f &from, const LVector3f &delta, 00790 float inflate_radius) const { 00791 // See CollisionSphere::intersects_line() for a derivation of the 00792 // formula here. 00793 float radius = _radius + inflate_radius; 00794 00795 double A = dot(delta, delta); 00796 00797 nassertr(A != 0.0, false); 00798 00799 LVector3f fc = from; 00800 fc[1] -= center_y; 00801 double B = 2.0f* dot(delta, fc); 00802 double fc_d2 = dot(fc, fc); 00803 double C = fc_d2 - radius * radius; 00804 00805 double radical = B*B - 4.0*A*C; 00806 00807 if (IS_NEARLY_ZERO(radical)) { 00808 // Tangent. 00809 t1 = t2 = -B / (2.0 * A); 00810 return true; 00811 00812 } else if (radical < 0.0) { 00813 // No real roots: no intersection with the line. 00814 return false; 00815 } 00816 00817 double reciprocal_2A = 1.0 / (2.0 * A); 00818 double sqrt_radical = sqrtf(radical); 00819 t1 = ( -B - sqrt_radical ) * reciprocal_2A; 00820 t2 = ( -B + sqrt_radical ) * reciprocal_2A; 00821 00822 return true; 00823 } 00824 00825 //////////////////////////////////////////////////////////////////// 00826 // Function: CollisionTube::intersects_parabola 00827 // Access: Protected 00828 // Description: Determine a point of intersection of a parametric 00829 // parabola with the tube. 00830 // 00831 // We only consider the segment of the parabola between 00832 // t1 and t2, which has already been computed as 00833 // corresponding to points p1 and p2. If there is an 00834 // intersection, t is set to the parametric point of 00835 // intersection, and true is returned; otherwise, false 00836 // is returned. 00837 //////////////////////////////////////////////////////////////////// 00838 bool CollisionTube:: 00839 intersects_parabola(double &t, const Parabolaf ¶bola, 00840 double t1, double t2, 00841 const LPoint3f &p1, const LPoint3f &p2) const { 00842 // I don't even want to think about the math to do this calculation 00843 // directly--it's even worse than sphere-parabola. So I'll use the 00844 // recursive subdivision solution again, just like I did for 00845 // sphere-parabola. 00846 00847 // First, see if the line segment (p1 - p2) comes sufficiently close 00848 // to the parabola. Do this by computing the parametric intervening 00849 // point and comparing its distance from the linear intervening 00850 // point. 00851 double tmid = (t1 + t2) * 0.5; 00852 00853 if (tmid != t1 && tmid != t2) { 00854 LPoint3f pmid = parabola.calc_point(tmid); 00855 LPoint3f pmid2 = (p1 + p2) * 0.5f; 00856 00857 if ((pmid - pmid2).length_squared() > 0.001f) { 00858 // Subdivide. 00859 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) { 00860 return true; 00861 } 00862 return intersects_parabola(t, parabola, tmid, t2, pmid, p2); 00863 } 00864 } 00865 00866 // The line segment is sufficiently close; compare the segment itself. 00867 double t1a, t2a; 00868 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) { 00869 return false; 00870 } 00871 00872 if (t2a < 0.0 || t1a > 1.0) { 00873 return false; 00874 } 00875 00876 t = max(t1a, 0.0); 00877 return true; 00878 } 00879 00880 //////////////////////////////////////////////////////////////////// 00881 // Function: CollisionTube::calculate_surface_point_and_normal 00882 // Access: Private 00883 // Description: Calculates a point that is exactly on the surface 00884 // of the tube and its corresponding normal, given 00885 // a point that is supposedly on the surface of the 00886 // tube. 00887 //////////////////////////////////////////////////////////////////// 00888 void CollisionTube:: 00889 calculate_surface_point_and_normal(const LPoint3f &surface_point, 00890 double extra_radius, 00891 LPoint3f &result_point, 00892 LVector3f &result_normal) const { 00893 // Convert the point into our canonical space for analysis. 00894 LPoint3f point = surface_point * _inv_mat; 00895 LVector3f normal; 00896 00897 if (point[1] <= 0.0) { 00898 // The point is on the first endcap. 00899 normal = point; 00900 if (!normal.normalize()) { 00901 normal.set(0.0, -1.0, 0.0); 00902 } 00903 point = normal * _radius; 00904 00905 } else if (point[1] >= _length) { 00906 // The point is on the second endcap. 00907 normal.set(point[0], point[1] - _length, point[2]); 00908 if (!normal.normalize()) { 00909 normal.set(0.0, 1.0, 0.0); 00910 } 00911 point = normal * _radius; 00912 point[1] += _length; 00913 00914 } else { 00915 // The point is within the cylinder part. 00916 LVector2d normal2d(point[0], point[2]); 00917 if (!normal2d.normalize()) { 00918 normal2d.set(0.0, 1.0); 00919 } 00920 normal.set(normal2d[0], 0.0, normal2d[1]); 00921 point.set(normal[0] * _radius, point[1], normal[2] * _radius); 00922 } 00923 00924 // Now convert the point and normal back into real space. 00925 result_point = point * _mat; 00926 result_normal = normal * _mat; 00927 } 00928 00929 //////////////////////////////////////////////////////////////////// 00930 // Function: CollisionTube::set_intersection_point 00931 // Access: Private 00932 // Description: After an intersection has been detected, record the 00933 // computed intersection point in the CollisionEntry, 00934 // and also compute the relevant normal based on that 00935 // point. 00936 //////////////////////////////////////////////////////////////////// 00937 void CollisionTube:: 00938 set_intersection_point(CollisionEntry *new_entry, 00939 const LPoint3f &into_intersection_point, 00940 double extra_radius) const { 00941 LPoint3f point; 00942 LVector3f normal; 00943 00944 calculate_surface_point_and_normal(into_intersection_point, 00945 extra_radius, 00946 point, 00947 normal); 00948 00949 if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) { 00950 normal = get_effective_normal(); 00951 } 00952 00953 new_entry->set_surface_normal(normal); 00954 new_entry->set_surface_point(point); 00955 // Also adjust the original point into the tube by the amount of 00956 // extra_radius, which should put it on the surface of the tube if 00957 // our collision was tangential. 00958 new_entry->set_interior_point(into_intersection_point - normal * extra_radius); 00959 } 00960 00961 //////////////////////////////////////////////////////////////////// 00962 // Function: CollisionTube::register_with_read_factory 00963 // Access: Public, Static 00964 // Description: Tells the BamReader how to create objects of type 00965 // CollisionTube. 00966 //////////////////////////////////////////////////////////////////// 00967 void CollisionTube:: 00968 register_with_read_factory() { 00969 BamReader::get_factory()->register_factory(get_class_type(), make_from_bam); 00970 } 00971 00972 //////////////////////////////////////////////////////////////////// 00973 // Function: CollisionTube::write_datagram 00974 // Access: Public, Virtual 00975 // Description: Writes the contents of this object to the datagram 00976 // for shipping out to a Bam file. 00977 //////////////////////////////////////////////////////////////////// 00978 void CollisionTube:: 00979 write_datagram(BamWriter *manager, Datagram &dg) { 00980 CollisionSolid::write_datagram(manager, dg); 00981 _a.write_datagram(dg); 00982 _b.write_datagram(dg); 00983 dg.add_float32(_radius); 00984 } 00985 00986 //////////////////////////////////////////////////////////////////// 00987 // Function: CollisionTube::make_from_bam 00988 // Access: Protected, Static 00989 // Description: This function is called by the BamReader's factory 00990 // when a new object of type CollisionTube is encountered 00991 // in the Bam file. It should create the CollisionTube 00992 // and extract its information from the file. 00993 //////////////////////////////////////////////////////////////////// 00994 TypedWritable *CollisionTube:: 00995 make_from_bam(const FactoryParams ¶ms) { 00996 CollisionTube *node = new CollisionTube(); 00997 DatagramIterator scan; 00998 BamReader *manager; 00999 01000 parse_params(params, scan, manager); 01001 node->fillin(scan, manager); 01002 01003 return node; 01004 } 01005 01006 //////////////////////////////////////////////////////////////////// 01007 // Function: CollisionTube::fillin 01008 // Access: Protected 01009 // Description: This internal function is called by make_from_bam to 01010 // read in all of the relevant data from the BamFile for 01011 // the new CollisionTube. 01012 //////////////////////////////////////////////////////////////////// 01013 void CollisionTube:: 01014 fillin(DatagramIterator &scan, BamReader *manager) { 01015 CollisionSolid::fillin(scan, manager); 01016 _a.read_datagram(scan); 01017 _b.read_datagram(scan); 01018 _radius = scan.get_float32(); 01019 recalc_internals(); 01020 }