C++ distance函数代码示例

bool CTargetFind::isWithinRange(position_t* pos, float range)
{
    return distance(m_PBattleEntity->loc.p, *pos) <= range;
}

/**
* Checks if the mob can detect the target using it's detection (sight, sound, etc)
* This is used to aggro and deaggro (Mobs start to deaggro after failing to detect target).
**/
bool CMobController::CanDetectTarget(CBattleEntity* PTarget, bool forceSight)
{
    if (PTarget->isDead() || PTarget->isMounted()) return false;

    float verticalDistance = abs(PMob->loc.p.y - PTarget->loc.p.y);

    if (verticalDistance > 8)
    {
        return false;
    }

    auto detects = PMob->m_Detects;
    auto currentDistance = distance(PTarget->loc.p, PMob->loc.p) + PTarget->getMod(Mod::STEALTH);

    bool detectSight = (detects & DETECT_SIGHT) || forceSight;
    bool hasInvisible = false;
    bool hasSneak = false;

    if (!PMob->m_TrueDetection)
    {
        hasInvisible = PTarget->StatusEffectContainer->HasStatusEffectByFlag(EFFECTFLAG_INVISIBLE);
        hasSneak = PTarget->StatusEffectContainer->HasStatusEffect(EFFECT_SNEAK);
    }

    if (detectSight && !hasInvisible && currentDistance < PMob->getMobMod(MOBMOD_SIGHT_RANGE) && isFaceing(PMob->loc.p, PTarget->loc.p, 40))
    {
        return CanSeePoint(PTarget->loc.p);
    }

    if ((PMob->m_Behaviour & BEHAVIOUR_AGGRO_AMBUSH) && currentDistance < 3 && !hasSneak)
    {
        return true;
    }

    if ((detects & DETECT_HEARING) && currentDistance < PMob->getMobMod(MOBMOD_SOUND_RANGE) && !hasSneak)
    {
        return CanSeePoint(PTarget->loc.p);
    }

    // everything below require distance to be below 20
    if (currentDistance > 20)
    {
        return false;
    }

    if ((detects & DETECT_LOWHP) && PTarget->GetHPP() < 75)
    {
        return CanSeePoint(PTarget->loc.p);
    }

    if ((detects & DETECT_MAGIC) && PTarget->PAI->IsCurrentState<CMagicState>() &&
        static_cast<CMagicState*>(PTarget->PAI->GetCurrentState())->GetSpell()->hasMPCost())
    {
        return CanSeePoint(PTarget->loc.p);
    }

    if ((detects & DETECT_WEAPONSKILL) && PTarget->PAI->IsCurrentState<CWeaponSkillState>())
    {
        return CanSeePoint(PTarget->loc.p);
    }

    if ((detects & DETECT_JOBABILITY) && PTarget->PAI->IsCurrentState<CAbilityState>())
    {
        return CanSeePoint(PTarget->loc.p);
    }

    return false;
}

    std::list<ESM::Pathgrid::Point> PathFinder::aStarSearch(const ESM::Pathgrid* pathGrid,int start,int goal,float xCell, float yCell)
    {
        cleanUpAStar();
        mGScore[start] = 0;
        mFScore[start] = distance(pathGrid->mPoints[start],pathGrid->mPoints[goal]);

        std::list<int> openset;
        std::list<int> closedset;
        openset.push_back(start);

        int current = -1;

        while(!openset.empty())
        {
            current = openset.front();
            openset.pop_front();

            if(current == goal) break;

            closedset.push_back(current);

            for(int j = 0;j<static_cast<int> (mGraph[current].edges.size());j++)
            {
                //int next = mGraph[current].edges[j].destination
                if(std::find(closedset.begin(),closedset.end(),mGraph[current].edges[j].destination) == closedset.end())
                {
                    int dest = mGraph[current].edges[j].destination;
                    float tentative_g = mGScore[current] + mGraph[current].edges[j].cost;
                    bool isInOpenSet = std::find(openset.begin(),openset.end(),dest) != openset.end();
                    if(!isInOpenSet
                        || tentative_g < mGScore[dest] )
                    {
                        mGraph[dest].parent = current;
                        mGScore[dest] = tentative_g;
                        mFScore[dest] = tentative_g + distance(pathGrid->mPoints[dest],pathGrid->mPoints[goal]);
                        if(!isInOpenSet)
                        {
                            std::list<int>::iterator it = openset.begin();
                            for(it = openset.begin();it!= openset.end();it++)
                            {
                                if(mGScore[*it]>mGScore[dest])
                                    break;
                            }
                            openset.insert(it,dest);
                        }
                    }
                }
            }

        }

        std::list<ESM::Pathgrid::Point> path;
        while(mGraph[current].parent != -1)
        {
            //std::cout << "not empty" << xCell;
            ESM::Pathgrid::Point pt = pathGrid->mPoints[current];
            pt.mX += xCell;
            pt.mY += yCell;
            path.push_front(pt);
            current = mGraph[current].parent;
        }

        if(path.empty())
        {
            ESM::Pathgrid::Point pt = pathGrid->mPoints[goal];
            pt.mX += xCell;
            pt.mY += yCell;
            path.push_front(pt);
        }

        return path;
    }

