C++ Contact::getCollisionModels方法代码示例

void DefaultCollisionGroupManager::createGroups(core::objectmodel::BaseContext* scene, const sofa::helper::vector<Contact::SPtr>& contacts)
{
    int groupIndex = 1;

    // Map storing group merging history
    std::map<simulation::Node*, simulation::Node::SPtr > mergedGroups;
    sofa::helper::vector< simulation::Node::SPtr > contactGroup;
    sofa::helper::vector< simulation::Node::SPtr > removedGroup;
    contactGroup.reserve(contacts.size());
    for(sofa::helper::vector<Contact::SPtr>::const_iterator cit = contacts.begin(); cit != contacts.end(); ++cit)
    {
        Contact* contact = cit->get();
        simulation::Node* group1 = getIntegrationNode(contact->getCollisionModels().first);
        simulation::Node* group2 = getIntegrationNode(contact->getCollisionModels().second);
        simulation::Node::SPtr group = NULL;
        if (group1==NULL || group2==NULL)
        {
        }
        else if (group1 == group2)
        {
            // same group, no new group necessary
            group = group1;
        }
        else if (simulation::Node* parent=group1->findCommonParent(group2))
        {
            // we can merge the groups
            // if solvers are compatible...
            bool mergeSolvers = (!group1->solver.empty() || !group2->solver.empty());
            SolverSet solver;
            if (mergeSolvers)
                solver = SolverMerger::merge(group1->solver[0], group2->solver[0]);


            if (!mergeSolvers || solver.odeSolver!=NULL)
            {
                bool group1IsColl = groupSet.find(group1)!=groupSet.end();
                bool group2IsColl = groupSet.find(group2)!=groupSet.end();
                if (!group1IsColl && !group2IsColl)
                {
                    char groupName[32];
                    snprintf(groupName,sizeof(groupName),"collision%d",groupIndex++);
                    // create a new group
                    group = parent->createChild(groupName);

                    group->moveChild(BaseNode::SPtr(group1));
                    group->moveChild(BaseNode::SPtr(group2));
                    groupSet.insert(group.get());
                }
                else if (group1IsColl)
                {
                    group = group1;
                    // merge group2 in group1
                    if (!group2IsColl)
                    {
                        group->moveChild(BaseNode::SPtr(group2));
                    }
                    else
                    {
                        // merge groups and remove group2
                        SolverSet solver2;
                        if (mergeSolvers)
                        {
                            solver2.odeSolver = group2->solver[0];
                            group2->removeObject(solver2.odeSolver);
                            if (!group2->linearSolver.empty())
                            {
                                solver2.linearSolver = group2->linearSolver[0];
                                group2->removeObject(solver2.linearSolver);
                            }
                            if (!group2->constraintSolver.empty())
                            {
                                solver2.constraintSolver = group2->constraintSolver[0];
                                group2->removeObject(solver2.constraintSolver);
                            }
                        }
                        while(!group2->object.empty())
                            group->moveObject(*group2->object.begin());
                        while(!group2->child.empty())
                            group->moveChild(BaseNode::SPtr(*group2->child.begin()));
                        parent->removeChild(group2);
                        groupSet.erase(group2);
                        mergedGroups[group2] = group;
                        if (solver2.odeSolver) solver2.odeSolver.reset();
                        if (solver2.linearSolver) solver2.linearSolver.reset();
                        if (solver2.constraintSolver) solver2.constraintSolver.reset();
                        // BUGFIX(2007-06-23 Jeremie A): we can't remove group2 yet, to make sure the keys in mergedGroups are unique.
                        removedGroup.push_back(group2);
                        //delete group2;
                    }
                }
                else
                {
                    // group1 is not a collision group while group2 is
                    group = group2;
                    group->moveChild(BaseNode::SPtr(group1));
                }
                if (!group->solver.empty())
                {
                    core::behavior::OdeSolver* solver2 = group->solver[0];
                    group->removeObject(solver2);
//.........这里部分代码省略.........

void ParallelCollisionPipeline::doCollisionResponse()
{
    core::objectmodel::BaseContext* scene = getContext();
    simulation::Node* node = dynamic_cast<simulation::Node*>(scene);
    if (node && !node->getLogTime()) node=NULL; // Only use node for time logging
    ctime_t t0 = 0;
    const std::string category = "collision";

    // then we start the creation of contacts
    if (contactManager==NULL) return; // can't go further
    VERBOSE(sout << "Create Contacts "<<contactManager->getName()<<sendl);
    if (node) t0 = node->startTime();
    contactManager->createContacts(narrowPhaseDetection->getDetectionOutputs());
    if (node) t0 = node->endTime(t0, category, contactManager, this);

    // finally we start the creation of collisionGroup

    const sofa::helper::vector<Contact*>& contacts = contactManager->getContacts();

    // First we remove all contacts with non-simulated objects and directly add them
    sofa::helper::vector<Contact*> notStaticContacts;
    long int c=0;
    for (sofa::helper::vector<Contact*>::const_iterator it = contacts.begin(); it!=contacts.end(); ++it)
    {
        c+=(long int)*it;
    }
    if(c!=contactSum)
    {
        std::cout << "RESET" << std::endl;
        doRealCollisionReset();
        contactSum=c;
    }
    else
    {
        //std::cerr<<"EQUAL!"<<c<<std::endl;
    }
    for (sofa::helper::vector<Contact*>::const_iterator it = contacts.begin(); it!=contacts.end(); ++it)
    {
        Contact* c = *it;
        if (!c->getCollisionModels().first->isSimulated())
        {
            c->createResponse(c->getCollisionModels().second->getContext());
        }
        else if (!c->getCollisionModels().second->isSimulated())
        {
            c->createResponse(c->getCollisionModels().first->getContext());
        }
        else
            notStaticContacts.push_back(c);
    }


    if (groupManager==NULL)
    {
        VERBOSE(sout << "Linking all contacts to Scene"<<sendl);
        for (sofa::helper::vector<Contact*>::const_iterator it = notStaticContacts.begin(); it!=notStaticContacts.end(); ++it)
        {
            (*it)->createResponse(scene);
        }
    }
    else
    {
        VERBOSE(sout << "Create Groups "<<groupManager->getName()<<sendl);
        if (node) t0 = node->startTime();
        groupManager->createGroups(scene, notStaticContacts);
        if (node) t0 = node->endTime(t0, category, groupManager, this);
    }
}