Created a KX_SoftBodyDeformer for real-time soft bodies.

Added SetDeformer/GetDeformer() to KX_GameObject.
Store mapping between graphics/soft body vertices (work-in-progress)
Real-time soft body integration is still very premature, but
for a quick preview, see this testfile:
http://bulletphysics.com/ftp/pub/test/index.php?dir=blender/&file=soft_test.blend

9 files changed, 306 insertions, 55 deletions

diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.cpp b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
index 9b9d5c95de9..6a5048a9f15 100644
--- a/source/gameengine/Converter/BL_BlenderDataConversion.cpp
+++ b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
@@ -743,7 +743,8 @@ RAS_MeshObject* BL_ConvertMesh(Mesh* mesh, Object* blenderobj, RAS_IRenderTools*
 	}
 
 	// Determine if we need to make a skinned mesh
-	if (mesh->dvert || mesh->key) {
+	if (mesh->dvert || mesh->key || ((blenderobj->gameflag & OB_SOFT_BODY) != 0)) 
+	{
 		meshobj = new BL_SkinMeshObject(mesh, lightlayer);
 		skinMesh = true;
 	}
@@ -1554,20 +1555,20 @@ static KX_GameObject *gameobject_from_blenderobject(
 			// not that we can have shape keys without dvert! 
 			BL_ShapeDeformer *dcont = new BL_ShapeDeformer((BL_DeformableGameObject*)gameobj, 
 															ob, (BL_SkinMeshObject*)meshobj);
-			((BL_DeformableGameObject*)gameobj)->m_pDeformer = dcont;
+			((BL_DeformableGameObject*)gameobj)->SetDeformer(dcont);
 			if (bHasArmature)
 				dcont->LoadShapeDrivers(ob->parent);
 		} else if (bHasArmature) {
 			BL_SkinDeformer *dcont = new BL_SkinDeformer((BL_DeformableGameObject*)gameobj,
 															ob, (BL_SkinMeshObject*)meshobj);
-			((BL_DeformableGameObject*)gameobj)->m_pDeformer = dcont;
+			((BL_DeformableGameObject*)gameobj)->SetDeformer(dcont);
 		} else if (bHasDvert) {
 			// this case correspond to a mesh that can potentially deform but not with the
 			// object to which it is attached for the moment. A skin mesh was created in
 			// BL_ConvertMesh() so must create a deformer too!
 			BL_MeshDeformer *dcont = new BL_MeshDeformer((BL_DeformableGameObject*)gameobj,
 														  ob, (BL_SkinMeshObject*)meshobj);
-			((BL_DeformableGameObject*)gameobj)->m_pDeformer = dcont;
+			((BL_DeformableGameObject*)gameobj)->SetDeformer(dcont);
 		}
 		
 		MT_Point3 min = MT_Point3(center) - MT_Vector3(extents);
@@ -2193,8 +2194,8 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 	
 				if (obj && blenderobj->parent && blenderobj->parent->type==OB_ARMATURE && blenderobj->partype==PARSKEL){
 					KX_GameObject *par = converter->FindGameObject(blenderobj->parent);
-					if (par && obj->m_pDeformer)
-						((BL_SkinDeformer*)obj->m_pDeformer)->SetArmature((BL_ArmatureObject*) par);
+					if (par && obj->GetDeformer())
+						((BL_SkinDeformer*)obj->GetDeformer())->SetArmature((BL_ArmatureObject*) par);
 				}
 			}
 		}
diff --git a/source/gameengine/Converter/BL_DeformableGameObject.h b/source/gameengine/Converter/BL_DeformableGameObject.h
index 315ad18c42c..126a1fcb1e7 100644
--- a/source/gameengine/Converter/BL_DeformableGameObject.h
+++ b/source/gameengine/Converter/BL_DeformableGameObject.h
@@ -83,9 +83,21 @@ public:
 		return (m_pDeformer) ? ((BL_MeshDeformer*)m_pDeformer)->GetMesh()->key : NULL;
 	}
 	
