private void DetectSoftSoft(SoftBody body1, SoftBody body2) { List <int> my = potentialTriangleLists.GetNew(); List <int> other = potentialTriangleLists.GetNew(); body1.dynamicTree.Query(other, my, body2.dynamicTree); for (int i = 0; i < other.Count; i++) { SoftBody.Triangle myTriangle = body1.dynamicTree.GetUserData(my[i]); SoftBody.Triangle otherTriangle = body2.dynamicTree.GetUserData(other[i]); FPVector point, normal; FP penetration; bool result; result = XenoCollide.Detect(myTriangle, otherTriangle, ref FPMatrix.InternalIdentity, ref FPMatrix.InternalIdentity, ref FPVector.InternalZero, ref FPVector.InternalZero, out point, out normal, out penetration); if (result) { int minIndexMy = FindNearestTrianglePoint(body1, my[i], ref point); int minIndexOther = FindNearestTrianglePoint(body2, other[i], ref point); RaiseCollisionDetected(body1.VertexBodies[minIndexMy], body2.VertexBodies[minIndexOther], ref point, ref point, ref normal, penetration); } } my.Clear(); other.Clear(); potentialTriangleLists.GiveBack(my); potentialTriangleLists.GiveBack(other); }
public static int FindNearestTrianglePoint(SoftBody sb, int id, ref FPVector point) { SoftBody.Triangle triangle = sb.dynamicTree.GetUserData(id); FPVector p; p = sb.VertexBodies[triangle.indices.I0].position; FPVector.Subtract(ref p, ref point, out p); FP length0 = p.sqrMagnitude; p = sb.VertexBodies[triangle.indices.I1].position; FPVector.Subtract(ref p, ref point, out p); FP length1 = p.sqrMagnitude; p = sb.VertexBodies[triangle.indices.I2].position; FPVector.Subtract(ref p, ref point, out p); FP length2 = p.sqrMagnitude; if (length0 < length1) { if (length0 < length2) { return(triangle.indices.I0); } else { return(triangle.indices.I2); } } else { if (length1 < length2) { return(triangle.indices.I1); } else { return(triangle.indices.I2); } } }
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody) { if (rigidBody.Shape is Multishape) { Multishape ms = (rigidBody.Shape as Multishape); ms = ms.RequestWorkingClone(); FPBBox transformedBoundingBox = softBody.BoundingBox; transformedBoundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation); int msLength = ms.Prepare(ref transformedBoundingBox); List <int> detected = potentialTriangleLists.GetNew(); softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox); foreach (int i in detected) { SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i); FPVector point, normal; FP penetration; bool result; for (int e = 0; e < msLength; e++) { ms.SetCurrentShape(e); result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref FPMatrix.InternalIdentity, ref rigidBody.position, ref FPVector.InternalZero, out point, out normal, out penetration); if (result) { int minIndex = FindNearestTrianglePoint(softBody, i, ref point); RaiseCollisionDetected(rigidBody, softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration); } } } detected.Clear(); potentialTriangleLists.GiveBack(detected); ms.ReturnWorkingClone(); } else { List <int> detected = potentialTriangleLists.GetNew(); softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox); foreach (int i in detected) { SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i); FPVector point, normal; FP penetration; bool result; result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref FPMatrix.InternalIdentity, ref rigidBody.position, ref FPVector.InternalZero, out point, out normal, out penetration); if (result) { int minIndex = FindNearestTrianglePoint(softBody, i, ref point); RaiseCollisionDetected(rigidBody, softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration); } } detected.Clear(); potentialTriangleLists.GiveBack(detected); } }