ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2025-02-21 01:31:25 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Adding fully function simulation

Browse Source
This commit is contained in:
Deans 2022-07-14 11:05:56 -05:00
parent 8537601ccb
commit ad5be89c4a
15 changed files with 1599 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
trick_sims/SIM_pool/RUN_break/input.py
View File

@ -2,7 +2,7 @@ import math
exec(open("./Modified_data/realtime.py").read())
dyn.table.numBalls = 16
dyn.table.numBalls = 11
dyn.table.balls = trick.TMM_declare_var_1d("Ball*", dyn.table.numBalls)
ballRadius = 0.02
@ -30,11 +30,11 @@ dyn.table.addBall(center_x+unit_pos[0]*6*(ballRadius+tol), center_y+unit_pos[1]*
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)
# 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]
@ -46,7 +46,7 @@ bumperWidth = 0.03
dyn.table.numTablePoints = 2
dyn.table.tableShape = trick.TMM_declare_var_1d("Point*", dyn.table.numTablePoints)
dyn.table.tableShape = trick.TMM_declare_var_1d("Vec*", dyn.table.numTablePoints)
dyn.table.tableShapeType = 3 # rectangle
dyn.table.addPointToTable(corners[0], corners[1])
dyn.table.addPointToTable(corners[2], corners[3])
@ -116,7 +116,7 @@ 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].renderedShape = trick.TMM_declare_var_1d("Vec*", dyn.table.bumpers[id].numPoints)
dyn.table.bumpers[id][0].shapeType = bumperShapeTypes[i]
for j in range(0, len(bumperShapes[i]), 2):
@ -130,8 +130,7 @@ dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
#==========================================
varServerPort = trick.var_server_get_port();
# This will definitely change to something else
PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example"
PoolTableDisplay_path = "models/graphics/cpp/build/PoolTableDisplay"
if (os.path.isfile(PoolTableDisplay_path)) :
@ -140,9 +139,9 @@ if (os.path.isfile(PoolTableDisplay_path)) :
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('=================================================================================================')
print('===================================')
print('PoolTableDisplay needs to be built.')
print('===================================')
# PoolTableDisplay_path = "models/graphics/java/dist/PoolTableDisplay.jar"

12
trick_sims/SIM_pool/RUN_hexagon/input.py
View File

@ -54,7 +54,7 @@ bumperWidth = 0.03
dyn.table.numTablePoints = 6
dyn.table.tableShape = trick.TMM_declare_var_1d("Point*", dyn.table.numTablePoints)
dyn.table.tableShape = trick.TMM_declare_var_1d("Vec*", dyn.table.numTablePoints)
dyn.table.tableShapeType = 0 # generic
for corner in corners:
dyn.table.addPointToTable(corner[0], corner[1])
@ -77,7 +77,7 @@ for i in range(len(corners)):
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.bumpers[id][0].renderedShape = trick.TMM_declare_var_1d("Vec*", dyn.table.bumpers[id].numPoints)
dyn.table.addPointToBumper(id, p1[0], p1[1])
dyn.table.addPointToBumper(id, p2[0], p2[1])
p3 = [(p1[0] + p2[0])/2,(p1[1] + p2[1])/2]
@ -91,7 +91,7 @@ dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
varServerPort = trick.var_server_get_port();
# This will definitely change to something else
PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example"
PoolTableDisplay_path = "models/graphics/cpp/build/PoolTableDisplay"
if (os.path.isfile(PoolTableDisplay_path)) :
@ -100,9 +100,9 @@ if (os.path.isfile(PoolTableDisplay_path)) :
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('=================================================================================================')
print('===================================')
print('PoolTableDisplay needs to be built.')
print('===================================')
# PoolTableDisplay_path = "models/graphics/java/dist/PoolTableDisplay.jar"

12
trick_sims/SIM_pool/RUN_test/input.py
View File

@ -42,7 +42,7 @@ bumperWidth = 0.03
dyn.table.numTablePoints = 2
dyn.table.tableShape = trick.TMM_declare_var_1d("Point*", dyn.table.numTablePoints)
dyn.table.tableShape = trick.TMM_declare_var_1d("Vec*", dyn.table.numTablePoints)
dyn.table.tableShapeType = 3 # rectangle
dyn.table.addPointToTable(corners[0], corners[1])
dyn.table.addPointToTable(corners[2], corners[3])
@ -112,7 +112,7 @@ 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].renderedShape = trick.TMM_declare_var_1d("Vec*", dyn.table.bumpers[id].numPoints)
dyn.table.bumpers[id][0].shapeType = bumperShapeTypes[i]
for j in range(0, len(bumperShapes[i]), 2):
@ -127,7 +127,7 @@ dyn_integloop.getIntegrator(trick.Euler, 6*dyn.table.numBalls)
varServerPort = trick.var_server_get_port();
# This will definitely change to something else
PoolTableDisplay_path = "models/graphics/libigl-example-project/build/example"
PoolTableDisplay_path = "models/graphics/cpp/build/PoolTableDisplay"
if (os.path.isfile(PoolTableDisplay_path)) :
@ -136,9 +136,9 @@ if (os.path.isfile(PoolTableDisplay_path)) :
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('=================================================================================================')
print('===================================')
print('PoolTableDisplay needs to be built.')
print('===================================')
# PoolTableDisplay_path = "models/graphics/java/dist/PoolTableDisplay.jar"

1
trick_sims/SIM_pool/S_define
View File

@ -17,7 +17,6 @@ class PoolTableSimObject : public Trick::SimObject {
("derivative") table.state_deriv() ;
("integration") trick_ret = table.state_integ() ;
("dynamic_event") table.collision() ;
// ("dynamic_event") table.bumperCollision() ;
}
};

16
trick_sims/SIM_pool/models/graphics/cpp/CMakeLists.txt Normal file
View File