+	virtual void	SetDeformer(class RAS_Deformer* deformer)
+	{
+		m_pDeformer = deformer;
+	}
+	virtual class RAS_Deformer* GetDeformer()
+	{
+		return m_pDeformer;
+	}
+
 public:
-	RAS_Deformer		*m_pDeformer;	
+	
 protected:	
+	
+	RAS_Deformer		*m_pDeformer;
+
 	class BL_ShapeActionActuator *m_activeAct;
 	double		m_lastframe;
 	Object*		m_blendobj;
diff --git a/source/gameengine/Converter/BL_SkinMeshObject.cpp b/source/gameengine/Converter/BL_SkinMeshObject.cpp
index 4f9f1a434b5..eb3f9d0588d 100644
--- a/source/gameengine/Converter/BL_SkinMeshObject.cpp
+++ b/source/gameengine/Converter/BL_SkinMeshObject.cpp
@@ -87,7 +87,7 @@ void BL_SkinMeshObject::UpdateBuckets(void* clientobj,double* oglmatrix,bool use
 			continue;
 
 		RAS_MeshSlot *slot = *it->m_slots[clientobj];
-		slot->m_pDeformer = ((BL_DeformableGameObject*)clientobj)->m_pDeformer;
+		slot->m_pDeformer = ((BL_DeformableGameObject*)clientobj)->GetDeformer();
 	}
 
 	RAS_MeshObject::UpdateBuckets(clientobj, oglmatrix, useObjectColor, rgbavec, visible, culled);
diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
index 46b53d07efc..c4411a58856 100644
--- a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
+++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
@@ -38,6 +38,8 @@
 #include "RAS_MeshObject.h"
 #include "KX_Scene.h"
 #include "SYS_System.h"
+#include "BL_SkinMeshObject.h"
+#include "BulletSoftBody/btSoftBody.h"
 
 #include "PHY_Pro.h" //todo cleanup
 #include "KX_ClientObjectInfo.h"
