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 AddBody(CircleBody c)
{
if (circles.Count <= 0)
{
this.Pos = c.Pos;
this.InvMass = c.InvMass;
this.InvInertia = c.InvInertia;
c.Pos = Vector2.Zero;
circles.Add(c);
return;
}
circles.Add(c);
float currentMass = (InvMass > 0.0f) ? 1f / InvMass : 0.0f;
float circleMass = (c.InvMass > 0.0f) ? 1f / c.InvMass : 0f;
//Calculate new center of mass
Vector2 newCom = ((currentMass * this.Pos) + (circleMass * c.Pos)) / (currentMass + circleMass);
//Calculate new inverse mass
currentMass += circleMass;
this.InvMass = 1f / currentMass;
c.Pos = c.Pos - this.Pos;
float inertia = 0f;
foreach (CircleBody circle in circles)
{
circle.Pos += this.Pos;
circle.Pos = circle.Pos - newCom;
float tempMass = 1f / circle.InvMass;
//Parallel axis theorem
inertia += ((tempMass * circle.Radius * circle.Radius)) + tempMass * circle.Pos.LengthSquared();
}
this.InvInertia = 1f / inertia;
this.Pos = newCom;
}
public void AddBody(CircleBody c)
{
if (circles.Count <= 0)
{
this.Pos = c.Pos;
this.InvMass = c.InvMass;
this.InvInertia = c.InvInertia;
c.Pos = Vector2.Zero;
circles.Add(c);
return;
}
circles.Add(c);
float currentMass = (InvMass > 0.0f) ? 1f/InvMass : 0.0f;
float circleMass = (c.InvMass > 0.0f) ? 1f / c.InvMass : 0f;
//Calculate new center of mass
Vector2 newCom = ((currentMass * this.Pos) + (circleMass * c.Pos)) / (currentMass + circleMass);
//Calculate new inverse mass
currentMass += circleMass;
this.InvMass = 1f / currentMass;
c.Pos = c.Pos - this.Pos;
float inertia = 0f;
foreach (CircleBody circle in circles)
{
circle.Pos += this.Pos;
circle.Pos = circle.Pos - newCom;
float tempMass = 1f / circle.InvMass;
//Parallel axis theorem
inertia += ((tempMass*circle.Radius*circle.Radius)) + tempMass * circle.Pos.LengthSquared();
}
this.InvInertia = 1f / inertia;
this.Pos = newCom;
}
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 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 void RemoveBody(CircleBody c)
{
circles.Remove(c);
}
public void RemoveBody(CircleBody c)
{
circles.Remove(c);
}
public override void Update(GameTime g)
{
base.Update(g);
cam.Update(g);
if (InputHandler.MouseState.LeftButton == ButtonState.Pressed && InputHandler.LastMouseState.LeftButton == ButtonState.Released)
{
mouseHeldDown = true;
mouseForceStart = cam.GetWorldMousePos();
}
else if (InputHandler.MouseState.LeftButton != ButtonState.Pressed)
{
mouseHeldDown = false;
}
if (InputHandler.MouseState.RightButton == ButtonState.Pressed)
{
float radius = rand.Next(10, 50) / 100f;
//RigidBody2D rb = new CircleBody(cam.GetWorldMousePos(), new Vector2(0, 0), 0.0f, MathHelper.Pi * radius*radius * 100, radius);
RigidBody2D rb = new CircleBody(cam.GetWorldMousePos(), Vector2.Zero, 0f, 2f, 0.2f);
engine.AddRigidBody(rb);
}
Vector2 force = Vector2.Zero;
Vector2 mouseWorldPos = cam.GetWorldMousePos();
if (mouseHeldDown)
{
force = mouseWorldPos - mouseForceStart;
foreach (RigidBody2D rb in engine.GetBodies())
{
rb.AddForce(force);
}
}
if (!InputHandler.IsKeyPressed(Keys.Space))
{
engine.Update(g);
}
else if (InputHandler.IsNewKeyPress(Keys.Right))
{
engine.Update(g);
}
}
private void setupEngine()
{
engine = new PhysicsEngine2D();
int bodyCount = 1;
for (int i = 0; i < bodyCount; i++)
{
engine.AddRigidBody(new CircleBody(new Vector2(i+2, 8), new Vector2(0, 0), 0f, 0f, 0.2f));
}
for (int i = 0; i < bodyCount; i++)
{
RigidBody2D rb = new CircleBody(new Vector2(i +2, 6), new Vector2(1 , 0), 0.0f, 10f, 0.2f);
engine.AddRigidBody(rb);
engine.AddConstraint(new DistanceConstraint2D(rb, engine.GetBodies()[0], 4));
}
engine.AddRigidBody(new LineBody(Vector2.UnitY, Vector2.Zero));
engine.AddRigidBody(new LineBody(Vector2.UnitX, Vector2.Zero));
engine.AddRigidBody(new LineBody(-Vector2.UnitY, new Vector2(0, 10)));
engine.AddRigidBody(new LineBody(-Vector2.UnitX, new Vector2(20, 0)));
}