int evaluateBoard(void* voidWorld) {
World* world = (World*)voidWorld;
int mouseXPos = world->mouseXPos;
int mouseYPos = world->mouseYPos;
int catXPos = world->catXPos;
int catYPos = world->catYPos;
int cheeseXPos = world->cheeseXPos;
int cheeseYPos = world->cheeseYPos;
int catMouseDist = calcDistance(catXPos, catYPos, mouseXPos, mouseYPos, world);
int mouseCheeseDist = calcDistance(mouseXPos, mouseYPos, cheeseXPos, cheeseYPos, world);
updateGameStatus(world); // in console mode, we haven't made any moves, so we need to update
if (world->gameStatus == CAT_WON) {
return MIN_EVALUATION;
}
if (world->gameStatus == MOUSE_WON) {
return MAX_EVALUATION;
}
if (world->gameStatus == DRAW) {
return 0;
}
return (CAT_MOUSE_SCORE*catMouseDist + MOUSE_CHEESE_SCORE*mouseCheeseDist);
}
int Pathfinder::getClosestNodeDistance(int& node, list<int>& targetNodes){
int lCost;
list<int>::iterator target_it;
lCost=calcDistance(node, targetNodes.front());
for(target_it=++targetNodes.begin(); target_it!=targetNodes.end(); ++target_it)
lCost=min(lCost,calcDistance(node, *target_it));
return lCost;
}
float32 Segment::distanceTo( const Vec2& p )
{
bool withinLine;
float32 d = calcDistanceToLine( p, m_p1, m_p2, &withinLine );
if ( !(m_p1 == m_p2) && withinLine ) {
return d;
} else {
return b2Min( calcDistance( p, m_p2 ), calcDistance( p, m_p1 ) );
}
}
int calcDistance(cv::Mat FirstPatch, cv::Mat SecondPatch, cv::Mat ThirdPatch)
{
float first2Second = 0;
float first2Third = 0;
first2Second = calcDistance(FirstPatch, SecondPatch);
first2Third = calcDistance(FirstPatch, ThirdPatch);
if (first2Second <= first2Third)
return 1;
return 2;
}
int Pathfinder::getClosestNodePos(int node,vector<int>& targetNodes){
int lCost=calcDistance(node, targetNodes[0]);
int closestNodePos=0;
for(int pos=0;pos<targetNodes.size();pos++){
//cout << "CALC: " << calcDistance(node, targetNodes[pos]) << endl;
if(lCost>calcDistance(node, targetNodes[pos])){
closestNodePos=pos;
lCost=calcDistance(node, targetNodes[pos]);
}
}
return closestNodePos;
}
list<int>::iterator Pathfinder::getClosestNode(int node,list<int>& targetNodes){
int lCost=calcDistance(node, targetNodes.front());
int tempCost;
list<int>::iterator closestNode=targetNodes.begin();
for(list<int>::iterator target_it=targetNodes.begin();target_it!=targetNodes.end();target_it++){
tempCost=calcDistance(node, *target_it);
if(lCost>tempCost){
closestNode=target_it;
lCost=tempCost;
}
}
return closestNode;
}
ngl::Vec3 NGLScene::calcCohesion(Member &_toCalc)
{
unsigned int neighbours = 0;
ngl::Vec3 memPos = _toCalc.getPosition();
ngl::Vec3 coVec = ngl::Vec3(0.0f, 0.0f, 0.0f);
for(unsigned int i = 0; i < makerBirds.BirdID.size(); i++)
{
float dist = calcDistance(getMemberPosition(*makerBirds.BirdID[i]), memPos);
if(dist < 50.0f && dist != 0.0f )
{
coVec += getMemberPosition(*makerBirds.BirdID[i]);
neighbours++;
}
}
if(!neighbours)
return coVec;
coVec /= neighbours;
coVec = coVec - getMemberPosition(_toCalc);
if(coVec.lengthSquared() != 0.0f)
{
coVec.normalize();
}
return coVec;
}
ngl::Vec3 NGLScene::calcAlignment(Member &_toCalc)
{
unsigned int neighbours = 0;
ngl::Vec3 alignVec = ngl::Vec3(0.0f, 0.0f, 0.0f);
ngl::Vec3 memPos = _toCalc.getPosition();
for(unsigned int i = 0; i < makerBirds.BirdID.size(); i++)
{
float dist = calcDistance(getMemberPosition(*makerBirds.BirdID[i]), memPos);
if(dist < 50.0f && dist != 0.0f )
{
alignVec += getMemberVelocity(*makerBirds.BirdID[i]);
neighbours++;
}
}
if(neighbours == 0)
{
// std::cout<<"No neighbours\n";
return alignVec;
}
alignVec /= neighbours;
if(alignVec.lengthSquared() != 0.0f)
{
alignVec.normalize();
}
// std::cout<<"Some neighbours\n";
return alignVec;
}
int main(){
int i;
int a,b;
scanf("%d %d %d",&n,&m,&s); //값들 입력
s--; //배열은 0부터 시작하므로 1 감소
for(i=0;i<n;i++){ //인접리스트 초기화
iList[i] = (Node*)malloc(sizeof(Node));
iList[i]->next = NULL;
iList[i]->value = (i);
}
for(i=0;i<m;i++){
scanf("%d %d",&a,&b);
a--; //배열은 0부터 시작하므로 1 감소
b--;
insertNode(a,b); //쌍방 연결이므로
insertNode(b,a);//양방향으로 두번 호출한다.
}
dList[s] = 1; //시작노드의 거리를 1로 초기화
calcDistance(s); //시작노드를 기준으로 거리 계산
DFS(iList[s]); //DFS호출
for(i=0;i<n;i++){ //출력
printf("%d",rList[i]+1);
if(i!=n-1){
printf(" ");
}
}
}
ngl::Vec3 NGLScene::calcSeparation(Member &_toCalc)
{
unsigned int neighbours = 0;
ngl::Vec3 sepVec = ngl::Vec3(0.0f, 0.0f, 0.0f);
ngl::Vec3 memPos = _toCalc.getPosition();
for(unsigned int i = 0; i < makerBirds.BirdID.size(); i++)
{
float dist = calcDistance(getMemberPosition(*makerBirds.BirdID[i]), memPos);
if(dist < 50.0f && dist != 0.0f )
{
sepVec.m_x += memPos.m_x - getMemberPosition(*makerBirds.BirdID[i]).m_x;
sepVec.m_y += memPos.m_y - getMemberPosition(*makerBirds.BirdID[i]).m_y;
sepVec.m_z += memPos.m_z - getMemberPosition(*makerBirds.BirdID[i]).m_z;
neighbours++;
}
}
if(neighbours == 0)
{
return sepVec;
}
sepVec /= neighbours;
sepVec = sepVec - getMemberPosition(_toCalc);
sepVec *= -1;
if(sepVec.lengthSquared() != 0.0f)
{
sepVec.normalize();
}
return sepVec;
}
long RangeFinder::getDistance()
{
long pingSamples[PING_SAMPLES];
long microseconds;
for (int x = 0; x < PING_SAMPLES; x++)
{
pingSamples[x] = ping();
}
microseconds = filterRange(pingSamples);
#if __RANGEFINDER_H__DEBUG
Serial.print("Filtered microseconds = ");
Serial.println(microseconds);
#endif
long distance_cm = calcDistance(microseconds, 1); // Calculating the distance
#if __RANGEFINDER_H__DEBUG
Serial.print("distance CM = ");
Serial.println(distance_cm); // printf the distance about CM
#endif
return distance_cm;
}
void SoundManager::playSound(int chunkIdx, Ogre::Vector3 soundPosition, Ogre::Camera* mCamera) {
if (!initialized) {
std::cout << "SoundManager: Manager not initialized." << std::endl;
return;
}
if (sounding) {
Mix_ChannelFinished(channelDoneWrapper);
int channel = Mix_PlayChannel(-1, chunks[chunkIdx], 0);
int dist = calcDistance(mCamera->getPosition(), soundPosition);
// put this sound in our list of active sounds.
Sound s;
s.soundPosition = soundPosition;
s.chunk = chunks[chunkIdx];
s.distance = dist;
s.channel = channel;
s.active = true;
activeSounds.push_back(s);
// Initialize sound position
int rightIntensity = calcPanning(mCamera, soundPosition);
Mix_SetPanning(s.channel, 254 - rightIntensity, rightIntensity);
Mix_SetDistance(s.channel, dist);
}
}
// Looks for rectangular contours
double twoSideLengthsHeuristic(std::vector<cv::Point>& contour) {
double sideLength[contour.size()];
double minLength = 1e100;
double maxLength = 0;
double totalLength = 0;
for (size_t i = 0; i < contour.size(); i++) {
double len = calcDistance(contour[i],
contour[(i + 1) % contour.size()]);
sideLength[i] = len;
if (len < minLength) {
minLength = len;
}
if (len > maxLength) {
maxLength = len;
}
totalLength += len;
}
double total = 0, diffA, diffB;
for (size_t i = 0; i < contour.size(); i++) {
diffA = fabs(sideLength[i] - maxLength);
diffB = fabs(sideLength[i] - minLength);
total += diffA > diffB ? diffB : diffA;
}
return total / totalLength;
}
void run(Vector *v)
{
while (true)
{
char airport[80], airport2[80];
printf("\nPlease enter two airport abbreviations (XXX XXX = done): ");
scanf("%s%s", airport, airport2);
if (strcmp(airport,"XXX") == 0 && strcmp(airport2, "XXX") == 0) break;
int idx = -1, idx2= -1;
if ((idx = findAirport(v, airport)) < 0)
{
printf("%s is not a valid airport\n", airport);
}
if ((idx2 = findAirport(v, airport2)) < 0)
{
printf("%s is not a valid airport\n", airport2);
}
if (idx < 0 || idx2 < 0) continue;
int distance = calcDistance (v, idx, idx2);
int passengers = calcPassengers (v, idx, idx2);
printf("%d passengers fly from the %d miles from\n", passengers, distance);
printf("%s, %s to %s, %s\n", v->cityArray[idx].name, v->cityArray[idx].state,
v->cityArray[idx2].name, v->cityArray[idx2].state);
}
}
/* Returns true if the ball is on the edge of the ball, or inside the ball. */
bool ballHasReachedFinish(b2Vec2 ball_position) {
float finish_x = cur_level->end.x;
float finish_y = cur_level->end.y;
float ball_x = ball_position.x;
float ball_y = ball_position.y;
/* Calculate the distances between the 4 corners of the finish square and the center of the ball. */
float distance1 = calcDistance(finish_x, finish_y, ball_x, ball_y);
float distance2 = calcDistance(finish_x, finish_y, ball_x, ball_y);
float distance3 = calcDistance(finish_x, finish_y, ball_x, ball_y);
float distance4 = calcDistance(finish_x, finish_y, ball_x, ball_y);
/* If one of them is lower than the radius of the ball, the ball has reached the finish. */
return ((distance1 <= ball_radius) || (distance2 <= ball_radius) ||
(distance3 <= ball_radius) || (distance4 <= ball_radius));
}
void kMeans() {
float numChanged = 0; // Tracks how many datapoints changed clusters each iteration
double minDistance;
double distance = 0;
int curCluster;
int itercount = 0;
do {
numChanged = 0.0;
int pointIndex;
for(pointIndex = 0; pointIndex < numDatapoints; pointIndex++) { // iterate through datapoints
minDistance = LONG_MAX;
int k;
for(k = 0; k < K; k++) { // iterate throught cluster centroids
distance = calcDistance(pointIndex, k);
if(distance < minDistance) {
minDistance = distance;
curCluster = k;
}
}
if(membership[pointIndex] != curCluster) {
numChanged += 1;
membership[pointIndex] = curCluster;
}
sumDatapointAndNewCentroid(pointIndex, curCluster);
newCentroidsSize[curCluster] = newCentroidsSize[curCluster] + 1;
}
// calculate new cluster centers
calcNewCentroids();
//printCentroids();
//printf("Datapoints that changed clusters: %f\n", numChanged/numDatapoints);
} while((numChanged/numDatapoints > threshold) && itercount++ < 500);
}
std::string gilbertclassifier::lookupClosest(sfs realTimeHit){
int k = 3;
std::vector<double> closest_d(k, 100000);
std::vector<int> indices(k, 1000);
std::vector<sfs> dbFeatures = db.getFeatures();
for (int i = 0; i < dbFeatures.size(); i++){
double d = calcDistance(dbFeatures.at(i), realTimeHit);
for (int j = 0; j < k; j++){
if(d < closest_d.at(j)){
for (int m = k-1; m > j; m--){
closest_d.at(m) = closest_d.at(m-1);
indices.at(m) = indices.at(m-1);
}
closest_d.at(j) = d;
indices.at(j) = i;
break;
}
}
}
std::vector<std::string> ids(k);
for (int i = 0; i < indices.size(); i++){
ids.at(i) = dbFeatures.at(indices.at(i)).id;
}
/*For testing purposes!!*/
ids.push_back("sound1");
/************************/
std::string classification= findMostRecurringId(ids, dbFeatures);
return classification;
}
void navigateToWaypoint(float xtarget, float ytarget)
{
while(!atTarget(xtarget, x) || (!atTarget(ytarget, y)))
{
float distance = calcDistance(xtarget, ytarget);
float angle = calcAngle(xtarget, ytarget);
float driveDistance = 0; //initialise
if (distance < stepDistance)
{
driveDistance = distance;
}
else
{
driveDistance = stepDistance;
}
// co-ordinate system is different so cos and sin are swapped - take from current location not origin
float tempWaypointX = x + (driveDistance * cos(angle));
float tempWaypointY = y + (driveDistance * sin(angle));
driveToWaypoint(tempWaypointX, tempWaypointY);
monteCarlo();
eraseDisplay();
drawMap();
drawParticles();
wait1Msec(100);
}
PlayTone(784, 15);
}
void pushTarget(struct aisTargetLog *targetLog, aisP *aisPacket, struct cntyCodes *cc, gpsPos *myPos){
time_t currentTime = time(NULL);
atl *pushList = targetLog;
while(pushList->next != NULL){
pushList = pushList->next;
}
//Country code stuff
char currCnty[3];
returnCntyName(currCnty, ret1st3Dgts(aisPacket->MMSI), cc);
pushList->next = malloc(sizeof(struct aisTargetLog));
memcpy(pushList->next->vesselName, aisPacket->vesselName, sizeof(aisPacket->vesselName));
memcpy(pushList->next->cnty, currCnty, sizeof(currCnty));;
pushList->next->msgType = aisPacket->msgType;
pushList->next->MMSI = aisPacket->MMSI;
pushList->next->heading = aisPacket->heading;
pushList->next->cog = aisPacket->cog;
pushList->next->sog = aisPacket->sog;
pushList->next->lat = aisPacket->lat;
pushList->next->lon = aisPacket->lon;
pushList->next->lastUpdate = currentTime;
pushList->next->dst = calcDistance(myPos->lat, myPos->lon, aisPacket->lat, aisPacket->lon);
pushList->next->length = 0;
pushList->next->next = NULL;
}
int calcDistance(cv::Mat FirstPatch, cv::Mat SecondPatch, cv::Mat ThirdPatch, cv::Mat FourthPatch)
{
float first2Second = calcDistance(FirstPatch, SecondPatch);
float first2Third = calcDistance(FirstPatch, ThirdPatch);
float first2Fourth = calcDistance(FirstPatch, FourthPatch);
if (first2Second<=first2Third)
{
if (first2Second<=first2Fourth)
return 1;
else
return 3;
}
else if (first2Third<= first2Fourth)
return 2;
else
return 3;
}
bool Plane::isBehind(const Vec3f &p) const
{
bool result = false;
if(calcDistance(p) < 0.0f)
result = true;
return result;
}
Side Plane::testSide( Vec2f const& p , float& dist )
{
dist = calcDistance( p );
if ( dist > WallThin )
return SIDE_FRONT;
else if ( dist < -WallThin )
return SIDE_BACK;
return SIDE_IN;
}
void Pathfinder::pqAddto(t_tmp& actualNode, priority_queue<t_tmp>& pq, const int& targetNet, list<int>& targetNodes, rt_dir direction){
t_tmp tmp;
int lCost, nextNode;
list<int>::iterator target_it;
nextNode=getDir(actualNode.node, direction);
if(!(graph[nextNode].isVisited(trace_id) || (isSource(nextNode) && graph[nextNode].net!=-targetNet))){
tmp.node=nextNode;
tmp.father=actualNode.node;
tmp.costAccumulated=actualNode.costAccumulated+calcCost(nextNode, direction);
lCost=calcDistance(nextNode, targetNodes.front());
for(target_it=++targetNodes.begin(); target_it!=targetNodes.end(); ++target_it)
lCost=min(lCost,calcDistance(nextNode, *target_it));
tmp.aStarCost=tmp.costAccumulated+lCost;
// cout << "(" << getPosX(actualNode.node) << " " <<getPosY(actualNode.node)<< " " <<getPosZ(actualNode.node)<< ") --(" << tmp.aStarCost <<")--> (" << getPosX(tempNode.node) << " " <<getPosY(tempNode.node)<< " " <<getPosZ(tempNode.node)<< ") " << endl;
pq.push(tmp);
}
}
void moveForwardSquare()
{
double xDist = 0;
double yDist = 0;
double angle = 0;
double distance = 0;
int prevTicksLeft = 0;
int prevTicksRight = 0;
drive_getTicks(&prevTicksLeft, &prevTicksRight);
while(distance < GRID_SIZE) {
getIR();
int changeVal;
if(irLeft < SENSOR_VALUE && irRight < SENSOR_VALUE){ // Wall either side
changeVal = (irLeft - irRight) * MULTIPLIER;
} else if ( irLeft < SENSOR_VALUE ) { // Wall to the left
changeVal = (irLeft - IR_LEFT) * MULTIPLIER;
} else if ( irRight < SENSOR_VALUE ) { // Wall to the right
changeVal = (IR_RIGHT - irRight) * MULTIPLIER;
} else { // If no walls to the side, carry on as normal
changeVal = 0;
}
drive_speed(BASE_SPEED_TICKS - changeVal, BASE_SPEED_TICKS + changeVal); // Set the new drive speed with the new changeVal
int ticksLeft, ticksRight;
drive_getTicks(&ticksLeft, &ticksRight); // get current ticks count
// calculate distances
double distRight = calcDistance(ticksRight, prevTicksRight); // Distance of left wheel
double distLeft = calcDistance(ticksLeft, prevTicksLeft); // Distance of right wheel
double distCentre = (distRight + distLeft) / 2; // The average of the left and right distance
angle = angle + (distRight - distLeft) / ROBOT_WIDTH;
// update prevTicks
prevTicksLeft = ticksLeft;
prevTicksRight = ticksRight;
xDist = xDist + distCentre * cos(angle);
yDist = yDist + distCentre * sin(angle);
distance = sqrt(xDist*xDist + yDist*yDist); // work out distance travelled using pythagoras
}
drive_speed(0,0);
}
void PizzaSpeedLevel::update(float df)
{
if (_timeRunning)
{
_spaceShip->setPosition(
_spaceShip->getPosition().x-df*50.0F*_timeSlider->getPercent()/10,
_spaceShip->getPosition().y+df*24.0F*_timeSlider->getPercent()/10 );
if (calcDistance(90,470,_spaceShip->getPosition().x,_spaceShip->getPosition().y)<10000) endLevel();
}
}
Position findClosest(Position p1, Position p2, apr_uint32_t movement)
{
double d = calcDistance(p1, p2);
if (d <= movement)
return p2;
double ratio = movement / d;
apr_int32_t dx = apr_int32_t((p2.first - p1.first) * ratio);
apr_int32_t dy = apr_int32_t((p2.second - p1.second) * ratio);
return Position(p1.first + dx, p1.second + dy);
}
int16_t calcSpeed(int16_t Xacc, int16_t Zacc, int16_t *distance) {
timeDiff = os_timerGetMs() - timeDiff;
X.curAcc = Xacc;
Z.curAcc = Zacc;
X.prevSpeed = X.curSpeed;
Z.prevSpeed = Z.curSpeed;
X.curSpeed = (timeDiff * X.curAcc) + X.prevSpeed;
Z.curSpeed = (timeDiff * Z.curAcc) + Z.prevSpeed;
*distance = calcDistance();
return X.curSpeed;
}
void m_Camera::Update(float deltaSeconds)
{
if (!this->bIsFollowing)
{
return;
}
//std::cout << "Camera XYZ: " << this->eye.x << " " << this->eye.y << " " << this->eye.z << std::endl;
// Find out where are target is...
m_Vec3d targetLocation;
if (!this->m_pTheMediator->GetPositionByID(this->object_to_follow_ID, targetLocation))
{ // No target with that ID, yo!
this->bIsFollowing = false;
return;
}
// Point at target
this->target = targetLocation;
// See how far away from the object we are.
float distanceToTarget = calcDistance(this->eye, this->target);
if (distanceToTarget > this->minFollowDistance)
{
// Move towards the object at speed
float changeThisStep = deltaSeconds * this->followSpeed;
if (abs(distanceToTarget - this->minFollowDistance) < 1.0f)
{
// Scale based on this value
changeThisStep = changeThisStep * abs(distanceToTarget - this->minFollowDistance);
}
if (this->eye.x < this->target.x) { this->eye.x += changeThisStep; }
else if (this->eye.x > this->target.x) { this->eye.x -= changeThisStep; }
// Calculate min height ABOVE the target
float targetHeight = this->target.y + this->minHeight;
if (this->eye.y < targetHeight) { this->eye.y += changeThisStep; }
else if (this->eye.y > targetHeight) { this->eye.y -= changeThisStep; }
if (this->eye.z < this->target.z) { this->eye.z += changeThisStep; }
else if (this->eye.z > this->target.z) { this->eye.z -= changeThisStep; }
}
return;
}
int main()
{
Point *a, *b;
int x, y, z;
printf("Digite tres inteiros do primeiro ponto\n");
scanf("%d %d %d", &x, &y, &z);
a = initPoint(x, y, z);
printf("Mais tres inteiros\n");
scanf("%d %d %d", &x, &y, &z);
b = initPoint(x, y, z);
printf("Distancia dos dois pontos: %f", calcDistance(a, b));
free(a);
free(b);
return 0;
}
double getDistance(QLineF line, QPointF pt)
{
//! 線分lineと点ptとの距離Lを求める
//! 参考：http://katahiromz.web.fc2.com/c/lineseg.html
QPointF AB, AP;
QPointF A = line.p1();
QPointF B = line.p2();
AB.setX(B.x() - A.x());
AB.setY(B.y() - A.y());
AP.setX(pt.x() - A.x());
AP.setY(pt.y() - A.y());
//! 線分の長さが0だった場合
if(AB.x() == 0 && AB.y() == 0){
return calcDistance(A, pt);
}
//! 通常の場合の計算
double r2 = AB.x() * AB.x() + AB.y() * AB.y();
double t = (AB.x() * AP.x() + AB.y() * AP.y()) / r2;
if(t < 0){
return calcDistance(A, pt);
}
else if(t > 1){
return calcDistance(B, pt);
}
else{
QPointF C;
C.setX((1 - t) * A.x() + t * B.x());
C.setY((1 - t) * A.y() + t * B.y());
return calcDistance(C, pt);
}
}
