

#include "stdafx.h"
#include "Solver.h"


inline void dnul(dvec &d)
{
  d.x[0]=d.x[1]=0.0;
  d.y[0]=d.y[1]=0.0;
  d.z[0]=d.z[1]=0.0;
}
 
inline void dassign(dvec &d,vec &a,vec &b)
{
  d.x[0]=a.x;
  d.x[1]=b.x;
 
  d.y[0]=a.y;
  d.y[1]=b.y;
 
  d.z[0]=a.z;
  d.z[1]=b.z;
}
 

inline void dsub(dvec &x,dvec &y, dvec &z)
{
  x.x[0]=y.x[0]-z.x[0];
  x.x[1]=y.x[1]-z.x[1];
 
  x.y[0]=y.y[0]-z.y[0];
  x.y[1]=y.y[1]-z.y[1];
 
  x.z[0]=y.z[0]-z.z[0];
  x.z[1]=y.z[1]-z.z[1];
}
 

inline void dmul(dvec &x, mfloat (&my)[2])
{
  x.x[0]*=my[0];
  x.x[1]*=my[1];
 
  x.y[0]*=my[0];
  x.y[1]*=my[1];
   
  x.z[0]*=my[0];
  x.z[1]*=my[1];
}
 

inline void dcoeffs(dvec &x,mfloat (&sqlen)[2],mfloat (&nnlen)[2],mfloat (&invlen)[2])
{
  sqlen[0]=SQ(x.x[0])+SQ(x.y[0])+SQ(x.z[0])+0.0000000001;
  sqlen[1]=SQ(x.x[1])+SQ(x.y[1])+SQ(x.z[1])+0.0000000001;
 
  nnlen[0]=sqrt(sqlen[0]);
  nnlen[1]=sqrt(sqlen[1]);
 
  invlen[0]=1.0/nnlen[0];
  invlen[1]=1.0/nnlen[1];
 
  sqlen[0]=invlen[0]*invlen[0];
  sqlen[1]=invlen[1]*invlen[1];
}
 

inline void dmad(dvec &x,dvec &y, mfloat (&my)[2])
{
  x.x[0]+=y.x[0]*my[0];
  x.x[1]+=y.x[1]*my[1];
 
  x.y[0]+=y.y[0]*my[0];
  x.y[1]+=y.y[1]*my[1];
 
  x.z[0]+=y.z[0]*my[0];
  x.z[1]+=y.z[1]*my[1];
}
 
 
 
