From abb1f76c716f0ed317d26a209e937f0747cdda45 Mon Sep 17 00:00:00 2001
From: NT_x86 <sumoner200@gmail.com>
Date: Tue, 28 Mar 2023 21:05:08 +0300
Subject: [PATCH] Joint solver basic implementation (axis aligned) Does not
 handle rotation at all. solveWorkspaceSnaps is called in the debug game
 creation (Application.cpp) Added isA to instance class

---
 src/include/DataModelV2/Instance.h       |  1 +
 src/include/DataModelV2/JointsService.h  |  2 +
 src/source/Application.cpp               |  5 ++-
 src/source/DataModelV2/Instance.cpp      |  7 +++
 src/source/DataModelV2/JointsService.cpp | 54 ++++++++++++++++++++++++
 5 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/src/include/DataModelV2/Instance.h b/src/include/DataModelV2/Instance.h
index e520204..8d70823 100644
--- a/src/include/DataModelV2/Instance.h
+++ b/src/include/DataModelV2/Instance.h
@@ -16,6 +16,7 @@ public:
 	virtual void update();
 	std::vector<Instance*> children; // All children.
 	std::string getClassName();
+	bool Instance::isA(std::string name);
 	Instance* findFirstChild(std::string);
 	std::vector<Instance* > getChildren();
 	std::vector<Instance* > getAllChildren();
diff --git a/src/include/DataModelV2/JointsService.h b/src/include/DataModelV2/JointsService.h
index 9957bf8..a94137c 100644
--- a/src/include/DataModelV2/JointsService.h
+++ b/src/include/DataModelV2/JointsService.h
@@ -10,4 +10,6 @@ public:
 	~JointsService(void);
 	void createSnap(PartInstance* Part1, PartInstance* Part2);
 	void destroyPartSnap(PartInstance* Part);
+	void JointsService::solvePartSnap(PartInstance* Part, PartInstance* Part2);
+	void JointsService::solveWorkspaceSnaps();
 };
diff --git a/src/source/Application.cpp b/src/source/Application.cpp
index 57e6824..0daacdc 100644
--- a/src/source/Application.cpp
+++ b/src/source/Application.cpp
@@ -210,7 +210,7 @@ void Application::onInit()  {
 	test->setParent(_dataModel->getWorkspace());
 	test->color = Color3(0.2F,0.3F,1);
 	test->setSize(Vector3(24,1,24));
-	test->setPosition(Vector3(0,0,0));
+	test->setPosition(Vector3(0,-10,0));
 	test->setCFrame(test->getCFrame() * Matrix3::fromEulerAnglesXYZ(0,toRadians(0),toRadians(0)));
 	test->setSurface(TOP, Enum::SurfaceType::Bumps);
 	test->setAnchored(true);
@@ -266,6 +266,7 @@ void Application::onInit()  {
 	test->setPosition(Vector3(1,4,0));
 	test->setSurface(TOP, Enum::SurfaceType::Bumps);
 	JointsSvc->createSnap(test, test3);
+	//JointsSvc->solvePartSnap(test3, test);
 
 	test = makePart();
 	test->setParent(_dataModel->getWorkspace());
@@ -294,6 +295,8 @@ void Application::onInit()  {
 	test->setSize(Vector3(4,1,2));
 	test->setPosition(Vector3(2,7,0));
 	test->setSurface(TOP, Enum::SurfaceType::Bumps);
+
+	JointsSvc->solveWorkspaceSnaps();
 #else
 	_dataModel->debugGetOpen();
 #endif
diff --git a/src/source/DataModelV2/Instance.cpp b/src/source/DataModelV2/Instance.cpp
index 4b9e4ad..4d94434 100644
--- a/src/source/DataModelV2/Instance.cpp
+++ b/src/source/DataModelV2/Instance.cpp
@@ -100,6 +100,13 @@ std::string Instance::getClassName()
 	return className;
 }
 
+bool Instance::isA(std::string name)
+{
+	if (className == name)
+		return true;
+	return false;
+}
+
 std::vector<Instance* > Instance::getChildren()
 {
 	return children;
diff --git a/src/source/DataModelV2/JointsService.cpp b/src/source/DataModelV2/JointsService.cpp
index 1c87544..554a3d6 100644
--- a/src/source/DataModelV2/JointsService.cpp
+++ b/src/source/DataModelV2/JointsService.cpp
@@ -31,4 +31,58 @@ void JointsService::destroyPartSnap(PartInstance* Part)
 			Snap->remove();
 		}
 	}
+}
+
+void JointsService::solvePartSnap(PartInstance* Part, PartInstance* Part2)
+{
+	float MoE = 0.05f;
+	float TopSurface = (Part->getPosition().y + Part->getSize().y / 2);
+	float RightRECT = (Part->getPosition().x + Part->getSize().x / 2) - MoE;
+	float LeftRECT = (Part->getPosition().x - Part->getSize().x / 2) + MoE;
+	float TopRECT = (Part->getPosition().z - Part->getSize().z / 2) + MoE;
+	float BottomRECT = (Part->getPosition().z + Part->getSize().z / 2) - MoE;
+
+	float BottomSurface2 = (Part2->getPosition().y - Part2->getSize().y / 2);
+	float RightRECT2 = (Part2->getPosition().x + Part2->getSize().x / 2);
+	float LeftRECT2 = (Part2->getPosition().x - Part2->getSize().x / 2);
+	float TopRECT2 = (Part2->getPosition().z - Part2->getSize().z / 2);
+	float BottomRECT2 = (Part2->getPosition().z + Part2->getSize().z / 2);
+
+	std::cout << "TopSurface: " << TopSurface << std::endl;
+	std::cout << "BottomSurface2: " << BottomSurface2 << std::endl;
+	if (TopSurface == BottomSurface2)
+	{
+		std::cout << "Same Y cordinate!!!" << std::endl;
+		//https://gamedev.stackexchange.com/questions/586/what-is-the-fastest-way-to-work-out-2d-bounding-box-intersection
+		bool Intersect = !(LeftRECT2 > RightRECT
+        || RightRECT2 < LeftRECT
+        || TopRECT2 > BottomRECT
+        || BottomRECT2 < TopRECT);
+		if (Intersect){
+			std::cout << "Intersects!!!" << std::endl;
+			createSnap(Part, Part2);
+		}else{
+			std::cout << "Does not Intersect!!!" << std::endl;
+		}
+	}else{
+		std::cout << "Y cordinate does not match!!!" << std::endl;
+	}
+}
+
+void JointsService::solveWorkspaceSnaps()
+{
+	std::vector<Instance* > children = g_dataModel->getWorkspace()->getAllChildren();
+	for(size_t i = 0; i < children.size(); i++)
+	{
+		PartInstance* Part1 = (PartInstance*)children.at(i);
+		if (Part1->isA("Part"))
+		{
+			for(size_t c = 0; c < children.size(); c++) //horrible
+			{
+				PartInstance* Part2 = (PartInstance*)children.at(c);
+				if (Part1 != Part2)
+					solvePartSnap(Part1, Part2);
+			}
+		}
+	}
 }
\ No newline at end of file