示例#1
0
        public float GetMarginNonVirtual()
        {
            switch (m_shapeType)
            {
            case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
            {
                SphereShape sphereShape = this as SphereShape;
                return(sphereShape.GetRadius());
            }

            case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
            {
                BoxShape convexShape = this as BoxShape;
                return(convexShape.GetMarginNV());
            }

            case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
            {
                TriangleShape triangleShape = this as TriangleShape;
                return(triangleShape.GetMarginNV());
            }

            case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
            {
                CylinderShape cylShape = this as CylinderShape;
                return(cylShape.GetMarginNV());
            }

            case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
            {
                CapsuleShape capsuleShape = this as CapsuleShape;
                return(capsuleShape.GetMarginNV());
            }

            case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
            /* fall through */
            case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
            {
                PolyhedralConvexShape convexHullShape = this as PolyhedralConvexShape;
                return(convexHullShape.GetMarginNV());
            }

            default:
                return(this.GetMargin());
            }

            // should never reach here
            Debug.Assert(false);
            return(0.0f);
        }
        public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
        {
            TriangleShape triangleShape = new TriangleShape(ref triangle[0], ref triangle[1], ref triangle[2]);

            triangleShape.SetMargin(m_triangleCollisionMargin);

            VoronoiSimplexSolver         simplexSolver           = BulletGlobals.VoronoiSimplexSolverPool.Get();
            GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = new GjkEpaPenetrationDepthSolver();

            //#define  USE_SUBSIMPLEX_CONVEX_CAST 1
            //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented ref code below
#if USE_SUBSIMPLEX_CONVEX_CAST
            SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(m_convexShape, triangleShape, simplexSolver);
#else
            //btGjkConvexCast	convexCaster(m_convexShape,&triangleShape,&simplexSolver);
            ContinuousConvexCollision convexCaster = BulletGlobals.ContinuousConvexCollisionPool.Get();
            convexCaster.Initialize(m_convexShape, triangleShape, simplexSolver, gjkEpaPenetrationSolver);
#endif //#USE_SUBSIMPLEX_CONVEX_CAST

            CastResult castResult = BulletGlobals.CastResultPool.Get();
            castResult.m_fraction = 1f;
            if (convexCaster.CalcTimeOfImpact(ref m_convexShapeFrom, ref m_convexShapeTo, ref m_triangleToWorld, ref m_triangleToWorld, castResult))
            {
                //add hit
                if (castResult.m_normal.LengthSquared() > 0.0001f)
                {
                    if (castResult.m_fraction < m_hitFraction)
                    {
                        /* btContinuousConvexCast's normal is already in world space */

                        /*
                         #ifdef USE_SUBSIMPLEX_CONVEX_CAST
                         *                              //rotate normal into worldspace
                         *                              castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
                         #endif //USE_SUBSIMPLEX_CONVEX_CAST
                         */
                        castResult.m_normal.Normalize();

                        ReportHit(ref castResult.m_normal, ref castResult.m_hitPoint, castResult.m_fraction, partId, triangleIndex);
                    }
                }
            }

            BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster);
            BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
            castResult.Cleanup();
        }
示例#3
0
        public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
        {
            if (!AabbUtil2.TestTriangleAgainstAabb2(triangle, ref m_aabbMin, ref m_aabbMax))
            {
                return;
            }

            //aabb filter is already applied!
            CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();

            ci.SetDispatcher(m_dispatcher);

            CollisionObject ob = m_triBody as CollisionObject;

            ///debug drawing of the overlapping triangles
            ///
#if false
            if (m_dispatchInfoPtr != null && m_dispatchInfoPtr.getDebugDraw() != null && ((m_dispatchInfoPtr.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawWireframe) > 0))
            {
                IndexedVector3 color = new IndexedVector3(1, 1, 0);
                IndexedMatrix  tr    = ob.GetWorldTransform();

                IndexedVector3[] transformedTriangles = new IndexedVector3[3];
                IndexedVector3.Transform(triangle, ref tr, transformedTriangles);

                m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[0], ref transformedTriangles[1], ref color);
                m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[1], ref transformedTriangles[2], ref color);
                m_dispatchInfoPtr.getDebugDraw().DrawLine(ref transformedTriangles[2], ref transformedTriangles[0], ref color);
            }
#endif
            if (m_convexBody.GetCollisionShape().IsConvex())
            {
                using (TriangleShape tm = BulletGlobals.TriangleShapePool.Get())
                {
                    tm.Initialize(ref triangle[0], ref triangle[1], ref triangle[2]);
                    tm.SetMargin(m_collisionMarginTriangle);

                    CollisionShape tmpShape = ob.GetCollisionShape();
                    ob.InternalSetTemporaryCollisionShape(tm);

                    CollisionAlgorithm colAlgo = ci.GetDispatcher().FindAlgorithm(m_convexBody, m_triBody, m_manifoldPtr);
                    ///this should use the btDispatcher, so the actual registered algorithm is used
                    //		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);

                    if (m_resultOut.GetBody0Internal() == m_triBody)
                    {
                        m_resultOut.SetShapeIdentifiersA(partId, triangleIndex);
                    }
                    else
                    {
                        m_resultOut.SetShapeIdentifiersB(partId, triangleIndex);
                    }

                    colAlgo.ProcessCollision(m_convexBody, m_triBody, m_dispatchInfoPtr, m_resultOut);
                    ci.GetDispatcher().FreeCollisionAlgorithm(colAlgo);
                    colAlgo = null;

                    ob.InternalSetTemporaryCollisionShape(tmpShape);
                }
            }
        }
