Пример #1
0
 public virtual void Initialize(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
 {
     m_convexA = shapeA;
     m_convexB1 = shapeB;
     m_simplexSolver = simplexSolver;
     m_penetrationDepthSolver = penetrationDepthSolver;
 }
Пример #2
0
 public virtual void Initialize(ConvexShape shapeA, ConvexShape shapeB)
 {
     m_convexA = shapeA;
     m_convexB1 = shapeB;
     m_simplexSolver = null;
     m_penetrationDepthSolver = null;
 }
Пример #3
0
 public void Initialize(ConvexShape shapeA, StaticPlaneShape plane)
 {
     m_convexA = shapeA;
     m_planeShape = plane;
     m_simplexSolver = null;
     m_penetrationDepthSolver = null;
 }
Пример #4
0
        public static void Initialize(ConvexShape shape0,ref IndexedMatrix wtrs0,
            ConvexShape shape1,ref IndexedMatrix wtrs1,
            ref GjkEpaSolver2Results results,
            GjkEpaSolver2MinkowskiDiff shapeR,
            bool withmargins)
        {
            /* Results		*/ 
            results.witnesses0 = IndexedVector3.Zero;
            results.witnesses1 = IndexedVector3.Zero;
            results.status = GjkEpaSolver2Status.Separated;
            /* Shape		*/ 
            shapeR.m_shapes[0] =	shape0;
            shapeR.m_shapes[1] =	shape1;

            shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis);
            shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1);
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1);

                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
            }
#endif

            shapeR.EnableMargin(withmargins);
        }
Пример #5
0
 public static bool	Distance(ConvexShape shape0,ref IndexedMatrix wtrs0,ConvexShape shape1,ref IndexedMatrix wtrs1,ref IndexedVector3 guess,ref GjkEpaSolver2Results results)
 {
     using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
     using (GJK gjk = BulletGlobals.GJKPool.Get())
     {
         Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
         gjk.Initialise();
         GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
         if (gjk_status == GJKStatus.Valid)
         {
             IndexedVector3 w0 = IndexedVector3.Zero;
             IndexedVector3 w1 = IndexedVector3.Zero;
             for (uint i = 0; i < gjk.m_simplex.rank; ++i)
             {
                 float p = gjk.m_simplex.p[i];
                 w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
                 IndexedVector3 temp = -gjk.m_simplex.c[i].d;
                 w1 += shape.Support(ref temp, 1) * p;
             }
             results.witnesses0 = wtrs0 * w0;
             results.witnesses1 = wtrs0 * w1;
             results.normal = w0 - w1;
             results.distance = results.normal.Length();
             results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
             return (true);
         }
         else
         {
             //GjkEpaSolver2Status
             results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed;
             return (false);
         }
     }
 }
        public GjkEpaPenetrationDepthSolver() { } // for pool

        public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
        {
            //float radialmargin = 0f;

            IndexedVector3 guessVector = (transA._origin - transB._origin);
            GjkEpaSolver2Results results = new GjkEpaSolver2Results();
            if (GjkEpaSolver2.Penetration(convexA, ref transA,
                                        convexB, ref transB,
                                        ref guessVector, ref results))
            {
                //	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
                //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
                wWitnessOnA = results.witnesses0;
                wWitnessOnB = results.witnesses1;
                v = results.normal;
                return true;
            }
            else
            {
                if (GjkEpaSolver2.Distance(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
                {
                    wWitnessOnA = results.witnesses0;
                    wWitnessOnB = results.witnesses1;
                    v = results.normal;
                    return false;
                }
            }
            return false;
        }
Пример #7
0
        public ContinuousConvexCollision(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
        {
            m_convexA = shapeA;
            m_convexB1 = shapeB;
            m_simplexSolver = simplexSolver;
            m_penetrationDepthSolver = penetrationDepthSolver;

        }
Пример #8
0
 public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB)
 {
     m_shapeA = shapeA;
     m_shapeB = shapeB;
     m_transA = IndexedMatrix.Identity;
     m_transB = IndexedMatrix.Identity;
     m_shapeType = BroadphaseNativeTypes.MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE;
 }
        public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
        {
            IndexedMatrix convexFromTrans = convexFromWorld;
            IndexedMatrix convexToTrans = convexToWorld;

            IndexedVector3 castShapeAabbMin;
            IndexedVector3 castShapeAabbMax;
            /* Compute AABB that encompasses angular movement */
            IndexedVector3 linVel, angVel;
            TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);

            // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
            IndexedMatrix R = IndexedMatrix.Identity;
            R.SetRotation(convexFromTrans.GetRotation());


            castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);

            /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
            // do a ray-shape query using convexCaster (CCD)
            for (int i = 0; i < m_overlappingObjects.Count; i++)
            {
                CollisionObject collisionObject = m_overlappingObjects[i];
                //only perform raycast if filterMask matches
                if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
                {
                    //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                    IndexedVector3 collisionObjectAabbMin;
                    IndexedVector3 collisionObjectAabbMax;
                    IndexedMatrix t = collisionObject.GetWorldTransform();
                    collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
                    AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
                    float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
                    IndexedVector3 hitNormal;

                    if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
                    {
                        IndexedMatrix wt = collisionObject.GetWorldTransform();
                        CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
                            collisionObject,
                                collisionObject.GetCollisionShape(),
                                ref wt,
                                resultCallback,
                                allowedCcdPenetration);
                    }
                }
            }

        }
        public void Initialize(ConvexShape objectA, ConvexShape objectB, BroadphaseNativeTypes shapeTypeA, BroadphaseNativeTypes shapeTypeB, float marginA, float marginB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
        {
            m_minkowskiA = objectA;
            m_minkowskiB = objectB;
            m_shapeTypeA = shapeTypeA;
            m_shapeTypeB = shapeTypeB;
            m_marginA = marginA;
            m_marginB = marginB;

            m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);

            m_simplexSolver = simplexSolver;
            m_penetrationDepthSolver = penetrationDepthSolver;
            m_ignoreMargin = false;
            m_lastUsedMethod = -1;
            m_catchDegeneracies = true;
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
            {
                BulletGlobals.g_streamWriter.WriteLine(String.Format("GjkPairDetector-alt [{0}] [{1}]", objectA.GetName(), objectB.GetName()));
            }

        }
