diff --git a/trick_sims/.gitignore b/trick_sims/.gitignore index 60d1a30c..4a06bade 100644 --- a/trick_sims/.gitignore +++ b/trick_sims/.gitignore @@ -25,7 +25,9 @@ T_main_* **/SIM_*/.trick **/SIM_*/trick.zip **/graphics/dist +**/graphics/java/dist **/graphics/manifest **/graphics/build +**/graphics/cpp/build jitlib build diff --git a/trick_sims/SIM_billiards/Modified_data/realtime.py b/trick_sims/SIM_billiards/Modified_data/realtime.py new file mode 100644 index 00000000..31546eb5 --- /dev/null +++ b/trick_sims/SIM_billiards/Modified_data/realtime.py @@ -0,0 +1,10 @@ + +trick.real_time_enable() +trick.exec_set_software_frame(0.01) +trick.itimer_enable() + +trick.exec_set_enable_freeze(True) +trick.exec_set_freeze_command(True) + +simControlPanel = trick.SimControlPanel() +trick.add_external_application(simControlPanel) diff --git a/trick_sims/SIM_billiards/RUN_break/input.py b/trick_sims/SIM_billiards/RUN_break/input.py new file mode 100644 index 00000000..e6e05b7c --- /dev/null +++ b/trick_sims/SIM_billiards/RUN_break/input.py @@ -0,0 +1,162 @@ +import math + +exec(open("./Modified_data/realtime.py").read()) + +dyn.table.numBalls = 16 +dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls) + +ballRadius = 0.02 +ballMass = 1 + +unit_pos = [math.sqrt(3)/2, 0.5] +unit_neg = [unit_pos[0], -unit_pos[1]] +center_x = 0.2; +center_y = 0; +tol = 1e-4; + +dyn.table.addBall(-.3, 0, ballMass, ballRadius, False) + +dyn.table.addBall(center_x, center_y, ballMass, ballRadius, False) + +dyn.table.addBall(center_x+unit_neg[0]*2*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*2*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False) + +dyn.table.addBall(center_x+unit_neg[0]*4*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y, ballMass, ballRadius, False) + +dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*6*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*6*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False) + +dyn.table.addBall(center_x+unit_neg[0]*8*(ballRadius+tol), center_y+unit_neg[1]*8*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*8*(ballRadius+tol), center_y+unit_pos[1]*8*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_neg[0]*8*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*8*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*8*(ballRadius+tol), center_y, ballMass, ballRadius, False) + +corners = [-.5, -.25, .5, .25] + + + +# Make a normal pool table +pocketRadius = 0.04 +bumperWidth = 0.03 + + +dyn.table.numTablePoints = 2 +dyn.table.tableShape = trick.TMM_declare_var_1d("Vec*", dyn.table.numTablePoints) +dyn.table.tableShapeType = 3 # rectangle +dyn.table.addPointToTable(corners[0], corners[1]) +dyn.table.addPointToTable(corners[2], corners[3]) + +# Pockets +dyn.table.numPockets = 6 +dyn.table.pockets = trick.TMM_declare_var_1d("Pocket*", dyn.table.numPockets) +pocketCenters = [[0, corners[1]], + [0, corners[3]], + [corners[0],corners[1]], + [corners[0],corners[3]], + [corners[2],corners[1]], + [corners[2],corners[3]] ] + +for coord in pocketCenters: + dyn.table.addPocket(coord[0], coord[1], pocketRadius) + + +bumperBorders =[[corners[0]+pocketRadius+bumperWidth, corners[1]+bumperWidth, 0.0-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom left + [0.0+pocketRadius+bumperWidth, corners[1]+bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom Right + [corners[0]+pocketRadius+bumperWidth, corners[3]-bumperWidth, 0.0-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top left + [0.0+pocketRadius+bumperWidth, corners[3]-bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top right + [corners[0]+bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[0]+bumperWidth, corners[3]-pocketRadius-bumperWidth], # Left + [corners[2]-bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[2]-bumperWidth, corners[3]-pocketRadius-bumperWidth]] # Right + +# triangles +bumperBorders.extend([[bumperBorders[0][0]-bumperWidth, bumperBorders[0][1]-bumperWidth, bumperBorders[0][0], bumperBorders[0][1]], # Bottom left + [bumperBorders[0][2], bumperBorders[0][1], bumperBorders[0][2]+bumperWidth, bumperBorders[0][1]-bumperWidth], + [bumperBorders[1][0]-bumperWidth, bumperBorders[1][1]-bumperWidth, bumperBorders[1][0], bumperBorders[1][1]], # Bottom Right + [bumperBorders[1][2], bumperBorders[1][1], bumperBorders[1][2]+bumperWidth, bumperBorders[1][1]-bumperWidth], + [bumperBorders[2][0]-bumperWidth, bumperBorders[2][1]+bumperWidth, bumperBorders[2][0], bumperBorders[2][1]], # Top left + [bumperBorders[2][2], bumperBorders[2][1], bumperBorders[2][2]+bumperWidth, bumperBorders[2][1]+bumperWidth], + [bumperBorders[3][0]-bumperWidth, bumperBorders[3][1]+bumperWidth, bumperBorders[3][0], bumperBorders[3][1]], # Top right + [bumperBorders[3][2], bumperBorders[3][1], bumperBorders[3][2]+bumperWidth, bumperBorders[3][1]+bumperWidth], + [bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][1]-bumperWidth], # Left + [bumperBorders[4][0]-bumperWidth, bumperBorders[4][3]+bumperWidth, bumperBorders[4][2], bumperBorders[4][3]], + [bumperBorders[5][0], bumperBorders[5][1], bumperBorders[5][2]+bumperWidth, bumperBorders[5][1]-bumperWidth], # Left + [bumperBorders[5][0]+bumperWidth, bumperBorders[5][3]+bumperWidth, bumperBorders[5][2], bumperBorders[5][3]], + ]) + + + +bumperShapes = [ [bumperBorders[0][0], bumperBorders[0][1]-bumperWidth, bumperBorders[0][2], bumperBorders[0][3]], + [bumperBorders[1][0], bumperBorders[1][1]-bumperWidth, bumperBorders[1][2], bumperBorders[1][3]], + [bumperBorders[2][0], bumperBorders[2][1]+bumperWidth, bumperBorders[2][2], bumperBorders[2][3]], + [bumperBorders[3][0], bumperBorders[3][1]+bumperWidth, bumperBorders[3][2], bumperBorders[3][3]], + [bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][3],], + [bumperBorders[5][0]+bumperWidth, bumperBorders[5][1], bumperBorders[5][2], bumperBorders[5][3],], + [bumperBorders[6][0], bumperBorders[6][1], bumperBorders[6][2], bumperBorders[6][3], bumperBorders[6][2], bumperBorders[6][1]], + [bumperBorders[7][0], bumperBorders[7][1], bumperBorders[7][2], bumperBorders[7][3], bumperBorders[7][0], bumperBorders[7][3]], + [bumperBorders[8][0], bumperBorders[8][1], bumperBorders[8][2], bumperBorders[8][3], bumperBorders[8][2], bumperBorders[8][1]], + [bumperBorders[9][0], bumperBorders[9][1], bumperBorders[9][2], bumperBorders[9][3], bumperBorders[9][0], bumperBorders[9][3]], + [bumperBorders[10][0], bumperBorders[10][1], bumperBorders[10][2], bumperBorders[10][3], bumperBorders[10][2], bumperBorders[10][1]], + [bumperBorders[11][0], bumperBorders[11][1], bumperBorders[11][2], bumperBorders[11][3], bumperBorders[11][0], bumperBorders[11][3]], + [bumperBorders[12][0], bumperBorders[12][1], bumperBorders[12][2], bumperBorders[12][3], bumperBorders[12][2], bumperBorders[12][1]], + [bumperBorders[13][0], bumperBorders[13][1], bumperBorders[13][2], bumperBorders[13][3], bumperBorders[13][0], bumperBorders[13][3]], + [bumperBorders[14][0], bumperBorders[14][1], bumperBorders[14][2], bumperBorders[14][3], bumperBorders[14][2], bumperBorders[14][1]], + [bumperBorders[15][0], bumperBorders[15][1], bumperBorders[15][2], bumperBorders[15][3], bumperBorders[15][0], bumperBorders[15][3]], + [bumperBorders[16][0], bumperBorders[16][1], bumperBorders[16][2], bumperBorders[16][3], bumperBorders[16][2], bumperBorders[16][1]], + [bumperBorders[17][0], bumperBorders[17][1], bumperBorders[17][2], bumperBorders[17][3], bumperBorders[17][0], bumperBorders[17][3]]] + +bumperShapeTypes = [3, 3, 3, 3, 3, 3, 2, 2,2,2,2, 2, 2, 2,2,2, 2, 2] + + +bumperNum = len(bumperBorders) +print("Num bumpers: ", len(bumperBorders)) +dyn.table.numBumpers = bumperNum +dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers) + +for i in range(bumperNum): + numPoints = len(bumperShapes[i])/2 + id = dyn.table.addBumper(numPoints, bumperBorders[i][0], bumperBorders[i][1], bumperBorders[i][2],bumperBorders[i][3]) + dyn.table.bumpers[id][0].numPoints = numPoints + dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Vec*", numPoints) + + dyn.table.bumpers[id][0].shapeType = bumperShapeTypes[i] + for j in range(0, len(bumperShapes[i]), 2): + dyn.table.addPointToBumper(id, bumperShapes[i][j],bumperShapes[i][j+1]) + + +dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls) + +#========================================== +# Start the Graphics Client +#========================================== +varServerPort = trick.var_server_get_port(); + +PoolTableDisplay_path = "models/graphics/cpp/build/PoolTableDisplay" + + +if (os.path.isfile(PoolTableDisplay_path)) : + PoolTableDisplay_cmd = PoolTableDisplay_path \ + + " " + str(varServerPort) + " &" ; + print(PoolTableDisplay_cmd) + os.system( PoolTableDisplay_cmd); +else : + print('===================================') + print('PoolTableDisplay needs to be built.') + print('===================================') + + +# PoolTableDisplay_path = "models/graphics/java/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/java and type \"make\".') +# print('=================================================================================================') diff --git a/trick_sims/SIM_billiards/RUN_hexagon/input.py b/trick_sims/SIM_billiards/RUN_hexagon/input.py new file mode 100644 index 00000000..5b65bbb0 --- /dev/null +++ b/trick_sims/SIM_billiards/RUN_hexagon/input.py @@ -0,0 +1,119 @@ +import math + +exec(open("./Modified_data/realtime.py").read()) + +dyn.table.numBalls = 4 +dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls) + +ballRadius = 0.02 +ballMass = 1 + +unit_pos = [math.sqrt(3)/2, 0.5] +unit_neg = [unit_pos[0], -unit_pos[1]] +center_x = 0.2; +center_y = 0; +tol = 1e-4; + +dyn.table.addBall(-.3, 0, ballMass, ballRadius, False) + +dyn.table.addBall(center_x, center_y, ballMass, ballRadius, False) + +dyn.table.addBall(center_x+unit_neg[0]*2*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False) +dyn.table.addBall(center_x+unit_pos[0]*2*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False) + +# dyn.table.addBall(center_x+unit_neg[0]*4*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y, ballMass, ballRadius, False) + +# dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*6*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*6*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False) + + + + + + +# Make a Hexagonal table +corners = [ [1, 0], + [1/2, math.sqrt(3)/2], + [-1/2, math.sqrt(3)/2], + [-1, 0], + [-1/2, -math.sqrt(3)/2], + [1/2, -math.sqrt(3)/2]] + +scale = 0.5 + +for i in range(len(corners)): + corners[i][0] *= scale + corners[i][1] *= scale + +pocketRadius = 0.04 +bumperWidth = 0.03 + + +dyn.table.numTablePoints = 6 +dyn.table.tableShape = trick.TMM_declare_var_1d("Vec*", dyn.table.numTablePoints) +dyn.table.tableShapeType = 0 # generic +for corner in corners: + dyn.table.addPointToTable(corner[0], corner[1]) + +# Pockets - Just put 1 in the center +dyn.table.numPockets = 1 +dyn.table.pockets = trick.TMM_declare_var_1d("Pocket*", dyn.table.numPockets) +dyn.table.addPocket(0, 0, pocketRadius) +# for coord in corners: +# dyn.table.addPocket(coord[0], coord[1], pocketRadius) + +# dyn.table.numBumpers = 0 +dyn.table.numBumpers = len(corners) +dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers) + +# Put bumpers along each edge +for i in range(len(corners)): + p1 = [corners[i][0], corners[i][1]] + p2 = [corners[(i+1) % 6][0], corners[(i+1) % 6][1]] + id = dyn.table.addBumper(3, p1[0], p1[1], p2[0], p2[1]) + dyn.table.bumpers[id][0].shapeType = 2 # Triangle i guess? + dyn.table.bumpers[id][0].numPoints = 3 + dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Vec*", dyn.table.bumpers[id].numPoints) + dyn.table.addPointToBumper(id, p1[0], p1[1]) + dyn.table.addPointToBumper(id, p2[0], p2[1]) + p3 = [(p1[0] + p2[0])/2,(p1[1] + p2[1])/2] + dyn.table.addPointToBumper(id, p3[0], p3[1]) + +dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls) + +#========================================== +# Start the Graphics Client +#========================================== +varServerPort = trick.var_server_get_port(); + +# This will definitely change to something else +PoolTableDisplay_path = "models/graphics/cpp/build/PoolTableDisplay" + + +if (os.path.isfile(PoolTableDisplay_path)) : + PoolTableDisplay_cmd = PoolTableDisplay_path \ + + " " + str(varServerPort) + " &" ; + print(PoolTableDisplay_cmd) + os.system( PoolTableDisplay_cmd); +else : + print('===================================') + print('PoolTableDisplay needs to be built.') + print('===================================') + + +# PoolTableDisplay_path = "models/graphics/java/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/java and type \"make\".') +# print('=================================================================================================') diff --git a/trick_sims/SIM_billiards/RUN_test/input.py b/trick_sims/SIM_billiards/RUN_test/input.py new file mode 100644 index 00000000..f8b990cc --- /dev/null +++ b/trick_sims/SIM_billiards/RUN_test/input.py @@ -0,0 +1,157 @@ +import math + +exec(open("./Modified_data/realtime.py").read()) + +dyn.table.numBalls = 2 +dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls) + +ballRadius = 0.02 +ballMass = 1 + +unit_pos = [math.sqrt(3)/2, 0.5] +unit_neg = [unit_pos[0], -unit_pos[1]] +center_x = 0.2; +center_y = 0; +tol = 1e-4; + +dyn.table.defaultCueBallX = -0.05; +dyn.table.defaultCueBallY = 0.1; +dyn.table.addBall(-0.05, 0.1, ballMass, ballRadius, False) + +dyn.table.addBall(-0.055, 0.15, ballMass, ballRadius, False) + +# dyn.table.addBall(center_x+unit_neg[0]*2*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*2*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False) + +# dyn.table.addBall(center_x+unit_neg[0]*4*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y, ballMass, ballRadius, False) + +# dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*6*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*6*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False) +# dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False) + +corners = [-.5, -.25, .5, .25] + + + +# Make a normal pool table +pocketRadius = 0.04 +bumperWidth = 0.03 + + +dyn.table.numTablePoints = 2 +dyn.table.tableShape = trick.TMM_declare_var_1d("Vec*", dyn.table.numTablePoints) +dyn.table.tableShapeType = 3 # rectangle +dyn.table.addPointToTable(corners[0], corners[1]) +dyn.table.addPointToTable(corners[2], corners[3]) + +# Pockets +dyn.table.numPockets = 6 +dyn.table.pockets = trick.TMM_declare_var_1d("Pocket*", dyn.table.numPockets) +pocketCenters = [[0, corners[1]], + [0, corners[3]], + [corners[0],corners[1]], + [corners[0],corners[3]], + [corners[2],corners[1]], + [corners[2],corners[3]] ] + +for coord in pocketCenters: + dyn.table.addPocket(coord[0], coord[1], pocketRadius) + + +bumperBorders =[[corners[0]+pocketRadius+bumperWidth, corners[1]+bumperWidth, 0.0-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom left + [0.0+pocketRadius+bumperWidth, corners[1]+bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom Right + [corners[0]+pocketRadius+bumperWidth, corners[3]-bumperWidth, 0.0-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top left + [0.0+pocketRadius+bumperWidth, corners[3]-bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top right + [corners[0]+bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[0]+bumperWidth, corners[3]-pocketRadius-bumperWidth], # Left + [corners[2]-bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[2]-bumperWidth, corners[3]-pocketRadius-bumperWidth]] # Right + +# triangles +bumperBorders.extend([[bumperBorders[0][0]-bumperWidth, bumperBorders[0][1]-bumperWidth, bumperBorders[0][0], bumperBorders[0][1]], # Bottom left + [bumperBorders[0][2], bumperBorders[0][1], bumperBorders[0][2]+bumperWidth, bumperBorders[0][1]-bumperWidth], + [bumperBorders[1][0]-bumperWidth, bumperBorders[1][1]-bumperWidth, bumperBorders[1][0], bumperBorders[1][1]], # Bottom Right + [bumperBorders[1][2], bumperBorders[1][1], bumperBorders[1][2]+bumperWidth, bumperBorders[1][1]-bumperWidth], + [bumperBorders[2][0]-bumperWidth, bumperBorders[2][1]+bumperWidth, bumperBorders[2][0], bumperBorders[2][1]], # Top left + [bumperBorders[2][2], bumperBorders[2][1], bumperBorders[2][2]+bumperWidth, bumperBorders[2][1]+bumperWidth], + [bumperBorders[3][0]-bumperWidth, bumperBorders[3][1]+bumperWidth, bumperBorders[3][0], bumperBorders[3][1]], # Top right + [bumperBorders[3][2], bumperBorders[3][1], bumperBorders[3][2]+bumperWidth, bumperBorders[3][1]+bumperWidth], + [bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][1]-bumperWidth], # Left + [bumperBorders[4][0]-bumperWidth, bumperBorders[4][3]+bumperWidth, bumperBorders[4][2], bumperBorders[4][3]], + [bumperBorders[5][0], bumperBorders[5][1], bumperBorders[5][2]+bumperWidth, bumperBorders[5][1]-bumperWidth], # Left + [bumperBorders[5][0]+bumperWidth, bumperBorders[5][3]+bumperWidth, bumperBorders[5][2], bumperBorders[5][3]], + ]) + + + +bumperShapes = [ [bumperBorders[0][0], bumperBorders[0][1]-bumperWidth, bumperBorders[0][2], bumperBorders[0][3]], + [bumperBorders[1][0], bumperBorders[1][1]-bumperWidth, bumperBorders[1][2], bumperBorders[1][3]], + [bumperBorders[2][0], bumperBorders[2][1]+bumperWidth, bumperBorders[2][2], bumperBorders[2][3]], + [bumperBorders[3][0], bumperBorders[3][1]+bumperWidth, bumperBorders[3][2], bumperBorders[3][3]], + [bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][3],], + [bumperBorders[5][0]+bumperWidth, bumperBorders[5][1], bumperBorders[5][2], bumperBorders[5][3],], + [bumperBorders[6][0], bumperBorders[6][1], bumperBorders[6][2], bumperBorders[6][3], bumperBorders[6][2], bumperBorders[6][1]], + [bumperBorders[7][0], bumperBorders[7][1], bumperBorders[7][2], bumperBorders[7][3], bumperBorders[7][0], bumperBorders[7][3]], + [bumperBorders[8][0], bumperBorders[8][1], bumperBorders[8][2], bumperBorders[8][3], bumperBorders[8][2], bumperBorders[8][1]], + [bumperBorders[9][0], bumperBorders[9][1], bumperBorders[9][2], bumperBorders[9][3], bumperBorders[9][0], bumperBorders[9][3]], + [bumperBorders[10][0], bumperBorders[10][1], bumperBorders[10][2], bumperBorders[10][3], bumperBorders[10][2], bumperBorders[10][1]], + [bumperBorders[11][0], bumperBorders[11][1], bumperBorders[11][2], bumperBorders[11][3], bumperBorders[11][0], bumperBorders[11][3]], + [bumperBorders[12][0], bumperBorders[12][1], bumperBorders[12][2], bumperBorders[12][3], bumperBorders[12][2], bumperBorders[12][1]], + [bumperBorders[13][0], bumperBorders[13][1], bumperBorders[13][2], bumperBorders[13][3], bumperBorders[13][0], bumperBorders[13][3]], + [bumperBorders[14][0], bumperBorders[14][1], bumperBorders[14][2], bumperBorders[14][3], bumperBorders[14][2], bumperBorders[14][1]], + [bumperBorders[15][0], bumperBorders[15][1], bumperBorders[15][2], bumperBorders[15][3], bumperBorders[15][0], bumperBorders[15][3]], + [bumperBorders[16][0], bumperBorders[16][1], bumperBorders[16][2], bumperBorders[16][3], bumperBorders[16][2], bumperBorders[16][1]], + [bumperBorders[17][0], bumperBorders[17][1], bumperBorders[17][2], bumperBorders[17][3], bumperBorders[17][0], bumperBorders[17][3]]] + +bumperShapeTypes = [3, 3, 3, 3, 3, 3, 2, 2,2,2,2, 2, 2, 2,2,2, 2, 2] + +bumperNum = len(bumperBorders) +print("Num bumpers: ", len(bumperBorders)) +dyn.table.numBumpers = bumperNum +dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers) + +for i in range(bumperNum): + numPoints = len(bumperShapes[i])/2 + id = dyn.table.addBumper(numPoints, bumperBorders[i][0], bumperBorders[i][1], bumperBorders[i][2],bumperBorders[i][3]) + dyn.table.bumpers[id][0].numPoints = numPoints + dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Vec*", numPoints) + + dyn.table.bumpers[id][0].shapeType = bumperShapeTypes[i] + for j in range(0, len(bumperShapes[i]), 2): + dyn.table.addPointToBumper(id, bumperShapes[i][j],bumperShapes[i][j+1]) + +dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls) + +#========================================== +# Start the Graphics Client +#========================================== +varServerPort = trick.var_server_get_port(); + +# This will definitely change to something else +PoolTableDisplay_path = "models/graphics/cpp/build/PoolTableDisplay" + + +if (os.path.isfile(PoolTableDisplay_path)) : + PoolTableDisplay_cmd = PoolTableDisplay_path \ + + " " + str(varServerPort) + " &" ; + print(PoolTableDisplay_cmd) + os.system( PoolTableDisplay_cmd); +else : + print('===================================') + print('PoolTableDisplay needs to be built.') + print('===================================') + + +# PoolTableDisplay_path = "models/graphics/java/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/java and type \"make\".') +# print('=================================================================================================') diff --git a/trick_sims/SIM_billiards/S_define b/trick_sims/SIM_billiards/S_define new file mode 100644 index 00000000..15dcd6ca --- /dev/null +++ b/trick_sims/SIM_billiards/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.001) dyn; diff --git a/trick_sims/SIM_billiards/S_overrides.mk b/trick_sims/SIM_billiards/S_overrides.mk new file mode 100644 index 00000000..1b84a37b --- /dev/null +++ b/trick_sims/SIM_billiards/S_overrides.mk @@ -0,0 +1,2 @@ +TRICK_CFLAGS += -Imodels +TRICK_CXXFLAGS += -Imodels \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/graphics/cpp/CMakeLists.txt b/trick_sims/SIM_billiards/models/graphics/cpp/CMakeLists.txt new file mode 100644 index 00000000..49364e36 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/cpp/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.16) +project(PoolTableDisplay) + +list(PREPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) + +# Libigl +option(LIBIGL_GLFW "Build target igl::glfw" ON) +option(LIBIGL_IMGUI "Build target igl::imgui" ON) + +include(libigl) + + +# Add your project files +file(GLOB SRC_FILES *.cpp) +add_executable(${PROJECT_NAME} ${SRC_FILES}) +target_link_libraries(${PROJECT_NAME} PUBLIC igl::glfw igl::imgui) diff --git a/trick_sims/SIM_billiards/models/graphics/cpp/README.md b/trick_sims/SIM_billiards/models/graphics/cpp/README.md new file mode 100644 index 00000000..2479c965 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/cpp/README.md @@ -0,0 +1,26 @@ +# Pool Table Display + +Uses the Libigl library for visualization. Connects to SIM_billiards + + +## Compile + +libigl must be downloaded from Github : https://github.com/libigl/libigl + +Clone it somewhere accessible, like your home directory. It does not need to be compiled. + +libigl depends on Eigen3, which may need to be downloaded and installed : https://eigen.tuxfamily.org/index.php?title=Main_Page + + +To compile: + mkdir build + cd build + cmake .. + make + +## Run + +From within the `build` directory: + + ./PoolTableDisplay + diff --git a/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp b/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp new file mode 100644 index 00000000..d4587478 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp @@ -0,0 +1,61 @@ +#include "Socket.hh" + +#define SOCKET_BUF_SIZE 20480 + +Socket::Socket() : _initialized(false) {} + +int Socket::init (std::string hostname, int port) { + _hostname = hostname; + _port = port; + _socket_fd = socket(AF_INET, SOCK_STREAM, 0); + if (_socket_fd < 0) { + std::cout << "Socket connection failed" << std::endl; + return -1; + } + + struct sockaddr_in serv_addr; + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(port); // convert to weird network byte format + + if(inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr)<=0) { + std::cout << "Invalid address/ Address not supported" << std::endl; + return -1; + } + + if (connect(_socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) { + std::cout << "Connection failed" << std::endl; + return -1; + } + + _initialized = true; +} + +int Socket::send (std::string message) { + // weird syntax I've never used before - since the send syscall that i'm trying to use is overloaded in this class, + // I have to append :: to the front of it so that the compiler knows to look in the global namespace + int success = ::send(_socket_fd, message.c_str(), message.size(), 0); + if (success < message.size()) { + std::cout << "Failed to send message" << std::endl; + } + return success; +} + +int Socket::operator<< (std::string message) { + return send(message); +} + +std::string Socket::receive () { + char buffer[SOCKET_BUF_SIZE]; + int numBytes = read(_socket_fd, buffer, SOCKET_BUF_SIZE); + if (numBytes < 0) { + std::cout << "Failed to read from socket" << std::endl; + } else if (numBytes < SOCKET_BUF_SIZE) { + buffer[numBytes] = '\0'; + } + + return std::string(buffer); +} + +void Socket::operator>> (std::string& ret) { + ret = receive(); +} \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/graphics/cpp/Socket.hh b/trick_sims/SIM_billiards/models/graphics/cpp/Socket.hh new file mode 100644 index 00000000..dd9680d1 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/cpp/Socket.hh @@ -0,0 +1,27 @@ +#include +#include +#include +#include +#include +#include +#include + +class Socket { + + public: + Socket (); + int init(std::string hostname, int port); + + int send (std::string message); + int operator<< (std::string message); + + std::string receive (); + void operator>> (std::string& ret); + + private: + int _port; + std::string _hostname; + int _socket_fd; + bool _initialized; + +}; diff --git a/trick_sims/SIM_billiards/models/graphics/cpp/cmake/libigl.cmake b/trick_sims/SIM_billiards/models/graphics/cpp/cmake/libigl.cmake new file mode 100644 index 00000000..7b0ce371 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/cpp/cmake/libigl.cmake @@ -0,0 +1,11 @@ +if(TARGET igl::core) + return() +endif() + +include(FetchContent) +FetchContent_Declare( + libigl + GIT_REPOSITORY https://github.com/libigl/libigl.git + GIT_TAG v2.4.0 +) +FetchContent_MakeAvailable(libigl) diff --git a/trick_sims/SIM_billiards/models/graphics/cpp/main.cpp b/trick_sims/SIM_billiards/models/graphics/cpp/main.cpp new file mode 100644 index 00000000..1ed067e0 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/cpp/main.cpp @@ -0,0 +1,967 @@ +#include +// #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "Socket.hh" + + +std::vector ball_colors = {Eigen::Vector3d(0.0,0.4,0.0), //green + Eigen::Vector3d(1.0,1.0,0.0), //yellow + Eigen::Vector3d(0.0,0.0,1.0), //blue + Eigen::Vector3d(1.0,0.0,0.0), //red + Eigen::Vector3d(0.5,0.0,0.3), //purple + Eigen::Vector3d(0.7,0.5,0.0), //orange + Eigen::Vector3d(0.0,0.0,0.0), //black + Eigen::Vector3d(0.3,0.07,0.15), //maroon + }; + + +const double layer_BALL = 0.005; +const double layer_TABLE = 0.001; +const double layer_BUMPER = 0.002; +const double layer_RAIL = 0.000; +const double layer_CUE = 0.008; +const double layer_POCKET = 0.002; + + + +class RenderedShape { + public: + RenderedShape() {} + + int baseIndex = 0; + + int getNumVertices() { + return vertices.size(); + } + + int getNumFaces() { + return faces.size(); + } + + std::vector getFacesWithBaseIndex(int newBase) { + std::vector newFaces; + int offset = newBase - baseIndex; + Eigen::Vector3i baseOffset(offset, offset, offset); + for (Eigen::Vector3i& face : faces) { + newFaces.emplace_back(face + baseOffset); + } + baseIndex = newBase; + + return newFaces; + } + + std::vector vertices; + std::vector faces; + std::vector colors; +}; + +class Point { + + public: + Point () : point(0, 0) {} + Point (double x, double y) : point(x, y, 0) {} + Point (double x, double y, double layer) : point(x, y, layer) {} + + Eigen::Vector3d toVec3 () const { + return Eigen::Vector3d(point); + } + + double x() const { + return point[0]; + } + + double y() const { + return point[1]; + } + + Eigen::Vector3d point; +}; + +class Polygon { + public: + Polygon(unsigned int verts, double layer) : vertexMax(verts), layer(layer) {} + + void addPoint(double x, double y) { + if (points.size() < vertexMax) + points.emplace_back(x, y, layer); + } + + void setColor(double r, double g, double b) { + color = Eigen::Vector3d(r, g, b); + } + + void setColor(Eigen::Vector3d& c) { + color = Eigen::Vector3d(c); + } + + void setLayer(double l) { + layer = l; + } + + Eigen::Vector3d getColor() const { + return color; + } + + // Works with any simple convex polygon where the points are in order + virtual RenderedShape* render() const { + RenderedShape * shape = new RenderedShape(); + + if (!isValid()) { + // Should maybe throw an error + std::cout << "Generic has incorrect number of corners" << std::endl; + return shape; + } + + for (int i = 0; i < points.size(); i++) { + shape->vertices.emplace_back(points[i].toVec3()); + shape->colors.emplace_back(color); + } + + for (int i = 1; i < points.size()-1; i++) { + shape->faces.emplace_back(0, i, i+1); + } + + return shape; + } + + virtual bool isValid() const { + return points.size() <= vertexMax; + } + + protected: + std::vector points; + Eigen::Vector3d color; + unsigned int vertexMax; + double layer; + int id; +}; + +class Circle : public Polygon { + public: + Circle (double x, double y, double r, double layer) : Polygon(1, layer), radius(r) { + addPoint(x, y); + } + + bool isValid() const { + return points.size() == vertexMax; + } + + RenderedShape *render() const { + // Circle is broken down into wedges in order to be rendered here + RenderedShape *shape = new RenderedShape(); + + if (!isValid()) { + std::cout << "Circle has incorrect number of points" << std::endl; + return shape; + } + + // Add colors + for (int i = 0; i < numWedges + 2; i++) { + shape->colors.emplace_back(color); + } + + // Add center + shape->vertices.emplace_back(points[0].toVec3()); + + // Add outside vertices + for (int i = 0; i <= numWedges; i++) { + shape->vertices.emplace_back(points[0].x() + radius * cos(2 * M_PI * i / numWedges), + points[0].y() + radius * sin(2 * M_PI * i / numWedges), + layer); + + } + + // Make the triangles + for (int j = 0; j <= numWedges; j++) { + shape->faces.emplace_back(0, j + 1, 1 + ((j + 1) % (numWedges + 1))); + } + + + return shape; + } + + private: + double radius; + int numWedges = 20; +}; + +class Rectangle : public Polygon { + public: + + // only need upper left and lower right corners + Rectangle(double layer) : Polygon(2, layer) {} + + bool isValid() const { + return points.size() == vertexMax; + } + + void addCorner (double x, double y) { + addPoint(x, y); + } + + RenderedShape* render() const { + RenderedShape *shape = new RenderedShape(); + + if (!isValid()) { + // Should maybe throw an error + std::cerr << "Rectangle has incorrect number of corners" << std::endl; + return shape; + } + + // Add colors + for (int i = 0; i < 6; i++) { + shape->colors.emplace_back(color); + } + + // Add triangles within rectangle + shape->vertices.emplace_back(points[0].x(), points[1].y(), layer); + shape->vertices.emplace_back(points[1].x(), points[0].y(), layer); + shape->vertices.emplace_back(points[0].toVec3()); + + shape->faces.emplace_back(0, 1, 2); + + shape->vertices.emplace_back(points[0].x(), points[1].y(), layer); + shape->vertices.emplace_back(points[1].toVec3()); + shape->vertices.emplace_back(points[1].x(), points[0].y(), layer); + + shape->faces.emplace_back(3, 4, 5); + + return shape; + } +}; + +class Triangle : public Polygon { + public: + Triangle (double layer) : Polygon(3, layer) {} + + + void addCorner (double x, double y) { + addPoint(x, y); + } + + RenderedShape* render() const { + RenderedShape *shape = new RenderedShape(); + + if (!isValid()) { + // Should maybe throw an error + std::cerr << "Triangle has incorrect number of corners" << std::endl; + return shape; + } + + // Add colors and vertices at the same time + for (int i = 0; i < 3; i++) { + shape->colors.emplace_back(color); + shape->vertices.emplace_back(points[i].toVec3()); + } + + // just the 1 triangle + shape->faces.emplace_back(0, 1, 2); + + return shape; + } +}; + +enum PolygonType { + GENERIC, + CIRCLE, + TRIANGLE, + RECTANGLE +}; + +class Table { + public: + Table () {} + + void clearMovingShapes() { + for (int i = 0; i < movingShapes.size(); i++) { + delete movingShapes[i]; + delete movingRenderedShapes[i]; + } + movingShapes.clear(); + movingRenderedShapes.clear(); + + } + + void updateMovingShape(int id) { + // TODO + } + + // Need to have an agreed upon way to send over variables + int addShape(std::vector shapeData, Eigen::Vector3d color, bool isStatic, PolygonType type, double layer) { + // std::cout << "In AddShape" << std::endl; + Polygon *shape; + + switch (type) { + + case GENERIC: { + // Number of points is just data / 2 i guess + // std::cout << "Creating generic polygon with " << shapeData.size()/2 << " points" << std::endl; + Polygon *newPolygon = new Polygon(shapeData.size()/2, layer); + for (int i = 0; i < shapeData.size(); i+=2) { + double x = shapeData[i]; + double y = shapeData[i+1]; + newPolygon->addPoint(x,y); + } + shape = newPolygon; + break; + } + case CIRCLE: { + // std::cout << "Adding circle" << std::endl; + if (shapeData.size() != 3) { + std::cout << "Bad shapedata size for circle" << std::endl; + return -1; + } + double x = shapeData[0]; + double y = shapeData[1]; + double r = shapeData[2]; + Circle *newCircle = new Circle(x, y, r, layer); + + shape = newCircle; + break; + } + case TRIANGLE: { + Triangle *newTriangle = new Triangle(layer); + if (shapeData.size() != 6) { + std::cout << "Bad shapedata size for triangle" << std::endl; + return -1; + } + for (int i = 0; i < shapeData.size(); i+=2) { + double x = shapeData[i]; + double y = shapeData[i+1]; + newTriangle->addCorner(x, y); + } + + shape = newTriangle; + break; + } + case RECTANGLE: { + // std::cout << "In rectangle" << std::endl; + Rectangle *newRectangle = new Rectangle(layer); + if (shapeData.size() != 4) { + std::cout << "Bad shapedata size for rectangle" << std::endl; + return -1; + } + for (int i = 0; i < shapeData.size(); i+=2) { + double x = shapeData[i]; + double y = shapeData[i+1]; + newRectangle->addCorner(x, y); + } + shape = newRectangle; + break; + } + default: { + break; + } + } + + shape->setColor(color); + + if (isStatic) { + std::cout << "Adding to static shapes" << std::endl; + staticShapes.emplace_back(shape); + } else { + // std::cout << "Adding to moving shapes" << std::endl; + movingShapes.emplace_back(shape); + } + + + return 0; + } + + // Call this once + void renderStaticShapes() { + staticRendered = true; + numStaticVertices = 0; + numStaticFaces = 0; + int i = 0; + staticRenderedShapes.clear(); + for (Polygon* shape : staticShapes) { + std::cout << "Rendering shape " << i++ << std::endl; + RenderedShape *renderedShape = shape->render(); + numStaticVertices += renderedShape->getNumVertices(); + numStaticFaces += renderedShape->getNumFaces(); + staticRenderedShapes.emplace_back(renderedShape); + } + } + + // Should think about how to make sure we aren't making big unnessary copies of stuff + std::tuple getMesh() { + + // if (!staticRendered) { + // renderStaticShapes(); + // } + + numStaticVertices = 0; + numStaticFaces = 0; + int i = 0; + staticRenderedShapes.clear(); + + for (Polygon* shape : staticShapes) { + RenderedShape *renderedShape = shape->render(); + numStaticVertices += renderedShape->getNumVertices(); + numStaticFaces += renderedShape->getNumFaces(); + staticRenderedShapes.emplace_back(renderedShape); + } + + int totalFaces = numStaticFaces; + int totalVertices = numStaticVertices; + for (Polygon* shape : movingShapes) { + RenderedShape *renderedShape = shape->render(); + totalVertices += renderedShape->getNumVertices(); + totalFaces += renderedShape->getNumFaces(); + movingRenderedShapes.push_back(renderedShape); + } + + // std::cout << "Total Vertices: " << totalVertices << std::endl; + // std::cout << "Total Faces: " << totalFaces << std::endl; + // std::cout << "Total Colors: " << totalVertices << std::endl; + + + // Now have to put all of these into giant matrices + Eigen::MatrixXd renderV; + renderV.resize(totalVertices, 3); + Eigen::MatrixXi renderF; + renderF.resize(totalFaces, 3); + Eigen::MatrixXd renderC; + renderC.resize(totalVertices, 3); + + // TODO: Ideally have some matrix with preloaded static shapes + // For now do them all here + + int vertexIndex = 0; + int faceIndex = 0; + for (RenderedShape* shape : staticRenderedShapes) { + // Add vertices and colors + for (int i = 0; i < shape->getNumVertices(); i++) { + renderV.row(i+vertexIndex) = shape->vertices[i]; + renderC.row(i+vertexIndex) = shape->colors[i]; + } + + auto newFaces = shape->getFacesWithBaseIndex(vertexIndex); + + // Add faces - with the correct offset + for (int i = 0; i < newFaces.size(); i++) { + renderF.row(i+faceIndex) = newFaces[i]; + } + + vertexIndex += shape->getNumVertices(); + faceIndex += newFaces.size(); + } + + for (RenderedShape* shape : movingRenderedShapes) { + // Add vertices and colors + for (int i = 0; i < shape->getNumVertices(); i++) { + renderV.row(i+vertexIndex) = shape->vertices[i]; + renderC.row(i+vertexIndex) = shape->colors[i]; + } + + auto newFaces = shape->getFacesWithBaseIndex(vertexIndex); + + // Add faces - with the correct offset + for (int i = 0; i < newFaces.size(); i++) { + renderF.row(i+faceIndex) = newFaces[i]; + } + + vertexIndex += shape->getNumVertices(); + faceIndex += newFaces.size(); + } + + return std::make_tuple(renderV, renderF, renderC); + } + + private: + std::vector staticShapes; + std::vector staticRenderedShapes; + std::vector movingRenderedShapes; + int numStaticVertices; + int numStaticFaces; + bool staticRendered = false; + + std::vector movingShapes; + +}; + +void printUsage() { + std::cout << "Usage: program " << std::endl; +} + +std::vector split (std::string& str, const char delim) { + std::stringstream ss(str); + std::string s; + std::vector ret; + while (std::getline(ss, s, delim)) { + ret.push_back(s); + } + return ret; +} + +std::vector parseTrickResponse(std::vector list) { + std::vector ret; + for (int i = 1; i < list.size(); i++) { + ret.push_back(stod(list[i])); + } + return ret; +} + +std::vector parseTrickResponseInt(std::vector list) { + std::vector ret; + for (int i = 1; i < list.size(); i++) { + ret.push_back(stoi(list[i])); + } + return ret; +} + +// void launchViewer (igl::opengl::glfw::Viewer *viewer) { +// std::cout << "About to launch the viewer" << std::endl; +// viewer->launch(); +// } + + + +// std::queue messageQueue; +// std::mutex messageLock; + +// std::mutex renderLock; + + +// igl::opengl::glfw::imgui::ImGuiMenu* menu; + +// bool mouse_down (igl::opengl::glfw::Viewer& viewer, int button, int modifier) { +// mousePressed = true; +// return true; +// } + +// bool mouse_up (igl::opengl::glfw::Viewer& viewer, int button, int modifier) { +// mousePressed = false; +// std::string cueRequest = ""; +// std::string templateString = "dyn.table.applyCueForce(%.3f, %.3f) \n"; + +// char buf[128]; +// sprintf(buf, templateString.c_str(), mouseX, mouseY); +// cueRequest += std::string(buf); + +// messageLock.lock(); +// messageQueue.push(cueRequest); +// messageLock.unlock(); + +// return true; +// } + +// bool mouse_move (igl::opengl::glfw::Viewer& viewer, int mouse_x, int mouse_y) { +// Eigen::Vector3f pos(mouse_x, mouse_y, 0); +// Eigen::Matrix4f model = viewer.core().view; +// Eigen::Vector3f unproj = igl::unproject(pos, model, viewer.core().proj, viewer.core().viewport); +// mouseX = unproj[0]; +// mouseY = -unproj[1]; +// return true; +// } + +// bool pre_draw (igl::opengl::glfw::Viewer& viewer) { + + // std::string reply; + // socket >> reply; + + // // std::cout << "Got data: " << reply << std::endl; + // std::vector replyData = parseTrickResponse(split(reply, '\t')); + // // numBalls = (int)replyData[0]; + + // if (replyData.size() <= 1) { + // // std::cout << "Received bad reply" << std::endl; + // return false; + // } + + // table.clearMovingShapes(); + + // Eigen::Vector2d cueBallPos(0,0); + // int cueBallIndex = 0; + + + // for (int i = 0; i < numBalls; i++) { + // double inPlay = replyData[1+(i*3 + 2)]; + // if (inPlay == 0) { + // continue; + // } + // std::vector circleData = {replyData[1+(i*3)], replyData[1+(i*3 + 1)], radii[i]}; + // Eigen::Vector3d circleColor; + // if (i == cueBallIndex) { + // circleColor = Eigen::Vector3d(1,1,1); + // cueBallPos = Eigen::Vector2d(replyData[1+(i*2)], replyData[1+(i*2 + 1)]); + // } else { + // circleColor = ball_colors[i % ball_colors.size()]; + + // } + // table.addShape(circleData, circleColor, false, CIRCLE, layer_BALL); + // } + + // if (mousePressed) { + // // Draw the cue + // double cue_width = 0.03; + // Eigen::Vector2d cue_end(mouseX, mouseY); + // Eigen::Vector2d vec = (cue_end - cueBallPos).normalized(); + // Eigen::Vector2d off1(-vec(1), vec(0)); + // Eigen::Vector2d off2(vec(1), -vec(0)); + // Eigen::Vector2d point1 = cue_end + (off1 * cue_width); + // Eigen::Vector2d point2 = cue_end + (off2 * cue_width); + // std::vector triangleData = {cueBallPos(0), cueBallPos(1), point1(0), point1(1), point2(0), point2(1)}; + // table.addShape(triangleData, Eigen::Vector3d(0, 0, 0), false, TRIANGLE, layer_CUE); + // } + + // renderLock.lock(); + // std::tie(V, F, C) = table.getMesh(); + + // renderLock.lock(); + + // viewer.data().clear(); + // viewer.core().orthographic = true; + // viewer.data().show_lines = false; + // viewer.data().set_face_based(false); + // viewer.data().double_sided = true; + // viewer.core().is_animating = true; + // viewer.core().camera_zoom = 2; + // viewer.data().set_mesh(V, F); + // viewer.data().set_colors(C); + + // renderLock.unlock(); +// } + +// void draw_viewer_menu () { +// ImGui::Text("Menu"); +// if (ImGui::Button("Reset Cue Ball", ImVec2(-1, 0))) +// { +// std::string message = "dyn.table.resetCueBall() \n"; +// messageLock.lock(); +// messageQueue.push(message); +// messageLock.unlock(); +// } + +// } + + +int main(int argc, char *argv[]) +{ + // Parse socket number out of argv + if (argc != 2) { + printUsage(); + return -1; + } + + bool socketOn = true; + int port = 0; + port = std::stoi(argv[1]); + if (port == 0) { + socketOn = false; + } + + std::cout << "Port received: " << port << std::endl; + Socket socket; + socket.init("localhost", port); + + std::string reply; + + socket << "trick.var_set_client_tag(\"PoolTableDisplay\") \n"; + socket << "trick.var_add(\"dyn.table.numBalls\")\ntrick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + auto parsed = split(reply, '\t'); + int numBalls = stoi(parsed[1]); + std::cout << "Number of balls received: " << numBalls << std::endl; + + std::string radiusRequest = ""; + char* templateString = "trick.var_add(\"dyn.table.balls[%d][0].radius\")\n"; + for (int i = 0; i < numBalls; i++) { + char buf[64]; + sprintf(buf, templateString, i); + radiusRequest += std::string(buf); + } + + socket << radiusRequest; + socket << "trick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + auto radii = parseTrickResponse(split(reply, '\t')); + + Table table; + + socket << "trick.var_add(\"dyn.table.numTablePoints\") \ntrick.var_add(\"dyn.table.tableShapeType\")\n \ntrick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + std::vector tableData = parseTrickResponse(split(reply, '\t')); + int numTablePoints = tableData[0]; + enum PolygonType tableShape = PolygonType((int)tableData[1]); + + std::string pointRequest = ""; + for (int i = 0; i < numTablePoints; i++) { + templateString = "trick.var_add(\"dyn.table.tableShape[%d][0]._x\")\ntrick.var_add(\"dyn.table.tableShape[%d][0]._y\")\n"; + char buf[256]; + sprintf(buf, templateString, i, i); + pointRequest += std::string(buf); + } + + socket << pointRequest; + socket << "trick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + std::vector tablePoints = parseTrickResponse(split(reply, '\t')); + table.addShape(tablePoints, Eigen::Vector3d(0.2, 0.6, 0.2), true, tableShape, layer_TABLE); + + + // Make the rail - translate each point on the table out from center by railWidth + std::vector railData; + + if (tableShape == RECTANGLE) { + // If it's a rectangle then the rail is a bigger rectangle + double railWidth = 0.07; + railData.push_back(tablePoints[0] - railWidth); + railData.push_back(tablePoints[1] - railWidth); + railData.push_back(tablePoints[2] + railWidth); + railData.push_back(tablePoints[3] + railWidth); + } else { + // If it's just a shape then rail is bigger shape + // Works with simple convex polygons + double railWidth = 0.15; + for (int i = 0; i < tablePoints.size(); i+=2) { + Eigen::Vector2d point(tablePoints[i], tablePoints[i+1]); + Eigen::Vector2d railPoint(tablePoints[i], tablePoints[i+1]); + point *= railWidth; + railPoint = railPoint + point; + railData.push_back(railPoint(0)); + railData.push_back(railPoint(1)); + } + } + + table.addShape(railData, Eigen::Vector3d(.3, .2, .15), true, tableShape, layer_RAIL); + + socket << "trick.var_add(\"dyn.table.numPockets\")\n \ntrick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + int numPockets = stoi(split(reply, '\t')[1]); + for (int i = 0; i < numPockets; i++) { + templateString = "trick.var_add(\"dyn.table.pockets[%d][0].pos._x\")\ntrick.var_add(\"dyn.table.pockets[%d][0].pos._y\")\n\ntrick.var_add(\"dyn.table.pockets[%d][0].radius\")\n"; + char buf[256]; + sprintf(buf, templateString, i, i, i); + std::string pocketRequest = std::string(buf); + + socket << pocketRequest; + socket << "trick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + std::vector pocketData = parseTrickResponse(split(reply, '\t')); + table.addShape(pocketData, Eigen::Vector3d(0.0, 0.0, 0.0), true, CIRCLE, layer_POCKET); + } + + // Bumpers + socket << "trick.var_add(\"dyn.table.numBumpers\")\n \ntrick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + int numBumpers = stoi(split(reply, '\t')[1]); + + std::cout << "Num bumpers: " << numBumpers << std::endl; + + for (int i = 0; i < numBumpers; i++) { + std::string bumperRequests = ""; + + templateString = "trick.var_add(\"dyn.table.bumpers[%d][0].numPoints\")\ntrick.var_add(\"dyn.table.bumpers[%d][0].shapeType\") \n"; + char buf[256]; + sprintf(buf, templateString, i, i); + bumperRequests += std::string(buf); + socket << bumperRequests; + socket << "trick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + std::vector bumperData = parseTrickResponseInt(split(reply, '\t')); + int bumperPoints = bumperData[0]; + enum PolygonType bumperShape = PolygonType((int)bumperData[1]); + + templateString = "trick.var_add(\"dyn.table.bumpers[%d][0].renderedShape[%d][0]._x\")\ntrick.var_add(\"dyn.table.bumpers[%d][0].renderedShape[%d][0]._y\")\n"; + bumperRequests = ""; + for (int j = 0; j < bumperPoints; j++) { + char buf[256]; + sprintf(buf, templateString, i, j, i, j); + bumperRequests += std::string(buf); + } + socket << bumperRequests; + socket << "trick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + std::vector bumperBorder = parseTrickResponse(split(reply, '\t')); + table.addShape(bumperBorder, Eigen::Vector3d(0.2,0.4,0.2), true, bumperShape, layer_BUMPER); + } + + + // socket << "trick.var_pause()\n"; + // Request all of the ball positions + std::string positionRequest = ""; + templateString = "trick.var_add(\"dyn.table.balls[%d][0].pos._x\")\ntrick.var_add(\"dyn.table.balls[%d][0].pos._y\")\n"; + for (int i = 0; i < numBalls; i++) { + char buf[128]; + sprintf(buf, templateString, i, i); + positionRequest += std::string(buf); + } + + socket << positionRequest; + socket << "trick.var_send() \ntrick.var_clear() \n"; + socket >> reply; + + auto positions = parseTrickResponse(split(reply, '\t')); + + + for (int i = 0; i < numBalls; i++) { + std::vector circleData = {positions[(i*2)], positions[(i*2 + 1)], radii[i]}; + Eigen::Vector3d circleColor = ball_colors[i % ball_colors.size()]; + table.addShape(circleData, circleColor, false, CIRCLE, layer_BALL); + } + + bool mousePressed = false; + double mouseX = 0; + double mouseY = 0; + + Eigen::MatrixXd V; + Eigen::MatrixXi F; + Eigen::MatrixXd C; + + std::tie(V, F, C) = table.getMesh(); + + igl::opengl::glfw::Viewer * view = new igl::opengl::glfw::Viewer(); + igl::opengl::glfw::imgui::ImGuiPlugin *plugin = new igl::opengl::glfw::imgui::ImGuiPlugin(); + igl::opengl::glfw::imgui::ImGuiMenu *menu = new igl::opengl::glfw::imgui::ImGuiMenu(); + + view->plugins.push_back(plugin); + plugin->widgets.push_back(menu); + + view->callback_mouse_down = [&](igl::opengl::glfw::Viewer& viewer, int button, int modifier) -> bool { + std::cout << "Mouse down" << std::endl; + mousePressed = true; + return false; + }; + + view->callback_mouse_up = [&] (igl::opengl::glfw::Viewer& viewer, int button, int modifier) -> bool { + mousePressed = false; + std::string cueRequest = ""; + std::string templateString = "dyn.table.applyCueForce(%.3f, %.3f) \n"; + + char buf[128]; + sprintf(buf, templateString.c_str(), mouseX, mouseY); + cueRequest += std::string(buf); + socket << cueRequest; + return false; + }; + + view->callback_mouse_move = [&] (igl::opengl::glfw::Viewer& viewer, int mouse_x, int mouse_y) { + Eigen::Vector3f pos(mouse_x, mouse_y, 0); + Eigen::Matrix4f model = viewer.core().view; + // Viewer is made for 3d, so we have to do math to figure out what the 2d mouse coordinates are + Eigen::Vector3f unproj = igl::unproject(pos, model, viewer.core().proj, viewer.core().viewport); + mouseX = unproj[0]; + mouseY = -unproj[1]; + return true; + }; + + view->callback_pre_draw = [&](igl::opengl::glfw::Viewer& viewer) -> bool { + // Look for new data and redraw + socket >> reply; + std::vector replyData = parseTrickResponse(split(reply, '\t')); + + if (replyData.size() <= 1) { + return false; + } + + table.clearMovingShapes(); + + Eigen::Vector2d cueBallPos; + int cueBallIndex = 0; + + for (int i = 0; i < numBalls; i++) { + double inPlay = replyData[1+(i*3 + 2)]; + if (inPlay == 0) { + continue; + } + std::vector circleData = {replyData[1+(i*3)], replyData[1+(i*3 + 1)], radii[i]}; + Eigen::Vector3d circleColor; + if (i == cueBallIndex) { + circleColor = Eigen::Vector3d(1,1,1); + cueBallPos = Eigen::Vector2d(replyData[1+(i*2)], replyData[1+(i*2 + 1)]); + } else { + circleColor = ball_colors[i % ball_colors.size()]; + } + table.addShape(circleData, circleColor, false, CIRCLE, layer_BALL); + } + + if (mousePressed) { + // Draw the cue + double cue_width = 0.03; + Eigen::Vector2d cue_end(mouseX, mouseY); + Eigen::Vector2d vec = (cue_end - cueBallPos).normalized(); + Eigen::Vector2d off1(-vec(1), vec(0)); + Eigen::Vector2d off2(vec(1), -vec(0)); + Eigen::Vector2d point1 = cue_end + (off1 * cue_width); + Eigen::Vector2d point2 = cue_end + (off2 * cue_width); + std::vector triangleData = {cueBallPos(0), cueBallPos(1), point1(0), point1(1), point2(0), point2(1)}; + table.addShape(triangleData, Eigen::Vector3d(0, 0, 0), false, TRIANGLE, layer_CUE); + } + + std::tie(V, F, C) = table.getMesh(); + + viewer.data().clear(); + viewer.core().orthographic = true; + viewer.data().show_lines = false; + viewer.data().set_face_based(false); + viewer.data().double_sided = true; + viewer.core().is_animating = true; + viewer.core().camera_zoom = 2; + + // Set mesh and colors to new positions + viewer.data().set_mesh(V, F); + viewer.data().set_colors(C); + + return false; + }; + + menu->callback_draw_viewer_menu = [&] () { + ImGui::Text("Menu"); + if (ImGui::Button("Reset Cue Ball", ImVec2(-1, 0))) + { + std::string message = "dyn.table.resetCueBall() \n"; + socket << message; + } + }; + + + // Initial viewer setup + view->core().orthographic = true; + view->core().camera_zoom = 2; + view->data().show_lines = false; + view->data().set_face_based(false); + view->data().double_sided = true; + view->core().is_animating = true; + + // Plot the mesh + view->data().set_mesh(V, F); + view->data().set_colors(C); + + // Viewer is blocking, have to launch it in a separate thread + + // Need to get nBalls and positions every time + socket << "trick.var_pause() \n"; + socket << "trick.var_add(\"dyn.table.numBalls\")\n"; + positionRequest = ""; + templateString = "trick.var_add(\"dyn.table.balls[%d][0].pos._x\")\ntrick.var_add(\"dyn.table.balls[%d][0].pos._y\")\ntrick.var_add(\"dyn.table.balls[%d][0].inPlay\")\n"; + for (int i = 0; i < numBalls; i++) { + char buf[128]; + sprintf(buf, templateString, i, i, i); + positionRequest += std::string(buf); + } + socket << positionRequest; + socket << "trick.var_ascii() \n"; + socket << "trick.var_cycle(0.010) \n"; + socket << "trick.var_unpause() \n"; + + view->launch(); +} diff --git a/trick_sims/SIM_billiards/models/graphics/java/src/PoolTableDisplay.java b/trick_sims/SIM_billiards/models/graphics/java/src/PoolTableDisplay.java new file mode 100644 index 00000000..c7eaddb1 --- /dev/null +++ b/trick_sims/SIM_billiards/models/graphics/java/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 = 8; + 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 = 1.0; + 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 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 \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 diff --git a/trick_sims/SIM_billiards/models/pool_table/include/ball.hh b/trick_sims/SIM_billiards/models/pool_table/include/ball.hh new file mode 100644 index 00000000..9f0e8740 --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/include/ball.hh @@ -0,0 +1,76 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: ( Simulate pool balls. ) +LIBRARY DEPENDENCY: + ((ball.o)) +*******************************************************************************/ +#ifndef _ball_hh_ +#define _ball_hh_ + +// #include +#include "common_geometry.hh" + +// monotonically increasing ID + +class Ball { + + public: + + Ball(double x, double y, double mass, double radius, bool isFixed, int id); + Ball () {} + + void setPos(double x, double y); + void setPos(double x, double y, double z); + + void setVel(double x, double y); + void setVel(double x, double y, double z); + + void setAccel(double x, double y); + void setAccel(double x, double y, double z); + + // void setRelativeVel(double x, double y); + void setRelativeVel(double x, double y, double z); + + // void setAngularVel(double x, double y); + void setAngularVel(double x, double y, double z); + + // void setAngularAccel(double x, double y); + void setAngularAccel(double x, double y, double z); + + void clearAllState(); + + + // Z component should always be 0, unless someone tries to add jumps in the future + // double pos[3]; + // double prevPos[3]; // Maybe don't need this anymore? + // 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]; + Vec pos; + Vec prevPos; + Vec vel; + Vec accel; + Vec relativeVel; + Vec w; + Vec angularAccel; + + // double color[3]; + + double mass; + double radius; + bool fixed; + bool isCue; + int sliding; + + bool inPlay = true; + + unsigned int id; +}; + +// Ball* CreateBall(double x, double y, double mass, double radius, bool isFixed); + +#endif \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/pool_table/include/bumper.hh b/trick_sims/SIM_billiards/models/pool_table/include/bumper.hh new file mode 100644 index 00000000..f84b702b --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/include/bumper.hh @@ -0,0 +1,32 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: ( Pool bumper class. ) +LIBRARY DEPENDENCY: + ((bumper.o)) +*******************************************************************************/ +#ifndef _bumper_hh_ +#define _bumper_hh_ + +#include +#include "common_geometry.hh" + +class Bumper { + public: + // Have to have a default constructor or trick freaks out + Bumper(); + Bumper(int numPoints, double x1, double y1, double x2, double y); + + void AddPointToRender(double x, double y); + void AddBorder (double x1, double y1, double x2, double y2); + int id; + Line border; + Vec ** renderedShape; + unsigned int numPoints; + enum PolygonType shapeType; + + + private: + int nextPointSlot = 0; + +}; + +#endif \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/pool_table/include/common_geometry.hh b/trick_sims/SIM_billiards/models/pool_table/include/common_geometry.hh new file mode 100644 index 00000000..53ca5dee --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/include/common_geometry.hh @@ -0,0 +1,116 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: ( Geometry primitives to be used elsewhere. ) +LIBRARY DEPENDENCY: + (()) +*******************************************************************************/ +#ifndef _common_hh_ +#define _common_hh_ + +#include "math.h" +#include "iostream" + +// Should maybe swap this for eigen stuff at some point +class Vec { + public: + Vec(double x, double y) : _x(x), _y(y), _z(0) {} + Vec(double x, double y, double z) : _x(x), _y(y), _z(z) {} + Vec() : _x(0), _y(0), _z(0) {} + + Vec operator+ (const Vec& other) { + Vec sum; + sum._x = _x + other._x; + sum._y = _y + other._y; + sum._z = _z + other._z; + return sum; + } + + Vec operator- (const Vec& other) { + Vec sum; + sum._x = _x - other._x; + sum._y = _y - other._y; + sum._z = _z - other._z; + return sum; + } + + Vec operator* (double scale) { + Vec ret; + ret._x = _x * scale; + ret._y = _y * scale; + ret._z = _z * scale; + return ret; + } + + double dot(const Vec& other) { + double ret = 0; + ret += _x * other._x; + ret += _y * other._y; + ret += _z * other._z; + + return ret; + } + + // Dot product + double operator* (const Vec& other) { + return dot(other); + } + + double& operator[] (int index) { + if (index == 0) { + return _x; + } else if (index == 1) { + return _y; + } else if (index == 2) { + return _z; + } + + // Throw an error i guess + } + + void setZero() { + _x = 0; + _y = 0; + _z = 0; + } + + double norm() { + return sqrt(_x*_x + _y*_y + _z*_z); + } + + Vec normalized () { + Vec ret(*this); + + return ret * (1.0 / this->norm()); + } + + double& x () { return _x; } + double& y () { return _y; } + double& z () { return _z; } + + double _x; + double _y; + double _z; + + private: + +}; + +class Line { + public: + Vec p1; + Vec p2; + + Line (Vec p1, Vec p2) : p1(p1), p2(p2) {} + Line () {} +}; + +enum PolygonType { + GENERIC, + CIRCLE, + TRIANGLE, + RECTANGLE, + QUAD +}; + + + +#endif \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/pool_table/include/pocket.hh b/trick_sims/SIM_billiards/models/pool_table/include/pocket.hh new file mode 100644 index 00000000..33df29c5 --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/include/pocket.hh @@ -0,0 +1,23 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: ( Pool pocket class. ) +LIBRARY DEPENDENCY: + (()) +*******************************************************************************/ +#ifndef _pocket_hh_ +#define _pocket_hh_ + +#include +#include "common_geometry.hh" + +class Pocket { + public: + // Have to have a default constructor or trick freaks out + Pocket() {} + Pocket(double x, double y, double r) : pos(x,y), radius(r) {} + + Vec pos; + double radius; + +}; + +#endif \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/pool_table/include/pool_table.hh b/trick_sims/SIM_billiards/models/pool_table/include/pool_table.hh new file mode 100644 index 00000000..dd61dee7 --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/include/pool_table.hh @@ -0,0 +1,95 @@ +/************************************************************************ +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 "pocket.hh" +#include + + +class PoolTable { + + public: + PoolTable () : numBalls(0), numAssociations(0) {} + + int default_data(); + int state_init(); + int state_deriv(); + int state_integ(); + double collision(); + // double bumperCollision(); + + int addBall (double x, double y, double mass, double radius, bool fixed); + int addBumper (int numPoints, double x1, double y1, double x2, double y2); + int addPointToBumper(int id, double x, double y); + int addPointToTable(double x, double y); + int addPocket(double x, double y, double r); + + int setBallPos(int id, double x, double y); + int setBallVel(int id, double v_x, double v_y); + + void applyCueForce(double x_end, double y_end); + void applyCueForce(double x_end, double y_end, double cueHorizontalDisplacement, double cueVerticalDisplacement, double cueAngle); + + void resetCueBall(double x, double y); + void resetCueBall(); + + double removeBall(int id); + + + // State variables + Ball** balls; + + // Table parameters + // Bumpers and pockets are used by sim, tableShape is just used by graphics client + Bumper** bumpers; + Pocket** pockets; + Vec** tableShape; + + unsigned int numBumpers; + unsigned int numPockets; + unsigned int numTablePoints; + + int nextBallSlot = 0; + int nextBumperSlot = 0; + int nextPocketSlot = 0; + int nextTablePointSlot = 0; + + enum PolygonType tableShapeType; + + // Ball-ball collisions + unsigned int numBalls; + unsigned int numAssociations; + REGULA_FALSI* ballAssociations; + + // Ball-bumper collisions + unsigned int bumperBallCombos; + REGULA_FALSI* bumperAssociations; + + // Ball-pocket collisions + unsigned int pocketBallCombos; + REGULA_FALSI* pocketAssociations; + + // Sim constants that should be user-controllable + double frictionRolling = 0.05; + double frictionSliding = 0.25; + double frictionScale = 1; + double frictionTolerance = 0.0005; + double coefficientOfElasticity = .95; + double cueForceScale = 0.6; + double cueMass = 1.0; + + int cueBallIndex = 0; + double defaultCueBallX = -0.3; + double defaultCueBallY = 0; + + bool allowCollisions = true; + +}; +#endif \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/pool_table/src/ball.cpp b/trick_sims/SIM_billiards/models/pool_table/src/ball.cpp new file mode 100644 index 00000000..b7a4a081 --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/src/ball.cpp @@ -0,0 +1,62 @@ +/********************************* 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 + +Ball::Ball(double x, double y, double mass, double radius, bool isFixed, int id) : + mass(mass), + radius(radius), + fixed(isFixed), + id(id) +{ + pos = Vec(x, y, 0); +} + +void Ball::setPos(double x, double y) { setPos(x, y, 0); } +void Ball::setPos(double x, double y, double z) { + pos = Vec(x, y, z); +} + +void Ball::setVel(double x, double y) { setVel(x, y, 0); } +void Ball::setVel(double x, double y, double z) { + vel = Vec(x, y, z); +} + +void Ball::setAccel(double x, double y) { setAccel(x, y, 0); } +void Ball::setAccel(double x, double y, double z) { + accel = Vec(x, y, z); +} + +void Ball::setRelativeVel(double x, double y, double z) { + relativeVel = Vec(x, y, z); +} + +void Ball::setAngularVel(double x, double y, double z) { + w = Vec(x, y, z); +} + +void Ball::setAngularAccel(double x, double y, double z) { + angularAccel = Vec(x, y, z); +} + +void Ball::clearAllState() { + setPos(0, 0, 0); + setVel(0, 0, 0); + setAccel(0, 0, 0); + setRelativeVel(0, 0, 0); + setAngularVel(0, 0, 0); + setAngularAccel(0, 0, 0); + sliding = false; +} + + + + diff --git a/trick_sims/SIM_billiards/models/pool_table/src/bumper.cpp b/trick_sims/SIM_billiards/models/pool_table/src/bumper.cpp new file mode 100644 index 00000000..435e9c79 --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/src/bumper.cpp @@ -0,0 +1,25 @@ +#include "bumper.hh" +#include "stdlib.h" + +#include "trick/memorymanager_c_intf.h" + +Bumper::Bumper() {} + +Bumper::Bumper(int numPoints, double x1, double y1, double x2, double y2) : + border(Line(Vec(x1, y1), Vec(x2, y2))), + numPoints(numPoints) +{ +} + + +void Bumper::AddPointToRender(double x, double y) { + int id = nextPointSlot++; + if (id < numPoints) { + Vec * point = (Vec*) TMM_declare_var_s("Vec"); + renderedShape[id] = (new (point) Vec(x, y)); + } +} + +void Bumper::AddBorder (double x1, double y1, double x2, double y2) { + border = Line(Vec(x1, y1), Vec(x2, y2)); +} \ No newline at end of file diff --git a/trick_sims/SIM_billiards/models/pool_table/src/pocket.cpp b/trick_sims/SIM_billiards/models/pool_table/src/pocket.cpp new file mode 100644 index 00000000..e69de29b diff --git a/trick_sims/SIM_billiards/models/pool_table/src/pool_table.cpp b/trick_sims/SIM_billiards/models/pool_table/src/pool_table.cpp new file mode 100644 index 00000000..dc82bd19 --- /dev/null +++ b/trick_sims/SIM_billiards/models/pool_table/src/pool_table.cpp @@ -0,0 +1,553 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: (Simulate a pool table.) +LIBRARY DEPENDENCY: + ((pool_table.o) + (ball.o)) +*******************************************************************************/ +#include +#include +#include "trick/integrator_c_intf.h" +#include "trick/memorymanager_c_intf.h" +#include "../include/pool_table.hh" +#include "trick/trick_math_proto.h" + + +// Leaving these here, but should probably go somewhere else +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; + } +} + +void mProduct (double product[2], double matrix[2][2], double vec[2]) { + product[0] = matrix[0][0] * vec[0] + matrix[1][0] * vec[1]; + product[1] = matrix[0][1] * vec[0] + matrix[1][1] * vec[1]; +} + +int PoolTable::default_data() { + // balls.clear(); + // bumpers.clear(); + + 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 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 unless very close to 0 + // Has weird behavior when velocity is very small, so only apply friction if velocity is greater than a tolerance + + if (!balls[i]->inPlay) { + continue; + } + + if (balls[i]->vel.norm() > frictionTolerance) { + Vec velocityNorm = balls[i]->vel.normalized(); + + 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++) { + if (!balls[i]->inPlay) { + continue; + } + + 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 + 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]); + load_indexed_deriv(n*i + inner_index++, balls[i]->accel[1]); + load_indexed_deriv(n*i + inner_index++, balls[i]->accel[2]); + + } + + int integration_step = integrate(); + + for (int i = 0; i < numBalls; i++) { + if (!balls[i]->inPlay) { + continue; + } + + // pos[0] pos[1] pos[2] vel[0] vel[1] vel[2] + 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++); + + balls[i]->accel[0] = 0; + balls[i]->accel[1] = 0; + balls[i]->accel[2] = 0; + + } + + return integration_step; +} + +double closestPointOnLine(Line& line, Vec pos, Vec& result, bool print) { + Vec a(line.p1); + Vec b(line.p2); + Vec diff = pos - a; + Vec m = b - a; + + double t = (diff * m) / (m * m); + + if (t < 0) + t = 0; + + if (t > 1) + t = 1; + + m = m * t; + result = a + m; + + return t; +} + +double PoolTable::removeBall(int id) { + balls[id]->inPlay = false; + balls[id]->clearAllState(); +} + + +double PoolTable::collision() { + + // Handle when the balls collide with others + + double now ; /* current integration time. */ + double event_tgo; + unsigned int ii,jj; + + now = get_integ_time() ; + event_tgo = BIG_TGO; + + std::vector collisionsToProcess; + + for (ii=1; iiinPlay) { + continue; + } + for (jj=0; jjinPlay) { + continue; + } + + unsigned int associationIndex = ii*(ii-1)/2+jj; + + // boundary is distance between balls - radiuses of balls + double distanceBetweenBalls = (balls[ii]->pos - balls[jj]->pos).norm(); + double error = distanceBetweenBalls - (balls[ii]->radius + balls[jj]->radius); + ballAssociations[associationIndex].error = error; + + double this_tgo = regula_falsi( now, &(ballAssociations[associationIndex])) ; + + if (this_tgo < event_tgo) { + event_tgo = this_tgo; + } + + if (this_tgo == 0) { + std::cout << "Found colliding balls" << std::endl; + + // Add this collision to a list of collisions to process + // probably don't need to do this + collisionsToProcess.push_back(ii); + collisionsToProcess.push_back(jj); + reset_regula_falsi( now, &(ballAssociations[associationIndex]) ); + + } else { + regula_falsi_set_upper (now, error, &ballAssociations[associationIndex]); + } + } + } + + // Handle collisions + for (int i = 0; i < collisionsToProcess.size(); i+=2) { + int index1 = collisionsToProcess[i]; + int index2 = collisionsToProcess[i+1]; + std::cout << "Collision detected between balls " << index1 << " and " << index2 << std::endl; + + Vec q1(balls[index1]->pos); + Vec q2(balls[index2]->pos); + + // dg = (q1 - q2) / (|q1 - q2|) + Vec diff = q1 - q2; + Vec dg = diff.normalized(); + + // 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(dg4, impulse, 4); + + // Impulse[0:1] is x and y components of what should be applied to v1, Impulse[2:3] goes to v2 + Vec impulse1(dg4[0], dg4[1], 0); + Vec impulse2(dg4[2], dg4[3], 0); + + Vec newV1(balls[index1]->vel); + Vec newV2(balls[index2]->vel); + + newV1 = ((newV1 * balls[index1]->mass) - impulse1) * (1.0 / balls[index1]->mass); + newV2 = ((newV2 * balls[index2]->mass) - impulse2) * (1.0 / balls[index2]->mass); + + std::cout << "Impulses applied: \n\tV1: " << newV1[0] << " " << newV1[1] << " \n\tV2: " << newV2[0] << " " << newV2[1] << std::endl; + + balls[index1]->vel[0] = newV1[0]; + balls[index1]->vel[1] = newV1[1]; + + balls[index2]->vel[0] = newV2[0]; + balls[index2]->vel[1] = newV2[1]; + } + + int numBumperCollisions = 0; + for (ii=0; iiinPlay) { + continue; + } + for (jj=0; jjborder, ball->pos, closestPointOnBumper, true); + double distanceToBumper = (ball->pos - closestPointOnBumper).norm() - ball->radius; + bumperAssociations[association_index].error = distanceToBumper; + + double this_tgo = regula_falsi( now, &(bumperAssociations[association_index])) ; + if (this_tgo < event_tgo) { + event_tgo = this_tgo; + } + + if (this_tgo == 0) { + numBumperCollisions++; + std::cout << "Found colliding ball " << ii << " and bumper " << jj << std::endl; + reset_regula_falsi( now, &(bumperAssociations[association_index]) ); + + if (numBumperCollisions > 1) { + // When a ball hits exactly a corner of 2 bumpers, both collisions will be detected and impulses applied. + // But since an impulse takes into consideration the original velocity, the second impulse applied + // just cancels out the first one most of the time. Both impulses should be identical since the + // collision point is the same, so make sure only 1 gets applied. This is kind of hacky + continue; + } + + Vec q1(ball->pos); + Vec q2(closestPointOnBumper); + + Vec diff = q1 - q2; + Vec dg = diff.normalized(); + + // 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] = {ball->vel[0], ball->vel[1], 0, 0}; + + // Calculate the impulse + // J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg + // Really need to get some better matrix math in here + // only used the dot product of the first 2 in the last term since it's supposed to be dg * minv * dg + // and since the bumpers are immovable the mass is infinite which inverse is 0 + // So this isn't robust at all + // Setting the CoE to be 1 always - perfectly elastic + // Otherwise there's a weird bouncy thing + double impulse = ((1.0 + 1.0) * dot(dg4, vel4, 4)) / (dot(dg4, dg4, 2)); + std::cout << "Impulse: " << impulse << std::endl; + scaleInPlace(dg4, impulse, 4); + + // Impulse[0:1] is x and y components of what should be applied to v1, Impulse[2:3] goes to v2 + Vec impulse1(dg4[0], dg4[1], 0); + Vec impulse2(dg4[2], dg4[3], 0); + + + Vec newV1 (ball->vel); + + newV1 = ((newV1 * ball->mass) - impulse1) * (1.0 / ball->mass); + + std::cout << "Impulses applied: \n\tV1: " << newV1[0] << " " << newV1[1] << std::endl; + + ball->vel[0] = newV1[0]; + ball->vel[1] = newV1[1]; + + } else { + regula_falsi_set_upper (now, distanceToBumper, &bumperAssociations[association_index]); + } + } + + } + if (numBumperCollisions > 0) { + std::cout << "In this time step, " << numBumperCollisions << " collisions applied" << std::endl; + } + + // Pockets + for (ii=0; iiinPlay) { + continue; + } + for (jj=0; jjpos - pocket->pos).norm() - (pocket->radius); + pocketAssociations[association_index].error = error; + + double this_tgo = regula_falsi( now, &(pocketAssociations[association_index])); + + if (this_tgo < event_tgo) { + event_tgo = this_tgo; + } + + if (this_tgo == 0) { + std::cout << "Found pocketed ball " << ii << std::endl; + reset_regula_falsi( now, &(pocketAssociations[association_index]) ); + removeBall(ii); + } + } + } + + return event_tgo; +} + +void PoolTable::resetCueBall() { + resetCueBall(defaultCueBallX, defaultCueBallY); +} + +void PoolTable::resetCueBall(double x, double y) { + balls[cueBallIndex]->clearAllState(); + balls[cueBallIndex]->setPos(x, y); + + balls[cueBallIndex]->inPlay = true; +} + +void PoolTable::applyCueForce(double x_end, double y_end) { + applyCueForce(x_end, y_end, 0, 0, 0); +} + +void PoolTable::applyCueForce(double x_end, double y_end, double cueHorizontalDisplacement, double cueVerticalDisplacement, double cueAngle) { + std::cout << "Applying cue force" << std::endl; + + // assume index 0 is cue ball + int cueIndex = 0; + Vec cueEnd(x_end, y_end); + Vec cueBallPos(balls[cueIndex]->pos); + + double r = balls[cueIndex]->radius; + double m = balls[cueIndex]->mass; + + double a = cueHorizontalDisplacement * r; + double b = cueVerticalDisplacement * r; + double c = abs(sqrt(pow(r,2) - pow(a,2) - pow(b,2))); + + double theta = cueAngle * M_PI / 180.0; + double angleScaling = 1.0 + (cueAngle / 25.0); + + Vec cueAxis = cueEnd - cueBallPos; + double cue_v = cueAxis.norm() * cueForceScale * angleScaling; + cueAxis = cueAxis.normalized(); + + double axis2[2] = {cueAxis[0], cueAxis[1]}; + + double force = (2 * m * cue_v) / (1.0 * (m / cueMass) + (5.0 / (2.0*pow(r, 2))) * (pow(a,2) + pow(b,2)*pow(cos(theta),2) + pow(c,2)*pow(sin(theta),2) - 2*b*c*sin(theta)*cos(theta))); + + // Velocity in the frame of the cue + double v_multiplier = -force/m * cos(theta); + Vec velocityAfterHit = cueAxis * v_multiplier; + + std::cout << "Velocity of ball after cue hit: " << velocityAfterHit[0] << " " << velocityAfterHit[1] << std::endl; + + balls[cueIndex]->vel = velocityAfterHit; + + // TODO: logic for angular and relative velocity + +} + + +// 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 (int numPoints, double x1, double y1, double x2, double y2) { + int id = nextBumperSlot++; + if (id < numBumpers) { + Bumper * bumper = (Bumper*) TMM_declare_var_s("Bumper"); + bumpers[id] = (new (bumper) Bumper(numPoints, x1, y1, x2, y2)); + return id; + } + return -1; +} + +int PoolTable::addPointToBumper(int id, double x, double y) { + Bumper * bumper = bumpers[id]; + bumper->AddPointToRender(x, y); + + return 0; +} + +int PoolTable::addPointToTable(double x, double y) { + int id = nextTablePointSlot++; + if (id < numTablePoints) { + Vec * point = (Vec*) TMM_declare_var_s("Vec"); + tableShape[id] = (new (point) Vec(x, y)); + return id; + } + return -1; +} + +int PoolTable::addPocket(double x, double y, double r) { + int id = nextPocketSlot++; + if (id < numPockets) { + Pocket * pocket = (Pocket*) TMM_declare_var_s("Pocket"); + pockets[id] = (new (pocket) Pocket(x, y, r)); + return id; + } + 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; +} + + +