示例#4
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();
            }
        }
 public void Initialize(SphereShape sphere, TriangleShape triangle, float contactBreakingThreshold)
 {
     m_sphere   = sphere;
     m_triangle = triangle;
     m_contactBreakingThreshold = contactBreakingThreshold;
 }
        }                                   // for pool

        public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle, float contactBreakingThreshold)
        {
            m_sphere   = sphere;
            m_triangle = triangle;
            m_contactBreakingThreshold = contactBreakingThreshold;
        }
示例#7
0
        public IndexedVector3 LocalGetSupportVertexWithoutMarginNonVirtual(ref IndexedVector3 localDir)
        {
            IndexedVector3 result = IndexedVector3.Zero;

#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
            {
                BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual " + GetName());
            }
#endif
            switch (m_shapeType)
            {
            case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
            {
                result = new IndexedVector3();
                break;
            }

            case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
            {
                BoxShape       convexShape = this as BoxShape;
                IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions();

                result = new IndexedVector3(MathUtil.FSel(localDir.X, halfExtents.X, -halfExtents.X),
                                            MathUtil.FSel(localDir.Y, halfExtents.Y, -halfExtents.Y),
                                            MathUtil.FSel(localDir.Z, halfExtents.Z, -halfExtents.Z));
#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
                {
                    BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Box");
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "halfEx", halfExtents);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", result);
                }
#endif
                break;
            }

            case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
            {
                TriangleShape    triangleShape = (TriangleShape)this;
                IndexedVector3   dir           = localDir;
                IndexedVector3[] vertices      = triangleShape.m_vertices1;
                IndexedVector3   dots          = new IndexedVector3(IndexedVector3.Dot(ref dir, ref vertices[0]), IndexedVector3.Dot(ref dir, ref vertices[1]), IndexedVector3.Dot(ref dir, ref vertices[2]));
                int            maxAxis         = MathUtil.MaxAxis(ref dots);
                IndexedVector3 sup             = vertices[maxAxis];
#if DEBUG
                if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
                {
                    BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Triangle");
                    BulletGlobals.g_streamWriter.WriteLine(String.Format("MaxAxis [{0}]", maxAxis));
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx0", vertices[0]);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx1", vertices[1]);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx2", vertices[2]);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dir", dir);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dots", dots);
                    MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sup", sup);
                }
#endif


                result = sup;
                break;
            }

            case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
            {
                CylinderShape cylShape = (CylinderShape)this;
                //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)

                IndexedVector3 halfExtents    = cylShape.GetImplicitShapeDimensions();
                IndexedVector3 v              = localDir;
                int            cylinderUpAxis = cylShape.GetUpAxis();
                int            XX             = 1;
                int            YY             = 0;
                int            ZZ             = 2;

                switch (cylinderUpAxis)
                {
                case 0:
                {
                    XX = 1;
                    YY = 0;
                    ZZ = 2;
                }
                break;

                case 1:
                {
                    XX = 0;
                    YY = 1;
                    ZZ = 2;
                }
                break;

                case 2:
                {
                    XX = 0;
                    YY = 2;
                    ZZ = 1;
                }
                break;

                default:
                    Debug.Assert(false);
                    break;
                }
                ;

                float radius     = halfExtents[XX];
                float halfHeight = halfExtents[cylinderUpAxis];

                IndexedVector3 tmp = new IndexedVector3();
                float          d;
                float          vx = v[XX];
                float          vz = v[ZZ];
                float          s  = (float)Math.Sqrt(vx * vx + vz * vz);
                if (s != 0f)
                {
                    d       = radius / s;
                    tmp[XX] = v[XX] * d;
                    tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight;
                    tmp[ZZ] = v[ZZ] * d;
                    result  = tmp;
                }
                else
                {
                    tmp[XX] = radius;
                    tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight;
                    tmp[ZZ] = 0.0f;
                    result  = tmp;
                }
                break;
            }

            case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
            {
                IndexedVector3 vec0 = localDir;

                CapsuleShape capsuleShape  = this as CapsuleShape;
                float        halfHeight    = capsuleShape.GetHalfHeight();
                int          capsuleUpAxis = capsuleShape.GetUpAxis();

                float          radius = capsuleShape.GetRadius();
                IndexedVector3 supVec = new IndexedVector3();

                float maxDot = float.MinValue;

                IndexedVector3 vec    = vec0;
                float          lenSqr = vec.LengthSquared();
                if (lenSqr < 0.0001f)
                {
                    vec = new IndexedVector3(1, 0, 0);
                }
                else
                {
                    float rlen = (1.0f) / (float)Math.Sqrt(lenSqr);
                    vec *= rlen;

                    //vec = IndexedVector3.Normalize(vec);
                }
                IndexedVector3 vtx;
                float          newDot;
                {
                    IndexedVector3 pos = new IndexedVector3();
                    pos[capsuleUpAxis] = halfHeight;

                    //vtx = pos +vec*(radius);
                    vtx    = pos + vec * (radius) - vec * capsuleShape.GetMarginNV();
                    newDot = IndexedVector3.Dot(ref vec, ref vtx);

                    if (newDot > maxDot)
                    {
                        maxDot = newDot;
                        supVec = vtx;
                    }
                }
                {
                    IndexedVector3 pos = new IndexedVector3();
                    pos[capsuleUpAxis] = -halfHeight;

                    //vtx = pos +vec*(radius);
                    vtx    = pos + vec * (radius) - vec * capsuleShape.GetMarginNV();
                    newDot = IndexedVector3.Dot(ref vec, ref vtx);

                    if (newDot > maxDot)
                    {
                        maxDot = newDot;
                        supVec = vtx;
                    }
                }
                result = supVec;
                break;
            }

            case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
            {
                ConvexPointCloudShape  convexPointCloudShape = (ConvexPointCloudShape)this;
                IList <IndexedVector3> points = convexPointCloudShape.GetUnscaledPoints();
                int            numPoints      = convexPointCloudShape.GetNumPoints();
                IndexedVector3 localScaling   = convexPointCloudShape.GetLocalScalingNV();
                result = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling);
                break;
            }

            case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
            {
                ConvexHullShape        convexHullShape = (ConvexHullShape)this;
                IList <IndexedVector3> points          = convexHullShape.GetUnscaledPoints();
                int            numPoints    = convexHullShape.GetNumPoints();
                IndexedVector3 localScaling = convexHullShape.GetLocalScalingNV();
                result = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling);
                break;
            }

            default:
                result = LocalGetSupportingVertexWithoutMargin(ref localDir);
                break;
            }

            // should never reach here
            //Debug.Assert(false);
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
            {
                BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual");
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir);
                MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", result);
            }
