コード例 #1
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            //resultOut = new ManifoldResult();
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject sphereObj = m_swapped ? body1 : body0;
            CollisionObject triObj    = m_swapped ? body0 : body1;

            SphereShape   sphere   = sphereObj.GetCollisionShape() as SphereShape;
            TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape;

            /// report a contact. internally this will be kept persistent, and contact reduction is done
            resultOut.SetPersistentManifold(m_manifoldPtr);
            using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get())
            {
                detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold());
                ClosestPointInput input = ClosestPointInput.Default();
                input.m_maximumDistanceSquared = float.MaxValue;
                sphereObj.GetWorldTransform(out input.m_transformA);
                triObj.GetWorldTransform(out input.m_transformB);

                bool swapResults = m_swapped;

                detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults);

                if (m_ownManifold)
                {
                    resultOut.RefreshContactPoints();
                }
            }
        }
コード例 #2
0
        public void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
        {
            IndexedMatrix transformA = input.m_transformA;
            IndexedMatrix transformB = input.m_transformB;

            IndexedVector3 point, normal;
            float          timeOfImpact = 1f;
            float          depth        = 0f;
            //	output.m_distance = float(1e30);
            //move sphere into triangle space
            IndexedMatrix sphereInTr = transformB.InverseTimes(ref transformA);

            IndexedVector3 temp = sphereInTr._origin;

            if (Collide(ref temp, out point, out normal, ref depth, ref timeOfImpact, m_contactBreakingThreshold))
            {
                if (swapResults)
                {
                    IndexedVector3 normalOnB = transformB._basis * normal;
                    IndexedVector3 normalOnA = -normalOnB;
                    IndexedVector3 pointOnA  = transformB * point + normalOnB * depth;
                    output.AddContactPoint(ref normalOnA, ref pointOnA, depth);
                }
                else
                {
                    IndexedVector3 p  = transformB._basis * normal;
                    IndexedVector3 p2 = transformB * point;
                    output.AddContactPoint(ref p, ref p2, depth);
                }
            }
        }
コード例 #3
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.getPersistentManifold().clearManifold();


            ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;

            IndexedVector3 normalOnB     = IndexedVector3.Zero;
            IndexedVector3 pointOnBWorld = IndexedVector3.Zero;

            {
                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
                {
                    gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
                    //TODO: if (dispatchInfo.m_useContinuous)
                    gjkPairDetector.SetMinkowskiA(min0);
                    gjkPairDetector.SetMinkowskiB(min1);

                    {
                        input.m_maximumDistanceSquared  = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();

                    gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
                    if (BulletGlobals.g_streamWriter != null)
                    {
                        BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
                    }
#endif
                }
                //BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
                //btVector3 v0,v1;
                //btVector3 sepNormalWorldSpace;
            }

            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
        }
コード例 #4
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject col0 = body0;
            CollisionObject col1 = body1;
            //resultOut = new ManifoldResult(body0, body1);
            BoxShape box0 = col0.GetCollisionShape() as BoxShape;
            BoxShape box1 = col1.GetCollisionShape() as BoxShape;

            //if (((String)col0.getUserPointer()).Contains("Box") &&
            //    ((String)col1.getUserPointer()).Contains("Box") )
            //{
            //    int ibreak = 0;
            //}
            /// report a contact. internally this will be kept persistent, and contact reduction is done
            resultOut.SetPersistentManifold(m_manifoldPtr);

#if !USE_PERSISTENT_CONTACTS
            m_manifoldPtr.ClearManifold();
#endif //USE_PERSISTENT_CONTACTS

            ClosestPointInput input = ClosestPointInput.Default();
            input.m_maximumDistanceSquared = float.MaxValue;
            input.m_transformA             = body0.GetWorldTransform();
            input.m_transformB             = body1.GetWorldTransform();

            BoxBoxDetector.GetClosestPoints(box0, box1, ref input, resultOut, dispatchInfo.getDebugDraw(), false);

#if USE_PERSISTENT_CONTACTS
            //  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
#endif //USE_PERSISTENT_CONTACTS
        }
