Add progress on SIM_pool

trick_sims/SIM_pool/Modified_data/realtime.py

@@ -0,0 +1,10 @@
+
+trick.real_time_enable()
+trick.exec_set_software_frame(0.1)
+trick.itimer_enable()
+
+trick.exec_set_enable_freeze(True)
+trick.exec_set_freeze_command(True)
+
+simControlPanel = trick.SimControlPanel()
+trick.add_external_application(simControlPanel)

trick_sims/SIM_pool/RUN_test/input.py

@@ -0,0 +1,30 @@
+
+
+exec(open("./Modified_data/realtime.py").read())
+
+dyn.table.numBalls = 2
+dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls)
+
+id1 = dyn.table.addBall(-2, .5, 1, 1, False);
+id2 = dyn.table.addBall(1, .5, 1, 1, False);
+
+dyn.table.setBallVel(id1, 0.5, 0);
+
+dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
+
+#==========================================
+# Start the Graphics Client
+#==========================================
+varServerPort = trick.var_server_get_port();
+PoolTableDisplay_path = "models/graphics/dist/PoolTableDisplay.jar"
+
+if (os.path.isfile(PoolTableDisplay_path)) :
+    PoolTableDisplay_cmd = "java -jar " \
+                   + PoolTableDisplay_path \
+                   + " " + str(varServerPort) + " &" ;
+    print(PoolTableDisplay_cmd)
+    os.system( PoolTableDisplay_cmd);
+else :
+    print('============================================================================================')
+    print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
+    print('============================================================================================')

trick_sims/SIM_pool/S_define

@@ -0,0 +1,24 @@
+/************************************************************
+PURPOSE:
+    ( Simulation of Pool Table. )
+LIBRARY DEPENDENCIES:
+    ((pool_table/src/pool_table.cpp))
+*************************************************************/
+#include "sim_objects/default_trick_sys.sm"
+##include "pool_table/include/pool_table.hh"
+
+class PoolTableSimObject : public Trick::SimObject {
+    public:
+    PoolTable table;
+
+    PoolTableSimObject() {
+        ("default_data")            table.default_data() ;
+        ("initialization")          table.state_init() ;
+        ("derivative")              table.state_deriv() ;
+        ("integration") trick_ret = table.state_integ() ;
+        ("dynamic_event")           table.collision() ;
+    }
+};
+
+PoolTableSimObject dyn;
+IntegLoop dyn_integloop(0.1) dyn;

trick_sims/SIM_pool/S_overrides.mk

@@ -0,0 +1,2 @@
+TRICK_CFLAGS += -Imodels -I/usr/local/include/eigen3/
+TRICK_CXXFLAGS += -Imodels -I/usr/local/include/eigen3/

trick_sims/SIM_pool/models/graphics/src/PoolTableDisplay.java

