panda3d/panda/src/graph/wrt.I

// Filename: wrt.cxx
// Created by:  drose (26Oct98)
// 
////////////////////////////////////////////////////////////////////

#include "wrt.h"
#include "nodeRelation.h"
#include "node.h"
#include "config_graph.h"

////////////////////////////////////////////////////////////////////
//     Function: get_cached_net_transition
//  Description: Returns the net transition from the root of the
//               graph, or the nearest ancestor with multiple parents,
//               to and including the indicated arc.  This is a
//               support function for wrt() and should not be called
//               directly.
//
//               This flavor of get_cached_net_transition() gets the
//               transition up to an arc, instead of a node, and does
//               not necessarily start at the root of the scene graph.
//               By starting at the nearest ancestor with multiple
//               parents, we allow ambiguous paths in the scene graph
//               without interfering with state caching.  Since there
//               are no nodes with multiple parents in the chain of
//               transitions returned by this function, the result is
//               always unambiguous; any wrt() ambiguity is resolved
//               later.
////////////////////////////////////////////////////////////////////
template<class TransitionWrapper>
void
get_cached_net_transition(NodeRelation *arc, Node *&root, UpdateSeq now,
			  TransitionWrapper &net, TypeHandle graph_type) {
  TransitionWrapper cur_cache = TransitionWrapper::init_from(net);
  Node *top_subtree = cur_cache.extract_from_cache(arc);

  if (cur_cache.is_cache_verified(now)) {
    // This arc's cached value has recently been verified, so we don't
    // even need to go anywhere--we trust it's still good.
    root = top_subtree;
    net = cur_cache;

  } else {
    // This arc's cache hasn't recently been verified, and we need to
    // verify it now.  This will entail at least walking up the scene
    // graph to the root, and possibly recomputing the cache as we go.

    // Get the node above the arc.
    Node *node = arc->get_parent();

    UpRelations::const_iterator uri;
    uri = node->_parents.find(graph_type);

    if (uri == node->_parents.end() || (*uri).second.size() != 1) {
      // The node has zero or multiple parents.  It's thus either the
      // top of the scene graph, or it's the first ancestor with
      // multiple parents; in either case, it's as far as we can take
      // the caching.  Stop here.

      if (uri == node->_parents.end() || (*uri).second.empty()) {
	// The node is the very top of the scene graph.  We'll treat
	// this as a special case by setting the node to NULL.  This
	// will tell our calling function that there's no need to look
	// further.
	node = NULL;
      }

      root = node;
      net.extract_from(arc);
      net.set_computed_verified(now);
      net.store_to_cache(arc, node);

    } else {
      // The node has exactly one parent.  Carry on.

      NodeRelation *parent = *(*uri).second.begin();
      get_cached_net_transition(parent, root, now, net, graph_type);

      // Now recompute our own cache.
      TransitionWrapper cur_value = TransitionWrapper::init_from(net);
      cur_value.extract_from(arc);

      net.cached_compose(cur_cache, cur_value, now);
      net.store_to_cache(arc, root);
    }
  }
}    


////////////////////////////////////////////////////////////////////
//     Function: get_cached_net_transition
//  Description: Returns the net transition from the root of the graph
//               to the indicated node.  This is a support function
//               for wrt() and should not be called directly.
//
//               If [arc_list_begin..arc_list_end) is nonempty, it
//               contains a list of NodeRelation pointers to resolve
//               ambiguities when a node with multiple parents is
//               encountered.
////////////////////////////////////////////////////////////////////
template<class InputIterator, class TransitionWrapper>
void
get_cached_net_transition(const Node *node, 
			  InputIterator arc_list_begin, 
			  InputIterator arc_list_end,
			  UpdateSeq now,
			  TransitionWrapper &result,
			  TypeHandle graph_type) {
  if (node == NULL) {
    // the NULL node is by convention equivalent to the root.
    result.make_identity();
    return;
  }

  UpRelations::const_iterator uri;
  uri = node->_parents.find(graph_type);

  if (uri == node->_parents.end()) {
    // This node has no parents.  Stop here.
    result.make_identity();
    return;
  }

  const UpRelationPointers &urp = (*uri).second;
  if (urp.empty()) {
    // Again, this node has no parents.
    result.make_identity();
    return;
  }

  const NodeRelation *parent_arc = (const NodeRelation *)NULL;

  if (urp.size() == 1) {
    // There is only one parent, so there's no doubt as to which arc
    // to choose.
    parent_arc = *(urp.begin());

  } else {
    // The node has multiple parents, and we have a list of arcs in
    // arc_list_begin .. arc_list_end.  If any of the parents is
    // mentioned in the list, we must choose that parent.
    for (InputIterator ri = arc_list_begin; 
	 parent_arc == (NodeRelation *)NULL && ri != arc_list_end; 
	 ++ri) {
      if ((*ri)->get_child() == node) {
	parent_arc = (*ri);
      }
    }

    if (parent_arc == (NodeRelation *)NULL) {
      // No, it wasn't mentioned.  Issue a warning and use the first
      // one.
      graph_cat.warning()
	<< *node << " has multiple parents; wrt() ambiguous.\n";
      parent_arc = *(urp.begin());
    }
  }

  // Get the net transition leading into this node.
  Node *root;
  get_cached_net_transition((NodeRelation *)parent_arc, root, now, result, 
			    graph_type);

  if (root != NULL) {
    // There's more to get.  The above function call was forced to
    // stop at a node with multiple parents.
    TransitionWrapper more = TransitionWrapper::init_from(result);
    get_cached_net_transition(root, arc_list_begin, arc_list_end, 
			      now, more, graph_type);
    more.compose_in_place(result);
    result = more;
  }
}