コード例 #5
0
        public void ComputeClosestPoints(ref IndexedMatrix transA, ref IndexedMatrix transB, PointCollector pointCollector)
        {
            if (m_convexB1 != null)
            {
                m_simplexSolver.Reset();
                GjkPairDetector   gjk   = new GjkPairDetector(m_convexA, m_convexB1, m_convexA.GetShapeType(), m_convexB1.GetShapeType(), m_convexA.GetMargin(), m_convexB1.GetMargin(), m_simplexSolver, m_penetrationDepthSolver);
                ClosestPointInput input = ClosestPointInput.Default();
                input.m_transformA = transA;
                input.m_transformB = transB;
                gjk.GetClosestPoints(ref input, pointCollector, null);
            }
            else
            {
                //convex versus plane
                ConvexShape      convexShape = m_convexA;
                StaticPlaneShape planeShape  = m_planeShape;

                bool           hasCollision  = false;
                IndexedVector3 planeNormal   = planeShape.GetPlaneNormal();
                float          planeConstant = planeShape.GetPlaneConstant();

                IndexedMatrix convexWorldTransform = transA;
                IndexedMatrix convexInPlaneTrans   = transB.Inverse() * convexWorldTransform;
                IndexedMatrix planeInConvex        = convexWorldTransform.Inverse() * transB;

                IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);

                IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
                float          distance   = IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant;

                IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
                IndexedVector3 vtxInPlaneWorld     = transB * vtxInPlaneProjected;
                IndexedVector3 normalOnSurfaceB    = transB._basis * planeNormal;

                pointCollector.AddContactPoint(
                    ref normalOnSurfaceB,
                    ref vtxInPlaneWorld,
                    distance);
            }
        }
コード例 #6
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                //swapped?
                m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
                m_ownManifold = true;
            }
            //resultOut = new ManifoldResult();
            resultOut.SetPersistentManifold(m_manifoldPtr);

            //comment-out next line to test multi-contact generation
            //resultOut.GetPersistentManifold().ClearManifold();


            ConvexShape    min0 = body0.GetCollisionShape() as ConvexShape;
            ConvexShape    min1 = body1.GetCollisionShape() as ConvexShape;
            IndexedVector3 normalOnB;
            IndexedVector3 pointOnBWorld;

#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
            if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
            {
                CapsuleShape capsuleA = min0 as CapsuleShape;
                CapsuleShape capsuleB = min1 as CapsuleShape;
                //IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
                //IndexedVector3 localScalingB = capsuleB.GetLocalScaling();

                float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
                                                    capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
                                                    body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);

                if (dist < threshold)
                {
                    Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON));
                    resultOut.AddContactPoint(ref normalOnB, ref pointOnBWorld, dist);
                }
                resultOut.RefreshContactPoints();
                return;
            }
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER



#if USE_SEPDISTANCE_UTIL2
            if (dispatchInfo.m_useConvexConservativeDistanceUtil)
            {
                m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(), body1.getWorldTransform());
            }

            if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f)
#endif //USE_SEPDISTANCE_UTIL2

            {
                ClosestPointInput input = ClosestPointInput.Default();

                using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
                {
                    gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
                    //TODO: if (dispatchInfo.m_useContinuous)
                    gjkPairDetector.SetMinkowskiA(min0);
                    gjkPairDetector.SetMinkowskiB(min1);

#if USE_SEPDISTANCE_UTIL2
                    if (dispatchInfo.m_useConvexConservativeDistanceUtil)
                    {
                        input.m_maximumDistanceSquared = float.MaxValue;
                    }
                    else
#endif //USE_SEPDISTANCE_UTIL2
                    {
                        input.m_maximumDistanceSquared  = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
                        input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
                    }

                    //input.m_stackAlloc = dispatchInfo.m_stackAllocator;
                    input.m_transformA = body0.GetWorldTransform();
                    input.m_transformB = body1.GetWorldTransform();


                    if (min0.IsPolyhedral() && min1.IsPolyhedral())
                    {
                        DummyResult dummy = new DummyResult();


                        PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
                        PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
                        if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
                        {
                            float threshold = m_manifoldPtr.GetContactBreakingThreshold();

                            float          minDist             = float.MinValue;
                            IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
                            bool           foundSepAxis        = true;

                            if (dispatchInfo.m_enableSatConvex)
                            {
                                foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                    polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                    body0.GetWorldTransform(),
                                    body1.GetWorldTransform(),
                                    out sepNormalWorldSpace);
                            }
                            else
                            {
#if ZERO_MARGIN
                                gjkPairDetector.SetIgnoreMargin(true);
                                gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
                                gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif

                                float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                if (l2 > MathUtil.SIMD_EPSILON)
                                {
                                    sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                    //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
                                    minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();

#if ZERO_MARGIN
                                    foundSepAxis = true;    //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
                                    foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
                                }
                            }
                            if (foundSepAxis)
                            {
                                //				printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());

                                PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                                                              body0.GetWorldTransform(),
                                                                              body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
                            }
                            if (m_ownManifold)
                            {
                                resultOut.RefreshContactPoints();
                            }

                            return;
                        }
                        else
                        {
                            //we can also deal with convex versus triangle (without connectivity data)
                            if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
                            {
                                m_vertices.Clear();
                                TriangleShape tri = polyhedronB as TriangleShape;
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
                                m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);

                                float          threshold           = m_manifoldPtr.GetContactBreakingThreshold();
                                IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
                                float          minDist             = float.MinValue;
                                float          maxDist             = threshold;

                                bool foundSepAxis = false;
                                if (false)
                                {
                                    polyhedronB.InitializePolyhedralFeatures();
                                    foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
                                        polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
                                        body0.GetWorldTransform(),
                                        body1.GetWorldTransform(),
                                        out sepNormalWorldSpace);
                                    //	 printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
                                }
                                else
                                {
#if ZERO_MARGIN
                                    gjkPairDetector.SetIgnoreMargin(true);
                                    gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
                                    gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN

                                    float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
                                    if (l2 > MathUtil.SIMD_EPSILON)
                                    {
                                        sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
                                        //minDist = gjkPairDetector.getCachedSeparatingDistance();
                                        //maxDist = threshold;
                                        minDist      = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
                                        foundSepAxis = true;
                                    }
                                }


                                if (foundSepAxis)
                                {
                                    PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
                                                                                  body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
                                }

                                if (m_ownManifold)
                                {
                                    resultOut.RefreshContactPoints();
                                }
                                return;
                            }
                        }
                    }


                    gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
                    float sepDist = 0.f;
                    if (dispatchInfo.m_useConvexConservativeDistanceUtil)
                    {
                        sepDist = gjkPairDetector.getCachedSeparatingDistance();
                        if (sepDist > MathUtil.SIMD_EPSILON)
                        {
                            sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
                            //now perturbe directions to get multiple contact points
                        }
                    }
