573 lines
14 KiB
C++
573 lines
14 KiB
C++
/**
|
|
* $LicenseInfo:firstyear=2014&license=fsviewerlgpl$
|
|
* Phoenix Firestorm Viewer Source Code
|
|
* Copyright (C) 2014, Nicky Dasmijn
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation;
|
|
* version 2.1 of the License only.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*
|
|
* The Phoenix Firestorm Project, Inc., 1831 Oakwood Drive, Fairmont, Minnesota 56031-3225 USA
|
|
* http://www.firestormviewer.org
|
|
* $/LicenseInfo$
|
|
*/
|
|
|
|
#include "llviewerprecompiledheaders.h"
|
|
#include <leap-motion/Leap.h>
|
|
|
|
#include "fsleapmaniptool.h"
|
|
|
|
#include "llviewerwindow.h"
|
|
#include "llviewerobject.h"
|
|
#include "lldrawable.h"
|
|
#include "pipeline.h"
|
|
#include "llvoavatarself.h"
|
|
#include "llviewershadermgr.h"
|
|
#include "llselectmgr.h"
|
|
|
|
namespace nd
|
|
{
|
|
namespace leap
|
|
{
|
|
struct Finger
|
|
{
|
|
Finger( )
|
|
{
|
|
clear();
|
|
}
|
|
|
|
void clear()
|
|
{
|
|
memset( this, 0, sizeof( Finger ) );
|
|
}
|
|
|
|
bool isValid( ) const
|
|
{ return mTimestamp > 0; }
|
|
|
|
U64 mId;
|
|
U64 mTimestamp;
|
|
|
|
float mTip[3];
|
|
float mDir[3];
|
|
float mFromLast[3];
|
|
float mLenFromLast;
|
|
float mWidth;
|
|
float mLength;
|
|
U16 mPartner;
|
|
LLViewerObject *mSelected;
|
|
};
|
|
|
|
struct Fingers
|
|
{
|
|
Fingers( )
|
|
{
|
|
clear();
|
|
}
|
|
|
|
void clear()
|
|
{
|
|
mTimestamp = 0;
|
|
for( int i = 0; i < eMaxFingers; ++i )
|
|
mFingers[ i ].clear();
|
|
mStoredFingers = 0;
|
|
}
|
|
|
|
Finger* getFinger( U64 aId )
|
|
{
|
|
for( int i = 0; i < mStoredFingers; ++i )
|
|
{
|
|
if( aId == mFingers[ i ].mId )
|
|
return mFingers + i;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
Finger* at( U16 aIndex )
|
|
{
|
|
return &mFingers[ aIndex ];
|
|
}
|
|
|
|
enum MAXFIGERS { eMaxFingers = 10 };
|
|
|
|
U64 mTimestamp;
|
|
U16 mStoredFingers;
|
|
Finger mFingers[eMaxFingers];
|
|
};
|
|
|
|
|
|
/* Do the rotation:
|
|
| 0 0 -1 | | x |
|
|
| -1 0 0 | * | y |
|
|
| 0 1 0 | | z |
|
|
|
|
in this case it's just value flipping.
|
|
*/
|
|
static LLVector3 toAgentCS( float aX, float aY, float aZ )
|
|
{
|
|
return LLVector3( -aZ, -aX, aY );
|
|
}
|
|
|
|
static float scaleY( float aIn )
|
|
{
|
|
return aIn - 150.f;
|
|
}
|
|
|
|
static LLVector3 scaleToSL( LLVector3 const &aIn )
|
|
{
|
|
return aIn * 1.f/25.f;
|
|
}
|
|
|
|
static void copy( Leap::Vector const &aFrom, float *aTo )
|
|
{
|
|
aTo[0] = aFrom[0];
|
|
aTo[1] = aFrom[1];
|
|
aTo[2] = aFrom[2];
|
|
}
|
|
|
|
static float length( float const *aVec )
|
|
{
|
|
return sqrt( aVec[0]*aVec[0] + aVec[1]*aVec[1] + aVec[2]*aVec[2] );
|
|
}
|
|
|
|
static float normalize( float *aVec )
|
|
{
|
|
float len = length( aVec );
|
|
if( fabs( len ) > 0.0001f )
|
|
{
|
|
aVec[0] /= len;
|
|
aVec[1] /= len;
|
|
aVec[2] /= len;
|
|
}
|
|
else
|
|
{
|
|
len = aVec[0] = aVec[1] = aVec[2] = 0.f;
|
|
}
|
|
return len;
|
|
}
|
|
|
|
static float dot( float const *aVec1, float const *aVec2, bool aUnitVectors = true )
|
|
{
|
|
float alpha = aVec1[0]*aVec2[0] + aVec1[1]*aVec2[1] + aVec1[2]*aVec2[2];
|
|
|
|
if( !aUnitVectors )
|
|
alpha /= ( length( aVec1 )*length(aVec2) );
|
|
|
|
return acos( alpha );
|
|
}
|
|
|
|
static void subtract( float const *aLeft, float const *aRight, float *aOut )
|
|
{
|
|
aOut[0] = aLeft[0] - aRight[0];
|
|
aOut[1] = aLeft[1] - aRight[1];
|
|
aOut[2] = aLeft[2] - aRight[2];
|
|
}
|
|
|
|
static void move( float const *aStart, float const *aDirecton, float aScale, float *aOut )
|
|
{
|
|
aOut[0] = aStart[0] + aDirecton[0]*aScale;
|
|
aOut[1] = aStart[1] + aDirecton[1]*aScale;
|
|
aOut[2] = aStart[2] + aDirecton[2]*aScale;
|
|
|
|
}
|
|
|
|
ManipTool::ManipTool()
|
|
{
|
|
mLastExaminedFrame = 0;
|
|
mLastStoredFrame = 0;
|
|
mNextRenderedFrame = 0;
|
|
mTotalStoredFrames = 0;
|
|
mFingersPerFrame = new Fingers[ eMaxKeptFrames ];
|
|
}
|
|
|
|
ManipTool::~ManipTool()
|
|
{
|
|
delete []mFingersPerFrame;
|
|
}
|
|
|
|
std::string ManipTool::getName()
|
|
{
|
|
return "Manipulation tool";
|
|
}
|
|
|
|
std::string ManipTool::getDebugString()
|
|
{
|
|
return "";
|
|
}
|
|
|
|
S32 ManipTool::getId()
|
|
{
|
|
return 11;
|
|
}
|
|
|
|
void ManipTool::clearSelection()
|
|
{
|
|
//for( std::vector< LLViewerObject* >::iterator itr = mHighlighted.begin(); itr != mHighlighted.end(); ++itr )
|
|
//{
|
|
// LLViewerObject *pO = *itr;
|
|
// LLSelectMgr::getInstance()->unhighlightObjectOnly( pO );
|
|
//}
|
|
//mHighlighted.clear();
|
|
|
|
LLSelectMgr::getInstance()->unhighlightAll();
|
|
|
|
}
|
|
|
|
void ManipTool::onLeapFrame( Leap::Frame const &aFrame )
|
|
{
|
|
if( (aFrame.timestamp() - mLastExaminedFrame ) < 16*1000 )
|
|
return;
|
|
|
|
if( aFrame.hands().count() > 2 )
|
|
return;
|
|
|
|
mLastExaminedFrame = aFrame.timestamp();
|
|
U16 curFrame = getNextFrameNo( mLastStoredFrame );
|
|
Fingers &curFingers = mFingersPerFrame[ curFrame ];
|
|
U16 curFinger = 0;
|
|
curFingers.clear();
|
|
curFingers.mTimestamp = mLastExaminedFrame;
|
|
|
|
Leap::HandList hands = aFrame.hands();
|
|
for( int i = 0; i < hands.count(); ++i )
|
|
{
|
|
for( int j = 0; j < hands[i].fingers().count(); ++j )
|
|
{
|
|
Leap::Finger oFinger( hands[i].fingers()[j] );
|
|
|
|
Finger &oF = curFingers.mFingers[ curFinger++ ];
|
|
|
|
oF.mId = oFinger.id();
|
|
oF.mTimestamp = mLastExaminedFrame;
|
|
copy( oFinger.direction(), oF.mDir );
|
|
copy( oFinger.tipPosition(), oF.mTip );
|
|
oF.mWidth = oFinger.width();
|
|
oF.mLength = oFinger.length();
|
|
}
|
|
}
|
|
|
|
curFingers.mStoredFingers = curFinger;
|
|
|
|
if( mTotalStoredFrames > 0 )
|
|
{
|
|
Fingers &prevFingers = mFingersPerFrame[ mLastStoredFrame ];
|
|
|
|
for( U16 i = 0; i < curFingers.mStoredFingers; ++i )
|
|
{
|
|
Finger &curFinger = curFingers.mFingers[i];
|
|
Finger const *prevFinger = prevFingers.getFinger( curFinger.mId );
|
|
if( !prevFinger )
|
|
continue;
|
|
|
|
subtract( curFinger.mTip, prevFinger->mTip, curFinger.mFromLast );
|
|
curFinger.mLenFromLast = normalize( curFinger.mFromLast );
|
|
}
|
|
}
|
|
|
|
mLastStoredFrame = curFrame;
|
|
++mTotalStoredFrames;
|
|
}
|
|
|
|
void ManipTool::onRenderFrame( Leap::Frame const &aFrame )
|
|
{
|
|
clearSelection();
|
|
doSelect( );
|
|
}
|
|
|
|
void ManipTool::findPartner( U16 aIndex )
|
|
{
|
|
Fingers &curFingers = mFingersPerFrame[ mNextRenderedFrame ];
|
|
Finger &curFinger = curFingers.mFingers[ aIndex ];
|
|
|
|
if( !curFinger.mSelected )
|
|
return;
|
|
|
|
for( U16 i = 0; i < curFingers.mStoredFingers; ++i )
|
|
{
|
|
if( curFingers.mFingers[i].mSelected == curFinger.mSelected )
|
|
{
|
|
if( !curFingers.mFingers[i].mPartner )
|
|
{
|
|
curFingers.mFingers[i].mPartner = aIndex;
|
|
curFinger.mPartner = i;
|
|
}
|
|
else
|
|
curFinger.mSelected = 0;
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ManipTool::selectWithFinger( U16 aIndex )
|
|
{
|
|
Finger &oFinger = mFingersPerFrame[ mNextRenderedFrame ].mFingers[ aIndex ];
|
|
GLfloat x( oFinger.mTip[0] );
|
|
GLfloat y( scaleY( oFinger.mTip[1] ) );
|
|
GLfloat z( oFinger.mTip[2] );
|
|
LLVector3 oV1( scaleToSL( toAgentCS( x, y, z) ) );
|
|
|
|
x += oFinger.mDir[0] * oFinger.mLength;
|
|
y += oFinger.mDir[1] * oFinger.mLength;
|
|
z += oFinger.mDir[2] * oFinger.mLength;
|
|
|
|
LLVector3 oV2( scaleToSL( toAgentCS( x, y, z ) ) );
|
|
|
|
oV1 *= gAgentAvatarp->getRotationRegion();
|
|
oV2 *= gAgentAvatarp->getRotationRegion();
|
|
|
|
oV1 += gAgentAvatarp->getPositionAgent( );
|
|
oV2 += gAgentAvatarp->getPositionAgent( );
|
|
|
|
LLVector4a oF1,oF2;
|
|
oF1.load3( oV1.mV );
|
|
oF2.load3( oV2.mV );
|
|
|
|
S32 nFace(0);
|
|
LLViewerObject *pHit = gPipeline.lineSegmentIntersectInWorld( oF1,oF2, FALSE, TRUE, &nFace );
|
|
|
|
if( pHit )
|
|
oFinger.mSelected = pHit;
|
|
|
|
findPartner( aIndex );
|
|
}
|
|
|
|
void ManipTool::doSelect()
|
|
{
|
|
Fingers &curFingers = mFingersPerFrame[ mNextRenderedFrame ];
|
|
for( U16 i = 0; i < curFingers.mStoredFingers; ++i )
|
|
selectWithFinger( i );
|
|
|
|
for( U16 i = 0; i < curFingers.mStoredFingers; ++i )
|
|
{
|
|
Finger &curFinger = curFingers.mFingers[ i ];
|
|
if( curFinger.mPartner < i )
|
|
continue;
|
|
|
|
if( !curFinger.mSelected )
|
|
continue;
|
|
|
|
LLColor4 oCol( 0.f, 0.f, 1.f );
|
|
if( curFinger.mPartner )
|
|
oCol.set( 0.f, 1.f, 0.f );
|
|
|
|
LLSelectMgr::getInstance()->highlightObjectOnly( curFinger.mSelected, oCol );
|
|
}
|
|
}
|
|
|
|
void ManipTool::renderCone( Finger const &aFinger )
|
|
{
|
|
GLfloat x( aFinger.mTip[0] );
|
|
GLfloat y( scaleY( aFinger.mTip[1] ) );
|
|
GLfloat z( aFinger.mTip[2] );
|
|
|
|
LLVector3 oTip( scaleToSL( toAgentCS( x, y, z ) ) );
|
|
|
|
float dist = aFinger.mWidth*2;
|
|
x -= aFinger.mDir[0] *dist;
|
|
y -= aFinger.mDir[1] *dist;
|
|
z -= aFinger.mDir[2] *dist;
|
|
|
|
LLVector3 oM( scaleToSL( toAgentCS( x, y, z) ) );
|
|
float r = 0.25;
|
|
int slices = 16;
|
|
float alpha = (F_PI*2)/slices;
|
|
|
|
for( int i = 0; i < slices; ++i )
|
|
{
|
|
GLfloat cx = cos( alpha*i )*r;
|
|
GLfloat cy = sin( alpha*i )*r;
|
|
|
|
GLfloat cx2 = cos( alpha*(i+1) )*r;
|
|
GLfloat cy2 = sin( alpha*(i+1) )*r;
|
|
|
|
LLVector3 oV1( oM );
|
|
LLVector3 oV2( oM );
|
|
|
|
oV1 += LLVector3( 0, cy, -cx );
|
|
oV2 += LLVector3( 0, cy2, -cx2 );
|
|
|
|
gGL.vertex3fv( oTip.mV );
|
|
gGL.vertex3fv( oV1.mV );
|
|
|
|
gGL.vertex3fv( oV1.mV );
|
|
gGL.vertex3fv( oV2.mV );
|
|
|
|
gGL.vertex3fv( oV2.mV );
|
|
gGL.vertex3fv( oTip.mV );
|
|
}
|
|
}
|
|
|
|
void ManipTool::renderMovementDirection( Finger const &aFinger )
|
|
{
|
|
float lenDir = aFinger.mLenFromLast;
|
|
U16 prevFrame = getPrevFrameNo( mNextRenderedFrame );
|
|
Finger *prevFinger = mFingersPerFrame[ prevFrame ].getFinger( aFinger.mId );
|
|
|
|
while( prevFinger && prevFinger->mTimestamp < aFinger.mTimestamp && (aFinger.mTimestamp - prevFinger->mTimestamp ) < getMaxBacktrackMicroseconds() )
|
|
{
|
|
float alpha = dot( aFinger.mFromLast, prevFinger->mFromLast );
|
|
if( alpha > F_PI/18 )
|
|
break;
|
|
|
|
lenDir += prevFinger->mLenFromLast;
|
|
|
|
prevFrame = getPrevFrameNo( prevFrame );
|
|
prevFinger = mFingersPerFrame[ prevFrame ].getFinger( aFinger.mId );
|
|
}
|
|
#if 0
|
|
float vec[3];
|
|
bool haveVec( false );
|
|
while( aFinger.mTimestamp > mFingersPerFrame[ prevFrame ].mTimestamp &&
|
|
(aFinger.mTimestamp > mFingersPerFrame[ prevFrame ].mTimestamp) < getMaxBacktrackMicroseconds() )
|
|
{
|
|
Finger *pFinger = mFingersPerFrame[ prevFrame ].getFinger( aFinger.mId );
|
|
|
|
if( pFinger )
|
|
{
|
|
if( !haveVec )
|
|
{
|
|
subtract( aFinger.mTip, pFinger->mTip, vec );
|
|
lenDir = normalize( vec );
|
|
haveVec = true;
|
|
}
|
|
else
|
|
{
|
|
float vec2[3];
|
|
|
|
subtract( aFinger.mTip, pFinger->mTip, vec2 );
|
|
float lenDir2 = normalize( vec );
|
|
|
|
float alpha = dot( vec, vec2 );
|
|
if( alpha > F_PI/18 )
|
|
break;
|
|
lenDir += lenDir2;
|
|
}
|
|
}
|
|
prevFrame = getPrevFrameNo( prevFrame );
|
|
}
|
|
#endif
|
|
|
|
if( lenDir > 0 )
|
|
{
|
|
GLfloat x( aFinger.mTip[0] );
|
|
GLfloat y( scaleY( aFinger.mTip[1] ) );
|
|
GLfloat z( aFinger.mTip[2] );
|
|
|
|
LLVector3 oStart( scaleToSL( toAgentCS( x, y, z ) ) );
|
|
|
|
x -= aFinger.mFromLast[0] *lenDir;
|
|
y -= aFinger.mFromLast[1] *lenDir;
|
|
z -= aFinger.mFromLast[2] *lenDir;
|
|
|
|
LLVector3 oEnd( scaleToSL( toAgentCS( x, y, z) ) );
|
|
gGL.vertex3fv( oStart.mV );
|
|
gGL.vertex3fv( oEnd.mV );
|
|
}
|
|
}
|
|
|
|
void ManipTool::renderMovementAngle( Finger const &aFinger, U16 aIndex )
|
|
{
|
|
if( !aFinger.mPartner || aFinger.mPartner < aIndex )
|
|
return;
|
|
|
|
U16 prevFrame = getPrevFrameNo( mNextRenderedFrame );
|
|
Finger *prevFinger = mFingersPerFrame[ prevFrame ].getFinger( aFinger.mId );
|
|
U16 startFrame(mNextRenderedFrame);
|
|
|
|
while( prevFinger &&
|
|
prevFinger->mTimestamp < aFinger.mTimestamp && (aFinger.mTimestamp - prevFinger->mTimestamp ) < getMaxBacktrackMicroseconds() &&
|
|
prevFinger->mPartner )
|
|
{
|
|
startFrame = prevFrame;
|
|
prevFrame = getPrevFrameNo( prevFrame );
|
|
prevFinger = mFingersPerFrame[ prevFrame ].getFinger( aFinger.mId );
|
|
}
|
|
|
|
if( mNextRenderedFrame == startFrame )
|
|
return;
|
|
|
|
Finger const *startFinger( mFingersPerFrame[ startFrame ].getFinger( aFinger.mId ) );
|
|
Finger const *startPartner( mFingersPerFrame[ startFrame ].at( startFinger->mPartner ) );
|
|
|
|
Finger const *endFinger( &aFinger );
|
|
Finger const *endPartner( mFingersPerFrame[ startFrame ].at( endFinger->mPartner ) );
|
|
|
|
float vecStart[3], vecEnd[3];
|
|
subtract( startFinger->mTip, startPartner->mTip, vecStart );
|
|
subtract( endFinger->mTip, endPartner->mTip, vecEnd );
|
|
|
|
float lenStart = normalize( vecStart );
|
|
float lenEnd = normalize( vecEnd );
|
|
|
|
float radius = lenStart>lenEnd?lenStart:lenEnd;
|
|
|
|
float alpha = dot( vecStart, vecEnd );
|
|
|
|
float ptStart[3], ptEnd[3];
|
|
move( startFinger->mTip, vecStart, radius/2, ptStart );
|
|
|
|
ptEnd[0] = ptStart[0]*cos(alpha) - ptStart[1]*sin(alpha);
|
|
ptEnd[1] = ptStart[1]*cos(alpha) + ptStart[0]*sin(alpha);
|
|
ptEnd[2] = ptStart[2];
|
|
|
|
gGL.vertex3fv( ptStart );
|
|
gGL.vertex3fv( ptEnd );
|
|
}
|
|
|
|
void ManipTool::renderFinger( Finger const &aFinger, U16 aIndex )
|
|
{
|
|
if( aFinger.mPartner )
|
|
gGL.diffuseColor4f( 0.f, 1.0f, 0.0f, 1.f);
|
|
else if( aFinger.mSelected )
|
|
gGL.diffuseColor4f( 0.f, 0.0f, 1.0f, 1.f);
|
|
else
|
|
gGL.diffuseColor4f( 1.f, 1.0f, 1.0f, 1.f);
|
|
|
|
renderCone( aFinger );
|
|
renderMovementDirection( aFinger );
|
|
renderMovementAngle( aFinger, aIndex );
|
|
}
|
|
|
|
void ManipTool::render()
|
|
{
|
|
LLGLEnable blend(GL_BLEND);
|
|
gGL.getTexUnit(0)->unbind(LLTexUnit::TT_TEXTURE);
|
|
|
|
gGL.matrixMode(LLRender::MM_MODELVIEW);
|
|
gGL.pushMatrix();
|
|
gGL.begin(LLRender::LINES);
|
|
|
|
LLQuaternion rot = gAgentAvatarp->getRotationRegion();
|
|
LLVector3 pos = gAgentAvatarp->getPositionAgent();
|
|
|
|
gGL.translatef( pos.mV[0], pos.mV[1], pos.mV[2] );
|
|
gGL.multMatrix( (GLfloat*) gAgentAvatarp->getRotationRegion().getMatrix4().mMatrix );
|
|
|
|
gGL.begin(LLRender::LINES);
|
|
|
|
Fingers &curFingers = mFingersPerFrame[ mNextRenderedFrame ];
|
|
for( U16 i = 0; i < curFingers.mStoredFingers; ++i )
|
|
{
|
|
Finger &curFinger = curFingers.mFingers[ i ];
|
|
renderFinger( curFinger, i );
|
|
}
|
|
|
|
gGL.end();
|
|
|
|
gGL.popMatrix();
|
|
mNextRenderedFrame = mLastStoredFrame;
|
|
}
|
|
}
|
|
}
|