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; }
public static void transformAabb(VInt3 halfExtents, VFixedPoint margin, VIntTransform trans, out VInt3 aabbMinOut, out VInt3 aabbMaxOut) { VInt3 halfExtentsWithMargin = halfExtents; halfExtentsWithMargin.x += margin; halfExtentsWithMargin.y += margin; halfExtentsWithMargin.z += margin; VInt3 center = trans.position; VInt3[] basis = trans.getTransposeBasis(); VInt3 extent = new VInt3(VInt3.Dot(basis[0].Abs(), halfExtents), VInt3.Dot(basis[1].Abs(), halfExtents), VInt3.Dot(basis[2].Abs(), halfExtents)); aabbMinOut = center - extent; aabbMaxOut = center + extent; }
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax) { VInt3 halfExtents = VInt3.zero; halfExtents[upAxis] = getHalfHeight(); 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 - VInt3.one * getRadius(); aabbMax = center + extent + VInt3.one * getRadius(); }
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; } }