Hi all,

Trying to implement SAT in my code to detect collisions (binary yes/no is fine for the moment - no requirement to get the actual contact point). However I'm having an issue that the objects seem to intersect a small amount before the collision is detected.

Below is my code, and attached is a screenshot and I would very much appreciate it if someone could point me in the right direction. I must be doing something silly somewhere as it seems so close.

I'm implementing SAT as described here: http://www.dyn4j.org/2010/01/sat/

I'm aware that the code is quite messy and obviously not optimised (that's for a later stage )

const std::vector<Vector>& vertices1	= entity1RigidBody->GetCollisionVertices();
const std::vector<Vector>& faces1	= entity1RigidBody->GetCollisionFaces();
const std::vector<Vector>& normals1	= entity1RigidBody->GetCollisionFaceNormals();

const std::vector<Vector>& vertices2	= entity2RigidBody->GetCollisionVertices();
const std::vector<Vector>& faces2	= entity2RigidBody->GetCollisionFaces();
const std::vector<Vector>& normals2	= entity2RigidBody->GetCollisionFaceNormals();

glm::mat4 entity1TransformMatrix = entity1Transform->GetTransform().GetMatrix();
glm::mat4 entity2TransformMatrix = entity2Transform->GetTransform().GetMatrix();

if(vertices1.size() == 0 || faces1.size() == 0 || normals1.size() == 0) {
	return false;
}

if(vertices2.size() == 0 || faces2.size() == 0 || normals2.size() == 0) {
	return false;
}

std::vector<Vector> axes;

for(unsigned int i = 0; i < normals1.size(); i++) {
	glm::vec4 vector(normals1[i].x, normals1[i].y, normals1[i].z, 0.0f);
	vector = vector * entity1TransformMatrix; // Transform the vertices

	Vector axis(vector.x, vector.y, vector.z);
	axes.push_back(axis);
}

for(unsigned int i = 0; i < normals2.size(); i++) {
	glm::vec4 vector(normals2[i].x, normals2[i].y, normals2[i].z, 0.0f);
	vector = vector * entity2TransformMatrix; // Transform the vertices

	Vector axis(vector.x, vector.y, vector.z);
	axes.push_back(axis);
}

// We have all the axes from the Normals (this should probably only be computed once...)
// For each axis, lets get a projection from each vertex onto each axis (!!!!)
for(unsigned int a = 0; a < axes.size(); a++) {
	double min1 = 0;
	double max1 = 0;
	double min2 = 0;
	double max2 = 0;

	Vector axis = axes[a];

	for(unsigned int face = 0; face < faces1.size(); face++) {
		Vector faceIndices = faces1[face];
		Vector faceVertices[3];

		faceVertices[0] = vertices1[faceIndices.x];
		faceVertices[1] = vertices1[faceIndices.y];
		faceVertices[2] = vertices1[faceIndices.z];

		for(unsigned int vert = 0; vert < 3; vert++) {
			Vector vertex = faceVertices[vert];
			glm::vec4 v(vertex.x, vertex.y, vertex.z, 0.0f);
			glm::vec4 transformedV = (v * entity1TransformMatrix); // Transform the vertices
			vertex.x = transformedV.x;
			vertex.y = transformedV.y;
			vertex.z = transformedV.z;

			double p = Vector::Dot(axis, vertex);
			if(p < min1) {
				min1 = p;
			} else if (p > max1) {
				max1 = 0;
			}
		}
	}

	for(unsigned int face = 0; face < faces2.size(); face++) {
		Vector faceIndices = faces2[face];
		Vector faceVertices[3];

		faceVertices[0] = vertices2[faceIndices.x];
		faceVertices[1] = vertices2[faceIndices.y];
		faceVertices[2] = vertices2[faceIndices.z];

		for(unsigned int vert = 0; vert < 3; vert++) {
			Vector vertex = faceVertices[vert];
			glm::vec4 v(vertex.x, vertex.y, vertex.z, 0.0f);
			glm::vec4 transformedV = (v * entity2TransformMatrix); // Transform the vertices
			vertex.x = transformedV.x;
			vertex.y = transformedV.y;
			vertex.z = transformedV.z;

			double p = Vector::Dot(axis, vertex);
			if(p < min2) {
				min2 = p;
			} else if (p > max1) {
				max2 = 0;
			}
		}
	}

	// Check if they overlap
	if(min1 < min2 && max1 < min2) {
		// No Overlap
		return false;
	}

	if(min1 > max2 && max1 > max2) {
		// No Overlap
		return false;
	}
}

