Panda3D
|
00001 // Filename: CollisionHandlerGravity.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 "collisionHandlerGravity.h" 00016 #include "collisionNode.h" 00017 #include "collisionEntry.h" 00018 #include "config_collide.h" 00019 #include "collisionPlane.h" 00020 #include "clockObject.h" 00021 00022 TypeHandle CollisionHandlerGravity::_type_handle; 00023 00024 //////////////////////////////////////////////////////////////////// 00025 // Function: CollisionHandlerGravity::Constructor 00026 // Access: Public 00027 // Description: 00028 //////////////////////////////////////////////////////////////////// 00029 CollisionHandlerGravity:: 00030 CollisionHandlerGravity() { 00031 _offset = 0.0f; 00032 _reach = 1.0f; 00033 _airborne_height = 0.0f; 00034 _impact_velocity = 0.0f; 00035 _gravity = 32.174f; 00036 _current_velocity = 0.0f; 00037 _max_velocity = 400.0f; 00038 _contact_normal = LVector3f::zero(); 00039 _legacy_mode = false; 00040 } 00041 00042 //////////////////////////////////////////////////////////////////// 00043 // Function: CollisionHandlerGravity::Destructor 00044 // Access: Public, Virtual 00045 // Description: 00046 //////////////////////////////////////////////////////////////////// 00047 CollisionHandlerGravity:: 00048 ~CollisionHandlerGravity() { 00049 } 00050 00051 //////////////////////////////////////////////////////////////////// 00052 // Function: CollisionHandlerGravity::set_highest_collision 00053 // Access: Protected 00054 // Description: 00055 // 00056 // 00057 // 00058 // 00059 // 00060 // 00061 //////////////////////////////////////////////////////////////////// 00062 #define OLD_COLLISION_HANDLER_GRAVITY 0 00063 #if OLD_COLLISION_HANDLER_GRAVITY 00064 float CollisionHandlerGravity:: 00065 set_highest_collision(const NodePath &target_node_path, const NodePath &from_node_path, const Entries &entries) { 00066 // Get the maximum height for all collisions with this node. 00067 bool got_max = false; 00068 float max_height = 0.0f; 00069 CollisionEntry *highest = NULL; 00070 00071 Entries::const_iterator ei; 00072 for (ei = entries.begin(); ei != entries.end(); ++ei) { 00073 CollisionEntry *entry = (*ei); 00074 nassertr(entry != (CollisionEntry *)NULL, 0.0f); 00075 nassertr(from_node_path == entry->get_from_node_path(), 0.0f); 00076 00077 if (entry->has_surface_point()) { 00078 LPoint3f point = entry->get_surface_point(target_node_path); 00079 if (collide_cat.is_debug()) { 00080 collide_cat.debug() 00081 << "Intersection point detected at " << point << "\n"; 00082 } 00083 00084 float height = point[2]; 00085 if (!got_max || height > max_height) { 00086 got_max = true; 00087 max_height = height; 00088 highest = entry; 00089 } 00090 } 00091 } 00092 //#*#_has_contact = got_max; 00093 00094 #if 0 00095 cout<<"\ncolliding with:\n"; 00096 for (Colliding::const_iterator i = _current_colliding.begin(); i != _current_colliding.end(); ++i) { 00097 (**i).write(cout, 2); 00098 } 00099 cout<<"\nhighest:\n"; 00100 highest->write(cout, 2); 00101 cout<<endl; 00102 #endif 00103 00104 if (_legacy_mode) { 00105 // We only collide with things we are impacting with. 00106 // Remove the collisions: 00107 _current_colliding.clear(); 00108 // Add only the one that we're impacting with: 00109 add_entry(highest); 00110 } 00111 00112 return max_height; 00113 } 00114 #else 00115 float CollisionHandlerGravity:: 00116 set_highest_collision(const NodePath &target_node_path, const NodePath &from_node_path, const Entries &entries) { 00117 // Get the maximum height for all collisions with this node. 00118 // This is really the distance to-the-ground, so it will 00119 // be negative when the avatar is above the ground. 00120 // Larger values (less negative) are higher elevation (assuming 00121 // the avatar is right-side-up (or the ray is plumb)). 00122 bool got_max = false; 00123 bool got_min = false; 00124 float max_height = 0.0f; 00125 float min_height = 0.0f; 00126 CollisionEntry *highest = NULL; 00127 CollisionEntry *lowest = NULL; 00128 00129 pvector<PT(CollisionEntry)> valid_entries; 00130 00131 Entries::const_iterator ei; 00132 for (ei = entries.begin(); ei != entries.end(); ++ei) { 00133 CollisionEntry *entry = (*ei); 00134 nassertr(entry != (CollisionEntry *)NULL, 0.0f); 00135 nassertr(from_node_path == entry->get_from_node_path(), 0.0f); 00136 00137 if (entry->has_surface_point()) { 00138 LPoint3f point = entry->get_surface_point(target_node_path); 00139 if (collide_cat.is_debug()) { 00140 collide_cat.debug() 00141 << "Intersection point detected at " << point << "\n"; 00142 } 00143 float height = point[2]; 00144 if(height < _offset + _reach) { 00145 valid_entries.push_back(entry); 00146 if (!got_max || height > max_height) { 00147 got_max = true; 00148 max_height = height; 00149 highest = entry; 00150 } 00151 } 00152 if (!got_min || height < min_height) { 00153 got_min = true; 00154 min_height = height; 00155 lowest = entry; 00156 } 00157 } 00158 } 00159 if (!got_max && got_min) { 00160 // We've fallen through the world, but we're also under some walkable 00161 // geometry. 00162 // Move us up to the lowest surface: 00163 got_max = true; 00164 max_height = min_height; 00165 highest = lowest; 00166 valid_entries.push_back(lowest); 00167 } 00168 //#*#_has_contact = got_max; 00169 00170 #if 0 00171 cout<<"\ncolliding with:\n"; 00172 for (Colliding::const_iterator i = _current_colliding.begin(); i != _current_colliding.end(); ++i) { 00173 (**i).write(cout, 2); 00174 } 00175 cout<<"\nhighest:\n"; 00176 highest->write(cout, 2); 00177 cout<<endl; 00178 #endif 00179 00180 // We only collide with things we are impacting with. 00181 // Remove the collisions: 00182 _current_colliding.clear(); 00183 if (_legacy_mode) { 00184 // Add only the one that we're impacting with: 00185 add_entry(highest); 00186 } else { 00187 _current_colliding.insert(valid_entries.begin(), valid_entries.end()); 00188 } 00189 00190 00191 // Set the contact normal so that other code can make use of the 00192 // surface slope: 00193 if (highest->get_into()->is_of_type(CollisionPlane::get_class_type())) { 00194 // This is asking: what is the normal of the plane that the avatar 00195 // is colliding with relative to the avatar. A positive y valye means 00196 // the avatar is facing downhill and a negative y value means the 00197 // avatar is facing uphill. 00198 //_contact_normal = DCAST(CollisionPlane, highest->get_into())->get_normal() * from_node_path.get_mat(highest->get_into_node_path()); 00199 //_contact_normal = DCAST(CollisionPlane, highest->get_into())->get_normal(); 00200 // This is asking: what is the normal of the avatar that the avatar 00201 // is colliding with relative to the plane. 00202 CPT(TransformState) transform = highest->get_into_node_path().get_transform(from_node_path); 00203 _contact_normal = DCAST(CollisionPlane, highest->get_into())->get_normal() * transform->get_mat(); 00204 } else { 00205 _contact_normal = highest->get_surface_normal(from_node_path); 00206 } 00207 00208 return max_height; 00209 } 00210 #endif 00211 00212 //////////////////////////////////////////////////////////////////// 00213 // Function: CollisionHandlerGravity::handle_entries 00214 // Access: Protected, Virtual 00215 // Description: Called by the parent class after all collisions have 00216 // been detected, this manages the various collisions 00217 // and moves around the nodes as necessary. 00218 // 00219 // The return value is normally true, but it may be 00220 // false to indicate the CollisionTraverser should 00221 // disable this handler from being called in the future. 00222 //////////////////////////////////////////////////////////////////// 00223 bool CollisionHandlerGravity:: 00224 handle_entries() { 00225 bool okflag = true; 00226 00227 FromEntries::const_iterator fi; 00228 for (fi = _from_entries.begin(); fi != _from_entries.end(); ++fi) { 00229 const NodePath &from_node_path = (*fi).first; 00230 const Entries &entries = (*fi).second; 00231 00232 Colliders::iterator ci; 00233 ci = _colliders.find(from_node_path); 00234 if (ci == _colliders.end()) { 00235 // Hmm, someone added a CollisionNode to a traverser and gave 00236 // it this CollisionHandler pointer--but they didn't tell us 00237 // about the node. 00238 collide_cat.error() 00239 << get_type() << " doesn't know about " 00240 << from_node_path << ", disabling.\n"; 00241 okflag = false; 00242 } else { 00243 ColliderDef &def = (*ci).second; 00244 float max_height = set_highest_collision(def._target, from_node_path, entries); 00245 00246 // Now set our height accordingly. 00247 #if OLD_COLLISION_HANDLER_GRAVITY 00248 float adjust = max_height + _offset; 00249 #else 00250 float adjust = max_height + _offset; 00251 #endif 00252 if (_current_velocity > 0.0f || !IS_THRESHOLD_ZERO(adjust, 0.001)) { 00253 if (collide_cat.is_debug()) { 00254 collide_cat.debug() 00255 << "Adjusting height by " << adjust << "\n"; 00256 } 00257 00258 if (_current_velocity > 0.0f || adjust) { 00259 // ...we have a vertical thrust, 00260 // ...or the node is above the floor, so it is airborne. 00261 float dt = ClockObject::get_global_clock()->get_dt(); 00262 // Fyi, the sign of _gravity is reversed. I think it makes the get_*() set_*() 00263 // more intuitive to do it this way. 00264 float gravity_adjust = _current_velocity * dt + 0.5 * -_gravity * dt * dt; 00265 if (adjust > 0.0f) { 00266 // ...the node is under the floor, so it has landed. 00267 // Keep the adjust to bring us up to the ground and 00268 // then add the gravity_adjust to get us airborne: 00269 adjust += max(0.0f, gravity_adjust); 00270 } else { 00271 // ...the node is above the floor, so it is airborne. 00272 adjust = max(adjust, gravity_adjust); 00273 } 00274 _current_velocity -= _gravity * dt; 00275 // Record the airborne height in case someone else needs it: 00276 _airborne_height = -(max_height + _offset) + adjust; 00277 assert(_airborne_height >= -0.001f); 00278 } 00279 00280 if (_airborne_height < 0.001f && _current_velocity < 0.001f) { 00281 // ...the node is under the floor, so it has landed. 00282 _impact_velocity = _current_velocity; 00283 // These values are used by is_on_ground(). 00284 _current_velocity = _airborne_height = 0.0f; 00285 } else if (_legacy_mode) { 00286 // ...we're airborne. 00287 _current_colliding.clear(); 00288 } 00289 00290 CPT(TransformState) trans = def._target.get_transform(); 00291 LVecBase3f pos = trans->get_pos(); 00292 pos[2] += adjust; 00293 def._target.set_transform(trans->set_pos(pos)); 00294 def.updated_transform(); 00295 00296 apply_linear_force(def, LVector3f(0.0f, 0.0f, adjust)); 00297 } else { 00298 // _impact_velocity = _current_velocity; 00299 _current_velocity = _airborne_height = 0.0f; 00300 if (collide_cat.is_spam()) { 00301 collide_cat.spam() 00302 << "Leaving height unchanged.\n"; 00303 } 00304 } 00305 } 00306 } 00307 00308 return okflag; 00309 } 00310 00311 //////////////////////////////////////////////////////////////////// 00312 // Function: CollisionHandlerGravity::apply_linear_force 00313 // Access: Protected, Virtual 00314 // Description: 00315 //////////////////////////////////////////////////////////////////// 00316 void CollisionHandlerGravity:: 00317 apply_linear_force(ColliderDef &def, const LVector3f &force) { 00318 }