some more work on bullet raycast

7 files changed, 116 insertions, 106 deletions

diff --git a/extern/bullet/Bullet/Bullet3_vc8.vcproj b/extern/bullet/Bullet/Bullet3_vc8.vcproj
index 125b4b6e0b1..c2f3e70180f 100644
--- a/extern/bullet/Bullet/Bullet3_vc8.vcproj
+++ b/extern/bullet/Bullet/Bullet3_vc8.vcproj
@@ -4,6 +4,7 @@
 	Version="8.00"
 	Name="Bullet3ContinuousCollision"
 	ProjectGUID="{FFD3C64A-30E2-4BC7-BC8F-51818C320400}"
+	SignManifests="true"
 	>
 	<Platforms>
 		<Platform
@@ -277,6 +278,14 @@
 				>
 			</File>
 			<File
+				RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.cpp"
+				>
+			</File>
+			<File
+				RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.h"
+				>
+			</File>
+			<File
 				RelativePath=".\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cpp"
 				>
 			</File>
@@ -429,14 +438,6 @@
 				>
 			</File>
 			<File
-				RelativePath=".\CollisionShapes\TriangleMesh.cpp"
-				>
-			</File>
-			<File
-				RelativePath=".\CollisionShapes\TriangleMesh.h"
-				>
-			</File>
-			<File
 				RelativePath=".\CollisionShapes\TriangleMeshShape.cpp"
 				>
 			</File>
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
index f27801d224b..418e0a7e241 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
@@ -16,17 +16,18 @@ RaycastCallback::RaycastCallback(const SimdVector3& from,const SimdVector3& to)
 	m_from(from),
 	m_to(to),
 	m_hitFraction(1.f),
-	m_hitProxy(0)
+	m_hitProxy(0),
+	m_hitFound(false)
 {
 
 }
 
 
-
+#include <stdio.h>
 void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
 {
+	
 
-	int hits_found=0;
 	const SimdVector3 &vert0=triangle[0];
 	const SimdVector3 &vert1=triangle[1];
 	const SimdVector3 &vert2=triangle[2];
@@ -53,8 +54,11 @@ void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
 	// Add an epsilon as a tolerance for the raycast,
 	// in case the ray hits exacly on the edge of the triangle.
 	// It must be scaled for the triangle size.
+	
 	if(distance < m_hitFraction)
 	{
+		
+
 		float edge_tolerance =triangleNormal.length2();		
 		edge_tolerance *= -0.0001f;
 		SimdVector3 point; point.setInterpolate3( m_from, m_to, distance);
@@ -65,6 +69,8 @@ void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
 
 			if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) 
 			{
+						
+
 				SimdVector3 v2p; v2p = vert2 -  point;
 				SimdVector3 cp1;
 				cp1 = v1p.cross( v2p);
@@ -72,23 +78,23 @@ void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
 				{
 					SimdVector3 cp2;
 					cp2 = v2p.cross(v0p);
+					
 					if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) 
 					{
 						m_hitFraction = distance;
 						if ( dist_a > 0 )
 						{
-							m_hitNormalWorld = triangleNormal;
+							m_hitNormalLocal = triangleNormal;
 						}
 						else
 						{
-							m_hitNormalWorld = -triangleNormal;
+							m_hitNormalLocal = -triangleNormal;
 						}
-						hits_found = 1;
+						
+						m_hitFound= true;
 					}
 				}
 			}
 		}
 	}
-	//return hits_found;
-
 }
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h
index 5dbffde4244..92dac112a36 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h
@@ -27,8 +27,8 @@ public:
 	BroadphaseProxy*	m_hitProxy;
 
 	//output
-	SimdVector3			m_hitNormalWorld;
-
+	SimdVector3			m_hitNormalLocal;
+	bool				m_hitFound;
 
 
 	RaycastCallback(const SimdVector3& from,const SimdVector3& to);
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();
+					
+				}
+			}
 		}
 	}