mirror of
https://github.com/nasa/trick.git
synced 2024-12-20 05:37:55 +00:00
parent
120b9c74c9
commit
c8ccfff15e
10
trick_sims/SIM_contact/Modified_data/realtime.py
Executable file
10
trick_sims/SIM_contact/Modified_data/realtime.py
Executable file
@ -0,0 +1,10 @@
|
|||||||
|
|
||||||
|
trick.real_time_enable()
|
||||||
|
trick.exec_set_software_frame(0.1)
|
||||||
|
trick.itimer_enable()
|
||||||
|
|
||||||
|
trick.exec_set_enable_freeze(True)
|
||||||
|
trick.exec_set_freeze_command(True)
|
||||||
|
|
||||||
|
simControlPanel = trick.SimControlPanel()
|
||||||
|
trick.add_external_application(simControlPanel)
|
25
trick_sims/SIM_contact/README.md
Normal file
25
trick_sims/SIM_contact/README.md
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
#SIM_contact
|
||||||
|
|
||||||
|
SIM_contact simulates the motion, and the collisions of any arbitrary numbers of moving balls.
|
||||||
|
|
||||||
|
Each ball is defined by a position, velocity, radius, and mass.
|
||||||
|
|
||||||
|
Collisions are purely elastic. In addition to conserving momentum, they also conserve kinetic energy.
|
||||||
|
|
||||||
|
A graphics client allows you to see the balls in motion:
|
||||||
|
|
||||||
|
![Graphics CLient showing two balls](images/TwoBalls.png)
|
||||||
|
|
||||||
|
##Examples
|
||||||
|
The configuration for the above display is:
|
||||||
|
|
||||||
|
```
|
||||||
|
dyn.contact.nballs = 2
|
||||||
|
dyn.contact.balls = trick.TMM_declare_var_1d("Ball*", dyn.contact.nballs)
|
||||||
|
dyn.contact.balls[0] = trick.make_Ball(0.0, 0.0, 0.5, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[1] = trick.make_Ball(3.0, 0.0, -0.5, 0.0, 0.5, 1.0)
|
||||||
|
```
|
||||||
|
|
||||||
|
The initial state of the first ball is pos=<0.0, 0.0>, vel=<0.5,0.0>, radius=0.5, and mass=1.0.
|
||||||
|
|
||||||
|
The initial state of the second ball is pos=<3.0, 0.0>, vel=<-0.5,0.0>, radius=0.5, and mass=1.0.
|
30
trick_sims/SIM_contact/RUN_test1/input.py
Executable file
30
trick_sims/SIM_contact/RUN_test1/input.py
Executable file
@ -0,0 +1,30 @@
|
|||||||
|
execfile("Modified_data/realtime.py")
|
||||||
|
|
||||||
|
# Variable Server Data should be copied at top of frame.
|
||||||
|
trick.var_set_copy_mode(2)
|
||||||
|
|
||||||
|
dyn.contact.nballs = 2
|
||||||
|
dyn.contact.balls = trick.TMM_declare_var_1d("Ball*", dyn.contact.nballs)
|
||||||
|
|
||||||
|
dyn.contact.balls[0] = trick.make_Ball(0.0, 0.0, 0.5, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[1] = trick.make_Ball(3.0, 0.0, -0.5, 0.0, 0.5, 1.0)
|
||||||
|
|
||||||
|
dyn_integloop.getIntegrator(trick.Euler, 2*dyn.contact.nballs)
|
||||||
|
|
||||||
|
|
||||||
|
#==========================================
|
||||||
|
# Start the Satellite Graphics Client
|
||||||
|
#==========================================
|
||||||
|
varServerPort = trick.var_server_get_port();
|
||||||
|
BallDisplay_path = "models/graphics/dist/BallDisplay.jar"
|
||||||
|
|
||||||
|
if (os.path.isfile(BallDisplay_path)) :
|
||||||
|
BallDisplay_cmd = "java -jar " \
|
||||||
|
+ BallDisplay_path \
|
||||||
|
+ " " + str(varServerPort) + " &" ;
|
||||||
|
print(BallDisplay_cmd)
|
||||||
|
os.system( BallDisplay_cmd);
|
||||||
|
else :
|
||||||
|
print('==================================================================================')
|
||||||
|
print('BallDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||||
|
print('==================================================================================')
|
35
trick_sims/SIM_contact/RUN_test2/input.py
Executable file
35
trick_sims/SIM_contact/RUN_test2/input.py
Executable file
@ -0,0 +1,35 @@
|
|||||||
|
execfile("Modified_data/realtime.py")
|
||||||
|
|
||||||
|
# Variable Server Data should be copied at top of frame.
|
||||||
|
trick.var_set_copy_mode(2)
|
||||||
|
|
||||||
|
dyn.contact.nballs = 5
|
||||||
|
dyn.contact.balls = trick.TMM_declare_var_1d("Ball*", dyn.contact.nballs)
|
||||||
|
# x, y, vx, vy, r, m
|
||||||
|
dyn.contact.balls[0] = trick.make_Ball( 0.0, 0.0, 0.67, 0.0, 2.0, 4.0)
|
||||||
|
dyn.contact.balls[1] = trick.make_Ball( 5.0, 0.0,-0.67, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[2] = trick.make_Ball( 7.0, 0.3,-0.67, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[3] = trick.make_Ball(-7.0, 0.0, 0.67, 0.1, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[4] = trick.make_Ball( 9.0, 0.0,-0.67, 0.0, 0.5, 1.0)
|
||||||
|
|
||||||
|
dyn_integloop.getIntegrator(trick.Euler, 2*dyn.contact.nballs)
|
||||||
|
|
||||||
|
# dyn.contact.nbounds = 5
|
||||||
|
# dyn.contact.bounds = trick.TMM_declare_var_1d("Boundary*", dyn.contact.nbounds)
|
||||||
|
|
||||||
|
#==========================================
|
||||||
|
# Start the Satellite Graphics Client
|
||||||
|
#==========================================
|
||||||
|
varServerPort = trick.var_server_get_port();
|
||||||
|
BallDisplay_path = "models/graphics/dist/BallDisplay.jar"
|
||||||
|
|
||||||
|
if (os.path.isfile(BallDisplay_path)) :
|
||||||
|
BallDisplay_cmd = "java -jar " \
|
||||||
|
+ BallDisplay_path \
|
||||||
|
+ " " + str(varServerPort) + " &" ;
|
||||||
|
print(BallDisplay_cmd)
|
||||||
|
os.system( BallDisplay_cmd);
|
||||||
|
else :
|
||||||
|
print('==================================================================================')
|
||||||
|
print('BallDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||||
|
print('==================================================================================')
|
35
trick_sims/SIM_contact/RUN_test3/input.py
Executable file
35
trick_sims/SIM_contact/RUN_test3/input.py
Executable file
@ -0,0 +1,35 @@
|
|||||||
|
execfile("Modified_data/realtime.py")
|
||||||
|
|
||||||
|
# Variable Server Data should be copied at top of frame.
|
||||||
|
trick.var_set_copy_mode(2)
|
||||||
|
|
||||||
|
dyn.contact.nballs = 5
|
||||||
|
dyn.contact.balls = trick.TMM_declare_var_1d("Ball*", dyn.contact.nballs)
|
||||||
|
# x, y, vx, vy, r
|
||||||
|
dyn.contact.balls[0] = trick.make_Ball(0.0, 0.0, 0.67, 0.0, 0.5, 5.0)
|
||||||
|
dyn.contact.balls[1] = trick.make_Ball(1.5, 0.0, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[2] = trick.make_Ball(3.0, 0.0, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[3] = trick.make_Ball(4.5, 0.0, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[4] = trick.make_Ball(6.0, 0.0, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
|
||||||
|
dyn_integloop.getIntegrator(trick.Euler, 2*dyn.contact.nballs)
|
||||||
|
|
||||||
|
# dyn.contact.nbounds = 5
|
||||||
|
# dyn.contact.bounds = trick.TMM_declare_var_1d("Boundary*", dyn.contact.nbounds)
|
||||||
|
|
||||||
|
#==========================================
|
||||||
|
# Start the Satellite Graphics Client
|
||||||
|
#==========================================
|
||||||
|
varServerPort = trick.var_server_get_port();
|
||||||
|
BallDisplay_path = "models/graphics/dist/BallDisplay.jar"
|
||||||
|
|
||||||
|
if (os.path.isfile(BallDisplay_path)) :
|
||||||
|
BallDisplay_cmd = "java -jar " \
|
||||||
|
+ BallDisplay_path \
|
||||||
|
+ " " + str(varServerPort) + " &" ;
|
||||||
|
print(BallDisplay_cmd)
|
||||||
|
os.system( BallDisplay_cmd);
|
||||||
|
else :
|
||||||
|
print('==================================================================================')
|
||||||
|
print('BallDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||||
|
print('==================================================================================')
|
37
trick_sims/SIM_contact/RUN_test4/input.py
Executable file
37
trick_sims/SIM_contact/RUN_test4/input.py
Executable file
@ -0,0 +1,37 @@
|
|||||||
|
execfile("Modified_data/realtime.py")
|
||||||
|
|
||||||
|
# Variable Server Data should be copied at top of frame.
|
||||||
|
trick.var_set_copy_mode(2)
|
||||||
|
|
||||||
|
dyn.contact.nballs = 7
|
||||||
|
dyn.contact.balls = trick.TMM_declare_var_1d("Ball*", dyn.contact.nballs)
|
||||||
|
# x, y, vx, vy, r
|
||||||
|
dyn.contact.balls[0] = trick.make_Ball(-5.0, 0.1, 0.5, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[1] = trick.make_Ball( 0.0, 0.0, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[2] = trick.make_Ball( 1.2, 0.6, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[3] = trick.make_Ball( 1.2,-0.6, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[4] = trick.make_Ball( 2.4, 1.2, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[5] = trick.make_Ball( 2.4, 0.0, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[6] = trick.make_Ball( 2.4,-1.2, 0.0, 0.0, 0.5, 1.0)
|
||||||
|
|
||||||
|
dyn_integloop.getIntegrator(trick.Euler, 2*dyn.contact.nballs)
|
||||||
|
|
||||||
|
# dyn.contact.nbounds = 5
|
||||||
|
# dyn.contact.bounds = trick.TMM_declare_var_1d("Boundary*", dyn.contact.nbounds)
|
||||||
|
|
||||||
|
#==========================================
|
||||||
|
# Start the Satellite Graphics Client
|
||||||
|
#==========================================
|
||||||
|
varServerPort = trick.var_server_get_port();
|
||||||
|
BallDisplay_path = "models/graphics/dist/BallDisplay.jar"
|
||||||
|
|
||||||
|
if (os.path.isfile(BallDisplay_path)) :
|
||||||
|
BallDisplay_cmd = "java -jar " \
|
||||||
|
+ BallDisplay_path \
|
||||||
|
+ " " + str(varServerPort) + " &" ;
|
||||||
|
print(BallDisplay_cmd)
|
||||||
|
os.system( BallDisplay_cmd);
|
||||||
|
else :
|
||||||
|
print('==================================================================================')
|
||||||
|
print('BallDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||||
|
print('==================================================================================')
|
33
trick_sims/SIM_contact/RUN_test5/input.py
Executable file
33
trick_sims/SIM_contact/RUN_test5/input.py
Executable file
@ -0,0 +1,33 @@
|
|||||||
|
execfile("Modified_data/realtime.py")
|
||||||
|
|
||||||
|
# Variable Server Data should be copied at top of frame.
|
||||||
|
trick.var_set_copy_mode(2)
|
||||||
|
|
||||||
|
dyn.contact.nballs = 3
|
||||||
|
dyn.contact.balls = trick.TMM_declare_var_1d("Ball*", dyn.contact.nballs)
|
||||||
|
# x, y, vx, vy, r, mass
|
||||||
|
dyn.contact.balls[0] = trick.make_Ball( -5.0, 2.0, 1.0, 0.0, 0.5, 1.0)
|
||||||
|
dyn.contact.balls[1] = trick.make_Ball( 5.0, 10.2, 0.0, 0.0, 10.0, 1000.0)
|
||||||
|
dyn.contact.balls[2] = trick.make_Ball( 5.0,-10.2, 0.0, 0.0, 10.0, 1000.0)
|
||||||
|
|
||||||
|
dyn_integloop.getIntegrator(trick.Euler, 2*dyn.contact.nballs)
|
||||||
|
|
||||||
|
# dyn.contact.nbounds = 5
|
||||||
|
# dyn.contact.bounds = trick.TMM_declare_var_1d("Boundary*", dyn.contact.nbounds)
|
||||||
|
|
||||||
|
#==========================================
|
||||||
|
# Start the Satellite Graphics Client
|
||||||
|
#==========================================
|
||||||
|
varServerPort = trick.var_server_get_port();
|
||||||
|
BallDisplay_path = "models/graphics/dist/BallDisplay.jar"
|
||||||
|
|
||||||
|
if (os.path.isfile(BallDisplay_path)) :
|
||||||
|
BallDisplay_cmd = "java -jar " \
|
||||||
|
+ BallDisplay_path \
|
||||||
|
+ " " + str(varServerPort) + " &" ;
|
||||||
|
print(BallDisplay_cmd)
|
||||||
|
os.system( BallDisplay_cmd);
|
||||||
|
else :
|
||||||
|
print('==================================================================================')
|
||||||
|
print('BallDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||||
|
print('==================================================================================')
|
23
trick_sims/SIM_contact/S_define
Executable file
23
trick_sims/SIM_contact/S_define
Executable file
@ -0,0 +1,23 @@
|
|||||||
|
/************************************************************
|
||||||
|
PURPOSE:
|
||||||
|
( Simulation of Ball Contact. )
|
||||||
|
LIBRARY DEPENDENCIES:
|
||||||
|
((contact/src/Contact.cpp))
|
||||||
|
*************************************************************/
|
||||||
|
#include "sim_objects/default_trick_sys.sm"
|
||||||
|
##include "contact/include/Contact.hh"
|
||||||
|
class ContactSimObject : public Trick::SimObject {
|
||||||
|
public:
|
||||||
|
Contact contact;
|
||||||
|
|
||||||
|
ContactSimObject() {
|
||||||
|
("default_data") contact.default_data() ;
|
||||||
|
("initialization") contact.state_init() ;
|
||||||
|
("derivative") contact.state_deriv() ;
|
||||||
|
("integration") trick_ret = contact.state_integ() ;
|
||||||
|
("dynamic_event") contact.collision() ;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
ContactSimObject dyn;
|
||||||
|
IntegLoop dyn_integloop(0.1) dyn;
|
2
trick_sims/SIM_contact/S_overrides.mk
Executable file
2
trick_sims/SIM_contact/S_overrides.mk
Executable file
@ -0,0 +1,2 @@
|
|||||||
|
TRICK_CFLAGS += -Imodels
|
||||||
|
TRICK_CXXFLAGS += -Imodels
|
BIN
trick_sims/SIM_contact/images/TwoBalls.png
Normal file
BIN
trick_sims/SIM_contact/images/TwoBalls.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 236 KiB |
21
trick_sims/SIM_contact/models/contact/include/Ball.hh
Normal file
21
trick_sims/SIM_contact/models/contact/include/Ball.hh
Normal file
@ -0,0 +1,21 @@
|
|||||||
|
/********************************* TRICK HEADER *******************************
|
||||||
|
PURPOSE: ( Simulate balls contacting boundaries. )
|
||||||
|
LIBRARY DEPENDENCY:
|
||||||
|
((Ball.o))
|
||||||
|
*******************************************************************************/
|
||||||
|
#ifndef _ball_hh_
|
||||||
|
#define _ball_hh_
|
||||||
|
|
||||||
|
class Ball {
|
||||||
|
public:
|
||||||
|
Ball(){}
|
||||||
|
double pos[2];
|
||||||
|
double vel[2];
|
||||||
|
double mass;
|
||||||
|
double radius;
|
||||||
|
Ball(double x, double y, double vx, double vy, double r, double m);
|
||||||
|
};
|
||||||
|
|
||||||
|
Ball* make_Ball(double x, double y, double vx, double vy, double r, double m);
|
||||||
|
|
||||||
|
#endif
|
27
trick_sims/SIM_contact/models/contact/include/Contact.hh
Executable file
27
trick_sims/SIM_contact/models/contact/include/Contact.hh
Executable file
@ -0,0 +1,27 @@
|
|||||||
|
/************************************************************************
|
||||||
|
PURPOSE: (Simulate a contact.)
|
||||||
|
LIBRARY DEPENDENCIES:
|
||||||
|
((contact/src/Contact.o))
|
||||||
|
**************************************************************************/
|
||||||
|
#ifndef _contact_hh_
|
||||||
|
#define _contact_hh_
|
||||||
|
#include "trick/regula_falsi.h"
|
||||||
|
#include "Ball.hh"
|
||||||
|
|
||||||
|
class Contact {
|
||||||
|
|
||||||
|
public:
|
||||||
|
Contact(){}
|
||||||
|
Ball ** balls;
|
||||||
|
unsigned int nballs;
|
||||||
|
REGULA_FALSI rf ;
|
||||||
|
|
||||||
|
void ballCollision(Ball &b1, Ball &b2);
|
||||||
|
int default_data();
|
||||||
|
int state_init();
|
||||||
|
int state_deriv();
|
||||||
|
int state_integ();
|
||||||
|
double collision();
|
||||||
|
|
||||||
|
};
|
||||||
|
#endif
|
22
trick_sims/SIM_contact/models/contact/src/Ball.cpp
Normal file
22
trick_sims/SIM_contact/models/contact/src/Ball.cpp
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
/********************************* TRICK HEADER *******************************
|
||||||
|
PURPOSE: ( Simulate balls contacting boundaries. )
|
||||||
|
LIBRARY DEPENDENCY:
|
||||||
|
((Ball.o))
|
||||||
|
*******************************************************************************/
|
||||||
|
#include "../include/Ball.hh"
|
||||||
|
#include "trick/memorymanager_c_intf.h"
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
Ball::Ball(double x, double y, double vx, double vy, double r, double m) {
|
||||||
|
pos[0] = x;
|
||||||
|
pos[1] = y;
|
||||||
|
vel[0] = vx;
|
||||||
|
vel[1] = vy;
|
||||||
|
radius = r;
|
||||||
|
mass = m;
|
||||||
|
}
|
||||||
|
|
||||||
|
Ball* make_Ball(double x, double y, double vx, double vy, double r, double m) {
|
||||||
|
Ball* b = (Ball*)TMM_declare_var_s("Ball");
|
||||||
|
return (new (b) Ball(x,y,vx,vy,r,m));
|
||||||
|
}
|
101
trick_sims/SIM_contact/models/contact/src/Contact.cpp
Executable file
101
trick_sims/SIM_contact/models/contact/src/Contact.cpp
Executable file
@ -0,0 +1,101 @@
|
|||||||
|
/********************************* TRICK HEADER *******************************
|
||||||
|
PURPOSE: ( Simulate balls contacting boundaries. )
|
||||||
|
LIBRARY DEPENDENCY:
|
||||||
|
((Contact.o)
|
||||||
|
(Ball.o))
|
||||||
|
*******************************************************************************/
|
||||||
|
#include <math.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include "trick/integrator_c_intf.h"
|
||||||
|
#include "../include/Contact.hh"
|
||||||
|
|
||||||
|
int Contact::default_data() {
|
||||||
|
balls = NULL;
|
||||||
|
nballs = 0;
|
||||||
|
return (0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Contact::state_init() {
|
||||||
|
rf.mode = Decreasing;
|
||||||
|
rf.error_tol = 0.001;
|
||||||
|
return (0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Contact::state_deriv() {
|
||||||
|
return(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Contact::state_integ() {
|
||||||
|
int integration_step;
|
||||||
|
for (unsigned int ii = 0; ii < nballs; ii++) {
|
||||||
|
// Be sure to fix load_indexed_state() so it only loads @ intermediate_step == 0
|
||||||
|
load_indexed_state( 2*ii, balls[ii]->pos[0]);
|
||||||
|
load_indexed_state( 2*ii+1, balls[ii]->pos[1]);
|
||||||
|
}
|
||||||
|
for (unsigned int ii = 0; ii < nballs; ii++) {
|
||||||
|
load_indexed_deriv( 2*ii, balls[ii]->vel[0]);
|
||||||
|
load_indexed_deriv( 2*ii+1, balls[ii]->vel[1]);
|
||||||
|
}
|
||||||
|
integration_step = integrate();
|
||||||
|
for (unsigned int ii = 0; ii < nballs; ii++) {
|
||||||
|
balls[ii]->pos[0] = unload_indexed_state( 2*ii );
|
||||||
|
balls[ii]->pos[1] = unload_indexed_state( 2*ii+1 );
|
||||||
|
}
|
||||||
|
return(integration_step);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Contact::ballCollision(Ball &b1, Ball &b2) {
|
||||||
|
double unorm[2];
|
||||||
|
double utang[2];
|
||||||
|
if ((b1.mass > 0.0) && (b2.mass > 0.0)) {
|
||||||
|
unorm[0] = ( b2.pos[0] - b1.pos[0] ) / ( b2.radius + b1.radius );
|
||||||
|
unorm[1] = ( b2.pos[1] - b1.pos[1] ) / ( b2.radius + b1.radius );
|
||||||
|
utang[0] = -unorm[1];
|
||||||
|
utang[1] = unorm[0];
|
||||||
|
double b1vn = unorm[0] * b1.vel[0] + unorm[1] * b1.vel[1];
|
||||||
|
double b1vt = utang[0] * b1.vel[0] + utang[1] * b1.vel[1];
|
||||||
|
double b2vn = unorm[0] * b2.vel[0] + unorm[1] * b2.vel[1];
|
||||||
|
double b2vt = utang[0] * b2.vel[0] + utang[1] * b2.vel[1];
|
||||||
|
double b1vtAfter = b1vt;
|
||||||
|
double b2vtAfter = b2vt;
|
||||||
|
double b1vnAfter = ( b1vn * (b1.mass - b2.mass) + 2.0 * b2.mass * b2vn ) / ( b1.mass + b2.mass );
|
||||||
|
double b2vnAfter = ( b2vn * (b2.mass - b1.mass) + 2.0 * b1.mass * b1vn ) / ( b1.mass + b2.mass );
|
||||||
|
b1.vel[0] = b1vnAfter * unorm[0] + b1vtAfter * utang[0];
|
||||||
|
b1.vel[1] = b1vnAfter * unorm[1] + b1vtAfter * utang[1];
|
||||||
|
b2.vel[0] = b2vnAfter * unorm[0] + b2vtAfter * utang[0];
|
||||||
|
b2.vel[1] = b2vnAfter * unorm[1] + b2vtAfter * utang[1];
|
||||||
|
} else {
|
||||||
|
std::cout << "ERROR: Balls must have positive mass." << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double Contact::collision() {
|
||||||
|
double tgo ; /* time-to-go */
|
||||||
|
double now ; /* current integration time. */
|
||||||
|
int first, second;
|
||||||
|
|
||||||
|
double min_dist = 1000000.0;
|
||||||
|
for (unsigned int ii = 0; ii < nballs; ii++) {
|
||||||
|
for (unsigned int jj = ii+1; jj < nballs; jj++) {
|
||||||
|
double xdiff = balls[ii]->pos[0] - balls[jj]->pos[0];
|
||||||
|
double ydiff = balls[ii]->pos[1] - balls[jj]->pos[1];
|
||||||
|
double dist = sqrt( xdiff * xdiff + ydiff * ydiff) - (balls[ii]->radius + balls[jj]->radius);
|
||||||
|
if (dist < min_dist) {
|
||||||
|
min_dist = dist;
|
||||||
|
first = ii;
|
||||||
|
second = jj;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
rf.error = min_dist;
|
||||||
|
|
||||||
|
now = get_integ_time() ; /* Get the current integration time */
|
||||||
|
tgo = regula_falsi( now, &rf ) ; /* Estimate remaining integration time. */
|
||||||
|
if (tgo == 0.0) { /* If we are at the event, it's action time! */
|
||||||
|
now = get_integ_time() ;
|
||||||
|
ballCollision(*balls[first], *balls[second]);
|
||||||
|
std::cout << "Ball["<<first<<"] and Ball["<<second<<"] collide at t = "<<now<<"." << std::endl;
|
||||||
|
reset_regula_falsi( now, &rf );
|
||||||
|
}
|
||||||
|
return (tgo) ;
|
||||||
|
}
|
36
trick_sims/SIM_contact/models/graphics/Makefile
Executable file
36
trick_sims/SIM_contact/models/graphics/Makefile
Executable file
@ -0,0 +1,36 @@
|
|||||||
|
SHELL = /bin/sh
|
||||||
|
|
||||||
|
PROJECT_NAME = BallDisplay
|
||||||
|
SRC_DIR = src
|
||||||
|
BUILD_DIR = build
|
||||||
|
CLASSES_DIR = $(BUILD_DIR)/classes
|
||||||
|
JAR_DIR = dist
|
||||||
|
MAIN_CLASS = BallDisplay
|
||||||
|
|
||||||
|
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)/BallDisplay.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 "-------------------------------------------------------------------------------"
|
280
trick_sims/SIM_contact/models/graphics/src/BallDisplay.java
Executable file
280
trick_sims/SIM_contact/models/graphics/src/BallDisplay.java
Executable file
@ -0,0 +1,280 @@
|
|||||||
|
/*
|
||||||
|
* Trick
|
||||||
|
* 2020 (c) National Aeronautics and Space Administration (NASA)
|
||||||
|
*
|
||||||
|
* @author penn
|
||||||
|
*/
|
||||||
|
|
||||||
|
import java.awt.Color;
|
||||||
|
import java.awt.Graphics2D;
|
||||||
|
import java.awt.Graphics;
|
||||||
|
import java.awt.RenderingHints;
|
||||||
|
import java.io.BufferedOutputStream;
|
||||||
|
import java.io.BufferedReader;
|
||||||
|
import java.io.DataOutputStream;
|
||||||
|
import java.io.IOException;
|
||||||
|
import java.io.InputStreamReader;
|
||||||
|
import java.net.Socket;
|
||||||
|
import javax.swing.BoxLayout;
|
||||||
|
import javax.swing.JFrame;
|
||||||
|
import javax.swing.JPanel;
|
||||||
|
|
||||||
|
class Ball {
|
||||||
|
public Color color;
|
||||||
|
public double x;
|
||||||
|
public double y;
|
||||||
|
public double radius;
|
||||||
|
public int identity;
|
||||||
|
public Ball (int id) {
|
||||||
|
identity = id;
|
||||||
|
x = 0.0;
|
||||||
|
y = 0.0;
|
||||||
|
radius = 0.5;
|
||||||
|
if (id == 0) {
|
||||||
|
color = Color.GREEN;
|
||||||
|
} else {
|
||||||
|
color = Color.LIGHT_GRAY;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
class RangeView extends JPanel {
|
||||||
|
|
||||||
|
private int scale;
|
||||||
|
private Color backGroundColor;
|
||||||
|
|
||||||
|
// Origin of world coordinates in jpanel coordinates.
|
||||||
|
private int worldOriginX;
|
||||||
|
private int worldOriginY;
|
||||||
|
|
||||||
|
public Ball[] balls;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Class constructor.
|
||||||
|
*/
|
||||||
|
public RangeView( int mapScale, int numberOfBalls) {
|
||||||
|
setScale(mapScale);
|
||||||
|
backGroundColor = Color.WHITE;
|
||||||
|
balls = new Ball[numberOfBalls];
|
||||||
|
for (int ii=0 ; ii<numberOfBalls ; ii++) {
|
||||||
|
balls[ii] = new Ball(ii);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setScale (int mapScale) {
|
||||||
|
if (mapScale < 4) {
|
||||||
|
scale = 4;
|
||||||
|
} else if (mapScale > 128) {
|
||||||
|
scale = 128;
|
||||||
|
} else {
|
||||||
|
scale = mapScale;
|
||||||
|
}
|
||||||
|
repaint();
|
||||||
|
}
|
||||||
|
|
||||||
|
public int getScale() {
|
||||||
|
return scale;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void drawCenteredCircle(Graphics2D g, int x, int y, int d) {
|
||||||
|
x = x-(d/2);
|
||||||
|
y = y-(d/2);
|
||||||
|
g.fillOval(x,y,d,d);
|
||||||
|
}
|
||||||
|
|
||||||
|
private void doDrawing(Graphics g) {
|
||||||
|
Graphics2D g2d = (Graphics2D) g;
|
||||||
|
|
||||||
|
RenderingHints rh = new RenderingHints(
|
||||||
|
RenderingHints.KEY_ANTIALIASING,
|
||||||
|
RenderingHints.VALUE_ANTIALIAS_ON);
|
||||||
|
|
||||||
|
rh.put(RenderingHints.KEY_RENDERING,
|
||||||
|
RenderingHints.VALUE_RENDER_QUALITY);
|
||||||
|
|
||||||
|
int width = getWidth();
|
||||||
|
int height = getHeight();
|
||||||
|
|
||||||
|
worldOriginX = (width/2);
|
||||||
|
worldOriginY = (height/2);
|
||||||
|
|
||||||
|
// Draw Background
|
||||||
|
g2d.setPaint(backGroundColor);
|
||||||
|
g2d.fillRect(0, 0, width, height);
|
||||||
|
|
||||||
|
// Draw balls
|
||||||
|
for (int ii = 0; ii < balls.length ; ii++) {
|
||||||
|
g2d.setPaint(balls[ii].color);
|
||||||
|
int bx = (int)(worldOriginX + scale * balls[ii].x);
|
||||||
|
int by = (int)(worldOriginY - scale * balls[ii].y);
|
||||||
|
drawCenteredCircle(g2d, bx, by, (int)(scale * 2 * balls[ii].radius));
|
||||||
|
g2d.setPaint(Color.BLACK);
|
||||||
|
g2d.drawString ( String.format("%d",ii), bx,by);
|
||||||
|
}
|
||||||
|
|
||||||
|
g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void paintComponent(Graphics g) {
|
||||||
|
super.paintComponent(g);
|
||||||
|
doDrawing(g);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class BallDisplay extends JFrame {
|
||||||
|
|
||||||
|
private RangeView rangeView;
|
||||||
|
private BufferedReader in;
|
||||||
|
private DataOutputStream out;
|
||||||
|
private JPanel panelGroup0;
|
||||||
|
private JPanel panelGroup1;
|
||||||
|
|
||||||
|
public BallDisplay() {
|
||||||
|
|
||||||
|
rangeView = null;
|
||||||
|
setTitle("Lander Range");
|
||||||
|
setSize(800, 500);
|
||||||
|
setLocationRelativeTo(null);
|
||||||
|
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
|
||||||
|
setFocusable(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void connectToServer(String host, int port ) throws IOException {
|
||||||
|
Socket socket = new Socket(host, port);
|
||||||
|
in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
|
||||||
|
out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setRangeView (RangeView arena) {
|
||||||
|
rangeView = arena;
|
||||||
|
panelGroup1 = new JPanel();
|
||||||
|
panelGroup1.setLayout(new BoxLayout(panelGroup1, BoxLayout.X_AXIS));
|
||||||
|
panelGroup1.add(rangeView);
|
||||||
|
add(panelGroup1);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void printHelpText() {
|
||||||
|
System.out.println(
|
||||||
|
"----------------------------------------------------------------------\n"
|
||||||
|
+ "usage: java jar BallDisplay.jar <port-number>\n"
|
||||||
|
+ "----------------------------------------------------------------------\n"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void main(String[] args) throws IOException, InterruptedException {
|
||||||
|
|
||||||
|
String host = "localHost";
|
||||||
|
int port = 0;
|
||||||
|
boolean boom = false;
|
||||||
|
|
||||||
|
// Handle program arguments.
|
||||||
|
int ii = 0;
|
||||||
|
while (ii < args.length) {
|
||||||
|
switch (args[ii]) {
|
||||||
|
case "-help" :
|
||||||
|
case "--help" : {
|
||||||
|
printHelpText();
|
||||||
|
System.exit(0);
|
||||||
|
} break;
|
||||||
|
default : {
|
||||||
|
port = (Integer.parseInt(args[ii]));
|
||||||
|
} break;
|
||||||
|
}
|
||||||
|
++ii;
|
||||||
|
}
|
||||||
|
|
||||||
|
boolean go = true;
|
||||||
|
double dt = 0.100; // Time between updates (seconds).
|
||||||
|
int mapScale = 16 ; // pixels per meter.
|
||||||
|
int nballs = 7;
|
||||||
|
|
||||||
|
if (port == 0) {
|
||||||
|
System.out.println("No variable server port specified.");
|
||||||
|
printHelpText();
|
||||||
|
System.exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
BallDisplay ballDisplay = new BallDisplay();
|
||||||
|
|
||||||
|
// Connect to the Trick simulation's variable server.
|
||||||
|
System.out.println("Connecting to: " + host + ":" + port);
|
||||||
|
ballDisplay.connectToServer(host, port);
|
||||||
|
ballDisplay.out.writeBytes("trick.var_set_client_tag(\"BallDisplay\") \n");
|
||||||
|
ballDisplay.out.flush();
|
||||||
|
|
||||||
|
|
||||||
|
// Get the number of balls.
|
||||||
|
ballDisplay.out.writeBytes(
|
||||||
|
"trick.var_add(\"dyn.contact.nballs\")\n" +
|
||||||
|
"trick.var_send() \n" +
|
||||||
|
"trick.var_clear() \n");
|
||||||
|
ballDisplay.out.flush();
|
||||||
|
try {
|
||||||
|
String line;
|
||||||
|
String field[];
|
||||||
|
line = ballDisplay.in.readLine();
|
||||||
|
field = line.split("\t");
|
||||||
|
nballs = Integer.parseInt( field[1]);
|
||||||
|
} catch (IOException | NullPointerException e ) {
|
||||||
|
go = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
RangeView rangeView = new RangeView(mapScale, nballs);
|
||||||
|
ballDisplay.setRangeView(rangeView);
|
||||||
|
ballDisplay.setVisible(true);
|
||||||
|
|
||||||
|
// Get the Radii of the balls.
|
||||||
|
for ( ii = 0; ii < nballs; ii ++) {
|
||||||
|
ballDisplay.out.writeBytes( String.format("trick.var_add(\"dyn.contact.balls[%d][0].radius\")\n", ii));
|
||||||
|
}
|
||||||
|
ballDisplay.out.flush();
|
||||||
|
ballDisplay.out.writeBytes(
|
||||||
|
"trick.var_send() \n" +
|
||||||
|
"trick.var_clear() \n");
|
||||||
|
ballDisplay.out.flush();
|
||||||
|
try {
|
||||||
|
String line;
|
||||||
|
String field[];
|
||||||
|
line = ballDisplay.in.readLine();
|
||||||
|
field = line.split("\t");
|
||||||
|
for ( ii=0; ii < nballs; ii++) {
|
||||||
|
rangeView.balls[ii].radius = Double.parseDouble( field[ii+1]);
|
||||||
|
}
|
||||||
|
} catch (IOException | NullPointerException e ) {
|
||||||
|
go = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the Positions of the balls, and update the display, periodically.
|
||||||
|
ballDisplay.out.writeBytes( "trick.var_pause() \n");
|
||||||
|
for ( ii = 0; ii < nballs; ii ++) {
|
||||||
|
ballDisplay.out.writeBytes(
|
||||||
|
String.format("trick.var_add(\"dyn.contact.balls[%d][0].pos[0]\")\n", ii)
|
||||||
|
+ String.format("trick.var_add(\"dyn.contact.balls[%d][0].pos[1]\")\n", ii)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
ballDisplay.out.writeBytes("trick.var_ascii() \n" +
|
||||||
|
String.format("trick.var_cycle(%.3f)\n", dt) +
|
||||||
|
"trick.var_unpause() \n" );
|
||||||
|
ballDisplay.out.flush();
|
||||||
|
while (go) {
|
||||||
|
try {
|
||||||
|
String line;
|
||||||
|
String field[];
|
||||||
|
line = ballDisplay.in.readLine();
|
||||||
|
// System.out.println("Sim->Client:" + line);
|
||||||
|
field = line.split("\t");
|
||||||
|
for ( ii=0; ii < nballs; ii++) {
|
||||||
|
rangeView.balls[ii].x = Double.parseDouble( field[2*ii+1]);
|
||||||
|
rangeView.balls[ii].y = Double.parseDouble( field[2*ii+2]);
|
||||||
|
}
|
||||||
|
} catch (IOException | NullPointerException e ) {
|
||||||
|
go = false;
|
||||||
|
}
|
||||||
|
// Update the scene.
|
||||||
|
rangeView.repaint();
|
||||||
|
} // while
|
||||||
|
|
||||||
|
} // main
|
||||||
|
} // class
|
Loading…
Reference in New Issue
Block a user