Merge pull request #2171 from AlexJ95:ovis-add-entity-LookAt

Author: alalek

modules/ovis/include/opencv2/ovis.hpp

@@ -165,6 +165,16 @@ class CV_EXPORTS_W WindowScene {
     CV_WRAP virtual void setEntityPose(const String& name, InputArray tvec = noArray(),
                                        InputArray rot = noArray(), bool invert = false) = 0;
 
+	/**
+     * Retrieves the current pose of an entity
+	 * @param name entity name
+     * @param R 3x3 rotation matrix
+     * @param tvec translation vector
+     * @param invert return the inverted pose
+     */
+	CV_WRAP virtual void getEntityPose(const String& name, OutputArray R = noArray(), OutputArray tvec = noArray(),
+                                       bool invert = false) = 0;
+
     /**
      * get a list of available entity animations
      * @param name entity name
@@ -236,6 +246,15 @@ class CV_EXPORTS_W WindowScene {
      */
     CV_WRAP virtual void setCameraLookAt(const String& target, InputArray offset = noArray()) = 0;
 
+	/**
+     * convenience method to orient an entity to a specific entity.
+	 * If target is an empty string the entity looks at the given offset point
+	 * @param origin entity to make look at
+     * @param target name of target entity
+	 * @param offset offset from entity centre
+     */
+	CV_WRAP virtual void setEntityLookAt(const String& origin, const String& target, InputArray offset = noArray()) = 0;
+
     /**
      * Retrieves the current camera pose
      * @param R 3x3 rotation matrix

modules/ovis/src/ovis.cpp

@@ -579,6 +579,36 @@ class WindowSceneImpl : public WindowScene
         node.setPosition(t);
     }
 
+	void getEntityPose(const String& name ,OutputArray R, OutputArray tvec, bool invert) CV_OVERRIDE
+    {
+		SceneNode* node = sceneMgr->getEntity(name)->getParentSceneNode();
+        Matrix3 _R;
+        // toOGRE.Inverse() == toOGRE
+        (node->getOrientation()*toOGRE).ToRotationMatrix(_R);
+
+        if (invert)
+        {
+            _R = _R.Transpose();
+        }
+
+        if (tvec.needed())
+        {
+            Vector3 _tvec = node->getPosition();
+
+            if (invert)
+            {
+                _tvec = _R * -_tvec;
+            }
+
+            Mat_<Real>(3, 1, _tvec.ptr()).copyTo(tvec);
+        }
+
+        if (R.needed())
+        {
+            Mat_<Real>(3, 3, _R[0]).copyTo(R);
+        }
+    }
+
     void getEntityAnimations(const String& name, std::vector<String>& out) CV_OVERRIDE
     {
         SceneNode& node = _getSceneNode(sceneMgr, name);
@@ -848,6 +878,26 @@ class WindowSceneImpl : public WindowScene
 
         camNode->lookAt(tgt->_getDerivedPosition() + _offset, Ogre::Node::TS_WORLD);
     }
+
+	void setEntityLookAt(const String& origin, const String& target, InputArray offset) CV_OVERRIDE
+    {
+		SceneNode* orig = sceneMgr->getEntity(origin)->getParentSceneNode();
+
+		Vector3 _offset = Vector3::ZERO;
+
+        if (!offset.empty())
+        {
+            offset.copyTo(Mat_<Real>(3, 1, _offset.ptr()));
+        }
+
+		if(target.compare("") != 0){
+        SceneNode* tgt = sceneMgr->getEntity(target)->getParentSceneNode();
+		orig->lookAt(tgt->_getDerivedPosition() + _offset, Ogre::Node::TS_WORLD, Ogre::Vector3::UNIT_Z);
+		}else{
+			orig->lookAt(_offset, Ogre::Node::TS_WORLD, Ogre::Vector3::UNIT_Z);
+		}
+    }
+
 };
 
 CV_EXPORTS_W void addResourceLocation(const String& path)