Panda3D
 All Classes Functions Variables Enumerations
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.174;
00036   _current_velocity = 0.0f;
00037   _max_velocity = 400.0f;
00038   _contact_normal = LVector3::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 PN_stdfloat 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   PN_stdfloat 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       LPoint3 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       PN_stdfloat 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 PN_stdfloat 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   PN_stdfloat max_height = 0.0f;
00125   PN_stdfloat 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       LPoint3 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       PN_stdfloat 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     // Add all of them.
00188     pvector<PT(CollisionEntry)>::iterator vi;
00189     for (vi = valid_entries.begin(); vi != valid_entries.end(); ++vi) {
00190       add_entry(*vi);
00191     }
00192   }
00193 
00194   
00195   // Set the contact normal so that other code can make use of the
00196   // surface slope:
00197   if (highest->get_into()->is_of_type(CollisionPlane::get_class_type())) {
00198     // This is asking: what is the normal of the plane that the avatar
00199     // is colliding with relative to the avatar.  A positive y valye means
00200     // the avatar is facing downhill and a negative y value means the
00201     // avatar is facing uphill.
00202     //_contact_normal = DCAST(CollisionPlane, highest->get_into())->get_normal() * from_node_path.get_mat(highest->get_into_node_path());
00203     //_contact_normal = DCAST(CollisionPlane, highest->get_into())->get_normal();
00204     // This is asking: what is the normal of the avatar that the avatar
00205     // is colliding with relative to the plane.
00206     CPT(TransformState) transform = highest->get_into_node_path().get_transform(from_node_path);
00207     _contact_normal = DCAST(CollisionPlane, highest->get_into())->get_normal() * transform->get_mat();
00208   } else {
00209     _contact_normal = highest->get_surface_normal(from_node_path);
00210   }
00211   
00212   return max_height;
00213 }
00214 #endif
00215 
00216 ////////////////////////////////////////////////////////////////////
00217 //     Function: CollisionHandlerGravity::handle_entries
00218 //       Access: Protected, Virtual
00219 //  Description: Called by the parent class after all collisions have
00220 //               been detected, this manages the various collisions
00221 //               and moves around the nodes as necessary.
00222 //
00223 //               The return value is normally true, but it may be
00224 //               false to indicate the CollisionTraverser should
00225 //               disable this handler from being called in the future.
00226 ////////////////////////////////////////////////////////////////////
00227 bool CollisionHandlerGravity::
00228 handle_entries() {
00229   bool okflag = true;
00230 
00231   FromEntries::const_iterator fi;
00232   for (fi = _from_entries.begin(); fi != _from_entries.end(); ++fi) {
00233     const NodePath &from_node_path = (*fi).first;
00234     const Entries &entries = (*fi).second;
00235 
00236     Colliders::iterator ci;
00237     ci = _colliders.find(from_node_path);
00238     if (ci == _colliders.end()) {
00239       // Hmm, someone added a CollisionNode to a traverser and gave
00240       // it this CollisionHandler pointer--but they didn't tell us
00241       // about the node.
00242       collide_cat.error()
00243         << get_type() << " doesn't know about "
00244         << from_node_path << ", disabling.\n";
00245       okflag = false;
00246     } else {
00247       ColliderDef &def = (*ci).second;
00248       PN_stdfloat max_height = set_highest_collision(def._target, from_node_path, entries);
00249 
00250       // Now set our height accordingly.
00251       #if OLD_COLLISION_HANDLER_GRAVITY
00252       PN_stdfloat adjust = max_height + _offset;
00253       #else
00254       PN_stdfloat adjust = max_height + _offset;
00255       #endif
00256       if (_current_velocity > 0.0f || !IS_THRESHOLD_ZERO(adjust, 0.001)) {
00257         if (collide_cat.is_debug()) {
00258           collide_cat.debug()
00259             << "Adjusting height by " << adjust << "\n";
00260         }
00261 
00262         if (_current_velocity > 0.0f || adjust) {
00263           // ...we have a vertical thrust,
00264           // ...or the node is above the floor, so it is airborne.
00265           PN_stdfloat dt = ClockObject::get_global_clock()->get_dt();
00266           // Fyi, the sign of _gravity is reversed. I think it makes the get_*() set_*()
00267           // more intuitive to do it this way.
00268           PN_stdfloat gravity_adjust = _current_velocity * dt + 0.5 * -_gravity * dt * dt;
00269           if (adjust > 0.0f) {
00270             // ...the node is under the floor, so it has landed.
00271             // Keep the adjust to bring us up to the ground and
00272             // then add the gravity_adjust to get us airborne:
00273             adjust += max((PN_stdfloat)0.0, gravity_adjust);
00274           } else {
00275             // ...the node is above the floor, so it is airborne.
00276             adjust = max(adjust, gravity_adjust);
00277           }
00278           _current_velocity -= _gravity * dt;
00279           // Record the airborne height in case someone else needs it: 
00280           _airborne_height = -(max_height + _offset) + adjust;
00281           assert(_airborne_height >= -0.001f);
00282         }
00283 
00284         if (_airborne_height < 0.001f && _current_velocity < 0.001f) {
00285           // ...the node is under the floor, so it has landed.
00286           _impact_velocity = _current_velocity;
00287           // These values are used by is_on_ground().
00288           _current_velocity = _airborne_height = 0.0f;
00289         } else if (_legacy_mode) {
00290           // ...we're airborne.
00291           _current_colliding.clear();
00292         }
00293 
00294         CPT(TransformState) trans = def._target.get_transform();
00295         LVecBase3 pos = trans->get_pos();
00296         pos[2] += adjust;
00297         def._target.set_transform(trans->set_pos(pos));
00298         def.updated_transform();
00299 
00300         apply_linear_force(def, LVector3(0.0f, 0.0f, adjust));
00301       } else {
00302         // _impact_velocity = _current_velocity;
00303         _current_velocity = _airborne_height = 0.0f;
00304         if (collide_cat.is_spam()) {
00305           collide_cat.spam()
00306             << "Leaving height unchanged.\n";
00307         }
00308       }
00309     }
00310   }
00311 
00312   return okflag;
00313 }
00314 
00315 ////////////////////////////////////////////////////////////////////
00316 //     Function: CollisionHandlerGravity::apply_linear_force
00317 //       Access: Protected, Virtual
00318 //  Description: 
00319 ////////////////////////////////////////////////////////////////////
00320 void CollisionHandlerGravity::
00321 apply_linear_force(ColliderDef &def, const LVector3 &force) {
00322 }
 All Classes Functions Variables Enumerations