double GLGPU3DDataset::Flux(int face) const
{
// TODO: pre-compute the flux
switch (face) {
case 0: return -dx() * dy() * Bz();
case 1: return -dy() * dz() * Bx();
case 2: return -dz() * dx() * By();
case 3: return dx() * dy() * Bz();
case 4: return dy() * dz() * Bx();
case 5: return dz() * dx() * By();
default: assert(false);
}
return 0.0;
}
static void test_mgcr(ITER *ip, int i, MAT *Q, MAT *R)
#endif
{
VEC vt, vt1;
static MAT *R1 = MNULL;
static VEC *r = VNULL, *r1 = VNULL;
VEC *rr;
int k, j;
Real sm;
/* check Q*Q^T = I */
vt.dim = vt.max_dim = ip->b->dim;
vt1.dim = vt1.max_dim = ip->b->dim;
Q = m_resize(Q, i + 1, ip->b->dim);
R1 = m_resize(R1, i + 1, i + 1);
r = v_resize(r, ip->b->dim);
r1 = v_resize(r1, ip->b->dim);
MEM_STAT_REG(R1, TYPE_MAT);
MEM_STAT_REG(r, TYPE_VEC);
MEM_STAT_REG(r1, TYPE_VEC);
m_zero(R1);
for (k = 1; k <= i; k++)
for (j = 1; j <= i; j++) {
vt.ve = Q->me[k];
vt1.ve = Q->me[j];
R1->me[k][j] = in_prod(&vt, &vt1);
}
for (j = 1; j <= i; j++)
R1->me[j][j] -= 1.0;
#ifndef MEX
if (m_norm_inf(R1) > MACHEPS * ip->b->dim)
printf(" ! (mgcr:) m_norm_inf(Q*Q^T) = %g\n", m_norm_inf(R1));
#endif
/* check (r_i,Ap_j) = 0 for j <= i */
ip->Ax(ip->A_par, ip->x, r);
v_sub(ip->b, r, r);
rr = r;
if (ip->Bx) {
ip->Bx(ip->B_par, r, r1);
rr = r1;
}
#ifndef MEX
printf(" ||r|| = %g\n", v_norm2(rr));
#endif
sm = 0.0;
for (j = 1; j <= i; j++) {
vt.ve = Q->me[j];
sm = max(sm, in_prod(&vt,rr));
}
#ifndef MEX
if (sm >= MACHEPS * ip->b->dim)
printf(" ! (mgcr:) max_j (r,Ap_j) = %g\n", sm);
#endif
}
// ABGR
int value_to_color_888(float v, float max)
{
int s;
int R;
int G;
int B;
float fact = max;
v = v / fact;
B = (int)255. * Bx(v);
G = (int)255. * Gx(v);
R = (int)255. * Rx(v);
s = (R & 0xFF) + ((G & 0xFF) << 8) + ((B & 0xFF) << 16);
return s;
}
// Normalises a value v in 0. and 1. and converts that to a rgb 565 short int
int value_to_color_565(float v, float max)
{
int s;
int R;
int G;
int B;
float fact = max;
v = v / fact;
B = (int)31. * Bx(v);
G = (int)63. * Gx(v);
R = (int)31. * Rx(v);
s = (R % 32) + ((G % 64) << 5) + ((B % 32) << 11);
// if (v == 1.)
// s = 0xFE00;
//printf("vla=%f, max=%f\n", v, max);
return s;
}
int sc_main( int ac, char* av[] )
{
sc_uint_base Bx(WIDTH), By(WIDTH);
for(unsigned int i = 0; i < WIDTH; i++){
cout << "i = " << i << endl;
for(unsigned int j=0; j < COUNT; j++ ){
/* ( By.range(WIDTH-1, i+1), By.range(i,0) ) = Bx;
sc_assert( By == Bx );
*/
Bx = j;
sc_assert( (bool) Bx[i] == (j & (1<<i)? 1 : 0) );
By[i] = Bx[i];
sc_assert( (bool) By[i] == (j & (1<<i)? 1 : 0) );
}
}
return 0;
}
float GLGPUDataset::QP(const float X0[], const float X1[]) const
{
const float *L = Lengths(),
*O = Origins();
float d[3] = {X1[0] - X0[0], X1[1] - X0[1], X1[2] - X0[2]};
int p[3] = {0}; // 0: not crossed; 1: positive; -1: negative
for (int i=0; i<3; i++) {
d[i] = X1[i] - X0[i];
if (d[i]>L[i]/2) {d[i] -= L[i]; p[i] = 1;}
else if (d[i]<-L[i]/2) {d[i] += L[i]; p[i] = -1;}
}
const float X[3] = {X0[0] - O[0], X0[1] - O[1], X0[2] - O[2]};
if (By()>0 && p[0]!=0) { // By>0
return p[0] * L[0] * (Bz()*X[1] - By()*X[2]);
} else if (p[1]!=0) {
return p[1] * L[1] * (Bx()*X[2] - Bz()*X[0]);
} else return 0.0;
}
int sc_main( int ac, char* av[] )
{
sc_int_base Bx(WIDTH),By(WIDTH);
for(unsigned int i = 0; i < WIDTH; i++){
cout << "i = " << i << endl;
for(unsigned int j=0; j < COUNT; j++ ){
/* ( By.range(WIDTH-1, i+1), By.range(i,0) ) = Bx;
assert( By == Bx );
*/
Bx = j;
By.range(WIDTH-1,i) = Bx.range(WIDTH-1,i);
if(i >= 1)
By.range(i-1,0) = Bx.range(i-1,0);
assert( Bx == By );
}
}
return 0;
}
double GLGPU3DDataset::GaugeTransformation(const double X0[], const double X1[]) const
{
double gx, gy, gz;
double dx = X1[0] - X0[0],
dy = X1[1] - X0[1],
dz = X1[2] - X0[2];
double x = X0[0] + 0.5*dx,
y = X0[1] + 0.5*dy,
z = X0[2] + 0.5*dz;
if (By()>0) { // Y-Z gauge
gx = dx * Kex();
gy =-dy * x * Bz(); // -dy*x^hat*Bz
gz = dz * x * By(); // dz*x^hat*By
} else { // X-Z gauge
gx = dx * y * Bz() + dx * Kex(); // dx*y^hat*Bz + dx*K
gy = 0;
gz =-dz * y * Bx(); // -dz*y^hat*Bx
}
return gx + gy + gz;
}
void STWarp<T>::multiscaleIteration(WarpingField<T> &warpField) {
for (int warpIter = 0; warpIter < params.warpIterations; ++warpIter) {
if(params.verbosity > 1) {
printf(" - warp iteration %02d\n",warpIter+1);
}
// Get derivatives
if(params.verbosity > 1) {
printf(" - computing derivatives...");
}
Video<T> Bx(videoB->size());
Video<T> By(videoB->size());
Video<T> Bt(videoB->size());
Video<T> C(videoB->size());
computePartialDerivatives(warpField,Bx,By,Bt,C);
if(params.verbosity >1) {
printf("done.\n");
}
// marginal warpField update
WarpingField<T> dWarpField = WarpingField<T>(warpField.size());
warpingIteration(warpField, Bx, By, Bt, C, dWarpField);
if (params.limitUpdate) {
if(params.verbosity > 1) {
printf(" - limiting warp update to [-1,1]");
}
dWarpField.clamp(-1,1);
}
// w <- w + dw
warpField.add(dWarpField);
denoiseWarpingField(warpField);
} // end of warp iteration
}
/* Loads an binary STL file by filename
* Returns 0 on success and -1 on failure */
int Shape::loadBinarySTL(string filename) {
// if(getFileType(filename) != BINARY_STL) {
// return -1;
// }
triangles.clear();
Min.x = Min.y = Min.z = numeric_limits<double>::infinity();
Max.x = Max.y = Max.z = -numeric_limits<double>::infinity();
ifstream file;
file.open(filename.c_str());
if(file.fail()) {
cerr << _("Error: Unable to open stl file - ") << filename << endl;
return -1;
}
/* Binary STL files have a meaningless 80 byte header
* followed by the number of triangles */
file.seekg(80, ios_base::beg);
unsigned int num_triangles;
unsigned char buffer[4];
file.read(reinterpret_cast <char *> (buffer), 4);
// Read platform independent 32-bit little-endian int.
num_triangles = buffer[0] | buffer[1] << 8 | buffer[2] << 16 | buffer[3] << 24;
triangles.reserve(num_triangles);
for(uint i = 0; i < num_triangles; i++)
{
double a,b,c;
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d N(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Ax(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Bx(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Cx(a,b,c);
// done in Triangle
/* Recalculate normal vector - can't trust an STL file! */
// Vector3d AA=Cx-Ax;
// Vector3d BB=Cx-Bx;
// N = AA.cross(BB).getNormalized();
/* attribute byte count - sometimes contains face color
information but is useless for our purposes */
unsigned short byte_count;
file.read(reinterpret_cast <char *> (buffer), 2);
byte_count = buffer[0] | buffer[1] << 8;
// Repress unused variable warning.
(void)&byte_count;
Triangle T(Ax,Bx,Cx);
//cout << "bin triangle "<< N << ":\n\t" << Ax << "/\n\t"<<Bx << "/\n\t"<<Cx << endl;
triangles.push_back(T);
}
file.close();
CenterAroundXY();
scale_factor = 1.0;
scale_factor_x=scale_factor_y=scale_factor_z = 1.0;
double vol = volume();
if (vol < 0) {
invertNormals();
vol = -vol;
}
cout << _("Shape has volume ") << vol << " mm^3"<<endl;
return 0;
}
void Filter::Kalman(Joint joint, double &dx, double &dy)
{
Kalmans[Kalman_count++] = joint;
Kalman_count = Kalman_count % Kalman_limit;
Kalman_num++;
if (Kalman_num > Kalman_limit)
{
Kalman_num = Kalman_limit;
}
if (Kalman_num < Kalman_limit)
{
dx = joint.Position.X;
dy = joint.Position.Y;
return;
}
else
{
//X, Y
int haha;
haha = 1;
double x[5], y[5];
int pos = Kalman_count;
for (int i = 0; i < 5; i++)
{
x[i] = Kalmans[pos].Position.X;
y[i] = Kalmans[pos].Position.Y;
pos++;
pos = pos%Kalman_limit;
}
//求系数Ax, Ay
double Ax[5] = {
/*a0*/ x[0],
/*a1*/ 4 * (x[1] - x[0]) - 3 * x[2] + 4 * x[3] / 3 - x[4] / 4,
/*a2*/ 11 * x[4] / 24 - 7 * x[3] / 3 + 19 * x[2] / 4 - 13 * (x[1] - x[0]) / 3,
/*a3*/ x[4] / 3 - 7 * x[3] / 6 + x[2] - (x[1] - x[0]) / 2,
/*a4*/ (x[4] - 4 * x[3] + 6 * x[2] + 4 * (x[1] - x[0])) / 24
};
double Ay[5] = {
/*a0*/ y[0],
/*a1*/ 4 * (y[1] - y[0]) - 3 * y[2] + 4 * y[3] / 3 - y[4] / 4,
/*a2*/ 11 * y[4] / 24 - 7 * y[3] / 3 + 19 * y[2] / 4 - 13 * (y[1] - y[0]) / 3,
/*a3*/ y[4] / 3 - 7 * y[3] / 6 + y[2] - (y[1] - y[0]) / 2,
/*a4*/ (y[4] - 4 * y[3] + 6 * y[2] + 4 * (y[1] - y[0])) / 24
};
//求转换矩阵Fx, Fy
Matrix Fx(4, 4,
new double[16]{
1, 1, -0.5, (Ax[1] + 6 * Ax[3] - 4 * Ax[4]) / (24 * Ax[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
Matrix Fy(4, 4,
new double[16]{
1, 1, -0.5, (Ay[1] + 6 * Ay[3] - 4 * Ay[4]) / (24 * Ay[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
//求ε(t|t-1)
Matrix ex(4, 4), ey(4, 4);
ex = Fx*Kalman_ex*(!Fx);
ey = Fy*Kalman_ey*(!Fy);
//cout << "ex" << endl; ex.print();
//cout << "ey" << endl; ey.print();
Matrix Bx(4, 1), By(4, 1);
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_vx" << endl; Kalman_vx.print();
//cout << "Kalman_C" << endl; Kalman_C.print();
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_C*ex" << endl; (Kalman_C*ex).print();
//cout << "Kalman_C*ex*(!Kalman_C)" << endl; (Kalman_C*ex*(!Kalman_C)).print();
//cout << "(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)))" << endl;
//(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))).print();
Bx = ex*(!Kalman_C)*(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)));
//cout << "Bx" << endl; Bx.print();
By = ey*(!Kalman_C)*(~(Kalman_vy + Kalman_C*ey*(!Kalman_C)));
Matrix I4(4, 4);
I4.SetIdentity();
Kalman_Sx = (I4 - Bx*Kalman_C)*(Fx*Kalman_Sx + Kalman_Gx) +
Bx*Matrix(1, 1, new double[1] {joint.Position.X});
//cout << "Kalman_Sx" << endl; Kalman_Sx.print();
Kalman_Sy = (I4 - By*Kalman_C)*(Fy*Kalman_Sy + Kalman_Gy) +
By*Matrix(1, 1, new double[1] {joint.Position.Y});
Kalman_ex = ex - Bx*Kalman_C*ex;
Kalman_ey = ey - By*Kalman_C*ey;
dx = Kalman_Sx.at(0, 0);
dy = Kalman_Sy.at(0, 0);
}
}
float Az(const float X[3], int slot=0) const {if (By()>0) return -X[0]*By(); else return X[1]*Bx();}
VEC *iter_cg1(ITER *ip)
#endif
{
STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL;
Real alpha;
double inner,nres;
VEC *rr; /* rr == r or rr == z */
if (ip == INULL)
error(E_NULL,"iter_cg");
if (!ip->Ax || !ip->b)
error(E_NULL,"iter_cg");
if ( ip->x == ip->b )
error(E_INSITU,"iter_cg");
if (!ip->stop_crit)
error(E_NULL,"iter_cg");
if ( ip->eps <= 0.0 )
ip->eps = MACHEPS;
r = v_resize(r,ip->b->dim);
p = v_resize(p,ip->b->dim);
q = v_resize(q,ip->b->dim);
MEM_STAT_REG(r,TYPE_VEC);
MEM_STAT_REG(p,TYPE_VEC);
MEM_STAT_REG(q,TYPE_VEC);
if (ip->Bx != (Fun_Ax)NULL) {
z = v_resize(z,ip->b->dim);
MEM_STAT_REG(z,TYPE_VEC);
rr = z;
}
else rr = r;
if (ip->x != VNULL) {
if (ip->x->dim != ip->b->dim)
error(E_SIZES,"iter_cg");
ip->Ax(ip->A_par,ip->x,p); /* p = A*x */
v_sub(ip->b,p,r); /* r = b - A*x */
}
else { /* ip->x == 0 */
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
v_copy(ip->b,r);
}
if (ip->Bx) (ip->Bx)(ip->B_par,r,p);
else v_copy(r,p);
inner = in_prod(p,r);
nres = sqrt(fabs(inner));
if (ip->info) ip->info(ip,nres,r,p);
if ( nres == 0.0) return ip->x;
for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ )
{
ip->Ax(ip->A_par,p,q);
inner = in_prod(q,p);
if (sqrt(fabs(inner)) <= MACHEPS*ip->init_res)
error(E_BREAKDOWN,"iter_cg1");
alpha = in_prod(p,r)/inner;
v_mltadd(ip->x,p,alpha,ip->x);
v_mltadd(r,q,-alpha,r);
rr = r;
if (ip->Bx) {
ip->Bx(ip->B_par,r,z);
rr = z;
}
nres = in_prod(r,rr);
if (nres < 0.0) {
warning(WARN_RES_LESS_0,"iter_cg");
break;
}
nres = sqrt(fabs(nres));
if (ip->info) ip->info(ip,nres,r,z);
if (ip->steps == 0) ip->init_res = nres;
if ( ip->stop_crit(ip,nres,r,z) ) break;
alpha = -in_prod(rr,q)/inner;
v_mltadd(rr,p,alpha,p);
}
#ifdef THREADSAFE
V_FREE(r); V_FREE(p); V_FREE(q); V_FREE(z);
#endif
return ip->x;
}
bool File::load_binarySTL(vector<Triangle> &triangles,
uint max_triangles, bool readnormals)
{
ifstream file;
ustring filename = _file->get_path();
file.open(filename.c_str(), ifstream::in | ifstream::binary);
if(file.fail()) {
cerr << _("Error: Unable to open stl file - ") << filename << endl;
return false;
}
// cerr << "loading bin " << filename << endl;
/* Binary STL files have a meaningless 80 byte header
* followed by the number of triangles */
file.seekg(80, ios_base::beg);
unsigned int num_triangles;
unsigned char buffer[4];
file.read(reinterpret_cast <char *> (buffer), 4);
// Read platform independent 32-bit little-endian int.
num_triangles = buffer[0] | buffer[1] << 8 | buffer[2] << 16 | buffer[3] << 24;
uint step = 1;
if (max_triangles > 0 && max_triangles < num_triangles) {
step = ceil(num_triangles/max_triangles);
triangles.reserve(max_triangles);
} else
triangles.reserve(num_triangles);
uint i = 0;
for(; i < num_triangles; i+=step)
{
if (step>1)
file.seekg(84 + 50*i, ios_base::beg);
double a,b,c;
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d N(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Ax(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Bx(a,b,c);
a = read_double (file);
b = read_double (file);
c = read_double (file);
Vector3d Cx(a,b,c);
if (file.eof()) {
cerr << _("Unexpected EOF reading STL file - ") << filename << endl;
break;
}
/* attribute byte count - sometimes contains face color
information but is useless for our purposes */
unsigned short byte_count;
file.read(reinterpret_cast <char *> (buffer), 2);
byte_count = buffer[0] | buffer[1] << 8;
// Repress unused variable warning.
(void)&byte_count;
Triangle T = Triangle(Ax,Bx,Cx);
if (readnormals)
if (T.Normal.dot(N) < 0) T.invertNormal();
// cout << "bin triangle "<< N << ":\n\t" << Ax << "/\n\t"<<Bx << "/\n\t"<<Cx << endl;
triangles.push_back(T);
}
file.close();
return true;
// cerr << "Read " << i << " triangles of " << num_triangles << " from file" << endl;
}