// We've reached this far - there must be a collision
return true;

You also need to use pairwise cross products of all edges as your axes, otherwise you will have lots of false positives (I'm sure you can find an example on any page describing the algorithm; keep in mind that its only needed for 3D, in 2D just using the face normals will be enough). However, if you're actually getting false negatives, then the problem must be elsewhere.

Also, if you care about performance at all, I wouldn't put all face normals into one set and using them together, since you're doing twice more work than you need to when calculating the projections: if you use a face normal for object A, you already know the furthest point along that axis on object A and don't need to calculate it again.

Anyhow, problems with the actual code:

1. Don't initialize min/max to 0, use min=+INF, max=-INF instead.
2. if (p > max1) max1 = 0; <- what is this?

This is an old brute-force SAT:

struct PROJECTION {
                float fMin;
                float fMax;
                bool Overlap(const PROJECTION& _pOther) const {
                        return !(fMax < _pOther.fMin || _pOther.fMax < fMin);
                }
};
 
bool CSat::TestFaces(const CShapeInstance* _psiShape0, const CShapeInstance* _psiShape1) const {
        const CConvexPolyhedron* pcpConvex0 = static_cast<const CConvexPolyhedron*>(_psiShape0->m_psShape);
        const CConvexPolyhedron* pcpConvex1 = static_cast<const CConvexPolyhedron*>(_psiShape1->m_psShape);
 
        for (unsigned int I = 0; I < pcpConvex0->m_ui32TotalFaces; ++I) {
                CVector3 vAxis = _psiShape0->m_mTransform.Rotation() * pcpConvex0->m_pvFaces[I];
 
                PROJECTION pProj0;
                PROJECTION pProj1;
                Project(pcpConvex0, _psiShape0->m_mTransform, vAxis, pProj0);
                Project(pcpConvex1, _psiShape1->m_mTransform, vAxis, pProj1);
 
                if (!pProj0.Overlap(pProj1)) {
                        return false;
                }
        }
 
        return true;
}
 
void CSat::Project(const CConvexPolyhedron* _pcpHull, const CMatrix4x4& _mTransform, const CVector3& _vAxis, PROJECTION& _pProj) const {
        _pProj.fMin = _pProj.fMax = _vAxis.Dot(_mTransform * _pcpHull->m_pvVerts[0]);
        for (unsigned int I = 1; I < _pcpHull->m_ui32TotalVerts; ++I) {
                float fProj = _vAxis.Dot(_mTransform * _pcpHull->m_pvVerts[I]);
                if (fProj < _pProj.fMin) {
                        _pProj.fMin = fProj;
                }
                if (fProj > _pProj.fMax) {
                        _pProj.fMax = fProj;
                }
        }
}
 
bool CSat::TestEdges(const CShapeInstance* _psiShape0, const CShapeInstance* _psiShape1) const {
        const CConvexPolyhedron* pcpConvex0 = static_cast<const CConvexPolyhedron*>(_psiShape0->m_psShape);
        const CConvexPolyhedron* pcpConvex1 = static_cast<const CConvexPolyhedron*>(_psiShape1->m_psShape);
 
        for (unsigned int I = 0; I < pcpConvex0->m_ui32TotalEdges; ++I) {
                CVector3 vAxis0 = _psiShape0->m_mTransform.Rotation() * pcpConvex0->m_pvEdges[I].vNormal;
 
                for (unsigned int J = 0; J < pcpConvex1->m_ui32TotalEdges; ++J) {
                        CVector3 vAxis1 = _psiShape1->m_mTransform.Rotation() * pcpConvex1->m_pvEdges[J].vNormal;
 
                        CVector3 vAxis = vAxis0.Cross(vAxis1);
 
                        PROJECTION pProj0;
                        PROJECTION pProj1;
                        Project(pcpConvex0, _psiShape0->m_mTransform, vAxis, pProj0);
                        Project(pcpConvex1, _psiShape1->m_mTransform, vAxis, pProj1);
 
                        if (!pProj0.Overlap(pProj1)) {
                                return false;
                        }
                }
        }
 
        return true;
}
 
bool CSat::Intersect(const CShapeInstance* _psiShape0, const CShapeInstance* _psiShape1) {
       if ( !TestFaces(_psiShape0, _psiShape1) ) {
                return false;
        }
        if ( !TestFaces( _psiShape1, _psiShape0 ) ) {
                return false;
        }
        if ( !TestEdges( _psiShape0, _psiShape1 ) ) {
                return false;
        }
        return true;
}

This thread discusses different issues with SAT as well optimizations in order to put in production:

http://www.gamedev.net/topic/667499-3d-sat-problem/

Anyhow, problems with the actual code:

1. Don't initialize min/max to 0, use min=+INF, max=-INF instead.
2. if (p > max1) max1 = 0; <- what is this?

Thanks for these - I've updated my code to use very large numbers for initialising min/max accordingly. With regards to your 2nd point - this was a coding error - I've since fixed that now.

This is an old brute-force SAT:

// Snippet Removed

This thread discusses different issues with SAT as well optimizations in order to put in production:

http://www.gamedev.net/topic/667499-3d-sat-problem/

Thanks for that code, I've refactored some of mine from being a horrible huge method just to try and make it a bit more readable like yours.

However, it turns out the problem was completely different to what I thought. I had originally had another test that preceeded the SAT test that basically just checked whether two spheres (1 for each rigidbody) were intersecting before then trying to do the more accurate test - this was working as expected - but then the result of the SAT test was always claiming a collision).

