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