double MinDistLP2 (const Point2d & lp1, const Point2d & lp2, const Point2d & p)
{
Vec2d v(lp1, lp2);
Vec2d vlp(lp1, p);
// dist(lam) = \| vlp \|^2 - 2 lam (v1p, v) + lam^2 \| v \|^2
// lam = (v * vlp) / (v * v);
// if (lam < 0) lam = 0;
// if (lam > 1) lam = 1;
double num = v*vlp;
double den = v*v;
if (num <= 0)
return Dist2 (lp1, p);
if (num >= den)
return Dist2 (lp2, p);
if (den > 0)
{
return vlp.Length2() - num * num /den;
}
else
return vlp.Length2();
}
double Dist2(const Line2d & g, const Line2d & h )
{
double dd = 0.0, d1,d2,d3,d4;
Point2d cp = CrossPoint(g,h);
if ( Parallel(g,h) || !IsOnLine(g,cp) || !IsOnLine(h,cp) )
{
d1 = Dist2(g.P1(),h.P1());
d2 = Dist2(g.P1(),h.P2());
d3 = Dist2(g.P2(),h.P1());
d4 = Dist2(g.P2(),h.P2());
if (d1<d2) d2 = d1;
if (d3<d4) d4 = d3;
dd = ( d2 < d4 ) ? d2 : d4;
}
return dd;
}
// greedy procedure to make sure that a perfect matching exists:
// 1. construct a matching among existing edges
// (greedy procedure: pick a node, check whether there are edges leading
// to unmatched nodes, if there are pick the edge with the smallest length).
// 2. take remaining unmatched nodes, construct kd-tree for them,
// assign an ordering to nodes (last visited time during left-most depth-first search),
// add edges between consecutive nodes (2*i,2*i+1)
void GeomPerfectMatching::CompleteInitialMatching() {
if (options.verbose)
printf("adding edges to make sure that a perfect matching exists...");
PointId p, q;
Edge* e;
double len, len_min;
int unmatched_num = 0, edge_num0 = edge_num;
// construct greedy matching
for (p = 0; p < node_num; p++) {
if (nodes[p].is_marked)
continue;
q = -1;
for (e = nodes[p].first[0]; e; e = e->next[0]) {
if (nodes[e->head[0]].is_marked)
continue;
len = Dist2(p, e->head[0]);
if (q < 0 || len_min > len) {
q = e->head[0];
len_min = len;
}
}
if (q >= 0) {
nodes[p].is_marked = nodes[q].is_marked = 1;
} else
unmatched_num++;
}
if (unmatched_num == 0) {
for (p = 0; p < node_num; p++)
nodes[p].is_marked = 0;
return;
}
//printf("%d unmatched\n", unmatched_num);
REAL* unmatched_coords = new REAL[unmatched_num * DIM];
int* rev_mapping = new int[unmatched_num];
unmatched_num = 0;
for (p = 0; p < node_num; p++) {
if (nodes[p].is_marked)
nodes[p].is_marked = 0;
else {
memcpy(unmatched_coords + unmatched_num * DIM, coords + p * DIM, DIM * sizeof(REAL));
rev_mapping[unmatched_num++] = p;
}
}
GPMKDTree* kd_tree = new GPMKDTree(DIM, unmatched_num, unmatched_coords, this);
kd_tree->AddPerfectMatching(rev_mapping);
delete kd_tree;
delete[] unmatched_coords;
delete[] rev_mapping;
if (options.verbose)
printf("done (%d edges)\n", edge_num - edge_num0);
}
/****************************************************************************
Calculates RMSD between two sets of vectors without roto-translation
*****************************************************************************/
double DumbRMSD2(struct vector *a, struct vector *b, int n)
{
int i;
double rmsd2=0;
for(i=0;i<n;i++)
{
rmsd2 += Dist2(a[i],b[i]);
}
return rmsd2/n;
}
double CalcTetBadness (const Point3d & p1, const Point3d & p2,
const Point3d & p3, const Point3d & p4, double h)
{
double vol, l, ll, lll, ll1, ll2, ll3, ll4, ll5, ll6;
double err;
Vec3d v1 (p1, p2);
Vec3d v2 (p1, p3);
Vec3d v3 (p1, p4);
vol = -Determinant (v1, v2, v3) / 6;
ll1 = v1.Length2();
ll2 = v2.Length2();
ll3 = v3.Length2();
ll4 = Dist2 (p2, p3);
ll5 = Dist2 (p2, p4);
ll6 = Dist2 (p3, p4);
ll = ll1 + ll2 + ll3 + ll4 + ll5 + ll6;
l = sqrt (ll);
lll = l * ll;
if (vol <= 1e-24 * lll)
return 1e24;
err = 0.0080187537 * lll / vol; // sqrt(216) / (6^4 * sqrt(2))
if (h > 0)
err += ll / (h * h) +
h * h * ( 1 / ll1 + 1 / ll2 + 1 / ll3 +
1 / ll4 + 1 / ll5 + 1 / ll6 ) - 12;
if (teterrpow == 2)
return err*err;
return pow (err, teterrpow);
}
/* compute the frame-based similarity of two vector */
float SCalcDTWDistance(float* Qry, int lenQry, float* Lib, int lenLib){
if (lenQry>QRY_LEN)
printf("Error: Query Length too large\n");
if (lenLib>TPL_LEN)
printf("Error: Template Length too large\n");
int r,t;
int iMin,iMax;
float minDist(10000);
for(t=0;t<lenQry;t++){
iMin = (int)ceil(0.5*t);
iMax = (int)floor(0.5*t+lenLib-0.5*lenQry+0.5);
if(iMax>=lenLib)
iMax = lenLib - 1;
for(r=0;r<lenLib;r++){
if(iMin<=r && r<=iMax){
if(t==0 || r==0 || t==1 || r==1)
pMatrix[t][r] = 0.0;
else{
float Dis = Dist2(Qry[t],Lib[r]);
pMatrix[t][r] = Min3(pMatrix[t-1][r-1]+Dis,pMatrix[t-1][r-2]+3*Dis,pMatrix[t-2][r-1]+3*Dis);
}
}else{
pMatrix[t][r] = 10000;
}
}
}
int LibY;
minDist = GetMinDis(pMatrix[lenQry-1],lenLib,&LibY);
return minDist;
}
////////////////////////////////////////////////////////////////////////////////////////
// Point In Standard Circle (True/False)
//
// Returns true if the given point is within the Circle
// _____
// / \
// / \
// | Circle |
// | |
// \ Pt /
// \_______/
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::PtInCircle(const CVec4 &Circle, float Radius) const
{
return (Dist2(Circle)<(Radius*Radius));
}
void GeomSearch3d :: GetLocals(Array<MiniElement2d> & locfaces, Array<INDEX> & findex,
INDEX fstind, const Point3d& p0, double xh)
{
hashcount++;
Point3d minp, maxp, midp;
minp=p0-Vec3d(xh,xh,xh); //lay cube over sphere
maxp=p0+Vec3d(xh,xh,xh);
MaxCoords(minext,minp); //cube may not be out of hash-region
MinCoords(maxextreal,maxp);
int cluster = faces->Get(fstind).Cluster();
int sx = int((minp.X()-minext.X())/elemsize.X()+1.);
int ex = int((maxp.X()-minext.X())/elemsize.X()+1.);
int sy = int((minp.Y()-minext.Y())/elemsize.Y()+1.);
int ey = int((maxp.Y()-minext.Y())/elemsize.Y()+1.);
int sz = int((minp.Z()-minext.Z())/elemsize.Z()+1.);
int ez = int((maxp.Z()-minext.Z())/elemsize.Z()+1.);
int ix,iy,iz,i,k;
int cnt1 = 0; // test, how efficient hashtable is
int cnt2 = 0;
int cnt3 = 0;
for (ix = sx; ix <= ex; ix++)
{
for (iy = sy; iy <= ey; iy++)
{
for (iz = sz; iz <= ez; iz++)
{
INDEX ind=ix+(iy-1)*size.i1+(iz-1)*size.i2*size.i1;
//go through all elements in one hash area
const Array <int> & area = *hashtable.Elem(ind);
for (k = 1; k <= area.Size(); k++)
{
cnt2++;
i = area.Get(k);
if (faces->Get(i).Cluster() == cluster &&
faces->Get(i).Valid() &&
faces->Get(i).HashValue() != hashcount &&
i != fstind)
{
cnt1++;
const MiniElement2d & face = faces->Get(i).Face();
const Point3d & p1 = (*points)[face.PNum(1)].P();
const Point3d & p2 = (*points)[face.PNum(2)].P();
const Point3d & p3 = (*points)[face.PNum(3)].P();
midp = Center (p1, p2, p3);
// if (Dist2 (midp, p0) <= xh*xh)
if((Dist2 (p1, p0) <= xh*xh) ||
(Dist2 (p2, p0) <= xh*xh) ||
(Dist2 (p3, p0) <= xh*xh) ||
(Dist2 (midp, p0) <= xh*xh) ) // by Jochen Wild
{
cnt3++;
locfaces.Append(faces->Get(i).Face());
findex.Append(i);
faces->Elem(i).SetHashValue(hashcount);
}
}
}
}
}
}
/*
if (faces->Size() != 0 && hashcount % 200 == 0)
{
(*mycout) << "n.o.f= " << faces->Size();
(*mycout) << ", n.o.lf= " << locfaces.Size();
(*mycout) << ", hashf= " << (double)cnt2/(double)faces->Size();
(*mycout) << " (" << (double)cnt1/(double)faces->Size();
(*mycout) << ", " << (double)cnt3/(double)faces->Size() << ")" << endl;
}
*/
}
int IntersectTriangleTriangle (const Point<3> ** tri1, const Point<3> ** tri2)
{
int i, j;
double diam = Dist (*tri1[0], *tri1[1]);
double epsrel = 1e-8;
double eps = diam * epsrel;
double eps2 = eps * eps;
int cnt = 0;
/*
int tri1pi[3];
int tri2pi[3];
*/
// int tri1p1 = -1;
/// int tri1p2 = -1;
// int tri2p1 = -1;
// int tri2p2 = -1;
// int tri1p3, tri2p3;
/*
for (i = 0; i < 3; i++)
tri1pi[i] = -1;
*/
for (i = 0; i <= 2; i++)
{
// tri2pi[i] = -1;
for (j = 0; j <= 2; j++)
{
if (Dist2 (*tri1[j], *tri2[i]) < eps2)
{
// tri2pi[i] = j;
// tri1pi[j] = i;
cnt++;
// tri1p2 = tri1p1;
// tri1p1 = j;
// tri2p2 = tri2p1;
// tri2p1 = i;
break;
}
}
}
switch (cnt)
{
case 0:
{
const Point<3> * line[2];
for (i = 0; i <= 2; i++)
{
line[0] = tri2[i];
line[1] = tri2[(i+1)%3];
if (IntersectTriangleLine (tri1, &line[0]))
{
(*testout) << "int1, line = " << *line[0] << " - " << *line[1] << endl;
return 1;
}
}
for (i = 0; i <= 2; i++)
{
line[0] = tri1[i];
line[1] = tri1[(i+1)%3];
if (IntersectTriangleLine (tri2, &line[0]))
{
(*testout) << "int2, line = " << *line[0] << " - " << *line[1] << endl;
return 1;
}
}
break;
}
default:
return 0;
}
return 0;
}
//==========================================================================
//
//
//
//==========================================================================
void GLSprite::Draw(int pass)
{
if (pass!=GLPASS_PLAIN && pass != GLPASS_ALL && pass!=GLPASS_TRANSLUCENT) return;
bool additivefog = false;
int rel = extralight*gl_weaponlight;
if (pass==GLPASS_TRANSLUCENT)
{
// The translucent pass requires special setup for the various modes.
// Brightmaps will only be used when doing regular drawing ops and having no fog
if (!gl_isBlack(Colormap.FadeColor) || level.flags&LEVEL_HASFADETABLE ||
RenderStyle.BlendOp != STYLEOP_Add)
{
gl_RenderState.EnableBrightmap(false);
}
gl_SetRenderStyle(RenderStyle, false,
// The rest of the needed checks are done inside gl_SetRenderStyle
trans > 1.f - FLT_EPSILON && gl_usecolorblending && gl_fixedcolormap < CM_FIRSTSPECIALCOLORMAP && actor &&
fullbright && gltexture && !gltexture->GetTransparent());
if (hw_styleflags == STYLEHW_NoAlphaTest)
{
gl_RenderState.EnableAlphaTest(false);
}
else
{
gl_RenderState.AlphaFunc(GL_GEQUAL,trans*gl_mask_sprite_threshold);
}
if (RenderStyle.BlendOp == STYLEOP_Fuzz)
{
float fuzzalpha=0.44f;
float minalpha=0.1f;
// fog + fuzz don't work well without some fiddling with the alpha value!
if (!gl_isBlack(Colormap.FadeColor))
{
float xcamera=FIXED2FLOAT(viewx);
float ycamera=FIXED2FLOAT(viewy);
float dist=Dist2(xcamera,ycamera, x,y);
if (!Colormap.FadeColor.a) Colormap.FadeColor.a=clamp<int>(255-lightlevel,60,255);
// this value was determined by trial and error and is scale dependent!
float factor=0.05f+exp(-Colormap.FadeColor.a*dist/62500.f);
fuzzalpha*=factor;
minalpha*=factor;
}
gl_RenderState.AlphaFunc(GL_GEQUAL,minalpha*gl_mask_sprite_threshold);
gl.Color4f(0.2f,0.2f,0.2f,fuzzalpha);
additivefog = true;
}
else if (RenderStyle.BlendOp == STYLEOP_Add && RenderStyle.DestAlpha == STYLEALPHA_One)
{
additivefog = true;
}
}
if (RenderStyle.BlendOp!=STYLEOP_Fuzz)
{
if (actor)
{
lightlevel = gl_SetSpriteLighting(RenderStyle, actor, lightlevel, rel, &Colormap, ThingColor, trans,
fullbright || gl_fixedcolormap >= CM_FIRSTSPECIALCOLORMAP, false);
}
else if (particle)
{
if (gl_light_particles)
{
lightlevel = gl_SetSpriteLight(particle, lightlevel, rel, &Colormap, trans, ThingColor);
}
else
{
gl_SetColor(lightlevel, rel, &Colormap, trans, ThingColor);
}
}
else return;
}
if (gl_isBlack(Colormap.FadeColor)) foglevel=lightlevel;
if (RenderStyle.Flags & STYLEF_FadeToBlack)
{
Colormap.FadeColor=0;
additivefog = true;
}
if (RenderStyle.Flags & STYLEF_InvertOverlay)
{
Colormap.FadeColor = Colormap.FadeColor.InverseColor();
additivefog=false;
}
gl_SetFog(foglevel, rel, &Colormap, additivefog);
if (gltexture) gltexture->BindPatch(Colormap.colormap,translation);
else if (!modelframe) gl_RenderState.EnableTexture(false);
if (!modelframe)
{
// [BB] Billboard stuff
const bool drawWithXYBillboard = ( !(actor && actor->renderflags & RF_FORCEYBILLBOARD)
//&& GLRenderer->mViewActor != NULL
&& (gl_billboard_mode == 1 || (actor && actor->renderflags & RF_FORCEXYBILLBOARD )) );
gl_RenderState.Apply();
gl.Begin(GL_TRIANGLE_STRIP);
if ( drawWithXYBillboard )
{
// Rotate the sprite about the vector starting at the center of the sprite
// triangle strip and with direction orthogonal to where the player is looking
// in the x/y plane.
float xcenter = (x1+x2)*0.5;
float ycenter = (y1+y2)*0.5;
float zcenter = (z1+z2)*0.5;
float angleRad = DEG2RAD(270. - float(GLRenderer->mAngles.Yaw));
Matrix3x4 mat;
mat.MakeIdentity();
mat.Translate( xcenter, zcenter, ycenter);
mat.Rotate(-sin(angleRad), 0, cos(angleRad), -GLRenderer->mAngles.Pitch);
mat.Translate( -xcenter, -zcenter, -ycenter);
Vector v1 = mat * Vector(x1,z1,y1);
Vector v2 = mat * Vector(x2,z1,y2);
Vector v3 = mat * Vector(x1,z2,y1);
Vector v4 = mat * Vector(x2,z2,y2);
if (gltexture)
{
gl.TexCoord2f(ul, vt);
gl.Vertex3fv(&v1[0]);
gl.TexCoord2f(ur, vt);
gl.Vertex3fv(&v2[0]);
gl.TexCoord2f(ul, vb);
gl.Vertex3fv(&v3[0]);
gl.TexCoord2f(ur, vb);
gl.Vertex3fv(&v4[0]);
}
else // Particle
{
gl.Vertex3fv(&v1[0]);
gl.Vertex3fv(&v2[0]);
gl.Vertex3fv(&v3[0]);
gl.Vertex3fv(&v4[0]);
}
}
else
{
if (gltexture)
{
gl.TexCoord2f(ul, vt);
gl.Vertex3f(x1, z1, y1);
gl.TexCoord2f(ur, vt);
gl.Vertex3f(x2, z1, y2);
gl.TexCoord2f(ul, vb);
gl.Vertex3f(x1, z2, y1);
gl.TexCoord2f(ur, vb);
gl.Vertex3f(x2, z2, y2);
}
else // Particle
{
gl.Vertex3f(x1, z1, y1);
gl.Vertex3f(x2, z1, y2);
gl.Vertex3f(x1, z2, y1);
gl.Vertex3f(x2, z2, y2);
}
}
gl.End();
}
else
{
gl_RenderModel(this, Colormap.colormap);
}
if (pass==GLPASS_TRANSLUCENT)
{
gl_RenderState.EnableBrightmap(true);
gl_RenderState.BlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
gl_RenderState.BlendEquation(GL_FUNC_ADD);
gl_RenderState.SetTextureMode(TM_MODULATE);
// [BB] Restore the alpha test after drawing a smooth particle.
if (hw_styleflags == STYLEHW_NoAlphaTest)
{
gl_RenderState.EnableAlphaTest(true);
}
else
{
gl_RenderState.AlphaFunc(GL_GEQUAL,gl_mask_sprite_threshold);
}
}
gl_RenderState.EnableTexture(true);
}
double MinDistTP2 (const Point3d & tp1, const Point3d & tp2,
const Point3d & tp3, const Point3d & p)
{
double lam1, lam2;
double res;
LocalCoordinates (Vec3d (tp1, tp2), Vec3d (tp1, tp3),
Vec3d (tp1, p), lam1, lam2);
int in1 = lam1 >= 0;
int in2 = lam2 >= 0;
int in3 = lam1+lam2 <= 1;
if (in1 && in2 && in3)
{
Point3d pp = tp1 + lam1 * Vec3d(tp1, tp2) + lam2 * Vec3d (tp1, tp3);
res = Dist2 (p, pp);
}
else
{
res = Dist2 (tp1, p);
if (!in1)
{
double hv = MinDistLP2 (tp1, tp3, p);
if (hv < res) res = hv;
}
if (!in2)
{
double hv = MinDistLP2 (tp1, tp2, p);
if (hv < res) res = hv;
}
if (!in3)
{
double hv = MinDistLP2 (tp2, tp3, p);
if (hv < res) res = hv;
}
/*
double d1 = MinDistLP2 (tp1, tp2, p);
double d2 = MinDistLP2 (tp1, tp3, p);
double d3 = MinDistLP2 (tp2, tp3, p);
res = min3 (d1, d2, d3);
*/
}
return res;
Vec3d pp1(tp1, p);
Vec3d v1(tp1, tp2), v2(tp1, tp3);
double c = pp1.Length2();
double cx = -2 * (pp1 * v1);
double cy = -2 * (pp1 * v2);
double cxx = v1.Length2();
double cxy = 2 * (v1 * v2);
double cyy = v2.Length2();
QuadraticPolynomial2V pol (-c, -cx, -cy, -cxx, -cxy, -cyy);
double res2 = - pol.MaxUnitTriangle ();
if (fabs (res - res2) > 1e-8)
cout << "res and res2 differ: " << res << " != " << res2 << endl;
return res2;
}
int IntersectTetTriangle (const Point<3> ** tet, const Point<3> ** tri,
const int * tetpi, const int * tripi)
{
int i, j;
double diam = Dist (*tri[0], *tri[1]);
double epsrel = 1e-8;
double eps = diam * epsrel;
double eps2 = eps * eps;
int cnt = 0;
int tetp1 = -1, tetp2 = -1;
int trip1 = -1, trip2 = -1;
int tetp3, tetp4, trip3;
/*
for (i = 0; i < 4; i++)
loctetpi[i] = -1;
*/
if (!tetpi)
{
for (i = 0; i <= 2; i++)
{
// loctripi[i] = -1;
for (j = 0; j <= 3; j++)
{
if (Dist2 (*tet[j], *tri[i]) < eps2)
{
// loctripi[i] = j;
// loctetpi[j] = i;
cnt++;
tetp2 = tetp1;
tetp1 = j;
trip2 = trip1;
trip1 = i;
break;
}
}
}
}
else
{
for (i = 0; i <= 2; i++)
{
// loctripi[i] = -1;
for (j = 0; j <= 3; j++)
{
if (tetpi[j] == tripi[i])
{
// loctripi[i] = j;
// loctetpi[j] = i;
cnt++;
tetp2 = tetp1;
tetp1 = j;
trip2 = trip1;
trip1 = i;
break;
}
}
}
}
// (*testout) << "cnt = " << cnt << endl;
// (*testout) << "tet-trig inters, cnt = " << cnt << endl;
// cnt .. number of common points
switch (cnt)
{
case 0:
{
Vec3d no, n;
int inpi[3];
// check, if some trigpoint is in tet:
for (j = 0; j < 3; j++)
inpi[j] = 1;
for (i = 1; i <= 4; i++)
{
int pi1 = i % 4;
int pi2 = (i+1) % 4;
int pi3 = (i+2) % 4;
int pi4 = (i+3) % 4;
Vec3d v1 (*tet[pi1], *tet[pi2]);
Vec3d v2 (*tet[pi1], *tet[pi3]);
Vec3d v3 (*tet[pi1], *tet[pi4]);
Cross (v1, v2, n);
// n /= n.Length();
double nl = n.Length();
if (v3 * n > 0)
n *= -1;
int outeri = 1;
for (j = 0; j < 3; j++)
{
Vec3d v(*tet[pi1], *tri[j]);
if ( v * n < eps * nl)
outeri = 0;
else
inpi[j] = 0;
}
if (outeri)
return 0;
}
if (inpi[0] || inpi[1] || inpi[2])
{
return 1;
}
// check, if some tet edge intersects triangle:
const Point<3> * line[2], *tetf[3];
for (i = 0; i <= 2; i++)
for (j = i+1; j <= 3; j++)
{
line[0] = tet[i];
line[1] = tet[j];
if (IntersectTriangleLine (tri, &line[0]))
return 1;
}
// check, if triangle line intersects tet face:
for (i = 0; i <= 3; i++)
{
for (j = 0; j <= 2; j++)
tetf[j] = tet[(i+j) % 4];
for (j = 0; j <= 2; j++)
{
line[0] = tri[j];
line[1] = tri[(j+1) % 3];
if (IntersectTriangleLine (&tetf[0], &line[0]))
return 1;
}
}
return 0;
//GH break;
}
case 1:
{
trip2 = 0;
while (trip2 == trip1)
trip2++;
trip3 = 3 - trip1 - trip2;
tetp2 = 0;
while (tetp2 == tetp1)
tetp2++;
tetp3 = 0;
while (tetp3 == tetp1 || tetp3 == tetp2)
tetp3++;
tetp4 = 6 - tetp1 - tetp2 - tetp3;
Vec3d vtri1 = *tri[trip2] - *tri[trip1];
Vec3d vtri2 = *tri[trip3] - *tri[trip1];
Vec3d ntri;
Cross (vtri1, vtri2, ntri);
// tri durch tet ?
// fehlt noch
// test 3 tet-faces:
for (i = 1; i <= 3; i++)
{
Vec3d vtet1, vtet2;
switch (i)
{
case 1:
{
vtet1 = *tet[tetp2] - *tet[tetp1];
vtet2 = *tet[tetp3] - *tet[tetp1];
break;
}
case 2:
{
vtet1 = *tet[tetp3] - *tet[tetp1];
vtet2 = *tet[tetp4] - *tet[tetp1];
break;
}
case 3:
{
vtet1 = *tet[tetp4] - *tet[tetp1];
vtet2 = *tet[tetp2] - *tet[tetp1];
break;
}
}
Vec3d ntet;
Cross (vtet1, vtet2, ntet);
Vec3d crline = Cross (ntri, ntet);
double lcrline = crline.Length();
if (lcrline < eps * eps * eps * eps) // new change !
continue;
if (vtri1 * crline + vtri2 * crline < 0)
crline *= -1;
crline /= lcrline;
double lam1, lam2, lam3, lam4;
LocalCoordinates (vtri1, vtri2, crline, lam1, lam2);
LocalCoordinates (vtet1, vtet2, crline, lam3, lam4);
if (lam1 > -epsrel && lam2 > -epsrel &&
lam3 > -epsrel && lam4 > -epsrel)
{
/*
(*testout) << "lcrline = " << lcrline
<< " eps = " << eps << " diam = " << diam << endl;
(*testout) << "hit, cnt == 1 "
<< "lam1,2,3,4 = " << lam1 << ", "
<< lam2 << ", " << lam3 << ", " << lam4
<< "\n";
*/
return 1;
}
}
return 0;
//GH break;
}
case 2:
{
// common edge
tetp3 = 0;
while (tetp3 == tetp1 || tetp3 == tetp2)
tetp3++;
tetp4 = 6 - tetp1 - tetp2 - tetp3;
trip3 = 3 - trip1 - trip2;
// (*testout) << "trip1,2,3 = " << trip1 << ", " << trip2 << ", " << trip3 << endl;
// (*testout) << "tetp1,2,3,4 = " << tetp1 << ", " << tetp2
// << ", " << tetp3 << ", " << tetp4 << endl;
Vec3d vtri = *tri[trip3] - *tri[trip1];
Vec3d vtet1 = *tet[tetp3] - *tri[trip1];
Vec3d vtet2 = *tet[tetp4] - *tri[trip1];
Vec3d n = *tri[trip2] - *tri[trip1];
n /= n.Length();
vtet1 -= (n * vtet1) * n;
vtet2 -= (n * vtet2) * n;
double lam1, lam2;
LocalCoordinates (vtet1, vtet2, vtri, lam1, lam2);
if (lam1 < -epsrel || lam2 < -epsrel)
return 0;
else
{
/*
(*testout) << "vtet1 = " << vtet1 << endl;
(*testout) << "vtet2 = " << vtet2 << endl;
(*testout) << "vtri = " << vtri << endl;
(*testout) << "lam1 = " << lam1 << " lam2 = " << lam2 << endl;
(*testout) << (lam1 * (vtet1 * vtet1) + lam2 * (vtet1 * vtet2))
<< " = " << (vtet1 * vtri) << endl;
(*testout) << (lam1 * (vtet1 * vtet2) + lam2 * (vtet2 * vtet2))
<< " = " << (vtet2 * vtri) << endl;
(*testout) << "tet = ";
for (j = 0; j < 4; j++)
(*testout) << (*tet[j]) << " ";
(*testout) << endl;
(*testout) << "tri = ";
for (j = 0; j < 3; j++)
(*testout) << (*tri[j]) << " ";
(*testout) << endl;
(*testout) << "hit, cnt == 2" << endl;
*/
return 1;
}
break;
}
case 3:
{
// common face
return 0;
}
}
(*testout) << "hit, cnt = " << cnt << endl;
return 1;
}
inline int Near (const Point2d & p1, const Point2d & p2,
const double eps = 1e-4 )
{
return Dist2(p1,p2) <= eps*eps;
}
void MeshFromSpline2D (SplineGeometry2d & geometry,
Mesh *& mesh,
MeshingParameters & mp)
{
PrintMessage (1, "Generate Mesh from spline geometry");
double h = mp.maxh;
Box<2> bbox = geometry.GetBoundingBox ();
if (bbox.Diam() < h)
{
h = bbox.Diam();
mp.maxh = h;
}
mesh = new Mesh;
mesh->SetDimension (2);
geometry.PartitionBoundary (h, *mesh);
// marks mesh points for hp-refinement
for (int i = 0; i < geometry.GetNP(); i++)
if (geometry.GetPoint(i).hpref)
{
double mindist = 1e99;
PointIndex mpi(0);
Point<2> gp = geometry.GetPoint(i);
Point<3> gp3(gp(0), gp(1), 0);
for (PointIndex pi = PointIndex::BASE;
pi < mesh->GetNP()+PointIndex::BASE; pi++)
if (Dist2(gp3, (*mesh)[pi]) < mindist)
{
mpi = pi;
mindist = Dist2(gp3, (*mesh)[pi]);
}
(*mesh)[mpi].Singularity(1.);
}
int maxdomnr = 0;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
if ( (*mesh)[si].domin > maxdomnr) maxdomnr = (*mesh)[si].domin;
if ( (*mesh)[si].domout > maxdomnr) maxdomnr = (*mesh)[si].domout;
}
mesh->ClearFaceDescriptors();
for (int i = 1; i <= maxdomnr; i++)
mesh->AddFaceDescriptor (FaceDescriptor (i, 0, 0, i));
// set Array<string*> bcnames...
// number of bcnames
int maxsegmentindex = 0;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
if ( (*mesh)[si].si > maxsegmentindex) maxsegmentindex = (*mesh)[si].si;
}
mesh->SetNBCNames(maxsegmentindex);
for ( int sindex = 0; sindex < maxsegmentindex; sindex++ )
mesh->SetBCName ( sindex, geometry.GetBCName( sindex+1 ) );
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
(*mesh)[si].SetBCName ( (*mesh).GetBCNamePtr( (*mesh)[si].si-1 ) );
Point3d pmin(bbox.PMin()(0), bbox.PMin()(1), -bbox.Diam());
Point3d pmax(bbox.PMax()(0), bbox.PMax()(1), bbox.Diam());
mesh->SetLocalH (pmin, pmax, mparam.grading);
mesh->SetGlobalH (h);
mesh->CalcLocalH();
int bnp = mesh->GetNP(); // boundary points
int hquad = mparam.quad;
for (int domnr = 1; domnr <= maxdomnr; domnr++)
if (geometry.GetDomainTensorMeshing (domnr))
{ // tensor product mesh
Array<PointIndex, PointIndex::BASE> nextpi(bnp);
Array<int, PointIndex::BASE> si1(bnp), si2(bnp);
PointIndex firstpi;
nextpi = -1;
si1 = -1;
si2 = -1;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
int p1 = -1, p2 = -2;
if ( (*mesh)[si].domin == domnr)
{ p1 = (*mesh)[si][0]; p2 = (*mesh)[si][1]; }
if ( (*mesh)[si].domout == domnr)
{ p1 = (*mesh)[si][1]; p2 = (*mesh)[si][0]; }
if (p1 == -1) continue;
nextpi[p1] = p2; // counter-clockwise
int index = (*mesh)[si].si;
if (si1[p1] != index && si2[p1] != index)
{ si2[p1] = si1[p1]; si1[p1] = index; }
if (si1[p2] != index && si2[p2] != index)
{ si2[p2] = si1[p2]; si1[p2] = index; }
}
PointIndex c1(0), c2, c3, c4; // 4 corner points
int nex = 1, ney = 1;
for (PointIndex pi = 1; pi <= si2.Size(); pi++)
if (si2[pi] != -1)
{ c1 = pi; break; }
for (c2 = nextpi[c1]; si2[c2] == -1; c2 = nextpi[c2], nex++);
for (c3 = nextpi[c2]; si2[c3] == -1; c3 = nextpi[c3], ney++);
for (c4 = nextpi[c3]; si2[c4] == -1; c4 = nextpi[c4]);
Array<PointIndex> pts ( (nex+1) * (ney+1) ); // x ... inner loop
pts = -1;
for (PointIndex pi = c1, i = 0; pi != c2; pi = nextpi[pi], i++)
pts[i] = pi;
for (PointIndex pi = c2, i = 0; pi != c3; pi = nextpi[pi], i++)
pts[(nex+1)*i+nex] = pi;
for (PointIndex pi = c3, i = 0; pi != c4; pi = nextpi[pi], i++)
pts[(nex+1)*(ney+1)-i-1] = pi;
for (PointIndex pi = c4, i = 0; pi != c1; pi = nextpi[pi], i++)
pts[(nex+1)*(ney-i)] = pi;
for (PointIndex pix = nextpi[c1], ix = 0; pix != c2; pix = nextpi[pix], ix++)
for (PointIndex piy = nextpi[c2], iy = 0; piy != c3; piy = nextpi[piy], iy++)
{
Point<3> p = (*mesh)[pix] + ( (*mesh)[piy] - (*mesh)[c2] );
pts[(nex+1)*(iy+1) + ix+1] = mesh -> AddPoint (p , 1, FIXEDPOINT);
}
for (int i = 0; i < ney; i++)
for (int j = 0; j < nex; j++)
{
Element2d el(QUAD);
el[0] = pts[i*(nex+1)+j];
el[1] = pts[i*(nex+1)+j+1];
el[2] = pts[(i+1)*(nex+1)+j+1];
el[3] = pts[(i+1)*(nex+1)+j];
el.SetIndex (domnr);
mesh -> AddSurfaceElement (el);
}
}
for (int domnr = 1; domnr <= maxdomnr; domnr++)
{
if (geometry.GetDomainTensorMeshing (domnr)) continue;
if ( geometry.GetDomainMaxh ( domnr ) > 0 )
h = geometry.GetDomainMaxh(domnr);
PrintMessage (3, "Meshing domain ", domnr, " / ", maxdomnr);
int oldnf = mesh->GetNSE();
mparam.quad = hquad || geometry.GetDomainQuadMeshing (domnr);
Meshing2 meshing (Box<3> (pmin, pmax));
for (PointIndex pi = PointIndex::BASE; pi < bnp+PointIndex::BASE; pi++)
meshing.AddPoint ( (*mesh)[pi], pi);
PointGeomInfo gi;
gi.trignum = 1;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
if ( (*mesh)[si].domin == domnr)
meshing.AddBoundaryElement ( (*mesh)[si][0] + 1 - PointIndex::BASE,
(*mesh)[si][1] + 1 - PointIndex::BASE, gi, gi);
if ( (*mesh)[si].domout == domnr)
meshing.AddBoundaryElement ( (*mesh)[si][1] + 1 - PointIndex::BASE,
(*mesh)[si][0] + 1 - PointIndex::BASE, gi, gi);
}
mparam.checkoverlap = 0;
meshing.GenerateMesh (*mesh, h, domnr);
for (SurfaceElementIndex sei = oldnf; sei < mesh->GetNSE(); sei++)
(*mesh)[sei].SetIndex (domnr);
// astrid
char * material;
geometry.GetMaterial( domnr, material );
if ( material )
{
(*mesh).SetMaterial ( domnr, material );
}
}
mparam.quad = hquad;
int hsteps = mp.optsteps2d;
mp.optimize2d = "smcm";
mp.optsteps2d = hsteps/2;
Optimize2d (*mesh, mp);
mp.optimize2d = "Smcm";
mp.optsteps2d = (hsteps+1)/2;
Optimize2d (*mesh, mp);
mp.optsteps2d = hsteps;
mesh->Compress();
mesh -> SetNextMajorTimeStamp();
extern void Render();
Render();
}
EXPORT int GenericAckermann( ROVER_PARAM *MyRover,
double RoverVelocity,
double *RotationCenter,
double *PointToControl,
double *WheelSteering,
double *WheelVelocity )
{
/* --- declare variables --- */
int i=0; // 'for' loops variable
// coordinates of the rotation center in the rover frame
//double Rx = RotationCenter[X];
//double Ry = RotationCenter[Y];
// Rover angular velocity in [rad/s] corresponding to the linear velocity
double RoverAngularVelocity = 0.;
// distance between RotationCenter and PoinToControl
double DistRC_PTC = 0.;
//static int PreviousState; // variable to 'remember' if the
#ifdef DEBUG
printf( "\nin GenericRoverManoeuvre.c->GenericAckermann()\n" );
#endif
// check the input Rover pointer is valid
if( MyRover == NULL )
{
printf( "\tERROR in GenericRoverManeuver.c->GenericAckermann() : MyRover is NULL\n\n" );
return -1;
} else if( RotationCenter == NULL )
{
printf( "\tERROR in GenericRoverManeuver.c->GenericAckermann() : RotationCenter is NULL\n\n" );
return -1;
}else if( PointToControl == NULL )
{
printf( "\tERROR in GenericRoverManeuver.c->GenericAckermann() : RoverPointToControl is NULL\n\n" );
return -1;
} else if( WheelVelocity == NULL )
{
printf( "\tERROR in GenericRoverManeuver.c->GenericAckermann() : WheelVelocity is NULL\n\n" );
return -1;
} else if( WheelSteering == NULL )
{
printf( "\tERROR in GenericRoverManeuver.c->GenericAckermann() : WheelSteering is NULL\n\n" );
return -1;
}
/* --- set wheel angle --- */
// the point to control influence only the wheel speed, wheel angle only set by rotation center
// different cases if Ry > or < 0 in order to turn RIGHT with positive linear velocity
// note : if the rotation center is on the X axis of the rover, it will turn LEFT
for( i=0 ; i<MyRover->WheelNumber ; i++ )
{
// check the steering wheel
if( MyRover->IsSteeringWheel[i] == TRUE )
{
double x = MyRover->WheelCoordCart[i][X];
double y = MyRover->WheelCoordCart[i][Y];
double Rx = RotationCenter[X];
double Ry = RotationCenter[Y];
// handle the case of Ry<0
// only flip the Y axis and inverse the sign of the angle to have the right angles
y *= SGN(RotationCenter[Y]);
Ry *= SGN(RotationCenter[Y]);
WheelSteering[i] = atan2( x-Rx, Ry-y ); // atan2(opp, hyp)
// handle the case of Ry<0
WheelSteering[i] *= SGN(RotationCenter[Y]);
//WheelSteering[i] = SGN(Ry) * atan2( x-Rx, ABS(Ry-y) ) * RAD2DEG; // atan2(opp, hyp) ; CORRECT ONLY IF RotationCenter is OUTSIDE THE ROVER
//printf( "\tx-Rx = %f\n", x-Rx );
//printf( "\tRy-y = %f\n", Ry-y );
//printf( "\n" );
//WheelSteering[i] = 0.;
}
else
{
//printf( "%d NoSpeed\n", i );
WheelSteering[i] = 0.;
}
}
/* --- set wheel angular velocity --- */
// note : the input RoverLinearVelocity is the linear velocity of the point to control ;
// i.e. if the point to control is (0;0), the RoverLinearVelocity is the linear velocity of the rover center
// compute rover angular velocity
DistRC_PTC = Dist2( RotationCenter[X], RotationCenter[Y], PointToControl[X], PointToControl[Y] );
// check case of point turn
if( DistRC_PTC == 0. )
RoverAngularVelocity = RoverVelocity;
else
{
// only if RotationCenter != RoverPointToControl, i.e. DistRC_PTC != 0.
RoverAngularVelocity = RoverVelocity / DistRC_PTC;
}
#ifdef DEBUG
printf( "RoverAngularVelocity = %3.2f rad/s\n", RoverAngularVelocity );
printf( "Wheel Velocity (Linear | Angular) :\n" );
#endif
// compute wheel velocity
for( i=0 ; i<MyRover->WheelNumber ; i++ )
{
// check the driving wheel
//printf( "drive : %d\n", MyRover->IsDrivingWheel[i] );
if( MyRover->IsDrivingWheel[i] == TRUE )
{
double x = MyRover->WheelCoordCart[i][X];
double y = MyRover->WheelCoordCart[i][Y];
double Rx = RotationCenter[X];
double Ry = RotationCenter[Y];
double WheelLinVelocity = RoverAngularVelocity * Dist2( x, y, Rx, Ry );
WheelVelocity[i] = WheelLinVelocity / MyRover->WheelRadius[i];
#ifdef DEBUG
printf( "\t%3.2f\t|\t%3.2f\n", WheelLinVelocity, WheelVelocity[i] );
#endif
}
else
{
//printf( "%d NoSpeed\n", i );
WheelVelocity[i] = 0.;
#ifdef DEBUG
printf( "\t%3.2f\t|\t%3.2f\n", 0., 0. );
#endif
}
}
return 0;
}
int LocalMove(struct s_polymer *p, struct s_polymer *oldp,struct s_polymer *fragment,struct s_potential *pot,int nmul,struct s_mc_parms *parms, double t)
{
int ok=0,nang=0;
int iw,ip,iapdbtocheck,i,m;
double deltaE;
double detL1,detL2,detA1,detA2,W1,W2,psisquared,e;
ip=irand(parms->npol);
iw=(nmul-1)+irand( (p+ip)->nback-nmul+1 );
//iw=20;
for(i=0; i<nmul; i++)
{
nang+=((((p+ip)->back)+iw-nmul+i)+1)->move;
}
deltaE = -GetEnergyMonomerRange(p,iw-nmul+1,iw+1,ip);
if(iw==((p+ip)->nback-1)) //pivot forward OK
{
for(i=0; i<nang; i++)
(fragment+ip)->d_ang[i]=parms->dw_mpivot*(0.05-frand());
if (!parms->nodihpot)
for(i=0; i<nmul+3; i++)
{
deltaE-=(((p+ip)->back)+iw-nmul+i+1)->e_dih;
}
m=0;
for(i=0; i<nmul; i++)
{
if( (((p+ip)->back)+iw+i-nmul+2)->move==1)
{
ok*=PivotForward((p+ip),iw-nmul+i+1,(fragment+ip)->d_ang[m],nmul-i-2,parms);
m++;
if(m==nang) break;
}
}
if(!parms->nosidechains)
{
ok*=AddSidechain(p,iw-nmul+1,iw+1,ip);
}
deltaE += EnergyMonomerRange(p,pot,iw-nmul+1,iw+1,ip,parms->npol,parms->shell,1,parms->nosidechains,parms->disentangle,parms->hb);
if (!parms->nodihpot)
for(i=0; i<nmul+3; i++)
{
deltaE+=EnergyDihedrals(p,pot,iw-nmul+i+1,ip,1);
}
ok*=Metropolis(deltaE,t,p->tables);
if(ok==0)
{
UpdateMonomerRange(oldp,p,iw-nmul+1,iw+1,ip,parms->shell);
}
else
{
//move accepted
UpdateMonomerRange(p,oldp,iw-nmul+1,iw+1,ip,parms->shell);
p->etot+=deltaE;
parms->acc++;
}
} //end of forward
else if(iw==nmul-1) //pivot backward OK
{
for(i=0; i<nang; i++)
(fragment+ip)->d_ang[i]=parms->dw_mpivot*(0.05-frand());
if (!parms->nodihpot)
for(i=0; i<nmul+3; i++)
{
deltaE-=(((p+ip)->back)+i)->e_dih;
}
m=0;
for(i=0; i<nmul; i++)
{
if( (((p+ip)->back)+iw-i)->move==1)
{
ok*=PivotBackward((p+ip),iw-i,(fragment+ip)->d_ang[m],nmul-i-2,parms);
m++;
if(m==nang) break;
}
}
if(!parms->nosidechains)
ok*=AddSidechain(p,0,iw+1,ip);
deltaE += EnergyMonomerRange(p,pot,0,iw+1,ip,parms->npol,parms->shell,1,parms->nosidechains,parms->disentangle,parms->hb);
if (!parms->nodihpot)
for(i=0; i<nmul+3; i++)
{
deltaE+=EnergyDihedrals(p,pot,i,ip,1);
}
ok*=Metropolis(deltaE,t,p->tables);
if(ok==0)
{
UpdateMonomerRange(oldp,p,0,iw+1,ip,parms->shell);
}
else
{
UpdateMonomerRange(p,oldp,0,iw+1,ip,parms->shell);
p->etot+=deltaE;
parms->acc++;
}
} //end of backward
else if(iw!=((p+ip)->nback-2))//pivot local
{
if((((p+ip)->back)+iw+1)->iapdb==0)
iapdbtocheck=1;
if((((p+ip)->back)+iw+1)->iapdb==1)
iapdbtocheck=2;
if((((p+ip)->back)+iw+1)->iapdb==2)
iapdbtocheck=0;
int out;
Gaussian_Angles((fragment+ip)->g_ang,nang);
Compute_G(fragment,p,ip,iw-nmul+1,nmul,nang,parms);
MatA((fragment+ip)->A,(fragment+ip)->G,nang,parms->bgs_a,parms->bgs_b);
Cholesky_2((fragment+ip)->L,(fragment+ip)->A,nang);
psisquared=Squared_n_Norma((fragment+ip)->g_ang,nang);
e=exp(-psisquared);
detL1=DetTriang((fragment+ip)->L,nang);
detA1=detL1*detL1;
W1=e * sqrt(detA1);
InvertTriang((fragment+ip)->Y,(fragment+ip)->L,nang);
TransposedMatOnVect((fragment+ip)->Y,(fragment+ip)->g_ang,(fragment+ip)->d_ang,nang);
if(!parms->nodihpot)
for(i=0; i<nmul+3; i++)
{
deltaE -= (((p+ip)->back)+iw-nmul+i+1)->e_dih;
}
if(!parms->noangpot)
{
deltaE-=(((p+ip)->back)+iw)->e_ang;
deltaE-=(((p+ip)->back)+iw+1)->e_ang;
}
m=0;
for(i=0; i<nmul; i++)
{
if( (((p+ip)->back)+iw+i-nmul+2)->move==1)
{
ok*=PivotForward((p+ip),iw-nmul+i+1,(fragment+ip)->d_ang[m],nmul-i-2,parms);
m++;
if(m==nang)
break;
}
}
double rc2=parms->r_cloose*parms->r_cloose;
double dihedral=Dihedral( (((p+ip)->back)+iw-iapdbtocheck)->pos, (((p+ip)->back)+iw+1-iapdbtocheck)->pos, (((p+ip)->back)+iw+2-iapdbtocheck)->pos, (((p+ip)->back)+iw+3-iapdbtocheck)->pos, p->tables, &out );
// fprintf(stderr,"Checking Dihedral: backbone atoms %d,%d,%d,%d\n",+iw-iapdbtocheck,+iw+1-iapdbtocheck,+iw+2-iapdbtocheck,+iw+3-iapdbtocheck);
if( DAbs (Dist2( (((p+ip)->back)+iw)->pos,(((p+ip)->back)+iw+1)->pos ) -(((p+ip)->back)+iw)->d2_next < rc2 ) && DAbs( Angle( (((p+ip)->back)+iw-1)->pos, (((p+ip)->back)+iw)->pos, (((p+ip)->back)+iw+1)->pos, (p+ip)->tables, &out) - (((p+ip)->back)+iw-1)->a_next) < 0.05 && DAbs(180-DAbs(dihedral)) < DELTAOMEGA)
{
if(!parms->nosidechains)
{
ok *= AddSidechain(p,iw-nmul+1,iw+1,ip);
}
deltaE += EnergyMonomerRange(p,pot,iw-nmul+1,iw+1,ip,parms->npol,parms->shell,1,parms->nosidechains,parms->disentangle,parms->hb);
if (!parms->nodihpot)
for(i=0; i<nmul+3; i++)
{
deltaE+=EnergyDihedrals(p,pot,iw-nmul+i+1,ip,1);
}
if(!parms->noangpot)
{
// fprintf(stderr,"\nCOMPUTING ANGLE ENERGY deltaE=%lf\t",deltaE);
deltaE+=EnergyAngles(p,pot,iw,ip,1);
deltaE+=EnergyAngles(p,pot,iw+1,ip,1);
// fprintf(stderr,"-> %lf\n",deltaE);
}
Compute_G(fragment,p,ip,iw-nmul+1,nmul,nang,parms);
MatA((fragment+ip)->A,(fragment+ip)->G,nang,parms->bgs_a,parms->bgs_b);
Cholesky_2((fragment+ip)->L,(fragment+ip)->A,nang);
detL2=DetTriang((fragment+ip)->L,nang);
detA2=detL2*detL2;
TransposedMatOnVect((fragment+ip)->L,(fragment+ip)->d_ang,(fragment+ip)->g_ang,nang);
psisquared=Squared_n_Norma((fragment+ip)->g_ang,nang);
e=exp(-psisquared);
W2=e*sqrt(detA2);
ok*=B_Metropolis(deltaE,t,W2,W1,p->tables);
if(ok==0) //move rejected
{
UpdateMonomerRange(oldp,p,iw-nmul+1,iw+1,ip,parms->shell);
}
else //move accepted
{
UpdateMonomerRange(p,oldp,iw-nmul+1,iw+1,ip,parms->shell);
p->etot+=deltaE;
parms->acc++;
}
}//end of loose condition
else //if not loose pivot
{
ok=0;
UpdateMonomerRange(oldp,p,iw-nmul+1,iw+1,ip,parms->shell);
}
}//end of local pivot
parms->mov++;
return ok;
}
Real Dist(const Real V1[restrict], const Real V2[restrict])
{
return sqrt(Dist2(V1, V2));
}
