//#####################################################################
// Copyright 2003-2008, Ronald Fedkiw, Eran Guendelman, Geoffrey Irving, Michael Lentine, Craig Schroeder, Tamar Shinar, Rachel Weinstein.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
#include <Rigids/Collisions/RIGID_BODY_COLLISIONS.h>
#include <Core/Arrays/INDIRECT_ARRAY.h>
#include <Core/Basic_Geometry/BOUNDED_HORIZONTAL_PLANE.h>
#include <Core/Basic_Geometry/SPHERE.h>
#include <Core/Data_Structures/HASHTABLE.h>
#include <Core/Implicit_Objects/ANALYTIC_IMPLICIT_OBJECT.h>
#include <Core/Implicit_Objects/IMPLICIT_OBJECT.h>
#include <Core/Implicit_Objects/IMPLICIT_OBJECT_TRANSFORMED.h>
#include <Core/Implicit_Objects/MULTIBODY_LEVELSET_IMPLICIT_OBJECT.h>
#include <Core/Log/DEBUG_UTILITIES.h>
#include <Core/Log/SCOPE.h>
#include <Core/Meshes/SEGMENTED_CURVE.h>
#include <Core/Meshes/TRIANGULATED_SURFACE.h>
#include <Rigids/Articulated_Rigid_Bodies/ARTICULATED_RIGID_BODY_2D.h>
#include <Rigids/Articulated_Rigid_Bodies/ARTICULATED_RIGID_BODY_3D.h>
#include <Rigids/Collisions/COLLISION_BODY_COLLECTION.h>
#include <Rigids/Collisions/COLLISION_GEOMETRY_SPATIAL_PARTITION.h>
#include <Rigids/Collisions/RIGID_BODY_COLLISION_MANAGER.h>
#include <Rigids/Collisions/RIGID_BODY_CONTACT_GRAPH.h>
#include <Rigids/Collisions/RIGID_BODY_INTERSECTIONS.h>
#include <Rigids/Collisions/RIGID_BODY_SKIP_COLLISION_CHECK.h>
#include <Rigids/Collisions/RIGID_COLLISION_GEOMETRY.h>
#include <Rigids/Collisions/RIGID_COLLISION_GEOMETRY_1D.h>
#include <Rigids/Collisions/RIGID_COLLISION_GEOMETRY_2D.h>
#include <Rigids/Collisions/RIGID_COLLISION_GEOMETRY_3D.h>
#include <Rigids/Collisions/RIGIDS_COLLISION_CALLBACKS.h>
#include <Rigids/Collisions/SOLVE_CONTACT.h>
#include <Rigids/Joints/JOINT_MESH.h>
#include <Rigids/Joints/NORMAL_JOINT.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY_COLLECTION.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY_COLLISION_PARAMETERS.h>
#include <Rigids/Rigid_Bodies/RIGID_CLUSTER_CONSTITUENT_ID.h>
#include <Rigids/Rigid_Body_Clusters/RIGID_BODY_CLUSTER_BINDINGS.h>
#include <cfloat>
namespace PhysBAM{
//#####################################################################
// Constructor
//#####################################################################
template<class TV> RIGID_BODY_COLLISIONS<TV>::
RIGID_BODY_COLLISIONS(RIGID_BODY_COLLECTION<TV>& rigid_body_collection_input,RIGID_BODY_COLLISION_PARAMETERS<TV>& parameters_input,
    RIGIDS_COLLISION_CALLBACKS<TV>& collision_callbacks_input,RIGIDS_EXAMPLE_FORCES_AND_VELOCITIES<TV>& rigids_example_forces_and_velocities_input)
    :parameters(parameters_input),collision_callbacks(collision_callbacks_input),verbose(false),prune_stacks_from_contact(false),prune_contact_using_velocity(false),collision_manager(0),
    skip_collision_check(*new RIGID_BODY_SKIP_COLLISION_CHECK),rigid_body_collection(rigid_body_collection_input),rigids_example_forces_and_velocities(rigids_example_forces_and_velocities_input),
    spatial_partition(new COLLISION_GEOMETRY_SPATIAL_PARTITION<TV,ARRAY<COLLISION_GEOMETRY<TV>*,COLLISION_GEOMETRY_ID>,COLLISION_GEOMETRY_ID>(rigid_body_collection.collision_body_list->bodies)),
    intersections(*new RIGID_BODY_INTERSECTIONS<TV>(rigid_body_collection)),contact_graph(*new RIGID_BODY_CONTACT_GRAPH<TV>(rigid_body_collection.rigid_body_particles)),
    store_collision_intersections_for_projection(false),use_static_body_masses(false),use_parent_normal(false),
    rigid_body_cluster_bindings(rigid_body_collection.rigid_body_cluster_bindings)
{
    Set_Collision_Pair_Iterations();
    Set_Contact_Level_Iterations();Set_Contact_Pair_Iterations();
    Set_Shock_Propagation_Iterations();Set_Shock_Propagation_Level_Iterations();Set_Shock_Propagation_Pair_Iterations();
    Set_Push_Out_Iterations();Set_Push_Out_Level_Iterations();Set_Push_Out_Pair_Iterations();
    Use_Freezing_With_Push_Out();Use_Gradual_Push_Out();Set_Desired_Separation_Distance();
    Use_Rolling_Friction(false);object_indices.Preallocate(50);

    if(parameters.use_analytic_collisions) Register_Analytic_Collisions();
}
//#####################################################################
// Destructor
//#####################################################################
template<class TV> RIGID_BODY_COLLISIONS<TV>::
~RIGID_BODY_COLLISIONS()
{
    delete &contact_graph;
    delete &intersections;
    delete spatial_partition;
    delete &skip_collision_check;
}
//#####################################################################
// Function Initialize_Data_Structures
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Initialize_Data_Structures(const bool reset)
{
    contact_graph.Initialize();skip_collision_check.Initialize(rigid_body_collection.rigid_body_particles.Size(),reset);
}
//#####################################################################
// Function Adjust_Bounding_Boxes
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Adjust_Bounding_Boxes(RIGID_BODY_COLLECTION<TV>& rigid_body_collection,const T false_thickness,const T extra_padding_distance)
{
    for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
        RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
        if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);
            if(rigid_body) if(rigid_body_collection.Is_Active(rigid_body->particle_index)){
                if(rigid_body->simplicial_object){ // only modify triangulated surface bounding box
                    assert(rigid_body->simplicial_object->bounding_box);
                    RANGE<TV>& box=*rigid_body->simplicial_object->bounding_box;
                    bool already_adjusted=false; // multiple rigid bodies can share a triangulated surface - don't adjust bounding box multiple times
                    for(COLLISION_GEOMETRY_ID j(0);j<i-1;j++) if(rigid_body_collection.collision_body_list->Is_Active(j)){
                        int rigid_body_id_j=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(j);
                        if(rigid_body_id_j>=0 && rigid_body_collection.Is_Active(rigid_body_id_j) && rigid_body_collection.Rigid_Body(rigid_body_id_j).simplicial_object &&
                            rigid_body->simplicial_object==rigid_body_collection.Rigid_Body(rigid_body_id_j).simplicial_object){
                            already_adjusted=true;break;}}
                    if(!already_adjusted){
                        TV delta,edge_lengths=box.Edge_Lengths();for(int d=0;d<TV::m;d++) if(edge_lengths[d]==0) delta[d]=false_thickness;
                        box.Change_Size(delta+extra_padding_distance);}
                    rigid_body->Update_Bounding_Box();}}}}
}
//#####################################################################
// Function Get_Deepest_Intersection_Point
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Get_Deepest_Intersection_Point(const int id_1,const int id_2,ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> >& particle_intersections,
    T& smallest_value,int& smallest_index,TV& collision_location,TV& collision_normal,TV& collision_relative_velocity,const bool ignore_separating,const T desired_separation_distance,
    bool& ignored_separating) const
{
    ignored_separating=false;
    particle_intersections.Remove_All();
    intersections.Append_All_Intersections(id_1,id_2,particle_intersections,desired_separation_distance);
    if(!particle_intersections.m){skip_collision_check.Set_Last_Checked(id_1,id_2);return false;}
    smallest_value=FLT_MAX;smallest_index=-1;
    for(int i=0;i<particle_intersections.m;i++){
        const RIGID_BODY_PARTICLE_INTERSECTION<TV>& intersection=particle_intersections(i);
        T phi=(*rigid_body_collection.Rigid_Body(intersection.levelset_body).implicit_object)(rigid_body_collection.Rigid_Body(intersection.particle_body).World_Space_Point(intersection.particle_location));
        if(phi<smallest_value){
            RIGID_BODY<TV> &body0=rigid_body_collection.Rigid_Body(intersection.particle_body),&body1=rigid_body_collection.Rigid_Body(intersection.levelset_body);
            RIGID_BODY<TV> &body_parent1=rigid_body_cluster_bindings.Get_Parent(body0),&body_parent2=rigid_body_cluster_bindings.Get_Parent(body1);
            if(ignore_separating){
                TV location=body0.World_Space_Point(intersection.particle_location);
                TV normal=use_parent_normal?body_parent2.Implicit_Geometry_Normal(location):body1.Implicit_Geometry_Normal(location);
                TV relative_velocity=RIGID_BODY<TV>::Relative_Velocity_At_Geometry1_Particle(body_parent1,body_parent2,location,intersection.particle_index);
                if(relative_velocity.Dot(normal)>=0){ignored_separating=true;continue;}
                collision_location=location;collision_normal=normal;collision_relative_velocity=relative_velocity;}
            smallest_value=phi;smallest_index=i;}}

    if(smallest_index>=0 && !ignore_separating){ // these quantities are already computed if we ignore_separating
        const RIGID_BODY_PARTICLE_INTERSECTION<TV>& intersection=particle_intersections(smallest_index);
        RIGID_BODY<TV> &body0=rigid_body_collection.Rigid_Body(intersection.particle_body),&body1=rigid_body_collection.Rigid_Body(intersection.levelset_body);
        collision_location=body0.World_Space_Point(intersection.particle_location);collision_normal=body1.Implicit_Geometry_Normal(collision_location);collision_relative_velocity=TV();}

    return smallest_index>=0;
}
//#####################################################################
// Function Update_Collision_Pair
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Update_Collision_Pair(const int id_1,const int id_2,const T dt,const T time,const bool mpi_one_ghost)
{
    RIGID_BODY<TV>& body0=rigid_body_collection.Rigid_Body(id_1),&body1=rigid_body_collection.Rigid_Body(id_2);
    VECTOR<std::string,2> key(typeid(*body0.implicit_object->object_space_implicit_object).name(),typeid(*body1.implicit_object->object_space_implicit_object).name());
    if(key.x==typeid(MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>).name()||key.y==typeid(MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>).name()){
        if(!body0.simplicial_object && !body1.simplicial_object) return Update_Analytic_Multibody_Collision(body0,body1,dt,time,mpi_one_ghost);}
    if(bool (**collision_function)(RIGID_BODY_COLLISIONS&,const int,const int,IMPLICIT_OBJECT<TV>*,IMPLICIT_OBJECT<TV>*,const T,const T,const bool)=analytic_collision_registry.Get_Pointer(key.Sorted()))
        return (*collision_function)(*this,id_1,id_2,body0.implicit_object->object_space_implicit_object,body1.implicit_object->object_space_implicit_object,dt,time,mpi_one_ghost);
    return Update_Levelset_Collision_Pair(id_1,id_2,dt,time,mpi_one_ghost);
}
//#####################################################################
// Function Update_Collision_Pair_Helper
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Update_Collision_Pair_Helper(RIGID_BODY<TV>& body0,RIGID_BODY<TV>& body1,const T dt,const T time,const TV& collision_location,const TV& collision_normal,const TV& collision_relative_velocity,const bool mpi_one_ghost)
{
    fractured_bodies=VECTOR<int,2>();
    RIGID_BODY<TV> &body_parent1=rigid_body_cluster_bindings.Get_Parent(body0),&body_parent2=rigid_body_cluster_bindings.Get_Parent(body1);
    // TODO: ok to store object space location of intersection point?
    if(store_collision_intersections_for_projection)
        rigid_body_particles_intersections.Set(std::make_tuple(body0.particle_index,body1.particle_index,collision_location));
    assert(Either_Body_Collides_With_The_Other(body0.particle_index,body1.particle_index));
    // TODO: put an assert where we compute joints that collisions are two-way (later make joint respect one-way collisions)
    if(!Body_Collides_With_The_Other(body0.particle_index,body1.particle_index)) body_parent1.is_temporarily_static=true;
    if(!Body_Collides_With_The_Other(body1.particle_index,body0.particle_index)) body_parent2.is_temporarily_static=true;
    TWIST<TV> impulse=collision_callbacks.Compute_Collision_Impulse(body_parent1,body_parent2,collision_location,collision_normal,collision_relative_velocity,
        RIGID_BODY<TV>::Coefficient_Of_Restitution(body_parent1,body_parent2),RIGID_BODY<TV>::Coefficient_Of_Friction(body_parent1,body_parent2),true,rolling_friction,true);

    RIGID_BODY<TV>::Apply_Impulse(body_parent1,body_parent2,collision_location,impulse.linear,impulse.angular,mpi_one_ghost);
    if(!fractured_bodies(0)){
        body_parent1.is_temporarily_static=false;
        int parent_id_1=body_parent1.particle_index;
        if(!body_parent1.Has_Infinite_Inertia()){
            collision_callbacks.Euler_Step_Position_With_New_Velocity(parent_id_1,dt,time);
            skip_collision_check.Set_Last_Moved(parent_id_1);}}
    if(!fractured_bodies(1)){
        body_parent2.is_temporarily_static=false;
        int parent_id_2=body_parent2.particle_index;
        if(!body_parent2.Has_Infinite_Inertia()){
            collision_callbacks.Euler_Step_Position_With_New_Velocity(parent_id_2,dt,time);
            skip_collision_check.Set_Last_Moved(parent_id_2);}}
    return fractured_bodies(0)||fractured_bodies(1);
}
//#####################################################################
// Function Update_Analytic_Multibody_Collision
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Update_Analytic_Multibody_Collision(const int id_1,const int id_2,MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>& multibody,IMPLICIT_OBJECT<TV>& levelset,const typename TV::SCALAR dt,const typename TV::SCALAR time,const bool mpi_one_ghost)
{
    bool return_val=false;
    IMPLICIT_OBJECT<TV>* levelset_base=&levelset;
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* levelset_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(levelset_base)) 
        levelset_base=levelset_transformed->object_space_implicit_object;
    for(int i=0;i<multibody.levelsets->m;i++){
        VECTOR<std::string,2> key(typeid(*dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >&>(*(*multibody.levelsets)(i)).object_space_implicit_object).name(),typeid(*levelset_base).name());
        if(bool (**collision_function)(RIGID_BODY_COLLISIONS&,const int,const int,IMPLICIT_OBJECT<TV>*,IMPLICIT_OBJECT<TV>*,const T,const T,const bool)=analytic_collision_registry.Get_Pointer(key.Sorted()))
            return_val|=(*collision_function)(*this,id_1,id_2,(*multibody.levelsets)(i),&levelset,dt,time,mpi_one_ghost);
        else PHYSBAM_FATAL_ERROR();}
    return return_val;
}
//#####################################################################
// Function Update_Analytic_Multibody_Collision
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Update_Analytic_Multibody_Collision(RIGID_BODY<TV>& body0,RIGID_BODY<TV>& body1,const typename TV::SCALAR dt,const typename TV::SCALAR time,const bool mpi_one_ghost)
{
    bool return_val=false;
    MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>* multibody0=dynamic_cast<MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>*>(body0.implicit_object->object_space_implicit_object);
    MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>* multibody1=dynamic_cast<MULTIBODY_LEVELSET_IMPLICIT_OBJECT<TV>*>(body1.implicit_object->object_space_implicit_object);
    if(multibody0 && multibody1) for(int i=0;i<multibody1->levelsets->m;i++) return_val|=Update_Analytic_Multibody_Collision(body0.particle_index,body1.particle_index,*multibody0,*(*multibody1->levelsets)(i),dt,time,mpi_one_ghost);
    else if(multibody0) return_val=Update_Analytic_Multibody_Collision(body0.particle_index,body1.particle_index,*multibody0,*body1.implicit_object->object_space_implicit_object,dt,time,mpi_one_ghost);
    else return_val=Update_Analytic_Multibody_Collision(body0.particle_index,body1.particle_index,*multibody1,*body0.implicit_object->object_space_implicit_object,dt,time,mpi_one_ghost);
    return return_val;
}
//#####################################################################
// Function Update_Box_Box_Collision
//#####################################################################
template<class TV> bool
Update_Box_Box_Collision(RIGID_BODY_COLLISIONS<TV>& rigid_body_collisions,const int i1,const int i2,IMPLICIT_OBJECT<TV>* object1,
    IMPLICIT_OBJECT<TV>* object2,const typename TV::SCALAR dt,const typename TV::SCALAR time,const bool mpi_one_ghost)
{
    RIGID_BODY<TV>& body0=rigid_body_collisions.rigid_body_collection.Rigid_Body(i1);
    RIGID_BODY<TV>& body1=rigid_body_collisions.rigid_body_collection.Rigid_Body(i2);
    FRAME<TV> transform1,transform2;
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object1)){
        transform1=*object_transformed->transform;object1=object_transformed->object_space_implicit_object;}
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object2)){
        transform2=*object_transformed->transform;object2=object_transformed->object_space_implicit_object;}
    RANGE<TV>& box1=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<RANGE<TV> >&>(*object1).analytic;
    RANGE<TV>& box2=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<RANGE<TV> >&>(*object2).analytic;
    ORIENTED_BOX<TV> box1_transformed(box1,body0.Frame());
    ORIENTED_BOX<TV> box2_transformed(box2,body1.Frame());

    TV collision_normal=body0.Frame().t-body1.Frame().t;collision_normal.Normalize();
    if(!box1_transformed.Intersection(box2_transformed)){rigid_body_collisions.skip_collision_check.Set_Last_Checked(i1,i2);return false;}
    rigid_body_collisions.pairs_processed_by_collisions.Set(VECTOR<int,2>(i1,i2).Sorted());
    if(collision_normal.Dot(body0.Twist().linear-body1.Twist().linear)>=0) return false;
    TV collision_location=(body0.Frame().t-body1.Frame().t)*.5+body0.Frame().t;
    rigid_body_collisions.Update_Collision_Pair_Helper(body0,body1,dt,time,collision_location,collision_normal,
        body0.Pointwise_Object_Velocity(collision_location)-body1.Pointwise_Object_Velocity(collision_location),mpi_one_ghost);
    return true;
}
//#####################################################################
// Function Update_Sphere_Sphere_Collision
//#####################################################################
template<class TV> bool
Update_Sphere_Sphere_Collision(RIGID_BODY_COLLISIONS<TV>& rigid_body_collisions,const int i1,const int i2,IMPLICIT_OBJECT<TV>* object1,
    IMPLICIT_OBJECT<TV>* object2,const typename TV::SCALAR dt,const typename TV::SCALAR time,const bool mpi_one_ghost)
{
    typedef typename TV::SCALAR T;
    RIGID_BODY<TV>& body0=rigid_body_collisions.rigid_body_collection.Rigid_Body(i1);
    RIGID_BODY<TV>& body1=rigid_body_collisions.rigid_body_collection.Rigid_Body(i2);
    FRAME<TV> transform1,transform2;
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object1)){
        transform1=*object_transformed->transform;object1=object_transformed->object_space_implicit_object;}
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object2)){
        transform2=*object_transformed->transform;object2=object_transformed->object_space_implicit_object;}
    SPHERE<TV>& sphere1=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<SPHERE<TV> >&>(*object1).analytic;
    SPHERE<TV>& sphere2=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<SPHERE<TV> >&>(*object2).analytic;

    TV sphere1_center=(body0.Frame()*transform1).t,sphere2_center=(body1.Frame()*transform2).t;
    TV collision_normal=body0.Frame().t-body1.Frame().t;collision_normal.Normalize();
    T d=(sphere1_center-sphere2_center).Magnitude(),r1=sphere1.radius,r2=sphere2.radius;
    if(d>r1+r2){rigid_body_collisions.skip_collision_check.Set_Last_Checked(i1,i2);return false;}
    rigid_body_collisions.pairs_processed_by_collisions.Set(VECTOR<int,2>(i1,i2).Sorted());
    if(collision_normal.Dot(body0.Twist().linear-body1.Twist().linear)>=0) return false;
    TV collision_location=sphere1_center+(T).5*(d+min(d,r2)-min(d,r1))*collision_normal;
    rigid_body_collisions.Update_Collision_Pair_Helper(body0,body1,dt,time,collision_location,collision_normal,
        body0.Pointwise_Object_Velocity(collision_location)-body1.Pointwise_Object_Velocity(collision_location),mpi_one_ghost);
    return true;
}
//#####################################################################
// Function Update_Box_Plane_Collision
//#####################################################################
template<class TV> bool
Update_Box_Plane_Collision(RIGID_BODY_COLLISIONS<TV>& rigid_body_collisions,const int i1,const int i2,IMPLICIT_OBJECT<TV>* object1,
    IMPLICIT_OBJECT<TV>* object2,const typename TV::SCALAR dt,const typename TV::SCALAR time,const bool mpi_one_ghost)
{
    typedef typename TV::SCALAR T;
    FRAME<TV> transform;
    RIGID_BODY<TV>* body0=&rigid_body_collisions.rigid_body_collection.Rigid_Body(i1);
    RIGID_BODY<TV>* body1=&rigid_body_collisions.rigid_body_collection.Rigid_Body(i2);
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object2)){
        transform=*object_transformed->transform;object2=object_transformed->object_space_implicit_object;}
    ANALYTIC_IMPLICIT_OBJECT<RANGE<TV> >* implicit_box=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<RANGE<TV> >*>(object2);
    if(!implicit_box){
        if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object1)){
            transform=*object_transformed->transform;object1=object_transformed->object_space_implicit_object;}
        exchange(object1,object2);
        exchange(body0,body1);
        implicit_box=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<RANGE<TV> >*>(object2);}
    RANGE<TV>& box=implicit_box->analytic;

    ARRAY<TV> points;bool intersect=false;
    for(int i=0;i<8;i++){
        TV point=box.min_corner;
        if(i>4) point(0)=box.max_corner(0);
        if(i%4==0||i%4==3) point(1)=box.max_corner(1);
        if(i%2==0) point(2)=box.max_corner(2);
        TV transformed_point=body0->Frame().Inverse()*body1->Frame()*point;
        if(transformed_point(0)<0){
            intersect=true;
            transformed_point(0)*=.5;
            points.Append(transformed_point);}}

    TV collision_normal=-body0->Frame().r.Rotated_Axis(0);
    if(!intersect){rigid_body_collisions.skip_collision_check.Set_Last_Checked(i1,i2);return false;}
    rigid_body_collisions.pairs_processed_by_collisions.Set(VECTOR<int,2>(i1,i2).Sorted());
    if(collision_normal.Dot(body0->Twist().linear-body1->Twist().linear)>=0) return false;

    TV collision_location;for(int i=0;i<points.m;i++) collision_location+=points(i);collision_location/=(T)points.m;collision_location=body0->Frame()*collision_location;
    rigid_body_collisions.Update_Collision_Pair_Helper(*body0,*body1,dt,time,collision_location,collision_normal,
        body0->Pointwise_Object_Velocity(collision_location)-body1->Pointwise_Object_Velocity(collision_location),mpi_one_ghost);
    return true;
}
//#####################################################################
// Function Update_Sphere_Plane_Collision
//#####################################################################
template<class TV> bool
Update_Sphere_Plane_Collision(RIGID_BODY_COLLISIONS<TV>& rigid_body_collisions,const int i1,const int i2,IMPLICIT_OBJECT<TV>* object1,
    IMPLICIT_OBJECT<TV>* object2,const typename TV::SCALAR dt,const typename TV::SCALAR time,const bool mpi_one_ghost)
{
    typedef typename TV::SCALAR T;
    FRAME<TV> transform;
    RIGID_BODY<TV>* body0=&rigid_body_collisions.rigid_body_collection.Rigid_Body(i1);
    RIGID_BODY<TV>* body1=&rigid_body_collisions.rigid_body_collection.Rigid_Body(i2);
    if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object2)){
        transform=*object_transformed->transform;object2=object_transformed->object_space_implicit_object;}
    ANALYTIC_IMPLICIT_OBJECT<SPHERE<TV> >* implicit_sphere=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<SPHERE<TV> >*>(object2);
    if(!implicit_sphere){
        if(IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >* object_transformed=dynamic_cast<IMPLICIT_OBJECT_TRANSFORMED<TV,FRAME<TV> >*>(object1)){
            transform=*object_transformed->transform;object1=object_transformed->object_space_implicit_object;}
        exchange(object1,object2);
        exchange(body0,body1);
        implicit_sphere=dynamic_cast<ANALYTIC_IMPLICIT_OBJECT<SPHERE<TV> >*>(object2);}
    SPHERE<TV>& sphere=implicit_sphere->analytic;

    TV sphere_center=(body1->Frame()*transform).t;
    TV collision_normal=-body0->Frame().r.Rotated_Axis(1);
    T separation=collision_normal.Dot(body0->Frame().t-sphere_center);
    if(separation>=sphere.radius){rigid_body_collisions.skip_collision_check.Set_Last_Checked(i1,i2);return false;}
    rigid_body_collisions.pairs_processed_by_collisions.Set(VECTOR<int,2>(i1,i2).Sorted());
    if(collision_normal.Dot(body0->Twist().linear-body1->Twist().linear)>=0) return false;

    T collision_depth=(T).5*(-sphere.radius+min(-separation,sphere.radius));
    TV collision_location=sphere_center-collision_depth*collision_normal;
    rigid_body_collisions.Update_Collision_Pair_Helper(*body0,*body1,dt,time,collision_location,collision_normal,
        body0->Pointwise_Object_Velocity(collision_location)-body1->Pointwise_Object_Velocity(collision_location),mpi_one_ghost);
    return true;
}
//#####################################################################
// Function Update_Levelset_Collision_Pair
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Update_Levelset_Collision_Pair(const int id_1,const int id_2,const T dt,const T time,const bool mpi_one_ghost)
{
    // get all points in levelset, and iterate until each is (at some point) no longer interpenetrating or moving out of the levelset
    ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> > particle_intersections;int i;
    particle_intersections.Preallocate(100);
    for(i=0;i<collision_pair_iterations;i++){
        T smallest_value;int smallest_index;TV collision_location,collision_normal,collision_relative_velocity;bool ignored_separating;
        bool found_intersection=Get_Deepest_Intersection_Point(id_1,id_2,particle_intersections,smallest_value,smallest_index,collision_location,collision_normal,
            collision_relative_velocity,true,0,ignored_separating);
        if(found_intersection || ignored_separating)
            pairs_processed_by_collisions.Set(VECTOR<int,2>(id_1,id_2).Sorted());
        if(!found_intersection){
            RIGID_BODY<TV>& body_1=rigid_body_collection.Rigid_Body(id_1);
            RIGID_BODY_STATE<TV> saved_state;body_1.Save_State(saved_state);
            collision_callbacks.Restore_Position(id_1);Euler_Step_Position(id_1,dt,time);
            bool ignored_separating_local=false;
            found_intersection=Get_Deepest_Intersection_Point(id_1,id_2,particle_intersections,smallest_value,smallest_index,collision_location,collision_normal,
                collision_relative_velocity,true,0,ignored_separating_local);
            ignored_separating|=ignored_separating_local;
            body_1.Restore_State(saved_state);
            body_1.Update_Bounding_Box();
            if(found_intersection || ignored_separating) pairs_processed_by_collisions.Set(VECTOR<int,2>(id_1,id_2).Sorted());
            if(!found_intersection){skip_collision_check.Set_Last_Checked(id_1,id_2);break;}
            else{
                RIGID_BODY_PARTICLE_INTERSECTION<TV>& intersection=particle_intersections(smallest_index);
                collision_location=rigid_body_collection.Rigid_Body(intersection.particle_body).World_Space_Point(intersection.particle_location);
                if(use_parent_normal){
                    RIGID_BODY<TV>& levelset_body_parent=rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(intersection.levelset_body));
                    collision_normal=levelset_body_parent.Implicit_Geometry_Normal(collision_location);}
                    else collision_normal=rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(intersection.levelset_body)).Implicit_Geometry_Normal(collision_location);}}
        const RIGID_BODY_PARTICLE_INTERSECTION<TV>& intersection=particle_intersections(smallest_index);
        RIGID_BODY<TV> &body0=rigid_body_collection.Rigid_Body(intersection.particle_body),&body1=rigid_body_collection.Rigid_Body(intersection.levelset_body);
        if(Update_Collision_Pair_Helper(body0,body1,dt,time,collision_location,collision_normal,collision_relative_velocity,mpi_one_ghost)) return false;}
    return i>0; // if i==0, we didn't process any collisions
}
//#####################################################################
// Function Get_Contact_Pairs
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Get_Contact_Pairs(const T dt,const T time,ARRAY<VECTOR<int,2> >& pairs)
{
    LOG::SCOPE("get contact pairs");

    typedef typename RIGID_BODY_CLUSTER_BINDINGS<TV>::CLUSTER T_CLUSTER;
    // advance all bodies using old velocities (and save new states)
    for(int i=0;i<rigid_body_collection.dynamic_rigid_body_particles.m;i++){
        int id=rigid_body_collection.dynamic_rigid_body_particles(i);
        if(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Contains(id))
            Euler_Step_Position(id,dt,time);}
    
    // reinitialize in candidate position with old velocities 
    if(parameters.use_projected_gauss_seidel)
        spatial_partition->Set_Collision_Body_Thickness(parameters.contact_proximity);
    else
        spatial_partition->Set_Collision_Body_Thickness(0);
    
    ARRAY<int> colliding_rigid_body_particles;
    if(rigid_body_cluster_bindings.collide_constituent_bodies) 
        for(auto&p:rigid_body_cluster_bindings.reverse_bindings){
            const T_CLUSTER& bindings=*p.second;
            for(RIGID_CLUSTER_CONSTITUENT_ID j(0);j<bindings.children.Size();j++)
                if(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Contains(bindings.children(j)))
                    colliding_rigid_body_particles.Append(bindings.children(j));}
    for(int n=0;n<rigid_body_collection.dynamic_rigid_body_particles.m;n++)
        if(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Contains(rigid_body_collection.dynamic_rigid_body_particles(n)))
            colliding_rigid_body_particles.Append(rigid_body_collection.dynamic_rigid_body_particles(n));
    for(int n=0;n<colliding_rigid_body_particles.m;n++){int p=colliding_rigid_body_particles(n);
        if(!rigid_body_collection.collision_body_list->Has_Collision_Body(p)) continue;
        COLLISION_GEOMETRY_ID id=rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Get(p);
        RIGID_BODY_STATE<TV> saved_state;rigid_body_collection.Rigid_Body(p).Save_State(saved_state);

        collision_callbacks.Reevolve_Body_With_Saved_State(p,dt,time); // Get X_n+1 
        skip_collision_check.Set_Last_Moved(p);
        spatial_partition->Get_Potential_Collisions_Using_Current_Position(id,object_indices,false);
        for(int k=0;k<object_indices.m;k++){
            int j=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(object_indices(k));
            if(j<0) continue;
            if(Either_Body_Collides_With_The_Other(p,j) &&
                intersections.Bounding_Boxes_Intersect(p,j,parameters.collision_bounding_box_thickness+(parameters.use_projected_gauss_seidel?parameters.contact_proximity:0))){ // this should *not* be ids, but should remain indices
                int particle_body,levelset_body;
                if((parameters.use_projected_gauss_seidel || intersections.Find_Any_Intersection(p,j,particle_body,levelset_body)))
                    pairs.Append_Unique(VECTOR<int,2>(j,p));}}
        rigid_body_collection.Rigid_Body(p).Restore_State(saved_state);
        rigid_body_collection.Rigid_Body(p).Update_Bounding_Box();} // restore body to its saved position
}
//#####################################################################
// Function Compute_Contact_Graph
//#####################################################################
// Computed by going through each body, advancing it using its new velocity while advancing the rest using the old velocities,
// and seeing which bodies it intersects. Assumes velocities haven't been updated to new velocities yet!
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Compute_Contact_Graph(const T dt,const T time,ARTICULATED_RIGID_BODY<TV>* articulated_rigid_body)
{
    LOG::SCOPE scope("rigid body compute contact graph");
    contact_graph.Reset(); // Call in case number of rigid bodies has changed

    ARRAY<VECTOR<int,2> > edge_pairs;
    if(parameters.perform_collisions)
        edge_pairs=contact_pairs_from_collisions;
    else
        Get_Contact_Pairs(dt,time,edge_pairs);

    if(prune_stacks_from_contact){
        ARRAY<int> body_stack(rigid_body_collection.rigid_body_particles.Size());
        HASHTABLE<pair<int,int> > stack_static_bodies;
        for(int i=0;i<contact_stack.m;i++){
            INDIRECT_ARRAY<ARRAY<int>,ARRAY<int>&> contact_subset=body_stack.Subset(contact_stack(i));
            contact_subset.Fill(i);
            for(int j=0;j<contact_stack(i).m;j++) if(rigid_body_collection.Rigid_Body(contact_stack(i)(j)).Has_Infinite_Inertia()) stack_static_bodies.Set(pair<int,int>(i,contact_stack(i)(j)));}
        for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
            int rigid_body_id=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(i);
            if(rigid_body_id>=0) if(rigid_body_collection.Is_Active(rigid_body_id) && rigid_body_collection.Rigid_Body(rigid_body_id).Has_Infinite_Inertia()) body_stack(rigid_body_id)=-2;}

        for(int i=0;i<edge_pairs.m;i++){
            VECTOR<int,2> state(body_stack.Subset(edge_pairs(i)));
            if(state(0)==state(1)) continue;
            if(state(0)==-2 && stack_static_bodies.Contains(pair<int,int>(state(1),edge_pairs(i)(0)))) continue;
            if(state(1)==-2 && stack_static_bodies.Contains(pair<int,int>(state(0),edge_pairs(i)(1)))) continue;
            for(int j=0;j<2;j++){int& stack_value=body_stack(edge_pairs(i)(j));if(stack_value!=-2) stack_value=-1;}}
        for(int i=contact_stack.m-1;i>=0;i--) if(body_stack.Subset(contact_stack(i)).Find(-1)){
            for(int j=0;j<contact_stack(i).m;j++) if(body_stack(contact_stack(i)(j))!=-2) body_stack(contact_stack(i)(j))=-1;
            contact_stack.Remove_Index_Lazy(i);}
        for(int i=edge_pairs.m-1;i>=0;i--){
            VECTOR<int,2> state(body_stack.Subset(edge_pairs(i)));
            if(!state.Find(-1) && !state.Find(0)) edge_pairs.Remove_Index_Lazy(i);}}
    if(prune_contact_using_velocity)
        for(int i=0;i<edge_pairs.m;i++)
            if(!pairs_scale.Contains(edge_pairs(i).Sorted()))
                pairs_scale.Set(edge_pairs(i).Sorted(),tuple<int,int,int>(parameters.contact_iterations,contact_level_iterations,1));
    for(int i=0;i<edge_pairs.m;i++) if(rigid_body_collection.Is_Active(edge_pairs(i)(0)) && rigid_body_collection.Is_Active(edge_pairs(i)(1))) contact_graph.Add_Edge(edge_pairs(i)(0),edge_pairs(i)(1));

    collision_callbacks.Restore_Positions(); // Get X_n

    contact_graph.directed_graph.Generate_Levels();

    precomputed_contact_pairs_for_level.Resize(contact_graph.Number_Of_Levels());saved_contact_pairs_for_level.Resize(contact_graph.Number_Of_Levels());
    for(int level=0;level<contact_graph.Number_Of_Levels();level++){
        ARRAY<VECTOR<int,2> >& pairs=precomputed_contact_pairs_for_level(level);
        pairs.Remove_All();
        saved_contact_pairs_for_level(level).Remove_All();
        for(int i=0;i<contact_graph.directed_graph.Nodes_In_Level(level).m;i++){
            int body_id=contact_graph.directed_graph.Nodes_In_Level(level)(i);
            if(!rigid_body_collection.Is_Active(body_id)){
                PHYSBAM_ASSERT(contact_graph.directed_graph.Nodes_In_Level(level).m==1);
                contact_graph.directed_graph.Nodes_In_Level(level).Remove_All();
                continue;}
            for(int j=0;j<contact_graph.directed_graph.Parents(body_id).m;j++){
                int other_body_id=contact_graph.directed_graph.Parents(body_id)(j);
                int other_body_level=contact_graph.directed_graph.Level_Of_Node(other_body_id);assert(other_body_level<=level);
                // only add the pair once if they are both parents of each other
                if(other_body_level!=level || !contact_graph.directed_graph.Parents(other_body_id).Contains(body_id) || body_id<other_body_id)
                    if(!parameters.perform_collisions || pairs_processed_by_collisions.Contains(VECTOR<int,2>(body_id,other_body_id).Sorted()))
                        pairs.Append(VECTOR<int,2>(body_id,other_body_id));}}}

    if(articulated_rigid_body) articulated_rigid_body->Generate_Process_List_Using_Contact_Graph(contact_graph);
}
//#####################################################################
// Function Process_Push_Out_Legacy
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Process_Push_Out_Legacy()
{
    LOG::SCOPE scope("rigid body push out legacy");
    skip_collision_check.Reset();
    ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> > particle_intersections;particle_intersections.Preallocate(100);
    bool need_another_iteration=true;int iteration=0;
    while(need_another_iteration && iteration++<push_out_iterations){
        need_another_iteration=false;
        if(use_freezing_with_push_out)
            for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
                RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
                if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);
                    if(rigid_body) if(rigid_body_collection.Is_Active(rigid_body->particle_index)) rigid_body_collection.Rigid_Body(rigid_body->particle_index).is_temporarily_static=false;}}
        for(int level=0;level<contact_graph.Number_Of_Levels();level++){
            ARRAY<VECTOR<int,2> > pairs=precomputed_contact_pairs_for_level(level);
            int iterations=0;bool need_more_iterations=true;T move_fraction=1;
            while(need_more_iterations && iterations++<push_out_level_iterations){need_more_iterations=false;
                if(use_gradual_push_out) move_fraction=(T)iterations/push_out_level_iterations;
                for(int i=0;i<pairs.m;i++){int id_1=pairs(i)(0),id_2=pairs(i)(1);
                    if(parameters.use_projected_gauss_seidel && !pairs_processed_by_contact.Contains(pairs(i).Sorted()))
                        continue;
                    int parent_id_1=rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(id_1)).particle_index;
                    int parent_id_2=rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(id_2)).particle_index;
                    if(skip_collision_check.Skip_Pair(id_1,id_2)) continue;
                    bool need_more_iterations2=true;int iterations2=0;
                    while(need_more_iterations2 && ++iterations2<=push_out_pair_iterations){need_more_iterations2=false;
                        particle_intersections.Remove_All();
                        intersections.Append_All_Intersections(id_1,id_2,particle_intersections,desired_separation_distance);
                        if(!particle_intersections.m){skip_collision_check.Set_Last_Checked(id_1,id_2);continue;}
                        T smallest_value=FLT_MAX;int smallest_index=-1;
                        TV collision_location,collision_normal;T collision_push_distance=0;
                        RIGID_BODY<TV> *collision_body0=0,*collision_body1=0;
                        for(int i=0;i<particle_intersections.m;i++){
                            const RIGID_BODY_PARTICLE_INTERSECTION<TV>& intersection=particle_intersections(i);
                            T phi=(*rigid_body_collection.Rigid_Body(intersection.levelset_body).implicit_object)(rigid_body_collection.Rigid_Body(intersection.particle_body).World_Space_Point(intersection.particle_location));
                            if(phi<smallest_value){
                                RIGID_BODY<TV> &body0=rigid_body_collection.Rigid_Body(intersection.particle_body),&body1=rigid_body_collection.Rigid_Body(intersection.levelset_body);
                                RIGID_BODY<TV> &body_parent1=rigid_body_cluster_bindings.Get_Parent(body0),&body_parent2=rigid_body_cluster_bindings.Get_Parent(body1);
                                TV location=body0.World_Space_Point(intersection.particle_location);
                                TV normal=use_parent_normal?body_parent2.Implicit_Geometry_Normal(location):body1.Implicit_Geometry_Normal(location);
                                T push_distance=move_fraction*max((T)0,desired_separation_distance-phi);
                                collision_location=location;collision_normal=normal;collision_push_distance=push_distance;
                                collision_body0=&body_parent1;collision_body1=&body_parent2;
                                smallest_value=phi;smallest_index=i;}}
                        RIGID_BODY<TV>::Apply_Push(*collision_body0,*collision_body1,collision_location,collision_normal,collision_push_distance);
                        rigid_body_collection.Rigid_Body(particle_intersections(smallest_index).particle_body).Update_Bounding_Box();
                        rigid_body_collection.Rigid_Body(particle_intersections(smallest_index).levelset_body).Update_Bounding_Box();
                        skip_collision_check.Set_Last_Moved(id_1);skip_collision_check.Set_Last_Moved(id_2);
                        if(parent_id_1!=id_1)
                            rigid_body_cluster_bindings.Clamp_Particles_To_Embedded_Positions(rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(id_1)).particle_index);
                        if(parent_id_2!=id_2)
                            rigid_body_cluster_bindings.Clamp_Particles_To_Embedded_Positions(rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(id_2)).particle_index);
                        need_more_iterations2=true;need_another_iteration=true;need_more_iterations=true;}}}
            if(use_freezing_with_push_out) for(int i=0;i<contact_graph.directed_graph.Nodes_In_Level(level).m;i++)
                rigid_body_collection.Rigid_Body(contact_graph.directed_graph.Nodes_In_Level(level)(i)).is_temporarily_static=true;}}
    if(use_freezing_with_push_out) for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
        RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
        if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);
            if(rigid_body) if(rigid_body_collection.Is_Active(rigid_body->particle_index)) rigid_body_collection.Rigid_Body(rigid_body->particle_index).is_temporarily_static=false;}}
}
//#####################################################################
// Function Process_Push_Out
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Process_Push_Out(const bool perform_collision_body_collisions,const T residual_push_out_depth)
{
    LOG::SCOPE scope("rigid body push out new");
    const bool kinematic_rigid_bodies_only=rigid_body_collection.simulated_rigid_body_particles.m==0;

    skip_collision_check.Reset();
    ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> > particle_intersections;
    particle_intersections.Preallocate(100);

    bool need_another_iteration=true;int iteration=0;
    while(need_another_iteration && iteration++<push_out_iterations){need_another_iteration=false;
        if(!kinematic_rigid_bodies_only){
            if(use_freezing_with_push_out) Clear_Temporarily_Static();
            for(int level=0;level<contact_graph.Number_Of_Levels();level++){
                const ARRAY<int>& rigid_bodies_in_level=contact_graph.directed_graph.Nodes_In_Level(level);
                int level_iteration=0;bool need_more_level_iterations=true;T move_fraction=1;
                while(need_more_level_iterations && level_iteration++<push_out_level_iterations){need_more_level_iterations=false;
                    if(use_gradual_push_out) move_fraction=(T)level_iteration/push_out_level_iterations;
                    for(int i=0;i<rigid_bodies_in_level.m;i++){
                        bool need_more_pair_iterations=true;int pair_iteration=0;
                        while(need_more_pair_iterations && pair_iteration++<push_out_pair_iterations){
                            need_more_pair_iterations=false;
                            if(Push_Out_From_Rigid_Body(rigid_body_collection.Rigid_Body(rigid_bodies_in_level(i)),particle_intersections,move_fraction,residual_push_out_depth)){
                                need_more_pair_iterations=need_more_level_iterations=need_another_iteration=true;
                                rigid_body_cluster_bindings.Clamp_Particles_To_Embedded_Positions();}}}}
                if(use_freezing_with_push_out) Set_Level_Temporarily_Static(level);}
            if(use_freezing_with_push_out) Clear_Temporarily_Static();}}

    // Process static/kinematic rigid bodies
    if(kinematic_rigid_bodies_only && perform_collision_body_collisions)
        for(int i=0;i<rigid_body_collection.static_and_kinematic_rigid_bodies.m;i++){
            RIGID_BODY<TV>& rigid_body=rigid_body_collection.Rigid_Body(rigid_body_collection.static_and_kinematic_rigid_bodies(i));
            if(Push_Out_From_Rigid_Body(rigid_body,particle_intersections,(T)1,residual_push_out_depth)) rigid_body_cluster_bindings.Clamp_Particles_To_Embedded_Positions();}
}
//#####################################################################
// Function Get_Rigid_Bodies_Intersecting_Rigid_Body
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Get_Rigid_Bodies_Intersecting_Rigid_Body(const int particle_index,ARRAY<int>& rigid_bodies,ARRAY<TV>& collision_locations,ARRAY<TV>& body_distances,
    ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> >& particle_intersections,const T residual_push_out_depth) const
{
    const RIGID_BODY<TV>& rigid_body=rigid_body_collection.Rigid_Body(particle_index);
    const int rigid_body_id=rigid_body.particle_index;
    const int rigid_body_level=contact_graph.directed_graph.Level_Of_Node(rigid_body_id);

    const ARRAY<VECTOR<int,2> >& level_contact_pairs=precomputed_contact_pairs_for_level(rigid_body_level);
    for(int i=0;i<level_contact_pairs.m;i++) if(level_contact_pairs(i)(0)==rigid_body_id || level_contact_pairs(i)(1)==rigid_body_id){
        const int other_body_id=level_contact_pairs(i)(0)==rigid_body_id?level_contact_pairs(i)(1):level_contact_pairs(i)(0);
        const RIGID_BODY<TV>& other_rigid_body=rigid_body_collection.Rigid_Body(other_body_id);
        if(skip_collision_check.Skip_Pair(rigid_body_id,other_body_id)
            || (collision_manager && !collision_manager->Either_Body_Collides_With_The_Other(other_rigid_body.particle_index,rigid_body.particle_index))) continue;
        T smallest_value;int smallest_index;TV collision_location,collision_normal,collision_relative_velocity;bool ignored_separating;
        if(Get_Deepest_Intersection_Point(rigid_body_id,other_body_id,particle_intersections,
                smallest_value,smallest_index,collision_location,collision_normal,collision_relative_velocity,false,
                desired_separation_distance-residual_push_out_depth,ignored_separating)){
            // reverse the direction of separation distance to be relative to rigid_body_id
            if(particle_intersections(smallest_index).levelset_body!=rigid_body_id) smallest_value=-smallest_value;
            rigid_bodies.Append(other_rigid_body.particle_index);collision_locations.Append(collision_location);
            smallest_value-=residual_push_out_depth;
            body_distances.Append(smallest_value*collision_normal);}
        else skip_collision_check.Set_Last_Checked(rigid_body_id,other_body_id);} // set last checked even if ignored_separating==true
}
//}################################################################
// Function Push_Out_From_Rigid_Body
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Push_Out_From_Rigid_Body(RIGID_BODY<TV>& rigid_body,ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> >& particle_intersections,const T move_fraction,const T residual_push_out_depth)
{
    static const int rb_dofs=(int)TV::m+(int)T_SPIN::m;
    RIGID_BODY<TV>& parent_rigid_body=rigid_body_collection.rigid_body_cluster_bindings.Get_Parent(rigid_body);
    T threshold=rigid_body.Length_Scale_Squared()*(T)1e-2;

    ARRAY<int> particle_interactions,rigid_body_interactions;ARRAY<TV> particle_distances,rigid_body_collision_locations,rigid_body_distances;
    if(rigid_body_collection.simulated_rigid_body_particles.m)
        Get_Rigid_Bodies_Intersecting_Rigid_Body(rigid_body.particle_index,rigid_body_interactions,rigid_body_collision_locations,rigid_body_distances,particle_intersections,residual_push_out_depth);
    particle_distances*=move_fraction;rigid_body_distances*=move_fraction;

    if(rigid_body_interactions.m==0) return false;

    ARRAY<SYMMETRIC_MATRIX<T,TV::m> > K_inverse(rigid_body_interactions.m);
    for(int i=0;i<rigid_body_interactions.m;i++){
        RIGID_BODY<TV>& parent_other_rigid_body=rigid_body_collection.rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(rigid_body_interactions(i)));
        if(!parent_other_rigid_body.Has_Infinite_Inertia() || use_static_body_masses) K_inverse(i)=parent_other_rigid_body.Impulse_Factor(rigid_body_collision_locations(i)).Inverse();
        else K_inverse(i)=SYMMETRIC_MATRIX<T,TV::m>::Identity_Matrix();}

    TV velocity;T_SPIN angular_velocity;
    if(!parent_rigid_body.Has_Infinite_Inertia()){
        // TODO: static deformable particles
        TV ms[2];VECTOR<T,T_SPIN::m> mrs[2];MATRIX<T,T_SPIN::m,TV::m> mr[2];MATRIX<T,T_SPIN::m> mrr[2];
        T m=0; // non static particles
        
        SYMMETRIC_MATRIX<T,TV::m> K_inverse_sum[2]; // non static bodies

        int number_of_static_bodies=0;TV centroid;
        for(int i=0;i<rigid_body_interactions.m;i++){
            if(parameters.use_projected_gauss_seidel && !pairs_processed_by_contact.Contains(VECTOR<int,2>(rigid_body.particle_index,rigid_body_interactions(i)).Sorted()))
                continue;
            RIGID_BODY<TV>& parent_other_rigid_body=rigid_body_collection.rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(rigid_body_interactions(i)));
            int index=parent_other_rigid_body.Has_Infinite_Inertia()?1:0;
            if(index>=0){centroid+=rigid_body_collision_locations(i)-parent_rigid_body.Frame().t;number_of_static_bodies++;}
            TV K_inverse_s=K_inverse(i)*rigid_body_distances(i),radius=rigid_body_collision_locations(i)-parent_rigid_body.Frame().t;
            K_inverse_sum[index]+=K_inverse(i);ms[index]+=K_inverse_s;mrs[index]+=radius.Cross(K_inverse_s);
            MATRIX<T,T_SPIN::m,TV::m> r_K_inverse=Cross_Product_Matrix(radius)*K_inverse(i);
            mr[index]+=r_K_inverse;mrr[index]+=r_K_inverse.Times_Transpose(Cross_Product_Matrix(radius));}

        if(number_of_static_bodies) centroid/=(T)number_of_static_bodies;

        // finish setting up the terms for the dynamic bodies, including diagonal terms and dynamic particle masses
        K_inverse_sum[0]+=parent_rigid_body.Mass()+m;
        mrr[0]+=parent_rigid_body.World_Space_Inertia_Tensor();

        // determine how static bodies affect the system
        DIAGONAL_MATRIX<T,TV::m> eigenvalues;MATRIX<T,TV::m> eigenvectors;
        int equation_type=0; // indicate which type of system we will be solving below
        if(number_of_static_bodies==0) equation_type=0; // 0 dof specified by static bodies
        else if(number_of_static_bodies==1) equation_type=1;
        else{ // need to figure out if the static bodies are effectively coincident or colinear
            SYMMETRIC_MATRIX<T,TV::m> R_R_transpose;
            for(int i=0;i<rigid_body_interactions.m;i++){
                RIGID_BODY<TV>& parent_other_rigid_body=rigid_body_collection.rigid_body_cluster_bindings.Get_Parent(rigid_body_collection.Rigid_Body(rigid_body_interactions(i)));
                if(parent_other_rigid_body.Has_Infinite_Inertia()) R_R_transpose+=Outer_Product(rigid_body_collision_locations(i)-parent_rigid_body.Frame().t-centroid);}
            auto [eigenvalues_,eigenvectors_]=R_R_transpose.Solve_Eigenproblem();
            eigenvalues=eigenvalues_;
            eigenvectors=eigenvectors_;
            int rank=eigenvalues.Vector().Componentwise_Greater_Equal(TV::All_Ones_Vector()*threshold).Number_True();
            if(rank==0) equation_type=1; // 3/6 dof (2/3 dof in 2d) specified by static bodies
            else if(rank==1 && TV::m==3) equation_type=2; // 5 dof specified by static bodies (not possible in 2d)
            else equation_type=3;} // full constrained by static bodies

        MATRIX<T,rb_dofs> M[2];
        for(int i=0;i<2;i++) if(equation_type!=(i?0:3)){
            M[i].Set_Submatrix(0,0,K_inverse_sum[i]);
                M[i].Set_Submatrix(TV::m,0,mr[i]);
            M[i].Set_Submatrix(0,TV::m,mr[i].Transposed());
            M[i].Set_Submatrix(TV::m,TV::m,mrr[i]);}

        // set up our system
        MATRIX<T,rb_dofs> A;
        VECTOR<T,rb_dofs> b;
        if(equation_type==0){A=M[0];b=VECTOR<T,rb_dofs>(ms[0],mrs[0]);}
        else if(equation_type==3){A=M[1];b=VECTOR<T,rb_dofs>(ms[1],mrs[1]);}
        else if(equation_type==1){ // one outer body static
            MATRIX<T,T_SPIN::m,rb_dofs> Z;
            MATRIX<T,TV::m,rb_dofs> Y;
            Z.Set_Submatrix(0,0,-Cross_Product_Matrix(centroid));Z.Set_Submatrix(0,TV::m,MATRIX<T,T_SPIN::m>::Identity_Matrix());
            Y.Set_Submatrix(0,0,MATRIX<T,TV::m>::Identity_Matrix());Y.Set_Submatrix(0,T_SPIN::m,-Cross_Product_Matrix(centroid));
            A.Set_Submatrix(0,0,Z*M[0]);
            A.Set_Submatrix(T_SPIN::m,0,Y*M[1]);
            b.Combine(Z*VECTOR<T,rb_dofs>(ms[0],mrs[0]),Y*VECTOR<T,rb_dofs>(ms[1],mrs[1]));}
        else if constexpr(TV::m==3){
            assert(equation_type==2);
            int u_index=eigenvalues.Vector().Arg_Max();
            MATRIX<T,TV::m,rb_dofs> Z_helper;
            Z_helper.Set_Submatrix(0,0,-Cross_Product_Matrix(centroid));Z_helper.Set_Submatrix(0,TV::m,MATRIX<T,T_SPIN::m>::Identity_Matrix());
            Z_helper=eigenvectors.Transpose_Times(Z_helper);
            MATRIX<T,TV::m,rb_dofs> Y;
            Y.Set_Submatrix(0,0,MATRIX<T,TV::m>::Identity_Matrix());Y.Set_Submatrix(0,T_SPIN::m,Cross_Product_Matrix(centroid));
            VECTOR<T,rb_dofs> b_helper[2]={VECTOR<T,rb_dofs>(ms[0],mrs[0]),VECTOR<T,rb_dofs>(ms[1],mrs[1])};
            A.Set_Submatrix(TV::m*(TV::m==3),0,Y*M[1]); // Don't compile index out of bounds in 2D, even though this case cannot happen in 2D.
            b.Array_View((int)TV::m,(int)TV::m)=Y*b_helper[1];
            MATRIX<T,1,rb_dofs> A_row;
            for(int i=0;i<TV::m;i++){
                Z_helper.Get_Submatrix(i,0,A_row);
                int matrix_index=i==u_index?0:1;
                A.Set_Submatrix(i,0,A_row*M[matrix_index]);
                b(i)=(A_row*b_helper[matrix_index]).x;}}

        // compute impulse as delta twist
        VECTOR<T,rb_dofs> delta_twist;
        if(equation_type==0 || equation_type==3) delta_twist=A.In_Place_Cholesky_Solve(b);
        else delta_twist=A.Inverse_Times(b);
        delta_twist.Extract(velocity,angular_velocity);}

    // apply push to other rigid bodies
    for(int i=0;i<rigid_body_interactions.m;i++) if(!rigid_body_collection.Rigid_Body(rigid_body_interactions(i)).Has_Infinite_Inertia()){
        RIGID_BODY<TV>& other_rigid_body=rigid_body_collection.Rigid_Body(rigid_body_interactions(i));
        RIGID_BODY<TV>& parent_other_rigid_body=rigid_body_collection.rigid_body_cluster_bindings.Get_Parent(other_rigid_body);
        TV impulse=K_inverse(i)*(-rigid_body_distances(i)+velocity+angular_velocity.Cross(rigid_body_collision_locations(i)-parent_rigid_body.Frame().t));
        T_SPIN other_angular_velocity=parent_other_rigid_body.World_Space_Inertia_Tensor_Inverse()*(rigid_body_collision_locations(i)-parent_other_rigid_body.Frame().t).Cross(impulse);
        parent_other_rigid_body.Frame().t+=impulse/parent_other_rigid_body.Mass();
        if(parameters.use_push_out_rotation){
            parent_other_rigid_body.Frame().r=ROTATION<TV>::From_Rotation_Vector(other_angular_velocity)*parent_other_rigid_body.Frame().r;
            parent_other_rigid_body.Frame().r.Normalize();}
        parent_other_rigid_body.Update_Angular_Velocity();
        parent_other_rigid_body.Update_Bounding_Box();
        skip_collision_check.Set_Last_Moved(other_rigid_body.particle_index);}

    // apply push to the rigid body
    if(!parent_rigid_body.Has_Infinite_Inertia()){
        parent_rigid_body.Frame().t+=velocity;
        if(parameters.use_push_out_rotation){
            parent_rigid_body.Frame().r=ROTATION<TV>::From_Rotation_Vector(angular_velocity)*parent_rigid_body.Frame().r;
            parent_rigid_body.Frame().r.Normalize();}
        parent_rigid_body.Update_Angular_Velocity();
        parent_rigid_body.Update_Bounding_Box();
        skip_collision_check.Set_Last_Moved(rigid_body.particle_index);}

    return true;
}
    