////////////////////////////////////////////////////////////////////
//     Function: get_uncached_net_transition
//  Description: Returns the net transition from the root of the graph
//               to the indicated node.  This is a support function
//               for wrt() and should not be called directly.
//
//               If [arc_list_begin..arc_list_end) is nonempty, it
//               contains a list of NodeRelation pointers to resolve
//               ambiguities when a node with multiple parents is
//               encountered.
//
//               This variant of get_cached_net_transition() does not
//               respect or update the cache values stored in the
//               arcs.  It's intended only as a debugging doublecheck
//               against the normal get_cached_net_transition()
//               function that does do this.
////////////////////////////////////////////////////////////////////
template<class InputIterator, class TransitionWrapper>
void
get_uncached_net_transition(const Node *node, 
			    InputIterator arc_list_begin, 
			    InputIterator arc_list_end,
			    TransitionWrapper &result,
			    TypeHandle graph_type) {
  if (node == NULL) {
    // the NULL node is by convention equivalent to the root.
    result.make_identity();
    return;
  }

  UpRelations::const_iterator uri;
  uri = node->_parents.find(graph_type);

  if (uri == node->_parents.end()) {
    // This node has no parents.  Stop here.
    result.make_identity();
    return;
  }

  const UpRelationPointers &urp = (*uri).second;
  if (urp.empty()) {
    // Again, this node has no parents.
    result.make_identity();
    return;
  }

  const NodeRelation *parent_arc = (const NodeRelation *)NULL;

  if (urp.size() == 1) {
    // There is only one parent, so there's no doubt as to which arc
    // to choose.
    parent_arc = *(urp.begin());

  } else {
    // The node has multiple parents, and we have a list of arcs in
    // arc_list_begin .. arc_list_end.  If any of the parents is
    // mentioned in the list, we must choose that parent.
    for (InputIterator ri = arc_list_begin; 
	 parent_arc == (NodeRelation *)NULL && ri != arc_list_end; 
	 ++ri) {
      if ((*ri)->get_child() == node) {
	parent_arc = (*ri);
      }
    }

    if (parent_arc == (NodeRelation *)NULL) {
      // No, it wasn't mentioned.  Issue a warning and use the first
      // one.
      graph_cat.warning()
	<< *node << " has multiple parents; wrt() ambiguous.\n";
      parent_arc = *(urp.begin());
    }
  }

  get_uncached_net_transition(parent_arc->get_parent(),
			      arc_list_begin, arc_list_end,
			      result,
			      graph_type);

  TransitionWrapper next = TransitionWrapper::init_from(result);
  next.extract_from(parent_arc);

  result.compose_in_place(next);
}



