Pekomin/Align.cpp at master · mgomezch/Pekomin · GitHub

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
60
61
#include <cmath>
#include <vector>

#include "Align.hpp"
#include "Mobile.hpp"
#include "Triple.hpp"
#include "util.hpp"

//#define DEBUG_ALIGN

#ifdef DEBUG_ALIGN
#include <iostream>
#endif

Align::Align(std::string name, Mobile *character, Mobile *target, double maxAngularVelocity, double targetRadius, double slowRadius):
        DirectKinematicA(name),
        character(character),
        target(target),
        maxAngularVelocity(maxAngularVelocity),
        targetRadius(targetRadius),
        slowRadius(slowRadius)
{}

std::vector<double> Align::getAngVel(unsigned int ticks, unsigned int delta_ticks) {
        double steering;
        double rotation, rotationSize, targetRotation;
        Triple direction;

        rotation = mapToRange(mapToRange(target->ang) - mapToRange(character->ang));
        rotationSize = abs(rotation);

        if (rotationSize < targetRadius) {
#ifdef DEBUG_ALIGN
                std::cout << "Align " << static_cast<void *>(this) << ": dentro de targetRadius" << std::endl;
#endif
                steering = target->vrot - character->vrot;
                if (abs(steering) > maxAngularVelocity) {
                        steering = maxAngularVelocity;
                }
                return std::vector<double>(1, steering);
        }

        targetRotation = maxAngularVelocity;
        if (rotationSize < slowRadius){
                targetRotation *= rotationSize / slowRadius;
#ifdef DEBUG_ALIGN
                std::cout << "Align " << static_cast<void *>(this) << ": entre targetRadius y slowRadius; targetRotation = " << targetRotation << "; rotation = " << rotation << std::endl;
#endif
        }
#ifdef DEBUG_ALIGN
        else {
                std::cout << "Align " << static_cast<void *>(this) << ": fuera de slowRadius; targetRotation = " << targetRotation << "; rotation = " << rotation << std::endl;
        }
#endif

        //targetRotation *= map_atan(20000 * target->vrot);

        steering = targetRotation * (rotation > 0 ? -1 : 1);

        return std::vector<double>(1, steering);
}