/*!
* This function make the integration of the ODE \f$ \f$ to calculate
* the new position of the particle. The result is stored in \c newPos.
* \param oldPos Actual position of the particle.
* \param deltas Deltas on each axis of the mesh.
* \param u X-component of the velocity.
* \param v Y-component of the velocity.
* \param dt Time step.
*/
inline floatTinyArray_t integrate(floatTinyArray_t &oldPos,
const floatTinyArray_t &deltas,
ScalarField_t &u,
ScalarField_t &v,
prec_t dt)
{
UVW = i_object.interpolate(oldPos, deltas, u, v); /// Interpolate de velocity
newPos(0) = oldPos(0) + dt * UVW(0); /// Use Euler to update the position.
newPos(1) = oldPos(1) + dt * UVW(1); /// in each coordinate.
return newPos;
}
inline floatTinyArray_t integrate(floatTinyArray_t &oldPos,
const floatTinyArray_t &deltas,
ScalarField_t &u,
ScalarField_t &v,
ScalarField_t &w,
prec_t dt)
{
UVW = i_object.interpolate(oldPos, deltas, u, v, w);
newPos(0) = oldPos(0) + dt * UVW(0);
newPos(1) = oldPos(1) + dt * UVW(1);
newPos(2) = oldPos(2) + dt * UVW(2);
return newPos;
}
unsigned long ReprojectXYZ(double x, double y, double z, int& u, int& v)
{
cv::Mat XYZ(3, 1, CV_64FC1);
cv::Mat UVW(3, 1, CV_64FC1);
double* d_ptr = 0;
double du = 0;
double dv = 0;
double dw = 0;
x *= 1000;
y *= 1000;
z *= 1000;
d_ptr = XYZ.ptr<double>(0);
d_ptr[0] = x;
d_ptr[1] = y;
d_ptr[2] = z;
UVW = camera_matrix_ * XYZ;
d_ptr = UVW.ptr<double>(0);
du = d_ptr[0];
dv = d_ptr[1];
dw = d_ptr[2];
u = cvRound(du/dw);
v = cvRound(dv/dw);
return ipa_Utils::RET_OK;
}
/*!
* This is a trilinear interpolation of the velocity to the actual position
* of the particle. The result is stored in the array UVW.
* \param pos Position of the particle.
* \param deltas Deltas on each axis of the mesh.
* \param u X-component of the velocity.
* \param v Y-component of the velocity.
* \param w Z-component of the velocity.
*/
inline floatTinyArray_t interpolate(floatTinyArray_t &pos,
const floatTinyArray_t &deltas,
ScalarField3D &u,
ScalarField3D &v,
ScalarField3D &w)
{
locate(pos, deltas);
UVW(0) = ( ( u(I ,J ,K ) * (1 - alpha) +
u(I+1,J ,K ) * alpha ) * (1 - beta) +
( u(I ,J+1,K ) * (1 - alpha) +
u(I+1,J+1,K ) * alpha ) * beta ) * (1 - gamma) +
( ( u(I ,J ,K+1) * (1 - alpha) +
u(I+1,J ,K+1) * alpha ) * (1 - beta) +
( u(I ,J+1,K+1) * (1 - alpha ) +
u(I+1,J+1,K+1) * alpha ) * beta ) * gamma;
UVW(1) = ( ( v(I ,J ,K ) * (1 - alpha) +
v(I+1,J ,K ) * alpha ) * (1 - beta) +
( v(I ,J+1,K ) * (1 - alpha) +
v(I+1,J+1,K ) * alpha ) * beta ) * (1 - gamma) +
( ( v(I ,J ,K+1) * (1 - alpha) +
v(I+1,J ,K+1) * alpha ) * (1 - beta) +
( v(I ,J+1,K+1) * (1 - alpha ) +
v(I+1,J+1,K+1) * alpha ) * beta ) * gamma;
UVW(2) = ( ( w(I ,J ,K ) * (1 - alpha) +
w(I+1,J ,K ) * alpha ) * (1 - beta) +
( w(I ,J+1,K ) * (1 - alpha) +
w(I+1,J+1,K ) * alpha ) * beta ) * (1 - gamma) +
( ( w(I ,J ,K+1) * (1 - alpha) +
w(I+1,J ,K+1) * alpha ) * (1 - beta) +
( w(I ,J+1,K+1) * (1 - alpha ) +
w(I+1,J+1,K+1) * alpha ) * beta ) * gamma;
return UVW;
}
/* get status of intersection between triangle (a, b, c) and sphere (p, r) */
int TSI_Status (double *a, double *b, double *c, double *p, double r)
{
/* macro internals */
double n [3], s [3], d [3], l [3];
double uv, lv, lu, tmp;
/* local variables */
double u, v, w, ba2, ac2, rsq, rsqeps;
double pa [3], pb [3], pc [3];
double ba [3], cb [3], ac [3];
double x [3], px [3];
unsigned char abc = 0x00;
SUB (p, a, pa);
SUB (p, b, pb);
SUB (p, c, pc);
rsq = r * r;
rsqeps = rsq - GEOMETRIC_EPSILON * rsq;
/* vertices inside */
if (DOT (pa, pa) < rsqeps) abc |= 0x01;
if (DOT (pb, pb) < rsqeps) abc |= 0x02;
if (DOT (pc, pc) < rsqeps) abc |= 0x04;
SUB (b, a, ba);
SUB (c, b, cb);
SUB (a, c, ac);
ba2 = DOT (ba, ba);
ac2 = DOT (ac, ac);
u = DOT (pa, ba) / ba2;
v = DOT (pb, cb) / DOT (cb, cb);
w = DOT (pc, ac) / ac2;
/* edges inside */
ADDMUL (a, u, ba, x); SUB (p, x, px);
if (0. <= u && u <= 1. && DOT (px, px) < rsqeps) abc |= 0x10;
ADDMUL (b, v, cb, x); SUB (p, x, px);
if (0. <= v && v <= 1. && DOT (px, px) < rsqeps) abc |= 0x20;
ADDMUL (c, w, ac, x); SUB (p, x, px);
if (0. <= w && w <= 1. && DOT (px, px) < rsqeps) abc |= 0x40;
switch (abc)
{
case 0x0:
{
SPHCENTER (x);
SUB (p, x, px);
UVW (x, u, v, w);
/* area inside */
if (u > 0. && v > 0. && w < 1. && DOT (px, px) < rsqeps) return TSI_BUBBLE;
}
break;
case 0x21:
case 0x31:
case 0x61:
case 0x71:
return TSI_A_BC;
case 0x42:
case 0x62:
case 0x52:
case 0x72:
return TSI_B_CA;
case 0x14:
case 0x34:
case 0x54:
case 0x74:
return TSI_C_AB;
case 0x10: return TSI_AB;
case 0x20: return TSI_BC;
case 0x40: return TSI_CA;
case 0x30: return TSI_AB_BC;
case 0x60: return TSI_BC_CA;
case 0x50: return TSI_CA_AB;
case 0x70: return TSI_AB_BC_CA;
}
switch (abc & 0x0f)
{
case 0x01: return TSI_A;
case 0x02: return TSI_B;
case 0x04: return TSI_C;
case 0x03: return TSI_A_B;
case 0x06: return TSI_B_C;
case 0x05: return TSI_C_A;
case 0x07: return TSI_A_B_C;
}
return TSI_OUT;
}
/* approximate intersection of triangle (a, b, c) and sphere (p, r) with second order triangular elements */
int TSI_Approx (double *a, double *b, double *c, double *p, double r, double **cc, int *ncc, int **tt, int *ntt)
{
/* macro internals */
double i1 [3], i2 [3], i3 [3], i4 [3];
double uv, lv, lu, tmp, pp [3], rp;
double n [3], s [3], d [3], l [3];
double i5 [3], i6 [3];
/* local variables */
double u, v, w, ba2, ac2, rsq, rsqeps;
double pa [3], pb [3], pc [3];
double ba [3], cb [3], ac [3];
double x [3], px [3];
unsigned char abc = 0x00;
/* nullify output */
*cc = NULL; *ncc = 0;
*tt = NULL; *ntt = 0;
SUB (p, a, pa);
SUB (p, b, pb);
SUB (p, c, pc);
rsq = r * r;
rsqeps = rsq - GEOMETRIC_EPSILON * rsq;
/* vertices inside */
if (DOT (pa, pa) < rsqeps) abc |= 0x01;
if (DOT (pb, pb) < rsqeps) abc |= 0x02;
if (DOT (pc, pc) < rsqeps) abc |= 0x04;
SUB (b, a, ba);
SUB (c, b, cb);
SUB (a, c, ac);
ba2 = DOT (ba, ba);
ac2 = DOT (ac, ac);
/* project center on triangle edges */
u = DOT (pa, ba) / ba2;
v = DOT (pb, cb) / DOT (cb, cb);
w = DOT (pc, ac) / ac2;
/* edges inside */
ADDMUL (a, u, ba, x); SUB (p, x, px);
if (0. <= u && u <= 1. && DOT (px, px) < rsqeps) abc |= 0x10;
ADDMUL (b, v, cb, x); SUB (p, x, px);
if (0. <= v && v <= 1. && DOT (px, px) < rsqeps) abc |= 0x20;
ADDMUL (c, w, ac, x); SUB (p, x, px);
if (0. <= w && w <= 1. && DOT (px, px) < rsqeps) abc |= 0x40;
switch (abc)
{
case 0x0:
{
SPHCENTER (x);
SUB (p, x, px);
UVW (x, u, v, w);
/* area inside */
if (u > 0. && v > 0. && w < 1. && DOT (px, px) < rsqeps)
{
TSI_DOBUBBLE (a, b, c);
return TSI_BUBBLE;
}
}
break;
case 0x21:
case 0x31:
case 0x61:
case 0x71:
TSI_V_VV (a, b, c);
return TSI_A_BC;
case 0x42:
case 0x62:
case 0x52:
case 0x72:
TSI_V_VV (b, c, a);
return TSI_B_CA;
case 0x14:
case 0x34:
case 0x54:
case 0x74:
TSI_V_VV (c, a, b);
return TSI_C_AB;
case 0x10:
TSI_VV (a, b, c);
return TSI_AB;
case 0x20:
TSI_VV (b, c, a);
return TSI_BC;
case 0x40:
TSI_VV (c, a, b);
return TSI_CA;
case 0x30:
TSI_VV_VV (a, b, c);
return TSI_AB_BC;
case 0x60:
TSI_VV_VV (b, c, a);
return TSI_BC_CA;
case 0x50:
TSI_VV_VV (c, a, b);
return TSI_CA_AB;
case 0x70:
TSI_VV_VV_VV (a, b, c);
return TSI_AB_BC_CA;
}
switch (abc & 0x0f)
{
case 0x01:
TSI_V (a, b, c);
return TSI_A;
case 0x02:
TSI_V (b, c, a);
return TSI_B;
case 0x04:
TSI_V (c, a, b);
return TSI_C;
case 0x03:
TSI_V_V (a, b, c);
return TSI_A_B;
case 0x06:
TSI_V_V (b, c, a);
return TSI_B_C;
case 0x05:
TSI_V_V (c, a, b);
return TSI_C_A;
case 0x07:
TSI_V_V_V (a, b, c);
return TSI_A_B_C;
}
return TSI_OUT;
}
inline floatTinyArray_t integrate(floatTinyArray_t &oldPos,
const floatTinyArray_t &deltas,
ScalarField_t &u,
ScalarField_t &v,
ScalarField_t &w,
ScalarField_t &unext,
ScalarField_t &vnext,
ScalarField_t &wnext,
prec_t dt)
{
UVW = i_object.interpolate(oldPos, deltas, u, v, w);
a(0) = UVW(0) * dt;
a(1) = UVW(1) * dt;
a(2) = UVW(2) * dt;
pos = oldPos;
pos(0) = pos(0) + a(0) * 0.5;
pos(1) = pos(1) + a(1) * 0.5;
pos(2) = pos(2) + a(2) * 0.5;
UVW = i_object.interpolate(pos, deltas, u, v, w);
UVWnext = i_object.interpolate(pos, deltas, unext, vnext, wnext);
UVW(0) = ( UVW(0) + UVWnext(0) ) * 0.5;
UVW(1) = ( UVW(1) + UVWnext(1) ) * 0.5;
UVW(2) = ( UVW(2) + UVWnext(2) ) * 0.5;
b(0) = UVW(0) * dt;
b(1) = UVW(1) * dt;
b(2) = UVW(2) * dt;
pos = oldPos;
pos(0) = pos(0) + b(0) * 0.5;
pos(1) = pos(1) + b(1) * 0.5;
pos(2) = pos(2) + b(2) * 0.5;
UVW = i_object.interpolate(pos, deltas, u, v, w);
UVWnext = i_object.interpolate(pos, deltas, unext, vnext, wnext);
UVW(0) = ( UVW(0) + UVWnext(0) ) * 0.5;
UVW(1) = ( UVW(1) + UVWnext(1) ) * 0.5;
UVW(2) = ( UVW(2) + UVWnext(2) ) * 0.5;
c(0) = UVW(0) * dt;
c(1) = UVW(1) * dt;
c(2) = UVW(2) * dt;
pos = oldPos;
pos(0) = pos(0) + c(0);
pos(1) = pos(1) + c(1);
pos(2) = pos(2) + c(2);
UVW = i_object.interpolate(pos, deltas, unext, vnext, wnext);
d(0) = UVW(0) * dt;
d(1) = UVW(1) * dt;
d(2) = UVW(2) * dt;
newPos(0) = oldPos(0) + (a(0) + 2 * b(0) + 2 * c(0) + d(0)) / 6.0;
newPos(1) = oldPos(1) + (a(1) + 2 * b(1) + 2 * c(1) + d(1)) / 6.0;
newPos(2) = oldPos(2) + (a(2) + 2 * b(2) + 2 * c(2) + d(2)) / 6.0;
return newPos;
}
