<commit_before>// This file is a part of the OpenSurgSim project.
// Copyright 2013, 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 "Examples/ExampleStapling/StaplerBehavior.h"

#include <boost/exception/to_string.hpp>

#include "Examples/ExampleStapling/StapleElement.h"
#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/DataStructures/DataGroup.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Graphics/SceneryRepresentation.h"
#include "SurgSim/Input/InputComponent.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintComponent.h"
#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
#include "SurgSim/Physics/FixedRepresentationBilateral3D.h"
#include "SurgSim/Physics/Fem3DRepresentationBilateral3D.h"
#include "SurgSim/Physics/Localization.h"
#include "SurgSim/Physics/RigidCollisionRepresentation.h"
#include "SurgSim/Physics/RigidRepresentationBilateral3D.h"

using SurgSim::Physics::ConstraintImplementation;
using SurgSim::Physics::FixedRepresentationBilateral3D;
using SurgSim::Physics::RigidRepresentationBilateral3D;
using SurgSim::Physics::Fem3DRepresentationBilateral3D;
using SurgSim::Physics::Localization;

StaplerBehavior::StaplerBehavior(const std::string& name):
	SurgSim::Framework::Behavior(name),
	m_numElements(0),
	m_button1Index(-1),
	m_button1IndexCached(false),
	m_buttonPreviouslyPressed(false)
{
}

void StaplerBehavior::setInputComponent(std::shared_ptr<SurgSim::Input::InputComponent> inputComponent)
{
	m_from = inputComponent;
}

void StaplerBehavior::setRepresentation(
	 std::shared_ptr<SurgSim::Framework::Representation> staplerRepresentation)
{
	m_representation = staplerRepresentation;
}

void StaplerBehavior::setVirtualStaple(
	const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& virtualTeeth)
{
	m_virtualTeeth = virtualTeeth;
}

void StaplerBehavior::enableStaplingForSceneElement(std::string sceneElementName)
{
	m_stapleEnabledSceneElements.push_back(sceneElementName);
}

void StaplerBehavior::filterCollisionMapForStapleEnabledRepresentations(
	SurgSim::Collision::Representation::ContactMapType* collisionsMap)
{
	for (auto it = collisionsMap->begin(); it != collisionsMap->end();)
	{
		if (std::find(m_stapleEnabledSceneElements.begin(),
					  m_stapleEnabledSceneElements.end(),
					  (*it).first->getSceneElement()->getName()) == m_stapleEnabledSceneElements.end())
		{
			// Representation's scene element is not in the m_stapleEnabledSceneElements.
			it = collisionsMap->erase(it);
		}
		else
		{
			++it;
		}
	}
}

std::shared_ptr<SurgSim::Physics::Representation> StaplerBehavior::findCorrespondingPhysicsRepresentation(
	std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation)
{
	std::shared_ptr<SurgSim::Physics::Representation> physicsRepresentation = nullptr;

	// Check if the collisionRepresenation is for a Rigid body.
	std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> rigidCollisionRepresentation =
		std::dynamic_pointer_cast<SurgSim::Physics::RigidCollisionRepresentation>(collisionRepresentation);

	if (rigidCollisionRepresentation != nullptr)
	{
		physicsRepresentation = rigidCollisionRepresentation->getRigidRepresentation();
	}
	else
	{
		// Check if the collisionRepresenation is for a deformable body.
		std::shared_ptr<SurgSim::Physics::DeformableCollisionRepresentation> deformableCollisionRepresentation =
			std::dynamic_pointer_cast<SurgSim::Physics::DeformableCollisionRepresentation>(collisionRepresentation);

		if (deformableCollisionRepresentation != nullptr)
		{
			physicsRepresentation = deformableCollisionRepresentation->getDeformableRepresentation();
		}
	}

	return physicsRepresentation;
}

void StaplerBehavior::filterCollisionMapForSupportedRepresentationTypes(
	SurgSim::Collision::Representation::ContactMapType* collisionsMap)
{
	for (auto it = collisionsMap->begin(); it != collisionsMap->end();)
	{
		if (findCorrespondingPhysicsRepresentation((*it).first) == nullptr)
		{
			// Representation type is not supported to be stapled.
			it = collisionsMap->erase(it);
		}
		else
		{
			++it;
		}
	}
}

