NtKinect: Kinect V2 C++ Programming with OpenCV on Windows10

BallFall game with Kinect V2


2016.07.22: created by
Japanese English
To Table of Contents

Prerequisite knowledge


BallFall Game

Let's make a game playing so that, with your skeleton, you bounces off the falling ball not to fall down to the ground. To simplify the explanation, we do not process game scores etc.


How to write program

  • Start using the Visual Studio's project KinectV2_skeleton.zip of "NtKinect: How to recognize human skeleton with Kinect V2" .
  • Add Ball.h to the project.
  • Copy Ball.h to the folder where the source files of this project are placed. Then add it to the project.

    Ball.h
    #pragma once
    #include <iostream>
    #include <sstream>
    #include <opencv2/opencv.hpp>
    using namespace std;
    
    class Ball
    {
    #define EPS 1.0e-6
     private:
      cv::Vec3f pos;
      cv::Vec3f vel;
      cv::Vec3f accel;
      double r;
     public:
     Ball() : pos(0, 0, 0), vel(0, 0, 0), accel(0, 0, 0), r(1.0) {}
      Ball(cv::Vec3f &pos, cv::Vec3f &v, cv::Vec3f &accel, double r) {
        this->pos = pos;
        this->vel = v;
        this->accel = accel;
        this->r = r;
      }
      ~Ball() {}
      void setPos(cv::Vec3f &pos) { this->pos = pos; }
      cv::Vec3f getPos() { return pos; }
      void setV(cv::Vec3f &v) { this->vel = v; }
      cv::Vec3f getV() { return vel; }
      void setAccel(cv::Vec3f &a) { accel = a; }
      cv::Vec3f getAccel() { return accel; }
      void setR(double r) { this->r = r; }
      double getR() { return r; }
      void step() {
        vel += accel;
        pos += vel;
      }
      bool bounce(pair<cv::Vec3f, cv::Vec3f> &seg) {
        cv::Vec3f nearPt = nearest(seg, pos);
        double len = cv::norm(pos, nearPt);
    
        cv::Vec3f oldPos = pos - vel;
        pair<cv::Vec3f, cv::Vec3f> trail(oldPos, pos);
        pair<cv::Vec3f, cv::Vec3f> pr = nearest(seg, trail);
        double len2 = cv::norm(pr.first, pr.second);
    
        if (len2 < EPS) {
          cv::Vec3f pt = oldPos - pr.second;
          cv::Vec3f par = seg.second - seg.first;
          cv::Vec3f normal = par.cross(cv::Vec3f(0, 0, 1));
          normal /= cv::norm(normal);
          double cs = pt.dot(normal);
          if (cs < 0){
    	normal *= -1.0; cs *= -1.0;
          }
          vel = 2 * cs * normal - pt;
          pos += vel;
          return true;
        }
        else if (len < r) {
          reflect(seg, nearPt);
          return true;
        }
        return false;
      }
      bool bounce(Ball &ball) {
        cv::Vec3f normal = pos - ball.pos;
        double len = cv::norm(normal);
        if (len >= r + ball.r) return false;
        if (len < 1.0) {
          pos[0] = -1;
          normal = pos - ball.pos;
          len = cv::norm(normal);
        }
        normal /= len;
        cv::Vec3f u = -vel;
        double cs = normal.dot(u);
        if (cs > 0) vel = 2 * cs * normal - u;
        pos += (r + ball.r - len) / 2 * normal;
    
        normal *= -1.0;
        u = -ball.vel;
        cs = normal.dot(u);
        if (cs > 0) ball.vel = 2 * cs * normal - u;
        ball.pos += (r + ball.r - len) / 2 * normal;
    
        return true;
      }
      pair<cv::Vec3f, cv::Vec3f> nearest(pair<cv::Vec3f, cv::Vec3f> &seg1, pair<cv::Vec3f, cv::Vec3f> &seg2) {
        pair<cv::Vec3f, cv::Vec3f> ans;
        cv::Vec3f ab = seg1.second - seg1.first, cd = seg2.second - seg2.first, ac = seg2.first - seg1.first;
        cv::Vec3f v = ab.cross(cd);
        if (v[2] > EPS) {
          cv::Vec3f u = ac.cross(cd), w = ac.cross(ab);
          double t = u[2] / v[2], s = w[2] / v[2];
          if (t >= 0 - EPS && t <= 1.0 + EPS && s >= 0 - EPS && s <= 1.0 + EPS) {
    	if (abs(t) < EPS) { ans.first = ans.second = seg1.first; }
    	else if (abs(t - 1) < EPS) { ans.first = ans.second = seg1.second; }
    	else if (abs(s) < EPS) { ans.first = ans.second = seg2.first; }
    	else if (abs(s - 1) < EPS) { ans.first = ans.second = seg2.second; }
    	else {
    	  ab[2] = 0.0;
    	  ans.first = ans.second = ab*t + seg1.first;
    	}
    	return ans;
          }
        }
        ans.first = seg1.first;
        ans.second = nearest(seg2, seg1.first);
        double minlen = cv::norm(ans.second, seg1.first);
        v = nearest(seg2, seg1.second);
        double len = cv::norm(v, seg1.second);
        if (len < minlen) { ans.first = seg1.first; ans.second = v; minlen = len; }
        v = nearest(seg1, seg2.first);
        len = cv::norm(v, seg2.first);
        if (len < minlen) { ans.first = v; ans.second = seg2.first; minlen = len; }
        v = nearest(seg1, seg2.second);
        len = cv::norm(v, seg2.second);
        if (len < minlen) { ans.first = v; ans.second = seg2.second; minlen = len; }
        return ans;
      }
    
      double minDistance(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &pt) {
        cv::Vec3f nearPt = nearest(seg, pt);
        return cv::norm(nearPt, pt);
      }
      cv::Vec3f nearest(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &pt) {
        cv::Vec3f p(seg.second - seg.first), q(pt - seg.first);
        double t = p.dot(q) / p.dot(p);
        if (t <= 0.0 + EPS) return seg.first;
        if (t >= 1.0 - EPS) return seg.second;
        p = p*t + seg.first;
        return p;
      }
      void reflect(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &nearPt) {
        cv::Vec3f normal = pos - nearPt;
        double len = cv::norm(normal);
        if (len < 1.0) { // if ball's center is on the segment, add 2 to ball.y
          pos[1] -= 2;
          normal = pos - nearPt;
          len = cv::norm(normal);
        }
        normal /= len;
        if (len<r) pos += (r - len) * normal;
        if (normal.dot(vel) > 0) { // same direction
          vel += 5 * normal;
        }
        else {
          cv::Vec3f u = -vel;
          vel = 2 * normal.dot(u) * normal - u; // reflect
        }
      }
      bool in(cv::Mat &image) {
        return pos[0] >= 0 && pos[0] < image.cols && pos[1] >= -30 && pos[1] < image.rows;
      }
      void draw(cv::Mat &image, int id = -1) {
        cv::Point pt((int)pos[0], (int)pos[1]);
        cv::circle(image, pt, r, cv::Scalar(255, 0, 255), -1);
        stringstream ss; ss << id;
        if (id >= 0) cv::putText(image, ss.str(), pt, cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
      }
      string tostring() {
        stringstream ss;
        ss << "ball[" << pos << " " << vel << " " << accel << " " << r << "]";
        return ss.str();
      }
    };
    
    
  • Add BallFall.h to the project.
  • Copy BallFall.h to the folder where the source files of this project are placed. Then add it to the project.

    BallFall.h
    #pragma once
    #include <time.h>
    #include <opencv2/opencv.hpp>
    #include "Ball.h"
    using namespace std;
    
    class BallFall
    {
    #define N_BALL	8
    
     private:
      cv::Mat image;
      vector<Ball> balls;
      cv::Vec3f mouse;
     public:
      BallFall() {
        srand((unsigned int)time(NULL));
      }
      ~BallFall() {}
      void start(int w = 640, int h = 480) {
        image = cv::Mat(h, w, CV_8UC3);
        for (int i = 0; i < N_BALL; i++) {
          Ball ball;
          randomBall(ball);
          balls.push_back(ball);
        }
        cv::namedWindow("ball fall");
        mouse[0] = -1;
      }
      void randomBall(Ball &b) {
        cv::Vec3f pos, v, accel;
        pos[0] = rand() % image.cols;
        pos[1] = 0; pos[2] = 0;
        v[0] = rand() % 4 - 2; v[1] = rand() % 1; v[2] = 0;
        accel[0] = 0; accel[1] = 1.0; accel[2] = 0;
        b.setPos(pos); b.setV(v); b.setAccel(accel); b.setR(30);
      }
      void step(vector<pair<cv::Vec3f, cv::Vec3f>>& segments) {
        for (int i = 0; i < balls.size(); i++) balls[i].step();
        for (int i = 0; i < balls.size(); i++) {
          for (int j = i + 1; j < balls.size(); j++)
    	balls[i].bounce(balls[j]);
        }
        for (int i = 0; i < balls.size(); i++) {
          for (int j = 0; j < segments.size(); j++)
    	balls[i].bounce(segments[j]);
          if (!balls[i].in(image)) randomBall(balls[i]);
        }
      }
      void draw(vector<pair<cv::Vec3f, cv::Vec3f>>& segments) {
        cv::rectangle(image, cv::Rect(0, 0, image.cols, image.rows), cv::Scalar(50, 50, 50), -1);
        draw(segments, image);
        cv::imshow("ball fall", image);
      }
    
      void draw(vector<pair<cv::Vec3f, cv::Vec3f>>& segments, cv::Mat& image) {
        for (int i = 0; i < balls.size(); i++) {
          balls[i].draw(image);
        }
        for (int i = 0; i < segments.size(); i++) {
          cv::Point p((int)segments[i].first[0], (int)segments[i].first[1]);
          cv::Point q((int)segments[i].second[0], (int)segments[i].second[1]);
          cv::line(image, p, q, cv::Scalar(255, 255, 0), 8);
        }
      }
    };
    
    
    
  • Change the contents of main.cpp as follows.
  • main.cpp
    #include <iostream>
    #include <sstream>
    
    #include "NtKinect.h"
    #include "BallFall.h"
    
    const int segment[] = {
      JointType_SpineBase, JointType_SpineMid,           // spine
      JointType_SpineMid, JointType_SpineShoulder,
      JointType_SpineShoulder, JointType_Neck,           // head
      JointType_Neck, JointType_Head,
    
      JointType_SpineShoulder, JointType_ShoulderLeft,   // left hand
      JointType_ShoulderLeft, JointType_ElbowLeft,
      JointType_ElbowLeft, JointType_WristLeft,
      JointType_WristLeft, JointType_HandLeft,
      JointType_HandLeft, JointType_HandTipLeft,
      JointType_HandLeft, JointType_ThumbLeft,
    
      JointType_SpineShoulder, JointType_ShoulderRight,  // right hand
      JointType_ShoulderRight, JointType_ElbowRight,
      JointType_ElbowRight, JointType_WristRight,
      JointType_WristRight, JointType_HandRight,
      JointType_HandRight, JointType_HandTipRight,
      JointType_HandRight, JointType_ThumbRight,
      
      JointType_SpineBase,  JointType_HipLeft,           // left leg
      JointType_HipLeft,  JointType_KneeLeft,
      JointType_KneeLeft,  JointType_AnkleLeft,
      JointType_AnkleLeft,  JointType_FootLeft,
    
      JointType_SpineBase,  JointType_HipRight,          // right leg
      JointType_HipRight,  JointType_KneeRight,
      JointType_KneeRight,  JointType_AnkleRight,
      JointType_AnkleRight,  JointType_FootRight,
    };
    const int segmentSize = sizeof(segment) / sizeof(int);
    
    using namespace std;
    
    void doJob() {
      NtKinect kinect;
      BallFall bf;
      bool flag = false;
      kinect.setRGB();
      bf.start(kinect.rgbImage.cols, kinect.rgbImage.rows);
      vector<pair<cv::Vec3f, cv::Vec3f>> skel;
      while (1) {
        kinect.setRGB();
        kinect.setSkeleton();
        skel.clear();
        for (int i = 0; i < kinect.skeleton.size(); i++) {
          for (int j = 0; j < segmentSize/2; j++) {
    	Joint joint1 = kinect.skeleton[i][segment[j * 2]];
    	Joint joint2 = kinect.skeleton[i][segment[j * 2 + 1]];
    	if (joint1.TrackingState != TrackingState_NotTracked
    	    && joint2.TrackingState != TrackingState_NotTracked) {
    	  ColorSpacePoint cp;
    	  kinect.coordinateMapper->MapCameraPointToColorSpace(joint1.Position, &cp);
    	  cv::Vec3f p1 = cv::Vec3f(cp.X, cp.Y, 0.0f);
    	  kinect.coordinateMapper->MapCameraPointToColorSpace(joint2.Position, &cp);
    	  cv::Vec3f p2 = cv::Vec3f(cp.X, cp.Y, 0.0f);
    	  skel.push_back(pair<cv::Vec3f, cv::Vec3f>(p1,p2));
    	}
          }
        }
        bf.step(skel);
        cv::Mat image;
        if (flag) image = kinect.rgbImage.clone();
        else {
          image = cv::Mat(kinect.rgbImage.rows,kinect.rgbImage.cols,CV_8UC3);
          cv::rectangle(image,cv::Rect(0,0,image.cols,image.rows),cv::Scalar(0,0,0), -1);
        }
        bf.draw(skel, image);
        cv::imshow("ball fall", image);
        auto key = cv::waitKey(1);
        if (key == 'q') break;
        if (key == 'v') flag = !flag;
      }
    }
    
    int main(int argc, char** argv) {
      try {
        doJob();
      } catch (exception &ex) {
        cout << ex.what() << endl;
        string s;
        cin >> s;
      }
      return 0;
    }
    
    
  • When you run the program, the skeleton is displayed as a lineman and the falling ball bounces back when it hits the skeleton. Exit with 'q' key. Whether to draw over the RGB image will be changed by 'v' key.

  •    

  • Please click here for this sample project KinectV2_ballFall.zip.
  • Since the above zip file may not include the latest "NtKinect.h", Download the latest version from here and replace old one with it.



    http://nw.tsuda.ac.jp/