//.........这里部分代码省略.........
                  if( idx1 > 0 && idx1 < idx2 )
                    {
                      bool ok = false;
                      double visiDouble = 0.0;

                      QString visiText = line.mid(idx1 + 1, idx2 - idx1 -1);

                      if( visiText.contains("/") )
                        {
                          QStringList visiList = visiText.split("/");

                          if( visiList.size() == 2 )
                            {
                              double arg1 = visiList.at(0).toDouble(&ok);

                              if( ok )
                                {
                                  double arg2 = visiList.at(1).toDouble(&ok);

                                  if( ok && arg2 > 0.0 )
                                    {
                                      visiDouble = arg1 / arg2;
                                    }
                                }
                            }
                        }
                      else
                        {
                          visiDouble = visiText.toDouble( &ok );
                        }

                      if( ok )
                        {
                          Distance distance(0);
                          distance.setMiles( visiDouble );

                          QString visibility = line.mid( 12, idx1 - 11 );

                          if( distance.getKilometers() > 5 )
                            {
                              visibility += distance.getText( true, 0 );
                            }
                          else
                            {
                              visibility += distance.getText( true, 1 );
                            }

                          if( line.contains("mile(s)") )
                            {
                              // This must be tested as first to prevent wrong handling!
                              if( ! line.endsWith( "mile(s):0") )
                                {
                                  line.replace( ":0", "" );
                                  visibility += line.mid( line.indexOf( "mile(s)" ) + 7 );
                                }
                            }
                          else if( line.contains("mile") && ! line.endsWith( "mile:0") )
                            {
                              if( ! line.endsWith( "mile:0") )
                                {
                                  line.replace( ":0", "" );
                                  visibility += line.mid( line.indexOf( "mile" ) + 4 );
                                }
                            }

                          reportItems.insert( "visibility", visibility );

/*!
 * @brief クエストに関わるモンスターの配置を行う / Place quest monsters
 * @return 成功したならばTRUEを返す
 */
bool place_quest_monsters(void)
{
    int i;

    /* Handle the quest monster placements */
    for (i = 0; i < max_quests; i++)
    {
        monster_race *r_ptr;
        u32b mode;
        int j;

        if (quest[i].status != QUEST_STATUS_TAKEN ||
                (quest[i].type != QUEST_TYPE_KILL_LEVEL &&
                 quest[i].type != QUEST_TYPE_RANDOM) ||
                quest[i].level != dun_level ||
                dungeon_type != quest[i].dungeon ||
                (quest[i].flags & QUEST_FLAG_PRESET))
        {
            /* Ignore it */
            continue;
        }

        r_ptr = &r_info[quest[i].r_idx];

        /* Hack -- "unique" monsters must be "unique" */
        if ((r_ptr->flags1 & RF1_UNIQUE) &&
                (r_ptr->cur_num >= r_ptr->max_num)) continue;

        mode = (PM_NO_KAGE | PM_NO_PET);

        if (!(r_ptr->flags1 & RF1_FRIENDS))
            mode |= PM_ALLOW_GROUP;

        for (j = 0; j < (quest[i].max_num - quest[i].cur_num); j++)
        {
            int k;

            for (k = 0; k < SAFE_MAX_ATTEMPTS; k++)
            {
                int x = 0, y = 0;
                int l;

                /* Find an empty grid */
                for (l = SAFE_MAX_ATTEMPTS; l > 0; l--)
                {
                    cave_type    *c_ptr;
                    feature_type *f_ptr;

                    y = randint0(cur_hgt);
                    x = randint0(cur_wid);

                    c_ptr = &cave[y][x];
                    f_ptr = &f_info[c_ptr->feat];

                    if (!have_flag(f_ptr->flags, FF_MOVE) && !have_flag(f_ptr->flags, FF_CAN_FLY)) continue;
                    if (!monster_can_enter(y, x, r_ptr, 0)) continue;
                    if (distance(y, x, p_ptr->y, p_ptr->x) < 10) continue;
                    if (c_ptr->info & CAVE_ICKY) continue;
                    else break;
                }

                /* Failed to place */
                if (!l) return FALSE;

                /* Try to place the monster */
                if (place_monster_aux(0, y, x, quest[i].r_idx, mode))
                {
                    /* Success */
                    break;
                }
                else
                {
                    /* Failure - Try again */
                    continue;
                }
            }

            /* Failed to place */
            if (k == SAFE_MAX_ATTEMPTS) return FALSE;
        }
    }

    return TRUE;
}

void slice_torus (struct msscene *ms, struct surface *current_surface, double fine_pixel, double probe_radius, struct face *fac)
{
	int k, j, i, nfocus, near1, naif;
	double anginc, bigrad;
	double focus[3], vect1[3], vect2[3], vect[3], qvect[3];
	double dtq, tcv[3];
	double *foci = (double *) NULL;
	char message[MAXLINE];
	struct leaf *lf;
	struct circle *cir1, *cir2, *cir3;
	struct circle *lfcir, *torcir;
	struct variety *vty, *atm1, *atm2;
	struct arc *a, *nxta;
	struct arc *torarc;
	struct vertex *torvtx[2];
	struct vertex *qvtx;
	struct vertex *conevtx;
	struct cycle *cyc;
	struct edge *edg;
    struct cept *ex;

	vty = fac -> vty;
	if (vty -> type != TORUS) {
		ex = new_cept (GEOMETRY_ERROR,  INCONSISTENCY,  FATAL_SEVERITY);
		add_function (ex, "slice_torus");
		add_source (ex, "msrender.c");
        add_long (ex, "variety type", (long) vty -> type);
		return;
	}
	if (vty -> tube) {
		slice_elbow (ms, current_surface, fine_pixel, fac);
		return;
	}

	if (debug >= 2) {
		sprintf (message,"render saddle face for atoms %5d %5d",
			vty -> atmnum[0], vty -> atmnum[1]);
		inform(message);
	}

	/* get pointers to atom varieties */
	atm1 = *(current_surface -> variety_handles + fac -> vty -> atmnum[0] - 1);
	atm2 = *(current_surface -> variety_handles + fac -> vty -> atmnum[1] - 1);

	/* check versus window */
	bigrad = distance (atm1 -> center, atm2 -> center) +
		atm1 -> radii[0] + atm2 -> radii[0];

	for (k = 0; k < 3; k++) {
		if (vty -> center[k] + bigrad < ms -> window[0][k]) return;
		if (vty -> center[k] - bigrad > ms -> window[1][k]) return;
	}
	/* leaf circle */
	lfcir = allocate_circle ();
	if (error()) {
		add_object (tail_cept, CIRCLE, "leaf circle");
		add_function (tail_cept, "slice_torus");
		return;
	}
	/* leaf */
	lf = allocate_leaf ();
	if (error()) {
		add_object (tail_cept, LEAF, "leaf");
		add_function (tail_cept, "slice_sphere");
		return;
	}
	/* torus circle radius, center, axis */
	torcir = new_circle (vty -> center, vty -> radii[0], vty -> axis);
	if (torcir == NULL) {
		add_object (tail_cept, CIRCLE, "torus circle");
		add_function (tail_cept, "slice_circle");
		return;
	}
	/* torus arc */
	torarc = allocate_arc ();
	if (error()) {
		add_object (tail_cept, ARC, "torus arc");
		add_function (tail_cept, "slice_torus");
		add_source (tail_cept, "msrender.c");
		return;
	}
	for (j = 0; j < 2; j++) {
		torvtx[j] = allocate_vertex ();
		if (error()) {
			add_object (tail_cept, VERTEX, "torus vertex");
			add_function (tail_cept, "slice_torus");
			add_source (tail_cept, "msrender.c");
			return;
		}
	}
	torarc -> cir = torcir;
	/* copy atom numbers from variety to leaf */
	for (k = 0; k < MAXPA; k++)
		lf -> atmnum[k] = fac -> vty -> atmnum[k];

	/* set up leaf fields */
	lf -> cir = lfcir;
	lf -> shape = fac -> shape;
	lf -> type = fac -> vty -> type;
	lf -> fac = fac;
//.........这里部分代码省略.........

void SpriteBehavior() // Din kod!
{
    SpritePtr i = gSpriteRoot; 
    SpritePtr j = i->next;
    
    int count = 0;
    float dPos;


    FPoint black_hole = {0,0};
    FPoint alignmentVel = {0,0};

    while(i != NULL){
	if(i->ai_type == DOG){
	    i = i->next;
	    continue;
	}

	count = 0;
	while(j != NULL){
	    
	    dPos = distance(i->position, j->position);

	    if(dPos < maxDist){
		count++;

		// Cohesion
		if(i->ai_type == SHEEP || j->ai_type != DOG){
		    black_hole.h += j->position.h;
		    black_hole.v += j->position.v;
		}

		if(j->ai_type != DOG){
		    // Alignment
		    alignmentVel.h += j->speed.h;
		    alignmentVel.v += j->speed.v;
		}
	    }

	    // Separation
	    if(dPos < minDist && (i != j)){
		i->speed.h += 1/(dPos + 0.1)*separationCoeff*(i->position.h - j->position.h);
		i->speed.v += 1/(dPos + 0.1)*separationCoeff*(i->position.v - j->position.v);
	    }

	    if(j->ai_type == DOG && dPos < maxDist){
		i->speed.h += 1/(dPos + 0.1)*10*separationCoeff*(i->position.h - j->position.h);
		i->speed.v += 1/(dPos + 0.1)*10*separationCoeff*(i->position.v - j->position.v);
	    }

	    j = j->next;
	}

	black_hole.h /= count;
	black_hole.v /= count;

	alignmentVel.h /= count;
	alignmentVel.v /= count;

	if(i->ai_type == SHEEP){
	    i->speed.h += cohesionCoeff*(black_hole.h - i->position.h);
	    i->speed.v += cohesionCoeff*(black_hole.v - i->position.v);

	    i->speed.h += alignmentCoeff*alignmentVel.h;
	    i->speed.v += alignmentCoeff*alignmentVel.v;
	}

	i->speed.h += i->randomness*((float)random()/RAND_MAX - 0.5);
	i->speed.v += i->randomness*((float)random()/RAND_MAX - 0.5);
	
	if(normFPoint(i->speed) > maxSpeed) {
	    i->speed.h = maxSpeed*normalizeFPoint(i->speed).h;
	    i->speed.v = maxSpeed*normalizeFPoint(i->speed).v;
	}

	i = i->next;
	j = gSpriteRoot;

	black_hole.h = 0;
	black_hole.v = 0;

	alignmentVel.h = 0;
	alignmentVel.v = 0;
    }
}

QImage Triangledetection::detect(const QImage &source, const QImage &imageBase)
{
    //QImage binary = detectionContour(extraireRouge(source));
    QImage binary = detectionContour(source);
    QImage detection = binary.convertToFormat(QImage::Format_RGB888);

    QVector<QPoint> ligne = hough(detection);
    std::cout << "-> Nombre de ligne detecté : " << ligne.size() << std::endl;

    QVector<QPoint> ligne_angle0,ligne_angle60,ligne_angle120;
    QPoint inter1,inter2,inter3;
    int l1,l2,l3;



    //Avoir les lignes avec des angles pouvant appartenir à un panneau (+ ou - 1°)
    avoirLigneAngle(ligne,ligne_angle0,90,1);
    avoirLigneAngle(ligne,ligne_angle0,270,1);

    avoirLigneAngle(ligne,ligne_angle60,150,1);
    avoirLigneAngle(ligne,ligne_angle60,330,1);

    avoirLigneAngle(ligne,ligne_angle120,210,1);
    avoirLigneAngle(ligne,ligne_angle120,30,1);

    //On determine les intersections et les longueurs des segments
    for(int i=0;i<ligne_angle0.size();i++)
    {
        for(int j=0;j<ligne_angle60.size();j++)
        {
            for(int k=0;k<ligne_angle120.size();k++)
            {

                inter1 = intersection(ligne_angle0[i],ligne_angle60[j]);
                inter2 = intersection(ligne_angle60[j],ligne_angle120[k]);
                inter3 = intersection(ligne_angle120[k],ligne_angle0[i]);

                l1 = distance(inter1,inter2);
                l2 = distance(inter2,inter3);
                l3 = distance(inter3,inter1);

                //Si les distance sont les mêmes et que tous les points sont dans l'image => on a un triangle
                if(l1 == l2 && l2 == l3 && l1 > 30 && l1 < 100 && estPointImage(detection,inter1) && estPointImage(detection,inter2) && estPointImage(detection,inter3))
                {
                    Triangle a;
                    a.p1 = inter1;
                    a.p2 = inter2;
                    a.p3 = inter3;
                    liste_triangle.push_back(a);
                }
            }
        }
    }

    std::cout<<"-> Nombre de triangle detectés avant élimination des doublons : " << liste_triangle.size() << std::endl;

    //On supprime les triangle doublons
    supprimerDoublon();

    //Dessiner les triangles à l'écran
    for(int i=0;i<liste_triangle.size();i++)
        dessiner(detection,liste_triangle[i],qRgb(0,255,127));


    //Generer les images avec les cercles reconnus
    for(int i=0;i<liste_triangle.size();i++)
    {
        int minX = liste_triangle[i].p1.x();
        int minY = liste_triangle[i].p1.y();
        int maxX = liste_triangle[i].p1.x();
        int maxY = liste_triangle[i].p1.y();
        if (liste_triangle[i].p2.x()<minX) minX = liste_triangle[i].p2.x();
        if (liste_triangle[i].p2.y()<minY) minY = liste_triangle[i].p2.y();
        if (liste_triangle[i].p2.x()>maxX) maxX = liste_triangle[i].p2.x();
        if (liste_triangle[i].p2.y()>maxY) maxY = liste_triangle[i].p2.y();
        if (liste_triangle[i].p3.x()<minX) minX = liste_triangle[i].p3.x();
        if (liste_triangle[i].p3.y()<minY) minY = liste_triangle[i].p3.y();
        if (liste_triangle[i].p3.x()>maxX) maxX = liste_triangle[i].p3.x();
        if (liste_triangle[i].p3.y()>maxY) maxY = liste_triangle[i].p3.y();


        QImage BlueRoadSigns = QImage(maxX-minX, maxY-minY, QImage::Format_RGB32);

        for(int row = 0;row<maxY-minY;row++)
        {
            for (int col=0;col<maxX-minX;col++)
            {
                QColor clrCurrent(imageBase.pixel(col+minX,row+minY));

                int red = clrCurrent.red();
                int green = clrCurrent.green();
                int blue = clrCurrent.blue();

                BlueRoadSigns.setPixel(col, row, qRgb(red,green,blue));
            }
        }

        liste_TrianglesReconnu.push_back(BlueRoadSigns);
    }

//.........这里部分代码省略.........

//.........这里部分代码省略.........
						{
							meanFeature.data[featureIndex++] = (features[i].GetBlockMeanByIntegralImage(col + (col2 * k), row + (row2 * k), k, k));
							//if (col == 95 && row == 0)
							//	log.WriteLog("%f\n", meanFeature.data[i]);
						}
					}
					
				}
				//log.WriteLog("\n");
				//ClassifySoftmaxRegressionSingle(meanFeature, softmaxOptTheta, vecLabel, vecConfidence);
				//if (vecLabel[0] == 5 ) continue;
				CRectEx rt;
				rt.SetRect(0,0,k*3+8,k*3+8);
				rt.Offset(col,row);

				//m_OvrDisp.DrawRect(rt,color[vecLabel[0]], 10,"%s(%d)",className[vecLabel[0]], vecLabel[0]);
				//log.WriteLog("%s,%d, %.1f, %.1f, %.1f, %.1f,%.1f,",className[vecLabel[0]], vecLabel[0], vecConfidence[0],vecConfidence[1],vecConfidence[2],vecConfidence[3], vecConfidence[4]);

				ClassifySoftmaxRegressionSingle(meanFeature, softmaxOptTheta5320, vecLabel, vecConfidence);
				if (k==start)
					vecCandidateLabel.push_back(vecLabel[0]-1);
				else if (vecLabel[0] == candidate)
				{
					//m_OvrDisp.DrawRect(rt,color[vecLabel[0]], 10,"%s(%d) %.1f %.1f %.1f %.1f",className[vecLabel[0]], vecLabel[0], vecConfidence[0]);
					//vecRect.push_back(rt);
					rtFinal = rt;
					found = TRUE;
				}

				log.WriteLog(",%s,%d,%.1f, %.1f, %.1f, %.1f\n",className[vecLabel[0]], vecLabel[0], vecConfidence[0],vecConfidence[1],vecConfidence[2],vecConfidence[3]);
				if (vecLabel[0] != 5 && vecConfidence[0] > 0.99)		
				{					
					//ClassifySoftmaxRegression(meanFeature, softmaxOptTheta100percentTraining, vecLabel, vecConfidence);
				//	m_OvrDisp.DrawRect(rt,color[vecLabel[0]], 10,"%s(%d) %.1f %.1f %.1f %.1f",className[vecLabel[0]], vecLabel[0], vecConfidence[0]);
				//	goto FINISH;
				}
				else
				{
					//m_OvrDisp.DrawRect(rt,color[vecLabel[0]], 10,"%s(%d) %f",className[vecLabel[0]], vecLabel[0], vecConfidence[0]);
				}


				//m_OvrDisp.DrawRect(rt,MCYAN);
				//log.WriteLog("%d,%f\n",vecLabel[0],vecConfidence[0]);
				
			}
		}

		if (k == start)
		{
			vector <int> histo;
			histo.resize(4);
			for (int l=0;l<(int)vecCandidateLabel.size();l++)
				histo[vecCandidateLabel[l]]++;

			candidate = distance(histo.begin(), max_element(histo.begin(), histo.end())) + 1;
			rtCadidate.SetRect(0,0,k*3+8,k*3+8);
			//rtCadidate.Offset(col,row);
		}
		
	}

	if (found == FALSE)
		printf("%d, %d, %d %d = %s\n", rtCadidate.left,rtCadidate.top,rtCadidate.right,rtCadidate.bottom, className[candidate]);
	else
		printf("%d, %d, %d %d = %s\n", rtCadidate.left,rtFinal.top,rtFinal.right,rtFinal.bottom, className[candidate]);

	printf("Elapsed time in %.0f msec (%d x %d image)\n", w.CheckTime(),m_Image.Width(), m_Image.Height());

	/*for (row = 0;row <= rowSize-3;row+=step)
		//for (row = 0;row < 1;row++)
	{
		for (col = 0;col <= colSize-3;col+=step)
			//for (col = 0;col < 1;col++)
		{
			//w.StartTime();
			//double error = ClassifyMeans(pooledFeaturesLarge,row, col,3,3, meanFeature, vecLabel);
			//if (vecLabel[0] == 0 ) continue;
			ClassifySoftmaxRegression(convolvedFeatures,row, col,3,3, softmaxOptTheta, vecLabel, vecConfidence);
			//if (vecLabel[0] == 5) continue;
			//if (vecConfidence[0] < thr) continue;

			//m_OvrDisp.DrawText(10 + (row *10),10, MGREEN, 20, "%.0f msec", w.CheckTime());

			CRectEx rt;
			rt.SetRect(0,0,63,63);
			rt.Offset(int((float)col / 3.0f * 64.0f),int((float)row / 3.0f * 64.0f));

			m_OvrDisp.DrawRect(rt,color[vecLabel[0]], 10,"%s(%d) %f",className[vecLabel[0]], vecLabel[0], vecConfidence[0]);
			//m_OvrDisp.DrawText(rt.CenterPoint().x,rt.CenterPoint().y,MGREEN, 10,"%s",className[vecLabel[0]]);
			//m_OvrDisp.DrawRect(rt,MRED, 10,"%d,%d,%d,%d",vecLabel[0], vecLabel[1],vecLabel[2],vecLabel[3]);
			//m_OvrDisp.DrawRect(rt,MRED);
		}
	}*/

	//m_OvrDisp.DrawText(10,10, MCYAN, 15, "%d x %d %.0f msec", m_Image.Width(), m_Image.Height(),w.CheckTime());
	delete [] features;
	return;

} 

