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();
}
