Panda3D

collisionBox.cxx

00001 // Filename: collisionBox.cxx
00002 // Created by:  amith tudur (31Jul09)
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 "collisionDSSolid.h"
00017 #include "collisionBox.h"
00018 #include "collisionLine.h"
00019 #include "collisionRay.h"
00020 #include "collisionSegment.h"
00021 #include "collisionHandler.h"
00022 #include "collisionEntry.h"
00023 #include "config_collide.h"
00024 #include "boundingBox.h"
00025 #include "datagram.h"
00026 #include "datagramIterator.h"
00027 #include "bamReader.h"
00028 #include "bamWriter.h"
00029 #include "nearly_zero.h"
00030 #include "cmath.h"
00031 #include "mathNumbers.h"
00032 #include "geom.h"
00033 #include "geomTristrips.h"
00034 #include "geomVertexWriter.h"
00035 #include "config_mathutil.h"
00036 #include "dcast.h"
00037 
00038 #include <math.h>
00039 
00040 PStatCollector CollisionBox::_volume_pcollector(
00041   "Collision Volumes:CollisionBox");
00042 PStatCollector CollisionBox::_test_pcollector(
00043   "Collision Tests:CollisionBox");
00044 TypeHandle CollisionBox::_type_handle;
00045 
00046 const int CollisionBox::plane_def[6][4] = {
00047   {0, 4, 5, 1},
00048   {4, 6, 7, 5},
00049   {6, 2, 3, 7},
00050   {2, 0, 1, 3},
00051   {1, 5, 7, 3},
00052   {2, 6, 4, 0},
00053 };
00054 
00055 ////////////////////////////////////////////////////////////////////
00056 //     Function: CollisionBox::make_copy
00057 //       Access: Public, Virtual
00058 //  Description:
00059 ////////////////////////////////////////////////////////////////////
00060 CollisionSolid *CollisionBox::
00061 make_copy() {
00062   return new CollisionBox(*this);
00063 }
00064 
00065 ////////////////////////////////////////////////////////////////////
00066 //     Function: CollisionBox::setup_box
00067 //       Access: Public, Virtual
00068 //  Description: Compute parameters for each of the box's sides
00069 ////////////////////////////////////////////////////////////////////
00070 void CollisionBox::
00071 setup_box(){
00072   for(int plane = 0; plane < 6; plane++) {
00073     LPoint3f array[4];
00074     array[0] = get_point(plane_def[plane][0]);
00075     array[1] = get_point(plane_def[plane][1]);
00076     array[2] = get_point(plane_def[plane][2]);
00077     array[3] = get_point(plane_def[plane][3]);
00078     setup_points(array, array+4, plane);
00079   }
00080 }
00081 
00082 ////////////////////////////////////////////////////////////////////
00083 //     Function: CollisionBox::setup_points
00084 //       Access: Private
00085 //  Description: Computes the plane and 2d projection of points that
00086 //               make up this side          
00087 ////////////////////////////////////////////////////////////////////
00088 void CollisionBox::
00089 setup_points(const LPoint3f *begin, const LPoint3f *end, int plane) {
00090   int num_points = end - begin;
00091   nassertv(num_points >= 3);
00092 
00093   _points[plane].clear();
00094 
00095   // Construct a matrix that rotates the points from the (X,0,Z) plane
00096   // into the 3-d plane.
00097   LMatrix4f to_3d_mat;
00098   calc_to_3d_mat(to_3d_mat, plane);
00099 
00100   // And the inverse matrix rotates points from 3-d space into the 2-d
00101   // plane.
00102   _to_2d_mat[plane].invert_from(to_3d_mat);
00103 
00104   // Now project all of the points onto the 2-d plane.
00105 
00106   const LPoint3f *pi;
00107   for (pi = begin; pi != end; ++pi) {
00108     LPoint3f point = (*pi) * _to_2d_mat[plane];
00109     _points[plane].push_back(PointDef(point[0], point[2]));
00110   }
00111 
00112   nassertv(_points[plane].size() >= 3);
00113 
00114 #ifndef NDEBUG
00115   /*
00116   // Now make sure the points define a convex polygon.
00117   if (is_concave()) {
00118     collide_cat.error() << "Invalid concave CollisionPolygon defined:\n";
00119     const LPoint3f *pi;
00120     for (pi = begin; pi != end; ++pi) {
00121       collide_cat.error(false) << "  " << (*pi) << "\n";
00122     }
00123     collide_cat.error(false)
00124       << "  normal " << normal << " with length " << normal.length() << "\n";
00125     _points.clear();
00126   }
00127   */
00128 #endif
00129 
00130   compute_vectors(_points[plane]);
00131 }
00132 
00133 ////////////////////////////////////////////////////////////////////
00134 //     Function: CollisionBox::test_intersection
00135 //       Access: Public, Virtual
00136 //  Description: First Dispatch point for box as a FROM object
00137 ////////////////////////////////////////////////////////////////////
00138 PT(CollisionEntry) CollisionBox::
00139 test_intersection(const CollisionEntry &entry) const {
00140   return entry.get_into()->test_intersection_from_box(entry);
00141 }
00142 
00143 ////////////////////////////////////////////////////////////////////
00144 //     Function: CollisionBox::xform
00145 //       Access: Public, Virtual
00146 //  Description: Transforms the solid by the indicated matrix.
00147 ////////////////////////////////////////////////////////////////////
00148 void CollisionBox::
00149 xform(const LMatrix4f &mat) {
00150   _center = _center * mat;
00151   for(int v = 0; v < 8; v++)
00152      _vertex[v] = _vertex[v] * mat;
00153   for(int p = 0; p < 6 ; p++)
00154      _planes[p] = set_plane(p);
00155   _x = _vertex[0].get_x()-_center.get_x(); 
00156   _y = _vertex[0].get_y()-_center.get_y();
00157   _z = _vertex[0].get_z()-_center.get_z();
00158   _radius = sqrt( _x*_x + _y*_y + _z*_z );
00159   setup_box();
00160   mark_viz_stale();
00161   mark_internal_bounds_stale();
00162 }
00163 
00164 ////////////////////////////////////////////////////////////////////
00165 //     Function: CollisionBox::get_collision_origin
00166 //       Access: Public, Virtual
00167 //  Description: Returns the point in space deemed to be the "origin"
00168 //               of the solid for collision purposes.  The closest
00169 //               intersection point to this origin point is considered
00170 //               to be the most significant.
00171 ////////////////////////////////////////////////////////////////////
00172 LPoint3f CollisionBox::
00173 get_collision_origin() const {
00174   return _center;
00175 }
00176 
00177 ////////////////////////////////////////////////////////////////////
00178 //     Function: CollisionBox::get_min
00179 //       Access: Public, Virtual
00180 //  Description: 
00181 ////////////////////////////////////////////////////////////////////
00182 LPoint3f CollisionBox::
00183 get_min() const {
00184   return _min;
00185 }
00186 
00187 ////////////////////////////////////////////////////////////////////
00188 //     Function: CollisionBox::get_max
00189 //       Access: Public, Virtual
00190 //  Description: 
00191 ////////////////////////////////////////////////////////////////////
00192 LPoint3f CollisionBox::
00193 get_max() const {
00194   return _max;
00195 }
00196 
00197 ////////////////////////////////////////////////////////////////////
00198 //     Function: CollisionBox::get_approx_center
00199 //       Access: Public, Virtual
00200 //  Description: 
00201 ////////////////////////////////////////////////////////////////////
00202 LPoint3f CollisionBox::
00203 get_approx_center() const {
00204   return (_min + _max) * 0.5f;
00205 }
00206 
00207 ////////////////////////////////////////////////////////////////////
00208 //     Function: CollisionBox::get_volume_pcollector
00209 //       Access: Public, Virtual
00210 //  Description: Returns a PStatCollector that is used to count the
00211 //               number of bounding volume tests made against a solid
00212 //               of this type in a given frame.
00213 ////////////////////////////////////////////////////////////////////
00214 PStatCollector &CollisionBox::
00215 get_volume_pcollector() {
00216   return _volume_pcollector;
00217 }
00218 
00219 ////////////////////////////////////////////////////////////////////
00220 //     Function: CollisionBox::get_test_pcollector
00221 //       Access: Public, Virtual
00222 //  Description: Returns a PStatCollector that is used to count the
00223 //               number of intersection tests made against a solid
00224 //               of this type in a given frame.
00225 ////////////////////////////////////////////////////////////////////
00226 PStatCollector &CollisionBox::
00227 get_test_pcollector() {
00228   return _test_pcollector;
00229 }
00230 
00231 ////////////////////////////////////////////////////////////////////
00232 //     Function: CollisionBox::output
00233 //       Access: Public, Virtual
00234 //  Description:
00235 ////////////////////////////////////////////////////////////////////
00236 void CollisionBox::
00237 output(ostream &out) const {
00238 }
00239 
00240 ////////////////////////////////////////////////////////////////////
00241 //     Function: CollisionBox::compute_internal_bounds
00242 //       Access: Protected, Virtual
00243 //  Description: Sphere is chosen as the Bounding Volume type for 
00244 //               speed and efficiency
00245 ////////////////////////////////////////////////////////////////////
00246 PT(BoundingVolume) CollisionBox::
00247 compute_internal_bounds() const {
00248   return new BoundingSphere(_center, _radius);
00249 }
00250 
00251 ////////////////////////////////////////////////////////////////////
00252 //     Function: CollisionBox::test_intersection_from_sphere
00253 //       Access: Public, Virtual
00254 //  Description: Double dispatch point for sphere as FROM object
00255 ////////////////////////////////////////////////////////////////////
00256 PT(CollisionEntry) CollisionBox::
00257 test_intersection_from_sphere(const CollisionEntry &entry) const {
00258 
00259   const CollisionSphere *sphere;
00260   DCAST_INTO_R(sphere, entry.get_from(), 0);
00261 
00262   CPT(TransformState) wrt_space = entry.get_wrt_space();
00263   CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
00264 
00265   const LMatrix4f &wrt_mat = wrt_space->get_mat();
00266 
00267   LPoint3f orig_center = sphere->get_center() * wrt_mat;
00268   LPoint3f from_center = orig_center;
00269   bool moved_from_center = false;
00270   float t = 1.0f;
00271   LPoint3f contact_point(from_center);
00272   float actual_t = 1.0f;
00273 
00274   LVector3f from_radius_v =
00275     LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00276   float from_radius_2 = from_radius_v.length_squared();
00277   float from_radius = csqrt(from_radius_2);
00278 
00279   int ip;
00280   float max_dist,dist;
00281   bool intersect;
00282   Planef plane;
00283   LVector3f normal;
00284   
00285   for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) {
00286     plane = get_plane( ip );
00287     if (_points[ip].size() < 3) {
00288       continue;
00289     }
00290     if (wrt_prev_space != wrt_space) {
00291         // If we have a delta between the previous position and the
00292         // current position, we use that to determine some more properties
00293         // of the collision.
00294         LPoint3f b = from_center;
00295         LPoint3f a = sphere->get_center() * wrt_prev_space->get_mat();
00296         LVector3f delta = b - a;
00297 
00298         // First, there is no collision if the "from" object is definitely
00299         // moving in the same direction as the plane's normal.
00300         float dot = delta.dot(plane.get_normal());
00301         if (dot > 0.1f) {
00302           continue; // no intersection
00303         }
00304 
00305         if (IS_NEARLY_ZERO(dot)) {
00306           // If we're moving parallel to the plane, the sphere is tested
00307           // at its final point.  Leave it as it is.
00308 
00309         } else {
00310           // Otherwise, we're moving into the plane; the sphere is tested
00311           // at the point along its path that is closest to intersecting
00312           // the plane.  This may be the actual intersection point, or it
00313           // may be the starting point or the final point.
00314           // dot is equal to the (negative) magnitude of 'delta' along the
00315           // direction of the plane normal
00316           // t = ratio of (distance from start pos to plane) to (distance
00317           // from start pos to end pos), along axis of plane normal
00318           float dist_to_p = plane.dist_to_plane(a);
00319           t = (dist_to_p / -dot);
00320             
00321           // also compute the actual contact point and time of contact
00322           // for handlers that need it
00323           actual_t = ((dist_to_p - from_radius) / -dot);
00324           actual_t = min(1.0f, max(0.0f, actual_t));
00325           contact_point = a + (actual_t * delta);
00326 
00327           if (t >= 1.0f) {
00328           // Leave it where it is.
00329 
00330           } else if (t < 0.0f) {
00331              from_center = a;
00332              moved_from_center = true;
00333           } else {
00334                 from_center = a + t * delta;
00335                 moved_from_center = true;
00336           }
00337         }
00338      }
00339 
00340     normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal();
00341      
00342     #ifndef NDEBUG
00343     /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
00344       std::cout
00345       << "polygon within " << entry.get_into_node_path()
00346       << " has normal " << normal << " of length " << normal.length()
00347       << "\n";
00348       normal.normalize();
00349     }*/
00350     #endif
00351 
00352     // The nearest point within the plane to our center is the
00353     // intersection of the line (center, center - normal) with the plane.
00354     
00355     if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) {
00356         // No intersection with plane?  This means the plane's effective
00357         // normal was within the plane itself.  A useless polygon.
00358         continue;
00359      }
00360 
00361     if (dist > from_radius || dist < -from_radius) {
00362       // No intersection with the plane.
00363       continue;
00364     }
00365 
00366     LPoint2f p = to_2d(from_center - dist * plane.get_normal(), ip);
00367     float edge_dist = 0.0f;
00368 
00369     const ClipPlaneAttrib *cpa = entry.get_into_clip_planes();
00370     if (cpa != (ClipPlaneAttrib *)NULL) {
00371       // We have a clip plane; apply it.
00372       Points new_points;
00373       if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) {
00374         // All points are behind the clip plane; just do the default
00375         // test.
00376         edge_dist = dist_to_polygon(p, _points[ip]);
00377       } else if (new_points.empty()) {
00378         // The polygon is completely clipped.
00379         continue;
00380       } else {
00381         // Test against the clipped polygon.
00382         edge_dist = dist_to_polygon(p, new_points);
00383       }
00384     } else {
00385       // No clip plane is in effect.  Do the default test. 
00386       edge_dist = dist_to_polygon(p, _points[ip]);
00387     }
00388 
00389     // Now we have edge_dist, which is the distance from the sphere
00390     // center to the nearest edge of the polygon, within the polygon's
00391     // plane. edge_dist<0 means the point is within the polygon.
00392     if(edge_dist < 0) {
00393       intersect = true;
00394       continue;
00395     }
00396 
00397     if((edge_dist > 0) && 
00398       ((edge_dist * edge_dist + dist * dist) > from_radius_2)) {
00399       // No intersection; the circle is outside the polygon.
00400       continue;
00401     }
00402 
00403     // The sphere appears to intersect the polygon.  If the edge is less
00404     // than from_radius away, the sphere may be resting on an edge of
00405     // the polygon.  Determine how far the center of the sphere must
00406     // remain from the plane, based on its distance from the nearest
00407     // edge.
00408 
00409     max_dist = from_radius;
00410     if (edge_dist >= 0.0f) {
00411       float max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, 0.0f);
00412       max_dist = csqrt(max_dist_2);
00413     }
00414 
00415     if (dist > max_dist) {
00416       // There's no intersection: the sphere is hanging off the edge.
00417       continue;
00418     }
00419     intersect = true;
00420   }
00421   if( !intersect )
00422     return NULL;
00423 
00424   if (collide_cat.is_debug()) {
00425     collide_cat.debug()
00426       << "intersection detected from " << entry.get_from_node_path()
00427       << " into " << entry.get_into_node_path() << "\n";
00428   }
00429   
00430   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00431 
00432   float into_depth = max_dist - dist;
00433   if (moved_from_center) {
00434     // We have to base the depth of intersection on the sphere's final
00435     // resting point, not the point from which we tested the
00436     // intersection.
00437     float orig_dist;
00438     plane.intersects_line(orig_dist, orig_center, -normal);
00439     into_depth = max_dist - orig_dist;
00440   }
00441 
00442   new_entry->set_surface_normal(normal);
00443   new_entry->set_surface_point(from_center - normal * dist);
00444   new_entry->set_interior_point(from_center - normal * (dist + into_depth));
00445   new_entry->set_contact_pos(contact_point);
00446   new_entry->set_contact_normal(plane.get_normal());
00447   new_entry->set_t(actual_t);
00448 
00449   return new_entry;
00450 }
00451 
00452 
00453 ////////////////////////////////////////////////////////////////////
00454 //     Function: CollisionBox::test_intersection_from_ray
00455 //       Access: Public, Virtual
00456 //  Description: Double dispatch point for ray as a FROM object
00457 ////////////////////////////////////////////////////////////////////
00458 PT(CollisionEntry) CollisionBox::
00459 test_intersection_from_ray(const CollisionEntry &entry) const {
00460   const CollisionRay *ray;
00461   DCAST_INTO_R(ray, entry.get_from(), 0);
00462   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00463 
00464   LPoint3f from_origin = ray->get_origin() * wrt_mat;
00465   LVector3f from_direction = ray->get_direction() * wrt_mat;
00466 
00467   int i, j;
00468   float t, near_t;
00469   bool intersect;
00470   Planef plane;
00471   Planef near_plane; 
00472 
00473   //Returns the details about the first plane of the box that the ray
00474   //intersects.
00475   for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
00476     plane = get_plane(i);
00477 
00478     if (!plane.intersects_line(t, from_origin, from_direction)) {
00479       // No intersection.  The ray is parallel to the plane.
00480       continue;
00481     }
00482 
00483     if (t < 0.0f) {
00484       // The intersection point is before the start of the ray, and so
00485       // the ray is entirely in front of the plane.
00486       continue;
00487     }
00488     LPoint3f plane_point = from_origin + t * from_direction;
00489     LPoint2f p = to_2d(plane_point, i);
00490 
00491     if (!point_is_inside(p, _points[i])){
00492       continue;
00493     }
00494     intersect = true;
00495     if(j) {
00496       if(t < near_t) {
00497         near_plane = plane;
00498         near_t = t;
00499       }
00500     }
00501     else {
00502       near_plane = plane;
00503       near_t = t;
00504     }
00505     ++j;
00506   }
00507    
00508 
00509   if(!intersect) {
00510      //No intersection with ANY of the box's planes has been detected
00511      return NULL;
00512   }
00513 
00514   if (collide_cat.is_debug()) {
00515     collide_cat.debug()
00516       << "intersection detected from " << entry.get_from_node_path()
00517       << " into " << entry.get_into_node_path() << "\n";
00518   }
00519 
00520   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00521 
00522   LPoint3f into_intersection_point = from_origin + near_t * from_direction;
00523 
00524   LVector3f normal =
00525     (has_effective_normal() && ray->get_respect_effective_normal()) 
00526     ? get_effective_normal() : near_plane.get_normal();
00527 
00528   new_entry->set_surface_normal(normal);
00529   new_entry->set_surface_point(into_intersection_point);
00530 
00531   return new_entry;
00532 }
00533 
00534 
00535 ////////////////////////////////////////////////////////////////////
00536 //     Function: CollisionBox::test_intersection_from_segment
00537 //       Access: Public, Virtual
00538 //  Description: Double dispatch point for segment as a FROM object
00539 ////////////////////////////////////////////////////////////////////
00540 PT(CollisionEntry) CollisionBox::
00541 test_intersection_from_segment(const CollisionEntry &entry) const {
00542   const CollisionSegment *seg;
00543   DCAST_INTO_R(seg, entry.get_from(), 0);
00544   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00545 
00546   LPoint3f from_origin = seg->get_point_a() * wrt_mat;
00547   LPoint3f from_extent = seg->get_point_b() * wrt_mat;
00548   LVector3f from_direction = from_extent - from_origin;
00549 
00550   int i, j;
00551   float t, near_t;
00552   bool intersect;
00553   Planef plane;
00554   Planef near_plane; 
00555 
00556   //Returns the details about the first plane of the box that the
00557   //segment intersects.
00558   for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) {
00559     plane = get_plane(i);
00560 
00561     if (!plane.intersects_line(t, from_origin, from_direction)) {
00562       // No intersection.  The segment is parallel to the plane.
00563       continue;
00564     }
00565 
00566     if (t < 0.0f || t > 1.0f) {
00567       // The intersection point is before the start of the segment,
00568       // or after the end of the segment, so the segment is either
00569       // entirely in front of or behind the plane.
00570       continue;
00571     }
00572     LPoint3f plane_point = from_origin + t * from_direction;
00573     LPoint2f p = to_2d(plane_point, i);
00574 
00575     if (!point_is_inside(p, _points[i])){
00576       continue;
00577     }
00578     intersect = true;
00579     if(j) {
00580       if(t < near_t) {
00581         near_plane = plane;
00582         near_t = t;
00583       }
00584     }
00585     else {
00586       near_plane = plane;
00587       near_t = t;
00588     }
00589     ++j;
00590   }
00591    
00592 
00593   if(!intersect) {
00594      //No intersection with ANY of the box's planes has been detected
00595      return NULL;
00596   }
00597 
00598   if (collide_cat.is_debug()) {
00599     collide_cat.debug()
00600       << "intersection detected from " << entry.get_from_node_path()
00601       << " into " << entry.get_into_node_path() << "\n";
00602   }
00603 
00604   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00605 
00606   LPoint3f into_intersection_point = from_origin + near_t * from_direction;
00607 
00608   LVector3f normal =
00609     (has_effective_normal() && seg->get_respect_effective_normal()) 
00610     ? get_effective_normal() : near_plane.get_normal();
00611 
00612   new_entry->set_surface_normal(normal);
00613   new_entry->set_surface_point(into_intersection_point);
00614 
00615   return new_entry;
00616 }
00617 
00618 
00619 ////////////////////////////////////////////////////////////////////
00620 //     Function: CollisionBox::test_intersection_from_box
00621 //       Access: Public, Virtual
00622 //  Description: Double dispatch point for box as a FROM object
00623 //               NOT Implemented.
00624 ////////////////////////////////////////////////////////////////////
00625 PT(CollisionEntry) CollisionBox::
00626 test_intersection_from_box(const CollisionEntry &entry) const {
00627   return NULL;
00628 }
00629 
00630 
00631 ////////////////////////////////////////////////////////////////////
00632 //     Function: CollisionBox::fill_viz_geom
00633 //       Access: Protected, Virtual
00634 //  Description: Fills the _viz_geom GeomNode up with Geoms suitable
00635 //               for rendering this solid.
00636 ////////////////////////////////////////////////////////////////////
00637 void CollisionBox::
00638 fill_viz_geom() {
00639   if (collide_cat.is_debug()) {
00640     collide_cat.debug()
00641       << "Recomputing viz for " << *this << "\n";
00642   }
00643 
00644   PT(GeomVertexData) vdata = new GeomVertexData
00645     ("collision", GeomVertexFormat::get_v3(),
00646      Geom::UH_static);
00647   GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00648   
00649   for(int i = 0; i < 6; i++) {
00650     for(int j = 0; j < 4; j++)
00651       vertex.add_data3f(get_point(plane_def[i][j]));
00652 
00653     PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
00654     body->add_consecutive_vertices(i*4, 4);
00655     body->close_primitive();
00656 
00657     PT(Geom) geom = new Geom(vdata);
00658     geom->add_primitive(body);
00659 
00660     _viz_geom->add_geom(geom, get_solid_viz_state());
00661     _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
00662   }
00663 }
00664 
00665 ////////////////////////////////////////////////////////////////////
00666 //     Function: CollisionBox::apply_clip_plane
00667 //       Access: Private
00668 //  Description: Clips the polygon by all of the clip planes named in
00669 //               the clip plane attribute and fills new_points up with
00670 //               the resulting points.
00671 //
00672 //               The return value is true if the set of points is
00673 //               unmodified (all points are behind all the clip
00674 //               planes), or false otherwise.
00675 ////////////////////////////////////////////////////////////////////
00676 bool CollisionBox::
00677 apply_clip_plane(CollisionBox::Points &new_points, 
00678                  const ClipPlaneAttrib *cpa,
00679                  const TransformState *net_transform, int plane_no) const {
00680   bool all_in = true;
00681 
00682   int num_planes = cpa->get_num_on_planes();
00683   bool first_plane = true;
00684 
00685   for (int i = 0; i < num_planes; i++) {
00686     NodePath plane_path = cpa->get_on_plane(i);
00687     PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node());
00688     if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) {
00689       CPT(TransformState) new_transform = 
00690         net_transform->invert_compose(plane_path.get_net_transform());
00691       
00692       Planef plane = plane_node->get_plane() * new_transform->get_mat();
00693       if (first_plane) {
00694         first_plane = false;
00695         if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) {
00696           all_in = false;
00697         }
00698       } else {
00699         Points last_points;
00700         last_points.swap(new_points);
00701         if (!clip_polygon(new_points, last_points, plane, plane_no)) {
00702           all_in = false;
00703         }
00704       }
00705     }
00706   }
00707 
00708   if (!all_in) {
00709     compute_vectors(new_points);
00710   }
00711 
00712   return all_in;
00713 }
00714 ////////////////////////////////////////////////////////////////////
00715 //     Function: CollisionBox::clip_polygon
00716 //       Access: Private
00717 //  Description: Clips the source_points of the polygon by the
00718 //               indicated clipping plane, and modifies new_points to
00719 //               reflect the new set of clipped points (but does not
00720 //               compute the vectors in new_points).
00721 //
00722 //               The return value is true if the set of points is
00723 //               unmodified (all points are behind the clip plane), or
00724 //               false otherwise.
00725 ////////////////////////////////////////////////////////////////////
00726 bool CollisionBox::
00727 clip_polygon(CollisionBox::Points &new_points, 
00728              const CollisionBox::Points &source_points,
00729              const Planef &plane, int plane_no) const {
00730   new_points.clear();
00731   if (source_points.empty()) {
00732     return true;
00733   }
00734 
00735   LPoint3f from3d;
00736   LVector3f delta3d;
00737   if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) {
00738     // The clipping plane is parallel to the polygon.  The polygon is
00739     // either all in or all out.
00740     if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) {
00741       // A point within the polygon is behind the clipping plane: the
00742       // polygon is all in.
00743       new_points = source_points;
00744       return true;
00745     }
00746     return false;
00747   }
00748 
00749   // Project the line of intersection into the 2-d plane.  Now we have
00750   // a 2-d clipping line.
00751   LPoint2f from2d = to_2d(from3d,plane_no);
00752   LVector2f delta2d = to_2d(delta3d,plane_no);
00753 
00754   float a = -delta2d[1];
00755   float b = delta2d[0];
00756   float c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0];
00757 
00758   // Now walk through the points.  Any point on the left of our line
00759   // gets removed, and the line segment clipped at the point of
00760   // intersection.
00761 
00762   // We might increase the number of vertices by as many as 1, if the
00763   // plane clips off exactly one corner.  (We might also decrease the
00764   // number of vertices, or keep them the same number.)
00765   new_points.reserve(source_points.size() + 1);
00766 
00767   LPoint2f last_point = source_points.back()._p;
00768   bool last_is_in = !is_right(last_point - from2d, delta2d);
00769   bool all_in = last_is_in;
00770   Points::const_iterator pi;
00771   for (pi = source_points.begin(); pi != source_points.end(); ++pi) {
00772     const LPoint2f &this_point = (*pi)._p;
00773     bool this_is_in = !is_right(this_point - from2d, delta2d);
00774 
00775     // There appears to be a compiler bug in gcc 4.0: we need to
00776     // extract this comparison outside of the if statement.
00777     bool crossed_over = (this_is_in != last_is_in);
00778     if (crossed_over) {
00779       // We have just crossed over the clipping line.  Find the point
00780       // of intersection.
00781       LVector2f d = this_point - last_point;
00782       float denom = (a * d[0] + b * d[1]);
00783       if (denom != 0.0) {
00784         float t = -(a * last_point[0] + b * last_point[1] + c) / denom;
00785         LPoint2f p = last_point + t * d;
00786 
00787         new_points.push_back(PointDef(p[0], p[1]));
00788         last_is_in = this_is_in;
00789       }
00790     } 
00791 
00792     if (this_is_in) {
00793       // We are behind the clipping line.  Keep the point.
00794       new_points.push_back(PointDef(this_point[0], this_point[1]));
00795     } else {
00796       all_in = false;
00797     }
00798 
00799     last_point = this_point;
00800   }
00801 
00802   return all_in;
00803 }
00804 
00805 
00806 ////////////////////////////////////////////////////////////////////
00807 //     Function: CollisionBox::
00808 //       Access: Private
00809 //  Description: Returns the linear distance from the 2-d point to the
00810 //               nearest part of the polygon defined by the points
00811 //               vector.  The result is negative if the point is
00812 //               within the polygon.
00813 ////////////////////////////////////////////////////////////////////
00814 float CollisionBox::
00815 dist_to_polygon(const LPoint2f &p, const CollisionBox::Points &points) const {
00816 
00817   // We know that that the polygon is convex and is defined with the
00818   // points in counterclockwise order.  Therefore, we simply compare
00819   // the signed distance to each line segment; we ignore any negative
00820   // values, and take the minimum of all the positive values.  
00821 
00822   // If all values are negative, the point is within the polygon; we
00823   // therefore return an arbitrary negative result.
00824   
00825   bool got_dist = false;
00826   float best_dist = -1.0f;
00827 
00828   size_t num_points = points.size();
00829   for (size_t i = 0; i < num_points - 1; ++i) {
00830     float d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p,
00831                                    points[i]._v);
00832     if (d >= 0.0f) {
00833       if (!got_dist || d < best_dist) {
00834         best_dist = d;
00835         got_dist = true;
00836       }
00837     }
00838   }
00839 
00840   float d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p,
00841                                  points[num_points - 1]._v);
00842   if (d >= 0.0f) {
00843     if (!got_dist || d < best_dist) {
00844       best_dist = d;
00845       got_dist = true;
00846     }
00847   }
00848 
00849   return best_dist;
00850 }
00851 
00852 ////////////////////////////////////////////////////////////////////
00853 //     Function: CollisionBox::dist_to_line_segment
00854 //       Access: Private, Static
00855 //  Description: Returns the linear distance of p to the line segment
00856 //               defined by f and t, where v = (t - f).normalize().
00857 //               The result is negative if p is left of the line,
00858 //               positive if it is right of the line.  If the result
00859 //               is positive, it is constrained by endpoints of the
00860 //               line segment (i.e. the result might be larger than it
00861 //               would be for a straight distance-to-line test).  If
00862 //               the result is negative, we don't bother.
00863 ////////////////////////////////////////////////////////////////////
00864 float CollisionBox::
00865 dist_to_line_segment(const LPoint2f &p,
00866                      const LPoint2f &f, const LPoint2f &t,
00867                      const LVector2f &v) {
00868   LVector2f v1 = (p - f);
00869   float d = (v1[0] * v[1] - v1[1] * v[0]);
00870   if (d < 0.0f) {
00871     return d;
00872   }
00873 
00874   // Compute the nearest point on the line.
00875   LPoint2f q = p + LVector2f(-v[1], v[0]) * d;
00876 
00877   // Now constrain that point to the line segment.
00878   if (v[0] > 0.0f) {
00879     // X+
00880     if (v[1] > 0.0f) {
00881       // Y+
00882       if (v[0] > v[1]) {
00883         // X-dominant.
00884         if (q[0] < f[0]) {
00885           return (p - f).length();
00886         } if (q[0] > t[0]) {
00887           return (p - t).length();
00888         } else {
00889           return d;
00890         }
00891       } else {
00892         // Y-dominant.
00893         if (q[1] < f[1]) {
00894           return (p - f).length();
00895         } if (q[1] > t[1]) {
00896           return (p - t).length();
00897         } else {
00898           return d;
00899         }
00900       }
00901     } else {
00902       // Y-
00903       if (v[0] > -v[1]) {
00904         // X-dominant.
00905         if (q[0] < f[0]) {
00906           return (p - f).length();
00907         } if (q[0] > t[0]) {
00908           return (p - t).length();
00909         } else {
00910           return d;
00911         }
00912       } else {
00913         // Y-dominant.
00914         if (q[1] > f[1]) {
00915           return (p - f).length();
00916         } if (q[1] < t[1]) {
00917           return (p - t).length();
00918         } else {
00919           return d;
00920         }
00921       }
00922     }
00923   } else {
00924     // X-
00925     if (v[1] > 0.0f) {
00926       // Y+
00927       if (-v[0] > v[1]) {
00928         // X-dominant.
00929         if (q[0] > f[0]) {
00930           return (p - f).length();
00931         } if (q[0] < t[0]) {
00932           return (p - t).length();
00933         } else {
00934           return d;
00935         }
00936       } else {
00937         // Y-dominant.
00938         if (q[1] < f[1]) {
00939           return (p - f).length();
00940         } if (q[1] > t[1]) {
00941           return (p - t).length();
00942         } else {
00943           return d;
00944         }
00945       }
00946     } else {
00947       // Y-
00948       if (-v[0] > -v[1]) {
00949         // X-dominant.
00950         if (q[0] > f[0]) {
00951           return (p - f).length();
00952         } if (q[0] < t[0]) {
00953           return (p - t).length();
00954         } else {
00955           return d;
00956         }
00957       } else {
00958         // Y-dominant.
00959         if (q[1] > f[1]) {
00960           return (p - f).length();
00961         } if (q[1] < t[1]) {
00962           return (p - t).length();
00963         } else {
00964           return d;
00965         }
00966       }
00967     }
00968   }
00969 }
00970 
00971 ////////////////////////////////////////////////////////////////////
00972 //     Function: CollisionBox::point_is_inside
00973 //       Access: Private
00974 //  Description: Returns true if the indicated point is within the
00975 //               polygon's 2-d space, false otherwise.
00976 ////////////////////////////////////////////////////////////////////
00977 bool CollisionBox::
00978 point_is_inside(const LPoint2f &p, const CollisionBox::Points &points) const {
00979   // We insist that the polygon be convex.  This makes things a bit simpler.
00980   // In the case of a convex polygon, defined with points in counterclockwise
00981   // order, a point is interior to the polygon iff the point is not right of
00982   // each of the edges.
00983   for (int i = 0; i < (int)points.size() - 1; i++) {
00984     if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) {
00985       return false;
00986     }
00987   }
00988   if (is_right(p - points[points.size() - 1]._p, 
00989     points[0]._p - points[points.size() - 1]._p)) {
00990     return false;
00991   }
00992 
00993   return true;
00994 }
00995 
00996 ////////////////////////////////////////////////////////////////////
00997 //     Function: CollisionBox::compute_point
00998 //       Access: Protected
00999 //  Description: Returns a point on the surface of the sphere.
01000 //               latitude and longitude range from 0.0 to 1.0.  This
01001 //               is used by fill_viz_geom() to create a visible
01002 //               representation of the sphere.
01003 ////////////////////////////////////////////////////////////////////
01004 Vertexf CollisionBox::
01005 compute_point(float latitude, float longitude) const {
01006   float s1, c1;
01007   csincos(latitude * MathNumbers::pi_f, &s1, &c1);
01008 
01009   float s2, c2;
01010   csincos(longitude * 2.0f * MathNumbers::pi_f, &s2, &c2);
01011 
01012   Vertexf p(s1 * c2, s1 * s2, c1);
01013   return p * get_radius() + get_center();
01014 }
01015 
01016 ////////////////////////////////////////////////////////////////////
01017 //     Function: CollisionBox::compute_vectors
01018 //       Access: Private, Static
01019 //  Description: Now that the _p members of the given points array
01020 //               have been computed, go back and compute all of the _v
01021 //               members.
01022 ////////////////////////////////////////////////////////////////////
01023 void CollisionBox::
01024 compute_vectors(Points &points) {
01025   size_t num_points = points.size();
01026   for (size_t i = 0; i < num_points; i++) {
01027     points[i]._v = points[(i + 1) % num_points]._p - points[i]._p;
01028     points[i]._v.normalize();
01029   }
01030 }
01031 
01032 ////////////////////////////////////////////////////////////////////
01033 //     Function: CollisionBox::register_with_read_factory
01034 //       Access: Public, Static
01035 //  Description: Factory method to generate a CollisionBox object
01036 ////////////////////////////////////////////////////////////////////
01037 void CollisionBox::
01038 register_with_read_factory() {
01039   BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox);
01040 }
01041 
01042 ////////////////////////////////////////////////////////////////////
01043 //     Function: CollisionBox::write_datagram
01044 //       Access: Public
01045 //  Description: Function to write the important information in
01046 //               the particular object to a Datagram
01047 ////////////////////////////////////////////////////////////////////
01048 void CollisionBox::
01049 write_datagram(BamWriter *manager, Datagram &me) {
01050   CollisionSolid::write_datagram(manager, me);
01051   _center.write_datagram(me);
01052   _min.write_datagram(me);
01053   _max.write_datagram(me);
01054   for(int i=0; i < 8; i++) {
01055     _vertex[i].write_datagram(me);
01056   }
01057   me.add_float32(_radius);
01058   me.add_float32(_x);
01059   me.add_float32(_y);
01060   me.add_float32(_z);
01061   for(int i=0; i < 6; i++) {
01062     _planes[i].write_datagram(me);
01063   }
01064   for(int i=0; i < 6; i++) {
01065     _to_2d_mat[i].write_datagram(me);
01066   }  
01067   for(int i=0; i < 6; i++) {  
01068     me.add_uint16(_points[i].size());
01069     for (size_t j = 0; j < _points[i].size(); j++) {
01070       _points[i][j]._p.write_datagram(me);
01071       _points[i][j]._v.write_datagram(me);
01072     }
01073   }
01074 }
01075 
01076 ////////////////////////////////////////////////////////////////////
01077 //     Function: CollisionBox::make_CollisionBox
01078 //       Access: Protected
01079 //  Description: Factory method to generate a CollisionBox object
01080 ////////////////////////////////////////////////////////////////////
01081 TypedWritable *CollisionBox::
01082 make_CollisionBox(const FactoryParams &params) {
01083   CollisionBox *me = new CollisionBox;
01084   DatagramIterator scan;
01085   BamReader *manager;
01086 
01087   parse_params(params, scan, manager);
01088   me->fillin(scan, manager);
01089   return me;
01090 }
01091 
01092 ////////////////////////////////////////////////////////////////////
01093 //     Function: CollisionBox::fillin
01094 //       Access: Protected
01095 //  Description: Function that reads out of the datagram (or asks
01096 //               manager to read) all of the data that is needed to
01097 //               re-create this object and stores it in the appropiate
01098 //               place
01099 ////////////////////////////////////////////////////////////////////
01100 void CollisionBox::
01101 fillin(DatagramIterator& scan, BamReader* manager) {
01102   CollisionSolid::fillin(scan, manager);
01103   _center.read_datagram(scan);
01104   _min.read_datagram(scan);
01105   _max.read_datagram(scan);
01106   for(int i=0; i < 8; i++) {
01107     _vertex[i].read_datagram(scan);
01108   }
01109   _radius = scan.get_float32();
01110   _x = scan.get_float32();
01111   _y = scan.get_float32();
01112   _z = scan.get_float32();
01113   for(int i=0; i < 6; i++) {
01114     _planes[i].read_datagram(scan);
01115   }
01116   for(int i=0; i < 6; i++) {
01117     _to_2d_mat[i].read_datagram(scan);
01118   }
01119   for(int i=0; i < 6; i++) {
01120     size_t size = scan.get_uint16();
01121     for (size_t j = 0; j < size; j++) {
01122       LPoint2f p;
01123       LVector2f v;
01124       p.read_datagram(scan);
01125       v.read_datagram(scan);
01126       _points[i].push_back(PointDef(p, v));
01127     }
01128   }
01129 }
 All Classes Functions Variables Enumerations