| #include "ForceLaw.h" |
| |
| #define MEM_ALIGN 64 |
| |
| //comment this out to leave out "inline" keywords |
| #define INLINEQ 1 |
| |
| #define POLY_ORDER 6 |
| |
| /* |
| int my_posix_memalign(void **memptr, size_t alignment, size_t size) { |
| int ret; |
| |
| ret = posix_memalign(memptr, alignment, size); |
| assert(ret==0); |
| |
| return ret; |
| } |
| */ |
| |
| //----------------------------------------------------------------------------- |
| |
| FGrid::FGrid() : |
| m_b(0.72), |
| m_c(0.01), |
| m_d(0.27), |
| m_e(0.0001), |
| m_f(360.0), |
| m_g(100.0), |
| m_h(0.67), |
| m_l(17.0), |
| m_rmax(3.116326355) { |
| }; |
| |
| |
| #ifdef INLINEQ |
| inline |
| #endif |
| float FGrid::fgor(float r) { |
| float f0 = m_c + 2.0/3.0*m_b*m_b*m_b; |
| float r2 = r*r; |
| float r4 = r2*r2; |
| float r6 = r4*r2; |
| float coshbr = coshf(m_b*r); |
| float r3fgor = tanhf(m_b*r) - m_b*r/coshbr/coshbr |
| + m_c*r*r2*(1.0 + m_d*r2)*expf(-1.0*m_d*r2) |
| + m_e*r2*(m_f*r2 + m_g*r4 + m_l*r6)*expf(-1.0*m_h*r2); |
| float rdiv = r + 1.0*(r<=0.0); |
| float rdiv3 = rdiv*rdiv*rdiv; |
| return (r3fgor/rdiv3 + (r<=0.0)*f0)*(r<=m_rmax)*(r>=0.0); |
| } |
| |
| |
| void FGrid::fgor_r2_interp(int nInterp, float **r2, float **f) { |
| //my_posix_memalign((void **)r2, MEM_ALIGN, nInterp*sizeof(float) ); |
| //my_posix_memalign((void **)f, MEM_ALIGN, nInterp*sizeof(float) ); |
| *r2 = (float *)malloc(nInterp*sizeof(float)); |
| *f = (float *)malloc(nInterp*sizeof(float)); |
| |
| double dr2 = (m_rmax*m_rmax)/(nInterp-1.0); |
| for(int i=0; i<nInterp; i++) { |
| (*r2)[i] = i*dr2; |
| (*f)[i] = fgor(sqrt(i*dr2)); |
| } |
| |
| return; |
| } |
| |
| //----------------------------------------------------------------------------- |
| |
| FGridEvalFit::FGridEvalFit(FGrid *fg) { |
| m_fg = fg; |
| } |
| |
| |
| #ifdef INLINEQ |
| inline |
| #endif |
| float FGridEvalFit::eval(float r2) { |
| return m_fg->fgor(sqrt(r2)); |
| } |
| |
| //----------------------------------------------------------------------------- |
| |
| |
| |
| // 0.263729 - 0.0686285 x + 0.00882248 x^2 - 0.000592487 x^3 + 0.0000164662 x^4 |
| |
| // 0.269327 - 0.0750978 x + 0.0114808 x^2 - 0.00109313 x^3 + 0.0000605491 x^4 - 1.47177*10^-6 x^5 |
| |
| // 0.271431 - 0.0783394 x + 0.0133122 x^2 - 0.00159485 x^3 + 0.000132336 x^4 - 6.63394*10^-6 x^5 + 1.47305*10^-7 x^6 |
| |
| FGridEvalPoly::FGridEvalPoly(FGrid *fg) { |
| m_fg = fg; |
| |
| #if POLY_ORDER == 6 |
| //6th order |
| m_a[0] = 0.271431; |
| m_a[1] = -0.0783394; |
| m_a[2] = 0.0133122; |
| m_a[3] = -0.00159485; |
| m_a[4] = 0.000132336; |
| m_a[5] = -0.00000663394; |
| m_a[6] = 0.000000147305; |
| #endif |
| |
| #if POLY_ORDER == 5 |
| //5th order |
| m_a[0] = 0.269327; |
| m_a[1] = -0.0750978; |
| m_a[2] = 0.0114808; |
| m_a[3] = -0.00109313; |
| m_a[4] = 0.0000605491; |
| m_a[5] = -0.00000147177; |
| m_a[6] = 0.0; |
| #endif |
| |
| #if POLY_ORDER == 4 |
| //4th order |
| m_a[0] = 0.263729; |
| m_a[1] = -0.0686285; |
| m_a[2] = 0.00882248; |
| m_a[3] = -0.000592487; |
| m_a[4] = 0.0000164622; |
| m_a[5] = 0.0; |
| m_a[6] = 0.0; |
| #endif |
| |
| m_r2min = 0.0; |
| m_r2max = fg->rmax()*fg->rmax(); |
| } |
| |
| |
| #ifdef INLINEQ |
| inline |
| #endif |
| float FGridEvalPoly::eval(float r2) { |
| float ret=0.0; |
| ret = m_a[0] + r2*(m_a[1] + r2*(m_a[2] + r2*(m_a[3] + r2*(m_a[4] + r2*(m_a[5] + r2*m_a[6]))))); |
| return ret*(r2 >= m_r2min)*(r2 <= m_r2max); |
| } |
| |
| //----------------------------------------------------------------------------- |
| |
| FGridEvalInterp::FGridEvalInterp(FGrid *fg, int nInterp) { |
| m_nInterp = nInterp; |
| fg->fgor_r2_interp(m_nInterp, &m_r2, &m_f); |
| m_r2min = m_r2[0]; |
| m_r2max = m_r2[m_nInterp-1]; |
| m_dr2 = (m_r2max - m_r2min)/(m_nInterp - 1.0); |
| m_oodr2 = 1.0/m_dr2; |
| } |
| |
| |
| FGridEvalInterp::~FGridEvalInterp() { |
| free(m_r2); |
| free(m_f); |
| } |
| |
| |
| #ifdef INLINEQ |
| inline |
| #endif |
| float FGridEvalInterp::eval(float r2) { |
| int inRange, indx; |
| float inRangef; |
| inRange = (r2 > m_r2min)*(r2 < m_r2max); |
| inRangef = 1.0*inRange; |
| indx = int((r2 - m_r2min)*m_oodr2)*inRange; |
| return inRangef*(m_f[indx]+(r2-m_r2[indx])*m_oodr2*(m_f[indx+1]-m_f[indx])); |
| } |
| |
| //----------------------------------------------------------------------------- |
| |
| ForceLawSR::ForceLawSR(FGridEval *fgore, float rsm) { |
| m_rsm = rsm; |
| m_rsm2 = rsm*rsm; |
| m_r2min = fgore->r2min(); |
| m_r2max = fgore->r2max(); |
| m_fgore = fgore; |
| } |
| |
| |
| #ifdef INLINEQ |
| inline |
| #endif |
| float ForceLawSR::f_over_r(float r2) { |
| float ret = powf(r2 + m_rsm2, -1.5) - m_fgore->eval(r2); |
| ret *= (r2>=m_r2min)*(r2<=m_r2max); |
| return ret; |
| //return ( powf(r2 + m_rsm2, -1.5) ); |
| } |