public Contact2D(Vector2 normal, float d, RigidBody2D a, RigidBody2D b, Vector2 pointA, Vector2 pointB)
{
Normal = normal;
Dist = d;
A = a;
B = b;
this.pointA = pointA;
this.pointB = pointB;
Normal = normal;
if (NoisyNormals)
{
Normal = new Vector2((float)(Normal.X + (rand.NextDouble() - 0.5) / 100f), (float)(Normal.Y + (rand.NextDouble() - 0.5) / 100f));
}
// compute denominator in impulse equation
float x = A.InvMass;
float y = B.InvMass;
float ran = Vector2.Dot(pointA - A.Pos, Normal);
float rbn = Vector2.Dot(pointB - B.Pos, Normal);
float z = ran * ran * A.InvInertia;
float w = rbn * rbn * B.InvInertia;
InvDenom = 1f / (x + y + z + w);
//I = (1+e)*N*(Vr • N) / (1/Ma + 1/Mb)
//Impulse = (Vector2.Dot(b.Vel - a.Vel, normal) / (a.InvMass + b.InvMass));
ImpulseN = 0;
ImpulseT = 0;
}
public override void AddContacts(RigidBody2D rb, float dt, ref List<Contact2D> contacts)
{
//Profiler says this method takes up a lot of processing time (AKA, it's run quite often)
if (rb as CircleBody != null)
{
CircleBody c = rb as CircleBody;
Vector2 normal = rb.Pos - this.Pos;
float normLen = normal.Length();
float dist = normLen - (this.radius + c.radius);
normal /= normLen;
Vector2 pa = this.Pos + normal * this.radius;
Vector2 pb = rb.Pos - normal * c.radius;
contacts.Add(new Contact2D(normal, dist, this, rb, pa, pb));
}
else if (rb as LineBody != null)
{
LineBody pb = rb as LineBody;
pb.AddContacts(this, dt, ref contacts);
}
else if (rb as CompoundBody != null)
{
CompoundBody cb = rb as CompoundBody;
foreach (CircleBody c in cb.Bodies)
{
this.AddContacts(c, dt, ref contacts);
}
}
else {
throw new NotImplementedException();
}
}
public void AddRigidBody(RigidBody2D rb)
{
if (rb as LineBody != null)
{
lines.Add(rb);
}
else
{
bodies.Add(rb);
}
}
public override void AddContacts(RigidBody2D rb, float dt, ref List<Contact2D> contacts)
{
if (rb as CircleBody != null)
{
CircleBody c = rb as CircleBody;
float dist = DistanceToPoint(c.Pos) - c.Radius;
Vector2 pointOnPlane = c.Pos - (normal * (dist+c.Radius));
Vector2 pointOnBall = c.Pos - (normal * c.Radius);
contacts.Add(new Contact2D(normal, dist, this, rb, pointOnPlane, pointOnBall));
}
else if (rb as LineBody != null)
{
}
else if (rb as CompoundBody != null)
{
}
else
{
throw new NotImplementedException();
}
}
public Constraint2D(RigidBody2D bodyA, RigidBody2D bodyB)
{
BodyA = bodyA;
BodyB = bodyB;
}
public abstract void AddContacts(RigidBody2D rb, float dt, ref List<Contact2D> contacts);
public override void AddContacts(RigidBody2D rb, float dt, ref List<Contact2D> contacts)
{
//Profiler says this method takes up a lot of processing time (AKA, it's run quite often)
if (rb as CircleBody != null)
{
CircleBody c = rb as CircleBody;
foreach (CircleBody circle in circles)
{
Vector2 tempPos = circle.Pos;
circle.Pos += this.Pos;
circle.AddContacts(c, dt, ref contacts);
Contact2D justAdded = contacts[contacts.Count - 1];
contacts[contacts.Count - 1] = new Contact2D(justAdded.Normal, justAdded.Dist, this, justAdded.B, justAdded.pointA, justAdded.pointB);
circle.Pos = tempPos;
}
}
else if (rb as LineBody != null)
{
foreach (CircleBody circle in circles)
{
Vector2 tempPos = circle.Pos;
circle.Pos += this.Pos;
circle.AddContacts(rb, dt, ref contacts);
Contact2D justAdded = contacts[contacts.Count - 1];
contacts[contacts.Count - 1] = new Contact2D(justAdded.Normal, justAdded.Dist, justAdded.A, this, justAdded.pointA, justAdded.pointB);
circle.Pos = tempPos;
}
}
else
{
throw new NotImplementedException();
}
}
public DistanceConstraint2D(RigidBody2D bodyA, RigidBody2D bodyB, float dist)
: base(bodyA, bodyB)
{
distance = dist;
}