Some minor collision and raycast bug fixes.

erayzesen · Sep 3, 2024 · 164cf53 · 164cf53
1 parent 525ee8f
commit 164cf53
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 24 deletions.
diff --git a/QuarkPhysics/qcollision.cpp b/QuarkPhysics/qcollision.cpp
@@ -177,7 +177,7 @@ void QCollision::PolylineAndPolygon(vector<QParticle*> &polylineParticles, vecto
 
 }
 
-void QCollision::CircleAndPolyline(vector<QParticle *> &circleParticles, vector<QParticle *> &polylineParticles,QAABB polylineAABB,vector<QCollision::Contact*> &contacts)
+void QCollision::CircleAndPolyline(vector<QParticle *> &circleParticles, vector<QParticle *> &polylineParticles,QAABB polylineAABB,vector<QCollision::Contact*> &contacts,bool circlesArePolygon)
 {
 
 	/* The algorithm detects collisions between one or more particles and a polyline. 
@@ -214,11 +214,41 @@ void QCollision::CircleAndPolyline(vector<QParticle *> &circleParticles, vector<
 
 		int collidedSideIndex=-1;
 
-		//All values
+		//Checking whether the cirlce particle is in the polyline. 
+		bool collisionIsPolygonal=false;
+		if (circlesArePolygon){
+			if (circleParticles!=polylineParticles){
+				if (PointInPolygonWN(pA->GetGlobalPosition(),polylineParticles)){
+					collisionIsPolygonal=true;
+				}else{
+					//intersection tests between target particle sides and polyline
+					if(circleParticles.size()>3 && polylineParticles.size()>3 ){
+						QParticle *ppA=circleParticles[ (ia-1+circleParticles.size() )%circleParticles.size() ];
+						QParticle *npA=circleParticles[ (ia+1 )%circleParticles.size() ];
+						for(size_t j=0;j<polylineParticles.size();++j ){
+							QParticle * pJ=polylineParticles[j];
+							QParticle *npJ=polylineParticles[ (j+1)% polylineParticles.size() ];
+							bool sideIntersectionA=LineIntersectionLine(ppA->GetGlobalPosition(),pA->GetGlobalPosition(),pJ->GetGlobalPosition(), npJ->GetGlobalPosition() ).isNaN()==false;
+
+							if( sideIntersectionA){
+								bool sideIntersectionB=LineIntersectionLine(pA->GetGlobalPosition(),npA->GetGlobalPosition(),pJ->GetGlobalPosition(), npJ->GetGlobalPosition() ).isNaN()==false;
+								if (sideIntersectionB){
+									collisionIsPolygonal=true;
+								}
+
+							}
+						}
 
+					}
+				}
+		}
+
+
+		}
 
-		//Checking whether the cirlce particle is in the polyline. 
-		if (PointInPolygonWN(pA->GetGlobalPosition(),polylineParticles) && circleParticles!=polylineParticles) {
+
+
+		if (collisionIsPolygonal) {
 
 			//B. If the circular particle belongs to a collection with 3 or more elements, a ray vector is prepared towards the angle bisector using its edges.
 			nearestSides.clear();
@@ -244,25 +274,27 @@ void QCollision::CircleAndPolyline(vector<QParticle *> &circleParticles, vector<
 			nearestSides.push_back(vector<QParticle*>{ pB, polylineParticles[ (ni+1)%polylineParticles.size() ]}  );
 
 			//Nearest particle is on the outside of the test particle sides.
-			bool isNearesParticleOnWrongSide=true;
-
-			for (size_t j=0;j<nearestSides.size();++j ){
-				QVector sideVec=nearestSides[j][1]->GetGlobalPosition()-nearestSides[j][0]->GetGlobalPosition();
-				QVector sidePerp=sideVec.Perpendicular();
-				QVector bVector=pA->GetGlobalPosition()-nearestSides[j][0]->GetGlobalPosition();
-				if(sidePerp.Dot(rayUnit)>0 ){
-					isNearesParticleOnWrongSide=false;
-				}
-			}
-			if(isNearesParticleOnWrongSide==true  ){
-				nearestSides.clear();
-				for(size_t j=0;j<polylineParticles.size();++j ){
-					int nj=(j+1)%polylineParticles.size();
+			if (circlesArePolygon){
+				bool isNearesParticleOnWrongSide=true;
 
-					QVector sideVec=polylineParticles[nj]->GetGlobalPosition()-polylineParticles[j]->GetGlobalPosition();
+				for (size_t j=0;j<nearestSides.size();++j ){
+					QVector sideVec=nearestSides[j][1]->GetGlobalPosition()-nearestSides[j][0]->GetGlobalPosition();
 					QVector sidePerp=sideVec.Perpendicular();
+					QVector bVector=pA->GetGlobalPosition()-nearestSides[j][0]->GetGlobalPosition();
 					if(sidePerp.Dot(rayUnit)>0 ){
-						nearestSides.push_back(vector<QParticle*>{ polylineParticles[ j], polylineParticles[ nj]}  );			
+						isNearesParticleOnWrongSide=false;
+					}
+				}
+				if(isNearesParticleOnWrongSide==true  ){
+					nearestSides.clear();
+					for(size_t j=0;j<polylineParticles.size();++j ){
+						int nj=(j+1)%polylineParticles.size();
+
+						QVector sideVec=polylineParticles[nj]->GetGlobalPosition()-polylineParticles[j]->GetGlobalPosition();
+						QVector sidePerp=sideVec.Perpendicular();
+						if(sidePerp.Dot(rayUnit)>0 ){
+							nearestSides.push_back(vector<QParticle*>{ polylineParticles[ j], polylineParticles[ nj]}  );			
+						}
 					}
 				}
 			}

diff --git a/QuarkPhysics/qcollision.h b/QuarkPhysics/qcollision.h
@@ -152,9 +152,11 @@ class QCollision
 	/** Checks collisions between circles and polyline. 
 	 * @param circleParticles A collection of particles representing one or more circles, each having a radius.
 	 * @param polylineParticles A collection of particles that make up a polyline.
+	 * @param polylineAABB AABB of polyline for tests between circle and AABB.
 	 * @param contacts A collection where collision contact information will be stored.
+	 * @param circlesArePolygon It defines whether circle particles is the part of a polygon.  
 	 */
-	static void CircleAndPolyline(vector<QParticle*> &circleParticles,vector<QParticle*> &polylineParticles,QAABB polylineAABB , vector<QCollision::Contact*> &contacts);
+	static void CircleAndPolyline(vector<QParticle*> &circleParticles,vector<QParticle*> &polylineParticles,QAABB polylineAABB , vector<QCollision::Contact*> &contacts, bool circlesArePolygon=false);
 
 
 	//Geometry Helper Methods

diff --git a/QuarkPhysics/qraycast.cpp b/QuarkPhysics/qraycast.cpp
@@ -49,8 +49,13 @@ vector<QBody *> QRaycast::GetPotentialBodies(QWorld *whichWorld, QVector rayPosi
 
 	for(int i=0;i<whichWorld->GetBodyCount();i++){
 		QBody* body=whichWorld->GetBodyAt(i);
+
 		if(body->GetEnabled()==false )
 			continue;
+
+		if ( (collidableLayers & body->GetLayersBit()) ==0 )
+			continue;
+
 		if(isRayToNegativeX){
 			if(body->GetAABB().GetMin().x>rayPosition.x || body->GetAABB().GetMax().x<rayEndPosition.x){
 				continue;
@@ -70,6 +75,7 @@ vector<QBody *> QRaycast::GetPotentialBodies(QWorld *whichWorld, QVector rayPosi
 				continue;
 			}
 		}
+
 		res.push_back(body);
 	}
 	return res;
@@ -242,6 +248,7 @@ void QRaycast::RaycastToPolygon(QBody *body, QMesh *mesh, QVector rayPosition, Q
 		//It's a containing body
 		if(enableContainingBodies){
 			nearContactPosition=rayPosition;
+			nearDistance=0.0f;
 
 		}else{
 			return;

diff --git a/QuarkPhysics/qworld.cpp b/QuarkPhysics/qworld.cpp
@@ -908,9 +908,10 @@ vector<QCollision::Contact*> QWorld::GetCollisions(QBody *bodyA, QBody *bodyB){
 
 
 				if(bodyA->simulationModel==QBody::SimulationModels::MASS_SPRING && bodyB->simulationModel==QBody::SimulationModels::MASS_SPRING){
-					//QCollision::CircleAndCircle(meshA->polygon,meshB->polygon,bboxB, contactList);
-					QCollision::CircleAndPolyline(meshA->polygon,meshB->polygon,bboxB,contactList);
-					QCollision::CircleAndPolyline(meshB->polygon,meshA->polygon,bboxA, contactList);
+					QCollision::CircleAndCircle(meshA->polygon,meshB->polygon,bboxB, contactList);
+
+					QCollision::CircleAndPolyline(meshA->polygon,meshB->polygon,bboxB,contactList,true);
+					QCollision::CircleAndPolyline(meshB->polygon,meshA->polygon,bboxA, contactList,true);
 
 
 				}