template<> bool RIGID_BODY_COLLISIONS<VECTOR<float,1> >::
Push_Out_From_Rigid_Body(RIGID_BODY<VECTOR<float,1> >& rigid_body,ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<VECTOR<float,1> > >& particle_intersections,const float move_fraction,
    const float residual_push_out_depth)
{
    PHYSBAM_NOT_IMPLEMENTED();
};
template<> bool RIGID_BODY_COLLISIONS<VECTOR<double,1> >::
Push_Out_From_Rigid_Body(RIGID_BODY<VECTOR<double,1> >& rigid_body,ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<VECTOR<double,1> > >& particle_intersections,const double move_fraction,
    const double residual_push_out_depth)
{
    PHYSBAM_NOT_IMPLEMENTED();
}
//#####################################################################
// Function Get_Rigid_Body_Depth
//#####################################################################
template<class TV> int  RIGID_BODY_COLLISIONS<TV>::
Get_Rigid_Body_Depth(int i,ARRAY<int,int>& depths)
{
    if(depths(i)!=-1) return depths(i);
    else{
        depths(i)=0;
        for(int child=0;child<contact_graph.directed_graph.Children(i).m;child++)
            depths(i)+=Get_Rigid_Body_Depth(contact_graph.directed_graph.Children(i)(child),depths);
        depths(i)++;
        return depths(i);}
}
//#####################################################################
// Function Compute_Contact_Frequency
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Compute_Contact_Frequency()
{
    T linear_threshold_squared=(T)1e-3,angular_threshold_squared=(T)1e-3;
    int stack_depth=2;

    ARRAY<ARRAY<int>,int> adj(contact_graph.directed_graph.Number_Nodes());
    for(int i=0;i<adj.m;i++) for(int j=0;j<contact_graph.directed_graph.Parents(i).m;j++){
        adj(i).Append(contact_graph.directed_graph.Parents(i)(j));adj(contact_graph.directed_graph.Parents(i)(j)).Append(i);}

    ARRAY<int,int> depths(adj.m);depths.Fill(-1);
    for(int i=0;i<adj.m;i++) if(depths(i)==-1) Get_Rigid_Body_Depth(i,depths);

    pairs_scale.Remove_All();
    for(int i=0;i<adj.m;i++){
        for(int j=0;j<adj(i).m;j++){
            VECTOR<int,2> sorted_pair=VECTOR<int,2>(i,adj(i)(j)).Sorted();
            if(!pairs_scale.Contains(sorted_pair)){
                TWIST<TV> relative_twist=rigid_body_collection.rigid_body_particles.twist(i)-rigid_body_collection.rigid_body_particles.twist(adj(i)(j));
                if(relative_twist.linear.Magnitude_Squared() <= linear_threshold_squared && relative_twist.angular.Magnitude_Squared() <= angular_threshold_squared){
                    T scale=(T).5*((relative_twist.linear.Magnitude_Squared()/linear_threshold_squared)+
                        (relative_twist.angular.Magnitude_Squared()/angular_threshold_squared));
                    if(depths(i) > stack_depth || depths(adj(i)(j)) > stack_depth) scale=0;
                    pairs_scale.Set(sorted_pair,tuple<int,int,int>((int)(scale*(parameters.contact_iterations-1)+1),(int)(scale*(contact_level_iterations-1)+1),
                            (int)((1-scale)*(contact_pair_iterations-1)+1)));}
                else pairs_scale.Set(sorted_pair,tuple<int,int,int>(parameters.contact_iterations,contact_level_iterations,1));}}}
}
//#####################################################################
// Function Construct_Stacks
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Construct_Stacks()
{
    T linear_threshold_squared=(T)1e-4,angular_threshold_squared=(T)1e-4;

    ARRAY<ARRAY<int>,int> adj(contact_graph.directed_graph.Number_Nodes());
    for(int i=0;i<adj.m;i++) for(int j=0;j<contact_graph.directed_graph.Parents(i).m;j++){
        adj(i).Append(contact_graph.directed_graph.Parents(i)(j));adj(contact_graph.directed_graph.Parents(i)(j)).Append(i);}

    ARRAY<int,int> visited(rigid_body_collection.rigid_body_particles.Size());

    ARRAY<int> stack;
    for(int i=0;i<visited.m;i++) if(!visited(i)){
        ARRAY<int> list;
        visited(i)=i;
        stack.Append(i);
        bool has_infinite_inertia_body=false;
        while(stack.Size()){
            int j=stack.Pop_Value();
            list.Append(j);
            if(rigid_body_collection.Rigid_Body(j).Has_Infinite_Inertia()){has_infinite_inertia_body=true;continue;}
            for(int k=0;k<adj(j).m;k++) if(visited(adj(j)(k))!=i){
                visited(adj(j)(k))=i;
                stack.Append(adj(j)(k));}}
        if(!has_infinite_inertia_body || list.m<=1) continue;
        for(int j=1;j<list.m;j++) if(rigid_body_collection.Rigid_Body(list(j)).Has_Infinite_Inertia()){exchange(list(j),list(0));break;}
        const TWIST<TV>& base_twist=rigid_body_collection.rigid_body_particles.twist(list(0));
        bool same_velocity=true;
        for(int j=1;j<list.m;j++){
            TWIST<TV> relative_twist=rigid_body_collection.rigid_body_particles.twist(list(j))-base_twist;
            if(relative_twist.linear.Magnitude_Squared() > linear_threshold_squared || relative_twist.angular.Magnitude_Squared() > angular_threshold_squared){
                same_velocity=false;break;}}
        if(!same_velocity) continue;
        contact_stack.Append(ARRAY<int>());
        for(int j=0;j<list.m;j++) contact_stack.Last().Append(list(j));}
}
//#####################################################################
// Function Check_For_Any_Interpenetration
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Check_For_Any_Interpenetration()
{
    spatial_partition->Set_Collision_Body_Thickness(0);
    for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
        RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
        if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);
            if(rigid_body) if(rigid_body_collection.Is_Active(rigid_body->particle_index) && rigid_body_collection.Rigid_Body(rigid_body->particle_index).Is_Simulated()){
            int particle_body=0,levelset_body=0;
            spatial_partition->Get_Potential_Collisions(i,object_indices,false);
            for(int t=0;t<object_indices.m;t++) if(object_indices(t)>=COLLISION_GEOMETRY_ID(0)){
                int id=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(object_indices(t));
                if(id>=0 && Either_Body_Collides_With_The_Other(rigid_body->particle_index,id) &&
                    intersections.Intersection_Check(rigid_body->particle_index,id,particle_body,levelset_body)){
                    LOG::cout<<"!!!! Interpenetration detected: id:"<<particle_body<<" \""<<rigid_body_collection.Rigid_Body(particle_body).name<<"\" point inside id: "<<levelset_body<<" \""
                             <<rigid_body_collection.Rigid_Body(levelset_body).name<<"\" levelset"<<std::endl;
                    return true;}}}}}
    return false;
}
//#####################################################################
// Function Print_Interpenetration_Statistics
//#####################################################################
template<class TV> ARRAY<VECTOR<int,2> > RIGID_BODY_COLLISIONS<TV>::
Find_All_Bounding_Box_Pairs(const T thickness)
{
    spatial_partition->Reinitialize();
    ARRAY<VECTOR<int,2> > pairs;
    for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
        RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
        if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);if(rigid_body){
            spatial_partition->Get_Potential_Collisions(i,object_indices);
            for(int t=0;t<object_indices.m;t++){int j=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(object_indices(t));
                if(j>=0 && Either_Body_Collides_With_The_Other(rigid_body->particle_index,j) && intersections.Bounding_Boxes_Intersect(rigid_body->particle_index,j,thickness))
                    pairs.Append(VECTOR<int,2>(j,rigid_body->particle_index));}}}}
    return pairs;
}
//#####################################################################
// Function Print_Interpenetration_Statistics
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Print_Interpenetration_Statistics()
{
    spatial_partition->Set_Collision_Body_Thickness(0);

    int num_interpenetration_points=0;int worst_id_1(0),worst_id_2(0);
    T smallest_phi=FLT_MAX,avg_phi=0;
    ARRAY<RIGID_BODY_PARTICLE_INTERSECTION<TV> > particle_intersections;
    particle_intersections.Preallocate(1000);

    for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
        RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
        if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);
            if(rigid_body) if(rigid_body_collection.Is_Active(rigid_body->particle_index) && rigid_body_collection.Rigid_Body(rigid_body->particle_index).Is_Simulated()){
            spatial_partition->Get_Potential_Collisions(i,object_indices);
            for(int t=0;t<object_indices.m;t++){int j=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(object_indices(t));
                if(j>=0 && Either_Body_Collides_With_The_Other(rigid_body->particle_index,j) && intersections.Bounding_Boxes_Intersect(rigid_body->particle_index,j))
                    intersections.Append_All_Intersections(rigid_body->particle_index,j,particle_intersections);}}}}
    num_interpenetration_points=particle_intersections.m;
    for(int i=0;i<particle_intersections.m;i++){
        const RIGID_BODY_PARTICLE_INTERSECTION<TV>& intersection=particle_intersections(i);
        TV location=rigid_body_collection.Rigid_Body(intersection.particle_body).World_Space_Point(intersection.particle_location);
        T phi=(*rigid_body_collection.Rigid_Body(intersection.levelset_body).implicit_object)(location);
        if(phi<smallest_phi){worst_id_1=intersection.particle_body;worst_id_2=intersection.levelset_body;smallest_phi=phi;}
        avg_phi+=phi;}
    if(num_interpenetration_points>0){
        avg_phi/=num_interpenetration_points;
        LOG::cout<<"Interpenetration statistics: num interpenetrating points = "<<num_interpenetration_points<<", smallest phi = "
           <<smallest_phi<<"(\""<<rigid_body_collection.Rigid_Body(worst_id_1).name<<"\" and \""<<rigid_body_collection.Rigid_Body(worst_id_2).name<<"\")"
           <<", avg phi = "<<avg_phi<<std::endl;}
    else LOG::cout<<"No interpenetration"<<std::endl;
}
//#####################################################################
// Function Get_Bounding_Box_Collision_Pairs_Of_Body
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Get_Bounding_Box_Collision_Pairs_Of_Body(ARRAY<VECTOR<int,2> >& pairs,int id,const T thickness)
{
    spatial_partition->Get_Potential_Collisions(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Get(id),object_indices,false);
    for(int t=0;t<object_indices.m;t++){int j=rigid_body_collection.collision_body_list->collision_geometry_id_to_geometry_id.Get(object_indices(t));
        if(j<0 || !Either_Body_Collides_With_The_Other(id,j)) continue;
        if(!rigid_body_collection.Rigid_Body(j).Has_Infinite_Inertia()){
            if(j<id) continue;} // to avoid processing the same pair of dynamic bodies twice
        if(!skip_collision_check.Skip_Pair(id,j)){
            if(intersections.Bounding_Boxes_Intersect(id,j,thickness)) pairs.Append(VECTOR<int,2>(id,j));
            else skip_collision_check.Set_Last_Checked(id,j);}}
}
//#####################################################################
// Function Get_Bounding_Box_Collision_Pairs
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Get_Bounding_Box_Collision_Pairs(const T dt,const T time,ARRAY<VECTOR<int,2> >& pairs,const bool add_contact_pairs,const bool reinitialize_spatial_partition,const T thickness)
{
    typedef typename RIGID_BODY_CLUSTER_BINDINGS<TV>::CLUSTER T_CLUSTER;
    pairs.Remove_All();

    if(reinitialize_spatial_partition)
        spatial_partition->Set_Collision_Body_Thickness(0);

    ARRAY<int> colliding_rigid_body_particles;
    if(rigid_body_cluster_bindings.collide_constituent_bodies) 
        for(auto&p:rigid_body_cluster_bindings.reverse_bindings){
            const T_CLUSTER& bindings=*p.second;
            for(RIGID_CLUSTER_CONSTITUENT_ID j(0);j<bindings.children.Size();j++)
                if(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Contains(bindings.children(j)))
                    colliding_rigid_body_particles.Append(bindings.children(j));}
    for(int n=0;n<rigid_body_collection.dynamic_rigid_body_particles.m;n++){
        int p=rigid_body_collection.dynamic_rigid_body_particles(n);
        if(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Contains(p))
            colliding_rigid_body_particles.Append(p);}
    
    for(int n=0;n<colliding_rigid_body_particles.m;n++){int p=colliding_rigid_body_particles(n);
        Get_Bounding_Box_Collision_Pairs_Of_Body(pairs,p,thickness);}

    if(add_contact_pairs){
        collision_callbacks.Restore_Positions();
        contact_pairs_from_collisions.Remove_All();
        Get_Contact_Pairs(dt,time,contact_pairs_from_collisions);
        pairs.Append(contact_pairs_from_collisions);
        for(int n=0;n<rigid_body_collection.dynamic_rigid_body_particles.m;n++)
            collision_callbacks.Euler_Step_Position_With_New_Velocity(rigid_body_collection.dynamic_rigid_body_particles(n),dt,time);}
}
//#####################################################################
// Function Register_Analytic_Collisions
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Register_Analytic_Collisions()
{
    const char* sphere=typeid(ANALYTIC_IMPLICIT_OBJECT<SPHERE<TV> >).name();
    const char* box=typeid(ANALYTIC_IMPLICIT_OBJECT<RANGE<TV> >).name();
    const char* plane=typeid(ANALYTIC_IMPLICIT_OBJECT<BOUNDED_HORIZONTAL_PLANE<TV> >).name();
    analytic_collision_registry.Set(VECTOR<std::string,2>(sphere,sphere),Update_Sphere_Sphere_Collision);
    analytic_collision_registry.Set(VECTOR<std::string,2>(sphere,plane).Sorted(),Update_Sphere_Plane_Collision);
    analytic_collision_registry.Set(VECTOR<std::string,2>(box,box),Update_Box_Box_Collision);
    analytic_collision_registry.Set(VECTOR<std::string,2>(box,plane).Sorted(),Update_Box_Plane_Collision);
    SOLVE_CONTACT::Register_Analytic_Contacts<TV>(analytic_contact_registry);
}
//#####################################################################
// Function Apply_Stacking_Contact
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Apply_Stacking_Contact()
{
    for(int i=0;i<contact_stack.m;i++){
        TWIST<TV>& static_twist=rigid_body_collection.rigid_body_particles.twist(contact_stack(i)(0));
        for(int j=1;j<contact_stack(i).m;j++){
            RIGID_BODY<TV>& rigid_body=rigid_body_collection.Rigid_Body(contact_stack(i)(j));
            collision_callbacks.Restore_Position(rigid_body.particle_index);
            rigid_body.Twist()=static_twist;rigid_body.Update_Angular_Momentum();}}
}
//#####################################################################
// Function Add_Elastic_Collisions
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Add_Elastic_Collisions(const T dt,const T time)
{
    skip_collision_check.Reset();pairs_processed_by_collisions.Remove_All();
    LOG::SCOPE scope("rigid body collisions");
    skip_collision_check.Reset();pairs_processed_by_collisions.Remove_All();
    rigid_body_particles_intersections.Remove_All();
    ARRAY<VECTOR<int,2> > pairs;pairs.Preallocate(10);bool need_another_iteration=true;
    for(int i=0;i<parameters.collision_iterations && need_another_iteration;i++){
        need_another_iteration=false;Get_Bounding_Box_Collision_Pairs(dt,time,pairs,i==parameters.collision_iterations-1,i==0);
        for(int j=0;j<pairs.m;j++){
            int id_1=pairs(j)(0),id_2=pairs(j)(1);
            if(Update_Collision_Pair(id_1,id_2,dt,time,false)){
                spatial_partition->Update_Body(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Get(id_1));
                spatial_partition->Update_Body(rigid_body_collection.collision_body_list->geometry_id_to_collision_geometry_id.Get(id_2));
                need_another_iteration=true;}}

        // force it to the last iteration (so it picks up contact pairs one time at least)
        if(!need_another_iteration && i<parameters.collision_iterations-1){
            i=parameters.collision_iterations-2;
            need_another_iteration=true;}}
}
//#####################################################################
// Function Process_Contact_Using_Graph
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Process_Contact_Using_Graph(const T dt,const T time,ARTICULATED_RIGID_BODY<TV>* articulated_rigid_body,const bool correct_contact_energy,const bool use_saved_pairs)
{
    LOG::SCOPE scope("rigid body contact");

    SOLVE_CONTACT::Solve(*this,collision_callbacks,rigid_body_collection,parameters,correct_contact_energy,use_saved_pairs,dt,time,mpi_rigid_velocity_save,mpi_rigid_angular_momentum_save);
}
//#####################################################################
// Function Shock_Propagation_Using_Graph
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Shock_Propagation_Using_Graph(const T dt,const T time,ARTICULATED_RIGID_BODY<TV>* articulated_rigid_body,const bool use_saved_pairs)
{
    LOG::SCOPE scope("shock propagation");
    skip_collision_check.Reset();bool need_another_iteration=true;int iteration=0;T epsilon_scale=1;
    ARRAY<ARRAY<VECTOR<int,2> > >& contact_pairs_for_level=use_saved_pairs?saved_contact_pairs_for_level:precomputed_contact_pairs_for_level;
    while(need_another_iteration && iteration++<shock_propagation_iterations){
        need_another_iteration=false;
        Clear_Temporarily_Static();
        for(int level=0;level<contact_graph.Number_Of_Levels();level++){
            ARRAY<VECTOR<int,2> >& pairs=contact_pairs_for_level(level);
            bool need_another_level_iteration=true;int level_iteration=0;
            while(need_another_level_iteration && level_iteration++<shock_propagation_level_iterations){need_another_level_iteration=false;
                if(parameters.use_epsilon_scaling_for_level) epsilon_scale=(T)level_iteration/shock_propagation_level_iterations;
                for(int i=0;i<pairs.m;i++){int id_1=pairs(i)(0),id_2=pairs(i)(1);
                    if(skip_collision_check.Skip_Pair(id_1,id_2)) continue;
                    if(!parameters.use_projected_gauss_seidel || pairs_processed_by_contact.Contains(pairs(i).Sorted()))
                        if(SOLVE_CONTACT::Update_Contact_Pair(*this,collision_callbacks,analytic_contact_registry,id_1,id_2,false,shock_propagation_pair_iterations,epsilon_scale,dt,time,false)){
                            if(!use_saved_pairs) saved_contact_pairs_for_level(level).Append_Unique(pairs(i));
                            need_another_level_iteration=true;
                            need_another_iteration=true;}}
                if(articulated_rigid_body && articulated_rigid_body->use_shock_propagation)
                    for(int i=0;i<articulated_rigid_body->shock_propagation_level_iterations;i++) for(int j=0;j<articulated_rigid_body->process_list(level).m;j++){
                        JOINT_ID joint_id=articulated_rigid_body->process_list(level)(j);
                        RIGID_BODY<TV> &parent=*articulated_rigid_body->Parent(joint_id),&child=*articulated_rigid_body->Child(joint_id);
                        bool old_temporarily_static_parent=parent.is_temporarily_static,old_temporarily_static_child=child.is_temporarily_static;
                        parent.is_temporarily_static=false;child.is_temporarily_static=false;
                        Apply_Prestabilization_To_Joint(dt,time,*articulated_rigid_body,joint_id,epsilon_scale);
                        parent.is_temporarily_static=old_temporarily_static_parent;child.is_temporarily_static=old_temporarily_static_child;
                        need_another_level_iteration=need_another_iteration=true;}}
            // note that the bodies involved in joints get frozen too, since both bodies in a joint must be at least this level or lower to get processed
            Set_Level_Temporarily_Static(level);}}

    Clear_Temporarily_Static();

    // enforce articulation constraints once more
    if(articulated_rigid_body && !articulated_rigid_body->use_shock_propagation && articulated_rigid_body->do_final_pass){
        articulated_rigid_body->Store_Velocities_And_Momenta();
        for(int i=0;i<articulated_rigid_body->shock_propagation_level_iterations;i++)
            for(int level=0;level<contact_graph.Number_Of_Levels();level++) for(int j=0;j<articulated_rigid_body->process_list(level).m;j++)
               Apply_Prestabilization_To_Joint(dt,time,*articulated_rigid_body,articulated_rigid_body->process_list(level)(j),epsilon_scale);
        articulated_rigid_body->Restore_Velocities_And_Momenta();}
    if(prune_stacks_from_contact) Apply_Stacking_Contact();
}
//#####################################################################
// Function Apply_Prestabilization_To_Joint
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Apply_Prestabilization_To_Joint(const T dt,const T time,ARTICULATED_RIGID_BODY<TV>& articulated_rigid_body,const JOINT_ID joint_id,const T epsilon_scale)
{
    int parent_id=articulated_rigid_body.Parent_Id(joint_id),child_id=articulated_rigid_body.Child_Id(joint_id);
    RIGID_BODY<TV> &parent=rigid_body_collection.Rigid_Body(parent_id),&child=rigid_body_collection.Rigid_Body(child_id);
    // revert to the saved positions & save the proposed positions in rigid_frame_save - restore rigid_frame_save below
    collision_callbacks.Exchange_Frame(parent_id);
    collision_callbacks.Exchange_Frame(child_id);
    parent.Update_Angular_Velocity();child.Update_Angular_Velocity(); // needed for relative velocity
    articulated_rigid_body.Apply_Prestabilization_To_Joint(joint_id,dt,articulated_rigid_body.use_epsilon_scale?epsilon_scale:(T)1);
    collision_callbacks.Save_Position(parent_id);collision_callbacks.Save_Position(child_id); // fix saved values & re-evolve bodies
    Euler_Step_Position(parent_id,dt,time);Euler_Step_Position(child_id,dt,time);
}
//#####################################################################
// Function Either_Body_Collides_With_The_Other
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Either_Body_Collides_With_The_Other(const int rigid_body_id_1,const int rigid_body_id_2) const
{
    return rigid_body_cluster_bindings.Valid_Cluster_Collision(rigid_body_id_1,rigid_body_id_2)
       && (!collision_manager || collision_manager->Either_Body_Collides_With_The_Other(rigid_body_id_1,rigid_body_id_2));
}
//#####################################################################
// Function Body_Collides_With_The_Other
//#####################################################################
template<class TV> bool RIGID_BODY_COLLISIONS<TV>::
Body_Collides_With_The_Other(const int rigid_body_id_1,const int rigid_body_id_2) const
{
    return rigid_body_cluster_bindings.Valid_Cluster_Collision(rigid_body_id_1,rigid_body_id_2)
       && (!collision_manager || collision_manager->Body_Collides_With_The_Other(rigid_body_id_1,rigid_body_id_2));
}
//#####################################################################
// Function Euler_Step_Position
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Euler_Step_Position(const int id,const T dt,const T time)
{
    collision_callbacks.Euler_Step_Position(id,dt,time);
    rigid_body_collection.Rigid_Body(id).Update_Bounding_Box();skip_collision_check.Set_Last_Moved(id);
}
//#####################################################################
// Function Set_Level_Temporarily_Static
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Set_Level_Temporarily_Static(const int level)
{
    const ARRAY<int>& nodes=contact_graph.directed_graph.Nodes_In_Level(level);
    for(int i=0;i<nodes.m;i++) if(rigid_body_collection.Is_Active(nodes(i))) rigid_body_collection.Rigid_Body(nodes(i)).is_temporarily_static=true;
}
//#####################################################################
// Function Clear_Temporarily_Static
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Clear_Temporarily_Static()
{
    for(COLLISION_GEOMETRY_ID i(0);i<rigid_body_collection.collision_body_list->bodies.Size();i++){
        RIGID_COLLISION_GEOMETRY<TV>* rigid_collision_geometry=dynamic_cast<RIGID_COLLISION_GEOMETRY<TV>*>(rigid_body_collection.collision_body_list->bodies(i));
        if(rigid_collision_geometry){RIGID_BODY<TV>* rigid_body=dynamic_cast<RIGID_BODY<TV>*>(&rigid_collision_geometry->rigid_body);
            if(rigid_body) if(rigid_body_collection.Is_Active(rigid_body->particle_index)) rigid_body_collection.Rigid_Body(rigid_body->particle_index).is_temporarily_static=false;}}
}
//#####################################################################
// Function Clean_Up_Fractured_Items_From_Lists
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Clean_Up_Fractured_Items_From_Lists(ARRAY<VECTOR<int,2> >& pairs,const int current_pair,const bool called_from_contact)
{
    if(added_bodies(0).m+added_bodies(1).m>0){
        // Go through the remaining pairs, and remove any pairs that contain either of the removed fractured bodies
        for(int k=pairs.m;k>=current_pair+1;k--){
            if(pairs(k)(0)==fractured_bodies(0) || pairs(k)(1)==fractured_bodies(0) || 
                pairs(k)(0)==fractured_bodies(1) || pairs(k)(1)==fractured_bodies(1))
                pairs.Remove_Index_Lazy(k);}
        // Go through the pairs processed by collisions, and add the cartesian product
        ARRAY<VECTOR<int,2> > keys;pairs_processed_by_collisions.Get_Keys(keys);
        for(int k=0;k<keys.m;k++){
            ARRAY<int> first_ids,second_ids;
            if(keys(k)(0)==fractured_bodies(0)) first_ids=added_bodies(0);
            else if(keys(k)(0)==fractured_bodies(1)) first_ids=added_bodies(1);
            if(keys(k)(1)==fractured_bodies(0)) second_ids=added_bodies(0);
            else if(keys(k)(1)==fractured_bodies(1)) second_ids=added_bodies(1);
            if(!first_ids.m && !second_ids.m) continue;
            pairs_processed_by_collisions.Delete(keys(k));
            if(!first_ids.m && second_ids.m) 
                for(int body=0;body<second_ids.m;body++) pairs_processed_by_collisions.Set(VECTOR<int,2>(keys(k)(0),second_ids(body)).Sorted());
            else if(first_ids.m && !second_ids.m)
                for(int body=0;body<first_ids.m;body++) pairs_processed_by_collisions.Set(VECTOR<int,2>(keys(k)(1),first_ids(body)).Sorted());
            else
                for(int body0=0;body0<first_ids.m;body0++)
                    for(int body1=0;body1<second_ids.m;body1++)
                        pairs_processed_by_collisions.Set(VECTOR<int,2>(first_ids(body0),second_ids(body1)).Sorted());}
        if(!called_from_contact){ // TODO: add new contact pairs to the list as well
            // Go through added bodies, and collide them against all other bodies and add them to the pairs list
            spatial_partition->Set_Collision_Body_Thickness(0);
            for(int k=0;k<added_bodies.m;k++) for(int body=0;body<added_bodies(k).m;body++)
                Get_Bounding_Box_Collision_Pairs_Of_Body(pairs,added_bodies(k)(body),
                    parameters.collision_bounding_box_thickness);}}
}
//#####################################################################
// Function Initialize_All_Contact_Projections
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Initialize_All_Contact_Projections(const bool enforce_rigid_rigid_contact_in_cg)
{
    // rigid/rigid
    if(rigid_body_collection.dynamic_rigid_body_particles.m && enforce_rigid_rigid_contact_in_cg){
        for(auto& intersection:rigid_body_particles_intersections){
            const RIGID_BODY<TV> &particle_body=rigid_body_collection.Rigid_Body(get<0>(intersection)),
                &levelset_body=rigid_body_collection.Rigid_Body(get<1>(intersection));
            if(rigid_body_collection.Is_Active(particle_body.particle_index) && rigid_body_collection.Is_Active(levelset_body.particle_index)) Create_Contact_Joint(particle_body,levelset_body,get<2>(intersection));}}
}
//#####################################################################
// Function Remove_Contact_Joints
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Remove_Contact_Joints()
{
    for(int i=0;i<contact_joints.m;i++) rigid_body_collection.articulated_rigid_body.joint_mesh.Remove_Articulation(contact_joints(i));
    contact_joints.Remove_All();
}
//#####################################################################
// Function Create_Contact_Joint
//#####################################################################
template<class TV> void RIGID_BODY_COLLISIONS<TV>::
Create_Contact_Joint(const RIGID_BODY<TV>& parent,const RIGID_BODY<TV>& child,const TV& location)
{
    NORMAL_JOINT<TV>* joint=new NORMAL_JOINT<TV>;
    rigid_body_collection.articulated_rigid_body.joint_mesh.Add_Articulation(parent.particle_index,child.particle_index,joint);
    T phi;const TV normal=child.Implicit_Geometry_Extended_Normal(location,phi);
    FRAME<TV> J(location,ROTATION<TV>::From_Rotated_Vector(TV::Axis_Vector(0),normal));
    joint->Set_Parent_To_Joint_Frame(J.Inverse_Times(parent.Frame())); // TODO: make setting up a joint from a frame part of the joint infrastructure
    joint->Set_Child_To_Joint_Frame(J.Inverse_Times(child.Frame()));

    contact_joints.Append(joint->id_number);
}
//#####################################################################
template class RIGID_BODY_COLLISIONS<VECTOR<float,1> >;
template class RIGID_BODY_COLLISIONS<VECTOR<float,2> >;
template class RIGID_BODY_COLLISIONS<VECTOR<float,3> >;
template class RIGID_BODY_COLLISIONS<VECTOR<double,1> >;
template class RIGID_BODY_COLLISIONS<VECTOR<double,2> >;
template class RIGID_BODY_COLLISIONS<VECTOR<double,3> >;
}

