<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.

#ifndef SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
#define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H

namespace SurgSim
{

namespace Math
{

/// A helper class for a triangle, used for the following two purposes:
///		- Clip against a given plane.
///		- Find the deepest point given a plane.
/// Created as a triangle and can become 4 or more sided polygon when clipped.
/// \note The vertices are stored in order, so that the edges of the polygon run between adjacent vertices
///    (and from the last vertex to the first).
/// \tparam T Accuracy of the calculation.
/// \tparam MOpt Eigen Matrix options.
template <class T, int MOpt>
class TriangleHelper
{
	static const size_t CAPACITY = 10;
	typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
	typedef boost::container::static_vector<Vector3, CAPACITY> Vertices;

public:
	/// Constructor using the triangle data to initialize.
	/// \param v0, v1, v2 The vertices of the triangle.
	/// \param n The normal of the triangle.
	TriangleHelper(const Vector3& v0, const Vector3& v1, const Vector3& v2, const Vector3& n)
		: m_normal(n), m_receiverBufferIndex(0)
	{
		m_vertices[0] = &v0;
		m_vertices[1] = &v1;
		m_vertices[2] = &v2;
		m_planeD = -m_vertices[0]->dot(m_normal);
	}

	/// Given a triangle, find the deepest vertex in the swept volume of that triangle.
	/// \param triangle The triangle against which the penetration is checked.
	/// \param [out] penetrationDepth The depth of the deepest point in this triangle to the triangle sent in.
	/// \param [out] penetationPoint0 The penetration point on this triangle.
	/// \param [out] penetationPoint1 The penetration point on the triangle sent in.
	void findDeepestPenetrationWithTriangle(const TriangleHelper& triangle, T* penetrationDepth,
											Vector3* penetationPoint0, Vector3* penetrationPoint1)
	{
		m_clippedVerticesBuffer[0].push_back(*m_vertices[0]);
		m_clippedVerticesBuffer[0].push_back(*m_vertices[1]);
		m_clippedVerticesBuffer[0].push_back(*m_vertices[2]);
		m_receiverBufferIndex = 1;

		Vector3 clipPlaneNormal;
		T clipPlaneD;

		for (size_t i = 0; i < 3; ++i)
		{
			triangle.getPrismPlane(i, &clipPlaneNormal, &clipPlaneD);
			clipAgainstPlane(clipPlaneNormal, clipPlaneD);
		}

		findDeepestVertexUnderPlane(triangle.m_normal, triangle.m_planeD, penetrationDepth, penetationPoint0);

		SURGSIM_ASSERT(*penetrationDepth <= T(0))
				<< "The distance from triangle is calculated as " << *penetrationDepth << ". At this point in the"
				<< " algorithm, the depth is expected to be negative.";

		*penetrationPoint1 = *penetationPoint0 - (triangle.m_normal * (*penetrationDepth));
		*penetrationDepth = -(*penetrationDepth);
	}

private:
	/// Get the bounding plane of the swept volume of this triangle.
	/// The swept volume of a triangle is an infinitely long prism.
	/// \param index There are three prism sides, the index indicates which one is to be calculated.
	/// \param planeNormal The outward facing normal of the prism plane.
	/// \param planeD d from the plane equation (n.x + d = 0) of the prism plane.
	void getPrismPlane(size_t index, Vector3* planeNormal, T* planeD) const
	{
		*planeNormal = *m_vertices[(index + 1) % 3] - *m_vertices[index];
		*planeNormal = planeNormal->cross(m_normal);
		planeNormal->normalize();
		*planeD = -m_vertices[index]->dot(*planeNormal);
	}

