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 LMatrix4f &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 LPoint3f 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 LPoint3f::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 LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00133 00134 LPoint3f from_center = sphere->get_center() * wrt_mat; 00135 LVector3f from_radius_v = 00136 LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; 00137 float from_radius = length(from_radius_v); 00138 00139 float 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 LVector3f from_normal = get_normal() * entry.get_inv_wrt_mat(); 00153 00154 LVector3f normal = ( 00155 has_effective_normal() && sphere->get_respect_effective_normal()) 00156 ? get_effective_normal() : get_normal(); 00157 00158 new_entry->set_surface_normal(normal); 00159 new_entry->set_surface_point(from_center - get_normal() * dist); 00160 new_entry->set_interior_point(from_center - get_normal() * from_radius); 00161 00162 return new_entry; 00163 } 00164 00165 //////////////////////////////////////////////////////////////////// 00166 // Function: CollisionPlane::test_intersection_from_line 00167 // Access: Public, Virtual 00168 // Description: 00169 //////////////////////////////////////////////////////////////////// 00170 PT(CollisionEntry) CollisionPlane:: 00171 test_intersection_from_line(const CollisionEntry &entry) const { 00172 const CollisionLine *line; 00173 DCAST_INTO_R(line, entry.get_from(), 0); 00174 00175 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00176 00177 LPoint3f from_origin = line->get_origin() * wrt_mat; 00178 LVector3f from_direction = line->get_direction() * wrt_mat; 00179 00180 float t; 00181 if (!_plane.intersects_line(t, from_origin, from_direction)) { 00182 // No intersection. The line is parallel to the plane. 00183 00184 if (_plane.dist_to_plane(from_origin) > 0.0f) { 00185 // The line is entirely in front of the plane. 00186 return NULL; 00187 } 00188 00189 // The line is entirely behind the plane. 00190 t = 0.0f; 00191 } 00192 00193 if (collide_cat.is_debug()) { 00194 collide_cat.debug() 00195 << "intersection detected from " << entry.get_from_node_path() 00196 << " into " << entry.get_into_node_path() << "\n"; 00197 } 00198 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00199 00200 LPoint3f into_intersection_point = from_origin + t * from_direction; 00201 00202 LVector3f normal = 00203 (has_effective_normal() && line->get_respect_effective_normal()) 00204 ? get_effective_normal() : get_normal(); 00205 00206 new_entry->set_surface_normal(normal); 00207 new_entry->set_surface_point(into_intersection_point); 00208 00209 return new_entry; 00210 } 00211 00212 //////////////////////////////////////////////////////////////////// 00213 // Function: CollisionPlane::test_intersection_from_ray 00214 // Access: Public, Virtual 00215 // Description: 00216 //////////////////////////////////////////////////////////////////// 00217 PT(CollisionEntry) CollisionPlane:: 00218 test_intersection_from_ray(const CollisionEntry &entry) const { 00219 const CollisionRay *ray; 00220 DCAST_INTO_R(ray, entry.get_from(), 0); 00221 00222 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00223 00224 LPoint3f from_origin = ray->get_origin() * wrt_mat; 00225 LVector3f from_direction = ray->get_direction() * wrt_mat; 00226 00227 float t; 00228 00229 if (_plane.dist_to_plane(from_origin) < 0.0f) { 00230 // The origin of the ray is behind the plane, so we don't need to 00231 // test further. 00232 t = 0.0f; 00233 00234 } else { 00235 if (!_plane.intersects_line(t, from_origin, from_direction)) { 00236 // No intersection. The ray is parallel to the plane. 00237 return NULL; 00238 } 00239 00240 if (t < 0.0f) { 00241 // The intersection point is before the start of the ray, and so 00242 // the ray is entirely in front of the plane. 00243 return NULL; 00244 } 00245 } 00246 00247 if (collide_cat.is_debug()) { 00248 collide_cat.debug() 00249 << "intersection detected from " << entry.get_from_node_path() 00250 << " into " << entry.get_into_node_path() << "\n"; 00251 } 00252 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00253 00254 LPoint3f into_intersection_point = from_origin + t * from_direction; 00255 00256 LVector3f normal = 00257 (has_effective_normal() && ray->get_respect_effective_normal()) 00258 ? get_effective_normal() : get_normal(); 00259 00260 new_entry->set_surface_normal(normal); 00261 new_entry->set_surface_point(into_intersection_point); 00262 00263 return new_entry; 00264 } 00265 00266 //////////////////////////////////////////////////////////////////// 00267 // Function: CollisionPlane::test_intersection_from_segment 00268 // Access: Public, Virtual 00269 // Description: 00270 //////////////////////////////////////////////////////////////////// 00271 PT(CollisionEntry) CollisionPlane:: 00272 test_intersection_from_segment(const CollisionEntry &entry) const { 00273 const CollisionSegment *segment; 00274 DCAST_INTO_R(segment, entry.get_from(), 0); 00275 00276 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00277 00278 LPoint3f from_a = segment->get_point_a() * wrt_mat; 00279 LPoint3f from_b = segment->get_point_b() * wrt_mat; 00280 LVector3f from_direction = from_b - from_a; 00281 00282 float t; 00283 if (_plane.dist_to_plane(from_a) < 0.0f) { 00284 // The first point of the line segment is behind the plane, so we 00285 // don't need to test further. 00286 t = 0.0f; 00287 00288 } else { 00289 if (!_plane.intersects_line(t, from_a, from_direction)) { 00290 // No intersection. The line segment is parallel to the plane. 00291 return NULL; 00292 } 00293 00294 if (t < 0.0f || t > 1.0f) { 00295 // The intersection point is before the start of the segment or 00296 // after the end of the segment. Therefore, the line segment is 00297 // entirely in front of the plane. 00298 return NULL; 00299 } 00300 } 00301 00302 if (collide_cat.is_debug()) { 00303 collide_cat.debug() 00304 << "intersection detected from " << entry.get_from_node_path() 00305 << " into " << entry.get_into_node_path() << "\n"; 00306 } 00307 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00308 00309 LPoint3f into_intersection_point = from_a + t * from_direction; 00310 00311 LVector3f normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00312 00313 new_entry->set_surface_normal(normal); 00314 new_entry->set_surface_point(into_intersection_point); 00315 00316 return new_entry; 00317 } 00318 00319 //////////////////////////////////////////////////////////////////// 00320 // Function: CollisionPlane::test_intersection_from_parabola 00321 // Access: Public, Virtual 00322 // Description: This is part of the double-dispatch implementation of 00323 // test_intersection(). It is called when the "from" 00324 // object is a parabola. 00325 //////////////////////////////////////////////////////////////////// 00326 PT(CollisionEntry) CollisionPlane:: 00327 test_intersection_from_parabola(const CollisionEntry &entry) const { 00328 const CollisionParabola *parabola; 00329 DCAST_INTO_R(parabola, entry.get_from(), 0); 00330 00331 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00332 00333 // Convert the parabola into local coordinate space. 00334 Parabolaf local_p(parabola->get_parabola()); 00335 local_p.xform(wrt_mat); 00336 00337 float t; 00338 if (_plane.dist_to_plane(local_p.calc_point(parabola->get_t1())) < 0.0f) { 00339 // The first point in the parabola is behind the plane, so we 00340 // don't need to test further. 00341 t = parabola->get_t1(); 00342 00343 } else { 00344 float t1, t2; 00345 if (!get_plane().intersects_parabola(t1, t2, local_p)) { 00346 // No intersection. The infinite parabola is entirely in front 00347 // of the plane. 00348 return NULL; 00349 } 00350 00351 if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) { 00352 if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) { 00353 // Both intersection points are within our segment of the 00354 // parabola. Choose the first of the two. 00355 t = min(t1, t2); 00356 } else { 00357 // Only t1 is within our segment. 00358 t = t1; 00359 } 00360 00361 } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) { 00362 // Only t2 is within our segment. 00363 t = t2; 00364 00365 } else { 00366 // Neither intersection point is within our segment. 00367 return NULL; 00368 } 00369 } 00370 00371 if (collide_cat.is_debug()) { 00372 collide_cat.debug() 00373 << "intersection detected from " << entry.get_from_node_path() 00374 << " into " << entry.get_into_node_path() << "\n"; 00375 } 00376 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00377 00378 LPoint3f into_intersection_point = local_p.calc_point(t); 00379 00380 LVector3f normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00381 00382 new_entry->set_surface_normal(normal); 00383 new_entry->set_surface_point(into_intersection_point); 00384 00385 return new_entry; 00386 } 00387 00388 //////////////////////////////////////////////////////////////////// 00389 // Function: CollisionPlane::fill_viz_geom 00390 // Access: Protected, Virtual 00391 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00392 // for rendering this solid. 00393 //////////////////////////////////////////////////////////////////// 00394 void CollisionPlane:: 00395 fill_viz_geom() { 00396 if (collide_cat.is_debug()) { 00397 collide_cat.debug() 00398 << "Recomputing viz for " << *this << "\n"; 00399 } 00400 00401 // Since we can't represent an infinite plane, we'll have to be 00402 // satisfied with drawing a big polygon. Choose four points on the 00403 // plane to be the corners of the polygon. 00404 00405 // We must choose four points fairly reasonably spread apart on 00406 // the plane. We'll start with a center point and one corner 00407 // point, and then use cross products to find the remaining three 00408 // corners of a square. 00409 00410 // The center point will be on the axis with the largest 00411 // coefficent. The first corner will be diagonal in the other two 00412 // dimensions. 00413 00414 LPoint3f cp; 00415 LVector3f p1, p2, p3, p4; 00416 00417 LVector3f normal = get_normal(); 00418 float D = _plane[3]; 00419 00420 if (fabs(normal[0]) > fabs(normal[1]) && 00421 fabs(normal[0]) > fabs(normal[2])) { 00422 // X has the largest coefficient. 00423 cp.set(-D / normal[0], 0.0f, 0.0f); 00424 p1 = LPoint3f(-(normal[1] + normal[2] + D)/normal[0], 1.0f, 1.0f) - cp; 00425 00426 } else if (fabs(normal[1]) > fabs(normal[2])) { 00427 // Y has the largest coefficient. 00428 cp.set(0.0f, -D / normal[1], 0.0f); 00429 p1 = LPoint3f(1.0f, -(normal[0] + normal[2] + D)/normal[1], 1.0f) - cp; 00430 00431 } else { 00432 // Z has the largest coefficient. 00433 cp.set(0.0f, 0.0f, -D / normal[2]); 00434 p1 = LPoint3f(1.0f, 1.0f, -(normal[0] + normal[1] + D)/normal[2]) - cp; 00435 } 00436 00437 p1.normalize(); 00438 p2 = cross(normal, p1); 00439 p3 = cross(normal, p2); 00440 p4 = cross(normal, p3); 00441 00442 static const double plane_scale = 10.0; 00443 00444 PT(GeomVertexData) vdata = new GeomVertexData 00445 ("collision", GeomVertexFormat::get_v3(), 00446 Geom::UH_static); 00447 GeomVertexWriter vertex(vdata, InternalName::get_vertex()); 00448 00449 vertex.add_data3f(cp + p1 * plane_scale); 00450 vertex.add_data3f(cp + p2 * plane_scale); 00451 vertex.add_data3f(cp + p3 * plane_scale); 00452 vertex.add_data3f(cp + p4 * plane_scale); 00453 00454 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static); 00455 body->add_consecutive_vertices(0, 4); 00456 body->close_primitive(); 00457 00458 PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static); 00459 border->add_consecutive_vertices(0, 4); 00460 border->add_vertex(0); 00461 border->close_primitive(); 00462 00463 PT(Geom) geom1 = new Geom(vdata); 00464 geom1->add_primitive(body); 00465 00466 PT(Geom) geom2 = new Geom(vdata); 00467 geom2->add_primitive(border); 00468 00469 _viz_geom->add_geom(geom1, get_solid_viz_state()); 00470 _viz_geom->add_geom(geom2, get_wireframe_viz_state()); 00471 00472 _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state()); 00473 _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state()); 00474 } 00475 00476 //////////////////////////////////////////////////////////////////// 00477 // Function: CollisionPlane::write_datagram 00478 // Access: Public 00479 // Description: Function to write the important information in 00480 // the particular object to a Datagram 00481 //////////////////////////////////////////////////////////////////// 00482 void CollisionPlane:: 00483 write_datagram(BamWriter *manager, Datagram &me) 00484 { 00485 CollisionSolid::write_datagram(manager, me); 00486 _plane.write_datagram(me); 00487 } 00488 00489 //////////////////////////////////////////////////////////////////// 00490 // Function: CollisionPlane::fillin 00491 // Access: Protected 00492 // Description: Function that reads out of the datagram (or asks 00493 // manager to read) all of the data that is needed to 00494 // re-create this object and stores it in the appropiate 00495 // place 00496 //////////////////////////////////////////////////////////////////// 00497 void CollisionPlane:: 00498 fillin(DatagramIterator& scan, BamReader* manager) 00499 { 00500 CollisionSolid::fillin(scan, manager); 00501 _plane.read_datagram(scan); 00502 } 00503 00504 //////////////////////////////////////////////////////////////////// 00505 // Function: CollisionPlane::make_CollisionPlane 00506 // Access: Protected 00507 // Description: Factory method to generate a CollisionPlane object 00508 //////////////////////////////////////////////////////////////////// 00509 TypedWritable* CollisionPlane:: 00510 make_CollisionPlane(const FactoryParams ¶ms) 00511 { 00512 CollisionPlane *me = new CollisionPlane; 00513 DatagramIterator scan; 00514 BamReader *manager; 00515 00516 parse_params(params, scan, manager); 00517 me->fillin(scan, manager); 00518 return me; 00519 } 00520 00521 //////////////////////////////////////////////////////////////////// 00522 // Function: CollisionPlane::register_with_factory 00523 // Access: Public, Static 00524 // Description: Factory method to generate a CollisionPlane object 00525 //////////////////////////////////////////////////////////////////// 00526 void CollisionPlane:: 00527 register_with_read_factory() 00528 { 00529 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPlane); 00530 }