public TEntity NearestBelow(IEntity target, Vector2 minDist = default(Vector2), Vector2 maxDist = default(Vector2)) { return(this.entities.OrderBy(e => Vector2.Distance(LowerLeft(target), LowerLeft(e))) .FirstOrDefault(e => InRange(MathUtils.Abs(LowerLeft(target) - LowerLeft(e)), minDist, maxDist))); }
public void SolveVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; float wA = c.BodyA.AngularVelocityInternal; float wB = c.BodyB.AngularVelocityInternal; float tangentx = c.Normal.Y; float tangenty = -c.Normal.X; float friction = c.Friction; Debug.Assert(c.PointCount == 1 || c.PointCount == 2); // Solve tangent constraints for (int j = 0; j < c.PointCount; ++j) { ContactConstraintPoint ccp = c.Points[j]; float lambda = ccp.TangentMass * -((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty); // MathUtils.Clamp the accumulated force float maxFriction = friction * ccp.NormalImpulse; float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction)); lambda = newImpulse - ccp.TangentImpulse; // Apply contact impulse float px = lambda * tangentx; float py = lambda * tangenty; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); ccp.TangentImpulse = newImpulse; } // Solve normal constraints if (c.PointCount == 1) { ContactConstraintPoint ccp = c.Points[0]; // Relative velocity at contact // Compute normal impulse float lambda = -ccp.NormalMass * ((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X + (c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias); // Clamp the accumulated impulse float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f); lambda = newImpulse - ccp.NormalImpulse; // Apply contact impulse float px = lambda * c.Normal.X; float py = lambda * c.Normal.Y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py; wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py; wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px); ccp.NormalImpulse = newImpulse; } else { // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). // Build the mini LCP for this contact patch // // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 // // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) // b = vn_0 - velocityBias // // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid // solution that satisfies the problem is chosen. // // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). // // Substitute: // // x = x' - a // // Plug into above equation: // // vn = A * x + b // = A * (x' - a) + b // = A * x' + b - A * a // = A * x' + b' // b' = b - A * a; ContactConstraintPoint cp1 = c.Points[0]; ContactConstraintPoint cp2 = c.Points[1]; float ax = cp1.NormalImpulse; float ay = cp2.NormalImpulse; Debug.Assert(ax >= 0.0f && ay >= 0.0f); // Relative velocity at contact // Compute normal velocity float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * cp1.rA.Y)) * c.Normal.X + (c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * cp1.rA.X)) * c.Normal.Y; float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X - (-wA * cp2.rA.Y)) * c.Normal.X + (c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y - (wA * cp2.rA.X)) * c.Normal.Y; float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay); float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay); float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by); float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by); while (true) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // if (xx >= 0.0f && xy >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; #if B2_DEBUG_SOLVER float k_errorTol = 1e-3f; // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); vn2 = Vector2.Dot(dv2, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + b2' // xx = -cp1.NormalMass * bx; xy = 0.0f; vn1 = 0.0f; vn2 = c.K.Col1.Y * xx + by; if (xx >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; #if B2_DEBUG_SOLVER // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif break; } // // Case 3: vn2 = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + b2' // xx = 0.0f; xy = -cp2.NormalMass * by; vn1 = c.K.Col2.X * xy + bx; vn2 = 0.0f; if (xy >= 0.0f && vn1 >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; #if B2_DEBUG_SOLVER // Postconditions dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn2 = Vector2.Dot(dv2, normal); Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = b2; xx = 0.0f; xy = 0.0f; vn1 = bx; vn2 = by; if (vn1 >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse float dx = xx - ax; float dy = xy - ay; // Apply incremental impulse float p1x = dx * c.Normal.X; float p1y = dx * c.Normal.Y; float p2x = dy * c.Normal.X; float p2y = dy * c.Normal.Y; float p12x = p1x + p2x; float p12y = p1y + p2y; c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x; c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y; wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x)); c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x; c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y; wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x)); // Accumulate cp1.NormalImpulse = xx; cp2.NormalImpulse = xy; break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } } c.BodyA.AngularVelocityInternal = wA; c.BodyB.AngularVelocityInternal = wB; } }
/// <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 slotName="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param slotName="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input) { Vector2 p1 = input.Point1; Vector2 p2 = input.Point2; Vector2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vector2 t = p1 + maxFraction * (p2 - p1); Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound); Vector2.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) Vector2 c = node.AABB.Center; Vector2 h = node.AABB.Extents; float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; float 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; Vector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Vector2.Min(p1, t); segmentAABB.UpperBound = Vector2.Max(p1, t); } } else { _raycastStack.Push(node.Child1); _raycastStack.Push(node.Child2); } } }
public override bool SolvePositionConstraints(SolverData data) { Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); Vec2 uA = Pool.PopVec2(); Vec2 uB = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2 PA = Pool.PopVec2(); Vec2 PB = Pool.PopVec2(); Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; qA.Set(aA); qB.Set(aB); Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), rB); uA.Set(cA).AddLocal(rA).SubLocal(m_groundAnchorA); uB.Set(cB).AddLocal(rB).SubLocal(m_groundAnchorB); float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LINEAR_SLOP) { uA.MulLocal(1.0f / lengthA); } else { uA.SetZero(); } if (lengthB > 10.0f * Settings.LINEAR_SLOP) { uB.MulLocal(1.0f / lengthB); } else { uB.SetZero(); } // Compute effective mass. float ruA = Vec2.Cross(rA, uA); float ruB = Vec2.Cross(rB, uB); float mA = InvMassA + InvIA * ruA * ruA; float mB = InvMassB + InvIB * ruB * ruB; float mass = mA + m_ratio * m_ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = m_constant - lengthA - m_ratio * lengthB; float linearError = MathUtils.Abs(C); float impulse = (-mass) * C; PA.Set(uA).MulLocal(-impulse); PB.Set(uB).MulLocal((-m_ratio) * impulse); cA.X += InvMassA * PA.X; cA.Y += InvMassA * PA.Y; aA += InvIA * Vec2.Cross(rA, PA); cB.X += InvMassB * PB.X; cB.Y += InvMassB * PB.Y; aB += InvIB * Vec2.Cross(rB, PB); data.Positions[IndexA].C.Set(cA); data.Positions[IndexA].A = aA; data.Positions[IndexB].C.Set(cB); data.Positions[IndexB].A = aB; Pool.PushRot(2); Pool.PushVec2(7); return(linearError < Settings.LINEAR_SLOP); }
private void OnUpdateControlDot(float x, float y) { if (x == 0 && y == 0) { Position(0, 0, Config.EMPTY); return; } if (this.allowDiagonal) { float angle = MathUtils.RadToDeg(MathUtils.Atan2(x, y)) + 180; if (this.CheckAngle(0, angle) || this.CheckAngle(360, angle)) { Position(0, -SIDE, Config.TUP); } else if (this.CheckAngle(45, angle)) { Position(-DIAGONAL, -DIAGONAL, Config.LEFT); } else if (this.CheckAngle(90, angle)) { Position(-SIDE, 0, Config.TLEFT); } else if (this.CheckAngle(135, angle)) { Position(-DIAGONAL, DIAGONAL, Config.DOWN); } else if (this.CheckAngle(180, angle)) { Position(0, SIDE, Config.TDOWN); } else if (this.CheckAngle(225, angle)) { Position(DIAGONAL, DIAGONAL, Config.RIGHT); } else if (this.CheckAngle(270, angle)) { Position(SIDE, 0, Config.TRIGHT); } else if (this.CheckAngle(315, angle)) { Position(DIAGONAL, -DIAGONAL, Config.UP); } else { Position(0, 0, Config.EMPTY); } } else { if (MathUtils.Abs(x) > MathUtils.Abs(y)) { if (x > 0) { Position(SIDE, 0, Config.RIGHT); } else if (x < 0) { Position(-SIDE, 0, Config.LEFT); } else if (x == 0) { Position(0, 0, Config.EMPTY); } } else { if (y > 0) { Position(0, SIDE, Config.DOWN); } else if (y < 0) { Position(0, -SIDE, Config.UP); } else if (y == 0) { Position(0, 0, Config.EMPTY); } } } }
public Vector2f Abs() { return(new Vector2f(MathUtils.Abs(x), MathUtils.Abs(y))); }
public static Vector2f Abs(Vector2f a) { return(new Vector2f(MathUtils.Abs(a.x), MathUtils.Abs(a.y))); }
public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) { // Console.WriteLine("Solving Island"); float h = step.Dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Vec2 c = b.Sweep.C; float a = b.Sweep.A; Vec2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.C0.Set(b.Sweep.C); b.Sweep.A0 = b.Sweep.A; if (b.Type == BodyType.Dynamic) { // Integrate velocities. // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force); v.X += h * (b.GravityScale * gravity.X + b.InvMass * b.Force.X); v.Y += h * (b.GravityScale * gravity.Y + b.InvMass * b.Force.Y); w += h * b.InvI * b.Torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * // exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v.MulLocal(MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f)); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } //Debug.Assert (v.x == 0); Positions[i].C.Set(c); Positions[i].A = a; Velocities[i].V.Set(v); Velocities[i].W = w; } timer.Reset(); // Solver data solverData.Step = step; solverData.Positions = Positions; solverData.Velocities = Velocities; // Initialize velocity constraints. solverDef.Step = step; solverDef.Contacts = Contacts; solverDef.Count = ContactCount; solverDef.Positions = Positions; solverDef.Velocities = Velocities; contactSolver.Init(solverDef); //Console.WriteLine("island init vel"); contactSolver.InitializeVelocityConstraints(); if (step.WarmStarting) { //Console.WriteLine("island warm start"); contactSolver.WarmStart(); } for (int i = 0; i < JointCount; ++i) { Joints[i].InitVelocityConstraints(solverData); } profile.SolveInit = timer.Milliseconds; // Solve velocity constraints timer.Reset(); //Console.WriteLine("island solving velocities"); for (int i = 0; i < step.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joints[j].SolveVelocityConstraints(solverData); } contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting contactSolver.StoreImpulses(); profile.SolveVelocity = timer.Milliseconds; // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vec2 c = Positions[i].C; float a = Positions[i].A; Vec2 v = Velocities[i].V; float w = Velocities[i].W; // Check for large velocities translation.X = v.X * h; translation.Y = v.Y * h; if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED) { float ratio = Settings.MAX_TRANSLATION / translation.Length(); v.X *= ratio; v.Y *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation); w *= ratio; } // Integrate c.X += h * v.X; c.Y += h * v.Y; a += h * w; Positions[i].A = a; Velocities[i].W = w; } // Solve position constraints timer.Reset(); bool positionSolved = false; for (int i = 0; i < step.PositionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { bool jointOkay = Joints[j].SolvePositionConstraints(solverData); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { Body body = Bodies[i]; body.Sweep.C.Set(Positions[i].C); body.Sweep.A = Positions[i].A; body.LinearVelocity.Set(Velocities[i].V); body.AngularVelocity = Velocities[i].W; body.SynchronizeTransform(); } profile.SolvePosition = timer.Milliseconds; Report(contactSolver.VelocityConstraints); if (allowSleep) { float minSleepTime = Single.MaxValue; const float linTolSqr = Settings.LINEAR_SLEEP_TOLERANCE * Settings.LINEAR_SLEEP_TOLERANCE; float angTolSqr = Settings.ANGULAR_SLEEP_TOLERANCE * Settings.ANGULAR_SLEEP_TOLERANCE; for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.Type == BodyType.Static) { continue; } if ((b.Flags & Body.TypeFlags.AutoSleep) == 0 || b.AngularVelocity * b.AngularVelocity > angTolSqr || Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = MathUtils.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= Settings.TIME_TO_SLEEP && positionSolved) { for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; b.Awake = false; } } } }
public void SolveToi(TimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; Positions[i].C.Set(b.Sweep.C); Positions[i].A = b.Sweep.A; Velocities[i].V.Set(b.LinearVelocity); Velocities[i].W = b.AngularVelocity; } toiSolverDef.Contacts = Contacts; toiSolverDef.Count = ContactCount; toiSolverDef.Step = subStep; toiSolverDef.Positions = Positions; toiSolverDef.Velocities = Velocities; toiContactSolver.Init(toiSolverDef); // Solve position constraints. for (int i = 0; i < subStep.PositionIterations; ++i) { bool contactsOkay = toiContactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // #if 0 // // Is the new position really safe? // for (int i = 0; i < m_contactCount; ++i) // { // Contact* c = m_contacts[i]; // Fixture* fA = c.GetFixtureA(); // Fixture* fB = c.GetFixtureB(); // // Body bA = fA.GetBody(); // Body bB = fB.GetBody(); // // int indexA = c.GetChildIndexA(); // int indexB = c.GetChildIndexB(); // // DistanceInput input; // input.proxyA.Set(fA.GetShape(), indexA); // input.proxyB.Set(fB.GetShape(), indexB); // input.transformA = bA.GetTransform(); // input.transformB = bB.GetTransform(); // input.useRadii = false; // // DistanceOutput output; // SimplexCache cache; // cache.count = 0; // Distance(&output, &cache, &input); // // if (output.distance == 0 || cache.count == 3) // { // cache.count += 0; // } // } // #endif // Leap of faith to new safe state. Bodies[toiIndexA].Sweep.C0.Set(Positions[toiIndexA].C); Bodies[toiIndexA].Sweep.A0 = Positions[toiIndexA].A; Bodies[toiIndexB].Sweep.C0.Set(Positions[toiIndexB].C); Bodies[toiIndexB].Sweep.A0 = Positions[toiIndexB].A; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. toiContactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < subStep.VelocityIterations; ++i) { toiContactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float h = subStep.Dt; // Integrate positions for (int i = 0; i < BodyCount; ++i) { Vec2 c = Positions[i].C; float a = Positions[i].A; Vec2 v = Velocities[i].V; float w = Velocities[i].W; // Check for large velocities translation.Set(v).MulLocal(h); if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED) { float ratio = Settings.MAX_TRANSLATION / translation.Length(); v.MulLocal(ratio); } float rotation = h * w; if (rotation * rotation > Settings.MaxRotationSquared) { float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation); w *= ratio; } // Integrate c.X += v.X * h; c.Y += v.Y * h; a += h * w; Positions[i].C.Set(c); Positions[i].A = a; Velocities[i].V.Set(v); Velocities[i].W = w; // Sync bodies Body body = Bodies[i]; body.Sweep.C.Set(c); body.Sweep.A = a; body.LinearVelocity.Set(v); body.AngularVelocity = w; body.SynchronizeTransform(); } Report(toiContactSolver.VelocityConstraints); }
public float LenManhattan() { return(MathUtils.Abs(this.x) + MathUtils.Abs(this.y)); }
public static void AbsToOut(Vector2f a, Vector2f outs) { outs.x = MathUtils.Abs(a.x); outs.y = MathUtils.Abs(a.y); }
public bool IsUnit(float margin) { return(MathUtils.Abs(Len2() - 1f) < margin); }
protected internal override void ProcessTouchPressed() { float x = MathUtils.BringToBounds(0, baseWidth, Touch.GetX() - GetScreenX()) / baseWidth - 0.5f; float y = MathUtils.BringToBounds(0, baseHeight, Touch.GetY() - GetScreenY()) / baseHeight - 0.5f; if (x == 0 && y == 0) { return; } if (MathUtils.Abs(x) > MathUtils.Abs(y)) { if (x > 0) { this.isRight = true; this.isClick = true; this.centerX = offsetX + x + (baseWidth - dotWidth) / 2 + dotWidth * 0.75f; this.centerY = offsetY + y + (baseHeight - dotHeight) / 2; if (listener != null) { listener.Right(); } } else if (x < 0) { this.isLeft = true; this.isClick = true; this.centerX = offsetX + x + (baseWidth - dotWidth) / 2 - dotWidth * 0.75f; this.centerY = offsetY + y + (baseHeight - dotHeight) / 2; if (listener != null) { listener.Left(); } } else if (x == 0) { FreeClick(); } } else { if (y > 0) { this.isDown = true; this.isClick = true; this.centerX = offsetX + x + (baseWidth - dotWidth) / 2 - 1; this.centerY = offsetY + y + (baseHeight - dotHeight) / 2 + dotHeight * 0.75f; if (listener != null) { listener.Down(); } } else if (y < 0) { this.isUp = true; this.isClick = true; this.centerX = offsetX + x + (baseWidth - dotWidth) / 2 - 1; this.centerY = offsetY + y + (baseHeight - dotHeight) / 2 - dotHeight * 0.75f; if (listener != null) { listener.Up(); } } else if (y == 0) { FreeClick(); } } }
public void SolveVelocityConstraints() { for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = Constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float wA = bodyA.AngularVelocityInternal; float wB = bodyB.AngularVelocityInternal; Vector2 vA = bodyA.LinearVelocityInternal; Vector2 vB = bodyB.LinearVelocityInternal; float invMassA = bodyA.InvMass; float invIA = bodyA.InvI; float invMassB = bodyB.InvMass; float invIB = bodyB.InvI; Vector2 normal = c.Normal; #if MATH_OVERLOADS Vector2 tangent = MathUtils.Cross(normal, 1.0f); #else Vector2 tangent = new Vector2(normal.Y, -normal.X); #endif float friction = c.Friction; Debug.Assert(c.PointCount == 1 || c.PointCount == 2); // Solve tangent constraints for (int j = 0; j < c.PointCount; ++j) { ContactConstraintPoint ccp = c.Points[j]; #if MATH_OVERLOADS // Relative velocity at contact Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA); // Compute tangent force float vt = Vector2.Dot(dv, tangent); #else // Relative velocity at contact Vector2 dv = new Vector2(vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y), vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X)); // Compute tangent force float vt = dv.X * tangent.X + dv.Y * tangent.Y; #endif float lambda = ccp.TangentMass * (-vt); // MathUtils.Clamp the accumulated force float maxFriction = friction * ccp.NormalImpulse; float newImpulse = MathUtils.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - ccp.TangentImpulse; #if MATH_OVERLOADS // Apply contact impulse Vector2 P = lambda * tangent; vA -= invMassA * P; wA -= invIA * MathUtils.Cross(ccp.rA, P); vB += invMassB * P; wB += invIB * MathUtils.Cross(ccp.rB, P); #else // Apply contact impulse Vector2 P = new Vector2(lambda * tangent.X, lambda * tangent.Y); vA.X -= invMassA * P.X; vA.Y -= invMassA * P.Y; wA -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X); vB.X += invMassB * P.X; vB.Y += invMassB * P.Y; wB += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X); #endif ccp.TangentImpulse = newImpulse; } // Solve normal constraints if (c.PointCount == 1) { ContactConstraintPoint ccp = c.Points[0]; #if MATH_OVERLOADS // Relative velocity at contact Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA); // Compute normal impulse float vn = Vector2.Dot(dv, normal); float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); // MathUtils.Clamp the accumulated impulse float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f); lambda = newImpulse - ccp.NormalImpulse; // Apply contact impulse Vector2 P = lambda * normal; vA -= invMassA * P; wA -= invIA * MathUtils.Cross(ccp.rA, P); vB += invMassB * P; wB += invIB * MathUtils.Cross(ccp.rB, P); #else // Relative velocity at contact Vector2 dv = new Vector2(vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y), vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X)); // Compute normal impulse float vn = dv.X * normal.X + dv.Y * normal.Y; float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias); // Clamp the accumulated impulse float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f); lambda = newImpulse - ccp.NormalImpulse; // Apply contact impulse Vector2 P = new Vector2(lambda * normal.X, lambda * normal.Y); vA.X -= invMassA * P.X; vA.Y -= invMassA * P.Y; wA -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X); vB.X += invMassB * P.X; vB.Y += invMassB * P.Y; wB += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X); #endif ccp.NormalImpulse = newImpulse; } else { // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). // Build the mini LCP for this contact patch // // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 // // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) // b = vn_0 - velocityBias // // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid // solution that satisfies the problem is chosen. // // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). // // Substitute: // // x = x' - a // // Plug into above equation: // // vn = A * x + b // = A * (x' - a) + b // = A * x' + b - A * a // = A * x' + b' // b' = b - A * a; ContactConstraintPoint cp1 = c.Points[0]; ContactConstraintPoint cp2 = c.Points[1]; Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse); Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); #if MATH_OVERLOADS // Relative velocity at contact Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity float vn1 = Vector2.Dot(dv1, normal); float vn2 = Vector2.Dot(dv2, normal); Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias); b -= MathUtils.Multiply(ref c.K, a); #else // Relative velocity at contact Vector2 dv1 = new Vector2(vB.X + (-wB * cp1.rB.Y) - vA.X - (-wA * cp1.rA.Y), vB.Y + (wB * cp1.rB.X) - vA.Y - (wA * cp1.rA.X)); Vector2 dv2 = new Vector2(vB.X + (-wB * cp2.rB.Y) - vA.X - (-wA * cp2.rA.Y), vB.Y + (wB * cp2.rB.X) - vA.Y - (wA * cp2.rA.X)); // Compute normal velocity float vn1 = dv1.X * normal.X + dv1.Y * normal.Y; float vn2 = dv2.X * normal.X + dv2.Y * normal.Y; Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias); b -= MathUtils.Multiply(ref c.K, ref a); // Inlining didn't help for the multiply. #endif while (true) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // Vector2 x = -MathUtils.Multiply(ref c.NormalMass, ref b); if (x.X >= 0.0f && x.Y >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y); // Apply incremental impulse Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y); Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y); Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y); vA.X -= invMassA * P12.X; vA.Y -= invMassA * P12.Y; wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X)); vB.X += invMassB * P12.X; vB.Y += invMassB * P12.Y; wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X)); #endif // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; #if B2_DEBUG_SOLVER float k_errorTol = 1e-3f; // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); vn2 = Vector2.Dot(dv2, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + b2' // x.X = -cp1.NormalMass * b.X; x.Y = 0.0f; vn1 = 0.0f; vn2 = c.K.Col1.Y * x.X + b.Y; if (x.X >= 0.0f && vn2 >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y); // Apply incremental impulse Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y); Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y); Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y); vA.X -= invMassA * P12.X; vA.Y -= invMassA * P12.Y; wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X)); vB.X += invMassB * P12.X; vB.Y += invMassB * P12.Y; wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X)); #endif // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; #if B2_DEBUG_SOLVER // Postconditions dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA); // Compute normal velocity vn1 = Vector2.Dot(dv1, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif break; } // // Case 3: vn2 = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + b2' // x.X = 0.0f; x.Y = -cp2.NormalMass * b.Y; vn1 = c.K.Col2.X * x.Y + b.X; vn2 = 0.0f; if (x.Y >= 0.0f && vn1 >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y); // Apply incremental impulse Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y); Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y); Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y); vA.X -= invMassA * P12.X; vA.Y -= invMassA * P12.Y; wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X)); vB.X += invMassB * P12.X; vB.Y += invMassB * P12.Y; wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X)); #endif // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; #if B2_DEBUG_SOLVER // Postconditions dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA); // Compute normal velocity vn2 = Vector2.Dot(dv2, normal); Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = b2; x.X = 0.0f; x.Y = 0.0f; vn1 = b.X; vn2 = b.Y; if (vn1 >= 0.0f && vn2 >= 0.0f) { #if MATH_OVERLOADS // Resubstitute for the incremental impulse Vector2 d = x - a; // Apply incremental impulse Vector2 P1 = d.X * normal; Vector2 P2 = d.Y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2)); #else // Resubstitute for the incremental impulse Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y); // Apply incremental impulse Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y); Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y); Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y); vA.X -= invMassA * P12.X; vA.Y -= invMassA * P12.Y; wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X)); vB.X += invMassB * P12.X; vB.Y += invMassB * P12.Y; wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X)); #endif // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } } bodyA.LinearVelocityInternal = vA; bodyA.AngularVelocityInternal = wA; bodyB.LinearVelocityInternal = vB; bodyB.AngularVelocityInternal = wB; } }
public static Vec2 Abs(Vec2 v) { return(new Vec2(MathUtils.Abs(v.x), MathUtils.Abs(v.y))); }
public override float GetScore(float sx, float sy, float tx, float ty) { return(MathUtils.Abs(sx - tx) + MathUtils.Abs(sy - ty)); }
public static Vec4 Abs(Vec4 v) { return(new Vec4(MathUtils.Abs(v.x), MathUtils.Abs(v.y), MathUtils.Abs(v.z), MathUtils.Abs(v.w))); }
private Matrix4x4 GetRandomTransformation(ref ItemReferenceData itemReference, ref ItemData itemData, System.Random rnd) { Matrix4x4 matrix = itemReference.transform; if (itemReference.randomOrientation) { matrix *= GetRandomOrientationMatrix(rnd, itemReference.anchorPlane); } Vector3 randPositionAmplitude = itemReference.randomPositionAmplitude; if (itemReference.procedural) { Vector3 refAnchorPosition = GetAnchorPosition(itemReference.anchorPlane, itemReference.availableProceduralVolume); Vector3 itemAnchorPosition = GetAnchorPosition(itemData.anchorPlane, itemData.itemBounds); matrix *= Matrix4x4.TRS(refAnchorPosition - itemAnchorPosition, Quaternion.identity, Vector3.one); // Add random amplitude based on the possible volume Vector3 anchorDirection = GetAnchorDirection(itemReference.anchorPlane); randPositionAmplitude += Vector3.Scale(itemReference.availableProceduralVolume.size, Vector3.one - MathUtils.Abs(anchorDirection)); Vector3 position = GetRandomPositionOffset(randPositionAmplitude, rnd); Quaternion rotation = Quaternion.Euler(GetRandomRotationOffset(itemReference.randomRotationAmplitude, rnd)); Vector3 scale = GetRandomScaleOffset(itemReference.randomScaleAmplitude, itemReference.uniformScale, rnd); return(matrix * Matrix4x4.TRS(position, rotation, scale)); } else { Vector3 position = GetRandomPositionOffset(randPositionAmplitude, rnd); Quaternion rotation = Quaternion.Euler(GetRandomRotationOffset(itemReference.randomRotationAmplitude, rnd)); Vector3 scale = GetRandomScaleOffset(itemReference.randomScaleAmplitude, itemReference.uniformScale, rnd); return(matrix * Matrix4x4.TRS(position, rotation, scale)); } }
public void AbsLocal() { x = MathUtils.Abs(x); y = MathUtils.Abs(y); }
public bool Intersects(Bounds boundingBox, out float result) { // X if (MathUtils.Abs(this.direction.x) < MathUtils.EPSILON && (this.origin.x < boundingBox.min.x || this.origin.x > boundingBox.max.x)) { //If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it can't be intersecting. result = 0; return(false); } float tmin = 0, tmax = float.MaxValue; float inverseDirection = 1 / this.direction.x; float t1 = (boundingBox.min.x - this.origin.x) * inverseDirection; float t2 = (boundingBox.max.x - this.origin.x) * inverseDirection; if (t1 > t2) { float temp = t1; t1 = t2; t2 = temp; } tmin = MathUtils.Max(tmin, t1); tmax = MathUtils.Min(tmax, t2); if (tmin > tmax) { result = 0; return(false); } // Y if (MathUtils.Abs(this.direction.y) < MathUtils.EPSILON && (this.origin.y < boundingBox.min.y || this.origin.y > boundingBox.max.y)) { //If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it can't be intersecting. result = 0; return(false); } inverseDirection = 1 / this.direction.y; t1 = (boundingBox.min.y - this.origin.y) * inverseDirection; t2 = (boundingBox.max.y - this.origin.y) * inverseDirection; if (t1 > t2) { float temp = t1; t1 = t2; t2 = temp; } tmin = MathUtils.Max(tmin, t1); tmax = MathUtils.Min(tmax, t2); if (tmin > tmax) { result = 0; return(false); } // Z if (MathUtils.Abs(this.direction.z) < MathUtils.EPSILON && (this.origin.z < boundingBox.min.z || this.origin.z > boundingBox.max.z)) { //If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it can't be intersecting. result = 0; return(false); } inverseDirection = 1 / this.direction.z; t1 = (boundingBox.min.z - this.origin.z) * inverseDirection; t2 = (boundingBox.max.z - this.origin.z) * inverseDirection; if (t1 > t2) { float temp = t1; t1 = t2; t2 = temp; } tmin = MathUtils.Max(tmin, t1); tmax = MathUtils.Min(tmax, t2); if (tmin > tmax) { result = 0; return(false); } result = tmin; return(true); }
public static void AbsToOut(Vector2f a, Vector2f xout) { xout.x = MathUtils.Abs(a.x); xout.y = MathUtils.Abs(a.y); }
public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; LocalCenterA.Set(BodyA.Sweep.LocalCenter); LocalCenterB.Set(BodyB.Sweep.LocalCenter); InvMassA = BodyA.InvMass; InvMassB = BodyB.InvMass; InvIA = BodyA.InvI; InvIB = BodyB.InvI; Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 d = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); // Compute the effective masses. Rot.MulToOutUnsafe(qA, d.Set(LocalAnchorA).SubLocal(LocalCenterA), rA); Rot.MulToOutUnsafe(qB, d.Set(LocalAnchorB).SubLocal(LocalCenterB), rB); d.Set(cB).SubLocal(cA).AddLocal(rB).SubLocal(rA); float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; // Compute motor Jacobian and effective mass. { Rot.MulToOutUnsafe(qA, LocalXAxisA, Axis); temp.Set(d).AddLocal(rA); A1 = Vec2.Cross(temp, Axis); A2 = Vec2.Cross(rB, Axis); MotorMass = mA + mB + iA * A1 * A1 + iB * A2 * A2; if (MotorMass > 0.0f) { MotorMass = 1.0f / MotorMass; } } // Prismatic constraint. { Rot.MulToOutUnsafe(qA, LocalYAxisA, Perp); temp.Set(d).AddLocal(rA); S1 = Vec2.Cross(temp, Perp); S2 = Vec2.Cross(rB, Perp); float k11 = mA + mB + iA * S1 * S1 + iB * S2 * S2; float k12 = iA * S1 + iB * S2; float k13 = iA * S1 * A1 + iB * S2 * A2; float k22 = iA + iB; if (k22 == 0.0f) { // For bodies with fixed rotation. k22 = 1.0f; } float k23 = iA * A1 + iB * A2; float k33 = mA + mB + iA * A1 * A1 + iB * A2 * A2; K.Ex.Set(k11, k12, k13); K.Ey.Set(k12, k22, k23); K.Ez.Set(k13, k23, k33); } // Compute motor and limit terms. if (m_limitEnabled) { float jointTranslation = Vec2.Dot(Axis, d); if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP) { LimitState = LimitState.Equal; } else if (jointTranslation <= LowerTranslation) { if (LimitState != LimitState.AtLower) { LimitState = LimitState.AtLower; Impulse.Z = 0.0f; } } else if (jointTranslation >= UpperTranslation) { if (LimitState != LimitState.AtUpper) { LimitState = LimitState.AtUpper; Impulse.Z = 0.0f; } } else { LimitState = LimitState.Inactive; Impulse.Z = 0.0f; } } else { LimitState = LimitState.Inactive; Impulse.Z = 0.0f; } if (m_motorEnabled == false) { MotorImpulse = 0.0f; } if (data.Step.WarmStarting) { // Account for variable time step. Impulse.MulLocal(data.Step.DtRatio); MotorImpulse *= data.Step.DtRatio; Vec2 P = Pool.PopVec2(); temp.Set(Axis).MulLocal(MotorImpulse + Impulse.Z); P.Set(Perp).MulLocal(Impulse.X).AddLocal(temp); float LA = Impulse.X * S1 + Impulse.Y + (MotorImpulse + Impulse.Z) * A1; float LB = Impulse.X * S2 + Impulse.Y + (MotorImpulse + Impulse.Z) * A2; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * LA; vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * LB; Pool.PushVec2(1); } else { Impulse.SetZero(); MotorImpulse = 0.0f; } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushRot(2); Pool.PushVec2(4); }
/// <summary> /// Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction /// between [0,tMax]. This uses a swept separating axis and may miss some intermediate, /// non-tunneling collision. If you change the time interval, you should call this function again. /// Note: use Distance to compute the contact point and normal at the time of impact. /// </summary> /// <param name="output"></param> /// <param name="input"></param> public void GetTimeOfImpact(TOIOutput output, TOIInput input) { // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. ++ToiCalls; output.State = TOIOutputState.Unknown; output.T = input.tMax; Distance.DistanceProxy proxyA = input.ProxyA; Distance.DistanceProxy proxyB = input.ProxyB; sweepA.Set(input.SweepA); sweepB.Set(input.SweepB); // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float tMax = input.tMax; float totalRadius = proxyA.Radius + proxyB.Radius; // djm: whats with all these constants? float target = MathUtils.Max(Settings.LINEAR_SLOP, totalRadius - 3.0f * Settings.LINEAR_SLOP); const float tolerance = 0.25f * Settings.LINEAR_SLOP; Debug.Assert(target > tolerance); float t1 = 0f; int iter = 0; cache.Count = 0; distanceInput.ProxyA = input.ProxyA; distanceInput.ProxyB = input.ProxyB; distanceInput.UseRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for (; ;) { sweepA.GetTransform(xfA, t1); sweepB.GetTransform(xfB, t1); // System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n", // sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y); // Get the distance between shapes. We can also use the results // to get a separating axis distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; pool.GetDistance().GetDistance(distanceOutput, cache, distanceInput); // System.out.printf("Dist: %f at points %f, %f and %f, %f. %d iterations\n", // distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y, // distanceOutput.pointB.x, distanceOutput.pointB.y, // distanceOutput.iterations); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0f) { // System.out.println("failure, overlapped"); // Failure! output.State = TOIOutputState.Overlapped; output.T = 0f; break; } if (distanceOutput.Distance < target + tolerance) { // System.out.println("touching, victory"); // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } // Initialize the separating axis. fcn.Initialize(cache, proxyA, sweepA, proxyB, sweepB, t1); // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of // vertices. bool done = false; float t2 = tMax; int pushBackIter = 0; for (; ;) { // Find the deepest point at t2. Store the witness point indices. float s2 = fcn.FindMinSeparation(indexes, t2); // System.out.printf("s2: %f\n", s2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! // System.out.println("separated"); output.State = TOIOutputState.Separated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // System.out.println("advancing"); // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = fcn.Evaluate(indexes[0], indexes[1], t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. // System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target, // tolerance); if (s1 < target - tolerance) { // System.out.println("failed?"); output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // System.out.println("touching?"); // Victory! t1 should hold the TOI (could be 0.0). output.State = TOIOutputState.Touching; output.T = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int rootIterCount = 0; float a1 = t1, a2 = t2; for (; ;) { // Use a mix of the secant rule and bisection. float t; if ((rootIterCount & 1) == 1) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } float s = fcn.Evaluate(indexes[0], indexes[1], t); if (MathUtils.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } ++rootIterCount; ++ToiRootIters; // djm: whats with this? put in settings? if (rootIterCount == 50) { break; } } ToiMaxRootIters = MathUtils.Max(ToiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.MAX_POLYGON_VERTICES) { break; } } ++iter; ++ToiIters; if (done) { // System.out.println("done"); break; } if (iter == MAX_ITERATIONS) { // System.out.println("failed, root finder stuck"); // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } // System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s) ToiMaxIters = MathUtils.Max(ToiMaxIters, iter); }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; Vec2 temp = Pool.PopVec2(); // Solve linear motor constraint. if (m_motorEnabled && LimitState != LimitState.Equal) { temp.Set(vB).SubLocal(vA); float Cdot = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA; float impulse = MotorMass * (m_motorSpeed - Cdot); float oldImpulse = MotorImpulse; float maxImpulse = data.Step.Dt * m_maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; Vec2 P = Pool.PopVec2(); P.Set(Axis).MulLocal(impulse); float LA = impulse * A1; float LB = impulse * A2; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * LA; vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * LB; Pool.PushVec2(1); } Vec2 Cdot1 = Pool.PopVec2(); temp.Set(vB).SubLocal(vA); Cdot1.X = Vec2.Dot(Perp, temp) + S2 * wB - S1 * wA; Cdot1.Y = wB - wA; // System.out.println(Cdot1); if (m_limitEnabled && LimitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2; temp.Set(vB).SubLocal(vA); Cdot2 = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA; Vec3 Cdot = Pool.PopVec3(); Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2); Cdot.NegateLocal(); Vec3 f1 = Pool.PopVec3(); Vec3 df = Pool.PopVec3(); f1.Set(Impulse); K.Solve33ToOut(Cdot.NegateLocal(), df); //Cdot.negateLocal(); not used anymore Impulse.AddLocal(df); if (LimitState == LimitState.AtLower) { Impulse.Z = MathUtils.Max(Impulse.Z, 0.0f); } else if (LimitState == LimitState.AtUpper) { Impulse.Z = MathUtils.Min(Impulse.Z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + // f1(1:2) Vec2 b = Pool.PopVec2(); Vec2 f2r = Pool.PopVec2(); temp.Set(K.Ez.X, K.Ez.Y).MulLocal(Impulse.Z - f1.Z); b.Set(Cdot1).NegateLocal().SubLocal(temp); temp.Set(f1.X, f1.Y); K.Solve22ToOut(b, f2r); f2r.AddLocal(temp); Impulse.X = f2r.X; Impulse.Y = f2r.Y; df.Set(Impulse).SubLocal(f1); Vec2 P = Pool.PopVec2(); temp.Set(Axis).MulLocal(df.Z); P.Set(Perp).MulLocal(df.X).AddLocal(temp); float LA = df.X * S1 + df.Y + df.Z * A1; float LB = df.X * S2 + df.Y + df.Z * A2; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * LA; vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * LB; Pool.PushVec2(3); Pool.PushVec3(3); } else { // Limit is inactive, just solve the prismatic constraint in block form. Vec2 df = Pool.PopVec2(); K.Solve22ToOut(Cdot1.NegateLocal(), df); Cdot1.NegateLocal(); Impulse.X += df.X; Impulse.Y += df.Y; Vec2 P = Pool.PopVec2(); P.Set(Perp).MulLocal(df.X); float LA = df.X * S1 + df.Y; float LB = df.X * S2 + df.Y; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * LA; vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * LB; Vec2 Cdot10 = Pool.PopVec2(); Cdot10.Set(Cdot1); Cdot1.X = Vec2.Dot(Perp, temp.Set(vB).SubLocal(vA)) + S2 * wB - S1 * wA; Cdot1.Y = wB - wA; if (MathUtils.Abs(Cdot1.X) > 0.01f || MathUtils.Abs(Cdot1.Y) > 0.01f) { // djm note: what's happening here? Mat33.Mul22ToOutUnsafe(K, df, temp); Cdot1.X += 0.0f; } Pool.PushVec2(3); } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(2); }
public override Vec3 Steer() { Entity self = this._behaviors.owner; float detectRadius = MIN_DETECTION_BOX_LENGTH * (1 + self.property.speed / self.maxSpeed); EntityUtils.GetEntitiesInCircle(self.battle.GetEntities(), self.property.position, detectRadius, ref this._temp); this.DebugDrawDetectRadius(detectRadius); Bio closestIntersectingObstacle = null; float distToClosestIp = float.MaxValue; Vec3 localPosOfClosestObstacle = Vec3.zero; int count = this._temp.Count; for (int i = 0; i < count; i++) { Bio bio = this._temp[i] as Bio; if (bio == null || bio == self || bio.isDead || !bio.volumetric || bio.property.dashing > 0 || bio.property.ignoreVolumetric > 0) { continue; } Vec3 localPos = self.PointToLocal(bio.property.position); if (localPos.x >= 0) { float expandedRadius = bio.size.z + self.size.z; if (MathUtils.Abs(localPos.z) < expandedRadius) { float cX = localPos.x; float cZ = localPos.z; float sqrtPart = MathUtils.Sqrt(expandedRadius * expandedRadius - cZ * cZ); float ip = cX - sqrtPart; if (ip <= 0.0f) { ip = cX + sqrtPart; } if (ip < distToClosestIp) { distToClosestIp = ip; closestIntersectingObstacle = bio; localPosOfClosestObstacle = localPos; } } } } this._temp.Clear(); if (closestIntersectingObstacle != null) { Vec3 steeringForce = Vec3.zero; const float minLocalXDistance = .3f; //限定最小的x轴距离,值越小,下面得到的x轴因子越大,侧向力就越大 float multiplier = 1.5f / MathUtils.Min(minLocalXDistance, localPosOfClosestObstacle.x); //侧向力和障碍物的x距离成反比,越近x轴的因子越大,侧向力越大 steeringForce.z = -closestIntersectingObstacle.size.z * multiplier / localPosOfClosestObstacle.z; //侧向力和障碍物的半径成正比,z轴距离成反比 const float brakingWeight = .2f; //制动力因子,值越大,速度减少越快 steeringForce.x = (closestIntersectingObstacle.size.x - localPosOfClosestObstacle.x) * brakingWeight; steeringForce = self.VectorToWorld(steeringForce); this.DebugDrawForce(steeringForce * 3); return(steeringForce); } return(Vec3.zero); }
public override bool SolvePositionConstraints(SolverData data) { Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); Vec2 d = Pool.PopVec2(); Vec2 axis = Pool.PopVec2(); Vec2 perp = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2 C1 = Pool.PopVec2(); Vec3 impulse = Pool.PopVec3(); Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; qA.Set(aA); qB.Set(aB); float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; // Compute fresh Jacobians Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(LocalCenterA), rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(LocalCenterB), rB); d.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA); Rot.MulToOutUnsafe(qA, LocalXAxisA, axis); float a1 = Vec2.Cross(temp.Set(d).AddLocal(rA), axis); float a2 = Vec2.Cross(rB, axis); Rot.MulToOutUnsafe(qA, LocalYAxisA, perp); float s1 = Vec2.Cross(temp.Set(d).AddLocal(rA), perp); float s2 = Vec2.Cross(rB, perp); C1.X = Vec2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = MathUtils.Abs(C1.X); float angularError = MathUtils.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (m_limitEnabled) { float translation = Vec2.Dot(axis, d); if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION); linearError = MathUtils.Max(linearError, MathUtils.Abs(translation)); active = true; } else if (translation <= LowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - LowerTranslation + Settings.LINEAR_SLOP, -Settings.MAX_LINEAR_CORRECTION, 0.0f); linearError = MathUtils.Max(linearError, LowerTranslation - translation); active = true; } else if (translation >= UpperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - UpperTranslation - Settings.LINEAR_SLOP, 0.0f, Settings.MAX_LINEAR_CORRECTION); linearError = MathUtils.Max(linearError, translation - UpperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = Pool.PopMat33(); K.Ex.Set(k11, k12, k13); K.Ey.Set(k12, k22, k23); K.Ez.Set(k13, k23, k33); Vec3 C = Pool.PopVec3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; K.Solve33ToOut(C.NegateLocal(), impulse); Pool.PushVec3(1); Pool.PushMat33(1); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = Pool.PopMat22(); K.Ex.Set(k11, k12); K.Ey.Set(k12, k22); // temp is impulse1 K.SolveToOut(C1.NegateLocal(), temp); C1.NegateLocal(); impulse.X = temp.X; impulse.Y = temp.Y; impulse.Z = 0.0f; Pool.PushMat22(1); } float Px = impulse.X * perp.X + impulse.Z * axis.X; float Py = impulse.X * perp.Y + impulse.Z * axis.Y; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA.X -= mA * Px; cA.Y -= mA * Py; aA -= iA * LA; cB.X += mB * Px; cB.Y += mB * Py; aB += iB * LB; data.Positions[IndexA].C.Set(cA); data.Positions[IndexA].A = aA; data.Positions[IndexB].C.Set(cB); data.Positions[IndexB].A = aB; Pool.PushVec2(7); Pool.PushVec3(1); Pool.PushRot(2); return(linearError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP); }
/// <summary> /// Raycast against this AABB using the specified points and maxfraction (found in input) /// </summary> /// <param name="output">The results of the raycast.</param> /// <param name="input">The parameters for the raycast.</param> /// <returns>True if the ray intersects the AABB</returns> public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true) { // From Real-time Collision Detection, p179. output = new RayCastOutput(); float tmin = -Settings.MaxFloat; float tmax = Settings.MaxFloat; Vector2 p = input.Point1; Vector2 d = input.Point2 - input.Point1; Vector2 absD = MathUtils.Abs(d); Vector2 normal = Vector2.Zero; for (int i = 0; i < 2; ++i) { float absD_i = i == 0 ? absD.X : absD.Y; float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y; float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y; float p_i = i == 0 ? p.X : p.Y; if (absD_i < Settings.Epsilon) { // Parallel. if (p_i < lowerBound_i || upperBound_i < p_i) { return(false); } } else { float d_i = i == 0 ? d.X : d.Y; float inv_d = 1.0f / d_i; float t1 = (lowerBound_i - p_i) * inv_d; float t2 = (upperBound_i - p_i) * inv_d; // Sign of the normal vector. float s = -1.0f; if (t1 > t2) { MathUtils.Swap(ref t1, ref t2); s = 1.0f; } // Push the min up if (t1 > tmin) { if (i == 0) { normal.X = s; } else { normal.Y = s; } tmin = t1; } // Pull the max down tmax = Math.Min(tmax, t2); if (tmin > tmax) { return(false); } } } // Does the ray start inside the box? // Does the ray intersect beyond the max fraction? if (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin)) { return(false); } // Intersection. output.Fraction = tmin; output.Normal = normal; return(true); }
public bool AproxEqualsBox(Vec2 vector, float tolerance) { return ((MathUtils.Abs(this.x - vector.x) <= tolerance) && (MathUtils.Abs(this.y - vector.y) <= tolerance)); }
public static Vec3 Abs(Vec3 v) { return(new Vec3(MathUtils.Abs(v.x), MathUtils.Abs(v.y), MathUtils.Abs(v.z))); }
public void Update() { if (gameOver || !hasStarted) { return; } currentTick++; // increasing the difficulty if (currentTick % 100 == 0 && speed < 0.2f) { speed += 0.01f; } if (up) { if (right) { ball.x = ball.x + MathUtils.Cos(bounceAngle) * speed; ball.y = ball.y + MathUtils.Sin(bounceAngle) * speed; } else { ball.x = ball.x - MathUtils.Sin(bounceAngle) * speed; ball.y = ball.y + MathUtils.Cos(bounceAngle) * speed; } } else { if (right) { ball.x = ball.x + MathUtils.Cos(bounceAngle) * speed; ball.y = ball.y - MathUtils.Sin(bounceAngle) * speed; } else { ball.x = ball.x - MathUtils.Sin(bounceAngle) * speed; ball.y = ball.y - MathUtils.Cos(bounceAngle) * speed; } } // verifies if it hits the walls if (ball.x > higherBoundGridX) { right = false; } else if (ball.x < lowerBoundGridX) { right = true; } // verifies if it hit the paddles if (MathUtils.Abs(ball.y, paddle2.y) <= 0.2f && HasCollided(paddle2.x, ball.x)) { up = false; bounceAngle = GetAngle(); } else if (MathUtils.Abs(ball.y, paddle1.y) <= 0.2f && HasCollided(paddle1.x, ball.x)) { up = true; bounceAngle = GetAngle(); } // pause to show the goal sign if (isGoalPause && currentTick > goalUnpauseTick) { isGoalPause = false; speed = START_SPEED; // reset the game depending on who scored if (ball.y < lowerBoundGridY) { // player 1 starts ball.x = paddle1.x; ball.y = lowerBoundGridY; up = true; right = true; } else if (ball.y > higherBoundGridY) { ball.x = paddle2.x; ball.y = higherBoundGridY; up = false; right = false; } } else if (ball.y < (lowerBoundGridY - 1) && !isGoalPause) { isGoalPause = true; // 2 seconds to show the Goal sign goalUnpauseTick = currentTick + FPS * 2; player2Score++; } else if (ball.y > (higherBoundGridY + 1) && !isGoalPause) { isGoalPause = true; // 2 seconds to show the Goal sign goalUnpauseTick = currentTick + FPS * 2; player1Score++; } }