Пример #11
0
        public void Initialize(ConvexShape objectA, ConvexShape objectB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
        {
            m_minkowskiA = objectA;
            m_minkowskiB = objectB;
            m_shapeTypeA = objectA.GetShapeType();
            m_shapeTypeB = objectB.GetShapeType();
            m_marginA    = objectA.GetMargin();
            m_marginB    = objectB.GetMargin();

            m_cachedSeparatingAxis = new IndexedVector3(0, 1, 0);


            m_simplexSolver          = simplexSolver;
            m_penetrationDepthSolver = penetrationDepthSolver;
            m_ignoreMargin           = false;
            m_lastUsedMethod         = -1;
            m_catchDegeneracies      = true;

            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJKDetector)
            {
                BulletGlobals.g_streamWriter.WriteLine(String.Format("GjkPairDetector [{0}] [{1}]", objectA.GetName(), objectB.GetName()));
            }
        }
Пример #12
0
 public BridgeTriangleConvexcastCallback() { } // for pool
 public BridgeTriangleConvexcastCallback(ConvexShape castShape, ref IndexedMatrix from, ref IndexedMatrix to,
     ConvexResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix triangleToWorld) :
     base(castShape, ref from, ref to, ref triangleToWorld, triangleMesh.GetMargin())
 {
     m_resultCallback = resultCallback;
     m_collisionObject = collisionObject;
     m_triangleMesh = triangleMesh;
 }
Пример #13
0
 public void Initialize(ConvexShape convexA, ConvexShape convexB, ISimplexSolverInterface simplexSolver)
 {
     m_convexA = convexA;
     m_convexB = convexB;
     m_simplexSolver = simplexSolver;
 }
