public virtual void DoSelfCollision(CollisionDetectedHandler collision) { bool flag = !this.selfCollision; if (!flag) { for (int i = 0; i < this.points.Count; i++) { this.queryList.Clear(); this.dynamicTree.Query(this.queryList, ref this.points[i].boundingBox); for (int j = 0; j < this.queryList.Count; j++) { SoftBody.Triangle userData = this.dynamicTree.GetUserData(this.queryList[j]); bool flag2 = userData.VertexBody1 != this.points[i] && userData.VertexBody2 != this.points[i] && userData.VertexBody3 != this.points[i]; if (flag2) { TSVector tSVector; TSVector normal; FP penetration; bool flag3 = XenoCollide.Detect(this.points[i].Shape, userData, ref this.points[i].orientation, ref TSMatrix.InternalIdentity, ref this.points[i].position, ref TSVector.InternalZero, out tSVector, out normal, out penetration); if (flag3) { int index = CollisionSystem.FindNearestTrianglePoint(this, this.queryList[j], ref tSVector); collision(this.points[i], this.points[index], tSVector, tSVector, normal, penetration); } } } } } }
private void DetectSoftSoft(SoftBody body1, SoftBody body2) { List <int> @new = this.potentialTriangleLists.GetNew(); List <int> new2 = this.potentialTriangleLists.GetNew(); body1.dynamicTree.Query(new2, @new, body2.dynamicTree); for (int i = 0; i < new2.Count; i++) { SoftBody.Triangle userData = body1.dynamicTree.GetUserData(@new[i]); SoftBody.Triangle userData2 = body2.dynamicTree.GetUserData(new2[i]); TSVector tSVector; TSVector tSVector2; FP penetration; bool flag = XenoCollide.Detect(userData, userData2, ref TSMatrix.InternalIdentity, ref TSMatrix.InternalIdentity, ref TSVector.InternalZero, ref TSVector.InternalZero, out tSVector, out tSVector2, out penetration); bool flag2 = flag; if (flag2) { int index = CollisionSystem.FindNearestTrianglePoint(body1, @new[i], ref tSVector); int index2 = CollisionSystem.FindNearestTrianglePoint(body2, new2[i], ref tSVector); this.RaiseCollisionDetected(body1.VertexBodies[index], body2.VertexBodies[index2], ref tSVector, ref tSVector, ref tSVector2, penetration); } } @new.Clear(); new2.Clear(); this.potentialTriangleLists.GiveBack(@new); this.potentialTriangleLists.GiveBack(new2); }
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody) { bool flag = rigidBody.Shape is Multishape; if (flag) { Multishape multishape = rigidBody.Shape as Multishape; multishape = multishape.RequestWorkingClone(); TSBBox boundingBox = softBody.BoundingBox; boundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation); int num = multishape.Prepare(ref boundingBox); List <int> @new = this.potentialTriangleLists.GetNew(); softBody.dynamicTree.Query(@new, ref rigidBody.boundingBox); foreach (int current in @new) { SoftBody.Triangle userData = softBody.dynamicTree.GetUserData(current); for (int i = 0; i < num; i++) { multishape.SetCurrentShape(i); TSVector tSVector; TSVector tSVector2; FP penetration; bool flag2 = XenoCollide.Detect(multishape, userData, ref rigidBody.orientation, ref TSMatrix.InternalIdentity, ref rigidBody.position, ref TSVector.InternalZero, out tSVector, out tSVector2, out penetration); bool flag3 = flag2; if (flag3) { int index = CollisionSystem.FindNearestTrianglePoint(softBody, current, ref tSVector); this.RaiseCollisionDetected(rigidBody, softBody.VertexBodies[index], ref tSVector, ref tSVector, ref tSVector2, penetration); } } } @new.Clear(); this.potentialTriangleLists.GiveBack(@new); multishape.ReturnWorkingClone(); } else { List <int> new2 = this.potentialTriangleLists.GetNew(); softBody.dynamicTree.Query(new2, ref rigidBody.boundingBox); foreach (int current2 in new2) { SoftBody.Triangle userData2 = softBody.dynamicTree.GetUserData(current2); TSVector tSVector3; TSVector tSVector4; FP penetration2; bool flag4 = XenoCollide.Detect(rigidBody.Shape, userData2, ref rigidBody.orientation, ref TSMatrix.InternalIdentity, ref rigidBody.position, ref TSVector.InternalZero, out tSVector3, out tSVector4, out penetration2); bool flag5 = flag4; if (flag5) { int index2 = CollisionSystem.FindNearestTrianglePoint(softBody, current2, ref tSVector3); this.RaiseCollisionDetected(rigidBody, softBody.VertexBodies[index2], ref tSVector3, ref tSVector3, ref tSVector4, penetration2); } } new2.Clear(); this.potentialTriangleLists.GiveBack(new2); } }
private void DetectRigidRigid(RigidBody body1, RigidBody body2) { bool flag = body1.Shape is Multishape; bool flag2 = body2.Shape is Multishape; bool flag3 = this.speculativeContacts || body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts; bool flag4 = !flag && !flag2; if (flag4) { TSVector tSVector; TSVector value; FP fP; bool flag5 = XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out tSVector, out value, out fP); if (flag5) { TSVector tSVector2; TSVector tSVector3; this.FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref tSVector, ref value, out tSVector2, out tSVector3); this.RaiseCollisionDetected(body1, body2, ref tSVector2, ref tSVector3, ref value, fP); } else { bool flag6 = flag3; if (flag6) { TSVector value2; TSVector value3; bool flag7 = GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out value2, out value3, out value); if (flag7) { TSVector value4 = value3 - value2; bool flag8 = value4.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude; if (flag8) { fP = value4 * value; bool flag9 = fP < FP.Zero; if (flag9) { this.RaiseCollisionDetected(body1, body2, ref value2, ref value3, ref value, fP); } } } } } } else { bool flag10 = flag & flag2; if (flag10) { Multishape multishape = body1.Shape as Multishape; Multishape multishape2 = body2.Shape as Multishape; multishape = multishape.RequestWorkingClone(); multishape2 = multishape2.RequestWorkingClone(); TSBBox boundingBox = body2.boundingBox; boundingBox.InverseTransform(ref body1.position, ref body1.orientation); int num = multishape.Prepare(ref boundingBox); boundingBox = body1.boundingBox; boundingBox.InverseTransform(ref body2.position, ref body2.orientation); int num2 = multishape2.Prepare(ref boundingBox); bool flag11 = num == 0 || num2 == 0; if (flag11) { multishape.ReturnWorkingClone(); multishape2.ReturnWorkingClone(); } else { for (int i = 0; i < num; i++) { multishape.SetCurrentShape(i); for (int j = 0; j < num2; j++) { multishape2.SetCurrentShape(j); TSVector tSVector; TSVector value; FP fP; bool flag12 = XenoCollide.Detect(multishape, multishape2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out tSVector, out value, out fP); if (flag12) { TSVector tSVector4; TSVector tSVector5; this.FindSupportPoints(body1, body2, multishape, multishape2, ref tSVector, ref value, out tSVector4, out tSVector5); this.RaiseCollisionDetected(body1, body2, ref tSVector4, ref tSVector5, ref value, fP); } else { bool flag13 = flag3; if (flag13) { TSVector value5; TSVector value6; bool flag14 = GJKCollide.ClosestPoints(multishape, multishape2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out value5, out value6, out value); if (flag14) { TSVector value7 = value6 - value5; bool flag15 = value7.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude; if (flag15) { fP = value7 * value; bool flag16 = fP < FP.Zero; if (flag16) { this.RaiseCollisionDetected(body1, body2, ref value5, ref value6, ref value, fP); } } } } } } } multishape.ReturnWorkingClone(); multishape2.ReturnWorkingClone(); } } else { bool flag17 = body2.Shape is Multishape; RigidBody rigidBody; RigidBody rigidBody2; if (flag17) { rigidBody = body2; rigidBody2 = body1; } else { rigidBody2 = body2; rigidBody = body1; } Multishape multishape3 = rigidBody.Shape as Multishape; multishape3 = multishape3.RequestWorkingClone(); TSBBox boundingBox2 = rigidBody2.boundingBox; boundingBox2.InverseTransform(ref rigidBody.position, ref rigidBody.orientation); int num3 = multishape3.Prepare(ref boundingBox2); bool flag18 = num3 == 0; if (flag18) { multishape3.ReturnWorkingClone(); } else { for (int k = 0; k < num3; k++) { multishape3.SetCurrentShape(k); TSVector tSVector; TSVector value; FP fP; bool flag19 = XenoCollide.Detect(multishape3, rigidBody2.Shape, ref rigidBody.orientation, ref rigidBody2.orientation, ref rigidBody.position, ref rigidBody2.position, out tSVector, out value, out fP); if (flag19) { TSVector tSVector6; TSVector tSVector7; this.FindSupportPoints(rigidBody, rigidBody2, multishape3, rigidBody2.Shape, ref tSVector, ref value, out tSVector6, out tSVector7); bool flag20 = this.useTerrainNormal && multishape3 is TerrainShape; if (flag20) { (multishape3 as TerrainShape).CollisionNormal(out value); TSVector.Transform(ref value, ref rigidBody.orientation, out value); } else { bool flag21 = this.useTriangleMeshNormal && multishape3 is TriangleMeshShape; if (flag21) { (multishape3 as TriangleMeshShape).CollisionNormal(out value); TSVector.Transform(ref value, ref rigidBody.orientation, out value); } } this.RaiseCollisionDetected(rigidBody, rigidBody2, ref tSVector6, ref tSVector7, ref value, fP); } else { bool flag22 = flag3; if (flag22) { TSVector value8; TSVector value9; bool flag23 = GJKCollide.ClosestPoints(multishape3, rigidBody2.Shape, ref rigidBody.orientation, ref rigidBody2.orientation, ref rigidBody.position, ref rigidBody2.position, out value8, out value9, out value); if (flag23) { TSVector value10 = value9 - value8; bool flag24 = value10.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude; if (flag24) { fP = value10 * value; bool flag25 = fP < FP.Zero; if (flag25) { this.RaiseCollisionDetected(rigidBody, rigidBody2, ref value8, ref value9, ref value, fP); } } } } } } multishape3.ReturnWorkingClone(); } } } }
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector normal, out FP penetration) { TSVector zero = TSVector.zero; TSVector zero2 = TSVector.zero; TSVector zero3 = TSVector.zero; point = (normal = TSVector.zero); penetration = FP.Zero; TSVector tSVector; support1.SupportCenter(out tSVector); TSVector.Transform(ref tSVector, ref orientation1, out tSVector); TSVector.Add(ref position1, ref tSVector, out tSVector); TSVector tSVector2; support2.SupportCenter(out tSVector2); TSVector.Transform(ref tSVector2, ref orientation2, out tSVector2); TSVector.Add(ref position2, ref tSVector2, out tSVector2); TSVector tSVector3; TSVector.Subtract(ref tSVector2, ref tSVector, out tSVector3); bool flag = tSVector3.IsNearlyZero(); if (flag) { tSVector3 = new TSVector(FP.EN4, 0, 0); } TSVector tSVector4 = tSVector3; TSVector.Negate(ref tSVector3, out normal); TSVector tSVector5; XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector5); TSVector tSVector6; XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector6); TSVector tSVector7; TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector7); bool flag2 = TSVector.Dot(ref tSVector7, ref normal) <= FP.Zero; bool result; if (flag2) { result = false; } else { TSVector.Cross(ref tSVector7, ref tSVector3, out normal); bool flag3 = normal.IsNearlyZero(); if (flag3) { TSVector.Subtract(ref tSVector7, ref tSVector3, out normal); normal.Normalize(); point = tSVector5; TSVector.Add(ref point, ref tSVector6, out point); TSVector.Multiply(ref point, FP.Half, out point); TSVector tSVector8; TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector8); penetration = TSVector.Dot(ref tSVector8, ref normal); result = true; } else { TSVector.Negate(ref normal, out tSVector4); TSVector tSVector9; XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector9); TSVector tSVector10; XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector10); TSVector tSVector11; TSVector.Subtract(ref tSVector10, ref tSVector9, out tSVector11); bool flag4 = TSVector.Dot(ref tSVector11, ref normal) <= FP.Zero; if (flag4) { result = false; } else { TSVector tSVector8; TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8); TSVector tSVector12; TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); FP x = TSVector.Dot(ref normal, ref tSVector3); bool flag5 = x > FP.Zero; if (flag5) { TSVector.Swap(ref tSVector7, ref tSVector11); TSVector.Swap(ref tSVector5, ref tSVector9); TSVector.Swap(ref tSVector6, ref tSVector10); TSVector.Negate(ref normal, out normal); Debug.Log("normal: " + normal); } int num = 0; int num2 = 0; bool flag6 = false; while (true) { bool flag7 = num2 > 34; if (flag7) { break; } num2++; TSVector.Negate(ref normal, out tSVector4); TSVector tSVector13; XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector13); TSVector tSVector14; XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector14); TSVector tSVector15; TSVector.Subtract(ref tSVector14, ref tSVector13, out tSVector15); bool flag8 = TSVector.Dot(ref tSVector15, ref normal) <= FP.Zero; if (flag8) { goto Block_7; } TSVector.Cross(ref tSVector7, ref tSVector15, out tSVector8); bool flag9 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero; if (flag9) { tSVector11 = tSVector15; tSVector9 = tSVector13; tSVector10 = tSVector14; TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8); TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); } else { TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8); bool flag10 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero; if (!flag10) { goto IL_385; } tSVector7 = tSVector15; tSVector5 = tSVector13; tSVector6 = tSVector14; TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector8); TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); } } result = false; return(result); Block_7: result = false; return(result); IL_385: while (true) { num++; TSVector.Subtract(ref tSVector11, ref tSVector7, out tSVector8); TSVector tSVector15; TSVector.Subtract(ref tSVector15, ref tSVector7, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); bool flag11 = normal.IsNearlyZero(); if (flag11) { break; } normal.Normalize(); FP x2 = TSVector.Dot(ref normal, ref tSVector7); bool flag12 = x2 >= 0 && !flag6; if (flag12) { flag6 = true; } TSVector.Negate(ref normal, out tSVector4); XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out zero); XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out zero2); TSVector.Subtract(ref zero2, ref zero, out zero3); TSVector.Subtract(ref zero3, ref tSVector15, out tSVector8); FP x3 = TSVector.Dot(ref tSVector8, ref normal); penetration = TSVector.Dot(ref zero3, ref normal); bool flag13 = x3 <= XenoCollide.CollideEpsilon || penetration <= FP.Zero || num > 34; if (flag13) { goto Block_15; } TSVector.Cross(ref zero3, ref tSVector3, out tSVector8); FP x4 = TSVector.Dot(ref tSVector8, ref tSVector7); bool flag14 = x4 >= FP.Zero; if (flag14) { x4 = TSVector.Dot(ref tSVector8, ref tSVector11); bool flag15 = x4 >= FP.Zero; if (flag15) { tSVector7 = zero3; tSVector5 = zero; tSVector6 = zero2; } else { tSVector15 = zero3; TSVector tSVector13 = zero; TSVector tSVector14 = zero2; } } else { x4 = TSVector.Dot(ref tSVector8, ref tSVector15); bool flag16 = x4 >= FP.Zero; if (flag16) { tSVector11 = zero3; tSVector9 = zero; tSVector10 = zero2; } else { tSVector7 = zero3; tSVector5 = zero; tSVector6 = zero2; } } } result = true; return(result); Block_15: bool flag17 = flag6; if (flag17) { TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8); TSVector tSVector15; FP fP = TSVector.Dot(ref tSVector8, ref tSVector15); TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8); FP fP2 = TSVector.Dot(ref tSVector8, ref tSVector3); TSVector.Cross(ref tSVector3, ref tSVector7, out tSVector8); FP fP3 = TSVector.Dot(ref tSVector8, ref tSVector15); TSVector.Cross(ref tSVector11, ref tSVector7, out tSVector8); FP fP4 = TSVector.Dot(ref tSVector8, ref tSVector3); FP fP5 = fP + fP2 + fP3 + fP4; bool flag18 = fP5 <= 0; if (flag18) { fP = 0; TSVector.Cross(ref tSVector11, ref tSVector15, out tSVector8); fP2 = TSVector.Dot(ref tSVector8, ref normal); TSVector.Cross(ref tSVector15, ref tSVector7, out tSVector8); fP3 = TSVector.Dot(ref tSVector8, ref normal); TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8); fP4 = TSVector.Dot(ref tSVector8, ref normal); fP5 = fP2 + fP3 + fP4; } FP x5 = FP.One / fP5; TSVector.Multiply(ref tSVector, fP, out point); TSVector.Multiply(ref tSVector5, fP2, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref tSVector9, fP3, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector tSVector13; TSVector.Multiply(ref tSVector13, fP4, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref tSVector2, fP, out tSVector12); TSVector.Add(ref tSVector12, ref point, out point); TSVector.Multiply(ref tSVector6, fP2, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref tSVector10, fP3, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector tSVector14; TSVector.Multiply(ref tSVector14, fP4, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref point, x5 * FP.Half, out point); } result = flag6; } } } return(result); }