std::shared_ptr<SurgSim::Physics::Constraint> StaplerBehavior::createBilateral3DConstraint(
	std::shared_ptr<SurgSim::Physics::Representation> stapleRep,
	std::shared_ptr<SurgSim::Physics::Representation> otherRep,
	SurgSim::Collision::Location contraintLocation)
{
	// Create a bilateral constraint between the physicsRepresentation and staple.
	// First find the points where the constraint is going to be applied.
	std::shared_ptr<Localization> stapleRepLocalization
		= stapleRep->createLocalization(contraintLocation);
	stapleRepLocalization->setRepresentation(stapleRep);

	std::shared_ptr<Localization> otherRepLocatization
		= otherRep->createLocalization(contraintLocation);
	otherRepLocatization->setRepresentation(otherRep);

	std::shared_ptr<SurgSim::Physics::Constraint> constraint = nullptr;

	// Create the Constraint with appropriate constraint implementation.
	switch (otherRep->getType())
	{
	case SurgSim::Physics::REPRESENTATION_TYPE_FIXED:
		constraint = std::make_shared<SurgSim::Physics::Constraint>(
						std::make_shared<SurgSim::Physics::ConstraintData>(),
						std::make_shared<RigidRepresentationBilateral3D>(),
						stapleRepLocalization,
						std::make_shared<FixedRepresentationBilateral3D>(),
						otherRepLocatization);
		break;

	case SurgSim::Physics::REPRESENTATION_TYPE_RIGID:
		constraint = std::make_shared<SurgSim::Physics::Constraint>(
						std::make_shared<SurgSim::Physics::ConstraintData>(),
						std::make_shared<RigidRepresentationBilateral3D>(),
						stapleRepLocalization,
						std::make_shared<RigidRepresentationBilateral3D>(),
						otherRepLocatization);
		break;

	case SurgSim::Physics::REPRESENTATION_TYPE_FEM3D:
		constraint = std::make_shared<SurgSim::Physics::Constraint>(
						std::make_shared<SurgSim::Physics::ConstraintData>(),
						std::make_shared<RigidRepresentationBilateral3D>(),
						stapleRepLocalization,
						std::make_shared<Fem3DRepresentationBilateral3D>(),
						otherRepLocatization);
		break;
	}

	return constraint;
}

