int YARPMEIDeviceDriver::vMove(void *spds)
{
int16 rc = 0;
double *cmds = (double *) spds;
for(int i = 0; i < _njoints; i++)
rc = v_move(i, cmds[i], _ref_accs[i]);
return rc;
}
int YARPMEIDeviceDriver::safeVMove(void *spds)
{
int16 rc = 0;
double *cmds = (double *) spds;
for(int i = 0; i < _njoints; i++)
{
if (!frames_left(i))
rc = v_move(i, cmds[i], _ref_accs[i]);
}
return rc;
}
//--------------------------------------------------------------------------
void Omu_Integrator::solve(int kk, double tstart, double tend,
const Omu_VariableVec &x, const Omu_VariableVec &u,
Omu_Program *sys, Omu_DependentVec &Ft,
Omu_StateVec &xt)
{
_kk = kk;
// store references to data required by residual() for sys->continuous calls
_sys = sys;
_x_ptr = &x;
_u_ptr = &u;
_Ft_ptr = &Ft;
_xt_ptr = &xt;
// prepare call arguments for high-level solve
v_move(xt, _nd, _n, _xc, 0);
if (_sa) {
m_move(xt.Sx, _nd, 0, _n, _nx, _xc.Sq, 0, 0);
m_move(xt.Su, _nd, 0, _n, _nu, _xc.Sq, 0, _nx);
}
v_move(xt, 0, _nx, _q, 0);
v_move(u, 0, _nu, _q, _nx);
// call high-level solve
solve(kk, tstart, tend, _xc, _dxc, _q, _Fcs[_k]);
// return results
v_move(_xc, 0, _n, xt, _nd);
if (_sa) {
m_move(_xc.Sq, 0, 0, _n, _nx, xt.Sx, _nd, 0);
m_move(_xc.Sq, 0, _nx, _n, _nu, xt.Su, _nd, 0);
}
// release references to data required by residual()
_sys = NULL;
_x_ptr = NULL;
_u_ptr = NULL;
_Ft_ptr = NULL;
_xt_ptr = NULL;
}
//--------------------------------------------------------------------------
void Omu_Integrator::residual(int kk, double t,
const Omu_StateVec &xc, const Omu_StateVec &dxc,
const Omu_Vec &q, Omu_DependentVec &Fc)
{
if (!_sys) {
m_error(E_NULL, "Omu_Integrator::residual");
}
// prepare call arguments
Omu_DependentVec &Ft = *_Ft_ptr;
Omu_StateVec &xt = *_xt_ptr;
v_move(q, 0, _nd, xt, 0);
v_move(xc, 0, _n, xt, _nd);
v_move(dxc, 0, _n, _dxt, _nd);
v_move(q, _nx, _nu, _ut, 0);
Ft.set_required_J(Fc.is_required_J());
// note: treat seed derivatives xc.Sq and dxc.Sq to support forward mode
// call continuous of Omu_Program
_sys->continuous(kk, t, xt, _ut, _dxt, Ft);
// return results
v_move(Ft, _nd, _n, Fc, 0);
if (Fc.is_required_J()) {
if (!Fc.Jx.is_constant())
m_move(Ft.Jx, _nd, _nd, _n, _n, Fc.Jx, 0, 0);
if (!Fc.Jdx.is_constant())
m_move(Ft.Jdx, _nd, _nd, _n, _n, Fc.Jdx, 0, 0);
if (!Fc.Jq.is_constant()) {
m_move(Ft.Jx, _nd, 0, _n, _nd, Fc.Jq, 0, 0);
m_move(Ft.Ju, _nd, 0, _n, _nu, Fc.Jq, 0, _nx);
}
}
}
//--------------------------------------------------------------------------
void Omu_Integrator::solve(int kk, double tstart, double tend,
Omu_StateVec &xc, Omu_StateVec &dxc, Omu_Vec &q,
Omu_DependentVec &Fc)
{
// call depreciated solve and store results in _xc
Omu_StateVec &xt = *_xt_ptr;
Omu_SVarVec &sx = (Omu_SVarVec &)*_x_ptr;
if (_sa) {
solve(kk, tstart, tend, sx, *_u_ptr, _sys, xt, xt.Sx, xt.Su);
m_move(xt.Sx, _nd, 0, _n, _nx, _xc.Sq, 0, 0);
m_move(xt.Su, _nd, 0, _n, _nu, _xc.Sq, 0, _nx);
}
else {
solve(kk, tstart, tend, sx, *_u_ptr, _sys, xt, MNULL, MNULL);
}
v_move(xt, _nd, _n, _xc, 0);
}
int main()
{
int i;
struct VEngine veng;
struct World world;
JoyState* joy;
v_init(&veng);
joy_init();
/* for a random seed, wait for user input */
i = 0;
while (1) {
vid_vsync();
key_poll();
v_putbgtile(5, 4, 37);
v_putbgtile(6, 4, 48);
v_putbgtile(7, 4, 51);
v_putbgtile(8, 4, 51);
v_putbgtile(9, 4, 51);
if (key_hit(KEY_A) || key_hit(KEY_START)) {
break;
}
i ++;
}
/* sqran(i); */
sqran(4);
init_world(&world, &veng);
/* obj_set_pos(dude, (16 * 7), (16 * 4)); */
v_draw_bg(world.map, &veng, world.map->dudex, world.map->dudey);
while(1)
{
vid_vsync();
joy = joy_poll();
key_poll();
if (key_hit(KEY_A)) {
/*
v_draw_more_bg(world.map, veng.mapofsx, veng.mapofsy - 4, 15, 4,
world.map->dudex - 6 - veng.dudeposx,
world.map->dudey - 4 - 4 - veng.dudeposy);
*/
}
if (key_hit(KEY_START)) {
v_init(&veng);
init_world(&world, &veng);
/*
obj_set_pos(veng.obj_buffer[veng.spr_dude], (16 * 7), (16 * 4));
*/
v_draw_bg(world.map, &veng,
world.map->dudex, world.map->dudey);
}
/*
if (key_hit(KEY_LEFT)) {
switch (r_move_left(&world)) {
case 1:
v_move_left(&veng);
break;
case 2:
v_shake_dude(&veng, 1, 0);
break;
}
} else if (key_hit(KEY_RIGHT)) {
switch (r_move_right(&world)) {
case 1:
v_move_right(&veng);
break;
case 2:
v_shake_dude(&veng, 1, 0);
break;
}
} else if (key_hit(KEY_UP)) {
switch (r_move_up(&world)) {
case 1:
v_move_up(&veng);
break;
case 2:
v_shake_dude(&veng, 0, 1);
break;
}
} else if (key_hit(KEY_DOWN)) {
switch (r_move_down(&world)) {
case 1:
v_move_down(&veng);
break;
case 2:
v_shake_dude(&veng, 0, 1);
break;
}
}
*/
if (joy->moving) {
switch (r_move(&world, joy->dx, joy->dy)) {
case 1:
v_move(&veng, joy->dx, joy->dy);
break;
case 2:
v_shake_dude(&veng, 0, 1);
break;
}
joy_handled();
}
v_move_co(&veng);
v_scroll_at_edge(&veng, &world);
v_draw_dude(&veng);
for (i = 0; i < world.monstercount; i ++) {
v_draw_monster(&veng, &world, i);
}
oam_copy(oam_mem, veng.obj_buffer, 1 + world.monstercount);
}
return 0;
}