void nbsolverobj::devaluate()
{
  int i,j;
  dvec dcpos;
  mfloat crad;
  __m128d ddirx,ddiry,ddirz;
  __m128d dcaccx,dcaccy,dcaccz;
  __m128d dcmas,dcrad,dcoeff,drmas,drrad,dsqlen,dnnlen,dinvlen;
  __m128d quadfactorpd,linlfactorpd;
  __m128d constpd1,constpdlv;
  __m128d a,b,c,a1,b1,c1,a2,b2,c2;
  mfloat l1,l2;


  constpd1=_mm_set1_pd(1.0);
  constpdlv=_mm_set1_pd(0.0000000001);


  for (i=0; i<len; i+=2)
  {
    dassign(dpts[i/2].pos,pts[i].pos,pts[i+1].pos);
    
    // calculate this invariant codepart beforehand
    dpts[i/2].mas[0]=pts[i].mas;
    dpts[i/2].mas[1]=pts[i+1].mas;
    dpts[i/2].rad[0]=pts[i].rad;
    dpts[i/2].rad[1]=pts[i+1].rad;
//    dpts[i/2].acc.x^=dpts[i/2].acc.x;
//    dpts[i/2].acc.y^=dpts[i/2].acc.y;
//    dpts[i/2].acc.z^=dpts[i/2].acc.z;
    // end of codepart
  }
 
  quadfactorpd=_mm_set1_pd(quadfactor);
  linlfactorpd=_mm_set1_pd(linlfactor);


  for (i=0; i<len; i++)
  {
//    dnul(dcacc);
    dcaccx=_mm_setzero_pd();
    dcaccy=_mm_setzero_pd();
    dcaccz=_mm_setzero_pd();

    dassign(dcpos,pts[i].pos,pts[i].pos);
    crad=pts[i].rad;
    dcmas=_mm_set1_pd(-pts[i].mas);
    dcrad=_mm_set1_pd(crad);


    for (j=0; j<(i+1)/2; j++)
    {
      drrad=_mm_load_pd((const double*)&dpts[j].rad);
      drrad=_mm_add_pd(drrad,dcrad);
      drmas=_mm_load_pd((const double*)&dpts[j].mas);
 

      ddirx=_mm_load_pd((const double*)&dpts[j].pos.x);
      a=_mm_load_pd((const double*)&dcpos.x);
      ddirx=_mm_sub_pd(ddirx,a);

      ddiry=_mm_load_pd((const double*)&dpts[j].pos.y);
      b=_mm_load_pd((const double*)&dcpos.y);
      ddiry=_mm_sub_pd(ddiry,b);

      ddirz=_mm_load_pd((const double*)&dpts[j].pos.z);
      c=_mm_load_pd((const double*)&dcpos.z);
      ddirz=_mm_sub_pd(ddirz,c);


//      coeffs(ddir,dsqlen,dnnlen,dinvlen);

      dsqlen=constpdlv;
      a=_mm_mul_pd(ddirx,ddirx);
      dsqlen=_mm_add_pd(dsqlen,a);
      b=_mm_mul_pd(ddiry,ddiry);
      dsqlen=_mm_add_pd(dsqlen,b);
      c=_mm_mul_pd(ddirz,ddirz);
      dsqlen=_mm_add_pd(dsqlen,c);

      dnnlen=_mm_sqrt_pd(dsqlen);
       
      dinvlen=_mm_div_pd(constpd1,dnnlen);
       
      dsqlen=_mm_mul_pd(dinvlen,dinvlen);
      

//      dcoeff=dsqlen*(dnnlen-drrad)*
//      (quadfactorpd*dsqlen+linlfactorpd*dinvlen);
      a=_mm_sub_pd(dnnlen,drrad);
      dcoeff=_mm_mul_pd(dsqlen,a);
      b=_mm_mul_pd(quadfactorpd,dsqlen);
      c=_mm_mul_pd(linlfactorpd,dinvlen);
      b=_mm_add_pd(b,c);
      dcoeff=_mm_mul_pd(dcoeff,b);
      

//      dmul(ddir,dcoeff);
//      ddir.x*=dcoeff;
//      ddir.y*=dcoeff;
//      ddir.z*=dcoeff;
      ddirx=_mm_mul_pd(ddirx,dcoeff);
      ddiry=_mm_mul_pd(ddiry,dcoeff);
      ddirz=_mm_mul_pd(ddirz,dcoeff);


//      dmad(dcacc,ddir,drmas);
//      dcacc.x+=ddir.x*drmas;
//      dcacc.y+=ddir.y*drmas;
//      dcacc.z+=ddir.z*drmas;
      a=_mm_mul_pd(ddirx,drmas);
      dcaccx=_mm_add_pd(dcaccx,a);
      b=_mm_mul_pd(ddiry,drmas);
      dcaccy=_mm_add_pd(dcaccy,b);
      c=_mm_mul_pd(ddirz,drmas);
      dcaccz=_mm_add_pd(dcaccz,c);


//      dmad(dpts[j].acc,ddir,dcmas);
//      dpts[j].acc.x+=ddir.x*dcmas;
//      dpts[j].acc.y+=ddir.y*dcmas;
//      dpts[j].acc.z+=ddir.z*dcmas;
      b=_mm_load_pd((const double*)&dpts[j].acc.x);
      a=_mm_mul_pd(ddirx,dcmas);
      b=_mm_add_pd(b,a);
      _mm_store_pd((double*)&dpts[j].acc.x,b);

      b1=_mm_load_pd((const double*)&dpts[j].acc.y);
      a1=_mm_mul_pd(ddiry,dcmas);
      b1=_mm_add_pd(b1,a1);
      _mm_store_pd((double*)&dpts[j].acc.y,b1);

      b2=_mm_load_pd((const double*)&dpts[j].acc.z);
      a2=_mm_mul_pd(ddirz,dcmas);
      b2=_mm_add_pd(b2,a2);
      _mm_store_pd((double*)&dpts[j].acc.z,b2);

    }
 

    _mm_storel_pd(&l1,dcaccx);
    _mm_storeh_pd(&l2,dcaccx);
    dpts[i/2].acc.x[i&1]=l1+l2;
    _mm_storel_pd(&l1,dcaccy);
    _mm_storeh_pd(&l2,dcaccy);
    dpts[i/2].acc.y[i&1]=l1+l2;
    _mm_storel_pd(&l1,dcaccz);
    _mm_storeh_pd(&l2,dcaccz);
    dpts[i/2].acc.z[i&1]=l1+l2;
  }
 

  for (i=0; i<len; i++)
  {
    pts[i].acc.x=dpts[i/2].acc.x[i&1];
    pts[i].acc.y=dpts[i/2].acc.y[i&1];
    pts[i].acc.z=dpts[i/2].acc.z[i&1];
  }
}

 