public void ProcessConstraints() { if (m_manifolds.Count + m_constraints.Count > 0) { m_solver.SolveGroup(m_bodies, m_bodies.Count, m_manifolds, 0, m_manifolds.Count, m_constraints, 0, m_constraints.Count, m_solverInfo, m_debugDrawer, m_dispatcher); } m_bodies.Clear(); m_manifolds.Clear(); m_constraints.Clear(); }
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); sut.Clear(); Assert.Equal(0, sut.Count); }
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 static void CollideTT(DbvtNode root0, DbvtNode root1, ICollide collideable) { CollideTTCount++; Debug.Assert(CollideTTCount < 2); CollideTTStack.Clear(); if (root0 != null && root1 != null) { int depth = 1; int treshold = DOUBLE_STACKSIZE - 4; CollideTTStack[0] = new sStkNN(root0, root1); do { sStkNN p = CollideTTStack[--depth]; if (depth > treshold) { CollideTTStack.Resize(CollideTTStack.Count * 2); treshold = CollideTTStack.Count - 4; } if (p.a == p.b) { if (p.a.IsInternal()) { CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.a._children[0]); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.a._children[1]); CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.a._children[1]); } } else if (DbvtAabbMm.Intersect(ref p.a.volume, ref p.b.volume)) { if (p.a.IsInternal()) { if (p.b.IsInternal()) { CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b._children[0]); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b._children[0]); CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b._children[1]); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b._children[1]); } else { CollideTTStack[depth++] = new sStkNN(p.a._children[0], p.b); CollideTTStack[depth++] = new sStkNN(p.a._children[1], p.b); } } else { if (p.b.IsInternal()) { CollideTTStack[depth++] = new sStkNN(p.a, p.b._children[0]); CollideTTStack[depth++] = new sStkNN(p.a, p.b._children[1]); } else { collideable.Process(p.a, p.b); } } } } while (depth > 0); } CollideTTCount--; }
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut) { ObjectArray <IndexedVector3> worldVertsB2 = new ObjectArray <IndexedVector3>(); ObjectArray <IndexedVector3> pVtxIn = worldVertsB1; ObjectArray <IndexedVector3> pVtxOut = worldVertsB2; pVtxOut.Capacity = pVtxIn.Count; int closestFaceA = -1; { float dmin = float.MaxValue; for (int face = 0; face < hullA.m_faces.Count; face++) { IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]); IndexedVector3 faceANormalWS = transA._basis * Normal; float d = IndexedVector3.Dot(faceANormalWS, separatingNormal); if (d < dmin) { dmin = d; closestFaceA = face; } } } if (closestFaceA < 0) { return; } Face polyA = hullA.m_faces[closestFaceA]; // clip polygon to back of planes of all faces of hull A that are adjacent to witness face int numContacts = pVtxIn.Count; int numVerticesA = polyA.m_indices.Count; for (int e0 = 0; e0 < numVerticesA; e0++) { IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]]; IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]]; IndexedVector3 edge0 = a - b; IndexedVector3 WorldEdge0 = transA._basis * edge0; IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0); IndexedVector3 worldA1 = transA * a; float planeEqWS1 = -worldA1.Dot(planeNormalWS1); //int otherFace=0; #if BLA1 int otherFace = polyA.m_connectedFaces[e0]; btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]); btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); #else IndexedVector3 planeNormalWS = planeNormalWS1; float planeEqWS = planeEqWS1; #endif //clip face ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS); //btSwap(pVtxIn,pVtxOut); ObjectArray <IndexedVector3> temp = pVtxIn; pVtxIn = pVtxOut; pVtxOut = temp; pVtxOut.Clear(); } //#define ONLY_REPORT_DEEPEST_POINT IndexedVector3 point; // only keep points that are behind the witness face { IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); float localPlaneEq = polyA.m_plane[3]; IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal; float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin); for (int i = 0; i < pVtxIn.Count; i++) { float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS; if (depth <= minDist) { // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); depth = minDist; } if (depth <= maxDist && depth >= minDist) { IndexedVector3 point2 = pVtxIn[i]; #if ONLY_REPORT_DEEPEST_POINT curMaxDist = depth; #else #if false if (depth < -3) { printf("error in btPolyhedralContactClipping depth = %f\n", depth); printf("likely wrong separatingNormal passed in\n"); } #endif resultOut.AddContactPoint(ref separatingNormal, ref point2, depth); #endif } } } #if ONLY_REPORT_DEEPEST_POINT if (curMaxDist < maxDist) { resultOut.AddContactPoint(ref separatingNormal, ref point, curMaxDist); } #endif //ONLY_REPORT_DEEPEST_POINT }
/// 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 override void Cleanup() { base.Cleanup(); m_mesh_parts.Clear(); }
public void GetTrianglesIntersectingSphere(ref BoundingSphere sphere, Vector3?referenceNormalVector, float?maxAngle, List <MyTriangle_Vertex_Normals> retTriangles, int maxNeighbourTriangles) { var aabb = BoundingBoxHelper.InitialBox; BoundingBoxHelper.AddSphere(ref sphere, ref aabb); AABB gi_aabb = new AABB(ref aabb.Min, ref aabb.Max); m_overlappedTriangles.Clear(); if (m_bvh.BoxQuery(ref gi_aabb, m_overlappedTriangles)) { // temporary variable for storing tirngle boundingbox info BoundingBox triangleBoundingBox = new BoundingBox(); for (int i = 0; i < m_overlappedTriangles.Count; i++) { var triangleIndex = m_overlappedTriangles[i]; // If we reached end of the buffer of neighbour triangles, we stop adding new ones. This is better behavior than throwing exception because of array overflow. if (retTriangles.Count == maxNeighbourTriangles) { return; } m_model.GetTriangleBoundingBox(triangleIndex, ref triangleBoundingBox); // First test intersection of triangleVertexes's bounding box with bounding sphere. And only if they overlap or intersect, do further intersection tests. if (MyUtils.IsBoxIntersectingSphere(triangleBoundingBox, ref sphere) == true) { //if (m_triangleIndices[value] != ignoreTriangleWithIndex) { // See that we swaped vertex indices!! MyTriangle_Vertexes triangle; MyTriangle_Normals triangleNormals; MyTriangle_Normals triangleBinormals; MyTriangle_Normals triangleTangents; MyTriangleVertexIndices triangleIndices = m_model.Triangles[triangleIndex]; m_model.GetVertex(triangleIndices.I0, triangleIndices.I2, triangleIndices.I1, out triangle.Vertex0, out triangle.Vertex1, out triangle.Vertex2); /* * triangle.Vertex0 = m_model.GetVertex(triangleIndices.I0); * triangle.Vertex1 = m_model.GetVertex(triangleIndices.I2); * triangle.Vertex2 = m_model.GetVertex(triangleIndices.I1); */ triangleNormals.Normal0 = m_model.GetVertexNormal(triangleIndices.I0); triangleNormals.Normal1 = m_model.GetVertexNormal(triangleIndices.I2); triangleNormals.Normal2 = m_model.GetVertexNormal(triangleIndices.I1); if (MinerWars.AppCode.Game.Render.MyRenderConstants.RenderQualityProfile.ForwardRender) { triangleBinormals.Normal0 = triangleNormals.Normal0; triangleBinormals.Normal1 = triangleNormals.Normal1; triangleBinormals.Normal2 = triangleNormals.Normal2; triangleTangents.Normal0 = triangleNormals.Normal0; triangleTangents.Normal1 = triangleNormals.Normal1; triangleTangents.Normal2 = triangleNormals.Normal2; } else { triangleBinormals.Normal0 = m_model.GetVertexBinormal(triangleIndices.I0); triangleBinormals.Normal1 = m_model.GetVertexBinormal(triangleIndices.I2); triangleBinormals.Normal2 = m_model.GetVertexBinormal(triangleIndices.I1); triangleTangents.Normal0 = m_model.GetVertexTangent(triangleIndices.I0); triangleTangents.Normal1 = m_model.GetVertexTangent(triangleIndices.I2); triangleTangents.Normal2 = m_model.GetVertexTangent(triangleIndices.I1); } MyPlane trianglePlane = new MyPlane(ref triangle); if (MyUtils.GetSphereTriangleIntersection(ref sphere, ref trianglePlane, ref triangle) != null) { Vector3 triangleNormal = MyUtils.GetNormalVectorFromTriangle(ref triangle); if ((referenceNormalVector.HasValue == false) || (maxAngle.HasValue == false) || ((MyUtils.GetAngleBetweenVectors(referenceNormalVector.Value, triangleNormal) <= maxAngle))) { MyTriangle_Vertex_Normals retTriangle; retTriangle.Vertexes = triangle; retTriangle.Normals = triangleNormals; retTriangle.Binormals = triangleBinormals; retTriangle.Tangents = triangleTangents; retTriangles.Add(retTriangle); } } } } } } }
public void GetTrianglesIntersectingSphere(ref BoundingSphereD sphere, Vector3?referenceNormalVector, float?maxAngle, List <MyTriangle_Vertex_Normals> retTriangles, int maxNeighbourTriangles) { var aabb = BoundingBox.CreateInvalid(); BoundingSphere sphereF = (BoundingSphere)sphere; aabb.Include(ref sphereF); AABB gi_aabb = new AABB(aabb.Min.ToBullet(), aabb.Max.ToBullet()); m_overlappedTriangles.Clear(); if (m_bvh.BoxQuery(ref gi_aabb, m_overlappedTriangles)) { // temporary variable for storing tirngle boundingbox info BoundingBox triangleBoundingBox = new BoundingBox(); for (int i = 0; i < m_overlappedTriangles.Count; i++) { var triangleIndex = m_overlappedTriangles[i]; // If we reached end of the buffer of neighbour triangles, we stop adding new ones. This is better behavior than throwing exception because of array overflow. if (retTriangles.Count == maxNeighbourTriangles) { return; } m_model.GetTriangleBoundingBox(triangleIndex, ref triangleBoundingBox); // First test intersection of triangleVertexes's bounding box with bounding sphere. And only if they overlap or intersect, do further intersection tests. if (triangleBoundingBox.Intersects(ref sphere)) { //if (m_triangleIndices[value] != ignoreTriangleWithIndex) { // See that we swaped vertex indices!! MyTriangle_Vertexes triangle; MyTriangle_Normals triangleNormals; //MyTriangle_Normals triangleTangents; MyTriangleVertexIndices triangleIndices = m_model.Triangles[triangleIndex]; m_model.GetVertex(triangleIndices.I0, triangleIndices.I2, triangleIndices.I1, out triangle.Vertex0, out triangle.Vertex1, out triangle.Vertex2); /* * triangle.Vertex0 = m_model.GetVertex(triangleIndices.I0); * triangle.Vertex1 = m_model.GetVertex(triangleIndices.I2); * triangle.Vertex2 = m_model.GetVertex(triangleIndices.I1); */ triangleNormals.Normal0 = m_model.GetVertexNormal(triangleIndices.I0); triangleNormals.Normal1 = m_model.GetVertexNormal(triangleIndices.I2); triangleNormals.Normal2 = m_model.GetVertexNormal(triangleIndices.I1); PlaneD trianglePlane = new PlaneD(triangle.Vertex0, triangle.Vertex1, triangle.Vertex2); if (MyUtils.GetSphereTriangleIntersection(ref sphere, ref trianglePlane, ref triangle) != null) { Vector3 triangleNormal = MyUtils.GetNormalVectorFromTriangle(ref triangle); if ((referenceNormalVector.HasValue == false) || (maxAngle.HasValue == false) || ((MyUtils.GetAngleBetweenVectors(referenceNormalVector.Value, triangleNormal) <= maxAngle))) { MyTriangle_Vertex_Normals retTriangle; retTriangle.Vertices = triangle; retTriangle.Normals = triangleNormals; retTriangles.Add(retTriangle); } } } } } } }