osg::ref_ptr<osg::DisplaySettings> OculusView::createDisplaySettings() const
{
SURGSIM_ASSERT(m_inputComponent != nullptr) << "No InputComponent is connected to this view.";
osg::ref_ptr<SurgSim::Devices::OculusDisplaySettings> displaySettings =
new SurgSim::Devices::OculusDisplaySettings(OsgView::createDisplaySettings());
SurgSim::DataStructures::DataGroup dataGroup;
m_inputComponent->getData(&dataGroup);
SurgSim::DataStructures::DataGroup::DynamicMatrixType leftProjectionMatrix;
SurgSim::DataStructures::DataGroup::DynamicMatrixType rightProjectionMatrix;
if (dataGroup.matrices().get(SurgSim::DataStructures::Names::LEFT_PROJECTION_MATRIX, &leftProjectionMatrix) &&
dataGroup.matrices().get(SurgSim::DataStructures::Names::RIGHT_PROJECTION_MATRIX, &rightProjectionMatrix))
{
displaySettings->setLeftEyeProjectionMatrix(leftProjectionMatrix.block<4,4>(0, 0));
displaySettings->setRightEyeProjectionMatrix(rightProjectionMatrix.block<4,4>(0, 0));
}
else
{
SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getLogger("OculusView")) <<
"No projection matrices for left/right eye.";
}
return displaySettings;
}
std::shared_ptr<ConstraintImplementation> Representation::getConstraintImplementation(
SurgSim::Physics::ConstraintType type)
{
auto implementation = ConstraintImplementation::getFactory().getImplementation(typeid(*this), type);
if (implementation == nullptr)
{
SURGSIM_LOG_SEVERE(m_logger) << getClassName() << ": Does not support constraint type (" << type << ").";
}
return implementation;
}
bool KeyboardTogglesComponentBehavior::doWakeUp()
{
bool result = true;
if (nullptr == m_inputComponent)
{
SURGSIM_LOG_SEVERE(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__ <<
"KeyboardTogglesComponentBehavior " << getName() << " does not have an Input Component.";
result = false;
}
return result;
}
bool KeyboardCallbackBehavior::doWakeUp()
{
bool result = true;
if (nullptr == m_inputComponent)
{
SURGSIM_LOG_SEVERE(Framework::Logger::getDefaultLogger()) <<
"KeyboardCallbackBehavior '" << getFullName() << "' does not have an Input Component.";
result = false;
}
return result;
}
bool OsgSkeletonRepresentation::doInitialize()
{
std::string shaderFilename;
if (m_skinningShaderFileName.empty())
{
SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Skinning shader file not set.";
return false;
}
getRuntime()->getApplicationData()->tryFindFile(m_skinningShaderFileName, &shaderFilename);
if (shaderFilename.empty())
{
SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Skinning shader file (" << m_skinningShaderFileName
<< ") not found.";
return false;
}
m_skinningShader = new osg::Shader(osg::Shader::VERTEX);
if (!m_skinningShader->loadShaderSourceFromFile(shaderFilename))
{
SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Error loading shader (" << shaderFilename << ").";
return false;
}
if (m_model == nullptr)
{
SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Model is not set.";
return false;
}
m_skeleton = dynamic_cast<osg::Node*>(m_model->getOsgNode().get());
if (m_skeleton == nullptr)
{
SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Model does not have a valid osgNode.";
return false;
}
if (!setupBones())
{
SURGSIM_LOG_SEVERE(m_logger) << getName() << ": Could not find any bones in model.";
return false;
}
// Setup the transform updater for the skeleton.
m_updateVisitor = new osgUtil::UpdateVisitor();
m_frameCount = 0;
m_updateVisitor->setTraversalNumber(m_frameCount);
m_base->accept(*m_updateVisitor);
// Add the bone skeleton as a child to m_transform
m_transform->addChild(m_base.get());
return true;
}
bool InputDeviceHandle::updateStates(AxisStates* axisStates, ButtonStates* buttonStates, bool* updated)
{
*updated = false;
while (m_state->handle.hasDataToRead())
{
struct input_event event;
size_t numRead;
if (! m_state->handle.readBytes(&event, sizeof(event), &numRead))
{
int64_t error = getSystemErrorCode();
if (error == ENODEV)
{
SURGSIM_LOG_SEVERE(m_state->logger) <<
"InputDeviceHandle: read failed; device has been disconnected! (stopping)";
return false; // stop updating this device!
}
else
{
SURGSIM_LOG_WARNING(m_state->logger) << "InputDeviceHandle: read failed with error " <<
error << ", " << getSystemErrorText(error);
}
}
else if (numRead != sizeof(event))
{
SURGSIM_LOG_WARNING(m_state->logger) << "InputDeviceHandle: reading produced " << numRead <<
" bytes (expected " << sizeof(event) << ")";
}
else
{
if (event.type == EV_REL)
{
if (event.code >= REL_X && event.code < (REL_X+3)) // Assume that X, Y, Z are consecutive
{
(*axisStates)[0 + (event.code - REL_X)] = event.value;
*updated = true;
}
else if (event.code >= REL_RX && event.code < (REL_RX+3)) // Assume that RX, RY, RZ are consecutive
{
(*axisStates)[3 + (event.code - REL_RX)] = event.value;
*updated = true;
}
}
else if (event.type == EV_ABS)
{
if (event.code >= ABS_X && event.code < (ABS_X+3)) // Assume that X, Y, Z are consecutive
{
(*axisStates)[0 + (event.code - ABS_X)] = event.value;
*updated = true;
}
else if (event.code >= ABS_RX && event.code < (ABS_RX+3)) // Assume that RX, RY, RZ are consecutive
{
(*axisStates)[3 + (event.code - ABS_RX)] = event.value;
*updated = true;
}
}
else if (event.type == EV_KEY)
{
for (size_t i = 0; i < m_state->buttonCodes.size(); ++i)
{
if (event.code == m_state->buttonCodes[i])
{
(*buttonStates)[i] = (event.value != 0);
*updated = true;
break;
}
}
}
}
}
return true;
}
std::shared_ptr<PhysicsManagerState> CcdCollisionLoop::doUpdate(const double& dt,
const std::shared_ptr<PhysicsManagerState>& state)
{
auto ccdState = state;
auto& collisionPairs = state->getCollisionPairs();
std::vector<std::shared_ptr<Collision::CollisionPair>> ccdPairs;
ccdPairs.reserve(collisionPairs.size());
std::copy_if(collisionPairs.cbegin(), collisionPairs.cend(), std::back_inserter(ccdPairs),
[](const std::shared_ptr<Collision::CollisionPair>& p)
{
return p->getType() == Collision::COLLISION_DETECTION_TYPE_CONTINUOUS;
});
double timeOfImpact = 0.0;
double localTimeOfImpact = 0.0;
std::vector<std::list<std::shared_ptr<Collision::Contact>>> oldContacts;
bool executedOnce = false;
size_t iterations = 0;
for (; iterations < m_maxIterations; ++iterations)
{
double epsilon = 1.0 / ((1 - timeOfImpact) * m_epsilonFactor);
ccdState = m_updateCcdData->update(localTimeOfImpact, ccdState);
ccdState = m_ccdCollision->update(dt, ccdState);
ccdState = m_contactFilter->update(dt, ccdState);
if (m_logger->getThreshold() <= SurgSim::Framework::LOG_LEVEL_DEBUG)
{
printContacts(ccdPairs);
}
// Find the first impact and filter all contacts beyond a given epsilon
if (!findEarliestContact(ccdPairs, &localTimeOfImpact))
{
break;
}
filterLaterContacts(&ccdPairs, epsilon, localTimeOfImpact);
restoreContacts(&ccdPairs, &oldContacts);
ccdState = m_constraintGeneration->update(dt, ccdState);
ccdState = m_buildMlcp->update(dt, ccdState);
ccdState = m_solveMlcp->update(dt, ccdState);
ccdState = m_pushResults->update(dt, ccdState);
executedOnce = true;
backupContacts(&ccdPairs, &oldContacts);
timeOfImpact += (1.0 - timeOfImpact) * localTimeOfImpact;
if (timeOfImpact > 1.0)
{
SURGSIM_LOG_SEVERE(m_logger) << "Calculated time of impact is greater " <<
"than the parametric upper bound of 1.0 (" <<
timeOfImpact << ")" << std::endl;
break;
}
// Lambda == 0 means we are no longer generating corrections. Exit the loop.
// We will take up the collision detection at the start of the next time step.
if (ccdState->getMlcpSolution().x.isZero())
{
break;
}
}
SURGSIM_LOG_IF(iterations == m_maxIterations, m_logger, WARNING) <<
"Maxed out iterations (" << m_maxIterations << ")";
restoreContacts(&ccdPairs, &oldContacts);
if (!executedOnce)
{
ccdState = m_constraintGeneration->update(dt, ccdState);
ccdState = m_buildMlcp->update(dt, ccdState);
ccdState = m_solveMlcp->update(dt, ccdState);
ccdState = m_pushResults->update(dt, ccdState);
}
return ccdState;
}
