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 "collisionFloorMesh.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 "geomTriangles.h" 00034 #include "geomLinestrips.h" 00035 #include "geomVertexWriter.h" 00036 #include <algorithm> 00037 PStatCollector CollisionFloorMesh::_volume_pcollector("Collision Volumes:CollisionFloorMesh"); 00038 PStatCollector CollisionFloorMesh::_test_pcollector("Collision Tests:CollisionFloorMesh"); 00039 TypeHandle CollisionFloorMesh::_type_handle; 00040 00041 //////////////////////////////////////////////////////////////////// 00042 // Function: CollisionPlane::make_copy 00043 // Access: Public, Virtual 00044 // Description: 00045 //////////////////////////////////////////////////////////////////// 00046 CollisionSolid *CollisionFloorMesh:: 00047 make_copy() { 00048 return new CollisionFloorMesh(*this); 00049 } 00050 00051 //////////////////////////////////////////////////////////////////// 00052 // Function: CollisionPlane::xform 00053 // Access: Public, Virtual 00054 // Description: Transforms the solid by the indicated matrix. 00055 //////////////////////////////////////////////////////////////////// 00056 void CollisionFloorMesh:: 00057 xform(const LMatrix4 &mat) { 00058 Vertices::iterator vi; 00059 for (vi=_vertices.begin();vi!=_vertices.end();++vi) { 00060 LPoint3 pt = (*vi) * mat; 00061 (*vi).set(pt[0],pt[1],pt[2]); 00062 } 00063 Triangles::iterator ti; 00064 for (ti=_triangles.begin();ti!=_triangles.end();++ti) { 00065 CollisionFloorMesh::TriangleIndices tri = *ti; 00066 LPoint3 v1 = _vertices[tri.p1]; 00067 LPoint3 v2 = _vertices[tri.p2]; 00068 LPoint3 v3 = _vertices[tri.p3]; 00069 00070 tri.min_x=min(min(v1[0],v2[0]),v3[0]); 00071 tri.max_x=max(max(v1[0],v2[0]),v3[0]); 00072 tri.min_y=min(min(v1[1],v2[1]),v3[1]); 00073 tri.max_y=max(max(v1[1],v2[1]),v3[1]); 00074 } 00075 CollisionSolid::xform(mat); 00076 } 00077 00078 //////////////////////////////////////////////////////////////////// 00079 // Function: CollisionPlane::get_collision_origin 00080 // Access: Public, Virtual 00081 // Description: Returns the point in space deemed to be the "origin" 00082 // of the solid for collision purposes. The closest 00083 // intersection point to this origin point is considered 00084 // to be the most significant. 00085 //////////////////////////////////////////////////////////////////// 00086 LPoint3 CollisionFloorMesh:: 00087 get_collision_origin() const { 00088 // No real sensible origin exists for a plane. We return 0, 0, 0, 00089 // without even bothering to ensure that that point exists on the 00090 // plane. 00091 return LPoint3::origin(); 00092 } 00093 00094 //////////////////////////////////////////////////////////////////// 00095 // Function: CollisionPlane::output 00096 // Access: Public, Virtual 00097 // Description: 00098 //////////////////////////////////////////////////////////////////// 00099 void CollisionFloorMesh:: 00100 output(ostream &out) const { 00101 out << "cfloor"; 00102 } 00103 00104 //////////////////////////////////////////////////////////////////// 00105 // Function: CollisionFloorMesh::compute_internal_bounds 00106 // Access: Protected, Virtual 00107 // Description: 00108 //////////////////////////////////////////////////////////////////// 00109 PT(BoundingVolume) CollisionFloorMesh:: 00110 compute_internal_bounds() const { 00111 if (_vertices.empty()) { 00112 return new BoundingBox; 00113 } 00114 00115 Vertices::const_iterator pi = _vertices.begin(); 00116 LPoint3 p = (*pi); 00117 00118 LPoint3 x = p; 00119 LPoint3 n = p; 00120 00121 for (++pi; pi != _vertices.end(); ++pi) { 00122 p = *pi; 00123 00124 n.set(min(n[0], p[0]), 00125 min(n[1], p[1]), 00126 min(n[2], p[2])); 00127 x.set(max(x[0], p[0]), 00128 max(x[1], p[1]), 00129 max(x[2], p[2])); 00130 } 00131 00132 return new BoundingBox(n, x); 00133 } 00134 00135 //////////////////////////////////////////////////////////////////// 00136 // Function: CollisionFloorMesh::test_intersection_from_ray 00137 // Access: Public, Virtual 00138 // Description: must be a vertical Ray!!! 00139 //////////////////////////////////////////////////////////////////// 00140 PT(CollisionEntry) CollisionFloorMesh:: 00141 test_intersection_from_ray(const CollisionEntry &entry) const { 00142 const CollisionRay *ray; 00143 DCAST_INTO_R(ray, entry.get_from(), 0); 00144 LPoint3 from_origin = ray->get_origin() * entry.get_wrt_mat(); 00145 00146 double fx = from_origin[0]; 00147 double fy = from_origin[1]; 00148 00149 CollisionFloorMesh::Triangles::const_iterator ti; 00150 for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) { 00151 TriangleIndices tri = *ti; 00152 //First do a naive bounding box check on the triangle 00153 if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) { 00154 continue; 00155 } 00156 00157 //okay, there's a good chance we'll be colliding 00158 LPoint3 p0 = _vertices[tri.p1]; 00159 LPoint3 p1 = _vertices[tri.p2]; 00160 LPoint3 p2 = _vertices[tri.p3]; 00161 PN_stdfloat p0x = p0[0]; 00162 PN_stdfloat p0y = p0[1]; 00163 PN_stdfloat e0x, e0y, e1x, e1y, e2x, e2y; 00164 PN_stdfloat u, v; 00165 00166 e0x = fx - p0x; e0y = fy - p0y; 00167 e1x = p1[0] - p0x; e1y = p1[1] - p0y; 00168 e2x = p2[0] - p0x; e2y = p2[1] - p0y; 00169 if (e1x == 0.0) { 00170 if (e2x == 0.0) continue; 00171 u = e0x / e2x; 00172 if (u < 0.0 || u > 1.0) continue; 00173 if (e1y == 0) continue; 00174 v = (e0y - (e2y * u)) / e1y; 00175 if (v < 0.0) continue; 00176 } else { 00177 PN_stdfloat d = (e2y * e1x) - (e2x * e1y); 00178 if (d == 0.0) continue; 00179 u = ((e0y * e1x) - (e0x * e1y)) / d; 00180 if (u < 0.0 || u > 1.0) continue; 00181 v = (e0x - (e2x * u)) / e1x; 00182 if (v < 0.0) continue; 00183 } 00184 if (u + v <= 0.0 || u + v > 1.0) continue; 00185 //we collided!! 00186 PN_stdfloat mag = u + v; 00187 PN_stdfloat p0z = p0[2]; 00188 00189 PN_stdfloat uz = (p2[2] - p0z) * mag; 00190 PN_stdfloat vz = (p1[2] - p0z) * mag; 00191 PN_stdfloat finalz = p0z + vz + (((uz - vz) * u) / (u + v)); 00192 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00193 00194 new_entry->set_surface_normal(LPoint3(0, 0, 1)); 00195 new_entry->set_surface_point(LPoint3(fx, fy, finalz)); 00196 return new_entry; 00197 } 00198 return NULL; 00199 } 00200 00201 00202 //////////////////////////////////////////////////////////////////// 00203 // Function: CollisionFloorMesh::test_intersection_from_sphere 00204 // Access: Public, Virtual 00205 // Description: 00206 //////////////////////////////////////////////////////////////////// 00207 PT(CollisionEntry) CollisionFloorMesh:: 00208 test_intersection_from_sphere(const CollisionEntry &entry) const { 00209 const CollisionSphere *sphere; 00210 DCAST_INTO_R(sphere, entry.get_from(), 0); 00211 LPoint3 from_origin = sphere->get_center() * entry.get_wrt_mat(); 00212 00213 double fx = from_origin[0]; 00214 double fy = from_origin[1]; 00215 00216 PN_stdfloat fz = PN_stdfloat(from_origin[2]); 00217 PN_stdfloat rad = sphere->get_radius(); 00218 CollisionFloorMesh::Triangles::const_iterator ti; 00219 for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) { 00220 TriangleIndices tri = *ti; 00221 //First do a naive bounding box check on the triangle 00222 if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) { 00223 continue; 00224 } 00225 00226 //okay, there's a good chance we'll be colliding 00227 LPoint3 p0 = _vertices[tri.p1]; 00228 LPoint3 p1 = _vertices[tri.p2]; 00229 LPoint3 p2 = _vertices[tri.p3]; 00230 PN_stdfloat p0x = p0[0]; 00231 PN_stdfloat p0y = p0[1]; 00232 PN_stdfloat e0x, e0y, e1x, e1y, e2x, e2y; 00233 PN_stdfloat u, v; 00234 00235 e0x = fx - p0x; e0y = fy - p0y; 00236 e1x = p1[0] - p0x; e1y = p1[1] - p0y; 00237 e2x = p2[0] - p0x; e2y = p2[1] - p0y; 00238 if (e1x == 0.0) { 00239 if (e2x == 0.0) continue; 00240 u = e0x / e2x; 00241 if (u < 0.0 || u > 1.0) continue; 00242 if (e1y == 0) continue; 00243 v = (e0y - (e2y * u)) / e1y; 00244 if (v < 0.0) continue; 00245 } else { 00246 PN_stdfloat d = (e2y * e1x) - (e2x * e1y); 00247 if (d == 0.0) continue; 00248 u = ((e0y * e1x) - (e0x * e1y)) / d; 00249 if (u < 0.0 || u > 1.0) continue; 00250 v = (e0x - (e2x * u)) / e1x; 00251 if (v < 0.0) continue; 00252 } 00253 if (u + v <= 0.0 || u + v > 1.0) continue; 00254 //we collided!! 00255 PN_stdfloat mag = u + v; 00256 PN_stdfloat p0z = p0[2]; 00257 00258 PN_stdfloat uz = (p2[2] - p0z) * mag; 00259 PN_stdfloat vz = (p1[2] - p0z) * mag; 00260 PN_stdfloat finalz = p0z+vz+(((uz - vz) *u)/(u+v)); 00261 PN_stdfloat dz = fz - finalz; 00262 if(dz > rad) 00263 return NULL; 00264 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00265 00266 new_entry->set_surface_normal(LPoint3(0, 0, 1)); 00267 new_entry->set_surface_point(LPoint3(fx, fy, finalz)); 00268 return new_entry; 00269 } 00270 return NULL; 00271 } 00272 00273 00274 00275 //////////////////////////////////////////////////////////////////// 00276 // Function: CollisionFloorMesh::fill_viz_geom 00277 // Access: Protected, Virtual 00278 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00279 // for rendering this solid. 00280 //////////////////////////////////////////////////////////////////// 00281 void CollisionFloorMesh:: 00282 fill_viz_geom() { 00283 if (collide_cat.is_debug()) { 00284 collide_cat.debug() 00285 << "Recomputing viz for " << *this << "\n"; 00286 } 00287 00288 PT(GeomVertexData) vdata = new GeomVertexData 00289 ("collision", GeomVertexFormat::get_v3(), 00290 Geom::UH_static); 00291 GeomVertexWriter vertex(vdata, InternalName::get_vertex()); 00292 00293 00294 PT(GeomTriangles) mesh = new GeomTriangles(Geom::UH_static); 00295 PT(GeomLinestrips) wire = new GeomLinestrips(Geom::UH_static); 00296 Triangles::iterator ti; 00297 Vertices::iterator vi; 00298 for (vi = _vertices.begin(); vi != _vertices.end(); vi++) { 00299 LPoint3 vert = *vi; 00300 vertex.add_data3(vert); 00301 } 00302 for (ti = _triangles.begin(); ti != _triangles.end(); ++ti) { 00303 CollisionFloorMesh::TriangleIndices tri = *ti; 00304 mesh->add_vertex(tri.p1); 00305 mesh->add_vertex(tri.p2); 00306 mesh->add_vertex(tri.p3); 00307 wire->add_vertex(tri.p1); 00308 wire->add_vertex(tri.p2); 00309 wire->add_vertex(tri.p3); 00310 wire->add_vertex(tri.p1); 00311 wire->close_primitive(); 00312 mesh->close_primitive(); 00313 } 00314 00315 PT(Geom) geom = new Geom(vdata); 00316 PT(Geom) geom2 = new Geom(vdata); 00317 geom->add_primitive(mesh); 00318 geom2->add_primitive(wire); 00319 _viz_geom->add_geom(geom, ((CollisionFloorMesh *)this)->get_solid_viz_state()); 00320 _viz_geom->add_geom(geom2, ((CollisionFloorMesh *)this)->get_wireframe_viz_state()); 00321 00322 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state()); 00323 _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state()); 00324 } 00325 00326 //////////////////////////////////////////////////////////////////// 00327 // Function: CollisionFloorMesh::get_volume_pcollector 00328 // Access: Public, Virtual 00329 // Description: Returns a PStatCollector that is used to count the 00330 // number of bounding volume tests made against a solid 00331 // of this type in a given frame. 00332 //////////////////////////////////////////////////////////////////// 00333 PStatCollector &CollisionFloorMesh:: 00334 get_volume_pcollector() { 00335 return _volume_pcollector; 00336 } 00337 00338 //////////////////////////////////////////////////////////////////// 00339 // Function: CollisionFloorMesh::get_test_pcollector 00340 // Access: Public, Virtual 00341 // Description: Returns a PStatCollector that is used to count the 00342 // number of intersection tests made against a solid 00343 // of this type in a given frame. 00344 //////////////////////////////////////////////////////////////////// 00345 PStatCollector &CollisionFloorMesh:: 00346 get_test_pcollector() { 00347 return _test_pcollector; 00348 } 00349 00350 //////////////////////////////////////////////////////////////////// 00351 // Function: CollisionFloorMesh::write_datagram 00352 // Access: Public 00353 // Description: Function to write the important information in 00354 // the particular object to a Datagram 00355 //////////////////////////////////////////////////////////////////// 00356 void CollisionFloorMesh:: 00357 write_datagram(BamWriter *manager, Datagram &me) 00358 { 00359 CollisionSolid::write_datagram(manager, me); 00360 me.add_uint16(_vertices.size()); 00361 for (size_t i = 0; i < _vertices.size(); i++) { 00362 _vertices[i].write_datagram(me); 00363 } 00364 me.add_uint16(_triangles.size()); 00365 for (size_t i = 0; i < _triangles.size(); i++) { 00366 me.add_uint32(_triangles[i].p1); 00367 me.add_uint32(_triangles[i].p2); 00368 me.add_uint32(_triangles[i].p3); 00369 me.add_stdfloat(_triangles[i].min_x); 00370 me.add_stdfloat(_triangles[i].max_x); 00371 me.add_stdfloat(_triangles[i].min_y); 00372 me.add_stdfloat(_triangles[i].max_y); 00373 00374 } 00375 } 00376 00377 //////////////////////////////////////////////////////////////////// 00378 // Function: CollisionFloorMesh::fillin 00379 // Access: Protected 00380 // Description: Function that reads out of the datagram (or asks 00381 // manager to read) all of the data that is needed to 00382 // re-create this object and stores it in the appropiate 00383 // place 00384 //////////////////////////////////////////////////////////////////// 00385 void CollisionFloorMesh:: 00386 fillin(DatagramIterator& scan, BamReader* manager) 00387 { 00388 CollisionSolid::fillin(scan, manager); 00389 unsigned int num_verts = scan.get_uint16(); 00390 for (size_t i = 0; i < num_verts; i++) { 00391 LPoint3 vert; 00392 vert.read_datagram(scan); 00393 00394 _vertices.push_back(vert); 00395 } 00396 unsigned int num_tris = scan.get_uint16(); 00397 for (size_t i = 0; i < num_tris; i++) { 00398 CollisionFloorMesh::TriangleIndices tri; 00399 00400 tri.p1 = scan.get_uint32(); 00401 tri.p2 = scan.get_uint32(); 00402 tri.p3 = scan.get_uint32(); 00403 00404 tri.min_x=scan.get_stdfloat(); 00405 tri.max_x=scan.get_stdfloat(); 00406 tri.min_y=scan.get_stdfloat(); 00407 tri.max_y=scan.get_stdfloat(); 00408 _triangles.push_back(tri); 00409 } 00410 } 00411 00412 //////////////////////////////////////////////////////////////////// 00413 // Function: CollisionPolygon::make_CollisionPolygon 00414 // Access: Protected 00415 // Description: Factory method to generate a CollisionPolygon object 00416 //////////////////////////////////////////////////////////////////// 00417 TypedWritable* CollisionFloorMesh:: 00418 make_CollisionFloorMesh(const FactoryParams ¶ms) { 00419 CollisionFloorMesh *me = new CollisionFloorMesh; 00420 DatagramIterator scan; 00421 BamReader *manager; 00422 00423 parse_params(params, scan, manager); 00424 me->fillin(scan, manager); 00425 return me; 00426 } 00427 00428 //////////////////////////////////////////////////////////////////// 00429 // Function: CollisionPolygon::register_with_factory 00430 // Access: Public, Static 00431 // Description: Factory method to generate a CollisionPolygon object 00432 //////////////////////////////////////////////////////////////////// 00433 void CollisionFloorMesh:: 00434 register_with_read_factory() { 00435 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionFloorMesh); 00436 } 00437 00438 00439 //////////////////////////////////////////////////////////////////// 00440 // Function: CollisionFloorMesh::write 00441 // Access: Public, Virtual 00442 // Description: 00443 //////////////////////////////////////////////////////////////////// 00444 void CollisionFloorMesh:: 00445 write(ostream &out, int indent_level) const { 00446 indent(out, indent_level) << (*this) << "\n"; 00447 } 00448 00449 //////////////////////////////////////////////////////////////////// 00450 // Function: CollisionFloorMesh::add_triangle 00451 // Access: Published 00452 // Description: store a triangle for processing 00453 //////////////////////////////////////////////////////////////////// 00454 void CollisionFloorMesh:: 00455 add_triangle(unsigned int pointA, unsigned int pointB, unsigned int pointC) { 00456 CollisionFloorMesh::TriangleIndices tri; 00457 tri.p1 = pointA; 00458 tri.p2 = pointB; 00459 tri.p3 = pointC; 00460 LPoint3 v1 = _vertices[pointA]; 00461 LPoint3 v2 = _vertices[pointB]; 00462 LPoint3 v3 = _vertices[pointC]; 00463 00464 tri.min_x=min(min(v1[0],v2[0]),v3[0]); 00465 tri.max_x=max(max(v1[0],v2[0]),v3[0]); 00466 tri.min_y=min(min(v1[1],v2[1]),v3[1]); 00467 tri.max_y=max(max(v1[1],v2[1]),v3[1]); 00468 00469 _triangles.push_back(tri); 00470 }