Пример #14
0
 public ShapeHull(ConvexShape convexShape)
 {
     m_shape = convexShape;
 }
Пример #15
0
        }                            // for pool

        public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
        {
            Initialize(objectA, objectB, simplexSolver, penetrationDepthSolver);
        }
Пример #16
0
        }                                      // for pool

        public ContinuousConvexCollision(ConvexShape shapeA, ConvexShape shapeB)
        {
            m_convexA  = shapeA;
            m_convexB1 = shapeB;
        }
Пример #17
0
	    public ShapeCache(ConvexShape s)
        {
            m_shapehull = new ShapeHull(s);
        }
Пример #18
0
 public TriangleConvexcastCallback() { } // for pool
 public TriangleConvexcastCallback(ConvexShape convexShape, ref IndexedMatrix convexShapeFrom, ref IndexedMatrix convexShapeTo, ref IndexedMatrix triangleToWorld, float triangleCollisionMargin)
 {
     m_convexShape = convexShape;
     m_convexShapeFrom = convexShapeFrom;
     m_convexShapeTo = convexShapeTo;
     m_triangleToWorld = triangleToWorld;
     m_triangleCollisionMargin = triangleCollisionMargin;
 }
Пример #19
0
 public void SetMinkowskiA(ConvexShape minkA)
 {
     m_minkowskiA = minkA;
 }
Пример #20
0
 public void SetMinkowskiB(ConvexShape minkB)
 {
     m_minkowskiB = minkB;
 }
Пример #21
0
 public static bool      Distance(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results)
 {
     using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
         using (GJK gjk = BulletGlobals.GJKPool.Get())
         {
             Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
             gjk.Initialise();
             GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
             if (gjk_status == GJKStatus.Valid)
             {
                 IndexedVector3 w0 = IndexedVector3.Zero;
                 IndexedVector3 w1 = IndexedVector3.Zero;
                 for (uint i = 0; i < gjk.m_simplex.rank; ++i)
                 {
                     float p = gjk.m_simplex.p[i];
                     w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
                     IndexedVector3 temp = -gjk.m_simplex.c[i].d;
                     w1 += shape.Support(ref temp, 1) * p;
                 }
                 results.witnesses0 = wtrs0 * w0;
                 results.witnesses1 = wtrs0 * w1;
                 results.normal     = w0 - w1;
                 results.distance   = results.normal.Length();
                 results.normal    /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
                 return(true);
             }
             else
             {
                 //GjkEpaSolver2Status
                 results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed;
                 return(false);
             }
         }
 }
Пример #22
0
        public GjkPairDetector() { } // for pool

        public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
        {
            Initialize(objectA, objectB, simplexSolver, penetrationDepthSolver);
        }
Пример #23
0
 public static bool Penetration(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results)
 {
     return(Penetration(shape0, ref wtrs0, shape1, ref wtrs1, ref guess, ref results, true));
 }
        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();
            }
        }
Пример #25
0
 public Convex2dShape(ConvexShape convexChildShape)
 {
     m_childConvexShape = convexChildShape;
     m_shapeType        = BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE;
 }
Пример #26
0
 public UniformScalingShape(ConvexShape childConvexShape, float uniformScalingFactor)
 {
     m_childConvexShape     = childConvexShape;
     m_uniformScalingFactor = uniformScalingFactor;
     m_shapeType            = BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE;
 }
Пример #27
0
 public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, BroadphaseNativeTypes shapeTypeA, BroadphaseNativeTypes shapeTypeB, float marginA, float marginB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
 {
     Initialize(objectA, objectB, shapeTypeA, shapeTypeB, marginA, marginB, simplexSolver,penetrationDepthSolver);
 }
		public KinematicCharacterController(PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight, int upAxis)
		{
			m_upAxis = upAxis;
			m_addedMargin = 0.02f;
			m_walkDirection = IndexedVector3.Zero;
			m_useGhostObjectSweepTest = true;
			m_ghostObject = ghostObject;
			m_stepHeight = stepHeight;
			m_turnAngle = 0f;
			m_convexShape = convexShape;
			m_useWalkDirection = true;	// use walk direction by default, legacy behavior
			m_velocityTimeInterval = 0.0f;
			m_verticalVelocity = 0.0f;
			m_verticalOffset = 0.0f;
			m_gravity = 9.8f * 3; // 3G acceleration.
			m_fallSpeed = 55.0f; // Terminal velocity of a sky diver in m/s.
			m_jumpSpeed = 10.0f; // ?
			m_wasOnGround = false;
			m_wasJumping = false;
			SetMaxSlope(MathUtil.DegToRadians(45.0f));
		}
