<commit_before>/*
Copyright (C) 2007 <SWGEmu>

This File is part of Core3.

This program is free software; you can redistribute
it and/or modify it under the terms of the GNU Lesser
General Public License as published by the Free Software
Foundation; either version 2 of the License,
or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for
more details.

You should have received a copy of the GNU Lesser General
Public License along with this program; if not, write to
the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

Linking Engine3 statically or dynamically with other modules
is making a combined work based on Engine3.
Thus, the terms and conditions of the GNU Lesser General Public License
cover the whole combination.

In addition, as a special exception, the copyright holders of Engine3
give you permission to combine Engine3 program with free software
programs or libraries that are released under the GNU LGPL and with
code included in the standard release of Core3 under the GNU LGPL
license (or modified versions of such code, with unchanged license).
You may copy and distribute such a system following the terms of the
GNU LGPL for Engine3 and the licenses of the other code concerned,
provided that you include the source code of that other code when
and as the GNU LGPL requires distribution of source code.

Note that people who make modified versions of Engine3 are not obligated
to grant this special exception for their modified versions;
it is their choice whether to do so. The GNU Lesser General Public License
gives permission to release a modified version without this exception;
this exception also makes it possible to release a modified version
which carries forward this exception.
*/

#include <limits>
#include "ConversationObserver.h"
#include "server/zone/objects/player/PlayerObject.h"
#include "server/zone/managers/conversation/ConversationManager.h"
#include "server/zone/objects/player/sessions/ConversationSession.h"

ConversationObserverImplementation::ConversationObserverImplementation(ConversationTemplate* conversationTemplate) {
	this->conversationTemplate = conversationTemplate;
}

int ConversationObserverImplementation::notifyObserverEvent(unsigned int eventType, Observable* observable, ManagedObject* arg1, long long arg2) {
	//Verify needed parameters
	if (eventType != ObserverEventType::CONVERSE && eventType != ObserverEventType::STARTCONVERSATION &&
		eventType != ObserverEventType::SELECTCONVERSATION && eventType != ObserverEventType::STOPCONVERSATION) {
		//Incorrect event type.
		return 0;
	}
	if (observable == NULL || arg1 == NULL) {
		return 0;
	}

	//Try to convert parameters to correct types.
	CreatureObject* npc;
	CreatureObject* player;
	int selectedOption;
	try {
		npc = cast<CreatureObject* >(observable);
		player = cast<CreatureObject* >(arg1);
		if (arg2 < std::numeric_limits<int>::min()) {
			selectedOption = std::numeric_limits<int>::min();
		} else if (arg2 > std::numeric_limits<int>::max()) {
			selectedOption = std::numeric_limits<int>::max();
		} else {
			selectedOption = arg2;
		}
	}
	catch (...) {
		//Failed to convert parameters. Keep observer.
		return 0;
	}

	//Call event method if conversation ended.
	if (eventType == ObserverEventType::STOPCONVERSATION) {
		//Cancel any active session.
		cancelConversationSession(player);
		//Keep observer.
		return 0;
	} else {
		if (eventType == ObserverEventType::STARTCONVERSATION) {
			//Cancel any existing sessions.
			cancelConversationSession(player);
			//Create a new session.
			createConversationSession(player);
		}
		//Select next conversation screen.
		Reference<ConversationScreen*> conversationScreen = getNextConversationScreen(player, selectedOption, npc);

		if (conversationScreen != NULL) {
			//Modify the conversation screen.
			conversationScreen = runScreenHandlers(player, npc, selectedOption, conversationScreen);
		}

		//Send the conversation screen to the player.
		sendConversationScreenToPlayer(player, npc, conversationScreen);

		if (conversationScreen == NULL)
			cancelConversationSession(player);
	}

	//Keep the observer.
	return 0;
}

void ConversationObserverImplementation::createConversationSession(CreatureObject* conversingPlayer) {
	conversingPlayer->addActiveSession(SessionFacadeType::CONVERSATION, new ConversationSession());
}

void ConversationObserverImplementation::cancelConversationSession(CreatureObject* conversingPlayer) {
	ManagedReference<Facade*> session = conversingPlayer->getActiveSession(SessionFacadeType::CONVERSATION);

	if (session != NULL) {
		session->cancelSession();
	}

	conversingPlayer->dropActiveSession(SessionFacadeType::CONVERSATION);
}

ConversationScreen* ConversationObserverImplementation::getNextConversationScreen(CreatureObject* conversingPlayer, int selectedOption, CreatureObject* conversingNPC) {
	//Get screen ID from last conversation screen.
	Reference<ConversationSession*> session = cast<ConversationSession* >(conversingPlayer->getActiveSession(SessionFacadeType::CONVERSATION));
	/*String lastScreenId = "";
	if (session != NULL) {
		lastScreenId = session->getLastConversationScreenName();
	}*/

	//Get last conversation screen.
	Reference<ConversationScreen* > lastConversationScreen = session->getLastConversationScreen();
	Reference<ConversationScreen* > nextConversationScreen;

	if (lastConversationScreen != NULL) {
		//Get the linked screen for the selected option.
		nextConversationScreen = conversationTemplate->getScreen(lastConversationScreen->getOptionLink(selectedOption));
	} else {
		//Get the initial screen.
		nextConversationScreen = conversationTemplate->getInitialScreen();
	}
	return nextConversationScreen;
}

ConversationScreen* ConversationObserverImplementation::runScreenHandlers(CreatureObject* conversingPlayer, CreatureObject* conversingNPC, int selectedOption, ConversationScreen* conversationScreen) {
	ConversationScreen* modifiedScreen = NULL;

	//Loop until a screen has been modified.
	while (modifiedScreen == NULL) {
		//Call appropriate screen handler.
		if (screenHandlers.contains(conversationScreen->getScreenID())) {
			//Copy the conversation screen to preserve the template.
			modifiedScreen = screenHandlers.get(conversationScreen->getScreenID())->handleScreen(conversingPlayer, conversingNPC, selectedOption, new ConversationScreen(*conversationScreen));

			//Check if another screen should be handled
			if (modifiedScreen == NULL) {
				//Get next screen.
				String screenId = screenHandlers.get(conversationScreen->getScreenID())->getNextScreenId();
				conversationScreen = getConversationScreen(screenId);
				if (conversationScreen == NULL) {
					//No new screen found.
					return NULL;
				}
			}
		} else {
			//No screen handler found for this screen, exit loop and return it unmodified.
			modifiedScreen = conversationScreen;
		}
	}

	return modifiedScreen;
}

void ConversationObserverImplementation::sendConversationScreenToPlayer(CreatureObject* conversingPlayer, CreatureObject* conversingNPC, ConversationScreen* conversationScreen) {
	if (conversationScreen != NULL) {
		//Send the screen to the player.
		conversationScreen->sendTo(conversingPlayer, conversingNPC);
	} else {
		//Clear screen ID from last conversation screen.
		ConversationSession* session = cast<ConversationSession* >(conversingPlayer->getActiveSession(SessionFacadeType::CONVERSATION));
		if (session != NULL) {
			session->setLastConversationScreen(NULL);
		}

		conversingPlayer->sendMessage(new StopNpcConversation(conversingPlayer, conversingNPC->getObjectID()));
	}
}
<commit_msg>[fixed] stability issues<commit_after>/*
Copyright (C) 2007 <SWGEmu>

This File is part of Core3.

This program is free software; you can redistribute
it and/or modify it under the terms of the GNU Lesser
General Public License as published by the Free Software
Foundation; either version 2 of the License,
or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for
more details.

You should have received a copy of the GNU Lesser General
Public License along with this program; if not, write to
the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

Linking Engine3 statically or dynamically with other modules
is making a combined work based on Engine3.
Thus, the terms and conditions of the GNU Lesser General Public License
cover the whole combination.

In addition, as a special exception, the copyright holders of Engine3
give you permission to combine Engine3 program with free software
programs or libraries that are released under the GNU LGPL and with
code included in the standard release of Core3 under the GNU LGPL
license (or modified versions of such code, with unchanged license).
You may copy and distribute such a system following the terms of the
GNU LGPL for Engine3 and the licenses of the other code concerned,
provided that you include the source code of that other code when
and as the GNU LGPL requires distribution of source code.

Note that people who make modified versions of Engine3 are not obligated
to grant this special exception for their modified versions;
it is their choice whether to do so. The GNU Lesser General Public License
gives permission to release a modified version without this exception;
this exception also makes it possible to release a modified version
which carries forward this exception.
*/

#include <limits>
#include "ConversationObserver.h"
#include "server/zone/objects/player/PlayerObject.h"
#include "server/zone/managers/conversation/ConversationManager.h"
#include "server/zone/objects/player/sessions/ConversationSession.h"

ConversationObserverImplementation::ConversationObserverImplementation(ConversationTemplate* conversationTemplate) {
	this->conversationTemplate = conversationTemplate;
}

int ConversationObserverImplementation::notifyObserverEvent(unsigned int eventType, Observable* observable, ManagedObject* arg1, long long arg2) {
	//Verify needed parameters
	if (eventType != ObserverEventType::CONVERSE && eventType != ObserverEventType::STARTCONVERSATION &&
		eventType != ObserverEventType::SELECTCONVERSATION && eventType != ObserverEventType::STOPCONVERSATION) {
		//Incorrect event type.
		return 0;
	}
	if (observable == NULL || arg1 == NULL) {
		return 0;
	}

	//Try to convert parameters to correct types.
	CreatureObject* npc;
	CreatureObject* player;
	int selectedOption;
	try {
		npc = cast<CreatureObject* >(observable);
		player = cast<CreatureObject* >(arg1);
		if (arg2 < std::numeric_limits<int>::min()) {
			selectedOption = std::numeric_limits<int>::min();
		} else if (arg2 > std::numeric_limits<int>::max()) {
			selectedOption = std::numeric_limits<int>::max();
		} else {
			selectedOption = arg2;
		}
	}
	catch (...) {
		//Failed to convert parameters. Keep observer.
		return 0;
	}

	//Call event method if conversation ended.
	if (eventType == ObserverEventType::STOPCONVERSATION) {
		//Cancel any active session.
		cancelConversationSession(player);
		//Keep observer.
		return 0;
	} else {
		if (eventType == ObserverEventType::STARTCONVERSATION) {
			//Cancel any existing sessions.
			cancelConversationSession(player);
			//Create a new session.
			createConversationSession(player);
		}
		//Select next conversation screen.
		Reference<ConversationScreen*> conversationScreen = getNextConversationScreen(player, selectedOption, npc);

		if (conversationScreen != NULL) {
			//Modify the conversation screen.
			conversationScreen = runScreenHandlers(player, npc, selectedOption, conversationScreen);
		}

		//Send the conversation screen to the player.
		sendConversationScreenToPlayer(player, npc, conversationScreen);

		if (conversationScreen == NULL)
			cancelConversationSession(player);
	}

	//Keep the observer.
	return 0;
}

void ConversationObserverImplementation::createConversationSession(CreatureObject* conversingPlayer) {
	conversingPlayer->addActiveSession(SessionFacadeType::CONVERSATION, new ConversationSession());
}

void ConversationObserverImplementation::cancelConversationSession(CreatureObject* conversingPlayer) {
	ManagedReference<Facade*> session = conversingPlayer->getActiveSession(SessionFacadeType::CONVERSATION);

	if (session != NULL) {
		session->cancelSession();
	}

	conversingPlayer->dropActiveSession(SessionFacadeType::CONVERSATION);
}

