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.
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();
}
};
|
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);
}
}
};
|
| 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; } |
Since the above zip file may not include the latest "NtKinect.h", Download the latest version from here and replace old one with it.