XnBool XnVPushDetector::IsPushDetected(const XnV3DVector& vImmediateVelocity, const XnV3DVector& vPreviousVelocity, XnFloat& fZAngle)
{
// Check if current motion is too slow
if (vImmediateVelocity.Magnitude() < m_fPushImmediateMinVelocity)
{
return false;
}
if (vPreviousVelocity.Magnitude() < m_fPushPreviousMinVelocity)
{
fZAngle = AngleBetweenVectors(vImmediateVelocity, XnV3DVector(0,0,-1));
if (fZAngle < m_fPushMaxAngleFromZ)
{
xnLogVerbose(XNV_NITE_MASK_EVENTS, "Push Detector %s [0x%08x]: "
"Immediate Velocity %5.3f over threshold %5.3f in the last %d ms (%d ms offset), "
"Previous Velocity %5.3f under threshold %5.3f in the last %d ms (%d ms offset), "
"Angle between Immediate and the Z Axis is %5.3f, under the threshold of %5.3f",
GetListenerName(), this,
vImmediateVelocity.Magnitude(), m_fPushImmediateMinVelocity, m_nPushImmediateDuration, m_nPushImmediateOffset,
vPreviousVelocity.Magnitude(), m_fPushPreviousMinVelocity, m_nPushPreviousDuration, m_nPushPreviousOffset,
fZAngle, m_fPushMaxAngleFromZ);
return true;
}
return false;
}
XnFloat fAngle = AngleBetweenVectors(vPreviousVelocity, vImmediateVelocity);
if (fAngle > m_fPushMinAngleImmediateAndPrevious)
{
fZAngle = AngleBetweenVectors(vImmediateVelocity, XnV3DVector(0,0,-1));
if (fZAngle < m_fPushMaxAngleFromZ)
{
xnLogVerbose(XNV_NITE_MASK_EVENTS, "Push Detector %s [0x%08x]: "
"Immediate Velocity %5.3f over threshold %5.3f in the last %d ms (%d ms offset), "
"Previous Velocity %5.3f over threshold %5.3f in the last %d ms (%d ms offset), "
"Angle between Immediate and the Z Axis is %5.3f, under the threshold of %5.3f, "
"Angle between Immediate and Previous direction is %5.3f, over the threshold of %5.3f",
GetListenerName(), this,
vImmediateVelocity.Magnitude(), m_fPushImmediateMinVelocity, m_nPushImmediateDuration, m_nPushImmediateOffset,
vPreviousVelocity.Magnitude(), m_fPushPreviousMinVelocity, m_nPushPreviousDuration, m_nPushPreviousOffset,
fZAngle, m_fPushMaxAngleFromZ,
fAngle, m_fPushMinAngleImmediateAndPrevious);
return true;
}
}
return false;
} // XnVPushDetector::IsPushDetected
void XnVPointDenoiser::UpdatePointDenoise(XnPoint3D &ptToChange, const XnPoint3D &ptDontChange)
{
XnV3DVector vDir = XnV3DVector(ptDontChange) - XnV3DVector(ptToChange);
XnFloat fLen = vDir.Normalize();
XnFloat fSoftThreshold = SoftThreshold(fLen, m_fDistanceThreshold);
XnV3DVector vToChange = ptToChange;
XnV3DVector vDontChange = ptDontChange;
if (fSoftThreshold == 0)
{
ptToChange = vToChange*(1-m_fCloseRatio) + vDontChange*m_fCloseRatio;
}
else
{
XnV3DVector vNewPoint = XnV3DVector(ptToChange) + (fSoftThreshold) * vDir;
ptToChange = vToChange*(1-m_fFarRatio) + vNewPoint*m_fFarRatio;
}
} // XnVPointDenoiser::UpdatePointDenoise
void DrawCircle(const XnPoint3D& ptCenter, double radius, int width, double r, double g, double b)
{
const int n=50;
glColor4f(r,g,b,1.0f);
glLineWidth(width);
double theta=0, dtheta = 2*XnVMathCommon::PI/n;
XnV3DVector vecCurr;
#ifdef USE_GLUT
glBegin(GL_LINE_LOOP);
for(int i=0; i<n; i++, theta+=dtheta) {
vecCurr = XnV3DVector(ptCenter) + radius * XnV3DVector(sin(theta), cos(theta), 0);
glVertex3f(vecCurr.X, vecCurr.Y, vecCurr.Z);
}
glEnd();
#else
GLfloat verts[n * 3];
int curr = 0;
for(int i=0; i<n; i++, theta+=dtheta, curr += 3)
{
vecCurr = XnV3DVector(ptCenter) + radius * XnV3DVector(sin(theta), cos(theta), 0);
verts[curr] = vecCurr.X;
verts[curr+1] = vecCurr.Y;
verts[curr+2] = vecCurr.Z;
}
glVertexPointer(3, GL_FLOAT, 0, verts);
glDrawArrays(GL_LINE_LOOP, 0, n);
glFlush();
#endif
}
// this function is called each frame
void glutDisplay (void)
{
// Read next available data
g_Context.WaitAnyUpdateAll();
// Process the data
g_pSessionManager->Update(&g_Context);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
#ifdef USE_GLUT
//glOrtho(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0);
glOrtho(0, 1.0, 1.0, 0, -1.0, 1.0);
#else
//glOrthof(0, GL_WIN_SIZE_X, 0, GL_WIN_SIZE_Y, -1.0, 1.0);
glOrthof(0, 1.0, 1.0, 0, -1.0, 1.0);
#endif
// draw
if (g_bDrawFrame)
{
DrawFrame(xnCreatePoint3D(0.0, 0.0, 0), xnCreatePoint3D(1.0, 1.0, 0), 8, g_fFrameR, g_fFrameG, g_fFrameB);
}
if (g_bDrawCircle)
{
XnV3DVector vec(0.5, 0.5, 0);
vec += XnV3DVector(sin(g_fCircleAngle), -cos(g_fCircleAngle), 0) * 0.3;
DrawCircle(xnCreatePoint3D(0.5, 0.5, 0), 0.3, 3, g_fCircleR, g_fCircleG, g_fCircleB);
DrawLine(xnCreatePoint3D(0.5, 0.5, 0), vec, 4, g_fCircleLineR, g_fCircleLineG, g_fCircleLineB);
}
// flip surfaces
#ifdef USE_GLUT
glutSwapBuffers();
#else
eglSwapBuffers(display, surface);
#endif
}
XnStatus XnVSwipeDetector::AddPoint(const XnPoint3D& pt, XnFloat fTime)
{
m_pMovementDetectionBuffer->AddPoint(pt, fTime);
if (m_pMovementDetectionBuffer->GetAvailableTimespan() < m_nMotionDetectionTime)
{
return XN_STATUS_NITE_NOT_ENOUGH_TIME;
}
XnV3DVector currentVelocity(m_pMovementDetectionBuffer->GetAverageVelocityByTime(m_nMotionDetectionTime, fTime));
if (m_pPendingEvent != NULL)
{
XnFloat fMotionVelocity = currentVelocity.Magnitude();
// we will wait for the final slowdown once our velocity is below
// a certain threshold
if (m_bWaitingForSlowdown)
{
if (fMotionVelocity <= m_fLowestVelocity)
{
m_fLowestVelocity = fMotionVelocity;
}
else
{
m_pPendingEvent->Raise(m_fPendingVelocity, m_fPendingAngle);
m_pSwipeCBs->Raise(m_ePendingDirection, m_fPendingVelocity, m_fPendingAngle);
m_pPendingEvent = NULL;
m_ePendingDirection = DIRECTION_ILLEGAL;
m_bWaitingForSlowdown = false;
if (m_bUseSteady)
{
m_bInSteady = true;
m_Steady.Reset();
}
}
}
else
{
m_bWaitingForSlowdown = true;
m_fLowestVelocity = fMotionVelocity;
} // if (m_bWaitingForSlowdown) else
return XN_STATUS_OK;
} // if (m_pPendingEvent != NULL)
currentVelocity.Z = 0;
XnFloat fMotionVelocity = currentVelocity.Magnitude();
currentVelocity.Normalize();
if (fMotionVelocity >= m_fMotionDetectionSpeed)
{
m_fPendingVelocity = fMotionVelocity;
XnFloat fHowCloseToX = acos(DotProduct(currentVelocity, XnV3DVector(1, 0, 0))) * (180.0f/XnVMathCommon::PI);
XnFloat fHowCloseToY = acos(DotProduct(currentVelocity, XnV3DVector(0, 1, 0))) * (180.0f/XnVMathCommon::PI);
if (fabs(fabs(fHowCloseToX) - 180) < m_fAngleXThreshold)
{
m_pPendingEvent = m_pSwipeLeftCBs;
m_ePendingDirection = DIRECTION_LEFT;
m_fPendingAngle = fHowCloseToX;
xnLogVerbose(XNV_NITE_MASK_EVENTS, "Swipe Detector %s [0x%08x]: Motion Velocity %5.2f m/s over threshold %5.2f in the last %d ms. Angle from X axis is %5.2f, within range [%5.2f-%5.2f] as LEFT",
GetListenerName(), this,
fMotionVelocity, m_fMotionDetectionSpeed, m_nMotionDetectionTime,
fabs(fHowCloseToX), 180-m_fAngleXThreshold, 180+m_fAngleXThreshold);
}
if (fabs(fHowCloseToX) < m_fAngleXThreshold)
{
m_pPendingEvent = m_pSwipeRightCBs;
m_ePendingDirection = DIRECTION_RIGHT;
m_fPendingAngle = fHowCloseToX;
xnLogVerbose(XNV_NITE_MASK_EVENTS, "Swipe Detector %s [0x%08x]: Motion Velocity %5.2f m/s over threshold %5.2f in the last %d ms. Angle from X axis is %5.2f, within range [%5.2f-%5.2f] as RIGHT",
GetListenerName(), this,
fMotionVelocity, m_fMotionDetectionSpeed, m_nMotionDetectionTime,
fHowCloseToX, -m_fAngleXThreshold, m_fAngleXThreshold);
}
if (fabs(fabs(fHowCloseToY) - 180) < m_fAngleYThreshold)
{
m_pPendingEvent = m_pSwipeDownCBs;
m_ePendingDirection = DIRECTION_DOWN;
m_fPendingAngle = fHowCloseToY;
xnLogVerbose(XNV_NITE_MASK_EVENTS, "Swipe Detector %s [0x%08x]: Motion Velocity %5.2f m/s over threshold %5.2f in the last %d ms. Angle from Y axis is %5.2f, within range [%5.2f-%5.2f] as DOWN",
GetListenerName(), this,
fMotionVelocity, m_fMotionDetectionSpeed, m_nMotionDetectionTime,
fabs(fHowCloseToY), 180-m_fAngleYThreshold, 180+m_fAngleYThreshold);
}
if (fabs(fHowCloseToY) < m_fAngleYThreshold)
{
m_pPendingEvent = m_pSwipeUpCBs;
m_ePendingDirection = DIRECTION_UP;
m_fPendingAngle = fHowCloseToY;
xnLogVerbose(XNV_NITE_MASK_EVENTS, "Swipe Detector %s [0x%08x]: Motion Velocity %5.2f m/s over threshold %5.2f in the last %d ms. Angle from Y axis is %5.2f, within range [%5.2f-%5.2f] as UP",
GetListenerName(), this,
fMotionVelocity, m_fMotionDetectionSpeed, m_nMotionDetectionTime,
fHowCloseToY, -m_fAngleYThreshold, m_fAngleYThreshold);
}
} // if (fMotionVelocity >= m_fMotionDetectionSpeed)
return XN_STATUS_OK;
} // XnVSwipeDetector::AddPoint