#endif //USE_SEPDISTANCE_UTIL2

                    //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

                    //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
                    if (m_numPerturbationIterations > 0 && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
                    {
                        IndexedVector3 v0, v1;

                        IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
                        sepNormalWorldSpace.Normalize();
                        TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);

                        bool        perturbeA  = true;
                        const float angleLimit = 0.125f * MathUtil.SIMD_PI;
                        float       perturbeAngle;
                        float       radiusA = min0.GetAngularMotionDisc();
                        float       radiusB = min1.GetAngularMotionDisc();
                        if (radiusA < radiusB)
                        {
                            perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
                            perturbeA     = true;
                        }
                        else
                        {
                            perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
                            perturbeA     = false;
                        }
                        if (perturbeAngle > angleLimit)
                        {
                            perturbeAngle = angleLimit;
                        }

                        IndexedMatrix unPerturbedTransform;
                        if (perturbeA)
                        {
                            unPerturbedTransform = input.m_transformA;
                        }
                        else
                        {
                            unPerturbedTransform = input.m_transformB;
                        }

                        for (int i = 0; i < m_numPerturbationIterations; i++)
                        {
                            if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
                            {
                                IndexedQuaternion perturbeRot    = new IndexedQuaternion(v0, perturbeAngle);
                                float             iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                                IndexedQuaternion rotq           = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);

                                if (perturbeA)
                                {
                                    input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
                                    input.m_transformB        = body1.GetWorldTransform();

                                    input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
                                    dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
                                }
                                else
                                {
                                    input.m_transformA        = body0.GetWorldTransform();
                                    input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
                                    dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
                                }

                                PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
                                gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
                            }
                        }
                    }



#if USE_SEPDISTANCE_UTIL2
                    if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
                    {
                        m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
                    }
#endif //USE_SEPDISTANCE_UTIL2
                }
            }

            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
        }
コード例 #7
0
 public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
 {
     GetClosestPoints(ref input, output, debugDraw, false);
 }
コード例 #8
0
        public void GetClosestPointsNonVirtual(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw)
        {
            m_cachedSeparatingDistance = 0f;

            float          distance = 0f;
            IndexedVector3 normalInB = IndexedVector3.Zero;
            IndexedVector3 pointOnA = IndexedVector3.Zero, pointOnB = IndexedVector3.Zero;
            IndexedMatrix  localTransA    = input.m_transformA;
            IndexedMatrix  localTransB    = input.m_transformB;
            IndexedVector3 positionOffset = (localTransA._origin + localTransB._origin) * .5f;


            IndexedVector3.Subtract(out localTransA._origin, ref localTransA._origin, ref positionOffset);
            IndexedVector3.Subtract(out localTransB._origin, ref localTransB._origin, ref positionOffset);
            //localTransB._origin -= positionOffset;

            bool check2d = m_minkowskiA.IsConvex2d() && m_minkowskiB.IsConvex2d();

            float marginA = m_marginA;
            float marginB = m_marginB;

#if TEST_NON_VIRTUAL
            float marginAv = m_minkowskiA.getMarginNonVirtual();
            float marginBv = m_minkowskiB.getMarginNonVirtual();
            Debug.Assert(marginA == marginAv);
            Debug.Assert(marginB == marginBv);
#endif //TEST_NON_VIRTUAL

            gNumGjkChecks++;

#if DEBUG_SPU_COLLISION_DETECTION
            spu_printf("inside gjk\n");
#endif
            //for CCD we don't use margins
            if (m_ignoreMargin)
            {
                marginA = 0f;
                marginB = 0f;
#if DEBUG_SPU_COLLISION_DETECTION
                spu_printf("ignoring margin\n");
#endif
            }

            m_curIter = 0;
            int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
            m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);

            bool isValid          = false;
            bool checkSimplex     = false;
            bool checkPenetration = true;
            m_degenerateSimplex = 0;

            m_lastUsedMethod = -1;
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transA", localTransA);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjk::getClosestPointsNonVirtual transB", localTransB);
            }
