bool TransferParticlesToPointCloudBehavior::doInitialize()
{
SURGSIM_ASSERT(m_source != nullptr) << "SetSource must be called prior to initialization";
SURGSIM_ASSERT(m_target != nullptr) << "SetTarget must be called prior to initialization";
return true;
}
void LinearSpring::initialize(const OdeState& state)
{
Spring::initialize(state);
SURGSIM_ASSERT(m_restLength >= 0.0) << "Spring rest length was not set, please call setRestLength()";
SURGSIM_ASSERT(m_stiffness >= 0.0) << "Spring stiffness was not set, please call setStiffness()";
}
CollisionPair::CollisionPair(const std::shared_ptr<Representation>& first,
const std::shared_ptr<Representation>& second) :
m_representations(first, second), m_isSwapped(false)
{
SURGSIM_ASSERT(first != second) << "Collision Representation cannot collide with itself";
SURGSIM_ASSERT(first != nullptr && second != nullptr) << "Collision Representation cannot be null";
}
void NovintCommonDevice::setInitializationName(const std::string& initializationName)
{
SURGSIM_ASSERT(!m_serialNumber.hasValue()) << "Cannot set initializationName for a NovintCommonDevice named " <<
getName() << ", which already has a serialNumber.";
SURGSIM_ASSERT(!isInitialized()) <<
"Cannot setInitializationName after the device named " << getName() << " has been initialized.";
m_initializationName.setValue(initializationName);
}
void OsgSkeletonRepresentation::setModel(std::shared_ptr<SurgSim::Framework::Asset> model)
{
SURGSIM_ASSERT(!isInitialized()) << "Cannot set model after OsgSkeletonRepresentation had been initialized.";
auto osgModel = std::dynamic_pointer_cast<OsgModel>(model);
SURGSIM_ASSERT(model == nullptr || osgModel != nullptr) << "OsgSkeletonRepresentation expects an OsgModel.";
m_model = osgModel;
}
bool convert<std::shared_ptr<SurgSim::Framework::Component>>::decode(
const Node& node,
std::shared_ptr<SurgSim::Framework::Component>& rhs) //NOLINT
{
bool result = false;
if (!node.IsMap())
{
return false;
}
SurgSim::Framework::Component::FactoryType& factory = SurgSim::Framework::Component::getFactory();
std::string className = node.begin()->first.as<std::string>();
SURGSIM_ASSERT(factory.isRegistered(className)) << "Class " << className << " is not registered in the factory.";
Node data = node.begin()->second;
if (data.IsMap() && data[NamePropertyName].IsDefined())
{
std::string name = data[NamePropertyName].as<std::string>();
if (rhs == nullptr)
{
if (data[IdPropertyName].IsDefined())
{
std::string id = data[IdPropertyName].as<std::string>();
RegistryType& registry = getRegistry();
auto sharedComponent = registry.find(id);
if (sharedComponent != registry.end())
{
SURGSIM_ASSERT(name == sharedComponent->second->getName() &&
className == sharedComponent->second->getClassName())
<< "The current node: " << std::endl << node << "has the same id as an instance "
<< "already registered, but the name and/or the className are different. This is "
<< "likely a problem with a manually assigned id.";
rhs = sharedComponent->second;
}
else
{
rhs = factory.create(className, name);
getRegistry()[id] = rhs;
}
}
else
{
rhs = factory.create(className, name);
}
}
std::vector<std::string> ignoredProperties;
ignoredProperties.push_back(NamePropertyName);
ignoredProperties.push_back(IdPropertyName);
rhs->decode(data, ignoredProperties);
result = true;
}
return result;
}
void OctreeShape::setOctree(std::shared_ptr<SurgSim::Framework::Asset> node)
{
SURGSIM_ASSERT(node != nullptr) << "Tried to set the shape with a nullptr";
auto octreeNode = std::dynamic_pointer_cast<NodeType>(node);
SURGSIM_ASSERT(octreeNode != nullptr)
<< "OctreeShape needs OctreeNode<SurgSim::DataStructures::EmptyData> but received " << node->getClassName();
SURGSIM_ASSERT(isValid(octreeNode))
<< "OctreeShape was passed an invalid Octree.";
m_rootNode = octreeNode;
}
void Asset::load(const std::string& fileName, const SurgSim::Framework::ApplicationData& data)
{
m_fileName = fileName;
SURGSIM_ASSERT(!m_fileName.empty()) << "File name is empty";
std::string path = data.findFile(m_fileName);
SURGSIM_ASSERT(!path.empty()) << "Can not locate file " << m_fileName;
SURGSIM_ASSERT(doLoad(path)) << "Failed to load file " << m_fileName;
}
bool PlyReader::requestProperty(const std::string& elementName,
const std::string& propertyName,
int dataType, int dataOffset,
int countType, int countOffset)
{
SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
SURGSIM_ASSERT(hasElement(elementName)) <<
"The element <" << elementName << "> has not been requested yet, you cannot access properties for it";
SURGSIM_ASSERT(hasProperty(elementName, propertyName)) <<
"The requested property <" << propertyName << "> cannot be found in element <" << elementName << ">.";
bool result = false;
bool scalar = isScalar(elementName, propertyName);
bool wantScalar = (countType == 0);
if (wantScalar && !scalar)
{
SURGSIM_FAILURE() << "Trying to access a list property as a scalar." <<
"for element <" << elementName << "> and property <" << propertyName << ">.";
}
else if (!wantScalar && scalar)
{
SURGSIM_FAILURE() << "Trying to access a scalar property as a list." <<
"for element <" << elementName << "> and property <" << propertyName << ">.";
}
if (hasProperty(elementName, propertyName) && (scalar == wantScalar))
{
auto itBegin = std::begin(m_requestedElements[elementName].requestedProperties);
auto itEnd = std::end(m_requestedElements[elementName].requestedProperties);
bool doAdd = std::find_if(itBegin, itEnd,
[propertyName](PropertyInfo p){return p.propertyName == propertyName;}) == itEnd;
if (doAdd)
{
PropertyInfo info;
info.propertyName = propertyName;
info.dataType = m_data->types[dataType];
info.dataOffset = dataOffset;
info.countType = m_data->types[countType];
info.countOffset = countOffset;
m_requestedElements[elementName].requestedProperties.push_back(info);
result = true;
}
}
return result;
}
void CollisionPair::setRepresentations(const std::shared_ptr<Representation>& first,
const std::shared_ptr<Representation>& second)
{
SURGSIM_ASSERT(first != second) << "Should try to collide with self";
SURGSIM_ASSERT(first != nullptr && second != nullptr) << "Collision Representation cannot be null";
// Invalidate the current contacts
clearContacts();
m_representations.first = first;
m_representations.second = second;
m_isSwapped = false;
}
bool KeyboardDevice::initialize()
{
SURGSIM_ASSERT(!isInitialized());
m_scaffold = KeyboardScaffold::getOrCreateSharedInstance();
SURGSIM_ASSERT(m_scaffold);
m_scaffold->registerDevice(this);
SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Initialized.";
return true;
}
bool NovintCommonDevice::initialize()
{
SURGSIM_ASSERT(!isInitialized());
std::shared_ptr<NovintScaffold> scaffold = NovintScaffold::getOrCreateSharedInstance();
SURGSIM_ASSERT(scaffold);
if (! scaffold->registerDevice(this))
{
return false;
}
m_scaffold = std::move(scaffold);
return true;
}
bool TrackIRDevice::initialize()
{
SURGSIM_ASSERT(!isInitialized()) << "TrackIR device already initialized.";
auto scaffold = TrackIRScaffold::getOrCreateSharedInstance();
SURGSIM_ASSERT(scaffold != nullptr) << "TrackIRDevice::initialize(): Failed to obtain a TrackIR scaffold.";
bool initialize = false;
if (scaffold->registerDevice(this))
{
m_scaffold = std::move(scaffold);
initialize = true;
}
return initialize;
}
void Constraint::setInformation(ConstraintType constraintType,
std::shared_ptr<ConstraintData> data,
std::shared_ptr<Representation> representation0,
const SurgSim::DataStructures::Location& location0,
std::shared_ptr<Representation> representation1,
const SurgSim::DataStructures::Location& location1)
{
m_constraintType = constraintType;
SURGSIM_ASSERT(data != nullptr) << "ConstraintData can't be nullptr";
SURGSIM_ASSERT(representation0 != nullptr) << "First representation can't be nullptr";
SURGSIM_ASSERT(representation1 != nullptr) << "Second representation can't be nullptr";
auto localization0 = representation0->createLocalization(location0);
SURGSIM_ASSERT(localization0 != nullptr) << "Could not create localization for " << representation0->getName();
auto localization1 = representation1->createLocalization(location1);
SURGSIM_ASSERT(localization1 != nullptr) << "Could not create localization for " << representation1->getName();
auto implementation0 = representation0->getConstraintImplementation(m_constraintType);
SURGSIM_ASSERT(implementation0 != nullptr) << "Could not get implementation for " << representation0->getName();
auto implementation1 = representation1->getConstraintImplementation(m_constraintType);
SURGSIM_ASSERT(implementation1 != nullptr) << "Could not get implementation for " << representation1->getName();
SURGSIM_ASSERT(implementation0->getNumDof() == implementation1->getNumDof())
<< "The number of DOFs does not match between the two implementations";
m_data = data;
m_implementations = std::make_pair(implementation0, implementation1);
m_localizations = std::make_pair(localization0, localization1);
m_numDof = implementation0->getNumDof();
}
bool Representation::doWakeUp()
{
if (getMaterial() == nullptr && !m_materialReference.empty())
{
std::vector<std::string> names;
boost::split(names, m_materialReference, boost::is_any_of("/"));
SURGSIM_ASSERT(names.size() == 2)
<< "Material reference needs to have 2 parts <scenelement>/<component>, '" << m_materialReference
<< "' in " << getFullName() << " doesn't.";
auto material = getScene()->getComponent(names[0], names[1]);
if (material != nullptr)
{
setMaterial(material);
}
else
{
SURGSIM_LOG_WARNING(Framework::Logger::getLogger("Graphics/Representation"))
<< "Can't find material " << m_materialReference << " in Scene, rendering of " << getFullName()
<< " is going to be compromised.";
}
}
return true;
}
bool CcdCollisionLoop::findEarliestContact(
const std::vector<std::shared_ptr<Collision::CollisionPair>>& ccdPairs,
double* currentTimeOfImpact)
{
SURGSIM_ASSERT(currentTimeOfImpact != nullptr) << "Please provide a valid currentTimeOfImpact variable";
double timeOfImpact = std::numeric_limits<double>::max();
// Find earliest time of impact
for (auto& pair : ccdPairs)
{
std::for_each(pair->getContacts().begin(),
pair->getContacts().end(),
[&timeOfImpact](const std::shared_ptr<Collision::Contact>& contact)
{
timeOfImpact = std::min<double>(timeOfImpact, contact->time);
});
}
// Did not find any contacts return false
if (!(timeOfImpact < std::numeric_limits<double>::max()))
{
return false;
}
*currentTimeOfImpact = timeOfImpact;
return true;
}
bool KeyboardDevice::finalize()
{
SURGSIM_ASSERT(isInitialized());
SURGSIM_LOG_INFO(m_scaffold->getLogger()) << "Device " << getName() << ": " << "Finalizing.";
m_scaffold.reset();
return true;
}
bool TrackIRDevice::finalize()
{
SURGSIM_ASSERT(isInitialized()) << "TrackIR device already finalized.";
bool ok = m_scaffold->unregisterDevice(this);
m_scaffold.reset();
return ok;
}
void KeyboardTogglesComponentBehavior::setInputComponent(std::shared_ptr<SurgSim::Framework::Component> inputComponent)
{
SURGSIM_ASSERT(nullptr != inputComponent) << "'inputComponent' cannot be 'nullptr'";
m_inputComponent = checkAndConvert<SurgSim::Input::InputComponent>(
inputComponent, "SurgSim::Input::InputComponent");
}
bool NovintCommonDevice::finalize()
{
SURGSIM_ASSERT(isInitialized());
bool result = m_scaffold->unregisterDevice(this);
m_scaffold.reset();
return result;
}
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;
}
void CcdCollisionLoop::restoreContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
std::vector<std::list<std::shared_ptr<Collision::Contact>>>* oldContacts)
{
SURGSIM_ASSERT(oldContacts != nullptr) << "Invalid container found.";
if (oldContacts->size() == 0)
{
return;
}
SURGSIM_ASSERT(oldContacts->size() == ccdPairs->size()) << "Contact size exception detected";
for (size_t i = 0; i < oldContacts->size(); ++i)
{
auto& newContacts = ccdPairs->at(i)->getContacts();
newContacts.splice(newContacts.end(), std::move(oldContacts->at(i)));
}
oldContacts->clear();
}
void OsgSkeletonRepresentation::loadModel(const std::string& fileName)
{
SURGSIM_ASSERT(!isInitialized()) << "Cannot load model after OsgSkeletonRepresentation had been initialized.";
auto model = std::make_shared<OsgModel>();
model->load(fileName);
setModel(model);
}
void DefaultContactCalculation::doCalculateContact(std::shared_ptr<CollisionPair> pair)
{
SURGSIM_ASSERT(!m_doAssert)
<< "Contact calculation not implemented for pairs with types ("
<< pair->getFirst()->getShapeType() << ", " << pair->getSecond()->getShapeType() << ").";
SURGSIM_LOG_ONCE(SurgSim::Framework::Logger::getDefaultLogger(), WARNING)
<< "Contact calculation not implemented for pairs with types ("
<< pair->getFirst()->getShapeType() << ", " << pair->getSecond()->getShapeType() << ").";
}
bool Fem3DPlyReaderDelegate::fileIsAcceptable(const PlyReader& reader)
{
bool result = FemPlyReaderDelegate::fileIsAcceptable(reader);
SURGSIM_ASSERT(!m_hasRotationDOF)
<< "Fem3DPlyReaderDelegate does not support rotational DOF, data will be ignored.";
return result;
}
void CcdCollisionLoop::backupContacts(std::vector<std::shared_ptr<Collision::CollisionPair>>* ccdPairs,
std::vector<std::list<std::shared_ptr<Collision::Contact>>>* oldContacts)
{
SURGSIM_ASSERT(oldContacts != nullptr) << "Invalid container found.";
for (auto& pair : (*ccdPairs))
{
oldContacts->push_back(std::move(pair->getContacts()));
pair->getContacts().clear();
}
}
void OctreeShape::loadOctree(const std::string& filePath)
{
auto rootNode = std::make_shared<NodeType>();
rootNode->load(filePath);
SURGSIM_ASSERT(isValid(rootNode))
<< "Loading failed " << filePath << " contains an invalid octree.";
setOctree(rootNode);
}
void* OctreeNodePlyReaderDelegateBase::beginVoxel(const std::string& elementName, size_t count)
{
SURGSIM_ASSERT(m_haveSpacing) << "Need spacing data for complete octree.";
SURGSIM_ASSERT(m_haveBounds) << "Need bounds data for complete octree.";
SURGSIM_ASSERT(m_haveDimensions) << "Need dimension data for complete octree.";
auto maxDimension = std::max(m_dimension.x, std::max(m_dimension.y, m_dimension.z));
m_numLevels = 1 + static_cast<int>(std::ceil(std::log(maxDimension) / std::log(2.0)));
SurgSim::Math::Vector3d octreeDimensions = SurgSim::Math::Vector3d::Ones() * std::pow(2.0, m_numLevels - 1);
Math::Vector3d spacing(m_spacing.x, m_spacing.y, m_spacing.z);
m_boundingBox.min() = Math::Vector3d(m_bounds.xMin, m_bounds.yMin, m_bounds.zMin);
m_boundingBox.max() = Math::Vector3d(m_bounds.xMin, m_bounds.yMin, m_bounds.zMin).array() +
octreeDimensions.array() * spacing.array();
initializeOctree();
return &m_voxel;
}
bool PlyReader::hasProperty(const std::string& elementName, const std::string& propertyName) const
{
SURGSIM_ASSERT(isValid()) << "'" << m_filename << "' is an invalid .ply file";
bool result = false;
PlyElement* element = find_element(m_data->plyFile, elementName.c_str());
if (element != nullptr)
{
int index;
result = find_property(element, propertyName.c_str(), &index) != nullptr;
}
return result;
}
void MassSpring1DRepresentation::init1D(
const std::vector<Vector3d> nodes,
std::vector<size_t> nodeBoundaryConditions,
double totalMass,
double stiffnessStretching, double dampingStretching,
double stiffnessBending, double dampingBending)
{
std::shared_ptr<SurgSim::Math::OdeState> state;
state = std::make_shared<SurgSim::Math::OdeState>();
state->setNumDof(getNumDofPerNode(), nodes.size());
SURGSIM_ASSERT(nodes.size() > 0) << "Number of nodes incorrect: " << nodes.size();
// Initialize the nodes position, velocity and mass
// Note: no need to apply the initialPose here, initialize will take care of it !
for (size_t massId = 0; massId < nodes.size(); massId++)
{
addMass(std::make_shared<Mass>(totalMass / static_cast<double>(nodes.size())));
SurgSim::Math::setSubVector(nodes[massId], massId, 3, &state->getPositions());
}
// Initialize the stretching springs
if (stiffnessStretching || dampingStretching)
{
for (size_t massId = 0; massId < nodes.size() - 1; massId++)
{
addSpring(createLinearSpring(state, massId, massId + 1, stiffnessStretching, dampingStretching));
}
}
// Initialize the bending springs
if (stiffnessBending || dampingBending)
{
for (size_t massId = 0; massId < nodes.size() - 2; massId++)
{
addSpring(createLinearSpring(state, massId, massId + 2, stiffnessBending, dampingBending));
}
}
// Sets the boundary conditions
for (auto boundaryCondition = std::begin(nodeBoundaryConditions);
boundaryCondition != std::end(nodeBoundaryConditions);
boundaryCondition++)
{
state->addBoundaryCondition(*boundaryCondition);
}
// setInitialState: Initialize all the states + apply initialPose if any
setInitialState(state);
}
