00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00028
00029
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
00041
00042
00043
00044 PhysicsCollisionHandler::
00045 ~PhysicsCollisionHandler() {
00046 }
00047
00048
00049
00050
00051
00052
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
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
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
00087
00088
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
00113
00114 LVector3 adjustment=force;
00115 physics_debug(
00116 " adjustment set "<<adjustment<<" len "<<adjustment.length());
00117
00118
00119
00120
00121
00122
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
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
00150
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
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
00170 physics_debug(
00171 " vel got larger "<<vel.length()<<" > "<<old_vel.length());
00172 } else {
00173
00174 physics_debug(
00175 " vel got smaller "<<vel.length()<<" < "<<old_vel.length());
00176 }
00177 if (vel.length() > 10.0f) {
00178
00179
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
00193
00194
00195
00196 void PhysicsCollisionHandler::
00197 apply_linear_force(ColliderDef &def, const LVector3 &force) {
00198 }
00199
00200
00201
00202
00203
00204
00205
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 }