"Инструкция точки останова (оператор __debugbreak(...

Ниже полный вариант с исправленной индексацией seed, без двойного запуска benchmark и с переиспользованием TRAC_IK для каждой размерности.

cpp
#pragma once

#define WIN32_LEAN_AND_MEAN
#include <Windows.h>

#include <cstddef>
#include <cfloat>
#include <cstdint>
#include <cmath>
#include <algorithm>
#include <vector>
#include <memory>

#include <kdl/chain.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/frames.hpp>
#include <kdl/joint.hpp>
#include <kdl/segment.hpp>
#include <trac_ik/trac_ik.hpp>

using namespace System;
using namespace System::Drawing;
using namespace System::Windows::Forms;
using namespace System::Collections::Generic;
using namespace System::Text;
using namespace System::Windows::Forms::DataVisualization::Charting;

typedef void(__cdecl* P_MANIP_INDEX)(
	int,
	bool,
	float,
	float,
	float,
	float,
	int,
	float,
	bool,
	float,
	unsigned int,
	float,
	float,
	const float*,
	int,
	float**,
	float*,
	float*,
	float*,
	float*,
	size_t*,
	float*,
	int,
	const float*
	);

typedef void(__cdecl* P_FREE)(
	float*
	);

typedef void(__cdecl* P_START_INDEX)(
	int,
	bool,
	float,
	float,
	float,
	float,
	int,
	float,
	bool,
	float,
	unsigned int,
	float,
	float,
	const float*,
	int,
	int,
	const float*,
	const float*
	);

class TracIkRunner final
{
private:
	int nSegments_;
	unsigned int nJoints_;
	double eps_;
	double baseLength_;

	KDL::Chain chain_;
	KDL::JntArray qMin_;
	KDL::JntArray qMax_;
	KDL::JntArray qInit_;
	KDL::JntArray qOut_;

	std::unique_ptr<trac_ik::TRAC_IK> solver_;

	LARGE_INTEGER frequency_;

public:
	TracIkRunner(
		int nSegments,
		double baseLength,
		double maxTime,
		double eps)
		:
		nSegments_(nSegments),
		nJoints_(0),
		eps_(eps),
		baseLength_(baseLength)
	{
		for (int i = 0; i < nSegments_; ++i) {
			chain_.addSegment(
				KDL::Segment(
					KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame(
						KDL::Vector(
							baseLength_,
							0.0,
							0.0
						)
					)
				)
			);
		}

		nJoints_ = chain_.getNrOfJoints();

		qMin_.resize(nJoints_);
		qMax_.resize(nJoints_);
		qInit_.resize(nJoints_);
		qOut_.resize(nJoints_);

		const double pi = 3.1415926535897932384626433832795;

		for (unsigned int i = 0; i < nJoints_; ++i) {
			qMin_(i) = -pi;
			qMax_(i) = pi;
			qInit_(i) = 0.0;
			qOut_(i) = 0.0;
		}

		solver_ = std::make_unique<trac_ik::TRAC_IK>(
			chain_,
			qMin_,
			qMax_,
			maxTime,
			eps_,
			trac_ik::SolveType::Speed
		);

		QueryPerformanceFrequency(&frequency_);
	}

	TracIkRunner(const TracIkRunner&) = delete;
	TracIkRunner& operator=(const TracIkRunner&) = delete;

	bool Solve(
		float targetX,
		float targetY,
		float targetAngle,
		double& outBestF,
		double& outBestX,
		double& outBestY,
		double& outMillis)
	{
		for (unsigned int i = 0; i < nJoints_; ++i) {
			qInit_(i) = 0.0;
			qOut_(i) = 0.0;
		}

		KDL::Frame target(
			KDL::Rotation::RotZ(
				static_cast<double>(targetAngle)
			),
			KDL::Vector(
				static_cast<double>(targetX),
				static_cast<double>(targetY),
				0.0
			)
		);

		KDL::Twist tolerances(
			KDL::Vector(
				eps_,
				eps_,
				0.0
			),
			KDL::Vector(
				0.0,
				0.0,
				eps_
			)
		);

		LARGE_INTEGER t0;
		LARGE_INTEGER t1;

		QueryPerformanceCounter(&t0);

		const int result = solver_->CartToJnt(
			qInit_,
			target,
			qOut_,
			tolerances
		);

		QueryPerformanceCounter(&t1);

		outMillis =
			1.0e3 *
			static_cast<double>(t1.QuadPart - t0.QuadPart) /
			static_cast<double>(frequency_.QuadPart);

		if (result < 0) {
			outBestF = std::numeric_limits<double>::quiet_NaN();
			outBestX = std::numeric_limits<double>::quiet_NaN();
			outBestY = std::numeric_limits<double>::quiet_NaN();
			return false;
		}

		const double pi = 3.1415926535897932384626433832795;

		double x = 0.0;
		double y = 0.0;
		double phi = 0.0;

		for (unsigned int i = 0; i < nJoints_; ++i) {
			phi += qOut_(i);
			x += baseLength_ * std::cos(phi);
			y += baseLength_ * std::sin(phi);
		}

		const double dx =
			x - static_cast<double>(targetX);

		const double dy =
			y - static_cast<double>(targetY);

		double da =
			phi - static_cast<double>(targetAngle);

		da = std::fmod(
			da + 3.0 * pi,
			2.0 * pi
		) - pi;

		const double f =
			std::sqrt(
				dx * dx +
				dy * dy +
				da * da
			);

		if (!std::isfinite(f) ||
			!std::isfinite(x) ||
			!std::isfinite(y) ||
			!std::isfinite(outMillis)) {
			return false;
		}

		outBestF = f;
		outBestX = x;
		outBestY = y;

		return true;
	}
};