Пример #29
0
 public void SetMinkowskiA(ConvexShape minkA)
 {
     m_minkowskiA = minkA;
 }
Пример #30
0
        public ShapeCache Cache(ConvexShape shape)
        {
	        ShapeCache sc=(ShapeCache)shape.GetUserPointer();
	        if(sc == null)
	        {
		        sc=new ShapeCache(shape);
		        sc.m_shapehull.BuildHull(shape.GetMargin());
		        m_shapecaches.Add(sc);
		        shape.SetUserPointer(sc);
		        /* Build edges	*/ 
		        int	ni=sc.m_shapehull.NumIndices();
		        int	nv=sc.m_shapehull.NumVertices();
		        IList<int>	pi=sc.m_shapehull.m_indices;
		        IList<IndexedVector3> pv=sc.m_shapehull.m_vertices;
		        IList<Edge> edges = new ObjectArray<Edge>(ni);
		        for(int i=0;i<ni;i+=3)
		        {
			        IndexedVector3	nrm= IndexedVector3.Normalize(IndexedVector3.Cross(pv[pi[i+1]]-pv[pi[i]],pv[pi[i+2]]-pv[pi[i]]));
			        for(int j=2,k=0;k<3;j=k++)
			        {
				        int	a=pi[i+j];
				        int	b=pi[i+k];
				        Edge e=edges[Math.Min(a,b)*nv+Math.Max(a,b)];
				        if(e != null)
				        {
					        sc.m_edges.Add(new Edge());
					        e=sc.m_edges[sc.m_edges.Count-1];
					        e.n[0]=nrm;e.n[1]=-nrm;
					        e.v[0]=a;e.v[1]=b;
				        }
				        else
				        {
					        e.n[1]=nrm;
				        }
			        }
		        }
	        }
	        return(sc);
        }
Пример #31
0
 public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, BroadphaseNativeTypes shapeTypeA, BroadphaseNativeTypes shapeTypeB, float marginA, float marginB, ISimplexSolverInterface simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
 {
     Initialize(objectA, objectB, shapeTypeA, shapeTypeB, marginA, marginB, simplexSolver, penetrationDepthSolver);
 }
Пример #32
0
 public virtual void Initialize(ConvexShape convexShape, ref IndexedMatrix convexShapeFrom, ref IndexedMatrix convexShapeTo, ref IndexedMatrix triangleToWorld, float triangleCollisionMargin)
 {
     m_convexShape = convexShape;
     m_convexShapeFrom = convexShapeFrom;
     m_convexShapeTo = convexShapeTo;
     m_triangleToWorld = triangleToWorld;
     m_triangleCollisionMargin = triangleCollisionMargin;
 }
        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);
        }
Пример #34
0
        }                          // for pool

        public GjkConvexCast(ConvexShape convexA, ConvexShape convexB, ISimplexSolverInterface simplexSolver)
        {
            m_convexA       = convexA;
            m_convexB       = convexB;
            m_simplexSolver = simplexSolver;
        }
Пример #35
0
 public ContinuousConvexCollision(ConvexShape shapeA, StaticPlaneShape plane)
 {
     m_convexA    = shapeA;
     m_planeShape = plane;
 }
Пример #36
0
 public void SetMinkowskiB(ConvexShape minkB)
 {
     m_minkowskiB = minkB;
 }
