Panda3D

physicsCollisionHandler.cxx

00001 // Filename: physicsCollisionHandler.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 "physicsCollisionHandler.h"
00016 #include "collisionNode.h"
00017 #include "collisionEntry.h"
00018 #include "collisionPolygon.h"
00019 #include "config_collide.h"
00020 #include "config_physics.h"
00021 #include "actorNode.h"
00022 #include "dcast.h"
00023 
00024 TypeHandle PhysicsCollisionHandler::_type_handle;
00025 
00026 ////////////////////////////////////////////////////////////////////
00027 //     Function: PhysicsCollisionHandler::Constructor
00028 //       Access: Public
00029 //  Description:
00030 ////////////////////////////////////////////////////////////////////
00031 PhysicsCollisionHandler::
00032 PhysicsCollisionHandler() {
00033   _almost_stationary_speed = 0.1f;
00034   _static_friction_coef=0.9f;
00035   _dynamic_friction_coef=0.5f;
00036   set_horizontal(false);
00037 }
00038 
00039 ////////////////////////////////////////////////////////////////////
00040 //     Function: PhysicsCollisionHandler::Destructor
00041 //       Access: Public, Virtual
00042 //  Description:
00043 ////////////////////////////////////////////////////////////////////
00044 PhysicsCollisionHandler::
00045 ~PhysicsCollisionHandler() {
00046 }
00047 
00048 ////////////////////////////////////////////////////////////////////
00049 //     Function: PhysicsCollisionHandler::apply_friction
00050 //       Access: 
00051 //  Description: The vel parameter will be modified in place to
00052 //               account for friction.
00053 ////////////////////////////////////////////////////////////////////
00054 void PhysicsCollisionHandler::
00055 apply_friction(ColliderDef &def, LVector3& vel, const LVector3& force,
00056     PN_stdfloat angle) {
00057   if (vel!=LVector3::zero()) {
00058     PN_stdfloat friction_coefficient=0.0f;
00059     // Determine the friction:
00060     if (vel.length()<_almost_stationary_speed) {
00061       physics_debug("  static friction");
00062       friction_coefficient=_static_friction_coef;
00063     } else {
00064       physics_debug("  dynamic friction");
00065       friction_coefficient=_dynamic_friction_coef;
00066     }
00067     // Apply the friction:
00068     physics_debug("  vel pre  friction "<<vel<<" len "<<vel.length());
00069     PN_stdfloat friction=friction_coefficient*angle;
00070     physics_debug("  friction "<<friction);
00071     if (friction<0.0f || friction>1.0f) {
00072       cerr<<"\n\nfriction error "<<friction<<endl;
00073       friction=1.0f;
00074     }
00075     #if 0
00076     PN_stdfloat dt=ClockObject::get_global_clock()->get_dt();
00077     vel *= (1.0f-friction) * dt * dt;
00078     #else
00079     vel *= 1.0f-friction;
00080     #endif
00081     physics_debug("  vel post friction "<<vel<<" len "<<vel.length());
00082   }
00083 }
00084 
00085 ////////////////////////////////////////////////////////////////////
00086 //     Function: PhysicsCollisionHandler::apply_net_shove
00087 //       Access: Protected, Virtual
00088 //  Description: 
00089 ////////////////////////////////////////////////////////////////////
00090 void PhysicsCollisionHandler::
00091 apply_net_shove(ColliderDef &def, const LVector3& net_shove,
00092     const LVector3 &force) {
00093   CollisionHandlerPusher::apply_net_shove(def, net_shove, force);
00094   if (force == LVector3::zero()) {
00095     return;
00096   }
00097   if (def._target.is_empty()) {
00098     return;
00099   }
00100   ActorNode *actor;
00101   DCAST_INTO_V(actor, def._target.node());
00102   LVector3 vel=actor->get_physics_object()->get_velocity();
00103   if (vel == LVector3::zero()) {
00104     return;
00105   }
00106   physics_debug("apply_linear_force() {");
00107   physics_debug("  vel            "<<vel<<" len "<<vel.length());
00108   physics_debug("  net_shove      "<<net_shove<<" len "<<net_shove.length());
00109   physics_debug("  force          "<<force<<" len "<<force.length());
00110   LVector3 old_vel=vel;
00111 
00112   // Copy the force vector while translating it 
00113   // into the physics object coordinate system:
00114   LVector3 adjustment=force;
00115   physics_debug(
00116       "  adjustment set "<<adjustment<<" len "<<adjustment.length());
00117 
00118   //NodePath np(def._node);
00119   //CPT(TransformState) trans = np.get_net_transform();
00120   //adjustment=adjustment*trans->get_mat();
00121   //physics_debug(
00122   //    "  adjustment trn "<<adjustment<<" len "<<adjustment.length());
00123 
00124   adjustment=adjustment*actor->get_physics_object()->get_lcs();
00125   physics_debug(
00126       "  adjustment lcs "<<adjustment<<" len "<<adjustment.length());
00127 
00128   adjustment.normalize();
00129   physics_debug(
00130       "  adjustment nrm "<<adjustment<<" len "<<adjustment.length());
00131 
00132   PN_stdfloat adjustmentLength=-(adjustment.dot(vel));
00133   physics_debug("  adjustmentLength "<<adjustmentLength);
00134   PN_stdfloat angle=-normalize(old_vel).dot(normalize(force));
00135   physics_debug("  angle "<<angle);
00136   // Are we in contact with something:
00137   if (angle>0.0f) {
00138     physics_debug("  positive contact");
00139     #if 0
00140     cerr<<"vel "<<vel<<endl;
00141     cerr<<"net_shove "<<net_shove<<endl;
00142     cerr<<"force "<<force<<endl;
00143     actor->get_physics_object()->add_impact(force, -vel);
00144     #else
00145     adjustment*=adjustmentLength;
00146     physics_debug(
00147         "  adjustment mul "<<adjustment<<" len "<<adjustment.length());
00148     
00149     // This adjustment to our velocity will not reflect us off the surface,
00150     // but will deflect us parallel (or tangent) to the surface:
00151     vel+=adjustment;
00152     physics_debug("  vel+adj "<<vel<<" len "<<vel.length());
00153     
00154     apply_friction(def, vel, force, angle);
00155     #endif
00156   } else if (adjustmentLength==0.0f) {
00157     physics_debug("  brushing contact");
00158   } else {
00159     physics_debug("  negative contact");
00160   }
00161 
00162   #ifndef NDEBUG //[
00163   if (IS_THRESHOLD_EQUAL(vel.length(), old_vel.length(), 0.0001f)) {
00164     // This is a check to see if vel is staying the same:
00165     physics_debug(
00166         "  vel is about the same length:  "
00167         <<vel.length()<<" ~ "<<old_vel.length());
00168   } else if (vel.length() > old_vel.length()) {
00169     // This is a check to avoid adding engergy:
00170     physics_debug(
00171         "  vel got larger  "<<vel.length()<<" > "<<old_vel.length());
00172   } else {
00173     // This is a check to avoid losing engergy:
00174     physics_debug(
00175         "  vel got smaller  "<<vel.length()<<" < "<<old_vel.length());
00176   }
00177   if (vel.length() > 10.0f) {
00178     // This is a check to see if the velocity is higher than I expect it
00179     // to go.  The check value is arbitrary.
00180     physics_debug("  vel.length() > 10.0f  "<<vel.length());
00181   }
00182   #endif //]
00183 
00184   physics_debug("  force "<<force<<" len "<<force.length());
00185   physics_debug("  vel "<<vel<<" len "<<vel.length());
00186   physics_debug("}");
00187   actor->set_contact_vector(adjustment);
00188   actor->get_physics_object()->set_velocity(vel);
00189 }
00190 
00191 ////////////////////////////////////////////////////////////////////
00192 //     Function: PhysicsCollisionHandler::apply_linear_force
00193 //       Access: Protected, Virtual
00194 //  Description: 
00195 ////////////////////////////////////////////////////////////////////
00196 void PhysicsCollisionHandler::
00197 apply_linear_force(ColliderDef &def, const LVector3 &force) {
00198 }
00199 
00200 ////////////////////////////////////////////////////////////////////
00201 //     Function: PhysicsCollisionHandler::validate_target
00202 //       Access: Protected, Virtual
00203 //  Description: Called internally to validate the target passed to
00204 //               add_collider().  Returns true if acceptable, false
00205 //               otherwise.
00206 ////////////////////////////////////////////////////////////////////
00207 bool PhysicsCollisionHandler::
00208 validate_target(const NodePath &target) {
00209   if (!CollisionHandlerPhysical::validate_target(target)) {
00210     return false;
00211   }
00212   nassertr_always(target.node()->is_of_type(ActorNode::get_class_type()), false);
00213   return true;
00214 }
 All Classes Functions Variables Enumerations