ConversationScreen* ConversationObserverImplementation::getNextConversationScreen(CreatureObject* conversingPlayer, int selectedOption, CreatureObject* conversingNPC) {
	//Get screen ID from last conversation screen.
	Reference<ConversationSession*> session = cast<ConversationSession* >(conversingPlayer->getActiveSession(SessionFacadeType::CONVERSATION));
	/*String lastScreenId = "";
	if (session != NULL) {
		lastScreenId = session->getLastConversationScreenName();
	}*/

	//Get last conversation screen.
	Reference<ConversationScreen* > lastConversationScreen;

	if (session != NULL)
		lastConversationScreen = session->getLastConversationScreen();

	Reference<ConversationScreen* > nextConversationScreen;

	if (lastConversationScreen != NULL) {
		//Get the linked screen for the selected option.
		nextConversationScreen = conversationTemplate->getScreen(lastConversationScreen->getOptionLink(selectedOption));
	} else {
		//Get the initial screen.
		nextConversationScreen = conversationTemplate->getInitialScreen();
	}
	return nextConversationScreen;
}

ConversationScreen* ConversationObserverImplementation::runScreenHandlers(CreatureObject* conversingPlayer, CreatureObject* conversingNPC, int selectedOption, ConversationScreen* conversationScreen) {
	ConversationScreen* modifiedScreen = NULL;

	//Loop until a screen has been modified.
	while (modifiedScreen == NULL) {
		//Call appropriate screen handler.
		if (screenHandlers.contains(conversationScreen->getScreenID())) {
			//Copy the conversation screen to preserve the template.
			modifiedScreen = screenHandlers.get(conversationScreen->getScreenID())->handleScreen(conversingPlayer, conversingNPC, selectedOption, new ConversationScreen(*conversationScreen));

			//Check if another screen should be handled
			if (modifiedScreen == NULL) {
				//Get next screen.
				String screenId = screenHandlers.get(conversationScreen->getScreenID())->getNextScreenId();
				conversationScreen = getConversationScreen(screenId);
				if (conversationScreen == NULL) {
					//No new screen found.
					return NULL;
				}
			}
		} else {
			//No screen handler found for this screen, exit loop and return it unmodified.
			modifiedScreen = conversationScreen;
		}
	}

	return modifiedScreen;
}

void ConversationObserverImplementation::sendConversationScreenToPlayer(CreatureObject* conversingPlayer, CreatureObject* conversingNPC, ConversationScreen* conversationScreen) {
	if (conversationScreen != NULL) {
		//Send the screen to the player.
		conversationScreen->sendTo(conversingPlayer, conversingNPC);
	} else {
		//Clear screen ID from last conversation screen.
		ConversationSession* session = cast<ConversationSession* >(conversingPlayer->getActiveSession(SessionFacadeType::CONVERSATION));
		if (session != NULL) {
			session->setLastConversationScreen(NULL);
		}

		conversingPlayer->sendMessage(new StopNpcConversation(conversingPlayer, conversingNPC->getObjectID()));
	}
}
<|endoftext|>


<commit_before>
/**
* @file GradientGoalDetector.cpp
*
* Implementation of class GradientGoalDetector
*
*/

#include "GradientGoalDetector.h"

#include "Tools/DataStructures/ArrayQueue.h"
#include "Tools/CameraGeometry.h"

#include "Tools/Debug/Stopwatch.h"

#include "Tools/ImageProcessing/BresenhamLineScan.h"

#include <Representations/Infrastructure/CameraInfoConstants.h>

GradientGoalDetector::GradientGoalDetector()
:
  cameraID(CameraInfo::Bottom)
{
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markPeaks", "mark found maximum u-v peaks in image", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:draw_scanlines","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:draw_response","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:draw_difference","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markFootScans","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markFootScanResponse","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markFootScanGoodPoints","..", false);

  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:use_horizon","..", false);
}

void GradientGoalDetector::execute(CameraInfo::CameraID id, bool horizon)
{
  cameraID = id;
  CANVAS_PX(cameraID);

  getGoalPercept().reset();

  //if(cameraID == CameraInfo::Top)
  {
    Vector3d zem = CameraGeometry::imagePixelToWorld(getCameraMatrix(), getImage().cameraInfo,
      getImage().cameraInfo.getOpticalCenterX(), getImage().cameraInfo.getOpticalCenterY(),
      300);

    zem.y = 0;
    zem.x = 500;
    zem.z = getCameraMatrix().translation.z;

    Vector2<int> pp = CameraGeometry::relativePointToImage(getCameraMatrix(), getImage().cameraInfo,zem);

    if(pp.x > 40 && pp.x < 640 - 40 && pp.y > 0 && pp.y < 480)
    {
      LINE_PX(ColorClasses::red, pp.x - 20, pp.y, pp.x+20, pp.y);
    }
  }

  Vector2d p1(0, getImage().cameraInfo.getOpticalCenterY());
  Vector2d p2(getImage().cameraInfo.resolutionWidth, getImage().cameraInfo.getOpticalCenterY());
  Vector2d direction(1,0);

  if(horizon) {
    p1 = getArtificialHorizon().begin();
    p2 = getArtificialHorizon().end();
    direction = getArtificialHorizon().getDirection();
  }

  int imageBorderOffset = 25;
  int heightOfHorizon = (int) ((p1.y + p2.y) * 0.5 + 0.5);
  // image over the horizon
  if(heightOfHorizon > (int) getImage().cameraInfo.resolutionHeight - 10)
  {
    return;
  }

  // adjust the scan height in case of the bottom camera
  //if(cameraID == CameraInfo::Bottom)
  {
    // clamp the scanline
    p1.y = Math::clamp((int) p1.y, imageBorderOffset + 5, (int)getImage().cameraInfo.resolutionHeight - imageBorderOffset - 5);
    p2.y = Math::clamp((int) p2.y, imageBorderOffset + 5 , (int)getImage().cameraInfo.resolutionHeight - imageBorderOffset - 5);
  }

  //double horizont = min(p1.y, p2.y);

  double threshold = params.gradientThreshold;
  double thresholdY = params.minY;

  double response = 0.0;
  double responseY = 0.0;
  double lastResponse = 0.0;
  double lastResponseY = 0.0;

  Pixel pixel;

  int aktIdx = 0;

  for(int i = 0; i < 5; i++)
  {
     features[i].clear();
  }

  if
  (
    p1.y > imageBorderOffset &&
    p2.y > imageBorderOffset &&
    p1.y < (int) getImage().height() - imageBorderOffset &&
    p2.y < (int) getImage().height() - imageBorderOffset
  )
  {
    bool isCandidate = false;
    bool isObstacle = false;

    Feature candidate;

    int diffVU = 0;
    int lastDiffVU = 0;

    for(int y = (int) p1.y - 12; y <= (int) p1.y + 12; y += 6)
    {
      valueBuffer.init();
      valueBufferY.init();
      pointBuffer.init();
      isCandidate = false;
      Vector2<int> pos((int) p1.x + 2, y);
      BresenhamLineScan scanner(pos, direction, getImage().cameraInfo);
      do
      {
        pointBuffer.add(pos);
        getImage().get(pos.x, pos.y, pixel);
        diffVU = (int) pixel.v - (int) pixel.u;
        valueBuffer.add(diffVU);
        valueBufferY.add(pixel.y);

        if(pos.x > 3)
        {
          response = valueBuffer[4] + 2 * valueBuffer[3]  + 4 * valueBuffer[2] + valueBuffer[1] * 2 + valueBuffer[0];
          //response = valueBuffer[4] + 2 * valueBuffer[3]  - valueBuffer[1] * 2 - valueBuffer[0];
          responseY = valueBufferY[2];
          response /= 10;
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:draw_scanlines",
            if(aktIdx == 2)
            {
              POINT_PX(ColorClasses::yellow, pos.x, pos.y );
            }
            else
            {
              POINT_PX(ColorClasses::gray, pos.x, pos.y );
            }
          );
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:draw_difference",
            if(aktIdx == 2)
            {
              LINE_PX(ColorClasses::green, (int) pointBuffer[3].x, (int) pointBuffer[3].y - lastDiffVU, (int) pointBuffer[4].x, (int) pointBuffer[4].y - diffVU);
              lastDiffVU = diffVU;
            }
          );
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:draw_response",
            if(aktIdx == 2)
            {
              LINE_PX(ColorClasses::red, (int) pointBuffer[1].x, (int) pointBuffer[1].y - (int) /*fabs*/(lastResponse), (int) pointBuffer[2].x, (int) pointBuffer[2].y - (int)/* fabs*/(response) );
              LINE_PX(ColorClasses::white, (int) pointBuffer[1].x, (int) pointBuffer[1].y - (int) fabs(lastResponseY / 3), (int) pointBuffer[2].x, (int) pointBuffer[2].y - (int) fabs(responseY / 3) );
              lastResponse = response;
              lastResponseY = responseY;
            }
          );
          if(/*fabs*/(response) >= threshold && fabs(responseY) >= thresholdY)
          {
            if(!isCandidate)
            {
              candidate.begin = pointBuffer[2];
              candidate.center = pointBuffer[2];
              candidate.end = pointBuffer[2];
              candidate.responseAtBegin.x = response;
              candidate.responseAtBegin.y = diffVU;
              if(diffVU < 0)
              {
                isObstacle = true;
              }
            }
            isCandidate = true;
            if(isObstacle)
            {
              candidate.possibleObstacle = isObstacle;
              DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
                POINT_PX(ColorClasses::orange, pos.x - 2, pos.y);
              );
            }
            else
            {
              DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
                POINT_PX(ColorClasses::blue, pos.x - 2, pos.y);
              );
            }
          }
          else
          {
            if(isCandidate)
            {
              candidate.end = pointBuffer[2];
              candidate.center = (candidate.end + candidate.begin) / 2;
              candidate.responseAtEnd.x = response;
              candidate.responseAtEnd.y = diffVU;
              features[aktIdx].push_back(candidate);
            }
            isCandidate = false;
            isObstacle = false;
          }//end if
        }
      }
      while(scanner.getNextWithCheck(pos));
      aktIdx++;
    }//end for
  }//end if
  Vector2<double> dir(-direction.y, direction.x);

  memset(&lastTestFeatureIdx, 0, sizeof(lastTestFeatureIdx));
  //bool goalPostFound = false;
  goalPosts.clear();

  //std::cout << std::endl << " ------ " << std::endl;
  for(unsigned i = 0; i < features[0].size(); i++)
  {
    const Feature& candidate = features[0][i];

    BresenhamLineScan footPointScanner(candidate.center, dir, getImage().cameraInfo);

    int goodPointsCount = 1;

    Vector2<int> pos = candidate.center;
    valueBuffer.init();
    valueBufferY.init();
    pointBuffer.init();

    Vector2d last(pos);

    for(int y = 1; y < 5; y++)
    {
      bool stop = false;
      for(int yy = 0; yy < 5; yy++)
      {
        if(footPointScanner.getNextWithCheck(pos))
        {
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
            POINT_PX(ColorClasses::skyblue, pos.x, pos.y);
          );
          pointBuffer.add(pos);
          getImage().get(pos.x, pos.y, pixel);
          int diffVU = (int) pixel.v - (int) pixel.u;
          valueBuffer.add(diffVU);
          valueBufferY.add(pixel.y);

          if(yy > 0)
          {
            response = valueBuffer[4] + 2 * valueBuffer[3]  + 4 * valueBuffer[2] + valueBuffer[1] * 2 + valueBuffer[0];
            //response = valueBuffer[4] + 2 * valueBuffer[3]  - valueBuffer[1] * 2 - valueBuffer[0];
            responseY = valueBufferY[2];
            response /= 10;
            if(/*fabs*/(response) < params.responseHoldFactor * threshold || fabs(responseY) < params.responseHoldFactor * thresholdY)
            {
              stop = true;
            }
          }
        }
      }
      unsigned j = lastTestFeatureIdx[y];
      while (!stop && j < features[y].size())
      {
        int dist = (pos - features[y][j].center).abs();
        if(dist < params.dist)
        {
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScanGoodPoints",
            POINT_PX(ColorClasses::red, features[y][j].center.x, features[y][j].center.y);
          );
          lastTestFeatureIdx[y] = j;
          goodPointsCount++;
          stop = true;
           //std::cout << "good (" << i << ") " << dist << " pos: " << pos << " point:" << features[y][j].center << std::endl;
        }
        j++;
      }
    }//end for

    if(goodPointsCount >= params.minGoodPoints)
    {

      bool footPointFound = false;
      bool stop = false;
      Pixel pixel1;
      Pixel pixel2;
      Pixel pixel3;

      int lastDiffVU = 0;
      int lastResponse = 0;

      while(!stop && footPointScanner.getNextWithCheck(pos))
      {
        pointBuffer.add(pos);
        getImage().get(pos.x, pos.y, pixel);
        int diffVU = (int) pixel.v - (int) pixel.u;
        valueBuffer.add(diffVU);
        valueBufferY.add(pixel.y);

        response = valueBuffer[4] + 2 * valueBuffer[3]  + 4 * valueBuffer[2] + valueBuffer[1] * 2 + valueBuffer[0];
        //response = valueBuffer[4] + 2 * valueBuffer[3]  - valueBuffer[1] * 2 - valueBuffer[0];
        responseY = valueBufferY[2];
        response /= 10;

        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
          if(getFieldColorPercept().isFieldColor(pixel))
          {
             POINT_PX(ColorClasses::green, (int) pointBuffer[2].x, (int) pointBuffer[2].y);
          }
          else
          {
            POINT_PX(ColorClasses::gray, (int) pointBuffer[2].x, (int) pointBuffer[2].y);
          }
        );
        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScanResponse",
          LINE_PX(ColorClasses::white, (int) pointBuffer[1].x + lastResponse, (int) pointBuffer[1].y, (int) pointBuffer[2].x + (int) response, (int) pointBuffer[2].y);
          LINE_PX(ColorClasses::orange, (int) pointBuffer[1].x + lastDiffVU, (int) pointBuffer[1].y, (int) pointBuffer[2].x + diffVU, (int) pointBuffer[2].y);
        );
        lastDiffVU = diffVU;
        lastResponse = (int) response;

        getImage().get((int) pointBuffer[4].x, (int) pointBuffer[4].y, pixel1);
        getImage().get((int) pointBuffer[3].x, (int) pointBuffer[3].y, pixel2);
        getImage().get((int) pointBuffer[2].x, (int) pointBuffer[2].y, pixel3);
        if(/*fabs*/(response) < params.responseHoldFactor * threshold || fabs(responseY) < params.responseHoldFactor * thresholdY)
        {
          stop = true;
          if
          (
            //c > params.minScanPointsAfterGoodPoints &&
            (getFieldColorPercept().isFieldColor(pixel1) || getFieldColorPercept().isFieldColor(pixel2) || getFieldColorPercept().isFieldColor(pixel3))
          )
          {
            footPointFound = true;
          }
          else
          {
            int t = 0;
            int count = 0;
            while( t < 10 && footPointScanner.getNextWithCheck(pos))
            {
              getImage().get(pos.x, pos.y, pixel);
              if(getFieldColorPercept().isFieldColor(pixel))
              {
                count++;
              }
              DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
                if(getFieldColorPercept().isFieldColor(pixel))
                {
                   POINT_PX(ColorClasses::green, pos.x, pos.y);
                }
                else
                {
                  POINT_PX(ColorClasses::black, pos.x, pos.y);
                }
              );
              t++;
            }
            if(count > 4)
            {
              footPointFound = true;
            }
          }
        }
      }//end while

      if(footPointFound)
      {
        //goalPostFound = true;
        GoalPercept::GoalPost post;

        post.basePoint = Vector2<int>(pointBuffer[2]);
        CameraGeometry::imagePixelToFieldCoord(
          getCameraMatrix(),
          getImage().cameraInfo,
          post.basePoint.x, post.basePoint.y, 0.0,
          post.position);

        post.positionReliable = true;
        goalPosts.push_back(post);
        getGoalPercept().add(post);

        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
          CIRCLE_PX(ColorClasses::yellowOrange, (int) pointBuffer[2].x, (int) pointBuffer[2].y, 10);
        );
      }
      else
      {
        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
          CIRCLE_PX(ColorClasses::red, (int) pointBuffer[2].x, (int) pointBuffer[2].y, 10);
        );
      }

    }//end if

    if(candidate.possibleObstacle)
    {
      DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
        POINT_PX(ColorClasses::pink, candidate.center.x, candidate.center.y);
      );
    }
    else
    {
      DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
        POINT_PX(ColorClasses::red, candidate.center.x, candidate.center.y);
      );
    }

  }//end for features[0].size()

  // exactly two posts are seen => assign site labels
  if(getGoalPercept().getNumberOfSeenPosts() == 2) {
    GoalPercept::GoalPost& postOne = getGoalPercept().getPost(0);
    GoalPercept::GoalPost& postTwo = getGoalPercept().getPost(1);

    // sort: which one is left or right
    if(postOne.basePoint.x > postTwo.basePoint.x)
    {
      postOne.type = GoalPercept::GoalPost::rightPost;
      postTwo.type = GoalPercept::GoalPost::leftPost;
    } else {
      postOne.type = GoalPercept::GoalPost::leftPost;
      postTwo.type = GoalPercept::GoalPost::rightPost;
    }
  }
}//end execute

