public void WriteCache(ref b2SimplexCache cache)
{
cache.metric = GetMetric();
cache.count = (ushort)m_count;
b2SimplexVertex[] vertices = { m_v1, m_v2, m_v3 };
for (int i = 0; i < m_count; ++i)
{
cache.indexA[i] = (byte)(vertices[i].indexA);
cache.indexB[i] = (byte)(vertices[i].indexB);
}
}
protected override void Draw(Settings settings)
{
base.Draw(settings);
b2DistanceInput input = b2DistanceInput.Create();
input.proxyA.Set(m_polygonA, 0);
input.proxyB.Set(m_polygonB, 0);
input.transformA = m_transformA;
input.transformB = m_transformB;
input.useRadii = true;
b2SimplexCache cache = b2SimplexCache.Create();
cache.count = 0;
b2DistanceOutput output = new b2DistanceOutput();
b2Simplex.b2Distance(ref output, ref cache, ref input);
m_debugDraw.DrawString(5, m_textLine, "distance = {0}", output.distance);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "iterations = {0}", output.iterations);
m_textLine += 15;
{
b2Color color = new b2Color(0.9f, 0.9f, 0.9f);
b2Vec2[] v = new b2Vec2[b2Settings.b2_maxPolygonVertices];
for (int i = 0; i < m_polygonA.VertexCount; ++i)
{
v[i] = b2Math.b2Mul(m_transformA, m_polygonA.Vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.VertexCount, color);
for (int i = 0; i < m_polygonB.VertexCount; ++i)
{
v[i] = b2Math.b2Mul(m_transformB, m_polygonB.Vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.VertexCount, color);
}
b2Vec2 x1 = output.pointA;
b2Vec2 x2 = output.pointB;
b2Color c1 = new b2Color(1.0f, 0.0f, 0.0f);
m_debugDraw.DrawPoint(x1, 4.0f, c1);
b2Color c2 = new b2Color(1.0f, 1.0f, 0.0f);
m_debugDraw.DrawPoint(x2, 4.0f, c2);
}
public void ReadCache(b2SimplexCache cache, b2DistanceProxy proxyA, b2Transform transformA, b2DistanceProxy proxyB, b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
b2SimplexVertex[] vertices = { m_v1, m_v2, m_v3 };
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
b2Vec2 wALocal = proxyA.GetVertex(v.indexA);
b2Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = Utils.b2Mul(transformA, wALocal);
v.wB = Utils.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.GetVertex(0);
b2Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = Utils.b2Mul(transformA, wALocal);
v.wB = Utils.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
public static bool TestOverlap(b2Shape shape1, b2Transform transform1, b2Shape shape2, b2Transform transform2)
{
b2DistanceInput input = new b2DistanceInput();
input.proxyA = new b2DistanceProxy();
input.proxyA.Set(shape1);
input.proxyB = new b2DistanceProxy();
input.proxyB.Set(shape2);
input.transformA = transform1;
input.transformB = transform2;
input.useRadii = true;
b2SimplexCache simplexCache = new b2SimplexCache();
simplexCache.count = 0;
b2DistanceOutput output = new b2DistanceOutput();
b2Distance.Distance(output, simplexCache, input);
//return output.distance < 10.0f * float.MinValue;
//改
return output.distance < 1e-10f;
}
public static void b2Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input)
{
Box2dPINVOKE.b2Distance__SWIG_1(b2DistanceOutput.getCPtr(output), b2SimplexCache.getCPtr(cache), b2DistanceInput.getCPtr(input));
}
public void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < m_bodyCount);
Debug.Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
m_positions[i].c = b.Sweep.c;
m_positions[i].a = b.Sweep.a;
m_velocities[i].v = b.LinearVelocity;
m_velocities[i].w = b.AngularVelocity;
}
b2ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
// Solve position constraints.
for (int i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if false
// Is the new position really safe?
for (int i = 0; i < m_contactCount; ++i)
{
b2Contact c = m_contacts[i];
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
b2DistanceInput input = new b2DistanceInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.transformA = bA.Transform;
input.transformB = bB.Transform;
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache = new b2SimplexCache();
cache.count = 0;
output = b2Distance(cache, input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB].Sweep.a0 = m_positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.dt;
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length;
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
b2Body body = m_bodies[i];
body.Sweep.c = c;
body.Sweep.a = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(contactSolver.m_velocityConstraints);
}
// TODO_ERIN might not need to return the separation
public float Initialize(b2SimplexCache cache, b2DistanceProxy proxyA, b2Sweep sweepA, b2DistanceProxy proxyB, b2Sweep sweepB, float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA = new b2Transform();
b2Transform xfB = new b2Transform();
m_sweepA.GetTransform(ref xfA, t1);
m_sweepB.GetTransform(ref xfB, t1);
if (count == 1)
{
m_type = Type.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointA = Utils.b2Mul(xfA, localPointA);
b2Vec2 pointB = Utils.b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float s = m_axis.Normalize();
return(s);
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = Type.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utils.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = Utils.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = Utils.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
b2Vec2 pointA = Utils.b2Mul(xfA, localPointA);
float s = Utils.b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
m_type = Type.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utils.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = Utils.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = Utils.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = Utils.b2Mul(xfB, localPointB);
float s = Utils.b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2SimplexCache obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
