From 2e3973dfdda9a5232c4daf4ac5b9ded5f7e6e571 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 21 May 2015 15:34:30 -0700
Subject: [PATCH] impl AvatarMotionState::set/getWorldTransform()

---
 interface/src/avatar/AvatarMotionState.cpp  | 29 ++++++++++++++-------
 interface/src/avatar/AvatarMotionState.h    |  3 ---
 libraries/physics/src/EntityMotionState.cpp |  1 -
 3 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/interface/src/avatar/AvatarMotionState.cpp b/interface/src/avatar/AvatarMotionState.cpp
index 592bc7685a..e63bcef0ae 100644
--- a/interface/src/avatar/AvatarMotionState.cpp
+++ b/interface/src/avatar/AvatarMotionState.cpp
@@ -11,6 +11,7 @@
 
 #include "Avatar.h"
 #include "AvatarMotionState.h"
+#include "BulletUtil.h"
 
 AvatarMotionState::AvatarMotionState(Avatar* avatar, btCollisionShape* shape) : ObjectMotionState(shape), _avatar(avatar) {
 }
@@ -19,14 +20,6 @@ AvatarMotionState::~AvatarMotionState() {
     _avatar = nullptr;
 }
 
-// virtual
-void AvatarMotionState::handleEasyChanges(uint32_t flags) {
-}
-
-// virtual
-void AvatarMotionState::handleHardAndEasyChanges(uint32_t flags, PhysicsEngine* engine) {
-}
-
 // virtual
 uint32_t AvatarMotionState::getAndClearIncomingDirtyFlags() {
     uint32_t dirtyFlags = 0;
@@ -58,12 +51,28 @@ bool AvatarMotionState::isMoving() const {
 
 // virtual
 void AvatarMotionState::getWorldTransform(btTransform& worldTrans) const {
-    // TODO: implement this
+    if (!_avatar) {
+        return;
+    }
+    worldTrans.setOrigin(glmToBullet(getObjectPosition()));
+    worldTrans.setRotation(glmToBullet(getObjectRotation()));
+    _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
+    _body->setAngularVelocity(glmToBullet(getObjectLinearVelocity()));
 }
 
 // virtual 
 void AvatarMotionState::setWorldTransform(const btTransform& worldTrans) {
-    // TODO: implement this
+    if (!_avatar) {
+        return;
+    }
+    // The PhysicsEngine does not move other Avatars.  
+    // Instead we enforce the current transform of the Avatar
+    btTransform newTransform;
+    newTransform.setOrigin(glmToBullet(getObjectPosition()));
+    newTransform.setRotation(glmToBullet(getObjectRotation()));
+    _body->setWorldTransform(newTransform);
+    _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
+    _body->setAngularVelocity(glmToBullet(getObjectLinearVelocity()));
 }
 
 // These pure virtual methods must be implemented for each MotionState type
diff --git a/interface/src/avatar/AvatarMotionState.h b/interface/src/avatar/AvatarMotionState.h
index 04318c6e88..8aa10542bc 100644
--- a/interface/src/avatar/AvatarMotionState.h
+++ b/interface/src/avatar/AvatarMotionState.h
@@ -23,9 +23,6 @@ public:
     AvatarMotionState(Avatar* avatar, btCollisionShape* shape);
     ~AvatarMotionState();
 
-    virtual void handleEasyChanges(uint32_t flags);
-    virtual void handleHardAndEasyChanges(uint32_t flags, PhysicsEngine* engine);
-
     virtual MotionType getMotionType() const { return _motionType; }
 
     virtual uint32_t getAndClearIncomingDirtyFlags();
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index 7ce6bdf682..6ad7d20b4e 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -88,7 +88,6 @@ void EntityMotionState::handleEasyChanges(uint32_t flags) {
     if ((flags & EntityItem::DIRTY_PHYSICS_ACTIVATION) && !_body->isActive()) {
         _body->activate();
     }
-
 }