C++ Simplex::setClosestPointToOrigin方法代码示例

	/**
	* @param includeMargin Indicate whether algorithm operates on objects with margin
	*/
	template<class T> std::unique_ptr<GJKResult<T>> GJKAlgorithm<T>::processGJK(const CollisionConvexObject3D &convexObject1,
			const CollisionConvexObject3D &convexObject2, bool includeMargin) const
	{ //GJK algorithm (see http://mollyrocket.com/849)

		//get point which belongs to the outline of the shape (Minkowski difference)
		Vector3<T> initialDirection = Vector3<T>(1.0, 0.0, 0.0);
		Point3<T> initialSupportPointA = convexObject1.getSupportPoint(initialDirection, includeMargin);
		Point3<T> initialSupportPointB = convexObject2.getSupportPoint(-initialDirection, includeMargin);
		Point3<T> initialPoint = initialSupportPointA - initialSupportPointB;

		Vector3<T> direction = initialPoint.vector(Point3<T>(0.0, 0.0, 0.0));
		Point3<T> closestPointOnSimplex = initialPoint;

		Simplex<T> simplex;
		simplex.addPoint(initialSupportPointA, initialSupportPointB);
		simplex.setBarycentric(0, 1.0);
		simplex.setClosestPointToOrigin(closestPointOnSimplex);

		T minimumToleranceMultiplicator = (T)1.0;

		for(unsigned int iterationNumber=0; iterationNumber<maxIteration; ++iterationNumber)
		{
			Point3<T> supportPointA = convexObject1.getSupportPoint(direction, includeMargin);
			Point3<T> supportPointB = convexObject2.getSupportPoint(-direction, includeMargin);
			Point3<T> newPoint = supportPointA - supportPointB;

			const Vector3<T> &vClosestPoint = -direction; //vector from origin to closest point of simplex
			T closestPointSquareDistance = vClosestPoint.dotProduct(vClosestPoint);
			T closestPointDotNewPoint = vClosestPoint.dotProduct(newPoint.toVector());

			//check termination conditions: new point is not more extreme that existing ones OR new point already exist in simplex
			T distanceTolerance = std::max(minimumTerminationTolerance*minimumToleranceMultiplicator, relativeTerminationTolerance*closestPointSquareDistance);
			if((closestPointSquareDistance-closestPointDotNewPoint) <= distanceTolerance || simplex.isPointInSimplex(newPoint))
			{
				if(closestPointDotNewPoint <= 0.0)
				{ //collision detected
					return std::unique_ptr<GJKResultCollide<T>>(new GJKResultCollide<T>(simplex));
				}else
				{
					return std::unique_ptr<GJKResultNoCollide<T>>(new GJKResultNoCollide<T>(std::sqrt(closestPointSquareDistance), simplex));
				}
			}

			simplex.addPoint(supportPointA, supportPointB);
			updateSimplex(simplex);

			closestPointOnSimplex = simplex.getClosestPointToOrigin();
			direction = closestPointOnSimplex.vector(Point3<T>(0.0, 0.0, 0.0));

			minimumToleranceMultiplicator += percentageIncreaseOfMinimumTolerance;
		}

		#ifdef _DEBUG
			logMaximumIterationReach();
		#endif

		return std::unique_ptr<GJKResultInvalid<T>>(new GJKResultInvalid<T>());
	}

	/**
	 * Update the simplex and return closest point to origin on the simplex
	 * @return Closest point to origin on the simplex
	 */
	template<class T> void GJKAlgorithm<T>::updateSimplex(Simplex<T> &simplex) const
	{
		Point3<T> closestPoint(0.0, 0.0, 0.0);
		T barycentrics[4];

		if(simplex.getSize() == 2)
		{ //simplex is a line (1D)

			const Point3<T> &pointA = simplex.getPoint(0);
			const Point3<T> &pointB = simplex.getPoint(1); //pointB is the last point added to the simplex

			closestPoint = LineSegment3D<T>(pointA, pointB).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics);
			simplex.setBarycentric(0, barycentrics[0]);
			simplex.setBarycentric(1, barycentrics[1]);
		}else if(simplex.getSize() == 3)
		{ //simplex is a triangle (2D)

			const Point3<T> &pointA = simplex.getPoint(0);
			const Point3<T> &pointB = simplex.getPoint(1);
			const Point3<T> &pointC = simplex.getPoint(2); //pointC is the last point added to the simplex

			const Vector3<T> co = pointC.vector(Point3<T>(0.0, 0.0, 0.0));
			const Vector3<T> cb = pointC.vector(pointB);
			const Vector3<T> ca = pointC.vector(pointA);
			const Vector3<T> normalAbc = cb.crossProduct(ca);

			closestPoint = Triangle3D<T>(pointA, pointB, pointC).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics);
			simplex.setBarycentric(0, barycentrics[0]);
			simplex.setBarycentric(1, barycentrics[1]);
			simplex.setBarycentric(2, barycentrics[2]);

			if(barycentrics[1]==0.0)
			{ //remove pointB
				simplex.removePoint(1);
			}
			if(barycentrics[0]==0.0)
			{ //remove pointA
				simplex.removePoint(0);
			}

			if(normalAbc.dotProduct(co) <= 0.0)
			{ //voronoi region -ABC => ABC
				simplex.swapPoints(0, 1); //swap pointA and pointB
			}
		}else if (simplex.getSize() == 4)
		{ //simplex is a tetrahedron (3D)

			const Point3<T> &pointA = simplex.getPoint(0);
			const Point3<T> &pointB = simplex.getPoint(1);
			const Point3<T> &pointC = simplex.getPoint(2);
			const Point3<T> &pointD = simplex.getPoint(3); //pointD is the last point added to the simplex

			const short voronoiRegionMask = 14; //test all voronoi regions except the one which doesn't include the new point added (pointD)
			closestPoint = Tetrahedron<T>(pointA, pointB, pointC, pointD).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics, voronoiRegionMask);
			simplex.setBarycentric(0, barycentrics[0]);
			simplex.setBarycentric(1, barycentrics[1]);
			simplex.setBarycentric(2, barycentrics[2]);
			simplex.setBarycentric(3, barycentrics[3]);

			if(barycentrics[2]==0.0)
			{ //remove pointC
				simplex.removePoint(2);
			}
			if(barycentrics[1]==0.0)
			{ //remove pointB
				simplex.removePoint(1);
			}
			if(barycentrics[0]==0.0)
			{ //remove pointA
				simplex.removePoint(0);
			}
		}else
		{
			std::ostringstream oss;
			oss << simplex.getSize();
			throw std::invalid_argument("Size of simplex unsupported: " + oss.str() + ".");
		}

		simplex.setClosestPointToOrigin(closestPoint);
	}