double MQuadrangle::etaShapeMeasure() { double AR = 1;//(minEdge()/maxEdge()); SVector3 v01 (_v[1]->x()-_v[0]->x(),_v[1]->y()-_v[0]->y(),_v[1]->z()-_v[0]->z()); SVector3 v12 (_v[2]->x()-_v[1]->x(),_v[2]->y()-_v[1]->y(),_v[2]->z()-_v[1]->z()); SVector3 v23 (_v[3]->x()-_v[2]->x(),_v[3]->y()-_v[2]->y(),_v[3]->z()-_v[2]->z()); SVector3 v30 (_v[0]->x()-_v[3]->x(),_v[0]->y()-_v[3]->y(),_v[0]->z()-_v[3]->z()); SVector3 a = crossprod(v01,v12); SVector3 b = crossprod(v12,v23); SVector3 c = crossprod(v23,v30); SVector3 d = crossprod(v30,v01); double sign = 1.0; if (dot(a,b) < 0 || dot(a,c) < 0 || dot(a,d) < 0 )sign = -1; // FIXME ... // if (a.z() > 0 || b.z() > 0 || c.z() > 0 || d.z() > 0) sign = -1; double a1 = 180 * angle3Vertices(_v[0], _v[1], _v[2]) / M_PI; double a2 = 180 * angle3Vertices(_v[1], _v[2], _v[3]) / M_PI; double a3 = 180 * angle3Vertices(_v[2], _v[3], _v[0]) / M_PI; double a4 = 180 * angle3Vertices(_v[3], _v[0], _v[1]) / M_PI; a1 = std::min(180.,a1); a2 = std::min(180.,a2); a3 = std::min(180.,a3); a4 = std::min(180.,a4); double angle = fabs(90. - a1); angle = std::max(fabs(90. - a2),angle); angle = std::max(fabs(90. - a3),angle); angle = std::max(fabs(90. - a4),angle); return sign*(1.-angle/90) * AR; }
// returns the cross field as a pair of othogonal vectors (NOT in parametric coordinates, but real 3D coordinates) Pair<SVector3, SVector3> frameFieldBackgroundMesh2D::compute_crossfield_directions(double u, double v, double angle_current) { // get the unit normal at that point GFace *face = dynamic_cast<GFace*>(gf); if(!face) { Msg::Error("Entity is not a face in background mesh"); return Pair<SVector3,SVector3>(SVector3(), SVector3()); } Pair<SVector3, SVector3> der = face->firstDer(SPoint2(u,v)); SVector3 s1 = der.first(); SVector3 s2 = der.second(); SVector3 n = crossprod(s1,s2); n.normalize(); SVector3 basis_u = s1; basis_u.normalize(); SVector3 basis_v = crossprod(n,basis_u); // normalize vector t1 that is tangent to gf at uv SVector3 t1 = basis_u * cos(angle_current) + basis_v * sin(angle_current) ; t1.normalize(); // compute the second direction t2 and normalize (t1,t2,n) is the tangent frame SVector3 t2 = crossprod(n,t1); t2.normalize(); return Pair<SVector3,SVector3>(SVector3(t1[0],t1[1],t1[2]), SVector3(t2[0],t2[1],t2[2])); }
double oop3d ( struct xyz *vec1, struct xyz *vec2, struct xyz *vec3, struct xyz *vec4 ) { /* compute oop angle between plane formed by 4 atoms */ struct xyz plane1vec1, plane1vec2; struct xyz plane2vec1, plane2vec2; struct xyz normvec1, normvec2; double angle; /* plane1 is atoms 1, 2, 3 * plane2 is atoms 2, 3, 4 */ plane1vec1 = subtractxyz( vec1, vec2 ); plane1vec2 = subtractxyz( vec3, vec2 ); plane2vec1 = subtractxyz( vec4, vec3 ); /* compute normal vectors */ plane2vec2.x = plane1vec2.x * (-1); plane2vec2.y = plane1vec2.y * (-1); plane2vec2.z = plane1vec2.z * (-1); normvec1 = crossprod( plane1vec1, plane1vec2 ); normvec2 = crossprod( plane2vec1, plane2vec2 ); /* compute angle between normal vectors */ angle = angl2vecxyz( normvec1, normvec2 ) * 180.0 / MATH_PI; /* return angle */ return angle; }
int drizzle_helper_functions::line_intersect(LocationType x1, LocationType x2, LocationType y1, LocationType y2, LocationType *result){ LocationType dx, dy, d; dx = x2 - x1; dy = y2 - y1; d = x1 - y1; double dyx = crossprod(dy,dx); if(!dyx) return 0; dyx = crossprod(d, dx)/dyx; if(dyx <= 0 || dyx >= 1) return 0; result->mX = y1.mX + dyx * dy.mX; result->mY = y1.mY + dyx * dy.mY; return 1; }
void MSubTriangle::getGradShapeFunctions(double u, double v, double w, double s[][3], int order) const { if(!_orig) return; if (_orig->getDim()==getDim()) return _orig->getGradShapeFunctions(u, v, w, s, order); int nsf = getNumShapeFunctions(); double gradsuvw[1256][3]; _orig->getGradShapeFunctions(u, v, w, gradsuvw, order); // work in the parametric space of the parent element double jac[3][3]; double invjac[3][3]; _orig->getJacobian(u, v, w, jac); inv3x3(jac, invjac); MEdge edge[2]; edge[0] = getBaseElement()->getEdge(0); edge[1] = getBaseElement()->getEdge(1); SVector3 tang[2]; tang[0] = edge[0].tangent(); tang[1] = edge[1].tangent(); SVector3 vect = crossprod(tang[0],tang[1]); tang[1] = crossprod(vect,tang[0]); double gradxyz[3]; double projgradxyz[3]; for (int i=0; i<nsf; ++i) { // (i) get the cartesian coordinates of the gradient gradxyz[0] = invjac[0][0] * gradsuvw[i][0] + invjac[0][1] * gradsuvw[i][1] + invjac[0][2] * gradsuvw[i][2]; gradxyz[1] = invjac[1][0] * gradsuvw[i][0] + invjac[1][1] * gradsuvw[i][1] + invjac[1][2] * gradsuvw[i][2]; gradxyz[2] = invjac[2][0] * gradsuvw[i][0] + invjac[2][1] * gradsuvw[i][1] + invjac[2][2] * gradsuvw[i][2]; // (ii) projection of the gradient on edges in the cartesian space SVector3 grad(&gradxyz[0]); double prodscal[2]; prodscal[0] = dot(tang[0],grad); prodscal[1] = dot(tang[1],grad); projgradxyz[0] = prodscal[0]*tang[0].x() + prodscal[1]*tang[1].x(); projgradxyz[1] = prodscal[0]*tang[0].y() + prodscal[1]*tang[1].y(); projgradxyz[2] = prodscal[0]*tang[0].z() + prodscal[1]*tang[1].z(); // (iii) get the parametric coordinates of the projection in the parametric space of the parent element s[i][0] = jac[0][0] * projgradxyz[0] + jac[0][1] * projgradxyz[1] + jac[0][2] * projgradxyz[2]; s[i][1] = jac[1][0] * projgradxyz[0] + jac[1][1] * projgradxyz[1] + jac[1][2] * projgradxyz[2]; s[i][2] = jac[2][0] * projgradxyz[0] + jac[2][1] * projgradxyz[1] + jac[2][2] * projgradxyz[2]; } }
static void _myGetFaceRep(MQuadrangle *t, int num, double *x, double *y, double *z, SVector3 *n, int numSubEdges) { int io = num%2; int ix = (num/2)/numSubEdges; int iy = (num/2)%numSubEdges; const double d = 2. / numSubEdges; const double ox = -1. + d*ix; const double oy = -1. + d*iy; SPoint3 pnt1, pnt2, pnt3; double J1[3][3], J2[3][3], J3[3][3]; if (io == 0){ t->pnt(ox, oy, 0, pnt1); t->pnt(ox + d, oy, 0, pnt2); t->pnt(ox + d, oy + d, 0, pnt3); t->getJacobian(ox, oy, 0, J1); t->getJacobian(ox + d, oy, 0, J2); t->getJacobian(ox + d, oy + d, 0, J3); } else{ t->pnt(ox, oy, 0, pnt1); t->pnt(ox + d, oy + d, 0, pnt2); t->pnt(ox, oy + d, 0, pnt3); t->getJacobian(ox, oy, 0, J1); t->getJacobian(ox + d, oy + d, 0, J2); t->getJacobian(ox, oy + d, 0, J3); } { SVector3 d1(J1[0][0], J1[0][1], J1[0][2]); SVector3 d2(J1[1][0], J1[1][1], J1[1][2]); n[0] = crossprod(d1, d2); n[0].normalize(); } { SVector3 d1(J2[0][0], J2[0][1], J2[0][2]); SVector3 d2(J2[1][0], J2[1][1], J2[1][2]); n[1] = crossprod(d1, d2); n[1].normalize(); } { SVector3 d1(J3[0][0], J3[0][1], J3[0][2]); SVector3 d2(J3[1][0], J3[1][1], J3[1][2]); n[2] = crossprod(d1, d2); n[2].normalize(); } x[0] = pnt1.x(); x[1] = pnt2.x(); x[2] = pnt3.x(); y[0] = pnt1.y(); y[1] = pnt2.y(); y[2] = pnt3.y(); z[0] = pnt1.z(); z[1] = pnt2.z(); z[2] = pnt3.z(); }
// Group descent update void gd_gaussian(double *b, double *x, double *r, int g, int *K1, int n, int l, int p, const char *penalty, double lam1, double lam2, double gamma, SEXP df, double *a) { // Calculate z int K = K1[g+1] - K1[g]; double *z = Calloc(K, double); for (int j=K1[g]; j<K1[g+1]; j++) z[j-K1[g]] = crossprod(x, r, n, j)/n + a[j]; double z_norm = norm(z,K); // Update b double len; if (strcmp(penalty, "grLasso")==0) len = S(z_norm, lam1) / (1+lam2); if (strcmp(penalty, "grMCP")==0) len = F(z_norm, lam1, lam2, gamma); if (strcmp(penalty, "grSCAD")==0) len = Fs(z_norm, lam1, lam2, gamma); if (len != 0 | a[K1[g]] != 0) { // If necessary, update beta and r for (int j=K1[g]; j<K1[g+1]; j++) { b[l*p+j] = len * z[j-K1[g]] / z_norm; double shift = b[l*p+j]-a[j]; for (int i=0; i<n; i++) r[i] -= x[n*j+i] * shift; } } // Update df if (len > 0) REAL(df)[l] += K * len / z_norm; Free(z); }
void frameFieldBackgroundMesh2D::eval_crossfield(double u, double v, STensor3 &cf) { double quadAngle = angle(u,v); Pair<SVector3, SVector3> dirs = compute_crossfield_directions(u,v,quadAngle); SVector3 n = crossprod(dirs.first(),dirs.second()); for (int i=0; i<3; i++) { cf(i,0) = dirs.first()[i]; cf(i,1) = dirs.second()[i]; cf(i,2) = n[i]; } // SVector3 t1,t2,n; // GFace *face = dynamic_cast<GFace*>(gf); // Pair<SVector3, SVector3> der = face->firstDer(SPoint2(u,v)); // SVector3 s1 = der.first(); // SVector3 s2 = der.second(); // n = crossprod(s1,s2); // n.normalize(); // s1.normalize(); // s2=crossprod(n,s1); // t1 = s1 * cos(quadAngle) + s2 * sin(quadAngle) ; // t1.normalize(); // t2 = crossprod(n,t1); // for (int i=0;i<3;i++){ // cf(i,0) = t1[i]; // cf(i,1) = t2[i]; // cf(i,2) = n[i]; // } }
SMetric3 metric_based_on_surface_curvature(const GFace *gf, double u, double v, bool surface_isotropic, double d_normal , double d_tangent_max) { if (gf->geomType() == GEntity::Plane)return SMetric3(1.e-12); double cmax, cmin; SVector3 dirMax,dirMin; cmax = gf->curvatures(SPoint2(u, v),&dirMax, &dirMin, &cmax,&cmin); if (cmin == 0)cmin =1.e-12; if (cmax == 0)cmax =1.e-12; double lambda1 = ((2 * M_PI) /( fabs(cmin) * CTX::instance()->mesh.minCircPoints ) ); double lambda2 = ((2 * M_PI) /( fabs(cmax) * CTX::instance()->mesh.minCircPoints ) ); SVector3 Z = crossprod(dirMax,dirMin); if (surface_isotropic) lambda2 = lambda1 = std::min(lambda2,lambda1); dirMin.normalize(); dirMax.normalize(); Z.normalize(); lambda1 = std::max(lambda1, CTX::instance()->mesh.lcMin); lambda2 = std::max(lambda2, CTX::instance()->mesh.lcMin); lambda1 = std::min(lambda1, CTX::instance()->mesh.lcMax); lambda2 = std::min(lambda2, CTX::instance()->mesh.lcMax); double lambda3 = std::min(d_normal, CTX::instance()->mesh.lcMax); lambda3 = std::max(lambda3, CTX::instance()->mesh.lcMin); lambda1 = std::min(lambda1, d_tangent_max); lambda2 = std::min(lambda2, d_tangent_max); SMetric3 curvMetric (1./(lambda1*lambda1),1./(lambda2*lambda2), 1./(lambda3*lambda3), dirMin, dirMax, Z ); return curvMetric; }
T vec3<T>::angle(const vec3<T>& a, const vec3<T>& b, const vec3<T>& c) { // if |a|=0 or |b|=0 then angle is not defined. We return NAN in this case. T ang = vec3<T>::angle(a,b); return (crossprod(a,b)*c<0) ? T(2.*M_PI)-ang : ang; }
// Test intersection between sphere and triangle // Inspired by Christer Ericson, http://realtimecollisiondetection.net/blog/?p=103 static bool testTriSphereIntersect(SPoint3 A, SPoint3 B, SPoint3 C, const SPoint3& P, const double rr) { // Test if separating plane between sphere and triangle plane const SVector3 PA(P, A), AB(A, B), AC(A, C); const SVector3 V = crossprod(AB, AC); // Normal to triangle plane const double d = dot(PA, V); // Dist. from P to triangle plane times norm of V const double e = dot(V, V); // Norm of V if (d * d > rr * e) return false; // Test if separating plane between sphere and triangle plane // Test if separating plane between sphere and triangle vertices const SVector3 PB(P, B), PC(P, B); const double aa = dot(PA, PA), ab = dot(PA, PB), ac = dot(PA, PC); const double bb = dot(PB, PB), bc = dot(PB, PC), cc = dot(PC, PC); if ((aa > rr) & (ab > aa) & (ac > aa)) return false; // For each triangle vertex, separation if vertex is outside sphere if ((bb > rr) & (ab > bb) & (bc > bb)) return false; // and P on opposite side to other two triangle vertices w.r.t if ((cc > rr) & (ac > cc) & (bc > cc)) return false; // plane of normal (vertex-P) through vertex // Test if separating plane between sphere and triangle edges const SVector3 BC(B, C); const double d1 = ab - aa, d2 = bc - bb, d3 = ac - cc; const double e1 = dot(AB, AB), e2 = dot(BC, BC), e3 = dot(AC, AC); const SVector3 PQ1 = PA * e1 - d1 * AB; // Q1 projection of P on line (AB) const SVector3 PQ2 = PB * e2 - d2 * BC; // Q2 projection of P on line (BC) const SVector3 PQ3 = PC * e3 + d3 * AC; // Q3 projection of P on line (AC) const SVector3 PQC = PC * e1 - PQ1; const SVector3 PQA = PA * e2 - PQ2; const SVector3 PQB = PB * e3 - PQ3; if ((dot(PQ1, PQ1) > rr * e1 * e1) & (dot(PQ1, PQC) > 0)) return false; // For A, separation if Q lies outside the sphere and if P and C if ((dot(PQ2, PQ2) > rr * e2 * e2) & (dot(PQ2, PQA) > 0)) return false; // are on opposite sides of plane through AB with normal PQ if ((dot(PQ3, PQ3) > rr * e3 * e3) & (dot(PQ3, PQB) > 0)) return false; // Same for other two vertices // Return true (intersection) if no separation at all return true; }
bool SOrientedBoundingBox::intersects(SOrientedBoundingBox &obb) const { SVector3 collide_axes[15]; for(int i = 0; i < 3; i++) { collide_axes[i] = getAxis(i); collide_axes[i + 3] = obb.getAxis(i); } SVector3 sizes[2]; sizes[0] = getSize(); sizes[1] = obb.getSize(); for(std::size_t i = 0; i < 3; i++) { for(std::size_t j = 3; j < 6; j++) { collide_axes[3 * i + j + 3] = crossprod(collide_axes[i], collide_axes[j]); } } SVector3 T = obb.getCenter() - getCenter(); for(std::size_t i = 0; i < 15; i++) { double val = 0.0; for(std::size_t j = 0; j < 6; j++) { val += 0.5 * (sizes[j < 3 ? 0 : 1])(j % 3) * std::abs(dot(collide_axes[j], collide_axes[i])); } if(std::abs(dot(collide_axes[i], T)) > val) { return false; } } return true; }
/* arc distance (along great circle) */ double arcdist(const Coord &x, const Coord &y) { // et angles aigus non orientes double n = norm(crossprod(x, y)); if (n>1.) n=1. ; double a = asin(n); if (squaredist(x,y) > 2.0) a = M_PI - a; return a; }
static void computeLevelset(GModel *gm, cartesianBox<double> &box) { // tolerance for desambiguation const double tol = box.getLC() * 1.e-12; std::vector<SPoint3> nodes; std::vector<int> indices; for (cartesianBox<double>::valIter it = box.nodalValuesBegin(); it != box.nodalValuesEnd(); ++it){ nodes.push_back(box.getNodeCoordinates(it->first)); indices.push_back(it->first); } Msg::Info(" %d nodes in the grid at level %d", (int)nodes.size(), box.getLevel()); std::vector<double> dist, localdist; std::vector<SPoint3> dummy; for (GModel::fiter fit = gm->firstFace(); fit != gm->lastFace(); fit++){ for (int i = 0; i < (*fit)->stl_triangles.size(); i += 3){ int i1 = (*fit)->stl_triangles[i]; int i2 = (*fit)->stl_triangles[i + 1]; int i3 = (*fit)->stl_triangles[i + 2]; GPoint p1 = (*fit)->point((*fit)->stl_vertices[i1]); GPoint p2 = (*fit)->point((*fit)->stl_vertices[i2]); GPoint p3 = (*fit)->point((*fit)->stl_vertices[i3]); SPoint2 p = ((*fit)->stl_vertices[i1] + (*fit)->stl_vertices[i2] + (*fit)->stl_vertices[i3]) * 0.33333333; SVector3 N = (*fit)->normal(p); SPoint3 P1(p1.x(), p1.y(), p1.z()); SPoint3 P2(p2.x(), p2.y(), p2.z()); SPoint3 P3(p3.x(), p3.y(), p3.z()); SVector3 NN(crossprod(P2 - P1, P3 - P1)); if (dot(NN, N) > 0) signedDistancesPointsTriangle(localdist, dummy, nodes, P1, P2, P3); else signedDistancesPointsTriangle(localdist, dummy, nodes, P2, P1, P3); if(dist.empty()) dist = localdist; else{ for (unsigned int j = 0; j < localdist.size(); j++){ // FIXME: if there is an ambiguity assume we are inside (to // avoid holes in the structure). This is definitely just a // hack, as it could create pockets of matter outside the // structure... if(dist[j] * localdist[j] < 0 && fabs(fabs(dist[j]) - fabs(localdist[j])) < tol){ dist[j] = std::max(dist[j], localdist[j]); } else{ dist[j] = (fabs(dist[j]) < fabs(localdist[j])) ? dist[j] : localdist[j]; } } } } } for (unsigned int j = 0; j < dist.size(); j++) box.setNodalValue(indices[j], dist[j]); if(box.getChildBox()) computeLevelset(gm, *box.getChildBox()); }
double angle3Vertices(const MVertex *p1, const MVertex *p2, const MVertex *p3) { SVector3 a(p1->x() - p2->x(), p1->y() - p2->y(), p1->z() - p2->z()); SVector3 b(p3->x() - p2->x(), p3->y() - p2->y(), p3->z() - p2->z()); SVector3 c = crossprod(a, b); double sinA = c.norm(); double cosA = dot(a, b); return atan2 (sinA, cosA); }
SMetric3 Centerline::metricBasedOnSurfaceCurvature(SVector3 dirMin, SVector3 dirMax, double cmin, double cmax, double h_n, double h_t1, double h_t2) { SVector3 dirNorm = crossprod(dirMax,dirMin); SMetric3 curvMetric (1./(h_t1*h_t1),1./(h_t2*h_t2),1./(h_n*h_n), dirMin, dirMax, dirNorm); return curvMetric; }
double sqDistPointSegment(const SPoint3 &p, const SPoint3 &s0, const SPoint3 &s1) { SVector3 d(s1 - s0); SVector3 d0(p - s0); SVector3 d1(p - s1); double dn2 = crossprod(d, d0).normSq(); double dt2 = std::max(0., std::max(-dot(d, d0), dot(d, d1))); dt2 *= dt2; return (dt2 + dn2) / d.normSq(); }
double GEdge::curvature(double par) const { SVector3 d1 = firstDer(par); SVector3 d2 = secondDer(par); double one_over_norm = 1. / norm(d1); SVector3 cross_prod = crossprod(d1,d2); return ( norm(cross_prod) * one_over_norm * one_over_norm * one_over_norm ); }
int badAngle(Pt a, Pt b, Pt c) { Pt p, q; p.x = a.x - b.x; p.y = a.y - b.y; q.x = c.x - b.x; q.y = c.y - b.y; if (crossprod(p.x,p.y,q.x,q.y) >= 0) return 1; return 0; }
// return oriented vector angle in range [-pi..pi], pole must be orthogonal to a and b double vectAngle(const Coord &a, const Coord &b, const Coord &pole) { double nab = 1./(norm(a)*norm(b)) ; Coord a_cross_b=crossprod(a, b)*nab ; double sinVect ; if (scalarprod(a_cross_b, pole) >= 0) sinVect=norm(a_cross_b) ; else sinVect=-norm(a_cross_b) ; double cosVect=scalarprod(a,b)*nab ; return atan2(sinVect,cosVect) ; }
void Pair::computeBoundaryQuadric() { // We'd need to compute a quadric which is perpendicular to // the one face attached to this pair. float A, B, C, D; U32 SetCtx = 0; Face *face = (Face *) m_Faces.Begin(SetCtx); face->planeEquation (A, B, C, D); IV3D normal, perpNormal, pairVector; normal.x = A; normal.y = B; normal.z = C; normalize3D (&normal); subtract3D ((IV3D*)&v1->v, (IV3D*)&v2->v, (IV3D*)&pairVector); normalize3D ((IV3D*)&pairVector); crossprod ((IV3D*)&normal, (IV3D*)&pairVector, (IV3D*)&perpNormal); // Form an equation of a plane perpendicular to the pair and its 1 face: A = perpNormal.x; B = perpNormal.y; C = perpNormal.z; // Solve for D: //converting all float type data to double to improve calculation accuracy. D = (float)(-((double)A*(double)v1->v.X()) - ((double)B*(double)v1->v.Y()) - ((double)C*(double)v1->v.Z())); float q[16]; // q is row major: q[0]=A*A; q[1]=A*B; q[2]=A*C; q[3]=A*D; q[4]=A*B; q[5]=B*B; q[6]=B*C; q[7]=B*D; q[8]=A*C; q[9]=B*C; q[10]=C*C; q[11]=C*D; q[12]=A*D; q[13]=B*D; q[14]=C*D; q[15]=D*D; m_pQuadric = new Matrix4x4(q); // Weight this quadric..."We *really* want to hang on to the boundaries...": (*m_pQuadric) *= BOUNDARY_QUADRIC_WEIGHT; }
double epi_line (double xl, double yl, Exterior Ex1, Interior I1, Glass G1, Exterior Ex2, Interior I2, Glass G2) { int i,j; double m2, vect1[3], vect2[3], vect3[3], nk[3], n2[3]; double p1l[3], K2[3], k2[3], D2t[3][3]; void crossprod (double a[3], double b[3], double c[3]); /* base O1 -> O2 */ vect1[0] = Ex2.x0 - Ex1.x0; vect1[1] = Ex2.y0 - Ex1.y0; vect1[2] = Ex2.z0 - Ex1.z0; /* coordinates of arbitrary point P1 in image1 */ p1l[0] = xl; p1l[1] = yl; p1l[2] = -I1.cc; /* beam O1 -> P in space */ matmul (vect2, Ex1.dm, p1l, 3,3,1); /* normale to epipolar plane */ crossprod (vect1,vect2,nk); /* normale to image2 */ vect3[0] = 0; vect3[1] = 0; vect3[2] = -I2.cc; /* normale to image 2, in space */ matmul (n2, Ex2.dm, vect3, 3,3,1); /* epipolar line in image2, in space */ crossprod (nk,n2,K2); /* epipolar line in image2 */ for (i=0; i<3; i++) for (j=0; j<3; j++) D2t[i][j] = Ex2.dm[j][i]; matmul (k2, D2t, K2, 3,3,1); m2 = k2[1] / k2[0]; return (m2); }
SMetric3 buildMetricTangentToCurve(SVector3 &t, double l_t, double l_n) { if (l_t == 0.0) return SMetric3(1.e-22); SVector3 a; if (fabs(t(0)) <= fabs(t(1)) && fabs(t(0)) <= fabs(t(2))){ a = SVector3(1,0,0); } else if (fabs(t(1)) <= fabs(t(0)) && fabs(t(1)) <= fabs(t(2))){ a = SVector3(0,1,0); } else{ a = SVector3(0,0,1); } SVector3 b = crossprod (t,a); SVector3 c = crossprod (b,t); b.normalize(); c.normalize(); t.normalize(); SMetric3 Metric (1./(l_t*l_t),1./(l_n*l_n),1./(l_n*l_n),t,b,c); // printf("bmttc %g %g %g %g %g\n",l_t,l_n,Metric(0,0),Metric(0,1),Metric(1,1)); return Metric; }
SMetric3 buildMetricTangentToSurface(SVector3 &t1, SVector3 &t2, double l_t1, double l_t2, double l_n) { t1.normalize(); t2.normalize(); SVector3 n = crossprod (t1,t2); n.normalize(); l_t1 = std::max(l_t1, CTX::instance()->mesh.lcMin); l_t2 = std::max(l_t2, CTX::instance()->mesh.lcMin); l_t1 = std::min(l_t1, CTX::instance()->mesh.lcMax); l_t2 = std::min(l_t2, CTX::instance()->mesh.lcMax); SMetric3 Metric (1./(l_t1*l_t1),1./(l_t2*l_t2),1./(l_n*l_n),t1,t2,n); return Metric; }
vec3<T>& vec3<T>::rotate(const vec3<T>& r) { T phi = norm(r); if (phi != 0) { // part of vector which is parallel to r vec3<T> par(r*(*this)/(r*r) * r); // part of vector which is perpendicular to r vec3<T> perp(*this - par); // rotation direction, size of perp vec3<T> rotdir(norm(perp) * normalized(crossprod(r,perp))); *this = par + cos(phi)*perp + sin(phi)*rotdir; } return *this; }
// Scan for violations in strong set int check_strong_set(int *e2, int *e, double *xTr, double *X, double *r, int *K1, int *K, double lam, int n, int J) { int violations = 0; for (int g = 0; g < J; g++) { if (e[g] == 0 && e2[g] == 1) { double *z = Calloc(K[g], double); for (int j = K1[g]; j < K1[g+1]; j++) { z[j-K1[g]] = crossprod(X, r, n, j) / n; } xTr[g] = norm(z, K[g]); if (xTr[g] > lam * sqrt(K[g])) { e[g] = 1; violations++; } Free(z); } }
int edge_normal (const MVertex *const vertex, const int zoneIndex, const GEdge *const gEdge, const CCon::FaceVector<MZoneBoundary<2>::GlobalVertexData<MEdge>::FaceDataB> &faces, SVector3 &boNormal, const int onlyFace = -1) { double par=0.0; // Note: const_cast used to match MVertex.cpp interface if(!reparamMeshVertexOnEdge(const_cast<MVertex*>(vertex), gEdge, par)) return 1; const SVector3 tangent(gEdge->firstDer(par)); // Tangent to the boundary face SPoint3 interior(0., 0., 0.); // An interior point SVector3 meshPlaneNormal(0.); // This normal is perpendicular to the // plane of the mesh // The interior point and mesh plane normal are computed from all elements in // the zone. int cFace = 0; int iFace = 0; int nFace = faces.size(); if ( onlyFace >= 0 ) { iFace = onlyFace; nFace = onlyFace + 1; } for(; iFace != nFace; ++iFace) { if(faces[iFace].zoneIndex == zoneIndex) { ++cFace; interior += faces[iFace].parentElement->barycenter(); // Make sure all the planes go in the same direction //**Required? SVector3 mpnt = faces[iFace].parentElement->getFace(0).normal(); if(dot(mpnt, meshPlaneNormal) < 0.) mpnt.negate(); meshPlaneNormal += mpnt; } } interior /= cFace; // Normal to the boundary edge (but unknown direction) boNormal = crossprod(tangent, meshPlaneNormal); boNormal.normalize(); // Direction vector from vertex to interior (inwards). The normal should // point in the same direction. if(dot(boNormal, SVector3(vertex->point(), interior)) < 0.) boNormal.negate(); return 0; }
double* angleaccel(double omega[3], struct copter quad) {/*Computes angular acceleration vector of quad*/ double tau[3]; double Iomega[3]; int i; for (i=1;i<4;i++){ Iomega[i] = quad.I[i]*omega[i]; } C = crossprod(omega,Iomega); /*Compute Torques*/ torques(tau,quad); ddtomega[1] = 1/quad.I[1]*(tau[1]); return ddtomega; }
static void recorditem(GLfloat * n1, GLfloat * n2, GLfloat * n3, GLenum shadeType) { GLfloat q0[3], q1[3]; DIFF3(n1, n2, q0); DIFF3(n2, n3, q1); crossprod(q0, q1, q1); normalize(q1); glBegin(shadeType); glNormal3fv(q1); glVertex3fv(n1); glVertex3fv(n2); glVertex3fv(n3); glEnd(); }
static void pentagon(int a, int b, int c, int d, int e, GLenum shadeType) { GLfloat n0[3], d1[3], d2[3]; DIFF3(dodec[a], dodec[b], d1); DIFF3(dodec[b], dodec[c], d2); crossprod(d1, d2, n0); normalize(n0); glBegin(shadeType); glNormal3fv(n0); glVertex3fv(&dodec[a][0]); glVertex3fv(&dodec[b][0]); glVertex3fv(&dodec[c][0]); glVertex3fv(&dodec[d][0]); glVertex3fv(&dodec[e][0]); glEnd(); }