int main() {
Screen screen;
long long previousTime = (long long)Util::getCurrentTimeInMiliseconds();
long long accumulateTime = 0;
Keyboard::startListening();
Point f_pos(50,20);
Color f_col(255,255,255,255);
Pixel f_plane(f_pos, f_col);
propeller.setDrawPosition(145,30);
bird.setPosition(50,20);
fish.setPosition(50,20);
roda1.setRadius(10);
roda1.setCenter(Point(40,50));
roda2.setRadius(10);
roda2.setCenter(Point(120,50));
parasut.setPosition(50,10);
//plane.setPosition(200,10);
//plane.fillColor(f_plane);
//plane.fillPattern(&bird);
ship.setPosition(10,250);
f_pos.x = 50; f_pos.y = 20;
Pixel f_ship(f_pos, f_col);
//ship.fillColor(f_ship);
f_pos.x = ship.getWidth() / 2;
f_pos.y = ship.getHeight() / 2;
ship.fillWithFloodFill(f_pos,&fish);
int ii = 0;
float speed = 0;
float initialPercentage = 0;
/* Game Clock */
while (true) {
if (accumulateTime>(SECONDS_PER_FRAME)) {
handleInput();
printf("%c[%d;%df",0x1B,199,99);
screen.beginBatch();
if (isPlane){
//planeEx += 0.05f;
//meledak.setPosition(meledak.getPosition().x - 2,meledak.getPosition().y - 2);
initialPercentage += 0.07;
plane.explode(initialPercentage);
speed += 0.1f;
propeller.applyGravity((int)speed);
applyGravity(¶sut,(int)speed);
screen.draw(¶sut);
if (!isPlaneGrounded){
isPlaneGrounded = roda1.applyGravity((int)speed);
roda2.applyGravity((int)speed);
}else{
propeller.setState(0);
isBanSelesai = !roda1.bounce();
roda2.bounce();
}
usleep(FRAMERATE/4);
//screen.draw(&meledak, planeEx);
if (isBanSelesai){
//screen.beginBatch();
//screen.endBatch();
isPlane = false;
//isAftermath = true;
//gotoxy(1, 10);
//printf("\t\tSHIP WIN!\n\n\n\n\n\n\n\n\n\n");
isFinish = true;
/* TODO:
- Animasi pesawat pecah
- Kasih Gravity tiap pecahan pesawat biar jatuh:
- baling-baling
- ban
- potongan badan pesawat
- pilot keluar dari pesawat pake parasut
- Bikin ban mantul-mantul di atas tanah -_-
*/
// gotoxy(10, 10);
// printf("SHIP WIN!\n\n\n\n\n\n\n\n\n\n");
// exit(0);
}
}
screen.draw(&plane, isFlipPlane);
screen.draw(&roda1);
screen.draw(&roda2);
propeller.draw(&screen);
if (isShip){
shipEx += 0.05f;
meledak.setPosition(meledak.getPosition().x - 2,meledak.getPosition().y - 2);
screen.draw(&meledak, shipEx);
if (shipEx > 2){
screen.beginBatch();
screen.endBatch();
//gotoxy(10, 10);
//printf("PLANE WIN!\n\n\n\n\n\n\n\n\n\n");
isFinish = true;
isShip = false;
}
}else if (!isPlane) screen.draw(&ship, isFlipShip);
// handle peluru
for (int i = 0; i < 100; i++){
if (b[i] != NULL){
Point st = b[i]->getPoint();
bool arah = b[i]->arah;
b[i] = bf.create(BulletFactory::LASER);
if (arah == true){ //pesawat yang nembak, pelurunya kebawah
b[i]->arah = true;
b[i]->rotate(180);
b[i]->setPoint(Point(st.x, st.y + 1));
if (st.y > 330) b[i] = NULL;
if (st.x > ship.getPosition().x - 10 && st.x < ship.getPosition().x + 150 && st.y > 220){ // COLLISION
meledak.setPosition(ship.getPosition().x,ship.getPosition().y);
isShip = true;
b[i] = NULL;
}
}else{
b[i]->arah = false;
b[i]->setPoint(Point(st.x, st.y - 1));
if (st.y < 0) b[i] = NULL;
if (st.x > plane.getPosition().x - 5 && st.x < plane.getPosition().x + 180 && st.y < 40){ // COLLISION
meledak.setPosition(plane.getPosition().x,plane.getPosition().y);
isPlane = true;
b[i] = NULL;
Point explosionCenter(plane.getWidth()/2,plane.getHeight()/2);
//Ini harus dipanggil sebelum meledak
plane.startExplode(explosionCenter);
}
}
if (b[i] != NULL) screen.draw(b[i]);
}
}
if (isAftermath) {
// speed += 1; // speed untuk gravity pull
// isPlaneGrounded = plane.applyGravity(speed); /* Kasih efek gravity, return valuenya bakal 1 kalo object nya udah sampe "tanah" */
// screen.draw(&plane, isFlipPlane); /* Ini buat gambar objek2 yang udah mulai jatoh ke tanah */
// usleep(FRAMERATE); // ini buat ngedelay kecepetan refresh frame biar gak terlalu cepet
initialPercentage += 0.01;
plane.explode(initialPercentage);
if(initialPercentage == 1) { // periksa kalo semua objek udah sampe tanah, berarti game nya pindah ke state finish
isAftermath = false;
isFinish = true;
screen.beginBatch();
screen.draw(&plane, isFlipPlane); // ini buat gambar object2 yang udah berserakan di tanah
screen.endBatch();
//gotoxy(10,10);
// printf("Ship Wins!\n\n\n\n\n");
// gotoxy(40,40);
// printf("\n");
}
}
if (isFinish) {
exit(0);
}
screen.endBatch();
while (accumulateTime<(SECONDS_PER_FRAME))
accumulateTime -= (SECONDS_PER_FRAME);
}
else {
long long currentTime = (long long)Util::getCurrentTimeInMiliseconds();
accumulateTime += (currentTime - previousTime);
previousTime = currentTime;
}
}
return 0;
}
void SparseTrackerAM::pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr)
{
boost::mutex::scoped_lock(mutex_);
struct timeval start_callback, end_callback;
struct timeval start_features, end_features;
struct timeval start_model, end_model;
struct timeval start_icp, end_icp;
gettimeofday(&start_callback, NULL);
// **** initialize ***********************************************************
if (!initialized_)
{
initialized_ = getBaseToCameraTf(cloud_in_ptr);
if (!initialized_) return;
}
// **** extract features *****************************************************
gettimeofday(&start_features, NULL);
// **** ab prediction
ros::Time cur_time = cloud_in_ptr->header.stamp;
double dt = (cur_time - prev_time_).toSec();
prev_time_ = cur_time;
tf::Transform predicted_f2b;
double pr_x, pr_y, pr_z, pr_roll, pr_pitch, pr_yaw;
if (use_alpha_beta_)
{
double cx, cy, cz, croll, cpitch, cyaw;
getXYZRPY(f2b_, cx, cy, cz, croll, cpitch, cyaw);
pr_x = cx + v_x_ * dt;
pr_y = cy + v_y_ * dt;
pr_z = cz + v_z_ * dt;
pr_roll = croll + v_roll_ * dt;
pr_pitch = cpitch + v_pitch_ * dt;
pr_yaw = cyaw + v_yaw_ * dt;
btVector3 pr_pos(pr_x, pr_y, pr_z);
btQuaternion pr_q;
pr_q.setRPY(pr_roll, pr_pitch, pr_yaw);
predicted_f2b.setOrigin(pr_pos);
predicted_f2b.setRotation(pr_q);
}
else
{
predicted_f2b = f2b_;
}
PointCloudOrb::Ptr orb_features_ptr = boost::make_shared<PointCloudOrb>();
bool orb_extract_result = extractOrbFeatures(cloud_in_ptr, orb_features_ptr);
pcl::transformPointCloud (*orb_features_ptr , *orb_features_ptr, eigenFromTf(predicted_f2b * b2c_));
orb_features_ptr->header.frame_id = fixed_frame_;
// FIXME - removed canny
PointCloudCanny::Ptr canny_features_ptr = boost::make_shared<PointCloudCanny>();
bool canny_extract_result;// = extractCannyFeatures(cloud_in_ptr, canny_features_ptr);
pcl::transformPointCloud (*canny_features_ptr , *canny_features_ptr, eigenFromTf(predicted_f2b * b2c_));
canny_features_ptr->header.frame_id = fixed_frame_;
gettimeofday(&end_features, NULL);
// **** ICP ******************************
gettimeofday(&start_icp, NULL);
bool orb_icp_result = false;
bool canny_icp_result = false;
if (orb_extract_result)
{
//printf("~ORB~\n");
orb_icp_result = OrbICP(orb_features_ptr);
double eps_roll_ = 0.3;
double eps_pitch_ = 0.3;
double eps_yaw_ = 0.3;
double eps_x_ = 0.3;
double eps_y_ = 0.3;
double eps_z_ = 0.3;
if (orb_icp_result)
{
tf::Transform corr = tfFromEigen(orb_reg_.getFinalTransformation());
// particles add error to motion
double c_x, c_y, c_z, c_roll, c_pitch, c_yaw;
double e_x, e_y, e_z, e_roll, e_pitch, e_yaw;
getXYZRPY(corr, c_x, c_y, c_z, c_roll, c_pitch, c_yaw);
for (int i = 0; i < 20; i++)
{
e_roll = getUrand() * eps_roll_ * c_roll;
e_pitch = getUrand() * eps_pitch_ * c_pitch;
e_yaw = getUrand() * eps_yaw_ * c_yaw;
e_x = getUrand() * eps_x_ * c_x;
e_y = getUrand() * eps_y_ * c_y;
e_z = getUrand() * eps_z_ * c_z;
btVector3 e_pos(c_x + e_x, c_y + e_y, c_z + e_z);
btQuaternion e_q;
e_q.setRPY(c_roll + e_roll, c_pitch + e_pitch, c_yaw + e_yaw);
tf::Transform e_corr;
e_corr.setOrigin(e_pos);
e_corr.setRotation(e_q);
pf2b_[i] = e_corr * pf2b_[i];
}
// end particles
tf::Transform measured_f2b = corr * predicted_f2b;
// **** ab estmation
if (use_alpha_beta_)
{
double m_x, m_y, m_z, m_roll, m_pitch, m_yaw;
getXYZRPY(measured_f2b, m_x, m_y, m_z, m_roll, m_pitch, m_yaw);
// residuals
double r_x = m_x - pr_x;
double r_y = m_y - pr_y;
double r_z = m_z - pr_z;
double r_roll = m_roll - pr_roll;
double r_pitch = m_pitch - pr_pitch;
double r_yaw = m_yaw - pr_yaw;
fixAngleD(r_roll);
fixAngleD(r_pitch);
fixAngleD(r_yaw);
// final position
double f_x = pr_x + alpha_ * r_x;
double f_y = pr_y + alpha_ * r_y;
double f_z = pr_z + alpha_ * r_z;
double f_roll = pr_roll + alpha_ * r_roll;
double f_pitch = pr_pitch + alpha_ * r_pitch;
double f_yaw = pr_yaw + alpha_ * r_yaw;
btVector3 f_pos(f_x, f_y, f_z);
btQuaternion f_q;
f_q.setRPY(f_roll, f_pitch, f_yaw);
f2b_.setOrigin(f_pos);
f2b_.setRotation(f_q);
// final velocity
v_x_ = v_x_ + beta_ * r_x / dt;
v_y_ = v_y_ + beta_ * r_y / dt;
v_z_ = v_z_ + beta_ * r_z / dt;
v_roll_ = v_roll_ + beta_ * r_roll / dt;
v_pitch_ = v_pitch_ + beta_ * r_pitch / dt;
v_yaw_ = v_yaw_ + beta_ * r_yaw / dt;
//printf("VEL: %f, %f, %f\n", v_x_, v_y_, v_z_);
}
else
{
f2b_ = measured_f2b;
}
}
}
if (!orb_icp_result)
printf("ERROR\n");
gettimeofday(&end_icp, NULL);
// **** ADD features to model
gettimeofday(&start_model, NULL);
if (model_->points.size() == 0)
{
*model_ += *orb_features_ptr;
}
else
{
pcl::KdTreeFLANN<PointOrb> tree_model;
tree_model.setInputCloud(model_);
std::vector<int> indices;
std::vector<float> distances;
indices.resize(1);
distances.resize(1);
for (int i = 0; i < orb_features_ptr->points.size(); ++i)
{
PointOrb& p = orb_features_ptr->points[i];
int n_found = tree_model.nearestKSearch(p, 1, indices, distances);
if (n_found == 0)
{
model_->points.push_back(p);
model_->width++;
}
else
{
if ( distances[0] > 0.01)
{
//found far away, insert new one
model_->points.push_back(p);
model_->width++;
}
else
{
// found near, modify old one
//PointOrb& q = model_->points[indices[0]];
//q.x = 0.5*(p.x + q.x);
//q.y = 0.5*(p.y + q.y);
//q.z = 0.5*(p.z + q.z);
}
}
}
}
gettimeofday(&end_model, NULL);
// *** broadcast tf **********************************************************
broadcastTF(cloud_in_ptr);
// *** counter **************************************************************
frame_count_++;
// **** print diagnostics ****************************************************
gettimeofday(&end_callback, NULL);
double features_dur = msDuration(start_features, end_features);
double model_dur = msDuration(start_model, end_model);
double icp_dur = msDuration(start_icp, end_icp);
double callback_dur = msDuration(start_callback, end_callback);
//int model_frames = orb_history_.getSize();
int icp_iterations = orb_reg_.getFinalIterations();
int orb_features_count = orb_features_ptr->points.size();
int canny_features_count =canny_features_ptr->points.size();
int model_size = model_->points.size();
printf("F[%d][%d] %.1f \t M[%d] %.1f \t ICP[%d] %.1f \t TOTAL %.1f\n",
orb_features_count, canny_features_count, features_dur,
model_size, model_dur,
icp_iterations,
icp_dur, callback_dur);
}