#endif

            {
                float squaredDistance = MathUtil.BT_LARGE_FLOAT;
                float delta           = 0f;

                float margin = marginA + marginB;

                m_simplexSolver.Reset();

                int count = 0;
                for (; ;)
                //while (true)
                {
                    count++;

                    IndexedVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * input.m_transformA._basis;
                    IndexedVector3 seperatingAxisInB = m_cachedSeparatingAxis * input.m_transformB._basis;


                    IndexedVector3 pInA = m_minkowskiA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    IndexedVector3 qInB = m_minkowskiB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);

                    IndexedVector3 pWorld = localTransA * pInA;
                    IndexedVector3 qWorld = localTransB * qInB;

                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    IndexedVector3 w = new IndexedVector3(pWorld.X - qWorld.X, pWorld.Y - qWorld.Y, pWorld.Z - qWorld.Z);
                    delta = IndexedVector3.Dot(ref m_cachedSeparatingAxis, ref w);

#if DEBUG
                    if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "m_cachedSeparatingAxis", m_cachedSeparatingAxis);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "w", w);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("simplex num vertices [{0}]", m_simplexSolver.NumVertices()));
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
                        MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                    }
#endif
                    // potential exit, they don't overlap
                    if ((delta > 0f) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
                    {
                        m_degenerateSimplex = 10;
                        checkSimplex        = true;
                        //checkPenetration = false;
                        break;
                    }

                    //exit 0: the new point is already in the simplex, or we didn't come any closer
                    if (m_simplexSolver.InSimplex(ref w))
                    {
                        m_degenerateSimplex = 1;
                        checkSimplex        = true;
                        break;
                    }
                    // are we getting any closer ?
                    float f0 = squaredDistance - delta;
                    float f1 = squaredDistance * REL_ERROR2;

                    if (f0 <= f1)
                    {
                        if (f0 <= 0f)
                        {
                            m_degenerateSimplex = 2;
                        }
                        else
                        {
                            m_degenerateSimplex = 11;
                        }
                        checkSimplex = true;
                        break;
                    }
                    //add current vertex to simplex
                    m_simplexSolver.AddVertex(ref w, ref pWorld, ref qWorld);

                    //calculate the closest point to the origin (update vector v)
                    IndexedVector3 newCachedSeparatingAxis;

                    if (!m_simplexSolver.Closest(out newCachedSeparatingAxis))
                    {
                        m_degenerateSimplex = 3;
                        checkSimplex        = true;
                        break;
                    }

                    if (newCachedSeparatingAxis.LengthSquared() < REL_ERROR2)
                    {
                        m_cachedSeparatingAxis = newCachedSeparatingAxis;
                        m_degenerateSimplex    = 6;
                        checkSimplex           = true;
                        break;
                    }

                    float previousSquaredDistance = squaredDistance;
                    squaredDistance = newCachedSeparatingAxis.LengthSquared();
#if DEBUG
                    if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                    {
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisA", seperatingAxisInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxisB", seperatingAxisInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pInA", pInA);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qInB", qInB);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "pWorld", pWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "qWorld", qWorld);
                        MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "newSeperatingAxis", newCachedSeparatingAxis);
                        BulletGlobals.g_streamWriter.WriteLine(String.Format("f0[{0:0.00000000}] f1[{1:0.00000000}] checkSimplex[{2}] degen[{3}]", f0, f1, checkSimplex, m_degenerateSimplex));
                    }
#endif

#if false
                    ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
                    if (squaredDistance > previousSquaredDistance)
                    {
                        m_degenerateSimplex = 7;
                        squaredDistance     = previousSquaredDistance;
                        checkSimplex        = false;
                        break;
                    }