<commit_msg>added asserts<commit_after>
/**
* @file GradientGoalDetector.cpp
*
* Implementation of class GradientGoalDetector
*
*/

#include "GradientGoalDetector.h"

#include "Tools/DataStructures/ArrayQueue.h"
#include "Tools/CameraGeometry.h"

#include "Tools/Debug/Stopwatch.h"

#include "Tools/ImageProcessing/BresenhamLineScan.h"

#include <Representations/Infrastructure/CameraInfoConstants.h>

#define IMG_GET(x,y,p) \
  if(!getImage().isInside(x,y)) { \
    std::cout << __FILE__ << ":" << __LINE__ << "SCHEISSE!!!" << std::endl; \
  } \
  getImage().get(x, y, p);

GradientGoalDetector::GradientGoalDetector()
:
  cameraID(CameraInfo::Bottom)
{
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markPeaks", "mark found maximum u-v peaks in image", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:draw_scanlines","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:draw_response","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:draw_difference","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markFootScans","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markFootScanResponse","..", false);
  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:markFootScanGoodPoints","..", false);

  DEBUG_REQUEST_REGISTER("NeoVision:GradientGoalDetector:use_horizon","..", false);
}

void GradientGoalDetector::execute(CameraInfo::CameraID id, bool horizon)
{
  cameraID = id;
  CANVAS_PX(cameraID);

  getGoalPercept().reset();

  Vector2d p1(0, getImage().cameraInfo.getOpticalCenterY());
  Vector2d p2(getImage().cameraInfo.resolutionWidth, getImage().cameraInfo.getOpticalCenterY());
  Vector2d direction(1,0);

  if(horizon) {
    p1 = getArtificialHorizon().begin();
    p2 = getArtificialHorizon().end();
    direction = getArtificialHorizon().getDirection();
  }

  int imageBorderOffset = 25;
  int heightOfHorizon = (int) ((p1.y + p2.y) * 0.5 + 0.5);
  // image over the horizon
  if(heightOfHorizon > (int) getImage().cameraInfo.resolutionHeight - 10)
  {
    return;
  }

  // adjust the scan height in case of the bottom camera
  //if(cameraID == CameraInfo::Bottom)
  {
    // clamp the scanline
    p1.y = Math::clamp((int) p1.y, imageBorderOffset + 5, (int)getImage().cameraInfo.resolutionHeight - imageBorderOffset - 5);
    p2.y = Math::clamp((int) p2.y, imageBorderOffset + 5 , (int)getImage().cameraInfo.resolutionHeight - imageBorderOffset - 5);
  }

  //double horizont = min(p1.y, p2.y);

  double threshold = params.gradientThreshold;
  double thresholdY = params.minY;

  double response = 0.0;
  double responseY = 0.0;
  double lastResponse = 0.0;
  double lastResponseY = 0.0;

  Pixel pixel;

  int aktIdx = 0;

  for(int i = 0; i < 5; i++)
  {
     features[i].clear();
  }

  if
  (
    p1.y > imageBorderOffset &&
    p2.y > imageBorderOffset &&
    p1.y < (int) getImage().height() - imageBorderOffset &&
    p2.y < (int) getImage().height() - imageBorderOffset
  )
  {
    bool isCandidate = false;
    bool isObstacle = false;

    Feature candidate;

    int diffVU = 0;
    int lastDiffVU = 0;

    for(int y = (int) p1.y - 12; y <= (int) p1.y + 12; y += 6)
    {
      valueBuffer.init();
      valueBufferY.init();
      pointBuffer.init();
      isCandidate = false;
      Vector2<int> pos((int) p1.x + 2, y);
      BresenhamLineScan scanner(pos, direction, getImage().cameraInfo);
      do
      {
        pointBuffer.add(pos);
        IMG_GET(pos.x, pos.y, pixel);
        diffVU = (int) pixel.v - (int) pixel.u;
        valueBuffer.add(diffVU);
        valueBufferY.add(pixel.y);

        if(pointBuffer.isFull())
        {
          response = valueBuffer[4] + 2 * valueBuffer[3]  + 4 * valueBuffer[2] + valueBuffer[1] * 2 + valueBuffer[0];
          //response = valueBuffer[4] + 2 * valueBuffer[3]  - valueBuffer[1] * 2 - valueBuffer[0];
          responseY = valueBufferY[2];
          response /= 10;
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:draw_scanlines",
            if(aktIdx == 2)
            {
              POINT_PX(ColorClasses::yellow, pos.x, pos.y );
            }
            else
            {
              POINT_PX(ColorClasses::gray, pos.x, pos.y );
            }
          );
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:draw_difference",
            if(aktIdx == 2)
            {
              LINE_PX(ColorClasses::green, (int) pointBuffer[3].x, (int) pointBuffer[3].y - lastDiffVU, (int) pointBuffer[4].x, (int) pointBuffer[4].y - diffVU);
              lastDiffVU = diffVU;
            }
          );
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:draw_response",
            if(aktIdx == 2)
            {
              LINE_PX(ColorClasses::red, (int) pointBuffer[1].x, (int) pointBuffer[1].y - (int) /*fabs*/(lastResponse), (int) pointBuffer[2].x, (int) pointBuffer[2].y - (int)/* fabs*/(response) );
              LINE_PX(ColorClasses::white, (int) pointBuffer[1].x, (int) pointBuffer[1].y - (int) fabs(lastResponseY / 3), (int) pointBuffer[2].x, (int) pointBuffer[2].y - (int) fabs(responseY / 3) );
              lastResponse = response;
              lastResponseY = responseY;
            }
          );
          if(/*fabs*/(response) >= threshold && fabs(responseY) >= thresholdY)
          {
            if(!isCandidate)
            {
              candidate.begin = pointBuffer[2];
              candidate.center = pointBuffer[2];
              candidate.end = pointBuffer[2];
              candidate.responseAtBegin.x = response;
              candidate.responseAtBegin.y = diffVU;
              if(diffVU < 0)
              {
                isObstacle = true;
              }
            }
            isCandidate = true;
            if(isObstacle)
            {
              candidate.possibleObstacle = isObstacle;
              DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
                POINT_PX(ColorClasses::orange, pos.x - 2, pos.y);
              );
            }
            else
            {
              DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
                POINT_PX(ColorClasses::blue, pos.x - 2, pos.y);
              );
            }
          }
          else
          {
            if(isCandidate)
            {
              candidate.end = pointBuffer[2];
              candidate.center = (candidate.end + candidate.begin) / 2;
              candidate.responseAtEnd.x = response;
              candidate.responseAtEnd.y = diffVU;
              features[aktIdx].push_back(candidate);
            }
            isCandidate = false;
            isObstacle = false;
          }//end if
        }
      }
      while(scanner.getNextWithCheck(pos));
      aktIdx++;
    }//end for
  }//end if
  Vector2<double> dir(-direction.y, direction.x);

  memset(&lastTestFeatureIdx, 0, sizeof(lastTestFeatureIdx));
  //bool goalPostFound = false;
  goalPosts.clear();

  //std::cout << std::endl << " ------ " << std::endl;
  for(unsigned i = 0; i < features[0].size(); i++)
  {
    const Feature& candidate = features[0][i];

    BresenhamLineScan footPointScanner(candidate.center, dir, getImage().cameraInfo);

    int goodPointsCount = 1;

    Vector2<int> pos = candidate.center;
    valueBuffer.init();
    valueBufferY.init();
    pointBuffer.init();

    Vector2d last(pos);

    for(int y = 1; y < 5; y++)
    {
      bool stop = false;
      for(int yy = 0; yy < 5; yy++)
      {
        if(footPointScanner.getNextWithCheck(pos))
        {
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
            POINT_PX(ColorClasses::skyblue, pos.x, pos.y);
          );
          pointBuffer.add(pos);
          IMG_GET(pos.x, pos.y, pixel);
          int diffVU = (int) pixel.v - (int) pixel.u;
          valueBuffer.add(diffVU);
          valueBufferY.add(pixel.y);

          if(yy > 0)
          {
            response = valueBuffer[4] + 2 * valueBuffer[3]  + 4 * valueBuffer[2] + valueBuffer[1] * 2 + valueBuffer[0];
            //response = valueBuffer[4] + 2 * valueBuffer[3]  - valueBuffer[1] * 2 - valueBuffer[0];
            responseY = valueBufferY[2];
            response /= 10;
            if(/*fabs*/(response) < params.responseHoldFactor * threshold || fabs(responseY) < params.responseHoldFactor * thresholdY)
            {
              stop = true;
            }
          }
        }
      }
      unsigned j = lastTestFeatureIdx[y];
      while (!stop && j < features[y].size())
      {
        int dist = (pos - features[y][j].center).abs();
        if(dist < params.dist)
        {
          DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScanGoodPoints",
            POINT_PX(ColorClasses::red, features[y][j].center.x, features[y][j].center.y);
          );
          lastTestFeatureIdx[y] = j;
          goodPointsCount++;
          stop = true;
           //std::cout << "good (" << i << ") " << dist << " pos: " << pos << " point:" << features[y][j].center << std::endl;
        }
        j++;
      }
    }//end for

    if(goodPointsCount >= params.minGoodPoints)
    {

      bool footPointFound = false;
      bool stop = false;
      Pixel pixel1;
      Pixel pixel2;
      Pixel pixel3;

      int lastDiffVU = 0;
      int lastResponse = 0;

      while(!stop && footPointScanner.getNextWithCheck(pos))
      {
        pointBuffer.add(pos);
        IMG_GET(pos.x, pos.y, pixel);
        int diffVU = (int) pixel.v - (int) pixel.u;
        valueBuffer.add(diffVU);
        valueBufferY.add(pixel.y);

        response = valueBuffer[4] + 2 * valueBuffer[3]  + 4 * valueBuffer[2] + valueBuffer[1] * 2 + valueBuffer[0];
        //response = valueBuffer[4] + 2 * valueBuffer[3]  - valueBuffer[1] * 2 - valueBuffer[0];
        responseY = valueBufferY[2];
        response /= 10;

        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
          if(getFieldColorPercept().isFieldColor(pixel))
          {
             POINT_PX(ColorClasses::green, (int) pointBuffer[2].x, (int) pointBuffer[2].y);
          }
          else
          {
            POINT_PX(ColorClasses::gray, (int) pointBuffer[2].x, (int) pointBuffer[2].y);
          }
        );
        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScanResponse",
          LINE_PX(ColorClasses::white, (int) pointBuffer[1].x + lastResponse, (int) pointBuffer[1].y, (int) pointBuffer[2].x + (int) response, (int) pointBuffer[2].y);
          LINE_PX(ColorClasses::orange, (int) pointBuffer[1].x + lastDiffVU, (int) pointBuffer[1].y, (int) pointBuffer[2].x + diffVU, (int) pointBuffer[2].y);
        );
        lastDiffVU = diffVU;
        lastResponse = (int) response;

        IMG_GET((int) pointBuffer[4].x, (int) pointBuffer[4].y, pixel1);
        IMG_GET((int) pointBuffer[3].x, (int) pointBuffer[3].y, pixel2);
        IMG_GET((int) pointBuffer[2].x, (int) pointBuffer[2].y, pixel3);
        if(/*fabs*/(response) < params.responseHoldFactor * threshold || fabs(responseY) < params.responseHoldFactor * thresholdY)
        {
          stop = true;
          if
          (
            //c > params.minScanPointsAfterGoodPoints &&
            (getFieldColorPercept().isFieldColor(pixel1) || getFieldColorPercept().isFieldColor(pixel2) || getFieldColorPercept().isFieldColor(pixel3))
          )
          {
            footPointFound = true;
          }
          else
          {
            int t = 0;
            int count = 0;
            while( t < 10 && footPointScanner.getNextWithCheck(pos))
            {
              IMG_GET(pos.x, pos.y, pixel);
              if(getFieldColorPercept().isFieldColor(pixel))
              {
                count++;
              }
              DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
                if(getFieldColorPercept().isFieldColor(pixel))
                {
                   POINT_PX(ColorClasses::green, pos.x, pos.y);
                }
                else
                {
                  POINT_PX(ColorClasses::black, pos.x, pos.y);
                }
              );
              t++;
            }
            if(count > 4)
            {
              footPointFound = true;
            }
          }
        }
      }//end while

      if(footPointFound)
      {
        //goalPostFound = true;
        GoalPercept::GoalPost post;

        post.basePoint = Vector2<int>(pointBuffer[2]);
        CameraGeometry::imagePixelToFieldCoord(
          getCameraMatrix(),
          getImage().cameraInfo,
          post.basePoint.x, post.basePoint.y, 0.0,
          post.position);

        post.positionReliable = true;
        goalPosts.push_back(post);
        getGoalPercept().add(post);

        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
          CIRCLE_PX(ColorClasses::yellowOrange, (int) pointBuffer[2].x, (int) pointBuffer[2].y, 10);
        );
      }
      else
      {
        DEBUG_REQUEST("NeoVision:GradientGoalDetector:markFootScans",
          CIRCLE_PX(ColorClasses::red, (int) pointBuffer[2].x, (int) pointBuffer[2].y, 10);
        );
      }

    }//end if

    if(candidate.possibleObstacle)
    {
      DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
        POINT_PX(ColorClasses::pink, candidate.center.x, candidate.center.y);
      );
    }
    else
    {
      DEBUG_REQUEST("NeoVision:GradientGoalDetector:markPeaks",
        POINT_PX(ColorClasses::red, candidate.center.x, candidate.center.y);
      );
    }

  }//end for features[0].size()

  // exactly two posts are seen => assign site labels
  if(getGoalPercept().getNumberOfSeenPosts() == 2) {
    GoalPercept::GoalPost& postOne = getGoalPercept().getPost(0);
    GoalPercept::GoalPost& postTwo = getGoalPercept().getPost(1);

    // sort: which one is left or right
    if(postOne.basePoint.x > postTwo.basePoint.x)
    {
      postOne.type = GoalPercept::GoalPost::rightPost;
      postTwo.type = GoalPercept::GoalPost::leftPost;
    } else {
      postOne.type = GoalPercept::GoalPost::leftPost;
      postTwo.type = GoalPercept::GoalPost::rightPost;
    }
  }
}//end execute

