Add Fh/Rot Fh to Bullet.
authorErwin Coumans <blender@erwincoumans.com>
Mon, 29 Sep 2008 06:58:49 +0000 (06:58 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Mon, 29 Sep 2008 06:58:49 +0000 (06:58 +0000)
source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.h
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h

index d7bd1e9c7cda72f1ee2be51a9b6f58f0c967b3a1..3d20ce8a3a558d0a9067c4d687997c4f2579a76f 100644 (file)
@@ -211,6 +211,9 @@ SG_Controller*      KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
        //parentcontroller is here be able to avoid collisions between parent/child
 
        PHY_IPhysicsController* parentctrl = NULL;
+       KX_BulletPhysicsController* parentKxCtrl = NULL;
+       CcdPhysicsController* ccdParent = NULL;
+
        
        if (destnode != destnode->GetRootSGParent())
        {
@@ -230,12 +233,15 @@ SG_Controller*    KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
                                KX_GameObject *clientgameobj = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
                                if (clientgameobj)
                                {
-                                       parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
+                                       parentKxCtrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
+                                       parentctrl = parentKxCtrl;
+                                       ccdParent = parentKxCtrl;
                                }
                        }
                }
        }
 
+       physicsreplica->setParentCtrl(ccdParent);
        physicsreplica->PostProcessReplica(motionstate,parentctrl);
        physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
        return physicsreplica;
index 68e0997aec0f086403c4a0ca22a90bb43c74f4a0..0e7a6d92ec11d44de45f9b97eb37ae9f9a616c16 100644 (file)
@@ -1028,10 +1028,16 @@ void    KX_ConvertBulletObject( class   KX_GameObject* gameobj,
        ci.m_do_anisotropic = shapeprops->m_do_anisotropic;
        ci.m_anisotropicFriction.setValue(shapeprops->m_friction_scaling[0],shapeprops->m_friction_scaling[1],shapeprops->m_friction_scaling[2]);
 
-       //smprop->m_do_fh = kxshapeprops->m_do_fh;
-       //smprop->m_do_rot_fh = kxshapeprops->m_do_rot_fh ;
 
-       
+//////////
+       //do Fh, do Rot Fh
+       ci.m_do_fh = shapeprops->m_do_fh;
+       ci.m_do_rot_fh = shapeprops->m_do_rot_fh ;
+       ci.m_fh_damping = smmaterial->m_fh_damping;
+       ci.m_fh_distance = smmaterial->m_fh_distance;
+       ci.m_fh_normal = smmaterial->m_fh_normal;
+       ci.m_fh_spring = smmaterial->m_fh_spring;
+       ci.m_radius = objprop->m_radius;
        
        
        ///////////////////
@@ -1098,6 +1104,10 @@ void     KX_ConvertBulletObject( class   KX_GameObject* gameobj,
                if (rbody && objprop->m_disableSleeping)
                        rbody->setActivationState(DISABLE_DEACTIVATION);
        }
+
+       CcdPhysicsController* parentCtrl = objprop->m_dynamic_parent ? (KX_BulletPhysicsController*)objprop->m_dynamic_parent->GetPhysicsController() : 0;
+       physicscontroller->setParentCtrl(parentCtrl);
+
        
        //Now done directly in ci.m_collisionFlags so that it propagates to replica
        //if (objprop->m_ghost)
index 8ec040e89ee9467fc078f238cd319a1316528ea0..7196a1ae095e58146000341b97e4cd12d0f79091 100644 (file)
@@ -54,7 +54,7 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
        m_newClientInfo = 0;
        m_registerCount = 0;
        m_softBodyTransformInitialized = false;
-
+       m_parentCtrl = 0;
        // copy pointers locally to allow smart release
        m_MotionState = ci.m_MotionState;
        m_collisionShape = ci.m_collisionShape;
@@ -628,6 +628,7 @@ void                CcdPhysicsController::WriteDynamicsToMotionState()
                // controller replication
 void           CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
 {
+       
        m_softBodyTransformInitialized=false;
        m_MotionState = motionstate;
        m_registerCount = 0;
index f001d6043cbe11c77a8d03ba2b1e8cf55a44cd53..a1245f6a3e9d429f5d6514a0051d2ef708cb7ace 100644 (file)
@@ -264,6 +264,14 @@ struct CcdConstructionInfo
        bool    m_do_anisotropic;
        btVector3 m_anisotropicFriction;
 
+       bool            m_do_fh;                 ///< Should the object have a linear Fh spring?
+       bool            m_do_rot_fh;             ///< Should the object have an angular Fh spring?
+       btScalar        m_fh_spring;             ///< Spring constant (both linear and angular)
+       btScalar        m_fh_damping;            ///< Damping factor (linear and angular) in range [0, 1]
+       btScalar        m_fh_distance;           ///< The range above the surface where Fh is active.    
+       bool            m_fh_normal;             ///< Should the object slide off slopes?
+       float           m_radius;//for fh backwards compatibility
+
 };
 
 
@@ -295,6 +303,9 @@ class CcdPhysicsController : public PHY_IPhysicsController
        void*           m_newClientInfo;
        int                     m_registerCount;        // needed when multiple sensors use the same controller
        CcdConstructionInfo     m_cci;//needed for replication
+
+       CcdPhysicsController* m_parentCtrl;
+
        void GetWorldOrientation(btMatrix3x3& mat);
 
        void CreateRigidbody();
@@ -318,6 +329,15 @@ class CcdPhysicsController : public PHY_IPhysicsController
 
                virtual ~CcdPhysicsController();
 
+               CcdConstructionInfo& getConstructionInfo()
+               {
+                       return m_cci;
+               }
+               const CcdConstructionInfo& getConstructionInfo() const
+               {
+                       return m_cci;
+               }
+
 
                btRigidBody* GetRigidBody();
                btCollisionObject*      GetCollisionObject();
@@ -422,6 +442,23 @@ class CcdPhysicsController : public PHY_IPhysicsController
                {
                        return m_cci.m_physicsEnv;
                }
+
+               void    setParentCtrl(CcdPhysicsController* parentCtrl)
+               {
+                       m_parentCtrl = parentCtrl;
+               }
+
+               CcdPhysicsController*   getParentCtrl()
+               {
+                       return m_parentCtrl;
+               }
+
+               const CcdPhysicsController*     getParentCtrl() const
+               {
+                       return m_parentCtrl;
+               }
+
+
                
 };
 
