Untitled

struct TransformAccessReadOnly
{
	ULONGLONG	pTransformData;
	int			index;
};

struct TransformData
{
	ULONGLONG pTransformArray;
	ULONGLONG pTransformIndices;
};

struct Matrix34
{
	Vector4 vec0;
	Vector4 vec1;
	Vector4 vec2;
};

Vector3 GetPosition(int playerIndex) {
	printf("Entering GetPosition with playerIndex: %d\n", playerIndex);
	//start of profiling1
	auto start1 = std::chrono::high_resolution_clock::now();

	// Read bodyPtr
	DWORD_PTR bodyPtr = DMA::MemReadPtr<DWORD_PTR>(sharedData.playerPtrs[playerIndex] + 0xE0);
	printf("bodyPtr = %p\n", bodyPtr);

	// Read bonePtr
	DWORD_PTR bonePtr = DMA::MemReadPtr<DWORD_PTR>(bodyPtr + 0x58);
	printf("bonePtr = %p\n", bonePtr);

	// Read temp
	DWORD_PTR temp = DMA::MemReadPtr<DWORD_PTR>(bonePtr + 0x10);
	printf("temp = %p\n", temp);

	// Read object1
	DWORD_PTR object1 = DMA::MemReadPtr<DWORD_PTR>(temp + 0x30);
	printf("object1 = %p\n", object1);

	// Read object2
	DWORD_PTR object2 = DMA::MemReadPtr<DWORD_PTR>(object1 + 0x30);
	printf("object2 = %p\n", object2);

	// Read transform
	DWORD_PTR transform = DMA::MemReadPtr<DWORD_PTR>(object2 + 0x8);
	printf("transform = %p\n", transform);

	// Read transform2
	DWORD_PTR transform2 = DMA::MemReadPtr<DWORD_PTR>(transform + 0x28);
	printf("transform2 = %p\n", transform2);

	// Read FinalTransform
	DWORD_PTR FinalTransform = DMA::MemReadPtr<DWORD_PTR>(transform2 + 0x10);
	printf("FinalTransform = %p\n", FinalTransform);

	// Read some_ptr
	DWORD_PTR some_ptr = DMA::MemReadPtr<DWORD_PTR>(FinalTransform + 0x38);
	printf("some_ptr = %p\n", some_ptr);

	// Read relation_array
	DWORD_PTR relation_array = DMA::MemReadPtr<DWORD_PTR>(some_ptr + 0x18);
	printf("relation_array = %p\n", relation_array);

	// Read dependency_index_array
	DWORD_PTR dependency_index_array = DMA::MemReadPtr<DWORD_PTR>(some_ptr + 0x20);
	printf("dependency_index_array = %p\n", dependency_index_array);

	__m128 result;

	const __m128 mulVec0 = { -2.000, 2.000, -2.000, 0.000 };
	const __m128 mulVec1 = { 2.000, -2.000, -2.000, 0.000 };
	const __m128 mulVec2 = { -2.000, -2.000, 2.000, 0.000 };

	TransformAccessReadOnly pTransformAccessReadOnly;
	pTransformAccessReadOnly.pTransformData = some_ptr;
	DMA::MemRead(FinalTransform + 0x40, &pTransformAccessReadOnly.index, sizeof(int));
	printf("pTransformAccessReadOnly.pTransformData: %llx\n", pTransformAccessReadOnly.pTransformData);
	printf("pTransformAccessReadOnly.index: %d\n", pTransformAccessReadOnly.index);

	TransformData transformData;
	transformData.pTransformArray = relation_array;
	transformData.pTransformIndices = dependency_index_array;
	printf("transformData.pTransformArray: %llx\n", transformData.pTransformArray);
	printf("transformData.pTransformIndices: %llx\n", transformData.pTransformIndices);

	SIZE_T sizeMatriciesBuf = sizeof(Matrix34) * pTransformAccessReadOnly.index + sizeof(Matrix34);
	printf("Size of MatriciesBuf: %d\n", sizeMatriciesBuf);
	SIZE_T sizeIndicesBuf = sizeof(int) * pTransformAccessReadOnly.index + sizeof(int);
	printf("Size of IndicesBuf: %d\n", sizeIndicesBuf);

	PVOID pMatriciesBuf = malloc(sizeMatriciesBuf);
	printf("pMatriciesBuf immediately after malloc: %p\n", pMatriciesBuf);
	PVOID pIndicesBuf = malloc(sizeIndicesBuf);
	printf("pIndicesBuf immediately after malloc: %p\n", pIndicesBuf);

	//end of profiling1
	auto stop1 = std::chrono::high_resolution_clock::now();
	auto duration1 = std::chrono::duration_cast<std::chrono::microseconds>(stop1 - start1);
	boneOneProfiling = duration1.count();

	//start of profiling2
	auto start2 = std::chrono::high_resolution_clock::now();

	if (pMatriciesBuf && pIndicesBuf)
	{
		printf("Memory allocation for buffers successful\n");

		DMA::MemRead(transformData.pTransformArray, pMatriciesBuf, sizeMatriciesBuf);
		printf("Matrices read into buffer successfully\n");
		for (int i = 0; i < 10; i++)
		{
			Matrix34 matrix34 = *(Matrix34*)((ULONGLONG)pMatriciesBuf + 0x30 * i);
			printf("Matrix34 %d: %f %f %f %f\n", i, matrix34.vec0.x, matrix34.vec0.y, matrix34.vec0.z, matrix34.vec0.w);
			printf("Matrix34 %d: %f %f %f %f\n", i, matrix34.vec1.x, matrix34.vec1.y, matrix34.vec1.z, matrix34.vec1.w);
			printf("Matrix34 %d: %f %f %f %f\n", i, matrix34.vec2.x, matrix34.vec2.y, matrix34.vec2.z, matrix34.vec2.w);
		}

		DMA::MemRead(transformData.pTransformIndices, pIndicesBuf, sizeIndicesBuf);
		printf("Indices read into buffer successfully\n");
		for (int i = 0; i < 10; i++)
		{
			int index = *(int*)((ULONGLONG)pIndicesBuf + 0x4 * i);
			printf("Index %d: %d\n", i, index);
		}

		PVOID pIndicesBufEnd = (char*)pIndicesBuf + sizeIndicesBuf;

		result = *(__m128*)((ULONGLONG)pMatriciesBuf + 0x30 * pTransformAccessReadOnly.index);
	

		// Printing pTransformAccessReadOnly.index
		printf("pTransformAccessReadOnly.index: %d\n", pTransformAccessReadOnly.index);

		// Printing the multiplication result
		unsigned int multiplicationResult = 0x4 * pTransformAccessReadOnly.index;
		printf("Multiplication result (0x4 * index): %u\n", multiplicationResult);

		// Printing the sum
		ULONGLONG sumResult = (ULONGLONG)pIndicesBuf + multiplicationResult;
		printf("Sum result (pIndicesBuf + multiplication result): %p\n", (void*)sumResult);

		if (sumResult + sizeof(int) > (ULONGLONG)pIndicesBufEnd) {
			printf("Error: Attempt to read outside of pIndicesBuf bounds!\n");
			//free(pMatriciesBuf);
			//free(pIndicesBuf);
			//return Vector3();
		}

		// Calculating transformIndex and printing it
		int transformIndex = *(int*)sumResult;
		printf("Transform Index: %d\n", transformIndex);

		const int maxMatrices = sizeMatriciesBuf / sizeof(Matrix34);
		if (transformIndex >= maxMatrices) {
			printf("Error: transformIndex too large!\n");
			//free(pMatriciesBuf);
			//free(pIndicesBuf);
			//return Vector3();
		}

		int pSafe = 0;
		while (transformIndex >= 0 && pSafe++ < 200)
		{
			printf("Processing transform index: %d\n", transformIndex);

			ULONGLONG calculatedAddress = (ULONGLONG)pMatriciesBuf + 0x30 * transformIndex;
			printf("Calculated address: %p\n", (void*)calculatedAddress);
			printf("Size of Matrix34: %zu\n", sizeof(Matrix34));
			ULONGLONG pMatriciesBufEnd = (ULONGLONG)pMatriciesBuf + sizeMatriciesBuf; 

			if (calculatedAddress < (ULONGLONG)pMatriciesBuf || calculatedAddress + sizeof(Matrix34) > pMatriciesBufEnd) {
				printf("Error: Attempt to read outside of pMatriciesBuf bounds!\n");
				//free(pMatriciesBuf);
				//free(pIndicesBuf);
				//return Vector3();
			}

			Matrix34 matrix34 = *(Matrix34*)((ULONGLONG)pMatriciesBuf + 0x30 * transformIndex);

			__m128 xxxx = _mm_castsi128_ps(_mm_shuffle_epi32(*(__m128i*)(&matrix34.vec1), 0x00));	// xxxx
			__m128 yyyy = _mm_castsi128_ps(_mm_shuffle_epi32(*(__m128i*)(&matrix34.vec1), 0x55));	// yyyy
			__m128 zwxy = _mm_castsi128_ps(_mm_shuffle_epi32(*(__m128i*)(&matrix34.vec1), 0x8E));	// zwxy
			__m128 wzyw = _mm_castsi128_ps(_mm_shuffle_epi32(*(__m128i*)(&matrix34.vec1), 0xDB));	// wzyw
			__m128 zzzz = _mm_castsi128_ps(_mm_shuffle_epi32(*(__m128i*)(&matrix34.vec1), 0xAA));	// zzzz
			__m128 yxwy = _mm_castsi128_ps(_mm_shuffle_epi32(*(__m128i*)(&matrix34.vec1), 0x71));	// yxwy
			__m128 tmp7 = _mm_mul_ps(*(__m128*)(&matrix34.vec2), result);

			result = _mm_add_ps(
				_mm_add_ps(
					_mm_add_ps(
						_mm_mul_ps(
							_mm_sub_ps(
								_mm_mul_ps(_mm_mul_ps(xxxx, mulVec1), zwxy),
								_mm_mul_ps(_mm_mul_ps(yyyy, mulVec2), wzyw)),
							_mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(tmp7), 0xAA))),
						_mm_mul_ps(
							_mm_sub_ps(
								_mm_mul_ps(_mm_mul_ps(zzzz, mulVec2), wzyw),
								_mm_mul_ps(_mm_mul_ps(xxxx, mulVec0), yxwy)),
							_mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(tmp7), 0x55)))),
					_mm_add_ps(
						_mm_mul_ps(
							_mm_sub_ps(
								_mm_mul_ps(_mm_mul_ps(yyyy, mulVec0), yxwy),
								_mm_mul_ps(_mm_mul_ps(zzzz, mulVec1), zwxy)),
							_mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(tmp7), 0x00))),
						tmp7)), *(__m128*)(&matrix34.vec0));

			// Printing pIndicesBuf
			printf("pIndicesBuf: %p\n", pIndicesBuf);

			// Printing the current value of transformIndex
			printf("Current transformIndex: %d\n", transformIndex);

			// Printing the multiplication result
			unsigned int multiplicationResult2 = 0x4 * transformIndex;
			printf("Multiplication result (0x4 * transformIndex): %u\n", multiplicationResult2);

			// Printing the sum
			ULONGLONG sumResult2 = (ULONGLONG)pIndicesBuf + multiplicationResult2;
			printf("Sum result (pIndicesBuf + multiplication result): %p\n", (void*)sumResult2);

			if (sumResult2 + sizeof(int) > (ULONGLONG)pIndicesBufEnd) {
				printf("Error in loop: Attempt to read outside of pIndicesBuf bounds!\n");
				//free(pMatriciesBuf);
				//free(pIndicesBuf);
				//return Vector3();
			}

			// Calculating the new value of transformIndex and printing it
			transformIndex = *(int*)sumResult2;
			printf("New Transform Index: %d\n", transformIndex);
		}

		free(pMatriciesBuf);
		free(pIndicesBuf);
		printf("Memory for buffers freed\n");
	}
	else {
		printf("Memory allocation for buffers failed\n");
	}

	//end of profiling2
	auto stop2 = std::chrono::high_resolution_clock::now();
	auto duration2 = std::chrono::duration_cast<std::chrono::microseconds>(stop2 - start2);
	boneTwoProfiling = duration2.count();

	printf("X: %f, Y: %f, Z: %f\n", result.m128_f32[0], result.m128_f32[1], result.m128_f32[2]);

	return Vector3(result.m128_f32[0], result.m128_f32[1], result.m128_f32[2]);
}