Exemple #1
0
 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);
     }
 }
Exemple #2
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);
        }
Exemple #3
0
        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);
                    }
                }
            }
        }
Exemple #4
0
 public override void getAabb(VIntTransform trans, out VInt3 aabbMin, out VInt3 aabbMax)
 {
     AabbUtils.transformAabb(getHalfExtent(), getMargin(), trans, out aabbMin, out aabbMax);
 }