Panda3D
|
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 }