bool VdfnForceFeedbackNode::DoEvalNode()
{
if(m_pEnabled)
m_pAspect->SetForcesEnabled(m_pEnabled->GetValue());
// check whether this is a simple control node
// in that case both (force and torque) are NULL
if(!m_pForceVec && !m_pTorque)
return true;
return m_pAspect->SetForce( (m_pForceVec ? m_pForceVec->GetValue() : VistaVector3D()),
(m_pTorque ? m_pTorque->GetValue() : VistaVector3D()) );
}
/*============================================================================*/
KinectHandler::KinectHandler( VistaSystem* pVistaSystem )
: IVdfnActionObject()
, m_pSceneGraph( pVistaSystem->GetGraphicsManager()->GetSceneGraph() )
{
VistaGeometryFactory oGeomFac( m_pSceneGraph );
m_pManRoot = m_pSceneGraph->NewTransformNode( m_pSceneGraph->GetRoot() );
m_pManRoot->Rotate( 0, 1, 0, 0 );
m_pManRoot->Translate( 0, 0, -1.5f );
m_vecJoints.resize( ELEM_COUNT );
VistaGeometry* pGeom = NULL;
VistaVector3D v3Offset;
VistaColor oFlesh( 1.0f, 0.8f, 0.6f );
VistaColor oShirt( 1.0f, 0.2f, 0.2f );
VistaColor oTrousers( 0.2f, 0.2f, 0.9f );
pGeom = oGeomFac.CreateEllipsoid( 0.1f, 0.13f, 0.1f, 32, 32, oFlesh );
v3Offset = VistaVector3D( 0.0f, -0.125f, 0.0f );
CreateElement( ELEM_HEAD, pGeom, v3Offset );
pGeom = oGeomFac.CreateEllipsoid( 0.2f, 0.28f, 0.15f, 32, 32, oShirt );
v3Offset = VistaVector3D( 0.0f, -0.28f, 0.0f );
CreateElement( ELEM_TORSO, pGeom, v3Offset );
pGeom = oGeomFac.CreateEllipsoid( 0.05f, 0.15f, 0.05f, 32, 32, oShirt );
v3Offset = VistaVector3D( 0.0f, -0.135f, 0.0f );
CreateElement( ELEM_RIGHT_UPPER_ARM, pGeom, v3Offset );
CreateElement( ELEM_RIGHT_LOWER_ARM, pGeom, v3Offset );
CreateElement( ELEM_LEFT_UPPER_ARM, pGeom, v3Offset );
CreateElement( ELEM_LEFT_LOWER_ARM, pGeom, v3Offset );
pGeom = oGeomFac.CreateEllipsoid( 0.05f, 0.1f, 0.025f, 32, 32, oFlesh );
v3Offset = VistaVector3D( 0.0f, -0.135f, 0.0f );
CreateElement( ELEM_RIGHT_HAND, pGeom, v3Offset );
CreateElement( ELEM_LEFT_HAND, pGeom, v3Offset );
pGeom = oGeomFac.CreateEllipsoid( 0.1f, 0.2f, 0.1f, 32, 32, oTrousers );
v3Offset = VistaVector3D( 0.0f, -0.19f, 0.0f );
CreateElement( ELEM_RIGHT_UPPER_LEG, pGeom, v3Offset );
CreateElement( ELEM_LEFT_UPPER_LEG, pGeom, v3Offset );
pGeom = oGeomFac.CreateEllipsoid( 0.065f, 0.2f, 0.065f, 32, 32, oTrousers );
v3Offset = VistaVector3D( 0.0f, -0.19f, 0.0f );
CreateElement( ELEM_RIGHT_LOWER_LEG, pGeom, v3Offset );
CreateElement( ELEM_LEFT_LOWER_LEG, pGeom, v3Offset );
pGeom = oGeomFac.CreateEllipsoid( 0.08f, 0.06f, 0.15f, 32, 32, oTrousers );
v3Offset = VistaVector3D( 0.0f, -0.04f, -0.06f );
CreateElement( ELEM_RIGHT_FOOT, pGeom, v3Offset );
CreateElement( ELEM_LEFT_FOOT, pGeom, v3Offset );
}
bool KinectHandler::SetPosition( const std::vector<VistaVector3D>& vecPos )
{
VistaVector3D v3Position;
VistaQuaternion qOrientation;
for( int i = 0; i < ELEM_COUNT; ++i )
{
v3Position = vecPos[S_afElementFromJoint[i]];
v3Position = m_matTransform * v3Position;
if( S_afElementToJoint[i] != NULL )
{
VistaVector3D v3To = vecPos[S_afElementToJoint[i]];
v3To = m_matTransform * v3To;
v3To = v3Position - v3To;
qOrientation = VistaQuaternion( VistaVector3D( 0, 1, 0 ), v3To );
}
m_vecJoints[i]->SetTranslation( v3Position );
m_vecJoints[i]->SetRotation( qOrientation );
}
return true;
}
void HandRenderer::DrawHand(HandModel *pModel,
HandGeometry *pGeometry) {
VistaTransformMatrix matModel;
VistaTransformMatrix matTransform;
VistaTransformMatrix matOrigin;
// these go to extra vis parameter class for model pso yo
float fPalmWidth = 0.09;
float fPalmBottomRadius = 0.01;
float fPalmDiameter = fPalmWidth/2.0f;
float fFingerDiameter = fPalmWidth/4.0f;
// for now we average the metacarpal lengths for palm height
float fPalmHeight =
(pGeometry->GetExtent(HandGeometry::I_MC) +
pGeometry->GetExtent(HandGeometry::M_MC) +
pGeometry->GetExtent(HandGeometry::R_MC) +
pGeometry->GetExtent(HandGeometry::L_MC))/4.0f/1000.0f;
// @todo get hand geometry straight, palm is too small...
fPalmHeight -= fPalmBottomRadius * 2.0f;
float fLRFactor =
(pModel->GetType() == HandModel::LEFT_HAND) ?
-1.0f : 1.0f;
// for now we will upload the same model transform matrix in
// advance to calling glDrawArrays. If this turns out to be
// slow, we might upload all the model matrices at once at the
// beginning and use DrawArraysInstanced to look up the
// specific matrix in the vertex shader by the instance id.
matOrigin.Compose(
pModel->GetPosition(),
pModel->GetOrientation(),
VistaVector3D(1,1,1));
// bottom palm cap
matTransform.Compose(
VistaVector3D(0, fPalmBottomRadius, 0),
VistaQuaternion(),
VistaVector3D(fPalmWidth,
fPalmBottomRadius*2.0f,
fPalmDiameter));
matModel = matOrigin * matTransform;
DrawSphere(matModel);
// top palm cap
matTransform.Compose(
VistaVector3D(0, fPalmBottomRadius+fPalmHeight, 0),
VistaQuaternion(),
VistaVector3D(fPalmWidth,
fPalmBottomRadius*2.0f,
fPalmDiameter));
matModel = matOrigin * matTransform;
DrawSphere(matModel);
// palm cylinder
matTransform.Compose(
VistaVector3D(0, fPalmBottomRadius + fPalmHeight/2.0f, 0),
VistaQuaternion(),
VistaVector3D(fPalmWidth, fPalmHeight, fPalmDiameter));
matModel = matOrigin * matTransform;
DrawCylinder(matModel);
// draw the fingers
for(int finger = 0 ; finger < 4 ; finger++) {
matTransform.SetToTranslationMatrix(
VistaVector3D((-fPalmWidth/2.0f + fPalmWidth/8.0f +
finger*fPalmWidth/4.0f) * fLRFactor,
fPalmBottomRadius + fPalmHeight,
0));
DrawFinger(
matOrigin * matTransform,
fFingerDiameter, fLRFactor,
pModel->GetJointAngle(4*(1+finger)),
pModel->GetJointAngle(4*(1+finger)+1),
pGeometry->GetExtent(3+4*finger+1),
pModel->GetJointAngle(4*(1+finger)+2),
pGeometry->GetExtent(3+4*finger+2),
pModel->GetJointAngle(4*(1+finger)+3),
pGeometry->GetExtent(3+4*finger+3),
false);
}
// draw the thumb
matTransform.SetToTranslationMatrix(
VistaVector3D((-fPalmWidth/2.0f + fPalmWidth/4.0f)*fLRFactor,
fPalmBottomRadius,
0));
DrawFinger(
matOrigin * matTransform,
fFingerDiameter*1.2, fLRFactor,
pModel->GetJointAngle(HandModel::T_CMC_F),
pModel->GetJointAngle(HandModel::T_CMC_A),
pGeometry->GetExtent(HandGeometry::T_MC),
pModel->GetJointAngle(HandModel::T_MCP),
pGeometry->GetExtent(HandGeometry::T_PP),
pModel->GetJointAngle(HandModel::T_IP),
pGeometry->GetExtent(HandGeometry::T_DP),
true);
matModel = VistaTransformMatrix(
0,0,0,0,
0,0,0,0,
0,0,0,0,
0,0,0,0);
DrawCylinder(matModel);
}
void HandRenderer::DrawFinger(VistaTransformMatrix matOrigin,
float fFingerDiameter, float fLRFactor,
float fAng1F, float fAng1A, float fLen1,
float fAng2, float fLen2,
float fAng3, float fLen3,
bool bThumb) {
// @todo invert axis here for left/right hand?
// rotate locally around X for flexion/extension
VistaAxisAndAngle aaaX = VistaAxisAndAngle(VistaVector3D(1,0,0), 0.0);
// rotate locally around Z for adduction/abduction
VistaAxisAndAngle aaaZ = VistaAxisAndAngle(VistaVector3D(0,0,1), 0.0);
VistaTransformMatrix matModel; // final applied transform
VistaTransformMatrix matTransform; // auxiliary for
// accumulative transforms
VistaTransformMatrix matSphereScale; // reused sphere scale
matSphereScale.SetToScaleMatrix(fFingerDiameter);
// start at first joint
matModel = matOrigin * matSphereScale;
if(!bThumb) {
DrawSphere(matModel);
}
else {
matOrigin *=
VistaTransformMatrix(
VistaAxisAndAngle(VistaVector3D(0,1,0),
Vista::DegToRad(-90*fLRFactor)));
}
// first joint abduction rotation
aaaZ.m_fAngle = Vista::DegToRad(fAng1A*fLRFactor);
matTransform = VistaTransformMatrix(aaaZ);
matOrigin *= matTransform;
// first joint flexion rotation
aaaX.m_fAngle = Vista::DegToRad(-fAng1F);
matTransform = VistaTransformMatrix(aaaX);
matOrigin *= matTransform;
// move to center of first segment
matTransform.SetToTranslationMatrix(
VistaVector3D(0, fLen1/1000.0f/2.0f, 0));
matOrigin *= matTransform;
// set scale and draw first segment
if(bThumb) {
matTransform.SetToScaleMatrix(
fFingerDiameter*1.5f,
fLen1/1000.0f*1.5f,
fFingerDiameter*1.5f);
matModel = matOrigin * matTransform;
DrawSphere(matModel);
}
else {
matTransform.SetToScaleMatrix(
fFingerDiameter, fLen1/1000.0f, fFingerDiameter);
matModel = matOrigin * matTransform;
DrawCylinder(matModel);
}
// move to second joint
matTransform.SetToTranslationMatrix(
VistaVector3D(0, fLen1/1000.0f/2.0f, 0));
matOrigin *= matTransform;
// draw scaled sphere
matModel = matOrigin * matSphereScale;
DrawSphere(matModel);
// second joint flexion rotation
aaaX.m_fAngle = Vista::DegToRad(-fAng2);
matTransform = VistaTransformMatrix(aaaX);
matOrigin *= matTransform;
// move to center of second segment
matTransform.SetToTranslationMatrix(
VistaVector3D(0, fLen2/1000.0f/2.0f, 0)); // rotation missing here
matOrigin *= matTransform;
// set scale and draw second segment cylinder
matTransform.SetToScaleMatrix(
fFingerDiameter, fLen2/1000.0f, fFingerDiameter);
matModel = matOrigin * matTransform;
DrawCylinder(matModel);
// move to third joint
matTransform.SetToTranslationMatrix(
VistaVector3D(0, fLen2/1000.0f/2.0f, 0)); // rotation missing here
matOrigin *= matTransform;
// draw scaled sphere
matModel = matOrigin * matSphereScale;
DrawSphere(matModel);
// third joint flexion rotation
aaaX.m_fAngle = Vista::DegToRad(-fAng3);
matTransform = VistaTransformMatrix(aaaX);
matOrigin *= matTransform;
// move to center of third segment
matTransform.SetToTranslationMatrix(
VistaVector3D(0, fLen3/1000.0f/2.0f, 0)); // rotation missing here
matOrigin *= matTransform;
// set scale and draw third segment cylinder
matTransform.SetToScaleMatrix(
fFingerDiameter, fLen3/1000.0f, fFingerDiameter);
matModel = matOrigin * matTransform;
DrawCylinder(matModel);
// move to tip
matTransform.SetToTranslationMatrix(
VistaVector3D(0, fLen3/1000.0f/2.0f, 0)); // rotation missing here
matOrigin *= matTransform;
// draw scaled sphere
matModel = matOrigin * matSphereScale;
DrawSphere(matModel);
}