Пример #37
0
 public void Initialize(ConvexShape convexA, ConvexShape convexB, ISimplexSolverInterface simplexSolver)
 {
     m_convexA       = convexA;
     m_convexB       = convexB;
     m_simplexSolver = simplexSolver;
 }
	    public UniformScalingShape(ConvexShape childConvexShape, float uniformScalingFactor)
        {
            m_childConvexShape = childConvexShape;
            m_uniformScalingFactor = uniformScalingFactor;
            m_shapeType = BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE;
        }
Пример #39
0
 public SingleSweepCallback() { } // for pool
 public SingleSweepCallback(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans, CollisionWorld world, ConvexResultCallback resultCallback, float allowedPenetration)
 {
     Initialize(castShape, ref convexFromTrans, ref convexToTrans, world, resultCallback, allowedPenetration);
 }
Пример #40
0
 public GjkConvexCast() { } // for pool 
 public GjkConvexCast(ConvexShape convexA, ConvexShape convexB, ISimplexSolverInterface simplexSolver)
 {
     m_convexA = convexA;
     m_convexB = convexB;
     m_simplexSolver = simplexSolver;
 }
Пример #41
0
 // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
 // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
 public virtual void ConvexSweepTest(ConvexShape castShape, IndexedMatrix convexFromWorld, IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
 {
     ConvexSweepTest(castShape, ref convexFromWorld, ref convexToWorld, resultCallback, allowedCcdPenetration);
 }
Пример #42
0
 public SubSimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolverInterface simplexSolver)
 {
     m_convexA       = shapeA;
     m_convexB       = shapeB;
     m_simplexSolver = simplexSolver;
 }
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject convexObj = m_isSwapped ? body1 : body0;
            CollisionObject planeObj  = m_isSwapped ? body0 : body1;

            ConvexShape      convexShape = convexObj.GetCollisionShape() as ConvexShape;
            StaticPlaneShape planeShape  = planeObj.GetCollisionShape() as StaticPlaneShape;

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

            planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
            IndexedMatrix convexInPlaneTrans;

            convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();

            IndexedVector3 vtx        = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
            IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
            float          distance   = (planeNormal.Dot(vtxInPlane) - planeConstant);

            IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
            IndexedVector3 vtxInPlaneWorld     = planeObj.GetWorldTransform() * vtxInPlaneProjected;

            hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
            resultOut.SetPersistentManifold(m_manifoldPtr);
            if (hasCollision)
            {
                /// report a contact. internally this will be kept persistent, and contact reduction is done
                IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
                IndexedVector3 pOnB             = vtxInPlaneWorld;
                resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance);
            }
            ////first perform a collision query with the non-perturbated collision objects
            //{
            //    IndexedQuaternion rotq = IndexedQuaternion.Identity;
            //    CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
            //}

            if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
            {
                IndexedVector3 v0;
                IndexedVector3 v1;
                TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
                //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

                float angleLimit = 0.125f * MathUtil.SIMD_PI;
                float perturbeAngle;
                float radius = convexShape.GetAngularMotionDisc();
                perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
                if (perturbeAngle > angleLimit)
                {
                    perturbeAngle = angleLimit;
                }
                IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
                for (int i = 0; i < m_numPerturbationIterations; i++)
                {
                    float             iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
                    IndexedQuaternion rotq           = new IndexedQuaternion(planeNormal, iterationAngle);
                    rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
                    CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
                }
            }

            if (m_ownManifold)
            {
                if (m_manifoldPtr.GetNumContacts() > 0)
                {
                    resultOut.RefreshContactPoints();
                }
            }
        }