	/// Clip the polygon given a plane. Any part of the polygon above this plane is clipped.
	/// \note This may alter the number of vertices in this polygon.
	/// \param planeN The normal of the clipping plane.
	/// \param planeD The d from plane eqn (nx + d) of the clipping plane.
	void clipAgainstPlane(const Vector3& planeN, T planeD)
	{
		// Loop through the edges starting from (m_vertices[0]->m_vertices[1]) to
		// (m_vertices[m_numVertices - 1]->m_vertices[0]).
		// The start vertex and end vertex can be either under/on/over the clipping plane.
		//		start	|	end		|	action
		// ----------------------------------------------
		//		under	|	under	|	add start to clipped vertices
		//		under	|	on		|	add start to clipped vertices
		//		under	|	over	|	add start to clipped vertices, clip the edge to the plane (creating a vertex)
		//		on		|	under	|	add start to clipped vertices
		//		on		|	on		|	add start to clipped vertices
		//		on		|	over	|	add start to clipped vertices
		//		over	|	under	|	clip the edge to the plane (creating a vertex)
		//		over	|	on		|	none
		//		over	|	over	|	none

		Vertices& clippedVertices = m_clippedVerticesBuffer[m_receiverBufferIndex];
		m_receiverBufferIndex = (m_receiverBufferIndex + 1) % 2;
		Vertices& originalVertices = m_clippedVerticesBuffer[m_receiverBufferIndex];
		clippedVertices.clear();

		static const T EPSILON = T(Geometry::DistanceEpsilon);

		// Calculate the signed distance of the vertices from the clipping plane.
		boost::container::static_vector<T, CAPACITY> signedDistanceFromPlane;
		for (auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
		{
			signedDistanceFromPlane.push_back((*it).dot(planeN) + planeD);
		}

		// Temp variable.
		T ratio;

		// Iterators for the end vertices of an edge.
		boost::container::static_vector<Vector3, CAPACITY>::iterator end;

		// Iterators for the signed distance from plane of the start and end vertices of an edge.
		auto startSignedDistance = signedDistanceFromPlane.begin();
		boost::container::static_vector<T, CAPACITY>::iterator endSignedDistance;

		// Iterate over the edges of the current polygon.
		for (auto start = originalVertices.begin(); start != originalVertices.end(); ++start, ++startSignedDistance)
		{
			// If the end has reached the end of list, point it back to the front of list.
			end = start + 1;
			endSignedDistance = startSignedDistance + 1;
			if (end == originalVertices.end())
			{
				end = originalVertices.begin();
				endSignedDistance = signedDistanceFromPlane.begin();
			}

			// If the vertex is under or on the plane, add to the clippedVertices.
			if (*startSignedDistance <= EPSILON)
			{
				clippedVertices.push_back(*start);
			}

			// If the edge runs from one side of the plane to another. Clip it.
			if ((*startSignedDistance < -EPSILON && *endSignedDistance > EPSILON) ||
				(*startSignedDistance > EPSILON && *endSignedDistance < -EPSILON))
			{
				ratio = *startSignedDistance / (*startSignedDistance - *endSignedDistance);
				clippedVertices.push_back(*start + (*end - *start) * ratio);
			}
		}
	}

	/// Find the deepest vertex of this polygon under the plane.<commit_msg>Specify typename keyword for iterator variable declaration.<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.

#ifndef SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
#define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H

namespace SurgSim
{

namespace Math
{

/// A helper class for a triangle, used for the following two purposes:
///		- Clip against a given plane.
///		- Find the deepest point given a plane.
/// Created as a triangle and can become 4 or more sided polygon when clipped.
/// \note The vertices are stored in order, so that the edges of the polygon run between adjacent vertices
///    (and from the last vertex to the first).
/// \tparam T Accuracy of the calculation.
/// \tparam MOpt Eigen Matrix options.
template <class T, int MOpt>
class TriangleHelper
{
	static const size_t CAPACITY = 10;
	typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
	typedef boost::container::static_vector<Vector3, CAPACITY> Vertices;

public:
	/// Constructor using the triangle data to initialize.
	/// \param v0, v1, v2 The vertices of the triangle.
	/// \param n The normal of the triangle.
	TriangleHelper(const Vector3& v0, const Vector3& v1, const Vector3& v2, const Vector3& n)
		: m_normal(n), m_receiverBufferIndex(0)
	{
		m_vertices[0] = &v0;
		m_vertices[1] = &v1;
		m_vertices[2] = &v2;
		m_planeD = -m_vertices[0]->dot(m_normal);
	}

	/// Given a triangle, find the deepest vertex in the swept volume of that triangle.
	/// \param triangle The triangle against which the penetration is checked.
	/// \param [out] penetrationDepth The depth of the deepest point in this triangle to the triangle sent in.
	/// \param [out] penetationPoint0 The penetration point on this triangle.
	/// \param [out] penetationPoint1 The penetration point on the triangle sent in.
	void findDeepestPenetrationWithTriangle(const TriangleHelper& triangle, T* penetrationDepth,
											Vector3* penetationPoint0, Vector3* penetrationPoint1)
	{
		m_clippedVerticesBuffer[0].push_back(*m_vertices[0]);
		m_clippedVerticesBuffer[0].push_back(*m_vertices[1]);
		m_clippedVerticesBuffer[0].push_back(*m_vertices[2]);
		m_receiverBufferIndex = 1;

		Vector3 clipPlaneNormal;
		T clipPlaneD;

		for (size_t i = 0; i < 3; ++i)
		{
			triangle.getPrismPlane(i, &clipPlaneNormal, &clipPlaneD);
			clipAgainstPlane(clipPlaneNormal, clipPlaneD);
		}

		findDeepestVertexUnderPlane(triangle.m_normal, triangle.m_planeD, penetrationDepth, penetationPoint0);

		SURGSIM_ASSERT(*penetrationDepth <= T(0))
				<< "The distance from triangle is calculated as " << *penetrationDepth << ". At this point in the"
				<< " algorithm, the depth is expected to be negative.";

		*penetrationPoint1 = *penetationPoint0 - (triangle.m_normal * (*penetrationDepth));
		*penetrationDepth = -(*penetrationDepth);
	}

private:
	/// Get the bounding plane of the swept volume of this triangle.
	/// The swept volume of a triangle is an infinitely long prism.
	/// \param index There are three prism sides, the index indicates which one is to be calculated.
	/// \param planeNormal The outward facing normal of the prism plane.
	/// \param planeD d from the plane equation (n.x + d = 0) of the prism plane.
	void getPrismPlane(size_t index, Vector3* planeNormal, T* planeD) const
	{
		*planeNormal = *m_vertices[(index + 1) % 3] - *m_vertices[index];
		*planeNormal = planeNormal->cross(m_normal);
		planeNormal->normalize();
		*planeD = -m_vertices[index]->dot(*planeNormal);
	}

	/// Clip the polygon given a plane. Any part of the polygon above this plane is clipped.
	/// \note This may alter the number of vertices in this polygon.
	/// \param planeN The normal of the clipping plane.
	/// \param planeD The d from plane eqn (nx + d) of the clipping plane.
	void clipAgainstPlane(const Vector3& planeN, T planeD)
	{
		// Loop through the edges starting from (m_vertices[0]->m_vertices[1]) to
		// (m_vertices[m_numVertices - 1]->m_vertices[0]).
		// The start vertex and end vertex can be either under/on/over the clipping plane.
		//		start	|	end		|	action
		// ----------------------------------------------
		//		under	|	under	|	add start to clipped vertices
		//		under	|	on		|	add start to clipped vertices
		//		under	|	over	|	add start to clipped vertices, clip the edge to the plane (creating a vertex)
		//		on		|	under	|	add start to clipped vertices
		//		on		|	on		|	add start to clipped vertices
		//		on		|	over	|	add start to clipped vertices
		//		over	|	under	|	clip the edge to the plane (creating a vertex)
		//		over	|	on		|	none
		//		over	|	over	|	none

		Vertices& clippedVertices = m_clippedVerticesBuffer[m_receiverBufferIndex];
		m_receiverBufferIndex = (m_receiverBufferIndex + 1) % 2;
		Vertices& originalVertices = m_clippedVerticesBuffer[m_receiverBufferIndex];
		clippedVertices.clear();

		static const T EPSILON = T(Geometry::DistanceEpsilon);

		// Calculate the signed distance of the vertices from the clipping plane.
		boost::container::static_vector<T, CAPACITY> signedDistanceFromPlane;
		for (auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
		{
			signedDistanceFromPlane.push_back((*it).dot(planeN) + planeD);
		}

		// Temp variable.
		T ratio;

		// Iterators for the end vertices of an edge.
		typename boost::container::static_vector<Vector3, CAPACITY>::iterator end;

		// Iterators for the signed distance from plane of the start and end vertices of an edge.
		auto startSignedDistance = signedDistanceFromPlane.begin();
		typename boost::container::static_vector<T, CAPACITY>::iterator endSignedDistance;

		// Iterate over the edges of the current polygon.
		for (auto start = originalVertices.begin(); start != originalVertices.end(); ++start, ++startSignedDistance)
		{
			// If the end has reached the end of list, point it back to the front of list.
			end = start + 1;
			endSignedDistance = startSignedDistance + 1;
			if (end == originalVertices.end())
			{
				end = originalVertices.begin();
				endSignedDistance = signedDistanceFromPlane.begin();
			}

			// If the vertex is under or on the plane, add to the clippedVertices.
			if (*startSignedDistance <= EPSILON)
			{
				clippedVertices.push_back(*start);
			}

			// If the edge runs from one side of the plane to another. Clip it.
			if ((*startSignedDistance < -EPSILON && *endSignedDistance > EPSILON) ||
				(*startSignedDistance > EPSILON && *endSignedDistance < -EPSILON))
			{
				ratio = *startSignedDistance / (*startSignedDistance - *endSignedDistance);
				clippedVertices.push_back(*start + (*end - *start) * ratio);
			}
		}
	}

	/// Find the deepest vertex of this polygon under the plane.
{'fraction_non_alphanumeric': 0.10761865112406328, 'fraction_numerical': 0.009297807382736608, 'mean_word_length': 3.6553617571059434, 'pattern_counts': {'":': 0, '<': 41, '<?xml version=': 0, '>': 31, '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': '4328099', 'n_tokens_mistral': 4920, 'n_tokens_neox': 4480, 'n_words': 1563}