void
spectral_test (mpf_t rop[], unsigned int T, mpz_t a, mpz_t m)
{
/* Knuth "Seminumerical Algorithms, Third Edition", section 3.3.4
(pp. 101-103). */
/* v[t] = min { sqrt (x[1]^2 + ... + x[t]^2) |
x[1] + a*x[2] + ... + pow (a, t-1) * x[t] is congruent to 0 (mod m) } */
/* Variables. */
unsigned int ui_t;
unsigned int ui_i, ui_j, ui_k, ui_l;
mpf_t f_tmp1, f_tmp2;
mpz_t tmp1, tmp2, tmp3;
mpz_t U[GMP_SPECT_MAXT][GMP_SPECT_MAXT],
V[GMP_SPECT_MAXT][GMP_SPECT_MAXT],
X[GMP_SPECT_MAXT],
Y[GMP_SPECT_MAXT],
Z[GMP_SPECT_MAXT];
mpz_t h, hp, r, s, p, pp, q, u, v;
/* GMP inits. */
mpf_init (f_tmp1);
mpf_init (f_tmp2);
for (ui_i = 0; ui_i < GMP_SPECT_MAXT; ui_i++)
{
for (ui_j = 0; ui_j < GMP_SPECT_MAXT; ui_j++)
{
mpz_init_set_ui (U[ui_i][ui_j], 0);
mpz_init_set_ui (V[ui_i][ui_j], 0);
}
mpz_init_set_ui (X[ui_i], 0);
mpz_init_set_ui (Y[ui_i], 0);
mpz_init (Z[ui_i]);
}
mpz_init (tmp1);
mpz_init (tmp2);
mpz_init (tmp3);
mpz_init (h);
mpz_init (hp);
mpz_init (r);
mpz_init (s);
mpz_init (p);
mpz_init (pp);
mpz_init (q);
mpz_init (u);
mpz_init (v);
/* Implementation inits. */
if (T > GMP_SPECT_MAXT)
T = GMP_SPECT_MAXT; /* FIXME: Lazy. */
/* S1 [Initialize.] */
ui_t = 2 - 1; /* NOTE: `t' in description == ui_t + 1
for easy indexing */
mpz_set (h, a);
mpz_set (hp, m);
mpz_set_ui (p, 1);
mpz_set_ui (pp, 0);
mpz_set (r, a);
mpz_pow_ui (s, a, 2);
mpz_add_ui (s, s, 1); /* s = 1 + a^2 */
/* S2 [Euclidean step.] */
while (1)
{
if (g_debug > DEBUG_1)
{
mpz_mul (tmp1, h, pp);
mpz_mul (tmp2, hp, p);
mpz_sub (tmp1, tmp1, tmp2);
if (mpz_cmpabs (m, tmp1))
{
printf ("***BUG***: h*pp - hp*p = ");
mpz_out_str (stdout, 10, tmp1);
printf ("\n");
}
}
if (g_debug > DEBUG_2)
{
printf ("hp = ");
mpz_out_str (stdout, 10, hp);
printf ("\nh = ");
mpz_out_str (stdout, 10, h);
printf ("\n");
fflush (stdout);
}
if (mpz_sgn (h))
mpz_tdiv_q (q, hp, h); /* q = floor(hp/h) */
else
mpz_set_ui (q, 1);
if (g_debug > DEBUG_2)
{
printf ("q = ");
mpz_out_str (stdout, 10, q);
printf ("\n");
fflush (stdout);
}
mpz_mul (tmp1, q, h);
mpz_sub (u, hp, tmp1); /* u = hp - q*h */
mpz_mul (tmp1, q, p);
mpz_sub (v, pp, tmp1); /* v = pp - q*p */
mpz_pow_ui (tmp1, u, 2);
mpz_pow_ui (tmp2, v, 2);
mpz_add (tmp1, tmp1, tmp2);
if (mpz_cmp (tmp1, s) < 0)
{
mpz_set (s, tmp1); /* s = u^2 + v^2 */
mpz_set (hp, h); /* hp = h */
mpz_set (h, u); /* h = u */
mpz_set (pp, p); /* pp = p */
mpz_set (p, v); /* p = v */
}
else
break;
}
/* S3 [Compute v2.] */
mpz_sub (u, u, h);
mpz_sub (v, v, p);
mpz_pow_ui (tmp1, u, 2);
mpz_pow_ui (tmp2, v, 2);
mpz_add (tmp1, tmp1, tmp2);
if (mpz_cmp (tmp1, s) < 0)
{
mpz_set (s, tmp1); /* s = u^2 + v^2 */
mpz_set (hp, u);
mpz_set (pp, v);
}
mpf_set_z (f_tmp1, s);
mpf_sqrt (rop[ui_t - 1], f_tmp1);
/* S4 [Advance t.] */
mpz_neg (U[0][0], h);
mpz_set (U[0][1], p);
mpz_neg (U[1][0], hp);
mpz_set (U[1][1], pp);
mpz_set (V[0][0], pp);
mpz_set (V[0][1], hp);
mpz_neg (V[1][0], p);
mpz_neg (V[1][1], h);
if (mpz_cmp_ui (pp, 0) > 0)
{
mpz_neg (V[0][0], V[0][0]);
mpz_neg (V[0][1], V[0][1]);
mpz_neg (V[1][0], V[1][0]);
mpz_neg (V[1][1], V[1][1]);
}
while (ui_t + 1 != T) /* S4 loop */
{
ui_t++;
mpz_mul (r, a, r);
mpz_mod (r, r, m);
/* Add new row and column to U and V. They are initialized with
all elements set to zero, so clearing is not necessary. */
mpz_neg (U[ui_t][0], r); /* U: First col in new row. */
mpz_set_ui (U[ui_t][ui_t], 1); /* U: Last col in new row. */
mpz_set (V[ui_t][ui_t], m); /* V: Last col in new row. */
/* "Finally, for 1 <= i < t,
set q = round (vi1 * r / m),
vit = vi1*r - q*m,
and Ut=Ut+q*Ui */
for (ui_i = 0; ui_i < ui_t; ui_i++)
{
mpz_mul (tmp1, V[ui_i][0], r); /* tmp1=vi1*r */
zdiv_round (q, tmp1, m); /* q=round(vi1*r/m) */
mpz_mul (tmp2, q, m); /* tmp2=q*m */
mpz_sub (V[ui_i][ui_t], tmp1, tmp2);
for (ui_j = 0; ui_j <= ui_t; ui_j++) /* U[t] = U[t] + q*U[i] */
{
mpz_mul (tmp1, q, U[ui_i][ui_j]); /* tmp=q*uij */
mpz_add (U[ui_t][ui_j], U[ui_t][ui_j], tmp1); /* utj = utj + q*uij */
}
}
/* s = min (s, zdot (U[t], U[t]) */
vz_dot (tmp1, U[ui_t], U[ui_t], ui_t + 1);
if (mpz_cmp (tmp1, s) < 0)
mpz_set (s, tmp1);
ui_k = ui_t;
ui_j = 0; /* WARNING: ui_j no longer a temp. */
/* S5 [Transform.] */
if (g_debug > DEBUG_2)
printf ("(t, k, j, q1, q2, ...)\n");
do
{
if (g_debug > DEBUG_2)
printf ("(%u, %u, %u", ui_t + 1, ui_k + 1, ui_j + 1);
for (ui_i = 0; ui_i <= ui_t; ui_i++)
{
if (ui_i != ui_j)
{
vz_dot (tmp1, V[ui_i], V[ui_j], ui_t + 1); /* tmp1=dot(Vi,Vj). */
mpz_abs (tmp2, tmp1);
mpz_mul_ui (tmp2, tmp2, 2); /* tmp2 = 2*abs(dot(Vi,Vj) */
vz_dot (tmp3, V[ui_j], V[ui_j], ui_t + 1); /* tmp3=dot(Vj,Vj). */
if (mpz_cmp (tmp2, tmp3) > 0)
{
zdiv_round (q, tmp1, tmp3); /* q=round(Vi.Vj/Vj.Vj) */
if (g_debug > DEBUG_2)
{
printf (", ");
mpz_out_str (stdout, 10, q);
}
for (ui_l = 0; ui_l <= ui_t; ui_l++)
{
mpz_mul (tmp1, q, V[ui_j][ui_l]);
mpz_sub (V[ui_i][ui_l], V[ui_i][ui_l], tmp1); /* Vi=Vi-q*Vj */
mpz_mul (tmp1, q, U[ui_i][ui_l]);
mpz_add (U[ui_j][ui_l], U[ui_j][ui_l], tmp1); /* Uj=Uj+q*Ui */
}
vz_dot (tmp1, U[ui_j], U[ui_j], ui_t + 1); /* tmp1=dot(Uj,Uj) */
if (mpz_cmp (tmp1, s) < 0) /* s = min(s,dot(Uj,Uj)) */
mpz_set (s, tmp1);
ui_k = ui_j;
}
else if (g_debug > DEBUG_2)
printf (", #"); /* 2|Vi.Vj| <= Vj.Vj */
}
else if (g_debug > DEBUG_2)
printf (", *"); /* i == j */
}
if (g_debug > DEBUG_2)
printf (")\n");
/* S6 [Advance j.] */
if (ui_j == ui_t)
ui_j = 0;
else
ui_j++;
}
while (ui_j != ui_k); /* S5 */
/* From Knuth p. 104: "The exhaustive search in steps S8-S10
reduces the value of s only rarely." */
#ifdef DO_SEARCH
/* S7 [Prepare for search.] */
/* Find minimum in (x[1], ..., x[t]) satisfying condition
x[k]^2 <= f(y[1], ...,y[t]) * dot(V[k],V[k]) */
ui_k = ui_t;
if (g_debug > DEBUG_2)
{
printf ("searching...");
/*for (f = 0; f < ui_t*/
fflush (stdout);
}
/* Z[i] = floor (sqrt (floor (dot(V[i],V[i]) * s / m^2))); */
mpz_pow_ui (tmp1, m, 2);
mpf_set_z (f_tmp1, tmp1);
mpf_set_z (f_tmp2, s);
mpf_div (f_tmp1, f_tmp2, f_tmp1); /* f_tmp1 = s/m^2 */
for (ui_i = 0; ui_i <= ui_t; ui_i++)
{
vz_dot (tmp1, V[ui_i], V[ui_i], ui_t + 1);
mpf_set_z (f_tmp2, tmp1);
mpf_mul (f_tmp2, f_tmp2, f_tmp1);
f_floor (f_tmp2, f_tmp2);
mpf_sqrt (f_tmp2, f_tmp2);
mpz_set_f (Z[ui_i], f_tmp2);
}
/* S8 [Advance X[k].] */
do
{
if (g_debug > DEBUG_2)
{
printf ("X[%u] = ", ui_k);
mpz_out_str (stdout, 10, X[ui_k]);
printf ("\tZ[%u] = ", ui_k);
mpz_out_str (stdout, 10, Z[ui_k]);
printf ("\n");
fflush (stdout);
}
if (mpz_cmp (X[ui_k], Z[ui_k]))
{
mpz_add_ui (X[ui_k], X[ui_k], 1);
for (ui_i = 0; ui_i <= ui_t; ui_i++)
mpz_add (Y[ui_i], Y[ui_i], U[ui_k][ui_i]);
/* S9 [Advance k.] */
while (++ui_k <= ui_t)
{
mpz_neg (X[ui_k], Z[ui_k]);
mpz_mul_ui (tmp1, Z[ui_k], 2);
for (ui_i = 0; ui_i <= ui_t; ui_i++)
{
mpz_mul (tmp2, tmp1, U[ui_k][ui_i]);
mpz_sub (Y[ui_i], Y[ui_i], tmp2);
}
}
vz_dot (tmp1, Y, Y, ui_t + 1);
if (mpz_cmp (tmp1, s) < 0)
mpz_set (s, tmp1);
}
}
while (--ui_k);
#endif /* DO_SEARCH */
mpf_set_z (f_tmp1, s);
mpf_sqrt (rop[ui_t - 1], f_tmp1);
#ifdef DO_SEARCH
if (g_debug > DEBUG_2)
printf ("done.\n");
#endif /* DO_SEARCH */
} /* S4 loop */
/* Clear GMP variables. */
mpf_clear (f_tmp1);
mpf_clear (f_tmp2);
for (ui_i = 0; ui_i < GMP_SPECT_MAXT; ui_i++)
{
for (ui_j = 0; ui_j < GMP_SPECT_MAXT; ui_j++)
{
mpz_clear (U[ui_i][ui_j]);
mpz_clear (V[ui_i][ui_j]);
}
mpz_clear (X[ui_i]);
mpz_clear (Y[ui_i]);
mpz_clear (Z[ui_i]);
}
mpz_clear (tmp1);
mpz_clear (tmp2);
mpz_clear (tmp3);
mpz_clear (h);
mpz_clear (hp);
mpz_clear (r);
mpz_clear (s);
mpz_clear (p);
mpz_clear (pp);
mpz_clear (q);
mpz_clear (u);
mpz_clear (v);
return;
}
void TForm1::compute_spring_forces()
{
double curtime = g_timer.GetTime();
if (g_last_iteration_time < 0)
{
g_last_iteration_time = curtime;
return;
}
double elapsed = curtime - g_last_iteration_time;
g_last_iteration_time = curtime;
unsigned int i;
// Clear the force that's applied to each ball
for(i=0; i<m_active_balls.size(); i++)
{
m_active_balls[i]->current_force.set(0,0,0);
}
if (simulationOn)
{
CBall* b = m_active_balls[0];
cVector3d old_p = b->getPos();
b->setPos(tool->m_deviceGlobalPos);
b->m_velocity = cDiv(elapsed,cSub(b->getPos(),old_p));
}
// Compute the current length of each spring and apply forces
// on each mass accordingly
for(i=0; i<m_active_springs.size(); i++)
{
CSpring* s = m_active_springs[i];
double d = cDistance(s->m_endpoint_1->getPos(),s->m_endpoint_2->getPos());
s->m_current_length = d;
// This spring's deviation from its rest length
//
// (positive deviation -> spring is too long)
double x = s->m_current_length - s->m_rest_length;
// Apply a force to ball 1 that pulls it toward ball 2
// when the spring is too long
cVector3d f1 = cMul(s->m_spring_constant*x*1.0,
cSub(s->m_endpoint_2->getPos(),s->m_endpoint_1->getPos()));
s->m_endpoint_1->current_force.add(f1);
// Add the opposite force to ball 2
s->m_endpoint_2->current_force.add(cMul(-1.0,f1));
}
// Update velocities and positions based on forces
for(i=0; i<m_active_balls.size(); i++)
{
CBall* b = m_active_balls[i];
// Certain forces don't get applied to the "haptic ball"
// when haptics are enabled...
if (simulationOn == 0 || i != 0) {
cVector3d f_damping = cMul(DAMPING_CONSTANT,b->m_velocity);
b->current_force.add(f_damping);
}
cVector3d f_gravity(0,GRAVITY_CONSTANT*b->m_mass,0);
b->current_force.add(f_gravity);
cVector3d p = b->getPos();
if (p.y - b->m_radius < FLOOR_Y_POSITION) {
double penetration = FLOOR_Y_POSITION - (p.y - b->m_radius);
b->current_force.add(0,m_floor_spring_constant*penetration,0);
}
cVector3d f_floor(0,0,0);
cVector3d a = cDiv(b->m_mass,b->current_force);
b->m_velocity.add(cMul(elapsed,a));
// We handle the 0th ball specially when haptics is enabled
if (simulationOn == 0 || i != 0) {
p.add(cMul(elapsed,b->m_velocity));
b->setPos(p);
}
}
// Set the haptic force appropriately to reflect the force
// applied to ball 0
m_haptic_force = cMul(HAPTIC_FORCE_CONSTANT,m_active_balls[0]->current_force);
}
bool TestExtMath::test_floor() {
VS(f_floor(4.3), 4.0);
VS(f_floor(9.999), 9.0);
VS(f_floor(-3.14), -4.0);
return Count(true);
}
static VALUE
nurat_divmod(VALUE self, VALUE other)
{
VALUE val = f_floor(f_div(self, other));
return rb_assoc_new(val, f_sub(self, f_mul(other, val)));
}
static VALUE
nurat_mod(VALUE self, VALUE other)
{
VALUE val = f_floor(f_div(self, other));
return f_sub(self, f_mul(other, val));
}
static VALUE
nurat_idiv(VALUE self, VALUE other)
{
return f_floor(f_div(self, other));
}