Пример #44
0
        /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
        public static void ObjectQuerySingle(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans,
                          CollisionObject collisionObject, CollisionShape collisionShape,
                          ref IndexedMatrix colObjWorldTransform,
                          ConvexResultCallback resultCallback, float allowedPenetration)
        {
            if (collisionShape.IsConvex())
            {

                BulletGlobals.StartProfile("convexSweepConvex");
                CastResult castResult = BulletGlobals.CastResultPool.Get();
                castResult.m_allowedPenetration = allowedPenetration;
                castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??

                ConvexShape convexShape = collisionShape as ConvexShape;
                VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
                GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();

                ContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
                convexCaster1.Initialize(castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver);
                //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
                //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);

                IConvexCast castPtr = convexCaster1;

                if (castPtr.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
                {
                    //add hit
                    if (castResult.m_normal.LengthSquared() > 0.0001f)
                    {
                        if (castResult.m_fraction < resultCallback.m_closestHitFraction)
                        {
                            castResult.m_normal.Normalize();
                            LocalConvexResult localConvexResult = new LocalConvexResult
                                        (
                                            collisionObject,
                                            //null, // updated to allow different ctor on struct
                                            ref castResult.m_normal,
                                            ref castResult.m_hitPoint,
                                            castResult.m_fraction
                                        );

                            bool normalInWorldSpace = true;
                            resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);

                        }
                    }
                }
                BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster1);
                BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free(gjkEpaPenetrationSolver);
                BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
                castResult.Cleanup();
                BulletGlobals.StopProfile();
            }
            else
            {
				if (collisionShape.IsConcave())
				{
					if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
					{
						BulletGlobals.StartProfile("convexSweepbtBvhTriangleMesh");
						BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
						IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
                        IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
                        IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
						// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation

						IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis,new IndexedVector3(0));

                        using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
                        {
                            tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, triangleMesh, ref colObjWorldTransform);
                            tccb.m_hitFraction = resultCallback.m_closestHitFraction;
                            tccb.m_allowedPenetration = allowedPenetration;

                            IndexedVector3 boxMinLocal;
                            IndexedVector3 boxMaxLocal;
                            castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
                            triangleMesh.PerformConvexCast(tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal);
                        }
						BulletGlobals.StopProfile();
					}
					else
					{
						if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
						{
                            CastResult castResult = BulletGlobals.CastResultPool.Get();
							castResult.m_allowedPenetration = allowedPenetration;
							castResult.m_fraction = resultCallback.m_closestHitFraction;
							StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
							ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);

							if (convexCaster1.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
							{
								//add hit
								if (castResult.m_normal.LengthSquared() > 0.0001f)
								{
									if (castResult.m_fraction < resultCallback.m_closestHitFraction)
									{
										castResult.m_normal.Normalize();
										LocalConvexResult localConvexResult = new LocalConvexResult
											(
											collisionObject,
                                            //null, // updated to allow different ctor on struct
											ref castResult.m_normal,
											ref castResult.m_hitPoint,
											castResult.m_fraction
											);

										bool normalInWorldSpace = true;
										resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
									}
								}
							}
                            castResult.Cleanup();
						}
						else
						{
							BulletGlobals.StartProfile("convexSweepConcave");
							ConcaveShape concaveShape = (ConcaveShape)collisionShape;
                            IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
                            IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
                            IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
                            // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
                            IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));

                            using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
                            {
                                tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
                                tccb.m_hitFraction = resultCallback.m_closestHitFraction;
                                tccb.m_allowedPenetration = allowedPenetration;
                                IndexedVector3 boxMinLocal;
                                IndexedVector3 boxMaxLocal;
                                castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);

							IndexedVector3 rayAabbMinLocal = convexFromLocal;
							MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
							//rayAabbMinLocal.setMin(convexToLocal);
							IndexedVector3 rayAabbMaxLocal = convexFromLocal;
							//rayAabbMaxLocal.setMax(convexToLocal);
							MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);

                                rayAabbMinLocal += boxMinLocal;
                                rayAabbMaxLocal += boxMaxLocal;
                                concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
                                BulletGlobals.StopProfile();
                            }
						}
					}
				}
				else
				{
					///@todo : use AABB tree or other BVH acceleration structure!
					if (collisionShape.IsCompound())
					{
						BulletGlobals.StartProfile("convexSweepCompound");
						CompoundShape compoundShape = (CompoundShape)collisionShape;
						for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
						{
							IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
							CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
							IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
							// replace collision shape so that callback can determine the triangle
							CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
							collisionObject.InternalSetTemporaryCollisionShape(childCollisionShape);

							LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
							my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;


							ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
								collisionObject,
								childCollisionShape,
								ref childWorldTrans,
								my_cb, allowedPenetration);
							// restore
							collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
						}
						BulletGlobals.StopProfile();
					}
				}
            }
        }