@@ -0,0 +1,333 @@
+/*
+ * Trick
+ * 2020 (c) National Aeronautics and Space Administration (NASA)
+ *
+ */
+
+import java.awt.Color;
+import java.awt.Graphics2D;
+import java.awt.Graphics;
+import java.awt.RenderingHints;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
+import java.io.BufferedOutputStream;
+import java.io.BufferedReader;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.net.Socket;
+import javax.swing.BoxLayout;
+import javax.swing.JButton;
+import javax.swing.JFrame;
+import javax.swing.JPanel;
+
+
+
+class Ball {
+    static int numColors = 9;
+    static Color[] colorList = {
+        Color.WHITE,
+        Color.YELLOW,
+        Color.BLUE,
+        Color.RED,
+        Color.MAGENTA,
+        Color.GREEN,
+        Color.MAGENTA,
+        Color.BLACK
+    };
+
+    public Color color;
+    public double x;
+    public double y;
+    public double radius;
+    public int identity;
+    public Ball (int id) {
+        identity = id;
+        x = 0.0;
+        y = 0.0;
+        radius = 0.5;
+        color = colorList[id % numColors];
+    }
+}
+
+class ControlPanel extends JPanel implements ActionListener {
+
+    private RangeView rangeView;
+    private JButton zoomOutButton, zoomInButton;
+
+    public ControlPanel(RangeView view) {
+
+        rangeView = view;
+        setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
+
+        zoomOutButton = new JButton("Zoom Out");
+        zoomOutButton.addActionListener(this);
+        zoomOutButton.setActionCommand("zoomout");
+        zoomOutButton.setToolTipText("Zoom Out");
+        add(zoomOutButton);
+
+        zoomInButton = new JButton("Zoom In");
+        zoomInButton.addActionListener(this);
+        zoomInButton.setActionCommand("zoomin");
+        zoomInButton.setToolTipText("Zoom In");
+        add(zoomInButton);
+    }
+
+    public void actionPerformed(ActionEvent e) {
+        String s = e.getActionCommand();
+        switch (s) {
+            case "zoomout":
+                rangeView.setScale( rangeView.getScale() / 2 );
+                break;
+            case "zoomin":
+                rangeView.setScale( rangeView.getScale() * 2 );
+                break;
+            default:
+                System.out.println("Unknown Action Command:" + s);
+                break;
+        }
+    }
+} // class ControlPanel
+
+class RangeView extends JPanel {
+
+    private int scale;
+    private Color backGroundColor;
+
+    // Origin of world coordinates in jpanel coordinates.
+    private int worldOriginX;
+    private int worldOriginY;
+
+    public Ball[] balls;
+
+    /**
+     * Class constructor.
+     */
+    public RangeView( int mapScale, int numberOfBalls) {
+        setScale(mapScale);
+        backGroundColor = new Color(0.2f, 0.6f, 0.2f);
+        balls = new Ball[numberOfBalls];
+        for (int ii=0 ; ii<numberOfBalls ; ii++) {
+            balls[ii] = new Ball(ii);
+        }
+    }
+
+    public void setScale (int mapScale) {
+        if (mapScale < 4) {
+            scale = 4;
+        } else if (mapScale > 128) {
+            scale = 128;
+        } else {
+            scale = mapScale;
+        }
+        repaint();
+    }
+
+    public int getScale() {
+        return scale;
+    }
+
+    public void drawCenteredCircle(Graphics2D g, int x, int y, int d) {
+        x = x-(d/2);
+        y = y-(d/2);
+        g.fillOval(x,y,d,d);
+    }
+
+    private void doDrawing(Graphics g) {
+        Graphics2D g2d = (Graphics2D) g;
+
+        RenderingHints rh = new RenderingHints(
+                RenderingHints.KEY_ANTIALIASING,
+                RenderingHints.VALUE_ANTIALIAS_ON);
+
+        rh.put(RenderingHints.KEY_RENDERING,
+               RenderingHints.VALUE_RENDER_QUALITY);
+
+        int width  = getWidth();
+        int height = getHeight();
+
+        worldOriginX = (width/2);
+        worldOriginY = (height/2);
+
+        // Draw Background
+        g2d.setPaint(backGroundColor);
+        g2d.fillRect(0, 0, width, height);
+
+        //  Draw balls
+        for (int ii = 0; ii < balls.length ; ii++) {
+            g2d.setPaint(balls[ii].color);
+            int bx = (int)(worldOriginX + scale * balls[ii].x);
+            int by = (int)(worldOriginY - scale * balls[ii].y);
+            drawCenteredCircle(g2d, bx, by, (int)(scale * 2 * balls[ii].radius));
+            g2d.setPaint(Color.BLACK);
+            g2d.drawString ( String.format("%d",ii), bx,by);
+        }
+
+        g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
+
+    }
+
+    @Override
+    public void paintComponent(Graphics g) {
+        super.paintComponent(g);
+        doDrawing(g);
+    }
+}
+
+public class PoolTableDisplay extends JFrame {
+
+    private RangeView rangeView;
+    private BufferedReader in;
+    private DataOutputStream out;
+
+    public PoolTableDisplay() {
+
+        rangeView = null;
+        setTitle("Ball Arena");
+        setSize(800, 500);
+        setLocationRelativeTo(null);
+        setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
+        setFocusable(true);
+    }
+
+    public void connectToServer(String host, int port ) throws IOException {
+        Socket socket = new Socket(host, port);
+        in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
+        out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
+    }
+
+    public void createGUI( int mapScale, int numberOfBalls ) {
+
+        rangeView = new RangeView(mapScale, numberOfBalls);
+        JPanel panel1 = new JPanel();
+        panel1.setLayout(new BoxLayout(panel1, BoxLayout.X_AXIS));
+        panel1.add(rangeView);
+
+        ControlPanel controlPanel = new ControlPanel(rangeView);
+        JPanel panel0 = new JPanel();
+        panel0.setLayout(new BoxLayout(panel0, BoxLayout.Y_AXIS));
+        panel0.add(panel1);
+        panel0.add(controlPanel);
+        add(panel0);
+        setVisible(true);
+    }
+
+    private static void  printHelpText() {
+        System.out.println(
+            "----------------------------------------------------------------------\n"
+          + "usage: java jar PoolTableDisplay.jar <port-number>\n"
+          + "----------------------------------------------------------------------\n"
+          );
+    }
+
+    public static void main(String[] args) throws IOException, InterruptedException {
+
+        String host = "localHost";
+        int port = 0;
+        boolean boom = false;
+
+        // Handle program arguments.
+        int ii = 0;
+        while (ii < args.length) {
+            switch (args[ii]) {
+                case "-help" :
+                case "--help" : {
+                    printHelpText();
+                    System.exit(0);
+                } break;
+                default : {
+                    port = (Integer.parseInt(args[ii]));
+                } break;
+            }
+            ++ii;
+        }
+
+        boolean go = true;
+        double dt = 0.100;  // Time between updates (seconds).
+        int mapScale = 32 ; // pixels per meter.
+        int nballs = 7;
+
+        if (port == 0) {
+            System.out.println("No variable server port specified.");
+            printHelpText();
+            System.exit(0);
+        }
+
+        PoolTableDisplay poolTableDisplay = new PoolTableDisplay();
+
+        // Connect to the Trick simulation's variable server.
+        System.out.println("Connecting to: " + host + ":" + port);
+        poolTableDisplay.connectToServer(host, port);
+        poolTableDisplay.out.writeBytes("trick.var_set_client_tag(\"PoolTableDisplay\") \n");
+        poolTableDisplay.out.flush();
+
+        // Get the number of  balls.
+        poolTableDisplay.out.writeBytes(
+            "trick.var_add(\"dyn.table.numBalls\")\n" +
+            "trick.var_send() \n" +
+            "trick.var_clear() \n");
+        poolTableDisplay.out.flush();
+        try {
+            String line;
+            String field[];
+            line = poolTableDisplay.in.readLine();
+            field = line.split("\t");
+            nballs = Integer.parseInt( field[1]);
+        } catch (IOException | NullPointerException e ) {
+            go = false;
+        }
+
+        poolTableDisplay.createGUI(mapScale, nballs);
+
+        // Get the Radii of the balls.
+        for ( ii = 0; ii < nballs; ii ++) {
+            poolTableDisplay.out.writeBytes( String.format("trick.var_add(\"dyn.table.balls[%d][0].radius\")\n", ii));
+        }
+        poolTableDisplay.out.flush();
+        poolTableDisplay.out.writeBytes(
+            "trick.var_send() \n" +
+            "trick.var_clear() \n");
+        poolTableDisplay.out.flush();
+        try {
+            String line;
+            String field[];
+            line = poolTableDisplay.in.readLine();
+            field = line.split("\t");
+            for ( ii=0; ii < nballs; ii++) {
+                // poolTableDisplay.rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]);
+            }
+        } catch (IOException | NullPointerException e ) {
+            go = false;
+        }
+
+        // Get the Positions of the balls, and update the display, periodically.
+        poolTableDisplay.out.writeBytes( "trick.var_pause() \n");
+        for ( ii = 0; ii < nballs; ii ++) {
+            poolTableDisplay.out.writeBytes(
+                  String.format("trick.var_add(\"dyn.table.balls[%d][0].pos[0]\")\n", ii)
+                + String.format("trick.var_add(\"dyn.table.balls[%d][0].pos[1]\")\n", ii)
+            );
+        }
+        poolTableDisplay.out.writeBytes("trick.var_ascii() \n" +
+                                   String.format("trick.var_cycle(%.3f)\n", dt) +
+                                   "trick.var_unpause() \n" );
+        poolTableDisplay.out.flush();
+        while (go) {
+            try {
+                String line;
+                String field[];
+                line = poolTableDisplay.in.readLine();
+                // System.out.println("Sim->Client:" + line);
+                field = line.split("\t");
+                for ( ii=0; ii < nballs; ii++) {
+                    poolTableDisplay.rangeView.balls[ii].x = Double.parseDouble( field[2*ii+1]);
+                    poolTableDisplay.rangeView.balls[ii].y = Double.parseDouble( field[2*ii+2]);
+                }
+            } catch (IOException | NullPointerException e ) {
+                go = false;
+            }
+            //  Update the scene.
+            poolTableDisplay.rangeView.repaint();
+        } // while
+    } // main
+} // class

