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