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.real_time_enable()
|
||||||
trick.exec_set_software_frame(0.1)
|
trick.exec_set_software_frame(0.01)
|
||||||
trick.itimer_enable()
|
trick.itimer_enable()
|
||||||
|
|
||||||
trick.exec_set_enable_freeze(True)
|
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())
|
exec(open("./Modified_data/realtime.py").read())
|
||||||
|
|
||||||
dyn.table.numBalls = 2
|
dyn.table.numBalls = 2
|
||||||
dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls)
|
dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls)
|
||||||
|
|
||||||
id1 = dyn.table.addBall(-2, .5, 1, 1, False);
|
ballRadius = 0.02
|
||||||
id2 = dyn.table.addBall(1, .5, 1, 1, False);
|
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)
|
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
|
# Start the Graphics Client
|
||||||
#==========================================
|
#==========================================
|
||||||
varServerPort = trick.var_server_get_port();
|
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)) :
|
if (os.path.isfile(PoolTableDisplay_path)) :
|
||||||
PoolTableDisplay_cmd = "java -jar " \
|
PoolTableDisplay_cmd = PoolTableDisplay_path \
|
||||||
+ PoolTableDisplay_path \
|
|
||||||
+ " " + str(varServerPort) + " &" ;
|
+ " " + str(varServerPort) + " &" ;
|
||||||
print(PoolTableDisplay_cmd)
|
print(PoolTableDisplay_cmd)
|
||||||
os.system( PoolTableDisplay_cmd);
|
os.system( PoolTableDisplay_cmd);
|
||||||
else :
|
else :
|
||||||
print('============================================================================================')
|
print('=================================================================================================')
|
||||||
print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
print('PoolTableDisplay needs to be built. Please \"cd\" into ../models/graphics/java and type \"make\".')
|
||||||
print('============================================================================================')
|
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() ;
|
("derivative") table.state_deriv() ;
|
||||||
("integration") trick_ret = table.state_integ() ;
|
("integration") trick_ret = table.state_integ() ;
|
||||||
("dynamic_event") table.collision() ;
|
("dynamic_event") table.collision() ;
|
||||||
|
// ("dynamic_event") table.bumperCollision() ;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
PoolTableSimObject dyn;
|
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 {
|
class Ball {
|
||||||
static int numColors = 9;
|
static int numColors = 8;
|
||||||
static Color[] colorList = {
|
static Color[] colorList = {
|
||||||
Color.WHITE,
|
Color.WHITE,
|
||||||
Color.YELLOW,
|
Color.YELLOW,
|
||||||
@ -45,7 +45,7 @@ class Ball {
|
|||||||
identity = id;
|
identity = id;
|
||||||
x = 0.0;
|
x = 0.0;
|
||||||
y = 0.0;
|
y = 0.0;
|
||||||
radius = 0.5;
|
radius = 1.0;
|
||||||
color = colorList[id % numColors];
|
color = colorList[id % numColors];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -159,8 +159,8 @@ class RangeView extends JPanel {
|
|||||||
int bx = (int)(worldOriginX + scale * balls[ii].x);
|
int bx = (int)(worldOriginX + scale * balls[ii].x);
|
||||||
int by = (int)(worldOriginY - scale * balls[ii].y);
|
int by = (int)(worldOriginY - scale * balls[ii].y);
|
||||||
drawCenteredCircle(g2d, bx, by, (int)(scale * 2 * balls[ii].radius));
|
drawCenteredCircle(g2d, bx, by, (int)(scale * 2 * balls[ii].radius));
|
||||||
g2d.setPaint(Color.BLACK);
|
// g2d.setPaint(Color.BLACK);
|
||||||
g2d.drawString ( String.format("%d",ii), bx,by);
|
// g2d.drawString ( String.format("%d",ii), bx,by);
|
||||||
}
|
}
|
||||||
|
|
||||||
g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
|
g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
|
||||||
@ -294,7 +294,7 @@ public class PoolTableDisplay extends JFrame {
|
|||||||
line = poolTableDisplay.in.readLine();
|
line = poolTableDisplay.in.readLine();
|
||||||
field = line.split("\t");
|
field = line.split("\t");
|
||||||
for ( ii=0; ii < nballs; ii++) {
|
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 ) {
|
} catch (IOException | NullPointerException e ) {
|
||||||
go = false;
|
go = false;
|
||||||
@ -0,0 +1 @@
|
|||||||
|
Subproject commit a9b087f9d8e9cce32ab68705128164cd5d4ad81a
|
||||||
@ -7,9 +7,9 @@ LIBRARY DEPENDENCY:
|
|||||||
#define _ball_hh_
|
#define _ball_hh_
|
||||||
|
|
||||||
// #include <Eigen/Core>
|
// #include <Eigen/Core>
|
||||||
|
#include "common_geometry.hh"
|
||||||
|
|
||||||
// monotonically increasing ID
|
// monotonically increasing ID
|
||||||
static int id = 0;
|
|
||||||
|
|
||||||
class Ball {
|
class Ball {
|
||||||
|
|
||||||
@ -17,20 +17,48 @@ class Ball {
|
|||||||
|
|
||||||
Ball(double x, double y, double mass, double radius, bool isFixed, int id);
|
Ball(double x, double y, double mass, double radius, bool isFixed, int id);
|
||||||
Ball () {}
|
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
|
// Z component should always be 0, unless someone tries to add jumps in the future
|
||||||
double pos[3];
|
// double pos[3];
|
||||||
double prevPos[3];
|
// double prevPos[3]; // Maybe don't need this anymore?
|
||||||
double vel[3];
|
// double vel[3];
|
||||||
// Used to store derivatives between deriv and integration steps
|
// // Used to store derivatives between deriv and integration steps
|
||||||
double accel[3];
|
// double accel[3];
|
||||||
|
|
||||||
// Relating to angular velocity
|
// // Relating to angular velocity
|
||||||
double relativeVel[3];
|
// double relativeVel[3];
|
||||||
double w[3];
|
// double w[3];
|
||||||
double angular_accel[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 mass;
|
||||||
double radius;
|
double radius;
|
||||||
@ -38,6 +66,8 @@ class Ball {
|
|||||||
bool isCue;
|
bool isCue;
|
||||||
int sliding;
|
int sliding;
|
||||||
|
|
||||||
|
bool inPlay = true;
|
||||||
|
|
||||||
unsigned int id;
|
unsigned int id;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -7,39 +7,25 @@ LIBRARY DEPENDENCY:
|
|||||||
#define _bumper_hh_
|
#define _bumper_hh_
|
||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include "common_geometry.hh"
|
||||||
// this should definitely go somewhere else
|
|
||||||
// maybe make a geometry libary
|
|
||||||
class Point {
|
|
||||||
public:
|
|
||||||
double x;
|
|
||||||
double y;
|
|
||||||
|
|
||||||
Point(double x, double y) : x(x), y(y) {}
|
|
||||||
Point() {}
|
|
||||||
};
|
|
||||||
|
|
||||||
class Line {
|
|
||||||
public:
|
|
||||||
Point p1;
|
|
||||||
Point p2;
|
|
||||||
|
|
||||||
Line (Point p1, Point p2) : p1(p1), p2(p2) {}
|
|
||||||
Line () {}
|
|
||||||
};
|
|
||||||
|
|
||||||
class Bumper {
|
class Bumper {
|
||||||
public:
|
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 AddPointToRender(double x, double y);
|
||||||
void AddBorder (double x1, double y1, double x2, double y2);
|
void AddBorder (double x1, double y1, double x2, double y2);
|
||||||
|
int id;
|
||||||
|
Line border;
|
||||||
|
Vec ** renderedShape;
|
||||||
|
unsigned int numPoints;
|
||||||
|
enum PolygonType shapeType;
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Actual line that can be collided with
|
int nextPointSlot = 0;
|
||||||
Line border;
|
|
||||||
|
|
||||||
// Shape that should be rendered
|
|
||||||
// Size should be dynamic
|
|
||||||
std::vector<Point *> renderedShape;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -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 "trick/regula_falsi.h"
|
||||||
#include "ball.hh"
|
#include "ball.hh"
|
||||||
#include "bumper.hh"
|
#include "bumper.hh"
|
||||||
|
#include "pocket.hh"
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
|
|
||||||
@ -17,42 +18,78 @@ class PoolTable {
|
|||||||
public:
|
public:
|
||||||
PoolTable () : numBalls(0), numAssociations(0) {}
|
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 default_data();
|
||||||
int state_init();
|
int state_init();
|
||||||
int state_deriv();
|
int state_deriv();
|
||||||
int state_integ();
|
int state_integ();
|
||||||
double collision();
|
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
|
// Sim constants that should be user-controllable
|
||||||
double frictionRolling = 0.05;
|
double frictionRolling = 0.05;
|
||||||
double frictionSliding = 0.25;
|
double frictionSliding = 0.25;
|
||||||
double frictionScale = 1;
|
double frictionScale = 1;
|
||||||
double frictionTolerance = 0.0005;
|
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
|
#endif
|
||||||
@ -6,30 +6,57 @@ LIBRARY DEPENDENCY:
|
|||||||
#include "../include/ball.hh"
|
#include "../include/ball.hh"
|
||||||
// #include "trick/memorymanager_c_intf.h"
|
// #include "trick/memorymanager_c_intf.h"
|
||||||
|
|
||||||
#include "trick/MemoryManager.hh"
|
// #include "trick/MemoryManager.hh"
|
||||||
extern Trick::MemoryManager* trick_MM;
|
// extern Trick::MemoryManager* trick_MM;
|
||||||
|
|
||||||
#include <new>
|
#include <new>
|
||||||
|
|
||||||
Ball::Ball(double x, double y, double mass, double radius, bool isFixed, int id) :
|
Ball::Ball(double x, double y, double mass, double radius, bool isFixed, int id) :
|
||||||
mass(mass),
|
mass(mass),
|
||||||
|
radius(radius),
|
||||||
fixed(isFixed),
|
fixed(isFixed),
|
||||||
id(id)
|
id(id)
|
||||||
{
|
{
|
||||||
pos[0] = x;
|
pos = Vec(x, y, 0);
|
||||||
pos[1] = y;
|
|
||||||
pos[2] = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Ball* CreateBall(double x, double y, double mass, double radius, bool isFixed) {
|
void Ball::setPos(double x, double y) { setPos(x, y, 0); }
|
||||||
// // Ball* b = (Ball*)TMM_declare_var_s("Ball");
|
void Ball::setPos(double x, double y, double z) {
|
||||||
// Ball *b = new Ball(x, y, mass, radius, isFixed, 0);
|
pos = Vec(x, y, z);
|
||||||
// trick_MM->declare_extern_var ( b, "Ball");
|
}
|
||||||
// // Ball *b = (Ball*)trick_MM->declare_var("Ball");
|
|
||||||
|
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 "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) {
|
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) {
|
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();
|
// balls.clear();
|
||||||
// bumpers.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;
|
numBalls = 0;
|
||||||
numBumpers = 0;
|
numBumpers = 0;
|
||||||
return 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;
|
return 0;
|
||||||
}
|
}
|
||||||
@ -85,15 +105,13 @@ int PoolTable::state_deriv() {
|
|||||||
|
|
||||||
for (int i = 0; i < numBalls; i++) {
|
for (int i = 0; i < numBalls; i++) {
|
||||||
// Frictional force a constant applied in the opposing direction of velocity.
|
// Frictional force a constant applied in the opposing direction of velocity.
|
||||||
// Magnitude of velocity is irrelevant
|
// Magnitude of velocity is irrelevant unless very close to 0
|
||||||
|
|
||||||
// std::cout << "ActuaVelocity: " << balls[i]->vel[0] << " " << balls[i]->vel[1] << std::endl;
|
|
||||||
// std::cout << "Velocity Norm: " << velocityNorm[0] << " " << velocityNorm[1] << std::endl;
|
|
||||||
|
|
||||||
// balls[i]->accel[0] = 0;
|
|
||||||
// balls[i]->accel[1] = 0;
|
|
||||||
|
|
||||||
// Has weird behavior when velocity is very small, so only apply friction if velocity is greater than a tolerance
|
// 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) {
|
if (abs(dv_mag(balls[i]->vel)) > frictionTolerance) {
|
||||||
double velocityNorm[3];
|
double velocityNorm[3];
|
||||||
dv_norm(velocityNorm, balls[i]->vel);
|
dv_norm(velocityNorm, balls[i]->vel);
|
||||||
@ -125,6 +143,10 @@ int PoolTable::state_integ() {
|
|||||||
int n = 6;
|
int n = 6;
|
||||||
|
|
||||||
for (int i = 0; i < numBalls; i++) {
|
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
|
// 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]
|
// pos[0] pos[1] vel[0] vel[1]
|
||||||
int inner_index = 0;
|
int inner_index = 0;
|
||||||
@ -153,6 +175,10 @@ int PoolTable::state_integ() {
|
|||||||
int integration_step = integrate();
|
int integration_step = integrate();
|
||||||
|
|
||||||
for (int i = 0; i < numBalls; i++) {
|
for (int i = 0; i < numBalls; i++) {
|
||||||
|
if (!balls[i]->inPlay) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
// pos[0] pos[1] vel[0] vel[1]
|
// pos[0] pos[1] vel[0] vel[1]
|
||||||
int inner_index = 0;
|
int inner_index = 0;
|
||||||
balls[i]->pos[0] = unload_indexed_state(n*i + inner_index++);
|
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[0] = unload_indexed_state(n*i + inner_index++);
|
||||||
balls[i]->vel[1] = 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]->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() {
|
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. */
|
double now ; /* current integration time. */
|
||||||
unsigned int first, second;
|
|
||||||
|
|
||||||
unsigned int association_index;
|
|
||||||
double event_tgo;
|
double event_tgo;
|
||||||
unsigned int ii,jj;
|
unsigned int ii,jj;
|
||||||
|
|
||||||
@ -188,14 +246,26 @@ double PoolTable::collision() {
|
|||||||
std::vector<unsigned int> collisionsToProcess;
|
std::vector<unsigned int> collisionsToProcess;
|
||||||
|
|
||||||
for (ii=1; ii<numBalls; ii++) {
|
for (ii=1; ii<numBalls; ii++) {
|
||||||
|
if (!balls[ii]->inPlay) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
for (jj=0; jj<ii; jj++) {
|
for (jj=0; jj<ii; jj++) {
|
||||||
|
if (!balls[jj]->inPlay) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
double diff[3];
|
double diff[3];
|
||||||
dv_sub(diff, balls[ii]->pos, balls[jj]->pos);
|
dv_sub(diff, balls[ii]->pos, balls[jj]->pos);
|
||||||
double distanceBetweenBalls = dv_mag(diff);
|
double distanceBetweenBalls = dv_mag(diff);
|
||||||
unsigned int associationIndex = ii*(ii-1)/2+jj;
|
unsigned int associationIndex = ii*(ii-1)/2+jj;
|
||||||
|
|
||||||
// boundary is distance between balls - radiuses of balls
|
// 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])) ;
|
double this_tgo = regula_falsi( now, &(ballAssociations[associationIndex])) ;
|
||||||
|
|
||||||
if (this_tgo < event_tgo) {
|
if (this_tgo < event_tgo) {
|
||||||
@ -203,20 +273,33 @@ double PoolTable::collision() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (this_tgo == 0) {
|
if (this_tgo == 0) {
|
||||||
|
std::cout << "Found colliding balls" << std::endl;
|
||||||
|
|
||||||
// Add this collision to a list of collisions to process
|
// Add this collision to a list of collisions to process
|
||||||
collisionsToProcess.push_back(ii);
|
collisionsToProcess.push_back(ii);
|
||||||
collisionsToProcess.push_back(jj);
|
collisionsToProcess.push_back(jj);
|
||||||
|
reset_regula_falsi( now, &(ballAssociations[associationIndex]) );
|
||||||
|
|
||||||
|
} else {
|
||||||
|
regula_falsi_set_upper (now, error, &ballAssociations[associationIndex]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Handle collisions
|
// Handle collisions
|
||||||
for (int i = 0; i < collisionsToProcess.size(); i+=2) {
|
for (int i = 0; i < collisionsToProcess.size(); i+=2) {
|
||||||
|
// if (allowCollisions == false) {
|
||||||
|
// return event_tgo;
|
||||||
|
// }
|
||||||
|
// allowCollisions = false;
|
||||||
int index1 = collisionsToProcess[i];
|
int index1 = collisionsToProcess[i];
|
||||||
int index2 = collisionsToProcess[i+1];
|
int index2 = collisionsToProcess[i+1];
|
||||||
|
std::cout << "Collision detected between balls " << index1 << " and " << index2 << std::endl;
|
||||||
|
|
||||||
double *q1 = balls[index1]->pos;
|
double q1[3];
|
||||||
double *q2 = balls[index2]->pos;
|
dv_copy(q1, balls[index1]->pos);
|
||||||
|
double q2[3];
|
||||||
|
dv_copy(q2, balls[index2]->pos);
|
||||||
|
|
||||||
|
|
||||||
// dg = (q1 - q2) / (|q1 - q2|)
|
// dg = (q1 - q2) / (|q1 - q2|)
|
||||||
@ -234,15 +317,261 @@ double PoolTable::collision() {
|
|||||||
// J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg
|
// J = ((-(1 + c) * dg * v) / (dg * M^-1 * dg^T) ) dg
|
||||||
// For now let's just pretend all the masses are 1
|
// For now let's just pretend all the masses are 1
|
||||||
double impulse = ((1.0 + coefficientOfElasticity) * dot(dg4, vel4, 4)) / (dot(dg4, dg4, 4));
|
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;
|
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
|
// Add a ball to the table and return the ID
|
||||||
int PoolTable::addBall (double x, double y, double mass, double radius, bool fixed) {
|
int PoolTable::addBall (double x, double y, double mass, double radius, bool fixed) {
|
||||||
int id = nextBallSlot++;
|
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
|
// 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
|
// 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) {
|
int PoolTable::addBumper (int numPoints, double x1, double y1, double x2, double y2) {
|
||||||
// bumpers.push_back(bumper);
|
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;
|
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) {
|
int PoolTable::setBallPos(int id, double x, double y) {
|
||||||
if (id < numBalls && balls[id] != NULL) {
|
if (id < numBalls && balls[id] != NULL) {
|
||||||
balls[id]->pos[0] = x;
|
balls[id]->pos[0] = x;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user