Initial Mercurial commit.

src/modules/physics/box2d/Source/Collision/b2CollideCircle.cpp

@@ -0,0 +1,168 @@
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Collision.h"
+#include "Shapes/b2CircleShape.h"
+#include "Shapes/b2PolygonShape.h"
+
+void b2CollideCircles(
+	b2Manifold* manifold,
+	const b2CircleShape* circle1, const b2XForm& xf1,
+	const b2CircleShape* circle2, const b2XForm& xf2)
+{
+	manifold->pointCount = 0;
+
+	b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
+	b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
+
+	b2Vec2 d = p2 - p1;
+	float32 distSqr = b2Dot(d, d);
+	float32 r1 = circle1->GetRadius();
+	float32 r2 = circle2->GetRadius();
+	float32 radiusSum = r1 + r2;
+	if (distSqr > radiusSum * radiusSum)
+	{
+		return;
+	}
+
+	float32 separation;
+	if (distSqr < B2_FLT_EPSILON)
+	{
+		separation = -radiusSum;
+		manifold->normal.Set(0.0f, 1.0f);
+	}
+	else
+	{
+		float32 dist = b2Sqrt(distSqr);
+		separation = dist - radiusSum;
+		float32 a = 1.0f / dist;
+		manifold->normal.x = a * d.x;
+		manifold->normal.y = a * d.y;
+	}
+
+	manifold->pointCount = 1;
+	manifold->points[0].id.key = 0;
+	manifold->points[0].separation = separation;
+
+	p1 += r1 * manifold->normal;
+	p2 -= r2 * manifold->normal;
+
+	b2Vec2 p = 0.5f * (p1 + p2);
+
+	manifold->points[0].localPoint1 = b2MulT(xf1, p);
+	manifold->points[0].localPoint2 = b2MulT(xf2, p);
+}
+
+void b2CollidePolygonAndCircle(
+	b2Manifold* manifold,
+	const b2PolygonShape* polygon, const b2XForm& xf1,
+	const b2CircleShape* circle, const b2XForm& xf2)
+{
+	manifold->pointCount = 0;
+
+	// Compute circle position in the frame of the polygon.
+	b2Vec2 c = b2Mul(xf2, circle->GetLocalPosition());
+	b2Vec2 cLocal = b2MulT(xf1, c);
+
+	// Find the min separating edge.
+	int32 normalIndex = 0;
+	float32 separation = -B2_FLT_MAX;
+	float32 radius = circle->GetRadius();
+	int32 vertexCount = polygon->GetVertexCount();
+	const b2Vec2* vertices = polygon->GetVertices();
+	const b2Vec2* normals = polygon->GetNormals();
+
+	for (int32 i = 0; i < vertexCount; ++i)
+	{
+		float32 s = b2Dot(normals[i], cLocal - vertices[i]);
+
+		if (s > radius)
+		{
+			// Early out.
+			return;
+		}
+
+		if (s > separation)
+		{
+			separation = s;
+			normalIndex = i;
+		}
+	}
+
+	// If the center is inside the polygon ...
+	if (separation < B2_FLT_EPSILON)
+	{
+		manifold->pointCount = 1;
+		manifold->normal = b2Mul(xf1.R, normals[normalIndex]);
+		manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
+		manifold->points[0].id.features.incidentVertex = b2_nullFeature;
+		manifold->points[0].id.features.referenceEdge = 0;
+		manifold->points[0].id.features.flip = 0;
+		b2Vec2 position = c - radius * manifold->normal;
+		manifold->points[0].localPoint1 = b2MulT(xf1, position);
+		manifold->points[0].localPoint2 = b2MulT(xf2, position);
+		manifold->points[0].separation = separation - radius;
+		return;
+	}
+
+	// Project the circle center onto the edge segment.
+	int32 vertIndex1 = normalIndex;
+	int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
+	b2Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
+
+	float32 length = e.Normalize();
+	b2Assert(length > B2_FLT_EPSILON);
+
+	// Project the center onto the edge.
+	float32 u = b2Dot(cLocal - vertices[vertIndex1], e);
+	b2Vec2 p;
+	if (u <= 0.0f)
+	{
+		p = vertices[vertIndex1];
+		manifold->points[0].id.features.incidentEdge = b2_nullFeature;
+		manifold->points[0].id.features.incidentVertex = (uint8)vertIndex1;
+	}
+	else if (u >= length)
+	{
+		p = vertices[vertIndex2];
+		manifold->points[0].id.features.incidentEdge = b2_nullFeature;
+		manifold->points[0].id.features.incidentVertex = (uint8)vertIndex2;
+	}
+	else
+	{
+		p = vertices[vertIndex1] + u * e;
+		manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
+		manifold->points[0].id.features.incidentVertex = 0;
+	}
+
+	b2Vec2 d = cLocal - p;
+	float32 dist = d.Normalize();
+	if (dist > radius)
+	{
+		return;
+	}
+
+	manifold->pointCount = 1;
+	manifold->normal = b2Mul(xf1.R, d);
+	b2Vec2 position = c - radius * manifold->normal;
+	manifold->points[0].localPoint1 = b2MulT(xf1, position);
+	manifold->points[0].localPoint2 = b2MulT(xf2, position);
+	manifold->points[0].separation = dist - radius;
+	manifold->points[0].id.features.referenceEdge = 0;
+	manifold->points[0].id.features.flip = 0;
+}