Panda3D
|
00001 // Filename: collisionPlane.cxx 00002 // Created by: drose (25Apr00) 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 00016 #include "collisionPlane.h" 00017 #include "collisionHandler.h" 00018 #include "collisionEntry.h" 00019 #include "collisionSphere.h" 00020 #include "collisionLine.h" 00021 #include "collisionRay.h" 00022 #include "collisionSegment.h" 00023 #include "config_collide.h" 00024 #include "pointerToArray.h" 00025 #include "geomNode.h" 00026 #include "geom.h" 00027 #include "datagram.h" 00028 #include "datagramIterator.h" 00029 #include "bamReader.h" 00030 #include "bamWriter.h" 00031 #include "boundingPlane.h" 00032 #include "geom.h" 00033 #include "geomTrifans.h" 00034 #include "geomLinestrips.h" 00035 #include "geomVertexWriter.h" 00036 00037 PStatCollector CollisionPlane::_volume_pcollector("Collision Volumes:CollisionPlane"); 00038 PStatCollector CollisionPlane::_test_pcollector("Collision Tests:CollisionPlane"); 00039 TypeHandle CollisionPlane::_type_handle; 00040 00041 //////////////////////////////////////////////////////////////////// 00042 // Function: CollisionPlane::make_copy 00043 // Access: Public, Virtual 00044 // Description: 00045 //////////////////////////////////////////////////////////////////// 00046 CollisionSolid *CollisionPlane:: 00047 make_copy() { 00048 return new CollisionPlane(*this); 00049 } 00050 00051 //////////////////////////////////////////////////////////////////// 00052 // Function: CollisionPlane::xform 00053 // Access: Public, Virtual 00054 // Description: Transforms the solid by the indicated matrix. 00055 //////////////////////////////////////////////////////////////////// 00056 void CollisionPlane:: 00057 xform(const LMatrix4 &mat) { 00058 _plane = _plane * mat; 00059 CollisionSolid::xform(mat); 00060 } 00061 00062 //////////////////////////////////////////////////////////////////// 00063 // Function: CollisionPlane::get_collision_origin 00064 // Access: Public, Virtual 00065 // Description: Returns the point in space deemed to be the "origin" 00066 // of the solid for collision purposes. The closest 00067 // intersection point to this origin point is considered 00068 // to be the most significant. 00069 //////////////////////////////////////////////////////////////////// 00070 LPoint3 CollisionPlane:: 00071 get_collision_origin() const { 00072 // No real sensible origin exists for a plane. We return 0, 0, 0, 00073 // without even bothering to ensure that that point exists on the 00074 // plane. 00075 return LPoint3::origin(); 00076 } 00077 00078 //////////////////////////////////////////////////////////////////// 00079 // Function: CollisionPlane::get_volume_pcollector 00080 // Access: Public, Virtual 00081 // Description: Returns a PStatCollector that is used to count the 00082 // number of bounding volume tests made against a solid 00083 // of this type in a given frame. 00084 //////////////////////////////////////////////////////////////////// 00085 PStatCollector &CollisionPlane:: 00086 get_volume_pcollector() { 00087 return _volume_pcollector; 00088 } 00089 00090 //////////////////////////////////////////////////////////////////// 00091 // Function: CollisionPlane::get_test_pcollector 00092 // Access: Public, Virtual 00093 // Description: Returns a PStatCollector that is used to count the 00094 // number of intersection tests made against a solid 00095 // of this type in a given frame. 00096 //////////////////////////////////////////////////////////////////// 00097 PStatCollector &CollisionPlane:: 00098 get_test_pcollector() { 00099 return _test_pcollector; 00100 } 00101 00102 //////////////////////////////////////////////////////////////////// 00103 // Function: CollisionPlane::output 00104 // Access: Public, Virtual 00105 // Description: 00106 //////////////////////////////////////////////////////////////////// 00107 void CollisionPlane:: 00108 output(ostream &out) const { 00109 out << "cplane, (" << _plane << ")"; 00110 } 00111 00112 //////////////////////////////////////////////////////////////////// 00113 // Function: CollisionPlane::compute_internal_bounds 00114 // Access: Protected, Virtual 00115 // Description: 00116 //////////////////////////////////////////////////////////////////// 00117 PT(BoundingVolume) CollisionPlane:: 00118 compute_internal_bounds() const { 00119 return new BoundingPlane(_plane); 00120 } 00121 00122 //////////////////////////////////////////////////////////////////// 00123 // Function: CollisionPlane::test_intersection_from_sphere 00124 // Access: Public, Virtual 00125 // Description: 00126 //////////////////////////////////////////////////////////////////// 00127 PT(CollisionEntry) CollisionPlane:: 00128 test_intersection_from_sphere(const CollisionEntry &entry) const { 00129 const CollisionSphere *sphere; 00130 DCAST_INTO_R(sphere, entry.get_from(), 0); 00131 00132 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00133 00134 LPoint3 from_center = sphere->get_center() * wrt_mat; 00135 LVector3 from_radius_v = 00136 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; 00137 PN_stdfloat from_radius = length(from_radius_v); 00138 00139 PN_stdfloat dist = dist_to_plane(from_center); 00140 if (dist > from_radius) { 00141 // No intersection. 00142 return NULL; 00143 } 00144 00145 if (collide_cat.is_debug()) { 00146 collide_cat.debug() 00147 << "intersection detected from " << entry.get_from_node_path() 00148 << " into " << entry.get_into_node_path() << "\n"; 00149 } 00150 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00151 00152 LVector3 normal = ( 00153 has_effective_normal() && sphere->get_respect_effective_normal()) 00154 ? get_effective_normal() : get_normal(); 00155 00156 new_entry->set_surface_normal(normal); 00157 new_entry->set_surface_point(from_center - get_normal() * dist); 00158 new_entry->set_interior_point(from_center - get_normal() * from_radius); 00159 00160 return new_entry; 00161 } 00162 00163 //////////////////////////////////////////////////////////////////// 00164 // Function: CollisionPlane::test_intersection_from_line 00165 // Access: Public, Virtual 00166 // Description: 00167 //////////////////////////////////////////////////////////////////// 00168 PT(CollisionEntry) CollisionPlane:: 00169 test_intersection_from_line(const CollisionEntry &entry) const { 00170 const CollisionLine *line; 00171 DCAST_INTO_R(line, entry.get_from(), 0); 00172 00173 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00174 00175 LPoint3 from_origin = line->get_origin() * wrt_mat; 00176 LVector3 from_direction = line->get_direction() * wrt_mat; 00177 00178 PN_stdfloat t; 00179 if (!_plane.intersects_line(t, from_origin, from_direction)) { 00180 // No intersection. The line is parallel to the plane. 00181 00182 if (_plane.dist_to_plane(from_origin) > 0.0f) { 00183 // The line is entirely in front of the plane. 00184 return NULL; 00185 } 00186 00187 // The line is entirely behind the plane. 00188 t = 0.0f; 00189 } 00190 00191 if (collide_cat.is_debug()) { 00192 collide_cat.debug() 00193 << "intersection detected from " << entry.get_from_node_path() 00194 << " into " << entry.get_into_node_path() << "\n"; 00195 } 00196 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00197 00198 LPoint3 into_intersection_point = from_origin + t * from_direction; 00199 00200 LVector3 normal = 00201 (has_effective_normal() && line->get_respect_effective_normal()) 00202 ? get_effective_normal() : get_normal(); 00203 00204 new_entry->set_surface_normal(normal); 00205 new_entry->set_surface_point(into_intersection_point); 00206 00207 return new_entry; 00208 } 00209 00210 //////////////////////////////////////////////////////////////////// 00211 // Function: CollisionPlane::test_intersection_from_ray 00212 // Access: Public, Virtual 00213 // Description: 00214 //////////////////////////////////////////////////////////////////// 00215 PT(CollisionEntry) CollisionPlane:: 00216 test_intersection_from_ray(const CollisionEntry &entry) const { 00217 const CollisionRay *ray; 00218 DCAST_INTO_R(ray, entry.get_from(), 0); 00219 00220 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00221 00222 LPoint3 from_origin = ray->get_origin() * wrt_mat; 00223 LVector3 from_direction = ray->get_direction() * wrt_mat; 00224 00225 PN_stdfloat t; 00226 00227 if (_plane.dist_to_plane(from_origin) < 0.0f) { 00228 // The origin of the ray is behind the plane, so we don't need to 00229 // test further. 00230 t = 0.0f; 00231 00232 } else { 00233 if (!_plane.intersects_line(t, from_origin, from_direction)) { 00234 // No intersection. The ray is parallel to the plane. 00235 return NULL; 00236 } 00237 00238 if (t < 0.0f) { 00239 // The intersection point is before the start of the ray, and so 00240 // the ray is entirely in front of the plane. 00241 return NULL; 00242 } 00243 } 00244 00245 if (collide_cat.is_debug()) { 00246 collide_cat.debug() 00247 << "intersection detected from " << entry.get_from_node_path() 00248 << " into " << entry.get_into_node_path() << "\n"; 00249 } 00250 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00251 00252 LPoint3 into_intersection_point = from_origin + t * from_direction; 00253 00254 LVector3 normal = 00255 (has_effective_normal() && ray->get_respect_effective_normal()) 00256 ? get_effective_normal() : get_normal(); 00257 00258 new_entry->set_surface_normal(normal); 00259 new_entry->set_surface_point(into_intersection_point); 00260 00261 return new_entry; 00262 } 00263 00264 //////////////////////////////////////////////////////////////////// 00265 // Function: CollisionPlane::test_intersection_from_segment 00266 // Access: Public, Virtual 00267 // Description: 00268 //////////////////////////////////////////////////////////////////// 00269 PT(CollisionEntry) CollisionPlane:: 00270 test_intersection_from_segment(const CollisionEntry &entry) const { 00271 const CollisionSegment *segment; 00272 DCAST_INTO_R(segment, entry.get_from(), 0); 00273 00274 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00275 00276 LPoint3 from_a = segment->get_point_a() * wrt_mat; 00277 LPoint3 from_b = segment->get_point_b() * wrt_mat; 00278 00279 PN_stdfloat dist_a = _plane.dist_to_plane(from_a); 00280 PN_stdfloat dist_b = _plane.dist_to_plane(from_b); 00281 00282 if (dist_a >= 0.0f && dist_b >= 0.0f) { 00283 // Entirely in front of the plane means no intersection. 00284 return NULL; 00285 } 00286 00287 if (collide_cat.is_debug()) { 00288 collide_cat.debug() 00289 << "intersection detected from " << entry.get_from_node_path() 00290 << " into " << entry.get_into_node_path() << "\n"; 00291 } 00292 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00293 00294 LVector3 normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00295 new_entry->set_surface_normal(normal); 00296 00297 PN_stdfloat t; 00298 LVector3 from_direction = from_b - from_a; 00299 if (_plane.intersects_line(t, from_a, from_direction)) { 00300 // It intersects the plane. 00301 if (t >= 0.0f && t <= 1.0f) { 00302 // Within the segment! Yay, that means we have a surface point. 00303 new_entry->set_surface_point(from_a + t * from_direction); 00304 } 00305 } 00306 00307 if (dist_a < dist_b) { 00308 // Point A penetrates deeper. 00309 new_entry->set_interior_point(from_a); 00310 00311 } else if (dist_b < dist_a) { 00312 // No, point B does. 00313 new_entry->set_interior_point(from_b); 00314 00315 } else { 00316 // Let's be fair and choose the center of the segment. 00317 new_entry->set_interior_point((from_a + from_b) * 0.5); 00318 } 00319 00320 return new_entry; 00321 } 00322 00323 //////////////////////////////////////////////////////////////////// 00324 // Function: CollisionPlane::test_intersection_from_parabola 00325 // Access: Public, Virtual 00326 // Description: This is part of the double-dispatch implementation of 00327 // test_intersection(). It is called when the "from" 00328 // object is a parabola. 00329 //////////////////////////////////////////////////////////////////// 00330 PT(CollisionEntry) CollisionPlane:: 00331 test_intersection_from_parabola(const CollisionEntry &entry) const { 00332 const CollisionParabola *parabola; 00333 DCAST_INTO_R(parabola, entry.get_from(), 0); 00334 00335 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00336 00337 // Convert the parabola into local coordinate space. 00338 LParabola local_p(parabola->get_parabola()); 00339 local_p.xform(wrt_mat); 00340 00341 PN_stdfloat t; 00342 if (_plane.dist_to_plane(local_p.calc_point(parabola->get_t1())) < 0.0f) { 00343 // The first point in the parabola is behind the plane, so we 00344 // don't need to test further. 00345 t = parabola->get_t1(); 00346 00347 } else { 00348 PN_stdfloat t1, t2; 00349 if (!get_plane().intersects_parabola(t1, t2, local_p)) { 00350 // No intersection. The infinite parabola is entirely in front 00351 // of the plane. 00352 return NULL; 00353 } 00354 00355 if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) { 00356 if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) { 00357 // Both intersection points are within our segment of the 00358 // parabola. Choose the first of the two. 00359 t = min(t1, t2); 00360 } else { 00361 // Only t1 is within our segment. 00362 t = t1; 00363 } 00364 00365 } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) { 00366 // Only t2 is within our segment. 00367 t = t2; 00368 00369 } else { 00370 // Neither intersection point is within our segment. 00371 return NULL; 00372 } 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 = local_p.calc_point(t); 00383 00384 LVector3 normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00385 00386 new_entry->set_surface_normal(normal); 00387 new_entry->set_surface_point(into_intersection_point); 00388 00389 return new_entry; 00390 } 00391 00392 //////////////////////////////////////////////////////////////////// 00393 // Function: CollisionPlane::fill_viz_geom 00394 // Access: Protected, Virtual 00395 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00396 // for rendering this solid. 00397 //////////////////////////////////////////////////////////////////// 00398 void CollisionPlane:: 00399 fill_viz_geom() { 00400 if (collide_cat.is_debug()) { 00401 collide_cat.debug() 00402 << "Recomputing viz for " << *this << "\n"; 00403 } 00404 00405 // Since we can't represent an infinite plane, we'll have to be 00406 // satisfied with drawing a big polygon. Choose four points on the 00407 // plane to be the corners of the polygon. 00408 00409 // We must choose four points fairly reasonably spread apart on 00410 // the plane. We'll start with a center point and one corner 00411 // point, and then use cross products to find the remaining three 00412 // corners of a square. 00413 00414 // The center point will be on the axis with the largest 00415 // coefficent. The first corner will be diagonal in the other two 00416 // dimensions. 00417 00418 LPoint3 cp; 00419 LVector3 p1, p2, p3, p4; 00420 00421 LVector3 normal = get_normal(); 00422 PN_stdfloat D = _plane[3]; 00423 00424 if (fabs(normal[0]) > fabs(normal[1]) && 00425 fabs(normal[0]) > fabs(normal[2])) { 00426 // X has the largest coefficient. 00427 cp.set(-D / normal[0], 0.0f, 0.0f); 00428 p1 = LPoint3(-(normal[1] + normal[2] + D)/normal[0], 1.0f, 1.0f) - cp; 00429 00430 } else if (fabs(normal[1]) > fabs(normal[2])) { 00431 // Y has the largest coefficient. 00432 cp.set(0.0f, -D / normal[1], 0.0f); 00433 p1 = LPoint3(1.0f, -(normal[0] + normal[2] + D)/normal[1], 1.0f) - cp; 00434 00435 } else { 00436 // Z has the largest coefficient. 00437 cp.set(0.0f, 0.0f, -D / normal[2]); 00438 p1 = LPoint3(1.0f, 1.0f, -(normal[0] + normal[1] + D)/normal[2]) - cp; 00439 } 00440 00441 p1.normalize(); 00442 p2 = cross(normal, p1); 00443 p3 = cross(normal, p2); 00444 p4 = cross(normal, p3); 00445 00446 static const double plane_scale = 10.0; 00447 00448 PT(GeomVertexData) vdata = new GeomVertexData 00449 ("collision", GeomVertexFormat::get_v3(), 00450 Geom::UH_static); 00451 GeomVertexWriter vertex(vdata, InternalName::get_vertex()); 00452 00453 vertex.add_data3(cp + p1 * plane_scale); 00454 vertex.add_data3(cp + p2 * plane_scale); 00455 vertex.add_data3(cp + p3 * plane_scale); 00456 vertex.add_data3(cp + p4 * plane_scale); 00457 00458 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static); 00459 body->add_consecutive_vertices(0, 4); 00460 body->close_primitive(); 00461 00462 PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static); 00463 border->add_consecutive_vertices(0, 4); 00464 border->add_vertex(0); 00465 border->close_primitive(); 00466 00467 PT(Geom) geom1 = new Geom(vdata); 00468 geom1->add_primitive(body); 00469 00470 PT(Geom) geom2 = new Geom(vdata); 00471 geom2->add_primitive(border); 00472 00473 _viz_geom->add_geom(geom1, get_solid_viz_state()); 00474 _viz_geom->add_geom(geom2, get_wireframe_viz_state()); 00475 00476 _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state()); 00477 _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state()); 00478 } 00479 00480 //////////////////////////////////////////////////////////////////// 00481 // Function: CollisionPlane::write_datagram 00482 // Access: Public 00483 // Description: Function to write the important information in 00484 // the particular object to a Datagram 00485 //////////////////////////////////////////////////////////////////// 00486 void CollisionPlane:: 00487 write_datagram(BamWriter *manager, Datagram &me) 00488 { 00489 CollisionSolid::write_datagram(manager, me); 00490 _plane.write_datagram(me); 00491 } 00492 00493 //////////////////////////////////////////////////////////////////// 00494 // Function: CollisionPlane::fillin 00495 // Access: Protected 00496 // Description: Function that reads out of the datagram (or asks 00497 // manager to read) all of the data that is needed to 00498 // re-create this object and stores it in the appropiate 00499 // place 00500 //////////////////////////////////////////////////////////////////// 00501 void CollisionPlane:: 00502 fillin(DatagramIterator& scan, BamReader* manager) 00503 { 00504 CollisionSolid::fillin(scan, manager); 00505 _plane.read_datagram(scan); 00506 } 00507 00508 //////////////////////////////////////////////////////////////////// 00509 // Function: CollisionPlane::make_CollisionPlane 00510 // Access: Protected 00511 // Description: Factory method to generate a CollisionPlane object 00512 //////////////////////////////////////////////////////////////////// 00513 TypedWritable* CollisionPlane:: 00514 make_CollisionPlane(const FactoryParams ¶ms) 00515 { 00516 CollisionPlane *me = new CollisionPlane; 00517 DatagramIterator scan; 00518 BamReader *manager; 00519 00520 parse_params(params, scan, manager); 00521 me->fillin(scan, manager); 00522 return me; 00523 } 00524 00525 //////////////////////////////////////////////////////////////////// 00526 // Function: CollisionPlane::register_with_factory 00527 // Access: Public, Static 00528 // Description: Factory method to generate a CollisionPlane object 00529 //////////////////////////////////////////////////////////////////// 00530 void CollisionPlane:: 00531 register_with_read_factory() 00532 { 00533 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPlane); 00534 }