Example #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);
        }
Example #2
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);
        }