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();
}