namespace TESTAGP {

	public ref struct BenchmarkCase sealed
	{
	public:
		float TargetX;
		float TargetY;
		float TargetAngle;

		double TracF;
		double TracX;
		double TracY;
		double TracMillis;
	};

	public ref class MyForm sealed : public Form
	{
	public:
		MyForm(HMODULE hLib)
			:
			hLib(hLib),
			experimentsStarted(false)
		{
			this->Text =
				L"AGP vs TRAC-IK - позиционирование с ориентацией";

			this->ClientSize =
				System::Drawing::Size(
					1350,
					860
				);

			this->SetStyle(
				ControlStyles::AllPaintingInWmPaint |
				ControlStyles::UserPaint |
				ControlStyles::OptimizedDoubleBuffer,
				true
			);

			fManipIndex =
				reinterpret_cast<P_MANIP_INDEX>(
					GetProcAddress(
						hLib,
						"AGP_Manip2D"
					)
				);

			pFree =
				reinterpret_cast<P_FREE>(
					GetProcAddress(
						hLib,
						"AGP_Free"
					)
				);

			pStartIndex =
				reinterpret_cast<P_START_INDEX>(
					GetProcAddress(
						hLib,
						"AgpStartManipND"
					)
				);

			if (!fManipIndex ||
				!pFree ||
				!pStartIndex) {
				MessageBox::Show(
					L"Не удалось получить адреса AGP_Manip2D / AGP_Free / AgpStartManipND из DLL",
					L"Ошибка",
					MessageBoxButtons::OK,
					MessageBoxIcon::Error
				);

				this->Close();
				return;
			}

			maxTheta =
				static_cast<float>(
					Math::PI
				);

			rParam = 1.05f;
			epsParam = 1.0e-9f;
			baseLength = 1.0f;
			stretchFactor = 1.0f;
			adaptiveAgp = true;

			randomEngine =
				gcnew Random(
					123456
				);

			uiFontBold11 =
				gcnew System::Drawing::Font(
					"Yu Gothic UI",
					11,
					FontStyle::Bold
				);

			uiFont10 =
				gcnew System::Drawing::Font(
					"Yu Gothic UI",
					10,
					FontStyle::Regular
				);

			InitUI();

			this->Shown +=
				gcnew EventHandler(
					this,
					&MyForm::OnFormShown
				);

			this->Resize +=
				gcnew EventHandler(
					this,
					&MyForm::OnResizeInternal
				);
		}

	private:
		initonly HMODULE hLib;
		initonly P_MANIP_INDEX fManipIndex;
		initonly P_FREE pFree;
		initonly P_START_INDEX pStartIndex;

		System::Drawing::Font^ uiFontBold11;
		System::Drawing::Font^ uiFont10;

		Random^ randomEngine;

		Chart^ chartTime;
		Chart^ chartF;
		TextBox^ tbStats;

		bool experimentsStarted;

		float maxTheta;
		float rParam;
		float epsParam;
		float baseLength;
		float stretchFactor;
		bool adaptiveAgp;

		double NextUniform(
			double a,
			double b)
		{
			return
				a +
				(b - a) *
				randomEngine->NextDouble();
		}

		unsigned int NextSeedU32()
		{
			const unsigned int hi =
				static_cast<unsigned int>(
					randomEngine->Next(
						1,
						Int32::MaxValue
					)
				);

			const unsigned int lo =
				static_cast<unsigned int>(
					randomEngine->Next(
						0,
						Int32::MaxValue
					)
				);

			return
				(hi << 1) ^
				lo ^
				0x9E3779B9u;
		}

		static void ComputeMeanStd(
			List<double>^ values,
			double% mean,
			double% stdDev)
		{
			const int n =
				values->Count;

			if (n <= 0) {
				mean =
					Double::NaN;

				stdDev =
					Double::NaN;

				return;
			}

			double sum = 0.0;

			for each (double v in values) {
				if (Double::IsNaN(v) ||
					Double::IsInfinity(v)) {
					continue;
				}

				sum += v;
			}

			int validCount = 0;

			for each (double v in values) {
				if (!Double::IsNaN(v) &&
					!Double::IsInfinity(v)) {
					++validCount;
				}
			}

			if (validCount <= 0) {
				mean =
					Double::NaN;

				stdDev =
					Double::NaN;

				return;
			}

			mean =
				sum /
				static_cast<double>(
					validCount
				);

			if (validCount == 1) {
				stdDev = 0.0;
				return;
			}

			double varianceSum = 0.0;

			for each (double v in values) {
				if (Double::IsNaN(v) ||
					Double::IsInfinity(v)) {
					continue;
				}

				const double d =
					v - mean;

				varianceSum +=
					d * d;
			}

			stdDev =
				Math::Sqrt(
					varianceSum /
					static_cast<double>(
						validCount - 1
					)
				);
		}

		static bool IsFinite(
			double value)
		{
			return
				!Double::IsNaN(value) &&
				!Double::IsInfinity(value);
		}

		static bool IsFinitePair(
			double x,
			double y)
		{
			return
				IsFinite(x) &&
				IsFinite(y);
		}

		BenchmarkCase^ GenerateRandomCase(
			int nSegments)
		{
			BenchmarkCase^ task =
				gcnew BenchmarkCase();

			const double maxReach =
				Math::Max(
					1.0,
					static_cast<double>(
						nSegments
					) *
					static_cast<double>(
						baseLength
					)
				);

			const double targetRadiusMin =
				0.30 *
				maxReach;

			const double targetRadiusMax =
				0.90 *
				maxReach;

			const double angle =
				NextUniform(
					0.0,
					2.0 * Math::PI
				);

			const double radius =
				NextUniform(
					targetRadiusMin,
					targetRadiusMax
				);

			const double targetAngle =
				NextUniform(
					-Math::PI,
					Math::PI
				);

			task->TargetX =
				static_cast<float>(
					radius *
					Math::Cos(angle)
				);

			task->TargetY =
				static_cast<float>(
					radius *
					Math::Sin(angle)
				);

			task->TargetAngle =
				static_cast<float>(
					targetAngle
				);

			task->TracF =
				Double::NaN;

			task->TracX =
				Double::NaN;

			task->TracY =
				Double::NaN;

			task->TracMillis =
				Double::NaN;

			return task;
		}

		void InitUI()
		{
			chartTime =
				gcnew Chart();

			chartF =
				gcnew Chart();

			tbStats =
				gcnew TextBox();

			chartTime->Parent =
				this;

			chartF->Parent =
				this;

			tbStats->Parent =
				this;

			chartTime->BorderlineDashStyle =
				ChartDashStyle::Solid;

			chartTime->BorderlineWidth =
				1;

			chartTime->BorderlineColor =
				Color::Black;

			chartF->BorderlineDashStyle =
				ChartDashStyle::Solid;

			chartF->BorderlineWidth =
				1;

			chartF->BorderlineColor =
				Color::Black;

			ChartArea^ areaTime =
				gcnew ChartArea(
					"TimeArea"
				);

			areaTime->AxisX->Title =
				"Порог maxIter";

			areaTime->AxisY->Title =
				"Время, мс";

			areaTime->AxisX->Minimum =
				100.0;

			areaTime->AxisX->Maximum =
				1000.0;

			areaTime->AxisY->Minimum =
				0.0;

			chartTime->ChartAreas->Add(
				areaTime
			);

			Legend^ legendTime =
				gcnew Legend(
					"LegendTime"
				);

			legendTime->Docking =
				Docking::Top;

			legendTime->Font =
				gcnew System::Drawing::Font(
					"Yu Gothic UI",
					10,
					FontStyle::Bold
				);

			chartTime->Legends->Add(
				legendTime
			);

			CreateSeriesWithStyle(
				chartTime,
				"AGP",
				"TimeArea",
				Color::Red,
				3
			);

			CreateSeriesWithStyle(
				chartTime,
				"TRAC IK",
				"TimeArea",
				Color::Blue,
				3
			);

			ChartArea^ areaF =
				gcnew ChartArea(
					"FArea"
				);

			areaF->AxisX->Title =
				"Порог maxIter";

			areaF->AxisY->Title =
				"Значение целевой функции f";

			areaF->AxisX->Minimum =
				100.0;

			areaF->AxisX->Maximum =
				1000.0;

			areaF->AxisY->Minimum =
				0.0;

			chartF->ChartAreas->Add(
				areaF
			);

			Legend^ legendF =
				gcnew Legend(
					"LegendF"
				);

			legendF->Docking =
				Docking::Top;

			legendF->Font =
				gcnew System::Drawing::Font(
					"Yu Gothic UI",
					10,
					FontStyle::Bold
				);

			chartF->Legends->Add(
				legendF
			);

			CreateSeriesWithStyle(
				chartF,
				"AGP",
				"FArea",
				Color::Red,
				3
			);

			CreateSeriesWithStyle(
				chartF,
				"TRAC IK",
				"FArea",
				Color::Blue,
				3
			);

			tbStats->Multiline =
				true;

			tbStats->ReadOnly =
				true;

			tbStats->ScrollBars =
				ScrollBars::Vertical;

			tbStats->Font =
				uiFont10;

			tbStats->Text =
				L"Подготовка benchmark: генерация хороших задач...\r\n";

			OnResizeInternal(
				nullptr,
				nullptr
			);
		}

		void CreateSeriesWithStyle(
			Chart^ chart,
			String^ name,
			String^ areaName,
			Color color,
			int borderWidth)
		{
			Series^ s =
				gcnew Series(
					name
				);

			s->ChartType =
				SeriesChartType::Line;

			s->ChartArea =
				areaName;

			s->Color =
				color;

			s->BorderWidth =
				borderWidth;

			s->MarkerStyle =
				MarkerStyle::Circle;

			s->MarkerSize =
				7;

			s->MarkerColor =
				color;

			chart->Series->Add(
				s
			);
		}

		void OnResizeInternal(
			Object^,
			EventArgs^)
		{
			const int margin =
				10;

			const int statsWidth =
				320;

			int totalWidth =
				this->ClientSize.Width;

			int totalHeight =
				this->ClientSize.Height;

			if (totalWidth < 500) {
				totalWidth = 500;
			}

			if (totalHeight < 320) {
				totalHeight = 320;
			}

			int chartsWidth =
				totalWidth -
				statsWidth -
				3 * margin;

			if (chartsWidth < 250) {
				chartsWidth = 250;
			}

			const int chartHeight =
				(totalHeight - 3 * margin) /
				2;

			chartTime->Location =
				Point(
					margin,
					margin
				);

			chartTime->Size =
				System::Drawing::Size(
					chartsWidth,
					chartHeight
				);

			chartF->Location =
				Point(
					margin,
					2 * margin + chartHeight
				);

			chartF->Size =
				System::Drawing::Size(
					chartsWidth,
					chartHeight
				);

			tbStats->Location =
				Point(
					2 * margin + chartsWidth,
					margin
				);

			tbStats->Size =
				System::Drawing::Size(
					statsWidth,
					totalHeight - 2 * margin
				);
		}

		void OnFormShown(
			Object^,
			EventArgs^)
		{
			if (experimentsStarted) {
				return;
			}

			experimentsStarted =
				true;

			try {
				RunAllExperiments();
			}
			catch (Exception^ ex) {
				tbStats->Text =
					L"Ошибка при выполнении benchmark:\r\n" +
					ex->ToString();
			}
		}

		bool RunAgpIndexSingle(
			int nSegments,
			BenchmarkCase^ task,
			int maxIter,
			unsigned int seed,
			double% outBestF,
			double% outBestX,
			double% outBestY,
			std::size_t% outIterations,
			double% outAchievedEps,
			double% outMillis)
		{
			float* bestQ =
				nullptr;

			float bestXf =
				0.0f;

			float bestYf =
				0.0f;

			float bestAf =
				0.0f;

			float bestFf =
				FLT_MAX;

			std::size_t actualIterations =
				0;

			float achievedEps =
				0.0f;

			const float* obsRaw =
				nullptr;

			const int obstacleCount =
				0;

			const float ta =
				task->TargetAngle;

			pStartIndex(
				nSegments,
				false,
				maxTheta,
				task->TargetX,
				task->TargetY,
				ta,
				maxIter,
				rParam,
				adaptiveAgp,
				epsParam,
				seed,
				baseLength,
				stretchFactor,
				obsRaw,
				obstacleCount,
				0,
				nullptr,
				nullptr
			);

			LARGE_INTEGER t0;
			LARGE_INTEGER t1;
			LARGE_INTEGER fq;

			QueryPerformanceFrequency(
				&fq
			);

			QueryPerformanceCounter(
				&t0
			);

			fManipIndex(
				nSegments,
				false,
				maxTheta,
				task->TargetX,
				task->TargetY,
				ta,
				maxIter,
				rParam,
				adaptiveAgp,
				epsParam,
				seed,
				baseLength,
				stretchFactor,
				obsRaw,
				obstacleCount,
				&bestQ,
				&bestXf,
				&bestYf,
				&bestAf,
				&bestFf,
				&actualIterations,
				&achievedEps,
				0,
				nullptr
			);

			QueryPerformanceCounter(
				&t1
			);

			outBestF =
				static_cast<double>(
					bestFf
				);

			outBestX =
				static_cast<double>(
					bestXf
				);

			outBestY =
				static_cast<double>(
					bestYf
				);

			outIterations =
				actualIterations;

			outAchievedEps =
				static_cast<double>(
					achievedEps
				);

			outMillis =
				1.0e3 *
				static_cast<double>(
					t1.QuadPart -
					t0.QuadPart
				) /
				static_cast<double>(
					fq.QuadPart
				);

			if (bestQ != nullptr) {
				pFree(
					bestQ
				);

				bestQ =
					nullptr;
			}

			if (!IsFinite(outBestF) ||
				!IsFinite(outBestX) ||
				!IsFinite(outBestY) ||
				!IsFinite(outAchievedEps) ||
				!IsFinite(outMillis)) {
				return false;
			}

			return true;
		}

		bool RunTracIkSingle(
			TracIkRunner& runner,
			BenchmarkCase^ task,
			double% outBestF,
			double% outBestX,
			double% outBestY,
			double% outMillis)
		{
			double bestF =
				Double::NaN;

			double bestX =
				Double::NaN;

			double bestY =
				Double::NaN;

			double millis =
				Double::NaN;

			const bool result =
				runner.Solve(
					task->TargetX,
					task->TargetY,
					task->TargetAngle,
					bestF,
					bestX,
					bestY,
					millis
				);

			outBestF =
				bestF;

			outBestX =
				bestX;

			outBestY =
				bestY;

			outMillis =
				millis;

			return
				result &&
				IsFinitePair(
					outBestF,
					outMillis
				);
		}

		void RunAllExperiments()
		{
			array<int>^ dimensions =
				gcnew array<int>
				{
					3,
					4,
					5,
					6,
					7,
					8
				};

			array<int>^ thresholds =
				gcnew array<int>
				{
					100,
					200,
					300,
					400,
					500,
					600,
					700,
					800,
					900,
					1000
				};

			const int steps =
				thresholds->Length;

			const int dimensionCount =
				dimensions->Length;

			const int goodTasksPerDimension =
				10;

			const int repeatsStochastic =
				100;

			const double tracMaxTime =
				0.002;

			array<List<BenchmarkCase^>^>^ goodTasksByDimension =
				gcnew array<List<BenchmarkCase^>^>(
					dimensionCount
				);

			for (int dimIndex = 0;
				dimIndex < dimensionCount;
				++dimIndex) {

				const int nSegments =
					dimensions[dimIndex];

				tbStats->AppendText(
					String::Format(
						L"Генерация хороших задач для размерности {0}...\r\n",
						nSegments
					)
				);

				Application::DoEvents();

				List<BenchmarkCase^>^ goodForDim =
					gcnew List<BenchmarkCase^>();

				TracIkRunner tracRunner(
					nSegments,
					static_cast<double>(
						baseLength
					),
					tracMaxTime,
					static_cast<double>(
						epsParam
					)
				);

				int attempts =
					0;

				while (goodForDim->Count <
					goodTasksPerDimension) {

					++attempts;

					BenchmarkCase^ task =
						GenerateRandomCase(
							nSegments
						);

					double f;
					double x;
					double y;
					double ms;

					const bool ok =
						RunTracIkSingle(
							tracRunner,
							task,
							f,
							x,
							y,
							ms
						);

					if (ok &&
						f <= 0.05) {

						task->TracF =
							f;

						task->TracX =
							x;

						task->TracY =
							y;

						task->TracMillis =
							ms;

						goodForDim->Add(
							task
						);

						tbStats->AppendText(
							String::Format(
								L"  Найдена хорошая задача {0}, ошибка = {1:F6}, время = {2:F4} мс\r\n",
								goodForDim->Count,
								f,
								ms
							)
						);
					}

					if ((attempts % 32) == 0) {
						Application::DoEvents();
					}
				}

				goodTasksByDimension[dimIndex] =
					goodForDim;

				Application::DoEvents();
			}

			const int totalGoodTasks =
				dimensionCount *
				goodTasksPerDimension;

			const int totalSeedCount =
				totalGoodTasks *
				repeatsStochastic;

			array<unsigned int>^ fixedSeeds =
				gcnew array<unsigned int>(
					totalSeedCount
				);

			for (int i = 0;
				i < totalSeedCount;
				++i) {

				fixedSeeds[i] =
					NextSeedU32();
			}

			array<double>^ agpTimeMean =
				gcnew array<double>(
					steps
				);

			array<double>^ agpTimeStd =
				gcnew array<double>(
					steps
				);

			array<double>^ agpFMean =
				gcnew array<double>(
					steps
				);

			array<double>^ agpFStd =
				gcnew array<double>(
					steps
				);

			array<double>^ tracTimeMean =
				gcnew array<double>(
					steps
				);

			array<double>^ tracTimeStd =
				gcnew array<double>(
					steps
				);

			array<double>^ tracFMean =
				gcnew array<double>(
					steps
				);

			array<double>^ tracFStd =
				gcnew array<double>(
					steps
				);

			for (int i = 0;
				i < steps;
				++i) {

				agpTimeMean[i] =
					Double::NaN;

				agpTimeStd[i] =
					Double::NaN;

				agpFMean[i] =
					Double::NaN;

				agpFStd[i] =
					Double::NaN;

				tracTimeMean[i] =
					Double::NaN;

				tracTimeStd[i] =
					Double::NaN;

				tracFMean[i] =
					Double::NaN;

				tracFStd[i] =
					Double::NaN;
			}

			array<List<double>^>^ agpTimeForThreshold =
				gcnew array<List<double>^>(
					steps
				);

			array<List<double>^>^ agpFForThreshold =
				gcnew array<List<double>^>(
					steps
				);

			array<List<double>^>^ tracTimeForThreshold =
				gcnew array<List<double>^>(
					steps
				);

			array<List<double>^>^ tracFForThreshold =
				gcnew array<List<double>^>(
					steps
				);

			for (int i = 0;
				i < steps;
				++i) {

				agpTimeForThreshold[i] =
					gcnew List<double>();

				agpFForThreshold[i] =
					gcnew List<double>();

				tracTimeForThreshold[i] =
					gcnew List<double>();

				tracFForThreshold[i] =
					gcnew List<double>();
			}

			List<double>^ agpTimesAll =
				gcnew List<double>();

			List<double>^ agpFAll =
				gcnew List<double>();

			List<double>^ tracTimesAll =
				gcnew List<double>();

			List<double>^ tracFAll =
				gcnew List<double>();

			for (int dimIndex = 0;
				dimIndex < dimensionCount;
				++dimIndex) {

				const int nSegments =
					dimensions[dimIndex];

				List<BenchmarkCase^>^ tasks =
					goodTasksByDimension[dimIndex];

				tbStats->AppendText(
					String::Format(
						L"\r\nBenchmark для размерности {0}...\r\n",
						nSegments
					)
				);

				Application::DoEvents();

				for (int caseIndex = 0;
					caseIndex < goodTasksPerDimension;
					++caseIndex) {

					BenchmarkCase^ task =
						tasks[caseIndex];

					const int globalTaskIndex =
						dimIndex *
						goodTasksPerDimension +
						caseIndex;

					if (IsFinite(task->TracF) &&
						IsFinite(task->TracMillis)) {

						tracFAll->Add(
							task->TracF
						);

						tracTimesAll->Add(
							task->TracMillis
						);
					}

					for (int thresholdIndex = 0;
						thresholdIndex < steps;
						++thresholdIndex) {

						const int maxIter =
							thresholds[thresholdIndex];

						if (IsFinite(task->TracF) &&
							IsFinite(task->TracMillis)) {

							tracFForThreshold[thresholdIndex]->Add(
								task->TracF
							);

							tracTimeForThreshold[thresholdIndex]->Add(
								task->TracMillis
							);
						}

						for (int r = 0;
							r < repeatsStochastic;
							++r) {

							const int seedIndex =
								globalTaskIndex *
								repeatsStochastic +
								r;

							if (seedIndex < 0 ||
								seedIndex >= fixedSeeds->Length) {

								throw gcnew IndexOutOfRangeException(
									String::Format(
										L"Некорректный индекс seed: {0}, размер массива: {1}",
										seedIndex,
										fixedSeeds->Length
									)
								);
							}

							const unsigned int seed =
								fixedSeeds[seedIndex];

							double bF;
							double bX;
							double bY;
							double achievedEps;
							double tMs;

							std::size_t iterations =
								0;

							const bool ok =
								RunAgpIndexSingle(
									nSegments,
									task,
									maxIter,
									seed,
									bF,
									bX,
									bY,
									iterations,
									achievedEps,
									tMs
								);

							if (ok) {
								agpFForThreshold[thresholdIndex]->Add(
									bF
								);

								agpTimeForThreshold[thresholdIndex]->Add(
									tMs
								);

								agpFAll->Add(
									bF
								);

								agpTimesAll->Add(
									tMs
								);
							}
						}

						Application::DoEvents();
					}

					tbStats->AppendText(
						String::Format(
							L"  Задача {0}/{1} завершена.\r\n",
							caseIndex + 1,
							goodTasksPerDimension
						)
					);

					Application::DoEvents();
				}
			}

			for (int thresholdIndex = 0;
				thresholdIndex < steps;
				++thresholdIndex) {

				if (agpTimeForThreshold[thresholdIndex]->Count > 0) {

					ComputeMeanStd(
						agpTimeForThreshold[thresholdIndex],
						agpTimeMean[thresholdIndex],
						agpTimeStd[thresholdIndex]
					);

					ComputeMeanStd(
						agpFForThreshold[thresholdIndex],
						agpFMean[thresholdIndex],
						agpFStd[thresholdIndex]
					);
				}

				if (tracTimeForThreshold[thresholdIndex]->Count > 0) {

					ComputeMeanStd(
						tracTimeForThreshold[thresholdIndex],
						tracTimeMean[thresholdIndex],
						tracTimeStd[thresholdIndex]
					);

					ComputeMeanStd(
						tracFForThreshold[thresholdIndex],
						tracFMean[thresholdIndex],
						tracFStd[thresholdIndex]
					);
				}

				UpdateCharts(
					thresholds,
					steps,
					thresholdIndex,
					agpTimeMean,
					agpFMean,
					tracTimeMean,
					tracFMean
				);

				tbStats->AppendText(
					String::Format(
						L"Порог {0} обработан: AGP runs = {1}, TRAC-IK tasks = {2}\r\n",
						thresholds[thresholdIndex],
						agpTimeForThreshold[thresholdIndex]->Count,
						tracTimeForThreshold[thresholdIndex]->Count
					)
				);

				Application::DoEvents();
			}

			double agpTimeMu;
			double agpTimeSigma;
			double agpFMu;
			double agpFSigma;

			double tracTimeMu;
			double tracTimeSigma;
			double tracFMu;
			double tracFSigma;

			ComputeMeanStd(
				agpTimesAll,
				agpTimeMu,
				agpTimeSigma
			);

			ComputeMeanStd(
				agpFAll,
				agpFMu,
				agpFSigma
			);

			ComputeMeanStd(
				tracTimesAll,
				tracTimeMu,
				tracTimeSigma
			);

			ComputeMeanStd(
				tracFAll,
				tracFMu,
				tracFSigma
			);

			StringBuilder^ sb =
				gcnew StringBuilder();

			sb->AppendLine();

			sb->AppendLine(
				L"=============================================="
			);

			sb->AppendLine(
				L"СВОДНАЯ СТАТИСТИКА"
			);

			sb->AppendLine(
				L"=============================================="
			);

			sb->AppendLine(
				L"Размерности: 3..8"
			);

			sb->AppendLine(
				L"Каждая размерность: 10 задач, успешно решённых TRAC-IK (f <= 0.05)"
			);

			sb->AppendLine(
				L"Каждая задача: случайная цель + угол, без препятствий"
			);

			sb->AppendFormat(
				L"Stochastic-повторы AGP: {0} разных seed на задачу\r\n",
				repeatsStochastic
			);

			sb->AppendFormat(
				L"Порогов maxIter: {0}\r\n",
				steps
			);

			sb->AppendLine(
				L"TRAC-IK запускается один раз для принятой задачи"
			);

			sb->AppendLine();

			sb->AppendLine(
				L"Общие результаты:"
			);

			sb->AppendFormat(
				L"  AGP runs:     {0}\r\n",
				agpTimesAll->Count
			);

			sb->AppendFormat(
				L"  TRAC-IK runs: {0}\r\n",
				tracTimesAll->Count
			);

			sb->AppendLine();

			sb->AppendLine(
				L"Средние значения по всем фактическим запускам:"
			);

			sb->AppendFormat(
				L"  AGP:     время = {0:F3} ± {1:F3} мс, f = {2:F6} ± {3:F6}\r\n",
				agpTimeMu,
				agpTimeSigma,
				agpFMu,
				agpFSigma
			);

			sb->AppendFormat(
				L"  TRAC-IK: время = {0:F3} ± {1:F3} мс, f = {2:F6} ± {3:F6}\r\n",
				tracTimeMu,
				tracTimeSigma,
				tracFMu,
				tracFSigma
			);

			sb->AppendLine();

			sb->AppendLine(
				L"Количество задач:"
			);

			sb->AppendFormat(
				L"  Всего задач: {0}\r\n",
				totalGoodTasks
			);

			sb->AppendFormat(
				L"  AGP запусков на задачу: {0}\r\n",
				steps *
				repeatsStochastic
			);

			sb->AppendFormat(
				L"  Ожидаемое число AGP запусков: {0}\r\n",
				totalGoodTasks *
				steps *
				repeatsStochastic
			);

			sb->AppendLine();

			sb->AppendLine(
				L"Параметры:"
			);

			sb->AppendFormat(
				L"  maxTheta = {0:G9}\r\n",
				maxTheta
			);

			sb->AppendFormat(
				L"  r = {0:G9}\r\n",
				rParam
			);

			sb->AppendFormat(
				L"  eps = {0:G9}\r\n",
				epsParam
			);

			sb->AppendFormat(
				L"  baseLength = {0:G9}\r\n",
				baseLength
			);

			sb->AppendFormat(
				L"  stretchFactor = {0:G9}\r\n",
				stretchFactor
			);

			sb->AppendFormat(
				L"  adaptiveAgp = {0}\r\n",
				adaptiveAgp
			);

			sb->AppendFormat(
				L"  TRAC-IK timeout = {0:F3} мс\r\n",
				tracMaxTime *
				1000.0
			);

			tbStats->AppendText(
				sb->ToString()
			);
		}

		void UpdateCharts(
			array<int>^ thresholds,
			int steps,
			int filledUpTo,
			array<double>^ agpTimeMean,
			array<double>^ agpFMean,
			array<double>^ tracTimeMean,
			array<double>^ tracFMean)
		{
			Series^ sAgpTime =
				chartTime->Series["AGP"];

			Series^ sTracTime =
				chartTime->Series["TRAC IK"];

			Series^ sAgpF =
				chartF->Series["AGP"];

			Series^ sTracF =
				chartF->Series["TRAC IK"];

			sAgpTime->Points->Clear();
			sTracTime->Points->Clear();
			sAgpF->Points->Clear();
			sTracF->Points->Clear();

			for (int i = 0;
				i <= filledUpTo &&
				i < steps;
				++i) {

				const double x =
					static_cast<double>(
						thresholds[i]
					);

				if (!Double::IsNaN(
					agpTimeMean[i]
				) &&
					!Double::IsInfinity(
						agpTimeMean[i]
					)) {

					sAgpTime->Points->AddXY(
						x,
						agpTimeMean[i]
					);
				}

				if (!Double::IsNaN(
					tracTimeMean[i]
				) &&
					!Double::IsInfinity(
						tracTimeMean[i]
					)) {

					sTracTime->Points->AddXY(
						x,
						tracTimeMean[i]
					);
				}

				if (!Double::IsNaN(
					agpFMean[i]
				) &&
					!Double::IsInfinity(
						agpFMean[i]
					)) {

					sAgpF->Points->AddXY(
						x,
						agpFMean[i]
					);
				}

				if (!Double::IsNaN(
					tracFMean[i]
				) &&
					!Double::IsInfinity(
						tracFMean[i]
					)) {

					sTracF->Points->AddXY(
						x,
						tracFMean[i]
					);
				}
			}

			chartTime->Invalidate();
			chartF->Invalidate();

			this->Refresh();
		}
	};
}

Нужно добавить ещё один include, поскольку здесь используется std::numeric_limits:

cpp
#include <limits>

То есть в верхней части итогового файла рядом с <memory> обязательно должен быть:

cpp
#include <memory>
#include <limits>

Если после этой версии падение остаётся именно на _Al.deallocate(...) в vector.h, тогда причина почти наверняка уже не в приведённой логике benchmark. Такое место обычно означает, что куча была повреждена раньше. В вашем случае тогда в первую очередь нужно проверить, что AGP_Manip2D, AgpStartManipND и AGP_Free объявлены в DLL абсолютно с такими же типами параметров и calling convention, как эти typedef. Особенно критично на Win32/x86 различие __cdecl/__stdcall.

Также TRAC-IK, Orocos KDL и TEST_AGP.exe должны быть собраны совместимо: одинаковая архитектура x64/x86, совместимый CRT (/MD с /MD, Debug /MDd с Debug /MDd) и одинаковый _ITERATOR_DEBUG_LEVEL. Несовпадение Debug/Release STL между бинарниками очень характерно проявляется падением именно внутри std::vector::deallocate.