1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112
|
// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// 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
//
// 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.
#include <vector>
#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
#include "SurgSim/Physics/PrepareCollisionPairs.h"
#include "SurgSim/Math/Aabb.h"
namespace SurgSim
{
namespace Physics
{
PrepareCollisionPairs::PrepareCollisionPairs(bool doCopyState) :
Computation(doCopyState),
m_timeSinceLog(0.0),
m_logger(Framework::Logger::getLogger("Physics/PrepareCollisionPairs"))
{
}
PrepareCollisionPairs::~PrepareCollisionPairs()
{
}
std::shared_ptr<PhysicsManagerState> PrepareCollisionPairs::doUpdate(
const double& dt,
const std::shared_ptr<PhysicsManagerState>& state)
{
std::shared_ptr<PhysicsManagerState> result = state;
auto& representations = result->getActiveCollisionRepresentations();
std::vector<std::shared_ptr<Collision::CollisionPair>> pairs;
auto end = std::end(representations);
for (auto first = std::begin(representations); first != end; ++first)
{
for (auto second = first; second != end; ++second)
{
if (!(*first)->isIgnoring(*second) && !(*second)->isIgnoring(*first))
{
auto pair = std::make_shared<Collision::CollisionPair>(*first, *second);
if (pair->getType() != Collision::COLLISION_DETECTION_TYPE_NONE && pair->mayIntersect())
{
pairs.push_back(pair);
}
}
}
}
result->setCollisionPairs(pairs);
if (m_logger->getThreshold() <= SURGSIM_LOG_LEVEL(DEBUG))
{
m_timeSinceLog += dt;
if (m_timeSinceLog > 5.0)
{
m_timeSinceLog = 0.0;
typedef std::tuple<std::string, std::string, Collision::CollisionDetectionType> PairType;
std::vector<PairType> names;
for (const auto& pair : pairs)
{
std::string first = pair->getFirst()->getFullName();
std::string second = pair->getSecond()->getFullName();
if (first < second)
{
names.emplace_back(first, second, pair->getType());
}
else
{
names.emplace_back(second, first, pair->getType());
}
}
std::sort(names.begin(), names.end(), [](const PairType & i, const PairType & j)
{
return (std::get<0>(i) < std::get<0>(j)) ||
((std::get<0>(i) == std::get<0>(j)) && (std::get<1>(i) < std::get<1>(j)));
});
std::string message = "All collision pairs for testing:\n";
for (const auto& name : names)
{
message += "\t" + std::get<0>(name) + " : " + std::get<1>(name) +
(
(std::get<2>(name) == SurgSim::Collision::COLLISION_DETECTION_TYPE_CONTINUOUS) ?
" CCD" : " DCD"
) + "\n";
}
SURGSIM_LOG_DEBUG(m_logger) << message;
}
}
return result;
}
}; // Physics
}; // SurgSim
|