some more work on bullet raycast
authorErwin Coumans <blender@erwincoumans.com>
Mon, 8 Aug 2005 17:08:42 +0000 (17:08 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Mon, 8 Aug 2005 17:08:42 +0000 (17:08 +0000)
extern/bullet/Bullet/Bullet3_vc8.vcproj
extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.cpp
extern/bullet/Bullet/NarrowPhaseCollision/RaycastCallback.h
source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.h
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp

index 125b4b6e0b117fc40f4d34017de337619ebca809..c2f3e70180f45ac3ae425973b61ae85e1eefcb9a 100644 (file)
@@ -4,6 +4,7 @@
        Version="8.00"
        Name="Bullet3ContinuousCollision"
        ProjectGUID="{FFD3C64A-30E2-4BC7-BC8F-51818C320400}"
+       SignManifests="true"
        >
        <Platforms>
                <Platform
                                RelativePath=".\NarrowPhaseCollision\ManifoldPoint.h"
                                >
                        </File>
+                       <File
+                               RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.cpp"
+                               >
+                       </File>
+                       <File
+                               RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.h"
+                               >
+                       </File>
                        <File
                                RelativePath=".\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cpp"
                                >
                                RelativePath=".\CollisionShapes\TriangleCallback.h"
                                >
                        </File>
-                       <File
-                               RelativePath=".\CollisionShapes\TriangleMesh.cpp"
-                               >
-                       </File>
-                       <File
-                               RelativePath=".\CollisionShapes\TriangleMesh.h"
-                               >
-                       </File>
                        <File
                                RelativePath=".\CollisionShapes\TriangleMeshShape.cpp"
                                >
index f27801d224b211354fab07d877cd1c0a07158999..418e0a7e241b574a1cbcec7916fa85cf7c0ace71 100644 (file)
@@ -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;
-
 }
index 5dbffde42449b86d586476ee5c790fa8faea1456..92dac112a36c66ee9ffa0d8dd4d639bb1468bd1f 100644 (file)
@@ -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);
index e6b85e3f42ac22ce727114a7d8e7f8c5e702d27f..c7f226e0883f9f3ea5edee1a768c042942ab45c1 100644 (file)
@@ -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)
 {
 }
index c16a31ee18d6df402a9f95db2a5a6b550e030c88..e0322d5cc67ea8f42e63f9f08e13d9135df89d4e 100644 (file)
@@ -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)
 {
 }
index 8c87940721a9a8a5591e7cb1b833bddf11b81b58..faef3e6c7d2c1ac87641ab9d3090c62e1cd2de53 100644 (file)
@@ -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);
+
 
 };
 
index fa3253003fc0947d163a23a645edebd46aa75726..5cbc901c68a701d591f65956a4273b11b2ef6033 100644 (file)
@@ -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();
+                                       
+                               }
+                       }
                }
        }