<|endoftext|>


<commit_before>// Copyright 2020 The TCMalloc Authors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <atomic>

#include "absl/types/optional.h"
#include "benchmark/benchmark.h"
#include "tcmalloc/central_freelist.h"
#include "tcmalloc/common.h"
#include "tcmalloc/mock_central_freelist.h"
#include "tcmalloc/mock_transfer_cache.h"
#include "tcmalloc/transfer_cache_internals.h"
#include "tcmalloc/transfer_cache_stats.h"

namespace tcmalloc {
namespace {

using TransferCacheEnv =
    FakeTransferCacheEnvironment<internal_transfer_cache::TransferCache<
        MinimalFakeCentralFreeList, FakeTransferCacheManager>>;

using LockFreeEnv =
    FakeTransferCacheEnvironment<internal_transfer_cache::LockFreeTransferCache<
        MinimalFakeCentralFreeList, FakeTransferCacheManager>>;

template <typename Env>
void BM_CrossThreadDraining(benchmark::State& state) {
  using Manager = typename Env::Manager;
  using Cache = typename Env::TransferCache;
  const int kBatchSize = Env::kBatchSize;
  const int kMaxObjectsToMove = Env::kMaxObjectsToMove;
  void* batch[kMaxObjectsToMove];

  struct CrossThreadState {
    CrossThreadState() : m{}, c{Cache(&m), Cache(&m)} {
      c[0].Init(1);
      c[1].Init(1);
    }
    Manager m;
    Cache c[2];
  };

  static CrossThreadState* s = nullptr;
  if (state.thread_index == 0) {
    s = new CrossThreadState();
    for (int i = 0; i < Env::kInitialCapacityInBatches / 2; ++i) {
      for (Cache& c : s->c) {
        c.freelist().AllocateBatch(batch, kBatchSize);
        c.InsertRange(batch, kBatchSize);
      }
    }
  }

  int src = state.thread_index % 2;
  int dst = (src + 1) % 2;
  for (auto iter : state) {
    benchmark::DoNotOptimize(batch);
    s->c[src].RemoveRange(batch, kBatchSize);
    benchmark::DoNotOptimize(batch);
    s->c[dst].InsertRange(batch, kBatchSize);
    benchmark::DoNotOptimize(batch);
  }
  if (state.thread_index == 0) {
    TransferCacheStats stats{};
    for (Cache& c : s->c) {
      TransferCacheStats other = c.GetHitRateStats();
      stats.insert_hits += other.insert_hits;
      stats.insert_misses += other.insert_misses;
      stats.remove_hits += other.remove_hits;
      stats.remove_misses += other.remove_misses;
    }

    state.counters["insert_hit_ratio"] =
        static_cast<double>(stats.insert_hits) /
        (stats.insert_hits + stats.insert_misses);
    state.counters["remove_hit_ratio"] =
        static_cast<double>(stats.remove_hits) /
        (stats.remove_hits + stats.remove_misses);
    delete s;
    s = nullptr;
  }
}

template <typename Env>
void BM_InsertRange(benchmark::State& state) {
  const int kBatchSize = Env::kBatchSize;
  const int kMaxObjectsToMove = Env::kMaxObjectsToMove;

  // optional to have more precise control of when the destruction occurs, as
  // we want to avoid polluting the timing with the dtor.
  absl::optional<Env> e;
  void* batch[kMaxObjectsToMove];
  for (auto iter : state) {
    state.PauseTiming();
    e.emplace();
    e->central_freelist().AllocateBatch(batch, kBatchSize);
    benchmark::DoNotOptimize(e);
    benchmark::DoNotOptimize(batch);
    state.ResumeTiming();

    e->transfer_cache().InsertRange(batch, kBatchSize);
  }
}

template <typename Env>
void BM_RemoveRange(benchmark::State& state) {
  const int kBatchSize = Env::kBatchSize;
  const int kMaxObjectsToMove = Env::kMaxObjectsToMove;

  // optional to have more precise control of when the destruction occurs, as
  // we want to avoid polluting the timing with the dtor.
  absl::optional<Env> e;
  void* batch[kMaxObjectsToMove];
  for (auto iter : state) {
    state.PauseTiming();
    e.emplace();
    e->Insert(kBatchSize);
    benchmark::DoNotOptimize(e);
    state.ResumeTiming();

    e->transfer_cache().RemoveRange(batch, kBatchSize);
    benchmark::DoNotOptimize(batch);
  }
}

BENCHMARK_TEMPLATE(BM_CrossThreadDraining, TransferCacheEnv)
    ->ThreadRange(2, 128);
BENCHMARK_TEMPLATE(BM_CrossThreadDraining, LockFreeEnv)->ThreadRange(2, 128);
BENCHMARK_TEMPLATE(BM_InsertRange, TransferCacheEnv);
BENCHMARK_TEMPLATE(BM_InsertRange, LockFreeEnv);
BENCHMARK_TEMPLATE(BM_RemoveRange, TransferCacheEnv);
BENCHMARK_TEMPLATE(BM_RemoveRange, LockFreeEnv);

}  // namespace
}  // namespace tcmalloc
<commit_msg>Make benchmark name shorter and reduce from 128 to 64 threads for the high contention test.<commit_after>// Copyright 2020 The TCMalloc Authors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <atomic>