trick_sims/SIM_pool/models/pool_table/include/ball.hh

@@ -0,0 +1,46 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ( Simulate pool balls. )
+LIBRARY DEPENDENCY:
+    ((ball.o))
+*******************************************************************************/
+#ifndef _ball_hh_
+#define _ball_hh_
+
+// #include <Eigen/Core>
+
+// monotonically increasing ID
+static int id = 0;
+
+class Ball {
+
+    public:
+
+        Ball(double x, double y, double mass, double radius, bool isFixed, int id);
+        Ball () {}
+        
+        // Z component should always be 0, unless someone tries to add jumps in the future
+        double pos[3];
+        double prevPos[3];
+        double vel[3];
+        // Used to store derivatives between deriv and integration steps
+        double accel[3];
+
+        // Relating to angular velocity
+        double relativeVel[3];
+        double w[3];
+        double angular_accel[3];
+
+        double color[3];
+    
+        double mass;
+        double radius;
+        bool fixed;
+        bool isCue;
+        int sliding;
+
+        unsigned int id;
+};
+
+// Ball* CreateBall(double x, double y, double mass, double radius, bool isFixed);
+
+#endif

trick_sims/SIM_pool/models/pool_table/include/bumper.hh

@@ -0,0 +1,46 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ( Pool bumper class. )
+LIBRARY DEPENDENCY:
+    ((bumper.o))
+*******************************************************************************/
+#ifndef _bumper_hh_
+#define _bumper_hh_
+
+#include <vector>
+
+// this should definitely go somewhere else
+// maybe make a geometry libary
+class Point {
+    public:
+        double x;
+        double y;
+
+        Point(double x, double y) : x(x), y(y) {}
+        Point() {}
+};
+
+class Line {
+    public:
+        Point p1;
+        Point p2;
+
+        Line (Point p1, Point p2) : p1(p1), p2(p2) {}
+        Line () {}
+};
+
+class Bumper {
+    public:
+        void AddPointToRender(double x, double y);
+        void AddBorder (double x1, double y1, double x2, double y2);
+
+    private:
+        // Actual line that can be collided with 
+        Line border;
+
+        // Shape that should be rendered
+        // Size should be dynamic
+        std::vector<Point *> renderedShape;
+    
+};
+
+#endif

