some more fixes in the raycast/mouse over

9 files changed, 189 insertions, 187 deletions

diff --git a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
index c7f226e0883..5857c614e2d 100644
--- a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
@@ -48,13 +48,8 @@ void	KX_BulletPhysicsController::RelativeTranslate(const MT_Vector3& dloc,bool l
 
 void	KX_BulletPhysicsController::RelativeRotate(const MT_Matrix3x3& drot,bool local)
 {
-	printf("he1\n");
 	float	rotval[12];
 	drot.getValue(rotval);
-
-
-	
-	printf("hi\n");
 	CcdPhysicsController::RelativeRotate(rotval,local);
 }
 
diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObject.h b/source/gameengine/Ketsji/KX_ConvertPhysicsObject.h
index cf231adfd2a..e38ff77f76e 100644
--- a/source/gameengine/Ketsji/KX_ConvertPhysicsObject.h
+++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObject.h
@@ -33,7 +33,7 @@
 #define KX_CONVERTPHYSICSOBJECTS
 
 /* These are defined by the build system... */
-//#define USE_SUMO_SOLID
+#define USE_SUMO_SOLID
 //#define USE_ODE
 
 //on visual studio 7/8, always enable BULLET for now 
diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
index 53fd0db08d0..cedd689e85f 100644
--- a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
+++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
@@ -1017,7 +1017,8 @@ void	KX_ConvertBulletObject(	class	KX_GameObject* gameobj,
 	bool isActor = objprop->m_isactor;
 	gameobj->getClientInfo()->m_type = (isActor ? KX_ClientObjectInfo::ACTOR : KX_ClientObjectInfo::STATIC);
 	// store materialname in auxinfo, needed for touchsensors
-	gameobj->getClientInfo()->m_auxilary_info = 0;//(matname.Length() ? (void*)(matname.ReadPtr()+2) : NULL);
+	//gameobj->getClientInfo()->m_auxilary_info = 0;//(matname.Length() ? (void*)(matname.ReadPtr()+2) : NULL);
+	//gameobj->getClientInfo()->m_auxilary_info = (matname.Length() ? (void*)(matname.ReadPtr()+2) : NULL);
 
 
 	gameobj->GetSGNode()->AddSGController(physicscontroller);
diff --git a/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp b/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp
index 107e8ebc185..0d383376d87 100644
--- a/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp
+++ b/source/gameengine/Ketsji/KX_MouseFocusSensor.cpp
@@ -275,10 +275,9 @@ bool KX_MouseFocusSensor::ParentObjectHasFocus(void)
 	bool result = false;
 
 	result = KX_RayCast::RayTest(physics_controller, physics_environment, frompoint3, topoint3, resultpoint, resultnormal, KX_RayCast::Callback<KX_MouseFocusSensor>(this));
-	if (result)
-	{
-		
-	}
+	
+	result = (m_hitObject!=0);
+
 	return result;
 
 }
diff --git a/source/gameengine/Ketsji/KX_RaySensor.cpp b/source/gameengine/Ketsji/KX_RaySensor.cpp
index b35d9b00540..01d49cef777 100644
--- a/source/gameengine/Ketsji/KX_RaySensor.cpp
+++ b/source/gameengine/Ketsji/KX_RaySensor.cpp
@@ -41,16 +41,15 @@
 #include "KX_GameObject.h"
 #include "KX_Scene.h"
 
-#include "KX_RayCast.h"
 #include "PHY_IPhysicsEnvironment.h"
-#include "PHY_IPhysicsController.h"
 #include "KX_IPhysicsController.h"
-
+#include "PHY_IPhysicsController.h"
 
 #ifdef HAVE_CONFIG_H
 #include <config.h>
 #endif
 
+
 KX_RaySensor::KX_RaySensor(class SCA_EventManager* eventmgr,
 					SCA_IObject* gameobj,
 					const STR_String& propname,
@@ -104,49 +103,7 @@ bool KX_RaySensor::IsPositiveTrigger()
 	return result;
 }
 
-bool KX_RaySensor::RayHit(KX_ClientObjectInfo* info, MT_Point3& hit_point, MT_Vector3& hit_normal, void* const data)
-{
 
-	KX_GameObject* obj = (KX_GameObject*)GetParent();
-	SCA_IObject *hitgameobj = info->m_gameobject;
-	if (hitgameobj == obj || info->m_type > KX_ClientObjectInfo::ACTOR)
-	{
-//		printf("false hit\n");
-		// false hit
-		return false;
-	}
-	
-	bool bFound = false;
-	if (m_propertyname.Length() == 0)
-	{
-		bFound = true;
-	}
-	else
-	{
-		if (m_bFindMaterial)
-		{
-			if (info->m_auxilary_info)
-			{
-				bFound = (m_propertyname== ((char*)info->m_auxilary_info));
-			}
-		}
-		else
-		{
-			bFound = hitgameobj->GetProperty(m_propertyname) != NULL;
-		}
-	}
-
-	if (bFound)
-	{
-		m_rayHit = true;
-		m_hitObject = hitgameobj;
-		m_hitPosition = hit_point;
-		m_hitNormal = hit_normal;
-			
-	}
-	
-	return true;
-}
 
 bool KX_RaySensor::Evaluate(CValue* event)
 {
@@ -213,64 +170,122 @@ bool KX_RaySensor::Evaluate(CValue* event)
 	MT_Point3 topoint = frompoint + (m_distance) * todir;
 	MT_Point3 resultpoint;
 	MT_Vector3 resultnormal;
-	PHY_IPhysicsEnvironment* physics_environment = m_scene->GetPhysicsEnvironment();
-	if (!physics_environment)
+	bool ready = false;
+	PHY_IPhysicsEnvironment* pe = m_scene->GetPhysicsEnvironment();
+
+	if (!pe)
 	{
 		std::cout << "WARNING: Ray sensor " << GetName() << ":  There is no physics environment!" << std::endl;
 		std::cout << "         Check universe for malfunction." << std::endl;
 		return false;
 	} 
-	KX_IPhysicsController* physics_controller = obj->GetPhysicsController();
 
-	// Use the parent's physics controller if obj has no physics controller.
+	KX_IPhysicsController *spc = obj->GetPhysicsController();
 	KX_GameObject *parent = obj->GetParent();
-	if (!physics_controller && parent)
-		physics_controller = parent->GetPhysicsController();
-
+	if (!spc && parent)
+		spc = parent->GetPhysicsController();
+	
 	if (parent)
 		parent->Release();
-		
-	KX_RayCast::RayTest(physics_controller, physics_environment, frompoint, topoint, resultpoint, resultnormal, KX_RayCast::Callback<KX_RaySensor>(this));
-
-// 	do {
-// 		PHY__Vector3 respos;
-// 		PHY__Vector3 resnormal;
-// 
-// 		PHY_IPhysicsController* hitCtrl = spe->rayTest(physCtrl,
-// 			frompoint.x(),frompoint.y(),frompoint.z(),
-// 			topoint.x(),topoint.y(),topoint.z(),
-// 			respos[0],respos[1],respos[2],
-// 			resnormal[0],resnormal[1],resnormal[2]);
-// 			
-// 		if (hitCtrl)
-// 		{
-// 
-// 			resultpoint = MT_Vector3(respos);
-// 			resultnormal = MT_Vector3(resnormal);
-// 			KX_ClientObjectInfo* info = static_cast<KX_ClientObjectInfo*>(hitCtrl->getNewClientInfo());
-// 			bool bFound = false;
-// 			
-// 			if (!info)
-// 			{
-// 				std::cout<< "WARNING:  Ray sensor " << GetName() << " cannot sense PHY_IPhysicsController  - no client info.\n" << std::endl;
-// 				ready = true;
-// 				break;
-// 			} 
-// 			
-// 			
-// 
-// 		}
-// 		else
-// 		{
-// 			ready = true;
-// 		}
-// 	}
-// 	while (!ready);
-// 	
+	
+	do {
+		PHY_IPhysicsController* physCtrl = 0;
+//			frompoint,
+//			topoint,
+//			resultpoint,
+//			resultnormal);
+			
+		if (physCtrl)
+		{
+	
+			KX_ClientObjectInfo* info = static_cast<KX_ClientObjectInfo*>(physCtrl->getNewClientInfo());
+			bool bFound = false;
+			
+			if (!info)
+			{
+				std::cout<< "WARNING:  Ray sensor ";// << GetName() << " cannot sense SM_Object " << hitObj << " - no client info.\n" << std::endl;
+				ready = true;
+				break;
+			} 
+			
+			SCA_IObject *hitgameobj = info->m_gameobject;
+			
+			if (hitgameobj == obj || info->m_type > KX_ClientObjectInfo::ACTOR)
+			{
+#ifdef NOPE
+				/*
+				// false hit
+				KX_IPhysicsController *hitspc =  (static_cast<KX_GameObject*> (hitgameobj))->GetPhysicsController();
+
+				if (hitspc)
+				{
+					/* We add 0.01 of fudge, so that if the margin && radius == 0., we don't endless loop. */
+					MT_Scalar marg = 0.01;// + hitspc->getmargin();
+
+					if (hitspc->GetSumoObject()->getShapeProps())
+					{
+						marg += 2*hitspc->GetSumoObject()->getShapeProps()->m_radius;
+					}
+					
+					/* Calculate the other side of this object */
+					MT_Point3 hitObjPos;
+					hitspc->GetWorldPosition(hitObjPos);
+					MT_Vector3 hitvector = hitObjPos - resultpoint;
+					if (hitvector.dot(hitvector) > MT_EPSILON)
+					{
+						hitvector.normalize();
+						marg *= 2.*todir.dot(hitvector);
+					}
+					frompoint = resultpoint + marg * todir;
+				} else {
+					ready = true;
+				}
+#endif
+			ready = true;
+			}
+			else
+			{
+				ready = true;
+				if (m_propertyname.Length() == 0)
+				{
+					bFound = true;
+				}
+				else
+				{
+					if (m_bFindMaterial)
+					{
+						if (info->m_auxilary_info)
+						{
+							bFound = (m_propertyname== ((char*)info->m_auxilary_info));
+						}
+					}
+					else
+					{
+						bFound = hitgameobj->GetProperty(m_propertyname) != NULL;
+					}
+				}
+
+				if (bFound)
+				{
+					m_rayHit = true;
+					m_hitObject = hitgameobj;
+					m_hitPosition = resultpoint;
+					m_hitNormal = resultnormal;
+						
+				}
+			}
+		}
+		else
+		{
+			ready = true;
+		}
+	}
+	while (!ready);
+	
 	
 	
 	/* now pass this result to some controller */
-	if (m_rayHit) 
+    if (m_rayHit) 
 	{
 		if (!m_bTriggered)
 		{
@@ -278,6 +293,10 @@ bool KX_RaySensor::Evaluate(CValue* event)
 			result = true;
 			m_bTriggered = true;
 		}
+		else
+		{
+			
+		}
 	}
 	else
 	{
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 03c9cab53bf..5ddacd05d05 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -120,9 +120,36 @@ void		CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dloc
 
 }
 
-void		CcdPhysicsController::RelativeRotate(const float drot[9],bool local)
+void		CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
 {
+	if (m_body )
+	{
+		SimdMatrix3x3 drotmat(	rotval[0],rotval[1],rotval[2],
+								rotval[4],rotval[5],rotval[6],
+								rotval[8],rotval[9],rotval[10]);
+
+
+		SimdMatrix3x3 currentOrn;
+		GetWorldOrientation(currentOrn);
+
+		SimdTransform xform = m_body->getCenterOfMassTransform();
+
+		xform.setBasis(xform.getBasis()*(local ? 
+		drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
+
+		m_body->setCenterOfMassTransform(xform);
+	}
+
+}
+
+void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
+{
+	float orn[4];
+	m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
+	SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
+	mat.setRotation(quat);
 }
+
 void		CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
 {
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
index 728a439866d..4f189b7f324 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
@@ -8,6 +8,8 @@
 ///	It contains the IMotionState and IDeformableMesh Interfaces.
 #include "SimdVector3.h"
 #include "SimdScalar.h"	
+#include "SimdMatrix3x3.h"
+
 class CollisionShape;
 
 extern float gDeactivationTime;
@@ -54,6 +56,8 @@ class CcdPhysicsController : public PHY_IPhysicsController
 	CollisionShape*			m_collisionShape;
 	void*		m_newClientInfo;
 
+	void GetWorldOrientation(SimdMatrix3x3& mat);
+
 	public:
 	
 		int				m_collisionDelay;
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index 5c117301ffb..b68e730b4c6 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -717,89 +717,7 @@ void		CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 	}
 }
 
-#ifdef DASHDASJKHASDJK
-class RaycastingQueryBox : public QueryBox
-{
-	
-	SimdVector3 m_aabbMin;
-	
-	SimdVector3 m_aabbMax;
-	
-	
-	
-public:
-	
-	RaycastCallback	m_raycastCallback;
-	
-	
-	RaycastingQueryBox(QueryBoxConstructionInfo& ci,const SimdVector3& from,const SimdVector3& to)
-		: QueryBox(ci),
-		m_raycastCallback(from,to)
-	{
-		for (int i=0;i<3;i++)
-		{
-			float fromI = from[i];
-			float toI = to[i];
-			if (fromI < toI)
-			{
-				m_aabbMin[i] = fromI;
-				m_aabbMax[i] = toI;
-			} else
-			{
-				m_aabbMin[i] = toI;
-				m_aabbMax[i] = fromI;
-			}
-		}
-		
-	}
-	virtual void AddCollider( BroadphaseProxy* proxy)
-	{
-		//perform raycast if wanted, and update the m_hitFraction
-		
-		if (proxy->GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
-		{
-			//do it
-			RigidBody* body = (RigidBody*)proxy->m_clientObject;
-			TriangleMeshInterface* meshInterface = (TriangleMeshInterface*)
-				body->m_minkowski1;
-			
-			//if the hit is closer, record the proxy!
-			float curFraction = m_raycastCallback.m_hitFraction;
-			
-			meshInterface->ProcessAllTriangles(&m_raycastCallback,m_aabbMin,m_aabbMax);
-			
-			if (m_raycastCallback.m_hitFraction < curFraction)
-			{
-				m_raycastCallback.m_hitProxy = proxy;
-			}
-			
-		}
-		
-	}
-};
 
-struct InternalVehicleRaycaster : public VehicleRaycaster
-{
-	
-	CcdPhysicsEnvironment* m_env;
-	
-public:
-	
-	InternalVehicleRaycaster(CcdPhysicsEnvironment* env)
-		:	m_env(env)
-	{
-		
-	}
-	
-	virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
-	{
-
-		return 0;
-	}
-	
-};
-
-#endif 
 int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
 														float pivotX,float pivotY,float pivotZ,
 														float axisX,float axisY,float axisZ)
@@ -869,7 +787,7 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 								float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 {
 
-	int minFraction = 1.f;
+	float minFraction = 1.f;
 
 	SimdTransform	rayFromTrans,rayToTrans;
 	rayFromTrans.setIdentity();
@@ -899,12 +817,15 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 			ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
 			VoronoiSimplexSolver	simplexSolver;
 			SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+			
 			if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
 			{
 				//add hit
 				rayResult.m_normal.normalize();
 				if (rayResult.m_fraction < minFraction)
 				{
+
+
 					minFraction = rayResult.m_fraction;
 					nearestHit = ctrl;
 					normalX = rayResult.m_normal.getX();
@@ -916,6 +837,42 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 				}
 			}
 		}
+		else
+		{
+				if (body->GetCollisionShape()->IsConcave())
+				{
+
+					TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
+					
+					SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
+
+					SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
+					SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
+
+					RaycastCallback	rcb(rayFromLocal,rayToLocal);
+					rcb.m_hitFraction = minFraction;
+
+					SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+
+					triangleMesh->ProcessAllTriangles(&rcb,-aabbMax,aabbMax);
+					if (rcb.m_hitFound)// && (rcb.m_hitFraction < minFraction))
+					{
+						nearestHit = ctrl;
+						minFraction = rcb.m_hitFraction;
+						SimdVector3 hitNormalWorld = body->getCenterOfMassTransform()(rcb.m_hitNormalLocal);
+
+						normalX = hitNormalWorld.getX();
+						normalY = hitNormalWorld.getY();
+						normalZ = hitNormalWorld.getZ();
+						SimdVector3 hitWorld;
+						hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
+						hitX = hitWorld.getX();
+						hitY = hitWorld.getY();
+						hitZ = hitWorld.getZ();
+						
+					}
+				}
+		}
 	}
 
 	return nearestHit;
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h
index 9e1ec5167bb..6749e7957ec 100644
--- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h
+++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h
@@ -8,7 +8,7 @@
 struct SM_ClientObjectInfo
 {
 	int			m_type;
-	void*		m_clientobject;
+	void*		m_clientobject1;
 	void*		m_auxilary_info;
 };