#include "absl/types/optional.h"
#include "benchmark/benchmark.h"
#include "tcmalloc/central_freelist.h"
#include "tcmalloc/common.h"
#include "tcmalloc/mock_central_freelist.h"
#include "tcmalloc/mock_transfer_cache.h"
#include "tcmalloc/transfer_cache_internals.h"
#include "tcmalloc/transfer_cache_stats.h"

namespace tcmalloc {
namespace {

using TransferCacheEnv =
    FakeTransferCacheEnvironment<internal_transfer_cache::TransferCache<
        MinimalFakeCentralFreeList, FakeTransferCacheManager>>;

using LockFreeEnv =
    FakeTransferCacheEnvironment<internal_transfer_cache::LockFreeTransferCache<
        MinimalFakeCentralFreeList, FakeTransferCacheManager>>;

template <typename Env>
void BM_CrossThread(benchmark::State& state) {
  using Manager = typename Env::Manager;
  using Cache = typename Env::TransferCache;
  const int kBatchSize = Env::kBatchSize;
  const int kMaxObjectsToMove = Env::kMaxObjectsToMove;
  void* batch[kMaxObjectsToMove];

  struct CrossThreadState {
    CrossThreadState() : m{}, c{Cache(&m), Cache(&m)} {
      c[0].Init(1);
      c[1].Init(1);
    }
    Manager m;
    Cache c[2];
  };

  static CrossThreadState* s = nullptr;
  if (state.thread_index == 0) {
    s = new CrossThreadState();
    for (int i = 0; i < Env::kInitialCapacityInBatches / 2; ++i) {
      for (Cache& c : s->c) {
        c.freelist().AllocateBatch(batch, kBatchSize);
        c.InsertRange(batch, kBatchSize);
      }
    }
  }

  int src = state.thread_index % 2;
  int dst = (src + 1) % 2;
  for (auto iter : state) {
    benchmark::DoNotOptimize(batch);
    s->c[src].RemoveRange(batch, kBatchSize);
    benchmark::DoNotOptimize(batch);
    s->c[dst].InsertRange(batch, kBatchSize);
    benchmark::DoNotOptimize(batch);
  }
  if (state.thread_index == 0) {
    TransferCacheStats stats{};
    for (Cache& c : s->c) {
      TransferCacheStats other = c.GetHitRateStats();
      stats.insert_hits += other.insert_hits;
      stats.insert_misses += other.insert_misses;
      stats.remove_hits += other.remove_hits;
      stats.remove_misses += other.remove_misses;
    }

    state.counters["insert_hit_ratio"] =
        static_cast<double>(stats.insert_hits) /
        (stats.insert_hits + stats.insert_misses);
    state.counters["remove_hit_ratio"] =
        static_cast<double>(stats.remove_hits) /
        (stats.remove_hits + stats.remove_misses);
    delete s;
    s = nullptr;
  }
}

template <typename Env>
void BM_InsertRange(benchmark::State& state) {
  const int kBatchSize = Env::kBatchSize;
  const int kMaxObjectsToMove = Env::kMaxObjectsToMove;

  // optional to have more precise control of when the destruction occurs, as
  // we want to avoid polluting the timing with the dtor.
  absl::optional<Env> e;
  void* batch[kMaxObjectsToMove];
  for (auto iter : state) {
    state.PauseTiming();
    e.emplace();
    e->central_freelist().AllocateBatch(batch, kBatchSize);
    benchmark::DoNotOptimize(e);
    benchmark::DoNotOptimize(batch);
    state.ResumeTiming();

    e->transfer_cache().InsertRange(batch, kBatchSize);
  }
}

template <typename Env>
void BM_RemoveRange(benchmark::State& state) {
  const int kBatchSize = Env::kBatchSize;
  const int kMaxObjectsToMove = Env::kMaxObjectsToMove;

  // optional to have more precise control of when the destruction occurs, as
  // we want to avoid polluting the timing with the dtor.
  absl::optional<Env> e;
  void* batch[kMaxObjectsToMove];
  for (auto iter : state) {
    state.PauseTiming();
    e.emplace();
    e->Insert(kBatchSize);
    benchmark::DoNotOptimize(e);
    state.ResumeTiming();

    e->transfer_cache().RemoveRange(batch, kBatchSize);
    benchmark::DoNotOptimize(batch);
  }
}

BENCHMARK_TEMPLATE(BM_CrossThread, TransferCacheEnv)->ThreadRange(2, 64);
BENCHMARK_TEMPLATE(BM_CrossThread, LockFreeEnv)->ThreadRange(2, 64);
BENCHMARK_TEMPLATE(BM_InsertRange, TransferCacheEnv);
BENCHMARK_TEMPLATE(BM_InsertRange, LockFreeEnv);
BENCHMARK_TEMPLATE(BM_RemoveRange, TransferCacheEnv);
BENCHMARK_TEMPLATE(BM_RemoveRange, LockFreeEnv);

}  // namespace
}  // namespace tcmalloc
<|endoftext|>


<commit_before>/** \file
  Task: Full-automatic 3D tiling acquisition.

  \author: Nathan Clack <clackn@janelia.hhmi.org>

  \copyright
  Copyright 2010 Howard Hughes Medical Institute.
  All rights reserved.
  Use is subject to Janelia Farm Research Campus Software Copyright 1.1
  license terms (http://license.janelia.org/license/jfrc_copyright_1_1.html).
 */

#include "common.h"
#include "AutoTileAcquisition.h"
#include "TiledAcquisition.h"
#include "StackAcquisition.h"
#include "Video.h"
#include "frame.h"
#include "devices\digitizer.h"
#include "devices\Microscope.h"
#include "devices\tiling.h"
#include "AdaptiveTiledAcquisition.h"

#define CHKJMP(expr) if(!(expr)) {warning("%s(%d)"ENDL"\tExpression indicated failure:"ENDL"\t%s"ENDL,__FILE__,__LINE__,#expr); goto Error;}
#define WARN(msg)    warning("%s(%d)"ENDL"\t%s"ENDL,__FILE__,__LINE__,msg)
#define DBG(...)     do{debug("%s(%d)"ENDL "\t",__FILE__,__LINE__); debug(__VA_ARGS__);}while(0)

namespace fetch
{

  namespace task
  {

    //
    // AutoTileAcquisition -  microscope task
    //

    namespace microscope {

      /** \class AutoTileAcquisition AutoTileAcquisition.h

      Microscope task fro automatic 3d tiling of a volume.

      The task operates in three phases:

      While zpos in bounds:
      -#  Explore the current slice to determine which tiles to acquire.
         - foreach tile in zone:
           - move to tile
           - acquire single image at probe depth (could use stage system to offset...just need to ensure correct tile is marked)
           - classify image
           - mark tile
         - postprocess marked tiles
           - close
           - dilate1
      -#  Run the TiledAcquisition task to collect those tiles.
         - since TiledAcquisition is a microscope task, it's run function will be directly
           invoked rather than running it asynchronously.
      -#  Run the Cut (vibratome) task to cut the imaged slice off.

      Questions:
      -#  How to define exploration zone? Z limits.
          How to iterate over tiles for exploration.
      -#  Feedback on marking.
          - can see tiles getting marked in view as it happens...view linked to current stage zpos
      -#  User interuption.
      -#  Speed?  How fast can I explore a given area?
          - initially assume I don't need to be efficient (take simple approach)

      */

      //Upcasting
      unsigned int AutoTileAcquisition::config(IDevice *d) {return config(dynamic_cast<device::Microscope*>(d));}
      unsigned int AutoTileAcquisition::run   (IDevice *d) {return run   (dynamic_cast<device::Microscope*>(d));}

      unsigned int AutoTileAcquisition::config(device::Microscope *d)
      {
        return 1; //success
Error:
        return 0;
      }

      static int _handle_wait_for_result(DWORD result, const char *msg)
      {
          return_val_if( result == WAIT_OBJECT_0  , 0 );
          return_val_if( result == WAIT_OBJECT_0+1, 1 );
          Guarded_Assert_WinErr( result != WAIT_FAILED );
          switch(result)
          { case WAIT_ABANDONED_0:   warning("TiledAcquisition: Wait 0 abandoned\r\n\t%s\r\n", msg); break;
            case WAIT_ABANDONED_0+1: warning("TiledAcquisition: Wait 1 abandoned\r\n\t%s\r\n", msg); break;
            case WAIT_TIMEOUT:       warning("TiledAcquisition: Wait timeout\r\n\t%s\r\n", msg);     break;
            default:
            ;
          }
          return -1;
      }

	  // DGA: Function for simulating an ellipsoid (with circular cross sections) in 3D so that we can test, eg., if the explorable region shrinks as desired
	  static bool insideSimulationOfEllipse(float maxzUm, float targetZUm, Vector3f tilepos)
	  {
		  float distanceFromXYCenterUm, ellipseMajorAxisUm = 2000, crossSectionRadiusUm, ellipseMinorAxisUm, zEllipseCenterUm; // DGA: Variables used to create ellipsoid in 3D
		  ellipseMinorAxisUm = (0.5*(maxzUm - 10000)); //DGA: Assumes initial offset of 10000 um, so want minor axis to be equal to the total height/2
		  zEllipseCenterUm = 10000 + ellipseMinorAxisUm; //DGA: Center of ellipse in Z
		  crossSectionRadiusUm = ellipseMajorAxisUm*sqrt(1 - pow((zEllipseCenterUm - targetZUm) / ellipseMinorAxisUm, 2)); //DGA: Calculate the cross section radius
		  crossSectionRadiusUm = (crossSectionRadiusUm > 250) ? crossSectionRadiusUm : 250; //DGA: Set a minimum cross section since the program will stop if there aren't any detected tiles
		  distanceFromXYCenterUm = sqrt((tilepos[0] - 50000)*(tilepos[0] - 50000) + (tilepos[1] - 50000)*(tilepos[1] - 50000)); //DGA: Calculate if the current tile is within the circular cross section, centered at (50 mm, 50 mm)
		  return (distanceFromXYCenterUm < crossSectionRadiusUm); //DGA: If the tile is within the ellipse, return true
	  }
      ///// CLASSIFY //////////////////////////////////////////////////
      template<class T>
      static int _classify(mylib::Array *src, int ichan, double intensity_thresh, double area_thresh)
      { T* data;
        mylib::Array tmp=*src,*image=&tmp;
        if(ichan>=0)
        { image=mylib::Get_Array_Plane(&tmp,ichan);
        }
        data = (T*)image->data;
        size_t i,count=0;
        if(!image->size) return 0;
        for(i=0;i<image->size;++i)
          count+=(data[i]>intensity_thresh);
#if 0
        mylib::Write_Image("classify.tif",image,mylib::DONT_PRESS);
#endif
        DBG("Fraction above thresh: %f\n\t\tintensity thresh: %f\n\t\tarea_thresh: %f\n",
          count/((double)image->size),
          intensity_thresh,
          area_thresh);
        return (count/((double)image->size))>area_thresh;
      }
      #define CLASSIFY(type_id,type) case type_id: return _classify<type>(image,ichan,intensity_thresh,area_thresh); break
      /**
      \returns 0 if background, 1 if foreground

      Image could be multiple channels.  Channels are assumed to plane-wise.
      */
      static int classify(mylib::Array *image, int ichan, double intensity_thresh, double area_thresh)
      {
        if(image->ndims<3)  // check that there are enough dimensions to select a channel
        { ichan=-1;
        } else if(ichan>=image->dims[image->ndims-1]) // is ichan sane?  If not, use chan 0.
        { ichan=0;
        }

        switch(image->type)
        {
          CLASSIFY( mylib::UINT8_TYPE   ,uint8_t );
          CLASSIFY( mylib::UINT16_TYPE  ,uint16_t);
          CLASSIFY( mylib::UINT32_TYPE  ,uint32_t);
          CLASSIFY( mylib::UINT64_TYPE  ,uint64_t);
          CLASSIFY( mylib::INT8_TYPE    , int8_t );
          CLASSIFY( mylib::INT16_TYPE   , int16_t);
          CLASSIFY( mylib::INT32_TYPE   , int32_t);
          CLASSIFY( mylib::INT64_TYPE   , int64_t);
          CLASSIFY( mylib::FLOAT32_TYPE ,float   );
          CLASSIFY( mylib::FLOAT64_TYPE ,double  );
          default:
            return 0;
        }
      }
      #undef CLASSIFY