@@ -765,7 +767,7 @@ void	KX_ConvertBulletObject(	class	KX_GameObject* gameobj,
 		}
 	case KX_BOUNDMESH:
 		{
-			if (!ci.m_mass)
+			if (!ci.m_mass ||objprop->m_softbody)
 			{				
 				// mesh shapes can be shared, check first if we already have a shape on that mesh
 				class CcdShapeConstructionInfo *sharedShapeInfo = CcdShapeConstructionInfo::FindMesh(meshobj, false);
@@ -988,6 +990,112 @@ void	KX_ConvertBulletObject(	class	KX_GameObject* gameobj,
 
 	physicscontroller->SetObject(gameobj->GetSGNode());
 
+	class KX_SoftBodyDeformer : public RAS_Deformer
+	{
+		btSoftBody*	m_softBody;
+		class BL_SkinMeshObject*	m_pMeshObject;
+		class BL_DeformableGameObject*	m_gameobj;
+
+
+	public:
+		KX_SoftBodyDeformer(btSoftBody* softBody,BL_SkinMeshObject*	pMeshObject,BL_DeformableGameObject*	gameobj)
+			: m_softBody(softBody),
+			m_pMeshObject(pMeshObject),
+			m_gameobj(gameobj)
+		{
+			//printf("KX_SoftBodyDeformer\n");
+		};
+
+		virtual ~KX_SoftBodyDeformer()
+		{
+			//printf("~KX_SoftBodyDeformer\n");
+		};
+		virtual void Relink(GEN_Map<class GEN_HashedPtr, void*>*map)
+		{
+			//printf("relink\n");
+		}
+		virtual bool Apply(class RAS_IPolyMaterial *polymat)
+		{
+			//printf("apply\n");
+			RAS_MeshSlot::iterator it;
+			RAS_MeshMaterial *mmat;
+			RAS_MeshSlot *slot;
+			size_t i;
+
+			// update the vertex in m_transverts
+			Update();
+
+			// The vertex cache can only be updated for this deformer:
+			// Duplicated objects with more than one ploymaterial (=multiple mesh slot per object)
+			// share the same mesh (=the same cache). As the rendering is done per polymaterial
+			// cycling through the objects, the entire mesh cache cannot be updated in one shot.
+			mmat = m_pMeshObject->GetMeshMaterial(polymat);
+			if(!mmat->m_slots[(void*)m_gameobj])
+				return true;
+
+			slot = *mmat->m_slots[(void*)m_gameobj];
+
+			// for each array
+			for(slot->begin(it); !slot->end(it); slot->next(it)) 
+			{
+				// for each vertex
+				// copy the untransformed data from the original mvert
+				int count = 0;
+				{
+
+					for(i=it.startvertex; i<it.endvertex; i++,count++)
+					{
+					}
+				}
+				btSoftBody::tNodeArray&   nodes(m_softBody->m_nodes);
+
+				if (count == m_softBody->m_userIndexMapping.size())
+				{
+					int index = 0;
+					for(i=it.startvertex; i<it.endvertex; i++,index++) {
+						RAS_TexVert& v = it.vertex[i];
+
+						MT_Point3 pt (
+							nodes[m_softBody->m_userIndexMapping[index]].m_x.getX(),
+							nodes[m_softBody->m_userIndexMapping[index]].m_x.getY(),
+							nodes[m_softBody->m_userIndexMapping[index]].m_x.getZ());
+						v.SetXYZ(pt);
+
+						//(m_transverts[v.getOrigIndex()]);
+					}
+				}
+
+			}
+
+			return true;
+		}
+		virtual bool Update(void)
+		{
+			//printf("update\n");
+			return true;//??
+		}
+		virtual RAS_Deformer *GetReplica()
+		{
+			//printf("getReplica\n");
+			return 0;
+		}
+	protected:
+		//class RAS_MeshObject	*m_pMesh;
+	};
+
+	///test for soft bodies
+	if (objprop->m_softbody && physicscontroller)
+	{
+		btSoftBody* softBody = physicscontroller->GetSoftBody();
+		if (softBody && gameobj->GetMesh(0))
+		{
+			//should be a mesh then, so add a soft body deformer
+			KX_SoftBodyDeformer* softbodyDeformer = new KX_SoftBodyDeformer(softBody, (BL_SkinMeshObject*)gameobj->GetMesh(0),(BL_DeformableGameObject*)gameobj);
+			gameobj->SetDeformer(softbodyDeformer);
+			
+		}
+	}
+
 }
 
 
diff --git a/source/gameengine/Ketsji/KX_GameObject.h b/source/gameengine/Ketsji/KX_GameObject.h
index 2da0be4df25..12e29cf3294 100644
--- a/source/gameengine/Ketsji/KX_GameObject.h
+++ b/source/gameengine/Ketsji/KX_GameObject.h
@@ -332,6 +332,14 @@ public:
 		m_pPhysicsController1 = physicscontroller;
 	}
 
+	virtual class RAS_Deformer* GetDeformer()
+	{
+		return 0;
+	}
+	virtual void	SetDeformer(class RAS_Deformer* deformer)
+	{
+
+	}
 
 	/**
 	 * @section Coordinate system manipulation functions
diff --git a/source/gameengine/Ketsji/KX_Scene.cpp b/source/gameengine/Ketsji/KX_Scene.cpp
index 04db96c298c..6eeb732c1e3 100644
--- a/source/gameengine/Ketsji/KX_Scene.cpp
+++ b/source/gameengine/Ketsji/KX_Scene.cpp
@@ -977,10 +977,10 @@ void KX_Scene::ReplaceMesh(class CValue* obj,void* meshobj)
 	{
 		BL_DeformableGameObject* newobj = static_cast<BL_DeformableGameObject*>( gameobj );
 		
-		if (newobj->m_pDeformer)
+		if (newobj->GetDeformer())
 		{
-			delete newobj->m_pDeformer;
-			newobj->m_pDeformer = NULL;
+			delete newobj->GetDeformer();
+			newobj->SetDeformer(NULL);
 		}
 
 		if (mesh->IsDeformed())
@@ -1030,7 +1030,7 @@ void KX_Scene::ReplaceMesh(class CValue* obj,void* meshobj)
 						NULL
 					);
 				}
-				newobj->m_pDeformer = shapeDeformer;
+				newobj->SetDeformer( shapeDeformer);
 			}
 			else if (bHasArmature) 
 			{
@@ -1042,14 +1042,14 @@ void KX_Scene::ReplaceMesh(class CValue* obj,void* meshobj)
 					static_cast<BL_ArmatureObject*>( parentobj )
 				);
 				releaseParent= false;
-				newobj->m_pDeformer = skinDeformer;
+				newobj->SetDeformer(skinDeformer);
 			}
 			else if (bHasDvert)
 			{
 				BL_MeshDeformer* meshdeformer = new BL_MeshDeformer(
 					newobj, oldblendobj, static_cast<BL_SkinMeshObject*>(mesh)
 				);
-				newobj->m_pDeformer = meshdeformer;
+				newobj->SetDeformer(meshdeformer);
 			}
 
 			// release parent reference if its not being used 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 41ae5e38309..ad79db16dc4 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -20,7 +20,11 @@ subject to the following restrictions:
 #include "CcdPhysicsEnvironment.h"
 #include "RAS_MeshObject.h"
 #include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody//btSoftBodyInternals.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "LinearMath/btConvexHull.h"
 
+#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
 
 class BP_Proxy;
 
@@ -135,11 +139,7 @@ btSoftBody* CcdPhysicsController::GetSoftBody()
 
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 
-btVector3 pts[3] = {btVector3(0,0,0),
-btVector3(0,1,0),
-btVector3(1,1,0)};
-int triangles[3] = {0,1,2};
-btSoftBodyWorldInfo sbi;
+
 
 void CcdPhysicsController::CreateRigidbody()
 {
@@ -151,8 +151,13 @@ void CcdPhysicsController::CreateRigidbody()
 
 	//create a collision object
 
+	int shapeType = m_cci.m_collisionShape ? m_cci.m_collisionShape->getShapeType() : 0;
+
 	//disable soft body until first sneak preview is ready
-	if (0)//m_cci.m_bSoft)
+	if (m_cci.m_bSoft && m_cci.m_collisionShape && 
+		(shapeType == CONVEX_HULL_SHAPE_PROXYTYPE))
+		//(shapeType == TRIANGLE_MESH_SHAPE_PROXYTYPE) |
+		//(shapeType == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)))
 	{
 		btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
 		rbci.m_linearDamping = m_cci.m_linearDamping;
@@ -161,54 +166,161 @@ void CcdPhysicsController::CreateRigidbody()
 		rbci.m_restitution = m_cci.m_restitution;
 
 		
-		sbi.m_broadphase = this->m_cci.m_physicsEnv->getBroadphase();
-		sbi.m_dispatcher = (btCollisionDispatcher*) m_cci.m_physicsEnv->getDispatcher();
-		
 		int nodecount = 0;
 		
-		
 		int numtriangles = 1;
 		
-		btVector3 p = trans.getOrigin();
+		btVector3 p(0,0,0);// = getOrigin();
 		btScalar h = 1.f;
 		
+		btSoftRigidDynamicsWorld* softDynaWorld = (btSoftRigidDynamicsWorld*)m_cci.m_physicsEnv->getDynamicsWorld();
+
 		PHY__Vector3	grav;
-		m_cci.m_physicsEnv->getGravity(grav);
-		sbi.m_gravity.setValue(grav[0],grav[1],grav[2]);
-
-		const btVector3	c[]={	p+h*btVector3(-1,-1,-1),
-		p+h*btVector3(+1,-1,-1),
-		p+h*btVector3(-1,+1,-1),
-		p+h*btVector3(+1,+1,-1),
-		p+h*btVector3(-1,-1,+1),
-		p+h*btVector3(+1,-1,+1),
-		p+h*btVector3(-1,+1,+1),
-		p+h*btVector3(+1,+1,+1)};
-
-		int i=0;
-		const int n=15;
+		grav[0] = softDynaWorld->getGravity().getX();
+		grav[1] = softDynaWorld->getGravity().getY();
+		grav[2] = softDynaWorld->getGravity().getZ();
+		softDynaWorld->getWorldInfo().m_gravity.setValue(grav[0],grav[1],grav[2]); //??
+
+	
 		//btSoftBody*	psb=btSoftBodyHelpers::CreateRope(sbi,	btVector3(-10,0,i*0.25),btVector3(10,0,i*0.25),	16,1+2);
-		btSoftBody* psb = btSoftBodyHelpers::CreateFromConvexHull(sbi,c,8);
 
-		m_object = psb;//btSoftBodyHelpers::CreateFromTriMesh(sbi,&pts[0].getX(),triangles,numtriangles);
+		btSoftBody* psb  = 0;
+
+		if (m_cci.m_collisionShape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE)
+		{
+			btConvexHullShape* convexHull = (btConvexHullShape* )m_cci.m_collisionShape;
+			btAlignedObjectArray<btVector3> transformedVertices;
+			transformedVertices.resize(convexHull->getNumPoints());
+			for (int i=0;i<convexHull->getNumPoints();i++)
+			{
+				transformedVertices[i] = trans(convexHull->getPoints()[i]);
+			}
+
+			//psb = btSoftBodyHelpers::CreateFromConvexHull(sbi,&transformedVertices[0],convexHull->getNumPoints());
+
+			{
+				int nvertices = convexHull->getNumPoints();
+				const btVector3* vertices = &transformedVertices[0];
+				btSoftBodyWorldInfo& worldInfo = softDynaWorld->getWorldInfo();
+
+				HullDesc		hdsc(QF_TRIANGLES,nvertices,vertices);
+				HullResult		hres;
+				HullLibrary		hlib;/*??*/ 
+				hdsc.mMaxVertices=nvertices;
+				hlib.CreateConvexHull(hdsc,hres);
+				
+				psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
+					&hres.m_OutputVertices[0],0);
+				for(int i=0;i<(int)hres.mNumFaces;++i)
+				{
+					const int idx[]={	hres.m_Indices[i*3+0],
+						hres.m_Indices[i*3+1],
+						hres.m_Indices[i*3+2]};
+					if(idx[0]<idx[1]) psb->appendLink(	idx[0],idx[1]);
+					if(idx[1]<idx[2]) psb->appendLink(	idx[1],idx[2]);
+					if(idx[2]<idx[0]) psb->appendLink(	idx[2],idx[0]);
+					psb->appendFace(idx[0],idx[1],idx[2]);
+				}
+				
+				{
+					for (int i=0;i<hlib.m_vertexIndexMapping.size();i++)
+						psb->m_userIndexMapping.push_back(hlib.m_vertexIndexMapping[i]);
+						//psb->m_userIndexMapping.push_back(hres.m_Indices[i]);
+				}
+
+				hlib.ReleaseResult(hres);
+				psb->randomizeConstraints();
+				
+			}
+
+
 
-		psb->m_cfg.collisions	=	btSoftBody::fCollision::SDF_RS;//btSoftBody::fCollision::CL_SS+	btSoftBody::fCollision::CL_RS;
 
-		sbi.m_sparsesdf.Reset();
-		sbi.m_sparsesdf.Initialize();
 
-		psb->generateBendingConstraints(2);
 
-		psb->m_cfg.kDF=1;
-		psb->activate();
-		psb->setActivationState(1);
-		psb->setDeactivationTime(1.f);
-		psb->m_cfg.piterations		=	4;
+		} else
+		{
+			/*
+			if (m_cci.m_collisionShape->getShapeType() ==SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
+			{
+				btScaledBvhTriangleMeshShape* scaledtrimeshshape = (btScaledBvhTriangleMeshShape*) m_cci.m_collisionShape;
+				btBvhTriangleMeshShape* trimeshshape = scaledtrimeshshape->getChildShape();
+
+				///only deal with meshes that have 1 sub part/component, for now
+				if (trimeshshape->getMeshInterface()->getNumSubParts()==1)
+				{
+					unsigned char* vertexBase;
+					PHY_ScalarType vertexType;
+					int numverts;
+					int vertexstride;
+					unsigned char* indexbase;
+					int indexstride;
+					int numtris;
+					PHY_ScalarType indexType;
+					trimeshshape->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numverts,vertexType,vertexstride,&indexbase,indexstride,numtris,indexType);
+					
+					psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,(const btScalar*)vertexBase,(const int*)indexbase,numtris);
+				}
+			} else
+			{
+				btBvhTriangleMeshShape* trimeshshape = (btBvhTriangleMeshShape*) m_cci.m_collisionShape;
+				///only deal with meshes that have 1 sub part/component, for now
+				if (trimeshshape->getMeshInterface()->getNumSubParts()==1)
+				{
+					unsigned char* vertexBase;
+					PHY_ScalarType vertexType;
+					int numverts;
+					int vertexstride;
+					unsigned char* indexbase;
+					int indexstride;
+					int numtris;
+					PHY_ScalarType indexType;
+					trimeshshape->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numverts,vertexType,vertexstride,&indexbase,indexstride,numtris,indexType);
+					
+					psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,(const btScalar*)vertexBase,(const int*)indexbase,numtris);
+				}
+			
+
+				//psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,&pts[0].getX(),triangles,numtriangles);
+			}
+			*/
+
+		}
+		
+		
+		m_object = psb;
+
+		//psb->m_cfg.collisions	=	btSoftBody::fCollision::SDF_RS;//btSoftBody::fCollision::CL_SS+	btSoftBody::fCollision::CL_RS;
+		psb->m_cfg.collisions	=	btSoftBody::fCollision::SDF_RS + btSoftBody::fCollision::CL_SS;
+		
+		//btSoftBody::Material*	pm=psb->appendMaterial();
+		btSoftBody::Material*	pm=psb->m_materials[0];
+		pm->m_kLST				=	0.1f;
+		//pm->m_kAST = 0.01f;
+		//pm->m_kVST = 0.001f;
+		psb->generateBendingConstraints(2,pm);
+/*
+		psb->m_cfg.kDF = 0.1f;//1.f;
+		psb->m_cfg.kDP		=	0.0001;
+		//psb->m_cfg.kDP		=	0.005;
+		psb->m_cfg.kCHR		=	0.1;
+		//psb->m_cfg.kVCF = 0.1f;
+		psb->m_cfg.kVCF = 0.0001f;
+		//psb->m_cfg.kAHR = 0.1f;
+		psb->m_cfg.kAHR = 0.0001f;
+		psb->m_cfg.kMT = 0.1f;
+		//psb->m_cfg.kDF=1;
+		*/
+
+//		psb->activate();
+//		psb->setActivationState(1);
+//		psb->setDeactivationTime(1.f);
+		//psb->m_cfg.piterations		=	4;
 		//psb->m_materials[0]->m_kLST	=	0.1+(i/(btScalar)(n-1))*0.9;
