TheCloud/panda3d
1
0
Fork 0
mirror of https://github.com/panda3d/panda3d.git synced 2025-10-04 10:54:24 -04:00
Code Issues Packages Projects Releases Wiki Activity

fluid pusher: bug fixes, cleaner code

Browse Source
This commit is contained in:
Darren Ranalli 2007-08-28 23:53:23 +00:00
parent 0ee7786c2f
commit f3b30fa975
7 changed files with 334 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
panda/src/collide/collisionEntry.I
View File

@ -25,7 +25,8 @@
INLINE CollisionEntry::
CollisionEntry() {
_flags = 0;
_t = 1.f;
// > 1. means collision didn't happen
_t = 2.f;
}
////////////////////////////////////////////////////////////////////
@ -148,6 +149,29 @@ get_t() const {
return _t;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::collided
// Access: Published
// Description: returns true if this represents an actual collision
// as opposed to a potential collision, needed for
// iterative collision resolution where path of
// collider changes mid-frame
////////////////////////////////////////////////////////////////////
INLINE bool CollisionEntry::
collided() const {
return ((0.f <= _t) && (_t <= 1.f));
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::reset_collided
// Access: Published
// Description: prepare for another collision test
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
reset_collided() {
_t = 2.f;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::get_respect_prev_transform
// Access: Published
@ -250,6 +274,62 @@ has_interior_point() const {
return (_flags & F_has_interior_point) != 0;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::set_contact_point
// Access: Published
// Description: Stores the point, on the surface of the "into"
// object, at which the collision is first detected.
//
// This point is specified in the coordinate space of
// the "into" object.
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_contact_point(const LPoint3f &point) {
_contact_point = point;
_flags |= F_has_contact_point;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::set_contact_normal
// Access: Published
// Description: Stores the surface normal of the "into" object at the
// contact point.
//
// This normal is specified in the coordinate space of
// the "into" object.
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_contact_normal(const LVector3f &normal) {
_contact_normal = normal;
_flags |= F_has_contact_normal;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::has_contact_point
// Access: Published
// Description: Returns true if the contact point has been specified,
// false otherwise. See get_contact_point(). Some
// types of collisions may not compute the contact
// point.
////////////////////////////////////////////////////////////////////
INLINE bool CollisionEntry::
has_contact_point() const {
return (_flags & F_has_contact_point) != 0;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::has_contact_normal
// Access: Published
// Description: Returns true if the contact normal has been specified,
// false otherwise. See get_contact_normal(). Some
// types of collisions may not compute the contact
// normal.
////////////////////////////////////////////////////////////////////
INLINE bool CollisionEntry::
has_contact_normal() const {
return (_flags & F_has_contact_normal) != 0;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::get_wrt_space
// Access: Public
@ -364,6 +444,12 @@ test_intersection(CollisionHandler *record,
#ifdef DO_PSTATS
((CollisionSolid *)get_into())->get_test_pcollector().add_level(1);
#endif // DO_PSTATS
// if there was no collision detected but the handler wants to know about all
// potential collisions, create a "didn't collide" collision entry for it
if (record->wants_all_potential_collidees() && result == (CollisionEntry *)NULL) {
result = new CollisionEntry(*this);
result->reset_collided();
}
if (result != (CollisionEntry *)NULL) {
record->add_entry(result);
}

81
panda/src/collide/collisionEntry.cxx
View File

@ -40,7 +40,9 @@ CollisionEntry(const CollisionEntry &copy) :
_flags(copy._flags),
_surface_point(copy._surface_point),
_surface_normal(copy._surface_normal),
_interior_point(copy._interior_point)
_interior_point(copy._interior_point),
_contact_point(copy._contact_point),
_contact_normal(copy._contact_normal)
{
}
@ -63,6 +65,8 @@ operator = (const CollisionEntry &copy) {
_surface_point = copy._surface_point;
_surface_normal = copy._surface_normal;
_interior_point = copy._interior_point;
_contact_point = copy._contact_point;
_contact_normal = copy._contact_normal;
}
////////////////////////////////////////////////////////////////////
@ -71,6 +75,8 @@ operator = (const CollisionEntry &copy) {
// Description: Returns the point, on the surface of the "into"
// object, at which a collision is detected. This can
// be thought of as the first point of intersection.
// However the contact point is the actual first point of
// intersection.
//
// The point will be converted into whichever coordinate
// space the caller specifies.
@ -162,6 +168,79 @@ get_all(const NodePath &space, LPoint3f &surface_point,
return all_ok;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::get_contact_point
// Access: Published
// Description: Returns the point, on the surface of the "into"
// object, at which a collision is detected. This can
// be thought of as the first point of intersection.
// The surface point is not always the initial point of
// intersection. We preserve the original implementation
// of surface_point detection so that the existing
// collision response code will still work, and provide
// the contact_point for collision response code that
// needs precise collision information.
//
// The point will be converted into whichever coordinate
// space the caller specifies.
////////////////////////////////////////////////////////////////////
LPoint3f CollisionEntry::
get_contact_point(const NodePath &space) const {
nassertr(has_contact_point(), LPoint3f::zero());
CPT(TransformState) transform = _into_node_path.get_transform(space);
return _contact_point * transform->get_mat();
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::get_contact_normal
// Access: Published
// Description: Returns the surface normal of the "into" object at
// the contact point.
//
// The normal will be converted into whichever coordinate
// space the caller specifies.
////////////////////////////////////////////////////////////////////
LVector3f CollisionEntry::
get_contact_normal(const NodePath &space) const {
nassertr(has_contact_normal(), LVector3f::zero());
CPT(TransformState) transform = _into_node_path.get_transform(space);
return _contact_normal * transform->get_mat();
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::get_all_contact_info
// Access: Published
// Description: Simultaneously transforms the surface point, surface
// normal, and interior point of the collision into the
// indicated coordinate space.
//
// Returns true if all three properties are available,
// or false if any one of them is not.
////////////////////////////////////////////////////////////////////
bool CollisionEntry::
get_all_contact_info(const NodePath &space, LPoint3f &contact_point,
LVector3f &contact_normal) const {
CPT(TransformState) transform = _into_node_path.get_transform(space);
const LMatrix4f &mat = transform->get_mat();
bool all_ok = true;
if (!has_contact_point()) {
contact_point = LPoint3f::zero();
all_ok = false;
} else {
contact_point = _contact_point * mat;
}
if (!has_contact_normal()) {
contact_normal = LVector3f::zero();
all_ok = false;
} else {
contact_normal = _contact_normal * mat;
}
return all_ok;
}
////////////////////////////////////////////////////////////////////
// Function: CollisionEntry::output
// Access: Published

19
panda/src/collide/collisionEntry.h
View File

@ -65,6 +65,8 @@ PUBLISHED:
INLINE void set_t(float t);
INLINE float get_t() const;
INLINE bool collided() const;
INLINE void reset_collided();
INLINE bool get_respect_prev_transform() const;
@ -76,6 +78,12 @@ PUBLISHED:
INLINE bool has_surface_normal() const;
INLINE bool has_interior_point() const;
INLINE void set_contact_point(const LPoint3f &point);
INLINE void set_contact_normal(const LVector3f &normal);
INLINE bool has_contact_point() const;
INLINE bool has_contact_normal() const;
LPoint3f get_surface_point(const NodePath &space) const;
LVector3f get_surface_normal(const NodePath &space) const;
LPoint3f get_interior_point(const NodePath &space) const;
@ -84,6 +92,12 @@ PUBLISHED:
LVector3f &surface_normal,
LPoint3f &interior_point) const;
LPoint3f get_contact_point(const NodePath &space) const;
LVector3f get_contact_normal(const NodePath &space) const;
bool get_all_contact_info(const NodePath &space,
LPoint3f &contact_point,
LVector3f &contact_normal) const;
void output(ostream &out) const;
void write(ostream &out, int indent_level = 0) const;
@ -119,6 +133,8 @@ private:
F_has_interior_point = 0x0004,
F_respect_prev_transform = 0x0008,
F_checked_clip_planes = 0x0010,
F_has_contact_point = 0x0020,
F_has_contact_normal = 0x0040,
};
int _flags;
@ -127,6 +143,9 @@ private:
LVector3f _surface_normal;
LPoint3f _interior_point;
LPoint3f _contact_point;
LVector3f _contact_normal;
public:
static TypeHandle get_class_type() {
return _type_handle;

200
panda/src/collide/collisionHandlerFluidPusher.cxx
View File

@ -55,7 +55,9 @@ add_entry(CollisionEntry *entry) {
(!entry->has_into() || entry->get_into()->is_tangible())) {
_from_entries[entry->get_from_node_path()].push_back(entry);
_has_contact = true;
if (entry->collided()) {
_has_contact = true;
}
}
}
@ -102,7 +104,12 @@ handle_entries() {
7. go to 2
*/
bool okflag = true;
// if all we got was potential collisions, don't bother
if (!_has_contact) {
return okflag;
}
if (!_horizontal) {
collide_cat.error() << "collisionHandlerFluidPusher::handle_entries is only supported in "
"horizontal mode" << endl;
@ -113,7 +120,7 @@ handle_entries() {
FromEntries::iterator fei;
for (fei = _from_entries.begin(); fei != _from_entries.end(); ++fei) {
NodePath from_node_path = fei->first;
Entries *entries_ptr = &fei->second;
Entries *orig_entries = &fei->second;
Colliders::iterator ci;
ci = _colliders.find(from_node_path);
@ -129,23 +136,14 @@ handle_entries() {
ColliderDef &def = (*ci).second;
// extract the collision entries into a vector that we can safely modify
Entries entries(*entries_ptr);
Entries entries(*orig_entries);
// extract out the initial set of collision solids
CollisionSolids SCS;
Entries::iterator ei;
for (ei = entries.begin(); ei != entries.end(); ++ei) {
SCS.push_back((*ei)->get_into());
}
// make a copy of the original collision entries that we can use to re-test the collisions
Entries SCS(*orig_entries);
// currently we only support spheres as the collider
const CollisionSphere *sphere;
DCAST_INTO_R(sphere, (*entries.front()).get_from(), 0);
// make a copy of the original from_nodepath that we can mess with
// in the process of calculating the final position
_from_node_path_copy = from_node_path.copy_to(from_node_path.get_parent());
DCAST_INTO_R(sphere, entries.front()->get_from(), 0);
// this is the original position delta for the entire frame, before collision response
LPoint3f M(from_node_path.get_pos_delta(*_root));
@ -154,10 +152,19 @@ handle_entries() {
}
// this is used to track position deltas every time we collide against a solid
LPoint3f N(M);
//collide_cat.info() << "N: " << N << endl;
const LPoint3f orig_pos(from_node_path.get_pos(*_root));
CPT(TransformState) prev_trans(from_node_path.get_prev_transform(*_root));
const LPoint3f orig_prev_pos(prev_trans->get_pos());
//collide_cat.info() << "orig_pos: " << orig_pos << endl;
//collide_cat.info() << "orig_prev_pos: " << orig_prev_pos << endl;
// this will hold the final calculated position at each iteration
LPoint3f PosX(orig_pos);
LPoint3f candidate_final_pos(orig_pos);
// this holds the position before reacting to collisions
LPoint3f uncollided_pos(candidate_final_pos);
//collide_cat.info() << "candidate_final_pos: " << candidate_final_pos << endl;
// unit vector facing back into original direction of motion
LVector3f reverse_vec(-M);
@ -165,25 +172,30 @@ handle_entries() {
reverse_vec[2] = 0.0f;
}
reverse_vec.normalize();
//collide_cat.info() << "reverse_vec: " << reverse_vec << endl;
// unit vector pointing out to the right relative to the direction of motion,
// looking into the direction of motion
const LVector3f right_unit(LVector3f::up().cross(reverse_vec));
//collide_cat.info() << "right_unit: " << right_unit << endl;
// if both of these become true, we're stuck in a 'corner'
bool left_halfspace_obstructed = false;
bool right_halfspace_obstructed = false;
LVector3f left_halfspace_normal;
LVector3f right_halfspace_normal;
float left_plane_dot = 200.0f;
float right_plane_dot = 200.0f;
// iterate until the mover runs out of movement or gets stuck
while (true) {
CollisionEntry *C = 0;
const CollisionEntry *C = 0;
// find the first (earliest) collision
for (ei = entries.begin(); ei != entries.end(); ++ei) {
CollisionEntry *entry = (*ei);
Entries::const_iterator cei;
for (cei = entries.begin(); cei != entries.end(); ++cei) {
const CollisionEntry *entry = (*cei);
nassertr(entry != (CollisionEntry *)NULL, false);
if ((C == 0) || (entry->get_t() < C->get_t())) {
if (entry->collided() && ((C == 0) || (entry->get_t() < C->get_t()))) {
nassertr(from_node_path == entry->get_from_node_path(), false);
C = entry;
break;
@ -195,105 +207,137 @@ handle_entries() {
break;
}
// calculate point of collision, move back to it
LPoint3f surface_point;
LVector3f surface_normal;
LPoint3f interior_point;
//collide_cat.info() << "t: " << C->get_t() << endl;
if (!C->get_all(def._target, surface_point, surface_normal, interior_point)) {
// move back to initial contact point
LPoint3f contact_point;
LVector3f contact_normal;
if (!C->get_all_contact_info(*_root, contact_point, contact_normal)) {
collide_cat.warning()
<< "Cannot shove on " << from_node_path << " for collision into "
<< C->get_into_node_path() << "; no normal/depth information.\n";
<< C->get_into_node_path() << "; no contact point/normal information.\n";
break;
}
uncollided_pos = candidate_final_pos;
candidate_final_pos[0] = contact_point[0];
candidate_final_pos[1] = contact_point[1];
//collide_cat.info() << "contact_point: " << contact_point << endl;
LVector3f proj_surface_normal(contact_normal);
if (_horizontal) {
surface_normal[2] = 0.0f;
surface_normal.normalize();
proj_surface_normal[2] = 0.0f;
}
collide_cat.info() << "normal: " << surface_normal << endl;
//collide_cat.info() << "normal: " << contact_normal << endl;
//collide_cat.info() << "proj_surface_normal: " << proj_surface_normal << endl;
// move back to the initial point of contact
_from_node_path_copy.set_pos(*_root, _from_node_path_copy.get_pos(*_root) +
-N * (1.0f - C->get_t()));
// calculate new position given that you collided with this thing
PosX += (surface_point - interior_point).length() * surface_normal;
LVector3f norm_proj_surface_normal(proj_surface_normal);
norm_proj_surface_normal.normalize();
//collide_cat.info() << "norm_proj_surface_normal: " << norm_proj_surface_normal << endl;
// check to see if we're stuck, given this collision
float dot = right_unit.dot(surface_normal);
float dot = right_unit.dot(norm_proj_surface_normal);
//collide_cat.info() << "dot: " << dot << endl;
if (dot > 0.0f) {
// positive dot means plane is coming from the left (looking along original
// direction of motion)
if (right_halfspace_obstructed) {
// we have obstructions from both directions, we're stuck
break;
}
left_halfspace_obstructed = true;
if (dot < left_plane_dot) {
if (right_halfspace_obstructed) {
// we have obstructions from both directions, we're stuck
break;
}
left_halfspace_obstructed = true;
left_halfspace_normal = norm_proj_surface_normal;
} else {
// detected collision has a steeper plane wrt fwd motion than a previous collision
// continue colliding against the shallower plane
norm_proj_surface_normal = left_halfspace_normal;
}
} else {
// negative dot means plane is coming from the right (looking along original
// direction of motion)
if (left_halfspace_obstructed) {
// we have obstructions from both directions, we're stuck
break;
}
right_halfspace_obstructed = true;
dot = -dot;
if (dot < right_plane_dot) {
if (left_halfspace_obstructed) {
// we have obstructions from both directions, we're stuck
break;
}
right_halfspace_obstructed = true;
right_halfspace_normal = norm_proj_surface_normal;
} else {
// detected collision has a steeper plane wrt fwd motion than a previous collision
// continue colliding against the shallower plane
norm_proj_surface_normal = right_halfspace_normal;
}
}
// calculate new position given that you collided with this thing
// project the final position onto the plane of the obstruction
LVector3f blocked_movement(uncollided_pos - contact_point);
if (_horizontal) {
blocked_movement[2] = 0.0f;
}
//collide_cat.info() << "blocked movement: " << blocked_movement << endl;
candidate_final_pos += (norm_proj_surface_normal *
-blocked_movement.dot(norm_proj_surface_normal));
// this is how the regular pusher pushes
//candidate_final_pos += (contact_point - interior_point).length() * norm_proj_surface_normal;
//collide_cat.info() << "candidate_final_pos: " << candidate_final_pos << endl;
// set up new current/last positions, re-calculate collisions
CPT(TransformState) prev_trans(_from_node_path_copy.get_prev_transform(*_root));
prev_trans->set_pos(_from_node_path_copy.get_pos(*_root));
_from_node_path_copy.set_prev_transform(*_root, prev_trans);
_from_node_path_copy.set_pos(*_root, PosX);
from_node_path.set_pos(*_root, candidate_final_pos);
CPT(TransformState) prev_trans(from_node_path.get_prev_transform(*_root));
prev_trans->set_pos(contact_point);
from_node_path.set_prev_transform(*_root, prev_trans);
// recalculate the position delta
N = _from_node_path_copy.get_pos_delta(*_root);
collide_cat.info() << "N: " << N << endl;
N = from_node_path.get_pos_delta(*_root);
if (_horizontal) {
N[2] = 0.0f;
}
//collide_cat.info() << "N: " << N << endl;
// calculate new collisions given new movement vector
CollisionEntry new_entry;
new_entry._from_node_path = _from_node_path_copy;
new_entry._from = sphere;
entries.clear();
CollisionSolids::iterator csi;
for (csi = SCS.begin(); csi != SCS.end(); ++csi) {
PT(CollisionEntry) result = (*csi)->test_intersection_from_sphere(new_entry);
if (result != (CollisionEntry *)NULL) {
collide_cat.info() << "new collision" << endl;
entries.push_back(result);
}
}
// for (cei = SCS.begin(); cei != SCS.end(); ++cei) {
// *cei->reset_collided();
// PT(CollisionEntry) result = (*cei)->get_from()->test_intersection(**cei);
// if (result != (CollisionEntry *)NULL) {
// collide_cat.info() << "new collision" << endl;
// entries.push_back(result);
// }
// }
}
LVector3f net_shove(PosX - orig_pos);
if (_horizontal) {
net_shove[2] = 0.0f;
}
// put things back where they were
from_node_path.set_pos(*_root, orig_pos);
// restore the appropriate previous position
prev_trans = from_node_path.get_prev_transform(*_root);
prev_trans->set_pos(orig_prev_pos);
from_node_path.set_prev_transform(*_root, prev_trans);
LVector3f net_shove(candidate_final_pos - orig_pos);
LVector3f force_normal(net_shove);
force_normal.normalize();
collide_cat.info() << "PosX: " << PosX << endl;
collide_cat.info() << "orig_pos: " << orig_pos << endl;
collide_cat.info() << "net_shove: " << net_shove << endl;
collide_cat.info() << endl;
//collide_cat.info() << "candidate_final_pos: " << candidate_final_pos << endl;
//collide_cat.info() << "orig_pos: " << orig_pos << endl;
//collide_cat.info() << "net_shove: " << net_shove << endl;
// This is the part where the node actually gets moved:
CPT(TransformState) trans = def._target.get_transform(*_root);
LVecBase3f pos = trans->get_pos();
pos += net_shove * trans->get_mat();
def._target.set_transform(*_root, trans->set_pos(pos));
def.updated_transform();
def._target.set_pos(*_root, candidate_final_pos);
// 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);
//collide_cat.info() << endl;
}
}

4
panda/src/collide/collisionHandlerFluidPusher.h
View File

@ -38,12 +38,8 @@ public:
virtual void add_entry(CollisionEntry *entry);
protected:
typedef pvector< CPT(CollisionSolid) > CollisionSolids;
virtual bool handle_entries();
NodePath _from_node_path_copy;
public:
static TypeHandle get_class_type() {
return _type_handle;

32
panda/src/collide/collisionPolygon.cxx
View File

@ -415,6 +415,13 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
LPoint3f from_center = orig_center;
bool moved_from_center = false;
float t = 1.0f;
LPoint3f contact_point(from_center);
float actual_t = 1.0f;
LVector3f from_radius_v =
LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
float from_radius_2 = from_radius_v.length_squared();
float from_radius = csqrt(from_radius_2);
if (wrt_prev_space != wrt_space) {
// If we have a delta between the previous position and the
@ -440,15 +447,25 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
// at the point along its path that is closest to intersecting
// the plane. This may be the actual intersection point, or it
// may be the starting point or the final point.
t = -(dist_to_plane(a) / dot);
// dot is equal to the (negative) magnitude of 'delta' along the
// direction of the plane normal
// t = ratio of (distance from start pos to plane) to (distance
// from start pos to end pos), along axis of plane normal
float dist_to_p = dist_to_plane(a);
t = (dist_to_p / -dot);
// also compute the actual contact point and time of contact
// for handlers that need it
actual_t = ((dist_to_p - from_radius) / -dot);
actual_t = min(1.0f, max(0.0f, actual_t));
contact_point = a + (actual_t * delta);
if (t >= 1.0f) {
// Leave it where it is.
t = 1.0f;
} else if (t < 0.0f) {
from_center = a;
moved_from_center = true;
t = 0.0f;
} else {
from_center = a + t * delta;
@ -457,11 +474,6 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
}
}
LVector3f from_radius_v =
LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
float from_radius_2 = from_radius_v.length_squared();
float from_radius = csqrt(from_radius_2);
LVector3f normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
#ifndef NDEBUG
if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) {
@ -559,7 +571,9 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
new_entry->set_surface_normal(normal);
new_entry->set_surface_point(from_center - normal * dist);
new_entry->set_interior_point(from_center - normal * (dist + into_depth));
new_entry->set_t(t);
new_entry->set_contact_point(contact_point);
new_entry->set_contact_normal(get_normal());
new_entry->set_t(actual_t);
return new_entry;
}

4
panda/src/collide/collisionTraverser.cxx
View File

@ -288,7 +288,9 @@ traverse(const NodePath &root) {
Handlers::iterator hi;
for (hi = _handlers.begin(); hi != _handlers.end(); ++hi) {
(*hi).first->set_root(root);
if ((*hi).first->wants_all_potential_collidees()) {
(*hi).first->set_root(root);
}
(*hi).first->begin_group();
}