void Vec3d :: GetNormal (Vec3d & n) const { if (fabs (X()) > fabs (Z())) { n.X() = -Y(); n.Y() = X(); n.Z() = 0; } else { n.X() = 0; n.Y() = Z(); n.Z() = -Y(); } double len = n.Length(); if (len == 0) { n.X() = 1; n.Y() = n.Z() = 0; } else n /= len; }
int SolveLinearSystemLS (const Vec3d & col1, const Vec3d & col2, const Vec2d & rhs, Vec3d & sol) { double a11 = col1 * col1; double a12 = col1 * col2; double a22 = col2 * col2; double det = a11 * a22 - a12 * a12; if (det*det <= 1e-24 * a11 * a22) { sol = Vec3d (0, 0, 0); return 1; } Vec2d invrhs; invrhs.X() = ( a22 * rhs.X() - a12 * rhs.Y()) / det; invrhs.Y() = (-a12 * rhs.X() + a11 * rhs.Y()) / det; sol.X() = invrhs.X() * col1.X() + invrhs.Y() * col2.X(); sol.Y() = invrhs.X() * col1.Y() + invrhs.Y() * col2.Y(); sol.Z() = invrhs.X() * col1.Z() + invrhs.Y() * col2.Z(); return 0; /* Vec3d inv1, inv2; int err = PseudoInverse (col1, col2, inv1, inv2); sol = rhs.X() * inv1 + rhs.Y() * inv2; return err; */ }
// loads geometry from STL file DLL_HEADER Ng_STL_Geometry * Ng_STL_LoadGeometry (const char * filename, int binary) { int i; STLGeometry geom; STLGeometry* geo; ifstream ist(filename); if (binary) { geo = geom.LoadBinary(ist); } else { geo = geom.Load(ist); } readtrias.SetSize(0); readedges.SetSize(0); Point3d p; Vec3d normal; double p1[3]; double p2[3]; double p3[3]; double n[3]; Ng_STL_Geometry * geo2 = Ng_STL_NewGeometry(); for (i = 1; i <= geo->GetNT(); i++) { const STLTriangle& t = geo->GetTriangle(i); p = geo->GetPoint(t.PNum(1)); p1[0] = p.X(); p1[1] = p.Y(); p1[2] = p.Z(); p = geo->GetPoint(t.PNum(2)); p2[0] = p.X(); p2[1] = p.Y(); p2[2] = p.Z(); p = geo->GetPoint(t.PNum(3)); p3[0] = p.X(); p3[1] = p.Y(); p3[2] = p.Z(); normal = t.Normal(); n[0] = normal.X(); n[1] = normal.Y(); n[2] = normal.Z(); Ng_STL_AddTriangle(geo2, p1, p2, p3, n); } return geo2; }
STLGeometry* NGInterface::loadSTL(string data) { strstream ist; STLGeometry geom, *geo; Point3d p; Vec3d normal; double p1[3], p2[3], p3[3], n[3]; ist << data; try { geo = geom.Load(ist); } catch (...) { cerr << "Problem in STL-file!" << endl; return NULL; } if (!geo->GetNT()) { cerr << "Problem in STL-file!" << endl; return NULL; } readtrias.SetSize(0); readedges.SetSize(0); for (int i = 1; i <= geo->GetNT(); i++) { const STLTriangle& t = geo->GetTriangle(i); p = geo->GetPoint(t.PNum(1)); p1[0] = p.X(); p1[1] = p.Y(); p1[2] = p.Z(); p = geo->GetPoint(t.PNum(2)); p2[0] = p.X(); p2[1] = p.Y(); p2[2] = p.Z(); p = geo->GetPoint(t.PNum(3)); p3[0] = p.X(); p3[1] = p.Y(); p3[2] = p.Z(); normal = t.Normal(); n[0] = normal.X(); n[1] = normal.Y(); n[2] = normal.Z(); addTriangleSTL(p1,p2,p3,n); } return (STLGeometry*)(geometry = geo); }
void WriteSTLFormat (const Mesh & mesh, const string & filename) { cout << "\nWrite STL Surface Mesh" << endl; ofstream outfile (filename.c_str()); int i, j, k; outfile.precision(10); outfile << "solid" << endl; for (i = 1; i <= mesh.GetNSE(); i++) { outfile << "facet normal "; const Point3d& p1 = mesh.Point(mesh.SurfaceElement(i).PNum(1)); const Point3d& p2 = mesh.Point(mesh.SurfaceElement(i).PNum(2)); const Point3d& p3 = mesh.Point(mesh.SurfaceElement(i).PNum(3)); Vec3d normal = Cross(p2-p1,p3-p1); if (normal.Length() != 0) { normal /= (normal.Length()); } outfile << normal.X() << " " << normal.Y() << " " << normal.Z() << "\n"; outfile << "outer loop\n"; outfile << "vertex " << p1.X() << " " << p1.Y() << " " << p1.Z() << "\n"; outfile << "vertex " << p2.X() << " " << p2.Y() << " " << p2.Z() << "\n"; outfile << "vertex " << p3.X() << " " << p3.Y() << " " << p3.Z() << "\n"; outfile << "endloop\n"; outfile << "endfacet\n"; } outfile << "endsolid" << endl; }
int SolveLinearSystem (const Vec3d & col1, const Vec3d & col2, const Vec3d & col3, const Vec3d & rhs, Vec3d & sol) { // changed by MW double matrix[3][3]; double locrhs[3]; int retval = 0; for(int i=0; i<3; i++) { matrix[i][0] = col1.X(i+1); matrix[i][1] = col2.X(i+1); matrix[i][2] = col3.X(i+1); locrhs[i] = rhs.X(i+1); } for(int i=0; i<2; i++) { int pivot = i; double maxv = fabs(matrix[i][i]); for(int j=i+1; j<3; j++) if(fabs(matrix[j][i]) > maxv) { maxv = fabs(matrix[j][i]); pivot = j; } if(fabs(maxv) > 1e-40) { if(pivot != i) { swap(matrix[i][0],matrix[pivot][0]); swap(matrix[i][1],matrix[pivot][1]); swap(matrix[i][2],matrix[pivot][2]); swap(locrhs[i],locrhs[pivot]); } for(int j=i+1; j<3; j++) { double fac = matrix[j][i] / matrix[i][i]; for(int k=i+1; k<3; k++) matrix[j][k] -= fac*matrix[i][k]; locrhs[j] -= fac*locrhs[i]; } } else retval = 1; } if(fabs(matrix[2][2]) < 1e-40) retval = 1; if(retval != 0) return retval; for(int i=2; i>=0; i--) { double sum = locrhs[i]; for(int j=2; j>i; j--) sum -= matrix[i][j]*sol.X(j+1); sol.X(i+1) = sum/matrix[i][i]; } return 0; /* double det = Determinant (col1, col2, col3); if (fabs (det) < 1e-40) return 1; sol.X() = Determinant (rhs, col2, col3) / det; sol.Y() = Determinant (col1, rhs, col3) / det; sol.Z() = Determinant (col1, col2, rhs) / det; return 0; */ /* Vec3d cr; Cross (col1, col2, cr); double det = cr * col3; if (fabs (det) < 1e-40) return 1; if (fabs(cr.Z()) > 1e-12) { // solve for 3. component sol.Z() = (cr * rhs) / det; // 2x2 system for 1. and 2. component double res1 = rhs.X() - sol.Z() * col3.X(); double res2 = rhs.Y() - sol.Z() * col3.Y(); sol.X() = (col2.Y() * res1 - col2.X() * res2) / cr.Z(); sol.Y() = (col1.X() * res2 - col1.Y() * res1) / cr.Z(); } else { det = Determinant (col1, col2, col3); if (fabs (det) < 1e-40) return 1; sol.X() = Determinant (rhs, col2, col3) / det; sol.Y() = Determinant (col1, rhs, col3) / det; sol.Z() = Determinant (col1, col2, rhs) / det; } return 0; */ }
void Transpose (Vec3d & v1, Vec3d & v2, Vec3d & v3) { Swap (v1.Y(), v2.X()); Swap (v1.Z(), v3.X()); Swap (v2.Z(), v3.Y()); }
void GeomSearch3d :: Create() { INDEX i,j,k; if (reset) { const double hashelemsizefactor = 4; reset = 0; /* minext=Point3d(MAXDOUBLE, MAXDOUBLE, MAXDOUBLE); maxext=Point3d(MINDOUBLE, MINDOUBLE, MINDOUBLE); */ ElemMaxExt(minext, maxext, faces->Get(1).Face()); Point3d maxp, minp; Vec3d midext(0,0,0); //get max Extension of Frontfaces for (i = 1; i <= faces->Size(); i++) { ElemMaxExt(minp, maxp, faces->Get(i).Face()); MinCoords(minp, minext); MaxCoords(maxp, maxext); midext+=maxp-minp; } maxextreal = maxext; maxext = maxext + 1e-4 * (maxext - minext); midext*=1./faces->Size(); Vec3d boxext = maxext - minext; //delete old Hashtable: if (size.i1 != 0) { for (i = 1; i <= size.i1*size.i2*size.i3; i++) { delete hashtable.Get(i); } } size.i1 = int (boxext.X()/midext.X()/hashelemsizefactor+1); size.i2 = int (boxext.Y()/midext.Y()/hashelemsizefactor+1); size.i3 = int (boxext.Z()/midext.Z()/hashelemsizefactor+1); // PrintMessage (5, "hashsizes = ", size.i1, ", ", size.i2, ", ", size.i3); elemsize.X()=boxext.X()/size.i1; elemsize.Y()=boxext.Y()/size.i2; elemsize.Z()=boxext.Z()/size.i3; //create Hasharrays: hashtable.SetSize(size.i1*size.i2*size.i3); for (i = 1; i <= size.i1; i++) { for (j = 1; j <= size.i2; j++) { for (k = 1; k <= size.i3; k++) { INDEX ind=i+(j-1)*size.i1+(k-1)*size.i2*size.i1; hashtable.Elem(ind) = new Array <int> (); } } } } else { //Clear all Hash-Arrays for (i = 1; i <= size.i1; i++) { for (j = 1; j <= size.i2; j++) { for (k = 1; k <= size.i3; k++) { INDEX ind=i+(j-1)*size.i1+(k-1)*size.i2*size.i1; hashtable.Elem(ind)->SetSize(0); } } } } //Faces in Hashtable einfuegen: for (i = 1; i <= faces->Size(); i++) { AddElem(faces->Get(i).Face(),i); } }