public static void Interpolate(PhysicsState a, PhysicsState b, float time)
        {
            float amount = (time - a.time) / (b.time - a.time);
            Body body = a.body;

            for (int i = 0; i < body.count; i++)
            {
                body.curr_shape.points[i].X = MathHelper.Lerp(a.curr_shape_positions[i].X, b.curr_shape_positions[i].X, amount);
                body.curr_shape.points[i].Y = MathHelper.Lerp(a.curr_shape_positions[i].Y, b.curr_shape_positions[i].Y, amount);

                PointMass pointmass = body.pointmass_list[i];

                pointmass.position.X = MathHelper.Lerp(a.pointmass_list_positions[i].X, b.pointmass_list_positions[i].X, amount);
                pointmass.position.Y = MathHelper.Lerp(a.pointmass_list_positions[i].Y, b.pointmass_list_positions[i].Y, amount);

                pointmass.velocity.X = MathHelper.Lerp(a.pointmass_list_velocities[i].X, b.pointmass_list_velocities[i].X, amount);
                pointmass.velocity.Y = MathHelper.Lerp(a.pointmass_list_velocities[i].Y, b.pointmass_list_velocities[i].Y, amount);

                pointmass.force.X = MathHelper.Lerp(a.pointmass_list_forces[i].X, b.pointmass_list_forces[i].X, amount);
                pointmass.force.Y = MathHelper.Lerp(a.pointmass_list_forces[i].Y, b.pointmass_list_forces[i].Y, amount);

            }

            body.UpdateBodyPositionVelocityForce(0);
        }
        public PhysicsHistory(Body body)
        {
            this.capacity = 512;
            this.curr_index = 0;
            this.body = body;
            this.states = new PhysicsState[capacity];

            for (int i = 0; i < states.Length; i++)
                states[i] = new PhysicsState(body);
        }