mirror of
https://github.com/nasa/trick.git
synced 2024-12-19 21:27:54 +00:00
Merge pull request #1306 from nasa/1299-sim-pool
Trick Billiards Simulation (#1299)
This commit is contained in:
Jacqueline Deans
GitHub
commit
790ec56816
2
trick_sims/.gitignore
vendored
2
trick_sims/.gitignore
vendored
@ -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
|
||||
|
||||
10
trick_sims/SIM_billiards/Modified_data/realtime.py
Normal file
10
trick_sims/SIM_billiards/Modified_data/realtime.py
Normal file
@ -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)
|
||||
162
trick_sims/SIM_billiards/RUN_break/input.py
Normal file
162
trick_sims/SIM_billiards/RUN_break/input.py
Normal file
@ -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('=================================================================================================')
|
||||
119
trick_sims/SIM_billiards/RUN_hexagon/input.py
Normal file
119
trick_sims/SIM_billiards/RUN_hexagon/input.py
Normal file
@ -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('=================================================================================================')
|
||||
157
trick_sims/SIM_billiards/RUN_test/input.py
Normal file
157
trick_sims/SIM_billiards/RUN_test/input.py
Normal file
@ -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('=================================================================================================')
|
||||
24
trick_sims/SIM_billiards/S_define
Normal file
24
trick_sims/SIM_billiards/S_define
Normal file
@ -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;
|
||||
2
trick_sims/SIM_billiards/S_overrides.mk
Normal file
2
trick_sims/SIM_billiards/S_overrides.mk
Normal file
@ -0,0 +1,2 @@
|
||||
TRICK_CFLAGS += -Imodels
|
||||
TRICK_CXXFLAGS += -Imodels
|
||||
16
trick_sims/SIM_billiards/models/graphics/cpp/CMakeLists.txt
Normal file
16
trick_sims/SIM_billiards/models/graphics/cpp/CMakeLists.txt
Normal file
@ -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)
|
||||
26
trick_sims/SIM_billiards/models/graphics/cpp/README.md
Normal file
26
trick_sims/SIM_billiards/models/graphics/cpp/README.md
Normal file
@ -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
|
||||
|
||||
61
trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp
Normal file
61
trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp
Normal file
@ -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();
|
||||
}
|
||||
27
trick_sims/SIM_billiards/models/graphics/cpp/Socket.hh
Normal file
27
trick_sims/SIM_billiards/models/graphics/cpp/Socket.hh
Normal file
@ -0,0 +1,27 @@
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <unistd.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
|
||||
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;
|
||||
|
||||
};
|
||||
@ -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)
|
||||
967
trick_sims/SIM_billiards/models/graphics/cpp/main.cpp
Normal file
967
trick_sims/SIM_billiards/models/graphics/cpp/main.cpp
Normal file
@ -0,0 +1,967 @@
|
||||
#include <igl/opengl/glfw/Viewer.h>
|
||||
// #include <igl/readOFF.h>
|
||||
#include <igl/opengl/glfw/imgui/ImGuiPlugin.h>
|
||||
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
|
||||
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <stdlib.h>
|
||||
#include <string>
|
||||
|
||||
#include "Socket.hh"
|
||||
|
||||
|
||||
std::vector<Eigen::Vector3d> 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<Eigen::Vector3i> getFacesWithBaseIndex(int newBase) {
|
||||
std::vector<Eigen::Vector3i> 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<Eigen::Vector3d> vertices;
|
||||
std::vector<Eigen::Vector3i> faces;
|
||||
std::vector<Eigen::Vector3d> 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<Point> 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<double> 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<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::MatrixXd> 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<Polygon *> staticShapes;
|
||||
std::vector<RenderedShape *> staticRenderedShapes;
|
||||
std::vector<RenderedShape *> movingRenderedShapes;
|
||||
int numStaticVertices;
|
||||
int numStaticFaces;
|
||||
bool staticRendered = false;
|
||||
|
||||
std::vector<Polygon *> movingShapes;
|
||||
|
||||
};
|
||||
|
||||
void printUsage() {
|
||||
std::cout << "Usage: program <portNumber>" << std::endl;
|
||||
}
|
||||
|
||||
std::vector<std::string> split (std::string& str, const char delim) {
|
||||
std::stringstream ss(str);
|
||||
std::string s;
|
||||
std::vector<std::string> ret;
|
||||
while (std::getline(ss, s, delim)) {
|
||||
ret.push_back(s);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
std::vector<double> parseTrickResponse(std::vector<std::string> list) {
|
||||
std::vector<double> ret;
|
||||
for (int i = 1; i < list.size(); i++) {
|
||||
ret.push_back(stod(list[i]));
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
std::vector<int> parseTrickResponseInt(std::vector<std::string> list) {
|
||||
std::vector<int> 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<std::string> 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<double> 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<double> 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<double> 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<double> 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<double> 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<double> 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<double> 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<int> 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<double> 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<double> 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<double> 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<double> 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<double> 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();
|
||||
}
|
||||
@ -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<numberOfBalls ; ii++) {
|
||||
balls[ii] = new Ball(ii);
|
||||
}
|
||||
}
|
||||
|
||||
public void setScale (int mapScale) {
|
||||
if (mapScale < 4) {
|
||||
scale = 4;
|
||||
} else if (mapScale > 128) {
|
||||
scale = 128;
|
||||
} else {
|
||||
scale = mapScale;
|
||||
}
|
||||
repaint();
|
||||
}
|
||||
|
||||
public int getScale() {
|
||||
return scale;
|
||||
}
|
||||
|
||||
public void drawCenteredCircle(Graphics2D g, int x, int y, int d) {
|
||||
x = x-(d/2);
|
||||
y = y-(d/2);
|
||||
g.fillOval(x,y,d,d);
|
||||
}
|
||||
|
||||
private void doDrawing(Graphics g) {
|
||||
Graphics2D g2d = (Graphics2D) g;
|
||||
|
||||
RenderingHints rh = new RenderingHints(
|
||||
RenderingHints.KEY_ANTIALIASING,
|
||||
RenderingHints.VALUE_ANTIALIAS_ON);
|
||||
|
||||
rh.put(RenderingHints.KEY_RENDERING,
|
||||
RenderingHints.VALUE_RENDER_QUALITY);
|
||||
|
||||
int width = getWidth();
|
||||
int height = getHeight();
|
||||
|
||||
worldOriginX = (width/2);
|
||||
worldOriginY = (height/2);
|
||||
|
||||
// Draw Background
|
||||
g2d.setPaint(backGroundColor);
|
||||
g2d.fillRect(0, 0, width, height);
|
||||
|
||||
// Draw balls
|
||||
for (int ii = 0; ii < balls.length ; ii++) {
|
||||
g2d.setPaint(balls[ii].color);
|
||||
int bx = (int)(worldOriginX + scale * balls[ii].x);
|
||||
int by = (int)(worldOriginY - scale * balls[ii].y);
|
||||
drawCenteredCircle(g2d, bx, by, (int)(scale * 2 * balls[ii].radius));
|
||||
// g2d.setPaint(Color.BLACK);
|
||||
// g2d.drawString ( String.format("%d",ii), bx,by);
|
||||
}
|
||||
|
||||
g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void paintComponent(Graphics g) {
|
||||
super.paintComponent(g);
|
||||
doDrawing(g);
|
||||
}
|
||||
}
|
||||
|
||||
public class PoolTableDisplay extends JFrame {
|
||||
|
||||
private RangeView rangeView;
|
||||
private BufferedReader in;
|
||||
private DataOutputStream out;
|
||||
|
||||
public PoolTableDisplay() {
|
||||
|
||||
rangeView = null;
|
||||
setTitle("Ball Arena");
|
||||
setSize(800, 500);
|
||||
setLocationRelativeTo(null);
|
||||
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
|
||||
setFocusable(true);
|
||||
}
|
||||
|
||||
public void connectToServer(String host, int port ) throws IOException {
|
||||
Socket socket = new Socket(host, port);
|
||||
in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
|
||||
out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
|
||||
}
|
||||
|
||||
public void createGUI( int mapScale, int numberOfBalls ) {
|
||||
|
||||
rangeView = new RangeView(mapScale, numberOfBalls);
|
||||
JPanel panel1 = new JPanel();
|
||||
panel1.setLayout(new BoxLayout(panel1, BoxLayout.X_AXIS));
|
||||
panel1.add(rangeView);
|
||||
|
||||
ControlPanel controlPanel = new ControlPanel(rangeView);
|
||||
JPanel panel0 = new JPanel();
|
||||
panel0.setLayout(new BoxLayout(panel0, BoxLayout.Y_AXIS));
|
||||
panel0.add(panel1);
|
||||
panel0.add(controlPanel);
|
||||
add(panel0);
|
||||
setVisible(true);
|
||||
}
|
||||
|
||||
private static void printHelpText() {
|
||||
System.out.println(
|
||||
"----------------------------------------------------------------------\n"
|
||||
+ "usage: java jar PoolTableDisplay.jar <port-number>\n"
|
||||
+ "----------------------------------------------------------------------\n"
|
||||
);
|
||||
}
|
||||
|
||||
public static void main(String[] args) throws IOException, InterruptedException {
|
||||
|
||||
String host = "localHost";
|
||||
int port = 0;
|
||||
boolean boom = false;
|
||||
|
||||
// Handle program arguments.
|
||||
int ii = 0;
|
||||
while (ii < args.length) {
|
||||
switch (args[ii]) {
|
||||
case "-help" :
|
||||
case "--help" : {
|
||||
printHelpText();
|
||||
System.exit(0);
|
||||
} break;
|
||||
default : {
|
||||
port = (Integer.parseInt(args[ii]));
|
||||
} break;
|
||||
}
|
||||
++ii;
|
||||
}
|
||||
|
||||
boolean go = true;
|
||||
double dt = 0.100; // Time between updates (seconds).
|
||||
int mapScale = 32 ; // pixels per meter.
|
||||
int nballs = 7;
|
||||
|
||||
if (port == 0) {
|
||||
System.out.println("No variable server port specified.");
|
||||
printHelpText();
|
||||
System.exit(0);
|
||||
}
|
||||
|
||||
PoolTableDisplay poolTableDisplay = new PoolTableDisplay();
|
||||
|
||||
// Connect to the Trick simulation's variable server.
|
||||
System.out.println("Connecting to: " + host + ":" + port);
|
||||
poolTableDisplay.connectToServer(host, port);
|
||||
poolTableDisplay.out.writeBytes("trick.var_set_client_tag(\"PoolTableDisplay\") \n");
|
||||
poolTableDisplay.out.flush();
|
||||
|
||||
// Get the number of balls.
|
||||
poolTableDisplay.out.writeBytes(
|
||||
"trick.var_add(\"dyn.table.numBalls\")\n" +
|
||||
"trick.var_send() \n" +
|
||||
"trick.var_clear() \n");
|
||||
poolTableDisplay.out.flush();
|
||||
try {
|
||||
String line;
|
||||
String field[];
|
||||
line = poolTableDisplay.in.readLine();
|
||||
field = line.split("\t");
|
||||
nballs = Integer.parseInt( field[1]);
|
||||
} catch (IOException | NullPointerException e ) {
|
||||
go = false;
|
||||
}
|
||||
|
||||
poolTableDisplay.createGUI(mapScale, nballs);
|
||||
|
||||
// Get the Radii of the balls.
|
||||
for ( ii = 0; ii < nballs; ii ++) {
|
||||
poolTableDisplay.out.writeBytes( String.format("trick.var_add(\"dyn.table.balls[%d][0].radius\")\n", ii));
|
||||
}
|
||||
poolTableDisplay.out.flush();
|
||||
poolTableDisplay.out.writeBytes(
|
||||
"trick.var_send() \n" +
|
||||
"trick.var_clear() \n");
|
||||
poolTableDisplay.out.flush();
|
||||
try {
|
||||
String line;
|
||||
String field[];
|
||||
line = poolTableDisplay.in.readLine();
|
||||
field = line.split("\t");
|
||||
for ( ii=0; ii < nballs; ii++) {
|
||||
poolTableDisplay.rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]);
|
||||
}
|
||||
} catch (IOException | NullPointerException e ) {
|
||||
go = false;
|
||||
}
|
||||
|
||||
// Get the Positions of the balls, and update the display, periodically.
|
||||
poolTableDisplay.out.writeBytes( "trick.var_pause() \n");
|
||||
for ( ii = 0; ii < nballs; ii ++) {
|
||||
poolTableDisplay.out.writeBytes(
|
||||
String.format("trick.var_add(\"dyn.table.balls[%d][0].pos[0]\")\n", ii)
|
||||
+ String.format("trick.var_add(\"dyn.table.balls[%d][0].pos[1]\")\n", ii)
|
||||
);
|
||||
}
|
||||
poolTableDisplay.out.writeBytes("trick.var_ascii() \n" +
|
||||
String.format("trick.var_cycle(%.3f)\n", dt) +
|
||||
"trick.var_unpause() \n" );
|
||||
poolTableDisplay.out.flush();
|
||||
while (go) {
|
||||
try {
|
||||
String line;
|
||||
String field[];
|
||||
line = poolTableDisplay.in.readLine();
|
||||
// System.out.println("Sim->Client:" + line);
|
||||
field = line.split("\t");
|
||||
for ( ii=0; ii < nballs; ii++) {
|
||||
poolTableDisplay.rangeView.balls[ii].x = Double.parseDouble( field[2*ii+1]);
|
||||
poolTableDisplay.rangeView.balls[ii].y = Double.parseDouble( field[2*ii+2]);
|
||||
}
|
||||
} catch (IOException | NullPointerException e ) {
|
||||
go = false;
|
||||
}
|
||||
// Update the scene.
|
||||
poolTableDisplay.rangeView.repaint();
|
||||
} // while
|
||||
} // main
|
||||
} // class
|
||||
76
trick_sims/SIM_billiards/models/pool_table/include/ball.hh
Normal file
76
trick_sims/SIM_billiards/models/pool_table/include/ball.hh
Normal file
@ -0,0 +1,76 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Simulate pool balls. )
|
||||
LIBRARY DEPENDENCY:
|
||||
((ball.o))
|
||||
*******************************************************************************/
|
||||
#ifndef _ball_hh_
|
||||
#define _ball_hh_
|
||||
|
||||
// #include <Eigen/Core>
|
||||
#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
|
||||
32
trick_sims/SIM_billiards/models/pool_table/include/bumper.hh
Normal file
32
trick_sims/SIM_billiards/models/pool_table/include/bumper.hh
Normal file
@ -0,0 +1,32 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Pool bumper class. )
|
||||
LIBRARY DEPENDENCY:
|
||||
((bumper.o))
|
||||
*******************************************************************************/
|
||||
#ifndef _bumper_hh_
|
||||
#define _bumper_hh_
|
||||
|
||||
#include <vector>
|
||||
#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
|
||||
@ -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
|
||||
23
trick_sims/SIM_billiards/models/pool_table/include/pocket.hh
Normal file
23
trick_sims/SIM_billiards/models/pool_table/include/pocket.hh
Normal file
@ -0,0 +1,23 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Pool pocket class. )
|
||||
LIBRARY DEPENDENCY:
|
||||
(())
|
||||
*******************************************************************************/
|
||||
#ifndef _pocket_hh_
|
||||
#define _pocket_hh_
|
||||
|
||||
#include <vector>
|
||||
#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
|
||||
@ -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 <vector>
|
||||
|
||||
|
||||
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
|
||||
62
trick_sims/SIM_billiards/models/pool_table/src/ball.cpp
Normal file
62
trick_sims/SIM_billiards/models/pool_table/src/ball.cpp
Normal file
@ -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 <new>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
25
trick_sims/SIM_billiards/models/pool_table/src/bumper.cpp
Normal file
25
trick_sims/SIM_billiards/models/pool_table/src/bumper.cpp
Normal file
@ -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));
|
||||
}
|
||||
553
trick_sims/SIM_billiards/models/pool_table/src/pool_table.cpp
Normal file
553
trick_sims/SIM_billiards/models/pool_table/src/pool_table.cpp
Normal file
@ -0,0 +1,553 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: (Simulate a pool table.)
|
||||
LIBRARY DEPENDENCY:
|
||||
((pool_table.o)
|
||||
(ball.o))
|
||||
*******************************************************************************/
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
#include "trick/integrator_c_intf.h"
|
||||
#include "trick/memorymanager_c_intf.h"
|
||||
#include "../include/pool_table.hh"
|
||||
#include "trick/trick_math_proto.h"
|
||||
|
||||
|
||||
// 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<numBalls; ii++) {
|
||||
for (jj=0; jj<ii; jj++) {
|
||||
unsigned int association_index = ii*(ii-1)/2+jj;
|
||||
ballAssociations[association_index].mode = Decreasing;
|
||||
ballAssociations[association_index].error_tol = 0.0000001;
|
||||
now = get_integ_time() ;
|
||||
reset_regula_falsi( now, &ballAssociations[association_index] );
|
||||
}
|
||||
}
|
||||
|
||||
// Bumper/ball collisions
|
||||
bumperBallCombos = numBalls * numBumpers;
|
||||
bumperAssociations = (REGULA_FALSI*)TMM_declare_var_1d("REGULA_FALSI", bumperBallCombos);
|
||||
for (ii=0; ii<numBalls; ii++) {
|
||||
for (jj=0; jj<numBumpers; jj++) {
|
||||
unsigned int association_index = (ii*numBumpers) + jj;
|
||||
bumperAssociations[association_index].mode = Any;
|
||||
bumperAssociations[association_index].error_tol = 0.0000001;
|
||||
now = get_integ_time() ;
|
||||
reset_regula_falsi( now, &bumperAssociations[association_index] );
|
||||
}
|
||||
}
|
||||
|
||||
// Need to do the same thing with pocket/ball associations
|
||||
pocketBallCombos = numBalls * numPockets;
|
||||
pocketAssociations = (REGULA_FALSI*)TMM_declare_var_1d("REGULA_FALSI", pocketBallCombos);
|
||||
for (ii=0; ii<numBalls; ii++) {
|
||||
for (jj=0; jj<numPockets; jj++) {
|
||||
unsigned int association_index = (ii*numPockets) + jj;
|
||||
pocketAssociations[association_index].mode = Decreasing;
|
||||
pocketAssociations[association_index].error_tol = 0.0000001;
|
||||
now = get_integ_time() ;
|
||||
reset_regula_falsi( now, &pocketAssociations[association_index] );
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PoolTable::state_deriv() {
|
||||
// derivative - determine the acceleration here
|
||||
// This is probably just friction
|
||||
// And angular stuff
|
||||
// F = MA => A = M^-1 * F
|
||||
|
||||
// Will have to account for rolling vs sliding here
|
||||
// For now, just rolling
|
||||
|
||||
for (int i = 0; i < numBalls; i++) {
|
||||
// Frictional force a constant applied in the opposing direction of velocity.
|
||||
// Magnitude of velocity is irrelevant 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<unsigned int> collisionsToProcess;
|
||||
|
||||
for (ii=1; ii<numBalls; ii++) {
|
||||
if (!balls[ii]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
for (jj=0; jj<ii; jj++) {
|
||||
if (!balls[jj]->inPlay) {
|
||||
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; ii<numBalls; ii++) {
|
||||
if (!balls[ii]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
for (jj=0; jj<numBumpers; jj++) {
|
||||
unsigned int association_index = (ii*numBumpers) + jj;
|
||||
|
||||
Ball *ball = balls[ii];
|
||||
Bumper *bumper = bumpers[jj];
|
||||
|
||||
Vec closestPointOnBumper;
|
||||
double t = closestPointOnLine(bumper->border, 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; ii<numBalls; ii++) {
|
||||
if (!balls[ii]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
for (jj=0; jj<numPockets; jj++) {
|
||||
unsigned int association_index = (ii*numPockets) + jj;
|
||||
Ball *ball = balls[ii];
|
||||
Pocket *pocket = pockets[jj];
|
||||
|
||||
// Ball is pocketed when center is within pocket radius
|
||||
double error = (ball->pos - 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user