std::ostream& operator<<(std::ostream& o, const Eigen::Transform<double, 3, Eigen::Affine>& t)
{
// o<<"T: trans="<<t.translation()<<" rot="<<Eigen::Quaterniond(t.rotation());
Eigen::AngleAxisd ax(t.rotation());
o << "T: trans=" << Eigen::Vector3d(t.translation()) << " rot=" << ax.angle() << " / " << ax.axis();
return o;
}
// rotate around given axis
Mat3 Mat3::rotation(float a, float x, float y, float z) {
Mat3 mat;
a = a * DEG_TO_RAD;
float c = cosf(a);
float s = sinf(a);
float t = 1.0f - c;
Vec3 ax(x,y,z);
ax.normalize();
float tx = t*ax.x;
float ty = t*ax.y;
float tz = t*ax.z;
float sx = s*ax.x;
float sy = s*ax.y;
float sz = s*ax.z;
float txy = tx * ax.y;
float tyz = tx * ax.z; // @todo is this correct?
float txz = tx * ax.z;
mat.m[0] = tx * ax.x + c;
mat.m[3] = txy - sz;
mat.m[6] = txz + sy;
mat.m[1] = txy + sz;
mat.m[4] = ty * ax.y + c;
mat.m[7] = tyz - sx;
mat.m[2] = txz - sy;
mat.m[5] = tyz + sx;
mat.m[8] = tz * ax.z +c;
return mat;
}
void RaycastVehicle::SetWheelAxle(int wheel, Vector3 axle)
{
btVector3 ax(axle.x_, axle.y_, axle.z_);
btRaycastVehicle* vehicle = vehicleData_->Get();
btWheelInfo& whInfo = vehicle->getWheelInfo(wheel);
whInfo.m_wheelAxleCS = ax;
}
void RigidAlignment::saveSphere(const char *dir)
{
for (int i = 0; i < m_nSubj; i++)
{
const float *axis = &faxis[i * 3];
float axis2[3];
updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2);
Vector ax(axis), ax2(axis2);
float inner = ax * ax2;
if (inner > 1) inner = 1;
else if (inner < -1) inner = -1;
float deg = acos(inner);
Mesh *sphere = new Mesh();
sphere->openFile(m_spherename);
// matrix
Vector ax3 = ax.cross(ax2); ax3.unit();
if (ax3.norm() != 0)
sphere->rotation(ax3.fv(), deg);
sphere->rotation(axis2, m_rot[i * 3]);
// output
char filename[1024];
sprintf(filename, "%s/%s.vtk", dir, m_filename[i]);
sphere->saveFile(filename, "vtk");
delete sphere;
}
}
Axisymetry_t Base_t::writeAxisymmetry( const vector<float>& refpt, const vector<float>& axis )
{
Array<float> apt(refpt), ax(axis);
int ier = cg_axisym_write( getFileID(), getID(), apt, ax );
check_error( "Base_t::writeAxiSymmetry", "cg_axisym_write", ier );
return Axisymetry_t(push( "Axisymmetry_t", 1 ));
}
void HLine::glCommands(bool select, bool marked, bool no_color){
if(!no_color){
wxGetApp().glColorEnsuringContrast(color);
}
GLfloat save_depth_range[2];
if(marked){
glGetFloatv(GL_DEPTH_RANGE, save_depth_range);
glDepthRange(0, 0);
glLineWidth(2);
}
glBegin(GL_LINES);
glVertex3d(A->m_p.X(), A->m_p.Y(), A->m_p.Z());
glVertex3d(B->m_p.X(), B->m_p.Y(), B->m_p.Z());
glEnd();
if(marked){
glLineWidth(1);
glDepthRange(save_depth_range[0], save_depth_range[1]);
}
#ifdef MULTIPLE_OWNERS
if(!A->m_p.IsEqual(B->m_p, wxGetApp().m_geom_tol))
{
gp_Pnt mid_point = A->m_p.XYZ() + (B->m_p.XYZ() - A->m_p.XYZ())/2;
gp_Dir dir = B->m_p.XYZ() - mid_point.XYZ();
gp_Ax1 ax(mid_point,dir);
}
#endif
EndedObject::glCommands(select,marked,no_color);
}
void toPose(const MathLib::Matrix4& mat4, tf::Pose& pose) {
MathLib::Matrix3 m1 = mat4.GetOrientation();
MathLib::Vector3 v1 = m1.GetRotationAxis();
tf::Vector3 ax(v1(0), v1(1), v1(2));
pose.setRotation(tf::Quaternion(ax, m1.GetRotationAngle()));
v1.Set(mat4.GetTranslation());
pose.setOrigin(tf::Vector3(v1(0),v1(1),v1(2)));
}
Vec3q SATSampler::operator()(const Vec2q &uv,const Vec2q &diff) const {
f32x4b fullMask=diff.x>=0.5f||diff.x>= 0.5f;
if(ForAll(fullMask)) return Vec3q(avg.x,avg.y,avg.z);
Vec2q tDiff=diff*floatq(0.5f);
Vec2q a=(uv-tDiff),b=(uv+tDiff);
a*=Vec2q(floatq(w),floatq(h));
b*=Vec2q(floatq(w),floatq(h));
i32x4 ax(a.x),ay(a.y);
i32x4 bx(b.x),by(b.y);
ax&=wMask; ay&=hMask;
bx&=wMask; by&=hMask;
union { __m128 count; float countf[4]; };
TSample sum[4];
i32x4 one(1);
if(ForAll(ax<=bx&&ay<=by)) {
count = (f32x4(by-ay+one)*f32x4(bx-ax+one)).m;
ComputeRect(ax,ay,bx,by,sum);
}
else for(int k=0;k<4;k++) {
if(ax[k]>bx[k]) {
if(ay[k]>by[k]) {
countf[k]=(bx[k]+1)*(by[k]+1)+(w-ax[k])*(h-ay[k]);
sum[k]=ComputeRect(0,0,bx[k],by[k])+ComputeRect(ax[k],ay[k],w-1,h-1);
}
else {
countf[k]=(bx[k]+1+w-ax[k])*(by[k]-ay[k]+1);
sum[k]=ComputeRect(0,ay[k],bx[k],by[k])+ComputeRect(ax[k],ay[k],w-1,by[k]);
}
}
else {
if(ay[k]>by[k]) {
countf[k]=(bx[k]-ax[k]+1)*(by[k]+h+1-ay[k]);
sum[k]=ComputeRect(ax[k],0,bx[k],by[k])+ComputeRect(ax[k],ay[k],bx[k],h-1);
}
else {
countf[k]=(by[k]-ay[k]+1)*(bx[k]-ax[k]+1);
sum[k]=ComputeRect(ax[k],ay[k],bx[k],by[k]);
}
}
}
union {
__m128 out[3];
struct { float ox[4]; float oy[4]; float oz[4]; } o;
};
o.ox[0]=sum[0].R(); o.oy[0]=sum[0].G(); o.oz[0]=sum[0].B();
o.ox[1]=sum[1].R(); o.oy[1]=sum[1].G(); o.oz[1]=sum[1].B();
o.ox[2]=sum[2].R(); o.oy[2]=sum[2].G(); o.oz[2]=sum[2].B();
o.ox[3]=sum[3].R(); o.oy[3]=sum[3].G(); o.oz[3]=sum[3].B();
return Condition(fullMask,Vec3q(avg.x,avg.y,avg.z),
Vec3q(out[0], out[1], out[2]) * Inv(floatq(count) * 255.0f));
}
Axisymetry_t Base_t::readAxisymmetry( vector<float>& refpt, vector<float>& axis ) const
{
Array<float> apt(3), ax(3);
int ier = cg_axisym_read( getFileID(), getID(), apt, ax );
check_error( "Base_t::readAxiSymmetry", "cg_axisym_read", ier );
refpt = apt;
axis = ax;
return Axisymetry_t(push( "Axisymmetry_t", 1 ));
}
// just use ADFun<double> constructor
void release_adfun_ctor(void)
{
CppAD::vector< CppAD::AD<double> > ax(1), ay(1);
ax[0] = 0.;
CppAD::Independent(ax);
ay[0] = fabs(ax[0]);
CppAD::ADFun<double> f(ax, ay);
return;
}
// multi_thread_checkpoint
bool multi_atomic(void)
{ bool ok = true;
// OpenMP setup
size_t num_threads = 4; // number of threads
omp_set_dynamic(0); // turn off dynamic thread adjustment
omp_set_num_threads( int(num_threads) ); // set number of OMP threads
// check that multi-threading is possible on this machine
if( omp_get_max_threads() < 2 )
{ std::cout << "This machine does not support multi-threading: ";
}
// create checkpoint version of algorithm
size_t n(1), m(1);
ad_vector ax(n), ay(m);
ax[0] = 2.0;
size_t length_of_sum = 5000;
long_sum_atomic atom_fun("long_sum", length_of_sum);
// setup for using CppAD in paralle mode
CppAD::thread_alloc::parallel_setup(num_threads, in_parallel, thread_num);
CppAD::thread_alloc::hold_memory(true);
CppAD::parallel_ad<double>();
// place to hold result for each thread
d_vector y(num_threads);
for(size_t thread = 0; thread < num_threads; thread++)
y[thread] = 0.0;
# pragma omp parallel for
for(int thread = 0; thread < int(num_threads); thread++)
{ ad_vector au(n), av(m);
au[0] = 1.0;
CppAD::Independent(au);
atom_fun(au, av);
CppAD::ADFun<double> f(au, av);
//
d_vector x(n), v(m);
x[0] = double( thread + 1 );
v = f.Forward(0, x);
//
// this assigment has false sharing; i.e., will case cache resets
// (conversion avoids boost vector conversion warning)
y[size_t(thread)] = v[0];
}
// check the results
for(size_t thread = 0; thread < num_threads; thread++)
{ double check = double( length_of_sum * (thread + 1) );
ok &= check == y[thread];
}
return ok;
}
NaGePlane::NaGePlane(const NaGePoint3D& P1, const NaGeVector3D& V1, const NaGeVector3D& V2)
{
NaGeVector3D v1 = V1, v2 = V2;
NaGeVector3D Dir = v1^v2;
NaGeAxisSystem ax(P1, Dir, v1);
itsLocation = ax;
UpdateEquation();
geomType = GEPLANE;
}
static expr_ref_vector mk_ineqs(app* x, app* y, app_ref_vector const& nums) {
ast_manager& m = nums.get_manager();
arith_util a(m);
expr_ref_vector result(m);
for (unsigned i = 0; i < nums.size(); ++i) {
expr_ref ax(a.mk_mul(nums[i], x), m);
result.push_back(a.mk_le(ax, y));
result.push_back(m.mk_not(a.mk_ge(ax, y)));
result.push_back(m.mk_not(a.mk_gt(y, ax)));
result.push_back(a.mk_lt(y, ax));
}
return result;
}
NaGePlane::NaGePlane(const NaGePoint3D& P1, const NaGePoint3D& P2, const NaGePoint3D& P3)
{
NaGeVector3D V1(P1, P2);
NaGeVector3D V2(P1, P3);
NaGeVector3D Dir = V1^V2;
NaGeAxisSystem ax(P1, Dir, V1);
itsLocation = ax;
UpdateEquation();
geomType = GEPLANE;
}
void NewtonEulerJointR::projectVectorDoF(const SiconosVector& v,
const BlockVector& q0,
SiconosVector& ans, int axis,
bool absoluteRef)
{
SiconosVector ax(3);
normalDoF(ax, q0, axis, absoluteRef);
double L = v(0)*ax(0) + v(1)*ax(1) + v(2)*ax(2);
ans(0) = ax(0) * L;
ans(1) = ax(1) * L;
ans(2) = ax(2) * L;
}
void Molecule::rotate(Vector axis, double theta) {
Eigen::Vector3d axisEig = {axis[0], axis[1], axis[2]};
Eigen::AngleAxisd ax(theta, axisEig);
Eigen::Matrix3d rot;
rot = ax;
Vector com = COM();
Eigen::Vector3d comEig= {com[0], com[1], com[2]};
for (int id : ids) {
Atom &a = state->idToAtom(id);
Eigen::Vector3d posEig = {a.pos[0], a.pos[1], a.pos[2]};
Eigen::Vector3d relEig = posEig-comEig;
relEig = rot * relEig;
a.pos = Vector(relEig[0], relEig[1], relEig[2]) + com;
}
}
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;
}
Point3 plDistributor::IPerpAxis(const Point3& p) const
{
const float kMinLengthSquared = 1.e-1f;
int minAx = p.MinComponent();
Point3 ax(0,0,0);
ax[minAx] = 1.f;
Point3 perp = p ^ ax;
if( perp.LengthSquared() < kMinLengthSquared )
{
// hmm, think we might be screwed, but this shouldn't happen.
}
return perp = perp.FNormalize();
}
int simEmbRotateAroundAxis(const float* positionIn,const float* quaternionIn,const float* axisVector,const float* axisPosition,float angle,float* positionOut,float* quaternionOut)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector m;
m.Q(0)=quaternionIn[3];
m.Q(1)=quaternionIn[0];
m.Q(2)=quaternionIn[1];
m.Q(3)=quaternionIn[2];
m.X(0)=positionIn[0];
m.X(1)=positionIn[1];
m.X(2)=positionIn[2];
C3Vector ax(axisVector);
C3Vector pos(axisPosition);
float alpha=-atan2(ax(1),ax(0));
float beta=atan2(-sqrt(ax(0)*ax(0)+ax(1)*ax(1)),ax(2));
m.X-=pos;
C7Vector r;
r.X.clear();
r.Q.setEulerAngles(0.0f,0.0f,alpha);
m=r*m;
r.Q.setEulerAngles(0.0f,beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,angle);
m=r*m;
r.Q.setEulerAngles(0.0f,-beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,-alpha);
m=r*m;
m.X+=pos;
quaternionOut[0]=m.Q(1);
quaternionOut[1]=m.Q(2);
quaternionOut[2]=m.Q(3);
quaternionOut[3]=m.Q(0);
positionOut[0]=m.X(0);
positionOut[1]=m.X(1);
positionOut[2]=m.X(2);
return(1);
}
NaGeCircle3d::NaGeCircle3d(const NaGePoint3D& P1, const NaGePoint3D& P2, const NaGePoint3D& P3)
{
NaGePoint3D p1=P1, p2=P2, p3=P3;
double x1, x2, x3, y1, y2, y3, z1, z2, z3;
x1 = p1.GetX(); y1 = p1.GetY(); z1 = p1.GetZ();
x2 = p2.GetX(); y2 = p2.GetY(); z2 = p2.GetZ();
x3 = p3.GetX(); y3 = p3.GetY(); z3 = p3.GetZ();
NaGeVector3D pv1(p1);
NaGeVector3D pv2(p2);
NaGeVector3D pv3(p3);
NaGeVector3D n1(pv1, pv2);
NaGeVector3D n2(pv2, pv3);
NaGeVector3D n3(pv3, pv1);
double Row1[3]={n1.GetX(), n1.GetY(), n1.GetZ()};
double Row2[3]={n2.GetX(), n2.GetY(), n2.GetZ()};
double Row3[3]={n3.GetX(), n3.GetY(), n3.GetZ()};
double b1, b2, b3;
b1 = ((p1.Distance(p2))/2) + (x1*n1.GetX() + y1*n1.GetY() + z1*n1.GetZ());
b2 = ((p2.Distance(p3))/2) + (x2*n2.GetX() + y2*n2.GetY() + z2*n2.GetZ());
b3 = ((p3.Distance(p1))/2) + (x3*n3.GetX() + y3*n3.GetY() + z3*n3.GetZ());
NaGeVector3D b(b1, b2, b3);
NaGeMatrix33 M(Row1, Row2, Row3);
M.Invert();
NaGeVector3D cen = M*b;
NaGeVector3D dir = n1.Crossed(n2);
NaGeVector3D xdir = dir.Crossed(n1);
NaGeAxisSystem ax(NaGePoint3D(cen.GetX(), cen.GetY(), cen.GetZ()),
dir, xdir);
itsLocation = ax;
itsRadius = p1.Distance(cen.Point());
cType = CIRCLE;
}
void XYView_helper::print(Printer* c, const Allocation&) const {
current_draw_view_ = v_;
c->push_clipping();
c->clip_rect(v_->left(), v_->bottom(), v_->right(), v_->top());
char buf[100];
float x, b;
v_->s2o().matrix(x, b, b, b, b, b);
sprintf(buf, "\n%g setlinewidth", x);
c->comment(buf);
// when printfile started printing at the level of the xyview
// the allocation was incorrect and was used by the background
// that was ok when the background was white...
// set the allocation the same as the clipping
Allocation a1;
Allotment ax(v_->left(), v_->width(), 0);
Allotment ay(v_->bottom(), v_->height(), 0);
a1.allot_x(ax);
a1.allot_y(ay);
body()->print(c, a1);
c->pop_clipping();
}
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;
}
//-----------------------------------------------------------------------------------------//
//--------------------------------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;
}
Word getAccum() { return !wordSize ? al() : ax(); }
void RigidAlignment::update(void)
{
// new point
for (int i = 0; i < m_nSubj; i++)
{
const float *axis = &faxis[i * 3];
// new axis
float axis2[3];
updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2);
Vector ax(axis), ax2(axis2);
float inner = ax * ax2;
if (inner > 1) inner = 1;
else if (inner < -1) inner = -1;
float deg = acos(inner);
// matrix
float mat[9];
Vector ax3 = ax.cross(ax2); ax3.unit();
if (ax3.norm() != 0)
{
Coordinate::rotation(ax3.fv(), deg, mat);
// axis rotation
for (int j = 0; j < m_nLM; j++)
{
float newp[3];
float *p = &fpoint[(i * m_nLM + j) * 3];
int id = m_point[i][j];
memcpy(p, m_sphere->vertex(id)->fv(), sizeof(float) * 3);
Coordinate::rotPoint(p, mat, newp);
memcpy(p, newp, sizeof(float) * 3);
}
}
// matrix
Coordinate::rotation(axis2, m_rot[i * 3], mat);
// rotation
for (int j = 0; j < m_nLM; j++)
{
float newp[3];
float *p = &fpoint[(i * m_nLM + j) * 3];
Coordinate::rotPoint(p, mat, newp);
memcpy(p, newp, sizeof(float) * 3);
}
}
// mean
memset(fmean, 0, sizeof(float) * m_nLM * 3);
for (int i = 0; i < m_nLM; i++)
for (int j = 0; j < m_nSubj; j++)
for (int k = 0; k < 3; k++)
fmean[i * 3 + k] += fpoint[(j * m_nLM + i) * 3 + k] / m_nSubj;
for (int i = 0; i < m_nLM; i++)
{
float norm = fmean[i * 3] * fmean[i * 3] + fmean[i * 3 + 1] * fmean[i * 3 + 1] + fmean[i * 3 + 2] * fmean[i * 3 + 2];
norm = sqrt(norm);
for (int j = 0; j < 3; j++)
fmean[i * 3 + j] /= norm;
}
/*for (int i = 0; i < m_nLM; i++)
cout << fmean[i * 3] << " " << fmean[i * 3 + 1] << " " << fmean[i * 3 + 2] << endl;*/
}
int main(int argc, char* argv[])
{
if (argc < 2) {
printf("Usage: %s <program name>\n", argv[0]);
exit(0);
}
filename = argv[1];
FILE* fp = fopen(filename, "rb");
if (fp == 0)
error("opening");
ram = (Byte*)malloc(0x10000);
memset(ram, 0, 0x10000);
if (ram == 0) {
fprintf(stderr, "Out of memory\n");
exit(1);
}
if (fseek(fp, 0, SEEK_END) != 0)
error("seeking");
length = ftell(fp);
if (length == -1)
error("telling");
if (fseek(fp, 0, SEEK_SET) != 0)
error("seeking");
if (length > 0x10000 - 0x100) {
fprintf(stderr, "%s is too long to be a .com file\n", filename);
exit(1);
}
if (fread(&ram[0x100], length, 1, fp) != 1)
error("reading");
fclose(fp);
Word segment = 0x1000;
setAX(0x0000);
setCX(0x00FF);
setDX(segment);
registers[3] = 0x0000;
setSP(0xFFFE);
registers[5] = 0x091C;
setSI(0x0100);
setDI(0xFFFE);
for (int i = 0; i < 4; ++i)
registers[8 + i] = segment;
Byte* byteData = (Byte*)®isters[0];
int bigEndian = (byteData[2] == 0 ? 1 : 0);
int byteNumbers[8] = {0, 2, 4, 6, 1, 3, 5, 7};
for (int i = 0 ; i < 8; ++i)
byteRegisters[i] = &byteData[byteNumbers[i] ^ bigEndian];
bool prefix = false;
for (int i = 0; i < 1000000000; ++i) {
if (!repeating) {
if (!prefix) {
segmentOverride = -1;
rep = 0;
}
prefix = false;
opcode = fetchByte();
}
wordSize = ((opcode & 1) != 0);
bool sourceIsRM = ((opcode & 2) != 0);
int operation = (opcode >> 3) & 7;
bool jump;
switch (opcode) {
case 0x00: case 0x01: case 0x02: case 0x03:
case 0x08: case 0x09: case 0x0a: case 0x0b:
case 0x10: case 0x11: case 0x12: case 0x13:
case 0x18: case 0x19: case 0x1a: case 0x1b:
case 0x20: case 0x21: case 0x22: case 0x23:
case 0x28: case 0x29: case 0x2a: case 0x2b:
case 0x30: case 0x31: case 0x32: case 0x33:
case 0x38: case 0x39: case 0x3a: case 0x3b: // alu rmv,rmv
data = readEA();
if (!sourceIsRM) {
destination = data;
source = getReg();
}
else {
destination = getReg();
source = data;
}
aluOperation = operation;
doALUOperation();
if (aluOperation != 7) {
if (!sourceIsRM)
finishWriteEA(data);
else
setReg(data);
}
break;
case 0x04: case 0x05: case 0x0c: case 0x0d:
case 0x14: case 0x15: case 0x1c: case 0x1d:
case 0x24: case 0x25: case 0x2c: case 0x2d:
case 0x34: case 0x35: case 0x3c: case 0x3d: // alu accum,i
destination = getAccum();
source = !wordSize ? fetchByte() : fetchWord();
aluOperation = operation;
doALUOperation();
if (aluOperation != 7)
setAccum();
break;
case 0x06: case 0x0e: case 0x16: case 0x1e: // PUSH segreg
push(registers[operation + 8]);
break;
case 0x07: case 0x17: case 0x1f: // POP segreg
registers[operation + 8] = pop();
break;
case 0x26: case 0x2e: case 0x36: case 0x3e: // segment override
segmentOverride = operation;
prefix = true;
break;
case 0x27: case 0x2f: // DA
if (af() || (al() & 0x0f) > 9) {
data = al() + (opcode == 0x27 ? 6 : -6);
setAL(data);
setAF(true);
if ((data & 0x100) != 0)
setCF(true);
}
setCF(cf() || al() > 0x9f);
if (cf())
setAL(al() + (opcode == 0x27 ? 0x60 : -0x60));
wordSize = false;
data = al();
setPZS();
break;
case 0x37: case 0x3f: // AA
if (af() || (al() & 0xf) > 9) {
setAL(al() + (opcode == 0x37 ? 6 : -6));
setAH(ah() + (opcode == 0x37 ? 1 : -1));
setCA();
}
else
clearCA();
setAL(al() & 0x0f);
break;
case 0x40: case 0x41: case 0x42: case 0x43:
case 0x44: case 0x45: case 0x46: case 0x47:
case 0x48: case 0x49: case 0x4a: case 0x4b:
case 0x4c: case 0x4d: case 0x4e: case 0x4f: // incdec rw
destination = rw();
wordSize = true;
setRW(incdec((opcode & 8) != 0));
break;
case 0x50: case 0x51: case 0x52: case 0x53:
case 0x54: case 0x55: case 0x56: case 0x57: // PUSH rw
push(rw());
break;
case 0x58: case 0x59: case 0x5a: case 0x5b:
case 0x5c: case 0x5d: case 0x5e: case 0x5f: // POP rw
setRW(pop());
break;
case 0x60: case 0x61: case 0x62: case 0x63:
case 0x64: case 0x65: case 0x66: case 0x67:
case 0x68: case 0x69: case 0x6a: case 0x6b:
case 0x6c: case 0x6d: case 0x6e: case 0x6f:
case 0xc0: case 0xc1: case 0xc8: case 0xc9: // invalid
case 0xcc: case 0xf0: case 0xf1: case 0xf4: // INT 3, LOCK, HLT
case 0x9b: case 0xce: case 0x0f: // WAIT, INTO, POP CS
case 0xd8: case 0xd9: case 0xda: case 0xdb:
case 0xdc: case 0xdd: case 0xde: case 0xdf: // escape
case 0xe4: case 0xe5: case 0xe6: case 0xe7:
case 0xec: case 0xed: case 0xee: case 0xef: // IN, OUT
fprintf(stderr, "Invalid opcode %02x", opcode);
runtimeError("");
break;
case 0x70: case 0x71: case 0x72: case 0x73:
case 0x74: case 0x75: case 0x76: case 0x77:
case 0x78: case 0x79: case 0x7a: case 0x7b:
case 0x7c: case 0x7d: case 0x7e: case 0x7f: // Jcond cb
switch (opcode & 0x0e) {
case 0x00: jump = of(); break;
case 0x02: jump = cf(); break;
case 0x04: jump = zf(); break;
case 0x06: jump = cf() || zf(); break;
case 0x08: jump = sf(); break;
case 0x0a: jump = pf(); break;
case 0x0c: jump = sf() != of(); break;
default: jump = sf() != of() || zf(); break;
}
jumpShort(fetchByte(), jump == ((opcode & 1) == 0));
break;
case 0x80: case 0x81: case 0x82: case 0x83: // alu rmv,iv
destination = readEA();
data = fetch(opcode == 0x81);
if (opcode != 0x83)
source = data;
else
source = signExtend(data);
aluOperation = modRMReg();
doALUOperation();
if (aluOperation != 7)
finishWriteEA(data);
break;
case 0x84: case 0x85: // TEST rmv,rv
data = readEA();
test(data, getReg());
break;
case 0x86: case 0x87: // XCHG rmv,rv
data = readEA();
finishWriteEA(getReg());
setReg(data);
break;
case 0x88: case 0x89: // MOV rmv,rv
ea();
finishWriteEA(getReg());
break;
case 0x8a: case 0x8b: // MOV rv,rmv
setReg(readEA());
break;
case 0x8c: // MOV rmw,segreg
ea();
wordSize = 1;
finishWriteEA(registers[modRMReg() + 8]);
break;
case 0x8d: // LEA
address = ea();
if (!useMemory)
runtimeError("LEA needs a memory address");
setReg(address);
break;
case 0x8e: // MOV segreg,rmw
wordSize = 1;
data = readEA();
registers[modRMReg() + 8] = data;
break;
case 0x8f: // POP rmw
writeEA(pop());
break;
case 0x90: case 0x91: case 0x92: case 0x93:
case 0x94: case 0x95: case 0x96: case 0x97: // XCHG AX,rw
data = ax();
setAX(rw());
setRW(data);
break;
case 0x98: // CBW
setAX(signExtend(al()));
break;
case 0x99: // CWD
setDX((ax() & 0x8000) == 0 ? 0x0000 : 0xffff);
break;
case 0x9a: // CALL cp
savedIP = fetchWord();
savedCS = fetchWord();
farCall();
break;
case 0x9c: // PUSHF
push((flags & 0x0fd7) | 0xf000);
break;
case 0x9d: // POPF
flags = pop() | 2;
break;
case 0x9e: // SAHF
flags = (flags & 0xff02) | ah();
break;
case 0x9f: // LAHF
setAH(flags & 0xd7);
break;
case 0xa0: case 0xa1: // MOV accum,xv
data = read(fetchWord());
setAccum();
break;
case 0xa2: case 0xa3: // MOV xv,accum
write(getAccum(), fetchWord());
break;
case 0xa4: case 0xa5: // MOVSv
stoS(lodS());
doRep();
break;
case 0xa6: case 0xa7: // CMPSv
lodDIS();
source = data;
sub();
doRep();
break;
case 0xa8: case 0xa9: // TEST accum,iv
data = fetch(wordSize);
test(getAccum(), data);
break;
case 0xaa: case 0xab: // STOSv
stoS(getAccum());
doRep();
break;
case 0xac: case 0xad: // LODSv
data = lodS();
setAccum();
doRep();
break;
case 0xae: case 0xaf: // SCASv
lodDIS();
destination = getAccum();
source = data;
sub();
doRep();
break;
case 0xb0: case 0xb1: case 0xb2: case 0xb3:
case 0xb4: case 0xb5: case 0xb6: case 0xb7:
setRB(fetchByte());
break;
case 0xb8: case 0xb9: case 0xba: case 0xbb:
case 0xbc: case 0xbd: case 0xbe: case 0xbf: // MOV rv,iv
setRW(fetchWord());
break;
case 0xc2: case 0xc3: case 0xca: case 0xcb: // RET
savedIP = pop();
savedCS = (opcode & 8) == 0 ? cs() : pop();
if (!wordSize)
setSP(sp() + fetchWord());
farJump();
break;
case 0xc4: case 0xc5: // LES/LDS
ea();
farLoad();
*modRMRW() = savedIP;
registers[8 + (!wordSize ? 0 : 3)] = savedCS;
break;
case 0xc6: case 0xc7: // MOV rmv,iv
ea();
finishWriteEA(fetch(wordSize));
break;
case 0xcd:
data = fetchByte();
if (data != 0x21) {
fprintf(stderr, "Unknown interrupt 0x%02x", data);
runtimeError("");
}
switch (ah()) {
case 2:
printf("%c", dl());
break;
case 0x4c:
printf("*** Bytes: %i\n", length);
printf("*** Cycles: %i\n", ios);
printf("*** EXIT code %i\n", al());
exit(0);
break;
default:
fprintf(stderr, "Unknown DOS call 0x%02x", data);
runtimeError("");
}
break;
case 0xcf:
ip = pop();
setCS(pop());
flags = pop() | 2;
break;
case 0xd0: case 0xd1: case 0xd2: case 0xd3: // rot rmv,n
data = readEA();
if ((opcode & 2) == 0)
source = 1;
else
source = cl();
while (source != 0) {
destination = data;
switch (modRMReg()) {
case 0: // ROL
data <<= 1;
doCF();
data |= (cf() ? 1 : 0);
setOFRotate();
break;
case 1: // ROR
setCF((data & 1) != 0);
data >>= 1;
if (cf())
data |= (!wordSize ? 0x80 : 0x8000);
setOFRotate();
break;
case 2: // RCL
data = (data << 1) | (cf() ? 1 : 0);
doCF();
setOFRotate();
break;
case 3: // RCR
data >>= 1;
if (cf())
data |= (!wordSize ? 0x80 : 0x8000);
setCF((destination & 1) != 0);
setOFRotate();
break;
case 4: // SHL
case 6:
data <<= 1;
doCF();
setOFRotate();
setPZS();
break;
case 5: // SHR
setCF((data & 1) != 0);
data >>= 1;
setOFRotate();
setAF(true);
setPZS();
break;
case 7: // SAR
setCF((data & 1) != 0);
data >>= 1;
if (!wordSize)
data |= (destination & 0x80);
else
data |= (destination & 0x8000);
setOFRotate();
setAF(true);
setPZS();
break;
}
--source;
}
finishWriteEA(data);
break;
case 0xd4: // AAM
data = fetchByte();
if (data == 0)
divideOverflow();
setAH(al() / data);
setAL(al() % data);
wordSize = true;
setPZS();
break;
case 0xd5: // AAD
data = fetchByte();
setAL(al() + ah()*data);
setAH(0);
setPZS();
break;
case 0xd6: // SALC
setAL(cf() ? 0xff : 0x00);
break;
case 0xd7: // XLATB
setAL(readByte(bx() + al()));
break;
case 0xe0: case 0xe1: case 0xe2: // LOOPc cb
setCX(cx() - 1);
jump = (cx() != 0);
switch (opcode) {
case 0xe0: if (zf()) jump = false; break;
case 0xe1: if (!zf()) jump = false; break;
}
jumpShort(fetchByte(), jump);
break;
case 0xe3: // JCXZ cb
jumpShort(fetchByte(), cx() == 0);
break;
case 0xe8: // CALL cw
call(ip + fetchWord());
break;
case 0xe9: // JMP cw
ip += fetchWord();
break;
case 0xea: // JMP cp
savedIP = fetchWord();
savedCS = fetchWord();
farJump();
break;
case 0xeb: // JMP cb
jumpShort(fetchByte(), true);
break;
case 0xf2: case 0xf3: // REP
rep = opcode == 0xf2 ? 1 : 2;
prefix = true;
break;
case 0xf5: // CMC
flags ^= 1;
break;
case 0xf6: case 0xf7: // math rmv
data = readEA();
switch (modRMReg()) {
case 0: case 1: // TEST rmv,iv
test(data, fetch(wordSize));
break;
case 2: // NOT iv
finishWriteEA(~data);
break;
case 3: // NEG iv
source = data;
destination = 0;
sub();
finishWriteEA(data);
break;
case 4: case 5: // MUL rmv, IMUL rmv
source = data;
destination = getAccum();
data = destination;
setSF();
setPF();
data *= source;
setAX(data);
if (!wordSize) {
if (modRMReg() == 4)
setCF(ah() != 0);
else {
if ((source & 0x80) != 0)
setAH(ah() - destination);
if ((destination & 0x80) != 0)
setAH(ah() - source);
setCF(ah() ==
((al() & 0x80) == 0 ? 0 : 0xff));
}
}
else {
setDX(data >> 16);
if (modRMReg() == 4) {
data |= dx();
setCF(dx() != 0);
}
else {
if ((source & 0x8000) != 0)
setDX(dx() - destination);
if ((destination & 0x8000) != 0)
setDX(dx() - source);
data |= dx();
setCF(dx() ==
((ax() & 0x8000) == 0 ? 0 : 0xffff));
}
}
setZF();
setOF(cf());
break;
case 6: case 7: // DIV rmv, IDIV rmv
source = data;
if (source == 0)
divideOverflow();
if (!wordSize) {
destination = ax();
if (modRMReg() == 6) {
div();
if (data > 0xff)
divideOverflow();
}
else {
destination = ax();
if ((destination & 0x8000) != 0)
destination |= 0xffff0000;
source = signExtend(source);
div();
if (data > 0x7f && data < 0xffffff80)
divideOverflow();
}
setAH(remainder);
setAL(data);
}
else {
destination = (dx() << 16) + ax();
div();
if (modRMReg() == 6) {
if (data > 0xffff)
divideOverflow();
}
else {
if (data > 0x7fff && data < 0xffff8000)
divideOverflow();
}
setDX(remainder);
setAX(data);
}
break;
}
break;
case 0xf8: case 0xf9: // STC/CLC
setCF(wordSize);
break;
case 0xfa: case 0xfb: // STI/CLI
setIF(wordSize);
break;
case 0xfc: case 0xfd: // STD/CLD
setDF(wordSize);
break;
case 0xfe: case 0xff: // misc
ea();
if ((!wordSize && modRMReg() >= 2 && modRMReg() <= 6) ||
modRMReg() == 7) {
fprintf(stderr, "Invalid instruction %02x %02x", opcode,
modRM);
runtimeError("");
}
switch (modRMReg()) {
case 0: case 1: // incdec rmv
destination = readEA2();
finishWriteEA(incdec(modRMReg() != 0));
break;
case 2: // CALL rmv
call(readEA2());
break;
case 3: // CALL mp
farLoad();
farCall();
break;
case 4: // JMP rmw
ip = readEA2();
break;
case 5: // JMP mp
farLoad();
farJump();
break;
case 6: // PUSH rmw
push(readEA2());
break;
}
break;
}
}
runtimeError("Timed out");
}
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]);
}
void cgTest() {
float mass = 0.01;
float kappa = 1.0 / (2.0*(4 + mass));
float *gauge[4];
for (int dir = 0; dir < 4; dir++) {
gauge[dir] = (float*)malloc(N*gaugeSiteSize*sizeof(float));
}
//constructGaugeField(gauge);
constructUnitGaugeField(gauge);
float *spinorIn = (float*)malloc(N*spinorSiteSize*sizeof(float));
float *spinorOut = (float*)malloc(N*spinorSiteSize*sizeof(float));
#ifdef EVEN_ODD
float *source = (float *)malloc(Nh*spinorSiteSize*sizeof(float));
float *tmp = (float *)malloc(Nh*spinorSiteSize*sizeof(float));
#else
float *source = (float *)malloc(N*spinorSiteSize*sizeof(float));
#endif
int i0 = 0;
int s0 = 0;
int c0 = 0;
constructPointSpinorField(spinorIn, i0, s0, c0);
//constructSpinorField(spinorIn);
// Prepare the source term
ax(2*kappa, spinorIn, N*spinorSiteSize);
// see output element
// Mat(source, gauge, spinorIn, kappa);
// printSpinorElement(source, 0);
#ifdef EVEN_ODD
float *spinorInOdd = spinorIn + Nh*spinorSiteSize;
dslashReference(tmp, gauge, spinorInOdd, 0, 0);
xpay(spinorIn, kappa, tmp, Nh*spinorSiteSize);
MatPCDag(source, gauge, tmp, kappa);
#else
MatDag(source, gauge, spinorIn, kappa);
#endif
cgCuda(spinorOut, gauge, source, kappa, 1e-7);
// cg_reference(spinorOut, gauge, source, kappa, 1e-7);
// Reconstruct the full inverse
#ifdef EVEN_ODD
float *spinorOutOdd = spinorOut + Nh*spinorSiteSize;
dslashReference(spinorOutOdd, gauge, spinorOut, 1, 0);
xpay(spinorInOdd, kappa, spinorOutOdd, Nh*spinorSiteSize);
#endif
printf("Result norm = %e\n", norm(spinorOut, N*spinorSiteSize));
// release memory
for (int dir = 0; dir < 4; dir++) free(gauge[dir]);
free(source);
#ifdef EVEN_ODD
free(tmp);
#endif
free(spinorIn);
free(spinorOut);
}
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]);
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
populationSize = 30;
ui->popSizeSpinBox->setValue(populationSize);
srand(static_cast <unsigned> (time(0)));
stopFlag = false;
pop = new Population(populationSize,true);
setup();
model = new PopulationModel(*pop);
ui->listView->setModel(model);
QVector<double> ax(11);
for(int i=0;i<11;i++){
ax[i] = i*100;
}
QCPCurve *newCurve = new QCPCurve(ui->citiesPlot->xAxis, ui->citiesPlot->yAxis);
newCurve->setKeyAxis(ui->citiesPlot->xAxis);
newCurve->setValueAxis(ui->citiesPlot->yAxis);
newCurve->setLineStyle(QCPCurve::lsNone);
newCurve->setPen(QPen(Qt::red));
newCurve->setScatterStyle(QCPScatterStyle(QCPScatterStyle::ssCircle, Qt::blue, Qt::darkBlue, 5));
for(int i=0; i<TourManager::numberOfCities(); i++){
int x = TourManager::getCity(i).getX();
int y = TourManager::getCity(i).getY();
newCurve->addData(x, y);
}
newCurve->addData(TourManager::getCity(0).getX(), TourManager::getCity(0).getY());
ui->citiesPlot->addPlottable(newCurve);
ui->citiesPlot->xAxis->setRange(0, 1000);
ui->citiesPlot->yAxis->setRange(0, 1000);
ui->citiesPlot->xAxis->setAutoTickStep(false);
ui->citiesPlot->yAxis->setAutoTickStep(false);
ui->citiesPlot->xAxis->setTickStep(100);
ui->citiesPlot->yAxis->setTickStep(100);
ui->citiesPlot->xAxis->setTickVector(ax);
ui->citiesPlot->yAxis->setTickVector(ax);
//ui->citiesPlot->xAxis->setTicks(false);
//ui->citiesPlot->yAxis->setTicks(false);
ui->citiesPlot->xAxis->setTickLabels(true);
ui->citiesPlot->yAxis->setTickLabels(true);
ui->citiesPlot->xAxis->grid()->setPen(Qt::SolidLine);
ui->citiesPlot->yAxis->grid()->setPen(Qt::SolidLine);
//ui->citiesPlot->yAxis->setScaleRatio(ui->citiesPlot->xAxis,1.0);
ui->statPlot->setBackground(Qt::gray);
ui->statPlot->addGraph();
ui->statPlot->addGraph();
ui->statPlot->graph(0)->setPen(QPen(Qt::red));
ui->statPlot->graph(1)->setPen(QPen(Qt::blue));
ui->statPlot->graph(0)->setName(tr("Best"));
ui->statPlot->graph(0)->addToLegend();
ui->statPlot->graph(1)->setName(tr("Average"));
ui->statPlot->graph(1)->addToLegend();
ui->statPlot->xAxis->setRangeLower(0);
QFont legendFont = font();
legendFont.setPointSize(9);
QMargins margins(0,0,0,0);
ui->statPlot->legend->setFont(legendFont);
ui->statPlot->legend->setMargins(margins);
ui->statPlot->legend->setVisible(true);
ui->statPlot->axisRect()->insetLayout()->setInsetAlignment(0, Qt::AlignTop|Qt::AlignRight);
//Population pop(50, true);
generation = 0;
ui->statPlot->graph(0)->addData(0, pop->getFittest().getDistance());
ui->statPlot->graph(1)->addData(0,pop->getAverage());
ui->statPlot->graph(1)->rescaleAxes();
ui->statPlot->xAxis->setRange(0, 101);
ui->statPlot->xAxis->setTickStep(1);
ui->statPlot->yAxis->setRangeLower(0);
ui->statPlot->setInteraction(QCP::iRangeZoom);
ui->statPlot->setInteraction(QCP::iRangeDrag);
//ui->statPlot->axisRect()->setRangeDrag(Qt::Horizontal);
GeneticEngine *ge = new GeneticEngine();
ui->mutationSpinBox->setValue(GeneticEngine::getMutationRate());
ui->tournamentSpinBox->setValue(GeneticEngine::getTournamentSize());
ui->tournamentSpinBox->setMaximum(pop->populationSize());
ui->elitismCheckBox->setChecked(GeneticEngine::getElitism());
ui->mutTypeComboBox->setCurrentIndex(GeneticEngine::getMutationType());
ui->crossTypeComboBox->setCurrentIndex(GeneticEngine::getCrossoverType());
ui->listView->setWindowTitle(tr("Generation :"));
connect(ui->listView->selectionModel(), SIGNAL(selectionChanged(QItemSelection,QItemSelection)),this, SLOT(drawPath(QItemSelection)));
connect(ui->runButton, SIGNAL(clicked(bool)),this, SLOT(run()));
connect(ui->repopulateButton, SIGNAL(clicked()), this, SLOT(restart()));
connect(ui->mutationSpinBox, SIGNAL(valueChanged(double)), ge,SLOT(setMutationRate(double)));
connect(ui->tournamentSpinBox, SIGNAL(valueChanged(int)), ge, SLOT(setTournamentSize(int)));
connect(ui->elitismCheckBox, SIGNAL(toggled(bool)), ge, SLOT(setElitism(bool)));
connect(ui->mutTypeComboBox, SIGNAL(currentIndexChanged(int)), ge, SLOT(setMutationType(int)));
connect(ui->crossTypeComboBox, SIGNAL(currentIndexChanged(int)), ge, SLOT(setCrossoverType(int)));
connect(ui->popSizeSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setPopulationSize(int)));
connect(ui->actionAbout_Qt, SIGNAL(triggered()), qApp, SLOT(aboutQt()));
connect(ui->actionSave, SIGNAL(triggered()), this, SLOT(save()));
connect(ui->actionRandom_Initialization, SIGNAL(triggered()), this, SLOT(randInit()));
connect(ui->actionLoad, SIGNAL(triggered()), this, SLOT(open()));
readSettings();
//qDebug()<<pop->getFittest().getDistance();
}