#endif //

                    //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);

                    //are we getting any closer ?
                    if (previousSquaredDistance - squaredDistance <= MathUtil.SIMD_EPSILON * previousSquaredDistance)
                    {
                        //m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
                        checkSimplex        = true;
                        m_degenerateSimplex = 12;
                        break;
                    }

                    m_cachedSeparatingAxis = newCachedSeparatingAxis;

                    //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
                    if (m_curIter++ > gGjkMaxIter)
                    {
                        //#if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION)

                        //        printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
                        //        printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
                        //        m_cachedSeparatingAxis.getX(),
                        //        m_cachedSeparatingAxis.getY(),
                        //        m_cachedSeparatingAxis.getZ(),
                        //        squaredDistance,
                        //        m_minkowskiA->getShapeType(),
                        //        m_minkowskiB->getShapeType());

                        //#endif
                        break;
                    }

                    bool check = (!m_simplexSolver.FullSimplex());
                    //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());

                    if (!check)
                    {
                        //do we need this backup_closest here ?
                        //m_simplexSolver.BackupClosest(ref m_cachedSeparatingAxis);
                        m_degenerateSimplex = 13;

                        break;
                    }
                }

                if (checkSimplex)
                {
                    m_simplexSolver.ComputePoints(out pointOnA, out pointOnB);
                    normalInB = m_cachedSeparatingAxis;
                    float lenSqr = m_cachedSeparatingAxis.LengthSquared();
                    //valid normal
                    if (lenSqr < 0.0001f)
                    {
                        m_degenerateSimplex = 5;
                    }
                    if (lenSqr > MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON)
                    {
                        //float rlen = 1 / normalInB.Length();
                        float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
                        //normalInB.Normalize();
                        normalInB *= rlen;
                        float s = (float)Math.Sqrt((float)squaredDistance);

                        Debug.Assert(s > 0f);
                        pointOnA -= m_cachedSeparatingAxis * (marginA / s);
                        pointOnB += m_cachedSeparatingAxis * (marginB / s);
                        distance  = ((1f / rlen) - margin);
                        isValid   = true;

                        m_lastUsedMethod = 1;
                    }
                    else
                    {
                        m_lastUsedMethod = 2;
                    }
                }

                bool catchDegeneratePenetrationCase =
                    (m_catchDegeneracies && m_penetrationDepthSolver != null && m_degenerateSimplex > 0 && ((distance + margin) < 0.01));

                //if (checkPenetration && !isValid)
                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
                {
                    //penetration case

                    //if there is no way to handle penetrations, bail ref
                    if (m_penetrationDepthSolver != null)
                    {
                        // Penetration depth case.
                        IndexedVector3 tmpPointOnA = IndexedVector3.Zero, tmpPointOnB = IndexedVector3.Zero;

                        gNumDeepPenetrationChecks++;
                        m_cachedSeparatingAxis = IndexedVector3.Zero;
                        bool isValid2 = m_penetrationDepthSolver.CalcPenDepth(
                            m_simplexSolver,
                            m_minkowskiA, m_minkowskiB,
                            ref localTransA, ref localTransB,
                            ref m_cachedSeparatingAxis, ref tmpPointOnA, ref tmpPointOnB,
                            debugDraw
                            );

#if DEBUG
                        if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
                        {
                            BulletGlobals.g_streamWriter.WriteLine("calcPenDepthResult");
                            BulletGlobals.g_streamWriter.WriteLine("lastMethodUsed : " + m_lastUsedMethod);

                            MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransA", localTransA);
                            MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "localTransB", localTransB);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sepAxis", m_cachedSeparatingAxis);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpA", tmpPointOnA);
                            MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "tmpB", tmpPointOnB);
                        }
#endif

                        if (isValid2)
                        {
                            IndexedVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
                            float          lenSqr       = tmpNormalInB.LengthSquared();
                            if (lenSqr <= (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                            {
                                tmpNormalInB = m_cachedSeparatingAxis;
                                lenSqr       = m_cachedSeparatingAxis.LengthSquared();
                            }

                            if (lenSqr > (MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
                            {
                                tmpNormalInB /= (float)Math.Sqrt(lenSqr);
                                float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
                                //only replace valid penetrations when the result is deeper (check)
                                if (!isValid || (distance2 < distance))
                                {
                                    distance  = distance2;
                                    pointOnA  = tmpPointOnA;
                                    pointOnB  = tmpPointOnB;
                                    normalInB = tmpNormalInB;
                                    isValid   = true;



                                    //FIXME! check2d THIS


                                    m_lastUsedMethod = 3;
                                }
                                else
                                {
                                    m_lastUsedMethod = 8;
                                }
                            }
                            else
                            {
                                //isValid = false;
                                m_lastUsedMethod = 9;
                            }
                        }
                        else
                        {
                            ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
                            ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
                            ///reports a valid positive distance. Use the results of the second GJK instead of failing.
                            ///thanks to Jacob.Langford for the reproduction case
                            ///http://code.google.com/p/bullet/issues/detail?id=250


                            if (m_cachedSeparatingAxis.LengthSquared() > 0f)
                            {
                                float distance2 = (tmpPointOnA - tmpPointOnB).Length() - margin;
                                //only replace valid distances when the distance is less
                                if (!isValid || (distance2 < distance))
                                {
                                    distance  = distance2;
                                    pointOnA  = tmpPointOnA;
                                    pointOnB  = tmpPointOnB;
                                    pointOnA -= m_cachedSeparatingAxis * marginA;
                                    pointOnB += m_cachedSeparatingAxis * marginB;
                                    normalInB = m_cachedSeparatingAxis;
                                    normalInB.Normalize();
                                    isValid          = true;
                                    m_lastUsedMethod = 6;
                                }
                                else
                                {
                                    m_lastUsedMethod = 5;
                                }
                            }
                        }
                    }
                }
            }

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
            {
                BulletGlobals.g_streamWriter.WriteLine("valid [{0}] distance[{1:0000.00000000}][{2:0000.00000000}] maxDistSq[{3:0000.00000000}]", isValid, distance, distance * distance, input.m_maximumDistanceSquared);
            }
#endif

            if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
            {
                m_cachedSeparatingAxis     = normalInB;
                m_cachedSeparatingDistance = distance;
                IndexedVector3 temp = pointOnB + positionOffset;
                output.AddContactPoint(
                    ref normalInB,
                    ref temp,
                    distance);
            }
        }
