public override void InitializeDemo() { SetCameraDistance(50); int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1); int vertStride = 1; int indexStride = 3; BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback(); gVertices = new ObjectArray<IndexedVector3>(totalVerts); gIndices = new ObjectArray<int>(totalTriangles * 3); SetVertexPositions(waveheight, 0.0f); gVertices.GetRawArray()[1].Y = 0.1f; int index=0; int i, j; for (i=0;i<NUM_VERTS_X-1;i++) { for (j=0;j<NUM_VERTS_Y-1;j++) { #if SWAP_WINDING #if SHIFT_INDICES gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; #else gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i; #endif //SHIFT_INDICES #else //SWAP_WINDING #if SHIFT_INDICES gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = j*NUM_VERTS_X+i+1; #if TEST_INCONSISTENT_WINDING gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; #else //TEST_INCONSISTENT_WINDING gIndices[index++] = (j+1)*NUM_VERTS_X+i; gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; #endif //TEST_INCONSISTENT_WINDING #else //SHIFT_INDICES gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = j*NUM_VERTS_X+i+1; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = (j+1)*NUM_VERTS_X+i; #endif //SHIFT_INDICES #endif //SWAP_WINDING } } m_indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, gIndices, indexStride, totalVerts, gVertices, vertStride); bool useQuantizedAabbCompression = true; IndexedVector3 aabbMin = new IndexedVector3 (-1000,-1000,-1000); IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000); trimeshShape = new BvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,true); CollisionShape groundShape = trimeshShape; TriangleInfoMap triangleInfoMap = new TriangleInfoMap(); InternalEdgeUtility.GenerateInternalEdgeInfo(trimeshShape, triangleInfoMap); m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); m_broadphase = new DbvtBroadphase(); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); m_dynamicsWorld.SetDebugDrawer(m_debugDraw); IndexedVector3 gravity = new IndexedVector3(0,-10,0); m_dynamicsWorld.SetGravity(ref gravity); float mass = 0.0f; IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-2,0)); ConvexHullShape colShape = new ConvexHullShape(new List<IndexedVector3>(), 0); for (int k=0;k<DemoMeshes.TaruVtxCount;k++) { IndexedVector3 vtx = DemoMeshes.TaruVtx[k]; colShape.AddPoint(ref vtx); } //this will enable polyhedral contact clipping, better quality, slightly slower //colShape.InitializePolyhedralFeatures(); //the polyhedral contact clipping can use either GJK or SAT test to find the separating axis m_dynamicsWorld.GetDispatchInfo().m_enableSatConvex=false; { //for (int i2 = 0; i2 < 1; i2++) //{ // startTransform._origin = new IndexedVector3(-10.0f + i2 * 3.0f, 2.2f + i2 * 0.1f, -1.3f); // RigidBody body = LocalCreateRigidBody(10, startTransform, colShape); // body.SetActivationState(ActivationState.DISABLE_DEACTIVATION); // body.SetLinearVelocity(new IndexedVector3(0, 0, -1)); // //body->setContactProcessingThreshold(0.f); //} } { BoxShape colShape2 = new BoxShape(new IndexedVector3(1, 1, 1)); //colShape.InitializePolyhedralFeatures(); m_collisionShapes.Add(colShape2); startTransform._origin = new IndexedVector3(-16.0f + i * 3.0f, 1.0f + i * 0.1f, -1.3f); RigidBody body = LocalCreateRigidBody(10, startTransform, colShape2); body.SetActivationState(ActivationState.DISABLE_DEACTIVATION); body.SetLinearVelocity(new IndexedVector3(0, 0, -1)); } startTransform = IndexedMatrix.Identity; staticBody = LocalCreateRigidBody(mass, startTransform,groundShape); //staticBody->setContactProcessingThreshold(-0.031f); staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT); //enable custom material callback staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK); m_debugDraw.SetDebugMode(DebugDrawModes.DBG_DrawText | DebugDrawModes.DBG_NoHelpText | DebugDrawModes.DBG_DrawWireframe | DebugDrawModes.DBG_DrawContactPoints); //base.InitializeDemo(); //ClientResetScene(); }
public IndexedVector3 LocalGetSupportVertexWithoutMarginNonVirtual(ref IndexedVector3 localDir) { singleResult = IndexedVector3.Zero; if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape) { BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual " + GetName()); } switch (m_shapeType) { case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE: { singleResult = new IndexedVector3(); break; } case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE: { BoxShape convexShape = this as BoxShape; IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions(); singleResult = 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 (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", singleResult); } 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 (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); } singleResult = 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; singleResult = tmp; } else { tmp[XX] = radius; tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight; tmp[ZZ] = 0.0f; singleResult = 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; } } singleResult = 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(); singleResult = 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(); singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling); break; } default: singleResult = LocalGetSupportingVertexWithoutMargin(ref localDir); break; } // should never reach here //Debug.Assert(false); 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", singleResult); } return(singleResult); }
public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls) { DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world; CompoundShape compoundshape = new CompoundShape(false); compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin); int ii = 1; for (int i = 0; i < pHullCount; i++) { int vertexCount = (int) pConvHulls[ii]; IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]); IndexedMatrix childTrans = IndexedMatrix.Identity; childTrans._origin = centroid; List<IndexedVector3> virts = new List<IndexedVector3>(); int ender = ((ii + 4) + (vertexCount*3)); for (int iii = ii + 4; iii < ender; iii+=3) { virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2])); } ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount); convexShape.SetMargin(world.WorldSettings.Params.collisionMargin); compoundshape.AddChildShape(ref childTrans, convexShape); ii += (vertexCount*3 + 4); } return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL); }
internal static object CreateHullShape2(object pWorld, int pHullCount, float[] pConvHulls) { CompoundShape compoundshape = new CompoundShape(false); var world = pWorld as DiscreteDynamicsWorld; compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin); int ii = 1; for (int i = 0; i < pHullCount; i++) { int vertexCount = (int) pConvHulls[ii]; IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]); IndexedMatrix childTrans = IndexedMatrix.Identity; childTrans._origin = centroid; List<IndexedVector3> virts = new List<IndexedVector3>(); int ender = ((ii + 4) + (vertexCount*3)); for (int iii = ii + 4; iii < ender; iii+=3) { virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2])); } ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount); convexShape.SetMargin(world.WorldSettings.Params.collisionMargin); compoundshape.AddChildShape(ref childTrans, convexShape); ii += (vertexCount*3 + 4); } return compoundshape; }