////////////////////////////////////////////////////////////////////
//     Function: cached_wrt_base
//  Description: The implementation of wrt_base, below, but always
//               uses the cached value.  This is the normal behavior.
////////////////////////////////////////////////////////////////////
template<class InputIterator1, class InputIterator2, class TransitionWrapper>
void
cached_wrt_base(const Node *from,
		InputIterator1 from_arcs_begin, InputIterator1 from_arcs_end,
		const Node *to, 
		InputIterator2 to_arcs_begin, InputIterator2 to_arcs_end,
		TransitionWrapper &result,
		TypeHandle graph_type) {

  UpdateSeq now = last_graph_update[graph_type];

  TransitionWrapper net_from_trans = TransitionWrapper::init_from(result);

  get_cached_net_transition(from, from_arcs_begin, from_arcs_end, now,
		     net_from_trans, graph_type);
  get_cached_net_transition(to, to_arcs_begin, to_arcs_end, now,
		     result, graph_type);
  
#ifndef NDEBUG
  if (paranoid_wrt) {
    // We don't entirely trust the caching to work flawlessly.  Go
    // ahead and compute the uncached transition just to doublecheck.
    TransitionWrapper check_from_trans =
      TransitionWrapper::init_from(result);
    TransitionWrapper check_to_trans = 
      TransitionWrapper::init_from(result);
    get_uncached_net_transition(from, from_arcs_begin, from_arcs_end,
				check_from_trans, graph_type);
    if (check_from_trans.compare_to(net_from_trans) != 0) {
      graph_cat.warning()
	<< "WRT cache from " << *from << " is invalid!\n"
	<< "  cached value is " << net_from_trans << "\n"
	<< "  should be " << check_from_trans << "\n";
      net_from_trans = check_from_trans;
    }
    
    get_uncached_net_transition(to, to_arcs_begin, to_arcs_end,
				check_to_trans, graph_type);
    if (check_to_trans.compare_to(result) != 0) {
      graph_cat.warning()
	<< "WRT cache to " << *to << " is invalid!\n"
	<< "  cached value is " << result << "\n"
	<< "  should be " << check_to_trans << "\n";
      result = check_to_trans;
    }
  }
#endif

  result.invert_compose_in_place(net_from_trans);
}

////////////////////////////////////////////////////////////////////
//     Function: uncached_wrt_base
//  Description: The implementation of wrt_base, below, but never
//               cached.
////////////////////////////////////////////////////////////////////
template<class InputIterator1, class InputIterator2, class TransitionWrapper>
void
uncached_wrt_base(const Node *from,
		  InputIterator1 from_arcs_begin, InputIterator1 from_arcs_end,
		  const Node *to, 
		  InputIterator2 to_arcs_begin, InputIterator2 to_arcs_end,
		  TransitionWrapper &result,
		  TypeHandle graph_type) {
  //  UpdateSeq now = last_graph_update[graph_type];

  TransitionWrapper net_from_trans = TransitionWrapper::init_from(result);
  get_uncached_net_transition(from, from_arcs_begin, from_arcs_end,
			      net_from_trans, graph_type);
  get_uncached_net_transition(to, to_arcs_begin, to_arcs_end,
			      result, graph_type);
  result.invert_compose_in_place(net_from_trans);
}

////////////////////////////////////////////////////////////////////
//     Function: wrt_base
//  Description: Returns the net transition from the node "to"
//               to the node "from".
//
//               If [from_arcs_begin..from_arcs_end) is nonempty, it
//               contains a list of NodeRelation pointers to resolve
//               ambiguities when a node with multiple parents is
//               encountered as an ancestor of the from node.  If any
//               of the parents is listed, that parent will be chosen.
//
//               Similarly with [to_arcs_begin..to_arcs_end).
//
//               wrt_base() is a low-level implementation; use the
//               various wrt() wrappers instead.
////////////////////////////////////////////////////////////////////
template<class InputIterator1, class InputIterator2, class TransitionWrapper>
INLINE void
wrt_base(const Node *from,
	 InputIterator1 from_arcs_begin, InputIterator1 from_arcs_end,
	 const Node *to, 
	 InputIterator2 to_arcs_begin, InputIterator2 to_arcs_end,
	 TransitionWrapper &result,
	 TypeHandle graph_type) {
  if (cache_wrt) {
    cached_wrt_base(from, from_arcs_begin, from_arcs_end,
		    to, to_arcs_begin, to_arcs_end,
		    result, graph_type);
  } else {
    uncached_wrt_base(from, from_arcs_begin, from_arcs_end,
		      to, to_arcs_begin, to_arcs_end,
		      result, graph_type);
  }
}

template<class TransitionWrapper>
INLINE void
wrt(const Node *from, const Node *to,
    TransitionWrapper &result, TypeHandle graph_type) {
  // In the normal wrt() interface, we have no from_arcs or to_arcs
  // list.  Supply empty lists for both.
  const NodeRelation *arc = NULL;
  wrt_base(from, &arc, &arc, to, &arc, &arc, result, graph_type);
}

template<class InputIterator, class TransitionWrapper>
INLINE void
wrt(const Node *from,
    InputIterator from_arcs_begin, InputIterator from_arcs_end,
    const Node *to, 
    TransitionWrapper &result, TypeHandle graph_type) {
  const NodeRelation *arc = NULL;
  wrt_base(from, from_arcs_begin, from_arcs_end, to, &arc, &arc,
	   result, graph_type);
}

template<class InputIterator, class TransitionWrapper>
INLINE void
wrt(const Node *from,
    const Node *to,  
    InputIterator to_arcs_begin, InputIterator to_arcs_end,
    TransitionWrapper &result, TypeHandle graph_type) {
  const NodeRelation *arc = NULL;
  wrt_base(from, &arc, &arc, to, to_arcs_begin, to_arcs_end,
	   result, graph_type);
}