So I've removed my sphere test for the moment, and refactored a bit of my code to make it more readable and unfortunately it's still going wrong. It's reporting a collision constantly. Looking deeper, it seems that the Projection for both meshes on each axis is always identical - which is obviously incorrect. So I need to dig around a bit deeper - I suspect I'm misunderstanding how to project the vertices onto the plane defined by the axes.

I've included my modified code for completeness sake, but I'll keep researching and checking my code - right now nothing is popping out at me.

const bool System::DoesCollide(Lucidity::Scene::Entity& entity1, Lucidity::Scene::Entity& entity2) const {
	Lucidity::Scene::TransformComponent* entity1Transform = entity1.GetComponent<Lucidity::Scene::TransformComponent>();
	Lucidity::Scene::TransformComponent* entity2Transform = entity2.GetComponent<Lucidity::Scene::TransformComponent>();

	Lucidity::Physics::RigidBody* entity1RigidBody = entity1.GetComponent<Lucidity::Physics::RigidBody>();
	Lucidity::Physics::RigidBody* entity2RigidBody = entity2.GetComponent<Lucidity::Physics::RigidBody>();

	if(!entity1Transform || !entity1RigidBody) {
		// Entity 1 has no Transform OR has no RigidBody
		return false;
	}

	if(!entity2Transform || !entity2RigidBody) {
		// Entity 2 has no Transform OR has no RigidBody
		return false;
	}

	Vector entity1Origin(entity1Transform->GetTransform().GetPositionX(), entity1Transform->GetTransform().GetPositionY(), entity1Transform->GetTransform().GetPositionZ());
	Vector entity2Origin(entity2Transform->GetTransform().GetPositionX(), entity2Transform->GetTransform().GetPositionY(), entity2Transform->GetTransform().GetPositionZ());

	Vector vectorBetween = entity1Origin - entity2Origin;
	float vectorDistance = vectorBetween.Length();
	float superDistance = entity1RigidBody->GetSuperRadius() + entity2RigidBody->GetSuperRadius();

	if(true) {
	//if(vectorDistance < superDistance) {
		const std::vector<Vector>& vertices1	= entity1RigidBody->GetCollisionVertices();
		const std::vector<Vector>& faces1		= entity1RigidBody->GetCollisionFaces();
		const std::vector<Vector>& normals1		= entity1RigidBody->GetCollisionFaceNormals();

		const std::vector<Vector>& vertices2	= entity2RigidBody->GetCollisionVertices();
		const std::vector<Vector>& faces2		= entity2RigidBody->GetCollisionFaces();
		const std::vector<Vector>& normals2		= entity2RigidBody->GetCollisionFaceNormals();

		glm::mat4 entity1TransformMatrix = entity1Transform->GetTransform().GetMatrix();
		glm::mat4 entity2TransformMatrix = entity2Transform->GetTransform().GetMatrix();

		if(vertices1.size() == 0 || faces1.size() == 0 || normals1.size() == 0) {
			return false;
		}

		if(vertices2.size() == 0 || faces2.size() == 0 || normals2.size() == 0) {
			return false;
		}

		std::vector<Vector> axes;

		for(unsigned int i = 0; i < normals1.size(); i++) {
			glm::vec4 normal(normals1[i].x, normals1[i].y, normals1[i].z, 0.0f);
			normal = entity1TransformMatrix * normal; // Transform the vertices
			normal = glm::normalize(normal);

			Vector axis(normal.x, normal.y, normal.z);
			axes.push_back(axis);
		}

		for(unsigned int i = 0; i < normals2.size(); i++) {
			glm::vec4 normal(normals2[i].x, normals2[i].y, normals2[i].z, 0.0f);
			normal = entity2TransformMatrix * normal; // Transform the vertices
			normal = glm::normalize(normal);

			Vector axis(normal.x, normal.y, normal.z);
			axes.push_back(axis);
		}

		// We have all the axes from the Normals (this should probably only be computed once...)
		// For each axis, lets get a projection from each vertex onto each axis (!!!!)
		std::vector<Vector>::iterator it = axes.begin();
		std::vector<Vector>::iterator itEnd = axes.end();
		for(it; it != itEnd; it++) {
			Vector axis = *it;
			 // TODO: Currently these projections don't seem to be correct
			Projection projection1 = projectOntoAxis(entity1TransformMatrix, axis, faces1, vertices1);
			Projection projection2 = projectOntoAxis(entity2TransformMatrix, axis, faces2, vertices2);

			// Check if they overlap
			if(projection1.max < projection2.min || projection2.max < projection1.min) {
				return false;
			}				
		}

		// We've reached this far - there must be a collision
		return true;
	}

	return false;
};

