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

//.........这里部分代码省略.........
                            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);
                }
                if (!group->linearSolver.empty())
                {
                    core::behavior::BaseLinearSolver* solver2 = group->linearSolver[0];
                    group->removeObject(solver2);
                }
                if (!group->constraintSolver.empty())
                {
                    core::behavior::ConstraintSolver* solver2 = group->constraintSolver[0];
                    group->removeObject(solver2);
                }
                if (solver.odeSolver)
                {
                    group->addObject(solver.odeSolver);
                }
                if (solver.linearSolver)
                {
                    group->addObject(solver.linearSolver);
                }
                if (solver.constraintSolver)
                {
                    group->addObject(solver.constraintSolver);
                }
                // perform init only once everyone has been added (in case of explicit dependencies)
                if (solver.odeSolver)
                {
                    solver.odeSolver->init();
                }
                if (solver.linearSolver)
                {
                    solver.linearSolver->init();
                }
                if (solver.constraintSolver)
                {
                    solver.constraintSolver->init();
                }
            }
        }
        contactGroup.push_back(group);
    }

    // now that the groups are final, attach contacts' response
    for(unsigned int i=0; i<contacts.size(); i++)
    {
        Contact* contact = contacts[i].get();
        simulation::Node::SPtr group = contactGroup[i];
        while (group!=NULL && mergedGroups.find(group.get())!=mergedGroups.end())
            group = mergedGroups[group.get()];
        if (group!=NULL)
            contact->createResponse(group.get());
        else
            contact->createResponse(scene);
    }

    // delete removed groups
    for (sofa::helper::vector<simulation::Node::SPtr>::iterator it = removedGroup.begin(); it!=removedGroup.end(); ++it)
    {
        simulation::Node::SPtr node = *it;
        node->detachFromGraph();
        node->execute<simulation::DeleteVisitor>(sofa::core::ExecParams::defaultInstance());
        it->reset();
    }
    removedGroup.clear();

    // finally recreate group vector
    groups.clear();
    for (std::set<simulation::Node::SPtr>::iterator it = groupSet.begin(); it!=groupSet.end(); ++it)
        groups.push_back(*it);
}

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);
    }
}