From cd2da6d50fb5fd5e470e85597ff8a5fbc25f4750 Mon Sep 17 00:00:00 2001
From: wheremyfoodat <gponiris2004@gmail.com>
Date: Sun, 25 Sep 2022 18:41:32 +0300
Subject: [PATCH] [PICA] Implement dp3

---
 include/PICA/shader.hpp              |  2 ++
 src/core/PICA/shader_interpreter.cpp | 24 ++++++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/include/PICA/shader.hpp b/include/PICA/shader.hpp
index 195733e0..18df2fc5 100644
--- a/include/PICA/shader.hpp
+++ b/include/PICA/shader.hpp
@@ -13,6 +13,7 @@ enum class ShaderType {
 namespace ShaderOpcodes {
 	enum : u32 {
 		ADD = 0x00,
+		DP3 = 0x01,
 		DP4 = 0x02,
 		MUL = 0x08,
 		MOVA = 0x12,
@@ -56,6 +57,7 @@ class PICAShader {
 
 	// Shader opcodes
 	void add(u32 instruction);
+	void dp3(u32 instruction);
 	void dp4(u32 instruction);
 	void loop(u32 instruction);
 	void mov(u32 instruction);
diff --git a/src/core/PICA/shader_interpreter.cpp b/src/core/PICA/shader_interpreter.cpp
index af75f47e..1a67e094 100644
--- a/src/core/PICA/shader_interpreter.cpp
+++ b/src/core/PICA/shader_interpreter.cpp
@@ -2,6 +2,7 @@
 
 void PICAShader::run() {
 	pc = 0;
+	loopIndex = 0;
 
 	while (true) {
 		const u32 instruction = loadedShader[pc++];
@@ -9,6 +10,7 @@ void PICAShader::run() {
 
 		switch (opcode) {
 			case ShaderOpcodes::ADD: add(instruction); break;
+			case ShaderOpcodes::DP3: dp3(instruction); break;
 			case ShaderOpcodes::DP4: dp4(instruction); break;
 			case ShaderOpcodes::END: return; // Stop running shader
 			case ShaderOpcodes::LOOP: loop(instruction); break;
@@ -147,6 +149,28 @@ void PICAShader::mova(u32 instruction) {
 		addrRegister.y() = static_cast<s32>(srcVector.y().toFloat32());
 }
 
+void PICAShader::dp3(u32 instruction) {
+	const u32 operandDescriptor = operandDescriptors[instruction & 0x7f];
+	const u32 src1 = (instruction >> 12) & 0x7f;
+	const u32 src2 = (instruction >> 7) & 0x1f; // src2 coming first because PICA moment
+	const u32 idx = (instruction >> 19) & 3;
+	const u32 dest = (instruction >> 21) & 0x1f;
+
+	if (idx) Helpers::panic("[PICA] DP3: idx != 0");
+	vec4f srcVec1 = getSourceSwizzled<1>(src1, operandDescriptor);
+	vec4f srcVec2 = getSourceSwizzled<2>(src2, operandDescriptor);
+
+	vec4f& destVector = getDest(dest);
+	f24 dot = srcVec1[0] * srcVec2[0] + srcVec1[1] * srcVec2[1] + srcVec1[2] * srcVec2[2];
+
+	u32 componentMask = operandDescriptor & 0xf;
+	for (int i = 0; i < 4; i++) {
+		if (componentMask & (1 << i)) {
+			destVector[3 - i] = dot;
+		}
+	}
+}
+
 void PICAShader::dp4(u32 instruction) {
 	const u32 operandDescriptor = operandDescriptors[instruction & 0x7f];
 	const u32 src1 = (instruction >> 12) & 0x7f;