Can I make this physics simulation any faster?

Question

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.

Answer 1

So the first thing I noticed was that when you do

previousTouch = currentTouch;
currentTouch.Clear();

this should also clear previous touch since List is a reference type.

The second thing is that there is no space partitioning so your collision check currently runs in O(n^2) as best case runtime.

The third thing is that it's weird that you calculate the atan2 of the vector between the two objects to get theta and then you only use theta to get the vector back.

float theta = (float)Math.Atan2(rVec.Y, rVec.X);
[...]
n = new Vector2((float)Math.Cos(theta), (float)Math.Sin(theta));

Fourth there is also something to be said about the effectiveness of lists, especially in a garbage collected environment, but that varies a lot depending on implementation details.

Fifth You can handle your collision phase differently if you don't care if objects intersect for a few frames, by just letting them gardually push each other out by applying force.
It looks like you are trying to guarantee zero intersect, which you are currently not doing. (An object can be moved out of the first collision and then back into it when resolving the second collision)

As a last sidenote I think that simulating n-body attraction is just always going to be a performance nightmare, so in some way you don't really need to worry about this rest, since that's what will most definitely ruin your performance. (Although space partitioning can also aleviate that a bit since you can only check with objects that are close enough to matter)

I probably didn't notice everything but I think that's the jist of it.

Answer 2

I found myself that to speed up my nbody. Instead of iterating through all of i & j. With i=0, i++, j=0, j++ and setting the force of object i to fG. I can use i=0, i++, j=i+1, j++ and set the force of object i to fG and object j to -fG.