index 763fc148a0472a5b40c70ba75a42a2343dda8c0a..483f829c2bd8c97404d13f57b1f13f8f10ca2629 100644 (file)
@@ -558,6 +558,8 @@ bool        CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
                (*it)->SynchronizeMotionStates(timeStep);
        }
 
+       processFhSprings(curTime,timeStep);
+
        float subStep = timeStep / float(m_numTimeSubSteps);
        for (i=0;i<m_numTimeSubSteps;i++)
        {
@@ -570,6 +572,11 @@ bool       CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
                (*it)->SynchronizeMotionStates(timeStep);
        }
 
+       for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
+       {
+               (*it)->SynchronizeMotionStates(timeStep);
+       }
+
        for (i=0;i<m_wrapperVehicles.size();i++)
        {
                WrapperVehicle* veh = m_wrapperVehicles[i];
@@ -585,6 +592,170 @@ bool      CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
        return true;
 }
 
+class ClosestRayResultCallbackNotMe : public btCollisionWorld::ClosestRayResultCallback
+{
+       btCollisionObject* m_owner;
+       btCollisionObject* m_parent;
+
+public:
+       ClosestRayResultCallbackNotMe(const btVector3& rayFromWorld,const btVector3& rayToWorld,btCollisionObject* owner,btCollisionObject* parent)
+               :btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld),
+               m_owner(owner)
+       {
+
+       }
+
+       virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+       {
+               //don't collide with self
+               if (proxy0->m_clientObject == m_owner)
+                       return false;
+
+               if (proxy0->m_clientObject == m_parent)
+                       return false;
+
+               return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
+       }
+
+};
+
+void   CcdPhysicsEnvironment::processFhSprings(double curTime,float timeStep)
+{
+       std::set<CcdPhysicsController*>::iterator it;
+       
+       for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
+       {
+               CcdPhysicsController* ctrl = (*it);
+               if (ctrl->GetRigidBody() && ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh)
+               {
+                       //printf("has Fh or RotFh\n");
+                       //re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
+                       //send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
+
+                       btRigidBody* body = ctrl->GetRigidBody();
+                       CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
+                       btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
+                       btRigidBody* cl_object = parentBody ? parentBody : body;
+
+                       if (body->isStaticOrKinematicObject())
+                               continue;
+
+                       btVector3 rayDirLocal(0,0,-10);
+
+                       //m_dynamicsWorld
+                       //ctrl->GetRigidBody();
+                       btVector3 rayFromWorld = body->getCenterOfMassPosition();
+                       //btVector3     rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
+                       //ray always points down the z axis in world space...
+                       btVector3       rayToWorld = rayFromWorld + rayDirLocal;
+
+                       ClosestRayResultCallbackNotMe   resultCallback(rayFromWorld,rayToWorld,body,parentBody);
+
+                       m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
+                       if (resultCallback.hasHit())
+                       {
+                               //we hit this one: resultCallback.m_collisionObject;
+                               CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
+
+                               if (controller)
+                               {
+                                       if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
+                                               continue;
+
+                                       btRigidBody* hit_object = controller->GetRigidBody();
+                                       if (!hit_object)
+                                               continue;
+
+                                       CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
+
+                                       float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
+                                       if (distance >= hitObjShapeProps.m_fh_distance)
+                                               continue;
+                                       
+                                       
+
+                                       //btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
+                                       btVector3 ray_dir = rayDirLocal.normalized();
+                                       btVector3 normal = resultCallback.m_hitNormalWorld;
+                                       normal.normalize();
+
+                                       
+                                       if (ctrl->getConstructionInfo().m_do_fh) 
+                                       {
+                                               btVector3 lspot = cl_object->getCenterOfMassPosition()
+                                                       + rayDirLocal * resultCallback.m_closestHitFraction;
+
+
+                                                       
+
+                                               lspot -= hit_object->getCenterOfMassPosition();
+                                               btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
+                                               btScalar rel_vel_ray = ray_dir.dot(rel_vel);
+                                               btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance; 
+                                               
+                                               btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
+                                               btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
+                                               
+                                               cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir)); 
+                                               if (hitObjShapeProps.m_fh_normal) 
+                                               {
+                                                       cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
+                                               }
+                                               
+                                               btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
+                                               
+                                               
+                                               if (ctrl->getConstructionInfo().m_do_anisotropic) {
+                                                       //Bullet basis contains no scaling/shear etc.
+                                                       const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
+                                                       btVector3 loc_lateral = lateral * lcs;
+                                                       const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
+                                                       loc_lateral *= friction_scaling;
+                                                       lateral = lcs * loc_lateral;
+                                               }
+
+                                               btScalar rel_vel_lateral = lateral.length();
+                                               
+                                               if (rel_vel_lateral > SIMD_EPSILON) {
+                                                       btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
+
+                                                       btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
+                                                       
+                                                       btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
+                                                       
+                                                       btVector3 friction = (rel_mom_lateral > max_friction) ?
+                                                               -lateral * (max_friction / rel_vel_lateral) :
+                                                               -lateral;
+                                                       
+                                                               cl_object->applyCentralImpulse(friction);
+                                               }
+                                       }
+
+                                       
+                                       if (ctrl->getConstructionInfo().m_do_rot_fh) {
+                                               btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
+
+                                               btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
+                                               btVector3 ang_vel = cl_object->getAngularVelocity();
+                                               
+                                               // only rotations that tilt relative to the normal are damped
+                                               ang_vel -= ang_vel.dot(normal) * normal;
+                                               
+                                               btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;  
+                                               
+                                               cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
+                                       }
+
+                               }
+
+
+                       }
+
+
+               }
+       }
+       
+}
 
 void           CcdPhysicsEnvironment::setDebugMode(int debugMode)
 {
index 56a639509c5938da43a854badf6ac7c07500f63f..3d9d5442b8ff8c64b458c17a67d1465b57ec7072 100644 (file)
@@ -74,6 +74,7 @@ protected:
 
        btContactSolverInfo     m_solverInfo;
        
+       void    processFhSprings(double curTime,float timeStep);
 
        public:
                CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);