From 998a22a43f9f15488055e0966f981cb16fc8b9db Mon Sep 17 00:00:00 2001
From: NT_x86 <sumoner200@gmail.com>
Date: Wed, 29 Mar 2023 21:00:15 +0300
Subject: [PATCH] Improved snap solver Uses ODE collision detection instead

---
 src/source/Application.cpp               |  4 +--
 src/source/DataModelV2/JointsService.cpp | 39 ++++++------------------
 src/source/XplicitNgine/XplicitNgine.cpp |  4 +--
 3 files changed, 14 insertions(+), 33 deletions(-)

diff --git a/src/source/Application.cpp b/src/source/Application.cpp
index 0daacdc..8fdb0b5 100644
--- a/src/source/Application.cpp
+++ b/src/source/Application.cpp
@@ -249,7 +249,7 @@ void Application::onInit()  {
 	test3->setSize(Vector3(4,1,2));
 	test3->setPosition(Vector3(4,3,0));
 	test3->setSurface(TOP, Enum::SurfaceType::Bumps);
-	JointsSvc->createSnap(test2, test3);
+	//JointsSvc->createSnap(test2, test3);
 
 
 	test = makePart();
@@ -265,7 +265,7 @@ void Application::onInit()  {
 	test->setSize(Vector3(4,1,2));
 	test->setPosition(Vector3(1,4,0));
 	test->setSurface(TOP, Enum::SurfaceType::Bumps);
-	JointsSvc->createSnap(test, test3);
+	//JointsSvc->createSnap(test, test3);
 	//JointsSvc->solvePartSnap(test3, test);
 
 	test = makePart();
diff --git a/src/source/DataModelV2/JointsService.cpp b/src/source/DataModelV2/JointsService.cpp
index 554a3d6..af84b3e 100644
--- a/src/source/DataModelV2/JointsService.cpp
+++ b/src/source/DataModelV2/JointsService.cpp
@@ -33,39 +33,20 @@ void JointsService::destroyPartSnap(PartInstance* Part)
 	}
 }
 
-void JointsService::solvePartSnap(PartInstance* Part, PartInstance* Part2)
+void JointsService::solvePartSnap(PartInstance* Part1, 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;
-
+	XplicitNgine* Phys = g_dataModel->getEngine();
+	float TopSurface = (Part1->getPosition().y + Part1->getSize().y / 2);
 	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;
+	const int N = 4;
 	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;
+		Phys->createBody(Part1);
+		Phys->createBody(Part2);
+		dContact contact[N];
+		int n = dCollide (Part1->physGeom[0],Part2->physGeom[0],N,&contact[0].geom,sizeof(dContact));
+		if (n > 0)
+			createSnap(Part1, Part2);
 	}
 }
 
diff --git a/src/source/XplicitNgine/XplicitNgine.cpp b/src/source/XplicitNgine/XplicitNgine.cpp
index dcfd8d8..0f91644 100644
--- a/src/source/XplicitNgine/XplicitNgine.cpp
+++ b/src/source/XplicitNgine/XplicitNgine.cpp
@@ -4,7 +4,6 @@
 
 XplicitNgine::XplicitNgine() 
 {
-	
 	physWorld = dWorldCreate();
 	physSpace = dHashSpaceCreate(0);
 	contactgroup = dJointGroupCreate(0);
@@ -159,7 +158,8 @@ void XplicitNgine::createBody(PartInstance* partInstance)
 			dGeomSetData(partInstance->physGeom[0], partInstance);
 
 		dMass mass;
-		mass.setBox(sqrt(partSize.x*2), sqrt(partSize.y*2), sqrt(partSize.z*2), 0.7F);
+		mass.setBox(sqrt(partSize.x*20), sqrt(partSize.y*20), sqrt(partSize.z*20), 0.01F);
+		//mass.setBox(sqrt(partSize.x*2), sqrt(partSize.y*2), sqrt(partSize.z*2), 0.7F);
 		dBodySetMass(partInstance->physBody, &mass);
 
 		// Create rigid body