コード例 #9
0
 public virtual void GetClosestPoints(ref ClosestPointInput input, IDiscreteCollisionDetectorInterfaceResult output, IDebugDraw debugDraw, bool swapResults)
 {
     GetClosestPointsNonVirtual(ref input, output, debugDraw);
 }
コード例 #10
0
        public virtual bool CalcTimeOfImpact(ref IndexedMatrix fromA, ref IndexedMatrix toA, ref IndexedMatrix fromB, ref IndexedMatrix toB, CastResult result)
        {
            m_simplexSolver.Reset();

            /// compute linear velocity for this interval, to interpolate
            //assume no rotation/angular velocity, assert here?
            IndexedVector3 linVelA, linVelB;

            linVelA = toA._origin - fromA._origin;
            linVelB = toB._origin - fromB._origin;

            float          radius = 0.001f;
            float          lambda = 0f;
            IndexedVector3 v      = new IndexedVector3(1, 0, 0);

            int maxIter = MAX_ITERATIONS;

            IndexedVector3 n         = IndexedVector3.Zero;
            bool           hasResult = false;
            IndexedVector3 c;
            IndexedVector3 r = (linVelA - linVelB);

            float lastLambda = lambda;
            //float epsilon = float(0.001);

            int numIter = 0;
            //first solution, using GJK


            IndexedMatrix identityTrans = IndexedMatrix.Identity;

            //	result.drawCoordSystem(sphereTr);

            PointCollector pointCollector = new PointCollector();


            using (GjkPairDetector gjk = BulletGlobals.GjkPairDetectorPool.Get())
            {
                gjk.Initialize(m_convexA, m_convexB, m_simplexSolver, null);//m_penetrationDepthSolver);
                ClosestPointInput input = ClosestPointInput.Default();

                //we don't use margins during CCD
                //	gjk.setIgnoreMargin(true);

                input.m_transformA = fromA;
                input.m_transformB = fromB;
                gjk.GetClosestPoints(ref input, pointCollector, null, false);

                hasResult = pointCollector.m_hasResult;
                c         = pointCollector.m_pointInWorld;

                if (hasResult)
                {
                    float dist = pointCollector.m_distance;
                    n = pointCollector.m_normalOnBInWorld;

                    //not close enough
                    while (dist > radius)
                    {
                        numIter++;
                        if (numIter > maxIter)
                        {
                            return(false); //todo: report a failure
                        }
                        float dLambda = 0f;

                        float projectedLinearVelocity = IndexedVector3.Dot(r, n);

                        dLambda = dist / (projectedLinearVelocity);

                        lambda = lambda - dLambda;

                        if (lambda > 1f || lambda < 0f)
                        {
                            return(false);
                        }

                        //todo: next check with relative epsilon
                        if (lambda <= lastLambda)
                        {
                            return(false);
                            //n.setValue(0,0,0);
                            //break;
                        }
                        lastLambda = lambda;

                        //interpolate to next lambda
                        result.DebugDraw(lambda);
                        input.m_transformA._origin = MathUtil.Interpolate3(fromA._origin, toA._origin, lambda);
                        input.m_transformB._origin = MathUtil.Interpolate3(fromB._origin, toB._origin, lambda);

                        gjk.GetClosestPoints(ref input, pointCollector, null, false);
                        if (pointCollector.m_hasResult)
                        {
                            if (pointCollector.m_distance < 0f)
                            {
                                result.m_fraction = lastLambda;
                                n = pointCollector.m_normalOnBInWorld;
                                result.m_normal   = n;
                                result.m_hitPoint = pointCollector.m_pointInWorld;

                                return(true);
                            }
                            c    = pointCollector.m_pointInWorld;
                            n    = pointCollector.m_normalOnBInWorld;
                            dist = pointCollector.m_distance;
                        }
                        else
                        {
                            //??
                            return(false);
                        }
                    }

                    //is n normalized?
                    //don't report time of impact for motion away from the contact normal (or causes minor penetration)
                    if (IndexedVector3.Dot(n, r) >= -result.m_allowedPenetration)
                    {
                        return(false);
                    }

                    result.m_fraction = lambda;
                    result.m_normal   = n;
                    result.m_hitPoint = c;
                    return(true);
                }
            }
            return(false);
        }
