diff --git a/panda/src/collide/collisionNode.I b/panda/src/collide/collisionNode.I index 6ab58baca5..bc8959f3c0 100644 --- a/panda/src/collide/collisionNode.I +++ b/panda/src/collide/collisionNode.I @@ -172,39 +172,3 @@ add_solid(CollisionSolid *solid) { mark_bound_stale(); return _solids.size() - 1; } - -//////////////////////////////////////////////////////////////////// -// Function: CollisionNode::clear_velocity -// Access: Published -// Description: Removes the velocity information associated with the -// node. See set_velocity(). -//////////////////////////////////////////////////////////////////// -INLINE void CollisionNode:: -clear_velocity() { - _flags &= ~F_has_velocity; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionNode::has_velocity -// Access: Published -// Description: Returns true if the node has an associated velocity, -// false otherwise. See set_velocity(). -//////////////////////////////////////////////////////////////////// -INLINE bool CollisionNode:: -has_velocity() const { - return (_flags & F_has_velocity) != 0; -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionNode::get_velocity -// Access: Published -// Description: Returns the instantaneous velocity of the node, in -// its own coordinate space. This represents the delta -// between its current position and its position last -// frame. See set_velocity(). -//////////////////////////////////////////////////////////////////// -INLINE const LVector3f &CollisionNode:: -get_velocity() const { - nassertr(has_velocity(), _velocity); - return _velocity; -} diff --git a/panda/src/collide/collisionNode.cxx b/panda/src/collide/collisionNode.cxx index edfd79c21c..656f439ac3 100644 --- a/panda/src/collide/collisionNode.cxx +++ b/panda/src/collide/collisionNode.cxx @@ -44,7 +44,6 @@ CollisionNode(const string &name) : PandaNode(name), _from_collide_mask(CollideMask::all_on()), _into_collide_mask(CollideMask::all_on()), - _velocity(0.0f, 0.0f, 0.0f), _flags(0) { // CollisionNodes are hidden by default. @@ -61,7 +60,6 @@ CollisionNode(const CollisionNode ©) : PandaNode(copy), _from_collide_mask(copy._from_collide_mask), _into_collide_mask(copy._into_collide_mask), - _velocity(copy._velocity), _flags(copy._flags), _solids(copy._solids) { @@ -254,31 +252,6 @@ output(ostream &out) const { } -//////////////////////////////////////////////////////////////////// -// Function: CollisionNode::set_velocity -// Access: Published, Virtual -// Description: Indicates the instantaneous velocity of the node. -// This is only meaningful for nodes that represent -// "colliders", that is, nodes added to a -// CollisionTraverser to be tested for collision into -// other objects. -// -// The velocity vector represents the delta from this -// node's position last frame to its current position. -// The collision traverser automatically clears it after -// it has performed the traversal. -// -// This velocity information is optional and, if -// available, is used by the collision traverser to help -// determine which walls the collider is actually -// intersecting with, and which it is simply passing by. -//////////////////////////////////////////////////////////////////// -void CollisionNode:: -set_velocity(const LVector3f &vel) { - _velocity = vel; - _flags |= F_has_velocity; -} - //////////////////////////////////////////////////////////////////// // Function: CollisionNode::recompute_bound // Access: Protected, Virtual diff --git a/panda/src/collide/collisionNode.h b/panda/src/collide/collisionNode.h index ac07806bf4..3958576c66 100644 --- a/panda/src/collide/collisionNode.h +++ b/panda/src/collide/collisionNode.h @@ -68,11 +68,6 @@ PUBLISHED: INLINE void remove_solid(int n); INLINE int add_solid(CollisionSolid *solid); - virtual void set_velocity(const LVector3f &vel); - INLINE void clear_velocity(); - INLINE bool has_velocity() const; - INLINE const LVector3f &get_velocity() const; - protected: virtual BoundingVolume *recompute_bound(); virtual BoundingVolume *recompute_internal_bound(); @@ -85,11 +80,9 @@ private: // this later. CollideMask _from_collide_mask; CollideMask _into_collide_mask; - LVector3f _velocity; enum Flags { F_collide_geom = 0x0001, - F_has_velocity = 0x0002, // Presently only 8 bits are written to the bam file. }; int _flags; diff --git a/panda/src/pgraph/nodePath.I b/panda/src/pgraph/nodePath.I index 19cbf9f2cb..cd836c3ca3 100644 --- a/panda/src/pgraph/nodePath.I +++ b/panda/src/pgraph/nodePath.I @@ -843,6 +843,59 @@ get_distance(const NodePath &other) const { return length(LVector3f(pos)); } +//////////////////////////////////////////////////////////////////// +// Function: NodePath::set_velocity +// Access: Published +// Description: Sets the velocity of this node. This is a delta from +// the position of this node in the previous frame, +// relative to the node's parent. The value should not +// be transformed by the node's transformation (it is in +// the coordinate system of the node's parent). This +// has meaning only to the collision subsystem. +//////////////////////////////////////////////////////////////////// +INLINE void NodePath:: +set_velocity(const LVector3f &velocity) { + nassertv_always(!is_empty()); + node()->set_velocity(velocity); +} + +//////////////////////////////////////////////////////////////////// +// Function: NodePath::clear_velocity +// Access: Published +// Description: Resets the velocity of this node to zero. See +// set_velocity(). +//////////////////////////////////////////////////////////////////// +INLINE void NodePath:: +clear_velocity() { + nassertv_always(!is_empty()); + node()->clear_velocity(); +} + +//////////////////////////////////////////////////////////////////// +// Function: NodePath::has_velocity +// Access: Published +// Description: Returns true if a nonzero velocity has been set on +// this node, false otherwise. +//////////////////////////////////////////////////////////////////// +INLINE bool NodePath:: +has_velocity() const { + nassertr_always(!is_empty(), false); + return node()->has_velocity(); +} + +//////////////////////////////////////////////////////////////////// +// Function: NodePath::get_velocity +// Access: Published +// Description: Returns the velocity previously set on this node via +// set_velocity(), or LVector3f::zero() if no velocity +// has been set. +//////////////////////////////////////////////////////////////////// +INLINE const LVector3f &NodePath:: +get_velocity() const { + nassertr_always(!is_empty(), LVector3f::zero()); + return node()->get_velocity(); +} + //////////////////////////////////////////////////////////////////// // Function: NodePath::set_color_scale // Access: Published diff --git a/panda/src/pgraph/nodePath.cxx b/panda/src/pgraph/nodePath.cxx index 7eb2fcb58c..71802b49a1 100644 --- a/panda/src/pgraph/nodePath.cxx +++ b/panda/src/pgraph/nodePath.cxx @@ -1502,6 +1502,107 @@ heads_up(const NodePath &other, const LPoint3f &point, const LVector3f &up) { set_quat(quat); } +//////////////////////////////////////////////////////////////////// +// Function: NodePath::get_velocity +// Access: Published +// Description: Returns the relative velocity to this node from the +// other node; i.e. the velocity of this node as seen +// from the other node. +//////////////////////////////////////////////////////////////////// +LVector3f NodePath:: +get_velocity(const NodePath &other) const { + if (other.is_empty()) { + return get_net_velocity(); + } + if (is_empty()) { + other.uncollapse_head(); + CPT(TransformState) net_transform = TransformState::make_identity(); + CPT(TransformState) parent_transform = net_transform; + LVector3f net_velocity = LVector3f::zero(); + r_get_net_velocity(other._head, net_velocity, net_transform, parent_transform); + return -net_velocity; + } + + nassertr(verify_complete(), LVector3f::zero()); + nassertr(other.verify_complete(), LVector3f::zero()); + + int a_count, b_count; + find_common_ancestor(*this, other, a_count, b_count); + + // Now compute the net velocity of each node, by working down from + // the common ancestor. + CPT(TransformState) a_transform = TransformState::make_identity(); + CPT(TransformState) a_parent_transform = a_transform; + LVector3f a_velocity = LVector3f::zero(); + r_get_partial_velocity(_head, a_count, a_velocity, a_transform, + a_parent_transform); + + CPT(TransformState) b_transform = TransformState::make_identity(); + CPT(TransformState) b_parent_transform = b_transform; + LVector3f b_velocity = LVector3f::zero(); + r_get_partial_velocity(other._head, b_count, b_velocity, b_transform, + b_parent_transform); + + LVector3f net_velocity = a_velocity - b_velocity; + + // Finally, convert the net velocity back into the parent's + // coordinate space. + CPT(TransformState) a_parent_inverse = + a_parent_transform->invert_compose(TransformState::make_identity()); + return net_velocity * a_parent_inverse->get_mat(); +} + +//////////////////////////////////////////////////////////////////// +// Function: NodePath::set_velocity +// Access: Published +// Description: Sets the velocity of this node, relative to the other +// node. This computes a new velocity that will have +// the indicated value when seen from the other node. +//////////////////////////////////////////////////////////////////// +void NodePath:: +set_velocity(const NodePath &other, const LVector3f &velocity) { + nassertv(_error_type == ET_ok && other._error_type == ET_ok); + nassertv_always(!is_empty()); + + // First, we perform a wrt to the parent, to get the conversion. + LVector3f rel_velocity; + if (has_parent()) { + rel_velocity = other.get_velocity(get_parent()); + } else { + rel_velocity = other.get_velocity(NodePath()); + } + + LVector3f new_velocity = velocity + rel_velocity; + set_velocity(new_velocity); +} + +//////////////////////////////////////////////////////////////////// +// Function: NodePath::get_net_velocity +// Access: Published +// Description: Returns the relative "velocity" of this node (as +// reported by get_velocity()) from the root of the +// scene graph, accumulating all velocity values set on +// ancestor nodes. The result is returned in the +// coordinate space of the node's parent (the same +// coordinate space in which get_velocity() is +// returned). +//////////////////////////////////////////////////////////////////// +LVector3f NodePath:: +get_net_velocity() const { + uncollapse_head(); + CPT(TransformState) net_transform = TransformState::make_identity(); + CPT(TransformState) parent_transform = net_transform; + LVector3f net_velocity = LVector3f::zero(); + r_get_net_velocity(_head, net_velocity, net_transform, parent_transform); + + // Convert the net velocity back from global coordinates into the + // parent's coordinate space. + CPT(TransformState) parent_inverse = + parent_transform->invert_compose(TransformState::make_identity()); + return net_velocity * parent_inverse->get_mat(); +} + + //////////////////////////////////////////////////////////////////// // Function: NodePath::set_color // Access: Published @@ -3206,11 +3307,15 @@ uncollapse_head() const { // that both have in common, if any. Fills a_count and // b_count with the number of nodes below the common // node in each path. +// +// The return value is the NodePathComponent of the node +// they have in common, or NULL if they have nothing in +// common. //////////////////////////////////////////////////////////////////// -void NodePath:: +NodePathComponent *NodePath:: find_common_ancestor(const NodePath &a, const NodePath &b, int &a_count, int &b_count) { - nassertv(!a.is_empty() && !b.is_empty()); + nassertr(!a.is_empty() && !b.is_empty(), NULL); a.uncollapse_head(); b.uncollapse_head(); @@ -3221,12 +3326,12 @@ find_common_ancestor(const NodePath &a, const NodePath &b, // Shorten up the longer one until they are the same length. while (ac->get_length() > bc->get_length()) { - nassertv(ac != (NodePathComponent *)NULL); + nassertr(ac != (NodePathComponent *)NULL, NULL); ac = ac->get_next(); a_count++; } while (bc->get_length() > ac->get_length()) { - nassertv(bc != (NodePathComponent *)NULL); + nassertr(bc != (NodePathComponent *)NULL, NULL); bc = bc->get_next(); b_count++; } @@ -3234,19 +3339,21 @@ find_common_ancestor(const NodePath &a, const NodePath &b, // Now shorten them both up until we reach the same component. while (ac != bc) { // These shouldn't go to NULL unless they both go there together. - nassertv(ac != (NodePathComponent *)NULL); - nassertv(bc != (NodePathComponent *)NULL); + nassertr(ac != (NodePathComponent *)NULL, NULL); + nassertr(bc != (NodePathComponent *)NULL, NULL); ac = ac->get_next(); a_count++; bc = bc->get_next(); b_count++; } + + return ac; } //////////////////////////////////////////////////////////////////// // Function: NodePath::r_get_net_state // Access: Private -// Description: Recursively determines the net state chnages to the +// Description: Recursively determines the net state changes to the // indicated component node from the root of the graph. //////////////////////////////////////////////////////////////////// CPT(RenderState) NodePath:: @@ -3311,6 +3418,62 @@ r_get_partial_transform(NodePathComponent *comp, int n) const { } } +//////////////////////////////////////////////////////////////////// +// Function: NodePath::r_get_net_velocity +// Access: Private +// Description: Recursively determines the net velocity of the +// indicated node; the result is returned in the global +// coordinate space. +//////////////////////////////////////////////////////////////////// +void NodePath:: +r_get_net_velocity(NodePathComponent *comp, LVector3f &net_vel, + CPT(TransformState) &net_transform, + CPT(TransformState) &parent_net_transform) const { + if (comp != (NodePathComponent *)NULL) { + r_get_net_velocity(comp->get_next(), net_vel, net_transform, + parent_net_transform); + + PandaNode *node = comp->get_node(); + if (node->has_velocity()) { + LVector3f vel = node->get_velocity(); + net_vel += vel * net_transform->get_mat(); + } + + CPT(TransformState) transform = node->get_transform(); + parent_net_transform = net_transform; + net_transform = net_transform->compose(transform); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: NodePath::r_get_partial_velocity +// Access: Private +// Description: Recursively determines the net velocity of the +// indicated component node from the nth node above it. +// If n exceeds the length of the path, this returns the +// net velocity from the root of the graph. +//////////////////////////////////////////////////////////////////// +void NodePath:: +r_get_partial_velocity(NodePathComponent *comp, int n, + LVector3f &net_vel, + CPT(TransformState) &net_transform, + CPT(TransformState) &parent_net_transform) const { + if (n != 0 && comp != (NodePathComponent *)NULL) { + r_get_partial_velocity(comp->get_next(), n - 1, net_vel, net_transform, + parent_net_transform); + + PandaNode *node = comp->get_node(); + if (node->has_velocity()) { + LVector3f vel = node->get_velocity(); + net_vel += vel * net_transform->get_mat(); + } + + CPT(TransformState) transform = node->get_transform(); + parent_net_transform = net_transform; + net_transform = net_transform->compose(transform); + } +} + //////////////////////////////////////////////////////////////////// // Function: NodePath::find_matches // Access: Private diff --git a/panda/src/pgraph/nodePath.h b/panda/src/pgraph/nodePath.h index 30c175d0f3..bac8a69ddc 100644 --- a/panda/src/pgraph/nodePath.h +++ b/panda/src/pgraph/nodePath.h @@ -415,6 +415,16 @@ PUBLISHED: INLINE float get_distance(const NodePath &other) const; + INLINE void set_velocity(const LVector3f &velocity); + INLINE void clear_velocity(); + INLINE bool has_velocity() const; + INLINE const LVector3f &get_velocity() const; + + LVector3f get_velocity(const NodePath &other) const; + void set_velocity(const NodePath &other, const LVector3f &velocity); + + LVector3f get_net_velocity() const; + // Methods that affect appearance of geometry: color, texture, etc. // These affect the state at the bottom level only. @@ -571,14 +581,23 @@ PUBLISHED: private: void uncollapse_head() const; - static void find_common_ancestor(const NodePath &a, const NodePath &b, - int &a_count, int &b_count); + static NodePathComponent * + find_common_ancestor(const NodePath &a, const NodePath &b, + int &a_count, int &b_count); CPT(RenderState) r_get_net_state(NodePathComponent *comp) const; CPT(RenderState) r_get_partial_state(NodePathComponent *comp, int n) const; CPT(TransformState) r_get_net_transform(NodePathComponent *comp) const; CPT(TransformState) r_get_partial_transform(NodePathComponent *comp, int n) const; + void r_get_net_velocity(NodePathComponent *comp, LVector3f &net_vel, + CPT(TransformState) &net_transform, + CPT(TransformState) &parent_net_transform) const; + void r_get_partial_velocity(NodePathComponent *comp, int n, + LVector3f &net_vel, + CPT(TransformState) &net_transform, + CPT(TransformState) &parent_net_transform) const; + void find_matches(NodePathCollection &result, const string &approx_path_str, int max_matches) const; diff --git a/panda/src/pgraph/pandaNode.I b/panda/src/pgraph/pandaNode.I index dcaae24f96..7b376403b4 100644 --- a/panda/src/pgraph/pandaNode.I +++ b/panda/src/pgraph/pandaNode.I @@ -119,6 +119,8 @@ CData() { _effects = RenderEffects::make_empty(); _transform = TransformState::make_identity(); _draw_mask = DrawMask::all_on(); + _velocity = LVector3f::zero(); + _has_velocity = false; _net_collide_mask = CollideMask::all_off(); _fixed_internal_bound = false; } @@ -139,6 +141,8 @@ CData(const PandaNode::CData ©) : _transform(copy._transform), _tag_data(copy._tag_data), _draw_mask(copy._draw_mask), + _velocity(copy._velocity), + _has_velocity(copy._has_velocity), _fixed_internal_bound(copy._fixed_internal_bound) { _net_collide_mask = CollideMask::all_off(); @@ -780,6 +784,61 @@ get_net_collide_mask() const { return cdata->_net_collide_mask; } +//////////////////////////////////////////////////////////////////// +// Function: PandaNode::set_velocity +// Access: Published +// Description: Sets the velocity of this node. This is a delta from +// the position of this node in the previous frame, +// relative to the node's parent. The value should not +// be transformed by the node's transformation (it is in +// the coordinate system of the node's parent). This +// has meaning only to the collision subsystem. +//////////////////////////////////////////////////////////////////// +INLINE void PandaNode:: +set_velocity(const LVector3f &velocity) { + CDWriter cdata(_cycler); + cdata->_velocity = velocity; + cdata->_has_velocity = (!velocity.almost_equal(LVector3f::zero())); +} + +//////////////////////////////////////////////////////////////////// +// Function: PandaNode::clear_velocity +// Access: Published +// Description: Resets the velocity of this node to zero. See +// set_velocity(). +//////////////////////////////////////////////////////////////////// +INLINE void PandaNode:: +clear_velocity() { + CDWriter cdata(_cycler); + cdata->_velocity = LVector3f::zero(); + cdata->_has_velocity = false; +} + +//////////////////////////////////////////////////////////////////// +// Function: PandaNode::has_velocity +// Access: Published +// Description: Returns true if a nonzero velocity has been set on +// this node, false otherwise. +//////////////////////////////////////////////////////////////////// +INLINE bool PandaNode:: +has_velocity() const { + CDReader cdata(_cycler); + return cdata->_has_velocity; +} + +//////////////////////////////////////////////////////////////////// +// Function: PandaNode::get_velocity +// Access: Published +// Description: Returns the velocity previously set on this node via +// set_velocity(), or LVector3f::zero() if no velocity +// has been set. +//////////////////////////////////////////////////////////////////// +INLINE const LVector3f &PandaNode:: +get_velocity() const { + CDReader cdata(_cycler); + return cdata->_velocity; +} + //////////////////////////////////////////////////////////////////// // Function: PandaNode::set_bound // Access: Published diff --git a/panda/src/pgraph/pandaNode.cxx b/panda/src/pgraph/pandaNode.cxx index 0a486612be..7da6a8fa55 100644 --- a/panda/src/pgraph/pandaNode.cxx +++ b/panda/src/pgraph/pandaNode.cxx @@ -1388,25 +1388,6 @@ as_light() { return NULL; } -//////////////////////////////////////////////////////////////////// -// Function: PandaNode::set_velocity -// Access: Published, Virtual -// Description: Indicates the instantaneous velocity of this node. -// This function is meaningless to most kinds of nodes; -// it is implemented only for CollisionNodes and is -// intended to inform the collision system of velocity. -// -// It is defined at this level only as an abstract -// interface to allow setting the velocity of a -// collision node without having to link with, or know -// anything about, the collision system. -// -// See CollisionNode::set_velocity(). -//////////////////////////////////////////////////////////////////// -void PandaNode:: -set_velocity(const LVector3f &) { -} - //////////////////////////////////////////////////////////////////// // Function: PandaNode::propagate_stale_bound // Access: Protected, Virtual diff --git a/panda/src/pgraph/pandaNode.h b/panda/src/pgraph/pandaNode.h index 27cc7255d9..fb04f67d21 100644 --- a/panda/src/pgraph/pandaNode.h +++ b/panda/src/pgraph/pandaNode.h @@ -162,6 +162,11 @@ PUBLISHED: INLINE CollideMask get_net_collide_mask() const; + INLINE void set_velocity(const LVector3f &velocity); + INLINE void clear_velocity(); + INLINE bool has_velocity() const; + INLINE const LVector3f &get_velocity() const; + virtual void output(ostream &out) const; virtual void write(ostream &out, int indent_level) const; @@ -186,7 +191,6 @@ PUBLISHED: virtual bool is_geom_node() const; virtual Light *as_light(); - virtual void set_velocity(const LVector3f &vel); protected: // Inherited from BoundedObject @@ -316,6 +320,9 @@ private: // This is the draw_mask of this particular node. DrawMask _draw_mask; + LVector3f _velocity; + bool _has_velocity; + // This is the union of all into_collide_mask bits for any // CollisionNodes at and below this level. It's conceptually // similar to a bounding volume--it represents the bounding volume