void kd_tree::neigh_check(array_1d<double> &v, int kk, 
array_1d<int> &neigh, array_1d<double> &dd, int where, int wherefrom){
    
    /*
    This routine provides the backend for nn_srch
    
    v is the point for which we are trying to find nearest neighbors
    
    kk is the number of nearest neighbors we are trying to find
    
    neigh stores the indices of the nearest neighbors
    
    dd stores the (normalized) parameter space distances from v to the nearest neighbors
    
    where indicates what node we are examining now
    
    wherefrom indicates what node we just came from (so this search does not backtrack)
    
    This routine will call itself such that it walks through the tree until all possible
    steps are ruled out (by being obviously farther away than the kkth nearest neighbor
    discovered so far).
    */
    
    int i,j,k,l,side,goon;
    double dtry,dwhere;
    
    /*on what side of where does v belong?*/
    if(v.get_data(tree.get_data(where,0))<data.get_data(where,tree.get_data(where,0)))side=1;
    else side=2;
    
    /*
    the parameter space distance between v and where in the dimension on which where splits
    the tree; if this is longer than the distance to the kkth nearest neighbor, there is no
    point in calculating the full parameter space distance bewtween v and where
    */
    dtry=fabs((v.get_data(tree.get_data(where,0))-data.get_data(where,tree.get_data(where,0)))/\
    (maxs.get_data(tree.get_data(where,0))-mins.get_data(tree.get_data(where,0))));
   
   
    if(dtry<=dd.get_data(kk-1)){
          
        dwhere=distance(where,v);
     
        goon=0;
        if(dwhere<dd.get_data(kk-1)){
            goon=1;
            //make sure that where isn't already one of the nearest neighbors
            for(k=0;k<kk;k++)if(neigh.get_data(k)==where)goon=0;
        }
     
        if(goon==1){
            //add where to the list of nearest neighbors
            for(i=kk-2;i>=0 && dd.get_data(i)>dwhere;i--){
                dd.set(i+1,dd.get_data(i));
                neigh.set(i+1,neigh.get_data(i));
            }
            i++;
        
            dd.set(i,dwhere);
            neigh.set(i,where);
        }
     
        if(wherefrom==tree.get_data(where,3) || wherefrom==tree.get_data(where,side)){
            /*inspect the other side of the tree as split by where (assuming we did not just
            come from there)*/
            if(tree.get_data(where,3-side)>-1){
                neigh_check(v,kk,neigh,dd,tree.get_data(where,3-side),where);
            }
        }
     
    }
   
    if(wherefrom==tree.get_data(where,3)){
        if(tree.get_data(where,side)>-1){
            //check the side of this node v is naturally on
            neigh_check(v,kk,neigh,dd,tree.get_data(where,side),where);
        } 
    }
    else{
        if(tree.get_data(where,3)>-1){
            //check the parent of this node, if that is not where I came from
            neigh_check(v,kk,neigh,dd,tree.get_data(where,3),where);
        }
    }
}

