double square(WAVESOURCE* self, HYSTERESIS* mem, double phase)
{
if(phase > 0)
{
return saw(self,mem,phase) - saw(self,mem,phase - self->half_phase);
}
else
{
return saw(self,mem,phase) - saw(self,mem,phase + self->half_phase);
}
}
double saw(double angle)
{
while (angle >= 2.0 * PI) angle -= 2.0 * PI;
if (angle >= PI) return -saw(angle - PI);
if (angle >= PI / 2.0) angle = PI - angle;
return angle / (PI / 2.0 ) * 2.0 - 1.0;
}
void mix_instrument(byte ins_id, float angle, byte vol)
{
float value;
byte snd;
switch (ins_id) {
default:
case 0:
value = sin(angle);
snd = (value + 1) / 2 * 255;
break;
case 1:
value = saw(angle);
snd = value * 32 + 127;
break;
case 2:
value = ((rand() % 256) - 128) / 255.0;
snd = value * 64 + 127;
break;
}
#if (MIX_WAY == 0)
snd_data = MIX(snd_data - 127, vol * snd / 64 - 127) + 127;
#else
byte m1, m2;
m1 = snd_data;
m2 = vol * snd / 64;
if (m1 + m2 != 0)
snd_data = MIX(m1, m2);
#endif
}
/* Gets the index of a frame, either returning an existing index if we've seen
* it before or adding it if not. */
static MVMuint64 get_frame_idx(MVMThreadContext *tc, MVMHeapSnapshotState *ss,
MVMFrame *frame) {
MVMuint64 idx;
if (!seen(tc, ss, frame, &idx)) {
idx = push_workitem(tc, ss, MVM_SNAPSHOT_COL_KIND_FRAME, frame);
saw(tc, ss, frame, idx);
}
return idx;
}
int main(int argc, char **argv)
{
// Parse parameters
namespace po = boost::program_options;
// Declare the supported options
po::options_description desc("Allowed options");
desc.add_options()
("help", "Show help message")
("debug", "Run in debug/test mode")
("urdf_path", po::value<std::string>(), "Optional, relative path to URDF in URDF package")
("config_pkg", po::value<std::string>(), "Optional, pass in existing config package to load");
// Process options
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if (vm.count("help"))
{
std::cout << desc << std::endl;
return 1;
}
// Start ROS Node
ros::init(argc, argv, "moveit_setup_assistant", ros::init_options::NoSigintHandler);
// ROS Spin
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle nh;
// Create Qt Application
QApplication qtApp(argc, argv);
// Load Qt Widget
moveit_setup_assistant::SetupAssistantWidget saw( NULL, vm );
saw.setMinimumWidth(1024);
saw.setMinimumHeight(768);
// saw.setWindowState( Qt::WindowMaximized );
saw.show();
signal(SIGINT, siginthandler);
// Wait here until Qt App is finished
const int result = qtApp.exec();
// Shutdown ROS
ros::shutdown();
return result;
}
float Oscillator::getValue() {
switch(type) {
case SINE:
return sine();
case SAW:
return saw();
case SQUARE:
return square();
case TRIANGLE:
return triangle();
case NOISE:
return noise();
}
}
/* Gets the index of a collectable, either returning an existing index if we've
* seen it before or adding it if not. */
static MVMuint64 get_collectable_idx(MVMThreadContext *tc,
MVMHeapSnapshotState *ss, MVMCollectable *collectable) {
MVMuint64 idx;
if (!seen(tc, ss, collectable, &idx)) {
if (collectable->flags & MVM_CF_STABLE)
idx = push_workitem(tc, ss, MVM_SNAPSHOT_COL_KIND_STABLE, collectable);
else if (collectable->flags & MVM_CF_TYPE_OBJECT)
idx = push_workitem(tc, ss, MVM_SNAPSHOT_COL_KIND_TYPE_OBJECT, collectable);
else if (collectable->flags & MVM_CF_FRAME)
idx = push_workitem(tc, ss, MVM_SNAPSHOT_COL_KIND_FRAME, collectable);
else
idx = push_workitem(tc, ss, MVM_SNAPSHOT_COL_KIND_OBJECT, collectable);
saw(tc, ss, collectable, idx);
}
return idx;
}
void main()
{
WAVESOURCE waves;
double d=6.0/100000.0, x=-3.1;
init_waves(&waves);
for(x=-3.1;x<3.1;x+=d)
{
saw(&waves,0,x);
// printf("%f, ",x);
}
for(x=-3.1;x<3.1;x+=d)
{
addsaw(x);
}
}
int ginit_main(int argc UNUSED_PARAM, char **argv)
{
FILE *mntlist;
bool ismnted_dev, ismnted_sys, ismnted_usr;
struct mntent *mntent;
/*
int fd = open("/dev/console", O_RDWR);
if (fd >= 0) {
dup2(fd, 0);
dup2(fd, 1);
dup2(fd, 2);
}
*/
/* If given an argv[] with an applet name, run it instead.
* Makes recovering simple by doing: init=/ginit bb
*/
if (argv[1] && argv[1][0] != '/') {
eprintf("running user requested applet %s\n", argv[1]);
return spawn_and_wait(argv+1);
}
#define saw(argv...) \
({ \
static const char *args[] = { argv, NULL }; \
/* These casts are fine -- see process_args for mem setup */ \
process_args((void *)args); \
spawn_and_wait((void *)args); \
})
/* First setup basic /dev */
if (saw("mountpoint", "-q", "/dev") != 0) {
/* Try /etc/fstab */
if (saw("mount", "-n", "/dev"))
/* Then devtmpfs */
if (saw("mount", "-n", "-t", "devtmpfs", "devtmpfs", "/dev"))
/* Finally normal tmpfs */
saw("mount", "-n", "-t", "tmpfs", "dev", "/dev");
} else {
eprintf("%s appears to be mounted; skipping its setup\n", "/dev");
}
/* If /dev is empty (e.g. tmpfs), run mdev to seed things */
if (access("/dev/console", F_OK) != 0) {
if (saw("mountpoint", "-q", "/sys") != 0) {
if (saw("mount", "-n", "/sys"))
saw("mount", "-n", "-t", "sysfs", "sysfs", "/sys");
} else {
eprintf("%s appears to be mounted; skipping its setup\n", "/sys");
}
/* Mount /proc as mdev will fork+exec /proc/self/exe */
if (saw("mountpoint", "-q", "/proc") != 0) {
/* Try /etc/fstab */
if (saw("mount", "-n", "/proc"))
saw("mount", "-n", "-t", "proc", "proc", "/proc");
}
saw("mdev", "-s");
}
/* Then seed the stuff we care about */
saw("mkdir", "-p", "/dev/pts", "/dev/shm");
/* Then mount /usr */
if (saw("mountpoint", "-q", "/usr") != 0) {
saw("mount", "-n", "/usr", "-o", "ro");
} else {
eprintf("%s appears to be mounted; skipping its setup\n", "/usr");
}
/* Now that we're all done, exec the real init */
if (!argv[1]) {
argv[0] = (void *)"/sbin/init";
argv[1] = NULL;
} else
++argv;
process_args(argv);
return execv(argv[0], argv);
}
uint16_t saw8reverse(uint16_t tick, uint32_t color) {
return saw(tick, color, 8, false);
}
uint16_t saw8forward(uint16_t tick, uint32_t color) {
return saw(tick, color, 8, true);
}
int main(void) {
int f,i,j,k,c,x,y,ix,iy,displayloop;
float thru,ctmp;
RGB rgb;
FILE *fp;
srand((int)time(NULL));
usingmap=0;
makingmap=1;
mmx=0;
mmy=0;
img=(uchar **)calloc(scrhei,sizeof(uchar *));
img2=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0; y<scrhei; y++) {
img[y]=(uchar *)calloc(scrwid,sizeof(uchar));
img2[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0; x<scrwid; x++) {
img[y][x]=255*y/scrhei;
img2[y][x]=myrnd()*255;
if (x<scrwid-1 && y<scrhei-1) {
pix[x][y][usingmap]=x;
piy[x][y][usingmap]=y;
for (i=0; i<=1; i++)
for (j=0; j<=1; j++)
amount[x][y][i][j][usingmap]=(float)1/4;
}
}
}
/* Originals from QB
op[0] = 1; damp[0] = .999; force[0] = .005;
op[1] = 1.02; damp[1] = .999; force[1] = .002;
op[2] = 0; damp[2] = .999; force[2] = .002;
op[3] = 1; damp[3] = .999; force[3] = .005;
op[4] = 1; damp[4] = .999; force[4] = .005;
op[5] = 0; damp[5] = .999; force[5] = .002;
*/
/* From QB later
name$(1) = "Velocity"
op(1) = 1: damp(1) = .999: force(1) = .002
name$(2) = "Rotation"
op(2) = 0: damp(2) = .999: force(2) = .002
name$(3) = "Drip"
op(3) = 1: damp(3) = .999: force(3) = .005
name$(4) = "Dribble"
op(4) = 1: damp(4) = .999: force(4) = .005
name$(5) = "Slide"
op(5) = 0: damp(5) = .999: force(5) = .002
name$(6) = "Accelerate"
op(6) = 1: damp(6) = .999: force(6) = .005
name$(7) = "xDisplace"
op(7) = 0: damp(7) = .999: force(7) = .005
name$(8) = "yDisplace"
op(8) = 0: damp(8) = .999: force(8) = .005
REM 9 and 10 are options for splitting displacements (no var)
name$(9) = "2d/3d split"
name$(10) = "Split"
*/
// 0 Accelerate
op[0] = 1;
damp[0] = .999;
force[0] = .005;
// 1 Velocity
op[1] = 1.02;
damp[1] = .999;
force[1] = .01;
// 2 Rotation
op[2] = 0;
damp[2] = .995;
force[2] = .03;
// 3 y splurge
op[3] = 0;
damp[3] = .999;
force[3] = .01;
// 4 Dribble
op[4] = 1;
damp[4] = 0;
force[4] = .01;
// 5 x splurge
op[5] = 0;
damp[5] = .999;
force[5] = .01;
op[6]=2;
damp[6]=.9999;
force[6]=.01;
op[7]=1;
damp[7]=.999;
force[7]=.01;
for (f=0; f<fs; f++) {
var[f] = op[f];
fon[f]=1;
}
allegrosetup(scrwid,scrhei);
_farsetsel(screen->seg);
starttimer();
while(!key[KEY_ESC]) {
moremap();
// Animate
for (x=0; x<scrwid; x++) {
for (y=0; y<scrhei; y++) {
c=0;
for (i=0; i<=1; i++) {
for (j=0; j<=1; j++) {
c=c+amount[x][y][i][j][usingmap]*img[piy[x][y][usingmap]+j][pix[x][y][usingmap]+i];
}
}
c--;
img2[y][x]=c;
}
}
/* for (y=0;y<scrhei;y++) {
for (x=0;x<scrwid;x++) {
_farpokeb(screen->seg, (unsigned long)screen->line[y]+x, img2[y][x]);
}
}*/
for (y=0; y<scrhei; y++) {
movedata(_my_ds(), img2[y], screen->seg, bmp_write_line(screen,y), scrwid);
}
for (f=0; f<fs; f++) {
if (fon[f]) {
hline(screen, scrwid/2, f*2, scrwid/2+(var[f] - op[f]) * scrwid * 4, 0);
}
}
toff=toff-(float)1/128;
for (c=0; c<=255; c++) {
thru=saw((float)c/255-toff);
rgb.r=huefor(thru,(float)0);
rgb.g=huefor(thru,(float)1/3);
rgb.b=huefor(thru,(float)2/3);
set_color(c,&rgb);
}
imgtmp=img;
img=img2;
img2=imgtmp;
for (i=1; i<=5; i++) {
mycircle(myrnd()*scrwid,myrnd()*scrhei,2+myrnd()*8,myrnd()*255);
}
framedone();
}
allegro_exit();
displayframespersecond();
}
int main(void) {
int f,i,j,k,c,x,y,ix,iy,displayloop;
int usingmap,makingmap,mmx,mmy,tmpmap,maploop;
float rx,ry,nrx,nry,px,py;
RGB rgb;
FILE *fp;
img=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0; y<scrhei; y++) {
img[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0; x<scrwid; x++) {
img[y][x]=myrnd()*255;
}
}
img2=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0; y<scrhei; y++) {
img2[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0; x<scrwid; x++) {
img2[y][x]=myrnd()*255;
}
}
srand((int)time(NULL));
usingmap=0;
makingmap=1;
mmx=0;
mmy=0;
/* Originals from QB
op[0] = 1; damp[0] = .999; force[0] = .005;
op[1] = 1.02; damp[1] = .999; force[1] = .002;
op[2] = 0; damp[2] = .999; force[2] = .002;
op[3] = 1; damp[3] = .999; force[3] = .005;
op[4] = 1; damp[4] = .999; force[4] = .005;
op[5] = 0; damp[5] = .999; force[5] = .002;
*/
// 0 Accelerate
op[0] = 1;
damp[0] = .999;
force[0] = .005;
// 1 Velocity
op[1] = 1.02;
damp[1] = .999;
force[1] = .01;
// 2 Rotation
op[2] = 0;
damp[2] = .999;
force[2] = .05;
// 3 Drip
op[3] = 1;
damp[3] = .999;
force[3] = .03;
// 4 Dribble
op[4] = 1;
damp[4] = .999;
force[4] = .01;
// 5 Slide
op[5] = 0;
damp[5] = .999;
force[5] = .01;
for (f=0; f<fs; f++) {
var[f] = op[f];
fon[f]=1;
}
allegro_init ();
install_keyboard ();
install_timer ();
set_gfx_mode (GFX_AUTODETECT, scrwid, scrhei, 0, 0);
set_pallete (desktop_palette);
_farsetsel(screen->seg);
for (c=0; c<=255; c++) {
rgb.r=saw(0,c);
rgb.g=saw(256/3,c);
rgb.b=saw(2*256/3,c);
set_color(c,&rgb);
}
while(!key[KEY_ESC]) {
// Generate some more of the map
for (maploop=1; maploop<scrwid*scrhei/15; maploop++) {
rx=(float)mmx/scrwid*2-1;
ry=(float)(mmy-scrhei/2)/scrwid*2;
if (fon[1]) {
rx = rx / var[1];
ry = ry / var[1];
}
if (fon[0]) {
rx = mysgn(rx)/var[1]*mypow(myabs(rx),1/var[6]);
ry = mysgn(ry)/var[1]*mypow(myabs(ry),1/var[6]);
}
if (fon[2]) {
nrx = rx * cos(var[2]) + ry * sin(var[2]);
nry = -rx * sin(var[2]) + ry * cos(var[2]);
rx = nrx;
ry=nry;
}
if (fon[3]) {
ry = ry / var[3];
}
if (fon[4]) {
ry = ((myabs(ry) - 1) / var[4] + 1) * mysgn(ry);
}
if (fon[5]) {
rx = rx + var[5] * mysgn(rx);
}
px=(rx+1)/2*scrwid;
py=scrhei/2+(ry)/2*scrwid;
ix=(int)px;
iy=(int)py;
amount[mmx][mmy][0][0][makingmap]=((float)ix+1-(float)px)*((float)(iy+1)-(float)py);
amount[mmx][mmy][1][0][makingmap]=((float)px-(float)ix)*((float)(iy+1)-(float)py);
amount[mmx][mmy][0][1][makingmap]=((float)ix+1-(float)px)*((float)py-(float)iy);
amount[mmx][mmy][1][1][makingmap]=((float)px-(float)ix)*((float)py-(float)iy);
pix[mmx][mmy][makingmap]=ix;
piy[mmx][mmy][makingmap]=iy;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
pix[mmx][mmy][makingmap]=scrwid/2;
piy[mmx][mmy][makingmap]=scrhei/2;
for (i=0; i<=1; i++) {
for (j=0; j<=1; j++) {
amount[mmx][mmy][i][j][makingmap]=0;
}
}
}
mmx++;
if (mmx>=scrwid) {
mmx=0;
mmy++;
if (mmy>=scrhei) {
mmy=0;
tmpmap=usingmap;
usingmap=makingmap;
makingmap=tmpmap;
for (f=0; f<fs; f++) {
perturb(f);
}
for (i=0; i<4; i++) {
f = myrnd() * fs;
if (myrnd()<.8) {
if (myrnd()<.5)
fon[f] = 1;
else
fon[f]=0;
}
}
}
}
}
// Animate
for (x=0; x<scrwid; x++) {
for (y=0; y<scrhei; y++) {
c=0;
for (i=0; i<=1; i++) {
for (j=0; j<=1; j++) {
c=c+amount[x][y][i][j][usingmap]*img[piy[x][y][usingmap]+j][pix[x][y][usingmap]+i];
}
}
c--;
img2[y][x]=c;
}
}
/* for (y=0;y<scrhei;y++) {
for (x=0;x<scrwid;x++) {
_farpokeb(screen->seg, (unsigned long)screen->line[y]+x, img2[y][x]);
}
}*/
for (y=0; y<scrhei; y++) {
movedata(_my_ds(), img2[y], screen->seg, bmp_write_line(screen,y), scrwid);
}
for (f=0; f<fs; f++) {
if (fon[f]) {
hline(screen, scrwid/2, f*2, scrwid/2+(var[f] - op[f]) * scrwid * 4, 0);
}
}
imgtmp=img;
img=img2;
img2=imgtmp;
for (i=1; i<=5; i++) {
mycircle(myrnd()*scrwid,myrnd()*scrhei,2+myrnd()*8,myrnd()*255);
}
}
}
int main(void) {
int f,i,j,k,c,x,y,ix,iy,displayloop;
int usingmap,makingmap,mmx,mmy,tmpmap,maploop;
float rx,ry,nrx,nry,px,py,thru,ctmp;
RGB rgb;
FILE *fp;
srand((int)time(NULL));
usingmap=0; makingmap=1; mmx=0; mmy=0;
img=(uchar **)calloc(scrhei,sizeof(uchar *));
img2=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0;y<scrhei;y++) {
img[y]=(uchar *)calloc(scrwid,sizeof(uchar));
img2[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0;x<scrwid;x++) {
img[y][x]=255*y/scrhei;
img2[y][x]=myrnd()*255;
if (x<scrwid-1 && y<scrhei-1) {
pix[x][y][usingmap]=x;
piy[x][y][usingmap]=y;
for (i=0;i<=1;i++)
for (j=0;j<=1;j++)
amount[x][y][i][j][usingmap]=(float)1/4;
}
}
}
/* Originals from QB
op[0] = 1; damp[0] = .999; force[0] = .005;
op[1] = 1.02; damp[1] = .999; force[1] = .002;
op[2] = 0; damp[2] = .999; force[2] = .002;
op[3] = 1; damp[3] = .999; force[3] = .005;
op[4] = 1; damp[4] = .999; force[4] = .005;
op[5] = 0; damp[5] = .999; force[5] = .002;
*/
// 0 Accelerate
op[0] = 1; damp[0] = .999; force[0] = .005;
// 1 Velocity
op[1] = 1.02; damp[1] = .999; force[1] = .01;
// 2 Rotation
op[2] = 0; damp[2] = .995; force[2] = .03;
// 3 y splurge
op[3] = 0; damp[3] = .999; force[3] = .01;
// 4 Dribble
op[4] = 1; damp[4] = 0; force[4] = .01;
// 5 x splurge
op[5] = 0; damp[5] = .999; force[5] = .01;
op[6]=2;damp[6]=.9999;force[6]=.01;
op[7]=1;damp[7]=.999;force[7]=.01;
for (f=0;f<fs;f++) {
var[f] = op[f];
fon[f]=1;
}
allegrosetup(scrwid,scrhei);
_farsetsel(screen->seg);
starttimer();
while(!key[KEY_ESC]) {
// Generate some more of the map
for (maploop=1;maploop<scrwid*scrhei/20;maploop++) {
rx=(float)mmx/scrwid*2-1;
ry=(float)(mmy-scrhei/2)/scrwid*2;
if (fon[0]) {
rx = mysgn(rx)/var[7]*mypow(myabs(rx),1/var[0]);
ry = mysgn(ry)/var[7]*mypow(myabs(ry),1/var[0]);
}
if (fon[1]) {
rx = rx / var[1]; ry = ry / var[1];
}
if (fon[2]) {
nrx = rx * cos(var[2]) + ry * sin(var[2]);
nry = -rx * sin(var[2]) + ry * cos(var[2]);
rx = nrx; ry=nry;
}
if (fon[3]) {
ry = ry - mysgn(ry) * sin(var[6]*pi*myabs(ry)) * var[3];
}
if (fon[4]) {
ry = ((myabs(ry) - 1) / var[4] + 1) * mysgn(ry);
}
if (fon[5]) {
rx = rx - mysgn(rx) * sin(var[6]*pi*myabs(rx)) * var[5];
}
px=(rx+1)/2*scrwid;
py=scrhei/2+(ry)/2*scrwid;
ix=(int)px;
iy=(int)py;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
ix=px; iy=py;
}
amount[mmx][mmy][0][0][makingmap]=((float)ix+1-(float)px)*((float)(iy+1)-(float)py);
amount[mmx][mmy][1][0][makingmap]=((float)px-(float)ix)*((float)(iy+1)-(float)py);
amount[mmx][mmy][0][1][makingmap]=((float)ix+1-(float)px)*((float)py-(float)iy);
amount[mmx][mmy][1][1][makingmap]=((float)px-(float)ix)*((float)py-(float)iy);
pix[mmx][mmy][makingmap]=ix;
piy[mmx][mmy][makingmap]=iy;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
pix[mmx][mmy][makingmap]=scrwid/2;
piy[mmx][mmy][makingmap]=scrhei/2;
for (i=0;i<=1;i++) {
for (j=0;j<=1;j++) {
amount[mmx][mmy][i][j][makingmap]=0;
}
}
}
mmx++;
if (mmx>=scrwid) {
mmx=0;
mmy++;
if (mmy>=scrhei) {
mmy=0;
tmpmap=usingmap;
usingmap=makingmap;
makingmap=tmpmap;
for (f=0;f<fs;f++) {
perturb(f);
}
}
}
}
// Animate
for (x=0; x<scrwid; x++) {
for (y=0; y<scrhei; y++) {
c=0;
for (i=0;i<=1;i++) {
for (j=0;j<=1;j++) {
c=c+amount[x][y][i][j][usingmap]*img[piy[x][y][usingmap]+j][pix[x][y][usingmap]+i];
}
}
c--;
img2[y][x]=c;
}
}
/* for (y=0;y<scrhei;y++) {
for (x=0;x<scrwid;x++) {
_farpokeb(screen->seg, (unsigned long)screen->line[y]+x, img2[y][x]);
}
}*/
for (y=0; y<scrhei; y++) {
movedata(_my_ds(), img2[y], screen->seg, bmp_write_line(screen,y), scrwid);
}
for (f=0;f<fs;f++) {
if (fon[f]) {
hline(screen, scrwid/2, f*2, scrwid/2+(var[f] - op[f]) * scrwid * 4, 0);
}
}
toff=toff-(float)1/128;
for (c=0;c<=255;c++) {
thru=saw((float)c/255-toff);
rgb.r=huefor(thru,(float)0);
rgb.g=huefor(thru,(float)1/3);
rgb.b=huefor(thru,(float)2/3);
set_color(c,&rgb);
}
imgtmp=img;
img=img2;
img2=imgtmp;
for (i=1;i<=5;i++) {
mycircle(myrnd()*scrwid,myrnd()*scrhei,2+myrnd()*8,myrnd()*255);
}
framedone();
}
allegro_exit();
displayframespersecond();
}
