public override void SupportMapping(ref TSVector direction, out TSVector result) { TSVector tSVector; TSVector.Normalize(ref direction, out tSVector); tSVector *= this.sphericalExpansion; FP y = TSVector.Dot(ref this.vecs[0], ref direction); int num = 0; FP fP = TSVector.Dot(ref this.vecs[1], ref direction); bool flag = fP > y; if (flag) { y = fP; num = 1; } fP = TSVector.Dot(ref this.vecs[2], ref direction); bool flag2 = fP > y; if (flag2) { num = 2; } result = this.vecs[num] + tSVector; }
public override void SupportMapping(ref TSVector direction, out TSVector result) { TSVector tSVector; TSVector.Normalize(ref direction, out tSVector); TSVector.Multiply(ref tSVector, this.sphericalExpansion, out tSVector); int num = 0; FP y = TSVector.Dot(ref this.points[0], ref direction); FP fP = TSVector.Dot(ref this.points[1], ref direction); bool flag = fP > y; if (flag) { y = fP; num = 1; } fP = TSVector.Dot(ref this.points[2], ref direction); bool flag2 = fP > y; if (flag2) { num = 2; } TSVector.Add(ref this.points[num], ref tSVector, out result); }
public void SupportMapping(ref TSVector direction, out TSVector result) { FP y = TSVector.Dot(ref this.owner.points[this.indices.I0].position, ref direction); FP fP = TSVector.Dot(ref this.owner.points[this.indices.I1].position, ref direction); TSVector position = this.owner.points[this.indices.I0].position; bool flag = fP > y; if (flag) { y = fP; position = this.owner.points[this.indices.I1].position; } fP = TSVector.Dot(ref this.owner.points[this.indices.I2].position, ref direction); bool flag2 = fP > y; if (flag2) { position = this.owner.points[this.indices.I2].position; } TSVector tSVector; TSVector.Normalize(ref direction, out tSVector); tSVector *= this.owner.triangleExpansion; result = position + tSVector; }
public void UpdatePosition() { bool flag = this.body1IsMassPoint; if (flag) { TSVector.Add(ref this.realRelPos1, ref this.body1.position, out this.p1); } else { TSVector.Transform(ref this.realRelPos1, ref this.body1.orientation, out this.p1); TSVector.Add(ref this.p1, ref this.body1.position, out this.p1); } bool flag2 = this.body2IsMassPoint; if (flag2) { TSVector.Add(ref this.realRelPos2, ref this.body2.position, out this.p2); } else { TSVector.Transform(ref this.realRelPos2, ref this.body2.orientation, out this.p2); TSVector.Add(ref this.p2, ref this.body2.position, out this.p2); } TSVector tSVector; TSVector.Subtract(ref this.p1, ref this.p2, out tSVector); this.penetration = TSVector.Dot(ref tSVector, ref this.normal); }
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector) { TSVector w = TSVector.Cross(fromVector, toVector); TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector)); q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude); q.Normalize(); return(q); }
public override void Iterate() { bool flag = this.skipConstraint; if (!flag) { FP x = TSVector.Dot(ref this.body1.linearVelocity, ref this.jacobian[0]); x += TSVector.Dot(ref this.body2.linearVelocity, ref this.jacobian[1]); FP y = this.accumulatedImpulse * this.softnessOverDt; FP fP = -this.effectiveMass * (x + this.bias + y); bool flag2 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMinimumDistance; if (flag2) { FP y2 = this.accumulatedImpulse; this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0); fP = this.accumulatedImpulse - y2; } else { bool flag3 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMaximumDistance; if (flag3) { FP y3 = this.accumulatedImpulse; this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0); fP = this.accumulatedImpulse - y3; } else { this.accumulatedImpulse += fP; } } bool flag4 = !this.body1.isStatic; if (flag4) { TSVector tSVector; TSVector.Multiply(ref this.jacobian[0], fP * this.body1.inverseMass, out tSVector); TSVector.Add(ref tSVector, ref this.body1.linearVelocity, out this.body1.linearVelocity); } bool flag5 = !this.body2.isStatic; if (flag5) { TSVector tSVector; TSVector.Multiply(ref this.jacobian[1], fP * this.body2.inverseMass, out tSVector); TSVector.Add(ref tSVector, ref this.body2.linearVelocity, out this.body2.linearVelocity); } } }
public override void SupportMapping(ref TSVector direction, out TSVector result) { FP y = FP.MinValue; int index = 0; for (int i = 0; i < this.vertices.Count; i++) { FP fP = TSVector.Dot(this.vertices[i], direction); bool flag = fP > y; if (flag) { y = fP; index = i; } } result = this.vertices[index] - this.shifted; }
public int PointOutsideOfPlane(TSVector p, TSVector a, TSVector b, TSVector c, TSVector d) { TSVector vector = TSVector.Cross(b - a, c - a); FP x = TSVector.Dot(p - a, vector); FP fP = TSVector.Dot(d - a, vector); bool flag = fP * fP < FP.EN8; int result; if (flag) { result = -1; } else { result = ((x * fP < FP.Zero) ? 1 : 0); } return(result); }
private static int FindExtremePoint(List <TSVector> points, ref TSVector dir) { int result = 0; FP y = FP.MinValue; for (int i = 1; i < points.Count; i++) { TSVector tSVector = points[i]; FP fP = TSVector.Dot(ref tSVector, ref dir); bool flag = fP > y; if (flag) { y = fP; result = i; } } return(result); }
private void UpdateArbiterContacts(Arbiter arbiter) { bool flag = arbiter.contactList.Count == 0; if (flag) { Stack <Arbiter> obj = this.removedArbiterStack; lock (obj) { this.removedArbiterStack.Push(arbiter); } } else { for (int i = arbiter.contactList.Count - 1; i >= 0; i--) { Contact contact = arbiter.contactList[i]; contact.UpdatePosition(); bool flag2 = contact.penetration < -this.contactSettings.breakThreshold; if (flag2) { Contact.Pool.GiveBack(contact); arbiter.contactList.RemoveAt(i); } else { TSVector value; TSVector.Subtract(ref contact.p1, ref contact.p2, out value); FP fP = TSVector.Dot(ref value, ref contact.normal); value -= fP * contact.normal; fP = value.sqrMagnitude; bool flag3 = fP > this.contactSettings.breakThreshold * this.contactSettings.breakThreshold * 100; if (flag3) { Contact.Pool.GiveBack(contact); arbiter.contactList.RemoveAt(i); } } } } }
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); }
public bool ClosestPtPointTriangle(TSVector p, TSVector a, TSVector b, TSVector c, ref SubSimplexClosestResult result) { result.UsedVertices.Reset(); TSVector tSVector = b - a; TSVector tSVector2 = c - a; TSVector vector = p - a; FP x = TSVector.Dot(tSVector, vector); FP fP = TSVector.Dot(tSVector2, vector); bool flag = x <= FP.Zero && fP <= FP.Zero; bool result2; if (flag) { result.ClosestPointOnSimplex = a; result.UsedVertices.UsedVertexA = true; result.SetBarycentricCoordinates(1, 0, 0, 0); result2 = true; } else { TSVector vector2 = p - b; FP fP2 = TSVector.Dot(tSVector, vector2); FP fP3 = TSVector.Dot(tSVector2, vector2); bool flag2 = fP2 >= FP.Zero && fP3 <= fP2; if (flag2) { result.ClosestPointOnSimplex = b; result.UsedVertices.UsedVertexB = true; result.SetBarycentricCoordinates(0, 1, 0, 0); result2 = true; } else { FP fP4 = x * fP3 - fP2 * fP; bool flag3 = fP4 <= FP.Zero && x >= FP.Zero && fP2 <= FP.Zero; if (flag3) { FP fP5 = x / (x - fP2); result.ClosestPointOnSimplex = a + fP5 * tSVector; result.UsedVertices.UsedVertexA = true; result.UsedVertices.UsedVertexB = true; result.SetBarycentricCoordinates(1 - fP5, fP5, 0, 0); result2 = true; } else { TSVector vector3 = p - c; FP x2 = TSVector.Dot(tSVector, vector3); FP fP6 = TSVector.Dot(tSVector2, vector3); bool flag4 = fP6 >= FP.Zero && x2 <= fP6; if (flag4) { result.ClosestPointOnSimplex = c; result.UsedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(0, 0, 1, 0); result2 = true; } else { FP fP7 = x2 * fP - x * fP6; bool flag5 = fP7 <= FP.Zero && fP >= FP.Zero && fP6 <= FP.Zero; if (flag5) { FP fP8 = fP / (fP - fP6); result.ClosestPointOnSimplex = a + fP8 * tSVector2; result.UsedVertices.UsedVertexA = true; result.UsedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(1 - fP8, 0, fP8, 0); result2 = true; } else { FP x3 = fP2 * fP6 - x2 * fP3; bool flag6 = x3 <= FP.Zero && fP3 - fP2 >= FP.Zero && x2 - fP6 >= FP.Zero; if (flag6) { FP fP8 = (fP3 - fP2) / (fP3 - fP2 + (x2 - fP6)); result.ClosestPointOnSimplex = b + fP8 * (c - b); result.UsedVertices.UsedVertexB = true; result.UsedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(0, 1 - fP8, fP8, 0); result2 = true; } else { FP y = FP.One / (x3 + fP7 + fP4); FP fP5 = fP7 * y; FP fP8 = fP4 * y; result.ClosestPointOnSimplex = a + tSVector * fP5 + tSVector2 * fP8; result.UsedVertices.UsedVertexA = true; result.UsedVertices.UsedVertexB = true; result.UsedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(1 - fP5 - fP8, fP5, fP8, 0); result2 = true; } } } } } } return(result2); }
public bool UpdateClosestVectorAndPoints() { bool needsUpdate = this._needsUpdate; if (needsUpdate) { this._cachedBC.Reset(); this._needsUpdate = false; switch (this.NumVertices) { case 0: this._cachedValidClosest = false; break; case 1: this._cachedPA = this._simplexPointsP[0]; this._cachedPB = this._simplexPointsQ[0]; this._cachedV = this._cachedPA - this._cachedPB; this._cachedBC.Reset(); this._cachedBC.SetBarycentricCoordinates(1f, FP.Zero, FP.Zero, FP.Zero); this._cachedValidClosest = this._cachedBC.IsValid; break; case 2: { TSVector tSVector = this._simplexVectorW[0]; TSVector value = this._simplexVectorW[1]; TSVector tSVector2 = tSVector * -1; TSVector tSVector3 = value - tSVector; FP fP = TSVector.Dot(tSVector3, tSVector2); bool flag = fP > 0; if (flag) { FP sqrMagnitude = tSVector3.sqrMagnitude; bool flag2 = fP < sqrMagnitude; if (flag2) { fP /= sqrMagnitude; tSVector2 -= fP * tSVector3; this._cachedBC.UsedVertices.UsedVertexA = true; this._cachedBC.UsedVertices.UsedVertexB = true; } else { fP = 1; tSVector2 -= tSVector3; this._cachedBC.UsedVertices.UsedVertexB = true; } } else { fP = 0; this._cachedBC.UsedVertices.UsedVertexA = true; } this._cachedBC.SetBarycentricCoordinates(1 - fP, fP, 0, 0); this._cachedPA = this._simplexPointsP[0] + fP * (this._simplexPointsP[1] - this._simplexPointsP[0]); this._cachedPB = this._simplexPointsQ[0] + fP * (this._simplexPointsQ[1] - this._simplexPointsQ[0]); this._cachedV = this._cachedPA - this._cachedPB; this.ReduceVertices(this._cachedBC.UsedVertices); this._cachedValidClosest = this._cachedBC.IsValid; break; } case 3: { TSVector p = default(TSVector); TSVector a = this._simplexVectorW[0]; TSVector b = this._simplexVectorW[1]; TSVector c = this._simplexVectorW[2]; this.ClosestPtPointTriangle(p, a, b, c, ref this._cachedBC); this._cachedPA = this._simplexPointsP[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsP[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsP[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsP[3] * this._cachedBC.BarycentricCoords[3]; this._cachedPB = this._simplexPointsQ[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsQ[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsQ[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsQ[3] * this._cachedBC.BarycentricCoords[3]; this._cachedV = this._cachedPA - this._cachedPB; this.ReduceVertices(this._cachedBC.UsedVertices); this._cachedValidClosest = this._cachedBC.IsValid; break; } case 4: { TSVector p = default(TSVector); TSVector a = this._simplexVectorW[0]; TSVector b = this._simplexVectorW[1]; TSVector c = this._simplexVectorW[2]; TSVector d = this._simplexVectorW[3]; bool flag3 = this.ClosestPtPointTetrahedron(p, a, b, c, d, ref this._cachedBC); bool flag4 = flag3; if (flag4) { this._cachedPA = this._simplexPointsP[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsP[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsP[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsP[3] * this._cachedBC.BarycentricCoords[3]; this._cachedPB = this._simplexPointsQ[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsQ[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsQ[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsQ[3] * this._cachedBC.BarycentricCoords[3]; this._cachedV = this._cachedPA - this._cachedPB; this.ReduceVertices(this._cachedBC.UsedVertices); this._cachedValidClosest = this._cachedBC.IsValid; } else { bool degenerate = this._cachedBC.Degenerate; if (degenerate) { this._cachedValidClosest = false; } else { this._cachedValidClosest = true; this._cachedV.x = (this._cachedV.y = (this._cachedV.z = FP.Zero)); } } break; } default: this._cachedValidClosest = false; break; } } return(this._cachedValidClosest); }
/// <summary> /// Calculates the dot product of two vectors. /// </summary> /// <param name="value1">The first vector.</param> /// <param name="value2">The second vector.</param> /// <returns>Returns the dot product of both.</returns> #region public static FP operator *(JVector value1, JVector value2) public static FP operator *(TSVector value1, TSVector value2) { return(TSVector.Dot(ref value1, ref value2)); }
/// <summary> /// Calculates the dot product of two vectors. /// </summary> /// <param name="vector1">The first vector.</param> /// <param name="vector2">The second vector.</param> /// <returns>Returns the dot product of both vectors.</returns> #region public static FP Dot(JVector vector1, JVector vector2) public static FP Dot(TSVector vector1, TSVector vector2) { return(TSVector.Dot(ref vector1, ref vector2)); }
public static bool Pointcast(ISupportMappable support, ref TSMatrix orientation, ref TSVector position, ref TSVector point) { TSVector tSVector; GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref point, out tSVector); TSVector.Subtract(ref point, ref tSVector, out tSVector); TSVector tSVector2; support.SupportCenter(out tSVector2); TSVector.Transform(ref tSVector2, ref orientation, out tSVector2); TSVector.Add(ref position, ref tSVector2, out tSVector2); TSVector.Subtract(ref point, ref tSVector2, out tSVector2); TSVector p = point; TSVector tSVector3; TSVector.Subtract(ref p, ref tSVector, out tSVector3); FP x = tSVector3.sqrMagnitude; FP eN = FP.EN4; int num = 15; VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew(); @new.Reset(); bool result; while (x > eN && num-- != 0) { TSVector q; GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector3, out q); TSVector w; TSVector.Subtract(ref p, ref q, out w); FP x2 = TSVector.Dot(ref tSVector3, ref w); bool flag = x2 > FP.Zero; if (flag) { FP x3 = TSVector.Dot(ref tSVector3, ref tSVector2); bool flag2 = x3 >= -(TSMath.Epsilon * TSMath.Epsilon); if (flag2) { GJKCollide.simplexSolverPool.GiveBack(@new); result = false; return(result); } @new.Reset(); } bool flag3 = [email protected](w); if (flag3) { @new.AddVertex(w, p, q); } bool flag4 = @new.Closest(out tSVector3); if (flag4) { x = tSVector3.sqrMagnitude; } else { x = FP.Zero; } } GJKCollide.simplexSolverPool.GiveBack(@new); result = true; return(result); }
public static bool Raycast(ISupportMappable support, ref TSMatrix orientation, ref TSMatrix invOrientation, ref TSVector position, ref TSVector origin, ref TSVector direction, out FP fraction, out TSVector normal) { VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew(); @new.Reset(); normal = TSVector.zero; fraction = FP.MaxValue; FP fP = FP.Zero; TSVector tSVector = direction; TSVector p = origin; TSVector tSVector2; GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector, out tSVector2); TSVector tSVector3; TSVector.Subtract(ref p, ref tSVector2, out tSVector3); int num = 15; FP x = tSVector3.sqrMagnitude; FP eN = FP.EN6; bool result; while (x > eN && num-- != 0) { TSVector q; GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector3, out q); TSVector w; TSVector.Subtract(ref p, ref q, out w); FP x2 = TSVector.Dot(ref tSVector3, ref w); bool flag = x2 > FP.Zero; if (flag) { FP fP2 = TSVector.Dot(ref tSVector3, ref tSVector); bool flag2 = fP2 >= -TSMath.Epsilon; if (flag2) { GJKCollide.simplexSolverPool.GiveBack(@new); result = false; return(result); } fP -= x2 / fP2; TSVector.Multiply(ref tSVector, fP, out p); TSVector.Add(ref origin, ref p, out p); TSVector.Subtract(ref p, ref q, out w); normal = tSVector3; } bool flag3 = [email protected](w); if (flag3) { @new.AddVertex(w, p, q); } bool flag4 = @new.Closest(out tSVector3); if (flag4) { x = tSVector3.sqrMagnitude; } else { x = FP.Zero; } } TSVector tSVector4; TSVector value; @new.ComputePoints(out tSVector4, out value); value -= origin; fraction = value.magnitude / direction.magnitude; bool flag5 = normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon; if (flag5) { normal.Normalize(); } GJKCollide.simplexSolverPool.GiveBack(@new); result = true; return(result); }
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); }
public void PrepareForIteration(FP timestep) { FP fP = this.body2.angularVelocity.y * this.relativePos2.z - this.body2.angularVelocity.z * this.relativePos2.y + this.body2.linearVelocity.x; FP fP2 = this.body2.angularVelocity.z * this.relativePos2.x - this.body2.angularVelocity.x * this.relativePos2.z + this.body2.linearVelocity.y; FP fP3 = this.body2.angularVelocity.x * this.relativePos2.y - this.body2.angularVelocity.y * this.relativePos2.x + this.body2.linearVelocity.z; fP = fP - this.body1.angularVelocity.y * this.relativePos1.z + this.body1.angularVelocity.z * this.relativePos1.y - this.body1.linearVelocity.x; fP2 = fP2 - this.body1.angularVelocity.z * this.relativePos1.x + this.body1.angularVelocity.x * this.relativePos1.z - this.body1.linearVelocity.y; fP3 = fP3 - this.body1.angularVelocity.x * this.relativePos1.y + this.body1.angularVelocity.y * this.relativePos1.x - this.body1.linearVelocity.z; FP fP4 = FP.Zero; TSVector zero = TSVector.zero; bool flag = !this.treatBody1AsStatic; if (flag) { fP4 += this.body1.inverseMass; bool flag2 = !this.body1IsMassPoint; if (flag2) { zero.x = this.relativePos1.y * this.normal.z - this.relativePos1.z * this.normal.y; zero.y = this.relativePos1.z * this.normal.x - this.relativePos1.x * this.normal.z; zero.z = this.relativePos1.x * this.normal.y - this.relativePos1.y * this.normal.x; FP x = zero.x * this.body1.invInertiaWorld.M11 + zero.y * this.body1.invInertiaWorld.M21 + zero.z * this.body1.invInertiaWorld.M31; FP y = zero.x * this.body1.invInertiaWorld.M12 + zero.y * this.body1.invInertiaWorld.M22 + zero.z * this.body1.invInertiaWorld.M32; FP z = zero.x * this.body1.invInertiaWorld.M13 + zero.y * this.body1.invInertiaWorld.M23 + zero.z * this.body1.invInertiaWorld.M33; zero.x = x; zero.y = y; zero.z = z; x = zero.y * this.relativePos1.z - zero.z * this.relativePos1.y; y = zero.z * this.relativePos1.x - zero.x * this.relativePos1.z; z = zero.x * this.relativePos1.y - zero.y * this.relativePos1.x; zero.x = x; zero.y = y; zero.z = z; } } TSVector zero2 = TSVector.zero; bool flag3 = !this.treatBody2AsStatic; if (flag3) { fP4 += this.body2.inverseMass; bool flag4 = !this.body2IsMassPoint; if (flag4) { zero2.x = this.relativePos2.y * this.normal.z - this.relativePos2.z * this.normal.y; zero2.y = this.relativePos2.z * this.normal.x - this.relativePos2.x * this.normal.z; zero2.z = this.relativePos2.x * this.normal.y - this.relativePos2.y * this.normal.x; FP x2 = zero2.x * this.body2.invInertiaWorld.M11 + zero2.y * this.body2.invInertiaWorld.M21 + zero2.z * this.body2.invInertiaWorld.M31; FP y2 = zero2.x * this.body2.invInertiaWorld.M12 + zero2.y * this.body2.invInertiaWorld.M22 + zero2.z * this.body2.invInertiaWorld.M32; FP z2 = zero2.x * this.body2.invInertiaWorld.M13 + zero2.y * this.body2.invInertiaWorld.M23 + zero2.z * this.body2.invInertiaWorld.M33; zero2.x = x2; zero2.y = y2; zero2.z = z2; x2 = zero2.y * this.relativePos2.z - zero2.z * this.relativePos2.y; y2 = zero2.z * this.relativePos2.x - zero2.x * this.relativePos2.z; z2 = zero2.x * this.relativePos2.y - zero2.y * this.relativePos2.x; zero2.x = x2; zero2.y = y2; zero2.z = z2; } } bool flag5 = !this.treatBody1AsStatic; if (flag5) { fP4 += zero.x * this.normal.x + zero.y * this.normal.y + zero.z * this.normal.z; } bool flag6 = !this.treatBody2AsStatic; if (flag6) { fP4 += zero2.x * this.normal.x + zero2.y * this.normal.y + zero2.z * this.normal.z; } this.massNormal = FP.One / fP4; FP fP5 = fP * this.normal.x + fP2 * this.normal.y + fP3 * this.normal.z; this.tangent.x = fP - this.normal.x * fP5; this.tangent.y = fP2 - this.normal.y * fP5; this.tangent.z = fP3 - this.normal.z * fP5; fP5 = this.tangent.x * this.tangent.x + this.tangent.y * this.tangent.y + this.tangent.z * this.tangent.z; bool flag7 = fP5 != FP.Zero; if (flag7) { fP5 = FP.Sqrt(fP5); this.tangent.x = this.tangent.x / fP5; this.tangent.y = this.tangent.y / fP5; this.tangent.z = this.tangent.z / fP5; } FP fP6 = FP.Zero; bool flag8 = this.treatBody1AsStatic; if (flag8) { zero.MakeZero(); } else { fP6 += this.body1.inverseMass; bool flag9 = !this.body1IsMassPoint; if (flag9) { zero.x = this.relativePos1.y * this.tangent.z - this.relativePos1.z * this.tangent.y; zero.y = this.relativePos1.z * this.tangent.x - this.relativePos1.x * this.tangent.z; zero.z = this.relativePos1.x * this.tangent.y - this.relativePos1.y * this.tangent.x; FP x3 = zero.x * this.body1.invInertiaWorld.M11 + zero.y * this.body1.invInertiaWorld.M21 + zero.z * this.body1.invInertiaWorld.M31; FP y3 = zero.x * this.body1.invInertiaWorld.M12 + zero.y * this.body1.invInertiaWorld.M22 + zero.z * this.body1.invInertiaWorld.M32; FP z3 = zero.x * this.body1.invInertiaWorld.M13 + zero.y * this.body1.invInertiaWorld.M23 + zero.z * this.body1.invInertiaWorld.M33; zero.x = x3; zero.y = y3; zero.z = z3; x3 = zero.y * this.relativePos1.z - zero.z * this.relativePos1.y; y3 = zero.z * this.relativePos1.x - zero.x * this.relativePos1.z; z3 = zero.x * this.relativePos1.y - zero.y * this.relativePos1.x; zero.x = x3; zero.y = y3; zero.z = z3; } } bool flag10 = this.treatBody2AsStatic; if (flag10) { zero2.MakeZero(); } else { fP6 += this.body2.inverseMass; bool flag11 = !this.body2IsMassPoint; if (flag11) { zero2.x = this.relativePos2.y * this.tangent.z - this.relativePos2.z * this.tangent.y; zero2.y = this.relativePos2.z * this.tangent.x - this.relativePos2.x * this.tangent.z; zero2.z = this.relativePos2.x * this.tangent.y - this.relativePos2.y * this.tangent.x; FP x4 = zero2.x * this.body2.invInertiaWorld.M11 + zero2.y * this.body2.invInertiaWorld.M21 + zero2.z * this.body2.invInertiaWorld.M31; FP y4 = zero2.x * this.body2.invInertiaWorld.M12 + zero2.y * this.body2.invInertiaWorld.M22 + zero2.z * this.body2.invInertiaWorld.M32; FP z4 = zero2.x * this.body2.invInertiaWorld.M13 + zero2.y * this.body2.invInertiaWorld.M23 + zero2.z * this.body2.invInertiaWorld.M33; zero2.x = x4; zero2.y = y4; zero2.z = z4; x4 = zero2.y * this.relativePos2.z - zero2.z * this.relativePos2.y; y4 = zero2.z * this.relativePos2.x - zero2.x * this.relativePos2.z; z4 = zero2.x * this.relativePos2.y - zero2.y * this.relativePos2.x; zero2.x = x4; zero2.y = y4; zero2.z = z4; } } bool flag12 = !this.treatBody1AsStatic; if (flag12) { fP6 += TSVector.Dot(ref zero, ref this.tangent); } bool flag13 = !this.treatBody2AsStatic; if (flag13) { fP6 += TSVector.Dot(ref zero2, ref this.tangent); } this.massTangent = FP.One / fP6; this.restitutionBias = this.lostSpeculativeBounce; this.speculativeVelocity = FP.Zero; FP y5 = this.normal.x * fP + this.normal.y * fP2 + this.normal.z * fP3; bool flag14 = this.Penetration > this.settings.allowedPenetration; if (flag14) { this.restitutionBias = this.settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, this.Penetration - this.settings.allowedPenetration); this.restitutionBias = TSMath.Clamp(this.restitutionBias, FP.Zero, this.settings.maximumBias); } FP y6 = timestep / this.lastTimeStep; this.accumulatedNormalImpulse *= y6; this.accumulatedTangentImpulse *= y6; FP y7 = -(this.tangent.x * fP + this.tangent.y * fP2 + this.tangent.z * fP3); FP x5 = this.massTangent * y7; FP y8 = -this.staticFriction * this.accumulatedNormalImpulse; bool flag15 = x5 < y8; if (flag15) { this.friction = this.dynamicFriction; } else { this.friction = this.staticFriction; } this.restitutionBias = TSMath.Max(-this.restitution * y5, this.restitutionBias); bool flag16 = this.penetration < -this.settings.allowedPenetration; if (flag16) { this.speculativeVelocity = this.penetration / timestep; this.lostSpeculativeBounce = this.restitutionBias; this.restitutionBias = FP.Zero; } else { this.lostSpeculativeBounce = FP.Zero; } TSVector tSVector; tSVector.x = this.normal.x * this.accumulatedNormalImpulse + this.tangent.x * this.accumulatedTangentImpulse; tSVector.y = this.normal.y * this.accumulatedNormalImpulse + this.tangent.y * this.accumulatedTangentImpulse; tSVector.z = this.normal.z * this.accumulatedNormalImpulse + this.tangent.z * this.accumulatedTangentImpulse; bool flag17 = !this.treatBody1AsStatic; if (flag17) { RigidBody expr_13D4_cp_0_cp_0 = this.body1; expr_13D4_cp_0_cp_0.linearVelocity.x = expr_13D4_cp_0_cp_0.linearVelocity.x - tSVector.x * this.body1.inverseMass; RigidBody expr_140B_cp_0_cp_0 = this.body1; expr_140B_cp_0_cp_0.linearVelocity.y = expr_140B_cp_0_cp_0.linearVelocity.y - tSVector.y * this.body1.inverseMass; RigidBody expr_1442_cp_0_cp_0 = this.body1; expr_1442_cp_0_cp_0.linearVelocity.z = expr_1442_cp_0_cp_0.linearVelocity.z - tSVector.z * this.body1.inverseMass; bool flag18 = !this.body1IsMassPoint; if (flag18) { FP x6 = this.relativePos1.y * tSVector.z - this.relativePos1.z * tSVector.y; FP x7 = this.relativePos1.z * tSVector.x - this.relativePos1.x * tSVector.z; FP x8 = this.relativePos1.x * tSVector.y - this.relativePos1.y * tSVector.x; FP y9 = x6 * this.body1.invInertiaWorld.M11 + x7 * this.body1.invInertiaWorld.M21 + x8 * this.body1.invInertiaWorld.M31; FP y10 = x6 * this.body1.invInertiaWorld.M12 + x7 * this.body1.invInertiaWorld.M22 + x8 * this.body1.invInertiaWorld.M32; FP y11 = x6 * this.body1.invInertiaWorld.M13 + x7 * this.body1.invInertiaWorld.M23 + x8 * this.body1.invInertiaWorld.M33; RigidBody expr_161E_cp_0_cp_0 = this.body1; expr_161E_cp_0_cp_0.angularVelocity.x = expr_161E_cp_0_cp_0.angularVelocity.x - y9; RigidBody expr_1640_cp_0_cp_0 = this.body1; expr_1640_cp_0_cp_0.angularVelocity.y = expr_1640_cp_0_cp_0.angularVelocity.y - y10; RigidBody expr_1662_cp_0_cp_0 = this.body1; expr_1662_cp_0_cp_0.angularVelocity.z = expr_1662_cp_0_cp_0.angularVelocity.z - y11; } } bool flag19 = !this.treatBody2AsStatic; if (flag19) { RigidBody expr_1699_cp_0_cp_0 = this.body2; expr_1699_cp_0_cp_0.linearVelocity.x = expr_1699_cp_0_cp_0.linearVelocity.x + tSVector.x * this.body2.inverseMass; RigidBody expr_16D0_cp_0_cp_0 = this.body2; expr_16D0_cp_0_cp_0.linearVelocity.y = expr_16D0_cp_0_cp_0.linearVelocity.y + tSVector.y * this.body2.inverseMass; RigidBody expr_1707_cp_0_cp_0 = this.body2; expr_1707_cp_0_cp_0.linearVelocity.z = expr_1707_cp_0_cp_0.linearVelocity.z + tSVector.z * this.body2.inverseMass; bool flag20 = !this.body2IsMassPoint; if (flag20) { FP x9 = this.relativePos2.y * tSVector.z - this.relativePos2.z * tSVector.y; FP x10 = this.relativePos2.z * tSVector.x - this.relativePos2.x * tSVector.z; FP x11 = this.relativePos2.x * tSVector.y - this.relativePos2.y * tSVector.x; FP y12 = x9 * this.body2.invInertiaWorld.M11 + x10 * this.body2.invInertiaWorld.M21 + x11 * this.body2.invInertiaWorld.M31; FP y13 = x9 * this.body2.invInertiaWorld.M12 + x10 * this.body2.invInertiaWorld.M22 + x11 * this.body2.invInertiaWorld.M32; FP y14 = x9 * this.body2.invInertiaWorld.M13 + x10 * this.body2.invInertiaWorld.M23 + x11 * this.body2.invInertiaWorld.M33; RigidBody expr_18E3_cp_0_cp_0 = this.body2; expr_18E3_cp_0_cp_0.angularVelocity.x = expr_18E3_cp_0_cp_0.angularVelocity.x + y12; RigidBody expr_1905_cp_0_cp_0 = this.body2; expr_1905_cp_0_cp_0.angularVelocity.y = expr_1905_cp_0_cp_0.angularVelocity.y + y13; RigidBody expr_1927_cp_0_cp_0 = this.body2; expr_1927_cp_0_cp_0.angularVelocity.z = expr_1927_cp_0_cp_0.angularVelocity.z + y14; } } this.lastTimeStep = timestep; this.newContact = false; }
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector) { TSVector tSVector = TSVector.Cross(fromVector, toVector); TSQuaternion result = new TSQuaternion(tSVector.x, tSVector.y, tSVector.z, TSVector.Dot(fromVector, toVector)); result.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude); result.Normalize(); return(result); }