From 35977fbb0fe3af91c7227b0872dc104fdaa0f21a Mon Sep 17 00:00:00 2001 From: samcake Date: Tue, 1 Jul 2014 17:35:06 -0700 Subject: [PATCH] After HIFI visit Trying to improve the orientation coming from the leap Shwo and tell to team of the current LeapMotion integration --- examples/leapOfFaith.js | 21 ++++++++++++-------- interface/src/devices/Leapmotion.cpp | 26 +++++++++++++++++++------ interface/src/devices/MotionTracker.cpp | 22 ++++++++++++--------- interface/src/devices/MotionTracker.h | 2 +- 4 files changed, 47 insertions(+), 24 deletions(-) diff --git a/examples/leapOfFaith.js b/examples/leapOfFaith.js index 6cd1a261eb..0ae7c46ccb 100644 --- a/examples/leapOfFaith.js +++ b/examples/leapOfFaith.js @@ -28,13 +28,14 @@ function quatToString( q ) { } function printSpatialEvent( label, spatialEvent ) { - /* var dataString = label + " " + - vec3ToString( spatialEvent.locTranslation ) + " " + - quatToString( spatialEvent.locRotation ) + " " + - vec3ToString( spatialEvent.absTranslation ) + " " + - quatToString( spatialEvent.absRotation ); - // print( dataString ) - */; + if ( label == "RightHand" ) { + var dataString = label + " " + + /*vec3ToString( spatialEvent.locTranslation ) + " " + + quatToString( spatialEvent.locRotation ) + " " +*/ + vec3ToString( spatialEvent.absTranslation ) + " " + + quatToString( spatialEvent.absRotation ); + print( dataString ); + } } function avatarToWorld( apos ) { @@ -141,7 +142,7 @@ function onSpatialEventHandler( jointName, look ) { var _look = look; return (function( spatialEvent ) { MyAvatar.setJointData(_jointName, spatialEvent.absRotation); - updateJointParticle(_jointName, avatarToWorld( spatialEvent.absTranslation ), _look ); + updateJointParticle(_jointName, avatarToWorld( spatialEvent.absTranslation ), _look ); printSpatialEvent(_jointName, spatialEvent ); }); } @@ -174,6 +175,10 @@ Script.update.connect(function(deltaTime) { } } } + + // simple test + // MyAvatar.setJointData("LeftArm", jointControllers[6].c.getLocRotation()); + // MyAvatar.setJointData("LeftForeArm", jointControllers[7].c.getLocRotation()); }); diff --git a/interface/src/devices/Leapmotion.cpp b/interface/src/devices/Leapmotion.cpp index 267b4c5602..6893a23c0d 100644 --- a/interface/src/devices/Leapmotion.cpp +++ b/interface/src/devices/Leapmotion.cpp @@ -249,14 +249,26 @@ void Leapmotion::init() { glm::quat quatFromLeapBase( float sideSign, const Leap::Matrix& basis ) { glm::vec3 xAxis = glm::normalize( sideSign * glm::vec3( basis.xBasis.x, basis.xBasis.y, basis.xBasis.z) ); - glm::vec3 yAxis = glm::normalize( glm::vec3( basis.yBasis.x, basis.yBasis.y, basis.yBasis.z) ); - glm::vec3 zAxis = glm::normalize( glm::vec3( basis.zBasis.x, basis.zBasis.y, basis.zBasis.z) ); - - glm::quat orientation = /* glm::inverse*/ (glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis))); - + glm::vec3 yAxis = glm::normalize( sideSign * glm::vec3( basis.yBasis.x, basis.yBasis.y, basis.yBasis.z) ); + glm::vec3 zAxis = glm::normalize( sideSign * glm::vec3( basis.zBasis.x, basis.zBasis.y, basis.zBasis.z) ); + zAxis = glm::normalize( glm::cross( xAxis, yAxis ) ); + yAxis = glm::normalize( glm::cross( zAxis, xAxis ) ); + glm::quat orientation = /*glm::inverse*/(glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis))); + // orientation = glm::normalize( orientation ); + return glm::quat(); return orientation; } +glm::quat quatFromLeapBase( float sideSign, const Leap::Vector& dir, const Leap::Vector& normal ) { + glm::vec3 xAxis = glm::normalize( sideSign * glm::vec3( dir.x, dir.y, dir.z) ); + glm::vec3 yAxis = glm::normalize( -glm::vec3( normal.x, normal.y, normal.z) ); + glm::vec3 zAxis = glm::normalize( glm::cross( xAxis, yAxis ) ); + // yAxis = glm::normalize( glm::cross( zAxis, xAxis ) ); + glm::quat orientation = /*glm::inverse*/(glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis))); + // orientation = glm::normalize( orientation ); + // return glm::quat(); + return orientation; +} glm::vec3 vec3FromLeapVector( const Leap::Vector& vec ) { return glm::vec3( vec.x * METERS_PER_MILLIMETER, vec.y * METERS_PER_MILLIMETER, vec.z * METERS_PER_MILLIMETER ); } @@ -295,7 +307,8 @@ void Leapmotion::update() { Index handIndex = 1 + ((1 - side)/2) * HAND_NUM_JOINTS; glm::vec3 pos = vec3FromLeapVector(hand.palmPosition()); - glm::quat ori = quatFromLeapBase(float(side), hand.basis() ); + // glm::quat ori = quatFromLeapBase(float(side), hand.basis() ); + glm::quat ori = quatFromLeapBase(float(side), hand.direction(), hand.palmNormal() ); JointTracker* palmJoint = editJointTracker( handIndex ); palmJoint->editLocFrame().setTranslation( pos ); @@ -332,6 +345,7 @@ void Leapmotion::update() { ljointTracker->editAbsFrame().setTranslation( vec3FromLeapVector( bp ) ); ljointTracker->editAbsFrame().setRotation(quatFromLeapBase( float(side), bone.basis() ) ); + // ljointTracker->editAbsFrame().setRotation(quatFromLeapBase( float(side), bone.direction(), bone.basis() ) ); ljointTracker->updateLocFromAbsTransform( parentJointTracker ); ljointTracker->activeFrame(); } diff --git a/interface/src/devices/MotionTracker.cpp b/interface/src/devices/MotionTracker.cpp index 2e60d5c175..b207c1d640 100644 --- a/interface/src/devices/MotionTracker.cpp +++ b/interface/src/devices/MotionTracker.cpp @@ -150,7 +150,8 @@ MotionTracker::JointTracker::JointTracker( const JointTracker& tracker ) : void MotionTracker::JointTracker::updateAbsFromLocTransform(const JointTracker* parentJoint) { if ( parentJoint ) { - editAbsFrame()._transform = glm::mult( parentJoint->getAbsFrame()._transform, getLocFrame()._transform ); + //editAbsFrame()._transform = glm::mult( parentJoint->getAbsFrame()._transform, getLocFrame()._transform ); + editAbsFrame()._transform = ( parentJoint->getAbsFrame()._transform * getLocFrame()._transform ); } else { editAbsFrame()._transform = getLocFrame()._transform; } @@ -159,8 +160,10 @@ void MotionTracker::JointTracker::updateAbsFromLocTransform(const JointTracker* void MotionTracker::JointTracker::updateLocFromAbsTransform(const JointTracker* parentJoint) { if ( parentJoint ) { - glm::mat4 ip = glm::inverse( glm::mat4( parentJoint->getAbsFrame()._transform ) ); - editLocFrame()._transform = glm::mult( ip, getAbsFrame()._transform ); + // glm::mat4 ip = glm::inverse( glm::mat4( parentJoint->getAbsFrame()._transform ) ); + glm::mat4 ip = glm::inverse( parentJoint->getAbsFrame()._transform ); + // editLocFrame()._transform = glm::mult( ip, getAbsFrame()._transform ); + editLocFrame()._transform = ( ip * getAbsFrame()._transform ); } else { editLocFrame()._transform = getAbsFrame()._transform; } @@ -178,23 +181,24 @@ MotionTracker::Frame::Frame() : void MotionTracker::Frame::setRotation( const glm::quat& rotation ) { glm::mat3x3 rot = glm::mat3_cast( rotation ); - _transform[0] = rot[0]; - _transform[1] = rot[1]; - _transform[2] = rot[2]; + _transform[0] = glm::vec4( rot[0], 0.f ); + _transform[1] = glm::vec4( rot[1], 0.f ); + _transform[2] = glm::vec4( rot[2], 0.f ); } void MotionTracker::Frame::getRotation( glm::quat& rotation ) const { - rotation = glm::quat_cast( glm::mat3( _transform[0], _transform[1], _transform[2] ) ); + // rotation = glm::quat_cast( glm::mat3( _transform[0], _transform[1], _transform[2] ) ); + rotation = glm::quat_cast( _transform ); } void MotionTracker::Frame::setTranslation( const glm::vec3& translation ) { - _transform[3] = translation; + _transform[3] = glm::vec4( translation, 1.f ); } void MotionTracker::Frame::getTranslation( glm::vec3& translation ) const { - translation = _transform[3]; + translation = glm::vec3( _transform[3] ); } diff --git a/interface/src/devices/MotionTracker.h b/interface/src/devices/MotionTracker.h index f9aa91677b..6029383838 100644 --- a/interface/src/devices/MotionTracker.h +++ b/interface/src/devices/MotionTracker.h @@ -30,7 +30,7 @@ public: public: Frame(); - glm::mat4x3 _transform; + glm::mat4 _transform; void setRotation( const glm::quat& rotation ); void getRotation( glm::quat& rotation ) const;