MotionState also ferries velocity

libraries/entities/src/EntityMotionState.cpp

@@ -44,12 +44,14 @@ EntityMotionState::~EntityMotionState() {
 //     it is an opportunity for outside code to update the object's simulation position
 void EntityMotionState::getWorldTransform (btTransform &worldTrans) const {
     btVector3 pos;
-    glmToBullet(_entity->getPosition() - _cachedWorldOffset, pos);
+    glmToBullet(_entity->getPositionInMeters() - _cachedWorldOffset, pos);
     worldTrans.setOrigin(pos);
 
     btQuaternion rot;
     glmToBullet(_entity->getRotation(), rot);
     worldTrans.setRotation(rot);
+
+    applyVelocities();
 }
 
 // This callback is invoked by the physics simulation at the end of each simulation frame...
@@ -62,6 +64,19 @@ void EntityMotionState::setWorldTransform (const btTransform &worldTrans) {
     glm::quat rot;
     bulletToGLM(worldTrans.getRotation(), rot);
     _entity->setRotation(rot);
+
+    glm::vec3 v;
+    getVelocity(v);
+    _entity->setVelocityInMeters(v);
+    getAngularVelocity(v);
+    _entity->setAngularVelocity(v);
+}
+
+void EntityMotionState::applyVelocities() const {
+    if (_body) {
+        setVelocity(_entity->getVelocityInMeters());
+        setAngularVelocity(_entity->getAngularVelocity());
+    }
 }
 
 void EntityMotionState::computeShapeInfo(ShapeInfo& info) {

libraries/entities/src/EntityMotionState.h

@@ -14,8 +14,11 @@
 
 #ifdef USE_BULLET_PHYSICS
 
+#include <AACube.h>
 #include <CustomMotionState.h>
 
+class EntityItem;
+
 class EntityMotionState : public CustomMotionState {
 public:
     static void setWorldOffset(const glm::vec3& offset);
@@ -24,10 +27,11 @@ public:
     EntityMotionState(EntityItem* item);
     virtual ~EntityMotionState();
 
-    virtual void getWorldTransform (btTransform &worldTrans) const;
+    void getWorldTransform (btTransform &worldTrans) const;
-    virtual void setWorldTransform (const btTransform &worldTrans);
+    void setWorldTransform (const btTransform &worldTrans);
+    void applyVelocities() const;
 
-    virtual void computeShapeInfo(ShapeInfo& info);
+    void computeShapeInfo(ShapeInfo& info);
 
     void getBoundingCubes(AACube& oldCube, AACube& newCube);
 

libraries/physics/src/CustomMotionState.cpp

@@ -13,6 +13,7 @@
 
 #include <math.h>
 
+#include "BulletUtil.h"
 #include "CustomMotionState.h"
 
 const float MIN_DENSITY = 200.0f;
@@ -58,4 +59,24 @@ void CustomMotionState::setVolume(float volume) {
     _volume = btMax(btMin(fabsf(volume), MAX_VOLUME), MIN_VOLUME);
 }
 
+void CustomMotionState::setVelocity(const glm::vec3& velocity) const {
+    btVector3 v;
+    glmToBullet(velocity, v);
+    _body->setLinearVelocity(v);
+}
+
+void CustomMotionState::setAngularVelocity(const glm::vec3& velocity) const {
+    btVector3 v;
+    glmToBullet(velocity, v);
+    _body->setAngularVelocity(v);
+}
+
+void CustomMotionState::getVelocity(glm::vec3& velocityOut) const {
+    bulletToGLM(_body->getLinearVelocity(), velocityOut);
+}
+
+void CustomMotionState::getAngularVelocity(glm::vec3& angularVelocityOut) const {
+    bulletToGLM(_body->getAngularVelocity(), angularVelocityOut);
+}
+
 #endif // USE_BULLET_PHYSICS

libraries/physics/src/CustomMotionState.h

@@ -15,6 +15,7 @@
 #ifdef USE_BULLET_PHYSICS
 
 #include <btBulletDynamicsCommon.h>
+#include <glm/glm.hpp>
 
 #include "ShapeInfo.h"
 
@@ -33,6 +34,8 @@ public:
     //virtual void getWorldTransform (btTransform &worldTrans) const;
     //virtual void setWorldTransform (const btTransform &worldTrans);
 
+    virtual void applyVelocities() const = 0;
+
     virtual void computeShapeInfo(ShapeInfo& info) = 0;
 
     MotionType getMotionType() const { return _motionType; }
@@ -44,6 +47,12 @@ public:
 
     float getMass() const { return _volume * _density; }
 
+    void setVelocity(const glm::vec3& velocity) const;
+    void setAngularVelocity(const glm::vec3& velocity) const;
+
+    void getVelocity(glm::vec3& velocityOut) const;
+    void getAngularVelocity(glm::vec3& angularVelocityOut) const;
+
     friend class PhysicsWorld;
 protected:
 

libraries/physics/src/PhysicsWorld.cpp

@@ -104,8 +104,10 @@ bool PhysicsWorld::addEntity(CustomMotionState* motionState) {
         shape->calculateLocalInertia(mass, inertia);
         btRigidBody* body = new btRigidBody(mass, motionState, shape, inertia);
         motionState->_body = body;
+        motionState->applyVelocities();
         // TODO: set dynamic/kinematic/static property from data stored in motionState
         _dynamicsWorld->addRigidBody(body);
+        return true;
     }
     return false;
 }
@@ -121,6 +123,7 @@ bool PhysicsWorld::removeEntity(CustomMotionState* motionState) {
         _shapeManager.releaseShape(info);
         delete body;
         motionState->_body = NULL;
+        return true;
     }
     return false;
 }