diff --git a/panda/src/collide/collisionEntry.I b/panda/src/collide/collisionEntry.I index 7e8fec5c28..45d640cfc2 100644 --- a/panda/src/collide/collisionEntry.I +++ b/panda/src/collide/collisionEntry.I @@ -25,11 +25,6 @@ INLINE CollisionEntry:: CollisionEntry() { _flags = 0; - _from_space = TransformState::make_identity(); - _into_space = TransformState::make_identity(); - _wrt_space = TransformState::make_identity(); - _inv_wrt_space = TransformState::make_identity(); - _wrt_prev_space = TransformState::make_identity(); } //////////////////////////////////////////////////////////////////// @@ -131,134 +126,105 @@ get_into_node_path() const { } //////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_from_space +// Function: CollisionEntry::get_respect_prev_transform // Access: Published -// Description: Returns the global coordinate space of the -// CollisionNode returned by get_from_node(), as of the -// time of the collision. This will be equivalent to a -// wrt() from the node to render. +// Description: Returns true if the collision was detected by a +// CollisionTraverser whose respect_prev_transform +// flag was set true, meaning we should consider motion +// significant in evaluating collisions. //////////////////////////////////////////////////////////////////// -INLINE const TransformState *CollisionEntry:: -get_from_space() const { - return _from_space; +INLINE bool CollisionEntry:: +get_respect_prev_transform() const { + return (_flags & F_respect_prev_transform) != 0; } -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_into_space -// Access: Published -// Description: Returns the global coordinate space of the -// CollisionNode or GeomNode returned by -// get_into_node(), as of the time of the collision. -//////////////////////////////////////////////////////////////////// -INLINE const TransformState *CollisionEntry:: -get_into_space() const { - return _into_space; -} //////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_wrt_space +// Function: CollisionEntry::set_surface_point // Access: Published -// Description: Returns the relative transform of the from node as -// seen from the into node. -//////////////////////////////////////////////////////////////////// -INLINE const TransformState *CollisionEntry:: -get_wrt_space() const { - return _wrt_space; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_inv_wrt_space -// Access: Published -// Description: Returns the relative transform of the into node as -// seen from the from node. -//////////////////////////////////////////////////////////////////// -INLINE const TransformState *CollisionEntry:: -get_inv_wrt_space() const { - return _inv_wrt_space; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_wrt_prev_space -// Access: Published -// Description: Returns the relative transform of the from node as -// seen from the into node, as of the previous frame -// (according to set_prev_transform(), set_fluid_pos(), -// etc.) -//////////////////////////////////////////////////////////////////// -INLINE const TransformState *CollisionEntry:: -get_wrt_prev_space() const { - return _wrt_prev_space; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_from_mat -// Access: Published -// Description: Returns the global coordinate space of the -// CollisionNode returned by get_from_node(), as of the -// time of the collision. This will be equivalent to a -// wrt() from the node to render. -//////////////////////////////////////////////////////////////////// -INLINE const LMatrix4f &CollisionEntry:: -get_from_mat() const { - return _from_space->get_mat(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_into_mat -// Access: Published -// Description: Returns the global coordinate space of the -// CollisionNode or GeomNode returned by -// get_into_node(), as of the time of the collision. -//////////////////////////////////////////////////////////////////// -INLINE const LMatrix4f &CollisionEntry:: -get_into_mat() const { - return _into_space->get_mat(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_wrt_mat -// Access: Published -// Description: Returns the relative transform of the from node as -// seen from the into node. -//////////////////////////////////////////////////////////////////// -INLINE const LMatrix4f &CollisionEntry:: -get_wrt_mat() const { - return _wrt_space->get_mat(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_inv_wrt_mat -// Access: Published -// Description: Returns the relative transform of the into node as -// seen from the from node. -//////////////////////////////////////////////////////////////////// -INLINE const LMatrix4f &CollisionEntry:: -get_inv_wrt_mat() const { - return _inv_wrt_space->get_mat(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_wrt_prev_mat -// Access: Published -// Description: Returns the relative transform of the from node as -// seen from the into node, as of the previous frame -// (according to set_prev_transform(), set_fluid_pos(), -// etc.) -//////////////////////////////////////////////////////////////////// -INLINE const LMatrix4f &CollisionEntry:: -get_wrt_prev_mat() const { - return _wrt_prev_space->get_mat(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::set_into_intersection_point -// Access: Published -// Description: +// Description: Stores the point, on the surface of the "into" +// object, at which a collision is detected. +// +// This point is specified in the coordinate space of +// the "into" object. //////////////////////////////////////////////////////////////////// INLINE void CollisionEntry:: -set_into_intersection_point(const LPoint3f &point) { - _into_intersection_point = point; - _flags |= F_has_into_intersection_point; +set_surface_point(const LPoint3f &point) { + _surface_point = point; + _flags |= F_has_surface_point; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::set_surface_normal +// Access: Published +// Description: Stores the surface normal of the "into" object at the +// point of the intersection. +// +// This normal is specified in the coordinate space of +// the "into" object. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionEntry:: +set_surface_normal(const LVector3f &normal) { + _surface_normal = normal; + _flags |= F_has_surface_normal; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::set_interior_point +// Access: Published +// Description: Stores the point, within the interior of the "into" +// object, which represents the depth to which the +// "from" object has penetrated. This can also be +// described as the intersection point on the surface of +// the "from" object (which is inside the "into" +// object). +// +// This point is specified in the coordinate space of +// the "into" object. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionEntry:: +set_interior_point(const LPoint3f &point) { + _interior_point = point; + _flags |= F_has_interior_point; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::has_surface_point +// Access: Published +// Description: Returns true if the surface point has been specified, +// false otherwise. See get_surface_point(). Some +// types of collisions may not compute the surface +// point. +//////////////////////////////////////////////////////////////////// +INLINE bool CollisionEntry:: +has_surface_point() const { + return (_flags & F_has_surface_point) != 0; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::has_surface_normal +// Access: Published +// Description: Returns true if the surface normal has been specified, +// false otherwise. See get_surface_normal(). Some +// types of collisions may not compute the surface +// normal. +//////////////////////////////////////////////////////////////////// +INLINE bool CollisionEntry:: +has_surface_normal() const { + return (_flags & F_has_surface_normal) != 0; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::has_interior_point +// Access: Published +// Description: Returns true if the interior point has been specified, +// false otherwise. See get_interior_point(). Some +// types of collisions may not compute the interior +// point. +//////////////////////////////////////////////////////////////////// +INLINE bool CollisionEntry:: +has_interior_point() const { + return (_flags & F_has_interior_point) != 0; } //////////////////////////////////////////////////////////////////// @@ -270,7 +236,7 @@ set_into_intersection_point(const LPoint3f &point) { //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: has_into_intersection_point() const { - return (_flags & F_has_into_intersection_point) != 0; + return has_surface_point(); } //////////////////////////////////////////////////////////////////// @@ -281,10 +247,9 @@ has_into_intersection_point() const { // call this if has_into_intersection_point() returns // false. //////////////////////////////////////////////////////////////////// -INLINE const LPoint3f &CollisionEntry:: +INLINE LPoint3f CollisionEntry:: get_into_intersection_point() const { - nassertr(has_into_intersection_point(), _into_intersection_point); - return _into_intersection_point; + return get_surface_point(get_into_node_path()); } //////////////////////////////////////////////////////////////////// @@ -296,9 +261,7 @@ get_into_intersection_point() const { //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: has_from_intersection_point() const { - // Since we derive the from_intersection_point from the - // into_intersection_point, this is really the same question. - return has_into_intersection_point(); + return has_surface_point(); } //////////////////////////////////////////////////////////////////// @@ -311,18 +274,7 @@ has_from_intersection_point() const { //////////////////////////////////////////////////////////////////// INLINE LPoint3f CollisionEntry:: get_from_intersection_point() const { - return get_into_intersection_point() * get_inv_wrt_mat(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::set_into_surface_normal -// Access: Published -// Description: -//////////////////////////////////////////////////////////////////// -INLINE void CollisionEntry:: -set_into_surface_normal(const LVector3f &normal) { - _into_surface_normal = normal; - _flags |= F_has_into_surface_normal; + return get_surface_point(get_from_node_path()); } //////////////////////////////////////////////////////////////////// @@ -334,7 +286,7 @@ set_into_surface_normal(const LVector3f &normal) { //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: has_into_surface_normal() const { - return (_flags & F_has_into_surface_normal) != 0; + return has_surface_normal(); } //////////////////////////////////////////////////////////////////// @@ -345,21 +297,9 @@ has_into_surface_normal() const { // to call this if has_into_surface_normal() returns // false. //////////////////////////////////////////////////////////////////// -INLINE const LVector3f &CollisionEntry:: +INLINE LVector3f CollisionEntry:: get_into_surface_normal() const { - nassertr(has_into_surface_normal(), _into_surface_normal); - return _into_surface_normal; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::set_from_surface_normal -// Access: Published -// Description: -//////////////////////////////////////////////////////////////////// -INLINE void CollisionEntry:: -set_from_surface_normal(const LVector3f &normal) { - _from_surface_normal = normal; - _flags |= F_has_from_surface_normal; + return get_surface_normal(get_into_node_path()); } //////////////////////////////////////////////////////////////////// @@ -371,7 +311,7 @@ set_from_surface_normal(const LVector3f &normal) { //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: has_from_surface_normal() const { - return (_flags & (F_has_into_surface_normal | F_has_from_surface_normal)) != 0; + return has_surface_normal(); } //////////////////////////////////////////////////////////////////// @@ -382,25 +322,9 @@ has_from_surface_normal() const { // the collided-from object. It is an error to call // this if has_from_surface_normal() returns false. //////////////////////////////////////////////////////////////////// -INLINE const LVector3f &CollisionEntry:: +INLINE LVector3f CollisionEntry:: get_from_surface_normal() const { - nassertr(has_from_surface_normal(), _from_surface_normal); - if ((_flags & F_has_from_surface_normal) == 0) { - ((CollisionEntry *)this)->compute_from_surface_normal(); - } - return _from_surface_normal; -} - - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::set_into_depth -// Access: Published -// Description: -//////////////////////////////////////////////////////////////////// -INLINE void CollisionEntry:: -set_into_depth(float depth) { - _into_depth = depth; - _flags |= F_has_into_depth; + return get_surface_normal(get_from_node_path()); } //////////////////////////////////////////////////////////////////// @@ -413,7 +337,7 @@ set_into_depth(float depth) { //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: has_into_depth() const { - return (_flags & F_has_into_depth) != 0; + return has_surface_point() && has_interior_point(); } //////////////////////////////////////////////////////////////////// @@ -428,18 +352,7 @@ has_into_depth() const { INLINE float CollisionEntry:: get_into_depth() const { nassertr(has_into_depth(), 0.0); - return _into_depth; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::set_from_depth -// Access: Published -// Description: -//////////////////////////////////////////////////////////////////// -INLINE void CollisionEntry:: -set_from_depth(float depth) { - _from_depth = depth; - _flags |= F_has_from_depth; + return (get_surface_point(get_into_node_path()) - get_interior_point(get_into_node_path())).length(); } //////////////////////////////////////////////////////////////////// @@ -452,7 +365,7 @@ set_from_depth(float depth) { //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: has_from_depth() const { - return (_flags & F_has_from_depth) != 0; + return has_surface_point() && has_interior_point(); } //////////////////////////////////////////////////////////////////// @@ -466,8 +379,82 @@ has_from_depth() const { //////////////////////////////////////////////////////////////////// INLINE float CollisionEntry:: get_from_depth() const { - nassertr(has_from_depth(), 0.0); - return _from_depth; + return (get_surface_point(get_from_node_path()) - get_interior_point(get_from_node_path())).length(); +} + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_wrt_space +// Access: Public +// Description: Returns the relative transform of the from node as +// seen from the into node. +//////////////////////////////////////////////////////////////////// +INLINE CPT(TransformState) CollisionEntry:: +get_wrt_space() const { + return _from_node_path.get_transform(_into_node_path); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_inv_wrt_space +// Access: Public +// Description: Returns the relative transform of the into node as +// seen from the from node. +//////////////////////////////////////////////////////////////////// +INLINE CPT(TransformState) CollisionEntry:: +get_inv_wrt_space() const { + return _into_node_path.get_transform(_from_node_path); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_wrt_prev_space +// Access: Public +// Description: Returns the relative transform of the from node as +// seen from the into node, as of the previous frame +// (according to set_prev_transform(), set_fluid_pos(), +// etc.) +//////////////////////////////////////////////////////////////////// +INLINE CPT(TransformState) CollisionEntry:: +get_wrt_prev_space() const { + if (get_respect_prev_transform()) { + return _from_node_path.get_prev_transform(_into_node_path); + } else { + return get_wrt_space(); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_wrt_mat +// Access: Public +// Description: Returns the relative transform of the from node as +// seen from the into node. +//////////////////////////////////////////////////////////////////// +INLINE const LMatrix4f &CollisionEntry:: +get_wrt_mat() const { + return get_wrt_space()->get_mat(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_inv_wrt_mat +// Access: Public +// Description: Returns the relative transform of the into node as +// seen from the from node. +//////////////////////////////////////////////////////////////////// +INLINE const LMatrix4f &CollisionEntry:: +get_inv_wrt_mat() const { + return get_inv_wrt_space()->get_mat(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_wrt_prev_mat +// Access: Public +// Description: Returns the relative transform of the from node as +// seen from the into node, as of the previous frame +// (according to set_prev_transform(), set_fluid_pos(), +// etc.) +//////////////////////////////////////////////////////////////////// +INLINE const LMatrix4f &CollisionEntry:: +get_wrt_prev_mat() const { + return get_wrt_prev_space()->get_mat(); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionEntry.cxx b/panda/src/collide/collisionEntry.cxx index 4a8faaed59..d7a1582141 100644 --- a/panda/src/collide/collisionEntry.cxx +++ b/panda/src/collide/collisionEntry.cxx @@ -33,15 +33,10 @@ CollisionEntry(const CollisionEntry ©) : _into_node(copy._into_node), _from_node_path(copy._from_node_path), _into_node_path(copy._into_node_path), - _from_space(copy._from_space), - _into_space(copy._into_space), - _wrt_space(copy._wrt_space), - _inv_wrt_space(copy._inv_wrt_space), - _wrt_prev_space(copy._wrt_prev_space), _flags(copy._flags), - _into_intersection_point(copy._into_intersection_point), - _into_surface_normal(copy._into_surface_normal), - _into_depth(copy._into_depth) + _surface_point(copy._surface_point), + _surface_normal(copy._surface_normal), + _interior_point(copy._interior_point) { } @@ -58,26 +53,101 @@ operator = (const CollisionEntry ©) { _into_node = copy._into_node; _from_node_path = copy._from_node_path; _into_node_path = copy._into_node_path; - _from_space = copy._from_space; - _into_space = copy._into_space; - _wrt_space = copy._wrt_space; - _inv_wrt_space = copy._inv_wrt_space; - _wrt_prev_space = copy._wrt_prev_space; _flags = copy._flags; - _into_intersection_point = copy._into_intersection_point; - _into_surface_normal = copy._into_surface_normal; - _into_depth = copy._into_depth; + _surface_point = copy._surface_point; + _surface_normal = copy._surface_normal; + _interior_point = copy._interior_point; } //////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::compute_from_surface_normal -// Access: Private -// Description: Computes the "from" surface normal by converting the -// "into" surface normal into the colliding object's -// space. +// Function: CollisionEntry::get_surface_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 point will be converted into whichever coordinate +// space the caller specifies. //////////////////////////////////////////////////////////////////// -void CollisionEntry:: -compute_from_surface_normal() { - _from_surface_normal = get_into_surface_normal() * get_inv_wrt_mat(); - _flags |= F_has_from_surface_normal; +LPoint3f CollisionEntry:: +get_surface_point(const NodePath &space) const { + nassertr(has_surface_point(), LPoint3f::zero()); + return _surface_point * _into_node_path.get_mat(space); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_surface_normal +// Access: Published +// Description: Returns the surface normal of the "into" object at +// the point at which a collision is detected. +// +// The normal will be converted into whichever coordinate +// space the caller specifies. +//////////////////////////////////////////////////////////////////// +LVector3f CollisionEntry:: +get_surface_normal(const NodePath &space) const { + nassertr(has_surface_normal(), LVector3f::zero()); + return _surface_normal * _into_node_path.get_mat(space); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_interior_point +// Access: Published +// Description: Returns the point, within the interior of the "into" +// object, which represents the depth to which the +// "from" object has penetrated. This can also be +// described as the intersection point on the surface of +// the "from" object (which is inside the "into" +// object). It can be thought of as the deepest point +// of intersection. +// +// The point will be converted into whichever coordinate +// space the caller specifies. +//////////////////////////////////////////////////////////////////// +LPoint3f CollisionEntry:: +get_interior_point(const NodePath &space) const { + if (!has_interior_point()) { + return get_surface_point(space); + } + return _interior_point * _into_node_path.get_mat(space); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionEntry::get_all +// 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(const NodePath &space, LPoint3f &surface_point, + LVector3f &surface_normal, LPoint3f &interior_point) const { + const LMatrix4f &mat = _into_node_path.get_mat(space); + bool all_ok = true; + + if (!has_surface_point()) { + surface_point = LPoint3f::zero(); + all_ok = false; + } else { + surface_point = _surface_point * mat; + } + + if (!has_surface_normal()) { + surface_normal = LVector3f::zero(); + all_ok = false; + } else { + surface_normal = _surface_normal * mat; + } + + if (!has_interior_point()) { + interior_point = surface_point; + all_ok = false; + } else { + interior_point = _interior_point * mat; + } + + return true; } diff --git a/panda/src/collide/collisionEntry.h b/panda/src/collide/collisionEntry.h index e955473c89..b0acf92c5e 100644 --- a/panda/src/collide/collisionEntry.h +++ b/panda/src/collide/collisionEntry.h @@ -62,41 +62,57 @@ PUBLISHED: INLINE const NodePath &get_from_node_path() const; INLINE const NodePath &get_into_node_path() const; - INLINE const TransformState *get_from_space() const; - INLINE const TransformState *get_into_space() const; - INLINE const TransformState *get_wrt_space() const; - INLINE const TransformState *get_inv_wrt_space() const; - INLINE const TransformState *get_wrt_prev_space() const; + INLINE bool get_respect_prev_transform() const; - INLINE const LMatrix4f &get_from_mat() const; - INLINE const LMatrix4f &get_into_mat() const; - INLINE const LMatrix4f &get_wrt_mat() const; - INLINE const LMatrix4f &get_inv_wrt_mat() const; - INLINE const LMatrix4f &get_wrt_prev_mat() const; + INLINE void set_surface_point(const LPoint3f &point); + INLINE void set_surface_normal(const LVector3f &normal); + INLINE void set_interior_point(const LPoint3f &point); + + INLINE bool has_surface_point() const; + INLINE bool has_surface_normal() const; + INLINE bool has_interior_point() 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; + bool get_all(const NodePath &space, + LPoint3f &surface_point, + LVector3f &surface_normal, + LPoint3f &interior_point) const; + + + // The following methods are all deprecated in favor of the above + // methods. They are here only temporarily to ease transition. - INLINE void set_into_intersection_point(const LPoint3f &point); INLINE bool has_into_intersection_point() const; - INLINE const LPoint3f &get_into_intersection_point() const; + INLINE LPoint3f get_into_intersection_point() const; INLINE bool has_from_intersection_point() const; INLINE LPoint3f get_from_intersection_point() const; - INLINE void set_into_surface_normal(const LVector3f &normal); INLINE bool has_into_surface_normal() const; - INLINE const LVector3f &get_into_surface_normal() const; + INLINE LVector3f get_into_surface_normal() const; - INLINE void set_from_surface_normal(const LVector3f &normal); INLINE bool has_from_surface_normal() const; - INLINE const LVector3f &get_from_surface_normal() const; + INLINE LVector3f get_from_surface_normal() const; - INLINE void set_into_depth(float depth); INLINE bool has_into_depth() const; INLINE float get_into_depth() const; - INLINE void set_from_depth(float depth); INLINE bool has_from_depth() const; INLINE float get_from_depth() const; +public: + INLINE CPT(TransformState) get_wrt_space() const; + INLINE CPT(TransformState) get_inv_wrt_space() const; + INLINE CPT(TransformState) get_wrt_prev_space() const; + + INLINE const LMatrix4f &get_wrt_mat() const; + INLINE const LMatrix4f &get_inv_wrt_mat() const; + INLINE const LMatrix4f &get_wrt_prev_mat() const; + + + private: INLINE void test_intersection(CollisionHandler *record, const CollisionTraverser *trav) const; @@ -109,27 +125,19 @@ private: PT(PandaNode) _into_node; NodePath _from_node_path; NodePath _into_node_path; - CPT(TransformState) _from_space; - CPT(TransformState) _into_space; - CPT(TransformState) _wrt_space; - CPT(TransformState) _inv_wrt_space; - CPT(TransformState) _wrt_prev_space; enum Flags { - F_has_into_intersection_point = 0x0001, - F_has_into_surface_normal = 0x0002, - F_has_from_surface_normal = 0x0004, - F_has_into_depth = 0x0008, - F_has_from_depth = 0x0010, + F_has_surface_point = 0x0001, + F_has_surface_normal = 0x0002, + F_has_interior_point = 0x0004, + F_respect_prev_transform = 0x0008, }; int _flags; - LPoint3f _into_intersection_point; - LVector3f _into_surface_normal; - LVector3f _from_surface_normal; - float _into_depth; - float _from_depth; + LPoint3f _surface_point; + LVector3f _surface_normal; + LPoint3f _interior_point; public: static TypeHandle get_class_type() { diff --git a/panda/src/collide/collisionHandlerFloor.cxx b/panda/src/collide/collisionHandlerFloor.cxx index 1070a8a85b..839aaffc8a 100644 --- a/panda/src/collide/collisionHandlerFloor.cxx +++ b/panda/src/collide/collisionHandlerFloor.cxx @@ -77,12 +77,7 @@ handle_entries() { okflag = false; } else { ColliderDef &def = (*ci).second; - if (!def.is_valid()) { - collide_cat.error() - << "Removing invalid collider " << from_node_path << " from " - << get_type() << "\n"; - _colliders.erase(ci); - } else { + { // Get the maximum height for all collisions with this node. bool got_max = false; float max_height = 0.0f; @@ -93,8 +88,8 @@ handle_entries() { nassertr(entry != (CollisionEntry *)NULL, false); nassertr(from_node_path == entry->get_from_node_path(), false); - if (entry->has_from_intersection_point()) { - LPoint3f point = entry->get_from_intersection_point(); + if (entry->has_surface_point()) { + LPoint3f point = entry->get_surface_point(def._target); if (collide_cat.is_debug()) { collide_cat.debug() << "Intersection point detected at " << point << "\n"; @@ -121,25 +116,15 @@ handle_entries() { _max_velocity * ClockObject::get_global_clock()->get_dt(); adjust = max(adjust, -max_adjust); } - - if (def._node != (PandaNode *)NULL) { - // If we are adjusting a plain PandaNode, get the - // transform and adjust just the Z value to preserve - // maximum precision. - CPT(TransformState) trans = def._node->get_transform(); - LVecBase3f pos = trans->get_pos(); - pos[2] += adjust; - def._node->set_transform(trans->set_pos(pos)); - } else { - // Otherwise, go ahead and do the matrix math to do things - // the old and clumsy way. - LMatrix4f mat; - def.get_mat(mat); - mat(3, 2) += adjust; - def.set_mat(mat); - } + CPT(TransformState) trans = def._target.get_transform(); + LVecBase3f pos = trans->get_pos(); + pos[2] += adjust; + def._target.set_transform(trans->set_pos(pos)); + def.updated_transform(); + apply_linear_force(def, LVector3f(0.0f, 0.0f, adjust)); + } else { if (collide_cat.is_spam()) { collide_cat.spam() diff --git a/panda/src/collide/collisionHandlerPhysical.I b/panda/src/collide/collisionHandlerPhysical.I index 7550a93f2a..0e1b431d47 100644 --- a/panda/src/collide/collisionHandlerPhysical.I +++ b/panda/src/collide/collisionHandlerPhysical.I @@ -17,49 +17,28 @@ //////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////// -// Function: CollisionHandlerPhysical::ColliderDef::set_drive_interface -// Access: Public -// Description: -//////////////////////////////////////////////////////////////////// -INLINE void CollisionHandlerPhysical::ColliderDef:: -set_drive_interface(DriveInterface *drive_interface) { - _drive_interface = drive_interface; - _node.clear(); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionHandlerPhysical::ColliderDef::set_node -// Access: Public -// Description: -//////////////////////////////////////////////////////////////////// -INLINE void CollisionHandlerPhysical::ColliderDef:: -set_node(PandaNode *node) { - _node = node; - _drive_interface.clear(); -} - //////////////////////////////////////////////////////////////////// // Function: CollisionHandlerPhysical::ColliderDef::set_target // Access: Public // Description: //////////////////////////////////////////////////////////////////// INLINE void CollisionHandlerPhysical::ColliderDef:: -set_target(const NodePath &target) { +set_target(const NodePath &target, DriveInterface *drive_interface) { _target = target; - if (!target.is_empty()) { - _node = target.node(); - } + _drive_interface = drive_interface; } //////////////////////////////////////////////////////////////////// -// Function: CollisionHandlerPhysical::ColliderDef::is_valid +// Function: CollisionHandlerPhysical::ColliderDef::updated_transform // Access: Public -// Description: Returns true if this ColliderDef is still valid; -// e.g. it refers to a valid node or drive interface. +// Description: Called by the handler when it has changed the +// transform on the target node, this applies the change +// to the drive interface if one is specified. //////////////////////////////////////////////////////////////////// -INLINE bool CollisionHandlerPhysical::ColliderDef:: -is_valid() const { - return (_node != (PandaNode *)NULL) || - (_drive_interface != (DriveInterface *)NULL); +INLINE void CollisionHandlerPhysical::ColliderDef:: +updated_transform() { + if (_drive_interface != (DriveInterface *)NULL) { + _drive_interface->set_mat(_target.get_mat()); + _drive_interface->force_dgraph(); + } } diff --git a/panda/src/collide/collisionHandlerPhysical.cxx b/panda/src/collide/collisionHandlerPhysical.cxx index f06f69aece..01cf4430f7 100644 --- a/panda/src/collide/collisionHandlerPhysical.cxx +++ b/panda/src/collide/collisionHandlerPhysical.cxx @@ -24,43 +24,6 @@ TypeHandle CollisionHandlerPhysical::_type_handle; -//////////////////////////////////////////////////////////////////// -// Function: CollisionHandlerPhysical::ColliderDef::get_mat -// Access: Public -// Description: Fills mat with the matrix representing the current -// position and orientation of this collider. -//////////////////////////////////////////////////////////////////// -void CollisionHandlerPhysical::ColliderDef:: -get_mat(LMatrix4f &mat) const { - if (_node != (PandaNode *)NULL) { - mat = _node->get_transform()->get_mat(); - } else if (_drive_interface != (DriveInterface *)NULL) { - mat = _drive_interface->get_mat(); - } else { - collide_cat.error() - << "Invalid CollisionHandlerPhysical::ColliderDef\n"; - } -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionHandlerPhysical::ColliderDef::set_mat -// Access: Public -// Description: Moves this collider to the position and orientation -// indicated by the given transform. -//////////////////////////////////////////////////////////////////// -void CollisionHandlerPhysical::ColliderDef:: -set_mat(const LMatrix4f &mat) { - if (_node != (PandaNode *)NULL) { - _node->set_transform(TransformState::make_mat(mat)); - } else if (_drive_interface != (DriveInterface *)NULL) { - _drive_interface->set_mat(mat); - _drive_interface->force_dgraph(); - } else { - collide_cat.error() - << "Invalid CollisionHandlerPhysical::ColliderDef\n"; - } -} - //////////////////////////////////////////////////////////////////// // Function: CollisionHandlerPhysical::Constructor // Access: Public @@ -140,6 +103,27 @@ add_collider(const NodePath &collider, const NodePath &target) { _colliders[collider].set_target(target); } +//////////////////////////////////////////////////////////////////// +// Function: CollisionHandlerPhysical::add_collider +// Access: Published +// Description: Adds a new collider to the list with a NodePath +// that will be updated with the collider's new +// position, or updates the existing collider with a new +// NodePath object. +// +// The indicated DriveInterface will also be updated +// with the target's new transform each frame. This +// method should be used when the target is directly +// controlled by a DriveInterface. +//////////////////////////////////////////////////////////////////// +void CollisionHandlerPhysical:: +add_collider(const NodePath &collider, const NodePath &target, + DriveInterface *drive_interface) { + nassertv(!collider.is_empty() && collider.node()->is_of_type(CollisionNode::get_class_type())); + nassertv(!target.is_empty()); + _colliders[collider].set_target(target, drive_interface); +} + //////////////////////////////////////////////////////////////////// // Function: CollisionHandlerPhysical::remove_collider // Access: Published @@ -189,16 +173,3 @@ void CollisionHandlerPhysical:: add_collider_node(CollisionNode *node, PandaNode *target) { add_collider(NodePath(node), NodePath(target)); } - -//////////////////////////////////////////////////////////////////// -// Function: CollisionHandlerPhysical::add_collider_drive -// Access: Published -// Description: Adds a new collider to the list with a DriveInterface -// pointer that needs to be told about the collider's -// new position, or updates the existing collider with a -// new DriveInterface pointer. -//////////////////////////////////////////////////////////////////// -void CollisionHandlerPhysical:: -add_collider_drive(CollisionNode *node, DriveInterface *drive_interface) { - _colliders[NodePath(node)].set_drive_interface(drive_interface); -} diff --git a/panda/src/collide/collisionHandlerPhysical.h b/panda/src/collide/collisionHandlerPhysical.h index 84da4ce96d..28d21ea2ea 100644 --- a/panda/src/collide/collisionHandlerPhysical.h +++ b/panda/src/collide/collisionHandlerPhysical.h @@ -46,6 +46,8 @@ public: PUBLISHED: void add_collider(const NodePath &collider, const NodePath &target); + void add_collider(const NodePath &collider, const NodePath &target, + DriveInterface *drive_interface); bool remove_collider(const NodePath &collider); bool has_collider(const NodePath &collider) const; void clear_colliders(); @@ -54,9 +56,6 @@ PUBLISHED: // transition to the above new NodePath-based methods. void add_collider_node(CollisionNode *node, PandaNode *target); - // add_collider_drive() is becoming obsolete. If you need it, let us know. - void add_collider_drive(CollisionNode *node, DriveInterface *drive_interface); - protected: typedef pvector< PT(CollisionEntry) > Entries; typedef pmap FromEntries; @@ -64,17 +63,12 @@ protected: class ColliderDef { public: - INLINE void set_drive_interface(DriveInterface *drive_interface); - INLINE void set_node(PandaNode *node); - INLINE void set_target(const NodePath &target); - INLINE bool is_valid() const; - - void get_mat(LMatrix4f &mat) const; - void set_mat(const LMatrix4f &mat); - - PT(DriveInterface) _drive_interface; - PT(PandaNode) _node; + INLINE void set_target(const NodePath &target, + DriveInterface *drive_interface = NULL); + INLINE void updated_transform(); + NodePath _target; + PT(DriveInterface) _drive_interface; }; typedef pmap Colliders; diff --git a/panda/src/collide/collisionHandlerPusher.cxx b/panda/src/collide/collisionHandlerPusher.cxx index 8286332933..ec86f3e919 100644 --- a/panda/src/collide/collisionHandlerPusher.cxx +++ b/panda/src/collide/collisionHandlerPusher.cxx @@ -91,12 +91,7 @@ handle_entries() { okflag = false; } else { ColliderDef &def = (*ci).second; - if (!def.is_valid()) { - collide_cat.error() - << "Removing invalid collider " << from_node_path << " from " - << get_type() << "\n"; - _colliders.erase(ci); - } else { + { // 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 @@ -113,20 +108,22 @@ handle_entries() { CollisionEntry *entry = (*ei); nassertr(entry != (CollisionEntry *)NULL, false); nassertr(from_node_path == entry->get_from_node_path(), false); - - if (!entry->has_from_surface_normal() || - !entry->has_from_depth()) { + + LPoint3f surface_point; + LVector3f normal; + LPoint3f 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() << "; no normal/depth information.\n"; + << entry->get_into_node_path() << "; no normal/depth information.\n"; } #endif } else { // Shove it just enough to clear the volume. - if (entry->get_from_depth() != 0.0f) { - LVector3f normal = entry->get_from_surface_normal(); + if (!surface_point.almost_equal(interior_point)) { if (_horizontal) { normal[2] = 0.0f; } @@ -137,7 +134,7 @@ handle_entries() { ShoveData sd; sd._vector = normal; - sd._length = entry->get_from_depth(); + sd._length = (surface_point - interior_point).length(); sd._valid = true; sd._entry = entry; @@ -145,7 +142,7 @@ handle_entries() { if (collide_cat.is_debug()) { collide_cat.debug() << "Shove on " << from_node_path << " from " - << *entry->get_into_node() << ": " << sd._vector + << entry->get_into_node_path() << ": " << sd._vector << " times " << sd._length << "\n"; } #endif @@ -248,23 +245,13 @@ handle_entries() { } #endif - if (def._node != (PandaNode *)NULL) { - // If we are adjusting a plain PandaNode, get the - // transform and adjust just the position to preserve - // maximum precision. - CPT(TransformState) trans = def._node->get_transform(); - LVecBase3f pos = trans->get_pos(); - pos += net_shove * trans->get_mat(); - def._node->set_transform(trans->set_pos(pos)); - - apply_linear_force(def, force_normal); - } else { - // Otherwise, go ahead and do the matrix math to do things - // the old and clumsy way. - LMatrix4f mat; - def.get_mat(mat); - def.set_mat(LMatrix4f::translate_mat(net_shove) * mat); - } + CPT(TransformState) trans = def._target.get_transform(); + LVecBase3f pos = trans->get_pos(); + pos += net_shove * trans->get_mat(); + def._target.set_transform(trans->set_pos(pos)); + def.updated_transform(); + + apply_linear_force(def, force_normal); } } } diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index d680e59a74..7c85b50b2e 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/panda/src/collide/collisionPlane.cxx @@ -108,9 +108,11 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { const CollisionSphere *sphere; DCAST_INTO_R(sphere, entry.get_from(), 0); - LPoint3f from_center = sphere->get_center() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_center = sphere->get_center() * wrt_mat; LVector3f from_radius_v = - LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_mat(); + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; float from_radius = length(from_radius_v); float dist = dist_to_plane(from_center); @@ -127,12 +129,10 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { PT(CollisionEntry) new_entry = new CollisionEntry(entry); LVector3f from_normal = get_normal() * entry.get_inv_wrt_mat(); - float from_depth = from_radius - dist; - new_entry->set_into_surface_normal(get_normal()); - new_entry->set_from_surface_normal(from_normal); - new_entry->set_from_depth(from_depth); - new_entry->set_into_intersection_point(from_center - get_normal() * dist); + new_entry->set_surface_normal(get_normal()); + new_entry->set_surface_point(from_center - get_normal() * dist); + new_entry->set_interior_point(from_center - get_normal() * from_radius); return new_entry; } @@ -147,8 +147,10 @@ test_intersection_from_ray(const CollisionEntry &entry) const { const CollisionRay *ray; DCAST_INTO_R(ray, entry.get_from(), 0); - LPoint3f from_origin = ray->get_origin() * entry.get_wrt_mat(); - LVector3f from_direction = ray->get_direction() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = ray->get_origin() * wrt_mat; + LVector3f from_direction = ray->get_direction() * wrt_mat; float t; if (!_plane.intersects_line(t, from_origin, from_direction)) { @@ -169,8 +171,8 @@ test_intersection_from_ray(const CollisionEntry &entry) const { PT(CollisionEntry) new_entry = new CollisionEntry(entry); LPoint3f into_intersection_point = from_origin + t * from_direction; - new_entry->set_into_surface_normal(get_normal()); - new_entry->set_into_intersection_point(into_intersection_point); + new_entry->set_surface_normal(get_normal()); + new_entry->set_surface_point(into_intersection_point); return new_entry; } diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index faf765cfee..303d33c860 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -16,7 +16,6 @@ // //////////////////////////////////////////////////////////////////// - #include "collisionPolygon.h" #include "collisionHandler.h" #include "collisionEntry.h" @@ -34,6 +33,7 @@ #include "bamReader.h" #include "bamWriter.h" #include "geomPolygon.h" +#include "transformState.h" #include @@ -273,16 +273,21 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { const CollisionSphere *sphere; DCAST_INTO_R(sphere, entry.get_from(), 0); - LPoint3f orig_center = sphere->get_center() * entry.get_wrt_mat(); + CPT(TransformState) wrt_space = entry.get_wrt_space(); + CPT(TransformState) wrt_prev_space = entry.get_wrt_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + LPoint3f orig_center = sphere->get_center() * wrt_mat; LPoint3f from_center = orig_center; bool moved_from_center = false; - if (entry.get_wrt_prev_space() != entry.get_wrt_space()) { + if (wrt_prev_space != wrt_space) { // If we have a delta between the previous position and the // current position, we use that to determine some more properties // of the collision. LPoint3f b = from_center; - LPoint3f a = sphere->get_center() * entry.get_wrt_prev_mat(); + LPoint3f a = sphere->get_center() * wrt_prev_space->get_mat(); LVector3f delta = b - a; // First, there is no collision if the "from" object is moving in @@ -317,7 +322,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } LVector3f from_radius_v = - LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_mat(); + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; float from_radius = length(from_radius_v); float dist = dist_to_plane(from_center); @@ -406,15 +411,9 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { into_depth = from_radius - dist_to_plane(orig_center); } - new_entry->set_into_surface_normal(get_normal()); - new_entry->set_into_depth(into_depth); - - LVector3f from_normal = get_normal() * entry.get_inv_wrt_mat(); - LVector3f from_depth_vec = (get_normal() * into_depth) * entry.get_inv_wrt_mat(); - new_entry->set_from_surface_normal(from_normal); - new_entry->set_from_depth(from_depth_vec.length()); - - new_entry->set_into_intersection_point(from_center - get_normal() * dist); + new_entry->set_surface_normal(get_normal()); + new_entry->set_surface_point(from_center - get_normal() * dist); + new_entry->set_interior_point(from_center - get_normal() * (dist + into_depth)); return new_entry; } @@ -435,8 +434,10 @@ test_intersection_from_ray(const CollisionEntry &entry) const { const CollisionRay *ray; DCAST_INTO_R(ray, entry.get_from(), 0); - LPoint3f from_origin = ray->get_origin() * entry.get_wrt_mat(); - LVector3f from_direction = ray->get_direction() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = ray->get_origin() * wrt_mat; + LVector3f from_direction = ray->get_direction() * wrt_mat; float t; if (!get_plane().intersects_line(t, from_origin, from_direction)) { @@ -462,8 +463,8 @@ test_intersection_from_ray(const CollisionEntry &entry) const { } PT(CollisionEntry) new_entry = new CollisionEntry(entry); - new_entry->set_into_surface_normal(get_normal()); - new_entry->set_into_intersection_point(plane_point); + new_entry->set_surface_normal(get_normal()); + new_entry->set_surface_point(plane_point); return new_entry; } @@ -484,8 +485,10 @@ test_intersection_from_segment(const CollisionEntry &entry) const { const CollisionSegment *segment; DCAST_INTO_R(segment, entry.get_from(), 0); - LPoint3f from_a = segment->get_point_a() * entry.get_wrt_mat(); - LPoint3f from_b = segment->get_point_b() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_a = segment->get_point_a() * wrt_mat; + LPoint3f from_b = segment->get_point_b() * wrt_mat; LPoint3f from_direction = from_b - from_a; float t; @@ -513,8 +516,8 @@ test_intersection_from_segment(const CollisionEntry &entry) const { } PT(CollisionEntry) new_entry = new CollisionEntry(entry); - new_entry->set_into_surface_normal(get_normal()); - new_entry->set_into_intersection_point(plane_point); + new_entry->set_surface_normal(get_normal()); + new_entry->set_surface_point(plane_point); return new_entry; } diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index 981b6da17a..f5e3483007 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -121,9 +121,11 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { const CollisionSphere *sphere; DCAST_INTO_R(sphere, entry.get_from(), 0); - LPoint3f from_center = sphere->get_center() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_center = sphere->get_center() * wrt_mat; LVector3f from_radius_v = - LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_mat(); + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; float from_radius = length(from_radius_v); LPoint3f into_center = _center; @@ -143,26 +145,20 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } PT(CollisionEntry) new_entry = new CollisionEntry(entry); - float dist = sqrtf(dist2); - LVector3f into_normal; + LVector3f surface_normal; float vec_length = vec.length(); if (IS_NEARLY_ZERO(vec_length)) { // If we don't have a collision normal (e.g. the centers are // exactly coincident), then make up an arbitrary normal--any one // is as good as any other. - into_normal.set(1.0, 0.0, 0.0); + surface_normal.set(1.0, 0.0, 0.0); } else { - into_normal = vec / vec_length; + surface_normal = vec / vec_length; } - LPoint3f into_intersection_point = into_normal * (dist - from_radius); - float into_depth = into_radius + from_radius - dist; - new_entry->set_into_surface_normal(into_normal); - new_entry->set_into_intersection_point(into_intersection_point); - new_entry->set_into_depth(into_depth); - - LVector3f from_depth_vec = (into_normal * into_depth) * entry.get_inv_wrt_mat(); - new_entry->set_from_depth(from_depth_vec.length()); + new_entry->set_surface_normal(surface_normal); + new_entry->set_surface_point(into_center + surface_normal * into_radius); + new_entry->set_interior_point(from_center - surface_normal * from_radius); return new_entry; } @@ -177,8 +173,10 @@ test_intersection_from_ray(const CollisionEntry &entry) const { const CollisionRay *ray; DCAST_INTO_R(ray, entry.get_from(), 0); - LPoint3f from_origin = ray->get_origin() * entry.get_wrt_mat(); - LVector3f from_direction = ray->get_direction() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = ray->get_origin() * wrt_mat; + LVector3f from_direction = ray->get_direction() * wrt_mat; double t1, t2; if (!intersects_line(t1, t2, from_origin, from_direction)) { @@ -208,7 +206,7 @@ test_intersection_from_ray(const CollisionEntry &entry) const { // at t1. into_intersection_point = from_origin + t1 * from_direction; } - new_entry->set_into_intersection_point(into_intersection_point); + new_entry->set_surface_point(into_intersection_point); return new_entry; } @@ -223,8 +221,10 @@ test_intersection_from_segment(const CollisionEntry &entry) const { const CollisionSegment *segment; DCAST_INTO_R(segment, entry.get_from(), 0); - LPoint3f from_a = segment->get_point_a() * entry.get_wrt_mat(); - LPoint3f from_b = segment->get_point_b() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_a = segment->get_point_a() * wrt_mat; + LPoint3f from_b = segment->get_point_b() * wrt_mat; LVector3f from_direction = from_b - from_a; double t1, t2; @@ -256,7 +256,7 @@ test_intersection_from_segment(const CollisionEntry &entry) const { // sphere or beyond it. The first intersection point is at t1. into_intersection_point = from_a + t1 * from_direction; } - new_entry->set_into_intersection_point(into_intersection_point); + new_entry->set_surface_point(into_intersection_point); return new_entry; } diff --git a/panda/src/collide/collisionTraverser.cxx b/panda/src/collide/collisionTraverser.cxx index 66978e4c4d..4eab14d3b4 100644 --- a/panda/src/collide/collisionTraverser.cxx +++ b/panda/src/collide/collisionTraverser.cxx @@ -452,7 +452,9 @@ r_traverse(CollisionLevelState &level_state) { CollisionEntry entry; entry._into_node = cnode; entry._into_node_path = level_state.get_node_path(); - entry._into_space = entry._into_node_path.get_net_transform(); + if (_respect_prev_transform) { + entry._flags |= CollisionEntry::F_respect_prev_transform; + } int num_colliders = level_state.get_num_colliders(); for (int c = 0; c < num_colliders; c++) { @@ -463,17 +465,6 @@ r_traverse(CollisionLevelState &level_state) { cnode->get_into_collide_mask()) != 0) { entry._from_node_path = level_state.get_collider_node_path(c); entry._from = level_state.get_collider(c); - entry._from_space = entry._from_node_path.get_net_transform(); - - entry._wrt_space = entry._from_node_path.get_transform(entry._into_node_path); - entry._inv_wrt_space = entry._into_node_path.get_transform(entry._from_node_path); - - if (_respect_prev_transform) { - entry._wrt_prev_space = entry._from_node_path.get_prev_transform(entry._into_node_path); - - } else { - entry._wrt_prev_space = entry._wrt_space; - } compare_collider_to_node(entry, level_state.get_parent_bound(c), @@ -502,7 +493,9 @@ r_traverse(CollisionLevelState &level_state) { CollisionEntry entry; entry._into_node = gnode; entry._into_node_path = level_state.get_node_path(); - entry._into_space = entry._into_node_path.get_net_transform(); + if (_respect_prev_transform) { + entry._flags |= CollisionEntry::F_respect_prev_transform; + } int num_colliders = level_state.get_num_colliders(); for (int c = 0; c < num_colliders; c++) { @@ -510,10 +503,6 @@ r_traverse(CollisionLevelState &level_state) { entry._from_node = level_state.get_collider_node(c); entry._from_node_path = level_state.get_collider_node_path(c); entry._from = level_state.get_collider(c); - entry._from_space = entry._from_node_path.get_net_transform(); - - entry._wrt_space = entry._from_node_path.get_transform(entry._into_node_path); - entry._inv_wrt_space = entry._into_node_path.get_transform(entry._from_node_path); compare_collider_to_geom_node(entry, level_state.get_parent_bound(c), @@ -641,7 +630,7 @@ compare_collider_to_solid(CollisionEntry &entry, } if (within_solid_bounds) { Colliders::const_iterator ci; - ci = _colliders.find(entry.get_from_node()); + ci = _colliders.find(entry.get_from_node_path()); nassertv(ci != _colliders.end()); entry.test_intersection((*ci).second, this); } @@ -663,7 +652,7 @@ compare_collider_to_geom(CollisionEntry &entry, Geom *geom, } if (within_geom_bounds) { Colliders::const_iterator ci; - ci = _colliders.find(entry.get_from_node()); + ci = _colliders.find(entry.get_from_node_path()); nassertv(ci != _colliders.end()); PTA_Vertexf coords; diff --git a/panda/src/collide/collisionTraverser.h b/panda/src/collide/collisionTraverser.h index 82f6ed4aae..3b442b226f 100644 --- a/panda/src/collide/collisionTraverser.h +++ b/panda/src/collide/collisionTraverser.h @@ -108,6 +108,12 @@ private: PT(CollisionHandler) _default_handler; TypeHandle _graph_type; + // A map of NodePaths is slightly dangerous, since there is a + // (small) possiblity that a particular NodePath's sorting order + // relative to other NodePaths will spontaneously change. This can + // only happen if two NodePaths get collapsed together due to a + // removal of a certain kind of instanced node; see the comments in + // NodePath.cxx. typedef pmap Colliders; Colliders _colliders; typedef pvector OrderedColliders; diff --git a/panda/src/collide/collisionTube.cxx b/panda/src/collide/collisionTube.cxx index 7d8a3093c0..f34c0d6b8c 100644 --- a/panda/src/collide/collisionTube.cxx +++ b/panda/src/collide/collisionTube.cxx @@ -31,6 +31,7 @@ #include "bamReader.h" #include "bamWriter.h" #include "cmath.h" +#include "transformState.h" TypeHandle CollisionTube::_type_handle; @@ -130,19 +131,24 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { const CollisionSphere *sphere; DCAST_INTO_R(sphere, entry.get_from(), 0); - LPoint3f from_a = sphere->get_center() * entry.get_wrt_mat(); + CPT(TransformState) wrt_space = entry.get_wrt_space(); + CPT(TransformState) wrt_prev_space = entry.get_wrt_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + LPoint3f from_a = sphere->get_center() * wrt_mat; LPoint3f from_b = from_a; - if (entry.get_wrt_prev_space() != entry.get_wrt_space()) { + if (wrt_prev_space != wrt_space) { // If the sphere is moving relative to the tube, it becomes a tube // itself. - from_a = sphere->get_center() * entry.get_wrt_prev_mat(); + from_a = sphere->get_center() * wrt_prev_space->get_mat(); } LVector3f from_direction = from_b - from_a; LVector3f from_radius_v = - LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_mat(); + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; float from_radius = length(from_radius_v); double t1, t2; @@ -189,8 +195,10 @@ test_intersection_from_ray(const CollisionEntry &entry) const { const CollisionRay *ray; DCAST_INTO_R(ray, entry.get_from(), 0); - LPoint3f from_origin = ray->get_origin() * entry.get_wrt_mat(); - LVector3f from_direction = ray->get_direction() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_origin = ray->get_origin() * wrt_mat; + LVector3f from_direction = ray->get_direction() * wrt_mat; double t1, t2; if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { @@ -235,8 +243,10 @@ test_intersection_from_segment(const CollisionEntry &entry) const { const CollisionSegment *segment; DCAST_INTO_R(segment, entry.get_from(), 0); - LPoint3f from_a = segment->get_point_a() * entry.get_wrt_mat(); - LPoint3f from_b = segment->get_point_b() * entry.get_wrt_mat(); + const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + + LPoint3f from_a = segment->get_point_a() * wrt_mat; + LPoint3f from_b = segment->get_point_b() * wrt_mat; LVector3f from_direction = from_b - from_a; double t1, t2; @@ -683,16 +693,9 @@ set_intersection_point(CollisionEntry *new_entry, // our collision was tangential. LPoint3f orig_point = into_intersection_point - normal * extra_radius; - // And the difference between point and orig_point is the depth to - // which the object has intersected the tube. - float depth = (point - orig_point).length(); - - new_entry->set_into_intersection_point(point); - new_entry->set_into_surface_normal(normal); - new_entry->set_into_depth(depth); - - LVector3f from_depth_vec = (normal * depth) * new_entry->get_inv_wrt_mat(); - new_entry->set_from_depth(from_depth_vec.length()); + new_entry->set_surface_point(point); + new_entry->set_surface_normal(normal); + new_entry->set_interior_point(orig_point); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionVisualizer.I b/panda/src/collide/collisionVisualizer.I index ebb6040459..0dac073e09 100644 --- a/panda/src/collide/collisionVisualizer.I +++ b/panda/src/collide/collisionVisualizer.I @@ -27,3 +27,30 @@ SolidInfo() { _detected_count = 0; _missed_count = 0; } + + +//////////////////////////////////////////////////////////////////// +// Function: CollisionVisualizer::set_viz_scale +// Access: Published +// Description: Scales the objects that are drawn to represent the +// normals and points of the collisions. By default, +// these objects are drawn at an arbitrary scale which +// is appropriate if the scene units are measured in +// feet. Change this scale accordinatly if the scene +// units are measured on some other scale or if you need +// to observe these objects from farther away. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionVisualizer:: +set_viz_scale(float viz_scale) { + _viz_scale = viz_scale; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionVisualizer::get_viz_scale +// Access: Published +// Description: Returns the value last set by set_viz_scale(). +//////////////////////////////////////////////////////////////////// +INLINE float CollisionVisualizer:: +get_viz_scale() const { + return _viz_scale; +} diff --git a/panda/src/collide/collisionVisualizer.cxx b/panda/src/collide/collisionVisualizer.cxx index 9fc1ecabbd..7ccf3ae432 100644 --- a/panda/src/collide/collisionVisualizer.cxx +++ b/panda/src/collide/collisionVisualizer.cxx @@ -45,6 +45,7 @@ CollisionVisualizer(const string &name) : PandaNode(name) { // We always want to render the CollisionVisualizer node itself // (even if it doesn't appear to have any geometry within it). set_bound(OmniBoundingVolume()); + _viz_scale = 1.0f; } //////////////////////////////////////////////////////////////////// @@ -167,28 +168,39 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { for (pi = viz_info._points.begin(); pi != viz_info._points.end(); ++pi) { const CollisionPoint &point = (*pi); - // Draw a tiny sphere at the collision point. + // Draw a small red sphere at the surface point, and a smaller + // white sphere at the interior point. { PT(GeomSphere) sphere = new GeomSphere; PTA_Vertexf verts; - verts.push_back(point._point); - verts.push_back(point._point + LVector3f(0.1f, 0.0f, 0.0f)); + verts.push_back(point._surface_point); + verts.push_back(point._surface_point + + LVector3f(0.1f * _viz_scale, 0.0f, 0.0f)); sphere->set_coords(verts); - sphere->set_colors(colors, G_OVERALL); + sphere->set_colors(colors, G_PER_PRIM); sphere->set_num_prims(1); + + if (point._interior_point != point._surface_point) { + verts.push_back(point._interior_point); + verts.push_back(point._interior_point + + LVector3f(0.05f * _viz_scale, 0.0f, 0.0f)); + sphere->set_num_prims(2); + } + CullableObject *object = new CullableObject(sphere, empty_state, xform_data._render_transform); trav->get_cull_handler()->record_object(object); } - // Draw the normal vector at the collision point. - if (!point._normal.almost_equal(LVector3f::zero())) { + // Draw the normal vector at the surface point. + if (!point._surface_normal.almost_equal(LVector3f::zero())) { PT(GeomLine) line = new GeomLine; PTA_Vertexf verts; - verts.push_back(point._point); - verts.push_back(point._point + point._normal); + verts.push_back(point._surface_point); + verts.push_back(point._surface_point + + point._surface_normal * _viz_scale); line->set_coords(verts); line->set_colors(colors, G_PER_VERTEX); line->set_num_prims(1); @@ -197,25 +209,6 @@ cull_callback(CullTraverser *trav, CullTraverserData &data) { new CullableObject(line, empty_state, xform_data._render_transform); trav->get_cull_handler()->record_object(object); - - // Also draw the depth vector. - if (point._depth != 0.0) { - PT(GeomLine) line = new GeomLine; - - PTA_Vertexf verts; - verts.push_back(point._point); - verts.push_back(point._point - point._normal * point._depth); - PTA_Colorf colors; - colors.push_back(Colorf(0.0f, 0.0f, 1.0f, 1.0f)); - - line->set_coords(verts); - line->set_colors(colors, G_OVERALL); - line->set_num_prims(1); - - CullableObject *object = - new CullableObject(line, empty_state, xform_data._render_transform); - trav->get_cull_handler()->record_object(object); - } } } } @@ -272,18 +265,15 @@ collision_tested(const CollisionEntry &entry, bool detected) { if (detected) { viz_info._solids[entry.get_into()]._detected_count++; - if (entry.has_into_intersection_point()) { + if (entry.has_surface_point()) { CollisionPoint p; - p._point = entry.get_into_intersection_point(); - if (entry.has_into_surface_normal()) { - p._normal = entry.get_into_surface_normal(); + p._surface_point = entry.get_surface_point(entry.get_into_node_path()); + if (entry.has_surface_normal()) { + p._surface_normal = entry.get_surface_normal(entry.get_into_node_path()); } else { - p._normal = LVector3f::zero(); - p._depth = 0.0; - } - if (entry.has_into_depth()) { - p._depth = entry.get_into_depth(); + p._surface_normal = LVector3f::zero(); } + p._interior_point = entry.get_interior_point(entry.get_into_node_path()); viz_info._points.push_back(p); } diff --git a/panda/src/collide/collisionVisualizer.h b/panda/src/collide/collisionVisualizer.h index 7966ebdbd9..ef9e5bcc57 100644 --- a/panda/src/collide/collisionVisualizer.h +++ b/panda/src/collide/collisionVisualizer.h @@ -41,6 +41,9 @@ PUBLISHED: CollisionVisualizer(const string &name); virtual ~CollisionVisualizer(); + INLINE void set_viz_scale(float viz_scale); + INLINE float get_viz_scale() const; + void clear(); public: @@ -69,9 +72,9 @@ private: class CollisionPoint { public: - LPoint3f _point; - LVector3f _normal; - float _depth; + LPoint3f _surface_point; + LVector3f _surface_normal; + LPoint3f _interior_point; }; typedef pvector Points; @@ -84,6 +87,8 @@ private: typedef map Data; Data _data; + float _viz_scale; + public: static TypeHandle get_class_type() { return _type_handle;