コード例 #11
0
        public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                                 ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
        {
            bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();


            float          minProj = float.MaxValue;
            IndexedVector3 minNorm = IndexedVector3.Zero;
            IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
            IndexedVector3 seperatingAxisInA, seperatingAxisInB;
            IndexedVector3 pInA, qInB, pWorld, qWorld, w;

#if USE_BATCHED_SUPPORT
            IndexedVector4[] supportVerticesABatch  = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector4[] supportVerticesBBatch  = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
            IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];


            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm    = sPenetrationDirections[i];
                IndexedVector3 negNorm = -norm;

                IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
                IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
                //seperatingAxisInABatch[i] = (-norm) * transA._basis;
                //seperatingAxisInBBatch[i] = norm * transB._basis;
            }

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        IndexedVector3 norm;
                        convexA.GetPreferredPenetrationDirection(i, out norm);
                        IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
                        sPenetrationDirections[numSampleDirections] = norm;
                        IndexedVector3 negNorm = -norm;
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        IndexedVector3 norm;
                        convexB.GetPreferredPenetrationDirection(i, out norm);
                        IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
                        sPenetrationDirections[numSampleDirections] = norm;
                        IndexedVector3 negNorm = -norm;
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
                        IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
                        numSampleDirections++;
                    }
                }
            }

            convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
            convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    // shouldn't this be Y ?
                    norm.Z = 0;
                }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = seperatingAxisInABatch[i];
                    seperatingAxisInB = seperatingAxisInBBatch[i];

                    pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
                    qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);

                    IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
                    IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
                    if (check2d)
                    {
                        // shouldn't this be Y ?
                        pWorld.Z = 0f;
                        qWorld.Z = 0f;
                    }

                    IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
                    float delta = IndexedVector3.Dot(ref norm, ref w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA    = pWorld;
                        minB    = qWorld;
                    }
                }
            }
#else
            int numSampleDirections = NUM_UNITSPHERE_POINTS;

            {
                int numPDA = convexA.GetNumPreferredPenetrationDirections();
                if (numPDA > 0)
                {
                    for (int i = 0; i < numPDA; i++)
                    {
                        IndexedVector3 norm;
                        convexA.GetPreferredPenetrationDirection(i, out norm);
                        norm = IndexedVector3.TransformNormal(norm, transA);
                        sPenetrationDirections[numSampleDirections] = norm;
                        numSampleDirections++;
                    }
                }
            }

            {
                int numPDB = convexB.GetNumPreferredPenetrationDirections();
                if (numPDB > 0)
                {
                    for (int i = 0; i < numPDB; i++)
                    {
                        IndexedVector3 norm = IndexedVector3.Zero;
                        convexB.GetPreferredPenetrationDirection(i, out norm);
                        norm = IndexedVector3.TransformNormal(norm, transB);
                        sPenetrationDirections[numSampleDirections] = norm;
                        numSampleDirections++;
                    }
                }
            }

            for (int i = 0; i < numSampleDirections; i++)
            {
                IndexedVector3 norm = sPenetrationDirections[i];
                if (check2d)
                {
                    norm.Z = 0f;
                }
                if (norm.LengthSquared() > 0.01f)
                {
                    seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
                    seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
                    pInA   = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
                    qInB   = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
                    pWorld = IndexedVector3.Transform(pInA, transA);
                    qWorld = IndexedVector3.Transform(qInB, transB);
                    if (check2d)
                    {
                        pWorld.Z = 0.0f;
                        qWorld.Z = 0.0f;
                    }

                    w = qWorld - pWorld;
                    float delta = IndexedVector3.Dot(norm, w);
                    //find smallest delta
                    if (delta < minProj)
                    {
                        minProj = delta;
                        minNorm = norm;
                        minA    = pWorld;
                        minB    = qWorld;
                    }
                }
            }
#endif //USE_BATCHED_SUPPORT

            //add the margins

            minA += minNorm * convexA.GetMarginNonVirtual();
            minB -= minNorm * convexB.GetMarginNonVirtual();
            //no penetration
            if (minProj < 0f)
            {
                return(false);
            }

            float extraSeparation = 0.5f;///scale dependent
            minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());

