/// <summary> /// Inverses the direction of a vector. /// </summary> /// <param name="value">The vector to inverse.</param> /// <returns>The negated vector.</returns> public static TSVector Negate(TSVector value) { TSVector result; TSVector.Negate(ref value, out result); return(result); }
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box) { TSMatrix tSMatrix; TSMath.Absolute(ref orientation, out tSMatrix); TSVector max; TSVector.Transform(ref this.halfSize, ref tSMatrix, out max); box.max = max; TSVector.Negate(ref max, out box.min); }
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector p1, out TSVector p2, out TSVector normal) { VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew(); @new.Reset(); p1 = (p2 = TSVector.zero); TSVector value = position1 - position2; TSVector tSVector = TSVector.Negate(value); TSVector tSVector2; GJKCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector, out tSVector2); TSVector tSVector3; GJKCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref value, out tSVector3); TSVector tSVector4 = tSVector2 - tSVector3; normal = TSVector.zero; int num = 15; FP x = tSVector4.sqrMagnitude; FP eN = FP.EN5; while (x > eN && num-- != 0) { TSVector tSVector5 = TSVector.Negate(tSVector4); GJKCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector5, out tSVector2); GJKCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref tSVector4, out tSVector3); TSVector w = tSVector2 - tSVector3; bool flag = [email protected](w); if (flag) { @new.AddVertex(w, tSVector2, tSVector3); } bool flag2 = @new.Closest(out tSVector4); if (flag2) { x = tSVector4.sqrMagnitude; normal = tSVector4; } else { x = FP.Zero; } } @new.ComputePoints(out p1, out p2); bool flag3 = normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon; if (flag3) { normal.Normalize(); } GJKCollide.simplexSolverPool.GiveBack(@new); return(true); }
private void FindSupportPoints(RigidBody body1, RigidBody body2, Shape shape1, Shape shape2, ref TSVector point, ref TSVector normal, out TSVector point1, out TSVector point2) { TSVector tSVector; TSVector.Negate(ref normal, out tSVector); TSVector tSVector2; this.SupportMapping(body1, shape1, ref tSVector, out tSVector2); TSVector tSVector3; this.SupportMapping(body2, shape2, ref normal, out tSVector3); TSVector.Subtract(ref tSVector2, ref point, out tSVector2); TSVector.Subtract(ref tSVector3, ref point, out tSVector3); FP scaleFactor = TSVector.Dot(ref tSVector2, ref normal); FP scaleFactor2 = TSVector.Dot(ref tSVector3, ref normal); TSVector.Multiply(ref normal, scaleFactor, out tSVector2); TSVector.Multiply(ref normal, scaleFactor2, out tSVector3); TSVector.Add(ref point, ref tSVector2, out point1); TSVector.Add(ref point, ref tSVector3, out point2); }
private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration) { bool flag = body1.IsColliderOnly || body2.IsColliderOnly; Arbiter arbiter = null; bool flag2 = flag; ArbiterMap arbiterMap; if (flag2) { arbiterMap = this.arbiterTriggerMap; } else { arbiterMap = this.arbiterMap; } bool flag3 = false; ArbiterMap obj = arbiterMap; lock (obj) { arbiterMap.LookUpArbiter(body1, body2, out arbiter); bool flag4 = arbiter == null; if (flag4) { arbiter = Arbiter.Pool.GetNew(); arbiter.body1 = body1; arbiter.body2 = body2; arbiterMap.Add(new ArbiterKey(body1, body2), arbiter); flag3 = true; } } bool flag5 = arbiter.body1 == body1; Contact contact; if (flag5) { TSVector.Negate(ref normal, out normal); contact = arbiter.AddContact(point1, point2, normal, penetration, this.contactSettings); } else { contact = arbiter.AddContact(point2, point1, normal, penetration, this.contactSettings); } bool flag6 = flag3; if (flag6) { bool flag7 = flag; if (flag7) { this.events.RaiseTriggerBeginCollide(contact); body1.arbitersTrigger.Add(arbiter); body2.arbitersTrigger.Add(arbiter); OverlapPairContact overlapPairContact = new OverlapPairContact(body1, body2); overlapPairContact.contact = contact; this.initialTriggers.Add(overlapPairContact); } else { this.events.RaiseBodiesBeginCollide(contact); this.addedArbiterQueue.Enqueue(arbiter); OverlapPairContact overlapPairContact2 = new OverlapPairContact(body1, body2); overlapPairContact2.contact = contact; this.initialCollisions.Add(overlapPairContact2); } } bool flag8 = !flag && contact != null; if (flag8) { this.events.RaiseContactCreated(contact); } }
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); }