void StaplerBehavior::createStaple()
{
	// Create the staple (not added to the scene right now).
	auto staple = std::make_shared<StapleElement>("staple_" + boost::to_string(m_numElements++));
	staple->setPose(m_representation->getPose());

	int toothId = 0;
	bool stapleAdded = false;
	for (auto virtualTooth = m_virtualTeeth.begin(); virtualTooth != m_virtualTeeth.end(); ++virtualTooth)
	{
		// The virtual tooth could be in contact with any number of objects in the scene.
		// Get its collisionMap.
		SurgSim::Collision::Representation::ContactMapType collisionsMap = (*virtualTooth)->getCollisions();

		// If the virtualTooth has no collision, continue to next loop iteration.
		if (collisionsMap.empty())
		{
			continue;
		}

		// Remove representations from the collision map that are not enabled to be stapled.
		filterCollisionMapForStapleEnabledRepresentations(&collisionsMap);

		// If the collision map is emptied after filtering, continue to next loop iteration.
		if (collisionsMap.empty())
		{<commit_msg>Fix Linux warning: switch not handling all cases in StaplerBehavior.cpp<commit_after>// This file is a part of the OpenSurgSim project.
// Copyright 2013, 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 "Examples/ExampleStapling/StaplerBehavior.h"

#include <boost/exception/to_string.hpp>

#include "Examples/ExampleStapling/StapleElement.h"
#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/DataStructures/DataGroup.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/Scene.h"
#include "SurgSim/Framework/SceneElement.h"
#include "SurgSim/Graphics/SceneryRepresentation.h"
#include "SurgSim/Input/InputComponent.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintComponent.h"
#include "SurgSim/Physics/DeformableCollisionRepresentation.h"
#include "SurgSim/Physics/DeformableRepresentation.h"
#include "SurgSim/Physics/FixedRepresentationBilateral3D.h"
#include "SurgSim/Physics/Fem3DRepresentationBilateral3D.h"
#include "SurgSim/Physics/Localization.h"
#include "SurgSim/Physics/RigidCollisionRepresentation.h"
#include "SurgSim/Physics/RigidRepresentationBilateral3D.h"

using SurgSim::Physics::ConstraintImplementation;
using SurgSim::Physics::FixedRepresentationBilateral3D;
using SurgSim::Physics::RigidRepresentationBilateral3D;
using SurgSim::Physics::Fem3DRepresentationBilateral3D;
using SurgSim::Physics::Localization;

StaplerBehavior::StaplerBehavior(const std::string& name):
	SurgSim::Framework::Behavior(name),
	m_numElements(0),
	m_button1Index(-1),
	m_button1IndexCached(false),
	m_buttonPreviouslyPressed(false)
{
}

void StaplerBehavior::setInputComponent(std::shared_ptr<SurgSim::Input::InputComponent> inputComponent)
{
	m_from = inputComponent;
}

void StaplerBehavior::setRepresentation(
	 std::shared_ptr<SurgSim::Framework::Representation> staplerRepresentation)
{
	m_representation = staplerRepresentation;
}

void StaplerBehavior::setVirtualStaple(
	const std::array<std::shared_ptr<SurgSim::Collision::Representation>, 2>& virtualTeeth)
{
	m_virtualTeeth = virtualTeeth;
}

void StaplerBehavior::enableStaplingForSceneElement(std::string sceneElementName)
{
	m_stapleEnabledSceneElements.push_back(sceneElementName);
}

void StaplerBehavior::filterCollisionMapForStapleEnabledRepresentations(
	SurgSim::Collision::Representation::ContactMapType* collisionsMap)
{
	for (auto it = collisionsMap->begin(); it != collisionsMap->end();)
	{
		if (std::find(m_stapleEnabledSceneElements.begin(),
					  m_stapleEnabledSceneElements.end(),
					  (*it).first->getSceneElement()->getName()) == m_stapleEnabledSceneElements.end())
		{
			// Representation's scene element is not in the m_stapleEnabledSceneElements.
			it = collisionsMap->erase(it);
		}
		else
		{
			++it;
		}
	}
}

std::shared_ptr<SurgSim::Physics::Representation> StaplerBehavior::findCorrespondingPhysicsRepresentation(
	std::shared_ptr<SurgSim::Collision::Representation> collisionRepresentation)
{
	std::shared_ptr<SurgSim::Physics::Representation> physicsRepresentation = nullptr;

	// Check if the collisionRepresenation is for a Rigid body.
	std::shared_ptr<SurgSim::Physics::RigidCollisionRepresentation> rigidCollisionRepresentation =
		std::dynamic_pointer_cast<SurgSim::Physics::RigidCollisionRepresentation>(collisionRepresentation);

	if (rigidCollisionRepresentation != nullptr)
	{
		physicsRepresentation = rigidCollisionRepresentation->getRigidRepresentation();
	}
	else
	{
		// Check if the collisionRepresenation is for a deformable body.
		std::shared_ptr<SurgSim::Physics::DeformableCollisionRepresentation> deformableCollisionRepresentation =
			std::dynamic_pointer_cast<SurgSim::Physics::DeformableCollisionRepresentation>(collisionRepresentation);

		if (deformableCollisionRepresentation != nullptr)
		{
			physicsRepresentation = deformableCollisionRepresentation->getDeformableRepresentation();
		}
	}

	return physicsRepresentation;
}

void StaplerBehavior::filterCollisionMapForSupportedRepresentationTypes(
	SurgSim::Collision::Representation::ContactMapType* collisionsMap)
{
	for (auto it = collisionsMap->begin(); it != collisionsMap->end();)
	{
		if (findCorrespondingPhysicsRepresentation((*it).first) == nullptr)
		{
			// Representation type is not supported to be stapled.
			it = collisionsMap->erase(it);
		}
		else
		{
			++it;
		}
	}
}

std::shared_ptr<SurgSim::Physics::Constraint> StaplerBehavior::createBilateral3DConstraint(
	std::shared_ptr<SurgSim::Physics::Representation> stapleRep,
	std::shared_ptr<SurgSim::Physics::Representation> otherRep,
	SurgSim::Collision::Location contraintLocation)
{
	// Create a bilateral constraint between the physicsRepresentation and staple.
	// First find the points where the constraint is going to be applied.
	std::shared_ptr<Localization> stapleRepLocalization
		= stapleRep->createLocalization(contraintLocation);
	stapleRepLocalization->setRepresentation(stapleRep);

	std::shared_ptr<Localization> otherRepLocatization
		= otherRep->createLocalization(contraintLocation);
	otherRepLocatization->setRepresentation(otherRep);

	std::shared_ptr<SurgSim::Physics::Constraint> constraint = nullptr;

	// Create the Constraint with appropriate constraint implementation.
	switch (otherRep->getType())
	{
	case SurgSim::Physics::REPRESENTATION_TYPE_FIXED:
		constraint = std::make_shared<SurgSim::Physics::Constraint>(
						std::make_shared<SurgSim::Physics::ConstraintData>(),
						std::make_shared<RigidRepresentationBilateral3D>(),
						stapleRepLocalization,
						std::make_shared<FixedRepresentationBilateral3D>(),
						otherRepLocatization);
		break;

	case SurgSim::Physics::REPRESENTATION_TYPE_RIGID:
		constraint = std::make_shared<SurgSim::Physics::Constraint>(
						std::make_shared<SurgSim::Physics::ConstraintData>(),
						std::make_shared<RigidRepresentationBilateral3D>(),
						stapleRepLocalization,
						std::make_shared<RigidRepresentationBilateral3D>(),
						otherRepLocatization);
		break;

	case SurgSim::Physics::REPRESENTATION_TYPE_FEM3D:
		constraint = std::make_shared<SurgSim::Physics::Constraint>(
						std::make_shared<SurgSim::Physics::ConstraintData>(),
						std::make_shared<RigidRepresentationBilateral3D>(),
						stapleRepLocalization,
						std::make_shared<Fem3DRepresentationBilateral3D>(),
						otherRepLocatization);
		break;

	default:
		SURGSIM_FAILURE() << "Stapling constraint not supported for this representation type";
		break;
	}

	return constraint;
}

void StaplerBehavior::createStaple()
{
	// Create the staple (not added to the scene right now).
	auto staple = std::make_shared<StapleElement>("staple_" + boost::to_string(m_numElements++));
	staple->setPose(m_representation->getPose());

	int toothId = 0;
	bool stapleAdded = false;
	for (auto virtualTooth = m_virtualTeeth.begin(); virtualTooth != m_virtualTeeth.end(); ++virtualTooth)
	{
		// The virtual tooth could be in contact with any number of objects in the scene.
		// Get its collisionMap.
		SurgSim::Collision::Representation::ContactMapType collisionsMap = (*virtualTooth)->getCollisions();

		// If the virtualTooth has no collision, continue to next loop iteration.
		if (collisionsMap.empty())
		{
			continue;
		}

		// Remove representations from the collision map that are not enabled to be stapled.
		filterCollisionMapForStapleEnabledRepresentations(&collisionsMap);

		// If the collision map is emptied after filtering, continue to next loop iteration.
		if (collisionsMap.empty())
		{
{'fraction_non_alphanumeric': 0.10986871034612727, 'fraction_numerical': 0.0038947170048369874, 'mean_word_length': 6.762067284251585, 'pattern_counts': {'":': 0, '<': 67, '<?xml version=': 0, '>': 101, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1074309', 'n_tokens_mistral': 5352, 'n_tokens_neox': 4836, 'n_words': 910}