static bool expandSegment(CollisionShape a, CollisionShape b, VIntTransform transformA, VIntTransform transformB, ref int numVerts, VFixedPoint lowerBound, VFixedPoint upperBound)
{
VInt3 q0 = aBuf[0] - bBuf[0];
VInt3 q1 = aBuf[1] - bBuf[1];
VInt3 v = q1 - q0;
VInt3 absV = v.Abs();
VFixedPoint x = absV.x, y = absV.y, z = absV.z;
VInt3 axis = VInt3.right;
if (x > y && z > y)
{
axis = VInt3.up;
}
else if (x > z)
{
axis = VInt3.forward;
}
VInt3 n = VInt3.Cross(axis, v).Normalize();
VInt3 q2;
doSupport(a, b, transformA, transformB, n, out aBuf[2], out bBuf[2], out q2);
return(expandTriangle(a, b, transformA, transformB, ref numVerts, lowerBound, upperBound));
}
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax)
{
VInt3 halfExtents = p0.Abs();
VInt3[] basis = t.getTransposeBasis();
VInt3 center = t.position;
VInt3 extent = new VInt3();
extent.x = VInt3.Dot(halfExtents, basis[0].Abs());
extent.y = VInt3.Dot(halfExtents, basis[1].Abs());
extent.z = VInt3.Dot(halfExtents, basis[2].Abs());
aabbMin = center - extent;
aabbMax = center + extent;
}
static void doBoxBoxGenerateContacts(VInt3 box0Extent, VInt3 box1Extent, VIntTransform transform0, VIntTransform transform1, PersistentManifold resultOut)
{
VFixedPoint ea0 = box0Extent.x, ea1 = box0Extent.y, ea2 = box0Extent.z;
VFixedPoint eb0 = box1Extent.x, eb1 = box1Extent.y, eb2 = box1Extent.z;
VIntTransform transform1To0 = transform0.Transform(transform1);
VInt3 position1To0 = transform1To0.position;
VFixedPoint tx = position1To0.x;
VFixedPoint ty = position1To0.y;
VFixedPoint tz = position1To0.z;
VInt3 col0 = transform1To0.right;
VInt3 col1 = transform1To0.up;
VInt3 col2 = transform1To0.forward;
VInt3 abs1To0Col0 = col0.Abs() + VInt3.one * Globals.EPS;
VInt3 abs1To0Col1 = col1.Abs() + VInt3.one * Globals.EPS;
VInt3 abs1To0Col2 = col2.Abs() + VInt3.one * Globals.EPS;
VInt3[] transBasis = transform1To0.getTransposeBasis();
VInt3 abs0To1Col0 = transBasis[0].Abs() + VInt3.one * Globals.EPS;
VInt3 abs0To1Col1 = transBasis[1].Abs() + VInt3.one * Globals.EPS;
VInt3 abs0To1Col2 = transBasis[2].Abs() + VInt3.one * Globals.EPS;
VFixedPoint[] sign = new VFixedPoint[6];
VFixedPoint[] overlap = new VFixedPoint[6];
VFixedPoint ra = VFixedPoint.Zero, rb = VFixedPoint.Zero, radiusSum = VFixedPoint.Zero;
//ua0
{
sign[0] = tx;
rb = VInt3.Dot(abs0To1Col0, box1Extent);
radiusSum = ea0 + rb;
overlap[0] = radiusSum - sign[0].Abs();
if (overlap[0] < VFixedPoint.Zero)
{
return;
}
}
//ua1
{
sign[1] = ty;
rb = VInt3.Dot(abs0To1Col1, box1Extent);
radiusSum = ea1 + rb;
overlap[1] = radiusSum - sign[1].Abs();
if (overlap[1] < VFixedPoint.Zero)
{
return;
}
}
//ua2
{
sign[2] = tz;
rb = VInt3.Dot(abs0To1Col2, box1Extent);
radiusSum = ea2 + rb;
overlap[2] = radiusSum - sign[2].Abs();
if (overlap[2] < VFixedPoint.Zero)
{
return;
}
}
//ub0
{
sign[3] = VInt3.Dot(position1To0, col0);
ra = VInt3.Dot(abs1To0Col0, box0Extent);
radiusSum = ra + eb0;
overlap[3] = radiusSum - sign[3].Abs();
if (overlap[3] < VFixedPoint.Zero)
{
return;
}
}
//ub1
{
sign[4] = VInt3.Dot(position1To0, col1);
ra = VInt3.Dot(abs1To0Col1, box0Extent);
radiusSum = ra + eb1;
overlap[4] = radiusSum - sign[4].Abs();
if (overlap[4] < VFixedPoint.Zero)
{
return;
}
}
//ub2
{
sign[5] = VInt3.Dot(position1To0, col2);
ra = VInt3.Dot(abs1To0Col2, box0Extent);
radiusSum = ra + eb2;
overlap[5] = radiusSum - sign[5].Abs();
if (overlap[5] < VFixedPoint.Zero)
{
return;
}
}
//ua0 x ub0
{
VFixedPoint absSign = (col0.y * tz - col0.z * ty).Abs();
VFixedPoint vtemp0 = abs1To0Col0.z * ea1;
VFixedPoint vtemp1 = abs1To0Col0.y * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col0.z * eb1;
VFixedPoint vtemp02 = abs0To1Col0.y * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua0 x ub1
{
VFixedPoint absSign = (col1.y * tz - col1.z * ty).Abs();
VFixedPoint vtemp0 = abs1To0Col1.z * ea1;
VFixedPoint vtemp1 = abs1To0Col1.y * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col0.z * eb0;
VFixedPoint vtemp02 = abs0To1Col0.x * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua0 x ub2
{
VFixedPoint absSign = (col2.y * tz - col2.z * ty).Abs();
VFixedPoint vtemp0 = abs1To0Col2.z * ea1;
VFixedPoint vtemp1 = abs1To0Col2.y * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col0.y * eb0;
VFixedPoint vtemp02 = abs0To1Col0.x * eb1;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua1 x ub0
{
VFixedPoint absSign = (col0.z * tx - col0.x * tz).Abs();
VFixedPoint vtemp0 = abs1To0Col0.z * ea0;
VFixedPoint vtemp1 = abs1To0Col0.x * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col1.z * eb1;
VFixedPoint vtemp02 = abs0To1Col1.y * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua1 x ub1
{
VFixedPoint absSign = (col1.z * tx - col1.x * tz).Abs();
VFixedPoint vtemp0 = abs1To0Col1.z * ea0;
VFixedPoint vtemp1 = abs1To0Col1.x * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col1.z * eb0;
VFixedPoint vtemp02 = abs0To1Col1.x * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua1 x ub2
{
VFixedPoint absSign = (col2.z * tx - col2.x * tz).Abs();
VFixedPoint vtemp0 = abs1To0Col2.z * ea0;
VFixedPoint vtemp1 = abs1To0Col2.x * ea2;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col1.y * eb0;
VFixedPoint vtemp02 = abs0To1Col1.x * eb1;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua2 x ub0
{
VFixedPoint absSign = (col0.x * ty - col0.y * tx).Abs();
VFixedPoint vtemp0 = abs1To0Col0.y * ea0;
VFixedPoint vtemp1 = abs1To0Col0.x * ea1;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col2.z * eb1;
VFixedPoint vtemp02 = abs0To1Col2.y * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua2 x ub1
{
VFixedPoint absSign = (col1.x * ty - col1.y * tx).Abs();
VFixedPoint vtemp0 = abs1To0Col1.y * ea0;
VFixedPoint vtemp1 = abs1To0Col1.x * ea1;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col2.z * eb0;
VFixedPoint vtemp02 = abs0To1Col2.x * eb2;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
//ua2 x ub2
{
VFixedPoint absSign = (col2.x * ty - col2.y * tx).Abs();
VFixedPoint vtemp0 = abs1To0Col2.y * ea0;
VFixedPoint vtemp1 = abs1To0Col2.x * ea1;
ra = vtemp0 + vtemp1;
VFixedPoint vtemp01 = abs0To1Col2.y * eb0;
VFixedPoint vtemp02 = abs0To1Col2.x * eb1;
rb = vtemp01 + vtemp02;
radiusSum = ra + rb;
if (absSign > radiusSum)
{
return;
}
}
VInt3 mtd = VInt3.zero;
int feature = 0;
VFixedPoint minOverlap = overlap[0];
for (int i = 1; i < 6; i++)
{
if (minOverlap > overlap[i])
{
feature = i;
minOverlap = overlap[i];
}
}
VIntTransform newTransformV;
VInt3 axis00 = transform0.right;
VInt3 axis01 = transform0.up;
VInt3 axis02 = transform0.forward;
VInt3 axis10 = transform1.right;
VInt3 axis11 = transform1.up;
VInt3 axis12 = transform1.forward;
VInt3 incidentFaceNormalInNew = VInt3.zero;
VInt3[] pts = new VInt3[4];
switch (feature)
{
case 0:
{
if (sign[0] <= VFixedPoint.Zero)
{
mtd = axis00;
newTransformV = new VIntTransform((transform0.position - axis00 * ea0), -axis02, axis01, axis00);
}
else
{
VInt3 nAxis00 = -axis00;
mtd = nAxis00;
newTransformV = new VIntTransform((transform0.position + axis00 * ea0), axis02, axis01, nAxis00);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform1);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, -localNormal, transform1ToNew, box1Extent);
calculateContacts(ea2, ea1, ea0, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, false);
break;
}
case 1:
{
if (sign[1] <= VFixedPoint.Zero)
{
mtd = axis01;
newTransformV = new VIntTransform((transform0.position - axis01 * ea1), axis00, -axis02, axis01);
}
else
{
VInt3 nAxis01 = -axis01;
mtd = nAxis01;
newTransformV = new VIntTransform((transform0.position + axis01 * ea1), axis00, axis02, nAxis01);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform1);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, -localNormal, transform1ToNew, box1Extent);
calculateContacts(ea0, ea2, ea1, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, false);
break;
}
case 2:
{
if (sign[2] <= VFixedPoint.Zero)
{
mtd = axis02;
newTransformV = new VIntTransform((transform0.position - axis02 * ea2), axis00, axis01, axis02);
}
else
{
VInt3 nAxis02 = -axis02;
mtd = nAxis02;
newTransformV = new VIntTransform((transform0.position + axis02 * ea2), axis00, -axis01, nAxis02);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform1);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, -localNormal, transform1ToNew, box1Extent);
calculateContacts(ea0, ea1, ea2, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, false);
break;
}
case 3:
{
if (sign[3] <= VFixedPoint.Zero)
{
mtd = axis10;
newTransformV = new VIntTransform((transform1.position + axis10 * eb0), axis12, axis11, -axis10);
}
else
{
mtd = -axis10;
newTransformV = new VIntTransform((transform1.position - axis10 * eb0), -axis12, axis11, axis10);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform0);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
calculateContacts(eb2, eb1, eb0, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, true);
break;
}
case 4:
{
if (sign[4] <= VFixedPoint.Zero)
{
mtd = axis11;
newTransformV = new VIntTransform((transform1.position + axis11 * eb1), axis10, axis12, -axis11);
}
else
{
mtd = -axis11;
newTransformV = new VIntTransform((transform1.position - axis11 * eb1), axis10, -axis12, axis11);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform0);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
calculateContacts(eb0, eb2, eb1, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, true);
break;
}
case 5:
{
if (sign[5] <= VFixedPoint.Zero)
{
mtd = axis12;
newTransformV = new VIntTransform((transform1.position + axis12 * eb2), axis10, -axis11, -axis12);
}
else
{
mtd = -axis12;
newTransformV = new VIntTransform((transform1.position - axis12 * eb2), axis10, axis11, axis12);
}
VIntTransform transform1ToNew = newTransformV.Transform(transform0);
VInt3 localNormal = newTransformV.InverseTransformDirection(mtd);
getIncidentPolygon(ref pts, ref incidentFaceNormalInNew, localNormal, transform1ToNew, box0Extent);
calculateContacts(eb0, eb1, eb2, pts, incidentFaceNormalInNew, localNormal, Globals.EPS, newTransformV, resultOut, true);
break;
}
default:
return;
}
}