some more work on bullet raycast

4 files changed, 91 insertions, 88 deletions

diff --git a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
index e6b85e3f42a..c7f226e0883 100644
--- a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
@@ -45,9 +45,19 @@ void	KX_BulletPhysicsController::RelativeTranslate(const MT_Vector3& dloc,bool l
 	CcdPhysicsController::RelativeTranslate(dloc[0],dloc[1],dloc[2],local);
 
 }
+
 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);
 }
+
 void	KX_BulletPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
 {
 }
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index c16a31ee18d..e0322d5cc67 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -116,9 +116,38 @@ void		CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dloc
 
 }
 
-void		CcdPhysicsController::RelativeRotate(const float drot[9],bool local)
+void		CcdPhysicsController::RelativeRotate(const float rotval[12],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)));
+
+		printf("hi\n");
+		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 8c87940721a..faef3e6c7d2 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;
@@ -87,7 +89,7 @@ class CcdPhysicsController : public PHY_IPhysicsController
 
 		// kinematic methods
 		virtual void		RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local);
-		virtual void		RelativeRotate(const float drot[9],bool local);
+		virtual void		RelativeRotate(const float rotval[12],bool local);
 		virtual	void		getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal);
 		virtual	void		setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal);
 		virtual	void		setPosition(float posX,float posY,float posZ);
@@ -130,6 +132,8 @@ class CcdPhysicsController : public PHY_IPhysicsController
 
 		void	SetAabb(const SimdVector3& aabbMin,const SimdVector3& aabbMax);
 
+		void GetWorldOrientation(SimdMatrix3x3& mat);
+
 
 };
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index fa3253003fc..5cbc901c68a 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -301,11 +301,12 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 {
 	
 	
-//	printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
 	
 	if (timeStep == 0.f)
 		return true;
 
+	printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
+
 	//clamp hardcoded for now
 	if (timeStep > 0.02)
 		timeStep = 0.02;
@@ -313,8 +314,8 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 	//this is needed because scaling is not known in advance, and scaling has to propagate to the shape
 	if (!m_scalingPropagated)
 	{
-		//SyncMotionStates(timeStep);
-		//m_scalingPropagated = true;
+		SyncMotionStates(timeStep);
+		m_scalingPropagated = true;
 	}
 
 
@@ -708,89 +709,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)
@@ -856,10 +775,12 @@ void		CcdPhysicsEnvironment::removeConstraint(int constraintid)
 	}
 	
 }
+
 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
 								float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 {
 
+	printf("raytest\n");
 	int minFraction = 1.f;
 
 	SimdTransform	rayFromTrans,rayToTrans;
@@ -879,13 +800,17 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 	for (i=m_controllers.begin();
 	!(i==m_controllers.end()); i++)
 	{
+
 		CcdPhysicsController* ctrl = (*i);
+		if (ctrl == ignoreClient)
+			continue;
+
 		RigidBody* body = ctrl->GetRigidBody();
 
 		if (body->GetCollisionShape()->IsConvex())
 		{
 			ConvexCast::CastResult rayResult;
-			rayResult.m_fraction = 1.f;
+			rayResult.m_fraction = minFraction;
 
 			ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
 			VoronoiSimplexSolver	simplexSolver;
@@ -906,6 +831,41 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 					hitZ = rayResult.m_hitTransformA.getOrigin().getZ();
 				}
 			}
+		} 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))
+				{
+					printf("hit %f\n",rcb.m_hitFraction);
+					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();
+					
+				}
+			}
 		}
 	}