I've made a little real time physics simulation that solves some bodies. CirclevCircle && CirclevOrientedRect I would like someone to tell me if I'm doing something drastically wrong. I know of ways to optimize this solution that require more programming. But so far is there anything I'm doing that could ruin my performance.
private void Simulate(float dt)
{
for (int i = 0; i < physObj.Count; i++)
{
List<Vector2> force = new List<Vector2>();
// Physics Objects
if (gravity)
{
for (int j = 0; j < physObj.Count && j != i; j++)
{
if (physObj[j].G != 0f)
{
// Calc radius between two objects
float r = Vector2.Distance(physObj[i].Center, physObj[j].Center);
// Calc components of the radius
Vector2 rVec = physObj[j].Center - physObj[i].Center;
// Calc direction based on components
float theta = (float)Math.Atan2(rVec.Y, rVec.X);
// Calc magnitude of the force of gravity Fg = G(m1m2 / r^2)
float mfG = physObj[j].G * (physObj[i].M * physObj[j].M / (r * r));
// Calc components of force of gravity using direction
Vector2 fGDir = new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta));
force.Add(fGDir * mfG);
}
}
}
// Sum Forces
for (int j = 0; j < force.Count; j++)
physObj[i].ApplyForce(force[j]);
}
// Test Collisions
if(check)
CheckCollision();
// Resolve Collisions
if(resolve)
HandleCollision();
// Update Velocity and Position
for (int i = 0; i < physObj.Count; i++)
{
// Update Velocity
physObj[i].UpdateVelocity(dt);
physObj[i].ResetForces();
// Update Position
physObj[i].UpdatePos(dt);
}
}
So here is were it checks collisions and puts them all into a list. I keep two lists of collision data. Current and previous touch. That way in the user code you can check if a collision has begun by using if(currentTouch && !previousTouch)
private void CheckCollision()
{
// Push current to previous and clear current
previousTouch = currentTouch;
currentTouch.Clear();
for (int i = 0; i < physObj.Count; i++)
{
if (physObj[i].isCollisionChecked)
{
for (int j = i + 1; j < physObj.Count; j++)
{
if (physObj[j].isCollisionChecked)
{
CollisionData data = physObj[i].collideHull.Intersects(physObj[j].collideHull);
if (data.touching)
currentTouch.Add(data);
}
}
}
}
}
Then I have my collision resolution or HandleCollision function which goes throuhg all of the collisions in list and resolves them with one iteration (I know its common to do multiple iterations to avoid jitter when multiple objects are colliding.)
private void ResolveCollision(CollisionData collisionData)
{
// Set some easily accessed objects
PhysObj physObj1 = collisionData.obj1 as PhysObj;
Shape physObj1Shape = collisionData.shape1;
PhysObj physObj2 = collisionData.obj2 as PhysObj;
Shape physObj2Shape = collisionData.shape2;
// Get the angle between the two objects
Vector2 rVec = physObj1.Center - physObj2.Center;
float theta = (float)Math.Atan2(rVec.Y, rVec.X);
if (physObj1.isCollidable && physObj2.isCollidable)
{
Vector2 n = Vector2.Zero;
// Direction of unit normal
float dir = 0f;
// Circle depth resolution
if (physObj1Shape.type == ShapeType.CIRCLE && physObj2Shape.type == ShapeType.CIRCLE)
{
// Resolve depth penetration
// Circles depth.X collisionData is actually the magnitude of the vector
float mDepth = collisionData.data.depth.X;
dir = collisionData.data.dir;
// Direction to resolve depth
n = new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta));
physObj1.Position -= (n * mDepth) / 2f;
physObj2.Position += (n * mDepth) / 2f;
} // Rectangle Circle depth resolution
else if ((physObj1Shape.type == ShapeType.RECT || physObj1Shape.type == ShapeType.ORECT) && physObj2Shape.type == ShapeType.CIRCLE)
{
// Resolve depth penetration
Vector2 depth = collisionData.data.depth;
// Get the direction of the collision
dir = collisionData.data.dir;
// Direction to resolve depth
n = new Vector2((float)Math.Cos(dir), (float)Math.Sin(dir));
physObj1.Position -= (depth * n) / 2f;
physObj2.Position += (depth * n) / 2f;
}
// Apply Force Normal
// Project the net force in the direction n
Vector2 fN = Vector2.Dot(physObj1.fNet + physObj2.fNet, n) * n;
physObj1.ApplyForce(fN);
physObj2.ApplyForce(-fN);
// Apply impulse
if (impulse)
physObj1.ApplyImpulse(physObj2, n);
}
}
I left quite a lot out for the sake of simplicity. But one other important thing I should note.
public void ApplyImpulse(PhysObj objHit, Vector2 n)
{
// Calculate change in momentum ( the equation would be (1 + E) * ((v1 - v2) * n / ( 1 / m1 + 1 / m2)) )
Vector2 p = (((Vector2.Dot((v - objHit.v), n)) / (1f / M + 1f / objHit.M)) * (1f + (E + objHit.E) / 2f)) * n;
// Calculate final velocities
v = v - (p / M);
objHit.v = objHit.v + (p / objHit.M);
}
E is elasticity, m is mass, v is velocity. There might be some inconsistencies in variables names or such since I changed a little bit from the original code I had.