Ejemplo n.º 1
0
bool containsOrigin(std::vector<MathVector> &simplex, MathVector &direction)
{
	MathVector a = simplex.back();
	MathVector b, c, ab, ac, abPerp, acPerp;
	MathVector ao = a.negate();

	if(simplex.size() == 3)
	{
		b = simplex[0];
		c = simplex[1];

		ab = b.subtractVectors(a);
		ac = c.subtractVectors(a);

		abPerp = tripleProduct(ac, ab, ab);
		acPerp = tripleProduct(ab, ac, ac);

		if(abPerp.dotProduct(ao) > 0)
		{
			simplex.erase(simplex.begin() + 1);
			direction = abPerp;
		} else if (acPerp.dotProduct(ao) > 0)
		{
			simplex.erase(simplex.begin());
			direction = acPerp;
		} else
		{
			return true;
		}
	} else
	{
		b = simplex[0];
		ab = b.subtractVectors(a);
		abPerp = tripleProduct(ab, ao, ab);

		if(abPerp.getX() == 0 || abPerp.getY() == 0)
		{
			direction = ab.perpendicular();
		} else
		{
			direction = abPerp;
		}
	}
	return false;
}
Ejemplo n.º 2
0
minkowskiDifference_t buildMinkowskiDifference(std::vector<MathVector> a, std::vector<MathVector> b)
{
	MathVector direction = MathVector(1,1);
	std::vector<MathVector> simplex;
	simplex.push_back(getSupportVertex(a, b, direction));
	minkowskiDifference_t difference;

	direction = direction.negate();

	while(true)
	{
		simplex.push_back(getSupportVertex(a, b, direction));
		if(simplex.back().dotProduct(direction) <= 0)
		{
			difference.colliding = false;
			difference.collisionNormal = MathVector(0,0);
			difference.collisionDepth = 0;
			return difference;
		} else if(containsOrigin(simplex, direction) && simplex.size() == 3)
		{
			while(true)
			{
				//Perform EPA to get collision normal and penetration distance
				Edge_t e = findClosestEdge(simplex);

				MathVector p = getSupportVertex(a, b, direction);
				double d = p.dotProduct(e.normal);
//				std::cout << d - e.distance << std::endl;
//				std::cout << "Simplex size: " << simplex.size() << std::endl;
				if(d - e.distance < TOLERANCE)
				{
					difference.collisionNormal = e.normal;
					difference.collisionDepth = d;
					difference.colliding = true;
					return difference;
				} else
				{
//					std::cout << "Closest edge not found in this iteration, adding point to simplex and continuing." << std::endl;
					simplex.insert((simplex.begin()+e.index),p);
				}
			}
		}
	}
}
Ejemplo n.º 3
0
MathVector getSupportVertex(std::vector<MathVector> a, std::vector<MathVector> b, MathVector direction)
{
	MathVector point0 = getFurthestPoint(direction,a);
	MathVector point1 = getFurthestPoint(direction.negate(),b);
	return point0.subtractVectors(point1);
}