      ///// EXPLORE  //////////////////////////////////////////////////

      /** Tests to make sure the cut/image cycle stays in z bounds.

      Only need to test max since stage only moves up as cuts progress.

      */
      static int PlaneInBounds(device::Microscope *dc,float maxz)
      { float x,y,z;
        dc->stage()->getPos(&x,&y,&z);
        return z<maxz;
      }

      /**
      Explores the current plane searching for tiles to image.  A heuristic classifier
      is used to target a tile for imaging based on a single snapshot acquired at a
      given depth (\c dz_um).

      Preconditions:
      - tiles to explore have been labelled as such

      Parameters to get from configuration:
      - dz_um:               zpiezo offset
      - maxz                 stage units(mm)
      - timeout_ms
      - ichan:               channel to use for classification, -1 uses all channels
      - intensity_threshold: use the expected pixel units
      - area_threshold:      0 to 1. The fraction of pixels that must be brighter than intensity threshold.

      \returns 0 if no tiles were targeted for imaging, otherwise 1.
      */
      static int explore(device::Microscope *dc)
      { Vector3f tilepos;
        unsigned any_explorable=0,
                 any_active=0;
        cfg::tasks::AutoTile cfg=dc->get_config().autotile();
        size_t iplane=dc->stage()->getPosInLattice().z();

        device::StageTiling* tiling = dc->stage()->tiling();
        tiling->markAddressable(iplane); // make sure the current plane is marked addressable
        tiling->setCursorToPlane(iplane);

		device::Digitizer::Config digcfg = dc->scanner._scanner2d._digitizer.get_config(); //DGA: Get the configuration of the digitizer to know if it is simulated
        device::TileSearchContext *ctx=0;
        while(  !dc->_agent->is_stopping()
              && tiling->nextSearchPosition(iplane,cfg.search_radius()/*radius - tiles*/,tilepos,&ctx))
              //&& tiling->nextInPlaneExplorablePosition(tilepos))
        { mylib::Array *im;
          any_explorable=1;
          tilepos[2]=dc->stage()->getTarget().z()*1000.0; // convert mm to um
          DBG("Exploring tile: %6.1f %6.1f %6.1f",tilepos.x(),tilepos.y(),tilepos.z());
          CHKJMP(dc->stage()->setPos(tilepos*0.001)); // convert um to mm
          CHKJMP(im=dc->snapshot(cfg.z_um(),cfg.timeout_ms()));
          tiling->markExplored();

          tiling->markDetected(classify(im,cfg.ichan(),cfg.intensity_threshold(),cfg.area_threshold()));
		  if (digcfg.kind() == cfg::device::Digitizer_DigitizerType_Simulated){	//DGA: If digitizer is simulated, then simulate an ellipsoidal volume
			  if (!insideSimulationOfEllipse(cfg.maxz_mm()*1000, dc->stage()->getTarget().z()*1000.0, tilepos)) tiling->markDetected(false); //DGA: If the tile is outside the simulated volume, mark as undetected (by default, simulation mode marks everything as detected)
		  }
          mylib::Free_Array(im);
        }
        if(!tiling->updateActive(iplane))
        { WARN("No tiles found to image.\n");
          goto Error;
        }
		if(!dc->_agent->is_stopping() && any_explorable)
          tiling->dilateActive(iplane);
		tiling->fillHolesInActive(iplane);
		if(ctx) delete ctx;
        return 1;
      Error:
        if(ctx) delete ctx;
        return 0;
      }

      unsigned int AutoTileAcquisition::run(device::Microscope *dc)
      { unsigned eflag=0; //success
        cfg::tasks::AutoTile cfg=dc->get_config().autotile();
        TiledAcquisition         nonadaptive_tiling;
        AdaptiveTiledAcquisition adaptive_tiling;
        MicroscopeTask *tile=0;
        Cut cut;
		device::StageTiling * tiling = dc->stage()->tiling(); //DGA: Pointer to tiling object

        tile=cfg.use_adaptive_tiling()?((MicroscopeTask*)&adaptive_tiling):((MicroscopeTask*)&nonadaptive_tiling);

        while(!dc->_agent->is_stopping() && PlaneInBounds(dc,cfg.maxz_mm()))
        {
          if(cfg.use_explore())
            CHKJMP(explore(dc));       // will return an error if no explorable tiles found on the plane
          CHKJMP(   tile->config(dc));
          CHKJMP(0==tile->run(dc));

          /* Assert the trip detector hasn't gone off.
           * Trip detector will signal acq task and microscope tasks to stop, but
           * we double check here as extra insurance against any extra cuts.
           */
          CHKJMP(dc->trip_detect.ok());

          CHKJMP(   cut.config(dc));
          CHKJMP(0==cut.run(dc));
		  if(tiling->useTwoDimensionalTiling_) //DGA: If using two dimensional tiling
		  {
			if (PlaneInBounds(dc,cfg.maxz_mm())) tiling->useDoneTilesAsExplorableTilesForTwoDimensionalTiling(); //DGA: If the next position is in bounds (ie, not beyond the max z), then update the tiling, otherwise do nothing.
		  }
		  else tiling->useCurrentDoneTilesAsNextExplorableTiles(); //DGA: After imaging tiles, set the next explorable tiles equal to the current done tiles

		  if(dc->getScheduleStopAfterNextCut()) //DGA: if a stop is scheduled
		  {
			dc->cutCompletedSoStop(); //DGA: Call function to stop autotile
			dc->setScheduleStopAfterNextCut(false); //DGA: Uncheck stop after next cut checkbox
		  }
        }

Finalize:
        return eflag;
Error:
        eflag=1;
        goto Finalize;
      }

    } // namespace microscope

  }   // namespace task
}     // namespace fetch
<commit_msg>fix/TilesSetActiveWhenExploringStopped: Now dilating active tiles after exploration is only performed if the task was not stopped prematurely<commit_after>/** \file
  Task: Full-automatic 3D tiling acquisition.

  \author: Nathan Clack <clackn@janelia.hhmi.org>

  \copyright
  Copyright 2010 Howard Hughes Medical Institute.
  All rights reserved.
  Use is subject to Janelia Farm Research Campus Software Copyright 1.1
  license terms (http://license.janelia.org/license/jfrc_copyright_1_1.html).
 */

#include "common.h"
#include "AutoTileAcquisition.h"
#include "TiledAcquisition.h"
#include "StackAcquisition.h"
#include "Video.h"
#include "frame.h"
#include "devices\digitizer.h"
#include "devices\Microscope.h"
#include "devices\tiling.h"
#include "AdaptiveTiledAcquisition.h"

#define CHKJMP(expr) if(!(expr)) {warning("%s(%d)"ENDL"\tExpression indicated failure:"ENDL"\t%s"ENDL,__FILE__,__LINE__,#expr); goto Error;}
#define WARN(msg)    warning("%s(%d)"ENDL"\t%s"ENDL,__FILE__,__LINE__,msg)
#define DBG(...)     do{debug("%s(%d)"ENDL "\t",__FILE__,__LINE__); debug(__VA_ARGS__);}while(0)

namespace fetch
{

  namespace task
  {

    //
    // AutoTileAcquisition -  microscope task
    //

    namespace microscope {

      /** \class AutoTileAcquisition AutoTileAcquisition.h

      Microscope task fro automatic 3d tiling of a volume.

      The task operates in three phases:

      While zpos in bounds:
      -#  Explore the current slice to determine which tiles to acquire.
         - foreach tile in zone:
           - move to tile
           - acquire single image at probe depth (could use stage system to offset...just need to ensure correct tile is marked)
           - classify image
           - mark tile
         - postprocess marked tiles
           - close
           - dilate1
      -#  Run the TiledAcquisition task to collect those tiles.
         - since TiledAcquisition is a microscope task, it's run function will be directly
           invoked rather than running it asynchronously.
      -#  Run the Cut (vibratome) task to cut the imaged slice off.

      Questions:
      -#  How to define exploration zone? Z limits.
          How to iterate over tiles for exploration.
      -#  Feedback on marking.
          - can see tiles getting marked in view as it happens...view linked to current stage zpos
      -#  User interuption.
      -#  Speed?  How fast can I explore a given area?
          - initially assume I don't need to be efficient (take simple approach)

      */

      //Upcasting
      unsigned int AutoTileAcquisition::config(IDevice *d) {return config(dynamic_cast<device::Microscope*>(d));}
      unsigned int AutoTileAcquisition::run   (IDevice *d) {return run   (dynamic_cast<device::Microscope*>(d));}

      unsigned int AutoTileAcquisition::config(device::Microscope *d)
      {
        return 1; //success
Error:
        return 0;
      }

      static int _handle_wait_for_result(DWORD result, const char *msg)
      {
          return_val_if( result == WAIT_OBJECT_0  , 0 );
          return_val_if( result == WAIT_OBJECT_0+1, 1 );
          Guarded_Assert_WinErr( result != WAIT_FAILED );
          switch(result)
          { case WAIT_ABANDONED_0:   warning("TiledAcquisition: Wait 0 abandoned\r\n\t%s\r\n", msg); break;
            case WAIT_ABANDONED_0+1: warning("TiledAcquisition: Wait 1 abandoned\r\n\t%s\r\n", msg); break;
            case WAIT_TIMEOUT:       warning("TiledAcquisition: Wait timeout\r\n\t%s\r\n", msg);     break;
            default:
            ;
          }
          return -1;
      }

