Panda3D

collisionHandlerFluidPusher.cxx

00001 // Filename: collisionHandlerFluidPusher.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 "collisionHandlerFluidPusher.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 CollisionHandlerFluidPusher::_type_handle;
00023 
00024 ////////////////////////////////////////////////////////////////////
00025 //     Function: CollisionHandlerFluidPusher::Constructor
00026 //       Access: Public
00027 //  Description:
00028 ////////////////////////////////////////////////////////////////////
00029 CollisionHandlerFluidPusher::
00030 CollisionHandlerFluidPusher() {
00031   _wants_all_potential_collidees = true;
00032 }
00033 
00034 ////////////////////////////////////////////////////////////////////
00035 //     Function: CollisionHandlerFluidPusher::add_entry
00036 //       Access: Public, Virtual
00037 //  Description: Called between a begin_group() .. end_group()
00038 //               sequence for each collision that is detected.
00039 ////////////////////////////////////////////////////////////////////
00040 void CollisionHandlerFluidPusher::
00041 add_entry(CollisionEntry *entry) {
00042   nassertv(entry != (CollisionEntry *)NULL);
00043   // skip over CollisionHandlerPhysical::add_entry, since it filters
00044   // out collidees by orientation; our collider can change direction
00045   // mid-frame, so it may collide with something that would have been
00046   // filtered out
00047   CollisionHandlerEvent::add_entry(entry);
00048 
00049   // filter out non-tangibles
00050   if (entry->get_from()->is_tangible() &&
00051       (!entry->has_into() || entry->get_into()->is_tangible())) {
00052 
00053     _from_entries[entry->get_from_node_path()].push_back(entry);
00054     if (entry->collided()) {
00055       _has_contact = true;
00056     }
00057   }
00058 }
00059 
00060 ////////////////////////////////////////////////////////////////////
00061 //     Function: CollisionHandlerFluidPusher::handle_entries
00062 //       Access: Protected, Virtual
00063 //  Description: Calculates a reasonable final position for a 
00064 //               collider given a set of collidees
00065 ////////////////////////////////////////////////////////////////////
00066 bool CollisionHandlerFluidPusher::
00067 handle_entries() {
00068   /*
00069     This pusher repeatedly calculates the first collision, calculates a new
00070     trajectory based on that collision, and repeats until the original motion is
00071     exhausted or the collider becomes "stuck". This solves the "acute collisions"
00072     problem where colliders could bounce their way through to the other side
00073     of a wall.
00074     
00075     Pseudocode:
00076     
00077     INPUTS
00078     PosA = collider's previous position
00079     PosB = collider's current position
00080     M = movement vector (PosB - PosA)
00081     BV = bounding sphere that includes collider at PosA and PosB
00082     CS = 'collision set', all 'collidables' within BV (collision polys, tubes, etc)
00083     
00084     VARIABLES
00085     N = movement vector since most recent collision (or start of frame)
00086     SCS = 'sub collision set', all collidables that could still be collided with
00087     C = single collider currently being collided with
00088     PosX = new position given movement along N interrupted by collision with C
00089     
00090     OUTPUTS
00091     final position is PosX
00092     
00093     1. N = M, SCS = CS, PosX = PosB
00094     2. compute, using SCS and N, which collidable C is the first collision
00095     3. if no collision found, DONE
00096     4. if movement in direction M is now blocked, then
00097        PosX = initial point of contact with C along N, DONE
00098     5. calculate PosX (and new N) assuming that there will be no more collisions
00099     6. remove C from SCS (assumes that you can't collide against a solid more than once per frame)
00100     7. go to 2
00101   */
00102   bool okflag = true;
00103 
00104   // if all we got was potential collisions, don't bother
00105   if (!_has_contact) {
00106     return okflag;
00107   }
00108 
00109   // for every fluid mover being pushed...
00110   FromEntries::iterator fei;
00111   for (fei = _from_entries.begin(); fei != _from_entries.end(); ++fei) {
00112     NodePath from_node_path = fei->first;
00113     Entries *orig_entries = &fei->second;
00114     
00115     Colliders::iterator ci;
00116     ci = _colliders.find(from_node_path);
00117     if (ci == _colliders.end()) {
00118       // Hmm, someone added a CollisionNode to a traverser and gave
00119       // it this CollisionHandler pointer--but they didn't tell us
00120       // about the node.
00121       collide_cat.error()
00122         << "CollisionHandlerFluidPusher doesn't know about "
00123         << from_node_path << ", disabling.\n";
00124       okflag = false;
00125     } else {
00126       ColliderDef &def = (*ci).second;
00127       
00128       // we do our math in this node's space
00129       NodePath wrt_node(*_root);
00130       
00131       // extract the collision entries into a vector that we can safely modify
00132       Entries entries(*orig_entries);
00133       
00134       // this is the original position delta for the entire frame, before collision response
00135       LVector3 M(from_node_path.get_pos_delta(wrt_node));
00136       // this is used to track position deltas every time we collide against a solid
00137       LVector3 N(M);
00138       
00139       const LPoint3 orig_pos(from_node_path.get_pos(wrt_node));
00140       CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
00141       const LPoint3 orig_prev_pos(prev_trans->get_pos());
00142       
00143       // currently we only support spheres as the collider
00144       const CollisionSphere *sphere;
00145       DCAST_INTO_R(sphere, entries.front()->get_from(), 0);
00146       
00147       from_node_path.set_pos(wrt_node, 0,0,0);
00148       LPoint3 sphere_offset = (sphere->get_center() *
00149                                 from_node_path.get_transform(wrt_node)->get_mat());
00150       from_node_path.set_pos(wrt_node, orig_pos);
00151       
00152       // this will hold the final calculated position at each iteration
00153       LPoint3 candidate_final_pos(orig_pos);
00154       // this holds the position before reacting to collisions
00155       LPoint3 uncollided_pos(candidate_final_pos);
00156       
00157       // unit vector facing back into original direction of motion
00158       LVector3 reverse_vec(-M);
00159       reverse_vec.normalize();
00160       
00161       // unit vector pointing out to the right relative to the direction of motion,
00162       // looking into the direction of motion
00163       const LVector3 right_unit(LVector3::up().cross(reverse_vec));
00164       
00165       // iterate until the mover runs out of movement or gets stuck
00166       while (true) {
00167         const CollisionEntry *C = 0;
00168         // find the first (earliest) collision
00169         Entries::const_iterator cei;
00170         for (cei = entries.begin(); cei != entries.end(); ++cei) {
00171           const CollisionEntry *entry = (*cei);
00172           nassertr(entry != (CollisionEntry *)NULL, false);
00173           if (entry->collided() && ((C == 0) || (entry->get_t() < C->get_t()))) {
00174             nassertr(from_node_path == entry->get_from_node_path(), false);
00175             C = entry;
00176           }
00177         }
00178         
00179         // if no collisions, we're done
00180         if (C == 0) {
00181           break;
00182         }
00183         
00184         // move back to initial contact position
00185         LPoint3 contact_pos;
00186         LVector3 contact_normal;
00187 
00188         if (!C->get_all_contact_info(wrt_node, contact_pos, contact_normal)) {
00189           collide_cat.warning()
00190             << "Cannot shove on " << from_node_path << " for collision into "
00191             << C->get_into_node_path() << "; no contact pos/normal information.\n";
00192           break;
00193         }
00194         // calculate the position of the target node at the point of contact
00195         contact_pos -= sphere_offset;
00196         
00197         uncollided_pos = candidate_final_pos;
00198         candidate_final_pos = contact_pos;
00199         
00200         LVector3 proj_surface_normal(contact_normal);
00201 
00202         LVector3 norm_proj_surface_normal(proj_surface_normal);
00203         norm_proj_surface_normal.normalize();
00204         
00205         LVector3 blocked_movement(uncollided_pos - contact_pos);
00206         
00207         PN_stdfloat push_magnitude(-blocked_movement.dot(proj_surface_normal));
00208         if (push_magnitude < 0.0f) {
00209           // don't ever push into plane
00210           candidate_final_pos = contact_pos;
00211         } else {
00212           // calculate new position given that you collided with this thing
00213           // project the final position onto the plane of the obstruction
00214           candidate_final_pos = uncollided_pos + (norm_proj_surface_normal * push_magnitude);
00215         }
00216         
00217         from_node_path.set_pos(wrt_node, candidate_final_pos);
00218         CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
00219         prev_trans = prev_trans->set_pos(contact_pos);
00220         from_node_path.set_prev_transform(wrt_node, prev_trans);
00221         
00222         {
00223           const LPoint3 new_pos(from_node_path.get_pos(wrt_node));
00224           CPT(TransformState) new_prev_trans(from_node_path.get_prev_transform(wrt_node));
00225           const LPoint3 new_prev_pos(new_prev_trans->get_pos());
00226         }
00227 
00228         // recalculate the position delta
00229         N = from_node_path.get_pos_delta(wrt_node);
00230         
00231         // calculate new collisions given new movement vector
00232         Entries::iterator ei;
00233         Entries new_entries;
00234         for (ei = entries.begin(); ei != entries.end(); ++ei) {
00235           CollisionEntry *entry = (*ei);
00236           nassertr(entry != (CollisionEntry *)NULL, false);
00237           // skip the one we just collided against
00238           if (entry != C) {
00239             entry->_from_node_path = from_node_path;
00240             entry->reset_collided();
00241             PT(CollisionEntry) result = entry->get_from()->test_intersection(**ei);
00242             if (result != (CollisionEntry *)NULL && result != (CollisionEntry *)0) {
00243               new_entries.push_back(result);
00244             }
00245           }
00246         }
00247         entries.swap(new_entries);
00248       }
00249       
00250       // put things back where they were
00251       from_node_path.set_pos(wrt_node, orig_pos);
00252       // restore the appropriate previous position
00253       prev_trans = from_node_path.get_prev_transform(wrt_node);
00254       prev_trans = prev_trans->set_pos(orig_prev_pos);
00255       from_node_path.set_prev_transform(wrt_node, prev_trans);
00256       
00257       LVector3 net_shove(candidate_final_pos - orig_pos);
00258       LVector3 force_normal(net_shove);
00259       force_normal.normalize();
00260       
00261       // This is the part where the node actually gets moved:
00262       def._target.set_pos(wrt_node, candidate_final_pos);
00263       
00264       // We call this to allow derived classes to do other
00265       // fix-ups as they see fit:
00266       apply_net_shove(def, net_shove, force_normal);
00267       apply_linear_force(def, force_normal);
00268     }
00269   }
00270   
00271   return okflag;
00272 }
 All Classes Functions Variables Enumerations