public override void ProcessAllTriangles(ITriangleCallback callback, ref Vector3 aabbMin, ref Vector3 aabbMax) { Vector3 halfExtents = (aabbMax - aabbMin) * .5f; float radius = halfExtents.Length(); Vector3 center = (aabbMax + aabbMin) * 0.5f; //this is where the triangles are generated, given AABB and plane equation (normal/constant) Vector3 tangentDir0 = Vector3.Zero; Vector3 tangentDir1 = Vector3.Zero; //tangentDir0/tangentDir1 can be precalculated TransformUtil.PlaneSpace1(ref m_planeNormal,ref tangentDir0,ref tangentDir1); Vector3 supVertex0 = Vector3.Zero; Vector3 supVertex1 = Vector3.Zero; Vector3 projectedCenter = center - (Vector3.Dot(m_planeNormal,center) - m_planeConstant)*m_planeNormal; ObjectArray<Vector3> triangle = new ObjectArray<Vector3>(3); triangle.Add(projectedCenter + tangentDir0*radius + tangentDir1*radius); triangle.Add(projectedCenter + tangentDir0 * radius - tangentDir1 * radius); triangle.Add(projectedCenter - tangentDir0 * radius - tangentDir1 * radius); callback.ProcessTriangle(triangle,0,0); triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; callback.ProcessTriangle(triangle,0,1); }
public void AddValue() { var sut = new ObjectArray(); sut.Add(0); sut.Add(2.657); sut.Add("string"); Assert.Equal("string", sut[2]); Assert.Equal(3, sut.Count); }
public void CheckLengthAferInsertion() { var sut = new ObjectArray(); var intArr = new int[] { 2, 3, 4 }; sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Insert(1, 4); Assert.Equal(6, sut.Count); }
///the clipFace method is used internally public static void ClipFace(ObjectArray <IndexedVector3> pVtxIn, ObjectArray <IndexedVector3> ppVtxOut, ref IndexedVector3 planeNormalWS, float planeEqWS) { int ve; float ds, de; int numVerts = pVtxIn.Count; if (numVerts < 2) { return; } IndexedVector3 firstVertex = pVtxIn[pVtxIn.Count - 1]; IndexedVector3 endVertex = pVtxIn[0]; IndexedVector3.Dot(ref planeNormalWS, ref firstVertex, out ds); ds += planeEqWS; for (ve = 0; ve < numVerts; ve++) { endVertex = pVtxIn[ve]; IndexedVector3.Dot(ref planeNormalWS, ref endVertex, out de); de += planeEqWS; if (ds < 0) { if (de < 0) { // Start < 0, end < 0, so output endVertex ppVtxOut.Add(endVertex); } else { // Start < 0, end >= 0, so output intersection ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de)))); } } else { if (de < 0) { // Start >= 0, end < 0 so output intersection and end ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de)))); ppVtxOut.Add(endVertex); } } firstVertex = endVertex; ds = de; } }
CollisionShape BuildLargeMesh() { //int vertStride = sizeof(IndexedVector3); //int indexStride = 3*sizeof(int); int vertStride = 1; int indexStride = 3; ObjectArray <IndexedVector3> vertexArray = new ObjectArray <IndexedVector3>(); for (int i = 0; i < vertices.Length; ++i) { vertexArray.Add(vertices[i]); } ObjectArray <int> intArray = new ObjectArray <int>(); for (int i = 0; i < indices.Length; ++i) { intArray.Add(indices[i]); } //TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(DemoMeshes.BUNNY_NUM_TRIANGLES, DemoMeshes.gBunnyIndices, 3, DemoMeshes.BUNNY_NUM_VERTICES, DemoMeshes.gBunnyVertices, 3); TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(numTriangles, intArray, indexStride, vertexArray.Count, vertexArray, vertStride); TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray); //TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true); return(triangleMesh); }
public void QueryNode(QuadTreeNode node, ObjectArray <QuadTreeNode> results, IndexedVector3 source, IndexedVector3 direction) { //if(node.children == null && node.Intersects(raySource,rayTarget)) // add the lowest level. if (node.children == null) { results.Add(node); #if DEBUG_ACCELERATOR if (BulletGlobals.gDebugDraw != null) { IndexedVector3 drawMin = new IndexedVector3(node.boundingBox.Min); IndexedVector3 drawMax = new IndexedVector3(node.boundingBox.Max); IndexedVector3 worldMin = LocalToWorld2(drawMin); IndexedVector3 worldMax = LocalToWorld2(drawMax); BulletGlobals.gDebugDraw.DrawAabb(worldMin, worldMax, new IndexedVector3(1, 1, 1)); } #endif } else { // simple rescursive for now. for (int i = 0; i < 4; ++i) { if (node.children[i].Intersects(source, direction)) { QueryNode(node.children[i], results, source, direction); } } } }
public void ContainsElement() { var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.True(sut.Contains(intArr)); }
public void CountElements() { var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.Equal(10, sut.Count); }
protected void BuildMeshParts(StridingMeshInterface meshInterface) { for (int i = 0; i < meshInterface.GetNumSubParts(); ++i) { GImpactMeshShapePart newpart = new GImpactMeshShapePart(meshInterface, i); m_mesh_parts.Add(newpart); } }
public void IndexOfElement() { var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.Equal(3, sut.IndexOf("string")); }
public void VerifyCountAfterRemove() { var sut = new ObjectArray(); var intArr = new int[] { 2, 3, 4 }; sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); sut.RemoveAt(2); Assert.Equal(9, sut.Count); }
public void InsertAndResize() { var sut = new ObjectArray(); var intArr = new int[] { 2, 3, 4 }; sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); sut.Insert(3, true); Assert.Equal(4, sut.IndexOf("string")); }
public bool BoxQuery(ref AABB box, ObjectArray <int> collided_results, bool graphics) { int curIndex = 0; int numNodes = GetNodeCount(); if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactBVH && !graphics) { BulletGlobals.g_streamWriter.WriteLine("QIQBVH BoxQuery [{0}]", numNodes); } //quantize box UShortVector3 quantizedMin; UShortVector3 quantizedMax; m_box_tree.QuantizePoint(out quantizedMin, ref box.m_min); m_box_tree.QuantizePoint(out quantizedMax, ref box.m_max); while (curIndex < numNodes) { //catch bugs in tree data bool aabbOverlap = m_box_tree.TestQuantizedBoxOverlap(curIndex, ref quantizedMin, ref quantizedMax); bool isLeafNode = IsLeafNode(curIndex); if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactBVH && !graphics) { BulletGlobals.g_streamWriter.WriteLine("QIQBVH BoxQuery [{0}] o[{1}] l[{2}]", curIndex, aabbOverlap ? 1 : 0, isLeafNode ? 1 : 0); } if (isLeafNode && aabbOverlap) { foreach (var i in GetNodeData(curIndex)) { collided_results.Add(i); } } if (aabbOverlap || isLeafNode) { //next subnode curIndex++; } else { //skip node curIndex += GetEscapeNodeIndex(curIndex); } } if (collided_results.Count > 0) { return(true); } return(false); }
public void GetElementsAtIndex() { var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.Equal(intArr, sut[4]); }
public void VerifyElementsAfterRemove() { var sut = new ObjectArray(); var intArr = new int[] { 2, 3, 4 }; sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); sut.RemoveAt(1); Assert.Equal("string", sut[2]); Assert.Equal(intArr, sut[3]); }
public void AddValueAndResize() { var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.Equal(intArr, sut[4]); Assert.Equal(10, sut.Count); }
public void IndexOfElementNotFound() { string str = "abc"; object s = str; var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.Equal(-1, sut.IndexOf(s)); }
public void ChangeValueAtIndex() { string s = "new string"; object stringObject = s; var sut = new ObjectArray(); var intArr = new int[] { 2, 3, 4 }; sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); sut[1] = stringObject; Assert.Equal("new string", sut[1]); }
public void IndexOfElement() { int val = 9; object objValue = val; var sut = new ObjectArray(); var intArr = new int[] { 2, 3, 4 }; sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); sut.Insert(1, objValue); Assert.Equal(1, sut.IndexOf(objValue)); }
public void NotContainsElement() { int valueType = 57585; object elem = valueType; var intArr = new int[] { 2, 3, 4 }; var sut = new ObjectArray(); sut.Add(0); sut.Add(true); sut.Add(3.4647); sut.Add("string"); sut.Add(intArr); sut.Add(6); sut.Add(0); sut.Add(8); sut.Add(9); sut.Add(10); Assert.False(sut.Contains(elem)); }
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray <DbvtNode> leafs, int depth) { if (root.IsInternal() && depth != 0) { FetchLeafs(pdbvt, root._children[0], leafs, depth - 1); FetchLeafs(pdbvt, root._children[1], leafs, depth - 1); DeleteNode(pdbvt, root); } else { leafs.Add(root); } }
public static void Split(ObjectArray <DbvtNode> leaves, ObjectArray <DbvtNode> left, ObjectArray <DbvtNode> right, ref IndexedVector3 org, ref IndexedVector3 axis) { left.Resize(0); right.Resize(0); for (int i = 0, ni = leaves.Count; i < ni; ++i) { if (IndexedVector3.Dot(axis, leaves[i].volume.Center() - org) < 0) { left.Add(leaves[i]); } else { right.Add(leaves[i]); } } }
private bool BoxQuery(ref AABB box, ObjectArray <int> collided_results, bool graphics) { int curIndex = 0; int numNodes = GetNodeCount(); //quantize box UShortVector3 quantizedMin; UShortVector3 quantizedMax; m_box_tree.QuantizePoint(out quantizedMin, ref box.m_min); m_box_tree.QuantizePoint(out quantizedMax, ref box.m_max); while (curIndex < numNodes) { //catch bugs in tree data bool aabbOverlap = m_box_tree.TestQuantizedBoxOverlap(curIndex, ref quantizedMin, ref quantizedMax); bool isLeafNode = IsLeafNode(curIndex); if (isLeafNode && aabbOverlap) { foreach (var i in GetNodeData(curIndex)) { collided_results.Add(i); } } if (aabbOverlap || isLeafNode) { //next subnode curIndex++; } else { //skip node curIndex += GetEscapeNodeIndex(curIndex); } } if (collided_results.Count > 0) { return(true); } return(false); }
protected void GImpactVsShapeFindPairs( ref IndexedMatrix trans0, ref IndexedMatrix trans1, GImpactShapeInterface shape0, CollisionShape shape1, ObjectArray <int> collided_primitives) { AABB boxshape = new AABB(); if (shape0.HasBoxSet()) { IndexedMatrix trans1to0 = trans0.Inverse(); //trans1to0 *= trans1; trans1to0 = trans1to0 * trans1; //trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0); shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max); } #endif shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives); } else { shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max); AABB boxshape0 = new AABB(); int i = shape0.GetNumChildShapes(); while (i-- != 0) { shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max); if (boxshape.HasCollision(ref boxshape0)) { collided_primitives.Add(i); } } } }
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { IndexedVector3 separatingNormal = separatingNormal1.Normalized(); IndexedVector3 c0 = transA * hullA.m_localCenter; IndexedVector3 c1 = transB * hullB.m_localCenter; IndexedVector3 DeltaC2 = c0 - c1; float curMaxDist = maxDist; int closestFaceB = -1; float dmax = float.MinValue; { for (int face = 0; face < hullB.m_faces.Count; face++) { IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); IndexedVector3 WorldNormal = transB._basis * Normal; float d = IndexedVector3.Dot(WorldNormal, separatingNormal); if (d > dmax) { dmax = d; closestFaceB = face; } } } // setup initial clip face (minimizing face from hull B) ObjectArray <IndexedVector3> worldVertsB1 = new ObjectArray <IndexedVector3>(); { Face polyB = hullB.m_faces[closestFaceB]; int numVertices = polyB.m_indices.Count; for (int e0 = 0; e0 < numVertices; e0++) { IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]]; // check this to see if it is transposed version worldVertsB1.Add(transB * b); } } if (closestFaceB >= 0) { ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut); } }
//! returns the indices of the primitives in the m_primitive_manager public bool RayQuery(ref IndexedVector3 ray_dir, ref IndexedVector3 ray_origin, ObjectArray <int> collided_results) { int curIndex = 0; int numNodes = GetNodeCount(); while (curIndex < numNodes) { AABB bound; GetNodeBound(curIndex, out bound); //catch bugs in tree data bool aabbOverlap = bound.CollideRay(ref ray_origin, ref ray_dir); bool isLeafNode = IsLeafNode(curIndex); if (isLeafNode && aabbOverlap) { foreach (var i in GetNodeData(curIndex)) { collided_results.Add(i); } } if (aabbOverlap || isLeafNode) { //next subnode curIndex++; } else { //skip node curIndex += GetEscapeNodeIndex(curIndex); } } if (collided_results.Count > 0) { return(true); } return(false); }
public CollisionShape BuildCorner() { // slope. IndexedVector3[] vertices = new IndexedVector3[] { new IndexedVector3(0, 0, 0), new IndexedVector3(1, 0, 0), new IndexedVector3(0, 0, 1), new IndexedVector3(1, 0, 1), new IndexedVector3(0, 1, 0), new IndexedVector3(1, 1, 0), new IndexedVector3(0, 1, 1), new IndexedVector3(1, 1, 1) }; //int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 0, 4, 6, 6, 2, 0, 2, 6, 7, 4,5,0,2,2,7,5}; //int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 6, 4,0,0,2,6, 7, 6, 2, 4, 5, 0, 2, 2, 7, 5 }; int[] indices = new int[] { 1, 4, 5, 1, 5, 7, 7, 3, 1, 3, 7, 6, 7, 5, 4, 4, 6, 7, 4, 1, 3, 3, 6, 4 }; int vertStride = 1; int indexStride = 3; ObjectArray <IndexedVector3> vertexArray = new ObjectArray <IndexedVector3>(); for (int i = 0; i < vertices.Length; ++i) { vertexArray.Add(vertices[i]); } ObjectArray <int> intArray = new ObjectArray <int>(); for (int i = 0; i < indices.Length; ++i) { intArray.Add(indices[i]); } TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length / 3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride); TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray); //TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true); return(triangleMesh); }
public static CollisionShape BuildCorner(IndexedVector3[] vertices, int[] indices) { int vertStride = 1; int indexStride = 3; ObjectArray <IndexedVector3> vertexArray = new ObjectArray <IndexedVector3>(); for (int i = 0; i < vertices.Length; ++i) { vertexArray.Add(vertices[i]); } ObjectArray <int> intArray = new ObjectArray <int>(); for (int i = 0; i < indices.Length; ++i) { intArray.Add(indices[i]); } TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length / 3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride); TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray); return(triangleMesh); }
//! returns the indices of the primitives in the m_primitive_manager public bool BoxQuery(ref AABB box, ObjectArray <int> collided_results) { int curIndex = 0; int numNodes = GetNodeCount(); while (curIndex < numNodes) { AABB bound = new AABB(); GetNodeBound(curIndex, out bound); //catch bugs in tree data bool aabbOverlap = bound.HasCollision(ref box); bool isleafnode = IsLeafNode(curIndex); if (isleafnode && aabbOverlap) { collided_results.Add(GetNodeData(curIndex)); } if (aabbOverlap || isleafnode) { //next subnode curIndex++; } else { //skip node curIndex += GetEscapeNodeIndex(curIndex); } } if (collided_results.Count > 0) { return(true); } return(false); }
public static void ResizeSolverConstraintList(ObjectArray<SolverConstraint> list, int newSize) { int listSize = list.Count; int sizeDiff = newSize - listSize; // grow if needed if (listSize < newSize) { for (int i = 0; i < sizeDiff; ++i) { list.Add(new SolverConstraint()); } } else { // Trim down for (int i = sizeDiff; i < 0; ++i) { list.RemoveAt(list.Count - 1); } } }
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) public virtual bool InitializePolyhedralFeatures() { #if USE_CONVEX_HULL_COMPUTER if (m_polyhedron != null) { m_polyhedron = null; } m_polyhedron = new ConvexPolyhedron(); ObjectArray <IndexedVector3> tmpVertices = new ObjectArray <IndexedVector3>(); for (int i = 0; i < GetNumVertices(); i++) { IndexedVector3 newVertex; GetVertex(i, out newVertex); tmpVertices.Add(newVertex); } ConvexHullComputer conv = new ConvexHullComputer(); //conv.compute(&tmpVertices[0].getX(), sizeof(IndexedVector3),tmpVertices.Count,0.0f,0.0f); conv.Compute(tmpVertices, 0, tmpVertices.Count, 0.0f, 0.0f); ObjectArray <IndexedVector3> faceNormals = new ObjectArray <IndexedVector3>(); int numFaces = conv.faces.size(); faceNormals.Resize(numFaces); ConvexHullComputer convexUtil = conv; m_polyhedron.m_faces.Resize(numFaces); int numVertices = convexUtil.vertices.Count; m_polyhedron.m_vertices.Resize(numVertices); for (int p = 0; p < numVertices; p++) { m_polyhedron.m_vertices[p] = convexUtil.vertices[p]; } for (int i = 0; i < numFaces; i++) { int face = convexUtil.faces[i]; //printf("face=%d\n",face); Edge firstEdge = convexUtil.edges[face]; Edge edge = firstEdge; IndexedVector3[] edges = new IndexedVector3[3]; int numEdges = 0; //compute face normals float maxCross2 = 0.0f; int chosenEdge = -1; do { int src = edge.GetSourceVertex(); m_polyhedron.m_faces[i].m_indices.Add(src); int targ = edge.GetTargetVertex(); IndexedVector3 wa = convexUtil.vertices[src]; IndexedVector3 wb = convexUtil.vertices[targ]; IndexedVector3 newEdge = wb - wa; newEdge.Normalize(); if (numEdges < 2) { edges[numEdges++] = newEdge; } edge = edge.GetNextEdgeOfFace(); } while (edge != firstEdge); float planeEq = 1e30f; if (numEdges == 2) { faceNormals[i] = IndexedVector3.Cross(edges[0], edges[1]); faceNormals[i].Normalize(); m_polyhedron.m_faces[i].m_plane[0] = -faceNormals[i].X; m_polyhedron.m_faces[i].m_plane[1] = -faceNormals[i].Y; m_polyhedron.m_faces[i].m_plane[2] = -faceNormals[i].Z; m_polyhedron.m_faces[i].m_plane[3] = planeEq; } else { Debug.Assert(false);//degenerate? faceNormals[i] = IndexedVector3.Zero; } for (int v = 0; v < m_polyhedron.m_faces[i].m_indices.Count; v++) { float eq = IndexedVector3.Dot(m_polyhedron.m_vertices[m_polyhedron.m_faces[i].m_indices[v]], faceNormals[i]); if (planeEq > eq) { planeEq = eq; } } m_polyhedron.m_faces[i].m_plane[3] = planeEq; } if (m_polyhedron.m_faces.Count > 0 && conv.vertices.Count > 0) { for (int f = 0; f < m_polyhedron.m_faces.Count; f++) { IndexedVector3 planeNormal = new IndexedVector3(m_polyhedron.m_faces[f].m_plane[0], m_polyhedron.m_faces[f].m_plane[1], m_polyhedron.m_faces[f].m_plane[2]); float planeEq = m_polyhedron.m_faces[f].m_plane[3]; IndexedVector3 supVec = LocalGetSupportingVertex(-planeNormal); if (IndexedVector3.Dot(supVec, planeNormal) < planeEq) { m_polyhedron.m_faces[f].m_plane[0] *= -1; m_polyhedron.m_faces[f].m_plane[1] *= -1; m_polyhedron.m_faces[f].m_plane[2] *= -1; m_polyhedron.m_faces[f].m_plane[3] *= -1; int numVerts = m_polyhedron.m_faces[f].m_indices.Count; for (int v = 0; v < numVerts / 2; v++) { int temp = m_polyhedron.m_faces[f].m_indices[v]; m_polyhedron.m_faces[f].m_indices[v] = m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v]; m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v] = temp; } } } } m_polyhedron.Initialize(); #endif return(true); }
public static CollisionShape BuildCorner(IndexedVector3[] vertices, int[] indices) { int vertStride = 1; int indexStride = 3; ObjectArray<IndexedVector3> vertexArray = new ObjectArray<IndexedVector3>(); for (int i = 0; i < vertices.Length; ++i) { vertexArray.Add(vertices[i]); } ObjectArray<int> intArray = new ObjectArray<int>(); for (int i = 0; i < indices.Length; ++i) { intArray.Add(indices[i]); } TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length / 3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride); TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray); return triangleMesh; }
///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) public virtual bool InitializePolyhedralFeatures() { #if USE_CONVEX_HULL_COMPUTER if (m_polyhedron != null) { m_polyhedron = null; } m_polyhedron = new ConvexPolyhedron(); ObjectArray<IndexedVector3> tmpVertices = new ObjectArray<IndexedVector3>(); for (int i = 0; i < GetNumVertices(); i++) { IndexedVector3 newVertex; GetVertex(i, out newVertex); tmpVertices.Add(newVertex); } ConvexHullComputer conv = new ConvexHullComputer(); //conv.compute(&tmpVertices[0].getX(), sizeof(IndexedVector3),tmpVertices.Count,0.0f,0.0f); conv.Compute(tmpVertices, 0, tmpVertices.Count, 0.0f, 0.0f); ObjectArray<IndexedVector3> faceNormals = new ObjectArray<IndexedVector3>(); int numFaces = conv.faces.size(); faceNormals.Resize(numFaces); ConvexHullComputer convexUtil = conv; m_polyhedron.m_faces.Resize(numFaces); int numVertices = convexUtil.vertices.Count; m_polyhedron.m_vertices.Resize(numVertices); for (int p = 0; p < numVertices; p++) { m_polyhedron.m_vertices[p] = convexUtil.vertices[p]; } for (int i = 0; i < numFaces; i++) { int face = convexUtil.faces[i]; //printf("face=%d\n",face); Edge firstEdge = convexUtil.edges[face]; Edge edge = firstEdge; IndexedVector3[] edges = new IndexedVector3[3]; int numEdges = 0; //compute face normals float maxCross2 = 0.0f; int chosenEdge = -1; do { int src = edge.GetSourceVertex(); m_polyhedron.m_faces[i].m_indices.Add(src); int targ = edge.GetTargetVertex(); IndexedVector3 wa = convexUtil.vertices[src]; IndexedVector3 wb = convexUtil.vertices[targ]; IndexedVector3 newEdge = wb - wa; newEdge.Normalize(); if (numEdges < 2) { edges[numEdges++] = newEdge; } edge = edge.GetNextEdgeOfFace(); } while (edge != firstEdge); float planeEq = 1e30f; if (numEdges == 2) { faceNormals[i] = IndexedVector3.Cross(edges[0], edges[1]); faceNormals[i].Normalize(); m_polyhedron.m_faces[i].m_plane[0] = -faceNormals[i].X; m_polyhedron.m_faces[i].m_plane[1] = -faceNormals[i].Y; m_polyhedron.m_faces[i].m_plane[2] = -faceNormals[i].Z; m_polyhedron.m_faces[i].m_plane[3] = planeEq; } else { Debug.Assert(false);//degenerate? faceNormals[i] = IndexedVector3.Zero; } for (int v = 0; v < m_polyhedron.m_faces[i].m_indices.Count; v++) { float eq = IndexedVector3.Dot(m_polyhedron.m_vertices[m_polyhedron.m_faces[i].m_indices[v]], faceNormals[i]); if (planeEq > eq) { planeEq = eq; } } m_polyhedron.m_faces[i].m_plane[3] = planeEq; } if (m_polyhedron.m_faces.Count > 0 && conv.vertices.Count > 0) { for (int f = 0; f < m_polyhedron.m_faces.Count; f++) { IndexedVector3 planeNormal = new IndexedVector3(m_polyhedron.m_faces[f].m_plane[0], m_polyhedron.m_faces[f].m_plane[1], m_polyhedron.m_faces[f].m_plane[2]); float planeEq = m_polyhedron.m_faces[f].m_plane[3]; IndexedVector3 supVec = LocalGetSupportingVertex(-planeNormal); if (IndexedVector3.Dot(supVec, planeNormal) < planeEq) { m_polyhedron.m_faces[f].m_plane[0] *= -1; m_polyhedron.m_faces[f].m_plane[1] *= -1; m_polyhedron.m_faces[f].m_plane[2] *= -1; m_polyhedron.m_faces[f].m_plane[3] *= -1; int numVerts = m_polyhedron.m_faces[f].m_indices.Count; for (int v = 0; v < numVerts / 2; v++) { int temp = m_polyhedron.m_faces[f].m_indices[v]; m_polyhedron.m_faces[f].m_indices[v] = m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v]; m_polyhedron.m_faces[f].m_indices[numVerts - 1 - v] = temp; } } } } m_polyhedron.Initialize(); #endif return true; }
public static void FetchLeafs(Dbvt pdbvt, DbvtNode root, ObjectArray<DbvtNode> leafs, int depth) { if (root.IsInternal() && depth != 0) { FetchLeafs(pdbvt, root._children[0], leafs, depth - 1); FetchLeafs(pdbvt, root._children[1], leafs, depth - 1); DeleteNode(pdbvt, root); } else { leafs.Add(root); } }
/// process all triangles within the provided axis-aligned bounding box /** basic algorithm: - convert input aabb to local coordinates (scale down and shift for local origin) - convert input aabb to a range of heightfield grid points (quantize) - iterate over all triangles in that subset of the grid */ public override void ProcessAllTriangles(ITriangleCallback callback, ref Vector3 aabbMin, ref Vector3 aabbMax) { // scale down the input aabb's so they are in local (non-scaled) coordinates Vector3 invScale = new Vector3(1f, 1f, 1f); invScale /= m_localScaling; Vector3 localAabbMin = aabbMin * invScale; Vector3 localAabbMax = aabbMax * invScale; // account for local origin localAabbMin += m_localOrigin; localAabbMax += m_localOrigin; //quantize the aabbMin and aabbMax, and adjust the start/end ranges int[] quantizedAabbMin = new int[3]; int[] quantizedAabbMax = new int[3]; QuantizeWithClamp(quantizedAabbMin, ref localAabbMin, 0); QuantizeWithClamp(quantizedAabbMax, ref localAabbMax, 1); // expand the min/max quantized values // this is to catch the case where the input aabb falls between grid points! for (int i = 0; i < 3; ++i) { quantizedAabbMin[i]--; quantizedAabbMax[i]++; } int startX = 0; int endX = m_heightStickWidth - 1; int startJ = 0; int endJ = m_heightStickLength - 1; switch (m_upAxis) { case 0: { if (quantizedAabbMin[1] > startX) startX = quantizedAabbMin[1]; if (quantizedAabbMax[1] < endX) endX = quantizedAabbMax[1]; if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; } case 1: { if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; }; case 2: { if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; if (quantizedAabbMin[1] > startJ) startJ = quantizedAabbMin[1]; if (quantizedAabbMax[1] < endJ) endJ = quantizedAabbMax[1]; break; } default: { //need to get valid m_upAxis Debug.Assert(false); break; } } ObjectArray<Vector3> vertices = new ObjectArray<Vector3>(); Vector3[] tempVectors = new Vector3[3]; for (int j = startJ; j < endJ; j++) { for (int x = startX; x < endX; x++) { if (m_flipQuadEdges || (m_useDiamondSubdivision && (((j + x) & 1) > 0))) { //first triangle GetVertex(x, j, ref tempVectors[0]); GetVertex(x + 1, j, ref tempVectors[1]); GetVertex(x + 1, j + 1, ref tempVectors[2]); vertices.Clear(); for (int a = 0; a < tempVectors.Length; ++a) { vertices.Add(tempVectors[a]); } callback.ProcessTriangle(vertices, x, j); //second triangle GetVertex(x, j, ref tempVectors[0]); GetVertex(x + 1, j + 1, ref tempVectors[1]); GetVertex(x, j + 1, ref tempVectors[2]); vertices.Clear(); for (int a = 0; a < tempVectors.Length; ++a) { vertices.Add(tempVectors[a]); } callback.ProcessTriangle(vertices, x, j); } else { //first triangle GetVertex(x, j, ref tempVectors[0]); GetVertex(x, j + 1, ref tempVectors[1]); GetVertex(x + 1, j, ref tempVectors[2]); vertices.Clear(); for (int a = 0; a < tempVectors.Length; ++a) { vertices.Add(tempVectors[a]); } callback.ProcessTriangle(vertices, x, j); //second triangle GetVertex(x + 1, j, ref tempVectors[0]); GetVertex(x, j + 1, ref tempVectors[1]); GetVertex(x + 1, j + 1, ref tempVectors[2]); vertices.Clear(); for (int a = 0; a < tempVectors.Length; ++a) { vertices.Add(tempVectors[a]); } callback.ProcessTriangle(vertices, x, j); } } } }
public static void Split(ObjectArray<DbvtNode> leaves, ObjectArray<DbvtNode> left, ObjectArray<DbvtNode> right, ref IndexedVector3 org, ref IndexedVector3 axis) { left.Resize(0); right.Resize(0); for (int i = 0, ni = leaves.Count; i < ni; ++i) { if (IndexedVector3.Dot(axis, leaves[i].volume.Center() - org) < 0) { left.Add(leaves[i]); } else { right.Add(leaves[i]); } } }
// Portable static method: prerequisite call: m_dynamicsWorld.getBroadphase().getOverlappingPairCache().setInternalGhostPairCallback(new btGhostPairCallback()); public static void GetCollidingObjectsInsidePairCachingGhostObject(DiscreteDynamicsWorld m_dynamicsWorld, PairCachingGhostObject m_pairCachingGhostObject, ObjectArray<CollisionObject> collisionArrayOut) { bool addOnlyObjectsWithNegativeDistance = true; // With "false" things don't change much, and the code is a bit faster and cleaner... collisionArrayOut.Resize(0); if (m_pairCachingGhostObject == null || m_dynamicsWorld == null) return; //#define USE_PLAIN_COLLISION_WORLD // We dispatch all collision pairs of the ghost object every step (slow) #if USE_PLAIN_COLLISION_WORLD //====================================================================================================== // I thought this line was no longer needed, but it seems to be necessary (and I believe it's an expensive call): m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(m_pairCachingGhostObject.getOverlappingPairCache(), m_dynamicsWorld.getDispatchInfo(), m_dynamicsWorld.getDispatcher()); // Maybe the call can be automatically triggered by some other Bullet call (I'm almost sure I could comment it out in another demo I made long ago...) // So by now the general rule is: in real projects, simply comment it out and see if it works! //====================================================================================================== // UPDATE: in dynamic worlds, the line above can be commented out and the broadphase pair can be retrieved through the call to findPair(...) below. // In collision worlds probably the above line is needed only if collision detection for all the bodies hasn't been made... This is something // I'm still not sure of... the general rule is to try to comment out the line above and try to use findPair(...) and see if it works whenever possible.... //====================================================================================================== #endif //USE_PLAIN_COLLISION_WORLD ObjectArray<BroadphasePair> collisionPairs = m_pairCachingGhostObject.GetOverlappingPairCache().GetOverlappingPairArray(); int numObjects = collisionPairs.Count; PersistentManifoldArray m_manifoldArray = new PersistentManifoldArray(); bool added; for (int i = 0; i < numObjects; i++) { m_manifoldArray.Resize(0); #if USE_PLAIN_COLLISION_WORLD const btBroadphasePair& collisionPair = collisionPairs[i]; if (collisionPair.m_algorithm) collisionPair.m_algorithm.getAllContactManifolds(m_manifoldArray); else { // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above... printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n"); } #else // USE_PLAIN_COLLISION_WORLD BroadphasePair cPair = collisionPairs[i]; //unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache: BroadphasePair collisionPair = m_dynamicsWorld.GetPairCache().FindPair(cPair.m_pProxy0, cPair.m_pProxy1); if (collisionPair == null) { continue; } if (collisionPair.m_algorithm != null) { collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray); } else { // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above... //printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n"); } #endif //USE_PLAIN_COLLISION_WORLD added = false; for (int j = 0; j < m_manifoldArray.Count; j++) { PersistentManifold manifold = m_manifoldArray[j]; // Here we are in the narrowphase, but can happen that manifold.getNumContacts()==0: if (addOnlyObjectsWithNegativeDistance) { for (int p = 0, numContacts = manifold.GetNumContacts(); p < numContacts; p++) { ManifoldPoint pt = manifold.GetContactPoint(p); if (pt.GetDistance() < 0.0) { // How can I be sure that the colObjs are all distinct ? I use the "added" flag. collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0())); added = true; break; } } if (added) { break; } } else if (manifold.GetNumContacts() > 0) { collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0())); break; } } } }
public CollisionShape BuildCorner() { // slope. IndexedVector3[] vertices = new IndexedVector3[]{new IndexedVector3(0,0,0),new IndexedVector3(1,0,0),new IndexedVector3(0,0,1),new IndexedVector3(1,0,1), new IndexedVector3(0,1,0),new IndexedVector3(1,1,0),new IndexedVector3(0,1,1),new IndexedVector3(1,1,1)}; //int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 0, 4, 6, 6, 2, 0, 2, 6, 7, 4,5,0,2,2,7,5}; //int[] indices = new int[] { 0, 4, 5, 4, 6, 7, 7, 5, 4, 6, 4,0,0,2,6, 7, 6, 2, 4, 5, 0, 2, 2, 7, 5 }; int[] indices = new int[] { 1,4,5, 1,5,7, 7,3,1, 3,7,6, 7,5,4, 4,6,7, 4,1,3, 3,6,4 }; int vertStride = 1; int indexStride = 3; ObjectArray<IndexedVector3> vertexArray = new ObjectArray<IndexedVector3>(); for (int i = 0; i < vertices.Length; ++i) { vertexArray.Add(vertices[i]); } ObjectArray<int> intArray = new ObjectArray<int>(); for (int i = 0; i < indices.Length; ++i) { intArray.Add(indices[i]); } TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(indices.Length/3, intArray, indexStride, vertexArray.Count, vertexArray, vertStride); TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray); //TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true); return triangleMesh; }
public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback) { ObjectArray<CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray(); BuildIslands(dispatcher,collisionWorld); int endIslandIndex=1; int startIslandIndex; int numElem = GetUnionFind().GetNumElements(); //BT_PROFILE("processIslands"); if(!m_splitIslands) { ObjectArray<PersistentManifold> manifolds = dispatcher.GetInternalManifoldPointer(); int maxNumManifolds = dispatcher.GetNumManifolds(); callback.ProcessIsland(collisionObjects,collisionObjects.Count,manifolds,maxNumManifolds, -1); } else { // Sort manifolds, based on islands // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); int numManifolds = m_islandmanifold.Count; if (numManifolds != 0 && m_islandmanifold[0].GetNumContacts() > 0) { int ibreak = 0; } //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) //m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); //((ObjectArray<PersistentManifold>)m_islandmanifold).Sort(); m_islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); //now process all active islands (sets of manifolds for now) int startManifoldIndex = 0; int endManifoldIndex = 1; //int islandId; // printf("Start Islands\n"); //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) { int islandId = GetUnionFind().GetElement(startIslandIndex).m_id; if (BulletGlobals.g_streamWriter != null && debugIslands) { BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId)); } bool islandSleeping = false; for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId);endIslandIndex++) { int i = GetUnionFind().GetElement(endIslandIndex).m_sz; CollisionObject colObj0 = collisionObjects[i]; m_islandBodies.Add(colObj0); if (!colObj0.IsActive()) { islandSleeping = true; //islandSleeping = false; } } //find the accompanying contact manifold for this islandId int numIslandManifolds = 0; PersistentManifold startManifold = null; if (startManifoldIndex<numManifolds) { int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]); if (curIslandId == islandId) { startManifold = m_islandmanifold[startManifoldIndex]; for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) { } /// Process the actual simulation, only if not sleeping/deactivated numIslandManifolds = endManifoldIndex-startManifoldIndex; } } if (!islandSleeping) { // pass shortedned list to callback. ObjectArray<PersistentManifold> subList = new ObjectArray<PersistentManifold>(); for(int i=0;i<numIslandManifolds;++i) { subList.Add(m_islandmanifold[startManifoldIndex+i]); } callback.ProcessIsland(m_islandBodies,m_islandBodies.Count,subList,numIslandManifolds, islandId); // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } if (numIslandManifolds != 0) { startManifoldIndex = endManifoldIndex; } m_islandBodies.Clear(); } } // else if(!splitIslands) }
public void QueryNode(QuadTreeNode node, ObjectArray<QuadTreeNode> results, IndexedVector3 source,IndexedVector3 direction) { //if(node.children == null && node.Intersects(raySource,rayTarget)) // add the lowest level. if (node.children == null) { results.Add(node); #if DEBUG_ACCELERATOR if (BulletGlobals.gDebugDraw != null) { IndexedVector3 drawMin = new IndexedVector3(node.boundingBox.Min); IndexedVector3 drawMax = new IndexedVector3(node.boundingBox.Max); IndexedVector3 worldMin = LocalToWorld2(drawMin); IndexedVector3 worldMax = LocalToWorld2(drawMax); BulletGlobals.gDebugDraw.DrawAabb(worldMin, worldMax, new IndexedVector3(1, 1, 1)); } #endif } else { // simple rescursive for now. for (int i = 0; i < 4; ++i) { if (node.children[i].Intersects(source,direction)) { QueryNode(node.children[i], results, source,direction); } } } }
CollisionShape BuildLargeMesh() { //int vertStride = sizeof(IndexedVector3); //int indexStride = 3*sizeof(int); int vertStride = 1; int indexStride = 3; ObjectArray<IndexedVector3> vertexArray = new ObjectArray<IndexedVector3>(); for (int i = 0; i < vertices.Length; ++i) { vertexArray.Add(vertices[i]); } ObjectArray<int> intArray = new ObjectArray<int>(); for (int i = 0; i < indices.Length; ++i) { intArray.Add(indices[i]); } //TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(DemoMeshes.BUNNY_NUM_TRIANGLES, DemoMeshes.gBunnyIndices, 3, DemoMeshes.BUNNY_NUM_VERTICES, DemoMeshes.gBunnyVertices, 3); TriangleIndexVertexArray indexVertexArray = new TriangleIndexVertexArray(numTriangles, intArray, indexStride, vertexArray.Count, vertexArray, vertStride); TriangleMeshShape triangleMesh = new TriangleMeshShape(indexVertexArray); //TriangleMeshShape triangleMesh = new BvhTriangleMeshShape(indexVertexArray,true,true); return triangleMesh; }
protected virtual void SolveConstraints(ContactSolverInfo solverInfo) { //sorted version of all btTypedConstraint, based on islandId ObjectArray<TypedConstraint> sortedConstraints = new ObjectArray<TypedConstraint>(GetNumConstraints()); if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("solveConstraints"); } for (int i = 0; i < GetNumConstraints(); i++) { sortedConstraints.Add(m_constraints[i]); } // btAssert(0); //sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); sortedConstraints.Sort(new SortConstraintOnIslandPredicate()); ObjectArray<TypedConstraint> constraintsPtr = GetNumConstraints() > 0 ? sortedConstraints : null; InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, m_constraintSolver, constraintsPtr, sortedConstraints.Count, m_debugDrawer, m_dispatcher1); if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("prepareSolve"); } m_constraintSolver.PrepareSolve(GetCollisionWorld().GetNumCollisionObjects(), GetCollisionWorld().GetDispatcher().GetNumManifolds()); if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld) { BulletGlobals.g_streamWriter.WriteLine("buildAndProcessIsland"); } /// solve all the constraints for this island m_islandManager.BuildAndProcessIslands(GetCollisionWorld().GetDispatcher(), GetCollisionWorld(), solverCallback); solverCallback.ProcessConstraints(); m_constraintSolver.AllSolved(solverInfo, m_debugDrawer); }
protected void GImpactVsShapeFindPairs( ref IndexedMatrix trans0, ref IndexedMatrix trans1, GImpactShapeInterface shape0, CollisionShape shape1, ObjectArray<int> collided_primitives) { AABB boxshape = new AABB(); if (shape0.HasBoxSet()) { IndexedMatrix trans1to0 = trans0.Inverse(); //trans1to0 *= trans1; trans1to0 = trans1to0 * trans1; //trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0); shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max); if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max); } shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives); } else { shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max); AABB boxshape0 = new AABB(); int i = shape0.GetNumChildShapes(); while (i-- != 0) { shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max); if (boxshape.HasCollision(ref boxshape0)) { collided_primitives.Add(i); } } } }
public virtual void InternalProcessAllTriangles(IInternalTriangleIndexCallback callback,ref Vector3 aabbMin,ref Vector3 aabbMax) { int numtotalphysicsverts = 0; int part,graphicssubparts = GetNumSubParts(); Object vertexbase = null; Object indexbase = null; int indexstride = 3; PHY_ScalarType type = PHY_ScalarType.PHY_FLOAT; PHY_ScalarType gfxindextype = PHY_ScalarType.PHY_INTEGER; int stride = 0,numverts = 0 ,numtriangles = 0; ObjectArray<Vector3> triangle = new ObjectArray<Vector3>(3); // ugly. - could do with either a static , or allowing these methods to take a list or a fixed array... triangle.Add(Vector3.Zero); triangle.Add(Vector3.Zero); triangle.Add(Vector3.Zero); float graphicsBase = 0f; Vector3 meshScaling = GetScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype for (part=0;part<graphicssubparts ;part++) { getLockedReadOnlyVertexIndexBase(out vertexbase,out numverts,out type,out stride,out indexbase,out indexstride,out numtriangles,out gfxindextype,part); numtotalphysicsverts+=numtriangles*3; //upper bound switch (gfxindextype) { case PHY_ScalarType.PHY_INTEGER: { ObjectArray<int> indexList = (ObjectArray<int>)indexbase; //ObjectArray<float> vertexList = (ObjectArray<float>)vertexbase; // hack for now - need to tidy this.. if (vertexbase is ObjectArray<Vector3>) { ObjectArray<Vector3> vertexList = (ObjectArray<Vector3>)vertexbase; for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++) { // FIXME - Work ref the properindexing on this. int index = gfxindex * indexstride; int triIndex = (gfxindex * indexstride); triangle[0] = vertexList[indexList[triIndex]] * meshScaling; triangle[1] = vertexList[indexList[triIndex+1]] * meshScaling; triangle[2] = vertexList[indexList[triIndex+2]] * meshScaling; callback.InternalProcessTriangleIndex(triangle, part, gfxindex); } } else if (vertexbase is ObjectArray<float>) { //triangle[0] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z); //triangle[1] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z); //triangle[2] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z); ObjectArray<float> vertexList = (ObjectArray<float>)vertexbase; for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++) { // FIXME - Work ref the properindexing on this. int index = gfxindex * indexstride; int triIndex = (gfxindex * indexstride); // ugly!! triangle[0] = new Vector3(vertexList[indexList[triIndex]], vertexList[indexList[triIndex] + 1], vertexList[indexList[triIndex] + 2]) * meshScaling; triangle[1] = new Vector3(vertexList[indexList[triIndex+1]], vertexList[indexList[triIndex+1] + 1], vertexList[indexList[triIndex+1] + 2]) * meshScaling; triangle[2] = new Vector3(vertexList[indexList[triIndex+2]], vertexList[indexList[triIndex+2] + 1], vertexList[indexList[triIndex+2] + 2]) * meshScaling; callback.InternalProcessTriangleIndex(triangle, part, gfxindex); } } else { Debug.Assert(false); // unsupported type .... } break; } case PHY_ScalarType.PHY_SHORT: { ObjectArray<ushort> indexList = (ObjectArray<ushort>)indexbase; if (vertexbase is ObjectArray<Vector3>) { ObjectArray<Vector3> vertexList = (ObjectArray<Vector3>)vertexbase; for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++) { // FIXME - Work ref the properindexing on this. int index = gfxindex * indexstride; int triIndex = (gfxindex * indexstride); triangle[0] = vertexList[indexList[triIndex]] * meshScaling; triangle[1] = vertexList[indexList[triIndex + 1]] * meshScaling; triangle[2] = vertexList[indexList[triIndex + 2]] * meshScaling; callback.InternalProcessTriangleIndex(triangle, part, gfxindex); } } else if (vertexbase is ObjectArray<float>) { //triangle[0] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z); //triangle[1] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z); //triangle[2] = new Vector3(vertexList[indexList[triIndex]] * meshScaling.X, vertexList[indexList[triIndex + 1]] * meshScaling.Y, vertexList[indexList[triIndex + 2]] * meshScaling.Z); ObjectArray<float> vertexList = (ObjectArray<float>)vertexbase; for (int gfxindex = 0; gfxindex < numtriangles; gfxindex++) { // FIXME - Work ref the properindexing on this. int index = gfxindex * indexstride; int triIndex = (gfxindex * indexstride); // ugly!! triangle[0] = new Vector3(vertexList[indexList[triIndex]], vertexList[indexList[triIndex] + 1], vertexList[indexList[triIndex] + 2]) * meshScaling; triangle[1] = new Vector3(vertexList[indexList[triIndex + 1]], vertexList[indexList[triIndex + 1] + 1], vertexList[indexList[triIndex + 1] + 2]) * meshScaling; triangle[2] = new Vector3(vertexList[indexList[triIndex + 2]], vertexList[indexList[triIndex + 2] + 1], vertexList[indexList[triIndex + 2] + 2]) * meshScaling; callback.InternalProcessTriangleIndex(triangle, part, gfxindex); } } else { Debug.Assert(false); // unsupported type .... } break; } default: { Debug.Assert((gfxindextype == PHY_ScalarType.PHY_INTEGER) || (gfxindextype == PHY_ScalarType.PHY_SHORT)); break; } } UnLockReadOnlyVertexBase(part); } }
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(); } }
///the clipFace method is used internally public static void ClipFace(ObjectArray<IndexedVector3> pVtxIn, ObjectArray<IndexedVector3> ppVtxOut, ref IndexedVector3 planeNormalWS, float planeEqWS) { int ve; float ds, de; int numVerts = pVtxIn.Count; if (numVerts < 2) return; IndexedVector3 firstVertex = pVtxIn[pVtxIn.Count - 1]; IndexedVector3 endVertex = pVtxIn[0]; ds = IndexedVector3.Dot(ref planeNormalWS, ref firstVertex); ds += planeEqWS; for (ve = 0; ve < numVerts; ve++) { endVertex = pVtxIn[ve]; de = IndexedVector3.Dot(ref planeNormalWS, ref endVertex); de += planeEqWS; if (ds < 0) { if (de < 0) { // Start < 0, end < 0, so output endVertex ppVtxOut.Add(endVertex); } else { // Start < 0, end >= 0, so output intersection ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de)))); } } else { if (de < 0) { // Start >= 0, end < 0 so output intersection and end ppVtxOut.Add(MathUtil.Vector3Lerp(ref firstVertex, ref endVertex, (float)(ds * 1.0f / (ds - de)))); ppVtxOut.Add(endVertex); } } firstVertex = endVertex; ds = de; } }
public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { IndexedVector3 separatingNormal = separatingNormal1.Normalized(); IndexedVector3 c0 = transA * hullA.m_localCenter; IndexedVector3 c1 = transB * hullB.m_localCenter; IndexedVector3 DeltaC2 = c0 - c1; float curMaxDist = maxDist; int closestFaceB = -1; float dmax = float.MinValue; { for (int face = 0; face < hullB.m_faces.Count; face++) { IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); IndexedVector3 WorldNormal = transB._basis * Normal; float d = IndexedVector3.Dot(WorldNormal, separatingNormal); if (d > dmax) { dmax = d; closestFaceB = face; } } } // setup initial clip face (minimizing face from hull B) ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>(); { Face polyB = hullB.m_faces[closestFaceB]; int numVertices = polyB.m_indices.Count; for (int e0 = 0; e0 < numVertices; e0++) { IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]]; // check this to see if it is transposed version worldVertsB1.Add(transB * b); } } if (closestFaceB >= 0) { ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut); } }
//InplaceSolverIslandCallback operator=(InplaceSolverIslandCallback& other) //{ // Debug.Assert(false); // //(void)other; // return *this; //} public virtual void ProcessIsland(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifolds, int numManifolds, int islandId) { if (islandId < 0) { if (numManifolds + m_numConstraints > 0) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id m_solver.SolveGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, m_solverInfo, m_debugDrawer, m_dispatcher); } } else { //also add all non-contact constraints/joints for this island ObjectArray<TypedConstraint> startConstraint = new ObjectArray<TypedConstraint>(); int numCurConstraints = 0; int i = 0; //find the first constraint for this island for (i = 0; i < m_numConstraints; i++) { if (DiscreteDynamicsWorld.GetConstraintIslandId(m_sortedConstraints[i]) == islandId) { // FIXME - Do we need add everything after i to mirror the pointer? for (int k = i; k < m_numConstraints; ++k) { startConstraint.Add(m_sortedConstraints[k]); } break; } } //count the number of constraints in this island for (; i < m_numConstraints; i++) { if (DiscreteDynamicsWorld.GetConstraintIslandId(m_sortedConstraints[i]) == islandId) { numCurConstraints++; } } if (m_solverInfo.m_minimumSolverBatchSize <= 1) { ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive if (numManifolds + numCurConstraints != 0) { m_solver.SolveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, m_solverInfo, m_debugDrawer, m_dispatcher); } } else { for (i = 0; i < numBodies; i++) { m_bodies.Add(bodies[i]); } for (i = 0; i < numManifolds; i++) { m_manifolds.Add(manifolds[i]); } for (i = 0; i < numCurConstraints; i++) { m_constraints.Add(startConstraint[i]); } if ((m_constraints.Count + m_manifolds.Count) > m_solverInfo.m_minimumSolverBatchSize) { ProcessConstraints(); } else { //printf("deferred\n"); } } // ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive // if (numManifolds + numCurConstraints > 0) // { ////solveGroup(ObjectArray<CollisionObject> bodies,int numBodies,ObjectArray<PersistentManifold> manifold,int numManifolds,ObjectArray<TypedConstraint> constraints,int numConstraints, ContactSolverInfo info,IDebugDraw debugDrawer, IDispatcher dispatcher); // m_solver.solveGroup(bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_dispatcher); // } } }