static inline
void _verifyTheTransform(const int v0, const Vector3f& p0, const int v1, const Vector3f& p1, CVertexInfo *pVertInfo)
{
float r1 = Distance2(p0, p1);
float r2 = Distance2(pVertInfo[v0].m_vInitPosition, pVertInfo[v1].m_vInitPosition);
float k;
if (r1<r2) k = r2 /r1;
else k=r1/r2;
if (k>15){
printf("Something wrong!!\n");
}
}
double Performance_Data::TTime2 (int period)
{
double time = Time2 (period);
if (time <= 0.0) {
time = Time0 () / 10.0;
} else {
double distance = Distance2 (period);
double speed = distance / time;
if (speed < 0.5) speed = 0.5;
time = Length () / speed;
}
if (time < 0.1) time = 0.1;
return (time);
}
//Two functions for weight computation
inline void computeSquareDistanceWeightsNoDamp(const Vector3d& center, const Vector3d refVertex[4], const int nv, double weights[4])
{
double sum_w = 0;
int i;
for (i=0; i<nv; i++){
const double dist2 = Distance2(center, refVertex[i]) + 1e-16;
const double w = 1.0 / dist2;
sum_w += w;
weights[i] = w;
}
const double inv_sum_w = 1.0/sum_w;
for (i=0; i<nv; i++){
weights[i] *= inv_sum_w;
}
}
void CMANETView::OnNetworkLinkSelfgenerate()
{
// TODO: Add your command handler code here
CMANETDoc* pDoc=GetDocument();
ASSERT_VALID(pDoc);
int RouterSize=pDoc->Routers.GetSize();
int i,j;
CRouter *ri,*rj;
for(i=0;i<RouterSize;i++)
{
ri=(CRouter*)(pDoc->Routers[i]);
for(j=i+1;j<RouterSize;j++)
{
rj=(CRouter*)(pDoc->Routers[j]);
int dis2i=ri->Coverage*ri->Coverage;
int dis2j=rj->Coverage*rj->Coverage;
int dis2ij=Distance2(ri,rj);
if((dis2ij!=-1)&&(dis2ij<=dis2i)&&(dis2ij<=dis2j))
{
BOOL repeated=0;
int neighborsize_i=ri->Neighbors.GetSize();
int neighborsize_j=rj->Neighbors.GetSize();
if(neighborsize_i<neighborsize_j)
{
int k=0;
while((k<neighborsize_i)&&(rj!=((CRouter*)(ri->Neighbors[k]))))
k++;
if(k<neighborsize_i)
repeated=1;
}
else
{
int k=0;
while((k<neighborsize_j)&&(ri!=((CRouter*)(rj->Neighbors[k]))))
k++;
if(k<neighborsize_j)
repeated=1;
}
if(!repeated)
Link=new CLink(ri,rj,DefaultLink.Frequency,DefaultLink.Duplex);
}
}
}
this->Invalidate();
}
//Two functions for weight computation
inline void computeSquareDistanceWeights(const Vector3d& center, const Vector3d refVertex[4], const int nv, const double &damp, double weights[4])
{
const double WEIGHTMAX=0.90;
const double WEIGHTMIN=(1.0-WEIGHTMAX)/3;
double sum_w = 0;
int i;
for (i=0; i<nv; i++){
const double dist2 = Distance2(center, refVertex[i]) + damp;
const double w = 1.0 / dist2;
sum_w += w;
weights[i] = w;
}
const double inv_sum_w = 1.0/sum_w;
for (i=0; i<nv; i++){
weights[i] *= inv_sum_w;
}
}
double CRodObj::estimatedVertexRadius(void)
{
Vector2i* pTriangle = (Vector2i*)m_pElement;
assert(pTriangle!=NULL);
int c=0;
double len=0;
for (int i=0; i<m_nElementCount; i+=2){
const Vector2i *pt = &pTriangle[i];
const int vx = pt->x;
const int vy = pt->y;
c++;
const double r= Distance2(m_pVertex[vx], m_pVertex[vy]);
len +=sqrt(r);
}
if (c) len = len/c *0.02;
return len;
}
double Performance_Data::Avg_Speed2 (int period)
{
double time = Time2 (period);
if (time <= 0.0) {
double length = Length ();
time = Time0 () / 10.0;
if (time < 0.1) time = 0.1;
return (length / time);
} else {
double distance = Distance2 (period);
double speed = distance / time;
if (speed < 0.5) speed = 0.5;
return (speed);
}
}
//Two functions for weight computation
inline void computeSquareDistanceWeights(const float3& center, const float3 refVertex[4], const int nv, QFLOAT weights[4])
{
const QFLOAT WEIGHTMAX=0.90f;
const QFLOAT WEIGHTMIN=(1.0-WEIGHTMAX)/3;
int i;
QFLOAT sum_w = 0;
for (i=0; i<nv; i++){
const QFLOAT dist2 = Distance2(center, refVertex[i]) + 1e-6;
const QFLOAT w = 1.0 / dist2;
sum_w += w;
weights[i] = w;
}
QFLOAT inv_sum_w = 1.0/sum_w;
for (i=0; i<nv; i++){
weights[i] *= inv_sum_w;
if (weights[i]>WEIGHTMAX)
weights[i]=WEIGHTMAX;
else if (weights[i]<WEIGHTMIN)
weights[i]=WEIGHTMIN;
}
}
int CheckCollisionGround( vector<z_face> &collision, vec center, float radius, float angle, float mindist)
{
float ground_skew = (float)cos(angle*PI180)*radius;
vec vpold_vp = mindist*vec(0,-1,0);
for( int i=0; i<collision.size(); i++)
{
vec normal = collision[i].normal;
float distance = PlaneDistance( normal, collision[i].a); // D = - (Ax+By+Cz)
// ---------------------------------------------------------
// najdeme kolidujuci bod na guli
vec ClosestPointOnSphere; // najblizsi bod gule k rovine aktualneho trojuholnika
// najdeme ho ako priesecnik priamky prechadzajucej stredom gule a smerovym vektorom = normalovemu vektoru roviny (trojuholnika)
// vypocitame ho, ale jednodusie tak, ze pripocitame (opocitame) k stredu vektor normala*radius
if( PlanePointDelta( normal, distance, center) > 0 )
{
// center je na strane normaly, blizsi bod je v opacnom smere ako smer normaly
ClosestPointOnSphere = -radius*normal+center;
}
else
{
// center je na opacnej strane ako normala, blizsi bod je v smere normaly
ClosestPointOnSphere = radius*normal+center;
}
// ---------------------------------------------------------
// najdeme kolidujuci bod na trojuholniku
// najprv najdeme kolidujuci bod vzhladom na rovinu v ktorej lezi trojuholnik
vec contactPointSphereToPlane; // kolidujuci bod na rovine trojuholnika s gulou
float distanceCenterToPlane; // vzdialenost stredu gule k rovine
// zistime ci rovina pretina gulu
if( SpherePlaneCollision( center, 0.9999f*radius, normal, distance, &distanceCenterToPlane)==1 )
{
// gula pretina rovinu
// kolidujuci bod je bod na rovine najblizsi k stredu gule
// je vzdialeny od roviny na distanceCenterToPlane, pretoze pocitame bod na rovine pouzijeme -
contactPointSphereToPlane = center-distanceCenterToPlane*normal;
}
else
{
// nie sme v kolizii z gulov, ak sa pohybujeme v smere od roviny, nemoze nastat ziadna kolizia
// ak sa pohybujeme v smere kolmom na normalovy vektor roviny, tak isto kolizia nehrozi
// kvoli nepresnosti vypoctov umoznime pohyb aj ked velmi malou castou smeruje do roviny
// if( DOT3( vpold_vp, center-ClosestPointOnSphere) >= 0)
if( DOT3( vpold_vp, center-ClosestPointOnSphere) > -0.000001f)
{
continue;
}
// gula nepretina rovinu
// kolidujuci bod je priesecnik roviny a priamky vedenej z bodu ClosestPointOnSphere
// v smere pohybu t.j. z vpold do vp
float t = LinePlaneIntersectionDirParameter( ClosestPointOnSphere, vpold_vp, normal, distance);
// t > 1.f, priesecnik z rovinou je dalej ako vpold_vp od bodu ClosestPointOnSphere
if(t>1.f)
continue; // za cely krok vpold_vp sa s tymto trojuholnikom nestretneme
else if( t<-radius/vpold_vp.Length()) // priesecnik je za gulou, v smere pohybu tuto rovinu nestretneme
continue;
else
contactPointSphereToPlane = ClosestPointOnSphere+t*vpold_vp;
}
// najdeme kolidujuci bod na trojuholniku
vec contactPointSphereToTriangle;
// ak sa bod contactPointSphereToPlane nenachadza v trojuholniku
// najdeme najblizsi bod trojuholnika k bodu contactPointSphereToTriangle
if( !PointInsidePolygon( contactPointSphereToPlane, collision[i].a, collision[i].b, collision[i].c) )
{
// najdeme najblizsi bod k contactPointSphereToPlane na hranach trojuholnika
// z tychto vyberieme najblizi k contactPointSphereToPlane
vec closest_ab = ClosestPointOnLine( collision[i].a, collision[i].b, contactPointSphereToPlane);
vec closest_bc = ClosestPointOnLine( collision[i].b, collision[i].c, contactPointSphereToPlane);
vec closest_ca = ClosestPointOnLine( collision[i].c, collision[i].a, contactPointSphereToPlane);
float dist_ab = Distance2( closest_ab, contactPointSphereToPlane);
float dist_bc = Distance2( closest_bc, contactPointSphereToPlane);
float dist_ca = Distance2( closest_ca, contactPointSphereToPlane);
if( dist_ab<dist_bc)
{
if(dist_ab<dist_ca)
contactPointSphereToTriangle = closest_ab;
else
contactPointSphereToTriangle = closest_ca;
}
else
{
if(dist_bc<dist_ca)
contactPointSphereToTriangle = closest_bc;
else
contactPointSphereToTriangle = closest_ca;
}
// kedze kolidujuci bod na trojuholniku je iny ako kolidujuci bod na rovine
// zmeni sa aj kolidujuci bod na guli - ClosestPointOnSphere
// vypocitame ho ako priesecnik gule a priamky z bodu contactPointSphereToTriangle
// v smere pohybu t.j. z vpold do vp
double t1,t2;
if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
ClosestPointOnSphere = t1*vpold_vp+contactPointSphereToTriangle;
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else if( t1>0 && t2<0)
{
// gula je v stene, vratime ju von zo steny
// berieme bod, ktory je blizsie k rovine
vec t1point = t1*vpold_vp+contactPointSphereToTriangle;
vec t2point = t2*vpold_vp+contactPointSphereToTriangle;
/* if(PlanePointDistance( normal, distance, t1point)<=PlanePointDistance( normal, distance, t2point) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
*/
if( ABS(t1) < ABS(t2) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// bod je vnutri trojuholnika
// contactPointSphereToTriangle = contactPointSphereToPlane;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
/* if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
return 1;
}
else if( t1>0 && t2<0)
{
return 1; // gula je v stene
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
return 1; // bod je vnutri trojuholnika
}*/
}
return 0;
}
vec CheckCollision( vector<z_face> &collision, vec vp, vec vpold, float radius)
{
// if(vpold_vp.Length()==0)return vpold;
vec vpold_vp = vp-vpold; // smer pohybu
vec vpold_vp_povodny = vpold_vp;// smer pohybu
int iter=0;
float radius2 = radius*radius;
vec newmove = vpold_vp;
vec newClosestPointOnSphere;
vec newContactPointSphereToTriangle;
do
{
float distanceCenterPointOnTriangle=1.e+15f;
int smykanie=0;
for( int i=0; i<collision.size(); i++)
{
vec normal = collision[i].normal;
vec vp_move;
vec center = vpold;
float distance = PlaneDistance( normal, collision[i].a); // D = - (Ax+By+Cz)
// ---------------------------------------------------------
// najdeme kolidujuci bod na guli
vec ClosestPointOnSphere; // najblizsi bod gule k rovine aktualneho trojuholnika
// najdeme ho ako priesecnik priamky prechadzajucej stredom gule a smerovym vektorom = normalovemu vektoru roviny (trojuholnika)
// vypocitame ho, ale jednodusie tak, ze pripocitame (opocitame) k stredu vektor normala*radius
if( PlanePointDelta( normal, distance, center) > 0 )
{
// center je na strane normaly, blizsi bod je v opacnom smere ako smer normaly
ClosestPointOnSphere = -radius*normal+center;
}
else
{
// center je na opacnej strane ako normala, blizsi bod je v smere normaly
ClosestPointOnSphere = radius*normal+center;
}
// ---------------------------------------------------------
// najdeme kolidujuci bod na trojuholniku
// najprv najdeme kolidujuci bod vzhladom na rovinu v ktorej lezi trojuholnik
vec contactPointSphereToPlane; // kolidujuci bod na rovine trojuholnika s gulou
float distanceCenterToPlane; // vzdialenost stredu gule k rovine
// zistime ci rovina pretina gulu
if( SpherePlaneCollision( center, 0.9999f*radius, normal, distance, &distanceCenterToPlane)==1 )
{
// gula pretina rovinu
// kolidujuci bod je bod na rovine najblizsi k stredu gule
// je vzdialeny od roviny na distanceCenterToPlane, pretoze pocitame bod na rovine pouzijeme -
contactPointSphereToPlane = center-distanceCenterToPlane*normal;
}
else
{
// nie sme v kolizii z gulov, ak sa pohybujeme v smere od roviny, nemoze nastat ziadna kolizia
// ak sa pohybujeme v smere kolmom na normalovy vektor roviny, tak isto kolizia nehrozi
// kvoli nepresnosti vypoctov umoznime pohyb aj ked velmi malou castou smeruje do roviny
// if( DOT3( vpold_vp, center-ClosestPointOnSphere) >= 0)
if( DOT3( vpold_vp, center-ClosestPointOnSphere) > -0.000001f)
{
continue;
}
// gula nepretina rovinu
// kolidujuci bod je priesecnik roviny a priamky vedenej z bodu ClosestPointOnSphere
// v smere pohybu t.j. z vpold do vp
float t = LinePlaneIntersectionDirParameter( ClosestPointOnSphere, vpold_vp, normal, distance);
// t > 1.f, priesecnik z rovinou je dalej ako vpold_vp od bodu ClosestPointOnSphere
if(t>1.f)
continue; // za cely krok vpold_vp sa s tymto trojuholnikom nestretneme
else if( t<-radius/vpold_vp.Length()) // priesecnik je za gulou, v smere pohybu tuto rovinu nestretneme
continue;
else
contactPointSphereToPlane = ClosestPointOnSphere+t*vpold_vp;
}
// najdeme kolidujuci bod na trojuholniku
vec contactPointSphereToTriangle;
// ak sa bod contactPointSphereToPlane nenachadza v trojuholniku
// najdeme najblizsi bod trojuholnika k bodu contactPointSphereToTriangle
if( !PointInsidePolygon( contactPointSphereToPlane, collision[i].a, collision[i].b, collision[i].c) )
{
// najdeme najblizsi bod k contactPointSphereToPlane na hranach trojuholnika
// z tychto vyberieme najblizi k contactPointSphereToPlane
vec closest_ab = ClosestPointOnLine( collision[i].a, collision[i].b, contactPointSphereToPlane);
vec closest_bc = ClosestPointOnLine( collision[i].b, collision[i].c, contactPointSphereToPlane);
vec closest_ca = ClosestPointOnLine( collision[i].c, collision[i].a, contactPointSphereToPlane);
float dist_ab = Distance2( closest_ab, contactPointSphereToPlane);
float dist_bc = Distance2( closest_bc, contactPointSphereToPlane);
float dist_ca = Distance2( closest_ca, contactPointSphereToPlane);
if( dist_ab<dist_bc)
{
if(dist_ab<dist_ca)
contactPointSphereToTriangle = closest_ab;
else
contactPointSphereToTriangle = closest_ca;
}
else
{
if(dist_bc<dist_ca)
contactPointSphereToTriangle = closest_bc;
else
contactPointSphereToTriangle = closest_ca;
}
// kedze kolidujuci bod na trojuholniku je iny ako kolidujuci bod na rovine
// zmeni sa aj kolidujuci bod na guli - ClosestPointOnSphere
// vypocitame ho ako priesecnik gule a priamky z bodu contactPointSphereToTriangle
// v smere pohybu t.j. z vpold do vp
double t1,t2;
if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
ClosestPointOnSphere = t1*vpold_vp+contactPointSphereToTriangle;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else if( t1>0 && t2<0)
{
// gula je v stene, vratime ju von zo steny
// berieme bod, ktory je blizsie k rovine
vec t1point = t1*vpold_vp+contactPointSphereToTriangle;
vec t2point = t2*vpold_vp+contactPointSphereToTriangle;
/* if(PlanePointDistance( normal, distance, t1point)<=PlanePointDistance( normal, distance, t2point) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
*/
if( ABS(t1) < ABS(t2) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
// bod je vnutri trojuholnika
contactPointSphereToTriangle = contactPointSphereToPlane;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
// zistime vzdialenost kontaktneho bodu na trojuholniku ku stredu gule
float dist = Distance2(contactPointSphereToTriangle,center);
if(dist<radius2) // ak je mensi ako polomer, gula je v kolizii z polygonom
{
if(dist<distanceCenterPointOnTriangle) // ak vzdialenost je mensia ako ineho bodu v kolizii, nahradime ho
{
distanceCenterPointOnTriangle=dist;
newmove = vp_move;
newClosestPointOnSphere = ClosestPointOnSphere;
newContactPointSphereToTriangle = contactPointSphereToTriangle;
}
}
else
{
if(distanceCenterPointOnTriangle>5.e+14f) // nenasiel sa ziaden bod vnutri gule
{
if( vp_move.Length2() < newmove.Length2() ) // berieme kratsi
{
newmove = vp_move;
newClosestPointOnSphere = ClosestPointOnSphere;
newContactPointSphereToTriangle = contactPointSphereToTriangle;
}
}
}
smykanie=1;
}
if(smykanie)
{
vec normal=vpold-newClosestPointOnSphere;
float distance = PlaneDistance( normal, newContactPointSphereToTriangle);
vec delta = LinePlaneIntersectionDir( newClosestPointOnSphere+vpold_vp, normal, normal, distance)-newContactPointSphereToTriangle;
// vec newvp = newClosestPointOnSphere+vpold_vp;
// float distancepoint = PlanePointDelta( normal, distance, newvp);
// vec intersec = -distancepoint*normal+newvp;
// vec delta = intersec-newContactPointSphereToTriangle;
// taky klzavy pohyb, ktory ide proti povodnemu pohybu zamietneme
if( DOT3(vpold_vp_povodny, delta) < 0)delta.clear();
vpold += newmove; // posunieme sa po najblizi kolidujuci bod
vpold += 0.000001f*normal;
vp = vpold + delta; // cielovy bod posunieme o deltu klzanim
vpold_vp = vp-vpold; // novy vektor pohybu
newmove = vpold_vp;
iter++;
}
else
{
vpold += newmove;
vpold_vp.clear();
iter=1000;
}
}
while( (vpold_vp.Length2()>1.e-8f)&&(iter<10) );
return vpold;
}
CFfloat CFpoint::Distance( const CFpoint& point ) const
{
return sqrt( Distance2( point ) );
}