@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 3.16)
project(PoolTableDisplay)
list(PREPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
# Libigl
option(LIBIGL_GLFW "Build target igl::glfw" ON)
option(LIBIGL_IMGUI "Build target igl::imgui" ON)
include(libigl)
# Add your project files
file(GLOB SRC_FILES *.cpp)
add_executable(${PROJECT_NAME} ${SRC_FILES})
target_link_libraries(${PROJECT_NAME} PUBLIC igl::glfw igl::imgui)

363
trick_sims/SIM_pool/models/graphics/cpp/LICENSE Normal file
View File

@ -0,0 +1,363 @@
Mozilla Public License, version 2.0
1. Definitions
1.1. "Contributor"
means each individual or legal entity that creates, contributes to the
creation of, or owns Covered Software.
1.2. "Contributor Version"
means the combination of the Contributions of others (if any) used by a
Contributor and that particular Contributor's Contribution.
1.3. "Contribution"
means Covered Software of a particular Contributor.
1.4. "Covered Software"
means Source Code Form to which the initial Contributor has attached the
notice in Exhibit A, the Executable Form of such Source Code Form, and
Modifications of such Source Code Form, in each case including portions
thereof.
1.5. "Incompatible With Secondary Licenses"
means
a. that the initial Contributor has attached the notice described in
Exhibit B to the Covered Software; or
b. that the Covered Software was made available under the terms of
version 1.1 or earlier of the License, but not also under the terms of
a Secondary License.
1.6. "Executable Form"
means any form of the work other than Source Code Form.
1.7. "Larger Work"
means a work that combines Covered Software with other material, in a
separate file or files, that is not Covered Software.
1.8. "License"
means this document.
1.9. "Licensable"
means having the right to grant, to the maximum extent possible, whether
at the time of the initial grant or subsequently, any and all of the
rights conveyed by this License.
1.10. "Modifications"
means any of the following:
a. any file in Source Code Form that results from an addition to,
deletion from, or modification of the contents of Covered Software; or
b. any new file in Source Code Form that contains any Covered Software.
1.11. "Patent Claims" of a Contributor
means any patent claim(s), including without limitation, method,
process, and apparatus claims, in any patent Licensable by such
Contributor that would be infringed, but for the grant of the License,
by the making, using, selling, offering for sale, having made, import,
or transfer of either its Contributions or its Contributor Version.
1.12. "Secondary License"
means either the GNU General Public License, Version 2.0, the GNU Lesser
General Public License, Version 2.1, the GNU Affero General Public
License, Version 3.0, or any later versions of those licenses.
1.13. "Source Code Form"
means the form of the work preferred for making modifications.
1.14. "You" (or "Your")
means an individual or a legal entity exercising rights under this
License. For legal entities, "You" includes any entity that controls, is
controlled by, or is under common control with You. For purposes of this
definition, "control" means (a) the power, direct or indirect, to cause
the direction or management of such entity, whether by contract or
otherwise, or (b) ownership of more than fifty percent (50%) of the
outstanding shares or beneficial ownership of such entity.
2. License Grants and Conditions
2.1. Grants
Each Contributor hereby grants You a world-wide, royalty-free,
non-exclusive license:
a. under intellectual property rights (other than patent or trademark)
Licensable by such Contributor to use, reproduce, make available,
modify, display, perform, distribute, and otherwise exploit its
Contributions, either on an unmodified basis, with Modifications, or
as part of a Larger Work; and
b. under Patent Claims of such Contributor to make, use, sell, offer for
sale, have made, import, and otherwise transfer either its
Contributions or its Contributor Version.
2.2. Effective Date
The licenses granted in Section 2.1 with respect to any Contribution
become effective for each Contribution on the date the Contributor first
distributes such Contribution.
2.3. Limitations on Grant Scope
The licenses granted in this Section 2 are the only rights granted under
this License. No additional rights or licenses will be implied from the
distribution or licensing of Covered Software under this License.
Notwithstanding Section 2.1(b) above, no patent license is granted by a
Contributor:
a. for any code that a Contributor has removed from Covered Software; or
b. for infringements caused by: (i) Your and any other third party's
modifications of Covered Software, or (ii) the combination of its
Contributions with other software (except as part of its Contributor
Version); or
c. under Patent Claims infringed by Covered Software in the absence of
its Contributions.
This License does not grant any rights in the trademarks, service marks,
or logos of any Contributor (except as may be necessary to comply with
the notice requirements in Section 3.4).
2.4. Subsequent Licenses
No Contributor makes additional grants as a result of Your choice to
distribute the Covered Software under a subsequent version of this
License (see Section 10.2) or under the terms of a Secondary License (if
permitted under the terms of Section 3.3).
2.5. Representation
Each Contributor represents that the Contributor believes its
Contributions are its original creation(s) or it has sufficient rights to
grant the rights to its Contributions conveyed by this License.
2.6. Fair Use
This License is not intended to limit any rights You have under
applicable copyright doctrines of fair use, fair dealing, or other
equivalents.
2.7. Conditions
Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted in
Section 2.1.
3. Responsibilities
3.1. Distribution of Source Form
All distribution of Covered Software in Source Code Form, including any
Modifications that You create or to which You contribute, must be under
the terms of this License. You must inform recipients that the Source
Code Form of the Covered Software is governed by the terms of this
License, and how they can obtain a copy of this License. You may not
attempt to alter or restrict the recipients' rights in the Source Code
Form.
3.2. Distribution of Executable Form
If You distribute Covered Software in Executable Form then:
a. such Covered Software must also be made available in Source Code Form,
as described in Section 3.1, and You must inform recipients of the
Executable Form how they can obtain a copy of such Source Code Form by
reasonable means in a timely manner, at a charge no more than the cost
of distribution to the recipient; and
b. You may distribute such Executable Form under the terms of this
License, or sublicense it under different terms, provided that the
license for the Executable Form does not attempt to limit or alter the
recipients' rights in the Source Code Form under this License.
3.3. Distribution of a Larger Work
You may create and distribute a Larger Work under terms of Your choice,
provided that You also comply with the requirements of this License for
the Covered Software. If the Larger Work is a combination of Covered
Software with a work governed by one or more Secondary Licenses, and the
Covered Software is not Incompatible With Secondary Licenses, this
License permits You to additionally distribute such Covered Software
under the terms of such Secondary License(s), so that the recipient of
the Larger Work may, at their option, further distribute the Covered
Software under the terms of either this License or such Secondary
License(s).
3.4. Notices
You may not remove or alter the substance of any license notices
(including copyright notices, patent notices, disclaimers of warranty, or
limitations of liability) contained within the Source Code Form of the
Covered Software, except that You may alter any license notices to the
extent required to remedy known factual inaccuracies.
3.5. Application of Additional Terms
You may choose to offer, and to charge a fee for, warranty, support,
indemnity or liability obligations to one or more recipients of Covered
Software. However, You may do so only on Your own behalf, and not on
behalf of any Contributor. You must make it absolutely clear that any
such warranty, support, indemnity, or liability obligation is offered by
You alone, and You hereby agree to indemnify every Contributor for any
liability incurred by such Contributor as a result of warranty, support,
indemnity or liability terms You offer. You may include additional
disclaimers of warranty and limitations of liability specific to any
jurisdiction.
4. Inability to Comply Due to Statute or Regulation
If it is impossible for You to comply with any of the terms of this License
with respect to some or all of the Covered Software due to statute,
judicial order, or regulation then You must: (a) comply with the terms of
this License to the maximum extent possible; and (b) describe the
limitations and the code they affect. Such description must be placed in a
text file included with all distributions of the Covered Software under
this License. Except to the extent prohibited by statute or regulation,
such description must be sufficiently detailed for a recipient of ordinary
skill to be able to understand it.
5. Termination
5.1. The rights granted under this License will terminate automatically if You
fail to comply with any of its terms. However, if You become compliant,
then the rights granted under this License from a particular Contributor
are reinstated (a) provisionally, unless and until such Contributor
explicitly and finally terminates Your grants, and (b) on an ongoing
basis, if such Contributor fails to notify You of the non-compliance by
some reasonable means prior to 60 days after You have come back into
compliance. Moreover, Your grants from a particular Contributor are
reinstated on an ongoing basis if such Contributor notifies You of the
non-compliance by some reasonable means, this is the first time You have
received notice of non-compliance with this License from such
Contributor, and You become compliant prior to 30 days after Your receipt
of the notice.
5.2. If You initiate litigation against any entity by asserting a patent
infringement claim (excluding declaratory judgment actions,
counter-claims, and cross-claims) alleging that a Contributor Version
directly or indirectly infringes any patent, then the rights granted to
You by any and all Contributors for the Covered Software under Section
2.1 of this License shall terminate.
5.3. In the event of termination under Sections 5.1 or 5.2 above, all end user
license agreements (excluding distributors and resellers) which have been
validly granted by You or Your distributors under this License prior to
termination shall survive termination.
6. Disclaimer of Warranty
Covered Software is provided under this License on an "as is" basis,
without warranty of any kind, either expressed, implied, or statutory,
including, without limitation, warranties that the Covered Software is free
of defects, merchantable, fit for a particular purpose or non-infringing.
The entire risk as to the quality and performance of the Covered Software
is with You. Should any Covered Software prove defective in any respect,
You (not any Contributor) assume the cost of any necessary servicing,
repair, or correction. This disclaimer of warranty constitutes an essential
part of this License. No use of any Covered Software is authorized under
this License except under this disclaimer.
7. Limitation of Liability
Under no circumstances and under no legal theory, whether tort (including
negligence), contract, or otherwise, shall any Contributor, or anyone who
distributes Covered Software as permitted above, be liable to You for any
direct, indirect, special, incidental, or consequential damages of any
character including, without limitation, damages for lost profits, loss of
goodwill, work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses, even if such party shall have been
informed of the possibility of such damages. This limitation of liability
shall not apply to liability for death or personal injury resulting from
such party's negligence to the extent applicable law prohibits such
limitation. Some jurisdictions do not allow the exclusion or limitation of
incidental or consequential damages, so this exclusion and limitation may
not apply to You.
8. Litigation
Any litigation relating to this License may be brought only in the courts
of a jurisdiction where the defendant maintains its principal place of
business and such litigation shall be governed by laws of that
jurisdiction, without reference to its conflict-of-law provisions. Nothing
in this Section shall prevent a party's ability to bring cross-claims or
counter-claims.
9. Miscellaneous
This License represents the complete agreement concerning the subject
matter hereof. If any provision of this License is held to be
unenforceable, such provision shall be reformed only to the extent
necessary to make it enforceable. Any law or regulation which provides that
the language of a contract shall be construed against the drafter shall not
be used to construe this License against a Contributor.
10. Versions of the License
10.1. New Versions
Mozilla Foundation is the license steward. Except as provided in Section
10.3, no one other than the license steward has the right to modify or
publish new versions of this License. Each version will be given a
distinguishing version number.
10.2. Effect of New Versions
You may distribute the Covered Software under the terms of the version
of the License under which You originally received the Covered Software,
or under the terms of any subsequent version published by the license
steward.
10.3. Modified Versions
If you create software not governed by this License, and you want to
create a new license for such software, you may create and use a
modified version of this License if you rename the license and remove
any references to the name of the license steward (except to note that
such modified license differs from this License).
10.4. Distributing Source Code Form that is Incompatible With Secondary
Licenses If You choose to distribute Source Code Form that is
Incompatible With Secondary Licenses under the terms of this version of
the License, the notice described in Exhibit B of this License must be
attached.
Exhibit A - Source Code Form License Notice
This Source Code Form is subject to the
terms of the Mozilla Public License, v.
2.0. If a copy of the MPL was not
distributed with this file, You can
obtain one at
http://mozilla.org/MPL/2.0/.
If it is not possible or desirable to put the notice in a particular file,
then You may include the notice in a location (such as a LICENSE file in a
relevant directory) where a recipient would be likely to look for such a
notice.
You may add additional accurate notices of copyright ownership.
Exhibit B - "Incompatible With Secondary Licenses" Notice
This Source Code Form is "Incompatible
With Secondary Licenses", as defined by
the Mozilla Public License, v. 2.0.

26
trick_sims/SIM_pool/models/graphics/cpp/README.md Normal file
View File

@ -0,0 +1,26 @@
# Pool Table Display
Uses the Libigl library for visualization. Connects to SIM_Billiards
## Compile
libigl must be downloaded from Github : https://github.com/libigl/libigl
Clone it somewhere accessible, like your home directory. It does not need to be compiled.
libigl depends on Eigen3, which also need to be downloaded and installed : https://eigen.tuxfamily.org/index.php?title=Main_Page
To compile:
mkdir build
cd build
cmake ..
make
## Run
From within the `build` directory:
./PoolTableDisplay

52
trick_sims/SIM_pool/models/graphics/cpp/Socket.cpp Normal file
View File

@ -0,0 +1,52 @@
#include "Socket.hh"
#define SOCKET_BUF_SIZE 2048
Socket::Socket (std::string hostname, int port) : hostname(hostname), port(port) {
socket_fd = socket(AF_INET, SOCK_STREAM, 0);
if (socket_fd < 0) {
std::cout << "Socket connection failed" << std::endl;
}
struct sockaddr_in serv_addr;
serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(port); // convert to weird network byte format
if(inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr)<=0) {
std::cout << "Invalid address/ Address not supported" << std::endl;
}
if (connect(socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
std::cout << "Connection failed" << std::endl;
}
}
int Socket::send (std::string message) {
// weird syntax I've never used before - since the send syscall that i'm trying to use is overloaded in this class,
// I have to append :: to the front of it so that the compiler knows to look in the global namespace
int success = ::send(socket_fd, message.c_str(), message.size(), 0);
if (success < message.size()) {
std::cout << "Failed to send message" << std::endl;
}
return success;
}
int Socket::operator<< (std::string message) {
return send(message);
}
std::string Socket::receive () {
char buffer[SOCKET_BUF_SIZE];
int numBytes = read(socket_fd, buffer, SOCKET_BUF_SIZE);
if (numBytes < 0) {
std::cout << "Failed to read from socket" << std::endl;
} else if (numBytes < SOCKET_BUF_SIZE) {
buffer[numBytes] = '\0';
}
return std::string(buffer);
}
void Socket::operator>> (std::string& ret) {
ret = receive();
}

25
trick_sims/SIM_pool/models/graphics/cpp/Socket.hh Normal file
View File

@ -0,0 +1,25 @@
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <string>
#include <iostream>
class Socket {
public:
Socket (std::string hostname, int port);
int send (std::string message);
int operator<< (std::string message);
std::string receive ();
void operator>> (std::string& ret);
private:
int port;
std::string hostname;
int socket_fd;
};

11
trick_sims/SIM_pool/models/graphics/cpp/cmake/libigl.cmake Normal file
View File

@ -0,0 +1,11 @@
if(TARGET igl::core)
return()
endif()
include(FetchContent)
FetchContent_Declare(
libigl
GIT_REPOSITORY https://github.com/libigl/libigl.git
GIT_TAG v2.4.0
)
FetchContent_MakeAvailable(libigl)

969
trick_sims/SIM_pool/models/graphics/cpp/main.cpp Normal file
View File

@ -0,0 +1,969 @@
#include <igl/opengl/glfw/Viewer.h>
#include <igl/readOFF.h>
#include <igl/opengl/glfw/imgui/ImGuiPlugin.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <string>
#include <thread>
#include <queue>
#include "Socket.hh"
// #include <sys/types.h>
// #include <sys/socket.h>
// #include <netinet/in.h>
// #include <unistd.h>
// #include <arpa/inet.h>
#include <mutex>
std::vector<Eigen::Vector3d> ball_colors = {Eigen::Vector3d(1.0,1.0,0.0), //yellow
Eigen::Vector3d(0.0,0.0,1.0), //blue
Eigen::Vector3d(1.0,0.0,0.0), //red
Eigen::Vector3d(0.5,0.0,0.3), //purple
Eigen::Vector3d(0.7,0.5,0.0), //orange
Eigen::Vector3d(0.0,0.7,0.0), //green
Eigen::Vector3d(0.3,0.07,0.15), //maroon
Eigen::Vector3d(0.0,0.0,0.0), //black
};
const double layer_BALL = 0.005;
const double layer_TABLE = 0.001;
const double layer_BUMPER = 0.002;
const double layer_RAIL = 0.000;
const double layer_CUE = 0.008;
const double layer_POCKET = 0.002;
class RenderedShape {
public:
RenderedShape() {}
int baseIndex = 0;
int getNumVertices() {
return vertices.size();
}
int getNumFaces() {
return faces.size();
}
std::vector<Eigen::Vector3i> getFacesWithBaseIndex(int newBase) {
std::vector<Eigen::Vector3i> newFaces;
int offset = newBase - baseIndex;
Eigen::Vector3i baseOffset(offset, offset, offset);
for (Eigen::Vector3i& face : faces) {
newFaces.emplace_back(face + baseOffset);
}
baseIndex = newBase;
return newFaces;
}
~RenderedShape() {
if (destructorPrint)
std::cout << "Destroying rendered Shape" << std::endl;
}
// std::vector<Eigen::Vector3d> getVerticesWithOffset(int newBase) {
// std::vector<Eigen::Vector3i> newFaces;
// int offset = newBase - baseIndex;
// Eigen::Vector3i baseOffset(offset, offset, offset);
// for (Eigen::Vector3i& face : faces) {
// newFaces.emplace_back(face + baseOffset);
// }
// baseIndex = newBase;
// return newFaces;
// }
std::vector<Eigen::Vector3d> vertices;
std::vector<Eigen::Vector3i> faces;
std::vector<Eigen::Vector3d> colors;
bool destructorPrint = false;
};
class Point {
public:
Point () : point(0, 0) {}
Point (double x, double y) : point(x, y, 0) {}
Point (double x, double y, double layer) : point(x, y, layer) {}
Eigen::Vector3d toVec3 () const {
return Eigen::Vector3d(point);
}
double x() const {
return point[0];
}
double y() const {
return point[1];
}
Eigen::Vector3d point;
};
class Polygon {
public:
Polygon(unsigned int verts, double layer) : vertexMax(verts), layer(layer) {}
void addPoint(double x, double y) {
if (points.size() < vertexMax)
points.emplace_back(x, y, layer);
}
void setColor(double r, double g, double b) {
color = Eigen::Vector3d(r, g, b);
}
void setColor(Eigen::Vector3d& c) {
color = Eigen::Vector3d(c);
}
void setLayer(double l) {
layer = l;
}
Eigen::Vector3d getColor() const {
return color;
}
~Polygon() {
if (destructorPrint)
std::cout << "Destroying a polygon" << std::endl;
}
// Works with any simple convex polygon where the points are in order
virtual RenderedShape* render() const {
RenderedShape * shape = new RenderedShape();
if (!isValid()) {
// Should maybe throw an error
std::cerr << "Generic has incorrect number of corners" << std::endl;
return shape;
}
for (int i = 0; i < points.size(); i++) {
shape->vertices.emplace_back(points[i].toVec3());
shape->colors.emplace_back(color);
}
for (int i = 1; i < points.size()-1; i++) {
shape->faces.emplace_back(0, i, i+1);
}
return shape;
}
virtual bool isValid() const {
return points.size() <= vertexMax;
}
protected:
std::vector<Point> points;
Eigen::Vector3d color;
unsigned int vertexMax;
double layer;
int id;
bool destructorPrint = false;
};
class Circle : public Polygon {
public:
Circle (double x, double y, double r, double layer) : Polygon(1, layer), radius(r) {
addPoint(x, y);
}
bool isValid() const {
return points.size() == vertexMax;
}
RenderedShape *render() const {
// Circle is broken down into wedges in order to be rendered here
RenderedShape *shape = new RenderedShape();
if (!isValid()) {
std::cout << "Circle has incorrect number of points" << std::endl;
return shape;
}
// Add colors
for (int i = 0; i < numWedges + 2; i++) {
shape->colors.emplace_back(color);
}
// Add center
shape->vertices.emplace_back(points[0].toVec3());
// std::cout << "Center: " << points[0].toVec3() << std::endl;
// Add outside vertices
for (int i = 0; i <= numWedges; i++) {
shape->vertices.emplace_back(points[0].x() + radius * cos(2 * M_PI * i / numWedges),
points[0].y() + radius * sin(2 * M_PI * i / numWedges),
layer);
}
// Make the triangles
for (int j = 0; j <= numWedges; j++) {
shape->faces.emplace_back(0, j + 1, 1 + ((j + 1) % (numWedges + 1)));
}
return shape;
}
private:
double radius;
int numWedges = 20;
};
// class Generic : public Polygon {
// public:
// Generic(int numPoints, double layer) : Polygon(numPoints, layer) {}
// // Assumes a simple convex polygon
// virtual RenderedShape* render() const {
// RenderedShape * shape = new RenderedShape();
// if (!isValid()) {
// // Should maybe throw an error
// std::cerr << "Generic has incorrect number of corners" << std::endl;
// return shape;
// }
// for (int i = 0; i < points.size(); i++) {
// shape->vertices.emplace_back(points[i].toVec3());
// shape->colors.emplace_back(color);
// }
// for (int i = 1; i < points.size()-1; i++) {
// shape->faces.emplace_back(0, i, i+1);
// }
// return shape;
// }
// };
class Rectangle : public Polygon {
public:
// only need upper left and lower right corners
Rectangle(double layer) : Polygon(2, layer) {}
bool isValid() const {
return points.size() == vertexMax;
}
void addCorner (double x, double y) {
addPoint(x, y);
}
RenderedShape* render() const {
RenderedShape *shape = new RenderedShape();
if (!isValid()) {
// Should maybe throw an error
std::cerr << "Rectangle has incorrect number of corners" << std::endl;
return shape;
}
// Add colors
for (int i = 0; i < 6; i++) {
shape->colors.emplace_back(color);
}
// Add triangles within rectangle
shape->vertices.emplace_back(points[0].x(), points[1].y(), layer);
shape->vertices.emplace_back(points[1].x(), points[0].y(), layer);
shape->vertices.emplace_back(points[0].toVec3());
shape->faces.emplace_back(0, 1, 2);
shape->vertices.emplace_back(points[0].x(), points[1].y(), layer);
shape->vertices.emplace_back(points[1].toVec3());
shape->vertices.emplace_back(points[1].x(), points[0].y(), layer);
shape->faces.emplace_back(3, 4, 5);
return shape;
}
};
class Triangle : public Polygon {
public:
Triangle (double layer) : Polygon(3, layer) {}
void addCorner (double x, double y) {
addPoint(x, y);
}
RenderedShape* render() const {
RenderedShape *shape = new RenderedShape();
if (!isValid()) {
// Should maybe throw an error
std::cerr << "Triangle has incorrect number of corners" << std::endl;
return shape;
}
// Add colors and vertices at the same time
for (int i = 0; i < 3; i++) {
shape->colors.emplace_back(color);
shape->vertices.emplace_back(points[i].toVec3());
}
// just the 1 triangle
shape->faces.emplace_back(0, 1, 2);
return shape;
}
};
enum PolygonType {
GENERIC,
CIRCLE,
TRIANGLE,
RECTANGLE
};
class Table {
public:
Table () {}
void clearMovingShapes() {
for (int i = 0; i < movingShapes.size(); i++) {
delete movingShapes[i];
delete movingRenderedShapes[i];
}
movingShapes.clear();
movingRenderedShapes.clear();
}
void updateMovingShape(int id) {
// TODO
}
// Need to have an agreed upon way to send over variables
int addShape(std::vector<double> shapeData, Eigen::Vector3d color, bool isStatic, PolygonType type, double layer) {
// std::cout << "In AddShape" << std::endl;
Polygon *shape;
switch (type) {
case GENERIC: {
// Number of points is just data / 2 i guess
// std::cout << "Creating generic polygon with " << shapeData.size()/2 << " points" << std::endl;
Polygon *newPolygon = new Polygon(shapeData.size()/2, layer);
for (int i = 0; i < shapeData.size(); i+=2) {
double x = shapeData[i];
double y = shapeData[i+1];
newPolygon->addPoint(x,y);
}
shape = newPolygon;
break;
}
case CIRCLE: {
// std::cout << "Adding circle" << std::endl;
if (shapeData.size() != 3) {
std::cout << "Bad shapedata size for circle" << std::endl;
return -1;
}
double x = shapeData[0];
double y = shapeData[1];
double r = shapeData[2];
Circle *newCircle = new Circle(x, y, r, layer);
shape = newCircle;
break;
}
case TRIANGLE: {
Triangle *newTriangle = new Triangle(layer);
if (shapeData.size() != 6) {
std::cout << "Bad shapedata size for triangle" << std::endl;
return -1;
}
for (int i = 0; i < shapeData.size(); i+=2) {
double x = shapeData[i];
double y = shapeData[i+1];
newTriangle->addCorner(x, y);
}
shape = newTriangle;
break;
}
case RECTANGLE: {
// std::cout << "In rectangle" << std::endl;
Rectangle *newRectangle = new Rectangle(layer);
if (shapeData.size() != 4) {
std::cout << "Bad shapedata size for rectangle" << std::endl;
return -1;
}
for (int i = 0; i < shapeData.size(); i+=2) {
double x = shapeData[i];
double y = shapeData[i+1];
newRectangle->addCorner(x, y);
}
shape = newRectangle;
break;
}
default: {
break;
}
}
shape->setColor(color);
if (isStatic) {
std::cout << "Adding to static shapes" << std::endl;
staticShapes.emplace_back(shape);
} else {
// std::cout << "Adding to moving shapes" << std::endl;
movingShapes.emplace_back(shape);
}
return 0;
}
// Call this once
void renderStaticShapes() {
staticRendered = true;
numStaticVertices = 0;
numStaticFaces = 0;
int i = 0;
staticRenderedShapes.clear();
for (Polygon* shape : staticShapes) {
std::cout << "Rendering shape " << i++ << std::endl;
RenderedShape *renderedShape = shape->render();
renderedShape->destructorPrint = true;
numStaticVertices += renderedShape->getNumVertices();
numStaticFaces += renderedShape->getNumFaces();
staticRenderedShapes.emplace_back(renderedShape);
}
}
// Should think about how to make sure we aren't making big unnessary copies of stuff
std::tuple<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::MatrixXd> getMesh() {
// if (!staticRendered) {
// renderStaticShapes();
// }
numStaticVertices = 0;
numStaticFaces = 0;
int i = 0;
staticRenderedShapes.clear();
for (Polygon* shape : staticShapes) {
RenderedShape *renderedShape = shape->render();
numStaticVertices += renderedShape->getNumVertices();
numStaticFaces += renderedShape->getNumFaces();
staticRenderedShapes.emplace_back(renderedShape);
}
int totalFaces = numStaticFaces;
int totalVertices = numStaticVertices;
for (Polygon* shape : movingShapes) {
RenderedShape *renderedShape = shape->render();
totalVertices += renderedShape->getNumVertices();
totalFaces += renderedShape->getNumFaces();
movingRenderedShapes.push_back(renderedShape);
}
// std::cout << "Total Vertices: " << totalVertices << std::endl;
// std::cout << "Total Faces: " << totalFaces << std::endl;
// std::cout << "Total Colors: " << totalVertices << std::endl;
// Now have to put all of these into giant matrices
Eigen::MatrixXd renderV;
renderV.resize(totalVertices, 3);
Eigen::MatrixXi renderF;
renderF.resize(totalFaces, 3);
Eigen::MatrixXd renderC;
renderC.resize(totalVertices, 3);
// TODO: Ideally have some matrix with preloaded static shapes
// For now do them all here
int vertexIndex = 0;
int faceIndex = 0;
for (RenderedShape* shape : staticRenderedShapes) {
// Add vertices and colors
for (int i = 0; i < shape->getNumVertices(); i++) {
renderV.row(i+vertexIndex) = shape->vertices[i];
renderC.row(i+vertexIndex) = shape->colors[i];
}
auto newFaces = shape->getFacesWithBaseIndex(vertexIndex);
// Add faces - with the correct offset
for (int i = 0; i < newFaces.size(); i++) {
renderF.row(i+faceIndex) = newFaces[i];
}
vertexIndex += shape->getNumVertices();
faceIndex += newFaces.size();
}
for (RenderedShape* shape : movingRenderedShapes) {
// Add vertices and colors
for (int i = 0; i < shape->getNumVertices(); i++) {
renderV.row(i+vertexIndex) = shape->vertices[i];
renderC.row(i+vertexIndex) = shape->colors[i];
}
auto newFaces = shape->getFacesWithBaseIndex(vertexIndex);
// Add faces - with the correct offset
for (int i = 0; i < newFaces.size(); i++) {
renderF.row(i+faceIndex) = newFaces[i];
}
vertexIndex += shape->getNumVertices();
faceIndex += newFaces.size();
}
return std::make_tuple(renderV, renderF, renderC);
}
private:
std::vector<Polygon *> staticShapes;
std::vector<RenderedShape *> staticRenderedShapes;
std::vector<RenderedShape *> movingRenderedShapes;
int numStaticVertices;
int numStaticFaces;
bool staticRendered = false;
std::vector<Polygon *> movingShapes;
};
void printUsage() {
std::cout << "Usage: program <portNumber>" << std::endl;
}
std::vector<std::string> split (std::string& str, const char delim) {
std::stringstream ss(str);
std::string s;
std::vector<std::string> ret;
while (std::getline(ss, s, delim)) {
ret.push_back(s);
}
return ret;
}
std::vector<double> parseTrickResponse(std::vector<std::string> list) {
std::vector<double> ret;
for (int i = 1; i < list.size(); i++) {
ret.push_back(stod(list[i]));
}
return ret;
}
void launchViewer (igl::opengl::glfw::Viewer *viewer) {
viewer->launch();
}
bool mousePressed = false;
double mouseX = 0;
double mouseY = 0;
std::queue<std::string> messageQueue;
std::mutex messageLock;
bool sendCue = false;
std::mutex renderLock;
Eigen::MatrixXd V;
Eigen::MatrixXi F;
Eigen::MatrixXd C;
igl::opengl::glfw::imgui::ImGuiMenu* menu;
bool mouse_down (igl::opengl::glfw::Viewer& viewer, int button, int modifier) {
mousePressed = true;
return true;
}
bool mouse_up (igl::opengl::glfw::Viewer& viewer, int button, int modifier) {
mousePressed = false;
std::string cueRequest = "";
std::string templateString = "dyn.table.applyCueForce(%.3f, %.3f) \n";
char buf[128];
sprintf(buf, templateString.c_str(), mouseX, mouseY);
cueRequest += std::string(buf);
messageLock.lock();
messageQueue.push(cueRequest);
messageLock.unlock();
return true;
}
bool mouse_move (igl::opengl::glfw::Viewer& viewer, int mouse_x, int mouse_y) {
Eigen::Vector3f pos(mouse_x, mouse_y, 0);
Eigen::Matrix4f model = viewer.core().view;
Eigen::Vector3f unproj = igl::unproject(pos, model, viewer.core().proj, viewer.core().viewport);
mouseX = unproj[0];
mouseY = -unproj[1];
return true;
}
bool pre_draw (igl::opengl::glfw::Viewer& viewer) {
renderLock.lock();
viewer.data().clear();
viewer.core().orthographic = true;
viewer.data().show_lines = false;
viewer.data().set_face_based(false);
viewer.data().double_sided = true;
viewer.core().is_animating = true;
viewer.core().camera_zoom = 2;
viewer.data().set_mesh(V, F);
viewer.data().set_colors(C);
renderLock.unlock();
}
void draw_viewer_menu () {
ImGui::Text("Menu");
if (ImGui::Button("Reset Cue Ball", ImVec2(-1, 0)))
{
std::string message = "dyn.table.resetCueBall() \n";
messageLock.lock();
messageQueue.push(message);
messageLock.unlock();
}
}
int main(int argc, char *argv[])
{
// Parse socket number out of argv
if (argc != 2) {
printUsage();
return -1;
}
int port = 0;
port = std::stoi(argv[1]);
if (port == 0) {
printUsage();
return -1;
}
std::cout << "Port received: " << port << std::endl;
Socket socket("localhost", port);
socket << "trick.var_set_client_tag(\"PoolTableDisplay\") \n";
// Should request all the static data first - table, bumper, and pocket shapes
// Hardcoded for now
socket << "trick.var_add(\"dyn.table.numBalls\")\ntrick.var_send() \ntrick.var_clear() \n";
std::string reply;
socket >> reply;
auto parsed = split(reply, '\t');
int numBalls = stoi(parsed[1]);
std::cout << "Number of balls received: " << numBalls << std::endl;
std::string radiusRequest = "";
char* templateString = "trick.var_add(\"dyn.table.balls[%d][0].radius\")\n";
for (int i = 0; i < numBalls; i++) {
char buf[64];
sprintf(buf, templateString, i);
radiusRequest += std::string(buf);
}
socket << radiusRequest;
socket << "trick.var_send() \ntrick.var_clear() \n";
socket >> reply;
// std::cout << "Radius reply: " << reply << std::endl;
auto radii = parseTrickResponse(split(reply, '\t'));
// To be able to account for balls being added and subtracted, probably need to request numBalls every time, and just delete the moving objects each render cycle
// Probably a more optimized way to do this
// TODO: figure that out
Table table;
socket << "trick.var_add(\"dyn.table.numTablePoints\") \ntrick.var_add(\"dyn.table.tableShapeType\")\n \ntrick.var_send() \ntrick.var_clear() \n";
socket >> reply;
std::cout << "Table shape reply: " << reply << std::endl;
std::vector<double> tableData = parseTrickResponse(split(reply, '\t'));
int numTablePoints = tableData[0];
enum PolygonType tableShape = PolygonType((int)tableData[1]);
std::string pointRequest = "";
for (int i = 0; i < numTablePoints; i++) {
templateString = "trick.var_add(\"dyn.table.tableShape[%d][0]._x\")\ntrick.var_add(\"dyn.table.tableShape[%d][0]._y\")\n";
char buf[256];
sprintf(buf, templateString, i, i);
pointRequest += std::string(buf);
}
socket << pointRequest;
socket << "trick.var_send() \ntrick.var_clear() \n";
socket >> reply;
std::vector<double> tablePoints = parseTrickResponse(split(reply, '\t'));
table.addShape(tablePoints, Eigen::Vector3d(0.2, 0.6, 0.2), true, tableShape, layer_TABLE);
// Make the rail - translate each point on the table out from center by railWidth
std::vector<double> railData;
if (tableShape == RECTANGLE) {
double railWidth = 0.07;
railData.push_back(tablePoints[0] - railWidth);
railData.push_back(tablePoints[1] - railWidth);
railData.push_back(tablePoints[2] + railWidth);
railData.push_back(tablePoints[3] + railWidth);
} else {
double railWidth = 0.15;
for (int i = 0; i < tablePoints.size(); i+=2) {
Eigen::Vector2d point(tablePoints[i], tablePoints[i+1]);
Eigen::Vector2d railPoint(tablePoints[i], tablePoints[i+1]);
point *= railWidth;
railPoint = railPoint + point;
railData.push_back(railPoint(0));
railData.push_back(railPoint(1));
}
}
table.addShape(railData, Eigen::Vector3d(.3, .2, .15), true, tableShape, layer_RAIL);
// Pockets
socket << "trick.var_add(\"dyn.table.numPockets\")\n \ntrick.var_send() \ntrick.var_clear() \n";
socket >> reply;
double numPockets = stod(split(reply, '\t')[1]);
for (int i = 0; i < numPockets; i++) {
templateString = "trick.var_add(\"dyn.table.pockets[%d][0].pos._x\")\ntrick.var_add(\"dyn.table.pockets[%d][0].pos._y\")\n\ntrick.var_add(\"dyn.table.pockets[%d][0].radius\")\n";
char buf[256];
sprintf(buf, templateString, i, i, i);
std::string pocketRequest = std::string(buf);
socket << pocketRequest;
socket << "trick.var_send() \ntrick.var_clear() \n";
socket >> reply;
std::vector<double> pocketData = parseTrickResponse(split(reply, '\t'));
table.addShape(pocketData, Eigen::Vector3d(0.0, 0.0, 0.0), true, CIRCLE, layer_POCKET);
}
// std::vector<double> table_corners = {-.5, -.25, .5, .25};
// Eigen::Vector3d table_color(0.2, 0.6, 0.2);
// table.addShape(table_corners, table_color, true, RECTANGLE);
// Bumpers
socket << "trick.var_add(\"dyn.table.numBumpers\")\n \ntrick.var_send() \ntrick.var_clear() \n";
socket >> reply;
double numBumpers = stod(split(reply, '\t')[1]);
std::cout << "Num bumpers: " << numBumpers << std::endl;
//templateString = "trick.var_add(\"dyn.table.bumpers[%d][0].border.p1.x\") \ntrick.var_add(\"dyn.table.bumpers[%d][0].border.p1.y\")\ntrick.var_add(\"dyn.table.bumpers[%d][0].border.p2.x\") \ntrick.var_add(\"dyn.table.bumpers[%d][0].border.p2.y\")\n";
for (int i = 0; i < numBumpers; i++) {
std::string bumperRequests = "";
templateString = "trick.var_add(\"dyn.table.bumpers[%d][0].numPoints\")\ntrick.var_add(\"dyn.table.bumpers[%d][0].shapeType\") \n";
char buf[256];
sprintf(buf, templateString, i, i);
bumperRequests += std::string(buf);
socket << bumperRequests;
socket << "trick.var_send() \ntrick.var_clear() \n";
socket >> reply;
std::vector<double> bumperData = parseTrickResponse(split(reply, '\t'));
int bumperPoints = bumperData[0];
enum PolygonType bumperShape = PolygonType((int)bumperData[1]);
templateString = "trick.var_add(\"dyn.table.bumpers[%d][0].renderedShape[%d][0]._x\")\ntrick.var_add(\"dyn.table.bumpers[%d][0].renderedShape[%d][0]._y\")\n";
bumperRequests = "";
for (int j = 0; j < bumperPoints; j++) {
char buf[256];
sprintf(buf, templateString, i, j, i, j);
bumperRequests += std::string(buf);
}
socket << bumperRequests;
socket << "trick.var_send() \ntrick.var_clear() \n";
socket >> reply;
std::vector<double> bumperBorder = parseTrickResponse(split(reply, '\t'));
table.addShape(bumperBorder, Eigen::Vector3d(0.2,0.4,0.2), true, bumperShape, layer_BUMPER);
}
// socket << "trick.var_pause()\n";
// Request all of the ball positions
std::string positionRequest = "";
templateString = "trick.var_add(\"dyn.table.balls[%d][0].pos._x\")\ntrick.var_add(\"dyn.table.balls[%d][0].pos._y\")\n";
for (int i = 0; i < numBalls; i++) {
char buf[128];
sprintf(buf, templateString, i, i);
positionRequest += std::string(buf);
}
socket << positionRequest;
socket << "trick.var_send() \ntrick.var_clear() \n";
socket >> reply;
auto positions = parseTrickResponse(split(reply, '\t'));
for (int i = 0; i < numBalls; i++) {
std::vector<double> circleData = {positions[(i*2)], positions[(i*2 + 1)], radii[i]};
Eigen::Vector3d circleColor = ball_colors[i % ball_colors.size()];
table.addShape(circleData, circleColor, false, CIRCLE, layer_BALL);
}
renderLock.lock();
std::tie(V, F, C) = table.getMesh();
renderLock.unlock();
igl::opengl::glfw::Viewer * viewer = new igl::opengl::glfw::Viewer();
igl::opengl::glfw::imgui::ImGuiPlugin *plugin = new igl::opengl::glfw::imgui::ImGuiPlugin();
viewer->plugins.push_back(plugin);
menu = new igl::opengl::glfw::imgui::ImGuiMenu();
plugin->widgets.push_back(menu);
viewer->callback_mouse_down = mouse_down;
viewer->callback_mouse_up = mouse_up;
viewer->callback_mouse_move = mouse_move;
viewer->callback_pre_draw = pre_draw;
menu->callback_draw_viewer_menu = draw_viewer_menu;
viewer->core().orthographic = true;
viewer->core().camera_zoom = 2;
viewer->data().show_lines = false;
viewer->data().set_face_based(false);
viewer->data().double_sided = true;
viewer->core().is_animating = true;
// Plot the mesh
viewer->data().set_mesh(V, F);
viewer->data().set_colors(C);
// Viewer is blocking, have to launch it in a separate thread
std::thread viewerThread (launchViewer, viewer);
// std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "After launch " << std::endl;
// Need to get nBalls and posititions every time
// Maybe should add some coordinating by ID logic too
socket << "trick.var_pause() \n";
socket << "trick.var_add(\"dyn.table.numBalls\")\n";
positionRequest = "";
templateString = "trick.var_add(\"dyn.table.balls[%d][0].pos._x\")\ntrick.var_add(\"dyn.table.balls[%d][0].pos._y\")\ntrick.var_add(\"dyn.table.balls[%d][0].inPlay\")\n";
for (int i = 0; i < numBalls; i++) {
char buf[128];
sprintf(buf, templateString, i, i, i);
positionRequest += std::string(buf);
}
socket << positionRequest;
socket << "trick.var_ascii() \n";
socket << "trick.var_cycle(0.010) \n";
socket << "trick.var_unpause() \n";
int badReplies = 10;
// Start the actual draw loop
int drawIteration = 0;
while (true) {
// std::cout << "Draw loop iteration" << drawIteration++ << std::endl;
messageLock.lock();
while (!messageQueue.empty()) {
std::string message = messageQueue.front();
messageQueue.pop();
socket.send(message);
}
messageLock.unlock();
std::string reply;
socket >> reply;
// std::cout << "Got data: " << reply << std::endl;
std::vector<double> replyData = parseTrickResponse(split(reply, '\t'));
// numBalls = (int)replyData[0];
if (replyData.size() <= 1) {
// std::cout << "Received bad reply" << std::endl;
badReplies--;
if (badReplies > 0)
continue;
else
break;
}
table.clearMovingShapes();
Eigen::Vector2d cueBallPos(0,0);
int cueBallIndex = 0;
for (int i = 0; i < numBalls; i++) {
double inPlay = replyData[1+(i*3 + 2)];
if (inPlay == 0) {
continue;
}
std::vector<double> circleData = {replyData[1+(i*3)], replyData[1+(i*3 + 1)], radii[i]};
Eigen::Vector3d circleColor;
if (i == cueBallIndex) {
circleColor = Eigen::Vector3d(1,1,1);
cueBallPos = Eigen::Vector2d(replyData[1+(i*2)], replyData[1+(i*2 + 1)]);
} else {
circleColor = ball_colors[i % ball_colors.size()];
}
table.addShape(circleData, circleColor, false, CIRCLE, layer_BALL);
}
if (mousePressed) {
// Draw the cue
double cue_width = 0.03;
Eigen::Vector2d cue_end(mouseX, mouseY);
Eigen::Vector2d vec = (cue_end - cueBallPos).normalized();
Eigen::Vector2d off1(-vec(1), vec(0));
Eigen::Vector2d off2(vec(1), -vec(0));
Eigen::Vector2d point1 = cue_end + (off1 * cue_width);
Eigen::Vector2d point2 = cue_end + (off2 * cue_width);
std::vector<double> triangleData = {cueBallPos(0), cueBallPos(1), point1(0), point1(1), point2(0), point2(1)};
table.addShape(triangleData, Eigen::Vector3d(0, 0, 0), false, TRIANGLE, layer_CUE);
}
renderLock.lock();
std::tie(V, F, C) = table.getMesh();
renderLock.unlock();
}
viewerThread.join();
}

51
trick_sims/SIM_pool/models/pool_table/include/common_geometry.hh
View File

@ -6,12 +6,15 @@ LIBRARY DEPENDENCY:
#ifndef _common_hh_
#define _common_hh_
#include "math.h"
#include "iostream"
// Should maybe swap this for eigen stuff at some point
class Vec {
public:
Vec(double x, double y) : _x(x), _y(y), _z(0) {}
Vec(double x, double y, double z) : _x(x), _y(y), _z(z) {}
Vec() {}
Vec() : _x(0), _y(0), _z(0) {}
Vec operator+ (const Vec& other) {
Vec sum;
@ -21,6 +24,14 @@ class Vec {
return sum;
}
Vec operator- (const Vec& other) {
Vec sum;
sum._x = _x - other._x;
sum._y = _y - other._y;
sum._z = _z - other._z;
return sum;
}
Vec operator* (double scale) {
Vec ret;
ret._x = _x * scale;
@ -29,7 +40,21 @@ class Vec {
return ret;
}
double& operator() (int index) {
double dot(const Vec& other) {
double ret = 0;
ret += _x * other._x;
ret += _y * other._y;
ret += _z * other._z;
return ret;
}
// Dot product
double operator* (const Vec& other) {
return dot(other);
}
double& operator[] (int index) {
if (index == 0) {
return _x;
} else if (index == 1) {
@ -41,20 +66,32 @@ class Vec {
// Throw an error i guess
}
void setZero() {
_x = 0;
_y = 0;
_z = 0;
}
double norm() {
return sqrt(_x*_x + _y*_y + _z*_z);
}
Vec normalized () {
Vec norm;
Vec ret(*this);
return ret * (1.0 / this->norm());
}
double& x () { return _x; }
double& y () { return _y; }
double& z () { return _z; }
private:
double _x;
double _y;
double _z;
private:
};
class Line {
@ -74,4 +111,6 @@ enum PolygonType {
QUAD
};
#endif

5
trick_sims/SIM_pool/models/pool_table/include/pocket.hh
View File

@ -13,10 +13,9 @@ 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) {}
Pocket(double x, double y, double r) : pos(x,y), radius(r) {}
double x;
double y;
Vec pos;
double radius;
};

2
trick_sims/SIM_pool/models/pool_table/include/pool_table.hh
View File

@ -82,7 +82,7 @@ class PoolTable {
double frictionScale = 1;
double frictionTolerance = 0.0005;
double coefficientOfElasticity = .95;
double cueForceScale = 1.0;
double cueForceScale = 0.6;
double cueMass = 1.0;
int cueBallIndex = 0;

224
trick_sims/SIM_pool/models/pool_table/src/pool_table.cpp
View File

@ -11,6 +11,8 @@ LIBRARY DEPENDENCY:
#include "../include/pool_table.hh"
#include "trick/trick_math_proto.h"
// Leaving these here, but should probably go somewhere else
double dot( /* Return: Scalar dot or inner product */
double vec1[], /* In: Vector 1 */
double vec2[], /* In: Vector 2 */
@ -34,9 +36,6 @@ void mProduct (double product[2], double matrix[2][2], double vec[2]) {
product[1] = matrix[0][1] * vec[0] + matrix[1][1] * vec[1];
}
int PoolTable::default_data() {
// balls.clear();
// bumpers.clear();
@ -66,6 +65,7 @@ int PoolTable::state_init() {
}
}
// Bumper/ball collisions
bumperBallCombos = numBalls * numBumpers;
bumperAssociations = (REGULA_FALSI*)TMM_declare_var_1d("REGULA_FALSI", bumperBallCombos);
for (ii=0; ii<numBalls; ii++) {
@ -112,9 +112,8 @@ int PoolTable::state_deriv() {
continue;
}
if (abs(dv_mag(balls[i]->vel)) > frictionTolerance) {
double velocityNorm[3];
dv_norm(velocityNorm, balls[i]->vel);
if (balls[i]->vel.norm() > frictionTolerance) {
Vec velocityNorm = balls[i]->vel.normalized();
balls[i]->accel[0] = - (frictionScale * frictionRolling * velocityNorm[0]);
balls[i]->accel[1] = - (frictionScale * frictionRolling * velocityNorm[1]);
@ -147,8 +146,6 @@ int PoolTable::state_integ() {
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;
load_indexed_state(n*i + inner_index++, balls[i]->pos[0]);
load_indexed_state(n*i + inner_index++, balls[i]->pos[1]);
@ -159,16 +156,14 @@ int PoolTable::state_integ() {
load_indexed_state(n*i + inner_index++, balls[i]->vel[2]);
// Derivatives of all of this junk
// vel[0] vel[1] accel[0] accel[1]
inner_index = 0;
load_indexed_deriv(n*i + inner_index++, balls[i]->vel[0]);
load_indexed_deriv(n*i + inner_index++, balls[i]->vel[1]);
load_indexed_deriv(n*i + inner_index++, balls[i]->vel[2]);
load_indexed_deriv(n*i + inner_index++, balls[i]->accel[0]); // Needs to be accel[0]
load_indexed_deriv(n*i + inner_index++, balls[i]->accel[1]); // Needs to be accel[1]
load_indexed_deriv(n*i + inner_index++, balls[i]->accel[2]); // Needs to be accel[2]
load_indexed_deriv(n*i + inner_index++, balls[i]->accel[0]);
load_indexed_deriv(n*i + inner_index++, balls[i]->accel[1]);
load_indexed_deriv(n*i + inner_index++, balls[i]->accel[2]);
}
@ -179,7 +174,7 @@ int PoolTable::state_integ() {
continue;
}
// pos[0] pos[1] vel[0] vel[1]
// pos[0] pos[1] pos[2] vel[0] vel[1] vel[2]
int inner_index = 0;
balls[i]->pos[0] = unload_indexed_state(n*i + inner_index++);
balls[i]->pos[1] = unload_indexed_state(n*i + inner_index++);
@ -198,19 +193,13 @@ int PoolTable::state_integ() {
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];
double closestPointOnLine(Line& line, Vec pos, Vec& result, bool print) {
Vec a(line.p1);
Vec b(line.p2);
Vec diff = pos - a;
Vec m = b - a;
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;
double t = (diff * m) / (m * m);
if (t < 0)
t = 0;
@ -218,10 +207,8 @@ double closestPointOnLine(Line& line, double pos[2], double result[2], bool prin
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;
m = m * t;
result = a + m;
return t;
}
@ -253,19 +240,14 @@ double PoolTable::collision() {
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
double distanceBetweenBalls = (balls[ii]->pos - balls[jj]->pos).norm();
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) {
@ -276,6 +258,7 @@ double PoolTable::collision() {
std::cout << "Found colliding balls" << std::endl;
// Add this collision to a list of collisions to process
// probably don't need to do this
collisionsToProcess.push_back(ii);
collisionsToProcess.push_back(jj);
reset_regula_falsi( now, &(ballAssociations[associationIndex]) );
@ -288,25 +271,16 @@ double PoolTable::collision() {
// 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[3];
dv_copy(q1, balls[index1]->pos);
double q2[3];
dv_copy(q2, balls[index2]->pos);
Vec q1(balls[index1]->pos);
Vec q2(balls[index2]->pos);
// dg = (q1 - q2) / (|q1 - q2|)
double diff[3];
dv_sub(diff, q1, q2);
double dg[3];
dv_norm(dg, diff);
Vec diff = q1 - q2;
Vec dg = diff.normalized();
// Have to stuff both velocities and dg values into 4d vector to do the calculation
// Otherwise I have to do more math
@ -320,22 +294,14 @@ double PoolTable::collision() {
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};
Vec impulse1(dg4[0], dg4[1], 0);
Vec impulse2(dg4[2], dg4[3], 0);
double newV1[3];
dv_copy(newV1, balls[index1]->vel);
double newV2[3];
dv_copy(newV2, balls[index2]->vel);
Vec newV1(balls[index1]->vel);
Vec 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);
newV1 = ((newV1 * balls[index1]->mass) - impulse1) * (1.0 / balls[index1]->mass);
newV2 = ((newV2 * balls[index2]->mass) - impulse2) * (1.0 / balls[index2]->mass);
std::cout << "Impulses applied: \n\tV1: " << newV1[0] << " " << newV1[1] << " \n\tV2: " << newV2[0] << " " << newV2[1] << std::endl;
@ -346,46 +312,22 @@ double PoolTable::collision() {
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;
Vec closestPointOnBumper;
double t = closestPointOnLine(bumper->border, ball->pos, closestPointOnBumper, true);
double distanceToBumper = (ball->pos - closestPointOnBumper).norm() - 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;
@ -394,25 +336,21 @@ double PoolTable::collision() {
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)
if (numBumperCollisions > 1) {
// When a ball hits exactly a corner of 2 bumpers, both collisions will be detected and impulses applied.
// But since an impulse takes into consideration the original velocity, the second impulse applied
// just cancels out the first one most of the time. Both impulses should be identical since the
// collision point is the same, so make sure only 1 gets applied. This is kind of hacky
continue;
}
double q1[3];
dv_copy(q1, ball->pos);
double q2[3];
dv_copy(q2, closestPointOnBumper);
q1[2] = 0;
q2[2] = 0;
Vec q1(ball->pos);
Vec q2(closestPointOnBumper);
// dg = (q1 - q2) / (|q1 - q2|)
double diff[3];
dv_sub(diff, q1, q2);
double dg[3];
dv_norm(dg, diff);
Vec diff = q1 - q2;
Vec dg = diff.normalized();
// Have to stuff both velocities and dg values into 4d vector to do the calculation
// Otherwise I have to do more math
@ -432,21 +370,13 @@ double PoolTable::collision() {
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};
Vec impulse1(dg4[0], dg4[1], 0);
Vec impulse2(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);
Vec newV1 (ball->vel);
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);
newV1 = ((newV1 * ball->mass) - impulse1) * (1.0 / ball->mass);
std::cout << "Impulses applied: \n\tV1: " << newV1[0] << " " << newV1[1] << std::endl;
@ -472,16 +402,11 @@ double PoolTable::collision() {
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);
double error = (ball->pos - pocket->pos).norm() - (pocket->radius);
pocketAssociations[association_index].error = error;
double this_tgo = regula_falsi( now, &(pocketAssociations[association_index]));
if (this_tgo < event_tgo) {
@ -489,7 +414,7 @@ double PoolTable::collision() {
}
if (this_tgo == 0) {
std::cout << "Found pocketed ball" << std::endl;
std::cout << "Found pocketed ball " << ii << std::endl;
reset_regula_falsi( now, &(pocketAssociations[association_index]) );
removeBall(ii);
}
@ -510,20 +435,17 @@ void PoolTable::resetCueBall(double x, double 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]};
Vec cueEnd(x_end, y_end);
Vec cueBallPos(balls[cueIndex]->pos);
double r = balls[cueIndex]->radius;
double m = balls[cueIndex]->mass;
@ -535,37 +457,21 @@ void PoolTable::applyCueForce(double x_end, double y_end, double cueHorizontalDi
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);
Vec cueAxis = cueEnd - cueBallPos;
double cue_v = cueAxis.norm() * cueForceScale * angleScaling;
cueAxis = cueAxis.normalized();
double axis2[2] = {cueAxis[0], cueAxis[1]};
double force = (2 * m * cue_v) / (1.0 * (m / cueMass) + (5.0 / (2.0*pow(r, 2))) * (pow(a,2) + pow(b,2)*pow(cos(theta),2) + pow(c,2)*pow(sin(theta),2) - 2*b*c*sin(theta)*cos(theta)));
// Velocity in the frame of the cue
double v_b[2] = {0, -force/m * cos(theta)};
double v_multiplier = -force/m * cos(theta);
Vec velocityAfterHit = cueAxis * v_multiplier;
// 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;
}
std::cout << "Velocity of ball after cue hit: " << velocityAfterHit[0] << " " << velocityAfterHit[1] << std::endl;
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];
balls[cueIndex]->vel = velocityAfterHit;
// TODO: logic for angular and relative velocity
@ -605,8 +511,8 @@ int PoolTable::addPointToBumper(int id, double x, double y) {
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));
Vec * point = (Vec*) TMM_declare_var_s("Vec");
tableShape[id] = (new (point) Vec(x, y));
return id;
}
return -1;