	  // DGA: Function for simulating an ellipsoid (with circular cross sections) in 3D so that we can test, eg., if the explorable region shrinks as desired
	  static bool insideSimulationOfEllipse(float maxzUm, float targetZUm, Vector3f tilepos)
	  {
		  float distanceFromXYCenterUm, ellipseMajorAxisUm = 2000, crossSectionRadiusUm, ellipseMinorAxisUm, zEllipseCenterUm; // DGA: Variables used to create ellipsoid in 3D
		  ellipseMinorAxisUm = (0.5*(maxzUm - 10000)); //DGA: Assumes initial offset of 10000 um, so want minor axis to be equal to the total height/2
		  zEllipseCenterUm = 10000 + ellipseMinorAxisUm; //DGA: Center of ellipse in Z
		  crossSectionRadiusUm = ellipseMajorAxisUm*sqrt(1 - pow((zEllipseCenterUm - targetZUm) / ellipseMinorAxisUm, 2)); //DGA: Calculate the cross section radius
		  crossSectionRadiusUm = (crossSectionRadiusUm > 250) ? crossSectionRadiusUm : 250; //DGA: Set a minimum cross section since the program will stop if there aren't any detected tiles
		  distanceFromXYCenterUm = sqrt((tilepos[0] - 50000)*(tilepos[0] - 50000) + (tilepos[1] - 50000)*(tilepos[1] - 50000)); //DGA: Calculate if the current tile is within the circular cross section, centered at (50 mm, 50 mm)
		  return (distanceFromXYCenterUm < crossSectionRadiusUm); //DGA: If the tile is within the ellipse, return true
	  }
      ///// CLASSIFY //////////////////////////////////////////////////
      template<class T>
      static int _classify(mylib::Array *src, int ichan, double intensity_thresh, double area_thresh)
      { T* data;
        mylib::Array tmp=*src,*image=&tmp;
        if(ichan>=0)
        { image=mylib::Get_Array_Plane(&tmp,ichan);
        }
        data = (T*)image->data;
        size_t i,count=0;
        if(!image->size) return 0;
        for(i=0;i<image->size;++i)
          count+=(data[i]>intensity_thresh);
#if 0
        mylib::Write_Image("classify.tif",image,mylib::DONT_PRESS);
#endif
        DBG("Fraction above thresh: %f\n\t\tintensity thresh: %f\n\t\tarea_thresh: %f\n",
          count/((double)image->size),
          intensity_thresh,
          area_thresh);
        return (count/((double)image->size))>area_thresh;
      }
      #define CLASSIFY(type_id,type) case type_id: return _classify<type>(image,ichan,intensity_thresh,area_thresh); break
      /**
      \returns 0 if background, 1 if foreground

      Image could be multiple channels.  Channels are assumed to plane-wise.
      */
      static int classify(mylib::Array *image, int ichan, double intensity_thresh, double area_thresh)
      {
        if(image->ndims<3)  // check that there are enough dimensions to select a channel
        { ichan=-1;
        } else if(ichan>=image->dims[image->ndims-1]) // is ichan sane?  If not, use chan 0.
        { ichan=0;
        }

        switch(image->type)
        {
          CLASSIFY( mylib::UINT8_TYPE   ,uint8_t );
          CLASSIFY( mylib::UINT16_TYPE  ,uint16_t);
          CLASSIFY( mylib::UINT32_TYPE  ,uint32_t);
          CLASSIFY( mylib::UINT64_TYPE  ,uint64_t);
          CLASSIFY( mylib::INT8_TYPE    , int8_t );
          CLASSIFY( mylib::INT16_TYPE   , int16_t);
          CLASSIFY( mylib::INT32_TYPE   , int32_t);
          CLASSIFY( mylib::INT64_TYPE   , int64_t);
          CLASSIFY( mylib::FLOAT32_TYPE ,float   );
          CLASSIFY( mylib::FLOAT64_TYPE ,double  );
          default:
            return 0;
        }
      }
      #undef CLASSIFY

      ///// EXPLORE  //////////////////////////////////////////////////

      /** Tests to make sure the cut/image cycle stays in z bounds.

      Only need to test max since stage only moves up as cuts progress.

      */
      static int PlaneInBounds(device::Microscope *dc,float maxz)
      { float x,y,z;
        dc->stage()->getPos(&x,&y,&z);
        return z<maxz;
      }

      /**
      Explores the current plane searching for tiles to image.  A heuristic classifier
      is used to target a tile for imaging based on a single snapshot acquired at a
      given depth (\c dz_um).

      Preconditions:
      - tiles to explore have been labelled as such

      Parameters to get from configuration:
      - dz_um:               zpiezo offset
      - maxz                 stage units(mm)
      - timeout_ms
      - ichan:               channel to use for classification, -1 uses all channels
      - intensity_threshold: use the expected pixel units
      - area_threshold:      0 to 1. The fraction of pixels that must be brighter than intensity threshold.

      \returns 0 if no tiles were targeted for imaging, otherwise 1.
      */
      static int explore(device::Microscope *dc)
      { Vector3f tilepos;
        unsigned any_explorable=0,
                 any_active=0;
        cfg::tasks::AutoTile cfg=dc->get_config().autotile();
        size_t iplane=dc->stage()->getPosInLattice().z();

        device::StageTiling* tiling = dc->stage()->tiling();
        tiling->markAddressable(iplane); // make sure the current plane is marked addressable
        tiling->setCursorToPlane(iplane);

		device::Digitizer::Config digcfg = dc->scanner._scanner2d._digitizer.get_config(); //DGA: Get the configuration of the digitizer to know if it is simulated
        device::TileSearchContext *ctx=0;
        while(  !dc->_agent->is_stopping()
              && tiling->nextSearchPosition(iplane,cfg.search_radius()/*radius - tiles*/,tilepos,&ctx))
              //&& tiling->nextInPlaneExplorablePosition(tilepos))
        { mylib::Array *im;
          any_explorable=1;
          tilepos[2]=dc->stage()->getTarget().z()*1000.0; // convert mm to um
          DBG("Exploring tile: %6.1f %6.1f %6.1f",tilepos.x(),tilepos.y(),tilepos.z());
          CHKJMP(dc->stage()->setPos(tilepos*0.001)); // convert um to mm
          CHKJMP(im=dc->snapshot(cfg.z_um(),cfg.timeout_ms()));
          tiling->markExplored();

          tiling->markDetected(classify(im,cfg.ichan(),cfg.intensity_threshold(),cfg.area_threshold()));
		  if (digcfg.kind() == cfg::device::Digitizer_DigitizerType_Simulated){	//DGA: If digitizer is simulated, then simulate an ellipsoidal volume
			  if (!insideSimulationOfEllipse(cfg.maxz_mm()*1000, dc->stage()->getTarget().z()*1000.0, tilepos)) tiling->markDetected(false); //DGA: If the tile is outside the simulated volume, mark as undetected (by default, simulation mode marks everything as detected)
		  }
          mylib::Free_Array(im);
        }
        if(!tiling->updateActive(iplane))
        { WARN("No tiles found to image.\n");
          goto Error;
        }
		if(!dc->_agent->is_stopping() && any_explorable) //DGA: Only dilate active tiles if it is not being stopped
          tiling->dilateActive(iplane);
		tiling->fillHolesInActive(iplane);
		if(ctx) delete ctx;
        return 1;
      Error:
        if(ctx) delete ctx;
        return 0;
      }

      unsigned int AutoTileAcquisition::run(device::Microscope *dc)
      { unsigned eflag=0; //success
        cfg::tasks::AutoTile cfg=dc->get_config().autotile();
        TiledAcquisition         nonadaptive_tiling;
        AdaptiveTiledAcquisition adaptive_tiling;
        MicroscopeTask *tile=0;
        Cut cut;
		device::StageTiling * tiling = dc->stage()->tiling(); //DGA: Pointer to tiling object

        tile=cfg.use_adaptive_tiling()?((MicroscopeTask*)&adaptive_tiling):((MicroscopeTask*)&nonadaptive_tiling);

        while(!dc->_agent->is_stopping() && PlaneInBounds(dc,cfg.maxz_mm()))
        {
          if(cfg.use_explore())
            CHKJMP(explore(dc));       // will return an error if no explorable tiles found on the plane
          CHKJMP(   tile->config(dc));
          CHKJMP(0==tile->run(dc));

          /* Assert the trip detector hasn't gone off.
           * Trip detector will signal acq task and microscope tasks to stop, but
           * we double check here as extra insurance against any extra cuts.
           */
          CHKJMP(dc->trip_detect.ok());

          CHKJMP(   cut.config(dc));
          CHKJMP(0==cut.run(dc));
		  if(tiling->useTwoDimensionalTiling_) //DGA: If using two dimensional tiling
		  {
			if (PlaneInBounds(dc,cfg.maxz_mm())) tiling->useDoneTilesAsExplorableTilesForTwoDimensionalTiling(); //DGA: If the next position is in bounds (ie, not beyond the max z), then update the tiling, otherwise do nothing.
		  }
		  else tiling->useCurrentDoneTilesAsNextExplorableTiles(); //DGA: After imaging tiles, set the next explorable tiles equal to the current done tiles

		  if(dc->getScheduleStopAfterNextCut()) //DGA: if a stop is scheduled
		  {
			dc->cutCompletedSoStop(); //DGA: Call function to stop autotile
			dc->setScheduleStopAfterNextCut(false); //DGA: Uncheck stop after next cut checkbox
		  }
        }

Finalize:
        return eflag;
Error:
        eflag=1;
        goto Finalize;
      }

    } // namespace microscope

  }   // namespace task
}     // namespace fetch
<|endoftext|>


<commit_before><commit_msg>Create callback function, add 'static' keyword<commit_after><|endoftext|>


<commit_before>//---------------------------------------------------------------------------//
/*
  Copyright (c) 2012, Stuart R. Slattery
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:

  *: Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.

  *: Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.

  *: Neither the name of the University of Wisconsin - Madison nor the
  names of its contributors may be used to endorse or promote products
  derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
//---------------------------------------------------------------------------//
/*!
 * \file MCLS_EpetraBlockJacobiPreconditioner.cpp
 * \author Stuart R. Slattery
 * \brief Block Jacobi preconditioning for Epetra.
 */
//---------------------------------------------------------------------------//

#include <algorithm>

#include "MCLS_EpetraBlockJacobiPreconditioner.hpp"

#include <Teuchos_Array.hpp>
#include <Teuchos_ArrayView.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_LAPACK.hpp>
#include <Teuchos_OrdinalTraits.hpp>

#include <Epetra_Map.h>