#if DEBUG_DRAW
            if (debugDraw)
            {
                IndexedVector3 color = new IndexedVector3(0, 1, 0);
                debugDraw.drawLine(minA, minB, color);
                color = new IndexedVector3(1, 1, 1);
                IndexedVector3 vec  = minB - minA;
                float          prj2 = IndexedVector3.Dot(minNorm, vec);
                debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
            }
#endif //DEBUG_DRAW



            GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
            gjkdet.Initialize(convexA, convexB, simplexSolver, null);

            float          offsetDist = minProj;
            IndexedVector3 offset     = minNorm * offsetDist;

            ClosestPointInput input = ClosestPointInput.Default();

            IndexedVector3 newOrg = transA._origin + offset;

            IndexedMatrix displacedTrans = transA;
            displacedTrans._origin = newOrg;

            input.m_transformA             = displacedTrans;
            input.m_transformB             = transB;
            input.m_maximumDistanceSquared = float.MaxValue;

            MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
            gjkdet.SetCachedSeperatingAxis(-minNorm);

            gjkdet.GetClosestPoints(ref input, res, debugDraw, false);

            float correctedMinNorm = minProj - res.m_depth;

            //the penetration depth is over-estimated, relax it
            float penetration_relaxation = 1f;
            minNorm *= penetration_relaxation;

            if (res.m_hasResult)
            {
                pa = res.m_pointInWorld - minNorm * correctedMinNorm;
                pb = res.m_pointInWorld;
                v  = minNorm;

#if DEBUG_DRAW
                if (debugDraw != null)
                {
                    IndexedVector3 color = new IndexedVector3(1, 0, 0);
                    debugDraw.drawLine(pa, pb, color);
                }
#endif//DEBUG_DRAW
            }

            BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
            return(res.m_hasResult);
        }
コード例 #12
0
        //public BoxBoxDetector(BoxShape box1, BoxShape box2)
        //{
        //    m_box1 = box1;
        //    m_box2 = box2;
        //}


        // Work in progress to copy redo the box detector to remove un-necessary allocations

        public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
        {
            IndexedMatrix transformA = input.m_transformA;
            IndexedMatrix transformB = input.m_transformB;

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
            }
#endif


            int    skip    = 0;
            Object contact = null;

            IndexedVector3 normal      = new IndexedVector3();
            float          depth       = 0f;
            int            return_code = -1;
            int            maxc        = 4;

            IndexedVector3 translationA = new IndexedVector3(transformA._origin);
            IndexedVector3 translationB = new IndexedVector3(transformB._origin);

            //IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);

            IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
            IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());

            //IndexedVector3 box1Margin = 2f * debugExtents;
            //IndexedVector3 box2Margin = 2f * debugExtents;

            IndexedBasisMatrix rotateA = transformA._basis.Transpose();
            IndexedBasisMatrix rotateB = transformB._basis.Transpose();

            for (int j = 0; j < 3; j++)
            {
                s_temp1[0 + 4 * j] = transformA._basis[j].X;
                s_temp2[0 + 4 * j] = transformB._basis[j].X;

                s_temp1[1 + 4 * j] = transformA._basis[j].Y;
                s_temp2[1 + 4 * j] = transformB._basis[j].Y;


                s_temp1[2 + 4 * j] = transformA._basis[j].Z;
                s_temp2[2 + 4 * j] = transformB._basis[j].Z;
            }

            //s_temp1[0] = rotateA._Row0.X;
            //s_temp1[1] = rotateA._Row0.Y;
            //s_temp1[2] = rotateA._Row0.Z;

            //s_temp1[4] = rotateA._Row1.X;
            //s_temp1[5] = rotateA._Row1.Y;
            //s_temp1[6] = rotateA._Row1.Z;

            //s_temp1[8] = rotateA._Row2.X;
            //s_temp1[9] = rotateA._Row2.X;
            //s_temp1[10] = rotateA._Row2.X;


            //s_temp2[0] = rotateB._Row0.X;
            //s_temp2[1] = rotateB._Row0.Y;
            //s_temp2[2] = rotateB._Row0.Z;

            //s_temp2[4] = rotateB._Row1.X;
            //s_temp2[5] = rotateB._Row1.Y;
            //s_temp2[6] = rotateB._Row1.Z;

            //s_temp2[8] = rotateB._Row2.X;
            //s_temp2[9] = rotateB._Row2.Y;
            //s_temp2[10] = rotateB._Row2.Z;

            DBoxBox2(ref translationA,
                     s_temp1,
                     ref box1Margin,
                     ref translationB,
                     s_temp2,
                     ref box2Margin,
                     ref normal, ref depth, ref return_code,
                     maxc, contact, skip,
                     output);
        }