|
public GetVertex ( int index ) : |
||
| index | int | |
| Результат | ||
// float FindMinSeparation(int* indexA, int* indexB, float t) const
public float FindMinSeparation(int[] indexes, float t)
{
SweepA.GetTransform(xfa, t);
SweepB.GetTransform(xfb, t);
switch (Type)
{
case Type.Points:
{
Rot.MulTransUnsafe(xfa.Q, Axis, axisA);
Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB);
Axis.NegateLocal();
indexes[0] = ProxyA.GetSupport(axisA);
indexes[1] = ProxyB.GetSupport(axisB);
localPointA.Set(ProxyA.GetVertex(indexes[0]));
localPointB.Set(ProxyB.GetVertex(indexes[1]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis);
return(separation);
}
case Type.FaceA:
{
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB);
normal.NegateLocal();
indexes[0] = -1;
indexes[1] = ProxyB.GetSupport(axisB);
localPointB.Set(ProxyB.GetVertex(indexes[1]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
float separation = Vec2.Dot(pointB.SubLocal(pointA), normal);
return(separation);
}
case Type.FaceB:
{
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA);
normal.NegateLocal();
indexes[1] = -1;
indexes[0] = ProxyA.GetSupport(axisA);
localPointA.Set(ProxyA.GetVertex(indexes[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
float separation = Vec2.Dot(pointA.SubLocal(pointB), normal);
return(separation);
}
default:
Debug.Assert(false);
indexes[0] = -1;
indexes[1] = -1;
return(0f);
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
{
ProxyA = proxyA;
ProxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
SweepA = sweepA;
SweepB = sweepB;
SweepA.GetTransform(xfa, t1);
SweepB.GetTransform(xfb, t1);
// log.debug("initializing separation.\n" +
// "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
// "distance: "+proxyA.
if (count == 1)
{
Type = Type.Points;
/*
* Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
* m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
* pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
*/
localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
Axis.Set(pointB).SubLocal(pointA);
float s = Axis.Normalize();
return s;
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
Type = Type.FaceB;
localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));
temp.Set(localPointB2).SubLocal(localPointB1);
Vec2.CrossToOutUnsafe(temp, 1f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
temp.Set(pointA).SubLocal(pointB);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
Type = Type.FaceA;
localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));
temp.Set(localPointA2).SubLocal(localPointA1);
Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
temp.Set(pointB).SubLocal(pointA);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return s;
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
{
ProxyA = proxyA;
ProxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
SweepA = sweepA;
SweepB = sweepB;
SweepA.GetTransform(xfa, t1);
SweepB.GetTransform(xfb, t1);
// log.debug("initializing separation.\n" +
// "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
// "distance: "+proxyA.
if (count == 1)
{
Type = Type.Points;
/*
* Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
* m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
* pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
*/
localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
Axis.Set(pointB).SubLocal(pointA);
float s = Axis.Normalize();
return(s);
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
Type = Type.FaceB;
localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));
temp.Set(localPointB2).SubLocal(localPointB1);
Vec2.CrossToOutUnsafe(temp, 1f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
temp.Set(pointA).SubLocal(pointB);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
Type = Type.FaceA;
localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));
temp.Set(localPointA2).SubLocal(localPointA1);
Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
temp.Set(pointB).SubLocal(pointA);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return(s);
}
}
