float locate(int d_,float h_0,float dis_) //求出h_有h_0与h_之间distance等于dis_
{
int count,polar,polar0,polartemp,n;
float h_;
double distemp,alt0_,azi0_,alt_,azi_;
alt0_=alt(d_,h_0,latitude,longitude);
azi0_=az(d_,h_0,latitude,longitude);
h_=h_0+stepinterval;
polar0=0;
polar=0; //辅助变量判断是否开始逼近
for (count=1,n=1;;n++)
{
alt_=alt(d_,h_,latitude,longitude);
azi_=az(d_,h_,latitude,longitude);
distemp=distance(alt0_,azi0_,alt_,azi_);
//printf("deviation=%f\n",distemp-dis_);
if (fabs(distemp-dis_)<definition)
{
break;
}
if (distemp<dis_)
{
polartemp=0;
if (polar0!=polartemp)
{
polar=1;
}
if (polar)
{
h_=h_+stepinterval/count; //开始逼近
count++;
}
else h_=h_+stepinterval; //继续增长
}
else if (distemp>=dis_)
{
polartemp=1;
if (polar0!=polartemp)
{
polar=1;
}
if (polar)
{
h_=h_-stepinterval/count;
count++;
}
else h_=h_+stepinterval;
}
polar0=polartemp;
}
return h_;
}
int store_a()
{
Pid_t i;
char *fname = tagged_output_name("acc");
FILE *fp = fopen(fname, "wt");
log("IO", "Storing accelerations in %s\n", fname);
myassert(fp != NULL, "Can't open file to write accelerations.");
fprintf(fp, "%ld\n", (long int)env.n_particles);
forall_particles(i)
fprintf(fp, "%.8g %.8g %.8g\n", (double)ax(i), (double)ay(i), (double)az(i));
//fprintf(fp, "%.8g %.8g %.8g %.8g\n", ax(i), ay(i), az(i), DIST(ax(i), ay(i), az(i)));
fclose(fp);
return 0;
}
void QxrdPowderPoint::setSettingsValue(QSettings *settings, QString name)
{
settings->beginGroup(name);
settings->setValue("n1", n1());
settings->setValue("n2", n2());
settings->setValue("n3", n3());
settings->setValue("x", x());
settings->setValue("y", y());
settings->setValue("r1", r1());
settings->setValue("r2", r2());
settings->setValue("az", az());
settings->endGroup();
}
Box::Box(Wm5::Box3d& box)
{
Vector3d c(box.Center[0],box.Center[1],box.Center[2]);
Vector3d ax(box.Axis[0][0],box.Axis[0][1],box.Axis[0][2]);
Vector3d ay(box.Axis[1][0],box.Axis[1][1],box.Axis[1][2]);
Vector3d az(box.Axis[2][0],box.Axis[2][1],box.Axis[2][2]);
double ex=box.Extent[0];
double ey=box.Extent[1];
double ez=box.Extent[2];
Vertex[0]=c-ex*ax-ey*ay+ez*az;
Vertex[1]=c+ex*ax-ey*ay+ez*az;
Vertex[2]=c+ex*ax-ey*ay-ez*az;
Vertex[3]=c-ex*ax-ey*ay-ez*az;
Vertex[4]=c-ex*ax+ey*ay+ez*az;
Vertex[5]=c+ex*ax+ey*ay+ez*az;
Vertex[6]=c+ex*ax+ey*ay-ez*az;
Vertex[7]=c-ex*ax+ey*ay-ez*az;
for (int i=0;i<6;i++)
{
Extendable[i]=true;
}
type=0;
}
void cnbint(
int i,
const double pos[][3],
const double vel[][3],
const double mass[],
int nnb,
int list[],
double f[3],
double fdot[3]){
const int NBMAX = 512;
assert(nnb <= NBMAX);
float xbuf[NBMAX] __attribute__ ((aligned(16)));
float ybuf[NBMAX] __attribute__ ((aligned(16)));
float zbuf[NBMAX] __attribute__ ((aligned(16)));
float vxbuf[NBMAX] __attribute__ ((aligned(16)));
float vybuf[NBMAX] __attribute__ ((aligned(16)));
float vzbuf[NBMAX] __attribute__ ((aligned(16)));
float mbuf[NBMAX] __attribute__ ((aligned(16)));
assert((unsigned long)xbuf % 16 == 0);
double xi = pos[i][0];
double yi = pos[i][1];
double zi = pos[i][2];
float vxi = vel[i][0];
float vyi = vel[i][1];
float vzi = vel[i][2];
for(int k=0; k<nnb; k++){
int j = list[k];
#if 1
int jj = list[k+4];
__builtin_prefetch(pos[jj]);
__builtin_prefetch(pos[jj]+2);
__builtin_prefetch(vel[jj]);
__builtin_prefetch(vel[jj]+2);
__builtin_prefetch(&mass[jj]);
#endif
#if 0
xbuf[k] = pos[j][0] - xi;
ybuf[k] = pos[j][1] - yi;
zbuf[k] = pos[j][2] - zi;
vxbuf[k] = vel[j][0] - vxi;
vybuf[k] = vel[j][1] - vyi;
vzbuf[k] = vel[j][2] - vzi;
mbuf[k] = mass[j];
#else
double xj = pos[j][0];
double yj = pos[j][1];
double zj = pos[j][2];
float vxj = vel[j][0];
float vyj = vel[j][1];
float vzj = vel[j][2];
float mj = mass[j];
xj -= xi;
yj -= yi;
zj -= zi;
vxj -= vxi;
vyj -= vyi;
vzj -= vzi;
xbuf[k] = xj;
ybuf[k] = yj;
zbuf[k] = zj;
vxbuf[k] = vxj;
vybuf[k] = vyj;
vzbuf[k] = vzj;
mbuf[k] = mj;
#endif
}
for(int k=nnb; k%4; k++){
xbuf[k] = 16.0f;
ybuf[k] = 16.0f;
zbuf[k] = 16.0f;
vxbuf[k] = 0.0f;
vybuf[k] = 0.0f;
vzbuf[k] = 0.0f;
mbuf[k] = 0.0f;
}
v4df ax(0.0), ay(0.0), az(0.0);
v4sf jx(0.0), jy(0.0), jz(0.0);
for(int k=0; k<nnb; k+=4){
v4sf dx(xbuf + k);
v4sf dy(ybuf + k);
v4sf dz(zbuf + k);
v4sf dvx(vxbuf + k);
v4sf dvy(vybuf + k);
v4sf dvz(vzbuf + k);
v4sf mj(mbuf + k);
v4sf r2 = dx*dx + dy*dy + dz*dz;
v4sf rv = dx*dvx + dy*dvy + dz*dvz;
rv *= v4sf(-3.0f);
// v4sf rinv1 = r2.rsqrt() & v4sf(mask);
#if 1
v4sf rinv1 = r2.rsqrt();
#else
v4sf rinv1 = v4sf(v4df(1.0) / v4df(r2).sqrt());
#endif
v4sf rinv2 = rinv1 * rinv1;
rv *= rinv2;
v4sf rinv3 = mj * rinv1 * rinv2;
dx *= rinv3; ax += v4df(dx);
dy *= rinv3; ay += v4df(dy);
dz *= rinv3; az += v4df(dz);
dvx *= rinv3; jx += dvx;
dvy *= rinv3; jy += dvy;
dvz *= rinv3; jz += dvz;
dx *= rv; jx += dx;
dy *= rv; jy += dy;
dz *= rv; jz += dz;
}
f[0] = ax.sum();
f[1] = ay.sum();
f[2] = az.sum();
fdot[0] = (v4df(jx)).sum();
fdot[1] = (v4df(jy)).sum();
fdot[2] = (v4df(jz)).sum();
assert(f[0] == f[0]);
assert(f[1] == f[1]);
assert(f[2] == f[2]);
assert(fdot[0] == fdot[0]);
assert(fdot[1] == fdot[1]);
assert(fdot[2] == fdot[2]);
}
void cnbint(
int i,
const double pos[][3],
const double vel[][3],
const double mass[],
int nnb,
int list[],
double f[3],
double fdot[3]){
const int NBMAX = 512;
assert(nnb <= NBMAX);
double xbuf[NBMAX] __attribute__ ((aligned(16)));
double ybuf[NBMAX] __attribute__ ((aligned(16)));
double zbuf[NBMAX] __attribute__ ((aligned(16)));
float vxbuf[NBMAX] __attribute__ ((aligned(16)));
float vybuf[NBMAX] __attribute__ ((aligned(16)));
float vzbuf[NBMAX] __attribute__ ((aligned(16)));
float mbuf[NBMAX] __attribute__ ((aligned(16)));
for(int k=0; k<nnb; k++){
int j = list[k];
xbuf[k] = pos[j][0];
ybuf[k] = pos[j][1];
zbuf[k] = pos[j][2];
vxbuf[k] = vel[j][0];
vybuf[k] = vel[j][1];
vzbuf[k] = vel[j][2];
mbuf[k] = mass[j];
}
for(int k=nnb; k%4; k++){
xbuf[k] = 16.0;
ybuf[k] = 16.0;
zbuf[k] = 16.0;
vxbuf[k] = 0.0f;
vybuf[k] = 0.0f;
vzbuf[k] = 0.0f;
mbuf[k] = 0.0f;
}
v4df xi(pos[i][0]);
v4df yi(pos[i][1]);
v4df zi(pos[i][2]);
v4sf vxi(vel[i][0]);
v4sf vyi(vel[i][1]);
v4sf vzi(vel[i][2]);
v4df ax(0.0), ay(0.0), az(0.0);
v4sf jx(0.0), jy(0.0), jz(0.0);
for(int k=0; k<nnb; k+=4){
v4df xj(xbuf + k);
v4df yj(ybuf + k);
v4df zj(zbuf + k);
v4sf vxj(vxbuf + k);
v4sf vyj(vybuf + k);
v4sf vzj(vzbuf + k);
v4sf mj(mbuf + k);
v4sf dx = v4sf::_v4sf(xj - xi);
v4sf dy = v4sf::_v4sf(yj - yi);
v4sf dz = v4sf::_v4sf(zj - zi);
v4sf dvx = vxj - vxi;
v4sf dvy = vyj - vyi;
v4sf dvz = vzj - vzi;
v4sf r2 = dx*dx + dy*dy + dz*dz;
v4sf rv = dx*dvx + dy*dvy + dz*dvz;
rv *= v4sf(-3.0f);
// v4sf rinv1 = r2.rsqrt() & v4sf(mask);
v4sf rinv1 = r2.rsqrt();
v4sf rinv2 = rinv1 * rinv1;
rv *= rinv2;
v4sf rinv3 = mj * rinv1 * rinv2;
dx *= rinv3; ax += v4df(dx);
dy *= rinv3; ay += v4df(dy);
dz *= rinv3; az += v4df(dz);
dvx *= rinv3; jx += dvx;
dvy *= rinv3; jy += dvy;
dvz *= rinv3; jz += dvz;
dx *= rv; jx += dx;
dy *= rv; jy += dy;
dz *= rv; jz += dz;
}
f[0] = ax.sum();
f[1] = ay.sum();
f[2] = az.sum();
fdot[0] = (v4df(jx)).sum();
fdot[1] = (v4df(jy)).sum();
fdot[2] = (v4df(jz)).sum();
assert(f[0] == f[0]);
assert(f[1] == f[1]);
assert(f[2] == f[2]);
assert(fdot[0] == fdot[0]);
assert(fdot[1] == fdot[1]);
assert(fdot[2] == fdot[2]);
}
//-----------------------------------------------------------------------------------------//
//--------------------------------DRAW NEW POINT TO THE GRAPHS-----------------------------//
//-----------------------------------------------------------------------------------------//
void RealTimePlotWindow::dataAvaible(QByteArray ext)
{
double przelicznik = 6103515625;//prescaler value
double memory = 2;
QVector<measurementData> data = ExtractData(ext);//extract data from dialogwinndow
quint16 newDataSize = data.size();
QVector<double> key(newDataSize);
QVector<double> ax(newDataSize);
QVector<double> ay(newDataSize);
QVector<double> az(newDataSize);
QVector<double> gx(newDataSize);
QVector<double> gy(newDataSize);
QVector<double> gz(newDataSize);
double keyPrescaler =0;
keyPrescaler = keyCounter / 200;
// qDebug()<<"znacznik";
for(double i = 0; i<newDataSize; i++)
{
key[i] = (double)keyPrescaler + (i/200);
// key[i] = (double)keyCounter;
ax[i] = ((double)data.at(i).ax.as_word)*przelicznik/100000000000;
ay[i] = ((double)data.at(i).ay.as_word)*(-1)*przelicznik/100000000000;
az[i] = ((double)data.at(i).az.as_word)*(-1)*przelicznik/100000000000;
gx[i] = ((double)data.at(i).gx.as_word)*przelicznik/100000000000;
gy[i] = ((double)data.at(i).gy.as_word)*(-1)*przelicznik/100000000000;
gz[i] = ((double)data.at(i).gz.as_word)*(-1)*przelicznik/100000000000;
}
if((newDataSize>0)&(triger == true))//get instValues 5 times per secound(depend of timmer interval)
{
(*instValues)[0] = ax[0];
(*instValues)[1] = ay[0];
(*instValues)[2] = az[0];
(*instValues)[3] = gx[0];
(*instValues)[4] = gy[0];
(*instValues)[5] = gz[0];
triger = false;
instValWindow->setInstValues(instValues);
}
// add new data to graphs
ui->accPlot->graph(0)->addData(key, ax);
ui->accPlot->graph(1)->addData(key, ay);
ui->accPlot->graph(2)->addData(key, az);
ui->gyroPlot->graph(0)->addData(key, gx);
ui->gyroPlot->graph(1)->addData(key, gy);
ui->gyroPlot->graph(2)->addData(key, gz);
// remove data of lines that's outside visible range:
ui->accPlot->graph(0)->removeDataBefore(keyPrescaler-memory);
ui->accPlot->graph(1)->removeDataBefore(keyPrescaler-memory);
ui->accPlot->graph(2)->removeDataBefore(keyPrescaler-memory);
// qDebug()<<"znacznik 4\n";
// rescale value (vertical) axis to fit the current data:
ui->accPlot->graph(0)->rescaleValueAxis();
ui->accPlot->graph(1)->rescaleValueAxis(true);
ui->accPlot->graph(2)->rescaleValueAxis(true);
// make key axis range scroll with the data (at a constant range size of 8):
ui->accPlot->xAxis->setRange(keyPrescaler, memory, Qt::AlignRight);
ui->accPlot->replot();
// remove data of lines that's outside visible range:
ui->gyroPlot->graph(0)->removeDataBefore(keyPrescaler-memory);
ui->gyroPlot->graph(1)->removeDataBefore(keyPrescaler-memory);
ui->gyroPlot->graph(2)->removeDataBefore(keyPrescaler-memory);
// rescale value (vertical) axis to fit the current data:
ui->gyroPlot->graph(0)->rescaleValueAxis();
ui->gyroPlot->graph(1)->rescaleValueAxis(true);
ui->gyroPlot->graph(2)->rescaleValueAxis(true);
// make key axis range scroll with the data (at a constant range size of 8):
ui->gyroPlot->xAxis->setRange(keyPrescaler, memory, Qt::AlignRight);
ui->gyroPlot->replot();
keyCounter += newDataSize;
MPS += newDataSize;
qDebug()<<"keyCounter:"<<keyCounter;
}
void main()
{
char c;
int day,n,i,j,sum=0,stata[Nday];
double altemp,aztemp;
float hour,dis_;
dis_=interval;
struct data
{
float time[Nhour];
double alt;
double az;
};
data mydata[Nday];
for (i=0;i<Nday;i++) //初始化结构体
{
for (j=0;j<Nhour;j++)
{
mydata[i].time[j]=0;
}
}
for (day=firstday;day<=lastday;day++)
{
for (hour=sunrise,n=0;hour<=sundown;)
{
altemp=alt(day,hour,latitude,longitude);
aztemp=az(day,hour,latitude,longitude);
if (altemp>0 && aztemp>=0) //使太阳在地平线以上
{
mydata[day-1].alt=altemp;
mydata[day-1].az=aztemp;
}
else
{
hour=locate(day,hour,dis_);
continue;
}
hour=locate(day,hour,dis_);
mydata[day-1].time[n]=dataformat(hour);
if (n==Nhour-2)
{
printf("Oh My God!!!");
scanf("%d",&i);
}
printf(" n=%2d hour=%6.3f day=%3d \n",n,hour,day);
n++;
stata[day-1]=n;
}
sum=sum+n;
}
printf("\nCaculate Over!! Sum=%d KB\n\n\n\nWritting in the file.............\n\n",sum*2/1024);
FILE *fp; //文件写入部分
if ((fp=fopen("data.txt","w"))==NULL)
{
printf("Cannot open the file!!!\n");
}
fputs("\n\n******Generated by Neil******\n",fp);
fprintf(fp,"\nlatitude=%6.3f\nlongitude=%6.3f\nsunrise=%6.3f\nsundown=%6.3f\nstepinterval=%6.3f\nfirstday=%6d\nlastday=%6d\ndefinition=%6.5f\ninterval=%6.3f\nNday=%6d\n\n",latitude,longitude,sunrise,sundown,stepinterval,firstday,lastday,definition,interval,Nday); //输出宏定义量的值
for (i=0;i<Nday;i++)
{
for (j=0;j<Nhour;j++)
{
if (mydata[i].time[j]==0)
{
break;
}
//fprintf(fp,"day=%3d number=%2d hour=%4.0f \n",i+1,j+1,mydata[i].time[j]);
fprintf(fp,"code unsigned char Timedatas[%d].data[%d]=%4.0f\n",i,j,mydata[i].time[j]);
}
}
for (i=0;i<Nday;i++)
{
fprintf(fp,"day=%3d %d\n",i+1,stata[i]);
}
fputs("\n\n******Generated by Neil******\n",fp);
fclose(fp); //写入完毕
printf("Congrutulation !!!!!!\n\n******Generated by Neil******\n\n"); //以下是执行退出部分
printf("All work have been done well!!\n\nYou can Press Enter to close me.\n\n");
c=getchar();
if (c==13)
{
exit(0);
}
else printf("All work have done well!!\nYou can Press Enter to close me.\n");
}
void DrawCoordinateSystem::projectPoints(){
cv::Mat_<double> rvec(3,1);
cv::Mat_<double> tvec(3,1);
Types::HomogMatrix homogMatrix;
cv::Mat_<double> rotMatrix;
rotMatrix.create(3,3);
// Try to read rotation and translation from rvec and tvec or homogenous matrix.
if(!in_rvec.empty()&&!in_tvec.empty()){
rvec = in_rvec.read();
tvec = in_tvec.read();
}
else if(!in_homogMatrix.empty()){
homogMatrix = in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
}
else
return;
// Get camera info properties.
Types::CameraInfo camera_matrix = in_camera_matrix.read();
vector<cv::Point3f> object_points;
vector<cv::Point2f> image_points;
// Create four points constituting ends of three lines.
object_points.push_back(cv::Point3f(0,0,0));
object_points.push_back(cv::Point3f(0.1,0,0));
object_points.push_back(cv::Point3f(0,0.1,0));
object_points.push_back(cv::Point3f(0,0,0.1));
// Project points taking into account the camera properties.
if (rectified) {
cv::Mat K, _r, _t;
cv::decomposeProjectionMatrix(camera_matrix.projectionMatrix(), K, _r, _t);
cv::projectPoints(object_points, rvec, tvec, K, cv::Mat(), image_points);
} else {
cv::projectPoints(object_points, rvec, tvec, camera_matrix.cameraMatrix(), camera_matrix.distCoeffs(), image_points);
}
// Draw lines between projected points.
Types::Line ax(image_points[0], image_points[1]);
ax.setCol(cv::Scalar(255, 0, 0));
Types::Line ay(image_points[0], image_points[2]);
ay.setCol(cv::Scalar(0, 255, 0));
Types::Line az(image_points[0], image_points[3]);
az.setCol(cv::Scalar(0, 0, 255));
// Add lines to container.
Types::DrawableContainer ctr;
ctr.add(ax.clone());
ctr.add(ay.clone());
ctr.add(az.clone());
CLOG(LINFO)<<"Image Points: "<<image_points;
// Write output to dataports.
out_csystem.write(ctr);
out_impoints.write(image_points);
}
QString QxrdPowderPoint::toString() const
{
return QObject::tr("[%1, %2, %3, %4, %5, %6, %7, %8]").arg(n1()).arg(n2()).arg(n3()).arg(x()).arg(y()).arg(r1()).arg(r2()).arg(az());
}
bool QxrdPowderPoint::operator != (const QxrdPowderPoint &pt) const
{
return n1()!=pt.n1() || n2()!=pt.n2() || n3()!=pt.n3() || x()!=pt.x() || y()!=pt.y() || r1()!=pt.r1() || r2() != pt.r2() || az()!=pt.az();
}
bool QxrdPowderPoint::operator == (const QxrdPowderPoint &pt) const
{
return n1()==pt.n1() && n2()==pt.n2() && n3()==pt.n3() && x()==pt.x() && y()==pt.y() && r1()==pt.r1() && r2() == pt.r2() && az()==pt.az();
}
int Ray::intersectsBox(const Vector3D &a, const Vector3D &b, Vector3D &in, Vector3D &out) const
{
Vector3D ax(b.x(), a.y(), a.z()); // a décalé en x
Vector3D az(a.x(), a.y(), b.z()); // a décalé en z
Vector3D bx(a.x(), b.y(), b.z()); // b décalé en x
Vector3D bz(b.x(), b.y(), a.z()); // b décalé en z
if(origine.x() > a.x() && origine.x() < b.x() && origine.y() > a.y() && origine.y() < b.y() && origine.z() > a.z() && origine.z() < b.z())
{
in=origine;
if(intersects(a, az, ax, out)){ // Intersection du plan a az ax
if(out.x() >= a.x() && out.x() <= b.x() && out.z() >= a.z() && out.z() <= b.z()){ // In da box
return 1;
}
}
if(intersects(a, az, bx, out)) { // Intersection du plan a az bx
if(out.y() >= a.y() && out.y() <= b.y() && out.z() >= a.z() && out.z() <= b.z()){ // In da box
return 1;
}
}
if(intersects(a, ax, bz, out)){ // Intersection du plan a ax bz
if(out.x() >= a.x() && out.x() <= b.x() && out.y() >= a.y() && out.y() <= b.y()){ // In da box
return 1;
}
}
if(intersects(b, bx, bz, out)){ // Intersection du plan b bx bz
if(out.x() >= a.x() && out.x() <= b.x() && out.z() >= a.z() && out.z() <= b.z()){ // In da box
return 1;
}
}
if(intersects(b, bx, az, out)){ // Intersection du plan b bx az
if(out.x() >= a.x() && out.x() <= b.x() && out.y() >= a.y() && out.y() <= b.y()){ // In da box
return 1;
}
}
if(intersects(b, bz, ax, out)){ // Intersection du plan b bz ax
if(out.y() >= a.y() && out.y() <= b.y() && out.z() >= a.z() && out.z() <= b.z()){ // In da box
return 1;
}
}
}
else { // Origine hors de la boite
int resu=0;
Vector3D aaxaz;
Vector3D aazbx;
Vector3D aaxbz;
Vector3D bbxbz;
Vector3D bbxaz;
Vector3D bbzax;
bool ifaaxaz = intersects(a, ax, az, aaxaz);
bool ifaazbx = intersects(a, az, bx, aazbx);
bool ifaaxbz = intersects(a, ax, bz, aaxbz);
bool ifbbxbz = intersects(b, bx, bz, bbxbz);
bool ifbbxaz = intersects(b, bx, az, bbxaz);
bool ifbbzax = intersects(b, bz, ax, bbzax);
if(ifaaxaz&&resu<2){ // Intersection sur le plan a ax az et pas encore deux points d'entrée/sortie
if(aaxaz.x() >= a.x() && aaxaz.x() <= b.x() && aaxaz.z() >= a.z() && aaxaz.z() <= b.z()){ // In da box
if(resu==0){
in=aaxaz;
++resu;
}
else{
if(resu==1&&aaxaz.distanceToPoint(in)>0.001){
out=aaxaz;
++resu;
}
}
}
}
if(ifaazbx&&resu<2){ // Intersection sur le plan a az bx et pas encore deux points d'entrée/sortie
if(aazbx.y() >= a.y() && aazbx.y() <= b.y() && aazbx.z() >= a.z() && aazbx.z() <= b.z()){ // In da box
if(resu==0){
in=aazbx;
++resu;
}
else{
if(resu==1&&aazbx.distanceToPoint(in)>0.001){
out=aazbx;
++resu;
}
}
}
}
if(ifaaxbz&&resu<2){ // Intersection sur le plan a ax bz et pas encore deux points d'entrée/sortie
if(aaxbz.x() >= a.x() && aaxbz.x() <= b.x() && aaxbz.y() >= a.y() && aaxbz.y() <= b.y()){ // In da box
if(resu==0){
in=aaxbz;
++resu;
}
else{
if(resu==1&&aaxbz.distanceToPoint(in)>0.001){
out=aaxbz;
++resu;
}
}
}
}
if(ifbbxbz&&resu<2){ // Intersection sur le plan b bx bz et pas encore deux points d'entrée/sortie
if(bbxbz.x() >= a.x() && bbxbz.x() <= b.x() && bbxbz.z() >= a.z() && bbxbz.z() <= b.z()){ // In da box
if(resu==0){
in=bbxbz;
++resu;
}
else{
if(resu==1&&bbxbz.distanceToPoint(in)>0.001){
out=bbxbz;
++resu;
}
}
}
}
if(ifbbxaz&&resu<2){ // Intersection sur le plan b bx az et pas encore deux points d'entrée/sortie
if(bbxaz.x() >= a.x() && bbxaz.x() <= b.x() && bbxaz.y() >= a.y() && bbxaz.y() <= b.y()){ // In da box
if(resu==0){
in=bbxaz;
++resu;
}
else{
if(resu==1&&bbxaz.distanceToPoint(in)>0.001){
out=bbxaz;
++resu;
}
}
}
}
if(ifbbzax&&resu<2){ // Intersection sur le plan b bz ax et pas encore deux points d'entrée/sortie
if(bbzax.y() >= a.y() && bbzax.y() <= b.y() && bbzax.z() >= a.z() && bbzax.z() <= b.z()){ // In da box
if(resu==0){
in=bbzax;
++resu;
}
else{
if(resu==1&&bbzax.distanceToPoint(in)>0.001){
out=bbzax;
++resu;
}
}
}
}
if(resu < 2) // Passe hors de la boite ou sur une arrête
{
return 0;
}
if(origine.distanceToPointSquared(in) > origine.distanceToPointSquared(out)) // Inversion si les deux points trouvés ne l'ont pas été dans le bon ordre
{
Vector3D tmp = in;
in = out;
out = tmp;
}
return 2;
}
}
int main( int argc, char ** argv )
{
char * in_file = NULL;
char * out_file = NULL;
unsigned int max_it = 2000; // Max nr. of iterations
bool print_input = false;
bool verbose = false;
int mode = MODE_PT; // 1: PocketTopo Optimize
// 2: Optimize2
// 3: OptimizeBest
// 4: PocketTopo Optimize with optimize_axis
// 5: OptimizeBest with optimize_axis
// 6: Optimize M at fixed G
// 7: Optimize iterative
double delta = 0.5; // required delta for OptimizeBest
double error;
unsigned int iter = 0;
int ac = 1;
while ( ac < argc ) {
if ( strncmp(argv[ac],"-i",2) == 0 ) {
max_it = atoi( argv[ac+1]);
if ( max_it < 200 ) max_it = 200;
ac += 2;
#ifdef EXPERIMENTAL
} else if ( strncmp(argv[ac],"-m", 2) == 0 ) {
mode = atoi( argv[ac+1] );
if ( mode < MODE_PT || mode >= MODE_MAX ) mode = MODE_PT;
ac += 2;
} else if ( strncmp(argv[ac],"-d", 2) == 0 ) {
delta = atof( argv[ac+1] );
if ( delta < 0.001 ) delta = 0.5;
ac += 2;
#endif
} else if ( strncmp(argv[ac],"-p", 2) == 0 ) {
print_input = true;
ac ++;
} else if ( strncmp(argv[ac],"-v", 2) == 0 ) {
verbose = true;
ac ++;
} else if ( strncmp(argv[ac],"-h", 2) == 0 ) {
usage();
ac ++;
} else {
break;
}
}
if ( ac >= argc ) {
usage();
return 0;
}
in_file = argv[ac];
ac ++;
if ( argc > ac ) {
out_file = argv[ac];
}
if ( verbose ) {
fprintf(stderr, "Calibration data file \"%s\"\n", in_file );
if ( out_file ) {
fprintf(stderr, "Calibration coeff file \"%s\"\n", out_file );
}
fprintf(stderr, "Max nr. iterations %d \n", max_it );
#ifdef EXPERIMENTAL
fprintf(stderr, "Optimization mode: nr. %d\n", mode );
#endif
}
FILE * fp = fopen( in_file, "r" );
if ( fp == NULL ) {
fprintf(stderr, "ERROR: cannot open input file \"%s\"\n", in_file);
return 0;
}
Calibration calib;
int16_t gx, gy, gz, mx, my, mz;
int grp;
int ignore;
int cnt = 0;
char line[256];
if ( fgets(line, 256, fp) == NULL ) { // empty file
fclose( fp );
return false;
}
if ( line[0] == '0' && line[1] == 'x' ) { // calib-coeff format
if ( verbose ) {
fprintf(stderr, "Input file format: calib-coeff\n");
}
int gx0, gy0, gz0, mx0, my0, mz0;
// skip coeffs;
for (int k=1; k<6; ++k ) fgets(line, 256, fp);
// skip one more line
fgets(line, 256, fp);
// printf("reading after: %s\n", line);
while ( fgets(line, 255, fp ) ) {
// fprintf(stderr, line );
char rem[32];
if ( line[0] == '#' ) continue;
sscanf( line, "G: %d %d %d M: %d %d %d %s %d %d",
&gx0, &gy0, &gz0, &mx0, &my0, &mz0, rem, &grp, &ignore);
gx = (int16_t)( gx0 );
gy = (int16_t)( gy0 );
gz = (int16_t)( gz0 );
mx = (int16_t)( mx0 );
my = (int16_t)( my0 );
mz = (int16_t)( mz0 );
calib.AddValues( gx, gy, gz, mx, my, mz, cnt, grp, ignore );
++cnt;
if ( verbose ) {
fprintf(stderr,
"%d G: %d %d %d M: %d %d %d [%d]\n",
cnt, gx, gy, gz, mx, my, mz, grp );
}
}
} else {
if ( verbose ) {
fprintf(stderr, "Input file format: calib-data\n");
}
rewind( fp );
unsigned int gx0, gy0, gz0, mx0, my0, mz0;
while ( fgets(line, 255, fp ) ) {
// fprintf(stderr, line );
if ( line[0] == '#' ) continue;
sscanf( line, "%x %x %x %x %x %x %d %d",
&gx0, &gy0, &gz0, &mx0, &my0, &mz0, &grp, &ignore);
gx = (int16_t)( gx0 & 0xffff );
gy = (int16_t)( gy0 & 0xffff );
gz = (int16_t)( gz0 & 0xffff );
mx = (int16_t)( mx0 & 0xffff );
my = (int16_t)( my0 & 0xffff );
mz = (int16_t)( mz0 & 0xffff );
calib.AddValues( gx, gy, gz, mx, my, mz, cnt, grp, ignore );
++cnt;
if ( verbose ) {
fprintf(stderr,
"%d G: %d %d %d M: %d %d %d [%d]\n",
cnt, gx, gy, gz, mx, my, mz, grp );
}
}
}
fclose( fp );
if ( print_input ) {
calib.PrintValues();
// { int i; scanf("%d", &i); }
// calib.CheckInput();
// { int i; scanf("%d", &i); }
calib.PrintGroups();
}
calib.PrepareOptimize();
switch ( mode ) {
case MODE_PT:
iter = calib.Optimize( delta, error, max_it );
break;
#ifdef EXPERIMENTAL
case MODE_GRAD:
iter = calib.Optimize2( delta, error, max_it );
break;
case MODE_DELTA:
iter = calib.OptimizeBest( delta, error, max_it );
break;
case MODE_FIX_G:
{
// TODO set the G coeffs
Vector b( 0.0037, -0.0725, -0.0300);
Vector ax( 1.7678, -0.0013, 0.0119);
Vector ay( -0.0023, 1.7657, -0.0016);
Vector az(-0.0112, -0.0016, 1.7660);
Matrix a( ax, ay, az );
iter = calib.Optimize( delta, error, max_it );
fprintf(stdout, "Iterations %d\n", iter);
fprintf(stdout, "Delta %.4f\n", delta );
fprintf(stdout, "Max error %.4f\n", error );
calib.PrintCoeffs();
delta = 0.5;
iter = 0;
error = 0.0;
calib.SetGCoeffs( a, b );
iter = calib.OptimizeM( delta, error, max_it );
}
break;
case MODE_ITER:
/*
Vector b( 0.0037, -0.0725, -0.0300);
Vector ax( 1.7678, -0.0013, 0.0119);
Vector ay( -0.0023, 1.7657, -0.0016);
Vector az(-0.0112, -0.0016, 1.7660);
Matrix a( ax, ay, az );
calib.SetGCoeffs( a, b );
*/
iter = calib.OptimizeIterative( delta, error, max_it );
break;
#ifdef USE_GUI
case MODE_PT_DISPLAY:
calib.SetShowGui( true );
iter = calib.Optimize( delta, error, max_it );
// OptimizeExp( delta, error, max_it, true );
break;
case MODE_DELTA_DISPLAY:
calib.SetShowGui( true );
iter = calib.OptimizeBest( delta, error, max_it, true );
break;
#endif
#endif // EXPERIMENTAL
default:
fprintf(stderr, "Unexpected calibration mode %d\n", mode );
break;
}
fprintf(stdout, "Iterations %d\n", iter);
fprintf(stdout, "Delta %.4f\n", delta );
fprintf(stdout, "Max error %.4f\n", error );
fprintf(stderr, "M dip angle %.2f\n", calib.GetDipAngle() );
if ( verbose ) {
calib.PrintCoeffs();
// calib.CheckInput();
}
#if 0
unsigned char data[48];
calib.GetCoeff( data );
for (int k=0; k<48; ++k) {
printf("0x%02x ", data[k] );
if ( ( k % 4 ) == 3 ) printf("\n");
}
#endif
if ( out_file != NULL ) {
calib.PrintCalibrationFile( out_file );
}
return 0;
}