#endif
            return(result);
        }
示例#8
0
        public virtual void GetAabbNonVirtual(ref IndexedMatrix t, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
        {
            switch (m_shapeType)
            {
            case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
            {
                SphereShape    sphereShape = this as SphereShape;
                float          radius      = sphereShape.GetImplicitShapeDimensions().X;// * convexShape->getLocalScaling().getX();
                float          margin      = radius + sphereShape.GetMarginNonVirtual();
                IndexedVector3 center      = t._origin;
                IndexedVector3 extent      = new IndexedVector3(margin);
                aabbMin = center - extent;
                aabbMax = center + extent;
            }
            break;

            case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
            /* fall through */
            case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
            {
                BoxShape       convexShape = this as BoxShape;
                float          margin      = convexShape.GetMarginNonVirtual();
                IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions();
                halfExtents += new IndexedVector3(margin);

                IndexedBasisMatrix abs_b  = t._basis.Absolute();
                IndexedVector3     center = t._origin;
                IndexedVector3     extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents));

                aabbMin = center - extent;
                aabbMax = center + extent;
                break;
            }

            case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
            {
                TriangleShape triangleShape = (TriangleShape)this;
                float         margin        = triangleShape.GetMarginNonVirtual();
                for (int i = 0; i < 3; i++)
                {
                    IndexedVector3 vec = new IndexedVector3();
                    vec[i] = 1f;
                    IndexedVector3 sv  = LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis);
                    IndexedVector3 tmp = t * sv;
                    aabbMax[i] = tmp[i] + margin;
                    vec[i]     = -1.0f;

                    tmp        = t * (LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis));
                    aabbMin[i] = tmp[i] - margin;
                }
            }
            break;

            case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
            {
                CapsuleShape   capsuleShape = this as CapsuleShape;
                float          r            = capsuleShape.GetRadius();
                IndexedVector3 halfExtents  = new IndexedVector3(r);
                int            m_upAxis     = capsuleShape.GetUpAxis();
                halfExtents[m_upAxis] = r + capsuleShape.GetHalfHeight();
                float nvMargin = capsuleShape.GetMarginNonVirtual();
                halfExtents += new IndexedVector3(nvMargin);

                IndexedBasisMatrix abs_b  = t._basis.Absolute();
                IndexedVector3     center = t._origin;
                IndexedVector3     extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents));

                aabbMin = center - extent;
                aabbMax = center + extent;
            }
            break;

            case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
            case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
            {
                PolyhedralConvexAabbCachingShape convexHullShape = (PolyhedralConvexAabbCachingShape)this;
                float margin = convexHullShape.GetMarginNonVirtual();
                convexHullShape.GetNonvirtualAabb(ref t, out aabbMin, out aabbMax, margin);
            }
            break;

            default:
                GetAabb(ref t, out aabbMin, out aabbMax);
                break;
            }

            // should never reach here
            Debug.Assert(false);
        }