public void rayTestInternal(Node root, Dispatcher dispatcher, VInt3 rayFrom, VInt3 rayTo, VInt3 aabbMin, VInt3 aabbMax, short collisionFilterGroup, short collisionFilterMask, ICollide policy)
{
if (root != null)
{
List <Node> stack = new List <Node>(DOUBLE_STACKSIZE);
stack.Add(root);
VInt3[] bounds = new VInt3[2];
do
{
Node node = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
VFixedPoint tmin = VFixedPoint.One, tmax = VFixedPoint.One;
bool result1 = AabbUtils.RayAabb2(rayFrom, rayTo, node.volume.Mins() - aabbMax, node.volume.Maxs() - aabbMin, ref tmin, ref tmax);
if (result1)
{
if (node.isinternal())
{
stack.Add(node.childs[0]);
stack.Add(node.childs[1]);
}
else
{
if (!dispatcher.needsCollision(collisionFilterGroup, collisionFilterMask, node.data.collisionFilterGroup, node.data.collisionFilterMask))
{
continue;
}
policy.Process(node);
}
}
} while (stack.Count > 0);
}
}
public static bool rayTestBox(VInt3 fromPos, VInt3 toPos, VInt3 boxHalfExtent, VIntTransform boxTransform, ref VFixedPoint t, ref VInt3 normal)
{
VInt3 rayFromPointLocal = boxTransform.InverseTransformPoint(fromPos);
VInt3 rayToPointLocal = boxTransform.InverseTransformPoint(toPos);
t = VFixedPoint.One;
VInt3 normalInLocal = VInt3.zero;
if (AabbUtils.RayAabb(rayFromPointLocal, rayToPointLocal, -boxHalfExtent, boxHalfExtent, ref t, ref normalInLocal))
{
normal = -boxTransform.TransformDirection(normalInLocal);
return(true);
}
return(false);
}
static void calculateContacts(VFixedPoint extentX, VFixedPoint extentY, VFixedPoint extentZ, VInt3[] pts,
VInt3 incidentfaceNormalInNew, VInt3 localNormal, VFixedPoint contactDist, VIntTransform transformNew,
PersistentManifold resultOut, bool flip)
{
localNormal = transformNew.TransformDirection(localNormal);
VFixedPoint nExtentX = -extentX;
VFixedPoint nExtentY = -extentY;
bool[] penetration = new bool[4];
bool[] area = new bool[4];
VInt3 bmin = new VInt3(VFixedPoint.MaxValue, VFixedPoint.MaxValue, VFixedPoint.MaxValue);
VInt3 bmax = new VInt3(VFixedPoint.MinValue, VFixedPoint.MinValue, VFixedPoint.MinValue);
VInt3 bound = new VInt3(extentX, extentY, VFixedPoint.MaxValue);
for (int i = 0; i < 4; i++)
{
bmin = VInt3.Min(bmin, pts[i]);
bmax = VInt3.Max(bmax, pts[i]);
VFixedPoint z = -pts[i].z;
if (contactDist > z)
{
penetration[i] = true;
VInt3 absPt = pts[i].Abs();
bool con = bound >= absPt;
if (con)
{
area[i] = true;
VInt3 localPointA = pts[i]; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = pts[i]; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, z);
resultOut.addManifoldPoint(contactPoint);
}
else
{
area[i] = false;
}
}
else
{
penetration[i] = false;
area[i] = false;
}
}
if (resultOut.getContactPointsNum() == PersistentManifold.MANIFOLD_CACHE_SIZE)
{
return;
}
{
{
VFixedPoint denom = incidentfaceNormalInNew.z;
{
VInt3 q0 = new VInt3(extentX, extentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
{
VInt3 q0 = new VInt3(extentX, nExtentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
{
VInt3 q0 = new VInt3(nExtentX, extentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
{
VInt3 q0 = new VInt3(nExtentX, nExtentY, VFixedPoint.Zero);
if (contains(pts, q0, bmin, bmax))
{
VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0);
VFixedPoint t = nom / denom;
VFixedPoint pen = -t;
if (contactDist > pen)
{
VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen);
resultOut.addManifoldPoint(contactPoint);
}
}
}
}
}
VInt3 ext = new VInt3(extentX, extentY, extentZ * VFixedPoint.Two);
VInt3 negExt = new VInt3(nExtentX, nExtentY, -(contactDist + Globals.EPS));
for (int start = 0, end = 3; start < 4; end = start++)
{
VInt3 p0 = pts[start];
VInt3 p1 = pts[end];
if (!penetration[start] && !penetration[end])
{
continue;
}
bool con0 = penetration[start] && area[start];
bool con1 = penetration[end] && area[end];
if (con0 && con1)
{
continue;
}
VFixedPoint tmin = VFixedPoint.Zero, tmax = VFixedPoint.Zero;
if (AabbUtils.RayAabb2(p0, p1, negExt, ext, ref tmin, ref tmax))
{
if (!con0 && tmin > VFixedPoint.Zero)
{
VInt3 intersectP = p0 * (VFixedPoint.One - tmin) + p1 * tmin;
VInt3 localPointA = intersectP; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = intersectP; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, -intersectP.z);
resultOut.addManifoldPoint(contactPoint);
}
if (!con1 && tmax < VFixedPoint.One)
{
VInt3 intersectP = p0 * (VFixedPoint.One - tmax) + p1 * tmax;
VInt3 localPointA = intersectP; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA);
VInt3 localPointB = intersectP; localPointB = transformNew.TransformPoint(localPointB);
ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, -intersectP.z);
resultOut.addManifoldPoint(contactPoint);
}
}
}
}
public override void getAabb(VIntTransform trans, out VInt3 aabbMin, out VInt3 aabbMax)
{
AabbUtils.transformAabb(getHalfExtent(), getMargin(), trans, out aabbMin, out aabbMax);
}