trick_sims/SIM_pool/models/pool_table/include/pool_table.hh

@@ -0,0 +1,58 @@
+/************************************************************************
+PURPOSE: (Simulate a pool table.)
+LIBRARY DEPENDENCIES:
+    ((pool_table/src/pool_table.o))
+**************************************************************************/
+
+#ifndef _POOL_TABLE_H_
+#define _POOL_TABLE_H_
+#include "trick/regula_falsi.h"
+#include "ball.hh"
+#include "bumper.hh"
+#include <vector>
+
+
+class PoolTable {
+
+    public:
+        PoolTable () : numBalls(0), numAssociations(0) {}
+
+        int addBall (double x, double y, double mass, double radius, bool fixed);
+        int addBumper (double x1, double y1, double x2, double y2);
+
+        int setBallPos(int id, double x, double y);
+        int setBallVel(int id, double v_x, double v_y);
+
+        // Ball ** balls;
+        // Bumper ** bumpers;
+        Ball** balls;
+        Bumper** bumpers;
+
+        // Ball-ball collisions
+        int nextBallSlot = 0;
+        unsigned int numBalls;
+        unsigned int numAssociations;
+        REGULA_FALSI* ballAssociations;
+
+        // Ball-bumper collisions
+        unsigned int numBumpers;
+        unsigned int numCombos;
+        REGULA_FALSI* bumperAssociations;        
+
+        //void ballCollision(Ball &b1, Ball &b2);
+
+        int default_data();
+        int state_init();
+        int state_deriv();
+        int state_integ();
+        double collision();
+
+        // Sim constants that should be user-controllable
+        double frictionRolling = 0.05;
+        double frictionSliding = 0.25; 
+        double frictionScale = 1;
+        double frictionTolerance = 0.0005;
+        double coefficientOfElasticity = 0.99;
+
+};
+#endif

