Panda3D

collisionFloorMesh.cxx

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 LMatrix4f &mat) {
00058 
00059   pvector<LPoint3f>::iterator vi;
00060   for (vi=_vertices.begin();vi!=_vertices.end();++vi) {
00061     LPoint3f pt = (*vi) * mat;
00062     (*vi).set(pt[0],pt[1],pt[2]);
00063   }
00064   pvector<CollisionFloorMesh::TriangleIndices>::iterator ti;
00065   for (ti=_triangles.begin();ti!=_triangles.end();++ti) {
00066     CollisionFloorMesh::TriangleIndices tri = *ti;
00067     LPoint3f v1 = _vertices[tri.p1];
00068     LPoint3f v2 = _vertices[tri.p2];
00069     LPoint3f v3 = _vertices[tri.p3];
00070     
00071     tri.min_x=min(min(v1[0],v2[0]),v3[0]);
00072     tri.max_x=max(max(v1[0],v2[0]),v3[0]);
00073     tri.min_y=min(min(v1[1],v2[1]),v3[1]);
00074     tri.max_y=max(max(v1[1],v2[1]),v3[1]);
00075   }
00076   CollisionSolid::xform(mat);
00077 }
00078 
00079 ////////////////////////////////////////////////////////////////////
00080 //     Function: CollisionPlane::get_collision_origin
00081 //       Access: Public, Virtual
00082 //  Description: Returns the point in space deemed to be the "origin"
00083 //               of the solid for collision purposes.  The closest
00084 //               intersection point to this origin point is considered
00085 //               to be the most significant.
00086 ////////////////////////////////////////////////////////////////////
00087 LPoint3f CollisionFloorMesh::
00088 get_collision_origin() const {
00089   // No real sensible origin exists for a plane.  We return 0, 0, 0,
00090   // without even bothering to ensure that that point exists on the
00091   // plane.
00092   return LPoint3f::origin();
00093 }
00094 
00095 ////////////////////////////////////////////////////////////////////
00096 //     Function: CollisionPlane::output
00097 //       Access: Public, Virtual
00098 //  Description:
00099 ////////////////////////////////////////////////////////////////////
00100 void CollisionFloorMesh::
00101 output(ostream &out) const {
00102   out << "cfloor";
00103 }
00104 
00105 ////////////////////////////////////////////////////////////////////
00106 //     Function: CollisionFloorMesh::compute_internal_bounds
00107 //       Access: Protected, Virtual
00108 //  Description:
00109 ////////////////////////////////////////////////////////////////////
00110 PT(BoundingVolume) CollisionFloorMesh::
00111 compute_internal_bounds() const {
00112   if (_vertices.empty()) {
00113     return new BoundingBox;
00114   }
00115 
00116 
00117   pvector<LPoint3f>::const_iterator pi = _vertices.begin();
00118   LPoint3f p = (*pi);
00119 
00120   LPoint3f x = p;
00121   LPoint3f n = p;
00122 
00123   for (++pi; pi != _vertices.end(); ++pi) {
00124     p = *pi;
00125 
00126     n.set(min(n[0], p[0]),
00127           min(n[1], p[1]),
00128           min(n[2], p[2]));
00129     x.set(max(x[0], p[0]),
00130           max(x[1], p[1]),
00131           max(x[2], p[2]));
00132   }
00133 
00134   return new BoundingBox(n, x);
00135 }
00136 
00137 ////////////////////////////////////////////////////////////////////
00138 //     Function: CollisionFloorMesh::test_intersection_from_ray
00139 //       Access: Public, Virtual
00140 //  Description: must be a vertical Ray!!!
00141 ////////////////////////////////////////////////////////////////////
00142 PT(CollisionEntry) CollisionFloorMesh::
00143 test_intersection_from_ray(const CollisionEntry &entry) const {
00144   const CollisionRay *ray;
00145   DCAST_INTO_R(ray, entry.get_from(), 0);
00146   LPoint3f from_origin = ray->get_origin() * entry.get_wrt_mat();
00147   
00148   double fx=from_origin[0];
00149   double fy=from_origin[1];
00150 
00151   CollisionFloorMesh::Triangles::const_iterator ti;
00152   for (ti=_triangles.begin();ti< _triangles.end();++ti) {
00153     TriangleIndices tri = *ti;
00154     //First do a naive bounding box check on the triangle
00155     if(fx<tri.min_x  || fx>=tri.max_x || fy<tri.min_y || fy>=tri.max_y) 
00156       continue;
00157     
00158     //okay, there's a good chance we'll be colliding
00159     LPoint3f p0=_vertices[tri.p1];
00160     LPoint3f p1=_vertices[tri.p2];
00161     LPoint3f p2=_vertices[tri.p3];
00162     float p0x = p0[0];
00163     float p0y = p0[1];
00164     float e0x,e0y,e1x,e1y,e2x,e2y;
00165     float u,v;
00166 
00167     e0x = fx - p0x; e0y = fy - p0y;
00168     e1x = p1[0] - p0x; e1y = p1[1] - p0y;
00169     e2x = p2[0] - p0x; e2y = p2[1] - p0y;
00170     if (e1x==0) {  
00171       if (e2x == 0) continue; 
00172       u = e0x/e2x;
00173       if (u<0 || u>1) continue;     
00174       if (e1y == 0) continue;
00175       v = ( e0y - (e2y*u))/e1y;
00176       if (v<0) continue; 
00177     } else {
00178       float d = (e2y * e1x)-(e2x * e1y);
00179       if (d==0) continue; 
00180       u = ((e0y * e1x) - (e0x * e1y))/d;
00181       if (u<0 || u>1) continue;
00182       v = (e0x - (e2x * u)) / e1x;
00183       if (v<0) continue;
00184       if (u + v > 1) continue; 
00185     }
00186     //we collided!!
00187     float mag = u + v;
00188     float p0z = p0[2];
00189    
00190     float uz = (p2[2] - p0z) *  mag;
00191     float vz = (p1[2] - p0z) *  mag;
00192     float finalz = p0z+vz+(((uz - vz) *u)/(u+v));
00193     PT(CollisionEntry) new_entry = new CollisionEntry(entry);    
00194     
00195     new_entry->set_surface_normal(LPoint3f(0,0,1));
00196     new_entry->set_surface_point(LPoint3f(fx,fy,finalz));
00197     return new_entry;
00198   }
00199   return NULL;
00200 }
00201 
00202 
00203 ////////////////////////////////////////////////////////////////////
00204 //     Function: CollisionFloorMesh::test_intersection_from_ray
00205 //       Access: Public, Virtual
00206 //  Description: must be a vertical Ray!!!
00207 ////////////////////////////////////////////////////////////////////
00208 PT(CollisionEntry) CollisionFloorMesh::
00209 test_intersection_from_sphere(const CollisionEntry &entry) const {
00210   const CollisionSphere *sphere;
00211   DCAST_INTO_R(sphere, entry.get_from(), 0);
00212   LPoint3f from_origin = sphere->get_center() * entry.get_wrt_mat();
00213   
00214   double fx = from_origin[0];
00215   double fy = from_origin[1];
00216   
00217   float  fz = float(from_origin[2]);
00218   float rad = sphere->get_radius();
00219   CollisionFloorMesh::Triangles::const_iterator ti;
00220   for (ti=_triangles.begin();ti< _triangles.end();++ti) {
00221     TriangleIndices tri = *ti;
00222     //First do a naive bounding box check on the triangle
00223     if(fx<tri.min_x  || fx>=tri.max_x || fy<tri.min_y || fy>=tri.max_y) 
00224       continue;
00225     
00226     //okay, there's a good chance we'll be colliding
00227     LPoint3f p0=_vertices[tri.p1];
00228     LPoint3f p1=_vertices[tri.p2];
00229     LPoint3f p2=_vertices[tri.p3];
00230     float p0x = p0[0];
00231     float p0y = p0[1];
00232     float e0x,e0y,e1x,e1y,e2x,e2y;
00233     float 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) {  
00239       if (e2x == 0) continue; 
00240       u = e0x/e2x;
00241       if (u<0 || u>1) continue;     
00242       if (e1y == 0) continue;
00243       v = ( e0y - (e2y*u))/e1y;
00244       if (v<0) continue; 
00245     } else {
00246       float d = (e2y * e1x)-(e2x * e1y);
00247       if (d==0) continue; 
00248       u = ((e0y * e1x) - (e0x * e1y))/d;
00249       if (u<0 || u>1) continue;
00250       v = (e0x - (e2x * u)) / e1x;
00251       if (v<0) continue;
00252       if (u + v > 1) continue; 
00253     }
00254     //we collided!!
00255     float mag = u + v;
00256     float p0z = p0[2];
00257    
00258     float uz = (p2[2] - p0z) *  mag;
00259     float vz = (p1[2] - p0z) *  mag;
00260     float finalz = p0z+vz+(((uz - vz) *u)/(u+v));
00261     float 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(LPoint3f(0,0,1));
00267     new_entry->set_surface_point(LPoint3f(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   pvector<CollisionFloorMesh::TriangleIndices>::iterator ti;
00297   pvector<LPoint3f>::iterator vi;
00298   for (vi = _vertices.begin(); vi != _vertices.end(); vi++) {
00299     LPoint3f vert = *vi;
00300     vertex.add_data3f(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_float32(_triangles[i].min_x);
00370     me.add_float32(_triangles[i].max_x);
00371     me.add_float32(_triangles[i].min_y);
00372     me.add_float32(_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     LPoint3f 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_float32();
00405     tri.max_x=scan.get_float32();
00406     tri.min_y=scan.get_float32();
00407     tri.max_y=scan.get_float32();
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 &params) {
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   LPoint3f v1 = _vertices[pointA];
00461   LPoint3f v2 = _vertices[pointB];
00462   LPoint3f 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 }
 All Classes Functions Variables Enumerations