40 PStatCollector CollisionFloorMesh::_volume_pcollector(
"Collision Volumes:CollisionFloorMesh");
41 PStatCollector CollisionFloorMesh::_test_pcollector(
"Collision Tests:CollisionFloorMesh");
56 xform(
const LMatrix4 &mat) {
57 Vertices::iterator vi;
58 for (vi=_vertices.begin();vi!=_vertices.end();++vi) {
59 LPoint3 pt = (*vi) * mat;
60 (*vi).set(pt[0],pt[1],pt[2]);
62 Triangles::iterator ti;
63 for (ti=_triangles.begin();ti!=_triangles.end();++ti) {
64 CollisionFloorMesh::TriangleIndices tri = *ti;
65 LPoint3 v1 = _vertices[tri.p1];
66 LPoint3 v2 = _vertices[tri.p2];
67 LPoint3 v3 = _vertices[tri.p3];
69 tri.min_x=min(min(v1[0],v2[0]),v3[0]);
70 tri.max_x=max(max(v1[0],v2[0]),v3[0]);
71 tri.min_y=min(min(v1[1],v2[1]),v3[1]);
72 tri.max_y=max(max(v1[1],v2[1]),v3[1]);
74 CollisionSolid::xform(mat);
86 return LPoint3::origin();
92 void CollisionFloorMesh::
93 output(std::ostream &out)
const {
101 compute_internal_bounds()
const {
102 if (_vertices.empty()) {
106 Vertices::const_iterator pi = _vertices.begin();
112 for (++pi; pi != _vertices.end(); ++pi) {
115 n.set(min(n[0], p[0]),
118 x.set(max(x[0], p[0]),
132 DCAST_INTO_R(ray, entry.
get_from(),
nullptr);
133 LPoint3 from_origin = ray->get_origin() * entry.get_wrt_mat();
135 double fx = from_origin[0];
136 double fy = from_origin[1];
138 CollisionFloorMesh::Triangles::const_iterator ti;
139 for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) {
140 TriangleIndices tri = *ti;
142 if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
147 LPoint3 p0 = _vertices[tri.p1];
148 LPoint3 p1 = _vertices[tri.p2];
149 LPoint3 p2 = _vertices[tri.p3];
150 PN_stdfloat p0x = p0[0];
151 PN_stdfloat p0y = p0[1];
152 PN_stdfloat e0x, e0y, e1x, e1y, e2x, e2y;
155 e0x = fx - p0x; e0y = fy - p0y;
156 e1x = p1[0] - p0x; e1y = p1[1] - p0y;
157 e2x = p2[0] - p0x; e2y = p2[1] - p0y;
159 if (e2x == 0.0)
continue;
161 if (u < 0.0 || u > 1.0)
continue;
162 if (e1y == 0)
continue;
163 v = (e0y - (e2y * u)) / e1y;
164 if (v < 0.0)
continue;
166 PN_stdfloat d = (e2y * e1x) - (e2x * e1y);
167 if (d == 0.0)
continue;
168 u = ((e0y * e1x) - (e0x * e1y)) / d;
169 if (u < 0.0 || u > 1.0)
continue;
170 v = (e0x - (e2x * u)) / e1x;
171 if (v < 0.0)
continue;
173 if (u + v <= 0.0 || u + v > 1.0)
continue;
175 PN_stdfloat mag = u + v;
176 PN_stdfloat p0z = p0[2];
178 PN_stdfloat uz = (p2[2] - p0z) * mag;
179 PN_stdfloat vz = (p1[2] - p0z) * mag;
180 PN_stdfloat finalz = p0z + vz + (((uz - vz) * u) / (u + v));
183 new_entry->set_surface_normal(LPoint3(0, 0, 1));
184 new_entry->set_surface_point(LPoint3(fx, fy, finalz));
195 test_intersection_from_sphere(
const CollisionEntry &entry)
const {
197 DCAST_INTO_R(sphere, entry.
get_from(),
nullptr);
198 LPoint3 from_origin = sphere->get_center() * entry.get_wrt_mat();
200 double fx = from_origin[0];
201 double fy = from_origin[1];
203 PN_stdfloat fz = PN_stdfloat(from_origin[2]);
204 PN_stdfloat rad = sphere->get_radius();
205 CollisionFloorMesh::Triangles::const_iterator ti;
206 for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) {
207 TriangleIndices tri = *ti;
209 if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
214 LPoint3 p0 = _vertices[tri.p1];
215 LPoint3 p1 = _vertices[tri.p2];
216 LPoint3 p2 = _vertices[tri.p3];
217 PN_stdfloat p0x = p0[0];
218 PN_stdfloat p0y = p0[1];
219 PN_stdfloat e0x, e0y, e1x, e1y, e2x, e2y;
222 e0x = fx - p0x; e0y = fy - p0y;
223 e1x = p1[0] - p0x; e1y = p1[1] - p0y;
224 e2x = p2[0] - p0x; e2y = p2[1] - p0y;
226 if (e2x == 0.0)
continue;
228 if (u < 0.0 || u > 1.0)
continue;
229 if (e1y == 0)
continue;
230 v = (e0y - (e2y * u)) / e1y;
231 if (v < 0.0)
continue;
233 PN_stdfloat d = (e2y * e1x) - (e2x * e1y);
234 if (d == 0.0)
continue;
235 u = ((e0y * e1x) - (e0x * e1y)) / d;
236 if (u < 0.0 || u > 1.0)
continue;
237 v = (e0x - (e2x * u)) / e1x;
238 if (v < 0.0)
continue;
240 if (u + v <= 0.0 || u + v > 1.0)
continue;
242 PN_stdfloat mag = u + v;
243 PN_stdfloat p0z = p0[2];
245 PN_stdfloat uz = (p2[2] - p0z) * mag;
246 PN_stdfloat vz = (p1[2] - p0z) * mag;
247 PN_stdfloat finalz = p0z+vz+(((uz - vz) *u)/(u+v));
248 PN_stdfloat dz = fz - finalz;
253 new_entry->set_surface_normal(LPoint3(0, 0, 1));
254 new_entry->set_surface_point(LPoint3(fx, fy, finalz));
266 void CollisionFloorMesh::
268 if (collide_cat.is_debug()) {
270 <<
"Recomputing viz for " << *
this <<
"\n";
281 Triangles::iterator ti;
282 Vertices::iterator vi;
283 for (vi = _vertices.begin(); vi != _vertices.end(); vi++) {
285 vertex.add_data3(vert);
287 for (ti = _triangles.begin(); ti != _triangles.end(); ++ti) {
288 CollisionFloorMesh::TriangleIndices tri = *ti;
289 mesh->add_vertex(tri.p1);
290 mesh->add_vertex(tri.p2);
291 mesh->add_vertex(tri.p3);
292 wire->add_vertex(tri.p1);
293 wire->add_vertex(tri.p2);
294 wire->add_vertex(tri.p3);
295 wire->add_vertex(tri.p1);
296 wire->close_primitive();
297 mesh->close_primitive();
302 geom->add_primitive(mesh);
303 geom2->add_primitive(wire);
307 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
308 _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state());
317 return _volume_pcollector;
326 return _test_pcollector;
338 for (
size_t i = 0; i < _vertices.size(); i++) {
339 _vertices[i].write_datagram(me);
342 for (
size_t i = 0; i < _triangles.size(); i++) {
359 void CollisionFloorMesh::
362 CollisionSolid::fillin(scan, manager);
364 for (
size_t i = 0; i < num_verts; i++) {
366 vert.read_datagram(scan);
368 _vertices.push_back(vert);
371 for (
size_t i = 0; i < num_tris; i++) {
372 CollisionFloorMesh::TriangleIndices tri;
382 _triangles.push_back(tri);
396 me->fillin(scan, manager);
412 void CollisionFloorMesh::
413 write(std::ostream &out,
int indent_level)
const {
414 indent(out, indent_level) << (*this) <<
"\n";
421 add_triangle(
unsigned int pointA,
unsigned int pointB,
unsigned int pointC) {
422 CollisionFloorMesh::TriangleIndices tri;
426 LPoint3 v1 = _vertices[pointA];
427 LPoint3 v2 = _vertices[pointB];
428 LPoint3 v3 = _vertices[pointC];
430 tri.min_x=min(min(v1[0],v2[0]),v3[0]);
431 tri.max_x=max(max(v1[0],v2[0]),v3[0]);
432 tri.min_y=min(min(v1[1],v2[1]),v3[1]);
433 tri.max_y=max(max(v1[1],v2[1]),v3[1]);
435 _triangles.push_back(tri);