internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; // Cdot = v + cross(w, r) FPVector2 Cdot = vA + MathUtils.Cross(wA, _rA); FPVector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); FPVector2 oldImpulse = _impulse; _impulse += impulse; FP maxImpulse = data.step.dt * MaxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.magnitude; } impulse = _impulse - oldImpulse; vA += _invMassA * impulse; wA += _invIA * MathUtils.Cross(_rA, impulse); data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; FP h = data.step.dt; FP inv_h = data.step.inv_dt; // Solve angular friction { FP Cdot = wB - wA + inv_h * CorrectionFactor * _angularError; FP impulse = -_angularMass * Cdot; FP oldImpulse = _angularImpulse; FP maxImpulse = h * _maxTorque; _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = _angularImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve linear friction { FPVector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError; FPVector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot); FPVector2 oldImpulse = _linearImpulse; _linearImpulse += impulse; FP maxImpulse = h * _maxForce; if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { _linearImpulse.Normalize(); _linearImpulse *= maxImpulse; } impulse = _linearImpulse - oldImpulse; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
//Extracted from Box2D /// <summary> /// Returns the convex hull from the given vertices. /// </summary> /// <param name="vertices">The vertices.</param> public static Vertices GetConvexHull(Vertices vertices) { if (vertices.Count <= 3) { return(vertices); } // Find the right most point on the hull int i0 = 0; FP x0 = vertices[0].x; for (int i = 1; i < vertices.Count; ++i) { FP x = vertices[i].x; if (x > x0 || (x == x0 && vertices[i].y < vertices[i0].y)) { i0 = i; x0 = x; } } int[] hull = new int[vertices.Count]; int m = 0; int ih = i0; for (; ;) { hull[m] = ih; int ie = 0; for (int j = 1; j < vertices.Count; ++j) { if (ie == ih) { ie = j; continue; } FPVector2 r = vertices[ie] - vertices[hull[m]]; FPVector2 v = vertices[j] - vertices[hull[m]]; FP c = MathUtils.Cross(ref r, ref v); if (c < 0.0f) { ie = j; } // Collinearity check if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { ie = j; } } ++m; ih = ie; if (ie == i0) { break; } } Vertices result = new Vertices(m); // Copy vertices. for (int i = 0; i < m; ++i) { result.Add(vertices[hull[i]]); } return(result); }
public override void Update(FP dt) { FPVector2 f = FPVector2.zero; foreach (Body worldBody in World.BodyList) { if (!IsActiveOn(worldBody)) { continue; } foreach (Body controllerBody in Bodies) { if (worldBody == controllerBody || (worldBody.IsStatic && controllerBody.IsStatic) || !controllerBody.Enabled) { continue; } FPVector2 d = controllerBody.Position - worldBody.Position; FP r2 = d.LengthSquared(); if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) { continue; } switch (GravityType) { case GravityType.DistanceSquared: f = Strength / r2 * worldBody.Mass * controllerBody.Mass * d; break; case GravityType.Linear: f = Strength / FP.Sqrt(r2) * worldBody.Mass * controllerBody.Mass * d; break; } worldBody.ApplyForce(ref f); } foreach (FPVector2 point in Points) { FPVector2 d = point - worldBody.Position; FP r2 = d.LengthSquared(); if (r2 <= Settings.Epsilon || r2 > MaxRadius * MaxRadius || r2 < MinRadius * MinRadius) { continue; } switch (GravityType) { case GravityType.DistanceSquared: f = Strength / r2 * worldBody.Mass * d; break; case GravityType.Linear: f = Strength / FP.Sqrt(r2) * worldBody.Mass * d; break; } worldBody.ApplyForce(ref f); } } }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a Shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func<RayCastInput, int, FP> callback, ref RayCastInput input) { FPVector2 p1 = input.Point1; FPVector2 p2 = input.Point2; FPVector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. FPVector2 absV = MathUtils.Abs(new FPVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) FP maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { FPVector2 t = p1 + maxFraction * (p2 - p1); FPVector2.Min(ref p1, ref t, out segmentAABB.LowerBound); FPVector2.Max(ref p1, ref t, out segmentAABB.UpperBound); } _raycastStack.Clear(); _raycastStack.Push(_root); while (_raycastStack.Count > 0) { int nodeId = _raycastStack.Pop(); if (nodeId == NullNode) { continue; } TreeNode<T> node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) FPVector2 c = node.AABB.Center; FPVector2 h = node.AABB.Extents; FP separation = FP.Abs(FPVector2.Dot(new FPVector2(-r.y, r.x), p1 - c)) - FPVector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; FP value = callback(subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; FPVector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = FPVector2.Min(p1, t); segmentAABB.UpperBound = FPVector2.Max(p1, t); } } else { _raycastStack.Push(node.Child1); _raycastStack.Push(node.Child2); } } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) { cache = new SimplexCache(); if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++GJKCalls; } // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3 <int> saveA = new FixedArray3 <int>(); FixedArray3 <int> saveB = new FixedArray3 <int>(); //FP distanceSqr1 = Settings.MaxFP; // Main iteration loop. int iter = 0; while (iter < Settings.MaxGJKIterations) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = simplex.V[i].IndexA; saveB[i] = simplex.V[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } //FPE: This code was not used anyway. // Compute closest point. //Vector2 p = simplex.GetClosestPoint(); //FP distanceSqr2 = p.LengthSquared(); // Ensure progress //if (distanceSqr2 >= distanceSqr1) //{ //break; //} //distanceSqr1 = distanceSqr2; // Get search direction. FPVector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.EpsilonSqr) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = simplex.V[simplex.Count]; vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d)); vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d)); vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]); vertex.W = vertex.WB - vertex.WA; simplex.V[simplex.Count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { ++GJKIters; } // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled { GJKMaxIters = Math.Max(GJKMaxIters, iter); } // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = (output.PointA - output.PointB).magnitude; output.Iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.UseRadii) { FP rA = input.ProxyA.Radius; FP rB = input.ProxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; FPVector2 normal = output.PointB - output.PointA; normal.Normalize(); output.PointA += rA * normal; output.PointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. FPVector2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }