From cef5c4b76508d826959fb46f3f77ba0af029046f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 29 Sep 2008 06:58:49 +0000 Subject: Add Fh/Rot Fh to Bullet. --- .../Ketsji/KX_BulletPhysicsController.cpp | 8 +- .../gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp | 16 +- .../Physics/Bullet/CcdPhysicsController.cpp | 3 +- .../Physics/Bullet/CcdPhysicsController.h | 37 +++++ .../Physics/Bullet/CcdPhysicsEnvironment.cpp | 171 +++++++++++++++++++++ .../Physics/Bullet/CcdPhysicsEnvironment.h | 1 + 6 files changed, 231 insertions(+), 5 deletions(-) (limited to 'source/gameengine') diff --git a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp index d7bd1e9c7cd..3d20ce8a3a5 100644 --- a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp +++ b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp @@ -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( (*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; diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp index 68e0997aec0..0e7a6d92ec1 100644 --- a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp +++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp @@ -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) diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp index 8ec040e89ee..7196a1ae095 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp @@ -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; diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h index f001d6043cb..a1245f6a3e9 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h @@ -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; + } + + }; diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 763fc148a04..483f829c2bd 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -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;iSynchronizeMotionStates(timeStep); } + for (it=m_controllers.begin(); it!=m_controllers.end(); it++) + { + (*it)->SynchronizeMotionStates(timeStep); + } + for (i=0;im_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::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(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) { diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index 56a639509c5..3d9d5442b8f 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -74,6 +74,7 @@ protected: btContactSolverInfo m_solverInfo; + void processFhSprings(double curTime,float timeStep); public: CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0); -- cgit v1.2.3