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);

	auto it = cachedPointers.find(playerIndex);
	if (it == cachedPointers.end()) {
		printf("No cached pointers for player index %d\n", playerIndex);
		return Vector3();
	}

	const PlayerPointers& pointers = it->second;
	printf("Found cached pointers for playerIndex: %d\n", playerIndex);

	__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 = pointers.some_ptr;
	DMA::MemRead(pointers.FinalTransform + 0x40, &pTransformAccessReadOnly.index, sizeof(int));
	printf("pTransformAccessReadOnly.pTransformData: %llx\n", pTransformAccessReadOnly.pTransformData);
	printf("pTransformAccessReadOnly.index: %d\n", pTransformAccessReadOnly.index);

	TransformData transformData;
	transformData.pTransformArray = pointers.relation_array;
	transformData.pTransformIndices = pointers.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);
	SIZE_T sizeIndicesBuf = sizeof(int) * pTransformAccessReadOnly.index + sizeof(int);

	PVOID pMatriciesBuf = malloc(sizeMatriciesBuf);
	PVOID pIndicesBuf = malloc(sizeIndicesBuf);

	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);
		}

		result = *(__m128*)((ULONGLONG)pMatriciesBuf + 0x30 * pTransformAccessReadOnly.index);
		int transformIndex = *(int*)((ULONGLONG)pIndicesBuf + 0x4 * pTransformAccessReadOnly.index);

		while (transformIndex >= 0)
		{
			printf("Processing transform index: %d\n", transformIndex);
			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));

			transformIndex = *(int*)((ULONGLONG)pIndicesBuf + 0x4 * transformIndex);
		}

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

	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]);
}