Panda3D

collisionHandlerGravity.cxx

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 }
 All Classes Functions Variables Enumerations