trick_sims/SIM_pool/models/pool_table/src/ball.cpp

@@ -0,0 +1,35 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: ( Simulate balls contacting boundaries. )
+LIBRARY DEPENDENCY:
+    ((ball.o))
+*******************************************************************************/
+#include "../include/ball.hh"
+// #include "trick/memorymanager_c_intf.h"
+
+#include "trick/MemoryManager.hh"
+extern Trick::MemoryManager* trick_MM;
+
+#include <new>
+
+Ball::Ball(double x, double y, double mass, double radius, bool isFixed, int id) :
+    mass(mass),
+    fixed(isFixed),
+    id(id)
+{
+    pos[0] = x;
+    pos[1] = y;
+    pos[2] = 0;
+}
+
+// Ball* CreateBall(double x, double y, double mass, double radius, bool isFixed) {
+//     // Ball* b = (Ball*)TMM_declare_var_s("Ball");
+//     Ball *b = new Ball(x, y, mass, radius, isFixed, 0);
+//     trick_MM->declare_extern_var ( b, "Ball");
+//     // Ball *b = (Ball*)trick_MM->declare_var("Ball");
+
+//     // return (new (b) Ball(x, y, mass, radius, isFixed, 0));
+//     return b;
+// }
+
+
+

trick_sims/SIM_pool/models/pool_table/src/bumper.cpp

@@ -0,0 +1,9 @@
+#include "bumper.hh"
+
+void Bumper::AddPointToRender(double x, double y) {
+
+}
+
+void Bumper::AddBorder (double x1, double y1, double x2, double y2) {
+    
+}

trick_sims/SIM_pool/models/pool_table/src/pool_table.cpp

