imstkSceneManager.cpp 5.84 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/*=========================================================================

   Library: iMSTK

   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
   & Imaging in Medicine, Rensselaer Polytechnic Institute.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0.txt

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

20
=========================================================================*/
21

22
// imstk
23
24
#include "imstkSceneManager.h"
#include "imstkCameraController.h"
25
#include "imstkSceneObjectController.h"
26
#include "imstkDynamicObject.h"
tuanthienbk's avatar
tuanthienbk committed
27
#include "imstkPbdObject.h"
28
#include "imstkDeformableObject.h"
29
#include "imstkVirtualCouplingPBDObject.h"
30
#include "imstkGeometryMap.h"
31
#include "imstkTimer.h"
32
#include "imstkPbdSolver.h"
33
34
35

#include "g3log/g3log.hpp"

36
37
namespace imstk
{
38
39
40
41
42
43
44
45
46
std::shared_ptr<Scene>
SceneManager::getScene()
{
    return m_scene;
}

void
SceneManager::initModule()
{
47
    // Start Camera Controller (asynchronous)
48
49
50
51
    if (auto camController = m_scene->getCamera()->getController())
    {
        this->startModuleInNewThread(camController);
    }
52
53
54
55

    // Init virtual coupling objects offsets
    for (auto obj : m_scene->getSceneObjects())
    {
Alexis Girault's avatar
Alexis Girault committed
56
57
58
59
        if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
        {
            virtualCouplingPBD->initOffsets();
        }
60
    }
61
62
}

63
64
65
void
SceneManager::runModule()
{
66
67
68
69
    StopWatch wwt;

    wwt.start();

70
    // Reset Contact forces to 0
Alexis Girault's avatar
Alexis Girault committed
71
72
    for (auto obj : m_scene->getSceneObjects())
    {
73
        if (auto defObj = std::dynamic_pointer_cast<DeformableObject>(obj))
Alexis Girault's avatar
Alexis Girault committed
74
        {
75
            defObj->getContactForce().setConstant(0.0);
Alexis Girault's avatar
Alexis Girault committed
76
        }
77
78
79
80
81
82
        else if (auto collidingObj = std::dynamic_pointer_cast<CollidingObject>(obj))
        {
            collidingObj->resetForce();
        }
        // todo: refactor pbd
        // description: so that the transform obtained from device can be applied
83
        if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
Alexis Girault's avatar
Alexis Girault committed
84
85
86
87
        {
            virtualCouplingPBD->resetCollidingGeometry();
        }
    }
88

89
90
91
    // Update objects controlled by the device controllers
    for (auto controller : m_scene->getSceneObjectControllers())
    {
92
        controller->updateControlledObjects();
93
94
    }

95
96
97
98
    // Compute collision data per interaction pair
    for (auto intPair : m_scene->getCollisionGraph()->getInteractionPairList())
    {
        intPair->computeCollisionData();
99
        intPair->computeContactForces();
100
101
    }

102
103
104
105
106
107
    // Apply forces on device
    for (auto controller : m_scene->getSceneObjectControllers())
    {
        controller->applyForces();
    }

108
109
    // Update the solvers
    for (auto solvers : m_scene->getSolvers())
110
    {
111
        solvers->solve();
112
113

        auto xx = std::dynamic_pointer_cast<PbdSolver>(solvers);
114
115
    }

116
    // Apply the geometry and apply maps to all the objects
117
118
    for (auto obj : m_scene->getSceneObjects())
    {
119
        obj->updateGeometries();
tuanthienbk's avatar
tuanthienbk committed
120
121
    }

122
    // Do collision detection and response for pbd objects
tuanthienbk's avatar
tuanthienbk committed
123
124
125
    for (auto intPair : m_scene->getCollisionGraph()->getPbdPairList())
    {
        intPair->resetConstraints();
126
        if (intPair->doBroadPhaseCollision())
tuanthienbk's avatar
tuanthienbk committed
127
        {
128
            intPair->doNarrowPhaseCollision();
tuanthienbk's avatar
tuanthienbk committed
129
        }
130
        intPair->resolveCollision();
tuanthienbk's avatar
tuanthienbk committed
131
    }
132

Hong Li's avatar
Hong Li committed
133
134
135
136
137
138
139
140
141
    // Update velocity of PBD objects
    for (auto obj : m_scene->getSceneObjects())
    {
        if (auto pbdObj = std::dynamic_pointer_cast<PbdObject>(obj))
        {
            pbdObj->updateVelocity();
        }
    }

142
143
144
145
146
147
148
149
150
151
152
153
154
155
    // Set the trackers of virtual coupling PBD objects to out-of-date
    for (auto obj : m_scene->getSceneObjects())
    {
        if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
        {
            virtualCouplingPBD->setTrackerToOutOfDate();
        }
    }

    // Set the trackers of the scene object controllers to out-of-date
    for (auto controller : m_scene->getSceneObjectControllers())
    {
        controller->setTrackerToOutOfDate();
    }
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182

    auto timeElapsed = wwt.getTimeElapsed(StopWatch::TimeUnitType::seconds);

    // Update time step size of the dynamic objects
    for (auto obj : m_scene->getSceneObjects())
    {
        if (obj->getType() == SceneObject::Type::Pbd)
        {
            if (auto dynaObj = std::dynamic_pointer_cast<PbdObject>(obj))
            {
                if (dynaObj->getDynamicalModel()->getTimeStepSizeType() == TimeSteppingType::realTime)
                {
                    dynaObj->getDynamicalModel()->setTimeStep(timeElapsed);
                }
            }
        }
        else if (obj->getType() == SceneObject::Type::FEMDeformable)
        {
            if (auto dynaObj = std::dynamic_pointer_cast<DeformableObject>(obj))
            {
                if (dynaObj->getDynamicalModel()->getTimeStepSizeType() == TimeSteppingType::realTime)
                {
                    dynaObj->getDynamicalModel()->setTimeStep(timeElapsed);
                }
            }
        }
    }
183
184

    m_scene->setFPS((size_t)(1./wwt.getTimeElapsed(StopWatch::TimeUnitType::seconds)));
185
186
}

187
188
189
void
SceneManager::cleanUpModule()
{
190
    // End Camera Controller
191
192
193
194
195
196
197
198
199
200
201
    if (auto camController = m_scene->getCamera()->getController())
    {
        camController->end();
        m_threadMap.at(camController->getName()).join();
    }
}
void
SceneManager::startModuleInNewThread(std::shared_ptr<Module> module)
{
    m_threadMap[module->getName()] = std::thread([module] { module->start(); });
}
202
} // imstk