some more fixes in the raycast/mouse over

8 files changed, 85 insertions, 90 deletions

diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
index fb7b364b1e6..c09599f39b1 100644
--- a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
@@ -15,7 +15,7 @@
 #include "AabbUtil2.h"
 #include "NarrowPhaseCollision/CollisionMargin.h"
 
-
+#include "stdio.h"
 
 TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface)
 : m_meshInterface(meshInterface),
@@ -143,6 +143,7 @@ void	TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const Sim
 			{
 				//check aabb in triangle-space, before doing this
 				callback->ProcessTriangle(triangle);
+	
 			}
 			
 		}
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
index 418e0a7e241..98eb1067063 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
@@ -23,11 +23,9 @@ RaycastCallback::RaycastCallback(const SimdVector3& from,const SimdVector3& to)
 }
 
 
-#include <stdio.h>
+
 void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
 {
-	
-
 	const SimdVector3 &vert0=triangle[0];
 	const SimdVector3 &vert1=triangle[1];
 	const SimdVector3 &vert2=triangle[2];
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp
index 80fea519887..a280c031a27 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp
@@ -114,6 +114,8 @@ bool	SubsimplexConvexCast::calcTimeOfImpact(
 
 	int numiter = MAX_ITERATIONS - maxIter;
 //	printf("number of iterations: %d", numiter);
+
+	//printf("lambd%f",lambda);
 	result.m_fraction = lambda;
 	result.m_normal = n;
 
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
index a6f6a4df5a4..d3f40077963 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
@@ -38,7 +38,8 @@ ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm()
 BoxTriangleCallback::BoxTriangleCallback(Dispatcher*  dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
   m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
 	  m_timeStep(0.f),
-	  m_stepCount(-1)
+	  m_stepCount(-1),
+	  m_triangleCount(0)
 {
 
 	  m_triangleProxy.SetClientObjectType(TRIANGLE_SHAPE_PROXYTYPE);
@@ -70,6 +71,9 @@ void	BoxTriangleCallback::ClearCache()
 void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle)
 {
  
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
+
 
 	RigidBody* triangleBody = (RigidBody*)m_triangleProxy.m_clientObject;
 
@@ -101,6 +105,7 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle)
 
 void	BoxTriangleCallback::SetTimeStepAndCounters(float timeStep,int stepCount,float collisionMarginTriangle,bool useContinuous)
 {
+	m_triangleCount = 0;
 	m_timeStep = timeStep;
 	m_stepCount = stepCount;
 	m_useContinuous = useContinuous;
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h
index 10ff0718d92..ad4c96c0ab4 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h
@@ -36,7 +36,8 @@ class BoxTriangleCallback : public TriangleCallback
 	float m_collisionMarginTriangle;
 	
 public:
-
+int	m_triangleCount;
+	
 	PersistentManifold*	m_manifoldPtr;
 
 	BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
index 03c9cab53bf..5ddacd05d05 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/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/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
index 728a439866d..4f189b7f324 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/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/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
index 5c117301ffb..b68e730b4c6 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/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;