-		psb->setTotalMass(20);
+		psb->setTotalMass(m_cci.m_mass);
+		psb->generateClusters(64);		
 		psb->setCollisionFlags(0);
-
-		m_object->setCollisionShape(rbci.m_collisionShape);
+//		m_object->setCollisionShape(rbci.m_collisionShape);
 		btTransform startTrans;
 
 		if (rbci.m_motionState)
@@ -218,8 +330,13 @@ void CcdPhysicsController::CreateRigidbody()
 		{
 			startTrans = rbci.m_startWorldTransform;
 		}
+		startTrans.setIdentity();
+
 		m_object->setWorldTransform(startTrans);
 		m_object->setInterpolationWorldTransform(startTrans);
+		m_MotionState->setWorldPosition(0,0,0);
+		m_MotionState->setWorldOrientation(0,0,0,1);
+
 
 	} else
 	{
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index 96aa9ad164e..8c5aae3f8c9 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -452,6 +452,8 @@ void	CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 
 }
 
+		
+
 void	CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
 {
 	//also remove constraint
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index 9b6a76d49ee..56a639509c5 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -216,7 +216,10 @@ protected:
 	
 		void	SyncMotionStates(float timeStep);
 
-		
+		class	btSoftRigidDynamicsWorld*	getDynamicsWorld()
+		{
+			return m_dynamicsWorld;
+		}
 	
 		class btConstraintSolver*	GetConstraintSolver();