int main() {
// distribute bodies in space (randomly)
for(int i=0; i<N; i++) {
B[i].m = (i < L)?1:0;
B[i].pos = triple_rand();
// B[i].pos = (position) { 0, -10 + 20*(i/2), -10 + 20*(i%2) }; // for debugging!
B[i].v = triple_zero();
}
// run simulation for M steps
#pragma omp parallel
for(int i=0; i<M; i++) {
// set forces to zero
#pragma omp for
for(int j=0; j<N; j++) {
F[j] = triple_zero();
}
// reset private copy
for(int j=0; j<N; j++) {
pF[j] = triple_zero();
}
// compute forces for each body (very naive)
#pragma omp for
for(int j=0; j<min(N,L); j++) {
for(int k=0; k<N; k++) {
if(j!=k) {
// comput distance vector
triple dist = SUB(B[k].pos, B[j].pos);
// compute absolute distance
double r = ABS(dist);
// compute strength of force (G = 1 (who cares))
// F = G * (m1 * m2) / r^2
double f = (B[j].m * B[k].m) / (r*r);
// compute current contribution to force
//force cur = MULS(NORM(dist), f);
double s = f / r;
force cur = MULS(dist,s);
// accumulate force
pF[j] = ADD(pF[j], cur);
}
}
}
// aggregate local data
#pragma omp critical
for(int j=0; j<N; j++) {
F[j] = ADD(pF[j],F[j]);
}
// apply forces
#pragma omp for
for(int j=0; j<min(N,L); j++) {
// update speed
// F = m * a
// a = F / m // m=1
// v' = v + a
B[j].v = ADD(B[j].v, DIVS(F[j], B[j].m));
// update position
// pos = pos + v * dt // dt = 1
B[j].pos = ADD(B[j].pos, B[j].v);
}
/* // debug print of positions and speed
for(int i=0; i<N; i++) {
printf("%2d - ", i);
triple_print(B[i].pos);
printf(" - ");
triple_print(B[i].v);
printf("\n");
}
printf("\n");
*/
}
// check result (impulse has to be zero)
impulse sum = triple_zero();
for(int i=0; i<N; i++) {
// impulse = m * v
sum = ADD(sum, MULS(B[i].v,B[i].m));
}
int success = EQ(sum, triple_zero());
printf("Verification: %s\n", ((success)?"OK":"ERR"));
if (!success) {
triple_print(sum); printf(" should be (0,0,0)\n");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
void adm_decouple_s(const adm_dwt_band_t_s *ref, const adm_dwt_band_t_s *dis, const adm_dwt_band_t_s *r, const adm_dwt_band_t_s *a, int w, int h, int ref_stride, int dis_stride, int r_stride, int a_stride)
{
#ifdef ADM_OPT_AVOID_ATAN
const float cos_1deg_sq = cos(1.0 * M_PI / 180.0) * cos(1.0 * M_PI / 180.0);
#endif
const float eps = 1e-30;
int ref_px_stride = ref_stride / sizeof(float);
int dis_px_stride = dis_stride / sizeof(float);
int r_px_stride = r_stride / sizeof(float);
int a_px_stride = a_stride / sizeof(float);
float oh, ov, od, th, tv, td;
float kh, kv, kd, tmph, tmpv, tmpd;
#ifdef ADM_OPT_AVOID_ATAN
float ot_dp, o_mag_sq, t_mag_sq;
#else
float oa, ta, diff;
#endif
int angle_flag;
int i, j;
for (i = 0; i < h; ++i) {
for (j = 0; j < w; ++j) {
oh = ref->band_h[i * ref_px_stride + j];
ov = ref->band_v[i * ref_px_stride + j];
od = ref->band_d[i * ref_px_stride + j];
th = dis->band_h[i * dis_px_stride + j];
tv = dis->band_v[i * dis_px_stride + j];
td = dis->band_d[i * dis_px_stride + j];
kh = DIVS(th, oh + eps);
kv = DIVS(tv, ov + eps);
kd = DIVS(td, od + eps);
kh = kh < 0.0f ? 0.0f : (kh > 1.0f ? 1.0f : kh);
kv = kv < 0.0f ? 0.0f : (kv > 1.0f ? 1.0f : kv);
kd = kd < 0.0f ? 0.0f : (kd > 1.0f ? 1.0f : kd);
tmph = kh * oh;
tmpv = kv * ov;
tmpd = kd * od;
#ifdef ADM_OPT_AVOID_ATAN
/* Determine if angle between (oh,ov) and (th,tv) is less than 1 degree.
* Given that u is the angle (oh,ov) and v is the angle (th,tv), this can
* be done by testing the inequvality.
*
* { (u.v.) >= 0 } AND { (u.v)^2 >= cos(1deg)^2 * ||u||^2 * ||v||^2 }
*
* Proof:
*
* cos(theta) = (u.v) / (||u|| * ||v||)
*
* IF u.v >= 0 THEN
* cos(theta)^2 = (u.v)^2 / (||u||^2 * ||v||^2)
* (u.v)^2 = cos(theta)^2 * ||u||^2 * ||v||^2
*
* IF |theta| < 1deg THEN
* (u.v)^2 >= cos(1deg)^2 * ||u||^2 * ||v||^2
* END
* ELSE
* |theta| > 90deg
* END
*/
ot_dp = oh * th + ov * tv;
o_mag_sq = oh * oh + ov * ov;
t_mag_sq = th * th + tv * tv;
angle_flag = (ot_dp >= 0.0f) && (ot_dp * ot_dp >= cos_1deg_sq * o_mag_sq * t_mag_sq);
#else
oa = atanf(DIVS(ov, oh + eps));
ta = atanf(DIVS(tv, th + eps));
if (oh < 0.0f)
oa += (float)M_PI;
if (th < 0.0f)
ta += (float)M_PI;
diff = fabsf(oa - ta) * 180.0f / M_PI;
angle_flag = diff < 1.0f;
#endif
if (angle_flag) {
tmph = th;
tmpv = tv;
tmpd = td;
}
r->band_h[i * r_px_stride + j] = tmph;
r->band_v[i * r_px_stride + j] = tmpv;
r->band_d[i * r_px_stride + j] = tmpd;
a->band_h[i * a_px_stride + j] = th - tmph;
a->band_v[i * a_px_stride + j] = tv - tmpv;
a->band_d[i * a_px_stride + j] = td - tmpd;
}
}
}
