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