예제 #1
0
파일: Euler.hpp 프로젝트: jesushl/TUNAM
/*!
 * 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;
	}
예제 #2
0
파일: Euler.hpp 프로젝트: jesushl/TUNAM
    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;
	}   
예제 #3
0
    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;
    }
예제 #4
0
파일: Trilinear.hpp 프로젝트: jesushl/TUNAM
/*!
 * 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;
	}
예제 #5
0
/* 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;
}
예제 #6
0
/* 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;
}
예제 #7
0
    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;
    }