From 8537601ccbbee1f862c3793d006097f8d37e400d Mon Sep 17 00:00:00 2001 From: Deans Date: Thu, 14 Jul 2022 09:17:00 -0500 Subject: [PATCH] Saving working state --- trick_sims/SIM_pool/Modified_data/realtime.py | 2 +- trick_sims/SIM_pool/RUN_break/input.py | 159 +++++++ trick_sims/SIM_pool/RUN_hexagon/input.py | 119 +++++ trick_sims/SIM_pool/RUN_test/input.py | 145 +++++- trick_sims/SIM_pool/S_define | 3 +- .../SIM_pool/models/graphics/java/Makefile | 36 ++ .../graphics/java/dist/PoolTableDisplay.jar | Bin 0 -> 6493 bytes .../SIM_pool/models/graphics/java/manifest | 1 + .../{ => java}/src/PoolTableDisplay.java | 10 +- .../models/graphics/libigl-example-project | 1 + .../models/pool_table/include/ball.hh | 54 ++- .../models/pool_table/include/bumper.hh | 38 +- .../pool_table/include/common_geometry.hh | 77 ++++ .../models/pool_table/include/pocket.hh | 24 + .../models/pool_table/include/pool_table.hh | 87 +++- .../SIM_pool/models/pool_table/src/ball.cpp | 53 ++- .../SIM_pool/models/pool_table/src/bumper.cpp | 20 +- .../SIM_pool/models/pool_table/src/pocket.cpp | 0 .../models/pool_table/src/pool_table.cpp | 429 ++++++++++++++++-- 19 files changed, 1130 insertions(+), 128 deletions(-) create mode 100644 trick_sims/SIM_pool/RUN_break/input.py create mode 100644 trick_sims/SIM_pool/RUN_hexagon/input.py create mode 100644 trick_sims/SIM_pool/models/graphics/java/Makefile create mode 100644 trick_sims/SIM_pool/models/graphics/java/dist/PoolTableDisplay.jar create mode 100644 trick_sims/SIM_pool/models/graphics/java/manifest rename trick_sims/SIM_pool/models/graphics/{ => java}/src/PoolTableDisplay.java (97%) create mode 160000 trick_sims/SIM_pool/models/graphics/libigl-example-project create mode 100644 trick_sims/SIM_pool/models/pool_table/include/common_geometry.hh create mode 100644 trick_sims/SIM_pool/models/pool_table/include/pocket.hh create mode 100644 trick_sims/SIM_pool/models/pool_table/src/pocket.cpp diff --git a/trick_sims/SIM_pool/Modified_data/realtime.py b/trick_sims/SIM_pool/Modified_data/realtime.py index acf31873..31546eb5 100644 --- a/trick_sims/SIM_pool/Modified_data/realtime.py +++ b/trick_sims/SIM_pool/Modified_data/realtime.py @@ -1,6 +1,6 @@ trick.real_time_enable() -trick.exec_set_software_frame(0.1) +trick.exec_set_software_frame(0.01) trick.itimer_enable() trick.exec_set_enable_freeze(True) diff --git a/trick_sims/SIM_pool/RUN_break/input.py b/trick_sims/SIM_pool/RUN_break/input.py new file mode 100644 index 00000000..6624fb96 --- /dev/null +++ b/trick_sims/SIM_pool/RUN_break/input.py @@ -0,0 +1,159 @@ +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("Point*", 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] + +dyn.table.numBumpers = len(bumperBorders) +dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers) + +for i in range(len(bumperBorders)): + id = dyn.table.addBumper(len(bumperShapes[i])/2, bumperBorders[i][0], bumperBorders[i][1], bumperBorders[i][2],bumperBorders[i][3]) + dyn.table.bumpers[id][0].numPoints = len(bumperShapes[i])/2 + dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Point*", dyn.table.bumpers[id].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/libigl-example-project/build/example" + + +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. Please \"cd\" into ../models/graphics/java and type \"make\".') + 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_pool/RUN_hexagon/input.py b/trick_sims/SIM_pool/RUN_hexagon/input.py new file mode 100644 index 00000000..2f2e0789 --- /dev/null +++ b/trick_sims/SIM_pool/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("Point*", 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("Point*", 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/libigl-example-project/build/example" + + +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. Please \"cd\" into ../models/graphics/java and type \"make\".') + 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_pool/RUN_test/input.py b/trick_sims/SIM_pool/RUN_test/input.py index cee34baa..65911c38 100644 --- a/trick_sims/SIM_pool/RUN_test/input.py +++ b/trick_sims/SIM_pool/RUN_test/input.py @@ -1,14 +1,123 @@ - +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) -id1 = dyn.table.addBall(-2, .5, 1, 1, False); -id2 = dyn.table.addBall(1, .5, 1, 1, False); +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("Point*", 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] + +dyn.table.numBumpers = len(bumperBorders) +dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers) + +for i in range(len(bumperBorders)): + id = dyn.table.addBumper(len(bumperShapes[i])/2, bumperBorders[i][0], bumperBorders[i][1], bumperBorders[i][2],bumperBorders[i][3]) + dyn.table.bumpers[id][0].numPoints = len(bumperShapes[i])/2 + dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Point*", dyn.table.bumpers[id].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.table.setBallVel(id1, 0.5, 0); dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls) @@ -16,15 +125,31 @@ dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls) # Start the Graphics Client #========================================== varServerPort = trick.var_server_get_port(); -PoolTableDisplay_path = "models/graphics/dist/PoolTableDisplay.jar" + +# This will definitely change to something else +PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example" + if (os.path.isfile(PoolTableDisplay_path)) : - PoolTableDisplay_cmd = "java -jar " \ - + PoolTableDisplay_path \ + PoolTableDisplay_cmd = PoolTableDisplay_path \ + " " + str(varServerPort) + " &" ; print(PoolTableDisplay_cmd) os.system( PoolTableDisplay_cmd); else : - print('============================================================================================') - print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".') - print('============================================================================================') + print('=================================================================================================') + print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".') + 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_pool/S_define b/trick_sims/SIM_pool/S_define index a7548bda..ea1d0f55 100644 --- a/trick_sims/SIM_pool/S_define +++ b/trick_sims/SIM_pool/S_define @@ -17,8 +17,9 @@ class PoolTableSimObject : public Trick::SimObject { ("derivative") table.state_deriv() ; ("integration") trick_ret = table.state_integ() ; ("dynamic_event") table.collision() ; + // ("dynamic_event") table.bumperCollision() ; } }; PoolTableSimObject dyn; -IntegLoop dyn_integloop(0.1) dyn; +IntegLoop dyn_integloop(0.001) dyn; diff --git a/trick_sims/SIM_pool/models/graphics/java/Makefile b/trick_sims/SIM_pool/models/graphics/java/Makefile new file mode 100644 index 00000000..55e5e663 --- /dev/null +++ b/trick_sims/SIM_pool/models/graphics/java/Makefile @@ -0,0 +1,36 @@ +SHELL = /bin/sh + +PROJECT_NAME = PoolTableDisplay +SRC_DIR = src +BUILD_DIR = build +CLASSES_DIR = $(BUILD_DIR)/classes +JAR_DIR = dist +MAIN_CLASS = PoolTableDisplay + +all: jar + +clean: + rm -rf $(BUILD_DIR) + rm -f manifest + +spotless: clean + rm -rf dist + +$(CLASSES_DIR): + @ mkdir -p $(CLASSES_DIR) + +compile: | $(CLASSES_DIR) + javac -sourcepath $(SRC_DIR) -d $(CLASSES_DIR) $(SRC_DIR)/PoolTableDisplay.java + +manifest: + @ echo "Main-Class: $(MAIN_CLASS)" > $@ + +$(JAR_DIR): + @ mkdir -p $(JAR_DIR) + +jar: compile manifest | $(JAR_DIR) + jar cvfm $(JAR_DIR)/$(PROJECT_NAME).jar manifest -C $(CLASSES_DIR) . + @ echo "-------------------------------------------------------------------------------" + @ echo " BUILD COMPLETE" + @ echo "The Java jar file (the Java Executable) is located at: $(JAR_DIR)/$(PROJECT_NAME).jar" + @ echo "-------------------------------------------------------------------------------" diff --git a/trick_sims/SIM_pool/models/graphics/java/dist/PoolTableDisplay.jar b/trick_sims/SIM_pool/models/graphics/java/dist/PoolTableDisplay.jar new file mode 100644 index 0000000000000000000000000000000000000000..9de0aeaa35fddc5f4ed8ba97f4cca20db4ffc6e3 GIT binary patch literal 6493 zcmaKw1yCGao3?QcKDbNJVQ>vj7<6!l;2zvP!EG4aJrLa8gA?2W1ed`rKtdpaz@O~y z|Gn?NyWiejU3I$p+})?Tp1S)vm%1`C3IG8E0|P(u7uW9Q4)(rSP<6mZow5lRlPD>jE0e^)|PN*nzg643PIoYQtXPPv* zKk;qvyMvSD z*iiNk55t+5_e(mhNJ#gXbg$j7+!6m{ecVn(xF)~f{^$Jn>%;neSH|4Y5oGCT?%`pb z9Hh3O4j3tzFj<}IBnHT#6Un)1e>QtpuHp)fjq@TCrH_AHaEfPR<5-CsA8oXv=b6LX zj^fStXc{8RS>Nh7+~2&<4Y)tOy)8fxtWOyMq-$mlA%jC2Cb`M*$T^@osKWqFR-PrY zQz$Q-z%uMgzn=&-2dU!XvY7^jVAH`rVGB12kn$q|#u%0%i( z@_Fz4=VCWd;#mjv8-;+%;L%gu2gFhK;AL7z>>=!%eMa7vdN`&P=Z8~)$TH5lWltkS zxlc+j$TCK-jI&w?&Fjv4u0>w_0zE@E=s3KcLAve!sKme5P-u?3EU-S{gd;Knw4@N? zH--;ECNZoYiEDS*Z1}}2#cUmU*rB9QCljJAuduh->B`7$_Z6PAqF1Y)@-gt`n^MIo zky2}FDvyer{D;p)ih4!eH5WPYHfbl{@gPmo-piFsn`ZeY4zZg$A>;FQrH!hMp*l*+ zP6|1cYbyTcP|KVNqGo=#SctEu!fwiU=9yXj$LX-Fnm)8waZ0)1ec#PaPVO6mK;x)yW zvyLp}JC*0G1iY=q;E$`Y{8X4K&qlA*^_OG&3Xv$38)jEi@0L6CKUFQb!#V$mh=6eY zyR`p()e!!!8d(=-Pj?qbb#rIy|EeA@9c3*_dHhE-OkG2gMASyiZK>!yjH|>Hvt5MX zkOoc$DKMG5Fdp7K1&MB#jN#SC=TX}4ud$8O#_2nLB&KLxX%g%0CmRJz1%oeo+vmjz zW1i{1KhHmYY&|^9&tLgq^6L@(t(&&zH@GI53mh4T_5@>_F^?He0gB=-Dn_Mk`ihkx zfoYz61BkPae1)?^(N!1aDmx09JjxvAB=Hb+hU3O{nU`?a?PyVNv;$|T==&>QjQ_H-o^cmU{wlz? z^K9gOE~EZL^S=J3K*)Dr+4drxknL(CZdB3vbW4zK+^LP9TB99fIkw1LPHLue(S5Vq$!$`9#LH>F%UVaN zRg4V&7HxAd2>FXyB7XewWc$6n-?pwexSGQE?F@>jnxZ~kGx!HCaIsJX`O9u-h=3Si zww;iD z#@Dtbma9l}%hy$uGX;is%{o^qQOj{uwhZ{uUtyj?XXJH^_Bj(;Y~B$ZrFtB7QZC5V zHb#lCbvjl?OD`AMKx;`x> zxGAs$cG9u{MG|*$Rx|W~4`si)DoTmb;Bp!tw>j5WC(!ytaiFTRnr0cHZ@9(#b;vqi zspH|v4`)tvrF+jCPv?;m4PJQ=fA1-UTEvW1j#HioFWG128ya8)l>!MfNDSDsCcVBj ziwm1q3emCbbjat;`YG8tJQ2<~q9c2{+{h;47RIR2Z$v=?-=_BJFNaPq-ZR1Z?}?_Y z5Wf>f;m7P8+9`EQ(PdyzF&q_VF>`j5dq>ZDgcE(3gUi}dJKdO`-m<4#p)FH-@o(;j zV+kbzV3D+1T6}C3l!AAbsaf;Q&xHw~sEekwI{}yHLQ>&+9dVRg)F7u9t z(`ycg#q`$I(IXX8)w1tl=TPWdEpS=Ei}U60Z;lFP@mdZY8SV;$KZ@Mu2V97J_}aO$ z_4sRF;_dm}WQ3?^$M#?_0eW@K&el_XtIn&gi%?X0mKeNP;UTfJP~<4T#i>$_rZ5f88>3Wh6#cjdVdhPt*M*7TlNPme zL&LEd)eQnt`(alN%ofJspuTk2ST=(c1|}8q3~Wu`JBY zWE;eJtNGbo@yKlT8+d|Cv6ZvJQgbuH)k|O68ilQ1y~RZp4(cB=MV*LIF;B&O6M-lI5vyyZva!|J=z zi_up}%WX)3ALz@#pY%H$ibYaox1lUAj*=@`P^t~&nmU}NUh?WuA`Y_|Eb{g5`Lb7} zP$JeU@V*{O1*M7m`KKsOp{y5`YGsS{#7`;Oy;K?iot54vlqDvpD%thv#>RLs?Z|Zx z&2JDslxLV-F7kiW`oS)ZQEr_6oz35vhZTk#&do_5XOHQ!12B5Qx#P6iJGkcK1|A5o zOTUyzaXMq}+XLD#<;sEw{2bF<5os0LKkgBx^wY`~|C!dV8l_A-K zWVg-gsyVXXHkKg?PN)bp-FJ3&`qERx8sIxbnCmR~Gh21CXu9k>82Oj;iGJSPpmo5D zT4gM9&K|F_f+zAqdMM_-hA|x6MQajwvM-;Q+==Y94m;6T2jySrL2G;(RncEQyF4CI5ka`&$DWEo>y7xKkZd4dJL-tB&z*{Z>KG=|m2 z!|Jta-7~CY4#yGI#fKY_NHmIu#Fu^9fkwf!+#P*`%Im6C;BFPq7w4e4O{MO|4;cMl zUP*<#!Avh-J8A8xiL0NH{ie4SW=W%}{2FqDqy6(spFtCfe6vl`W(Ikj>(~CRHi+bl zXy-ybYO?1!maZ~`qRlPB)Nh; zYi4ZhEy-2lcGEAa=|PtS!+O3GlgqHcR;h&KJ}woLmWYeBXg!^PYC+CTP0PlmpALq; z)!3|^J%Jle%(p=4>>|k)55S&Zs3Mojbn2|~v3v4}>CK0Ccb9}p?>t!ZWLFs0H;fL| zC8A2~LRj(?LcgA+4|Nx1hgmMfK%`cqimdcs`*4B)t94 z;(gR1O3elVA&lMiz*_RSDZ!ZL8vk9hU-}lUV>>RF(KP4s(~i)m{a zp>_Df%L?0+x&3WSzi_`Y84cjw5lLn*es%xyO{!#*;3_uSo8bN$!VmVtk(dOv1l7i1 z{kn)6=br{iI0m=-x4CRx?DuMC2UyCc*&tv8l;P z4asXrArd1Zrd`J#PBZ7olS({De82xLG5vc#a?xQ+WXp*7wUJrz?M1vznF%YHKjZ!v8DqpKQHOA9ujn-YY!&okv(EA0r>_r8f zbRC}S zQr72TY#2~_k!vMtGNhc_rjsMA=kGU#WpC2Ip3AY&O!EKi3fP5?`2M1w=00O7&H9A zvl}Ml=?DmNAMofFH}Pq1 zu4GhuqXY@a8yKg1`n?x432fvWU>Kom2)F-Cpm*}-5-sO&`O36gj&c)1mOz(WDC^p{K1LT@jR+W?TBjYW%v!4!YM^M}F5ULfPjvC6Do$_Phw{2eC zVLq6|v~wZ1m%X9pd3kgZOnG8VO|+IJY>X*_sK@fH+}7{?+&Ld4g%9?HN9+aX8+^se z{6>Tq?myot$lbWTN^|^$czxYMGia0S7vOcBRNbT{R=)dL8B2Q&Q+l!=tTu;AEHP~3 z7VOQupP2{8(4HO?a2w?uN6p95_)N@fALl{}=yur2VbRyi!OU;VehuuUV9Xs6EGz9l zQ8Z9geVx&kDnc;@VoY(RAk^yF7C*6}enkHOb(jLjC6@gI^e{TzjtL6gncZ zxnENm;Cg2#?NMYmjSPx_qe)<9^mOPfytN-(x7?Rn1@mED1XKl-LH46oPoaLax-mlH zUyKzIxeMCvFIir?JMzxwxc8t5(X3P6O>&YmCgrDcbOdy_Ujrp1nuMl7q-rW%&Kl1b zQ_agDo5510m{(x-;pKdFo#<`4Sp`HJ`ElV436vVqr6YkX8j;LIwq_^qb)M6Es+7F1 z-p_fDA(JClM*wk>uy$tbQ5eWdzn`PhQGk%VsYTgSg$PQC8GUrp3G5m*t6%{nu4>Tr zMSSz!csKM@)NqJ{yFr-WkX(o*cKvh01yReAh zbhI7$hW);5V>p))KYqKEBBZ9OW10hX@cFzD3y^--55IEUtg%(v?cfzfh@Cf4n0w2D zEn;|vNxG0@;9Hy;!F4hr6F+Wn=flU-zuiE^o`9U_M)|Owpf-#zvmQG$l9w446uk{9 z-9yjN!PcR^J6McVZ?fa~%-N+z!W z*)$;k%*yrvxOm<_(EuB{3OBN&VVpd06yF-l`I2dTi z+t1>nzR7>GNRDpa&6wFz3%P~fX4I{BKJIXM56Zuc(jdcO^k8%<%CRT?;^9%;Y`n_$ zDl9UoBgrGt1!?Lf(cxB;@kl;=;!%wB1L>W2ceF)!IN*(Y$rW$w70amiBZhFn2Sm_; z#3P||57%MsJyv%_)uo`f`&xkbrktctvyhwU7ka#W2J-u{IMT5wy|I~%T|jGfW-P1J zLPfY!uC%#HnpPrvd1|FS3FkXr(Y!-1{v5&C9f0i_iD>Nf`{HAy*ja0BMWM?;KAkyG zwsO}o*{ba5yhOG<-fWMZ9ua3DQ1P8#vJ&=tk!A(a{6jfymOPcX_xqm7pOhv_ z_$o|R6>#SLKC!+UJI!mB>5R-Sd3_8x_7~MzmE#nSVr-%=GRKY9>$gG0^hP|hO4CjX;TsXX-N^000{pb>HmLT{|D*+cm0de{{j5} zwEi8X{{i}cN&NTlU)I0j{eM2;?^yK@a{o&kzlVPT_<#ET4p06-*S|!B{J$aCfBOI4 yqyIrHf5{sCe*l+%n*aVa|FrRcNdo8JHTCMssJ}T60s`9a!}~W!o+JHp_5T29KzCdK literal 0 HcmV?d00001 diff --git a/trick_sims/SIM_pool/models/graphics/java/manifest b/trick_sims/SIM_pool/models/graphics/java/manifest new file mode 100644 index 00000000..5c286ff4 --- /dev/null +++ b/trick_sims/SIM_pool/models/graphics/java/manifest @@ -0,0 +1 @@ +Main-Class: PoolTableDisplay diff --git a/trick_sims/SIM_pool/models/graphics/src/PoolTableDisplay.java b/trick_sims/SIM_pool/models/graphics/java/src/PoolTableDisplay.java similarity index 97% rename from trick_sims/SIM_pool/models/graphics/src/PoolTableDisplay.java rename to trick_sims/SIM_pool/models/graphics/java/src/PoolTableDisplay.java index 573ffe7d..c7eaddb1 100644 --- a/trick_sims/SIM_pool/models/graphics/src/PoolTableDisplay.java +++ b/trick_sims/SIM_pool/models/graphics/java/src/PoolTableDisplay.java @@ -24,7 +24,7 @@ import javax.swing.JPanel; class Ball { - static int numColors = 9; + static int numColors = 8; static Color[] colorList = { Color.WHITE, Color.YELLOW, @@ -45,7 +45,7 @@ class Ball { identity = id; x = 0.0; y = 0.0; - radius = 0.5; + radius = 1.0; color = colorList[id % numColors]; } } @@ -159,8 +159,8 @@ class RangeView extends JPanel { 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.setPaint(Color.BLACK); + // g2d.drawString ( String.format("%d",ii), bx,by); } g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20); @@ -294,7 +294,7 @@ public class PoolTableDisplay extends JFrame { line = poolTableDisplay.in.readLine(); field = line.split("\t"); for ( ii=0; ii < nballs; ii++) { - // poolTableDisplay.rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]); + poolTableDisplay.rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]); } } catch (IOException | NullPointerException e ) { go = false; diff --git a/trick_sims/SIM_pool/models/graphics/libigl-example-project b/trick_sims/SIM_pool/models/graphics/libigl-example-project new file mode 160000 index 00000000..a9b087f9 --- /dev/null +++ b/trick_sims/SIM_pool/models/graphics/libigl-example-project @@ -0,0 +1 @@ +Subproject commit a9b087f9d8e9cce32ab68705128164cd5d4ad81a diff --git a/trick_sims/SIM_pool/models/pool_table/include/ball.hh b/trick_sims/SIM_pool/models/pool_table/include/ball.hh index 55f99791..9f0e8740 100644 --- a/trick_sims/SIM_pool/models/pool_table/include/ball.hh +++ b/trick_sims/SIM_pool/models/pool_table/include/ball.hh @@ -7,9 +7,9 @@ LIBRARY DEPENDENCY: #define _ball_hh_ // #include +#include "common_geometry.hh" // monotonically increasing ID -static int id = 0; class Ball { @@ -17,20 +17,48 @@ class Ball { 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]; - double vel[3]; - // Used to store derivatives between deriv and integration steps - double accel[3]; + // 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]; + // // 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 color[3]; double mass; double radius; @@ -38,6 +66,8 @@ class Ball { bool isCue; int sliding; + bool inPlay = true; + unsigned int id; }; diff --git a/trick_sims/SIM_pool/models/pool_table/include/bumper.hh b/trick_sims/SIM_pool/models/pool_table/include/bumper.hh index f90eb7ab..f84b702b 100644 --- a/trick_sims/SIM_pool/models/pool_table/include/bumper.hh +++ b/trick_sims/SIM_pool/models/pool_table/include/bumper.hh @@ -7,39 +7,25 @@ LIBRARY DEPENDENCY: #define _bumper_hh_ #include - -// this should definitely go somewhere else -// maybe make a geometry libary -class Point { - public: - double x; - double y; - - Point(double x, double y) : x(x), y(y) {} - Point() {} -}; - -class Line { - public: - Point p1; - Point p2; - - Line (Point p1, Point p2) : p1(p1), p2(p2) {} - Line () {} -}; +#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: - // Actual line that can be collided with - Line border; - - // Shape that should be rendered - // Size should be dynamic - std::vector renderedShape; + int nextPointSlot = 0; }; diff --git a/trick_sims/SIM_pool/models/pool_table/include/common_geometry.hh b/trick_sims/SIM_pool/models/pool_table/include/common_geometry.hh new file mode 100644 index 00000000..7a6fc0b3 --- /dev/null +++ b/trick_sims/SIM_pool/models/pool_table/include/common_geometry.hh @@ -0,0 +1,77 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: ( Geometry primitives to be used elsewhere. ) +LIBRARY DEPENDENCY: + (()) +*******************************************************************************/ +#ifndef _common_hh_ +#define _common_hh_ + +// 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() {} + + 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& 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 + } + + Vec normalized () { + Vec norm; + + } + + double& x () { return _x; } + double& y () { return _y; } + double& z () { return _z; } + + + private: + double _x; + double _y; + double _z; +}; + +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_pool/models/pool_table/include/pocket.hh b/trick_sims/SIM_pool/models/pool_table/include/pocket.hh new file mode 100644 index 00000000..6bf45424 --- /dev/null +++ b/trick_sims/SIM_pool/models/pool_table/include/pocket.hh @@ -0,0 +1,24 @@ +/********************************* 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) : x(x), y(y), radius(r) {} + + double x; + double y; + double radius; + +}; + +#endif \ No newline at end of file diff --git a/trick_sims/SIM_pool/models/pool_table/include/pool_table.hh b/trick_sims/SIM_pool/models/pool_table/include/pool_table.hh index 5a0b79d0..e4bc40d1 100644 --- a/trick_sims/SIM_pool/models/pool_table/include/pool_table.hh +++ b/trick_sims/SIM_pool/models/pool_table/include/pool_table.hh @@ -9,6 +9,7 @@ LIBRARY DEPENDENCIES: #include "trick/regula_falsi.h" #include "ball.hh" #include "bumper.hh" +#include "pocket.hh" #include @@ -17,42 +18,78 @@ class PoolTable { public: PoolTable () : numBalls(0), numAssociations(0) {} - int addBall (double x, double y, double mass, double radius, bool fixed); - int addBumper (double x1, double y1, double x2, double y2); - - int setBallPos(int id, double x, double y); - int setBallVel(int id, double v_x, double v_y); - - // Ball ** balls; - // Bumper ** bumpers; - Ball** balls; - Bumper** bumpers; - - // Ball-ball collisions - int nextBallSlot = 0; - unsigned int numBalls; - unsigned int numAssociations; - REGULA_FALSI* ballAssociations; - - // Ball-bumper collisions - unsigned int numBumpers; - unsigned int numCombos; - REGULA_FALSI* bumperAssociations; - - //void ballCollision(Ball &b1, Ball &b2); - int default_data(); int state_init(); int state_deriv(); int state_integ(); double collision(); + // 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 = 0.99; + double coefficientOfElasticity = .95; + double cueForceScale = 1.0; + 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_pool/models/pool_table/src/ball.cpp b/trick_sims/SIM_pool/models/pool_table/src/ball.cpp index 3d6b3665..b7a4a081 100644 --- a/trick_sims/SIM_pool/models/pool_table/src/ball.cpp +++ b/trick_sims/SIM_pool/models/pool_table/src/ball.cpp @@ -6,30 +6,57 @@ LIBRARY DEPENDENCY: #include "../include/ball.hh" // #include "trick/memorymanager_c_intf.h" -#include "trick/MemoryManager.hh" -extern Trick::MemoryManager* trick_MM; +// #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[0] = x; - pos[1] = y; - pos[2] = 0; + pos = Vec(x, y, 0); } -// Ball* CreateBall(double x, double y, double mass, double radius, bool isFixed) { -// // Ball* b = (Ball*)TMM_declare_var_s("Ball"); -// Ball *b = new Ball(x, y, mass, radius, isFixed, 0); -// trick_MM->declare_extern_var ( b, "Ball"); -// // Ball *b = (Ball*)trick_MM->declare_var("Ball"); +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; +} -// // return (new (b) Ball(x, y, mass, radius, isFixed, 0)); -// return b; -// } diff --git a/trick_sims/SIM_pool/models/pool_table/src/bumper.cpp b/trick_sims/SIM_pool/models/pool_table/src/bumper.cpp index d2927cf3..435e9c79 100644 --- a/trick_sims/SIM_pool/models/pool_table/src/bumper.cpp +++ b/trick_sims/SIM_pool/models/pool_table/src/bumper.cpp @@ -1,9 +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_pool/models/pool_table/src/pocket.cpp b/trick_sims/SIM_pool/models/pool_table/src/pocket.cpp new file mode 100644 index 00000000..e69de29b diff --git a/trick_sims/SIM_pool/models/pool_table/src/pool_table.cpp b/trick_sims/SIM_pool/models/pool_table/src/pool_table.cpp index 0ee9da8a..d5b376e9 100644 --- a/trick_sims/SIM_pool/models/pool_table/src/pool_table.cpp +++ b/trick_sims/SIM_pool/models/pool_table/src/pool_table.cpp @@ -29,6 +29,11 @@ void scaleInPlace (double vec[], double scale, int dim) { } } +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]; +} + @@ -36,14 +41,6 @@ int PoolTable::default_data() { // balls.clear(); // bumpers.clear(); - - // Dev Testing only - should be deleted - // int id1 = addBall(0.5, 0.5, 1, 1, false); - // int id2 = addBall(0.75, 0.5, 1, 1, false); - - // setBallVel(id1, 0.1, 0); - /////////////////////////////////////////// - numBalls = 0; numBumpers = 0; return 0; @@ -69,7 +66,30 @@ int PoolTable::state_init() { } } - // Need to do the same thing with rail/ball associations + bumperBallCombos = numBalls * numBumpers; + bumperAssociations = (REGULA_FALSI*)TMM_declare_var_1d("REGULA_FALSI", bumperBallCombos); + for (ii=0; iivel[0] << " " << balls[i]->vel[1] << std::endl; - // std::cout << "Velocity Norm: " << velocityNorm[0] << " " << velocityNorm[1] << std::endl; - - // balls[i]->accel[0] = 0; - // balls[i]->accel[1] = 0; - + // 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 (abs(dv_mag(balls[i]->vel)) > frictionTolerance) { double velocityNorm[3]; dv_norm(velocityNorm, balls[i]->vel); @@ -125,6 +143,10 @@ int PoolTable::state_integ() { int n = 6; for (int i = 0; i < numBalls; i++) { + if (!balls[i]->inPlay) { + continue; + } + // State - Need to load 4 values for each ball, but will have to have more when we add angular stuff // pos[0] pos[1] vel[0] vel[1] int inner_index = 0; @@ -153,6 +175,10 @@ int PoolTable::state_integ() { int integration_step = integrate(); for (int i = 0; i < numBalls; i++) { + if (!balls[i]->inPlay) { + continue; + } + // pos[0] pos[1] vel[0] vel[1] int inner_index = 0; balls[i]->pos[0] = unload_indexed_state(n*i + inner_index++); @@ -162,23 +188,55 @@ int PoolTable::state_integ() { 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 0; + return integration_step; +} + +double closestPointOnLine(Line& line, double pos[2], double result[2], bool print) { + double m[3]; + double a[3] = {line.p1.x, line.p1.y}; + double b[3] = {line.p2.x, line.p2.y}; + double diff[3]; + + dv_sub(diff, pos, a); + dv_sub(m, b, a); + + double t = dot(diff, m, 2) / dot(m, m, 2); + + // if (print) + // std::cout << "t: " << t << std::endl; + + if (t < 0) + t = 0; + + if (t > 1) + t = 1; + + scaleInPlace(m, t, 2); + dv_add(result, a, m); + // if (print) + // std::cout << "Result: " << result[0] << " " << result[1] << std::endl; + + return t; +} + +double PoolTable::removeBall(int id) { + balls[id]->inPlay = false; + balls[id]->clearAllState(); } -// Maybe need a separate scheduled job to handle pockets? -// And the cue? -// Maybe see if there's some "callback" that can handle user input -// There must be, since other sims have control panels too double PoolTable::collision() { - // Handle when the balls collide with others or with a bumper + + // Handle when the balls collide with others double now ; /* current integration time. */ - unsigned int first, second; - - unsigned int association_index; double event_tgo; unsigned int ii,jj; @@ -188,14 +246,26 @@ double PoolTable::collision() { std::vector collisionsToProcess; for (ii=1; iiinPlay) { + continue; + } for (jj=0; jjinPlay) { + continue; + } + double diff[3]; dv_sub(diff, balls[ii]->pos, balls[jj]->pos); double distanceBetweenBalls = dv_mag(diff); unsigned int associationIndex = ii*(ii-1)/2+jj; // boundary is distance between balls - radiuses of balls - ballAssociations[associationIndex].error = distanceBetweenBalls - (balls[ii]->radius + balls[jj]->radius); + double error = distanceBetweenBalls - (balls[ii]->radius + balls[jj]->radius); + + // std::cout << "Distance between centers: " << distanceBetweenBalls << std::endl; + // std::cout << "Radiuses: " << balls[ii]->radius << " " << balls[jj]->radius << std::endl; + // std::cout << "Error calculation: " << error << std::endl; + ballAssociations[associationIndex].error = error; double this_tgo = regula_falsi( now, &(ballAssociations[associationIndex])) ; if (this_tgo < event_tgo) { @@ -203,20 +273,33 @@ double PoolTable::collision() { } if (this_tgo == 0) { + std::cout << "Found colliding balls" << std::endl; + // Add this collision to a list of collisions to process 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) { + // if (allowCollisions == false) { + // return event_tgo; + // } + // allowCollisions = false; int index1 = collisionsToProcess[i]; int index2 = collisionsToProcess[i+1]; + std::cout << "Collision detected between balls " << index1 << " and " << index2 << std::endl; - double *q1 = balls[index1]->pos; - double *q2 = balls[index2]->pos; + double q1[3]; + dv_copy(q1, balls[index1]->pos); + double q2[3]; + dv_copy(q2, balls[index2]->pos); // dg = (q1 - q2) / (|q1 - q2|) @@ -234,15 +317,261 @@ double PoolTable::collision() { // J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg // For now let's just pretend all the masses are 1 double impulse = ((1.0 + coefficientOfElasticity) * dot(dg4, vel4, 4)) / (dot(dg4, dg4, 4)); - scaleInPlace(dg, impulse, 4); + 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 + double impulse1[3] = {dg4[0], dg4[1], 0}; + double impulse2[3] = {dg4[2], dg4[3], 0}; + double newV1[3]; + dv_copy(newV1, balls[index1]->vel); + double newV2[3]; + dv_copy(newV2, balls[index2]->vel); + + scaleInPlace(newV1, balls[index1]->mass, 3); + scaleInPlace(newV2, balls[index2]->mass, 3); + + dv_sub(newV1, newV1, impulse1); + dv_sub(newV2, newV2, impulse2); + + scaleInPlace(newV1, 1.0 / balls[index1]->mass, 3); + scaleInPlace(newV2, 1.0 / balls[index2]->mass, 3); + + 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]; + } + + // std::cout << "Ball 0 pos: " << balls[0]->pos[0] << " " << balls[0]->pos[1] << std::endl; + // std::cout << "Bumper 0 pos: " << bumpers[0]->border.p1.x << " " << bumpers[0]->border.p1.y << " " << bumpers[0]->border.p2.x << " " << bumpers[0]->border.p2.y << std::endl; + + int numBumperCollisions = 0; + for (ii=0; iiinPlay) { + continue; + } + // if (numBumperCollisions > 0) + // break; + for (jj=0; jj 0) + // break; + unsigned int association_index = (ii*numBumpers) + jj; + Ball *ball = balls[ii]; + Bumper *bumper = bumpers[jj]; + //Point ballPos(ball->pos[0], ball->pos[1]); + double closestPointOnBumper[3] = {0, 0, 0}; + if (ii == 0 && jj == 3) { + double t = closestPointOnLine(bumper->border, ball->pos, closestPointOnBumper, true); + // std::cout << "Closest point on bumper: " << closestPointOnBumper[0] << " " << closestPointOnBumper[1] << std::endl; + + } else { + double t = closestPointOnLine(bumper->border, ball->pos, closestPointOnBumper, false); + } + double diff[3]; + + // dv_sub(diff, ball->pos, closestPointOnBumper); + diff[0] = ball->pos[0] - closestPointOnBumper[0]; + diff[1] = ball->pos[1] - closestPointOnBumper[1]; + diff[2] = 0; + + double distanceToBumper = abs(dv_mag(diff)) - ball->radius; + + bumperAssociations[association_index].error = distanceToBumper; + // if (ii == 0 && jj == 3) { + // std::cout << "Ball pos: " << balls[ii]->pos[0] << " " << balls[ii]->pos[1] << std::endl; + // std::cout << "Closest point on bumper: " << closestPointOnBumper[0] << " " << closestPointOnBumper[1] << std::endl; + // std::cout << "Distance to bumper: " << bumperAssociations[association_index].error << std::endl; + // } + 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) + continue; + + double q1[3]; + dv_copy(q1, ball->pos); + double q2[3]; + dv_copy(q2, closestPointOnBumper); + q1[2] = 0; + q2[2] = 0; + + + // dg = (q1 - q2) / (|q1 - q2|) + double diff[3]; + dv_sub(diff, q1, q2); + double dg[3]; + dv_norm(dg, diff); + + // Have to stuff both velocities and dg values into 4d vector to do the calculation + // Otherwise I have to do more math + double dg4[4] = {dg[0], dg[1], -dg[0], -dg[1]}; + double vel4[4] = {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 + double impulse1[3] = {dg4[0], dg4[1], 0}; + double impulse2[3] = {dg4[2], dg4[3], 0}; + + double newV1[3]; + dv_copy(newV1, ball->vel); + // double newV2[3] = {0, 0}; + + scaleInPlace(newV1, ball->mass, 3); + // scaleInPlace(newV2, balls[index2]->mass, 3); + + dv_sub(newV1, newV1, impulse1); + // dv_sub(newV2, newV2, impulse2); + + scaleInPlace(newV1, 1.0 / ball->mass, 3); + // scaleInPlace(newV2, 1.0 / balls[index2]->mass, 3); + + 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; jjx, pocket->y, 0}; + + double diff[3]; + dv_sub(diff, ball->pos, pocketPos); + double distance = dv_mag(diff); + + // Ball is pocketed when center is within pocket radius + double error = distance - (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" << 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; + double cueEnd[3] = {x_end, y_end}; + double cueBallPos[3] = {balls[cueIndex]->pos[0], balls[cueIndex]->pos[1]}; + + 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); + + double cueAxis[3]; + dv_sub(cueAxis, cueEnd, cueBallPos); + double cue_v = dv_mag(cueAxis) * cueForceScale * angleScaling; + dv_norm(cueAxis, cueAxis); + 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_b[2] = {0, -force/m * cos(theta)}; + + // Rotate to table frame + double y_unit[2] = {0, 1}; + double rot = acos(dot(axis2, y_unit, 2) / 1.0); + if (cueAxis[0] < 0) { + rot = -rot; + } + + double rotationMatrix[2][2]; + rotationMatrix[0][0] = cos(rot); + rotationMatrix[0][1] = -sin(rot); + rotationMatrix[1][0] = sin(rot); + rotationMatrix[1][1] = cos(rot); + + double v_table[2]; + mProduct(v_table, rotationMatrix, v_b); + + std::cout << "Velocity of ball after cue hit: " << v_table[0] << " " << v_table[1] << std::endl; + + balls[cueIndex]->vel[0] = v_table[0]; + balls[cueIndex]->vel[1] = v_table[1]; + + // 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++; @@ -256,11 +585,45 @@ int PoolTable::addBall (double x, double y, double mass, double radius, bool fix // Add a bumper to the table and return the ID // Only takes in the actual effective line, need to add something else for the rendered shape -int PoolTable::addBumper (double x1, double y1, double x2, double y2) { - // bumpers.push_back(bumper); +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) { + Point * point = (Point*) TMM_declare_var_s("Point"); + tableShape[id] = (new (point) Point(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;