From ab922b135368bfd4ccaf0032a8e55c11e31fefe1 Mon Sep 17 00:00:00 2001 From: David Rose Date: Wed, 1 Oct 2003 18:26:34 +0000 Subject: [PATCH] support CollisionTube --- panda/src/egg2pg/eggLoader.cxx | 240 +++++++++++++++++++++++++++++---- panda/src/egg2pg/eggLoader.h | 2 + 2 files changed, 218 insertions(+), 24 deletions(-) diff --git a/panda/src/egg2pg/eggLoader.cxx b/panda/src/egg2pg/eggLoader.cxx index e2ec44a03b..6a27240718 100644 --- a/panda/src/egg2pg/eggLoader.cxx +++ b/panda/src/egg2pg/eggLoader.cxx @@ -60,12 +60,14 @@ #include "selectiveChildNode.h" #include "collisionNode.h" #include "collisionSphere.h" +#include "collisionTube.h" #include "collisionPlane.h" #include "collisionPolygon.h" #include "parametricCurve.h" #include "nurbsCurve.h" #include "classicNurbsCurve.h" #include "nurbsCurveInterface.h" +#include "look_at.h" #include #include @@ -1583,6 +1585,10 @@ make_collision_solids(EggGroup *start_group, EggGroup *egg_group, case EggGroup::CST_sphere: make_collision_sphere(egg_group, cnode, start_group->get_collide_flags()); break; + + case EggGroup::CST_tube: + make_collision_tube(egg_group, cnode, start_group->get_collide_flags()); + break; } if ((start_group->get_collide_flags() & EggGroup::CF_descend) != 0) { @@ -1697,17 +1703,8 @@ make_collision_sphere(EggGroup *egg_group, CollisionNode *cnode, for (vi = vertices.begin(); vi != vertices.end(); ++vi) { EggVertex *vtx = (*vi); - if (vtx->get_num_dimensions() == 3) { - center += vtx->get_pos3(); - num_vertices++; - - } else if (vtx->get_num_dimensions() == 4) { - LPoint4d p4 = vtx->get_pos4(); - if (p4[3] != 0.0) { - center += LPoint3d(p4[0], p4[1], p4[2]) / p4[3]; - num_vertices++; - } - } + center += vtx->get_pos3(); + num_vertices++; } if (num_vertices > 0) { @@ -1720,19 +1717,9 @@ make_collision_sphere(EggGroup *egg_group, CollisionNode *cnode, double radius2 = 0.0; for (vi = vertices.begin(); vi != vertices.end(); ++vi) { EggVertex *vtx = (*vi); - if (vtx->get_num_dimensions() == 3) { - LPoint3d p3 = vtx->get_pos3(); - LVector3d v = p3 * mat - center; - radius2 = max(radius2, v.length_squared()); - - } else if (vtx->get_num_dimensions() == 4) { - LPoint4d p4 = vtx->get_pos4(); - if (p4[3] != 0.0) { - LPoint3d p3 = LPoint3d(p4[0], p4[1], p4[2]) / p4[3]; - LVector3d v = p3 * mat - center; - radius2 = max(radius2, v.length_squared()); - } - } + LPoint3d p3 = vtx->get_pos3(); + LVector3d v = p3 * mat - center; + radius2 = max(radius2, v.length_squared()); } float radius = sqrtf(radius2); @@ -1744,6 +1731,208 @@ make_collision_sphere(EggGroup *egg_group, CollisionNode *cnode, } } +//////////////////////////////////////////////////////////////////// +// Function: EggLoader::make_collision_tube +// Access: Private +// Description: Creates a single CollisionTube corresponding +// to the polygons associated with this group. +//////////////////////////////////////////////////////////////////// +void EggLoader:: +make_collision_tube(EggGroup *egg_group, CollisionNode *cnode, + EggGroup::CollideFlags flags) { + EggGroup *geom_group = find_collision_geometry(egg_group); + if (geom_group != (EggGroup *)NULL) { + // Collect all of the vertices. + pset vertices; + + EggGroup::const_iterator ci; + for (ci = geom_group->begin(); ci != geom_group->end(); ++ci) { + if ((*ci)->is_of_type(EggPrimitive::get_class_type())) { + EggPrimitive *prim = DCAST(EggPrimitive, *ci); + EggPrimitive::const_iterator pi; + for (pi = prim->begin(); pi != prim->end(); ++pi) { + vertices.insert(*pi); + } + } + } + + // Now store the 3-d values in a vector for convenient access (and + // also determine the centroid). We compute this in node space. + size_t num_vertices = vertices.size(); + if (num_vertices != 0) { + LMatrix4d mat = egg_group->get_vertex_to_node(); + pvector vpos; + vpos.reserve(num_vertices); + + LPoint3d center(0.0, 0.0, 0.0); + pset::const_iterator vi; + for (vi = vertices.begin(); vi != vertices.end(); ++vi) { + EggVertex *vtx = (*vi); + LPoint3d pos = vtx->get_pos3() * mat; + vpos.push_back(pos); + center += vtx->get_pos3(); + } + center /= (double)num_vertices; + + // Now that we have the centroid, we have to try to figure out + // the cylinder's major axis. Start by finding a point farthest + // from the centroid. + size_t i; + double radius2 = 0.0; + LPoint3d far_a = center; + for (i = 0; i < num_vertices; i++) { + double dist2 = (vpos[i] - center).length_squared(); + if (dist2 > radius2) { + radius2 = dist2; + far_a = vpos[i]; + } + } + + // The point we have found above, far_a, must be one one of the + // endcaps. Now find another point, far_b, that is the farthest + // from far_a. This will be a point on the other endcap. + radius2 = 0.0; + LPoint3d far_b = center; + for (i = 0; i < num_vertices; i++) { + double dist2 = (vpos[i] - far_a).length_squared(); + if (dist2 > radius2) { + radius2 = dist2; + far_b = vpos[i]; + } + } + + // Now we have far_a and far_b, one point on each endcap. + // However, these points are not necessarily centered on the + // endcaps, so we haven't figured out the cylinder's axis yet + // (the line between far_a and far_b will probably pass through + // the cylinder at an angle). + + // So we still need to determine the full set of points in each + // endcap. To do this, we pass back through the set of points, + // categorizing each point into either "endcap a" or "endcap b". + // We also leave a hefty chunk of points in the middle + // uncategorized; this helps prevent us from getting a little + // bit lopsided with points near the middle that may appear to + // be closer to the wrong endcap. + LPoint3d cap_a_center(0.0, 0.0, 0.0); + LPoint3d cap_b_center(0.0, 0.0, 0.0); + int num_a = 0; + int num_b = 0; + + // This is the threshold length; points farther away from the + // center than this are deemed to be in one endcap or the other. + double center_length = (far_a - far_b).length() / 4.0; + double center_length2 = center_length * center_length; + + for (i = 0; i < num_vertices; i++) { + double dist2 = (vpos[i] - center).length_squared(); + if (dist2 > center_length2) { + // This point is farther away from the center than + // center_length; therefore it belongs in an endcap. + double dist_a2 = (vpos[i] - far_a).length_squared(); + double dist_b2 = (vpos[i] - far_b).length_squared(); + if (dist_a2 < dist_b2) { + // It's in endcap a. + cap_a_center += vpos[i]; + num_a++; + } else { + // It's in endcap b. + cap_b_center += vpos[i]; + num_b++; + } + } + } + + if (num_a > 0 && num_b > 0) { + cap_a_center /= (double)num_a; + cap_b_center /= (double)num_b; + + + // Now we finally have the major axis of the cylinder. + LVector3d axis = cap_b_center - cap_a_center; + axis.normalize(); + + // If the axis is *almost* parallel with a major axis, assume + // it is meant to be exactly parallel. + if (IS_THRESHOLD_ZERO(axis[0], 0.01)) { + axis[0] = 0.0; + } + if (IS_THRESHOLD_ZERO(axis[1], 0.01)) { + axis[1] = 0.0; + } + if (IS_THRESHOLD_ZERO(axis[2], 0.01)) { + axis[2] = 0.0; + } + axis.normalize(); + + // Transform all of the points so that the major axis is along + // the Y axis, and the origin is the center. This is very + // similar to the CollisionTube's idea of its canonical + // orientation (although not exactly the same, since it is + // centered on the origin instead of having point_a on the + // origin). It makes it easier to determine the length and + // radius of the cylinder. + LMatrix4d mat; + look_at(mat, axis, LVector3d(0.0, 0.0, 1.0), CS_zup_right); + mat.set_row(3, center); + LMatrix4d inv_mat; + inv_mat.invert_from(mat); + + for (i = 0; i < num_vertices; i++) { + vpos[i] = vpos[i] * inv_mat; + } + + double max_radius2 = 0.0; + + // Now determine the radius. + for (i = 0; i < num_vertices; i++) { + LVector2d v(vpos[i][0], vpos[i][2]); + double radius2 = v.length_squared(); + if (radius2 > max_radius2) { + max_radius2 = radius2; + } + } + + // And with the radius, we can determine the length. We need + // to know the radius first because we want the round endcaps + // to enclose all points. + double min_y = 0.0; + double max_y = 0.0; + + for (i = 0; i < num_vertices; i++) { + LVector2d v(vpos[i][0], vpos[i][2]); + double radius2 = v.length_squared(); + + if (vpos[i][1] < min_y) { + // Adjust the Y pos to account for the point's distance + // from the axis. + double factor = sqrt(max_radius2 - radius2); + min_y = min(min_y, vpos[i][1] + factor); + + } else if (vpos[i][1] > max_y) { + double factor = sqrt(max_radius2 - radius2); + max_y = max(max_y, vpos[i][1] - factor); + } + } + + double length = max_y - min_y; + double radius = sqrt(max_radius2); + + // Finally, we have everything we need to define the cylinder. + LVector3d half = axis * (length / 2.0); + LPoint3d point_a = center - half; + LPoint3d point_b = center + half; + + CollisionTube *cstube = + new CollisionTube(LCAST(float, point_a), LCAST(float, point_b), + radius); + apply_collision_flags(cstube, flags); + cnode->add_solid(cstube); + } + } + } +} + //////////////////////////////////////////////////////////////////// // Function: EggLoader::apply_collision_flags // Access: Private @@ -2028,6 +2217,9 @@ do_expand_object_type(EggGroup *egg_group, const pset &expanded, } else if (cmp_nocase_uh(object_type, "sphere") == 0) { egg_syntax = " { Sphere descend }"; + + } else if (cmp_nocase_uh(object_type, "tube") == 0) { + egg_syntax = " { Tube descend }"; } else if (cmp_nocase_uh(object_type, "trigger") == 0) { egg_syntax = " { Polyset descend intangible }"; diff --git a/panda/src/egg2pg/eggLoader.h b/panda/src/egg2pg/eggLoader.h index 676beef795..512a673ecb 100644 --- a/panda/src/egg2pg/eggLoader.h +++ b/panda/src/egg2pg/eggLoader.h @@ -112,6 +112,8 @@ private: EggGroup::CollideFlags flags); void make_collision_sphere(EggGroup *egg_group, CollisionNode *cnode, EggGroup::CollideFlags flags); + void make_collision_tube(EggGroup *egg_group, CollisionNode *cnode, + EggGroup::CollideFlags flags); void apply_collision_flags(CollisionSolid *solid, EggGroup::CollideFlags flags); EggGroup *find_collision_geometry(EggGroup *egg_group);