Merge pull request #3778 from samcake/temp1

Introduce the Transform class and use it in the gpu api and for rendering models

interface/src/Application.cpp

@@ -109,6 +109,7 @@ static unsigned STARFIELD_SEED = 1;
 
 static const int BANDWIDTH_METER_CLICK_MAX_DRAG_LENGTH = 6; // farther dragged clicks are ignored
 
+
 const qint64 MAXIMUM_CACHE_SIZE = 10737418240;  // 10GB
 
 static QTimer* idleTimer = NULL;
@@ -187,7 +188,8 @@ Application::Application(int& argc, char** argv, QElapsedTimer &startup_time) :
         _lastNackTime(usecTimestampNow()),
         _lastSendDownstreamAudioStats(usecTimestampNow()),
         _isVSyncOn(true),
-        _aboutToQuit(false)
+        _aboutToQuit(false),
+        _viewTransform(new gpu::Transform())
 {
 
     // read the ApplicationInfo.ini file for Name/Version/Domain information
@@ -836,12 +838,14 @@ void Application::controlledBroadcastToNodes(const QByteArray& packet, const Nod
 }
 
 bool Application::event(QEvent* event) {
+
     // handle custom URL
     if (event->type() == QEvent::FileOpen) {
         
         QFileOpenEvent* fileEvent = static_cast<QFileOpenEvent*>(event);
-        if (fileEvent->url().isValid()) {
+        
-            openUrl(fileEvent->url());
+        if (!fileEvent->url().isEmpty()) {
+            AddressManager::getInstance().handleLookupString(fileEvent->url().toLocalFile());
         }
         
         return false;
@@ -2795,6 +2799,8 @@ void Application::updateShadowMap() {
 
         // store view matrix without translation, which we'll use for precision-sensitive objects
         updateUntranslatedViewMatrix();
+        // TODO: assign an equivalent viewTransform object to the application to match the current path which uses glMatrixStack 
+        // setViewTransform(viewTransform);
 
         glEnable(GL_POLYGON_OFFSET_FILL);
         glPolygonOffset(1.1f, 4.0f); // magic numbers courtesy http://www.eecs.berkeley.edu/~ravir/6160/papers/shadowmaps.ppt
@@ -2902,6 +2908,19 @@ void Application::displaySide(Camera& whichCamera, bool selfAvatarOnly) {
     // store view matrix without translation, which we'll use for precision-sensitive objects
     updateUntranslatedViewMatrix(-whichCamera.getPosition());
 
+    // Equivalent to what is happening with _untranslatedViewMatrix and the _viewMatrixTranslation
+    // the viewTransofmr object is updatded with the correct values and saved,
+    // this is what is used for rendering the Entities and avatars
+    gpu::Transform viewTransform;
+    viewTransform.setTranslation(whichCamera.getPosition());
+    viewTransform.setRotation(rotation);
+    viewTransform.postTranslate(eyeOffsetPos);
+    viewTransform.postRotate(eyeOffsetOrient);
+    if (whichCamera.getMode() == CAMERA_MODE_MIRROR) {
+         viewTransform.setScale(gpu::Transform::Vec3(-1.0f, 1.0f, 1.0f));
+    }
+    setViewTransform(viewTransform);
+
     glTranslatef(_viewMatrixTranslation.x, _viewMatrixTranslation.y, _viewMatrixTranslation.z);
 
     //  Setup 3D lights (after the camera transform, so that they are positioned in world space)
@@ -3019,13 +3038,16 @@ void Application::displaySide(Camera& whichCamera, bool selfAvatarOnly) {
         }
     }
 
+
+
     bool mirrorMode = (whichCamera.getMode() == CAMERA_MODE_MIRROR);
     {
         PerformanceTimer perfTimer("avatars");
         _avatarManager.renderAvatars(mirrorMode ? Avatar::MIRROR_RENDER_MODE : Avatar::NORMAL_RENDER_MODE,
             false, selfAvatarOnly);   
     }
-    
+
+
     {
         PROFILE_RANGE("DeferredLighting"); 
         PerformanceTimer perfTimer("lighting");
@@ -3084,7 +3106,7 @@ void Application::displaySide(Camera& whichCamera, bool selfAvatarOnly) {
             emit renderingInWorldInterface();
         }
     }
-    
+
     if (Menu::getInstance()->isOptionChecked(MenuOption::Wireframe)) {
         glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
     }
@@ -3095,6 +3117,10 @@ void Application::updateUntranslatedViewMatrix(const glm::vec3& viewMatrixTransl
     _viewMatrixTranslation = viewMatrixTranslation;
 }
 
+void Application::setViewTransform(const gpu::Transform& view) {
+    (*_viewTransform) = view;
+}
+
 void Application::loadTranslatedViewMatrix(const glm::vec3& translation) {
     glLoadMatrixf((const GLfloat*)&_untranslatedViewMatrix);
     glTranslatef(translation.x + _viewMatrixTranslation.x, translation.y + _viewMatrixTranslation.y,

interface/src/Application.h

@@ -232,6 +232,9 @@ public:
     const glm::vec3& getViewMatrixTranslation() const { return _viewMatrixTranslation; }
     void setViewMatrixTranslation(const glm::vec3& translation) { _viewMatrixTranslation = translation; }
 
+    const gpu::TransformPointer& getViewTransform() const { return _viewTransform; }
+    void setViewTransform(const gpu::Transform& view);
+
     /// if you need to access the application settings, use lockSettings()/unlockSettings()
     QSettings* lockSettings() { _settingsMutex.lock(); return _settings; }
     void unlockSettings() { _settingsMutex.unlock(); }
@@ -523,6 +526,7 @@ private:
     QRect _mirrorViewRect;
     RearMirrorTools* _rearMirrorTools;
 
+    gpu::TransformPointer _viewTransform;
     glm::mat4 _untranslatedViewMatrix;
     glm::vec3 _viewMatrixTranslation;
     glm::mat4 _projectionMatrix;

interface/src/gpu/Batch.cpp

@@ -21,7 +21,11 @@ Batch::Batch() :
     _commandOffsets(),
     _params(),
     _resources(),
-    _data(){
+    _data(),
+    _buffers(),
+    _streamFormats(),
+    _transforms()
+{
 }
 
 Batch::~Batch() {
@@ -32,8 +36,10 @@ void Batch::clear() {
     _commandOffsets.clear();
     _params.clear();
     _resources.clear();
-    _buffers.clear();
     _data.clear();
+    _buffers.clear();
+    _streamFormats.clear();
+    _transforms.clear();
 }
 
 uint32 Batch::cacheResource(Resource* res) {
@@ -128,3 +134,22 @@ void Batch::setIndexBuffer(Type type, const BufferPointer& buffer, Offset offset
     _params.push_back(_buffers.cache(buffer));
     _params.push_back(type);
 }
+
+void Batch::setModelTransform(const TransformPointer& model) {
+    ADD_COMMAND(setModelTransform);
+
+    _params.push_back(_transforms.cache(model));
+}
+
+void Batch::setViewTransform(const TransformPointer& view) {
+    ADD_COMMAND(setViewTransform);
+
+    _params.push_back(_transforms.cache(view));
+}
+
+void Batch::setProjectionTransform(const TransformPointer& proj) {
+    ADD_COMMAND(setProjectionTransform);
+
+    _params.push_back(_transforms.cache(proj));
+}
+

interface/src/gpu/Batch.h

@@ -14,10 +14,10 @@
 #include <assert.h>
 #include "InterfaceConfig.h"
 
+#include "Transform.h"
+
 #include <vector>
 
-#include "gpu/Format.h"
-#include "gpu/Resource.h"
 #include "gpu/Stream.h"
 
 #if defined(NSIGHT_FOUND)
@@ -50,6 +50,10 @@ enum Primitive {
     NUM_PRIMITIVES,
 };
 
+typedef ::Transform Transform;
+typedef QSharedPointer< ::gpu::Transform > TransformPointer;
+typedef std::vector< TransformPointer > Transforms;
+
 class Batch {
 public:
     typedef Stream::Slot Slot;
@@ -60,11 +64,16 @@ public:
 
     void clear();
 
+    // Drawcalls
     void draw(Primitive primitiveType, uint32 numVertices, uint32 startVertex = 0);
     void drawIndexed(Primitive primitiveType, uint32 nbIndices, uint32 startIndex = 0);
     void drawInstanced(uint32 nbInstances, Primitive primitiveType, uint32 nbVertices, uint32 startVertex = 0, uint32 startInstance = 0);
     void drawIndexedInstanced(uint32 nbInstances, Primitive primitiveType, uint32 nbIndices, uint32 startIndex = 0, uint32 startInstance = 0);
 
+    // Input Stage
+    // InputFormat
+    // InputBuffers
+    // IndexBuffer
     void setInputFormat(const Stream::FormatPointer& format);
 
     void setInputStream(Slot startChannel, const BufferStream& stream); // not a command, just unroll into a loop of setInputBuffer
@@ -72,6 +81,16 @@ public:
 
     void setIndexBuffer(Type type, const BufferPointer& buffer, Offset offset);
 
+    // Transform Stage
+    // Vertex position is transformed by ModelTransform from object space to world space
+    // Then by the inverse of the ViewTransform from world space to eye space
+    // finaly projected into the clip space by the projection transform
+    // WARNING: ViewTransform transform from eye space to world space, its inverse is composed
+    // with the ModelTransformu to create the equivalent of the glModelViewMatrix
+    void setModelTransform(const TransformPointer& model);
+    void setViewTransform(const TransformPointer& view);
+    void setProjectionTransform(const TransformPointer& proj);
+
 
     // TODO: As long as we have gl calls explicitely issued from interface
     // code, we need to be able to record and batch these calls. THe long 
@@ -138,11 +157,13 @@ public:
         COMMAND_drawIndexedInstanced,
 
         COMMAND_setInputFormat,
-
         COMMAND_setInputBuffer,
-
         COMMAND_setIndexBuffer,
 
+        COMMAND_setModelTransform,
+        COMMAND_setViewTransform,
+        COMMAND_setProjectionTransform,
+
         // TODO: As long as we have gl calls explicitely issued from interface
         // code, we need to be able to record and batch these calls. THe long 
         // term strategy is to get rid of any GL calls in favor of the HIFI GPU API
@@ -266,6 +287,7 @@ public:
     
     typedef Cache<Buffer>::Vector BufferCaches;
     typedef Cache<Stream::Format>::Vector StreamFormatCaches;
+    typedef Cache<Transform>::Vector TransformCaches;
 
     typedef unsigned char Byte;
     typedef std::vector<Byte> Bytes;
@@ -299,11 +321,12 @@ public:
     CommandOffsets _commandOffsets;
     Params _params;
     Resources _resources;
+    Bytes _data;
 
     BufferCaches _buffers;
     StreamFormatCaches _streamFormats;
+    TransformCaches _transforms;
 
-    Bytes _data;
 protected:
 };
 

interface/src/gpu/GLBackend.cpp

@@ -24,11 +24,13 @@ GLBackend::CommandCall GLBackend::_commandCalls[Batch::NUM_COMMANDS] =
     (&::gpu::GLBackend::do_drawIndexedInstanced),
 
     (&::gpu::GLBackend::do_setInputFormat),
-
     (&::gpu::GLBackend::do_setInputBuffer),
-
     (&::gpu::GLBackend::do_setIndexBuffer),
 
+    (&::gpu::GLBackend::do_setModelTransform),
+    (&::gpu::GLBackend::do_setViewTransform),
+    (&::gpu::GLBackend::do_setProjectionTransform),
+
     (&::gpu::GLBackend::do_glEnable),
     (&::gpu::GLBackend::do_glDisable),
 
@@ -111,18 +113,16 @@ static const GLenum _elementTypeToGLType[NUM_TYPES]= {
 
 
 GLBackend::GLBackend() :
-
     _needInputFormatUpdate(true),
     _inputFormat(0),
-
     _inputBuffersState(0),
     _inputBuffers(_inputBuffersState.size(), BufferPointer(0)),
     _inputBufferOffsets(_inputBuffersState.size(), 0),
     _inputBufferStrides(_inputBuffersState.size(), 0),
-
     _indexBuffer(0),
     _indexBufferOffset(0),
-    _inputAttributeActivation(0)
+    _inputAttributeActivation(0),
+    _transform()
 {
 
 }
@@ -183,6 +183,7 @@ void GLBackend::checkGLError() {
 
 void GLBackend::do_draw(Batch& batch, uint32 paramOffset) {
     updateInput();
+    updateTransform();
 
     Primitive primitiveType = (Primitive)batch._params[paramOffset + 2]._uint;
     GLenum mode = _primitiveToGLmode[primitiveType];
@@ -195,6 +196,7 @@ void GLBackend::do_draw(Batch& batch, uint32 paramOffset) {
 
 void GLBackend::do_drawIndexed(Batch& batch, uint32 paramOffset) {
     updateInput();
+    updateTransform();
 
     Primitive primitiveType = (Primitive)batch._params[paramOffset + 2]._uint;
     GLenum mode = _primitiveToGLmode[primitiveType];
@@ -425,6 +427,82 @@ void GLBackend::do_setIndexBuffer(Batch& batch, uint32 paramOffset) {
     CHECK_GL_ERROR();
 }
 
+// Transform Stage
+
+void GLBackend::do_setModelTransform(Batch& batch, uint32 paramOffset) {
+    TransformPointer modelTransform = batch._transforms.get(batch._params[paramOffset]._uint);
+
+    if (_transform._model.isNull() || (modelTransform != _transform._model)) {
+        _transform._model = modelTransform;
+        _transform._invalidModel = true;
+    }
+}
+
+void GLBackend::do_setViewTransform(Batch& batch, uint32 paramOffset) {
+    TransformPointer viewTransform = batch._transforms.get(batch._params[paramOffset]._uint);
+
+    if (_transform._view.isNull() || (viewTransform != _transform._view)) {
+        _transform._view = viewTransform;
+        _transform._invalidView = true;
+    }
+}
+
+void GLBackend::do_setProjectionTransform(Batch& batch, uint32 paramOffset) {
+    TransformPointer projectionTransform = batch._transforms.get(batch._params[paramOffset]._uint);
+
+    if (_transform._projection.isNull() || (projectionTransform != _transform._projection)) {
+        _transform._projection = projectionTransform;
+        _transform._invalidProj = true;
+    }
+}
+
+void GLBackend::updateTransform() {
+    if (_transform._invalidProj) {
+        // TODO: implement the projection matrix assignment to gl state
+    /*    if (_transform._lastMode != GL_PROJECTION) {
+            glMatrixMode(GL_PROJECTION);
+            _transform._lastMode = GL_PROJECTION;
+        }
+        CHECK_GL_ERROR();*/
+        _transform._invalidProj;
+    }
+
+    if (_transform._invalidModel || _transform._invalidView) {
+        if (!_transform._model.isNull()) {
+            if (_transform._lastMode != GL_MODELVIEW) {
+                glMatrixMode(GL_MODELVIEW);
+                _transform._lastMode = GL_MODELVIEW;
+            }
+            Transform::Mat4 modelView;
+            if (!_transform._view.isNull()) {
+                Transform mvx;
+                Transform::inverseMult(mvx, (*_transform._view), (*_transform._model));
+                mvx.getMatrix(modelView);
+            } else {
+                _transform._model->getMatrix(modelView);
+            }
+            glLoadMatrixf(reinterpret_cast< const GLfloat* >(&modelView));
+        } else {
+            if (!_transform._view.isNull()) {
+                if (_transform._lastMode != GL_MODELVIEW) {
+                    glMatrixMode(GL_MODELVIEW);
+                    _transform._lastMode = GL_MODELVIEW;
+                }
+                Transform::Mat4 modelView;
+                _transform._view->getInverseMatrix(modelView);
+                glLoadMatrixf(reinterpret_cast< const GLfloat* >(&modelView));
+            } else {
+                // TODO: eventually do something about the matrix when neither view nor model is specified?
+                // glLoadIdentity();
+            }
+        }
+        CHECK_GL_ERROR();
+
+        _transform._invalidModel = false;
+        _transform._invalidView = false;
+    }
+}
+
 // TODO: As long as we have gl calls explicitely issued from interface
 // code, we need to be able to record and batch these calls. THe long 
 // term strategy is to get rid of any GL calls in favor of the HIFI GPU API

interface/src/gpu/GLBackend.h

@@ -52,11 +52,22 @@ public:
 
 protected:
 
+    // Draw Stage
+    void do_draw(Batch& batch, uint32 paramOffset);
+    void do_drawIndexed(Batch& batch, uint32 paramOffset);
+    void do_drawInstanced(Batch& batch, uint32 paramOffset);
+    void do_drawIndexedInstanced(Batch& batch, uint32 paramOffset);
+
+    // Input Stage
+    void do_setInputFormat(Batch& batch, uint32 paramOffset);
+    void do_setInputBuffer(Batch& batch, uint32 paramOffset);
+    void do_setIndexBuffer(Batch& batch, uint32 paramOffset);
+    void updateInput();
     bool _needInputFormatUpdate;
     Stream::FormatPointer _inputFormat;
-
     typedef std::bitset<MAX_NUM_INPUT_BUFFERS> InputBuffersState;
     InputBuffersState _inputBuffersState;
+
     Buffers _inputBuffers;
     Offsets _inputBufferOffsets;
     Offsets _inputBufferStrides;
@@ -68,18 +79,31 @@ protected:
     typedef std::bitset<MAX_NUM_ATTRIBUTES> InputActivationCache;
     InputActivationCache _inputAttributeActivation;
 
-    void do_draw(Batch& batch, uint32 paramOffset);
+    // Transform Stage
-    void do_drawIndexed(Batch& batch, uint32 paramOffset);
+    void do_setModelTransform(Batch& batch, uint32 paramOffset);
-    void do_drawInstanced(Batch& batch, uint32 paramOffset);
+    void do_setViewTransform(Batch& batch, uint32 paramOffset);
-    void do_drawIndexedInstanced(Batch& batch, uint32 paramOffset);
+    void do_setProjectionTransform(Batch& batch, uint32 paramOffset);
 
-    void updateInput();
+    void updateTransform();
-    void do_setInputFormat(Batch& batch, uint32 paramOffset);
+    struct TransformStageState {
-
+        TransformPointer _model;
-    void do_setInputBuffer(Batch& batch, uint32 paramOffset);
+        TransformPointer _view;
+        TransformPointer _projection;
+        bool _invalidModel;
+        bool _invalidView;
+        bool _invalidProj;
 
-    void do_setVertexBuffer(Batch& batch, uint32 paramOffset);
+        GLenum _lastMode;
-    void do_setIndexBuffer(Batch& batch, uint32 paramOffset);
+
+        TransformStageState() :
+            _model(0),
+            _view(0),
+            _projection(0),
+            _invalidModel(true),
+            _invalidView(true),
+            _invalidProj(true),
+            _lastMode(GL_TEXTURE) {}
+    } _transform;
 
     // TODO: As long as we have gl calls explicitely issued from interface
     // code, we need to be able to record and batch these calls. THe long 

interface/src/renderer/Model.cpp

@@ -551,8 +551,19 @@ bool Model::render(float alpha, RenderMode mode, RenderArgs* args) {
     }
 
     // Let's introduce a gpu::Batch to capture all the calls to the graphics api
-    gpu::Batch batch;
+    _renderBatch.clear();
+    gpu::Batch& batch = _renderBatch;
+    GLBATCH(glPushMatrix)();
 
+    // Capture the view matrix once for the rendering of this model
+    if (_transforms.empty()) {
+        _transforms.push_back(gpu::TransformPointer(new gpu::Transform()));
+    }
+    (*_transforms[0]) = gpu::Transform((*Application::getInstance()->getViewTransform()));
+    // apply entity translation offset to the viewTransform  in one go (it's a preTranslate because viewTransform goes from world to eye space)
+    _transforms[0]->preTranslate(-_translation);
+
+    batch.setViewTransform(_transforms[0]);
 
     GLBATCH(glDisable)(GL_COLOR_MATERIAL);
     
@@ -677,11 +688,12 @@ bool Model::render(float alpha, RenderMode mode, RenderArgs* args) {
     GLBATCH(glBindBuffer)(GL_ELEMENT_ARRAY_BUFFER, 0);
     GLBATCH(glBindTexture)(GL_TEXTURE_2D, 0);
 
+    GLBATCH(glPopMatrix)();
+
     // Render!
     {
         PROFILE_RANGE("render Batch");
         ::gpu::GLBackend::renderBatch(batch);
-        batch.clear();
     }
 
     // restore all the default material settings
@@ -1849,20 +1861,17 @@ int Model::renderMeshes(gpu::Batch& batch, RenderMode mode, bool translucent, fl
         }
 
         GLBATCH(glPushMatrix)();
-        //Application::getInstance()->loadTranslatedViewMatrix(_translation);
-        GLBATCH(glLoadMatrixf)((const GLfloat*)&Application::getInstance()->getUntranslatedViewMatrix());
-        glm::vec3 viewMatTranslation = Application::getInstance()->getViewMatrixTranslation();
-        GLBATCH(glTranslatef)(_translation.x + viewMatTranslation.x, _translation.y + viewMatTranslation.y,
-            _translation.z + viewMatTranslation.z);
 
         const MeshState& state = _meshStates.at(i);
         if (state.clusterMatrices.size() > 1) {
             GLBATCH(glUniformMatrix4fv)(skinLocations->clusterMatrices, state.clusterMatrices.size(), false,
                 (const float*)state.clusterMatrices.constData());
+            batch.setModelTransform(gpu::TransformPointer());
         } else {
-            GLBATCH(glMultMatrixf)((const GLfloat*)&state.clusterMatrices[0]);
+            gpu::TransformPointer modelTransform(new gpu::Transform(state.clusterMatrices[0]));
+            batch.setModelTransform(modelTransform);
         }
-        
+
         if (mesh.blendshapes.isEmpty()) {
             batch.setInputFormat(networkMesh._vertexFormat);
             batch.setInputStream(0, *networkMesh._vertexStream);

interface/src/renderer/Model.h

@@ -16,6 +16,7 @@
 #include <QObject>
 #include <QUrl>
 
+#include "Transform.h"
 #include <AABox.h>
 #include <AnimationCache.h>
 #include <PhysicsEntity.h>
@@ -36,11 +37,9 @@ class ViewFrustum;
 typedef QSharedPointer<AnimationHandle> AnimationHandlePointer;
 typedef QWeakPointer<AnimationHandle> WeakAnimationHandlePointer;
 
-namespace gpu {
-    class Batch;
-}
 
 #include "gpu/Stream.h"
+#include "gpu/Batch.h"
 
 /// A generic 3D model displaying geometry loaded from a URL.
 class Model : public QObject, public PhysicsEntity {
@@ -283,7 +282,9 @@ private:
     QUrl _url;
 
     gpu::Buffers _blendedVertexBuffers;
-    
+    gpu::Transforms _transforms;
+    gpu::Batch  _renderBatch;
+
     QVector<QVector<QSharedPointer<Texture> > > _dilatedTextures;
     
     QVector<Model*> _attachments;

interface/src/ui/MetavoxelEditor.cpp

@@ -919,7 +919,9 @@ void SetSpannerTool::applyEdit(const AttributePointer& attribute, const SharedOb
         Application::getInstance()->setupWorldLight();
         
         Application::getInstance()->updateUntranslatedViewMatrix();
-        
+        // TODO: assign an equivalent viewTransform object to the application to match the current path which uses glMatrixStack 
+        // setViewTransform(viewTransform);
+
         const glm::vec4 OPAQUE_WHITE(1.0f, 1.0f, 1.0f, 1.0f);
         spannerData->getRenderer()->render(OPAQUE_WHITE, SpannerRenderer::DIFFUSE_MODE, glm::vec3(), 0.0f);
         

libraries/shared/src/Transform.cpp

@@ -0,0 +1,71 @@
+//
+//  Transform.cpp
+//  shared/src/gpu
+//
+//  Created by Sam Gateau on 11/4/2014.
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "Transform.h"
+
+
+void Transform::evalRotationScale(Quat& rotation, Vec3& scale, const Mat3& rotationScaleMatrix) {
+    const float ACCURACY_THREASHOLD = 0.00001f;
+
+    // Following technique taken from:
+    // http://callumhay.blogspot.com/2010/10/decomposing-affine-transforms.html
+    // Extract the rotation component - this is done using polar decompostion, where
+    // we successively average the matrix with its inverse transpose until there is
+    // no/a very small difference between successive averages
+    float norm;
+    int count = 0;
+    Mat3 rotationMat = rotationScaleMatrix;
+    do {
+        Mat3 currInvTranspose = glm::inverse(glm::transpose(rotationMat));
+        
+        Mat3 nextRotation = 0.5f * (rotationMat + currInvTranspose);
+
+        norm = 0.0;
+        for (int i = 0; i < 3; i++) {
+            float n = static_cast<float>(
+                         fabs(rotationMat[0][i] - nextRotation[0][i]) +
+                         fabs(rotationMat[1][i] - nextRotation[1][i]) +
+                         fabs(rotationMat[2][i] - nextRotation[2][i]));
+            norm = (norm > n ? norm : n);
+        }
+        rotationMat = nextRotation;
+    } while (count < 100 && norm > ACCURACY_THREASHOLD);
+
+
+    // extract scale of the matrix as the length of each axis
+    Mat3 scaleMat = glm::inverse(rotationMat) * rotationScaleMatrix;
+
+    scale = glm::max(Vec3(ACCURACY_THREASHOLD), Vec3(scaleMat[0][0], scaleMat[1][1], scaleMat[2][2]));
+
+    // Let's work on a local matrix containing rotation only
+    Mat3 matRot(
+        rotationScaleMatrix[0] / scale.x,
+        rotationScaleMatrix[1] / scale.y,
+        rotationScaleMatrix[2] / scale.z);
+
+    // Beware!!! needs to detect for the case there is a negative scale
+    // Based on the determinant sign we just can flip the scale sign of one component: we choose X axis
+    float determinant = glm::determinant(matRot);
+    if (determinant < 0.f) {
+        scale.x = -scale.x;
+        matRot[0] *= -1.f;
+    }
+
+    // Beware: even though the matRot is supposed to be normalized at that point,
+    // glm::quat_cast doesn't always return a normalized quaternion...
+   // rotation = glm::normalize(glm::quat_cast(matRot));
+    rotation = (glm::quat_cast(matRot));
+}
+
+
+
+
+ 

libraries/shared/src/Transform.h

@@ -0,0 +1,397 @@
+//
+//  Transform.h
+//  shared/src/gpu
+//
+//  Created by Sam Gateau on 11/4/2014.
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+#ifndef hifi_gpu_Transform_h
+#define hifi_gpu_Transform_h
+
+#include <assert.h>
+
+#include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+#include <glm/gtc/matrix_transform.hpp>
+#include <glm/gtx/quaternion.hpp>
+
+#include <bitset>
+
+class Transform {
+public:
+    typedef glm::mat4 Mat4;
+    typedef glm::mat3 Mat3;
+    typedef glm::vec4 Vec4;
+    typedef glm::vec3 Vec3;
+    typedef glm::vec2 Vec2;
+    typedef glm::quat Quat;
+
+    Transform() :
+        _translation(0),
+        _rotation(1.0f, 0, 0, 0),
+        _scale(1.0f),
+        _flags(FLAG_CACHE_INVALID_BITSET) // invalid cache
+    {
+    }
+    Transform(const Transform& transform) :
+        _translation(transform._translation),
+        _rotation(transform._rotation),
+        _scale(transform._scale),
+        _flags(transform._flags)
+    {
+        invalidCache();
+    }
+    Transform(const Mat4& raw) {
+        evalFromRawMatrix(raw);
+    }
+    ~Transform() {}
+
+    void setIdentity();
+
+    const Vec3& getTranslation() const;
+    void setTranslation(const Vec3& translation);
+    void preTranslate(const Vec3& translation);
+    void postTranslate(const Vec3& translation);
+
+    const Quat& getRotation() const;
+    void setRotation(const Quat& rotation);
+    void preRotate(const Quat& rotation);
+    void postRotate(const Quat& rotation);
+
+    const Vec3& getScale() const;
+    void setScale(float scale);
+    void setScale(const Vec3& scale);
+    void postScale(float scale);
+    void postScale(const Vec3& scale);
+
+    bool isIdentity() const { return (_flags & ~Flags(FLAG_CACHE_INVALID_BITSET)).none(); }
+    bool isTranslating() const { return _flags[FLAG_TRANSLATION]; }
+    bool isRotating() const { return _flags[FLAG_ROTATION]; }
+    bool isScaling() const { return _flags[FLAG_SCALING]; }
+    bool isUniform() const { return !isNonUniform(); }
+    bool isNonUniform() const { return _flags[FLAG_NON_UNIFORM]; }
+
+    void evalFromRawMatrix(const Mat4& matrix);
+    void evalFromRawMatrix(const Mat3& rotationScalematrix);
+
+    Mat4& getMatrix(Mat4& result) const;
+    Mat4& getInverseMatrix(Mat4& result) const;
+
+    Transform& evalInverse(Transform& result) const;
+
+    static void evalRotationScale(Quat& rotation, Vec3& scale, const Mat3& rotationScaleMatrix);
+
+    static Transform& mult(Transform& result, const Transform& left, const Transform& right);
+
+    // Left will be inversed before the multiplication
+    static Transform& inverseMult(Transform& result, const Transform& left, const Transform& right);
+
+
+protected:
+
+    enum Flag {
+        FLAG_CACHE_INVALID = 0,
+
+        FLAG_TRANSLATION,
+        FLAG_ROTATION,
+        FLAG_SCALING,
+        FLAG_NON_UNIFORM,
+        FLAG_ZERO_SCALE,
+
+        FLAG_PROJECTION,
+
+        NUM_FLAGS,
+
+        FLAG_CACHE_INVALID_BITSET = 1,
+    };
+    typedef std::bitset<NUM_FLAGS> Flags;
+
+
+    // TRS
+    Vec3 _translation;
+    Quat _rotation;
+    Vec3 _scale;
+
+    mutable Flags _flags;
+
+    // Cached transform
+    mutable Mat4 _matrix;
+
+    bool isCacheInvalid() const { return _flags[FLAG_CACHE_INVALID]; }
+    void validCache() const { _flags.set(FLAG_CACHE_INVALID, false); }
+    void invalidCache() const { _flags.set(FLAG_CACHE_INVALID, true); }
+
+    void flagTranslation() { _flags.set(FLAG_TRANSLATION, true); }
+    void flagRotation() { _flags.set(FLAG_ROTATION, true); }
+
+    void flagScaling() { _flags.set(FLAG_SCALING, true); }
+    void unflagScaling() { _flags.set(FLAG_SCALING, false); }
+
+
+    void flagUniform() { _flags.set(FLAG_NON_UNIFORM, false); }
+    void flagNonUniform() { _flags.set(FLAG_NON_UNIFORM, true); }
+
+    void updateCache() const;
+};
+
+inline void Transform::setIdentity() {
+    _translation = Vec3(0);
+    _rotation = Quat(1.0f, 0, 0, 0);
+    _scale = Vec3(1.0f);
+    _flags = Flags(FLAG_CACHE_INVALID_BITSET);
+}
+
+inline const Transform::Vec3& Transform::getTranslation() const {
+    return _translation;
+}
+
+inline void Transform::setTranslation(const Vec3& translation) {
+    invalidCache();
+    flagTranslation();
+    _translation = translation;
+}
+
+inline void Transform::preTranslate(const Vec3& translation) {
+    invalidCache();
+    flagTranslation();
+    _translation += translation;
+}
+
+inline void Transform::postTranslate(const Vec3& translation) {
+    invalidCache();
+    flagTranslation();
+
+    Vec3 scaledT = translation;
+    if (isScaling()) scaledT *= _scale;
+
+    if (isRotating()) {
+        _translation += glm::rotate(_rotation, scaledT);
+    } else {
+        _translation += scaledT;
+    }
+}
+
+inline const Transform::Quat& Transform::getRotation() const {
+    return _rotation;
+}
+
+inline void Transform::setRotation(const Quat& rotation) {
+    invalidCache();
+    flagRotation();
+    _rotation = rotation;
+}
+
+inline void Transform::preRotate(const Quat& rotation) {
+    invalidCache();
+    if (isRotating()) {
+        _rotation = rotation * _rotation;
+    } else {
+        _rotation = rotation;
+    }
+    flagRotation();
+    _translation = glm::rotate(rotation, _translation);
+}
+
+inline void Transform::postRotate(const Quat& rotation) {
+    invalidCache();
+
+    if (isNonUniform()) {
+        Quat newRot;
+        Vec3 newScale;
+        Mat3 scaleRot(glm::mat3_cast(rotation));
+        scaleRot[0] *= _scale;
+        scaleRot[1] *= _scale;
+        scaleRot[2] *= _scale;
+        evalRotationScale(newRot, newScale, scaleRot);
+
+        if (isRotating()) {
+            _rotation *= newRot;
+        } else {
+            _rotation = newRot;
+        }
+        setScale(newScale);
+    } else {
+        if (isRotating()) {
+            _rotation *= rotation;
+        } else {
+            _rotation = rotation;
+        }
+    }
+    flagRotation();
+}
+
+inline const Transform::Vec3& Transform::getScale() const {
+    return _scale;
+}
+
+inline void Transform::setScale(float scale) {
+    invalidCache();
+    flagUniform();
+    if (scale == 1.0f) {
+        unflagScaling();
+    } else {
+        flagScaling();
+    }
+    _scale = Vec3(scale);
+}
+
+inline void Transform::setScale(const Vec3& scale) {
+    if ((scale.x == scale.y) && (scale.x == scale.z)) {
+        setScale(scale.x);
+    } else {
+        invalidCache();
+        flagScaling();
+        flagNonUniform();
+        _scale = scale;
+    }
+}
+
+inline void Transform::postScale(float scale) {
+    if (scale == 1.0f) return;
+    if (isScaling()) {
+        // if already scaling, just invalid cache and aply uniform scale
+        invalidCache();
+        _scale *= scale;
+    } else {
+        setScale(scale);
+    }
+}
+
+inline void Transform::postScale(const Vec3& scale) {
+    invalidCache();
+    if (isScaling()) {
+        _scale *= scale;
+    } else {
+        _scale = scale;
+    }
+    flagScaling();
+}
+
+inline Transform::Mat4& Transform::getMatrix(Transform::Mat4& result) const {
+    updateCache();
+    result = _matrix;
+    return result;
+}
+
+inline Transform::Mat4& Transform::getInverseMatrix(Transform::Mat4& result) const {
+    Transform inverse;
+    evalInverse(inverse);
+    return inverse.getMatrix(result);
+}
+
+inline void Transform::evalFromRawMatrix(const Mat4& matrix) {
+    // for now works only in the case of TRS transformation
+    if ((matrix[0][3] == 0) && (matrix[1][3] == 0) && (matrix[2][3] == 0) && (matrix[3][3] == 1.f)) {
+        setTranslation(Vec3(matrix[3]));
+        evalFromRawMatrix(Mat3(matrix));
+    }
+}
+
+inline void Transform::evalFromRawMatrix(const Mat3& rotationScaleMatrix) {
+    Quat rotation;
+    Vec3 scale;
+    evalRotationScale(rotation, scale, rotationScaleMatrix);
+    setRotation(rotation);
+    setScale(scale);
+}
+
+inline Transform& Transform::evalInverse(Transform& inverse) const {
+    inverse.setIdentity();
+    if (isScaling()) {
+        // TODO: At some point we will face the case when scale is 0 and so 1/0 will blow up...
+        // WHat should we do for this one?
+        assert(_scale.x != 0);
+        assert(_scale.y != 0);
+        assert(_scale.z != 0);
+        if (isNonUniform()) {
+            inverse.setScale(Vec3(1.0f/_scale.x, 1.0f/_scale.y, 1.0f/_scale.z));
+        } else {
+            inverse.setScale(1.0f/_scale.x);
+        }
+    }
+    if (isRotating()) {
+        inverse.postRotate(glm::conjugate(_rotation));
+    }
+    if (isTranslating()) {
+        inverse.postTranslate(-_translation);
+    }
+    return inverse;
+}
+
+inline Transform& Transform::mult( Transform& result, const Transform& left, const Transform& right) {
+    result = left;
+    if (right.isTranslating()) {
+        result.postTranslate(right.getTranslation());
+    }
+    if (right.isRotating()) {
+        result.postRotate(right.getRotation());
+    }
+    if (right.isScaling()) {
+        result.postScale(right.getScale());
+    }
+
+    // HACK: In case of an issue in the Transform multiplication results, to make sure this code is
+    // working properly uncomment the next 2 lines and compare the results, they should be the same...
+    // Transform::Mat4 mv = left.getMatrix() * right.getMatrix();
+    // Transform::Mat4 mv2 = result.getMatrix();
+
+    return result;
+}
+
+inline Transform& Transform::inverseMult( Transform& result, const Transform& left, const Transform& right) {
+    result.setIdentity();
+
+    if (left.isScaling()) {
+        const Vec3& s = left.getScale();
+        result.setScale(Vec3(1.0f / s.x, 1.0f / s.y, 1.0f / s.z));
+    }
+    if (left.isRotating()) {
+        result.postRotate(glm::conjugate(left.getRotation()));
+    }
+    if (left.isTranslating() || right.isTranslating()) {
+        result.postTranslate(right.getTranslation() - left.getTranslation());
+    }
+    if (right.isRotating()) {
+        result.postRotate(right.getRotation());
+    }
+    if (right.isScaling()) {
+        result.postScale(right.getScale());
+    }
+
+    // HACK: In case of an issue in the Transform multiplication results, to make sure this code is
+    // working properly uncomment the next 2 lines and compare the results, they should be the same...
+    // Transform::Mat4 mv = left.getMatrix() * right.getMatrix();
+    // Transform::Mat4 mv2 = result.getMatrix();
+
+    return result;
+}
+
+inline void Transform::updateCache() const {
+    if (isCacheInvalid()) {
+        if (isRotating()) {
+            Mat3 rot = glm::mat3_cast(_rotation);
+
+            if (isScaling()) {
+                rot[0] *= _scale.x;
+                rot[1] *= _scale.y;
+                rot[2] *= _scale.z;
+            }
+
+            _matrix[0] = Vec4(rot[0], 0.f);
+            _matrix[1] = Vec4(rot[1], 0.f);
+            _matrix[2] = Vec4(rot[2], 0.f);
+        } else {
+            _matrix[0] = Vec4(_scale.x, 0.f, 0.f, 0.f);
+            _matrix[1] = Vec4(0.f, _scale.y, 0.f, 0.f);
+            _matrix[2] = Vec4(0.f, 0.f, _scale.z, 0.f);
+        }
+
+        _matrix[3] = Vec4(_translation, 1.0f);
+        validCache();
+    }
+}
+
+#endif