Пример #45
0
        public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

            ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
            ///body0.m_worldTransform,
            float resultFraction = 1.0f;


            float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
            float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();

            if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
                squareMot1 < body1.GetCcdSquareMotionThreshold())
            {
                return(resultFraction);
            }

            //An adhoc way of testing the Continuous Collision Detection algorithms
            //One object is approximated as a sphere, to simplify things
            //Starting in penetration should report no time of impact
            //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
            //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)


            /// Convex0 against sphere for Convex1
            {
                ConvexShape convex0 = body0.GetCollisionShape() as ConvexShape;

                SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
                sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(convex0, sphere1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                                              body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {
                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }

                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }

                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere1);
                    result.Cleanup();
                }
            }

            /// Sphere (for convex0) against Convex1
            {
                ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;

                SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
                sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                CastResult           result         = BulletGlobals.CastResultPool.Get();
                VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
                {
                    ccd1.Initialize(sphere0, convex1, voronoiSimplex);
                    //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
                    if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
                                              body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
                    {
                        //store result.m_fraction in both bodies

                        if (body0.GetHitFraction() > result.m_fraction)
                        {
                            body0.SetHitFraction(result.m_fraction);
                        }

                        if (body1.GetHitFraction() > result.m_fraction)
                        {
                            body1.SetHitFraction(result.m_fraction);
                        }

                        if (resultFraction > result.m_fraction)
                        {
                            resultFraction = result.m_fraction;
                        }
                    }
                    BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
                    BulletGlobals.SphereShapePool.Free(sphere0);
                    result.Cleanup();
                }
            }

            return(resultFraction);
        }
Пример #46
0
 public void Initialize(ConvexShape castShape, ref IndexedMatrix from, ref IndexedMatrix to,
     ConvexResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh, ref IndexedMatrix triangleToWorld)
 {
     base.Initialize(castShape, ref from, ref to, ref triangleToWorld, triangleMesh.GetMargin());
     m_resultCallback = resultCallback;
     m_collisionObject = collisionObject;
     m_triangleMesh = triangleMesh;
 }
Пример #47
0
 public ShapeHull(ConvexShape convexShape)
 {
     m_shape = convexShape;
 }
Пример #48
0
        public void Initialize(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans, CollisionWorld world, ConvexResultCallback resultCallback, float allowedPenetration)
        {
            m_convexFromTrans = convexFromTrans;
            m_convexToTrans = convexToTrans;
            m_world = world;
            m_resultCallback = resultCallback;
            m_allowedCcdPenetration = allowedPenetration;
            m_castShape = castShape;
            IndexedVector3 unnormalizedRayDir = (m_convexToTrans._origin - m_convexFromTrans._origin);
            IndexedVector3 rayDir = unnormalizedRayDir;
            rayDir.Normalize();
            ///what about division by zero? -. just set rayDirection[i] to INF/1e30
            m_rayDirectionInverse.X = MathUtil.CompareFloat(rayDir.X, 0.0f) ? float.MaxValue : 1f / rayDir.X;
            m_rayDirectionInverse.Y = MathUtil.CompareFloat(rayDir.Y, 0.0f) ? float.MaxValue : 1f / rayDir.Y;
            m_rayDirectionInverse.Z = MathUtil.CompareFloat(rayDir.Z, 0.0f) ? float.MaxValue : 1f / rayDir.Z;

            m_signs[0] = m_rayDirectionInverse.X < 0.0;
            m_signs[1] = m_rayDirectionInverse.Y < 0.0;
            m_signs[2] = m_rayDirectionInverse.Z < 0.0;

            m_lambda_max = rayDir.Dot(ref unnormalizedRayDir);
        }
