void RigidBody::_body_inout(int p_status, ObjectID p_instance, int p_body_shape, int p_local_shape) {
bool body_in = p_status == 1;
ObjectID objid = p_instance;
Object *obj = ObjectDB::get_instance(objid);
Node *node = obj ? obj->cast_to<Node>() : NULL;
Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(objid);
ERR_FAIL_COND(!body_in && !E);
if (body_in) {
if (!E) {
E = contact_monitor->body_map.insert(objid, BodyState());
//E->get().rc=0;
E->get().in_tree = node && node->is_inside_tree();
if (node) {
node->connect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree, make_binds(objid));
node->connect(SceneStringNames::get_singleton()->tree_exited, this, SceneStringNames::get_singleton()->_body_exit_tree, make_binds(objid));
if (E->get().in_tree) {
emit_signal(SceneStringNames::get_singleton()->body_entered, node);
}
}
}
//E->get().rc++;
if (node)
E->get().shapes.insert(ShapePair(p_body_shape, p_local_shape));
if (E->get().in_tree) {
emit_signal(SceneStringNames::get_singleton()->body_shape_entered, objid, node, p_body_shape, p_local_shape);
}
} else {
//E->get().rc--;
if (node)
E->get().shapes.erase(ShapePair(p_body_shape, p_local_shape));
bool in_tree = E->get().in_tree;
if (E->get().shapes.empty()) {
if (node) {
node->disconnect(SceneStringNames::get_singleton()->tree_entered, this, SceneStringNames::get_singleton()->_body_enter_tree);
node->disconnect(SceneStringNames::get_singleton()->tree_exited, this, SceneStringNames::get_singleton()->_body_exit_tree);
if (in_tree)
emit_signal(SceneStringNames::get_singleton()->body_exited, obj);
}
contact_monitor->body_map.erase(E);
}
if (node && in_tree) {
emit_signal(SceneStringNames::get_singleton()->body_shape_exited, objid, obj, p_body_shape, p_local_shape);
}
}
}
void Area::_body_inout(int p_status,const RID& p_body, int p_instance, int p_body_shape,int p_area_shape) {
bool body_in = p_status==PhysicsServer::AREA_BODY_ADDED;
ObjectID objid=p_instance;
Object *obj = ObjectDB::get_instance(objid);
Node *node = obj ? obj->cast_to<Node>() : NULL;
Map<ObjectID,BodyState>::Element *E=body_map.find(objid);
ERR_FAIL_COND(!body_in && !E);
locked=true;
if (body_in) {
if (!E) {
E = body_map.insert(objid,BodyState());
E->get().rc=0;
E->get().in_tree=node && node->is_inside_tree();
if (node) {
node->connect(SceneStringNames::get_singleton()->enter_tree,this,SceneStringNames::get_singleton()->_body_enter_tree,make_binds(objid));
node->connect(SceneStringNames::get_singleton()->exit_tree,this,SceneStringNames::get_singleton()->_body_exit_tree,make_binds(objid));
if (E->get().in_tree) {
emit_signal(SceneStringNames::get_singleton()->body_enter,node);
}
}
}
E->get().rc++;
if (node)
E->get().shapes.insert(ShapePair(p_body_shape,p_area_shape));
if (E->get().in_tree) {
emit_signal(SceneStringNames::get_singleton()->body_enter_shape,objid,node,p_body_shape,p_area_shape);
}
} else {
E->get().rc--;
if (node)
E->get().shapes.erase(ShapePair(p_body_shape,p_area_shape));
bool eraseit=false;
if (E->get().rc==0) {
if (node) {
node->disconnect(SceneStringNames::get_singleton()->enter_tree,this,SceneStringNames::get_singleton()->_body_enter_tree);
node->disconnect(SceneStringNames::get_singleton()->exit_tree,this,SceneStringNames::get_singleton()->_body_exit_tree);
if (E->get().in_tree)
emit_signal(SceneStringNames::get_singleton()->body_exit,obj);
}
eraseit=true;
}
if (node && E->get().in_tree) {
emit_signal(SceneStringNames::get_singleton()->body_exit_shape,objid,obj,p_body_shape,p_area_shape);
}
if (eraseit)
body_map.erase(E);
}
locked=false;
}
void RootCluster::calculateClusterPathsToEachNode(size_t nodesCount)
{
m_cluster_vectors_leading_to_nodes.clear();
m_cluster_vectors_leading_to_nodes.resize(nodesCount);
recPathToCluster(this, Clusters());
for (unsigned i = 0; i < m_cluster_vectors_leading_to_nodes.size(); ++i)
{
size_t paths = m_cluster_vectors_leading_to_nodes[i].size();
for (size_t j = 1; j < paths; ++j)
{
for (size_t k = 0; k < j; ++k)
{
// For each pair of paths.
// Find the lowest common ancestor by finding where the two
// paths from the root cluster to node i diverge.
Clusters pathJ = m_cluster_vectors_leading_to_nodes[i][j];
Clusters pathK = m_cluster_vectors_leading_to_nodes[i][k];
size_t lcaIndex = 0;
while ((lcaIndex < pathJ.size()) &&
(lcaIndex < pathK.size()) &&
(pathJ[lcaIndex] == pathK[lcaIndex]))
{
++lcaIndex;
}
COLA_ASSERT(lcaIndex > 0);
// lcaIndex will be the clusters/nodes that need to overlap
// due to these two paths to node i.
size_t lcaChildJIndex = i;
size_t lcaChildKIndex = i;
Cluster *lcaChildJCluster = nullptr;
Cluster *lcaChildKCluster = nullptr;
// lcaIndex < path{J,K}.size() means the child J or K of
// the lca is a Cluster. At least one of them will always
// be a cluster.
COLA_ASSERT((lcaIndex < pathJ.size()) ||
(lcaIndex < pathK.size()));
if (lcaIndex < pathJ.size())
{
lcaChildJCluster = pathJ[lcaIndex];
lcaChildJIndex = lcaChildJCluster->clusterVarId;
}
if (lcaIndex < pathK.size())
{
lcaChildKCluster = pathK[lcaIndex];
lcaChildKIndex = lcaChildKCluster->clusterVarId;
}
// We want to exclude the overlapping children of the lca
// from having non-overlap constraints generated for them
// (siblings of a particular cluster usually have
// non-overlap constraints generated for them).
Cluster *lcaCluster = pathJ[lcaIndex - 1];
lcaCluster->m_cluster_cluster_overlap_exceptions.insert(
ShapePair(lcaChildJIndex, lcaChildKIndex));
if (lcaChildJCluster)
{
// In cluster J, replace node i with cluster K for the
// purpose of non-overlap with siblings, and remember
// this replacement so we can still generate non-overlap
// constraints between multiple nodes that are children
// of the same overlapping clusters.
lcaChildJCluster->m_overlap_replacement_map[i] =
lcaChildKCluster;
lcaChildJCluster->m_nodes_replaced_with_clusters.insert(i);
}
if (lcaChildKCluster)
{
// In cluster K, replace node i with cluster J for the
// purpose of non-overlap with siblings, and remember
// this replacement so we can still generate non-overlap
// constraints between multiple nodes that are children
// of the same overlapping clusters.
lcaChildKCluster->m_overlap_replacement_map[i] =
lcaChildJCluster;
lcaChildKCluster->m_nodes_replaced_with_clusters.insert(i);
}
}
}
}
}