const Projection System::projectOntoAxis(const glm::mat4& transform, const Vector& axis, const std::vector<Vector>& faces, const std::vector<Vector>& vertices) const {
	Projection projection;
	projection.min = 1000.0f;
	projection.max = -1000.0f;

	glm::vec4 glmAxis = glm::vec4(axis.x, axis.y, axis.z, 0.0f);

	for(unsigned int face = 0; face < faces.size(); face++) {
		Vector faceIndices = faces[face];
		Vector faceVertices[3];

		faceVertices[0] = vertices[faceIndices.x];
		faceVertices[1] = vertices[faceIndices.y];
		faceVertices[2] = vertices[faceIndices.z];

		for(unsigned int vert = 0; vert < 3; ++vert) {
			Vector vertex = faceVertices[vert];
			glm::vec4 v(vertex.x, vertex.y, vertex.z, 0.0f);
			v = (transform * v); // Transform the vertices
					
			float p = glm::dot(v, glmAxis);

			if(p < projection.min) {
				projection.min = p;
			} else if (p > projection.max) {
				projection.max = p;
			}

			//std::cout << "Vertex [ " << vertex.x << ", " << vertex.y << ", " << vertex.z << " ] - " << projection.min << "/" << projection.max << std::endl;
		}
	}

	return projection;
};

Test all your collision routines separately for only two objects. This is much more transparently.

// Check if they overlap
if(projection1.max < projection2.min || projection2.max < projection1.min) {
return false;
}

This look suspecious. If the projection does overlap then you should keep looking for some other axis.

You should look for a separating axis coming for the first and the second shape normals, and not only the first.

I encourage you to run the code I've posted first in order to make sure your math and your normals are correct.

Example:

        // Check if there is a separating axis coming from the first hull axes.
        if ( !TestFaces(_psiShape0, _psiShape1) ) {
                // Axis found, exit no collision.
                return false;
        }
        // Check if there is a separating axis coming from the second hull axes.
        if ( !TestFaces( _psiShape1, _psiShape0 ) ) {
                // Axis found, exit no collision.
                return false;
        }
        // Check if there is a separating axis coming from the cross products of the edge normals.
        if ( !TestEdges( _psiShape0, _psiShape1 ) ) {
                // Axis found, exit no collision.
                return false;
        }
        // No axis found, exit with collision.
        return true;

Oh! You are using 0 as the 4th component of your vectors, which makes it ignore the translation component of the transformation matrices, and makes sense for transforming normals/directions. But for transforming actual vertex positions (near the end of your code), you need to set the 4th component to 1.

@Irlan, he does use both objects' face normals, and the if statement you quoted checks that the intervals don't intersect, so it is correct to return false in that case.

However, as I mentioned, just testing the face normals is not enough in 3D - if two boxes are (almost) touching edge to edge, all face normals will report intersection, while the objects actually aren't touching. Your list of axes also needs to include cross products of all pairs of edge vectors.

Thanks for your response, that equality test was modified from your example - although I appear to have gotten the true/false part the wrong way around so shall modify it accordingly.