Пример #49
0
        }                                         // for pool

        public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                                         ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
        {
            //float radialmargin = 0f;

            IndexedVector3       guessVector = (transA._origin - transB._origin);
            GjkEpaSolver2Results results     = new GjkEpaSolver2Results();

            if (GjkEpaSolver2.Penetration(convexA, ref transA,
                                          convexB, ref transB,
                                          ref guessVector, ref results))
            {
                //	debugDraw.drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
                //resultOut.addContactPoint(results.normal,results.witnesses[1],-results.depth);
                wWitnessOnA = results.witnesses0;
                wWitnessOnB = results.witnesses1;
                v           = results.normal;
                return(true);
            }
            else
            {
                if (GjkEpaSolver2.Distance(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
                {
                    wWitnessOnA = results.witnesses0;
                    wWitnessOnB = results.witnesses1;
                    v           = results.normal;
                    return(false);
                }
            }
            return(false);
        }
Пример #50
0
        public virtual void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
        {
            BulletGlobals.StartProfile("convexSweepTest");
            /// use the broadphase to accelerate the search for objects, based on their aabb
            /// and for each object with ray-aabb overlap, perform an exact ray test
            /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical

            IndexedMatrix convexFromTrans;
            IndexedMatrix convexToTrans;
            convexFromTrans = convexFromWorld;
            convexToTrans = convexToWorld;
            IndexedVector3 castShapeAabbMin;
            IndexedVector3 castShapeAabbMax;
            /* Compute AABB that encompasses angular movement */
            {
                IndexedVector3 linVel;
                IndexedVector3 angVel;
                TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
                IndexedVector3 zeroLinVel = new IndexedVector3();
                IndexedMatrix R = IndexedMatrix.Identity;
                R.SetRotation(convexFromTrans.GetRotation());
                castShape.CalculateTemporalAabb(ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
            }

#if !USE_BRUTEFORCE_RAYBROADPHASE
            SingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
            convexCB.Initialize(castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration);
            IndexedVector3 tempFrom = convexFromTrans._origin;
            IndexedVector3 tempTo = convexToTrans._origin;
            m_broadphasePairCache.RayTest(ref tempFrom, ref tempTo, convexCB, ref castShapeAabbMin, ref castShapeAabbMax);
            convexCB.Cleanup();
            BulletGlobals.SingleSweepCallbackPool.Free(convexCB);
#else
	        /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
	        // do a ray-shape query using convexCaster (CCD)
	        int i;
	        for (i=0;i<m_collisionObjects.Count;i++)
	        {
		        CollisionObject	collisionObject= m_collisionObjects[i];
		        //only perform raycast if filterMask matches
		        if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle())) 
                {
			        //RigidcollisionObject* collisionObject = ctrl.GetRigidcollisionObject();
			        IndexedVector3 collisionObjectAabbMin = new IndexedVector3();
                    IndexedVector3 collisionObjectAabbMax = new IndexedVector3();
			        collisionObject.GetCollisionShape().GetAabb(collisionObject.GetWorldTransform(),ref collisionObjectAabbMin,ref collisionObjectAabbMax);
			        AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
			        float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
			        IndexedVector3 hitNormal = new IndexedVector3();
                    IndexedVector3 fromOrigin = convexFromWorld._origin;
                    IndexedVector3 toOrigin = convexToWorld._origin;
                    if (AabbUtil2.RayAabb(ref fromOrigin, ref toOrigin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, ref hitNormal))
			        {
                        IndexedMatrix trans = collisionObject.GetWorldTransform();
				        ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
					        collisionObject,
						        collisionObject.GetCollisionShape(),
						        ref trans,
						        resultCallback,
						        allowedCcdPenetration);
			        }
		        }
	        }
#endif //USE_BRUTEFORCE_RAYBROADPHASE
            BulletGlobals.StopProfile();
        }
Пример #51
0
 public void Initialize(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolverInterface simplexSolver)
 {
     m_convexA       = shapeA;
     m_convexB       = shapeB;
     m_simplexSolver = simplexSolver;
 }