//---------------------------------------------------------------------------------------------------
// Public: PerfectMatch
// Purpose: Handles the degenerate case where v and this voxel
// have the same vertices.
//---------------------------------------------------------------------------------------------------
bool CXDMVoxel::PerfectMatch( const CXDMVoxel & v ) const
{
if ( v.nGeneration != this->nGeneration )
return false;
if ( v.bVoxelPointsUp != this->bVoxelPointsUp )
return false;
Float fError = fSideLength / Float( 10.0 );
Bool pVertexMatch[3];
for( Int i = 0; i < 3; i ++ )
{
pVertexMatch[i] = false;
for( Int j = 0; j < 3; j ++ )
{
SVector3 oDisp = v.pVertex[i] - this->pVertex[j];
if ( oDisp.GetLength() < fError )
pVertexMatch[i] = true;
}
}
return ( pVertexMatch[0] && pVertexMatch[1] && pVertexMatch[2] );
}
//---------------------------------------------------------------------------------------------------
//
// Private: CheckAxis
//
// Return true if the axis perpendicular to u = v2 - v1 is a separating axis (i.e., no overlap)
// Therefore, we project to this perpendicular axis
//
// Currently only works with 2D objects on the x-y plane
//
//
//---------------------------------------------------------------------------------------------------
bool CXDMVoxel::CheckSeparatingAxis( const CXDMVoxel & oVoxel, const CXDMVoxel & oTestVoxel,
const SVector3 &oV1, const SVector3 &oV2) const
{
SVector3 oDir = oV2 - oV1;
oDir.Normalize();
// rotate 90 degress y <- x, x <- -y
Float tmp = oDir.m_fY;
oDir.m_fY = oDir.m_fX;
oDir.m_fX = - tmp;
Float fMin = MAX_FLOAT;
Float fMax = MIN_FLOAT;
for ( int i = 0; i < 3; i ++ )
{
Float fAxisProjection = Dot( oTestVoxel.pVertex[i], oDir );
if ( fAxisProjection < fMin )
fMin = fAxisProjection;
if ( fAxisProjection > fMax )
fMax = fAxisProjection;
}
for ( int i = 0; i < 3; i ++ )
{
Float fVertex = Dot( oVoxel.pVertex[i], oDir );
if( fVertex <= fMax && fVertex >= fMin ) // if intersecting
{
return false;
}
}
return true;
}
// 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 Filler::improvement(GEntity* ge,MElementOctree* octree,SPoint3 point,double h1,SVector3 direction){
double x,y,z;
double average;
double h2;
double coeffA,coeffB;
x = point.x() + h1*direction.x();
y = point.y() + h1*direction.y();
z = point.z() + h1*direction.z();
if(inside_domain(octree,x,y,z)){
h2 = get_size(x,y,z);
}
else h2 = h1;
coeffA = 1.0;
coeffB = 0.16;
if(h2>h1){
average = coeffA*h1 + (1.0-coeffA)*h2;
}
else{
average = coeffB*h1 + (1.0-coeffB)*h2;
}
return average;
}
double distMaxStraight(MElement *el)
{
const polynomialBasis *lagrange = (polynomialBasis*)el->getFunctionSpace();
const polynomialBasis *lagrange1 = (polynomialBasis*)el->getFunctionSpace(1);
int nV = lagrange->points.size1();
int nV1 = lagrange1->points.size1();
int dim = lagrange1->dimension;
SPoint3 sxyz[256];
for (int i = 0; i < nV1; ++i) {
sxyz[i] = el->getVertex(i)->point();
}
for (int i = nV1; i < nV; ++i) {
double f[256];
lagrange1->f(lagrange->points(i, 0), lagrange->points(i, 1),
dim < 3 ? 0 : lagrange->points(i, 2), f);
for (int j = 0; j < nV1; ++j)
sxyz[i] += sxyz[j] * f[j];
}
double maxdx = 0.0;
for (int iV = nV1; iV < nV; iV++) {
SVector3 d = el->getVertex(iV)->point()-sxyz[iV];
double dx = d.norm();
if (dx > maxdx) maxdx = dx;
}
return maxdx;
}
SMetric3 max_edge_curvature_metric(const GVertex *gv)
{
SMetric3 val (1.e-12);
std::list<GEdge*> l_edges = gv->edges();
for (std::list<GEdge*>::const_iterator ite = l_edges.begin();
ite != l_edges.end(); ++ite){
GEdge *_myGEdge = *ite;
Range<double> range = _myGEdge->parBounds(0);
SMetric3 cc;
if (gv == _myGEdge->getBeginVertex()) {
SVector3 t = _myGEdge->firstDer(range.low());
t.normalize();
double l_t = ((2 * M_PI) /( fabs(_myGEdge->curvature(range.low()))
* CTX::instance()->mesh.minCircPoints ));
double l_n = 1.e12;
cc = buildMetricTangentToCurve(t,l_t,l_n);
}
else {
SVector3 t = _myGEdge->firstDer(range.high());
t.normalize();
double l_t = ((2 * M_PI) /( fabs(_myGEdge->curvature(range.high()))
* CTX::instance()->mesh.minCircPoints ));
double l_n = 1.e12;
cc = buildMetricTangentToCurve(t,l_t,l_n);
}
val = intersection(val,cc);
}
return val;
}
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;
}
SMetric3 max_edge_curvature_metric(const GEdge *ge, double u)
{
SVector3 t = ge->firstDer(u);
t.normalize();
double l_t = ((2 * M_PI) /( fabs(ge->curvature(u))
* CTX::instance()->mesh.minCircPoints ));
double l_n = 1.e12;
return buildMetricTangentToCurve(t,l_t,l_n);
}
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);
}
bool GEdge::XYZToU(const double X, const double Y, const double Z,
double &u, const double relax) const
{
const int MaxIter = 25;
const int NumInitGuess = 11;
double err;//, err2;
int iter;
Range<double> uu = parBounds(0);
double uMin = uu.low();
double uMax = uu.high();
SVector3 Q(X, Y, Z), P;
double init[NumInitGuess];
for (int i = 0; i < NumInitGuess; i++)
init[i] = uMin + (uMax - uMin) / (NumInitGuess - 1) * i;
for(int i = 0; i < NumInitGuess; i++){
u = init[i];
double uNew = u;
//err2 = 1.0;
iter = 1;
SVector3 dPQ = P - Q;
err = dPQ.norm();
if (err < 1.e-8 * CTX::instance()->lc) return true;
while(iter++ < MaxIter && err > 1e-8 * CTX::instance()->lc) {
SVector3 der = firstDer(u);
uNew = u - relax * dot(dPQ,der) / dot(der,der);
uNew = std::min(uMax,std::max(uMin,uNew));
P = position(uNew);
dPQ = P - Q;
err = dPQ.norm();
//err2 = fabs(uNew - u);
u = uNew;
}
if (err < 1e-8 * CTX::instance()->lc) return true;
}
if(relax > 1.e-2) {
// Msg::Info("point %g %g %g on edge %d : Relaxation factor = %g",
// Q.x(), Q.y(), Q.z(), 0.75 * relax);
return XYZToU(Q.x(), Q.y(), Q.z(), u, 0.75 * relax);
}
// Msg::Error("Could not converge reparametrisation of point (%e,%e,%e) on edge %d",
// Q.x(), Q.y(), Q.z(), tag());
return false;
}
//--------------------------------------------------------------------------------------------------------
//
// GetAlignRotation
// -- Given *UNIT vector* oRef, oVec, return a matrix that rotates oVec -> oRef
//
//--------------------------------------------------------------------------------------------------------
SMatrix3x3 GetAlignmentRotation( const SVector3 &oVecDir, const SVector3 &oRefDir )
{
SVector3 oAxis = Cross( oVecDir, oRefDir );
Float fAngle = acos( Dot( oVecDir, oRefDir ) );
oAxis.Normalize();
SMatrix3x3 oRes;
oRes.BuildRotationAboutAxis( oAxis, fAngle );
return oRes;
}
static void CalculateOrthoShadow( const CCameraEntity& cam, Light& light )
{
SVector3 target = gCasterBounds.getCenter();
SVector3 size = gCasterBounds.getMax() - gCasterBounds.getMin();
float radius = size.length() * 0.5f;
// figure out the light camera matrix that encloses whole scene
light.mWorldMat.spaceFromAxisZ();
light.mWorldMat.getOrigin() = target - light.mWorldMat.getAxisZ() * radius;
light.setOrthoParams( radius*2, radius*2, radius*0.1f, radius*2 );
light.setOntoRenderContext();
}
static void _relocateVertexOfPyramid(MVertex *ver,
const std::vector<MElement *> <,
double relax)
{
if(ver->onWhat()->dim() != 3) return;
double x = 0.0, y = 0.0, z = 0.0;
int N = 0;
MElement *pyramid = NULL;
for(std::size_t i = 0; i < lt.size(); i++) {
double XCG = 0.0, YCG = 0.0, ZCG = 0.0;
if(lt[i]->getNumVertices() == 5)
pyramid = lt[i];
else {
for(std::size_t j = 0; j < lt[i]->getNumVertices(); j++) {
XCG += lt[i]->getVertex(j)->x();
YCG += lt[i]->getVertex(j)->y();
ZCG += lt[i]->getVertex(j)->z();
}
x += XCG;
y += YCG;
z += ZCG;
N += lt[i]->getNumVertices();
}
}
x /= N;
y /= N;
z /= N;
if(pyramid) {
MFace q = pyramid->getFace(4);
double A = q.approximateArea();
SVector3 n = q.normal();
n.normalize();
SPoint3 c = q.barycenter();
SVector3 d(x - c.x(), y - c.y(), z - c.z());
if(dot(n, d) < 0) n = n * (-1.0);
double H = .5 * sqrt(fabs(A));
double XOPT = c.x() + relax * H * n.x();
double YOPT = c.y() + relax * H * n.y();
double ZOPT = c.z() + relax * H * n.z();
double FULL_MOVE_OBJ =
objective_function(1.0, ver, XOPT, YOPT, ZOPT, lt, true);
// printf("relax %g obj %g\n",relax,FULL_MOVE_OBJ);
if(FULL_MOVE_OBJ > 0.1) {
ver->x() = XOPT;
ver->y() = YOPT;
ver->z() = ZOPT;
return;
}
}
}
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;
}
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;
}
Bool Equivilent( const CSymmetryType & oSymOps, const SVector3 &v1, const SVector3 &v2, Float fError )
{
const vector<SMatrix3x3> &vOperatorList = oSymOps.GetOperatorList();
for( Size_Type i = 0; i < vOperatorList.size(); i ++ )
{
SVector3 oTmp = vOperatorList[i] * v1;
SVector3 oDiff = oTmp - v2;
Float fDiff = oDiff.GetLength();
if( fDiff < fError )
{
return true;
}
}
return false;
}
double taylorDistanceSq1D(const GradientBasis *gb, const fullMatrix<double> &nodesXYZ,
const std::vector<SVector3> &tanCAD)
{
const int nV = nodesXYZ.size1();
fullMatrix<double> dxyzdX(nV, 3);
gb->getGradientsFromNodes(nodesXYZ, &dxyzdX, 0, 0);
// const double dx = nodesXYZ(1, 0) - nodesXYZ(0, 0), dy = nodesXYZ(1, 1) - nodesXYZ(0, 1),
// dz = nodesXYZ(1, 2) - nodesXYZ(0, 2), h = 0.5*sqrt(dx*dx+dy*dy+dz*dz)/double(nV-1);
double distSq = 0.;
for (int i=0; i<nV; i++) {
SVector3 tanMesh(dxyzdX(i, 0), dxyzdX(i, 1), dxyzdX(i, 2));
const double h = 0.25*tanMesh.normalize(); // Half of "local edge length"
SVector3 diff = (dot(tanCAD[i], tanMesh) > 0) ?
tanCAD[i] - tanMesh : tanCAD[i] + tanMesh;
distSq += h*h*diff.normSq();
}
return distSq;
}
double taylorDistanceSq2D(const GradientBasis *gb, const fullMatrix<double> &nodesXYZ,
const std::vector<SVector3> &normCAD)
{
const int nV = nodesXYZ.size1();
fullMatrix<double> dxyzdX(nV, 3), dxyzdY(nV, 3);
gb->getGradientsFromNodes(nodesXYZ, &dxyzdX, &dxyzdY, 0);
double distSq = 0.;
for (int i=0; i<nV; i++) {
const double nz = dxyzdX(i, 0) * dxyzdY(i, 1) - dxyzdX(i, 1) * dxyzdY(i, 0);
const double ny = -dxyzdX(i, 0) * dxyzdY(i, 2) + dxyzdX(i, 2) * dxyzdY(i, 0);
const double nx = dxyzdX(i, 1) * dxyzdY(i, 2) - dxyzdX(i, 2) * dxyzdY(i, 1);
SVector3 normMesh(nx, ny, nz);
const double h = 0.25*sqrt(normMesh.normalize()); // Half of sqrt of "local area", to be adjusted w.r.t. el. type?
SVector3 diff = (dot(normCAD[i], normMesh) > 0) ?
normCAD[i] - normMesh : normCAD[i] + normMesh;
distSq += h*h*diff.normSq();
}
return distSq;
}
void Centerline::buildKdTree()
{
FILE * f = Fopen("myPOINTS.pos","w");
fprintf(f, "View \"\"{\n");
int nbPL = 3; //10 points per line
//int nbNodes = (lines.size()+1) + (nbPL*lines.size());
int nbNodes = (colorp.size()) + (nbPL*lines.size());
ANNpointArray nodes = annAllocPts(nbNodes, 3);
int ind = 0;
std::map<MVertex*, int>::iterator itp = colorp.begin();
while (itp != colorp.end()){
MVertex *v = itp->first;
nodes[ind][0] = v->x();
nodes[ind][1] = v->y();
nodes[ind][2] = v->z();
itp++; ind++;
}
for(unsigned int k = 0; k < lines.size(); ++k){
MVertex *v0 = lines[k]->getVertex(0);
MVertex *v1 = lines[k]->getVertex(1);
SVector3 P0(v0->x(),v0->y(), v0->z());
SVector3 P1(v1->x(),v1->y(), v1->z());
for (int j= 1; j < nbPL+1; j++){
double inc = (double)j/(double)(nbPL+1);
SVector3 Pj = P0+inc*(P1-P0);
nodes[ind][0] = Pj.x();
nodes[ind][1] = Pj.y();
nodes[ind][2] = Pj.z();
ind++;
}
}
kdtree = new ANNkd_tree(nodes, nbNodes, 3);
for(int i = 0; i < nbNodes; ++i){
fprintf(f, "SP(%g,%g,%g){%g};\n",
nodes[i][0], nodes[i][1],nodes[i][2],1.0);
}
fprintf(f,"};\n");
fclose(f);
}
void highOrderTools::computeMetricInfo(GFace *gf,
MElement *e,
fullMatrix<double> &J,
fullMatrix<double> &JT,
fullVector<double> &D)
{
int nbNodes = e->getNumVertices();
// printf("ELEMENT --\n");
for (int j = 0; j < nbNodes; j++){
SPoint2 param;
reparamMeshVertexOnFace(e->getVertex(j), gf, param);
// printf("%g %g vs %g %g %g\n",param.x(),param.y(),
// e->getVertex(j)->x(),e->getVertex(j)->y(),e->getVertex(j)->z());
Pair<SVector3,SVector3> der = gf->firstDer(param);
int XJ = j;
int YJ = j + nbNodes;
int ZJ = j + 2 * nbNodes;
int UJ = j;
int VJ = j + nbNodes;
J(XJ,UJ) = der.first().x();
J(YJ,UJ) = der.first().y();
J(ZJ,UJ) = der.first().z();
J(XJ,VJ) = der.second().x();
J(YJ,VJ) = der.second().y();
J(ZJ,VJ) = der.second().z();
JT(UJ,XJ) = der.first().x();
JT(UJ,YJ) = der.first().y();
JT(UJ,ZJ) = der.first().z();
JT(VJ,XJ) = der.second().x();
JT(VJ,YJ) = der.second().y();
JT(VJ,ZJ) = der.second().z();
SVector3 ss = getSSL(e->getVertex(j));
GPoint gp = gf->point(param);
D(XJ) = (gp.x() - ss.x());
D(YJ) = (gp.y() - ss.y());
D(ZJ) = (gp.z() - ss.z());
}
}
void MSubLine::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 = _orig->getNumShapeFunctions();
double gradsuvw[1256][3];
_orig->getGradShapeFunctions(u, v, w, gradsuvw, order);
double jac[3][3];
double invjac[3][3];
_orig->getJacobian(u, v, w, jac);
inv3x3(jac, invjac);
MEdge edge = getBaseElement()->getEdge(0);
SVector3 tang = edge.tangent();
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 = dot(tang,grad);
projgradxyz[0] = prodscal * tang.x();
projgradxyz[1] = prodscal * tang.y();
projgradxyz[2] = prodscal * tang.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];
}
}
//---------------------------------------------------------------------------------------------------
// MinDistance calculation
// -- clearly this could be optimized. Let's get it right first.
//
// Precondition: All of the Z must be exactly the same!
//
//---------------------------------------------------------------------------------------------------
bool CXDMVoxel::Connected( const SVector3 & oV1, const SVector3 & oV2,
const SVector3 & p, Float fError )
{
SVector3 oDist_Top = p - oV2;
SVector3 oDist = p - oV1;
oDist_Top.m_fZ = 0; // zeroing out the out of plane components
oDist.m_fZ = 0;
// check for vertex overlap
if ( ( fabs( oDist_Top.m_fX ) < fError
&& fabs( oDist_Top.m_fY ) < fError
)
||
( fabs( oDist.m_fX ) < fError
&& fabs( oDist.m_fY ) < fError
)
)
{
return true;
}
SVector3 oLine = oV2 - oV1;
oLine.m_fZ = 0;
Float fLineLength = oLine.GetLength();
DEBUG_ASSERT( fLineLength > std::numeric_limits<Float>::epsilon(),
"CXDMVoxel:: MinDistance: numerical limit reached for this floaing point numbers\n" );
SVector3 oLineDir = oLine / fLineLength;
Float fProjected = Dot( oDist, oLineDir );
if( fProjected < 0 || fProjected > fLineLength )
return false;
SVector3 oPerpDir( -oLineDir.m_fY, oLineDir.m_fX, oLineDir.m_fZ );
Float fPerpDist = Dot( oDist, oPerpDir );
return ( fabs(fPerpDist) < fError );
//----------------------------------------
// -- Also correct, but I'm afraid of dividing and sqrt-ing very small numbers
// SVector3 oDistPerp = oDist - fProjected * oLineDir;
// return ( oDistPerp.GetLength() < fError );
//----------------------------------------
}
double goldenSectionSearch(const GEdge *ge, const SPoint3 &q, double x1,
double x2, double x3, double tau)
{
// Create a new possible center in the area between x2 and x3, closer to x2
double x4 = x2 + GOLDEN2 * (x3 - x2);
// Evaluate termination criterion
if (fabs(x3 - x1) < tau * (fabs(x2) + fabs(x4)))
return (x3 + x1) / 2;
const SVector3 dp4 = q - ge->position(x4);
const SVector3 dp2 = q - ge->position(x2);
const double d4 = dp4.norm();
const double d2 = dp2.norm();
if (d4 < d2)
return goldenSectionSearch(ge, q, x2, x4, x3, tau);
else
return goldenSectionSearch(ge,q , x4, x2, x1, tau);
}
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;
}
static void gTryPositionViewer( const SVector3& newPos, const SVector3& finalPos )
{
// if we're in megamap mode - allow it!
if( !gAppSettings.followMode ) {
gViewer.getOrigin() = newPos;
return;
}
// else, if the final needed position is very far away - allow it
SVector3 toFinal = gViewer.getOrigin() - finalPos;
const float FAR_AWAY = 20.0f;
if( toFinal.lengthSq() > FAR_AWAY * FAR_AWAY ) {
gViewer.getOrigin() = newPos;
return;
}
// else, check the new position for collisions
const CLevelMesh& level = CGameInfo::getInstance().getLevelMesh();
SVector3 targetPos = newPos;
level.fitSphere( targetPos, VIEWER_R );
gViewer.getOrigin() = targetPos;
}
GPoint GEdge::closestPoint(const SPoint3 &q, double &t) const
{
// printf("looking for closest point in curve %d to point %g %g\n",tag(),q.x(),q.y());
const int nbSamples = 100;
Range<double> interval = parBounds(0);
double tMin = std::min(interval.high(), interval.low());
double tMax = std::max(interval.high(), interval.low());
double DMIN = 1.e22;
double topt = tMin;
const double DT = (tMax - tMin) / (nbSamples - 1.) ;
for (int i = 0; i < nbSamples; i++){
t = tMin + i * DT;
const SVector3 dp = q - position(t);
const double D = dp.norm();
if (D < DMIN) {
topt = t;
DMIN = D;
}
}
// printf("parameter %g as an initial guess (dist = %g)\n",topt,DMIN);
if (topt == tMin)
t = goldenSectionSearch (this, q, topt, topt + DT/2, topt + DT, 1.e-9);
else if (topt == tMax)
t = goldenSectionSearch (this, q, topt - DT, topt - DT/2 , topt, 1.e-9);
else
t = goldenSectionSearch (this, q, topt - DT, topt, topt + DT, 1.e-9);
const SVector3 dp = q - position(t);
// const double D = dp.norm();
// printf("after golden section parameter %g (dist = %g)\n",t,D);
return point(t);
}
void CActorEntity::update( float timeAlpha )
{
bool alive = mGameEntity->isAlive();
if( alive ) {
// fade out the outline
float dt = CSystemTimer::getInstance().getDeltaTimeS();
mOutlineTTL -= dt;
if( mOutlineTTL < 0.0f )
mOutlineTTL = 0.0f;
SMatrix4x4& m = mWorldMat;
SVector3 pos = samplePos( timeAlpha );
SVector3 dir = samplePos( timeAlpha + 0.1f ) - pos;
if( dir.lengthSq() < 1.0e-3f )
dir = m.getAxisZ();
else
dir.normalize();
if( mGameEntity->getType() == ENTITY_BLOCKER ) {
double tt = CSystemTimer::getInstance().getTimeS();
D3DXMatrixRotationY( &m, tt * 0.2f );
m.getOrigin() = pos;
m.getOrigin().y += sinf( tt * 0.6f ) * 0.2f;
} else {
m.getOrigin() = pos;
m.getAxisZ() = dir;
m.getAxisZ().y *= 0.2f;
m.getAxisZ().normalize();
m.getAxisY().set( 0, 1, 0 );
m.getAxisX() = m.getAxisY().cross( m.getAxisZ() );
m.getAxisX().normalize();
m.getAxisY() = m.getAxisZ().cross( m.getAxisX() );
}
} else {
mOutlineTTL = 0.0f;
}
}
//------------------------------
//
// ReadFundamentalZoneFile
//
//------------------------------
bool ReadFundamentalZoneFile( vector<SVector3> & vFZEulerAngleList,
const string & sFilename )
{
char *pBuffer = NULL;
Size_Type nBufSize = 0;
vector< vector<string> > vsTokens;
pBuffer = ReadFileToBuf(nBufSize, sFilename);
if(pBuffer == NULL){
return false;
}
string sBuffStr( pBuffer, nBufSize );
GeneralLib::Tokenize( vsTokens, sBuffStr, ",# \t\n");
for(Size_Type i = 0; i < vsTokens.size(); i ++){
RUNTIME_ASSERT( (vsTokens[i].size() >= 3),
"[ReadFundamentalZoneFile] ERROR: Invalid Euler Angles\n");
Float fPhi = DEGREE_TO_RADIAN( atof( vsTokens[i][0].c_str() ) );
Float fTheta = DEGREE_TO_RADIAN( atof( vsTokens[i][1].c_str() ) );
Float fPsi = DEGREE_TO_RADIAN( atof( vsTokens[i][2].c_str() ) );
SVector3 newAngles;
newAngles.Set( fPhi, fTheta, fPsi );
vFZEulerAngleList.push_back( newAngles );
}
delete [] pBuffer;
return true;
}
void elasticitySolver::setLagrangeMultipliers(int phys, double tau,
const SVector3 &d, int tag,
simpleFunction<double> *f)
{
LagrangeMultiplierField field;
field._tau = tau;
field._tag = tag;
field._f = f;
field._d = d.unit();
field.g = new groupOfElements(
_dim - 1, phys); // LM applied on entity of dimension = dim-1!
LagrangeMultiplierFields.push_back(field);
LagrangeMultiplierSpaces.push_back(
new ScalarLagrangeFunctionSpaceOfElement(tag));
}
static void gInitiallyPlaceViewer()
{
// TBD
/*
const CGameInfo& gi = CGameInfo::getInstance();
const CGameMap& gmap = gi.getGameMap();
const CGameReplay& replay = gi.getReplay();
const CLevelMesh& level = gi.getLevelMesh();
const CReplayEntity::SState& pl1st = replay.getEntity( replay.getPlayer(0).entityAI ).getTurnState( 0 );
int posx = pl1st.posx;
int posy = pl1st.posy;
const int XOFF[4] = { 2, 2, -2, -2 };
const int YOFF[4] = { 2, -2, 2, -2 };
const float ROT[4] = { D3DX_PI/4*7, D3DX_PI/4*5, D3DX_PI/4*1, D3DX_PI/4*3 };
*/
gViewer.identify();
/*
gViewer.getOrigin().set( posx, VIEWER_Y, -posy );
for( int i = 0; i < 4; ++i ) {
int px = posx+XOFF[i];
int py = posy+YOFF[i];
//if( gmap.isBlood( gmap.getCell(px,py).type ) ) {
if( !level.collideSphere( SVector3(px,VIEWER_Y,-py), VIEWER_R ) ) {
D3DXMatrixRotationY( &gViewer, ROT[i] );
gViewer.getOrigin().set( px, VIEWER_Y, -py );
break;
}
}
*/
gLastViewerValidPos = gViewer.getOrigin();
gViewerVel.set(0,0,0);
gViewerZVel.set(0,0,0);
}