private void RenderBody(Graphics gfx, Body2 body) { if (body.Shape is Circle) { Circle c = body.Shape as Circle; Vec2 beginning = body.Position.Sub(c.Radius); gfx.DrawEllipse(Pens.Red, new Rectangle((int)beginning.X, (int)beginning.Y, (int)c.Radius * 2, (int)c.Radius * 2)); } if (body.Shape is Poly2) { Poly2 p = body.Shape as Poly2; for (int i = 0; i < p.Vertices.Count; ++i) { Math.Matrix.Mat2 rot = Math.Matrix.Mat2.Rotation(body.Rotation); int j = i + 1 < p.Vertices.Count ? i + 1 : 0; Vec2 v1 = body.Position.Add(rot.Mul(p.Vertices[i])); Vec2 v2 = body.Position.Add(rot.Mul(p.Vertices[j])); Vec2 mid = v1.Add(v2).Div(2); Vec2 dir = mid.Add(rot.Mul(p.Normals[i]).Mul(10)); gfx.DrawLine(Pens.Red, new Point((int)v1.X, (int)v1.Y), new Point((int)v2.X, (int)v2.Y)); gfx.DrawLine(Pens.Green, new Point((int)mid.X, (int)mid.Y), new Point((int)dir.X, (int)dir.Y)); gfx.FillEllipse(Brushes.Blue, new Rectangle((int)v1.X - 1, (int)v1.Y - 2, 4, 4)); gfx.FillEllipse(Brushes.Blue, new Rectangle((int)v2.X - 1, (int)v2.Y - 2, 4, 4)); } } }
public void Initialize() { Vec2 c = new Vec2(); I = 0; Area = 0; float k_inv3 = 1.0f / 3.0f; for (int i = 0; i < Vertices.Count; ++i) { Vec2 p1 = new Vec2(Vertices[i]); int j = i + 1 < Vertices.Count ? i + 1 : 0; Vec2 p2 = new Vec2(Vertices[j]); float D = p1.Cross(p2); float triangleArea = 0.5f * D; Area += triangleArea; c.Add(Vec2.Mul(Vec2.Add(p1, p2), triangleArea * k_inv3)); float x2 = p1.X * p1.X + p2.X * p1.X + p2.X * p2.X; float y2 = p1.Y * p1.Y + p2.Y * p1.Y + p2.Y * p2.Y; I += (0.25f * k_inv3 * D) * (x2 + y2); } c.Mul(1.0f / Area); for (int i = 0; i < Vertices.Count; ++i) { // internalVertices[i].Sub(c); } }
public void Update() { Controls(); // Basic Euler integration: _position.Add(velocity); Shoot(); UpdateScreenPosition(); }
public void Vector2Test() { Vec2 vec2A = Vec2.Zero; vec2A.Add(new Vec2(1f, 1f)); vec2A.Add(new Vec2(3f, 2f)); Assert.AreEqual(vec2A, new Vec2(4f, 3f)); var vec2B = (vec2A + new Vec2(0f, 2f) * 0.5f) / 2 - new Vec2(2f, 2f); Assert.AreEqual(vec2B, Vec2.Zero); var vec2C = Vec2.Zero.Add(new Vec2(3, 4)); Assert.AreEqual(vec2C, new Vec2(3, 4)); Assert.AreEqual(vec2C.LenSqr, 25); Assert.AreEqual(vec2C.Len, 5); Assert.AreEqual(Vec2.Zero, new Vec2(0, 0)); vec2C.NormSelf(); Assert.AreEqual(vec2C, new Vec2(3f, 4f) / 5f); // check normself Assert.AreEqual(vec2C.Len, 1); // check length after normself }
protected override void RenderSelf(GLContext glContext) { if (_startPoint == null || _vector == null) return; var endPoint = _startPoint.Clone().Add(_vector.Clone().Scale(Scale)); LineSegment.RenderLine(_startPoint, endPoint, Color, LineWidth, true); var smallVec = endPoint.Clone().Subtract(_startPoint).Normalize().Scale(-10); var left = new Vec2(-smallVec.y, smallVec.x); var right = new Vec2(smallVec.y, -smallVec.x); left.Add(smallVec).Add(endPoint); right.Add(smallVec).Add(endPoint); LineSegment.RenderLine(endPoint, left, Color, LineWidth, true); LineSegment.RenderLine(endPoint, right, Color, LineWidth, true); }
/******* PRIVATE METHODS *******************************************************************/ void Move() { // TODO: implement Assignment 3 here (and in methods called from here). velocity.Add(acceleration); _position.Add(velocity); // Example methods (replace/extend): //CheckBoundaryCollisions(); //CheckBlockOverlaps(); // TIP: You can use the Collision class to pass information between methods, e.g.: // Collision firstCollision = FindEarliestCollision(); if (firstCollision != null) { ResolveCollision(firstCollision); } }
//create on click private void canvasArea_MouseLeftButtonDown(object sender, MouseButtonEventArgs e) { Vec2 currentVel = new Vec2(0, 0); currentVel = currentVel.Add(creatorVelocity); var currentRad = creatorRadius; if (creatorIsBody) { Body newBody; newBody = new Body((int)e.GetPosition(canvasArea).X, (int)e.GetPosition(canvasArea).Y, 0, 0, currentRad); bodies.Add(newBody); DrawObjects(); } else { BallV newBall; newBall = new BallV((int)e.GetPosition(canvasArea).X, (int)e.GetPosition(canvasArea).Y, currentVel, currentRad); balls.Add(newBall); DrawObjects(); } }
public void SolveVelocityConstraints() { for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; int pointCount = vc.PointCount; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; //Debug.Assert(wA == 0); //Debug.Assert(wB == 0); Vec2 normal = vc.Normal; //Vec2.crossToOutUnsafe(normal, 1f, tangent); tangent.X = 1.0f * vc.Normal.Y; tangent.Y = (-1.0f) * vc.Normal.X; float friction = vc.Friction; Debug.Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; //Vec2.crossToOutUnsafe(wA, vcp.rA, temp); //Vec2.crossToOutUnsafe(wB, vcp.rB, dv); //dv.addLocal(vB).subLocal(vA).subLocal(temp); Vec2 a = vcp.RA; dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y; dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X; // Compute tangent force float vt = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed; float lambda = vcp.TangentMass * (-vt); // Clamp the accumulated force float maxFriction = friction * vcp.NormalImpulse; float newImpulse = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - vcp.TangentImpulse; vcp.TangentImpulse = newImpulse; // Apply contact impulse // Vec2 P = lambda * tangent; float Px = tangent.X * lambda; float Py = tangent.Y * lambda; // vA -= invMassA * P; vA.X -= Px * mA; vA.Y -= Py * mA; wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px); // vB += invMassB * P; vB.X += Px * mB; vB.Y += Py * mB; wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px); //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA); //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB); } // Solve normal constraints if (vc.PointCount == 1) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0]; Vec2 a1 = vcp.RA; // Relative velocity at contact //Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA); //Vec2.crossToOut(wA, vcp.rA, temp1); //Vec2.crossToOut(wB, vcp.rB, dv); //dv.addLocal(vB).subLocal(vA).subLocal(temp1); dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y; dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X; // Compute normal impulse float vn = dv.X * normal.X + dv.Y * normal.Y; float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias); // Clamp the accumulated impulse float a = vcp.NormalImpulse + lambda; float newImpulse = (a > 0.0f ? a : 0.0f); lambda = newImpulse - vcp.NormalImpulse; //Debug.Assert(newImpulse == 0); vcp.NormalImpulse = newImpulse; // Apply contact impulse float Px = normal.X * lambda; float Py = normal.Y * lambda; // vA -= invMassA * P; vA.X -= Px * mA; vA.Y -= Py * mA; wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px); //Debug.Assert(vA.x == 0); // vB += invMassB * P; vB.X += Px * mB; vB.Y += Py * mB; wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px); //Debug.Assert(vB.x == 0); } 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 = a + d // // a := old total impulse // x := new total impulse // d := incremental impulse // // For the current iteration we extend the formula for the incremental impulse // to compute the new total impulse: // // vn = A * d + b // = A * (x - a) + b // = A * x + b - A * a // = A * x + b' // b' = b - A * a; ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0]; ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1]; a.X = cp1.NormalImpulse; a.Y = cp2.NormalImpulse; Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f); // Relative velocity at contact // Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA); dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y; dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X; // Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y; dv2.Y = wB * cp2.RB.X + vB.Y - 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; b.X = vn1 - cp1.VelocityBias; b.Y = vn2 - cp2.VelocityBias; //Console.WriteLine("b is " + b.x + "," + b.y); // Compute b' Mat22 R = vc.K; b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y); b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y); //Console.WriteLine("b' is " + b.x + "," + b.y); // final float k_errorTol = 1e-3f; // B2_NOT_USED(k_errorTol); for (; ;) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // // Vec2 x = - Mul(c.normalMass, b); Mat22.MulToOutUnsafe(vc.NormalMass, b, x); x.MulLocal(-1); if (x.X >= 0.0f && x.Y >= 0.0f) { //Console.WriteLine("case 1"); // Get the incremental impulse // Vec2 d = x - a; d.Set(x).SubLocal(a); // Apply incremental impulse // Vec2 P1 = d.x * normal; // Vec2 P2 = d.y * normal; P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); /* * vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - * Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); * * // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal); * * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias) * < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA))); Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA))); // Compute normal velocity vn1 = Vec2.Dot(_dv1, normal); vn2 = Vec2.Dot(_dv2, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I); Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I); } break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + ' // x.X = (-cp1.NormalMass) * b.X; x.Y = 0.0f; vn1 = 0.0f; vn2 = vc.K.Ex.Y * x.X + b.Y; if (x.X >= 0.0f && vn2 >= 0.0f) { //Console.WriteLine("case 2"); // Get the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse // Vec2 P1 = d.x * normal; // Vec2 P2 = d.y * normal; P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - * Cross(wA, cp1.rA); * * // Compute normal velocity vn1 = Dot(dv1, normal); * * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA))); // Compute normal velocity vn1 = Vec2.Dot(_dv1, normal); Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I); } break; } // // Case 3: wB = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + ' // x.X = 0.0f; x.Y = (-cp2.NormalMass) * b.Y; vn1 = vc.K.Ey.X * x.Y + b.X; vn2 = 0.0f; if (x.Y >= 0.0f && vn1 >= 0.0f) { //Console.WriteLine("case 3"); // Resubstitute for the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); cp1.NormalImpulse = x.X; cp2.NormalImpulse = x.Y; /* * #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA - * Cross(wA, cp2.rA); * * // Compute normal velocity vn2 = Dot(dv2, normal); * * Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif */ if (DEBUG_SOLVER) { // Postconditions Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA))); // Compute normal velocity vn2 = Vec2.Dot(_dv2, normal); Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I); } break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = ; x.X = 0.0f; x.Y = 0.0f; vn1 = b.X; vn2 = b.Y; if (vn1 >= 0.0f && vn2 >= 0.0f) { //Console.WriteLine("case 4"); // Resubstitute for the incremental impulse d.Set(x).SubLocal(a); // Apply incremental impulse /* * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); * * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); */ P1.Set(normal).MulLocal(d.X); P2.Set(normal).MulLocal(d.Y); temp1.Set(P1).AddLocal(P2); temp2.Set(temp1).MulLocal(mA); vA.SubLocal(temp2); temp2.Set(temp1).MulLocal(mB); vB.AddLocal(temp2); //Debug.Assert(vA.x == 0); //Debug.Assert(vB.x == 0); wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2)); wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2)); // Accumulate //Debug.Assert(x.x == 0 && x.y == 0); 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; } } Velocities[indexA].V.Set(vA); Velocities[indexA].W = wA; Velocities[indexB].V.Set(vB); Velocities[indexB].W = wB; //Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA); //Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB); } }
public void Step() { //FollowMouse (); position.Add(velocity); UpdateScreenPosition(); }
public void Update() { _position.Add(velocity); UpdateScreenPosition(); }