first push

xuepengqiang

2019-12-26 025429cbd5dd3dc6f130b8de07664665b0c4b55a

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
#include "human_velocity.h"
#include <iomanip>
#include "math_utils.h"
HumanVelocity::HumanVelocity()
{
    printf("34343333333333");
}
HumanVelocity::HumanVelocity(int bottom_distance, int base_distance)
{
    this->MAX_SIZE = 10;
    for (int i = 0; i <this->MAX_SIZE ; ++i) {
        this->coords_structure.push_back({0,0,0});
        this->move_dist.push_back(0);
    }
    this->RPS = 25;
    this->total_distance = 0.0;
    this->store_pointer = 0;
    this->init_size = 0;
    this->real_world_rate = base_distance/bottom_distance;
    MathUtils mu;
    this->math_utils = mu;
 
}
 
void HumanVelocity::add_next_point(int point_x, int point_y, double point_z, double pers_rate)
{
    printf("add_next_point");
    this->coords_structure[this->store_pointer][0] = point_x;
    this->coords_structure[this->store_pointer][1] = point_y;
    this->coords_structure[this->store_pointer][2] = point_z; 
    this->store_pointer = (this->store_pointer + 1)%this->MAX_SIZE;
    this->init_size ++;
    if(this->init_size>1)
    {
//        printf("coords::::", this->coords_structure[(this->store_pointer -2 + this->MAX_SIZE)%this->MAX_SIZE]);
        double pers_distance = this->math_utils.cal_distance(this->coords_structure[(this->store_pointer -2 + this->MAX_SIZE)%this->MAX_SIZE],
                this->coords_structure[(this->store_pointer -1 + this->MAX_SIZE)%this->MAX_SIZE]);
        pers_distance = (pers_distance / pers_rate)*this->real_world_rate*0.1;
 
        double move_dist = sqrt(pow(pers_distance,2) + pow(
                                                           this->coords_structure[(this->store_pointer - 2 + this->MAX_SIZE) % this->MAX_SIZE][2] -
                                                           this->coords_structure[(this->store_pointer - 1 + this->MAX_SIZE) % this->MAX_SIZE][2],2));
        move_dist *= (this->coords_structure[this->store_pointer][2]/10);  
 
        this->total_distance += move_dist;
        this->move_dist[this->store_pointer-1] = move_dist;
    }
 
    this->total_distance -= this->move_dist[this->store_pointer];
}
 
 
double HumanVelocity::cal_human_velocity(int rps)
{
    this->RPS = rps;
    double h_velocity = this->total_distance*this->RPS/(this->MAX_SIZE - 2);
 
    return h_velocity/100;
}