public void RemoveBody(PhysicsBody body)
 {
     Broadphase.RemoveBody(body);
 }
        public PhysicsBody[] Solve(double time, PhysicsBody[][] bodies)
        {
            int count = bodies.Length / PhysicsOptions.BatchSize;

            if (bodies.Length % PhysicsOptions.BatchSize != 0)
            {
                count++;
            }


            Parallel.For(0, count, (base_idx) =>
            {
                for (int batch_idx = base_idx * PhysicsOptions.BatchSize; batch_idx < PhysicsOptions.BatchSize && batch_idx < bodies.Length; batch_idx++)
                {
                    //Check for collisions and resolve them
                    var batch = bodies[batch_idx];
                    for (int i0 = 0; i0 < batch.Length; i0++)
                    {
                        for (int i1 = i0 + 1; i1 < batch.Length; i1++)
                        {
                            PhysicsBody o0 = null;
                            PhysicsBody o1 = null;
                            if (batch[i0].Kind < batch[i1].Kind)
                            {
                                o0 = batch[i0];
                                o1 = batch[i1];
                            }
                            else
                            {
                                o0 = batch[i1];
                                o1 = batch[i0];
                            }

                            switch (o0.Kind)
                            {
                            case EntityType.FixedPlane:
                                switch (o1.Kind)
                                {
                                case EntityType.FixedPlane:
                                    //Don't check
                                    break;

                                case EntityType.OrientedBoundingBox:

                                    break;

                                case EntityType.Point:

                                    break;

                                case EntityType.Sphere:
                                    //Distance from sphere to plane <= radius of sphere
                                    break;

                                case EntityType.Triangle:

                                    break;
                                }
                                break;

                            case EntityType.OrientedBoundingBox:
                                switch (o1.Kind)
                                {
                                case EntityType.OrientedBoundingBox:

                                    break;

                                case EntityType.Point:

                                    break;

                                case EntityType.Sphere:

                                    break;

                                case EntityType.Triangle:

                                    break;
                                }
                                break;

                            case EntityType.Point:
                                switch (o1.Kind)
                                {
                                case EntityType.Point:
                                    //Doesn't make sense
                                    break;

                                case EntityType.Sphere:
                                    //Distance between center and point squared <= radius squared
                                    break;

                                case EntityType.Triangle:

                                    break;
                                }
                                break;

                            case EntityType.Sphere:
                                switch (o1.Kind)
                                {
                                case EntityType.Sphere:
                                    //Distance between centers squared <= sum of radius squared
                                    break;

                                case EntityType.Triangle:

                                    break;
                                }
                                break;

                            case EntityType.Triangle:
                                switch (o1.Kind)
                                {
                                case EntityType.Triangle:

                                    break;
                                }
                                break;
                            }
                        }
                    }
                }
            });
            return(null);
        }
 public void AddBody(PhysicsBody body)
 {
     Broadphase.AddBody(body);
 }
        public PhysicsBody[][] Solve(double time)
        {
            //Check all points within range of each body and make a list of all intersecting axes
            //Create a set for each 'group' of interactions
            HashSet <PhysicsBody>         netSet    = null;
            List <HashSet <PhysicsBody> > finalSets = null;

            for (int a = 0; a < 3; a++)
            {
                HashSet <PhysicsBody>         collisionNetSets = new HashSet <PhysicsBody>();
                List <HashSet <PhysicsBody> > collisionSets    = new List <HashSet <PhysicsBody> >();

                for (int i = 0; i < _objs[a].Count; i++)
                {
                    var obj_i = _objs[a][i];
                    for (int j = i + 1; j < _objs[a].Count; j++)
                    {
                        var obj_j = _objs[a][j];

                        float intersec_dist = (obj_i.CenterOfMass[a] + obj_i.BoundingRadius) - (obj_j.CenterOfMass[a] - obj_j.BoundingRadius);
                        if (intersec_dist >= 0)
                        {
                            //Intersection (>0) or contact (=0)
                            collisionNetSets.Add(obj_j);
                            collisionNetSets.Add(obj_i);

                            bool added = false;
                            for (int k = 0; k < collisionSets.Count; k++)
                            {
                                if (collisionSets[k].Contains(obj_i))
                                {
                                    added = true;
                                    collisionSets[k].Add(obj_j);
                                    break;
                                }
                                else if (collisionSets[k].Contains(obj_j))
                                {
                                    added = true;
                                    collisionSets[k].Add(obj_i);
                                    break;
                                }
                            }
                            if (!added)
                            {
                                var set = new HashSet <PhysicsBody>();
                                set.Add(obj_j);
                                set.Add(obj_i);
                                collisionSets.Add(set);
                            }
                        }
                    }
                }

                if (a == 0)
                {
                    finalSets = collisionSets;
                    netSet    = collisionNetSets;
                }
                else
                {
                    netSet = netSet.Intersect(collisionNetSets).ToHashSet();
                }
            }

            var collisionGroups = new PhysicsBody[finalSets.Count][];

            for (int i = 0; i < finalSets.Count; i++)
            {
                collisionGroups[i] = finalSets[i].Intersect(netSet).ToArray();
            }

            return(collisionGroups);
        }