Panda3D

collisionHandlerPusher.cxx

00001 // Filename: collisionHandlerPusher.cxx
00002 // Created by:  drose (16Mar02)
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 "collisionHandlerPusher.h"
00016 #include "collisionNode.h"
00017 #include "collisionEntry.h"
00018 #include "collisionPolygon.h"
00019 #include "config_collide.h"
00020 #include "dcast.h"
00021 
00022 TypeHandle CollisionHandlerPusher::_type_handle;
00023 
00024 ///////////////////////////////////////////////////////////////////
00025 //       Class : ShoveData
00026 // Description : The ShoveData class is used within
00027 //               CollisionHandlerPusher::handle_entries(), to track
00028 //               multiple shoves onto a given collider.  It's not
00029 //               exported outside this file.
00030 ////////////////////////////////////////////////////////////////////
00031 class ShoveData {
00032 public:
00033   LVector3 _vector;
00034   PN_stdfloat _length;
00035   bool _valid;
00036   CollisionEntry *_entry;
00037 };
00038 
00039 ////////////////////////////////////////////////////////////////////
00040 //     Function: CollisionHandlerPusher::Constructor
00041 //       Access: Public
00042 //  Description:
00043 ////////////////////////////////////////////////////////////////////
00044 CollisionHandlerPusher::
00045 CollisionHandlerPusher() {
00046   _horizontal = pushers_horizontal;
00047 }
00048 
00049 ////////////////////////////////////////////////////////////////////
00050 //     Function: CollisionHandlerPusher::Destructor
00051 //       Access: Public, Virtual
00052 //  Description:
00053 ////////////////////////////////////////////////////////////////////
00054 CollisionHandlerPusher::
00055 ~CollisionHandlerPusher() {
00056 }
00057 
00058 ////////////////////////////////////////////////////////////////////
00059 //     Function: CollisionHandlerPusher::handle_entries
00060 //       Access: Protected, Virtual
00061 //  Description: Called by the parent class after all collisions have
00062 //               been detected, this manages the various collisions
00063 //               and moves around the nodes as necessary.
00064 //
00065 //               The return value is normally true, but it may be
00066 //               false to indicate the CollisionTraverser should
00067 //               disable this handler from being called in the future.
00068 ////////////////////////////////////////////////////////////////////
00069 bool CollisionHandlerPusher::
00070 handle_entries() {
00071   bool okflag = true;
00072 
00073   FromEntries::const_iterator fi;
00074   for (fi = _from_entries.begin(); fi != _from_entries.end(); ++fi) {
00075     const NodePath &from_node_path = (*fi).first;
00076     const Entries &entries = (*fi).second;
00077 
00078     Colliders::iterator ci;
00079     ci = _colliders.find(from_node_path);
00080     if (ci == _colliders.end()) {
00081       // Hmm, someone added a CollisionNode to a traverser and gave
00082       // it this CollisionHandler pointer--but they didn't tell us
00083       // about the node.
00084       collide_cat.error()
00085         << "CollisionHandlerPusher doesn't know about "
00086         << from_node_path << ", disabling.\n";
00087       okflag = false;
00088     } else {
00089       ColliderDef &def = (*ci).second;
00090       {
00091         // How to apply multiple shoves from different solids onto the
00092         // same collider?  One's first intuition is to vector sum all
00093         // the shoves.  However, this causes problems when two parallel
00094         // walls shove on the collider, because we end up with a double
00095         // shove.  We hack around this by testing if two shove vectors
00096         // share nearly the same direction, and if so, we keep only the
00097         // longer of the two.
00098         
00099         typedef epvector<ShoveData> Shoves;
00100         Shoves shoves;
00101 
00102         Entries::const_iterator ei;
00103         for (ei = entries.begin(); ei != entries.end(); ++ei) {
00104           CollisionEntry *entry = (*ei);
00105           nassertr(entry != (CollisionEntry *)NULL, false);
00106           nassertr(from_node_path == entry->get_from_node_path(), false);
00107 
00108           LPoint3 surface_point;
00109           LVector3 normal;
00110           LPoint3 interior_point;
00111 
00112           if (!entry->get_all(def._target, surface_point, normal, interior_point)) {
00113             #ifndef NDEBUG          
00114             if (collide_cat.is_debug()) {
00115               collide_cat.debug()
00116                 << "Cannot shove on " << from_node_path << " for collision into "
00117                 << entry->get_into_node_path() << "; no normal/depth information.\n";
00118             }
00119             #endif            
00120           } else {
00121             // Shove it just enough to clear the volume.
00122             if (!surface_point.almost_equal(interior_point)) {
00123               if (_horizontal) {
00124                 normal[2] = 0.0f;
00125               }
00126               // Just to be on the safe size, we normalize the normal
00127               // vector, even though it really ought to be unit-length
00128               // already (unless we just forced it horizontal, above).
00129               normal.normalize();
00130 
00131               ShoveData sd;
00132               sd._vector = normal;
00133               sd._length = (surface_point - interior_point).length();
00134               sd._valid = true;
00135               sd._entry = entry;
00136               
00137               #ifndef NDEBUG          
00138               if (collide_cat.is_debug()) {
00139                 collide_cat.debug()
00140                   << "Shove on " << from_node_path << " from "
00141                   << entry->get_into_node_path() << ": " << sd._vector
00142                   << " times " << sd._length << "\n";
00143               }
00144               #endif
00145               
00146               shoves.push_back(sd);
00147             }
00148           }
00149         }
00150         
00151         if (!shoves.empty()) {
00152           // Now we look for two shoves that are largely in the same
00153           // direction, so we can combine them into a single shove of
00154           // the same magnitude; we also check for two shoves at 90
00155           // degrees, so we can detect whether we are hitting an inner
00156           // or an outer corner.
00157 
00158           Shoves::iterator si;
00159           for (si = shoves.begin(); si != shoves.end(); ++si) {
00160             ShoveData &sd = (*si);
00161             Shoves::iterator sj;
00162             for (sj = shoves.begin(); sj != si; ++sj) {
00163               ShoveData &sd2 = (*sj);
00164               if (sd2._valid) {
00165                 PN_stdfloat d = sd._vector.dot(sd2._vector);
00166                 if (collide_cat.is_debug()) {
00167                   collide_cat.debug()
00168                     << "Considering dot product " << d << "\n";
00169                 }
00170                 
00171                 if (d > 0.9) {
00172                   // These two shoves are largely in the same direction;
00173                   // save the larger of the two.
00174                   if (sd2._length < sd._length) {
00175                     sd2._valid = false;
00176                   } else {
00177                     sd._valid = false;
00178                   }
00179                 } else {
00180                   // These two shoves are not in the same direction.
00181                   // If they are both from polygons that are a child
00182                   // of the same node, try to determine the shape of
00183                   // the corner (convex or concave).
00184                   const CollisionSolid *s1 = sd._entry->get_into();
00185                   const CollisionSolid *s2 = sd2._entry->get_into();
00186                   if (s1 != (CollisionSolid *)NULL &&
00187                       s2 != (CollisionSolid *)NULL &&
00188                       s1->is_exact_type(CollisionPolygon::get_class_type()) &&
00189                       s2->is_exact_type(CollisionPolygon::get_class_type()) &&
00190                       sd._entry->get_into_node_path() ==
00191                       sd2._entry->get_into_node_path()) {
00192                     const CollisionPolygon *p1 = DCAST(CollisionPolygon, s1);
00193                     const CollisionPolygon *p2 = DCAST(CollisionPolygon, s2);
00194                     if (p1->dist_to_plane(p2->get_collision_origin()) < 0 &&
00195                         p2->dist_to_plane(p1->get_collision_origin()) < 0) {
00196                       // Each polygon is behind the other one.  That
00197                       // means we have a convex corner, and therefore
00198                       // we should discard one of the shoves (or the
00199                       // user will get stuck coming at a convex
00200                       // corner).
00201                       if (collide_cat.is_debug()) {
00202                         collide_cat.debug()
00203                           << "Discarding shove from convex corner.\n";
00204                       }
00205 
00206                       // This time, unlike the case of two parallel
00207                       // walls above, we discard the larger of the two
00208                       // shoves, not the smaller.  This is because as
00209                       // we slide off the convex corner, the wall we
00210                       // are sliding away from will get a bigger and
00211                       // bigger shove--and we need to keep ignoring
00212                       // the same wall as we slide.
00213                       if (sd2._length < sd._length) {
00214                         sd._valid = false;
00215                       } else {
00216                         sd2._valid = false;
00217                       }
00218                     }
00219                   }
00220                 }
00221               }
00222             }
00223           }
00224           
00225           // Now we can determine the net shove.
00226           LVector3 net_shove(0.0f, 0.0f, 0.0f);
00227           LVector3 force_normal(0.0f, 0.0f, 0.0f);
00228           for (si = shoves.begin(); si != shoves.end(); ++si) {
00229             const ShoveData &sd = (*si);
00230             if (sd._valid) {
00231               net_shove += sd._vector * sd._length;
00232               force_normal += sd._vector;
00233             }
00234           }
00235 
00236           #ifndef NDEBUG          
00237           if (collide_cat.is_debug()) {
00238             collide_cat.debug()
00239               << "Net shove on " << from_node_path << " is: "
00240               << net_shove << "\n";
00241           }
00242           #endif
00243           
00244           // This is the part where the node actually gets moved:
00245           CPT(TransformState) trans = def._target.get_transform();
00246           LVecBase3 pos = trans->get_pos();
00247           pos += net_shove * trans->get_mat();
00248           def._target.set_transform(trans->set_pos(pos));
00249           def.updated_transform();
00250           
00251           // We call this to allow derived classes to do other
00252           // fix-ups as they see fit:
00253           apply_net_shove(def, net_shove, force_normal);
00254           apply_linear_force(def, force_normal);
00255         }
00256       }
00257     }
00258   }
00259 
00260   return okflag;
00261 }
00262 
00263 ////////////////////////////////////////////////////////////////////
00264 //     Function: CollisionHandlerPusher::apply_net_shove
00265 //       Access: Protected, Virtual
00266 //  Description: This is an optional hook for derived classes to do
00267 //               some work with the ColliderDef and the force vector.
00268 ////////////////////////////////////////////////////////////////////
00269 void CollisionHandlerPusher::
00270 apply_net_shove(ColliderDef &def, const LVector3 &net_shove, 
00271     const LVector3 &force_normal) {
00272 }
00273 
00274 ////////////////////////////////////////////////////////////////////
00275 //     Function: CollisionHandlerPusher::apply_linear_force
00276 //       Access: Protected, Virtual
00277 //  Description: This is an optional hook for derived classes to do
00278 //               some work with the ColliderDef and the force vector.
00279 ////////////////////////////////////////////////////////////////////
00280 void CollisionHandlerPusher::
00281 apply_linear_force(ColliderDef &def, const LVector3 &force_normal) {
00282 }
 All Classes Functions Variables Enumerations