00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00026
00027
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
00044
00045
00046
00047 CollisionHandlerGravity::
00048 ~CollisionHandlerGravity() {
00049 }
00050
00051
00052
00053
00054
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
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
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
00106
00107 _current_colliding.clear();
00108
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
00118
00119
00120
00121
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
00161
00162
00163 got_max = true;
00164 max_height = min_height;
00165 highest = lowest;
00166 valid_entries.push_back(lowest);
00167 }
00168
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
00181
00182 _current_colliding.clear();
00183 if (_legacy_mode) {
00184
00185 add_entry(highest);
00186 } else {
00187
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
00196
00197 if (highest->get_into()->is_of_type(CollisionPlane::get_class_type())) {
00198
00199
00200
00201
00202
00203
00204
00205
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
00218
00219
00220
00221
00222
00223
00224
00225
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
00240
00241
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
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
00264
00265 PN_stdfloat dt = ClockObject::get_global_clock()->get_dt();
00266
00267
00268 PN_stdfloat gravity_adjust = _current_velocity * dt + 0.5 * -_gravity * dt * dt;
00269 if (adjust > 0.0f) {
00270
00271
00272
00273 adjust += max((PN_stdfloat)0.0, gravity_adjust);
00274 } else {
00275
00276 adjust = max(adjust, gravity_adjust);
00277 }
00278 _current_velocity -= _gravity * dt;
00279
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
00286 _impact_velocity = _current_velocity;
00287
00288 _current_velocity = _airborne_height = 0.0f;
00289 } else if (_legacy_mode) {
00290
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
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
00317
00318
00319
00320 void CollisionHandlerGravity::
00321 apply_linear_force(ColliderDef &def, const LVector3 &force) {
00322 }