mirror of
https://github.com/nasa/trick.git
synced 2024-12-19 21:27:54 +00:00
Saving working state
This commit is contained in:
Deans
parent
b869f851f5
commit
8537601ccb
@ -1,6 +1,6 @@
|
||||
|
||||
trick.real_time_enable()
|
||||
trick.exec_set_software_frame(0.1)
|
||||
trick.exec_set_software_frame(0.01)
|
||||
trick.itimer_enable()
|
||||
|
||||
trick.exec_set_enable_freeze(True)
|
||||
|
||||
159
trick_sims/SIM_pool/RUN_break/input.py
Normal file
159
trick_sims/SIM_pool/RUN_break/input.py
Normal file
@ -0,0 +1,159 @@
|
||||
import math
|
||||
|
||||
exec(open("./Modified_data/realtime.py").read())
|
||||
|
||||
dyn.table.numBalls = 16
|
||||
dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls)
|
||||
|
||||
ballRadius = 0.02
|
||||
ballMass = 1
|
||||
|
||||
unit_pos = [math.sqrt(3)/2, 0.5]
|
||||
unit_neg = [unit_pos[0], -unit_pos[1]]
|
||||
center_x = 0.2;
|
||||
center_y = 0;
|
||||
tol = 1e-4;
|
||||
|
||||
dyn.table.addBall(-.3, 0, ballMass, ballRadius, False)
|
||||
|
||||
dyn.table.addBall(center_x, center_y, ballMass, ballRadius, False)
|
||||
|
||||
dyn.table.addBall(center_x+unit_neg[0]*2*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*2*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
|
||||
dyn.table.addBall(center_x+unit_neg[0]*4*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y, ballMass, ballRadius, False)
|
||||
|
||||
dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*6*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*6*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
|
||||
dyn.table.addBall(center_x+unit_neg[0]*8*(ballRadius+tol), center_y+unit_neg[1]*8*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*8*(ballRadius+tol), center_y+unit_pos[1]*8*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_neg[0]*8*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*8*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
dyn.table.addBall(center_x+unit_pos[0]*8*(ballRadius+tol), center_y, ballMass, ballRadius, False)
|
||||
|
||||
corners = [-.5, -.25, .5, .25]
|
||||
|
||||
|
||||
|
||||
# Make a normal pool table
|
||||
pocketRadius = 0.04
|
||||
bumperWidth = 0.03
|
||||
|
||||
|
||||
dyn.table.numTablePoints = 2
|
||||
dyn.table.tableShape = trick.TMM_declare_var_1d("Point*", dyn.table.numTablePoints)
|
||||
dyn.table.tableShapeType = 3 # rectangle
|
||||
dyn.table.addPointToTable(corners[0], corners[1])
|
||||
dyn.table.addPointToTable(corners[2], corners[3])
|
||||
|
||||
# Pockets
|
||||
dyn.table.numPockets = 6
|
||||
dyn.table.pockets = trick.TMM_declare_var_1d("Pocket*", dyn.table.numPockets)
|
||||
pocketCenters = [[0, corners[1]],
|
||||
[0, corners[3]],
|
||||
[corners[0],corners[1]],
|
||||
[corners[0],corners[3]],
|
||||
[corners[2],corners[1]],
|
||||
[corners[2],corners[3]] ]
|
||||
|
||||
for coord in pocketCenters:
|
||||
dyn.table.addPocket(coord[0], coord[1], pocketRadius)
|
||||
|
||||
|
||||
bumperBorders =[[corners[0]+pocketRadius+bumperWidth, corners[1]+bumperWidth, 0.0-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom left
|
||||
[0.0+pocketRadius+bumperWidth, corners[1]+bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom Right
|
||||
[corners[0]+pocketRadius+bumperWidth, corners[3]-bumperWidth, 0.0-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top left
|
||||
[0.0+pocketRadius+bumperWidth, corners[3]-bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top right
|
||||
[corners[0]+bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[0]+bumperWidth, corners[3]-pocketRadius-bumperWidth], # Left
|
||||
[corners[2]-bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[2]-bumperWidth, corners[3]-pocketRadius-bumperWidth]] # Right
|
||||
|
||||
# triangles
|
||||
bumperBorders.extend([[bumperBorders[0][0]-bumperWidth, bumperBorders[0][1]-bumperWidth, bumperBorders[0][0], bumperBorders[0][1]], # Bottom left
|
||||
[bumperBorders[0][2], bumperBorders[0][1], bumperBorders[0][2]+bumperWidth, bumperBorders[0][1]-bumperWidth],
|
||||
[bumperBorders[1][0]-bumperWidth, bumperBorders[1][1]-bumperWidth, bumperBorders[1][0], bumperBorders[1][1]], # Bottom Right
|
||||
[bumperBorders[1][2], bumperBorders[1][1], bumperBorders[1][2]+bumperWidth, bumperBorders[1][1]-bumperWidth],
|
||||
[bumperBorders[2][0]-bumperWidth, bumperBorders[2][1]+bumperWidth, bumperBorders[2][0], bumperBorders[2][1]], # Top left
|
||||
[bumperBorders[2][2], bumperBorders[2][1], bumperBorders[2][2]+bumperWidth, bumperBorders[2][1]+bumperWidth],
|
||||
[bumperBorders[3][0]-bumperWidth, bumperBorders[3][1]+bumperWidth, bumperBorders[3][0], bumperBorders[3][1]], # Top right
|
||||
[bumperBorders[3][2], bumperBorders[3][1], bumperBorders[3][2]+bumperWidth, bumperBorders[3][1]+bumperWidth],
|
||||
[bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][1]-bumperWidth], # Left
|
||||
[bumperBorders[4][0]-bumperWidth, bumperBorders[4][3]+bumperWidth, bumperBorders[4][2], bumperBorders[4][3]],
|
||||
[bumperBorders[5][0], bumperBorders[5][1], bumperBorders[5][2]+bumperWidth, bumperBorders[5][1]-bumperWidth], # Left
|
||||
[bumperBorders[5][0]+bumperWidth, bumperBorders[5][3]+bumperWidth, bumperBorders[5][2], bumperBorders[5][3]],
|
||||
])
|
||||
|
||||
|
||||
|
||||
bumperShapes = [ [bumperBorders[0][0], bumperBorders[0][1]-bumperWidth, bumperBorders[0][2], bumperBorders[0][3]],
|
||||
[bumperBorders[1][0], bumperBorders[1][1]-bumperWidth, bumperBorders[1][2], bumperBorders[1][3]],
|
||||
[bumperBorders[2][0], bumperBorders[2][1]+bumperWidth, bumperBorders[2][2], bumperBorders[2][3]],
|
||||
[bumperBorders[3][0], bumperBorders[3][1]+bumperWidth, bumperBorders[3][2], bumperBorders[3][3]],
|
||||
[bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][3],],
|
||||
[bumperBorders[5][0]+bumperWidth, bumperBorders[5][1], bumperBorders[5][2], bumperBorders[5][3],],
|
||||
[bumperBorders[6][0], bumperBorders[6][1], bumperBorders[6][2], bumperBorders[6][3], bumperBorders[6][2], bumperBorders[6][1]],
|
||||
[bumperBorders[7][0], bumperBorders[7][1], bumperBorders[7][2], bumperBorders[7][3], bumperBorders[7][0], bumperBorders[7][3]],
|
||||
[bumperBorders[8][0], bumperBorders[8][1], bumperBorders[8][2], bumperBorders[8][3], bumperBorders[8][2], bumperBorders[8][1]],
|
||||
[bumperBorders[9][0], bumperBorders[9][1], bumperBorders[9][2], bumperBorders[9][3], bumperBorders[9][0], bumperBorders[9][3]],
|
||||
[bumperBorders[10][0], bumperBorders[10][1], bumperBorders[10][2], bumperBorders[10][3], bumperBorders[10][2], bumperBorders[10][1]],
|
||||
[bumperBorders[11][0], bumperBorders[11][1], bumperBorders[11][2], bumperBorders[11][3], bumperBorders[11][0], bumperBorders[11][3]],
|
||||
[bumperBorders[12][0], bumperBorders[12][1], bumperBorders[12][2], bumperBorders[12][3], bumperBorders[12][2], bumperBorders[12][1]],
|
||||
[bumperBorders[13][0], bumperBorders[13][1], bumperBorders[13][2], bumperBorders[13][3], bumperBorders[13][0], bumperBorders[13][3]],
|
||||
[bumperBorders[14][0], bumperBorders[14][1], bumperBorders[14][2], bumperBorders[14][3], bumperBorders[14][2], bumperBorders[14][1]],
|
||||
[bumperBorders[15][0], bumperBorders[15][1], bumperBorders[15][2], bumperBorders[15][3], bumperBorders[15][0], bumperBorders[15][3]],
|
||||
[bumperBorders[16][0], bumperBorders[16][1], bumperBorders[16][2], bumperBorders[16][3], bumperBorders[16][2], bumperBorders[16][1]],
|
||||
[bumperBorders[17][0], bumperBorders[17][1], bumperBorders[17][2], bumperBorders[17][3], bumperBorders[17][0], bumperBorders[17][3]]]
|
||||
|
||||
bumperShapeTypes = [3, 3, 3, 3, 3, 3, 2, 2,2,2,2, 2, 2, 2,2,2, 2, 2]
|
||||
|
||||
dyn.table.numBumpers = len(bumperBorders)
|
||||
dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers)
|
||||
|
||||
for i in range(len(bumperBorders)):
|
||||
id = dyn.table.addBumper(len(bumperShapes[i])/2, bumperBorders[i][0], bumperBorders[i][1], bumperBorders[i][2],bumperBorders[i][3])
|
||||
dyn.table.bumpers[id][0].numPoints = len(bumperShapes[i])/2
|
||||
dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Point*", dyn.table.bumpers[id].numPoints)
|
||||
|
||||
dyn.table.bumpers[id][0].shapeType = bumperShapeTypes[i]
|
||||
for j in range(0, len(bumperShapes[i]), 2):
|
||||
dyn.table.addPointToBumper(id, bumperShapes[i][j],bumperShapes[i][j+1])
|
||||
|
||||
|
||||
dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
|
||||
|
||||
#==========================================
|
||||
# Start the Graphics Client
|
||||
#==========================================
|
||||
varServerPort = trick.var_server_get_port();
|
||||
|
||||
# This will definitely change to something else
|
||||
PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example"
|
||||
|
||||
|
||||
if (os.path.isfile(PoolTableDisplay_path)) :
|
||||
PoolTableDisplay_cmd = PoolTableDisplay_path \
|
||||
+ " " + str(varServerPort) + " &" ;
|
||||
print(PoolTableDisplay_cmd)
|
||||
os.system( PoolTableDisplay_cmd);
|
||||
else :
|
||||
print('=================================================================================================')
|
||||
print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||
print('=================================================================================================')
|
||||
|
||||
|
||||
# PoolTableDisplay_path = "models/graphics/java/dist/PoolTableDisplay.jar"
|
||||
|
||||
# if (os.path.isfile(PoolTableDisplay_path)) :
|
||||
# PoolTableDisplay_cmd = "java -jar " \
|
||||
# + PoolTableDisplay_path \
|
||||
# + " " + str(varServerPort) + " &" ;
|
||||
# print(PoolTableDisplay_cmd)
|
||||
# os.system( PoolTableDisplay_cmd);
|
||||
# else :
|
||||
# print('=================================================================================================')
|
||||
# print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||
# print('=================================================================================================')
|
||||
119
trick_sims/SIM_pool/RUN_hexagon/input.py
Normal file
119
trick_sims/SIM_pool/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("Point*", dyn.table.numTablePoints)
|
||||
dyn.table.tableShapeType = 0 # generic
|
||||
for corner in corners:
|
||||
dyn.table.addPointToTable(corner[0], corner[1])
|
||||
|
||||
# Pockets - Just put 1 in the center
|
||||
dyn.table.numPockets = 1
|
||||
dyn.table.pockets = trick.TMM_declare_var_1d("Pocket*", dyn.table.numPockets)
|
||||
dyn.table.addPocket(0, 0, pocketRadius)
|
||||
# for coord in corners:
|
||||
# dyn.table.addPocket(coord[0], coord[1], pocketRadius)
|
||||
|
||||
# dyn.table.numBumpers = 0
|
||||
dyn.table.numBumpers = len(corners)
|
||||
dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers)
|
||||
|
||||
# Put bumpers along each edge
|
||||
for i in range(len(corners)):
|
||||
p1 = [corners[i][0], corners[i][1]]
|
||||
p2 = [corners[(i+1) % 6][0], corners[(i+1) % 6][1]]
|
||||
id = dyn.table.addBumper(3, p1[0], p1[1], p2[0], p2[1])
|
||||
dyn.table.bumpers[id][0].shapeType = 2 # Triangle i guess?
|
||||
dyn.table.bumpers[id][0].numPoints = 3
|
||||
dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Point*", dyn.table.bumpers[id].numPoints)
|
||||
dyn.table.addPointToBumper(id, p1[0], p1[1])
|
||||
dyn.table.addPointToBumper(id, p2[0], p2[1])
|
||||
p3 = [(p1[0] + p2[0])/2,(p1[1] + p2[1])/2]
|
||||
dyn.table.addPointToBumper(id, p3[0], p3[1])
|
||||
|
||||
dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
|
||||
|
||||
#==========================================
|
||||
# Start the Graphics Client
|
||||
#==========================================
|
||||
varServerPort = trick.var_server_get_port();
|
||||
|
||||
# This will definitely change to something else
|
||||
PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example"
|
||||
|
||||
|
||||
if (os.path.isfile(PoolTableDisplay_path)) :
|
||||
PoolTableDisplay_cmd = PoolTableDisplay_path \
|
||||
+ " " + str(varServerPort) + " &" ;
|
||||
print(PoolTableDisplay_cmd)
|
||||
os.system( PoolTableDisplay_cmd);
|
||||
else :
|
||||
print('=================================================================================================')
|
||||
print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||
print('=================================================================================================')
|
||||
|
||||
|
||||
# PoolTableDisplay_path = "models/graphics/java/dist/PoolTableDisplay.jar"
|
||||
|
||||
# if (os.path.isfile(PoolTableDisplay_path)) :
|
||||
# PoolTableDisplay_cmd = "java -jar " \
|
||||
# + PoolTableDisplay_path \
|
||||
# + " " + str(varServerPort) + " &" ;
|
||||
# print(PoolTableDisplay_cmd)
|
||||
# os.system( PoolTableDisplay_cmd);
|
||||
# else :
|
||||
# print('=================================================================================================')
|
||||
# print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||
# print('=================================================================================================')
|
||||
@ -1,14 +1,123 @@
|
||||
|
||||
import math
|
||||
|
||||
exec(open("./Modified_data/realtime.py").read())
|
||||
|
||||
dyn.table.numBalls = 2
|
||||
dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls)
|
||||
|
||||
id1 = dyn.table.addBall(-2, .5, 1, 1, False);
|
||||
id2 = dyn.table.addBall(1, .5, 1, 1, False);
|
||||
ballRadius = 0.02
|
||||
ballMass = 1
|
||||
|
||||
unit_pos = [math.sqrt(3)/2, 0.5]
|
||||
unit_neg = [unit_pos[0], -unit_pos[1]]
|
||||
center_x = 0.2;
|
||||
center_y = 0;
|
||||
tol = 1e-4;
|
||||
|
||||
dyn.table.defaultCueBallX = -0.05;
|
||||
dyn.table.defaultCueBallY = 0.1;
|
||||
dyn.table.addBall(-0.05, 0.1, ballMass, ballRadius, False)
|
||||
|
||||
dyn.table.addBall(-0.055, 0.15, ballMass, ballRadius, False)
|
||||
|
||||
# dyn.table.addBall(center_x+unit_neg[0]*2*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
# dyn.table.addBall(center_x+unit_pos[0]*2*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
|
||||
# dyn.table.addBall(center_x+unit_neg[0]*4*(ballRadius+tol), center_y+unit_neg[1]*4*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
# dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y+unit_pos[1]*4*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
# dyn.table.addBall(center_x+unit_pos[0]*4*(ballRadius+tol), center_y, ballMass, ballRadius, False)
|
||||
|
||||
# dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*6*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
# dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*6*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
# dyn.table.addBall(center_x+unit_neg[0]*6*(ballRadius+tol), center_y+unit_neg[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
# dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*2*(ballRadius+tol), ballMass, ballRadius, False)
|
||||
|
||||
corners = [-.5, -.25, .5, .25]
|
||||
|
||||
|
||||
|
||||
# Make a normal pool table
|
||||
pocketRadius = 0.04
|
||||
bumperWidth = 0.03
|
||||
|
||||
|
||||
dyn.table.numTablePoints = 2
|
||||
dyn.table.tableShape = trick.TMM_declare_var_1d("Point*", dyn.table.numTablePoints)
|
||||
dyn.table.tableShapeType = 3 # rectangle
|
||||
dyn.table.addPointToTable(corners[0], corners[1])
|
||||
dyn.table.addPointToTable(corners[2], corners[3])
|
||||
|
||||
# Pockets
|
||||
dyn.table.numPockets = 6
|
||||
dyn.table.pockets = trick.TMM_declare_var_1d("Pocket*", dyn.table.numPockets)
|
||||
pocketCenters = [[0, corners[1]],
|
||||
[0, corners[3]],
|
||||
[corners[0],corners[1]],
|
||||
[corners[0],corners[3]],
|
||||
[corners[2],corners[1]],
|
||||
[corners[2],corners[3]] ]
|
||||
|
||||
for coord in pocketCenters:
|
||||
dyn.table.addPocket(coord[0], coord[1], pocketRadius)
|
||||
|
||||
|
||||
bumperBorders =[[corners[0]+pocketRadius+bumperWidth, corners[1]+bumperWidth, 0.0-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom left
|
||||
[0.0+pocketRadius+bumperWidth, corners[1]+bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[1]+bumperWidth], # Bottom Right
|
||||
[corners[0]+pocketRadius+bumperWidth, corners[3]-bumperWidth, 0.0-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top left
|
||||
[0.0+pocketRadius+bumperWidth, corners[3]-bumperWidth, corners[2]-pocketRadius-bumperWidth,corners[3]-bumperWidth], # Top right
|
||||
[corners[0]+bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[0]+bumperWidth, corners[3]-pocketRadius-bumperWidth], # Left
|
||||
[corners[2]-bumperWidth, corners[1]+pocketRadius+bumperWidth, corners[2]-bumperWidth, corners[3]-pocketRadius-bumperWidth]] # Right
|
||||
|
||||
# triangles
|
||||
bumperBorders.extend([[bumperBorders[0][0]-bumperWidth, bumperBorders[0][1]-bumperWidth, bumperBorders[0][0], bumperBorders[0][1]], # Bottom left
|
||||
[bumperBorders[0][2], bumperBorders[0][1], bumperBorders[0][2]+bumperWidth, bumperBorders[0][1]-bumperWidth],
|
||||
[bumperBorders[1][0]-bumperWidth, bumperBorders[1][1]-bumperWidth, bumperBorders[1][0], bumperBorders[1][1]], # Bottom Right
|
||||
[bumperBorders[1][2], bumperBorders[1][1], bumperBorders[1][2]+bumperWidth, bumperBorders[1][1]-bumperWidth],
|
||||
[bumperBorders[2][0]-bumperWidth, bumperBorders[2][1]+bumperWidth, bumperBorders[2][0], bumperBorders[2][1]], # Top left
|
||||
[bumperBorders[2][2], bumperBorders[2][1], bumperBorders[2][2]+bumperWidth, bumperBorders[2][1]+bumperWidth],
|
||||
[bumperBorders[3][0]-bumperWidth, bumperBorders[3][1]+bumperWidth, bumperBorders[3][0], bumperBorders[3][1]], # Top right
|
||||
[bumperBorders[3][2], bumperBorders[3][1], bumperBorders[3][2]+bumperWidth, bumperBorders[3][1]+bumperWidth],
|
||||
[bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][1]-bumperWidth], # Left
|
||||
[bumperBorders[4][0]-bumperWidth, bumperBorders[4][3]+bumperWidth, bumperBorders[4][2], bumperBorders[4][3]],
|
||||
[bumperBorders[5][0], bumperBorders[5][1], bumperBorders[5][2]+bumperWidth, bumperBorders[5][1]-bumperWidth], # Left
|
||||
[bumperBorders[5][0]+bumperWidth, bumperBorders[5][3]+bumperWidth, bumperBorders[5][2], bumperBorders[5][3]],
|
||||
])
|
||||
|
||||
|
||||
|
||||
bumperShapes = [ [bumperBorders[0][0], bumperBorders[0][1]-bumperWidth, bumperBorders[0][2], bumperBorders[0][3]],
|
||||
[bumperBorders[1][0], bumperBorders[1][1]-bumperWidth, bumperBorders[1][2], bumperBorders[1][3]],
|
||||
[bumperBorders[2][0], bumperBorders[2][1]+bumperWidth, bumperBorders[2][2], bumperBorders[2][3]],
|
||||
[bumperBorders[3][0], bumperBorders[3][1]+bumperWidth, bumperBorders[3][2], bumperBorders[3][3]],
|
||||
[bumperBorders[4][0], bumperBorders[4][1], bumperBorders[4][2]-bumperWidth, bumperBorders[4][3],],
|
||||
[bumperBorders[5][0]+bumperWidth, bumperBorders[5][1], bumperBorders[5][2], bumperBorders[5][3],],
|
||||
[bumperBorders[6][0], bumperBorders[6][1], bumperBorders[6][2], bumperBorders[6][3], bumperBorders[6][2], bumperBorders[6][1]],
|
||||
[bumperBorders[7][0], bumperBorders[7][1], bumperBorders[7][2], bumperBorders[7][3], bumperBorders[7][0], bumperBorders[7][3]],
|
||||
[bumperBorders[8][0], bumperBorders[8][1], bumperBorders[8][2], bumperBorders[8][3], bumperBorders[8][2], bumperBorders[8][1]],
|
||||
[bumperBorders[9][0], bumperBorders[9][1], bumperBorders[9][2], bumperBorders[9][3], bumperBorders[9][0], bumperBorders[9][3]],
|
||||
[bumperBorders[10][0], bumperBorders[10][1], bumperBorders[10][2], bumperBorders[10][3], bumperBorders[10][2], bumperBorders[10][1]],
|
||||
[bumperBorders[11][0], bumperBorders[11][1], bumperBorders[11][2], bumperBorders[11][3], bumperBorders[11][0], bumperBorders[11][3]],
|
||||
[bumperBorders[12][0], bumperBorders[12][1], bumperBorders[12][2], bumperBorders[12][3], bumperBorders[12][2], bumperBorders[12][1]],
|
||||
[bumperBorders[13][0], bumperBorders[13][1], bumperBorders[13][2], bumperBorders[13][3], bumperBorders[13][0], bumperBorders[13][3]],
|
||||
[bumperBorders[14][0], bumperBorders[14][1], bumperBorders[14][2], bumperBorders[14][3], bumperBorders[14][2], bumperBorders[14][1]],
|
||||
[bumperBorders[15][0], bumperBorders[15][1], bumperBorders[15][2], bumperBorders[15][3], bumperBorders[15][0], bumperBorders[15][3]],
|
||||
[bumperBorders[16][0], bumperBorders[16][1], bumperBorders[16][2], bumperBorders[16][3], bumperBorders[16][2], bumperBorders[16][1]],
|
||||
[bumperBorders[17][0], bumperBorders[17][1], bumperBorders[17][2], bumperBorders[17][3], bumperBorders[17][0], bumperBorders[17][3]]]
|
||||
|
||||
bumperShapeTypes = [3, 3, 3, 3, 3, 3, 2, 2,2,2,2, 2, 2, 2,2,2, 2, 2]
|
||||
|
||||
dyn.table.numBumpers = len(bumperBorders)
|
||||
dyn.table.bumpers = trick.TMM_declare_var_1d("Bumper*", dyn.table.numBumpers)
|
||||
|
||||
for i in range(len(bumperBorders)):
|
||||
id = dyn.table.addBumper(len(bumperShapes[i])/2, bumperBorders[i][0], bumperBorders[i][1], bumperBorders[i][2],bumperBorders[i][3])
|
||||
dyn.table.bumpers[id][0].numPoints = len(bumperShapes[i])/2
|
||||
dyn.table.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Point*", dyn.table.bumpers[id].numPoints)
|
||||
|
||||
dyn.table.bumpers[id][0].shapeType = bumperShapeTypes[i]
|
||||
for j in range(0, len(bumperShapes[i]), 2):
|
||||
dyn.table.addPointToBumper(id, bumperShapes[i][j],bumperShapes[i][j+1])
|
||||
|
||||
dyn.table.setBallVel(id1, 0.5, 0);
|
||||
|
||||
dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
|
||||
|
||||
@ -16,15 +125,31 @@ dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
|
||||
# Start the Graphics Client
|
||||
#==========================================
|
||||
varServerPort = trick.var_server_get_port();
|
||||
PoolTableDisplay_path = "models/graphics/dist/PoolTableDisplay.jar"
|
||||
|
||||
# This will definitely change to something else
|
||||
PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example"
|
||||
|
||||
|
||||
if (os.path.isfile(PoolTableDisplay_path)) :
|
||||
PoolTableDisplay_cmd = "java -jar " \
|
||||
+ PoolTableDisplay_path \
|
||||
PoolTableDisplay_cmd = PoolTableDisplay_path \
|
||||
+ " " + str(varServerPort) + " &" ;
|
||||
print(PoolTableDisplay_cmd)
|
||||
os.system( PoolTableDisplay_cmd);
|
||||
else :
|
||||
print('============================================================================================')
|
||||
print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||
print('============================================================================================')
|
||||
print('=================================================================================================')
|
||||
print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||
print('=================================================================================================')
|
||||
|
||||
|
||||
# PoolTableDisplay_path = "models/graphics/java/dist/PoolTableDisplay.jar"
|
||||
|
||||
# if (os.path.isfile(PoolTableDisplay_path)) :
|
||||
# PoolTableDisplay_cmd = "java -jar " \
|
||||
# + PoolTableDisplay_path \
|
||||
# + " " + str(varServerPort) + " &" ;
|
||||
# print(PoolTableDisplay_cmd)
|
||||
# os.system( PoolTableDisplay_cmd);
|
||||
# else :
|
||||
# print('=================================================================================================')
|
||||
# print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||
# print('=================================================================================================')
|
||||
|
||||
@ -17,8 +17,9 @@ class PoolTableSimObject : public Trick::SimObject {
|
||||
("derivative") table.state_deriv() ;
|
||||
("integration") trick_ret = table.state_integ() ;
|
||||
("dynamic_event") table.collision() ;
|
||||
// ("dynamic_event") table.bumperCollision() ;
|
||||
}
|
||||
};
|
||||
|
||||
PoolTableSimObject dyn;
|
||||
IntegLoop dyn_integloop(0.1) dyn;
|
||||
IntegLoop dyn_integloop(0.001) dyn;
|
||||
|
||||
36
trick_sims/SIM_pool/models/graphics/java/Makefile
Normal file
36
trick_sims/SIM_pool/models/graphics/java/Makefile
Normal file
@ -0,0 +1,36 @@
|
||||
SHELL = /bin/sh
|
||||
|
||||
PROJECT_NAME = PoolTableDisplay
|
||||
SRC_DIR = src
|
||||
BUILD_DIR = build
|
||||
CLASSES_DIR = $(BUILD_DIR)/classes
|
||||
JAR_DIR = dist
|
||||
MAIN_CLASS = PoolTableDisplay
|
||||
|
||||
all: jar
|
||||
|
||||
clean:
|
||||
rm -rf $(BUILD_DIR)
|
||||
rm -f manifest
|
||||
|
||||
spotless: clean
|
||||
rm -rf dist
|
||||
|
||||
$(CLASSES_DIR):
|
||||
@ mkdir -p $(CLASSES_DIR)
|
||||
|
||||
compile: | $(CLASSES_DIR)
|
||||
javac -sourcepath $(SRC_DIR) -d $(CLASSES_DIR) $(SRC_DIR)/PoolTableDisplay.java
|
||||
|
||||
manifest:
|
||||
@ echo "Main-Class: $(MAIN_CLASS)" > $@
|
||||
|
||||
$(JAR_DIR):
|
||||
@ mkdir -p $(JAR_DIR)
|
||||
|
||||
jar: compile manifest | $(JAR_DIR)
|
||||
jar cvfm $(JAR_DIR)/$(PROJECT_NAME).jar manifest -C $(CLASSES_DIR) .
|
||||
@ echo "-------------------------------------------------------------------------------"
|
||||
@ echo " BUILD COMPLETE"
|
||||
@ echo "The Java jar file (the Java Executable) is located at: $(JAR_DIR)/$(PROJECT_NAME).jar"
|
||||
@ echo "-------------------------------------------------------------------------------"
|
||||
BIN
trick_sims/SIM_pool/models/graphics/java/dist/PoolTableDisplay.jar
vendored
Normal file
BIN
trick_sims/SIM_pool/models/graphics/java/dist/PoolTableDisplay.jar
vendored
Normal file
Binary file not shown.
1
trick_sims/SIM_pool/models/graphics/java/manifest
Normal file
1
trick_sims/SIM_pool/models/graphics/java/manifest
Normal file
@ -0,0 +1 @@
|
||||
Main-Class: PoolTableDisplay
|
||||
@ -24,7 +24,7 @@ import javax.swing.JPanel;
|
||||
|
||||
|
||||
class Ball {
|
||||
static int numColors = 9;
|
||||
static int numColors = 8;
|
||||
static Color[] colorList = {
|
||||
Color.WHITE,
|
||||
Color.YELLOW,
|
||||
@ -45,7 +45,7 @@ class Ball {
|
||||
identity = id;
|
||||
x = 0.0;
|
||||
y = 0.0;
|
||||
radius = 0.5;
|
||||
radius = 1.0;
|
||||
color = colorList[id % numColors];
|
||||
}
|
||||
}
|
||||
@ -159,8 +159,8 @@ class RangeView extends JPanel {
|
||||
int bx = (int)(worldOriginX + scale * balls[ii].x);
|
||||
int by = (int)(worldOriginY - scale * balls[ii].y);
|
||||
drawCenteredCircle(g2d, bx, by, (int)(scale * 2 * balls[ii].radius));
|
||||
g2d.setPaint(Color.BLACK);
|
||||
g2d.drawString ( String.format("%d",ii), bx,by);
|
||||
// g2d.setPaint(Color.BLACK);
|
||||
// g2d.drawString ( String.format("%d",ii), bx,by);
|
||||
}
|
||||
|
||||
g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
|
||||
@ -294,7 +294,7 @@ public class PoolTableDisplay extends JFrame {
|
||||
line = poolTableDisplay.in.readLine();
|
||||
field = line.split("\t");
|
||||
for ( ii=0; ii < nballs; ii++) {
|
||||
// poolTableDisplay.rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]);
|
||||
poolTableDisplay.rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]);
|
||||
}
|
||||
} catch (IOException | NullPointerException e ) {
|
||||
go = false;
|
||||
@ -0,0 +1 @@
|
||||
Subproject commit a9b087f9d8e9cce32ab68705128164cd5d4ad81a
|
||||
@ -7,9 +7,9 @@ LIBRARY DEPENDENCY:
|
||||
#define _ball_hh_
|
||||
|
||||
// #include <Eigen/Core>
|
||||
#include "common_geometry.hh"
|
||||
|
||||
// monotonically increasing ID
|
||||
static int id = 0;
|
||||
|
||||
class Ball {
|
||||
|
||||
@ -18,19 +18,47 @@ class Ball {
|
||||
Ball(double x, double y, double mass, double radius, bool isFixed, int id);
|
||||
Ball () {}
|
||||
|
||||
void setPos(double x, double y);
|
||||
void setPos(double x, double y, double z);
|
||||
|
||||
void setVel(double x, double y);
|
||||
void setVel(double x, double y, double z);
|
||||
|
||||
void setAccel(double x, double y);
|
||||
void setAccel(double x, double y, double z);
|
||||
|
||||
// void setRelativeVel(double x, double y);
|
||||
void setRelativeVel(double x, double y, double z);
|
||||
|
||||
// void setAngularVel(double x, double y);
|
||||
void setAngularVel(double x, double y, double z);
|
||||
|
||||
// void setAngularAccel(double x, double y);
|
||||
void setAngularAccel(double x, double y, double z);
|
||||
|
||||
void clearAllState();
|
||||
|
||||
|
||||
// Z component should always be 0, unless someone tries to add jumps in the future
|
||||
double pos[3];
|
||||
double prevPos[3];
|
||||
double vel[3];
|
||||
// Used to store derivatives between deriv and integration steps
|
||||
double accel[3];
|
||||
// double pos[3];
|
||||
// double prevPos[3]; // Maybe don't need this anymore?
|
||||
// double vel[3];
|
||||
// // Used to store derivatives between deriv and integration steps
|
||||
// double accel[3];
|
||||
|
||||
// Relating to angular velocity
|
||||
double relativeVel[3];
|
||||
double w[3];
|
||||
double angular_accel[3];
|
||||
// // Relating to angular velocity
|
||||
// double relativeVel[3];
|
||||
// double w[3];
|
||||
// double angular_accel[3];
|
||||
Vec pos;
|
||||
Vec prevPos;
|
||||
Vec vel;
|
||||
Vec accel;
|
||||
Vec relativeVel;
|
||||
Vec w;
|
||||
Vec angularAccel;
|
||||
|
||||
double color[3];
|
||||
// double color[3];
|
||||
|
||||
double mass;
|
||||
double radius;
|
||||
@ -38,6 +66,8 @@ class Ball {
|
||||
bool isCue;
|
||||
int sliding;
|
||||
|
||||
bool inPlay = true;
|
||||
|
||||
unsigned int id;
|
||||
};
|
||||
|
||||
|
||||
@ -7,39 +7,25 @@ LIBRARY DEPENDENCY:
|
||||
#define _bumper_hh_
|
||||
|
||||
#include <vector>
|
||||
|
||||
// this should definitely go somewhere else
|
||||
// maybe make a geometry libary
|
||||
class Point {
|
||||
public:
|
||||
double x;
|
||||
double y;
|
||||
|
||||
Point(double x, double y) : x(x), y(y) {}
|
||||
Point() {}
|
||||
};
|
||||
|
||||
class Line {
|
||||
public:
|
||||
Point p1;
|
||||
Point p2;
|
||||
|
||||
Line (Point p1, Point p2) : p1(p1), p2(p2) {}
|
||||
Line () {}
|
||||
};
|
||||
#include "common_geometry.hh"
|
||||
|
||||
class Bumper {
|
||||
public:
|
||||
// Have to have a default constructor or trick freaks out
|
||||
Bumper();
|
||||
Bumper(int numPoints, double x1, double y1, double x2, double y);
|
||||
|
||||
void AddPointToRender(double x, double y);
|
||||
void AddBorder (double x1, double y1, double x2, double y2);
|
||||
int id;
|
||||
Line border;
|
||||
Vec ** renderedShape;
|
||||
unsigned int numPoints;
|
||||
enum PolygonType shapeType;
|
||||
|
||||
|
||||
private:
|
||||
// Actual line that can be collided with
|
||||
Line border;
|
||||
|
||||
// Shape that should be rendered
|
||||
// Size should be dynamic
|
||||
std::vector<Point *> renderedShape;
|
||||
int nextPointSlot = 0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -0,0 +1,77 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Geometry primitives to be used elsewhere. )
|
||||
LIBRARY DEPENDENCY:
|
||||
(())
|
||||
*******************************************************************************/
|
||||
#ifndef _common_hh_
|
||||
#define _common_hh_
|
||||
|
||||
// Should maybe swap this for eigen stuff at some point
|
||||
class Vec {
|
||||
public:
|
||||
Vec(double x, double y) : _x(x), _y(y), _z(0) {}
|
||||
Vec(double x, double y, double z) : _x(x), _y(y), _z(z) {}
|
||||
Vec() {}
|
||||
|
||||
Vec operator+ (const Vec& other) {
|
||||
Vec sum;
|
||||
sum._x = _x + other._x;
|
||||
sum._y = _y + other._y;
|
||||
sum._z = _z + other._z;
|
||||
return sum;
|
||||
}
|
||||
|
||||
Vec operator* (double scale) {
|
||||
Vec ret;
|
||||
ret._x = _x * scale;
|
||||
ret._y = _y * scale;
|
||||
ret._z = _z * scale;
|
||||
return ret;
|
||||
}
|
||||
|
||||
double& operator() (int index) {
|
||||
if (index == 0) {
|
||||
return _x;
|
||||
} else if (index == 1) {
|
||||
return _y;
|
||||
} else if (index == 2) {
|
||||
return _z;
|
||||
}
|
||||
|
||||
// Throw an error i guess
|
||||
}
|
||||
|
||||
Vec normalized () {
|
||||
Vec norm;
|
||||
|
||||
}
|
||||
|
||||
double& x () { return _x; }
|
||||
double& y () { return _y; }
|
||||
double& z () { return _z; }
|
||||
|
||||
|
||||
private:
|
||||
double _x;
|
||||
double _y;
|
||||
double _z;
|
||||
};
|
||||
|
||||
class Line {
|
||||
public:
|
||||
Vec p1;
|
||||
Vec p2;
|
||||
|
||||
Line (Vec p1, Vec p2) : p1(p1), p2(p2) {}
|
||||
Line () {}
|
||||
};
|
||||
|
||||
enum PolygonType {
|
||||
GENERIC,
|
||||
CIRCLE,
|
||||
TRIANGLE,
|
||||
RECTANGLE,
|
||||
QUAD
|
||||
};
|
||||
|
||||
#endif
|
||||
24
trick_sims/SIM_pool/models/pool_table/include/pocket.hh
Normal file
24
trick_sims/SIM_pool/models/pool_table/include/pocket.hh
Normal file
@ -0,0 +1,24 @@
|
||||
/********************************* 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) : x(x), y(y), radius(r) {}
|
||||
|
||||
double x;
|
||||
double y;
|
||||
double radius;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -9,6 +9,7 @@ LIBRARY DEPENDENCIES:
|
||||
#include "trick/regula_falsi.h"
|
||||
#include "ball.hh"
|
||||
#include "bumper.hh"
|
||||
#include "pocket.hh"
|
||||
#include <vector>
|
||||
|
||||
|
||||
@ -17,42 +18,78 @@ class PoolTable {
|
||||
public:
|
||||
PoolTable () : numBalls(0), numAssociations(0) {}
|
||||
|
||||
int addBall (double x, double y, double mass, double radius, bool fixed);
|
||||
int addBumper (double x1, double y1, double x2, double y2);
|
||||
|
||||
int setBallPos(int id, double x, double y);
|
||||
int setBallVel(int id, double v_x, double v_y);
|
||||
|
||||
// Ball ** balls;
|
||||
// Bumper ** bumpers;
|
||||
Ball** balls;
|
||||
Bumper** bumpers;
|
||||
|
||||
// Ball-ball collisions
|
||||
int nextBallSlot = 0;
|
||||
unsigned int numBalls;
|
||||
unsigned int numAssociations;
|
||||
REGULA_FALSI* ballAssociations;
|
||||
|
||||
// Ball-bumper collisions
|
||||
unsigned int numBumpers;
|
||||
unsigned int numCombos;
|
||||
REGULA_FALSI* bumperAssociations;
|
||||
|
||||
//void ballCollision(Ball &b1, Ball &b2);
|
||||
|
||||
int default_data();
|
||||
int state_init();
|
||||
int state_deriv();
|
||||
int state_integ();
|
||||
double collision();
|
||||
// double bumperCollision();
|
||||
|
||||
int addBall (double x, double y, double mass, double radius, bool fixed);
|
||||
int addBumper (int numPoints, double x1, double y1, double x2, double y2);
|
||||
int addPointToBumper(int id, double x, double y);
|
||||
int addPointToTable(double x, double y);
|
||||
int addPocket(double x, double y, double r);
|
||||
|
||||
int setBallPos(int id, double x, double y);
|
||||
int setBallVel(int id, double v_x, double v_y);
|
||||
|
||||
void applyCueForce(double x_end, double y_end);
|
||||
void applyCueForce(double x_end, double y_end, double cueHorizontalDisplacement, double cueVerticalDisplacement, double cueAngle);
|
||||
|
||||
void resetCueBall(double x, double y);
|
||||
void resetCueBall();
|
||||
|
||||
double removeBall(int id);
|
||||
|
||||
|
||||
// State variables
|
||||
Ball** balls;
|
||||
|
||||
// Table parameters
|
||||
// Bumpers and pockets are used by sim, tableShape is just used by graphics client
|
||||
Bumper** bumpers;
|
||||
Pocket** pockets;
|
||||
Vec** tableShape;
|
||||
|
||||
unsigned int numBumpers;
|
||||
unsigned int numPockets;
|
||||
unsigned int numTablePoints;
|
||||
|
||||
int nextBallSlot = 0;
|
||||
int nextBumperSlot = 0;
|
||||
int nextPocketSlot = 0;
|
||||
int nextTablePointSlot = 0;
|
||||
|
||||
enum PolygonType tableShapeType;
|
||||
|
||||
// Ball-ball collisions
|
||||
unsigned int numBalls;
|
||||
unsigned int numAssociations;
|
||||
REGULA_FALSI* ballAssociations;
|
||||
|
||||
// Ball-bumper collisions
|
||||
unsigned int bumperBallCombos;
|
||||
REGULA_FALSI* bumperAssociations;
|
||||
|
||||
// Ball-pocket collisions
|
||||
unsigned int pocketBallCombos;
|
||||
REGULA_FALSI* pocketAssociations;
|
||||
|
||||
// Sim constants that should be user-controllable
|
||||
double frictionRolling = 0.05;
|
||||
double frictionSliding = 0.25;
|
||||
double frictionScale = 1;
|
||||
double frictionTolerance = 0.0005;
|
||||
double coefficientOfElasticity = 0.99;
|
||||
double coefficientOfElasticity = .95;
|
||||
double cueForceScale = 1.0;
|
||||
double cueMass = 1.0;
|
||||
|
||||
int cueBallIndex = 0;
|
||||
double defaultCueBallX = -0.3;
|
||||
double defaultCueBallY = 0;
|
||||
|
||||
bool allowCollisions = true;
|
||||
|
||||
};
|
||||
#endif
|
||||
@ -6,30 +6,57 @@ LIBRARY DEPENDENCY:
|
||||
#include "../include/ball.hh"
|
||||
// #include "trick/memorymanager_c_intf.h"
|
||||
|
||||
#include "trick/MemoryManager.hh"
|
||||
extern Trick::MemoryManager* trick_MM;
|
||||
// #include "trick/MemoryManager.hh"
|
||||
// extern Trick::MemoryManager* trick_MM;
|
||||
|
||||
#include <new>
|
||||
|
||||
Ball::Ball(double x, double y, double mass, double radius, bool isFixed, int id) :
|
||||
mass(mass),
|
||||
radius(radius),
|
||||
fixed(isFixed),
|
||||
id(id)
|
||||
{
|
||||
pos[0] = x;
|
||||
pos[1] = y;
|
||||
pos[2] = 0;
|
||||
pos = Vec(x, y, 0);
|
||||
}
|
||||
|
||||
// Ball* CreateBall(double x, double y, double mass, double radius, bool isFixed) {
|
||||
// // Ball* b = (Ball*)TMM_declare_var_s("Ball");
|
||||
// Ball *b = new Ball(x, y, mass, radius, isFixed, 0);
|
||||
// trick_MM->declare_extern_var ( b, "Ball");
|
||||
// // Ball *b = (Ball*)trick_MM->declare_var("Ball");
|
||||
void Ball::setPos(double x, double y) { setPos(x, y, 0); }
|
||||
void Ball::setPos(double x, double y, double z) {
|
||||
pos = Vec(x, y, z);
|
||||
}
|
||||
|
||||
void Ball::setVel(double x, double y) { setVel(x, y, 0); }
|
||||
void Ball::setVel(double x, double y, double z) {
|
||||
vel = Vec(x, y, z);
|
||||
}
|
||||
|
||||
void Ball::setAccel(double x, double y) { setAccel(x, y, 0); }
|
||||
void Ball::setAccel(double x, double y, double z) {
|
||||
accel = Vec(x, y, z);
|
||||
}
|
||||
|
||||
void Ball::setRelativeVel(double x, double y, double z) {
|
||||
relativeVel = Vec(x, y, z);
|
||||
}
|
||||
|
||||
void Ball::setAngularVel(double x, double y, double z) {
|
||||
w = Vec(x, y, z);
|
||||
}
|
||||
|
||||
void Ball::setAngularAccel(double x, double y, double z) {
|
||||
angularAccel = Vec(x, y, z);
|
||||
}
|
||||
|
||||
void Ball::clearAllState() {
|
||||
setPos(0, 0, 0);
|
||||
setVel(0, 0, 0);
|
||||
setAccel(0, 0, 0);
|
||||
setRelativeVel(0, 0, 0);
|
||||
setAngularVel(0, 0, 0);
|
||||
setAngularAccel(0, 0, 0);
|
||||
sliding = false;
|
||||
}
|
||||
|
||||
// // return (new (b) Ball(x, y, mass, radius, isFixed, 0));
|
||||
// return b;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,9 +1,25 @@
|
||||
#include "bumper.hh"
|
||||
#include "stdlib.h"
|
||||
|
||||
#include "trick/memorymanager_c_intf.h"
|
||||
|
||||
Bumper::Bumper() {}
|
||||
|
||||
Bumper::Bumper(int numPoints, double x1, double y1, double x2, double y2) :
|
||||
border(Line(Vec(x1, y1), Vec(x2, y2))),
|
||||
numPoints(numPoints)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void Bumper::AddPointToRender(double x, double y) {
|
||||
|
||||
int id = nextPointSlot++;
|
||||
if (id < numPoints) {
|
||||
Vec * point = (Vec*) TMM_declare_var_s("Vec");
|
||||
renderedShape[id] = (new (point) Vec(x, y));
|
||||
}
|
||||
}
|
||||
|
||||
void Bumper::AddBorder (double x1, double y1, double x2, double y2) {
|
||||
|
||||
border = Line(Vec(x1, y1), Vec(x2, y2));
|
||||
}
|
||||
@ -29,6 +29,11 @@ void scaleInPlace (double vec[], double scale, int dim) {
|
||||
}
|
||||
}
|
||||
|
||||
void mProduct (double product[2], double matrix[2][2], double vec[2]) {
|
||||
product[0] = matrix[0][0] * vec[0] + matrix[1][0] * vec[1];
|
||||
product[1] = matrix[0][1] * vec[0] + matrix[1][1] * vec[1];
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -36,14 +41,6 @@ int PoolTable::default_data() {
|
||||
// balls.clear();
|
||||
// bumpers.clear();
|
||||
|
||||
|
||||
// Dev Testing only - should be deleted
|
||||
// int id1 = addBall(0.5, 0.5, 1, 1, false);
|
||||
// int id2 = addBall(0.75, 0.5, 1, 1, false);
|
||||
|
||||
// setBallVel(id1, 0.1, 0);
|
||||
///////////////////////////////////////////
|
||||
|
||||
numBalls = 0;
|
||||
numBumpers = 0;
|
||||
return 0;
|
||||
@ -69,7 +66,30 @@ int PoolTable::state_init() {
|
||||
}
|
||||
}
|
||||
|
||||
// Need to do the same thing with rail/ball associations
|
||||
bumperBallCombos = numBalls * numBumpers;
|
||||
bumperAssociations = (REGULA_FALSI*)TMM_declare_var_1d("REGULA_FALSI", bumperBallCombos);
|
||||
for (ii=0; 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;
|
||||
}
|
||||
@ -85,15 +105,13 @@ int PoolTable::state_deriv() {
|
||||
|
||||
for (int i = 0; i < numBalls; i++) {
|
||||
// Frictional force a constant applied in the opposing direction of velocity.
|
||||
// Magnitude of velocity is irrelevant
|
||||
|
||||
// std::cout << "ActuaVelocity: " << balls[i]->vel[0] << " " << balls[i]->vel[1] << std::endl;
|
||||
// std::cout << "Velocity Norm: " << velocityNorm[0] << " " << velocityNorm[1] << std::endl;
|
||||
|
||||
// balls[i]->accel[0] = 0;
|
||||
// balls[i]->accel[1] = 0;
|
||||
|
||||
// Magnitude of velocity is irrelevant unless very close to 0
|
||||
// Has weird behavior when velocity is very small, so only apply friction if velocity is greater than a tolerance
|
||||
|
||||
if (!balls[i]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (abs(dv_mag(balls[i]->vel)) > frictionTolerance) {
|
||||
double velocityNorm[3];
|
||||
dv_norm(velocityNorm, balls[i]->vel);
|
||||
@ -125,6 +143,10 @@ int PoolTable::state_integ() {
|
||||
int n = 6;
|
||||
|
||||
for (int i = 0; i < numBalls; i++) {
|
||||
if (!balls[i]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// State - Need to load 4 values for each ball, but will have to have more when we add angular stuff
|
||||
// pos[0] pos[1] vel[0] vel[1]
|
||||
int inner_index = 0;
|
||||
@ -153,6 +175,10 @@ int PoolTable::state_integ() {
|
||||
int integration_step = integrate();
|
||||
|
||||
for (int i = 0; i < numBalls; i++) {
|
||||
if (!balls[i]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// pos[0] pos[1] vel[0] vel[1]
|
||||
int inner_index = 0;
|
||||
balls[i]->pos[0] = unload_indexed_state(n*i + inner_index++);
|
||||
@ -162,23 +188,55 @@ int PoolTable::state_integ() {
|
||||
balls[i]->vel[0] = unload_indexed_state(n*i + inner_index++);
|
||||
balls[i]->vel[1] = unload_indexed_state(n*i + inner_index++);
|
||||
balls[i]->vel[2] = unload_indexed_state(n*i + inner_index++);
|
||||
|
||||
balls[i]->accel[0] = 0;
|
||||
balls[i]->accel[1] = 0;
|
||||
balls[i]->accel[2] = 0;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
return integration_step;
|
||||
}
|
||||
|
||||
double closestPointOnLine(Line& line, double pos[2], double result[2], bool print) {
|
||||
double m[3];
|
||||
double a[3] = {line.p1.x, line.p1.y};
|
||||
double b[3] = {line.p2.x, line.p2.y};
|
||||
double diff[3];
|
||||
|
||||
dv_sub(diff, pos, a);
|
||||
dv_sub(m, b, a);
|
||||
|
||||
double t = dot(diff, m, 2) / dot(m, m, 2);
|
||||
|
||||
// if (print)
|
||||
// std::cout << "t: " << t << std::endl;
|
||||
|
||||
if (t < 0)
|
||||
t = 0;
|
||||
|
||||
if (t > 1)
|
||||
t = 1;
|
||||
|
||||
scaleInPlace(m, t, 2);
|
||||
dv_add(result, a, m);
|
||||
// if (print)
|
||||
// std::cout << "Result: " << result[0] << " " << result[1] << std::endl;
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
double PoolTable::removeBall(int id) {
|
||||
balls[id]->inPlay = false;
|
||||
balls[id]->clearAllState();
|
||||
}
|
||||
|
||||
// Maybe need a separate scheduled job to handle pockets?
|
||||
// And the cue?
|
||||
// Maybe see if there's some "callback" that can handle user input
|
||||
// There must be, since other sims have control panels too
|
||||
|
||||
double PoolTable::collision() {
|
||||
// Handle when the balls collide with others or with a bumper
|
||||
|
||||
// Handle when the balls collide with others
|
||||
|
||||
double now ; /* current integration time. */
|
||||
unsigned int first, second;
|
||||
|
||||
unsigned int association_index;
|
||||
double event_tgo;
|
||||
unsigned int ii,jj;
|
||||
|
||||
@ -188,14 +246,26 @@ double PoolTable::collision() {
|
||||
std::vector<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;
|
||||
}
|
||||
|
||||
double diff[3];
|
||||
dv_sub(diff, balls[ii]->pos, balls[jj]->pos);
|
||||
double distanceBetweenBalls = dv_mag(diff);
|
||||
unsigned int associationIndex = ii*(ii-1)/2+jj;
|
||||
|
||||
// boundary is distance between balls - radiuses of balls
|
||||
ballAssociations[associationIndex].error = distanceBetweenBalls - (balls[ii]->radius + balls[jj]->radius);
|
||||
double error = distanceBetweenBalls - (balls[ii]->radius + balls[jj]->radius);
|
||||
|
||||
// std::cout << "Distance between centers: " << distanceBetweenBalls << std::endl;
|
||||
// std::cout << "Radiuses: " << balls[ii]->radius << " " << balls[jj]->radius << std::endl;
|
||||
// std::cout << "Error calculation: " << error << std::endl;
|
||||
ballAssociations[associationIndex].error = error;
|
||||
double this_tgo = regula_falsi( now, &(ballAssociations[associationIndex])) ;
|
||||
|
||||
if (this_tgo < event_tgo) {
|
||||
@ -203,20 +273,33 @@ double PoolTable::collision() {
|
||||
}
|
||||
|
||||
if (this_tgo == 0) {
|
||||
std::cout << "Found colliding balls" << std::endl;
|
||||
|
||||
// Add this collision to a list of collisions to process
|
||||
collisionsToProcess.push_back(ii);
|
||||
collisionsToProcess.push_back(jj);
|
||||
reset_regula_falsi( now, &(ballAssociations[associationIndex]) );
|
||||
|
||||
} else {
|
||||
regula_falsi_set_upper (now, error, &ballAssociations[associationIndex]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Handle collisions
|
||||
for (int i = 0; i < collisionsToProcess.size(); i+=2) {
|
||||
// if (allowCollisions == false) {
|
||||
// return event_tgo;
|
||||
// }
|
||||
// allowCollisions = false;
|
||||
int index1 = collisionsToProcess[i];
|
||||
int index2 = collisionsToProcess[i+1];
|
||||
std::cout << "Collision detected between balls " << index1 << " and " << index2 << std::endl;
|
||||
|
||||
double *q1 = balls[index1]->pos;
|
||||
double *q2 = balls[index2]->pos;
|
||||
double q1[3];
|
||||
dv_copy(q1, balls[index1]->pos);
|
||||
double q2[3];
|
||||
dv_copy(q2, balls[index2]->pos);
|
||||
|
||||
|
||||
// dg = (q1 - q2) / (|q1 - q2|)
|
||||
@ -234,15 +317,261 @@ double PoolTable::collision() {
|
||||
// J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg
|
||||
// For now let's just pretend all the masses are 1
|
||||
double impulse = ((1.0 + coefficientOfElasticity) * dot(dg4, vel4, 4)) / (dot(dg4, dg4, 4));
|
||||
scaleInPlace(dg, impulse, 4);
|
||||
scaleInPlace(dg4, impulse, 4);
|
||||
|
||||
// Impulse[0:1] is x and y components of what should be applied to v1, Impulse[2:3] goes to v2
|
||||
double impulse1[3] = {dg4[0], dg4[1], 0};
|
||||
double impulse2[3] = {dg4[2], dg4[3], 0};
|
||||
|
||||
double newV1[3];
|
||||
dv_copy(newV1, balls[index1]->vel);
|
||||
double newV2[3];
|
||||
dv_copy(newV2, balls[index2]->vel);
|
||||
|
||||
scaleInPlace(newV1, balls[index1]->mass, 3);
|
||||
scaleInPlace(newV2, balls[index2]->mass, 3);
|
||||
|
||||
dv_sub(newV1, newV1, impulse1);
|
||||
dv_sub(newV2, newV2, impulse2);
|
||||
|
||||
scaleInPlace(newV1, 1.0 / balls[index1]->mass, 3);
|
||||
scaleInPlace(newV2, 1.0 / balls[index2]->mass, 3);
|
||||
|
||||
std::cout << "Impulses applied: \n\tV1: " << newV1[0] << " " << newV1[1] << " \n\tV2: " << newV2[0] << " " << newV2[1] << std::endl;
|
||||
|
||||
balls[index1]->vel[0] = newV1[0];
|
||||
balls[index1]->vel[1] = newV1[1];
|
||||
|
||||
balls[index2]->vel[0] = newV2[0];
|
||||
balls[index2]->vel[1] = newV2[1];
|
||||
}
|
||||
|
||||
// std::cout << "Ball 0 pos: " << balls[0]->pos[0] << " " << balls[0]->pos[1] << std::endl;
|
||||
// std::cout << "Bumper 0 pos: " << bumpers[0]->border.p1.x << " " << bumpers[0]->border.p1.y << " " << bumpers[0]->border.p2.x << " " << bumpers[0]->border.p2.y << std::endl;
|
||||
|
||||
int numBumperCollisions = 0;
|
||||
for (ii=0; ii<numBalls; ii++) {
|
||||
if (!balls[ii]->inPlay) {
|
||||
continue;
|
||||
}
|
||||
// if (numBumperCollisions > 0)
|
||||
// break;
|
||||
for (jj=0; jj<numBumpers; jj++) {
|
||||
// if (numBumperCollisions > 0)
|
||||
// break;
|
||||
unsigned int association_index = (ii*numBumpers) + jj;
|
||||
Ball *ball = balls[ii];
|
||||
Bumper *bumper = bumpers[jj];
|
||||
//Point ballPos(ball->pos[0], ball->pos[1]);
|
||||
double closestPointOnBumper[3] = {0, 0, 0};
|
||||
if (ii == 0 && jj == 3) {
|
||||
double t = closestPointOnLine(bumper->border, ball->pos, closestPointOnBumper, true);
|
||||
// std::cout << "Closest point on bumper: " << closestPointOnBumper[0] << " " << closestPointOnBumper[1] << std::endl;
|
||||
|
||||
} else {
|
||||
double t = closestPointOnLine(bumper->border, ball->pos, closestPointOnBumper, false);
|
||||
}
|
||||
double diff[3];
|
||||
|
||||
// dv_sub(diff, ball->pos, closestPointOnBumper);
|
||||
diff[0] = ball->pos[0] - closestPointOnBumper[0];
|
||||
diff[1] = ball->pos[1] - closestPointOnBumper[1];
|
||||
diff[2] = 0;
|
||||
|
||||
double distanceToBumper = abs(dv_mag(diff)) - ball->radius;
|
||||
|
||||
bumperAssociations[association_index].error = distanceToBumper;
|
||||
// if (ii == 0 && jj == 3) {
|
||||
// std::cout << "Ball pos: " << balls[ii]->pos[0] << " " << balls[ii]->pos[1] << std::endl;
|
||||
// std::cout << "Closest point on bumper: " << closestPointOnBumper[0] << " " << closestPointOnBumper[1] << std::endl;
|
||||
// std::cout << "Distance to bumper: " << bumperAssociations[association_index].error << std::endl;
|
||||
// }
|
||||
double this_tgo = regula_falsi( now, &(bumperAssociations[association_index])) ;
|
||||
if (this_tgo < event_tgo) {
|
||||
event_tgo = this_tgo;
|
||||
}
|
||||
|
||||
if (this_tgo == 0) {
|
||||
numBumperCollisions++;
|
||||
std::cout << "Found colliding ball " << ii << " and bumper " << jj << std::endl;
|
||||
|
||||
reset_regula_falsi( now, &(bumperAssociations[association_index]) );
|
||||
|
||||
if (numBumperCollisions > 1)
|
||||
continue;
|
||||
|
||||
double q1[3];
|
||||
dv_copy(q1, ball->pos);
|
||||
double q2[3];
|
||||
dv_copy(q2, closestPointOnBumper);
|
||||
q1[2] = 0;
|
||||
q2[2] = 0;
|
||||
|
||||
|
||||
// dg = (q1 - q2) / (|q1 - q2|)
|
||||
double diff[3];
|
||||
dv_sub(diff, q1, q2);
|
||||
double dg[3];
|
||||
dv_norm(dg, diff);
|
||||
|
||||
// Have to stuff both velocities and dg values into 4d vector to do the calculation
|
||||
// Otherwise I have to do more math
|
||||
double dg4[4] = {dg[0], dg[1], -dg[0], -dg[1]};
|
||||
double vel4[4] = {ball->vel[0], ball->vel[1], 0, 0};
|
||||
|
||||
// Calculate the impulse
|
||||
// J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg
|
||||
// Really need to get some better matrix math in here
|
||||
// only used the dot product of the first 2 in the last term since it's supposed to be dg * minv * dg
|
||||
// and since the bumpers are immovable the mass is infinite which inverse is 0
|
||||
// So this isn't robust at all
|
||||
// Setting the CoE to be 1 always - perfectly elastic
|
||||
// Otherwise there's a weird bouncy thing
|
||||
double impulse = ((1.0 + 1.0) * dot(dg4, vel4, 4)) / (dot(dg4, dg4, 2));
|
||||
std::cout << "Impulse: " << impulse << std::endl;
|
||||
scaleInPlace(dg4, impulse, 4);
|
||||
|
||||
// Impulse[0:1] is x and y components of what should be applied to v1, Impulse[2:3] goes to v2
|
||||
double impulse1[3] = {dg4[0], dg4[1], 0};
|
||||
double impulse2[3] = {dg4[2], dg4[3], 0};
|
||||
|
||||
double newV1[3];
|
||||
dv_copy(newV1, ball->vel);
|
||||
// double newV2[3] = {0, 0};
|
||||
|
||||
scaleInPlace(newV1, ball->mass, 3);
|
||||
// scaleInPlace(newV2, balls[index2]->mass, 3);
|
||||
|
||||
dv_sub(newV1, newV1, impulse1);
|
||||
// dv_sub(newV2, newV2, impulse2);
|
||||
|
||||
scaleInPlace(newV1, 1.0 / ball->mass, 3);
|
||||
// scaleInPlace(newV2, 1.0 / balls[index2]->mass, 3);
|
||||
|
||||
std::cout << "Impulses applied: \n\tV1: " << newV1[0] << " " << newV1[1] << std::endl;
|
||||
|
||||
ball->vel[0] = newV1[0];
|
||||
ball->vel[1] = newV1[1];
|
||||
|
||||
} else {
|
||||
regula_falsi_set_upper (now, distanceToBumper, &bumperAssociations[association_index]);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
if (numBumperCollisions > 0) {
|
||||
std::cout << "In this time step, " << numBumperCollisions << " collisions applied" << std::endl;
|
||||
}
|
||||
|
||||
// Pockets
|
||||
for (ii=0; 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];
|
||||
double pocketPos[3] = {pocket->x, pocket->y, 0};
|
||||
|
||||
double diff[3];
|
||||
dv_sub(diff, ball->pos, pocketPos);
|
||||
double distance = dv_mag(diff);
|
||||
|
||||
// Ball is pocketed when center is within pocket radius
|
||||
double error = distance - (pocket->radius);
|
||||
|
||||
pocketAssociations[association_index].error = error;
|
||||
double this_tgo = regula_falsi( now, &(pocketAssociations[association_index]));
|
||||
|
||||
if (this_tgo < event_tgo) {
|
||||
event_tgo = this_tgo;
|
||||
}
|
||||
|
||||
if (this_tgo == 0) {
|
||||
std::cout << "Found pocketed ball" << std::endl;
|
||||
reset_regula_falsi( now, &(pocketAssociations[association_index]) );
|
||||
removeBall(ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return event_tgo;
|
||||
}
|
||||
|
||||
void PoolTable::resetCueBall() {
|
||||
resetCueBall(defaultCueBallX, defaultCueBallY);
|
||||
}
|
||||
|
||||
void PoolTable::resetCueBall(double x, double y) {
|
||||
balls[cueBallIndex]->clearAllState();
|
||||
balls[cueBallIndex]->setPos(x, y);
|
||||
|
||||
balls[cueBallIndex]->inPlay = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PoolTable::applyCueForce(double x_end, double y_end) {
|
||||
applyCueForce(x_end, y_end, 0, 0, 0);
|
||||
}
|
||||
|
||||
|
||||
void PoolTable::applyCueForce(double x_end, double y_end, double cueHorizontalDisplacement, double cueVerticalDisplacement, double cueAngle) {
|
||||
std::cout << "Applying cue force" << std::endl;
|
||||
|
||||
// assume index 0 is cue ball
|
||||
int cueIndex = 0;
|
||||
double cueEnd[3] = {x_end, y_end};
|
||||
double cueBallPos[3] = {balls[cueIndex]->pos[0], balls[cueIndex]->pos[1]};
|
||||
|
||||
double r = balls[cueIndex]->radius;
|
||||
double m = balls[cueIndex]->mass;
|
||||
|
||||
double a = cueHorizontalDisplacement * r;
|
||||
double b = cueVerticalDisplacement * r;
|
||||
double c = abs(sqrt(pow(r,2) - pow(a,2) - pow(b,2)));
|
||||
|
||||
double theta = cueAngle * M_PI / 180.0;
|
||||
double angleScaling = 1.0 + (cueAngle / 25.0);
|
||||
|
||||
double cueAxis[3];
|
||||
dv_sub(cueAxis, cueEnd, cueBallPos);
|
||||
double cue_v = dv_mag(cueAxis) * cueForceScale * angleScaling;
|
||||
dv_norm(cueAxis, cueAxis);
|
||||
double axis2[2] = {cueAxis[0], cueAxis[1]};
|
||||
|
||||
double force = (2 * m * cue_v) / (1.0 * (m / cueMass) + (5.0 / (2.0*pow(r, 2))) * (pow(a,2) + pow(b,2)*pow(cos(theta),2) + pow(c,2)*pow(sin(theta),2) - 2*b*c*sin(theta)*cos(theta)));
|
||||
|
||||
// Velocity in the frame of the cue
|
||||
double v_b[2] = {0, -force/m * cos(theta)};
|
||||
|
||||
// Rotate to table frame
|
||||
double y_unit[2] = {0, 1};
|
||||
double rot = acos(dot(axis2, y_unit, 2) / 1.0);
|
||||
if (cueAxis[0] < 0) {
|
||||
rot = -rot;
|
||||
}
|
||||
|
||||
double rotationMatrix[2][2];
|
||||
rotationMatrix[0][0] = cos(rot);
|
||||
rotationMatrix[0][1] = -sin(rot);
|
||||
rotationMatrix[1][0] = sin(rot);
|
||||
rotationMatrix[1][1] = cos(rot);
|
||||
|
||||
double v_table[2];
|
||||
mProduct(v_table, rotationMatrix, v_b);
|
||||
|
||||
std::cout << "Velocity of ball after cue hit: " << v_table[0] << " " << v_table[1] << std::endl;
|
||||
|
||||
balls[cueIndex]->vel[0] = v_table[0];
|
||||
balls[cueIndex]->vel[1] = v_table[1];
|
||||
|
||||
// TODO: logic for angular and relative velocity
|
||||
|
||||
}
|
||||
|
||||
|
||||
// Add a ball to the table and return the ID
|
||||
int PoolTable::addBall (double x, double y, double mass, double radius, bool fixed) {
|
||||
int id = nextBallSlot++;
|
||||
@ -256,11 +585,45 @@ int PoolTable::addBall (double x, double y, double mass, double radius, bool fix
|
||||
|
||||
// Add a bumper to the table and return the ID
|
||||
// Only takes in the actual effective line, need to add something else for the rendered shape
|
||||
int PoolTable::addBumper (double x1, double y1, double x2, double y2) {
|
||||
// bumpers.push_back(bumper);
|
||||
int PoolTable::addBumper (int numPoints, double x1, double y1, double x2, double y2) {
|
||||
int id = nextBumperSlot++;
|
||||
if (id < numBumpers) {
|
||||
Bumper * bumper = (Bumper*) TMM_declare_var_s("Bumper");
|
||||
bumpers[id] = (new (bumper) Bumper(numPoints, x1, y1, x2, y2));
|
||||
return id;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int PoolTable::addPointToBumper(int id, double x, double y) {
|
||||
Bumper * bumper = bumpers[id];
|
||||
bumper->AddPointToRender(x, y);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PoolTable::addPointToTable(double x, double y) {
|
||||
int id = nextTablePointSlot++;
|
||||
if (id < numTablePoints) {
|
||||
Point * point = (Point*) TMM_declare_var_s("Point");
|
||||
tableShape[id] = (new (point) Point(x, y));
|
||||
return id;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int PoolTable::addPocket(double x, double y, double r) {
|
||||
int id = nextPocketSlot++;
|
||||
if (id < numPockets) {
|
||||
Pocket * pocket = (Pocket*) TMM_declare_var_s("Pocket");
|
||||
pockets[id] = (new (pocket) Pocket(x, y, r));
|
||||
return id;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int PoolTable::setBallPos(int id, double x, double y) {
|
||||
if (id < numBalls && balls[id] != NULL) {
|
||||
balls[id]->pos[0] = x;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user