I actually went through this morning and wrote a very simple test to ensure my understanding of the maths involved was correct. It appears to be all correct (currently not doing the Edge test - will implement that once I know the face tests are working). This seems to behave has expected - however it is obviously vastly simplified (only has 2 defined axes to test (ones that I can verify on paper) and the vertex co-ordinates are in world space, not local space so that I didn't need to include a transformation matrix.

bool simpleSatTest() {
	std::vector<Vector> axes;

	// Object 1 Vertices
	std::vector<Vector> object1vertices;
	std::vector<Vector> object1faces;

	object1vertices.push_back(Vector(3.0f, 1.0f, 0.0f));
	object1vertices.push_back(Vector(4.0f, 1.0f, 0.0f));
	object1vertices.push_back(Vector(3.5f, 1.0f, 0.0f));

	object1faces.push_back(Vector(0.0f, 1.0f, 2.0f));
	
	// Object 2 Vertices
	std::vector<Vector> object2vertices;
	std::vector<Vector> object2faces;

	object2vertices.push_back(Vector(3.0f, 1.0f, 0.0f));
	object2vertices.push_back(Vector(4.0f, 1.0f, 0.0f));
	object2vertices.push_back(Vector(3.0f, 2.0f, 0.0f));
	object2vertices.push_back(Vector(4.0f, 2.0f, 0.0f));

	object2faces.push_back(Vector(0.0f, 1.0f, 2.0f));
	object2faces.push_back(Vector(2.0f, 3.0f, 0.0f));

	// Axes to test
	axes.push_back(Vector(1.0f, 0.0f, 0.0f)); // X-Axis
	axes.push_back(Vector(0.0f, 1.0f, 0.0f)); // Y-Axis

	for(unsigned int a = 0; a < axes.size(); ++a) {
		float Obj1Min = 10000.0f;
		float Obj1Max = -10000.0f;

		float Obj2Min = 10000.0f;
		float Obj2Max = -10000.0f;

		Vector axis = axes[a];

		for(unsigned int f = 0; f < object1faces.size(); ++f) {
			Vector face = object1faces[f];
			int index[3];
			index[0] = face.x;
			index[1] = face.y;
			index[2] = face.z;

			for(unsigned int v = 0; v < 3; ++v) {
				Vector vertex = object1vertices[index[v]];
				float result = dot(vertex, axis);
				if(result < Obj1Min) {
					Obj1Min = result;
				} else if (result > Obj1Max) {
					Obj1Max = result;
				}
			}
		}

		for(unsigned int f = 0; f < object2faces.size(); ++f) {
			Vector face = object2faces[f];
			int index[3];
			index[0] = face.x;
			index[1] = face.y;
			index[2] = face.z;

			for(unsigned int v = 0; v < 3; ++v) {
				Vector vertex = object2vertices[index[v]];
				float result = dot(vertex, axis);
				if(result < Obj2Min) {
					Obj2Min = result;
				} else if (result > Obj2Max) {
					Obj2Max = result;
				}
			}
		}

		if(Obj1Max < Obj2Min || Obj2Max < Obj1Min) {
			std::cout << "Non-Collision Hyperplane found" << std::endl;
		} else {
			std::cout << "Collision Hyperplane found" << std::endl;
		}
	}

	std::cout << "Test complete" << std::endl;

	return true;
}

I am generating axes from both objects normals - as per here:

for(unsigned int i = 0; i < normals1.size(); i++) {
	glm::vec4 normal(normals1[i].x, normals1[i].y, normals1[i].z, 0.0f);
	normal = entity1TransformMatrix * normal; // Transform the vertices
	normal = glm::normalize(normal);

	Vector axis(normal.x, normal.y, normal.z);
	axes.push_back(axis);
}

for(unsigned int i = 0; i < normals2.size(); i++) {
	glm::vec4 normal(normals2[i].x, normals2[i].y, normals2[i].z, 0.0f);
	normal = entity2TransformMatrix * normal; // Transform the vertices
	normal = glm::normalize(normal);

	Vector axis(normal.x, normal.y, normal.z);
	axes.push_back(axis);
}

Oh! You are using 0 as the 4th component of your vectors, which makes it ignore the translation component of the transformation matrices, and makes sense for transforming normals/directions. But for transforming actual vertex positions (near the end of your code), you need to set the 4th component to 1.

Well derp... I'll try modifying that and seeing what happens! Thanks

d07RiV is correct.

I would instead keep a 3x3 rotation matrix inside the transform class and rotate the normals using it. This is much more faster and you don't need to normalize the normals (in both ways).

Test a box-box collision since it is easy to inspect the face and edge normals.

Hurrah! Changing the vertex vec4 to have a 1.0f at the end has made it all magically work (at least at first inspection - need to run a full set of tests on it).

With regards to the 3x3 rotation matrix, the matrix kept in the Transform is for the entity and used in rendering so I need a full Translation * Rotation * Scale matrix. However, it might be one of the optimizations I look at doing.

Thanks very much all!