24 TypeHandle CollisionHandlerGravity::_type_handle;
29 CollisionHandlerGravity::
30 CollisionHandlerGravity() {
33 _airborne_height = 0.0f;
34 _impact_velocity = 0.0f;
36 _current_velocity = 0.0f;
37 _max_velocity = 400.0f;
38 _contact_normal = LVector3::zero();
45 CollisionHandlerGravity::
46 ~CollisionHandlerGravity() {
52 #define OLD_COLLISION_HANDLER_GRAVITY 0
53 #if OLD_COLLISION_HANDLER_GRAVITY
54 PN_stdfloat CollisionHandlerGravity::
55 set_highest_collision(
const NodePath &target_node_path,
const NodePath &from_node_path,
const Entries &entries) {
58 PN_stdfloat max_height = 0.0f;
61 Entries::const_iterator ei;
62 for (ei = entries.begin(); ei != entries.end(); ++ei) {
64 nassertr(entry !=
nullptr, 0.0f);
69 if (collide_cat.is_debug()) {
71 <<
"Intersection point detected at " << point <<
"\n";
74 PN_stdfloat height = point[2];
75 if (!got_max || height > max_height) {
85 cout<<
"\ncolliding with:\n";
86 for (Colliding::const_iterator i = _current_colliding.begin(); i != _current_colliding.end(); ++i) {
90 highest->write(cout, 2);
97 _current_colliding.clear();
105 PN_stdfloat CollisionHandlerGravity::
106 set_highest_collision(
const NodePath &target_node_path,
const NodePath &from_node_path,
const Entries &entries) {
111 bool got_max =
false;
112 bool got_min =
false;
113 PN_stdfloat max_height = 0.0f;
114 PN_stdfloat min_height = 0.0f;
120 Entries::const_iterator ei;
121 for (ei = entries.begin(); ei != entries.end(); ++ei) {
123 nassertr(entry !=
nullptr, 0.0f);
128 if (collide_cat.is_debug()) {
130 <<
"Intersection point detected at " << point <<
"\n";
132 PN_stdfloat height = point[2];
133 if(height < _offset + _reach) {
134 valid_entries.push_back(entry);
135 if (!got_max || height > max_height) {
141 if (!got_min || height < min_height) {
148 if (!got_max && got_min) {
152 max_height = min_height;
154 valid_entries.push_back(lowest);
159 cout<<
"\ncolliding with:\n";
160 for (Colliding::const_iterator i = _current_colliding.begin(); i != _current_colliding.end(); ++i) {
161 (**i).write(cout, 2);
163 cout<<
"\nhighest:\n";
164 highest->write(cout, 2);
170 _current_colliding.clear();
177 for (vi = valid_entries.begin(); vi != valid_entries.end(); ++vi) {
185 if (highest->
get_into()->is_of_type(CollisionPlane::get_class_type())) {
215 bool CollisionHandlerGravity::
219 FromEntries::const_iterator fi;
220 for (fi = _from_entries.begin(); fi != _from_entries.end(); ++fi) {
221 const NodePath &from_node_path = (*fi).first;
222 const Entries &entries = (*fi).second;
224 Colliders::iterator ci;
225 ci = _colliders.
find(from_node_path);
226 if (ci == _colliders.end()) {
230 << get_type() <<
" doesn't know about "
231 << from_node_path <<
", disabling.\n";
234 ColliderDef &def = (*ci).second;
235 PN_stdfloat max_height = set_highest_collision(def._target, from_node_path, entries);
238 #if OLD_COLLISION_HANDLER_GRAVITY
239 PN_stdfloat adjust = max_height + _offset;
241 PN_stdfloat adjust = max_height + _offset;
243 if (_current_velocity > 0.0f || !IS_THRESHOLD_ZERO(adjust, 0.001)) {
244 if (collide_cat.is_debug()) {
246 <<
"Adjusting height by " << adjust <<
"\n";
249 if (_current_velocity > 0.0f || adjust) {
255 PN_stdfloat gravity_adjust = _current_velocity * dt + 0.5 * -_gravity * dt * dt;
260 adjust += std::max((PN_stdfloat)0.0, gravity_adjust);
263 adjust = std::max(adjust, gravity_adjust);
265 _current_velocity -= _gravity * dt;
267 _airborne_height = -(max_height + _offset) + adjust;
268 assert(_airborne_height >= -0.001f);
271 if (_airborne_height < 0.001f && _current_velocity < 0.001f) {
273 _impact_velocity = _current_velocity;
275 _current_velocity = _airborne_height = 0.0f;
276 }
else if (_legacy_mode) {
278 _current_colliding.clear();
282 LVecBase3 pos = trans->get_pos();
284 def._target.set_transform(trans->set_pos(pos));
285 def.updated_transform();
287 apply_linear_force(def, LVector3(0.0f, 0.0f, adjust));
290 _current_velocity = _airborne_height = 0.0f;
291 if (collide_cat.is_spam()) {
293 <<
"Leaving height unchanged.\n";
305 void CollisionHandlerGravity::
306 apply_linear_force(ColliderDef &def,
const LVector3 &force) {