286 lines
9.8 KiB
C++
286 lines
9.8 KiB
C++
/**
|
|
* PANDA 3D SOFTWARE
|
|
* Copyright (c) Carnegie Mellon University. All rights reserved.
|
|
*
|
|
* All use of this software is subject to the terms of the revised BSD
|
|
* license. You should have received a copy of this license along
|
|
* with this source code in a file named "LICENSE."
|
|
*
|
|
* @file collisionHandlerPusher.cxx
|
|
* @author drose
|
|
* @date 2002-03-16
|
|
*/
|
|
|
|
#include "collisionHandlerPusher.h"
|
|
#include "collisionNode.h"
|
|
#include "collisionEntry.h"
|
|
#include "collisionPolygon.h"
|
|
#include "config_collide.h"
|
|
#include "dcast.h"
|
|
#include "epvector.h"
|
|
|
|
TypeHandle CollisionHandlerPusher::_type_handle;
|
|
|
|
/**
|
|
* The ShoveData class is used within
|
|
* CollisionHandlerPusher::handle_entries(), to track multiple shoves onto a
|
|
* given collider. It's not exported outside this file.
|
|
*/
|
|
class ShoveData {
|
|
public:
|
|
LVector3 _vector;
|
|
PN_stdfloat _length;
|
|
bool _valid;
|
|
CollisionEntry *_entry;
|
|
};
|
|
|
|
/**
|
|
*
|
|
*/
|
|
CollisionHandlerPusher::
|
|
CollisionHandlerPusher() {
|
|
_horizontal = pushers_horizontal;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
CollisionHandlerPusher::
|
|
~CollisionHandlerPusher() {
|
|
}
|
|
|
|
/**
|
|
* Serializes this object, to implement pickle support.
|
|
*/
|
|
void CollisionHandlerPusher::
|
|
write_datagram(Datagram &dg) const {
|
|
CollisionHandlerPhysical::write_datagram(dg);
|
|
|
|
dg.add_bool(_horizontal);
|
|
}
|
|
|
|
/**
|
|
* Restores the object state from the given datagram, previously obtained using
|
|
* __getstate__.
|
|
*/
|
|
void CollisionHandlerPusher::
|
|
read_datagram(DatagramIterator &scan) {
|
|
CollisionHandlerPhysical::read_datagram(scan);
|
|
|
|
_horizontal = scan.get_bool();
|
|
}
|
|
|
|
/**
|
|
* Called by the parent class after all collisions have been detected, this
|
|
* manages the various collisions and moves around the nodes as necessary.
|
|
*
|
|
* The return value is normally true, but it may be false to indicate the
|
|
* CollisionTraverser should disable this handler from being called in the
|
|
* future.
|
|
*/
|
|
bool CollisionHandlerPusher::
|
|
handle_entries() {
|
|
bool okflag = true;
|
|
|
|
FromEntries::const_iterator fi;
|
|
for (fi = _from_entries.begin(); fi != _from_entries.end(); ++fi) {
|
|
const NodePath &from_node_path = (*fi).first;
|
|
const Entries &entries = (*fi).second;
|
|
|
|
Colliders::iterator ci;
|
|
ci = _colliders.find(from_node_path);
|
|
if (ci == _colliders.end()) {
|
|
// Hmm, someone added a CollisionNode to a traverser and gave it this
|
|
// CollisionHandler pointer--but they didn't tell us about the node.
|
|
collide_cat.error()
|
|
<< "CollisionHandlerPusher doesn't know about "
|
|
<< from_node_path << ", disabling.\n";
|
|
okflag = false;
|
|
} else {
|
|
ColliderDef &def = (*ci).second;
|
|
{
|
|
// How to apply multiple shoves from different solids onto the same
|
|
// collider? One's first intuition is to vector sum all the shoves.
|
|
// However, this causes problems when two parallel walls shove on the
|
|
// collider, because we end up with a double shove. We hack around
|
|
// this by testing if two shove vectors share nearly the same
|
|
// direction, and if so, we keep only the longer of the two.
|
|
|
|
typedef epvector<ShoveData> Shoves;
|
|
Shoves shoves;
|
|
|
|
Entries::const_iterator ei;
|
|
for (ei = entries.begin(); ei != entries.end(); ++ei) {
|
|
CollisionEntry *entry = (*ei);
|
|
nassertr(entry != nullptr, false);
|
|
nassertr(from_node_path == entry->get_from_node_path(), false);
|
|
|
|
LPoint3 surface_point;
|
|
LVector3 normal;
|
|
LPoint3 interior_point;
|
|
|
|
if (!entry->get_all(def._target, surface_point, normal, interior_point)) {
|
|
#ifndef NDEBUG
|
|
if (collide_cat.is_debug()) {
|
|
collide_cat.debug()
|
|
<< "Cannot shove on " << from_node_path << " for collision into "
|
|
<< entry->get_into_node_path() << "; no normal/depth information.\n";
|
|
}
|
|
#endif
|
|
} else {
|
|
// Shove it just enough to clear the volume.
|
|
if (!surface_point.almost_equal(interior_point)) {
|
|
if (_horizontal) {
|
|
normal[2] = 0.0f;
|
|
}
|
|
// Just to be on the safe size, we normalize the normal vector,
|
|
// even though it really ought to be unit-length already (unless
|
|
// we just forced it horizontal, above).
|
|
normal.normalize();
|
|
|
|
ShoveData sd;
|
|
sd._vector = normal;
|
|
sd._length = (surface_point - interior_point).length();
|
|
sd._valid = true;
|
|
sd._entry = entry;
|
|
|
|
#ifndef NDEBUG
|
|
if (collide_cat.is_debug()) {
|
|
collide_cat.debug()
|
|
<< "Shove on " << from_node_path << " from "
|
|
<< entry->get_into_node_path() << ": " << sd._vector
|
|
<< " times " << sd._length << "\n";
|
|
}
|
|
#endif
|
|
|
|
shoves.push_back(sd);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!shoves.empty()) {
|
|
// Now we look for two shoves that are largely in the same
|
|
// direction, so we can combine them into a single shove of the same
|
|
// magnitude; we also check for two shoves at 90 degrees, so we can
|
|
// detect whether we are hitting an inner or an outer corner.
|
|
|
|
Shoves::iterator si;
|
|
for (si = shoves.begin(); si != shoves.end(); ++si) {
|
|
ShoveData &sd = (*si);
|
|
Shoves::iterator sj;
|
|
for (sj = shoves.begin(); sj != si; ++sj) {
|
|
ShoveData &sd2 = (*sj);
|
|
if (sd2._valid) {
|
|
PN_stdfloat d = sd._vector.dot(sd2._vector);
|
|
if (collide_cat.is_debug()) {
|
|
collide_cat.debug()
|
|
<< "Considering dot product " << d << "\n";
|
|
}
|
|
|
|
if (d > 0.9) {
|
|
// These two shoves are largely in the same direction; save
|
|
// the larger of the two.
|
|
if (sd2._length < sd._length) {
|
|
sd2._valid = false;
|
|
} else {
|
|
sd._valid = false;
|
|
}
|
|
} else {
|
|
// These two shoves are not in the same direction. If they
|
|
// are both from polygons that are a child of the same node,
|
|
// try to determine the shape of the corner (convex or
|
|
// concave).
|
|
const CollisionSolid *s1 = sd._entry->get_into();
|
|
const CollisionSolid *s2 = sd2._entry->get_into();
|
|
if (s1 != nullptr &&
|
|
s2 != nullptr &&
|
|
s1->is_of_type(CollisionPolygon::get_class_type()) &&
|
|
s2->is_of_type(CollisionPolygon::get_class_type()) &&
|
|
sd._entry->get_into_node_path() ==
|
|
sd2._entry->get_into_node_path()) {
|
|
const CollisionPolygon *p1 = DCAST(CollisionPolygon, s1);
|
|
const CollisionPolygon *p2 = DCAST(CollisionPolygon, s2);
|
|
if (p1->dist_to_plane(p2->get_collision_origin()) < 0 &&
|
|
p2->dist_to_plane(p1->get_collision_origin()) < 0) {
|
|
// Each polygon is behind the other one. That means we
|
|
// have a convex corner, and therefore we should discard
|
|
// one of the shoves (or the user will get stuck coming
|
|
// at a convex corner).
|
|
if (collide_cat.is_debug()) {
|
|
collide_cat.debug()
|
|
<< "Discarding shove from convex corner.\n";
|
|
}
|
|
|
|
// This time, unlike the case of two parallel walls
|
|
// above, we discard the larger of the two shoves, not
|
|
// the smaller. This is because as we slide off the
|
|
// convex corner, the wall we are sliding away from will
|
|
// get a bigger and bigger shove--and we need to keep
|
|
// ignoring the same wall as we slide.
|
|
if (sd2._length < sd._length) {
|
|
sd._valid = false;
|
|
} else {
|
|
sd2._valid = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Now we can determine the net shove.
|
|
LVector3 net_shove(0.0f, 0.0f, 0.0f);
|
|
LVector3 force_normal(0.0f, 0.0f, 0.0f);
|
|
for (si = shoves.begin(); si != shoves.end(); ++si) {
|
|
const ShoveData &sd = (*si);
|
|
if (sd._valid) {
|
|
net_shove += sd._vector * sd._length;
|
|
force_normal += sd._vector;
|
|
}
|
|
}
|
|
|
|
#ifndef NDEBUG
|
|
if (collide_cat.is_debug()) {
|
|
collide_cat.debug()
|
|
<< "Net shove on " << from_node_path << " is: "
|
|
<< net_shove << "\n";
|
|
}
|
|
#endif
|
|
|
|
// This is the part where the node actually gets moved:
|
|
CPT(TransformState) trans = def._target.get_transform();
|
|
LVecBase3 pos = trans->get_pos();
|
|
pos += net_shove * trans->get_mat();
|
|
def._target.set_transform(trans->set_pos(pos));
|
|
def.updated_transform();
|
|
|
|
// We call this to allow derived classes to do other fix-ups as they
|
|
// see fit:
|
|
apply_net_shove(def, net_shove, force_normal);
|
|
apply_linear_force(def, force_normal);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return okflag;
|
|
}
|
|
|
|
/**
|
|
* This is an optional hook for derived classes to do some work with the
|
|
* ColliderDef and the force vector.
|
|
*/
|
|
void CollisionHandlerPusher::
|
|
apply_net_shove(ColliderDef &def, const LVector3 &net_shove,
|
|
const LVector3 &force_normal) {
|
|
}
|
|
|
|
/**
|
|
* This is an optional hook for derived classes to do some work with the
|
|
* ColliderDef and the force vector.
|
|
*/
|
|
void CollisionHandlerPusher::
|
|
apply_linear_force(ColliderDef &def, const LVector3 &force_normal) {
|
|
}
|