namespace MCLS
{

//---------------------------------------------------------------------------//
/*!
 * \brief Constructor.
 */
EpetraBlockJacobiPreconditioner::EpetraBlockJacobiPreconditioner(
    const Teuchos::RCP<Teuchos::ParameterList>& params )
    : d_plist( params )
{
    MCLS_REQUIRE( Teuchos::nonnull(d_plist) );
}

//---------------------------------------------------------------------------//
/*!
 * \brief Get the valid parameters for this preconditioner.
 */
Teuchos::RCP<const Teuchos::ParameterList>
EpetraBlockJacobiPreconditioner::getValidParameters() const
{
    Teuchos::RCP<Teuchos::ParameterList> plist = Teuchos::parameterList();
    plist->set<int>("Jacobi Block Size", 0);
    return plist;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Get the current parameters being used for this preconditioner.
 */
Teuchos::RCP<const Teuchos::ParameterList>
EpetraBlockJacobiPreconditioner::getCurrentParameters() const
{
    return d_plist;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Set the parameters for the preconditioner. The preconditioner will
 * modify this list with default parameters that are not defined.
 */
void EpetraBlockJacobiPreconditioner::setParameters(
    const Teuchos::RCP<Teuchos::ParameterList>& params )
{
    MCLS_REQUIRE( Teuchos::nonnull(params) );
    d_plist = params;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Set the operator with the preconditioner.
 */
void EpetraBlockJacobiPreconditioner::setOperator(
    const Teuchos::RCP<const matrix_type>& A )
{
    MCLS_REQUIRE( Teuchos::nonnull(A) );
    d_A = A;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Build the preconditioner.
 */
void EpetraBlockJacobiPreconditioner::buildPreconditioner()
{
    MCLS_REQUIRE( Teuchos::nonnull(d_A) );
    MCLS_REQUIRE( d_A->Filled() );

    // Get the block size.
    int block_size = d_plist->get<int>("Jacobi Block Size");

    // We require that all blocks are local.

    MCLS_REQUIRE( d_A->NumMyRows() % block_size == 0 );

    // Get the number of blocks.
    int num_blocks = d_A->NumMyRows() / block_size;

    // Build the block preconditioner.
    d_preconditioner = Teuchos::rcp(
	new Epetra_CrsMatrix( Copy, d_A->RowMatrixRowMap(), block_size ) );

    // Populate the preconditioner with inverted blocks.
    Teuchos::SerialDenseMatrix<int,double> block( block_size, block_size );
    int col_start = 0;
    int global_row = 0;
    Teuchos::Array<int> block_cols( block_size );
    for ( int n = 0; n < num_blocks; ++n )
    {
	// Starting row/column for the block.
	col_start = block_size*n;

	// Extract the block. Note that I form the tranposed local block to
	// facilitate constructing the preconditioner in the second group of
	// loops. I grab each individual element here because I want the
	// zero's to build the block, but there's probably a cheaper way of
	// doing the extraction.
	for ( int i = 0; i < block_size; ++i )
	{
	    global_row = d_A->RowMatrixRowMap().GID(col_start+i);
	    for ( int j = 0; j < block_size; ++j )
	    {
		block_cols[j] = d_A->RowMatrixColMap().GID(col_start+j);
	    }
	    for ( int j = 0; j < block_size; ++j )
	    {
		block(j,i) =
		    getMatrixComponentFromGlobal( d_A, global_row, block_cols[j] );
	    }
	}

	// Invert the block.
	invertSerialDenseMatrix( block );

	// Add the block to the preconditioner.
	for ( int i = 0; i < block_size; ++i )
	{
	    global_row =
		d_preconditioner->RowMatrixRowMap().GID(col_start+i);

	    d_preconditioner->InsertGlobalValues(
		global_row, block_size, block[i], block_cols.getRawPtr() );
	}
    }

    d_preconditioner->FillComplete();

    MCLS_ENSURE( Teuchos::nonnull(d_preconditioner) );
    MCLS_ENSURE( d_preconditioner->Filled() );
}

//---------------------------------------------------------------------------//
/*!
 * \brief Invert a Teuchos::SerialDenseMatrix block.
 */
void EpetraBlockJacobiPreconditioner::invertSerialDenseMatrix(
    Teuchos::SerialDenseMatrix<int,double>& block )
{
    // Make a LAPACK object.
    Teuchos::LAPACK<int,double> lapack;

    // Compute the LU-factorization of the block.
    int ipiv = 0;
    int info = 0;
    lapack.GETRF( block.numRows(), block.numCols(), block.values(),
		  block.stride(), &ipiv, &info );
    MCLS_CHECK( info == 0 );

    // Compute the inverse of the block from the LU-factorization.
    Teuchos::Array<double> work( block.numRows() );
    lapack.GETRI(
        block.numCols(), block.values(), block.stride(),
	&ipiv, work.getRawPtr(), work.size(), &info );
    MCLS_CHECK( info == 0 );
    MCLS_CHECK( work[0] == block.numRows() );
}

//---------------------------------------------------------------------------//
/*!
 * \brief Get a local component of an operator given a global row and column
 * index.
 */
double EpetraBlockJacobiPreconditioner::getMatrixComponentFromGlobal(
    const Teuchos::RCP<const matrix_type>& matrix,
    const int global_row, const int global_col )
{
    const Epetra_Map row_map = matrix->RowMatrixRowMap();
    const Epetra_Map col_map = matrix->RowMatrixColMap();

    MCLS_REQUIRE( row_map.MyGID(global_row) );

    int local_row = row_map.LID( global_row );
    int local_col = col_map.LID( global_col );

    // If the block column is not local, then we get a zero for this entry.
    if ( local_col == Teuchos::OrdinalTraits<int>::invalid() )
    {
	return 0.0;
    }

    int max_size = matrix->MaxNumEntries();
    int num_entries = 0;
    Teuchos::Array<int> local_indices( max_size );
    Teuchos::Array<double> local_values( max_size );
    matrix->ExtractMyRowCopy( local_row, max_size, num_entries,
			      local_values.getRawPtr(),
			      local_indices.getRawPtr() );
    local_values.resize( num_entries );
    local_indices.resize( num_entries );

    Teuchos::Array<int>::iterator local_idx_it =
	std::find( local_indices.begin(), local_indices.end(), local_col );

    if ( local_idx_it != local_indices.end() )
    {
	return local_values[ std::distance( local_indices.begin(),
					    local_idx_it ) ];
    }

    return 0.0;
}

//---------------------------------------------------------------------------//

} // end namespace MCLS

//---------------------------------------------------------------------------//
// end MCLS_EpetraBlockJacobiPreconditioner.cpp
//---------------------------------------------------------------------------//
<commit_msg>removing whitespace in blaock jacobi preconditioner<commit_after>//---------------------------------------------------------------------------//
/*
  Copyright (c) 2012, Stuart R. Slattery
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:

  *: Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.

  *: Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.

  *: Neither the name of the University of Wisconsin - Madison nor the
  names of its contributors may be used to endorse or promote products
  derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
//---------------------------------------------------------------------------//
/*!
 * \file MCLS_EpetraBlockJacobiPreconditioner.cpp
 * \author Stuart R. Slattery
 * \brief Block Jacobi preconditioning for Epetra.
 */
//---------------------------------------------------------------------------//

#include <algorithm>

#include "MCLS_EpetraBlockJacobiPreconditioner.hpp"

#include <Teuchos_Array.hpp>
#include <Teuchos_ArrayView.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_LAPACK.hpp>
#include <Teuchos_OrdinalTraits.hpp>

#include <Epetra_Map.h>

namespace MCLS
{

//---------------------------------------------------------------------------//
/*!
 * \brief Constructor.
 */
EpetraBlockJacobiPreconditioner::EpetraBlockJacobiPreconditioner(
    const Teuchos::RCP<Teuchos::ParameterList>& params )
    : d_plist( params )
{
    MCLS_REQUIRE( Teuchos::nonnull(d_plist) );
}

//---------------------------------------------------------------------------//
/*!
 * \brief Get the valid parameters for this preconditioner.
 */
Teuchos::RCP<const Teuchos::ParameterList>
EpetraBlockJacobiPreconditioner::getValidParameters() const
{
    Teuchos::RCP<Teuchos::ParameterList> plist = Teuchos::parameterList();
    plist->set<int>("Jacobi Block Size", 1);
    return plist;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Get the current parameters being used for this preconditioner.
 */
Teuchos::RCP<const Teuchos::ParameterList>
EpetraBlockJacobiPreconditioner::getCurrentParameters() const
{
    return d_plist;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Set the parameters for the preconditioner. The preconditioner will
 * modify this list with default parameters that are not defined.
 */
void EpetraBlockJacobiPreconditioner::setParameters(
    const Teuchos::RCP<Teuchos::ParameterList>& params )
{
    MCLS_REQUIRE( Teuchos::nonnull(params) );
    d_plist = params;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Set the operator with the preconditioner.
 */
void EpetraBlockJacobiPreconditioner::setOperator(
    const Teuchos::RCP<const matrix_type>& A )
{
    MCLS_REQUIRE( Teuchos::nonnull(A) );
    d_A = A;
}

//---------------------------------------------------------------------------//
/*!
 * \brief Build the preconditioner.
 */
void EpetraBlockJacobiPreconditioner::buildPreconditioner()
{
    MCLS_REQUIRE( Teuchos::nonnull(d_A) );
    MCLS_REQUIRE( d_A->Filled() );

    // Get the block size.
    int block_size = d_plist->get<int>("Jacobi Block Size");

    // We require that all blocks are local.
    MCLS_REQUIRE( d_A->NumMyRows() % block_size == 0 );

    // Get the number of blocks.
    int num_blocks = d_A->NumMyRows() / block_size;

    // Build the block preconditioner.
    d_preconditioner = Teuchos::rcp(
	new Epetra_CrsMatrix( Copy, d_A->RowMatrixRowMap(), block_size ) );

    // Populate the preconditioner with inverted blocks.
    Teuchos::SerialDenseMatrix<int,double> block( block_size, block_size );
    int col_start = 0;
    int global_row = 0;
    Teuchos::Array<int> block_cols( block_size );
    for ( int n = 0; n < num_blocks; ++n )
    {
	// Starting row/column for the block.
	col_start = block_size*n;

	// Extract the block. Note that I form the tranposed local block to
	// facilitate constructing the preconditioner in the second group of
	// loops. I grab each individual element here because I want the
	// zero's to build the block, but there's probably a cheaper way of
	// doing the extraction.
	for ( int i = 0; i < block_size; ++i )
	{
	    global_row = d_A->RowMatrixRowMap().GID(col_start+i);
	    for ( int j = 0; j < block_size; ++j )
	    {
		block_cols[j] = d_A->RowMatrixColMap().GID(col_start+j);
	    }
	    for ( int j = 0; j < block_size; ++j )
	    {
		block(j,i) =
		    getMatrixComponentFromGlobal( d_A, global_row, block_cols[j] );
	    }
	}

	// Invert the block.
	invertSerialDenseMatrix( block );

	// Add the block to the preconditioner.
	for ( int i = 0; i < block_size; ++i )
	{
	    global_row =
		d_preconditioner->RowMatrixRowMap().GID(col_start+i);

	    d_preconditioner->InsertGlobalValues(
		global_row, block_size, block[i], block_cols.getRawPtr() );
	}
    }

    d_preconditioner->FillComplete();

    MCLS_ENSURE( Teuchos::nonnull(d_preconditioner) );
    MCLS_ENSURE( d_preconditioner->Filled() );
}

//---------------------------------------------------------------------------//
/*!
 * \brief Invert a Teuchos::SerialDenseMatrix block.
 */
void EpetraBlockJacobiPreconditioner::invertSerialDenseMatrix(
    Teuchos::SerialDenseMatrix<int,double>& block )
{
    // Make a LAPACK object.
    Teuchos::LAPACK<int,double> lapack;

    // Compute the LU-factorization of the block.
    int ipiv = 0;
    int info = 0;
    lapack.GETRF( block.numRows(), block.numCols(), block.values(),
		  block.stride(), &ipiv, &info );
    MCLS_CHECK( info == 0 );

    // Compute the inverse of the block from the LU-factorization.
    Teuchos::Array<double> work( block.numRows() );
    lapack.GETRI(
        block.numCols(), block.values(), block.stride(),
	&ipiv, work.getRawPtr(), work.size(), &info );
    MCLS_CHECK( info == 0 );
    MCLS_CHECK( work[0] == block.numRows() );
}

//---------------------------------------------------------------------------//
/*!
 * \brief Get a local component of an operator given a global row and column
 * index.
 */
double EpetraBlockJacobiPreconditioner::getMatrixComponentFromGlobal(
    const Teuchos::RCP<const matrix_type>& matrix,
    const int global_row, const int global_col )
{
    const Epetra_Map row_map = matrix->RowMatrixRowMap();
    const Epetra_Map col_map = matrix->RowMatrixColMap();

    MCLS_REQUIRE( row_map.MyGID(global_row) );

    int local_row = row_map.LID( global_row );
    int local_col = col_map.LID( global_col );

    // If the block column is not local, then we get a zero for this entry.
    if ( local_col == Teuchos::OrdinalTraits<int>::invalid() )
    {
	return 0.0;
    }

    int max_size = matrix->MaxNumEntries();
    int num_entries = 0;
    Teuchos::Array<int> local_indices( max_size );
    Teuchos::Array<double> local_values( max_size );
    matrix->ExtractMyRowCopy( local_row, max_size, num_entries,
			      local_values.getRawPtr(),
			      local_indices.getRawPtr() );
    local_values.resize( num_entries );
    local_indices.resize( num_entries );

    Teuchos::Array<int>::iterator local_idx_it =
	std::find( local_indices.begin(), local_indices.end(), local_col );

    if ( local_idx_it != local_indices.end() )
    {
	return local_values[ std::distance( local_indices.begin(),
					    local_idx_it ) ];
    }

    return 0.0;
}

//---------------------------------------------------------------------------//

} // end namespace MCLS

//---------------------------------------------------------------------------//
// end MCLS_EpetraBlockJacobiPreconditioner.cpp
//---------------------------------------------------------------------------//
<|endoftext|>