00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00043
00044
00045
00046 CollisionSolid *CollisionFloorMesh::
00047 make_copy() {
00048 return new CollisionFloorMesh(*this);
00049 }
00050
00051
00052
00053
00054
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
00080
00081
00082
00083
00084
00085
00086 LPoint3 CollisionFloorMesh::
00087 get_collision_origin() const {
00088
00089
00090
00091 return LPoint3::origin();
00092 }
00093
00094
00095
00096
00097
00098
00099 void CollisionFloorMesh::
00100 output(ostream &out) const {
00101 out << "cfloor";
00102 }
00103
00104
00105
00106
00107
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
00137
00138
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
00153 if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
00154 continue;
00155 }
00156
00157
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
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
00204
00205
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
00222 if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
00223 continue;
00224 }
00225
00226
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
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
00277
00278
00279
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
00328
00329
00330
00331
00332
00333 PStatCollector &CollisionFloorMesh::
00334 get_volume_pcollector() {
00335 return _volume_pcollector;
00336 }
00337
00338
00339
00340
00341
00342
00343
00344
00345 PStatCollector &CollisionFloorMesh::
00346 get_test_pcollector() {
00347 return _test_pcollector;
00348 }
00349
00350
00351
00352
00353
00354
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
00379
00380
00381
00382
00383
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
00414
00415
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
00430
00431
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
00441
00442
00443
00444 void CollisionFloorMesh::
00445 write(ostream &out, int indent_level) const {
00446 indent(out, indent_level) << (*this) << "\n";
00447 }
00448
00449
00450
00451
00452
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 }