void NSForchheimerTerm<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{
for (std::size_t cell=0; cell < workset.numCells; ++cell) {
for (std::size_t qp=0; qp < numQPs; ++qp) {
normV(cell,qp) = 0.0;
for (std::size_t i=0; i < numDims; ++i) {
normV(cell,qp) += V(cell,qp,i)*V(cell,qp,i);
}
if (normV(cell,qp) > 0)
normV(cell,qp) = std::sqrt(normV(cell,qp));
else
normV(cell,qp) = 0.0;
for (std::size_t i=0; i < numDims; ++i) {
ForchTerm(cell,qp,i) = phi(cell,qp)*rho(cell,qp)*F(cell,qp)*normV(cell,qp)*V(cell,qp,i)/std::sqrt(K(cell,qp));
}
}
}
}
void WahbaRotM(float acc[3], float gyr[3], float mag[3], WahbaRotMStruct *WStruct) {
static float a, g, ga, gm, m, A[3][3], dt;
real (*RotM)[3];
a = WStruct->sun_sensor_gain;
m = 1;
g = 0.0;
ga = 1 - a;
gm = 1 - m;
static float AccErr, *w_a, *w_g, *w_m, *g_va, *g_vm; //,*g_vg;
w_a = WStruct->w_a;
w_g = WStruct->w_g;
w_m = WStruct->w_m;
RotM = WStruct->RotM;
g_va = WStruct->g_va;
//g_vg=WStruct->g_vg;
g_vm = WStruct->g_vm;
dt = WStruct->dt;
static real Rbe[3][3];
real accn[3], magn[3], An, Mn;
An = normV(acc);
if (An == 0) { An = 1; }
Mn = normV(mag);
if (Mn == 0) { Mn = 1; }
for (int i = 0; i < 3; i++) {
accn[i] = acc[i] / An;
magn[i] = mag[i] / Mn;
}
for (int i = 0; i < 3; i++) {
A[i][0] = a * accn[0] * w_a[i] + m * magn[0] * w_m[i]
+ ga * g_va[0] * w_a[i] + gm * g_vm[0] * w_m[i];
A[i][1] = a * accn[1] * w_a[i] + m * magn[1] * w_m[i]
+ ga * g_va[1] * w_a[i] + gm * g_vm[1] * w_m[i];
A[i][2] = a * accn[2] * w_a[i] + m * magn[2] * w_m[i]
+ ga * g_va[2] * w_a[i] + gm * g_vm[2] * w_m[i];
}
// for(int i=0;i<3;i++){
// A[i][0]=a*accn[0]*w_a[i] + m*magn[0]*w_m[i];
// A[i][1]=a*accn[1]*w_a[i] + m*magn[1]*w_m[i];
// A[i][2]=a*accn[2]*w_a[i] + m*magn[2]*w_m[i];
// }
// real A1[3][3]={{1.21,3.214,0.1},{0,0,0},{2.12,0.120,0.43}};
//real A1[3][3]={{1.001,2,3},{0,0,0},{2,4,6}}; //check svd
WahbaJacobiSVDRotM(A, RotM);
dRpropagat(Rbe, RotM, gyr, dt);
// mulMatrTrVec(w_m,Rbe,magn);
// w_m[0]=sqrtv(w_m[0]*w_m[0]+w_m[1]*w_m[1]);
// w_m[1]=0;
mulMatrVec(g_va, Rbe, w_a);
mulMatrVec(g_vm, Rbe, w_m);
//mulMatr(Rr,RtM,Rbe);
//Conversion to euler takes 7-8us...
rotmtx2euler((const float *) RotM, (float *) WStruct->Euler);
rotmtx2quat((const float *) RotM, (Quat4 *) WStruct->q);
// Calculate W from dR
static real tmp;
static real tr;
mulMatrMatrTr(WStruct->dR, RotM, WStruct->RotM_prev);
tr = ((WStruct->dR[0][0] + WStruct->dR[1][1] + WStruct->dR[2][2]) - 1.0) / 2.0;
tr = SATUR(tr, 1);
tmp = acosv(tr);
WStruct->dth = tmp;
if (fabsv(tmp) < TOL) {
tmp = 0.5 / dt; // x/sin(x)|x->0
} else {
tmp = 0.5 / dt * tmp / sinv(tmp);
}
WStruct->W[0] = tmp * (WStruct->dR[2][1] - WStruct->dR[1][2]);
WStruct->W[1] = tmp * (WStruct->dR[0][2] - WStruct->dR[2][0]);
WStruct->W[2] = tmp * (WStruct->dR[1][0] - WStruct->dR[0][1]);
for (int i = 0; i < 3; i++) {
WStruct->RotM_prev[0][i] = RotM[0][i];
WStruct->RotM_prev[1][i] = RotM[1][i];
WStruct->RotM_prev[2][i] = RotM[2][i];
}
}
int main() {
#if defined(__GNUC__) && (__GNUC__ == 4) && (__GNUC_MINOR__ > 4)
std::cout << "gcc " << __GNUC__ << "." << __GNUC_MINOR__ << std::endl;
#endif
#ifdef USE_SSEVECT
std::cout << "sse vector enabled in cmssw" << std::endl;
#endif
std::cout << sizeof(Basic2DVectorF) << std::endl;
std::cout << sizeof(Basic2DVectorD) << std::endl;
std::cout << sizeof(Basic3DVectorF) << std::endl;
std::cout << sizeof(Basic3DVectorD) << std::endl;
std::cout << sizeof(Basic3DVectorLD) << std::endl;
Basic3DVectorF x(2.0f,4.0f,5.0f);
Basic3DVectorF y(-3.0f,2.0f,-5.0f);
Basic3DVectorD xd(2.0,4.0,5.0);
Basic3DVectorD yd = y;
Basic3DVectorLD xld(2.0,4.0,5.0);
Basic3DVectorLD yld = y;
Basic2DVectorF x2(2.0f,4.0f);
Basic2DVectorF y2 = y.xy();
Basic2DVectorD xd2(2.0,4.0);
Basic2DVectorD yd2 = yd.xy();
{
std::cout << dotV(x,y) << std::endl;
std::cout << normV(x) << std::endl;
std::cout << norm(x) << std::endl;
std::cout << min(x.mathVector(),y.mathVector()) << std::endl;
std::cout << max(x.mathVector(),y.mathVector()) << std::endl;
std::cout << dotV(x,yd) << std::endl;
std::cout << dotV(xd,y) << std::endl;
std::cout << dotV(xd,yd) << std::endl;
std::cout << normV(xd) << std::endl;
std::cout << norm(xd) << std::endl;
std::cout << dotV(xld,yld) << std::endl;
std::cout << normV(xld) << std::endl;
std::cout << norm(xld) << std::endl;
Basic3DVectorF z = x.cross(y);
std::cout << z << std::endl;
std::cout << -z << std::endl;
Basic3DVectorD zd = x.cross(yd);
std::cout << zd << std::endl;
std::cout << -zd << std::endl;
std::cout << xd.cross(y)<< std::endl;
std::cout << xd.cross(yd)<< std::endl;
Basic3DVectorLD zld = x.cross(yld);
std::cout << zld << std::endl;
std::cout << -zld << std::endl;
std::cout << xld.cross(y)<< std::endl;
std::cout << xld.cross(yld)<< std::endl;
std::cout << z.eta() << " " << (-z).eta() << std::endl;
std::cout << zd.eta() << " " << (-zd).eta() << std::endl;
std::cout << zld.eta() << " " << (-zld).eta() << std::endl;
#if defined( __GXX_EXPERIMENTAL_CXX0X__)
auto s = x+xd - 3.1*z;
std::cout << s << std::endl;
auto s2 = x+xld - 3.1*zd;
std::cout << s2 << std::endl;
#endif
}
{
std::cout << dotV(x2,y2) << std::endl;
std::cout << normV(x2) << std::endl;
std::cout << norm(x2) << std::endl;
std::cout << min(x2.mathVector(),y2.mathVector()) << std::endl;
std::cout << max(x2.mathVector(),y2.mathVector()) << std::endl;
std::cout << dotV(x2,yd2) << std::endl;
std::cout << dotV(xd2,y2) << std::endl;
std::cout << dotV(xd2,yd2) << std::endl;
std::cout << normV(xd2) << std::endl;
std::cout << norm(xd2) << std::endl;
Basic2DVectorF z2(x2); z2-=y2;
std::cout << z2 << std::endl;
std::cout << -z2 << std::endl;
Basic2DVectorD zd2 = x2-yd2;
std::cout << zd2 << std::endl;
std::cout << -zd2 << std::endl;
std::cout << x2.cross(y2) << std::endl;
std::cout << x2.cross(yd2) << std::endl;
std::cout << xd2.cross(y2)<< std::endl;
std::cout << xd2.cross(yd2)<< std::endl;
#if defined( __GXX_EXPERIMENTAL_CXX0X__)
auto s2 = x2+xd2 - 3.1*z2;
std::cout << s2 << std::endl;
#endif
}
{
std::cout << "f" << std::endl;
Basic3DVectorF vx(2.0f,4.0f,5.0f);
Basic3DVectorF vy(-3.0f,2.0f,-5.0f);
vx+=vy;
std::cout << vx << std::endl;
Basic3DVectorF vz(1.f,1.f,1.f);
addScaleddiff(vz,0.1f,vx,vy);
std::cout << vz << std::endl;
}
{
std::cout << "d" << std::endl;
Basic3DVectorD vx(2.0,4.0,5.0);
Basic3DVectorD vy(-3.0,2.0,-5.0);
vx+=vy;
std::cout << vx << std::endl;
Basic3DVectorD vz(1.,1.,1);
addScaleddiff(vz,0.1,vx,vy);
std::cout << vz << std::endl;
}
std::cout << "std::vector" << std::endl;
std::vector<Basic3DVectorF> vec1; vec1.reserve(50);
std::vector<float> vecf(21);
std::vector<Basic3DVectorF> vec2(51);
std::vector<Basic3DVectorF> vec3; vec3.reserve(23456);
}