double kd_tree::distance(int dex, array_1d<double> &vv){
    return distance(vv,dex);
}

bool AiFollow::execute (const MWWorld::Ptr& actor, CharacterController& characterController, AiState& state, float duration)
{
    MWWorld::Ptr target = getTarget();

    if (target.isEmpty() || !target.getRefData().getCount() || !target.getRefData().isEnabled()  // Really we should be checking whether the target is currently registered
                                                                                                 // with the MechanicsManager
            )
        return false; // Target is not here right now, wait for it to return

    actor.getClass().getCreatureStats(actor).setDrawState(DrawState_Nothing);

    AiFollowStorage& storage = state.get<AiFollowStorage>();

    // AiFollow requires the target to be in range and within sight for the initial activation
    if (!mActive)
    {
        storage.mTimer -= duration;

        if (storage.mTimer < 0)
        {
            if ((actor.getRefData().getPosition().asVec3() - target.getRefData().getPosition().asVec3()).length2()
                    < 500*500
                    && MWBase::Environment::get().getWorld()->getLOS(actor, target))
                mActive = true;
            storage.mTimer = 0.5f;
        }
    }
    if (!mActive)
        return false;

    ESM::Position pos = actor.getRefData().getPosition(); //position of the actor

    float followDistance = 180;
    // When there are multiple actors following the same target, they form a group with each group member at 180*(i+1) distance to the target
    int i=0;
    std::list<int> followers = MWBase::Environment::get().getMechanicsManager()->getActorsFollowingIndices(target);
    followers.sort();
    for (std::list<int>::iterator it = followers.begin(); it != followers.end(); ++it)
    {
        if (*it == mFollowIndex)
            followDistance *= (i+1);
        ++i;
    }

    if(!mAlwaysFollow) //Update if you only follow for a bit
    {
         //Check if we've run out of time
        if (mRemainingDuration != 0)
        {
            mRemainingDuration -= duration;
            if (duration <= 0)
                return true;
        }

        if((pos.pos[0]-mX)*(pos.pos[0]-mX) +
            (pos.pos[1]-mY)*(pos.pos[1]-mY) +
            (pos.pos[2]-mZ)*(pos.pos[2]-mZ) < followDistance*followDistance) //Close-ish to final position
        {
            if(actor.getCell()->isExterior()) //Outside?
            {
                if(mCellId == "") //No cell to travel to
                    return true;
            }
            else
            {
                if(mCellId == actor.getCell()->getCell()->mName) //Cell to travel to
                    return true;
            }
        }
    }

    //Set the target destination from the actor
    ESM::Pathgrid::Point dest = target.getRefData().getPosition().pos;

    float dist = distance(dest, pos.pos[0], pos.pos[1], pos.pos[2]);
    const float threshold = 10;

    if (storage.mMoving)  //Stop when you get close
        storage.mMoving = (dist > followDistance);
    else
        storage.mMoving = (dist > followDistance + threshold);

    if(!storage.mMoving)
    {
        actor.getClass().getMovementSettings(actor).mPosition[1] = 0;

        // turn towards target anyway
        float directionX = target.getRefData().getPosition().pos[0] - actor.getRefData().getPosition().pos[0];
        float directionY = target.getRefData().getPosition().pos[1] - actor.getRefData().getPosition().pos[1];
        zTurn(actor, std::atan2(directionX,directionY), osg::DegreesToRadians(5.f));
    }
    else
    {
        pathTo(actor, dest, duration); //Go to the destination
    }

    //Check if you're far away
    if(dist > 450)
        actor.getClass().getCreatureStats(actor).setMovementFlag(MWMechanics::CreatureStats::Flag_Run, true); //Make NPC run
    else if(dist  < 325) //Have a bit of a dead zone, otherwise npc will constantly flip between running and not when right on the edge of the running threshhold
//.........这里部分代码省略.........

		void PathSearch::update(long timeslice)
		{

			startTime = clock();

			while (1)
			{

				currentTime = clock();
				float _elapsedTime = currentTime - startTime;

				if (_elapsedTime < timeslice)
				{
					SearchNode* _currentNode = searchQueue->front();
					searchQueue->pop();

					if (_currentNode != goalNode)
					{

						for (int i = 0; i < _currentNode->edges.size(); i++)
							if (!nodeState[_currentNode->edges[i]])
							{

								PlannerNode* _newPlanner = new PlannerNode;
								_newPlanner->parent = _currentNode->myPlanner;
								_newPlanner->Vertex = _currentNode->edges[i];
								_currentNode->edges[i]->myPlanner = _newPlanner;
								_currentNode->edges[i]->heuristicCost = distance(_currentNode->edges[i]->data, goalNode->data);
								_currentNode->edges[i]->givenCost = _currentNode->givenCost + distance(_currentNode->edges[i]->data, _currentNode->myPlanner->Vertex->data);
								cleanMe.push_back(_newPlanner);

								searchQueue->push(_currentNode->edges[i]);
								nodeState[_currentNode->edges[i]] = true;

							}

					}
					else
					{

						PlannerNode* _cPlanner = goalNode->myPlanner;

						while (1)
						{

							solution.push_back(_cPlanner);

							if (_cPlanner->Vertex == startNode)
								break;

							if (_cPlanner->Vertex != startNode && _cPlanner->Vertex != goalNode)
								_cPlanner->Vertex->data->setFill(0xFF0000FF);

							_cPlanner = _cPlanner->parent;

						}

						reachedGoal = true;
						return;

					}

				}
				else return;

			}

		}

Handle<Value> ZipFile::readFileSync(const Arguments& args)
{
    HandleScope scope;

    if (args.Length() != 1 || !args[0]->IsString())
        return ThrowException(Exception::TypeError(
          String::New("first argument must be a file name inside the zip")));

    std::string name = TOSTR(args[0]);
  
    // TODO - enforce valid index
    ZipFile* zf = ObjectWrap::Unwrap<ZipFile>(args.This());

    if (zf->Busy())
      return ThrowException(Exception::Error(String::New("Zipfile already in use..")));

    struct zip_file *zf_ptr;

    int idx = -1;
    
    std::vector<std::string>::iterator it = std::find(zf->names.begin(), zf->names.end(), name);
    if (it!=zf->names.end()) {
        idx = distance(zf->names.begin(), it);
    }

    if (idx == -1) {
        std::stringstream s;
        s << "No file found by the name of: '" << name << "\n";
        return ThrowException(Exception::Error(String::New(s.str().c_str())));
    }

    if ((zf_ptr=zip_fopen_index(zf->archive, idx, 0)) == NULL) {
        zip_fclose(zf_ptr);
        std::stringstream s;
        s << "cannot open file #" << idx << " in " << name << ": archive error: " << zip_strerror(zf->archive) << "\n";
        return ThrowException(Exception::Error(String::New(s.str().c_str())));
    }

    struct zip_stat st;
    zip_stat_index(zf->archive, idx, 0, &st);
  
    std::vector<unsigned char> data;
    data.clear();
    data.resize( st.size );
    
    int result =  0;
    result = (int)zip_fread( zf_ptr, reinterpret_cast<void*> (&data[0]), data.size() );

    if (result < 0) {
        zip_fclose(zf_ptr);
        std::stringstream s;
        s << "error reading file #" << idx << " in " << name << ": archive error: " << zip_file_strerror(zf_ptr) << "\n";
        return ThrowException(Exception::Error(String::New(s.str().c_str())));
    }

    #if NODE_VERSION_AT_LEAST(0,3,0)
        node::Buffer *retbuf = Buffer::New((char *)&data[0],data.size());
    #else
        node::Buffer *retbuf = Buffer::New(data.size());
        std::memcpy(retbuf->data(), (char *)&data[0], data.size());
    #endif
    
    zip_fclose(zf_ptr);
    return scope.Close(retbuf->handle_);
}

//.........这里部分代码省略.........
            {  0.0,  1.0, 1.0 },
            {  0.0, -1.0, 1.0 },
            {  1.0,  0.0, 1.0 },
            { -1.0,  0.0, 1.0 }
        };

        for (std::vector<Particle>::iterator p_it = body_.particles.begin (); p_it != body_.particles.end (); ++p_it)
        {
          for (size_t plane_i = 0; plane_i < 4; ++plane_i)
          {
            float d = planes[plane_i][0] * p_it->position[0] +
                      planes[plane_i][1] * p_it->position[1] +
                      planes[plane_i][2] - particle_radius_;

            if (d < 0.f)
              p_it->force += collision_stiffness_ * (-d) * vec2 (planes[plane_i][0],
                                                                 planes[plane_i][1]);
          }
        }
    }


    /** \todo (Part 1) Compute force of the interactive mass spring
     \li Required coefficients are given as spring_stiffness_ and spring_damping_
     */
    if (mouse_spring_.active)
    {
        Particle& particle = body_.particles[ mouse_spring_.particle_index ];

        vec2 pos0 = particle.position;
        vec2 pos1 = mouse_spring_.mouse_position;

        particle.force += -(spring_stiffness_ * norm (pos0 - pos1) +
                            spring_damping_ * dot (particle.velocity, pos0 - pos1) / norm (pos0 - pos1)) * (pos0 - pos1) / norm (pos0 - pos1);
    }


    /** \todo (Part 1) Compute spring forces
     \li Required information about springs in the scene are found in body_.springs
     \li Required coefficients are given as spring_stiffness_ and spring_damping_
     */

    for (std::vector<Spring>::iterator s_it = body_.springs.begin (); s_it != body_.springs.end (); ++s_it)
    {
      const vec2 &pos0 = s_it->particle0->position;
      const vec2 &pos1 = s_it->particle1->position;
      const vec2 &vel0 = s_it->particle0->velocity;
      const vec2 &vel1 = s_it->particle1->velocity;

      vec2 force = -(spring_stiffness_ * (norm (pos0 - pos1) - s_it->rest_length) +
                     spring_damping_ * dot (vel0 - vel1, pos0 - pos1) / norm (pos0 - pos1)) * (pos0 - pos1) / norm (pos0 - pos1);
      s_it->particle0->force += force;
      s_it->particle1->force += -force;
    }



    /** \todo (Part 2) Compute more forces in part 2 of the exercise: triangle-area forces, binding forces, etc.
     */
    if (area_forces_) {
        for (std::vector<Triangle>::iterator t_it = body_.triangles.begin (); t_it != body_.triangles.end (); ++t_it)
        {
            /*
            F_i = - k_A * (area - A) * d_area/d_i


            */

            const vec2& p0 = t_it->particle0->position;
            const vec2& p1 = t_it->particle1->position;
            const vec2& p2 = t_it->particle2->position;
            vec2 d_area_0 = perp(0.5 * (p1 - p2));
            vec2 d_area_1 = perp(0.5 * (p2 - p0));
            vec2 d_area_2 = perp(0.5 * (p0 - p1));

            float tmp = - area_stiffness_ * (t_it->area() - t_it->rest_area);

            t_it->particle0->force += tmp * d_area_0;
            t_it->particle1->force += tmp * d_area_1;
            t_it->particle2->force += tmp * d_area_2;
        }
    }


    if (equilibrium_forces_) {
        for (std::vector<Particle>::iterator p_it1 = body_.particles.begin (); p_it1 != body_.particles.end (); ++p_it1) {
            for (std::vector<Particle>::iterator p_it2 = p_it1+1; p_it2 != body_.particles.end (); ++p_it2) {
                const vec2 &p1 = p_it1->position;
                const vec2 &p2 = p_it2->position;
                float dist = distance(p1, p2);
                vec2 d = p1 - p2;
                float f = 0.1f / (dist*dist);

                p_it1->force += f*d;
                p_it2->force += -f*d;
            }
        }

    }
}