public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
Matrix wrotationB, Vector3 positionB, ConvexShape shapeB,
float pmargin)
{
for (int i = 0; i < _simplex.Length; i++)
{
_simplex[i] = new MinkowskiVertice();
}
for (int i = 0; i < _wrotations.Length; i++)
{
_wrotations[i] = new Matrix();
}
for (int i = 0; i < _positions.Length; i++)
{
_positions[i] = new Vector3();
}
_wrotations[0] = wrotationA; _positions[0] = positionA;
_shapes[0] = shapeA;
_wrotations[0].Translation = Vector3.Zero;
_wrotations[1] = wrotationB; _positions[1] = positionB;
_shapes[1] = shapeB;
_wrotations[1].Translation = Vector3.Zero;
//sablock = sa->BeginBlock();
_margin = pmargin;
_failed = false;
}
public void Support(Vector3 d, ref MinkowskiVertice v)
{
v.Ray = d;
v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin;
}