Panda3D
 All Classes Functions Variables Enumerations
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 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 &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   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 }
 All Classes Functions Variables Enumerations