template<class InputIterator1, class InputIterator2, class TransitionWrapper>
INLINE void
wrt(const Node *from,
    InputIterator1 from_arcs_begin, InputIterator1 from_arcs_end,
    const Node *to, 
    InputIterator2 to_arcs_begin, InputIterator2 to_arcs_end,
    TransitionWrapper &result, TypeHandle graph_type) {
  wrt_base(from, from_arcs_begin, from_arcs_end,
	   to, to_arcs_begin, to_arcs_end,
	   result, graph_type);
}

template<class TransitionWrapper>
INLINE void
uncached_wrt(const Node *from, const Node *to,
	     TransitionWrapper &result, TypeHandle graph_type) {
  const NodeRelation *arc = NULL;
  uncached_wrt_base(from, &arc, &arc, to, &arc, &arc, result, graph_type);
}

template<class InputIterator, class TransitionWrapper>
INLINE void
uncached_wrt(const Node *from,
	     InputIterator from_arcs_begin, InputIterator from_arcs_end,
	     const Node *to, 
	     TransitionWrapper &result, TypeHandle graph_type) {
  const NodeRelation *arc = NULL;
  uncached_wrt_base(from, from_arcs_begin, from_arcs_end, to, &arc, &arc,
		    result, graph_type);
}

template<class InputIterator, class TransitionWrapper>
INLINE void
uncached_wrt(const Node *from,
	     const Node *to,  
	     InputIterator to_arcs_begin, InputIterator to_arcs_end,
	     TransitionWrapper &result, TypeHandle graph_type) {
  const NodeRelation *arc = NULL;
  uncached_wrt_base(from, &arc, &arc, to, to_arcs_begin, to_arcs_end,
		    result, graph_type);
}

template<class InputIterator1, class InputIterator2, class TransitionWrapper>
INLINE void
uncached_wrt(const Node *from,
	     InputIterator1 from_arcs_begin, InputIterator1 from_arcs_end,
	     const Node *to, 
	     InputIterator2 to_arcs_begin, InputIterator2 to_arcs_end,
	     TransitionWrapper &result, TypeHandle graph_type) {
  uncached_wrt_base(from, from_arcs_begin, from_arcs_end,
		    to, to_arcs_begin, to_arcs_end,
		    result, graph_type);
}

template<class TransitionWrapper>
Node *
wrt_subtree(NodeRelation *arc, Node *to, TransitionWrapper &result, 
	    TypeHandle graph_type) {
  if (!cache_wrt) {
    // If we aren't caching wrt, do this the hard way.
    uncached_wrt(arc->get_child(), &arc, &arc + 1, to, result, graph_type);
    return to;
  }

  UpdateSeq now = last_graph_update[graph_type];

  // First, determine the net transition up to the top of the current
  // subtree.
  Node *top_subtree;
  get_cached_net_transition(arc, top_subtree, now, result, graph_type);

  if (top_subtree == to || to == (Node *)NULL) {
    // If the top of the subtree is the node we asked to wrt to,
    // excellent!  Stop here.

  } else {

    // Otherwise, it must be the case that the node we want to wrt to is
    // some descendent of the top_subtree node.  It also therefore
    // follows that this node has exactly one parent.
    
    nassertr(to->get_num_parents(graph_type) == 1, NULL);
    NodeRelation *to_arc = to->get_parent(graph_type, 0);
    
    // Save the result from the first pass.
    TransitionWrapper net_from_trans = result;
    
    // Now determine the net transition to the top of the subtree from
    // this arc.  It had better be the same subtree!
    Node *top_subtree_2;
    get_cached_net_transition(to_arc, top_subtree_2, now, result, graph_type);
    nassertr(top_subtree == top_subtree_2, NULL);
    
    // And now compute the actual wrt.
    result.invert_compose_in_place(net_from_trans);
  }

#ifndef NDEBUG
  if (paranoid_wrt) {
    // Now check the results.
    TransitionWrapper check_trans =
      TransitionWrapper::init_from(result);
    uncached_wrt(arc->get_child(), &arc, &arc + 1, to, check_trans, 
		 graph_type);

    if (check_trans.compare_to(result) != 0) {
      graph_cat.warning()
	<< "WRT subtree cache from " << *arc->get_child() << " to ";
      if (to == (Node *)NULL) {
	graph_cat.warning(false) << "(top)";
      } else {
	graph_cat.warning(false) << *to;
      }
      graph_cat.warning(false)
	<< " is invalid!\n"
	<< "  cached value is " << result << "\n"
	<< "  should be " << check_trans << "\n";
      result = check_trans;
    }
  }
#endif

  return top_subtree;
}