@@ -0,0 +1,284 @@
+/********************************* TRICK HEADER *******************************
+PURPOSE: (Simulate a pool table.)
+LIBRARY DEPENDENCY:
+    ((pool_table.o)
+     (ball.o))
+*******************************************************************************/
+#include <math.h>
+#include <iostream>
+#include "trick/integrator_c_intf.h"
+#include "trick/memorymanager_c_intf.h"
+#include "../include/pool_table.hh"
+#include "trick/trick_math_proto.h"
+
+double dot(                  /* Return: Scalar dot or inner product */
+                 double vec1[],        /* In: Vector 1 */
+                 double vec2[],        /* In: Vector 2 */
+                 int dim)
+{                  
+    double dot = 0;
+    for (int i = 0; i < dim; i++) {
+        dot += vec1[i] * vec2[i];
+    }                    
+    return dot;
+}
+
+void scaleInPlace (double vec[], double scale, int dim) {
+    for (int i = 0; i < dim; i++) {
+        vec[i] *= scale;
+    }
+}
+
+
+
+
+int PoolTable::default_data() {
+    // balls.clear();
+    // bumpers.clear();
+
+
+    // Dev Testing only - should be deleted
+    // int id1 = addBall(0.5, 0.5, 1, 1, false);
+    // int id2 = addBall(0.75, 0.5, 1, 1, false);
+
+    // setBallVel(id1, 0.1, 0);
+    ///////////////////////////////////////////
+
+    numBalls = 0;
+    numBumpers = 0;
+    return 0;
+}
+
+// Input stage runs before this, which populates balls and bumpers
+int PoolTable::state_init() {
+    
+    // Do regula falsi setup here
+
+    // Vars for ball/ball collisions
+    double now ;
+    numAssociations = (numBalls*(numBalls-1))/2;
+    ballAssociations = (REGULA_FALSI*)TMM_declare_var_1d("REGULA_FALSI", numAssociations);
+    unsigned int ii,jj;
+    for (ii=1; ii<numBalls; ii++) {
+        for (jj=0; jj<ii; jj++) {
+            unsigned int association_index = ii*(ii-1)/2+jj;
+            ballAssociations[association_index].mode = Decreasing;
+            ballAssociations[association_index].error_tol = 0.0000001;
+            now = get_integ_time() ;
+            reset_regula_falsi( now, &ballAssociations[association_index] );
+        }
+    }
+
+    // Need to do the same thing with rail/ball associations
+
+    return 0;
+}
+
+int PoolTable::state_deriv() {
+    // derivative - determine the acceleration here
+    // This is probably just friction
+    // And angular stuff
+    // F = MA => A = M^-1 * F
+
+    // Will have to account for rolling vs sliding here
+    // For now, just rolling
+
+    for (int i = 0; i < numBalls; i++) {
+        // Frictional force a constant applied in the opposing direction of velocity.
+        // Magnitude of velocity is irrelevant
+
+        // std::cout << "ActuaVelocity: " << balls[i]->vel[0] << " " << balls[i]->vel[1] << std::endl;
+        // std::cout << "Velocity Norm: " << velocityNorm[0] << " " << velocityNorm[1] << std::endl;
+
+        // balls[i]->accel[0] = 0;
+        // balls[i]->accel[1] = 0;
+
+        // Has weird behavior when velocity is very small, so only apply friction if velocity is greater than a tolerance
+        if (abs(dv_mag(balls[i]->vel)) > frictionTolerance) {
+            double velocityNorm[3];
+            dv_norm(velocityNorm, balls[i]->vel);
+
+            balls[i]->accel[0] = - (frictionScale * frictionRolling * velocityNorm[0]);
+            balls[i]->accel[1] = - (frictionScale * frictionRolling * velocityNorm[1]);
+            balls[i]->accel[2] = 0;
+        } else {
+            balls[i]->vel[0] = 0;
+            balls[i]->vel[1] = 0;
+            balls[i]->vel[2] = 0;
+
+            balls[i]->accel[0] = 0;
+            balls[i]->accel[1] = 0;
+            balls[i]->accel[2] = 0;
+
+        }
+
+
+    }
+
+    return 0;
+}
+
+int PoolTable::state_integ() {
+    // Apply the acceleration by changing the velocity by the appropriate amount over the time step 
+
+    // How many state variables are needed for each ball
+    int n = 6;
+
+    for (int i = 0; i < numBalls; i++) {
+        // State - Need to load 4 values for each ball, but will have to have more when we add angular stuff
+        // pos[0] pos[1] vel[0] vel[1]
+        int inner_index = 0;
+        load_indexed_state(n*i + inner_index++, balls[i]->pos[0]);
+        load_indexed_state(n*i + inner_index++, balls[i]->pos[1]);
+        load_indexed_state(n*i + inner_index++, balls[i]->pos[2]);
+
+        load_indexed_state(n*i + inner_index++, balls[i]->vel[0]);
+        load_indexed_state(n*i + inner_index++, balls[i]->vel[1]);
+        load_indexed_state(n*i + inner_index++, balls[i]->vel[2]);
+
+        // Derivatives of all of this junk
+        // vel[0] vel[1] accel[0] accel[1]
+
+        inner_index = 0;
+        load_indexed_deriv(n*i + inner_index++, balls[i]->vel[0]);
+        load_indexed_deriv(n*i + inner_index++, balls[i]->vel[1]);
+        load_indexed_deriv(n*i + inner_index++, balls[i]->vel[2]);
+
+        load_indexed_deriv(n*i + inner_index++, balls[i]->accel[0]);     // Needs to be accel[0]
+        load_indexed_deriv(n*i + inner_index++, balls[i]->accel[1]);     // Needs to be accel[1]
+        load_indexed_deriv(n*i + inner_index++, balls[i]->accel[2]);     // Needs to be accel[2]
+
+    }
+
+    int integration_step = integrate();
+
+    for (int i = 0; i < numBalls; i++) {
+        // pos[0] pos[1] vel[0] vel[1]
+        int inner_index = 0;
+        balls[i]->pos[0] = unload_indexed_state(n*i + inner_index++);
+        balls[i]->pos[1] = unload_indexed_state(n*i + inner_index++);
+        balls[i]->pos[2] = unload_indexed_state(n*i + inner_index++);
+
+        balls[i]->vel[0] = unload_indexed_state(n*i + inner_index++);
+        balls[i]->vel[1] = unload_indexed_state(n*i + inner_index++);
+        balls[i]->vel[2] = unload_indexed_state(n*i + inner_index++);
+    }
+
+    return 0;
+}
+
+// Maybe need a separate scheduled job to handle pockets?
+// And the cue?
+// Maybe see if there's some "callback" that can handle user input
+// There must be, since other sims have control panels too
+
+double PoolTable::collision() {
+    // Handle when the balls collide with others or with a bumper
+
+    double now ; /* current integration time. */
+    unsigned int first, second;
+
+    unsigned int association_index;
+    double event_tgo;
+    unsigned int ii,jj;
+
+    now = get_integ_time() ;
+    event_tgo = BIG_TGO;
+
+    std::vector<unsigned int> collisionsToProcess;
+
+    for (ii=1; ii<numBalls; ii++) {
+        for (jj=0; jj<ii; jj++) {
+            double diff[3];
+            dv_sub(diff, balls[ii]->pos, balls[jj]->pos);
+            double distanceBetweenBalls = dv_mag(diff);
+            unsigned int associationIndex = ii*(ii-1)/2+jj;
+
+            // boundary is distance between balls - radiuses of balls
+            ballAssociations[associationIndex].error = distanceBetweenBalls - (balls[ii]->radius + balls[jj]->radius);
+            double this_tgo = regula_falsi( now, &(ballAssociations[associationIndex])) ;
+            
+            if (this_tgo < event_tgo) {
+                event_tgo = this_tgo;
+            }
+
+            if (this_tgo == 0) {
+                // Add this collision to a list of collisions to process
+                collisionsToProcess.push_back(ii);
+                collisionsToProcess.push_back(jj);
+            }
+        }
+    }
+
+    // Handle collisions
+    for (int i = 0; i < collisionsToProcess.size(); i+=2) {
+        int index1 = collisionsToProcess[i];
+        int index2 = collisionsToProcess[i+1];
+
+        double *q1 = balls[index1]->pos;
+        double *q2 = balls[index2]->pos;
+
+
+        // dg = (q1 - q2) / (|q1 - q2|)
+        double diff[3];
+        dv_sub(diff, q1, q2);
+        double dg[3];
+        dv_norm(dg, diff);
+
+        // Have to stuff both velocities and dg values into 4d vector to do the calculation
+        // Otherwise I have to do more math
+        double dg4[4] = {dg[0], dg[1], -dg[0], -dg[1]};
+        double vel4[4] = {balls[index1]->vel[0], balls[index1]->vel[1], balls[index2]->vel[0], balls[index2]->vel[1]};
+
+        // Calculate the impulse
+        // J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg
+        // For now let's just pretend all the masses are 1
+        double impulse = ((1.0 + coefficientOfElasticity) * dot(dg4, vel4, 4)) / (dot(dg4, dg4, 4));
+        scaleInPlace(dg, impulse, 4);
+
+        
+
+    }
+
+    return event_tgo;
+}
+
+// Add a ball to the table and return the ID
+int PoolTable::addBall (double x, double y, double mass, double radius, bool fixed) {
+    int id = nextBallSlot++;
+    if (id < numBalls) {
+        Ball * ball = (Ball*) TMM_declare_var_s("Ball");
+        balls[id] = (new (ball) Ball(x,y,mass,radius, fixed, id));
+        return id;
+    }
+    return -1;
+}
+
+// Add a bumper to the table and return the ID
+// Only takes in the actual effective line, need to add something else for the rendered shape
+int PoolTable::addBumper (double x1, double y1, double x2, double y2) {
+    // bumpers.push_back(bumper);
+    return -1;
+}
+
+int PoolTable::setBallPos(int id, double x, double y) {
+    if (id < numBalls && balls[id] != NULL) {
+        balls[id]->pos[0] = x;
+        balls[id]->pos[1] = y;
+        return 1;
+    }
+
+    return -1;
+}
+
+int PoolTable::setBallVel(int id, double v_x, double v_y) {
+    if (id < numBalls && balls[id] != NULL) {
+        balls[id]->vel[0] = v_x;
+        balls[id]->vel[1] = v_y;
+        return 1;
+    }
+    return -1;
+}
+
+
+