Panda3D
 All Classes Functions Variables Enumerations
collisionFloorMesh.cxx
1 // Filename: collisionPlane.cxx
2 // Created by: drose (25Apr00)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 
16 #include "collisionFloorMesh.h"
17 #include "collisionHandler.h"
18 #include "collisionEntry.h"
19 #include "collisionSphere.h"
20 #include "collisionLine.h"
21 #include "collisionRay.h"
22 #include "collisionSegment.h"
23 #include "config_collide.h"
24 #include "pointerToArray.h"
25 #include "geomNode.h"
26 #include "geom.h"
27 #include "datagram.h"
28 #include "datagramIterator.h"
29 #include "bamReader.h"
30 #include "bamWriter.h"
31 #include "boundingBox.h"
32 #include "boundingPlane.h"
33 #include "geom.h"
34 #include "geomTriangles.h"
35 #include "geomLinestrips.h"
36 #include "geomVertexWriter.h"
37 #include <algorithm>
38 PStatCollector CollisionFloorMesh::_volume_pcollector("Collision Volumes:CollisionFloorMesh");
39 PStatCollector CollisionFloorMesh::_test_pcollector("Collision Tests:CollisionFloorMesh");
40 TypeHandle CollisionFloorMesh::_type_handle;
41 
42 ////////////////////////////////////////////////////////////////////
43 // Function: CollisionPlane::make_copy
44 // Access: Public, Virtual
45 // Description:
46 ////////////////////////////////////////////////////////////////////
47 CollisionSolid *CollisionFloorMesh::
48 make_copy() {
49  return new CollisionFloorMesh(*this);
50 }
51 
52 ////////////////////////////////////////////////////////////////////
53 // Function: CollisionPlane::xform
54 // Access: Public, Virtual
55 // Description: Transforms the solid by the indicated matrix.
56 ////////////////////////////////////////////////////////////////////
58 xform(const LMatrix4 &mat) {
59  Vertices::iterator vi;
60  for (vi=_vertices.begin();vi!=_vertices.end();++vi) {
61  LPoint3 pt = (*vi) * mat;
62  (*vi).set(pt[0],pt[1],pt[2]);
63  }
64  Triangles::iterator ti;
65  for (ti=_triangles.begin();ti!=_triangles.end();++ti) {
67  LPoint3 v1 = _vertices[tri.p1];
68  LPoint3 v2 = _vertices[tri.p2];
69  LPoint3 v3 = _vertices[tri.p3];
70 
71  tri.min_x=min(min(v1[0],v2[0]),v3[0]);
72  tri.max_x=max(max(v1[0],v2[0]),v3[0]);
73  tri.min_y=min(min(v1[1],v2[1]),v3[1]);
74  tri.max_y=max(max(v1[1],v2[1]),v3[1]);
75  }
77 }
78 
79 ////////////////////////////////////////////////////////////////////
80 // Function: CollisionPlane::get_collision_origin
81 // Access: Public, Virtual
82 // Description: Returns the point in space deemed to be the "origin"
83 // of the solid for collision purposes. The closest
84 // intersection point to this origin point is considered
85 // to be the most significant.
86 ////////////////////////////////////////////////////////////////////
89  // No real sensible origin exists for a plane. We return 0, 0, 0,
90  // without even bothering to ensure that that point exists on the
91  // plane.
92  return LPoint3::origin();
93 }
94 
95 ////////////////////////////////////////////////////////////////////
96 // Function: CollisionPlane::output
97 // Access: Public, Virtual
98 // Description:
99 ////////////////////////////////////////////////////////////////////
100 void CollisionFloorMesh::
101 output(ostream &out) const {
102  out << "cfloor";
103 }
104 
105 ////////////////////////////////////////////////////////////////////
106 // Function: CollisionFloorMesh::compute_internal_bounds
107 // Access: Protected, Virtual
108 // Description:
109 ////////////////////////////////////////////////////////////////////
111 compute_internal_bounds() const {
112  if (_vertices.empty()) {
113  return new BoundingBox;
114  }
115 
116  Vertices::const_iterator pi = _vertices.begin();
117  LPoint3 p = (*pi);
118 
119  LPoint3 x = p;
120  LPoint3 n = p;
121 
122  for (++pi; pi != _vertices.end(); ++pi) {
123  p = *pi;
124 
125  n.set(min(n[0], p[0]),
126  min(n[1], p[1]),
127  min(n[2], p[2]));
128  x.set(max(x[0], p[0]),
129  max(x[1], p[1]),
130  max(x[2], p[2]));
131  }
132 
133  return new BoundingBox(n, x);
134 }
135 
136 ////////////////////////////////////////////////////////////////////
137 // Function: CollisionFloorMesh::test_intersection_from_ray
138 // Access: Public, Virtual
139 // Description: must be a vertical Ray!!!
140 ////////////////////////////////////////////////////////////////////
142 test_intersection_from_ray(const CollisionEntry &entry) const {
143  const CollisionRay *ray;
144  DCAST_INTO_R(ray, entry.get_from(), 0);
145  LPoint3 from_origin = ray->get_origin() * entry.get_wrt_mat();
146 
147  double fx = from_origin[0];
148  double fy = from_origin[1];
149 
150  CollisionFloorMesh::Triangles::const_iterator ti;
151  for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) {
152  TriangleIndices tri = *ti;
153  //First do a naive bounding box check on the triangle
154  if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
155  continue;
156  }
157 
158  //okay, there's a good chance we'll be colliding
159  LPoint3 p0 = _vertices[tri.p1];
160  LPoint3 p1 = _vertices[tri.p2];
161  LPoint3 p2 = _vertices[tri.p3];
162  PN_stdfloat p0x = p0[0];
163  PN_stdfloat p0y = p0[1];
164  PN_stdfloat e0x, e0y, e1x, e1y, e2x, e2y;
165  PN_stdfloat u, v;
166 
167  e0x = fx - p0x; e0y = fy - p0y;
168  e1x = p1[0] - p0x; e1y = p1[1] - p0y;
169  e2x = p2[0] - p0x; e2y = p2[1] - p0y;
170  if (e1x == 0.0) {
171  if (e2x == 0.0) continue;
172  u = e0x / e2x;
173  if (u < 0.0 || u > 1.0) continue;
174  if (e1y == 0) continue;
175  v = (e0y - (e2y * u)) / e1y;
176  if (v < 0.0) continue;
177  } else {
178  PN_stdfloat d = (e2y * e1x) - (e2x * e1y);
179  if (d == 0.0) continue;
180  u = ((e0y * e1x) - (e0x * e1y)) / d;
181  if (u < 0.0 || u > 1.0) continue;
182  v = (e0x - (e2x * u)) / e1x;
183  if (v < 0.0) continue;
184  }
185  if (u + v <= 0.0 || u + v > 1.0) continue;
186  //we collided!!
187  PN_stdfloat mag = u + v;
188  PN_stdfloat p0z = p0[2];
189 
190  PN_stdfloat uz = (p2[2] - p0z) * mag;
191  PN_stdfloat vz = (p1[2] - p0z) * mag;
192  PN_stdfloat finalz = p0z + vz + (((uz - vz) * u) / (u + v));
193  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
194 
195  new_entry->set_surface_normal(LPoint3(0, 0, 1));
196  new_entry->set_surface_point(LPoint3(fx, fy, finalz));
197  return new_entry;
198  }
199  return NULL;
200 }
201 
202 
203 ////////////////////////////////////////////////////////////////////
204 // Function: CollisionFloorMesh::test_intersection_from_sphere
205 // Access: Public, Virtual
206 // Description:
207 ////////////////////////////////////////////////////////////////////
209 test_intersection_from_sphere(const CollisionEntry &entry) const {
210  const CollisionSphere *sphere;
211  DCAST_INTO_R(sphere, entry.get_from(), 0);
212  LPoint3 from_origin = sphere->get_center() * entry.get_wrt_mat();
213 
214  double fx = from_origin[0];
215  double fy = from_origin[1];
216 
217  PN_stdfloat fz = PN_stdfloat(from_origin[2]);
218  PN_stdfloat rad = sphere->get_radius();
219  CollisionFloorMesh::Triangles::const_iterator ti;
220  for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) {
221  TriangleIndices tri = *ti;
222  //First do a naive bounding box check on the triangle
223  if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
224  continue;
225  }
226 
227  //okay, there's a good chance we'll be colliding
228  LPoint3 p0 = _vertices[tri.p1];
229  LPoint3 p1 = _vertices[tri.p2];
230  LPoint3 p2 = _vertices[tri.p3];
231  PN_stdfloat p0x = p0[0];
232  PN_stdfloat p0y = p0[1];
233  PN_stdfloat e0x, e0y, e1x, e1y, e2x, e2y;
234  PN_stdfloat u, v;
235 
236  e0x = fx - p0x; e0y = fy - p0y;
237  e1x = p1[0] - p0x; e1y = p1[1] - p0y;
238  e2x = p2[0] - p0x; e2y = p2[1] - p0y;
239  if (e1x == 0.0) {
240  if (e2x == 0.0) continue;
241  u = e0x / e2x;
242  if (u < 0.0 || u > 1.0) continue;
243  if (e1y == 0) continue;
244  v = (e0y - (e2y * u)) / e1y;
245  if (v < 0.0) continue;
246  } else {
247  PN_stdfloat d = (e2y * e1x) - (e2x * e1y);
248  if (d == 0.0) continue;
249  u = ((e0y * e1x) - (e0x * e1y)) / d;
250  if (u < 0.0 || u > 1.0) continue;
251  v = (e0x - (e2x * u)) / e1x;
252  if (v < 0.0) continue;
253  }
254  if (u + v <= 0.0 || u + v > 1.0) continue;
255  //we collided!!
256  PN_stdfloat mag = u + v;
257  PN_stdfloat p0z = p0[2];
258 
259  PN_stdfloat uz = (p2[2] - p0z) * mag;
260  PN_stdfloat vz = (p1[2] - p0z) * mag;
261  PN_stdfloat finalz = p0z+vz+(((uz - vz) *u)/(u+v));
262  PN_stdfloat dz = fz - finalz;
263  if(dz > rad)
264  return NULL;
265  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
266 
267  new_entry->set_surface_normal(LPoint3(0, 0, 1));
268  new_entry->set_surface_point(LPoint3(fx, fy, finalz));
269  return new_entry;
270  }
271  return NULL;
272 }
273 
274 
275 
276 ////////////////////////////////////////////////////////////////////
277 // Function: CollisionFloorMesh::fill_viz_geom
278 // Access: Protected, Virtual
279 // Description: Fills the _viz_geom GeomNode up with Geoms suitable
280 // for rendering this solid.
281 ////////////////////////////////////////////////////////////////////
282 void CollisionFloorMesh::
283 fill_viz_geom() {
284  if (collide_cat.is_debug()) {
285  collide_cat.debug()
286  << "Recomputing viz for " << *this << "\n";
287  }
288 
289  PT(GeomVertexData) vdata = new GeomVertexData
290  ("collision", GeomVertexFormat::get_v3(),
291  Geom::UH_static);
292  GeomVertexWriter vertex(vdata, InternalName::get_vertex());
293 
294 
295  PT(GeomTriangles) mesh = new GeomTriangles(Geom::UH_static);
296  PT(GeomLinestrips) wire = new GeomLinestrips(Geom::UH_static);
297  Triangles::iterator ti;
298  Vertices::iterator vi;
299  for (vi = _vertices.begin(); vi != _vertices.end(); vi++) {
300  LPoint3 vert = *vi;
301  vertex.add_data3(vert);
302  }
303  for (ti = _triangles.begin(); ti != _triangles.end(); ++ti) {
305  mesh->add_vertex(tri.p1);
306  mesh->add_vertex(tri.p2);
307  mesh->add_vertex(tri.p3);
308  wire->add_vertex(tri.p1);
309  wire->add_vertex(tri.p2);
310  wire->add_vertex(tri.p3);
311  wire->add_vertex(tri.p1);
312  wire->close_primitive();
313  mesh->close_primitive();
314  }
315 
316  PT(Geom) geom = new Geom(vdata);
317  PT(Geom) geom2 = new Geom(vdata);
318  geom->add_primitive(mesh);
319  geom2->add_primitive(wire);
320  _viz_geom->add_geom(geom, ((CollisionFloorMesh *)this)->get_solid_viz_state());
321  _viz_geom->add_geom(geom2, ((CollisionFloorMesh *)this)->get_wireframe_viz_state());
322 
323  _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
324  _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state());
325 }
326 
327 ////////////////////////////////////////////////////////////////////
328 // Function: CollisionFloorMesh::get_volume_pcollector
329 // Access: Public, Virtual
330 // Description: Returns a PStatCollector that is used to count the
331 // number of bounding volume tests made against a solid
332 // of this type in a given frame.
333 ////////////////////////////////////////////////////////////////////
335 get_volume_pcollector() {
336  return _volume_pcollector;
337 }
338 
339 ////////////////////////////////////////////////////////////////////
340 // Function: CollisionFloorMesh::get_test_pcollector
341 // Access: Public, Virtual
342 // Description: Returns a PStatCollector that is used to count the
343 // number of intersection tests made against a solid
344 // of this type in a given frame.
345 ////////////////////////////////////////////////////////////////////
348  return _test_pcollector;
349 }
350 
351 ////////////////////////////////////////////////////////////////////
352 // Function: CollisionFloorMesh::write_datagram
353 // Access: Public
354 // Description: Function to write the important information in
355 // the particular object to a Datagram
356 ////////////////////////////////////////////////////////////////////
359 {
360  CollisionSolid::write_datagram(manager, me);
361  me.add_uint16(_vertices.size());
362  for (size_t i = 0; i < _vertices.size(); i++) {
363  _vertices[i].write_datagram(me);
364  }
365  me.add_uint16(_triangles.size());
366  for (size_t i = 0; i < _triangles.size(); i++) {
367  me.add_uint32(_triangles[i].p1);
368  me.add_uint32(_triangles[i].p2);
369  me.add_uint32(_triangles[i].p3);
370  me.add_stdfloat(_triangles[i].min_x);
371  me.add_stdfloat(_triangles[i].max_x);
372  me.add_stdfloat(_triangles[i].min_y);
373  me.add_stdfloat(_triangles[i].max_y);
374 
375  }
376 }
377 
378 ////////////////////////////////////////////////////////////////////
379 // Function: CollisionFloorMesh::fillin
380 // Access: Protected
381 // Description: Function that reads out of the datagram (or asks
382 // manager to read) all of the data that is needed to
383 // re-create this object and stores it in the appropiate
384 // place
385 ////////////////////////////////////////////////////////////////////
386 void CollisionFloorMesh::
387 fillin(DatagramIterator& scan, BamReader* manager)
388 {
389  CollisionSolid::fillin(scan, manager);
390  unsigned int num_verts = scan.get_uint16();
391  for (size_t i = 0; i < num_verts; i++) {
392  LPoint3 vert;
393  vert.read_datagram(scan);
394 
395  _vertices.push_back(vert);
396  }
397  unsigned int num_tris = scan.get_uint16();
398  for (size_t i = 0; i < num_tris; i++) {
400 
401  tri.p1 = scan.get_uint32();
402  tri.p2 = scan.get_uint32();
403  tri.p3 = scan.get_uint32();
404 
405  tri.min_x=scan.get_stdfloat();
406  tri.max_x=scan.get_stdfloat();
407  tri.min_y=scan.get_stdfloat();
408  tri.max_y=scan.get_stdfloat();
409  _triangles.push_back(tri);
410  }
411 }
412 
413 ////////////////////////////////////////////////////////////////////
414 // Function: CollisionPolygon::make_CollisionPolygon
415 // Access: Protected
416 // Description: Factory method to generate a CollisionPolygon object
417 ////////////////////////////////////////////////////////////////////
421  DatagramIterator scan;
422  BamReader *manager;
423 
424  parse_params(params, scan, manager);
425  me->fillin(scan, manager);
426  return me;
427 }
428 
429 ////////////////////////////////////////////////////////////////////
430 // Function: CollisionPolygon::register_with_factory
431 // Access: Public, Static
432 // Description: Factory method to generate a CollisionPolygon object
433 ////////////////////////////////////////////////////////////////////
437 }
438 
439 
440 ////////////////////////////////////////////////////////////////////
441 // Function: CollisionFloorMesh::write
442 // Access: Public, Virtual
443 // Description:
444 ////////////////////////////////////////////////////////////////////
445 void CollisionFloorMesh::
446 write(ostream &out, int indent_level) const {
447  indent(out, indent_level) << (*this) << "\n";
448 }
449 
450 ////////////////////////////////////////////////////////////////////
451 // Function: CollisionFloorMesh::add_triangle
452 // Access: Published
453 // Description: store a triangle for processing
454 ////////////////////////////////////////////////////////////////////
456 add_triangle(unsigned int pointA, unsigned int pointB, unsigned int pointC) {
458  tri.p1 = pointA;
459  tri.p2 = pointB;
460  tri.p3 = pointC;
461  LPoint3 v1 = _vertices[pointA];
462  LPoint3 v2 = _vertices[pointB];
463  LPoint3 v3 = _vertices[pointC];
464 
465  tri.min_x=min(min(v1[0],v2[0]),v3[0]);
466  tri.max_x=max(max(v1[0],v2[0]),v3[0]);
467  tri.min_y=min(min(v1[1],v2[1]),v3[1]);
468  tri.max_y=max(max(v1[1],v2[1]),v3[1]);
469 
470  _triangles.push_back(tri);
471 }
void add_triangle(unsigned int pointA, unsigned int pointB, unsigned int pointC)
store a triangle for processing
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
An axis-aligned bounding box; that is, a minimum and maximum coordinate triple.
Definition: boundingBox.h:31
An infinite ray, with a specific origin and direction.
Definition: collisionRay.h:31
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
Definition: bamReader.h:122
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
Definition: lvecBase3.h:1373
The abstract base class for all things that can collide with other things in the world, and all the things they can collide with (except geometry).
Base class for objects that can be written to and read from Bam files.
Definition: typedWritable.h:37
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
Definition: bamWriter.h:73
A spherical collision volume or object.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
static const LPoint3f & origin(CoordinateSystem cs=CS_default)
Returns the origin of the indicated coordinate system.
Definition: lpoint3.h:438
PN_uint32 get_uint32()
Extracts an unsigned 32-bit integer.
PN_uint16 get_uint16()
Extracts an unsigned 16-bit integer.
CollisionFloorMesh()
This is only for the convenience of CollisionPolygon.
static TypedWritable * make_CollisionFloorMesh(const FactoryParams &params)
Factory method to generate a CollisionPolygon object.
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
Definition: datagram.I:240
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
A lightweight class that represents a single element that may be timed and/or counted via stats...
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the &quot;origin&quot; of the solid for collision purposes.
This is a 4-by-4 transform matrix.
Definition: lmatrix.h:451
Defines a single collision event.
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
A container for geometry primitives.
Definition: geom.h:58
An instance of this class is passed to the Factory when requesting it to do its business and construc...
Definition: factoryParams.h:40
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
This object represents a solid made entirely of triangles, which will only be tested again z axis ali...
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
Definition: factory.I:90
void add_uint16(PN_uint16 value)
Adds an unsigned 16-bit integer to the datagram.
Definition: datagram.I:181
static void register_with_read_factory()
Factory method to generate a CollisionPolygon object.
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
Definition: bamReader.I:213
void add_uint32(PN_uint32 value)
Adds an unsigned 32-bit integer to the datagram.
Definition: datagram.I:192
Defines a series of line strips.
Defines a series of disconnected triangles.
Definition: geomTriangles.h:25
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
A class to retrieve the individual data elements previously stored in a Datagram. ...
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
Definition: datagram.h:43